复杂水域慎思型避碰轨迹规划

秦操　杨荣武

摘要：为解决复杂水域的船舶自主避碰问题，提出一种基于A*算法的慎思型避碰轨迹规划算法，旨在满足船舶操纵性约束、静态与动态障碍物约束和《国际海上避碰规则》（International Regulations for Preventing Collisions at Sea，COLREGs）约束下，规划出一条最经济的航行轨迹。通过无人三体船自主避碰试验和模拟试验，验证算法的有效性，具有较高的参考价值。

关键词：船舶; 自主避碰; 慎思型轨迹规划; COLREGs; 操纵性约束

中图分类号：  U675.73

文献标志码：A

Deliberative collision avoidance trajectory planning for complex waters

QIN Cao1， YANG Rongwu2

（1. Collaborative Innovation Center for Advanced Ship and Deep-Sea Exploration， Shanghai Jiao Tong University，

Shanghai 200240， China; 2. Seastel Marine System （Shanghai） Co.， Ltd.， Shanghai 200241， China）

Abstract：

To solve the problem of ship autonomous collision avoidance in complex waters， a deliberative collision avoidance trajectory planning algorithm based on A* algorithm is proposed， which aims to plan a most economical sailing trajectory with the ship maneuverability constraints， static and dynamic obstacle constraints as well as International Regulations for Preventing Collisions at Sea （COLREGs） constraints. The effectiveness of the algorithm is verified by the unmaned trimaran autonomous collision avoidance test and the simulation test， which has high reference value.

Key words：

ship; autonomous collision avoidance; deliberative trajectory planning; COLREGs; maneuverability constraint

0 引 言

近年來海上碰撞事故频发，不仅造成了巨大的人员伤亡和经济损失，还带来了严重的环境污染。欧洲海事安全局发布的《2017海上事故年度回顾报告》显示，海上事故的一半为船舶碰撞及搁浅事故，其中60%以上是人的因素造成的[1]。为避免人因事故，船舶自主避碰系统的研究受到越来越多的关注[2]。另外，随着无人商船技术的发展，各国船级社陆续发布了有关规范，其中中国船级社《智能船舶规范》[3]第2.6节要求智能船舶应具有在狭窄水道自动避碰的能力，以及在复杂环境条件下自主航行的能力。《自主货物运输船舶指南》[4]第3.3.1节也提出了自主货船应根据感知到的场景信息和船舶自身信息进行综合分析决策，并遵守《国际海上避碰规则》（International Regulations for Preventing Collisions at Sea， COLREGs）。因此，对船舶自主避碰系统的研发和改进也越来越迫切。

轨迹规划是船舶自主避碰系统的核心，其目标是给出一条无碰撞危险的、动力学可行的、满足航行规则的、最经济的轨迹来引导船舶安全航行。规划算法需要满足操纵性约束、障碍物约束、COLREGs约束和实时性约束，并达到最优轨迹规划目标[2]。目前的计算能力无法同时保证实时性和轨迹最优性，于是产生了放弃全局最优而保证高重规划率的反应型规划算法，以及降低重规划率来追求全局最优的慎思型规划算法。

近年来，一些慎思型规划算法被提出，主要包括图基法和采样法两类[2]。SHAH等[5]提出的适应性风险应急响应规划（adaptive-risk and contingency-aware planning，  ARCAP）算法为图基法的代表，它采用A*算法[6]在五维状态空间中搜索路径，在 COLREGs 约束下最小化碰撞风险，并利用自适应采样的方式加快搜索速度。YANG等[7]提出了一种基于采样法的算法，它基于快速搜索随机树（rapidly-exploring random tree， RRT）算法[8]框架建立了机动自动化（maneuver automation， MA）运动基元库来满足操纵性约束，并且可以同时满足COLREGs约束和实时性约束。

本文提出一种基于A*算法同时考虑操纵性约束、静态与动态障碍物约束、COLREGs约束的慎思型避碰轨迹规划算法。通过无人三体船自主避碰试验和模拟试验，验证算法的有效性。

1 慎思型避碰轨迹规划算法实现

1.1 问题的数学描述

为清晰地描述慎思型避碰轨迹规划问题，现对一些重要的概念进行定义：连续的船舶状态

s=（x，y，ψ，u，t）

构成连续的船舶状态空间S，其中x和y为大地坐标，ψ为艏向角，u为船舶纵向速度，t为时间。轨迹是连续的船舶状态s沿时间维度遞增构成的序列。连续的船舶状态空间S离散后得到的最小单元称为网格，一个状态只对应一个网格。定义离散的控制基元空间Uc，d，其由控制基元uc，d=（ud，ψd，T）构成，其中ud和ψd分别为期望的纵向速度和期望的艏向角，T为执行时间。船舶执行Uc，d中的一个控制基元对应着其在S中从一个状态变换到另一个状态的一段连续的航行轨迹，称为轨迹基元utra=（u0，ud，dψ，T），u0是轨迹基元初始状态的纵向速度，dψ是轨迹基元的转艏角度。一系列连续的轨迹基元串联构成一条完整的轨迹。

