基于改进动态窗口法的无人水面艇自主避碰算法

刘渐道 刘文 张英俊 李元奎

摘要：为研究避碰规则、无人水面艇（unmanned surface vessel， USV）运动学特点和海上交通复杂度等因素约束下的USV自主避碰技术，在分析初始动态窗口法的基础上，考虑《国际海上避碰规则》（International Regulations for Preventing Collisions at Sea， COLREGs）关于避碰行动时机、避让幅度、复航时机等方面的要求，建立融合避碰规则的动态窗口模型，设计融合避碰规则的动态窗口法。通过对比仿真实验验证该方法的可行性和有效性，具有一定的现实意义。

关键词： 无人水面艇（USV）; 动态窗口法; COLREGs; 自主避碰

中图分类号： U675.96;TP273+.5

文献标志码： A

Automatic collision avoidance algorithm of unmanned surface

vessels based on improved dynamic window approach

LIU Jiandao1， LIU Wen2， ZHANG Yingjun1， LI Yuankui1

（1.Navigation College， Dalian Maritime University， Dalian 116026， Liaoning， China;

2.National Engineering Laboratory for Transportation Safety & Emergency Informatics，

China Transport Telecommunications & Information Center， Beijing 100011， China）

Abstract：

In order to study the automatic collision avoidance technology of unmanned surface vessels （USVs）， considering the

constraints of collision avoidance rules，

USV kinematic characteristics and the complexity of maritime traffic， the dynamic window model subject to collision avoidance rules is established based on the analysis of the original dynamic window approach. In the model， the requirements of

International Regulations for Preventing Collisions at Sea （COLREGs）， such as the time of collision avoidance action， the range of collision avoidance action and the time to go back to the channel， are considered. The dynamic window algorithm subject to collision avoidance rules is designed. The feasibility and effectiveness of the method are verified by comparative simulation experiments， which shows that the method is of certain practical significance.

Key words：

unmanned surface vessel （USV）; dynamic window approach; COLREGs; automatic collision avoidance

收稿日期： 2020-05-11

修回日期： 2020-07-17

基金項目：

国家自然科学基金（51679025，51709032）;中央高校基本科研业务费专项资金（3132019313）; 大连海事大学教改项目（2020Y6）

作者简介：

刘渐道（1983—），男，河北唐山人，讲师，博士研究生，研究方向为船舶智能航行，（E-mail）liujiandao@dlmu.edu.cn;

刘文（1987—），男，吉林扶余人，高级工程师，博士，研究方向为水上智能运输系统，（E-mail）liuwen@cttic.cn;

张英俊（1965—），男，山东聊城人，教授，博导，博士，研究方向为智能运输理论与系统，（E-mail）zhangyj@dlmu.edu.cn

0 引 言

无人水面艇（unmanned surface vessel， USV）作为一种轻量化智能水上运载工具，可应用于军事作战、海事监管和海洋环境检测等领域，受到越来越多的关注。它集速度快、体积小、机动性强等特点于一身，可以快速、高效、准确地完成任务。在复杂的海洋环境中，存在很多威胁USV航行安全的障碍物，因此，自主避碰技术是USV的核心技术之一。自主避碰技术是平面机器人路径规划领域中的重要部分，包括全局路径规划技术和局部避碰技术。具体地，全局路径规划技术是在环境信息已知的情况下，根据任务目标（如时间最短或距离最短）给机器人规划出一条可行路径;局部避碰技术指机器人在行进过程中遇到动态障碍物时采取正确合理的规避行动，及时有效地调整速度或方向，尽可能快速安全航行。

