避碰决策基于matlab多无人船舶避碰决策含Matlab源码 2348期

Posted 2023-02-22 海神之光

⛄一、多无人船舶避碰决策

1 无人驾驶船舶智能避碰决策面临的问题
相比于车辆在陆地道路上的行驶, 无人驾驶船舶航行的海洋环境更复杂、多变, 在避碰智能决策方面面临如下2个难点。

1. 海洋环境复杂、多变。
   首先, 在海洋环境下, 风、流、涌、浪等的时变性较强, 极大地影响了船舶航行安全。其次, 近海水域有较强的结构化通航特征, 分道通航种类较多且助航信息量大, 例如, 灯浮、灯标、航道建筑物、航行信号灯和不守规则的小型渔船等外部环境因素。因此, 无人驾驶船舶避碰智能决策的设计需要考虑多源信息的约束, 并能从中提取有效信息。

2. 无人驾驶船舶的终极目标是最大限度地实现“拟人化”。
   在有人驾驶船舶的操纵过程中, 驾驶员会将人、船、航线视为一个整体, 在海上交通规则、驾驶经验及意图的指导下进行反应式驾驶, 通过值函数求解决策模型中的最优策略, 使无人驾驶船舶状态对行为映射中的回报最大;在避碰智能决策设计上, 无人驾驶船舶需要借鉴有人驾驶船舶的操作人员处理复杂交通场景的决策过程, 合理学习其驾驶经验及规则中的模糊定义, 在操作过程中智能化地实现“拟人化”决策。

2 智能决策设计准则
针对无人驾驶船舶智能决策所面临的问题, 结合海上交通规则, 智能决策应满足以下准则:

1. 自主规划能力。
   针对识别的目标和障碍物, 在现有电子海图上应能合理规划最优避碰路线、避让时间和恢复原始航线的时间。

2. 实时性。
   因海洋环境的不可预测性, 智能决策系统必须能实时针对环境变化调整航态。

3. 航向稳定性和航向保持。
   除执行转弯、追越、避障等动作外, 应能保持航向, 不能有较大的偏航。若受到扰动而偏航, 应在扰动消失后自行恢复航线航行。

4. 航速控制。
   在正常航态下, 一般应在最大和最小航速限制范围内航行。在智能决策过程中, 通常应能纵向调整航速以避障;在特定海域内, 可根据当地规则控制航速;在紧急情况或意外发生时, 可实现紧急制动。

5. 避碰操作。
   在航行中, 应具有避碰能力, 所采取的行动包括横向转向和纵向加减速、停船等动作。

⛄二、部分源代码

clear,clc,close all %清除工作区，命令行窗口，关闭所有图窗
pause(0.01) %暂停执行0.01秒，然后继续执行
figure(1) %创建一个图窗
hold on
xlim([-1000 1700]); %x的范围从-1000到1700
ylim([-600 1100]); %y的范围从-600到1100
hold on
xlabel(‘东’); %给X轴定义标签
ylabel(‘北’); %给Y轴定义标签
grid on %打开坐标区域网格线

%定义船舶形状，（这里后面绘制船的时候需要用到）
x_shape = [0,0.3,0.7,1,0.5,0,-0.5,-1,-0.7,-0.3,0]; %通过11个坐标点构成闭环图形
y_shape = [6,4.6,2,0,-1.2,-2,-1.2,0,2,4.6,6];
scale = 12; %这里应该是在定义船舶形状时，1就代表12
[m_shape, n_shape] = size(x_shape); %返回x_shape的行数1和列数11

%定义船舶起始点
startpoint_x = 0;
%startpoint_x = 4;
startpoint_y = -500;

%定义船舶终点
destination_x = 0;
%destination_x = 1000;
%destination_x = 500;
destination_y = 1000;
%绘制起点和终点
plot(startpoint_x, startpoint_y, ‘o’, ‘MarkerFaceColor’,‘g’) %绘制起点，用圆表示，颜色为绿色
hold on
plot(destination_x, destination_y, ‘o’, ‘MarkerFaceColor’,‘g’)
hold on
text(startpoint_x+20, startpoint_y,‘起点’,‘Fontsize’,10 ); %在起点右边插入文档，字体大小为10
hold on
text(destination_x+20, destination_y,‘终点’,‘Fontsize’,10 );
hold on

%定义USV（无人水面艇unmanned surface vessel）简称USV
x_usv = startpoint_x;
y_usv = startpoint_y; %位置
v_usv = 4; %速度
h_usv = pi/2; %航向向北
%h_usv = 0; %航向(正东为0，逆时针)
delta_t = 1; %时间改变量

%定义其他船舶
n_intruder = 6; %他船数量