算法的目标是在包含障碍物的状态空间内找到一条从初始状态s0到最终状态sg的最经济的轨迹，并满足船舶操纵性约束、障碍物约束和COLREGs约束。

1.2 慎思型避碰轨迹规划算法架构

算法主要包含3个模块：全局轨迹搜索器、可行轨迹生成器和状态代价评估器。全局轨迹搜索器采用A*算法框架，在船舶状态空间内搜索代价最小的全局轨迹，并在搜索过程中不断地调用可行轨迹生成器和状态代价评估器。可行轨迹生成器采用控制基元理论，生成满足操纵性约束的可行轨迹基元以完成完整的全局估计。状态代价评估器用于计算状态的代价，作为搜索该往哪个状态延伸的评价指标。算法架构见图1。

1.3 全局轨迹搜索器

全局轨迹搜索器在船舶状态空间内搜索从初始状态到最终状态的代价最小的全局轨迹。全局轨迹搜索器基于A*算法，通过可行轨迹生成器生成的轨迹基元延伸到后续子状态，直至搜索到最终状态，然后通过最终状态不断地回溯前面的父状态来获得一条全局轨迹。本文在A*算法的基础上进行如下改进：（1） 将搜索空间的维度由传统A*算法的二维提高到四维，增加ψ维度以便确定后续子状态的延伸方向，增加u维度以便有更多的速度选择。（2） 状态的延伸不再由当前网格状态延伸到其邻接网格状态，而是采用可行轨迹生成器向外延伸，以满足船舶操纵性要求，并提高搜索效率。

全局轨迹搜索器是慎思型避碰轨迹规划算法的“大脑”，其搜索流程如下：

步骤1 生成优先队列

Q，将初始状态s0加入Q中。

步骤2 如果Q为空，则返回“不存在轨迹”;否则，将Q中代价最小的状态s取出，并将s对应的网格关闭，如果s在终点状态sg的一定范围内，则递归地回溯s的父状态来获得一条轨迹并返回。

步骤3 调用可行轨迹生成器（见第1.4节），以s为起点生成若干轨迹基元，找到轨迹基元末端状态s′所对应的网格。若该网格已关闭，则抛弃该段轨迹基元。若该网格未关闭，则调用状态代价评估器（见第1.5节）计算s′的代价：若该网格中有状态在Q中，则比较该状态与状态s′的代价，只将代价较小的状态保留在Q中;若该网格内没有其他的状态，则将状态s′加入Q中。重复调用步骤2。

以x、y坐标维度举例说明，如图2所示：灰色实心圆表示Q中的状态，黑色实心圆表示已经从Q中取出的状态;步骤2是从Q中取出代价最小的状态s，并关闭对应的网格;步骤3是s延伸扩展出3个子状态s′1、s′2和s′3，因为s′1对应的网格中有Q中的另一个状态，所以保留代价较小的子状态s′1，因为子状态s′2、s′3对应的网格中没有其他状态，所以将其加入Q。

1.4 可行轨迹生成器

全局轨迹搜索器在搜索过程中需要由某一状态扩展到后续子状态，由于船舶操纵性限制，船舶从一个状态出发，并不能到达任意的下一状态。可行轨迹生成器采用控制基元理论，从某一状态出发沿着轨迹基元延伸到下一状态，满足操纵性约束。控制基元理论是一种处理操纵性约束的方法，其基本思想是船舶执行相同的控制基元形成的轨迹段是一条固定不变的轨迹基元，不会随位置、艏向和时刻的变化发生改变。传统的生成轨迹基元的方法

是给定目标uc，d=（ud，ψd，T），分别用速度控制器和艏向控制器实现ud和ψd，执行时间T后生成的轨迹就是轨迹基元[5]。为实现船舶从静止启动、匀速航行到紧急制动，设置两

挡速度（设计航速0.8 m/s和停止0 m/s），设置9挡转艏角度（-60°、-45°、-30°、-15°、0°、15°、30°、45°、60°）和一挡执行时间（8 s），构成轨迹基元库，见图3，其中轨迹基元

utra=（u0，ud，dψ，T）。轨迹基元的初始状态和最终状态都可能对应两种不同速度，因此全局轨迹搜索器增加u维度以便实现不同速度选择。传统A*算法以当前状态的前、后、左、右4个方向进行后续状态的延伸，而可行轨迹生成器只能向当前状态的前方一定范围进行延伸，以满足操纵性约束，因此全局轨迹搜索器需增加ψ维度以便确定后续子状态的延伸方向。

1.5 状态代价评估器

1.5.1 状态代价函数

在全局轨迹搜索器搜索的过程中，状态代价评估器用来计算当前状态s′的代价f（s′）：

