[人工智能]船舶自动避碰系统(静态避碰和动态避碰-人工势场)——Matlab实现

1.情景解析

????????根据《国际海上避碰规则》第十四条相关规定，当两艘船在相反或者接近相反的航向上相遇并有碰撞危险的一种会遇局面。此时两船具有相同的避让责任，各自应向右转向。在方位上进行分辨即相对方位在355°—5°，同时艏向角差值的绝对值小于5°

????????交叉相遇局面分为左舷交叉相遇和右舷交叉相遇两种相遇方式。

????????左舷交叉相遇：他船位于本船左舷侧。他船相对本船方位247.5°-355°他船有让路责任，从本船船尾驶过；本船船直行。

????????右舷交叉相遇：他船位于本船右舷侧。他船相对本船方位5°-112.5°，他船直行；本船有让路责任，从他船船尾驶过。

2.建模

1. 构造本船模型、定义障碍物范围圆
2. 定义DCPA隶属函数和TCPA隶属函数来判断碰撞危险度
3. 多艘来船判断危险程度
4. 避船方式判断
5. 复航时机判断
6. 基于人工势场的静态障碍物避碰

3.仿真结果

a.有对向来船（动态障碍）和静态障碍：仿真示例

代码：

clc
clear
close all
%% 信息
set = 1;
case_set =1;
%对遇1，右交叉2，左交叉3，追越4
if case_set == 1
    %本船, 速度为海里
    X_o = 0;
    Y_o = 0;
    Yt = 20;
    Xt = 20;
    v_o = 20;
    fai_o = atan((Yt-Y_o)/(Xt-X_o));
    %目标船
    v_t = 15;
    fai_t = 225;
    fai_t = fai_t/180*pi;
    X_t = 18;
    Y_t = 18;
elseif case_set == 2
    %本船, 速度为海里
    X_o = 0;
    Y_o = 0;
    Yt = 20;
    Xt = 20;
    v_o = 20;
    fai_o = atan((Yt-Y_o)/(Xt-X_o));
    %目标船
    v_t = 10;
    fai_t = 270;
    fai_t = fai_t/180*pi;
    X_t = 16;
    Y_t = 10;
elseif case_set == 3
    %本船, 速度为海里
    X_o = 0;
    Y_o = 0;
    Yt = 20;
    Xt = 20;
    v_o = 20;
    fai_o = atan((Yt-Y_o)/(Xt-X_o));
    %目标船
    v_t = 5;
    fai_t = 90;
    fai_t = fai_t/180*pi;
    X_t = 10;
    Y_t = 12;
elseif case_set == 4
    %本船, 速度为海里
    X_o = 0;
    Y_o = 0;
    Yt = 20;
    Xt = 20;
    v_o = 20;
    fai_o = atan((Yt-Y_o)/(Xt-X_o));
    %目标船
    v_t = 2.5;
    fai_t = 45;
    fai_t = fai_t/180*pi;
    X_t = 10;
    Y_t = 10;
end
%% 相对信息
%两船距离R_t
%目标船的相对速度v_tao
%目标船相对速度的角度fai_tao
%目标船相对本船的真实方位a_t
%目标船的相对方位theta_t
[R_t,v_tao,fai_tao,a_t,theta_t,DCPA,TCPA,d1,d2,gamma] = xd_data(X_o,X_t,Y_o,Y_t,v_o,v_t,fai_o,fai_t);

%% 会遇局面判断
%对遇
if (theta_t<=5/180*pi||theta_t>=355/180*pi)&& (abs(pi-abs(fai_t-fai_o))<=5/180*pi)
    i = 1;
    %右交叉
elseif theta_t>5/180*pi && theta_t<112.5/180*pi
    i = 2;
    %左交叉
elseif theta_t>247.5/180*pi && theta_t<355/180*pi
    i = 3;
    %追越
else
    i = 4;
end

t = 1/3000;
X = [X_o, Y_o];
Y = [X_t, Y_t];
fai = [fai_o];
am = [];
r = 0;
Rt = sqrt((Xt-X_o)^2+(Yt-Y_o)^2);
mk = 1;
mkk = [mk];

