无人艇航行规则及运动学约束下的改进RRT轨迹规划方法

文龙贻彬， 刘 友， 穆京京， 胡常青， 唐军武

（1.青岛海洋科学与技术试点国家实验室，青岛266237；2.中国航天科技集团有限公司，北京100048；3.北京航天控制仪器研究所，北京100039）

0 引言

目前，无人水面艇（以下简称无人艇）在国内外军事和民用领域的应用日益增多，而无人艇能够自主航行的关键则取决于路径规划技术［1］。其中，基于动态环境中的实时避碰与轨迹规划又是无人艇路径规划的关键所在。

在路径规划领域，RRT算法是一种应用非常广泛的方法。与其它方法相比，RRT算法有着无需对系统进行建模、无需对搜索区域进行几何划分、在搜索空间的覆盖率高、搜索的范围广、可以尽可能的探索未知区域等优点。但将其直接应用于无人艇上也存在着诸多缺陷，如均匀随机采样降低算法收敛速度、生成的路径不够平滑、没有考虑运动学约束等。因此，针对RRT算法在轨迹规划上的应用，国内外学者提出了很多改进方法。 Webb 等［2］提出了 Kinodynamic RRT*方法， 通过状态方程进行采样，并构建了一种控制器使规划出来的路径符合动力学约束。Palmieri等［3］提出了Theta*-RRT方法，将Theta*与RRT方法相结合，提高了轨迹规划的速度。Elbanhawi等［4］将车辆动力学约束要求与B样条曲线特性相结合，从而规划出平滑轨迹。庄佳园等［5］提出了一种改进的RRT算法用于无人艇的局部路径规划，通过引入抑制因子等手段，提高了算法速度。但上述方法均未考虑动态障碍物。

针对以上问题，本文提出了双层RRT算法。在第一层框架中考虑动态障碍物对无人艇避碰决策的影响，因此首先结合海事规则、最短会遇时间与安全距离构建最优位置窗口，然后基于改进的RRT算法实时规划出一条联通路径。在第二层框架中则考虑到无人艇的运动学约束，将上一层的联通路径点作为分段启发点，然后结合速度运动模型来限制无人艇的拐角与转弯半径，并基于速度运动模型得到的弧长计算出每一个节点的代价值，最终在动态障碍物环境中得到一条可行平滑轨迹。

1 基于改进RRT的路径搜索算法

1.1 改进的探索点及步长选择策略

传统的RRT算法有着很大的随机性，这有利于对未知环境的探索，但同时会让算法收敛速度变慢，降低算法的实时性。此外，传统的RRT算法的搜索步长是固定的。当选取较大步长时，可以降低扩展次数，增加算法收敛效率，但如果周边环境复杂，大步长容易直接碰到障碍物区域，从而生成无效解，反过来又降低了算法效率；当步长较小时，可以增加算法的避碰能力，但由于增加了扩展次数，会降低算法的实时性。

为了解决上述问题，往往会采用基于概率的目标偏向策略、引入动态步长、增加引力分量［6］等方法。但上述方法中的概率参数、引力系数等因子的取值与周边环境的复杂程度、障碍物分布情况相关，因而难以定量建立取值公式。而动态步长策略达到阈值后，也会失效。针对上述方法的缺陷，本文改进了探索点的概率选择方法并引入一步步长的策略，更好地兼顾了算法的随机性与收敛性。针对基于概率的目标偏向策略来引导随机树的生长方向的方法，可以得到如下探索点的取值公式

式（1）中，xe为探索点，xrandom为随机采样点，xgoal为终点，pn为启发式采样概率，pr为随机数，且pr∈ ［0， 1］。

然后，基于步长可以得到新的采样点xnew

式（2）中，xnear为随机树中离xe最近的节点，ρ为步长。

可以认为，随着随机采样点探索次数的增多，算法收敛的概率会逐步增大。计f为随机采样点探索成功的次数，即xe＝xrandom，且能生成xnew。每成功一次，f＝f＋1；当目标采样点探索失败，即xe＝xgoal，但无法生成xnew，则定f＝0。

因此，可定义如下的采样概率公式

式（3）中，λ为比例因子，0＜λ＜1。λ越小，算法的随机性越大，通常取λ＝0.1以兼顾算法随机性与收敛性。

由于传统动态步长策略在达到阈值后，动态步长将不再发生变化。结合传统动态步长方法［7］以及一步步长策略，建立如下动态步长公式

式（4）中，ρ为动态步长；d为当前节点到周边障碍物的最近距离；dt为距离阈值；α为衰减系数；ρmin＝ρmaxe-α，ρmax为 最 大 步 长；d（xnear，xgoal）为xnear与xgoal间 的 距 离；pos＝pr为0到1间的随机数；ξ＝