目前，许多国内外学者对船舶避碰路径规划问题做了相关研究，所采用的研究方法包括遗传算法、Dijkstra算法、A*算法、蚁群算法、势场法、速度障碍法、向量场直方图法等[1]。倪生科等[2]针对不同会遇态势下的船舶避碰路径规划问题，提出一种混合遗传算法，使路径规划性能和效率都得到了提高;LAZAROWSKA[3-4]和TSOU等[5]利用蚁群算法研究了多船会遇局面下的避碰路径规划;LYU等[6]改进了传统的人工势场法，用来避让动、静态障碍物，并研究了目标船采取不协调避碰行动时本船应采取的行动;PERERA等[7]根据避碰规则和专家知识，采用贝叶斯网络、并行决策等多种方法，提出基于模糊逻辑的避碰决策系统;陈超等[8]提出基于可视图的A*算法来解决全局路径规划问题;庄佳园等[9]提出一种基于电子海图的距离寻优Dijkstra算法，为解决USV全局路径规划问题提供了一种方法;KUWATA等[10]提出基于速度障碍法的USV避碰方法，并结合《国际海上避碰规则》（International Regulations for Preventing Collisions at Sea， COLREGs）制定USV避碰行动方案;吴博等[11]研究了基于速度障碍法并考虑风、流影响的USV自主避碰算法;卢艳爽[12]考虑海事规则，设计了一种基于速度障碍法的局部避碰算法;BENJAMIN等[13]在满足部分航行约束条件下，利用多目标优化方法解决无人艇避碰问题;LOE[14]将动态窗口法用于USV的自主避碰，但未考虑避碰规则等因素对避碰的影响;唐平鹏等[15]利用动态窗口法求解USV避碰角速度和线速度，以USV的偏航角度和速度为优化目标，以最小回转半径和障碍物动态为约束条件，通过快速搜索USV的动态窗口法获取USV避碰方案。

综合分析上述研究成果发现，研究人员关注USV自主避碰方法的理论研究，虽然通过仿真实验验证了一些方法的可行性，但还存在以下问题：有的研究避让场景较简单;部分研究虽然实现了动、静态障碍物的安全避让，但是没有或较少考虑避碰规则;有的研究忽略了船舶运动性能对避让方案的影响。根据目前的研究状况以及未来的发展趋势，充分考虑避碰规则、USV运动学特点和海上交通复杂度等因素是USV自主避碰技术的发展方向。因此，本文选取与USV运动学性能结合较好的动态窗口法，并充分考虑避碰规则和动、静态障碍物等因素的影响进行研究，使其更符合USV的实际避碰情况。

1 动态窗口法

1.1 动态窗口法

基于动态窗口法的避碰方法首次由FOX等[16]于1997年提出，被用于机器人的局部避障。将该方法应用到USV自主避碰领域时要考虑USV的运动学特点，利用USV在给定时间窗内能达到的速度和方向进行避碰计算。

动态窗口法假定在给定的时间Δt内，USV以恒定的纵向速度u和角速度r航行，因此USV的运动轨迹是一条直线或定半径的圆弧。当r≠0时，在第i个时间段Δt内，USV运动轨迹——圆弧的圆心（ox，i，oy，i）和半径Rr，i可以表示为

ox，i=x-uirisin φ，

oy，i=y+uiricos φ，

Rr，i=uiri （1）

式中：ui和ri为USV在第i个时间段Δt内的纵向速度和角速度;（x，y）为USV的位置;φ为航向。

通过式（2）～（4）可确定USV在给定时间窗Δt内可以达到的速度矢量（u，r）的集合：

USV在给定时间窗Δt内能达到的纵向速度u和角速度r分别为

u∈[uc-u·Δt，uc+u·Δt]

r∈[rc-r·Δt，rc+r·Δt] （2）

其约束为

u∈[umin，umax]，

r∈[rmin，rmax]（3）

式中：uc和rc分别为USV当前的纵向速度和角速度。

为保证USV安全，在最大减速条件下，USV速度应能在撞击障碍物之前减为0，则

u≤（2d（u，r）u·）1/2

r≤（2d（u，r）r·）1/2 （4）

式中，d（u，r）为（u，r）对应轨迹与最近障碍物的距离。

假设USV在时刻t的速度矢量为（ut，rt），在确定USV在时刻t+1相对于时刻t的位移和航向时，由于时间间隔很短，可以将两点之间圆弧轨迹看作直线轨迹，则位置和航向的更新公式为

xt+1=xt+utΔtcos φt

yt+1=yt+utΔtsin φt

φt+1=φt+rtΔt （5）

1.2 评价函数

为便于工程计算，将速度矢量（u，r）离散化，每个（u，r）对应一条轨迹，利用评价函数确定最优轨迹。具体方法是将各速度矢量代入评价函数中计算其对应的评价函数值，取最大值对应的速度矢量作为USV的速度矢量，并采用该速度矢量航行，更新USV位置，直至到达目的地。评价函数设计的原则为：USV能够安全避开障碍物，尽快向目的地前进。评价函数为

