基于漂角估计的无人船局部动态避障方法

刘淑霞， 李立刚， 金久才， 戴永寿

(1.中国石油大学(华东)，山东 青岛 266000； 2.自然资源部第一海洋研究所，山东 青岛 266000)

0 引言

无人船(Unmanned Surface Vhicles,USV)因无需人员操作，可以在海洋中承担长时间、大范围、低成本的海洋科研与工程任务，而具有广阔的应用前景[1]。在执行任务时，无人船不仅要跟踪预定航线，而且要具备局部动态避障能力，同时还要考虑实时性约束、无人船运动学约束、国际海上避碰规则(COLREGS)约束和风浪流的干扰[2]。

动态窗口法(DWA)因考虑无人船运动学约束，被广泛应用于无人船的局部动态避障[3]。王宁佳[4]提出将速度障碍法和动态窗口法相结合，通过划定绝对避障区和在评价函数中引入目标区域影响这一指标，提高了避障的安全性;周晓周[5]提出在动态窗口法的目标函数中添加最小避碰时间、最短偏离距离、复航安全距离3个指标，用以确定最优航向和避碰时间;SINGH等[6]在动态窗口法的目标函数中加入了考虑海况的权重系数，当海况发生变化时，通过增加距离权重和降低速度权重来保证无人船的安全。

但上述文献都忽略了动态障碍物状态的变化和风浪流干扰给无人船避障带来的具体影响。因此，本文提出基于漂角估计和补偿的无人船局部动态避障方法。根据预测的障碍物运动状态和国际海上避碰规则改进了动态窗口法，并提出利用横向位置误差估计并补偿漂角，使该无人船局部动态避障方法更加适用于海洋扰动环境，更加具有实用性。

1 传统动态窗口法及其基本原理

传统动态窗口法的基本原理为：在速度空间中，采样多组速度，并在前向周期内模拟无人船在这些速度组合下的轨迹，按照一定评价标准选取最优轨迹对应的速度和航向驱动无人船避障[7]。动态窗口法的基本流程如图1所示。

图1 传统动态窗口法流程图Fig.1 Flow chart of traditional DWA

首先，因受到加减速能力限制无人船能够达到的速度范围为

(1)

其次，由于受到最大、最小速度限制，无人船能够达到的速度范围为

Vs={v∈[vmin,vmax]∧ω∈[ωmin,ωmax]}

(2)

式中:vmin和vmax分别为最小和最大速度;ωmin和ωmax分别为最小和最大角速度。

最后，无人船也受到制动距离约束，以最大减速度停船而不能与障碍物发生碰撞。制动距离约束下,无人船能够达到的速度范围为

(3)

式中，d(v,ω)为无人船与障碍物的最近距离。

无人船的速度空间为

Vr=Vd∩Vs∩Va。

(4)

对上述速度空间进行采样后，预测不同航向和航速组合下的轨迹，并按照预测轨迹的航向与目标点航向的夹角、预测轨迹对应的速度和预测轨迹与障碍物的最短距离3个指标评价出最优轨迹，选择最优轨迹对应的航向和航速驱动无人船避障，评价函数为

G(v,ω)=k1·h(v,ω)+k2·d(v,ω)+k3·V(v,ω)

(5)

式中:h(v,ω)为预测轨迹的航向与目标点航向的夹角;d(v,ω)为预测轨迹与障碍物的最短距离;V(v,ω)为预测轨迹对应的速度;k1，k2和k3分别为3个指标的权重系数。

2 基于漂角估计和补偿的局部动态避障方法设计

首先，采用改进的动态窗口法规划无人船的速度和航向;其次，利用横向位置误差估计并补偿漂角。局部动态避障方法的流程如图2所示。

图2 局部动态避障方法流程图Fig.2 Flow chart of local dynamic obstacle avoidance method

2.1 改进动态窗口法

考虑国际海上避碰规则和障碍物状态变化对传统动态窗口法进行了改进，提出根据COLREGS计算速度空间和根据预测的障碍物状态选择无碰预测轨迹，提高了无人船局部动态避障的规范性和安全性。

2.1.1 根据COLREGS计算速度空间

根据COLREGS无人船的会遇被划分为追越、对遇、交叉相遇3种情况[8]，如图3所示。但无人船只需在对遇、追越和右侧交叉相遇情况下采取避碰行动[9]，如图4所示。