即当xe＝xgoal时， 随着d（xnear，xgoal）减小，d（xnear，xstart）增大， 一步步长成功的可能性会更大。因此，采取一步步长策略的概率会逐步增大。而当xe＝xrandom时，则依然使用传统动态步长的策略：xnear离障碍物较远时，采用大步长ρmax，加快算法收敛速度；xnear在障碍物附近时，基于指数衰减函数得到的小步长来确保航迹平滑度。这样，就兼顾了算法的实时性与路径的平滑度。

1.2 基于海事规则的四向扩展随机树

在实际航行过程中，无人艇应遵循 《国际海上避碰规则公约》（COLREGS）。根据 COLREGS、船舶避碰的经验数据以及实际船只在复杂海洋环境中的航行范围，为无人艇定义了追越（Overtaking， OT）、 对遇（Head-on， HO）、 右交叉（Crossing from right， CFR）、 左交叉（Crossing from left，CFL）四种冲突局面，并得到冲突局面的关系函数和航行规则表。

式（5）中，P为目标船只所在范围，如图1所示；Δθ为无人艇与目标船只航向角角度差，并定义无人艇艏向为0°，顺时针为正，则可知：fc∈｛HO，OT，CFR，CFL｝， 具体如表1所示。

根据COLREGS，可以得到无人艇在遇到目标船只的避碰转向USVturn

图1 无人艇与目标船只相对位置Fig.1 Relative position of USV and target vessel

表1 航行规则表Table 1 Navigation rules table

此外，最短会遇时间TCPA（Time to Closest Point of Approaching）即指无人艇与目标船只在保持现有的航行状态下会遇时两船距离最近时所需要的时间，可以对目标船只位置进行预测。TCPA的计算公式［7］如下

式（7）中，RT为两船相对距离，αT为相对方向，φR为相对速度航向，vR为相对速度。

根据USVturn、TCPA及安全距离范围可以得到无人艇的最优位置窗口，如图2所示。根据表1及式（6），可知图2中的无人艇与目标船只处于对遇局面，无人艇应向右拐弯，应在无人艇右方构建位置窗口。其中，Lvessel为目标船只的船长，vvessel为目标船只的速度，dmin、dmax分别为避碰距离的上限与下限。根据实际航行经验，dmin应略大于安全距离，dmax约为安全距离的3倍。根据过往航行数据，由此形成的最优位置窗口就是无人艇在避碰过程中的必经区域。

图2 最优位置窗口示意图Fig.2 Diagram of optimal position window

相对于传统RRT算法的单随机树，只能从起点生成，并向终点生长，如图3所示。在实际航行中，可以根据态势程度的不同在走廊中选取一点作为无人艇的必经点，然后在起点与必经点、必经点与终点间分别构造两棵扩展随机树，从而构建出了四向扩展随机树。四向扩展随机树能够并行搜索，从而提高了算法的效率，构建的必经点也增强了算法的启发性，如图4所示。

图3 单搜索随机树Fig.3 Single search random tree

图4 四向扩展随机树Fig.4 Four-way extended random tree

基于改进RRT的路径搜索算法的流程图如图5所示。由于RRT算法搜索出来的路径是由各枝叶节点连接而成，因此在搜索出联通路径后，还需要使用最短路径算法来优化路径。

图5 基于改进RRT的路径搜索算法流程图Fig.5 Flowchart of path search algorithm based on improved RRT

2 基于RRT的轨迹生成方法

基于改进RRT的路径搜索算法只是找到了一条路径，却没有考虑无人艇的运动学模型。实际上，控制器不一定能跟随此路径。为了解决上述问题，本文提出了基于RRT的轨迹生成方法。

2.1 速度运动模型

在动力学上，无人艇是具有非完整约束的。因此，无人艇的动力学可以用速度运动模型g（x，y，θ，v，ω）进行描述。其中，x、y为位置，θ为方向，v为速度，ω为角速度。则可以得到下一时刻状态点的方程

对速度运动模型进行闭式求解［8］，可以计算出从当前点到下一状态点的速度、角速度、转弯半径以及圆心点坐标，如图6所示。

图6 速度运动模型示意图Fig.6 Schematic diagram of velocity motion model

图6中，A为初始状态点；B为下一时刻状态点；θ为A到B的航向角度差；θUSV为无人艇的拐角大小，且θ＝2θUSV；ξ为两点间的Euclidean距离；R为无人艇的转弯半径，其取值大小为

因此，在已知A、B点的前提下，可以求得轨迹；在已知A、θUSV、ξ的前提下，也能求出B点。

