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

            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)=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)