式中：g（s′）是从初始状态s0到当前状态s′的实际代价;h（s′）是从当前状态s′到最终状态sg的预估代价;e是系数，用以调节g（s′）与h（s′）之间的权重;s为s′的父状态，s′由s执行一个控制基元得到;pc，s′表示在执行这个控制基元过程中的碰撞概率，计算方法详见第1.5.2节;g（s′）由父状态s的实际代价g（s）、从s到s′的航行过程中可能发生碰撞的代价ce和非碰撞代价cs′三部分构成;ce为一个定值;cs′由耗时tss′、路程dss′和违反COLREGs的代价cCOLREGs三部分构成，cCOLREGs的计算详见第1.5.3节;h（s′）由从s′到sg的估计时间ts′sg和估计路程ds′sg构成;ωc、tmax和dmax为系数。

第二个场景包含复杂的静态障碍物和2艘来船，本船从地图右上方向左下方航行，与第一艘来船形成对遇局面，与第二艘来船形成右舷交叉会遇局面。慎思型避碰轨迹规划算法给出的轨迹见图8a，不考虑COLREGs的规划算法给出的轨迹见图8b。如果不考虑COLREGs，算法在依次面对两个会遇局面时都会选择向左避让，虽然全局轨迹会更短，但是违反了COLREGs。考虑COLREGs的慎思型避碰轨迹规划算法虽然规划出的轨迹较长，但在依次面对两个会遇局面时均能遵守COLREGs避开来船，因此更合理。

4 结 论

本文提出一种慎思型避碰轨迹规划算法，在A*算法的基础上进行了大量的改进，将状态由二维空间扩展到四维空间，利用控制基元方法延伸状态并同时满足操纵性要求，重新设计了代价函数来避开复杂障碍物和满足COLREGs约束。通过三体船的自主避碰航行试验和仿真试验，验证了算法的有效性，具有较高的参考价值。

算法仍存在问题和改进的空间：（1）动态障碍物的预测轨迹与其真实航行轨迹之间的差别是一个重要影响因素，如果他船也是具有自主规划能力的智能船舶，则轨迹规划会陷入一种博弈局面，未来的研究可以考虑预测他船航行意图。（2）在环境干扰（风浪流）较大的情况下，轨迹跟随会变得非常困难，如何设计更好的轨迹跟随方法也是后续的研究方向。（3）减小网格大小、增加轨迹基元的数量都能进一步优化规划轨迹，但是计算量会大大增加，给满足实时性约束带来挑战，因此如何在轨迹最优性与实时性之间进行权衡也是今后研究的一个难点。（4）鉴于上述问题和不足，算法的实用性需要进一步研究与验证。

参考文献：

[1]EMSA. Annual overview of marine casualties and incidents 2017[R]. European Maritime Safety Agency， 2018： 15-41.

[2]CAMPBELL S， NAEEM W， IRWIN G W. A review on improving the autonomy of unmanned surface vehicles through intelligent collision avoidance manoeuvres[J]. Annual Reviews in Control， 2012， 36（2）： 267-283. DOI： 10.1016/j.arcontrol.2012.09.008.

[3]中国船级社（CCS）. 智能船舶规范[S]. 2015.

[4]中国船级社（CCS）. 自主货物运输船舶指南[S]. 2018.

[5]SHAH B C， VEC P， BERTASKA I R， et al. Resolution-adaptive risk-aware trajectory planning for surface vehicles operating in congested civilian traffic[J]. Autonomous Robots， 2016， 40（7）： 1139-1163. DOI： 10.1007/s10514-015-9529-x.

[6]HART P E， NILSSON N J， RAPHAEL B. A formal basis for the heuristic determination of minimum cost paths[J]. IEEE Transactions on Systems Science and Cybernetics， 1968， 4（2）： 100-107.

[7]YANG Rongwu， XU Jinsong， WANG Xin， et al. Parallel trajectory planning for shipborne autonomous collision avoidance system[J]. Applied Ocean Research， 2019， 91： 101875. DOI： 10.1016/j.apor.2019.101875.

[8]LAVALLE S M. Rapidly-exploring random trees： a new tool for path planning[R]. Iowa： Iowa State University， 1998.

[9]赵劲松， 王逢辰. 船舶避碰学原理[M]. 大连： 大连海事大学出版社， 1999.

[10]IMO. Convention on the international regulations for preventing collisions at sea[S]. http：//www.imo.org/conventions.

[11]KUWATA Y， WOLF M T， ZARZHITSKY D， et al. Safe maritime autonomous navigation with COLREGs using velocity obstacles[J]. IEEE Journal of Oceanic Engineering， 2013， 39（1）： 110-119. DOI： 10.1109/JOE.2013.2254214.

[12]COLITO J. Autonomous mission planning and execution for unmanned surface vehicles in compliance with the marine rules of the road[D]. Washington： University of Washington， 2007.

[13]SVEC P， SHAH B C， BERTASKA I R， et al. Dynamics-aware target following for an autonomous surface vehicle operating under COLREGs in civilian traffic[C]//2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE， 2013： 3871-3878.

（編辑 赵勉）

收稿日期： 2019-11-26

修回日期： 2020-02-24

作者简介：

秦操（1994—），男，湖北荆州人，硕士研究生，研究方向为船舶自主避碰，（E-mail）403502830@qq.com;

杨荣武（1984—），男，福建漳州人，博士，研究方向为船舶自主避碰，（E-mail）yangrongwu@seastel.com