Gdw（u，r）=αd（u，r）+βφhead（u，r）+

γvspeed（u，r） （6）

式中：距离函数d（u，r）的值越大，说明当前轨迹距离障碍物越远;用航向函数φhead（u，r）衡量预估轨迹终点航向与目的地方向的对准程度，φhead（u，r）=

180°-θ（其中θ為相对于USV航向的目的地的方位角，如图1所示），φhead（u，r）值越大，表示对准度越高;用速度函数vspeed（u，r）评估当前轨迹上USV的速度。评价函数的3个组成部分都是必需的，缺一不可。α、β和γ分别为距离函数、航向函数和速度函数对应的权重因数。在设置权重因数时，如果仅最大化φhead（u，r）函数，则USV向目的地航行，很有可能与障碍物发生碰撞;如果最大化d（u，r）和vspeed（u，r）函数，虽然能够避开障碍物，但是USV不能向目的地运动，产生绕远现象。因此，应设置合理的权重因数，使目标函数值最大，使得USV不仅能避开障碍物，而且能

向目的地以较快速度航行。

评价函数的3个组成部分的量纲不同，在计算前要将每个部分做归一化处理：

z′=z-zminzmax-zmin  （7）

式中：z∈{d（u，r），φhead（u，r），vspeed（u，r）};zmin和zmax分别为z的最小值和最大值。

2 考虑避碰规则的动态窗口模型

2.1 会遇局面划分

当USV与他船形成不同会遇局面时，应采取符合避碰规则的行动。将USV与目标船互见中构成碰撞危险的局面分成3类，见图2。

（1）对遇。当USV与目标船在相反或接近相反的航向上航行且有碰撞危险时，即航向角度差Δφ满足180-Δφ<2.5°时，USV应向右转向，从目标船的左舷通过。

（2）交叉。当USV与目标船的航向交叉且有碰撞危险时，如果航向角度差Δφ满足67.5°≤Δφ≤177.5°，则构成左舷交叉，此时USV为直航船，目标船为让路船;如果航向角度差Δφ满足182.5°≤Δφ≤292.5°，则构成右舷交叉，此时目标船为直航船，USV为让路船。

（3）追越。当USV与目标船的航向角度差Δφ满足Δφ<67.5°时，USV应向左转向，从目标船的左舷通过。

2.2 融合避碰规则的动态窗口模型

在避让船舶时，USV应遵守避碰规则，这已成为研究USV智能避碰的共识。由于初始动态窗口法没有考虑USV对避碰规则的遵守和适用问题，所以对动态窗口法进行改进，让其选择的速度矢量及形成的避碰路径符合避碰规则要求。COLREGs第16条规定，须给他船让路的船，应尽可能及早地采取大幅度的行动，宽裕地让清他船。采取避碰行动的时机、避让幅度、何时复航以及安全会遇距离是让路船采取避碰行动的4个关键要素。因此，建立避碰时机模型、避让幅度模型和复航时机模型，并将其融入动态窗口法中，是建立融合避碰规则的动态窗口模型的关键。

当USV与目标船会遇时，通过计算最近会遇距离dCPA和到达最近会遇点的时间tCPA确定是否有碰撞危险。确定避碰时机模型的方法包括基于船舶领域的方法、基于船舶碰撞危险度的方法，以及采用时间或距离来确定的方法等。本文采用时间或距离确定避碰时机。建立的避碰时机模型为dRNG=vrtCPA，其中vr为他船相对于USV的速度。当USV与目标船有碰撞危险，且二者之间的距离接近dRNG时，USV应采取避碰行动。该模型能够反映不同会遇局面下采取避碰行动的时机，例如：在对遇局面下，两船间的相对速度较大，采取避让行动的距离应该较远;在追越局面下，由于两船间的相对速度较小，让路船采取避让行动的距离比对遇局面的近一些，符合航海实际。实际情况下可以根据需要设置USV开始采取避让行动的距离dRNG，例如：在对遇局面下，当vr=10 m/s时，若取tCPA=1 min，则有dRNG=600 m，由于USV的机动性较好，该距离能保证USV有足够的时间采取避让行动。