图3 会遇情况示意图Fig.3 Diagram of the encounter situations

图4 避碰行动示意图Fig.4 Diagram of collision avoidance operations

因此，在上述应采取避碰行动的情况下，无人船需向其他船只右侧行驶，则无人船的速度v和航向角α范围为

Vg={v∈[vrmin,vrmax]∧α∈
[ωrmin·Δt,ωrmax·Δt]∧α∈(ψ,∞)}

(6)

式中:vrmin,vrmax，ωrmin和ωrmax分别为传统动态窗口法速度空间中的最小、最大速度和最小、最大角速度;ψ为其他船只的方位角，即

(7)

式中，(xv,yv)和(xu,yu)分别为其他船只和无人船的位置。

如果下一时刻其他船只未遵守COLREGS，则按照传统方法选取动态窗口法的速度空间。

综上，上述方法有效避免了无人船过度自由地航行，进而降低了无人船与其他船只的碰撞风险。

2.1.2 根据预测的障碍物状态选择无碰预测轨迹

传统动态窗口法根据障碍物在t时刻的横截面宽度和位置,规划t～t+1时刻内无人船的速度和航向，忽略了两者在t～t+1时刻内的变化，可能导致避障失败，无人船与障碍物的碰撞情况如图5所示。

图5 无人船与障碍物碰撞示意图Fig.5 Diagram of collision between USV and obstacle

因此，有必要预测障碍物在t～t+1时刻的运动状态，并作为规划航向和航速的依据，以减少碰撞情况的发生，障碍物的横截面宽度和位置预测如图6所示。

图6 预测障碍物运动状态示意图Fig.6 Schematic diagram of predicting state of obstacle

因为间隔时间较短，横截面宽度的变化量不大，所以可以通过计算其变化率预测下一时刻的横截面宽度值。

t-1～t时刻障碍物横截面宽度的变化率为

(8)

式中，dt-1和dt分别表示t-1和t时刻障碍物的横截面宽度。

t+1时刻横截面宽度的预测值为

(9)

t+1时刻障碍物的影响范围是以该时刻横截面宽度为直径的圆。式中:dt和dt+1分别表示t和t+1时刻障碍物的横截面宽度。

通过预测下一时刻的位置，可以预测出障碍物在一段时间内的运动轨迹。

假定t～t+1时刻内障碍物的速度和航向保持不变，则t+1时刻障碍物的位置为

(10)

障碍物在t～t+1时刻内的运动轨迹所在的直线为

(11)

根据障碍物的影响范围和运动轨迹可计算出无人船预测轨迹到障碍物的最短距离。

(12)

式中:Q点为预测轨迹末端向障碍物运动轨迹所在线段作垂线的交点;rQ为障碍物在Q点时的影响范围半径，即

(13)

式中，tQ为障碍物运动到Q点(xQ,yQ)的时刻，即

(14)

(15)

式中，rt和rt+1分别为t和t+1时刻障碍物的影响范围半径。

根据预测轨迹末端到障碍物影响范围的最短距离l,将速度空间中与障碍物碰撞的预测轨迹删除，则无人船受减速距离限制能达到的速度范围为

(16)

通过上述方法为无人船规划出了更加安全的航向、航速进行避障。

2.2 利用横向位置误差估计漂角

风浪流干扰使无人船偏离实际航向，不利于避障的安全和航行的稳定。因此，本文将风浪流干扰对无人船的影响等效为漂角[10]，提出利用横向位置误差对其进行估计和补偿。风浪流干扰对无人船的影响如图7所示。

图7 风浪流干扰对无人船的影响原理图Fig.7 Diagram of impact of wind,wave and current on USV

(17)

式中:αt为t时刻无人船的期望航向角；v和ω分别为无人船的纵向速度和角速度；α为无人船在t～t+1时刻内的期望航向角。

在大地坐标系中，假设无人船当前实际位置为(xu,yu)，由图7的几何关系可得横向位置误差ye为

(18)

将式(18)对时间t求导，得

(19)

(20)

无人船三自由度运动学模型[11]为

(21)

式中:x，y和φ分别为在大地坐标系下的横、纵坐标和航向角；μ为在船体坐标系下的横向速度。

(22)

根据三角函数转换式

(23)

以及μ,v,β和U之间的几何关系，可将式(22)重写为

(24)

由三角函数转换式

sin(a+b)=sina·cosb+cosa·sinb

(25)