dist = [];
gamma_all = [];
DCPA_all = [];
TCPA_all = [];
fai_o_all = [];
d1_all = [];
d2_all = [];
while Rt > 1
    mk = mk + 1;
    [R_t,v_tao,fai_tao,a_t,theta_t,DCPA,TCPA,d1,d2,gamma] =  xd_data(X_o,X_t,Y_o,Y_t,v_o,v_t,fai_o,fai_t);
    %对遇避碰--安全航行
    beta = 0; %转向角度
    if gamma > 0.4 && r == 0 %没调过角度
        r = 1;
        gamma_before = gamma;
        while abs(DCPA)<d2 && (i==1 || i == 2 || i==4)
            %采取避碰
            temp = 1; %表示转过角度
            beta = beta + 1;
            fai_o = fai_o + 1/180*pi;
            [R_t,v_tao,fai_tao,a_t,theta_t,DCPA,TCPA,d1,d2,gamma] =  xd_data(X_o,X_t,Y_o,Y_t,v_o,v_t,fai_o,fai_t);
        end
        while abs(DCPA)<d2 && (i==3)
            %采取避碰
            temp = 1; %表示转过角度
            beta = beta + 1;
            fai_o = fai_o - 1/180*pi;
            [R_t,v_tao,fai_tao,a_t,theta_t,DCPA,TCPA,d1,d2,gamma] =  xd_data(X_o,X_t,Y_o,Y_t,v_o,v_t,fai_o,fai_t);
        end
        disp('*****************避碰******************')
        disp(['避碰转角点: ',num2str([X_o,Y_o])])
        disp(['避碰转角: ',num2str(beta),'度'])
        disp(['避碰前危险度: ',num2str(gamma_before)])
        disp(['避碰后危险度: ',num2str(gamma)])
        
        mkk = [mkk; mk];
    end
    if r == 1
        C_fh = atan((Xt-X_o)/(Yt-Y_o));
        [R_t,v_tao,fai_tao,a_t,theta_t,DCPA,TCPA,d1,d2,gamma] =  xd_data(X_o,X_t,Y_o,Y_t,v_o,v_t,C_fh,fai_t);
        if TCPA < 0
            mkk = [mkk; mk];
            fai_o = C_fh;
            r = 2;
            disp('*****************复航******************')
            disp(['目的转角点: ',num2str([X_o,Y_o])])
            disp(['目的角度为: ',num2str(C_fh*180/pi),'度'])
            disp(['复航后危险度: ',num2str(gamma)])
            
            disp('**********避碰到复航时间***************')
            disp(['时间: ',num2str(round((mkk(end)-mkk(end-1))*t,2)),'h'])
        end
    end
    if r == 0 ||r == 2
        [R_t,v_tao,fai_tao,a_t,theta_t,DCPA,TCPA,d1,d2,gamma] = xd_data(X_o,X_t,Y_o,Y_t,v_o,v_t,fai_o,fai_t);
        Xo = [X_o, Y_o];
        Xsum = [Xt,Yt;X_t,Y_t;17,16];
        Vsum = [0,0;v_t * sin(fai_t),v_t * cos(fai_t); 0,0];
        fai_all = [0, fai_t];
        vo = [v_o*sin(fai_o) v_o*cos(fai_o)];
        fai_o = pi/2 - untitled(Xo, Xsum, vo,Vsum, set);

        
    end
    X_o = X_o + v_o * t * sin(fai_o);
    Y_o = Y_o + v_o * t * cos(fai_o);
    X_t = X_t + v_t * t * sin(fai_t);
    Y_t = Y_t + v_t * t * cos(fai_t);
    X = [X; X_o, Y_o];
    Y = [Y; X_t, Y_t];
    fai =[fai;fai_o];
    am = [am; gamma];
    Rt = sqrt((Xt-X_o)^2+(Yt-Y_o)^2);
    
    dist = [dist R_t];
    gamma_all = [gamma_all gamma];
    DCPA_all = [DCPA_all, DCPA];
    TCPA_all = [TCPA_all, TCPA];
    fai_o_all = [fai_o_all, fai_o];
    d1_all = [d1_all, d1];
    d2_all = [d2_all, d2];