COLREGs对避让幅度的要求为：应大得足以使他船用视觉或雷达观测时容易察觉到，避免对航向、航速做一连串小的变动，并且满足两船在安全会遇距离dSPD上驶过。转向幅度的确定如图3所示：USV位于O点，Y轴为USV船首方向，X轴为USV正横方向，目标船位于M点，

vo和vt分别为USV和目标船的速度。USV在改向前与目标船的最近会遇距离dCPA小于安全会遇距离dSPD，有碰撞危险。在点A3处，USV采取避让行动，向右转向ΔC，以dSPD通过目标船。USV改向前后目标船的相对运动线分别为l1和l2，l2的平行线为l3。通过平面几何计算，可以确定转向幅度ΔC。与动态窗口模型相结合，转向幅度ΔC=rΔt，其中Δt为动态窗口预测时间。因此，要选择使USV达到一定的转向幅度的合适的角速度。例如，当ΔC=30°和Δt=5 s时，需要USV的角速度为r=0.11 rad/s。

除对转向幅度有要求外，在不同会遇局面下，应选择符合避碰规则要求及海员通常做法的转向方向。例如，当USV追越他船时，如当时环境许可，尽可能从他船左舷追越。因此，在追越局面下，应选择控制USV向左转向的速度矢量。

确定复航时机是USV完成避碰行动的重要步骤。不同会遇局面下复航时机的确定方法不同，本文确定复航时机的原则是：在两船之间无碰撞危险时，USV开始复航，向设定的目的地运动。例如，对于对遇局面，当他船位于USV正横位置时，USV复航，之后尽快向目的地运动。建立的复航时机模型为Rθ=π/2，其中Rθ为他船相对于USV的舷角。

在动态窗口目标函数Gdw（u，r）的基础上，添加规则函数gcolreg（u，r）及其权重因数η。因此，融合了避碰规则的动态窗口法的目标函数为

G（u，r）=αd（u，r）+βφhead（u，r）+

γvspeed（u，r）+ηgcolreg（u，r）  （8）

其中，gcolreg（u，r）用来计算USV采取避让行动的距离以及确定转向幅度和复航时机。在开始采取避让行动及确定避让幅度后，评估每个矢量符合避碰规则的程度。ri的符合度δi的确定方法如下：

δi=ri/r*，ri≤r*

1-（ri-r*）/r*，ri>r*（9）

式中：r*為安全避让所需的角速度。

综上，利用式（8）计算各速度矢量对应的评价函数值，取最大值对应的速度矢量航行。该模型不但能够保障USV安全避让障碍物，而且充分考虑了避碰规则约束。

3 避碰算法设计

用本文设计的基于动态窗口法的避碰算法计算USV在给定时间窗内能达到的速度矢量，并根据避碰规则约束，选择符合避碰规则要求的速度矢量进行避碰，过程包括碰撞危险判断、会遇局面划分、选择最佳速度矢量等。基于动态窗口法的USV自主避碰算法流程见图4。

（1）初始化环境信息，主要包括USV的起点和目的地信息以及动、静态障碍物信息。动态障碍物信息主要包括目标船纵向航速、航向、当前位置等，静态障碍物信息主要指其位置和形状信息;本算法假设动、静态障碍物的信息已知;实际海上航行时这些信息可以通过AIS和雷達获取。

（2）初始化动态窗口信息，主要包括动态窗口的速度矢量、动态窗口的更新时间等。

（3）判断动、静态障碍物与USV是否存在碰撞

危险。如USV与静态障碍物存在碰撞危险，则保持安全距离避让;

如USV与目标船存在碰撞危险，则根据避碰规则对会遇局面进行划分，判断USV为让路船或直航船。

（4）如果USV为让路船，则进行迭代计算。在每次迭代过程中，结合避碰的不同阶段，依据上文所述的避碰时机模型、避让幅度模型和复航时机模型，选择最优速度矢量，也就是选择使G（u，r）的值最大的避让速度矢量（u，r）航行，并更新USV的位置。如果USV为直航船，则保持航向和航速航行，并更新USV的位置。为避免USV频繁调整航行状态，对

（Δu，Δr）设置阈值，如果（Δu，Δr）小于阈值，则保持上一时间段的航向和航速航行。