2.2 非完整约束

考虑到无人艇的非完整约束，将无人艇的最大转角作为约束条件，从而限制随机采样点的生成范围。为了尽快找到可行轨迹且尽可能地利用第一层算法所搜索到的路径信息，令式（1）中的pn＝1， 即一直使用一步步长策略，并基于拐角约束构建如下采样公式

由于θ∈ ［-θmax，θmax］， 结合式（9）可知

此外，根据船舶的相关设计规范，转弯半径与船长的比值一般在3～7.5之间。在考虑无人艇的最小转弯半径与拐角约束的情况下，可以计算出ξ值。

2.3 基于速度运动模型的代价值计算方法

传统RRT算法的代价值是路径节点A（x1，y1）与路径节点B（x2，y2）的Euclidean距离。但当考虑到无人艇的动力学模型时， 状态点A′（x1，y1，θ1）与状态点B′（x2，y2，θ2）间的代价值就不能仅仅只考虑Euclidean距离，还需要考虑艏向角对代价值的影响。因此，令

式（12）中，θ为A′到B′的航向角度差，R为A′到B′的转弯半径，fcost即为A′到B′的实际轨迹长度，以此作为节点A′的控制能耗代价。

然后，将计算出来的控制能耗代价当作一个启发值，每一次都选取代价值最小的节点开始进行轨迹搜索。

此外，考虑无人艇的最小转弯半径与拐角约束的情况下计算出来的ξ值可能会比较大，这就会导致相距较远。若只将这两个节点加入随机树，虽然生成的轨迹依然是平滑的，但很难生成最优轨迹。如图7（a）所示，轨迹AGD是明显优于轨迹ABCD的。因此，需要在之间进行子节点的扩充，如图7（b）所示。在A、B节点间进行子节点扩充，并分别计算cost值，再将这些子节点加入随机树，且扩充的子节点的父节点都为即A节点。

图7 中间节点扩展示意图Fig.7 Diagram of intermediate node expansion

2.4 动态窗口

上述的轨迹生成方法是在搜索到的路径信息基础上得到的轨迹，是假设轨迹不会碰到障碍物。实际上，路径无障碍并不代表轨迹无障碍。

图8 动态窗口示意图Fig.8 Diagram of dynamic window

3 仿真实验

无人艇与目标船只的航向角皆以北向为0°且顺时针为正，dmin＝50m、dmax＝100m，无人艇最小转弯半径为R＝30m，最大拐角θmax＝30°。 为了更好地描述目标船只运动趋势，结合藤井模型［9］，以半椭圆表示目标船只。其中，椭圆的朝向即为目标船只的运动方向，椭圆短轴为船长的3倍，长轴为船长的7倍，并设定目标船只船长为2m。

图9 基于RRT的轨迹生成方法流程图Fig.9 Trajectory generation method based on RRT

本文基于3种不同的会遇局面进行仿真实验，并通过转折数目（转折角度大于θmax）、最大转折角度、路径长度、与障碍物最近距离、规划时间来评价路径的优劣。

（1）对遇局面

设初始运动参数如下：无人艇航速v＝5m／s、航向θ＝270°， 目标船只航速v＝5m／s、 航向θ＝90°、距离无人艇100m。由图10可知，无人艇与目标船只将构成对遇局面。仿真图中的白色线条为无人艇的路径，s为无人艇当前位置，g为无人艇终点（后文相同）。

图10 对遇局面下无人艇动态航迹Fig.10 USV dynamic track under HO situation

（2）追越局面

设初始运动参数如下：无人艇航速v＝20m／s、航向θ＝270°， 目标船只航速v＝3m／s、 航向θ＝270°、距离无人艇50m。由图11可知，无人艇与目标船只将构成追越局面。

图11 追越局面下无人艇动态航迹Fig.11 USV dynamic track under OT situation

（3）交叉局面

设初始运动参数如下：无人艇航速v＝10m／s、航向θ＝0°，目标船只航速v＝10m／s、航向θ＝270°、距离无人艇112m。由图12可知，无人艇与目标船只将构成右交叉局面。

图12 右交叉局面下无人艇动态航迹Fig.12 USV dynamic track under CFR situation

由图10～图12可知，改进的双层RRT算法得到的无人艇避碰轨迹都符合COLREGS。相较传统RRT算法与第一层算法，该改进算法能得到更加平滑的轨迹，在保持一定安全距离的同时，满足了无人艇的运动特性。

表2～表4为上述3种会遇局面的实验对比结果。

表2 对遇实验结果对比Table 2 Comparison of HO experiment results

