✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
船舶运动规划是海上自主水面船舶（MASS）自主航行的核心问题。本文提出了一种新的模型预测人工势场（MPAPF）运动规划方法，用于考虑避碰的复杂遭遇场景规则。建立了一个新的舰船域，其中设计了一个闭区间势场函数来表示舰船域的不可侵犯性。采用在运动规划期间具有预定义速度的 Nomoto 模型来生成符合船舶运动学的可跟随路径。为解决传统人工势场(APF)方法的局部最优问题，保证复杂遭遇场景下的避撞安全，提出了一种基于模型预测策略和人工势场的运动规划方法——MPAPF。该方法将船舶运动规划问题转化为具有包括机动性在内的多重约束的非线性优化问题、航行规则、通航航道等。4 个案例研究的仿真结果表明，与 APF、A-star 和 rapidly 的变体相比，所提出的 MPAPF 算法可以解决上述问题，并生成可行的运动路径，以避免复杂遭遇场景中的船舶碰撞-探索随机树（RRT）。
⛄ 部分代码
% Description: This is a demo for MPAPF (Model Predictive Artificial
% Potential Field), and case1_1.m is the program used in Case 1, sta-
% obstacles collision avoidance, Fig. 16.
% How to use: RUN this file and input the prediction step (we suggest
% that it should be 1~10, in this program, 1 step may cost you 120 s, while
% 10 steps may be 100 times higher than 1 step.) The program
% will real-time display the results and finally generate a gif named '1.gif'
% Reference:
clc
close all
clear
addpath lib
global mat_point
global end_point
global ship
global ship1
global parameter
global current_step
global targetship
global tmp_end_point
global tsID
global end_point_targetship
global start_point
%% 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);
%% loop
for current_step=1:parameter.searching_step
pause(0.02)
steable = 0;
if current_step>=3
if path1(current_step-1,5)-path1(current_step-2,5)<=1 && path1(current_step-1,5)-path1(current_step-2,5)>=-1
steable = 1;
end
end
if ~exist('targetship','var')
targetship=init_ship(ship,'others',1000);
end
[iscv,cvship] = iscollisionavoidacne(ship,targetship);
for tship = 2:size(targetship,2)
if steable && targetship(tship).situation == 0
targetship(tship).situation=COLREGS_situation(ship,targetship(tship));
switch targetship(tship).situation
case 1 %head-on
targetship(tship).shipdomain = parameter.situs1*targetship(tship).length/parameter.scale;
case 2 %crossing give-wall
targetship(tship).shipdomain = parameter.situs2*targetship(tship).length/parameter.scale;
case 3 %crossing stand-on
targetship(tship).shipdomain = parameter.situs3*targetship(tship).length/parameter.scale;
case 0 %other
targetship(tship).shipdomain = parameter.situs0*targetship(tship).length/parameter.scale;
end
end
end
for pship = 1:parameter.prediction_step
for tship = 1:size(targetship,2)
tmp_tship = shipdynamic(targetship(tship),0,parameter.prediction_step*parameter.time);
targetship(tship).predicted_position(pship,:)=tmp_tship.position;
end
end
% You can change the prediction position of target ship here.
% [result_mat_1, ~]=ga(@PSO_MPC_Cost_Function,2,[],[],[],[],[0 -ship1.delta],[0 ship1.delta]);
[result_mat, ~]=particleswarm(@PSO_MPC_Cost_Function,parameter.navars,LB_follower,UB_follower);
% You can change the optimizer here, particleswarm is a PSO-based
% optimizer in matlab toolbox, and it is the fastest optimizer we
% tested (among GA\fmincon\PSO etc.).
tmp_tship=ship;
for pship = 1:parameter.prediction_step
tmp_tship = shipdynamic(tmp_tship,result_mat(2*pship));
ship.predicted_position(pship,:)=tmp_tship.position;
end
ship = shipdynamic(ship,result_mat(2));
path1(current_step,:)=[ship.position,...
navi_risk(ship.position),... navigation risk
ship.speed,...
mod(ship.yaw,360),...
ship.yaw_rate,...
ship.rudder*180/pi];
%% for display, if you close them all, it will be faster.
if norm(ship.position-end_point)<=parameter.end1
leader_stop=1;
else
if current_step>=2
delete(hline1);
delete(hshipd);
end
if exist('hshipt','var')
delete(hshipt);delete(hdomain);delete(hdomain1);
% delete(hshipt3);delete(hdomain3);delete(hdomain13);
end
subplot( 'Position',[0.0686204431736955 0.348877374784111 0.429433722500199 0.583657167530225])
box on
hline1=plot(path1(:,1),path1(:,2),'Color',[0 0 255]/255,'LineWidth',1);
hshipd=shipDisplay3([ship.yaw 0 0],path1(current_step,1),path1(current_step,2),0,0.3,[0 1 0]);
plot(ship.predicted_position(:,1),ship.predicted_position(:,2),'--','Color',[0 0 200]/255,'LineWidth',1);
if size(targetship,2) >1
hshipt=shipDisplay3([targetship(2).yaw 0 0],targetship(2).position(1),targetship(2).position(2),0,0.1,[1 0 0]);
hdomain=domaindisplay(targetship(2).position(1),targetship(2).position(2),R_tmp,targetship(2).yaw,[1 1 1]);
hdomain1=domaindisplay(targetship(2).position(1),targetship(2).position(2),R_tmp*parameter.shipdomain,targetship(2).yaw,[1 1 1]);
if ~isempty(tsPath{2})
hlinet=plot(tsPath{2}(:,1),tsPath{2}(:,2),'Color',[255 0 0]/255,'LineWidth',1);
end
end
legend1=legend('start','target','static obstacles','detection area','path by own algorithm','own ship');
set(legend1,...
'Position',[0.322658966237086 0.232429764580638 0.17964731814842 0.252224199288256]);
subplot( 'Position',[0.553551686703887 0.739205526770294 0.334524660471765 0.195164075993097])
h2=plot(1:5:5*current_step,path1(1:current_step,7),'--','Color',[0 0 255]/255,'LineWidth',1);
xlabel(' time (s)');
ylabel('rudder (deg)');
axis([0 5*current_step -30 30]);
set(gca,'FontSize',10)
subplot( 'Position',[0.554653817490069 0.349740932642487 0.333809864188702 0.27979274611399])
h3=plot(1:5:5*current_step,path1(1:current_step,5),'-.','Color',[0 0 255]/255,'LineWidth',1);
axis([0 5*current_step 0 360]);
xlabel(' time (s)');
ylabel('course angle (deg)');
end
if norm(ship.position-end_point)<=parameter.end1
break
end
for tship = 2:size(targetship,2)
tsID = tship;
end_point_targetship=[1 1];
[result_mat_ts, ~]=particleswarm(@MPC_Cost_Function_Targetship,parameter.navars,LB_follower,UB_follower);
targetship(tship)=shipdynamic(targetship(tship),result_mat_ts(2));
tsPath{tship}=[tsPath{tship};
targetship(tship).position,...
navi_risk(targetship(tship).position),...
targetship(tship).speed,...
targetship(tship).yaw,...
targetship(tship).yaw_rate,...
targetship(tship).rudder*180/pi];
[dcpa,tcpa]=DCPA_TCPA(ship,targetship(tship));
end
F=getframe(gcf);
I=frame2im(F);
[I,map]=rgb2ind(I,256);
if current_step == 1
imwrite(I,map,'1.gif','gif', 'Loopcount',inf,'DelayTime',0.2);
elseif mod(current_step,5) == 1
imwrite(I,map,'1.gif','gif','WriteMode','append','DelayTime',0.2);
end% for gif
end
toc
⛄ 运行结果
⛄ 参考文献
He Z, Chu X, Liu C, et al. A novel model predictive artificial potential field based ship motion planning method con-sidering COLREGs for complex encounter scenarios[J]. ISA Transactions, 2022.