以及μ,v,β和U之间的几何关系，将式(24)重写为

(26)

当无人船控制系统能够准确跟踪期望航向时，期望航向角和实际航向角保持一致，即φ=α，则可由式(26)得漂角的大小为

(27)

当无人船的实际航向角与期望航向角存在误差时，即φ≠α，由式(26)得漂角的大小为

(28)

通过无人船的横向误差，已经估计出了无人船由于风浪流干扰产生的漂角。利用估计出的漂角可实现对动态窗口法规划出的期望航向角α的补偿，补偿后无人船航向角为

φ′=α+β

。

(29)

综上，本节所提方法能够使无人船抵抗风浪流干扰且稳定地航行。

3 仿真验证及结果分析

为了验证所提方法的有效性和优越性，利用Matlab进行仿真实验。根据路径跟踪控制实验船实验数据，计算得无人船的运动参数设置如表1所示。

表1 无人船运动参数

3.1 改进动态窗口法局部动态避障仿真实验

为了验证改进动态窗口法的有效性和优越性，分别采用传统动态窗口法和改进动态窗口法进行无人船局部动态避障仿真实验。无人船与障碍物初始参数设置如表2所示。

表2 无人船与障碍物初始参数

评价函数中的参数设置为：k1=0.2，k2=0.2，k3=0.1。基于上述仿真条件，实验结果如图8和图9所示。

对比图8(a)～8(c)可以看出，本文所提方法能够使无人船在右交叉相遇、对遇、追越3种情况下向右行驶，提高了无人船局部动态避障的规范性。

图8 增加COLREGS约束前后的无人船避障轨迹Fig.8 Obstacle avoidance trajectory of USV before and after adding the restrictions of COLREGS

图9(a)～9(c)为预测障碍物运动状态前后无人船分别在20 s，40 s和70 s的避障轨迹，可以看出，无人船与障碍物的安全距离明显增加。由图9(d)可以看出，改进后无人船与障碍物始终保持安全距离。

图9 预测障碍物运动状态前后无人船避障轨迹和无人船与障碍物的距离Fig.9 Obstacle avoidance trajectory of USV before and after predicting the obstacle movement state,and the distance between USV and obstacle

3.2 进行漂角估计和补偿的局部动态避障仿真实验

为验证漂角估计和补偿对于抵抗风浪流干扰效果，进行海洋环境扰动下的无人船局部动态避障仿真实验。为模拟海洋环境，设置随机漂角，如图10所示。

图10 漂角的大小Fig.10 Size of drift angle

图11所示为漂角估计和补偿对于无人船局部动态避障稳定性的有效性验证结果。图11(a)～11(c)为有风浪流和无风浪流干扰下无人船分别在20 s，40 s和70 s的避障轨迹。可以看出，受到风浪流干扰时，无人船的实际航向与规划的航向相比，发生了较大偏移。

图11 风浪流干扰下无人船的避障轨迹Fig.11 Obstacle avoidance trajectory under wind,wave and current disturbances

图12为进行漂角补偿后无人船分别在20 s,40 s和70 s的避障轨迹。可以发现，漂角补偿后，无人船的航向不再发生偏移，无人船能够抵抗风浪流干扰稳定地航行。

图12 漂角补偿后无人船的避障轨迹Fig.12 Obstacle avoidance trajectory after drift angle compensation

图13为漂角的估计值和实际值的对比。可以看出，本文提出的方法可对漂角进行有效的估计。图14对比了漂角补偿前后无人船与障碍物影响范围的距离。可以发现，漂角补偿后无人船与障碍物影响范围的距离均大于1.679 m，保证了无人船的航行安全。

图13 漂角的估计值和实际值Fig.13 Estimated and actual drift angle

图14 漂角补偿前后无人船与障碍物的距离Fig.14 Distance between USV and obstacle before and after drift angle compensation

4 结束语

本文对无人船局部动态避障方法进行了深入研究，提出了基于改进动态窗口法结合漂角估计的方法。考虑COLREGS选取速度空间，并根据预测障碍物的运动轨迹和影响范围选择无人船的无碰预测轨迹，保证了无人船避障行动的规范性，进一步提高了安全性。利用无人船实际位置与期望避障路径的横向误差估计无人船由于风浪流干扰产生的漂角，并在规划的航向中进行了补偿，提高了无人船局部动态避障的稳定性。通过仿真实验，验证了本文所提方法的有效性和优越性。