（5）在USV的位置更新后，判断USV是否已到达目的地。如果未到达目的地，则进行下一次迭代，更新动态窗口，直至USV到达目的地，提示避让障碍物成功。如果达到设置的最大迭代次数时USV还未到达目的地，算法也会结束，提示避让障碍物失败。

4 仿真验证

通过仿真验证基于改进动态窗口法的USV避碰算法的有效性。仿真环境为MATLAB R2015b。以文献[14]的初始动态窗口法和本文提出的融合避碰规则的动态窗口法为避碰策略进行对比验证。仿真实验参数设置如下：USV最大速度为22 m/s，最大加速度为0.6 m/s2，最大角速度为0.35 rad/s，最大角加速度为0.1 rad/s2;算法最大迭代次数为200;目标函数的权重因数分别为α=1，β=1，γ=1，η=0.6。分别模拟两船对遇、追越、交叉相遇及动、静态障碍物混合场景下的USV避让过程。不同会遇局面下USV和目标船的初始参数设置见表1。

仿真1、2和3分别对应对遇、追越和交叉相遇局面，两船存在碰撞危险，假设目标船保速保向航行，不采取避碰行动。不同会遇局面下USV的避让过程见图5～10，其中：图5、7、9为采用初始动态窗口法的避让过程;图6、8、10为采用融合避碰规则的动态窗口法的避让过程，其中dRNG取150 m，采取避碰行动时的角速度r取0.15 rad/s。

仿真1（对遇局面）结果分析。在避让时机选择方面，图5a显示在两船之间的距离约为50 m时采取避让行动，图6a显示在两船之间的距离为150 m时采取避让行动;因为两船对遇航行，相对速度大，应尽早采取避让行动，所以后者的避让时机选择符合避碰规则要求。在避让幅度和避让方向方面，图5 显示USV向左小幅度转向，图6显示USV向右大幅度转向，后者符合避碰规则要求。从整个避让过程看，采用初始动态窗口法时USV虽然能够避开目标船，但是避让时机晚、避让幅度小，而且选择向左转向显然不符合避碰规则的要求。融合避碰规则的动态窗口法在避让时机、避让幅度等方面表现更好，满足COLREGs关于避让行动早、大、宽、清的要求。

仿真2（追越局面）结果分析。在避让时机选择方面，图7a显示在两船之间的距离约为40 m时采取避让行动，图8a显示在两船之间的距离为80 m时采取避让行动。追越局面下采取的避让行动时机较对遇局面下的可以迟一些，但根据避碰规则要求，

也应尽早进行，因此后者的避让时机选择更为合理。在避让幅度和避让方向方面，图7b显示USV向右小幅度转向，图8b显示USV向左大幅度转向，显然后者符合COLREGs关于追越的避让要求。从避让路径看，虽然图8c中的路径长度明显大于图7c中的路径长度，但是从安全性和遵守COLREGs的角度看，融合避碰规则的动态窗口法明显占优。

仿真3（交叉相遇局面）结果分析。在避让时机选择方面，图9a显示在两船之间的距离约为50 m时采取避让行动，图8a显示在两船之间的距离约为140 m时采取避让行动，因此后者的避让时机更早一些。在避让幅度和避让方向方面，图9b和图10b都显示USV向右转向，但图10b显示USV向右大幅度转向，显然后者符合避碰规则要求。从避让路径看，虽然图9c中的路径长度明显短于图10c中的路径长度，但是从安全性和遵守避碰规则的角度看，融合避碰规则的动态窗口法明显占优。

仿真4：为充分考虑海上交通复杂度的影响，模拟动、静态障碍物混合场景下USV在受限水域的避让过程。USV和目标船1、2、3的初始参数设置如表1所示，静态障碍物的设置如图11所示。图11a显示USV先向左转向避让最近的静态障碍物，然后

向目的地航行。图11b显示USV通过一艘目标船船尾后，与另一艘目标船形成对遇局面，USV向右转向，确保以设定的最小安全会遇距离驶过这艘目标船;USV与第3艘目标船无碰撞危险。图11c显示了USV在驶向目的地的过程中形成的避让路径。

