✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。
🍎个人主页：Matlab科研工作室
🍊个人信条：格物致知。
更多Matlab仿真内容点击👇
⛄ 内容介绍
船舶自动避碰是航海研究的热点之一,国内外专家学者提出了许多方法并开发了多种系统.然而,如何测试和验证这些方法和系统仍然是一个有待解决的问题.实船海上试验风险高费用大,同时灵活性差花费时间长;许多研究人员所采用的单机仿真试验则难以有效仿真复杂的海上航行情况.本文分析了现有船舶自动避碰系统的不足,提出了基于人工势场法的船舶自动避碰系统.根据船舶的操纵特性,建立了引力场和斥力场函数,实现船舶自动避碰航行的合力和方向,在电子海图所建立的仿真环境中,验证了人工势场法可用于船舶自动避碰系统中
⛄ 部分代码
clc
clear
close all
%% 信息
%
X_o = 0;
Y_o = 0;
Yt = 100;
Xt = 100;
v_o = 20;
fai_o = atan((Yt-Y_o)/(Xt-X_o));
%目标船?
% v_t_all = [10 1 1];
% fai_t_all = [270 215 215];
%
% X_t_all = [16 50 84];
% Y_t_all = [10 47 50];
v_t_all = [5, 5, 5];
fai_t_all = [330,150,400];
X_t_all = [32,25,60];
Y_t_all = [22,35,50];
fai_t_all = fai_t_all./180.*pi;
set = 1;
xx1=X_o;
yy1=Y_o;
yy2=Yt;
xx2=Xt;
%% 相对信息
%两船距离 R_t
%?标船的相对速度 v_tao
%?标船相对速度的?度 fai_tao
%?标船相对本船的真实?位 a_t
%?标船的相对?位 theta_t
t = 1/3600;
X = [X_o, Y_o];
Y = [X_t_all, Y_t_all];
fai = [fai_o];
am = [];
r = 0;
Rt = sqrt((Xt-X_o)^2+(Yt-Y_o)^2);
mk = 1;
mkk = [mk];
fh_temp = 0;
bp_goal = [];
dist = [];
fai_o_all = [];
while Rt > 1
mk = mk + 1;
for j = 1:length(v_t_all)
v_t = v_t_all(j);
fai_t = fai_t_all(j);
X_t = X_t_all(j);
Y_t = Y_t_all(j);
gamma_all(j) = xd_data2(X_o,X_t,Y_o,Y_t,v_o,v_t,fai_o,fai_t);
end
[~, index] = sort(gamma_all,'descend');
beta_all = 0; %转向角度
for j = index
v_t = v_t_all(j);
fai_t = fai_t_all(j);
X_t = X_t_all(j);
Y_t = Y_t_all(j);
[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 gamma > 0.4
disp(['船',num2str(j),'有危险'])
if sum(bp_goal==j)==0
bp_goal = [bp_goal j];
end
%% 会遇局面判断 ?
%对遇
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
beta = 0;
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 = 2; %表示左转
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
beta_all = beta_all + beta;
end
end
if beta_all ~=0
disp('*****************避碰******************')
disp(['避碰转角点：',num2str([X_o,Y_o])])
disp(['避碰转角: ',num2str(beta_all),'度'])
mkk = [mkk; mk];
beta_all=0;
r = 1;
end
if r == 1
for j = bp_goal
C_fh = atan((Xt-X_o)/(Yt-Y_o));
v_t = v_t_all(j);
fai_t = fai_t_all(j);
X_t = X_t_all(j);
Y_t = Y_t_all(j);
[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
fh_temp = 1;
else
fh_temp = 0;
break
end
end
if fh_temp==1
fh_temp = 0;
mkk = [mkk; mk];
fai_o = C_fh;
r = 0;
bp_goal = [];
disp('*****************复航******************')
disp(['目的转角点: ',num2str([X_o,Y_o])])
disp(['目的角度为: ',num2str(C_fh*180/pi),'度'])
end
end
if r == 0 || r==2
Xo = [X_o, Y_o];
Xsum = [Xt,Yt;[X_t_all',Y_t_all']];
V_all = [0,v_t_all];
fai_all = [0, fai_t_all];
fai_o = main_rgsc(Xo,Xsum,V_all,fai_all,v_o, fai_o, set);
end
X_o = X_o + v_o * t * sin(fai_o);
Y_o = Y_o + v_o * t * cos(fai_o);
X_t_all = X_t_all + v_t_all .* t .* sin(fai_t_all);
Y_t_all = Y_t_all + v_t_all .* t .* cos(fai_t_all);
X = [X; X_o, Y_o];
Y = [Y; X_t_all, Y_t_all];
fai =[fai;fai_o];
am = [am; gamma];
Rt = sqrt((Xt-X_o)^2+(Yt-Y_o)^2);
R_tt = sqrt((X_t_all - ones(1,3)*X_o).^2 + (Y_t_all - ones(1,3)*Y_o).^2);
dist = [dist; R_tt];
fai_o_all = [fai_o_all, fai_o];
end
mkk = [mkk; mk];
ly = size(Y,2)/2;
figure(1)
plot(((2:mk)-1)*t*60, dist,'LineWidth',2)
[M,N]=min(dist);
xlabel('时间/min')
ylabel('距离')
legend('来船1','来船2','来船3')
str=['(',num2str(N(1)-1),',',num2str(M(1)),')'];
text(((N(1)-1)-1)*t*60,dist(N(1),1)-2,str);
str=['(',num2str(N(2)-1),',',num2str(M(2)),')'];
text(((N(2)-1)-1)*t*60,dist(N(2),2)-1,str);
str=['(',num2str(N(3)-1),',',num2str(M(3)),')'];
text(((N(3)-1)-1)*t*60,dist(N(3),3)-1,str);
figure(2)
plot(((2:mk)-1)*t*60, am,'LineWidth',2)
xlabel('时间/min')
ylabel('危险度')
figure(3)
plot(((2:mk)-1)*t*60, fai(2:end)/pi*180,'LineWidth',2)
xlabel('时间/min')
ylabel('本船航向')
fig = figure;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k=1;
figure(4)
axis([xx1 xx2 yy1 yy2])
hold on
color1=[0 0.4470 0.7410;0.9290 0.6940 0.1250;0.4940 0.1840 0.5560];
num2=floor(size(X,1)/10);
plot(X(1,1),X(1,2),'.r')
plot(Y(1,1),Y(1,1+ly),'LineWidth',2,'Color',color1(1,:))
plot(Y(1,2),Y(1,2+ly),'LineWidth',2,'Color',color1(2,:))
plot(Y(1,3),Y(1,3+ly),'LineWidth',2,'Color',color1(3,:))
drawnow
for num1=10:-1:2
for j = 1:size(Y,2)/2
pause(0.05)
plot(Y(num1*num2:-1:(num1-1)*num2,j),Y(num1*num2:-1:(num1-1)*num2,j+ly),'LineWidth',2,'Color',color1(j,:))
%%%%%%%%%%%%%%%%%%%%%此处读入图像数据，将其保存在 im ??
frame = getframe;
im{k} = frame2im(frame);
k=k+1;
plot(X((12-num1)*num2:-1:((12-num1)-1)*num2,1),X((12-num1)*num2:-1:((12-num1)-1)*num2,2),'.r')
%%%%%%%%%%%%%%%%%%%%%此处读入图像数据，将其保存在 im
frame = getframe;
im{k} = frame2im(frame);
k=k+1;
end
end
for j = 1:size(Y,2)/2
str=['(',num2str(Y(1,j)),',',num2str(Y(1,j+ly)),')'];
text(Y(1,j),Y(1,j+ly),str);
str=['(',num2str(Y(end,j)),',',num2str(Y(end,j+ly)),')'];
text(Y(end,j)-3,Y(end,j+ly)-1,str);
end
plot(X((num1-1)*num2:-1:1,1),X((num1-1)*num2:-1:1,2),'.r')
for j = 1:size(Y,2)/2
H=plot(Y((num1-1)*num2:-1:1,j),Y((num1-1)*num2:-1:1,j+ly),'LineWidth',2,'Color',color1(j,:))
%%%%%%%%%%%%%%%%%%%%%此处读入图像数据，将其保存在 im ??
frame = getframe;
im{k} = frame2im(frame);
k=k+1;
end
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;
for j = size(mkk):-1:1
i = mkk(j);
for kz = 1:ly
hold on
rectangle('Position',[Y(i,kz)-0.3,Y(i,kz+ly)-0.3,0.6,0.6],'Curvature',[1,1], 'FaceColor','k')
end
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 50 -2 50]);
legend('','来船1','来船2','来船3');
if j==size(mkk)
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(round(real(am(mkk(1:end-1))),2))];
text(X(mkk(1:end-1),1),X(mkk(1:end-1),2),cellstr(str))
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%此处将读入的图像写出 为 GIF 详细参考 imwrite 函数
filename = '3.gif'; % Specify the output file name
for idx = 1:size(im,2)
[A,map] = rgb2ind(im{idx},256);
if idx == 1
imwrite(A,map,filename,'gif','LoopCount',Inf,'DelayTime',0.05);
else
imwrite(A,map,filename,'gif','WriteMode','append','DelayTime',0.05);
end
end
⛄ 运行结果
⛄ 参考文献
[1] 钟碧良, 刘先杰, 吴建生,等. 基于人工势场法的船舶自动避碰系统研究[J]. 广州航海学院学报, 2008, 16(1):8-11.
[2] 宁君, 李学健, 李伟,等. 基于改进人工势场法的船舶自动避碰研究[J]. 舰船科学技术, 2021, 43(12):7.
[3] 钟丰仰, 朱俊翰, 廖启闵,等. 基于人工势场法的船舶避碰方法及系统:, CN113495556A[P]. 2021.
