【路径规划】基于模型预测人工势场MPAPF求解考虑复杂遭遇场景的 COLREG船舶运动规划附matlab代码

本文提出了一种基于模型预测和人工势场的MPAPF运动规划方法，适用于解决海上自主船舶的避碰问题。在考虑舰船动力学和通航规则的同时，MPAPF能有效避免复杂遭遇场景中的碰撞，通过与传统方法的比较，展示出更好的性能。仿真结果证明了其在路径规划中的可行性和优势。

✅作者简介：热爱科研的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

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

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

% 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

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.