基于模型预测人工势场的船舶运动规划方法，考虑复杂遭遇场景下的COLREG（Matlab代码实现）-CSDN博客

本文链接：https://blog.csdn.net/qq_46009436/article/details/147058403

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码、文章下载

💥1 概述

船舶运动规划是海上自主水面船（MASS）自主航行的核心问题。本文提出了一种考虑避碰规则的复杂遭遇场景下的新型模型预测人工势场（MPAPF）运动规划方法。建立了一种新的船舶领域，并设计了一个闭区间势场函数来表示船舶领域的不可侵犯性。在运动规划过程中，采用具有预设速度的Nomoto模型生成符合船舶运动学的可跟踪路径。为解决传统人工势场（APF）方法的局部最优问题，并确保复杂遭遇场景下的避碰安全性，提出了一种基于模型预测策略和人工势场的运动规划方法，即MPAPF。在此方法中，船舶运动规划问题被转化为一个包含操纵性、航行规则、可航水道等多约束的非线性优化问题。4个案例研究的仿真结果表明，与APF的各种变体、A*算法和快速探索随机树（RRT）相比，所提出的MPAPF算法能够解决上述问题，并生成可行的运动路径，以避免复杂遭遇场景下的船舶碰撞。

文献来源：

引言

船舶碰撞事故是船舶航行事故的主要类型，可能会带来重大的生命威胁和财产损失。对船舶碰撞事故潜在原因的调查表明，超过75%的碰撞事故是由于人为错误决策导致的。在一定程度上，开发海上自主水面船（MASS）可以减少船舶碰撞事故，特别是避免人为错误。

近年来，随着人工智能、大数据、先进传感器等技术的发展，MASS受到了广泛关注。然而，由于其非线性动力学特性、较大的惯性、复杂的航行态势理解以及与航行规则的安全避碰，MASS仍然面临重大挑战。国际海上避碰规则（COLREGs）在不同遭遇情况下提供了具体的行动建议，这些建议应在航行中予以考虑。因此，如何在考虑船舶动力学和COLREGs约束的情况下进行决策规划是一个重大挑战，尤其是在复杂的遭遇场景中。

作为一种决策规划方法，运动规划最初用于将机器人期望的运动任务分解为满足运动学和动力学约束的离散运动。与机器人不同，船舶始终在不断变化的环境中航行，这使得在较长时间内预测船舶的航行状态变得困难，因为不可忽视的干扰因素。因此，船舶运动规划旨在预先模拟船舶运动，生成动态可行且安全的轨迹，以避免在自主航行过程中的碰撞。

船舶规划算法通常被提出以实现自动避碰甚至自主航行。He等人提出了一种动态抗碰撞A星（DAA*）算法，用于解决复杂场景中与目标船舶的动态碰撞避免问题。生成的轨迹表明，所提出的DAA能够实现无碰撞。然而，A及其变体是基于网格的方法，这增加了处理车辆运动学的难度。相比之下，其他算法，如快速探索随机树（RRT）、速度障碍（VO）、优化算法和人工势场（APF）能够更好地处理运动学和动力学约束。Chiang等人研究了一种COLREGS-RRT算法，用于在动态环境中识别无碰撞且符合COLREGs的轨迹。Mou等人分析了给定功率与船舶速度之间的关系，提出了一种基于VO和船舶领域的最优轨迹规划方法。Du等人使用VO估计潜在碰撞风险，并提出了一种考虑船舶操纵性的非线性VO运动规划算法。Ni等人提出了一种多基因算法（MGA）和线性扩展避碰系统。此外，船舶运动的数学模型和船舶操纵控制机制被采用以提高避碰系统的可靠性。

由于其平滑路径和较小的计算量，APF及其变体已广泛用于运动规划。Lyu和Yi使用基于电子海图的矢量环境信息模型构建了导航中不同障碍物的排斥场。然而，该算法未能解决传统APF方法陷入局部最优的问题。为此，一些学者提出了结合APF和其他算法的混合算法，可以利用不同算法的优势和劣势。Sang等人使用APF-A算法进行避障，其中A用于在APF存在局部最优时引导目标脱离死点区域。Farhad等人提出了一种基于静电势场（EPF）的路径规划算法，并使用高势能值防止机器人陷入局部最优。Asl等人使用无性繁殖优化（ARO）算法优化APF，仿真结果表明，混合算法比传统APF更快，并且能够处理局部最优问题。在复杂的航行场景中，除了航行规则约束外，还需要考虑限制排斥范围的障碍物排斥函数，以避免无法解决的势场。然而，排斥范围的限制可能导致规划器未能及时检测到障碍物，从而导致避障失败，尤其是在船舶操纵性约束的情况下。由于其处理多约束问题和预测未来运动的能力，模型预测控制（MPC）被广泛应用于运动控制领域。MPC可以在不确定的环境中，根据多个约束条件计算未来时刻的最优控制输入序列。Xie等人提出了改进的Q学习甲虫天线搜索（I-Q-BSAS）算法，并将其与MPC集成，旨在实现多船遭遇场景下的船舶避碰。总之，MPC在处理具有延迟运动和多约束对象（如船舶）的运动规划方面具有显著优势。