end
len = size(TCPA_all,2);
excel_cell{1,1} = 'TCPA';
excel_cell{1,2} = 'DCPA';
excel_cell{1,3} = 'gamma';
excel_cell{1,4} = 'd1';
excel_cell{1,5} = 'd2';
for i = 1:len
excel_cell{i+1,1} = TCPA_all(i);
excel_cell{i+1,2} = DCPA_all(i);
excel_cell{i+1,3} = gamma_all(i);
excel_cell{i+1,4} = d1_all(i);
excel_cell{i+1,5} = d2_all(i);
end
xlswrite(['single_case_set_',num2str(case_set),'.xlsx'], excel_cell)



mkk = [mkk; mk];

figure()
plot(((2:mk)-1)*t*60, dist,'LineWidth',2)
xlabel('时间/min')
ylabel('距离')
figure()
plot(((2:mk)-1)*t*60, gamma_all,'LineWidth',2)
xlabel('时间/min')
ylabel('危险度')
figure()
plot(((2:mk)-1)*t*60, DCPA_all,'LineWidth',2)
xlabel('时间/min')
ylabel('DCPA')
figure()
plot(((2:mk)-1)*t*60, TCPA_all,'LineWidth',2)
xlabel('时间/min')
ylabel('TCPA')
figure()
plot(((2:mk)-1)*t*60, fai_o_all/pi*180,'LineWidth',2)
xlabel('时间/min')
ylabel('本船航向')


figure()
plot(X(:,1),X(:,2),'.r')
hold on
plot(Y(:,1),Y(:,2),'.b')

V0 = 10;
L = 200/1875;
kAD = 10 ^ (0.3591*log10(V0) + 0.0952);
kDT = 10 ^ (0.5441*log10(V0) - 0.0795);
Rfore = (1 + 1.34 * sqrt(kAD^2 +(kDT/2)^2) ) * L;
Raft = (1 + 0.67 * sqrt(kAD^2 +(kDT/2)^2) ) * L;
Rstarb = (0.2 + kDT) * L;
Rport = (0.2 + 0.75*kDT) * L;

hold on
plot(Xsum(3:end,1),Xsum(3:end,2),'o','linewidth',2)
hold on
for j = 1:size(mkk)
    i = mkk(j);
    hold on
    rectangle('Position',[Y(i,1)-0.3,Y(i,2)-0.3,0.6,0.6],'Curvature',[1,1],  'FaceColor','k')
    syms x y
    x = x - X(i,1);
    y = y - X(i,2);
    
    z= [cos(fai(i)),-sin(fai(i));sin(fai(i)),cos(fai(i))]*[x;y];
    x = z(1);
    y = z(2);
    hold on
    h = ezplot((2*y/((1+sign(y))*Rfore-(1-sign(y))*Raft))^2 + (2*x/((1+sign(x))*Rstarb-(1-sign(x))*Rport))^2==1,[-2 20 -2 20]);
end
hold on
str=[repmat('  fai:',[size(mkk,1)-1,1]) num2str(round(fai_o_all(mkk(1:end-1))'/pi*180)) repmat(',  gamma:',[size(mkk,1)-1,1]) num2str(gamma_all(mkk(1:end-1))')];
text(X(mkk(1:end-1),1),X(mkk(1:end-1),2),cellstr(str))

function y = sgn(x)
if x >= 0
    y = 1;
else
    y = -1;
end
end

结果

b.三船情景一

c.三船情景二

d.基于人工势场法的避障

请点赞+收藏

链接：https://pan.baidu.com/s/1zbRB78vu1q_vAM6OkBZ7CQ?
提取码：or09

阅读世界，共赴山海

423全民读书节，邀你共读