表3 追越实验结果对比Table 3 Comparison of OT experiment results

表4 右交叉实验结果对比Table 4 Comparison of CFR experiment results

由表2～表4可知，传统RRT算法、第一层算法、第二层算法都能迅速地规划出路径。但第一层算法的规划时间小于传统RRT算法，可见基于改进RRT的路径搜索算法是有效的，能够在解决动态避碰问题的同时，加快算法的运行效率。第二层算法的规划时间虽然略大于第一层算法，但依然能够迅速地找到路径，且总体来说依然比传统RRT算法要快。可见，本文的双层RRT算法是可以满足无人艇避碰实时性要求的。第二层算法的转折数目始终为0，最大转折角度为19.1°，这两个指标好于传统RRT算法与第一层算法。可见，基于RRT的轨迹生成方法是有效的，可以得到平滑轨迹并且满足了无人艇的运动特性。此外，第二层算法与障碍物的最近距离是传统RRT算法的两倍以上，可见本文算法得到的轨迹更加安全。

4 实船验证

针对算法开展实船实验。其中，无人艇长7.5m、宽2.5m，量化安全距离为30m，无人艇的最小转弯半径R为船长的3倍，无人艇的最大拐角θmax＝30°、dmin＝40m、dmax＝100m。

实验过程中，无人艇在每组场景下保持6kn以上的航速航行，并依靠导航雷达与激光雷达获取障碍物信息。有人驾驶艇则充当会遇船只，为了更好地验证算法的有效性，有人驾驶艇保持恒向恒速，不轻易采取大的机动动作。当两船会遇时，采取双层RRT算法实现动态避碰。

（1）对遇局面

目标船只速度约为10kn，方向由东向西。无人艇最高航速为10kn，原始航线为图13中的绿色虚线，1为起点，2为终点。无人艇轨迹如图13、图14（a）中的红色虚线所示，目标船只轨迹如图14（a）中的蓝色实线所示。无人艇与障碍物的最近距离为41.43m，如图14（b）所示。

图13 两船对遇实验控制界面示意图Fig.13 Control interface diagram of two ship HO experiment

图14 两船对遇实验Fig.14 Diagram of two ship HO experiment

（2）追越局面

目标船只速度约为2kn，方向由西向东。无人艇最高航速为10kn，原始航线为图15中的绿色虚线，1为起点，2为终点。无人艇轨迹如图 15、图16（a）中的红色虚线所示，目标船只轨迹如图16（a）中的蓝色实线所示。无人艇与障碍物的最近距离为37.31m，如图16（b）所示。

图15 两船追越实验控制界面示意图Fig.15 Control interface diagram of two ship OT experiment

图16 两船追越实验Fig.16 Diagram of two ship OT experiment

（3）交叉局面

目标船只速度约为10kn，方向由南向北。无人艇最高航速为10kn，原始航线为图17中的绿色虚线，1为起点，2为终点。无人艇轨迹如图17、图18（a）中的红色虚线所示，目标船只轨迹如图18（a）中的蓝色实线所示。无人艇与障碍物的最近距离为67.65m，如图18（b）所示。

图17 两船右交叉实验控制界面示意图Fig.17 Control interface diagram of two ship CFR experiment

图18 两船右交叉实验Fig.18 Diagram of two ship CFR experiment

表5 实船实验结果对比Table 5 Comparison of actual ship tests results

算法的平均规划时间为0.015s，表明本文提出的算法符合无人艇的实时性需求。由图14、图16、图18及表5可知，无人艇的轨迹平滑，满足无人艇的运动约束和 《国家海上避碰规则公约》。从相对距离示意图也可以看出，避碰路径是满足安全距离的。

通过上述实船实验可以看出，本文提出的算法是有效的。

5 结论

本文针对无人艇的动态避碰进行了研究，所提出的改进双层RRT算法考虑了无人艇的运动特性和 《国际海上避碰规则公约》所带来的影响。在第一层算法中，基于 《国际海上避碰规则公约》、最短会遇时间建立了最优位置窗口，从而构造出了四向扩展随机树，让无人艇在避碰过程中能够根据目标船只的信息产生正确的决策，并搜索出对应路径。在第二层算法中，则通过结合速度运动模型实现了无人艇的轨迹生成。仿真分析和实船实验均验证了本文所提方法的有效性。其中，转折数目始终为0，最大转折角度指标远好于传统RRT算法，表明本文算法是可以得到平滑轨迹的。此外，算法运行时间亦满足无人艇实际航行与避碰的实时性要求。未来，可进一步结合无人艇的动力学特征，基于最低能耗、最小加加速度等优化目标对轨迹进行优化，生成更优的轨迹。