表1显示了典型路径规划方法的比较，这些方法在方法类型（路径、轨迹和运动规划）、考虑静态障碍物（数量和形状）、考虑障碍物领域（大于实际障碍物范围的区域）、运动学、多动态障碍物遭遇情况、自身代理的避碰动作（速度变化）以及紧急避碰动作等方面进行了评估。

在船舶运动规划算法设计中，应在真实遭遇场景中考虑航行规则，如COLREGs。然而，在船舶运动规划中考虑航行规则并不容易，因为COLREGs并未规定特定遭遇场景的具体操纵动作。例如，COLREGs规则14规定，当两船迎头相遇时，禁止相互从右舷通过，但并未为两艘船提供具体动作。为解决这一问题，一些研究在运动规划过程中禁止搜索目标船（TS）的右舷侧。然而，这些方法不适用于多船遭遇场景，因为它们可能会陷入局部最优，导致无法生成可行路径。一些学者增加了矢量约束以扩大可搜索区域。Lyu和Yi提出了一种基于APF的无人水面艇（USV）避碰方法，并增加了一个新的虚拟力以符合COLREGs，但未考虑风险量化。其他学者引入船舶领域以量化航行风险。Woo和Kim利用Coldwell的船舶领域识别基于深度强化学习（DRL）的避碰系统的奖励。Tsou提出了一种基于电子海图的避碰方法，其船舶领域是一个半径为1.5海里的圆形。他们使用了一个更简单的领域，但在面对经典遭遇场景时，它无法反映正确的避碰动作。Wang提出了一个四元数船舶领域模型，用于描述遭遇情况下TS不同侧的航行风险。然而，不对称的船舶领域在描述特定遭遇场景（如右舷迎头船）时可能无法符合COLREGs规则。此外，其他研究引入了偏心船舶领域以应对船舶规划。Mou等人引入了一个幻影船以生成偏心船舶领域，并使用VO在多船遭遇情况下生成无碰撞路径。结果表明，该方法能够生成符合良好船艺的无碰撞路线。

尽管一些与人工智能相关的运动规划算法已成功应用于多个动态规划场景，但它们并不总能满足实时和复杂的规划需求。相比之下，传统的APF方法可以生成平滑路径且计算量小，但在考虑COLREGs规则和船舶操纵性约束以及避免陷入局部最优方面存在不足。为解决这些问题，本文尝试做出以下贡献：

基于设计的模型预测策略和船舶运动学模型，将路径规划问题转化为运动规划问题，并通过非线性后退优化问题求解。
生成符合COLREGs规则的新船舶领域，并根据遭遇场景使船舶领域偏心化。
提出一种针对不同类型障碍物的新APF函数，其中静态障碍物被定义为点型障碍物，目标船舶被视为领域型障碍物。

本文的结构如下：第2节介绍了改进的势场函数、四元数船舶领域模型、船舶运动学模型和遭遇模型。第3节根据不同的遭遇场景引入势场，并提出了一种基于模型预测人工势场（MPAPF）的船舶运动规划算法。第4节进行了4个案例的仿真实验。第5节总结了研究成果并提出了未来的研究方向。

结论

本研究提出了一种基于模型预测的船舶运动规划方法，即模型预测人工势场（MPAPF）方法，该方法能够克服传统人工势场（APF）和其他算法的不足，例如容易陷入局部最优、未考虑船舶运动学以及无法结合国际海上避碰规则（COLREGs）。本研究提出了一种基于闭合曲线的势场，并将一种新的、符合COLREGs的船舶领域应用于势场。船舶运动学基于一阶非线性Nomoto模型进行了考虑。根据COLREGs中的典型遭遇场景，通过引入幻影目标船（TS）和幻影目标对势场进行了修改。四个案例的仿真结果表明，所提出的MPAPF算法能够在多船遭遇情况下避开静态和动态障碍物，并且避碰动作能够很好地符合COLREGs。

在未来的运动规划研究中，需要考虑本船（OS）和目标船（TS）的速度变化。非线性优化问题的相对较长计算时间也需要在船舶运动规划中加以解决。此外，还应考虑环境干扰和系统不确定性，以通过更可行的规划路径提高船舶安全性。

📚2 运行结果

