💥💥💞💞欢迎来到本博客❤️❤️💥💥
🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。
⛳️座右铭：行百里者，半于九十。
目录
💥1 概述
📚2 运行结果
🎉3 参考文献
🌈4 Matlab代码实现
💥1 概述
船舶运动规划是海上自主水面舰艇（MASS）自主导航的核心问题。该文提出一种考虑避碰规则的复杂遭遇场景模型预测人工势场（MPAPF）运动规划方法。建立了一个新的船舶域，其中设计了一个闭区间势场函数来表示船舶域的不可侵犯属性。在运动规划过程中，采用具有预定义速度的Nomoto模型来生成符合船舶运动学的可遵循路径。为解决传统人工势场（APF）方法的局部最优问题，保证复杂遭遇场景下的防撞安全，提出一种基于模型预测策略和人工势场的运动规划方法MPAPF。该方法将船舶运动规划问题转化为具有机动性、导航规则、通航航道等多重约束的非线性优化问题。4个算例的仿真结果表明，与APF、A星和快速探索随机树（RRT）的变体相比，所提出的MPAPF算法能够解决上述问题，并生成可行的运动路径，避免复杂遭遇场景下的船舶碰撞。
文献来源：
📚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);