通过仿真1、2和3可以看出：采用初始动态窗口法和融合避碰规则的动态窗口法都能够避让动态障碍物，但是初始动态窗口法没有考虑避让时机、避让幅度、避让方向和复航时机等因素，因此融合避碰规则的动态窗口法在避让的安全性及符合规则方面表现更好。仿真4表明融合避碰规则的动态窗口法同样适用于动、静态障碍物混合场景下的避让。

5 结 论

针对无人水面艇（USV）避让动、静态障碍物的问题，采用动态窗口法建立融合避碰规则的USV避碰模型。结合USV的运动学特点，较充分地考虑依照《国际海上避碰规则》（COLREGs）关于早、大、宽、清的要求进行避让。通过仿真对比初始动态窗口法与融合避碰规则的动态窗口法，证明融合避碰规则的动态窗口法在保证避让障碍物的同时，在遵守规则及安全性方面相对于初始动态窗口法有明显优势，具有一定的实际意义。

参考文献：

[1]STATHEROS T， HOWELLS G， MCDONALD-MAIER K. Autonomous ship collision avoidance navigation concepts， technologies and techniques[J]. The Journal of Navigation， 2008， 61： 129-142.

[2]倪生科， 劉正江， 蔡垚， 等. 基于混合遗传算法的船舶避碰路径规划[J]. 上海海事大学学报， 2019， 40（1）： 21-26. DOI： 10.13340/j.jsmu.2019.01.004.

[3]LAZAROWSKA A. Ships trajectory planning for collision avoidance at sea based on ant colony optimization[J]. The Journal of Navigation， 2015， 68： 291-307.

[4]LAZAROWSKA A. Multi-criteria ACO-based algorithm for ships trajectory planning[J].The International Journal on Marine Navigation and Safety of Sea Transportation， 2017， 11（1）： 31-36. DOI： 10.12716/1001.11.01.02.

[5]TSOU M C， HSUEH C K. The study of ship collision avoidance route planning by ant colony algorithm[J]. Journal of Marine Science and Technology， 2010， 18（5）： 746-756.

[6]LYU Hongguang， YIN Yong. COLREGS-constrained real-time path planning for autonomous ships using modified artificial potential fields[J]. The Journal of Navigation， 2019， 72（3）： 588-608. DOI： 10.1017/S0373463318000796.

[7]PERERA L P， CARVALHO J P， GUEDES C G. Fuzzy logic based decision making system for collision avoidance of ocean navigation under critical collision conditions[J]. Journal of Marine Science and Technology， 2011， 16（1）： 84-99. DOI： 10.1007/s00773-010-0106-x.

[8]陈超， 唐坚. 基于可视图法的水面无人艇路径规划设计[J]. 中国造船， 2013， 54（1）： 129-135.

[9]庄佳园， 万磊， 廖煜雷， 等. 基于电子海图的水面无人艇全局路径规划研究[J]. 计算机科学， 2011， 38（9）： 211-219.

[10]KUWATA Y K， WOLF M T， ZARZHITSKY D， et al. Safe maritime autonomous navigation with COLREGs， using velocity obstacles[J]. IEEE Journal of Oceanic Engineering， 2014， 39（1）： 110-119.

[11]吴博， 熊勇， 文元桥. 基于速度障碍原理无人艇自动避碰算法研究[J]. 大连海事大学学报， 2014， 40（2）： 57-60.

[12]卢艳爽. 水面无人艇路径规划算法研究[D]. 哈尔滨： 哈尔滨工程大学， 2010.

[13]BENJAMIN M， CURCIO J， LEONARD J， et al. Navigation of unmanned marine vehicles in accordance with the rules of the road[C]∥IEEE International Conference on Robotics and Automation. ICRA， 2006： 3581-3587. DOI： 10.1109/ROBOT.2006.1642249.

[14]LOE. Collision avoidance concepts for marine surface craft[R]. Norway： Norwegian University of Science and Technology， 2007.

[15]唐平鹏， 张汝波， 史长亭， 等. 水面无人艇分层策略局部危险规避[J]. 应用科学学报， 2013， 31（4）： 418-426. DOI： 10.3969/j.issn.0255-8297.2013.04.013.

[16]FOX D， BURGARD W， THRUN S. The dynamic window approach to collision avoidance[J]. IEEE Robotics & Automation Magazine， 1997， 4（1）： 23-33. DOI： 10.1109/100.580977.

（编辑 赵勉）