输入预测步长（我们建议应该是1~10，在这个程序中，1步可能花费你120秒，而10步可能比1步高100倍。

部分代码：

%% initialization
static_obs_num = [12;6];
mailme = 0;
% static_obs_area = [0.8, 2, 7, 8;
% 2, 0.2, 10, 2];
static_obs_area = [0.5, 0, 4.5, 4;
4.5, 2, 6.5 3.5];
dynamic_ships = [1;1;1;1];
parameter.waterspeed = 0/1852;
parameter.waterangle = 45;
parameter.water = [sind(parameter.waterangle) cosd(parameter.waterangle)]*parameter.waterspeed;
parameter.minpotential = 0.001;
parameter.minpotential4ship = 0.01;
parameter.minobstacle = 0.03;
parameter.maxobstacle = 0.2;
parameter.safeobstacle = 5;
parameter.amplification = 5;
parameter.safecv = 2.5;%n mile safety collision aviodance distance
parameter.dangercv = 0.5;% danger collision aviodance distance
parameter.shipdomain = 5;
parameter.tsNum = 1;
%% simulation environment
map_size = [8,4.5];
start_point = [1 1];
end_point = [7,4];
tmp_end_point = end_point;
parameter.endbeta = -log(parameter.minpotential)/(norm(end_point-start_point)*2)^2;
mat_point = init_obstacles(static_obs_num,static_obs_area); % Generate static obstacles randomly
ship.speed = 8.23; % meters per second equal to 16 knots
ship.v = 0;
ship.data = [... data = [ U K T n3]
6 0.08 20 0.4
9 0.18 27 0.6
12 0.23 21 0.3 ];
% interpolate to find K and T as a function of U from MSS toolbox 'frigate'
prompt = 'Prediction Step\n'; % input the prediction step, 1~10
parameter.prediction_step = input(prompt);
ship.k = interp1(ship.data(:,1),ship.data(:,2),ship.speed,'linear','extrap'); %ship dynamic model you can change the dynamic model in shipdynamic.m
ship.T = interp1(ship.data(:,1),ship.data(:,3),ship.speed,'linear','extrap');
ship.n3 = interp1(ship.data(:,1),ship.data(:,4),ship.speed,'linear','extrap');
ship.Ddelta = 10*pi/180; % max rudder rate (rad/s)
ship.delta = 30*pi/180; % max rudder angle (rad)
ship.length = 100;
ship.p_leader = -8;
ship.alpha_leader = 3;
ship.yaw = 45;
ship.yaw_rate = 0;
ship.rudder = 0;
ship.rudder_rate = 0;
ship.position = [1 1];
ship.gamma = 1;
ship.lamda = log(1/parameter.minpotential4ship-1)/((parameter.shipdomain)^2-1);
ship.prediction_postion = [];
tsPath{parameter.tsNum} = [];
ship1 = ship;
parameter.scale = 1852; % 1852m in real world ,1 in simulation;
parameter.time = 5; % time per step
parameter.searching_step = 3000; % max searching step
parameter.map_size = map_size; % map size for display
parameter.map_min = 0.05; % minmum map details
parameter.end1 = 0.05;
% parameter.situs1 = [6.1 1.75 3.25 1.75];
parameter.situs1 = [6.1 3.9 3.25 1.75]; % a quaternion ship domain proposed in Wang 2010,situs1 means in head-on situation
parameter.situs2 = [6.1 3.9 3.25 1.75]; % a quaternion ship domain proposed in Wang 2010,situs2 means in crossing and give-way situation
parameter.situs3 = [0.0 0.0 0.00 0.00]; % a quaternion ship domain proposed in Wang 2010,situs3 means in crossing and stand-on situation
parameter.situs0 = [6.0 6.0 1.75 1.75]; % a quaternion ship domain proposed in Wang 2010,situs0 means in norm naviation situation
ship_scale = 1;
leader_stop = 0;
tic
draw2();
set(gcf,'position',[200 200 1361/1.5 750/2]);
hold on
LB_follower = [];
UB_follower = [];
for i = 1 : parameter.prediction_step
LB_follower = [LB_follower 0 -ship.delta];% lower limiting value
UB_follower = [UB_follower 0 ship.delta];% upper limiting value
end
parameter.navars = 2*parameter.prediction_step;% number of navars
targetship=init_ship(ship,'others',1000);

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。(文章内容仅供参考，具体效果以运行结果为准)

[1]He, Zhibo, et al. “A Novel Model Predictive Artificial Potential Field Based Ship Motion Planning Method Considering COLREGs for Complex Encounter Scenarios.” ISA Transactions, Elsevier BV, Sept. 2022, doi:10.1016/j.isatra.2022.09.007.