%第一艘障碍船
x_intruder(1) = 1000;
y_intruder(1) = 300; %他船1位置
v_intruder(1) = 5; %他船1船速
h_intruder(1) = pi; %方向向西，航向270

%第二艘障碍船
x_intruder(2) = -20;
y_intruder(2) = 450; %他船2位置
v_intruder(2) = 2; %他船2船速
h_intruder(2) = 3*pi/2; %方向向南，航向180

%第三艘障碍船
x_intruder(3) = 1600;
y_intruder(3) = 700; %他船3位置
v_intruder(3) = 5; %他船3船速
h_intruder(3) = pi; %航向270

%第四艘障碍船
x_intruder(4) = 1200;
y_intruder(4) = 300; %他船4位置
v_intruder(4) = 5; %他船4船速
h_intruder(4) = pi; %航向270

%第五艘障碍船
x_intruder(5) = 1000;
y_intruder(5) = 0; %他船5位置
v_intruder(5) = 2; %他船5船速
h_intruder(5) = pi; %航向270

%第六艘障碍船
x_intruder(6) = -800;
y_intruder(6) = -200; %他船6位置
v_intruder(6) = 2; %他船6船速
h_intruder(6) = 0; %航向为90

%定义安全参数(safety raius cpa, TCPA）安全会遇时间
cpa = 100; %应该是他船的船舶领域
tcpa = 60; %到达最近会遇距离位置点的最近会遇时间

%Define the proportionality constant of PN guidance method(定义PN制导方法的比例常数)
N1 = 10; %这里需要搞明白pn制导的意思

%定义其他的参数
tmatrix = zeros(2); %生成两行两列的0矩阵

%对相关的参数进行数值变量自动存储为64位双精度浮点值。
double h_usv
double print_h_usv
double tangent_course_sight %切线视线
double delta_tangent_course_sight
double course_sight_rate %航向转角速度
%relative_v_usv_tangent = vpa(4)

%pre_h_usv = 0;
pre_h_usv = pi/2; %代表本船预设的航行方向
previous_tangent_course_angle = pi/3; %代表初始设定的切线视角
current_avoiding_intruder = 0; %初始时本船避碰的船舶序号为0
distance = 100; %初始时据本船的距离为100
intruder_index = 0; %他船序号索引

for time = 1:1000
x_usv = x_usv + v_usv*cos(h_usv)delta_t;
%x_usv = x_usv + v_usvsin(h_usv)delta_t;
y_usv = y_usv + v_usvsin(h_usv)delta_t; %下一时刻的USV船位计算
%y_usv = y_usv + v_usvcos(h_usv)delta_t;
%这里的公式有问题，正常x_usv = x_usv+v_usvcos(h_usv)*delta_t，公式里的h_usv角度应该是和x轴的夹角
%计算他船在下一时刻的位置
for i = 1:n_intruder
x_intruder(i) = x_intruder(i) + v_intruder(i)*cos(h_intruder(i))delta_t;
y_intruder(i) = y_intruder(i) + v_intruder(i)sin(h_intruder(i))delta_t;
end
h_usv_origional = atan2(destination_y - y_usv, destination_x -x_usv); %计算USV在t时刻的位置和终点位置的连线与x轴正半轴的夹角
%如果此时求出来的角度为负值，说明此时本船位置和终点位置的连线在x轴正半轴的下方，此时角为负数，按照设定，应加上2pi变正数
if h_usv_origional < 0
h_usv_origional = h_usv_origional + 2pi;
end
%检查碰撞情况
%check_collision的状态标志输出将只等于0,1,2,3
% status_flag = 0, 2tcpa时间内无碰撞
% status_flag = 1, 警告，碰撞时间2tcpa内
% status_flag = 2, 碰撞在tcpa时间内(不需要转弯)
% status_flag = 3, This will be added in check colregs part!!!
% c = 0,（决定躲开）不存在碰撞危险
% c = 1,（不躲）存在碰撞危险
for j = 1:n_intruder
status_flag(j) = 0; %初始化为0
c(j) = 0; %初始化为0
%下面的函数是检测本船与他船有无碰撞的，输入有：本船的位置、本船船速，本船航向、他船位置、他船船速、他船航向、船舶领域和最近会遇时间
[c(j), status_flag(j)] = check_collision(x_usv, y_usv, v_usv, h_usv_origional, x_intruder(j), y_intruder(j), v_intruder(j), h_intruder(j), cpa, tcpa);
end

⛄三、运行结果

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]王程博,张新宇,张加伟,刘硕.未知环境中无人驾驶船舶智能避碰决策方法[J].中国舰船研究. 2018,13(06)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除