无人水面机器人(unmanned surface vehicle,USV)在海上航行时需要具备自主规避障碍的能力，在线路径规划提供了一种远程障碍规避的手段。在线路径规划是指基于雷达获得的船舶、岛屿等障碍信息，实时规划自身的速度和航向以产生避开障碍、到达目标的最优或次最优路径(无障碍物路径)。根据对环境的获知程度，路径规划分为全局路径规划和局部路径规划，在线路径规划是一种局部路径规划，因为部分环境是未知的。

无人水面机器人在海洋中在线路径规划的特点是：1)海洋环境具有动态、突发性、不可预测性。2)USV具有6个自由度、偏航、前进、横移，也会出现横倾、纵倾，体积小、速度快、灵活性高、瞬时的失误可能造成无法弥补的损失等运动特性。3)USV预先定义好的航线和驾驶规则。常用的在线路径规划算法采用局部避障算法结合全局规划算法，如障碍物边界追踪结合融入A^*搜索算法^[1]。然而这种方法并没有考虑国际海上避碰规则公约(convention on the international regulations for preventing collisions at sea,COLREGs)^[2]。COLREGs指出了当存在碰撞的危险时，应该采取的避碰行为。当USV与其他船舶相遇时，其导航算法必须遵守COLREGs，才能安全躲避其他船舶，这样其他船舶上的驾驶人就能预测它所采取的安全行为。许多学者提出了遵守COLREGs的导航方法，如模糊逻辑^[3]、区间优化^[4]。然而这些算法对多个COLREGs规则的应用不是很好，而且在实时性需求方面并不能达到理想。Yoshiaki等^[5]将COLREGs规定的可行速度区与速度障碍区同时作为约束条件，以交叉检测的碰撞时间和下个路径点的期望速度作为代价函数，在速度－航向(v-θ)空间搜索最好的速度矢量。这种方法能快速有效地避开障碍物，然而他们也指出，使用的速度避障法只是一种局部规划器，要想实现远程距离的路径规划必须加入全局规划器。COLREGs本身就包含有避碰行动信息，与速度避障法能非常好的融合，同时使速度避障法更好地运用于USV与船舶会遇的避碰行动中，使避碰行动完全合理合法。祖迪等^[6]采用MILP (mixed-integer linear programming)进行移动机器人的路径规划，将移动机器人的本体动力学和速度避障法角度约束结合，取得比较好的效果。

本文对USV在动态环境下进行在线路径规划进行研究，采用基于速度避障法的局部规划器规避障碍，基于MILP的全局规划器实现距离时间的优化，使规划的路径既能遵守COLREGs规则，安全地避开障碍，同时获得全局路径优化。

本文研究的USV在线路径规划问题如下：

1)目标：1个远程静态路径点。

2)障碍物：视一定范围内的水面船舶为运动障碍物，通过传感器获得其速度、航向和位置。

3)USV运动模型：包含运动学和动力学，应用其在仿真中可实时模拟USV的运动状态。

4)假设条件：因为船体本身具有的惯性比较大，所以假设极短的规划时间Δt内，障碍物保持匀速运动，即USV相对运动障碍物的速度改变量等于USV自身的速度改变量。

由此，在线路径规划的任务即是在每个规划时间间隔搜索出最优的速度矢量增量，使其既能遵守COLREGs规则躲避运动的障碍物，同时能搜索到达目标的全局最优或次最优(无碰撞)的优化路径。

基于线性速度障碍物LV-Obstacle的一阶算法原理在文献^[7]有详细的说明，本文在此基础上进行改进，提出基于COLREGs规则避障的改进速度避障算法。速度避障法二维原理图如图 1所示。将USV缩小为一个点U，障碍物膨胀为一个圆，圆心为点O，两者速度矢量分别为v_U和v_O。相对速度为v_UO=v_U－v_O。USV与障碍物中心的连线为L_UO，相对速度v_UO与L_UO的夹角为γ_UO，USV与障碍物圆的2条交线分别为UN和UM,相对速度v_UO与UN和UM的夹角分别为Δγ_UO^R和Δγ_UO^L，相对速度v_UO延长线与障碍物圆的最短交点为D。

图 1 速度避障法二维原理图 Fig. 1 Schematics of velocity obstacle

图选项

1)膨胀半径：R=V_r/γ，碰撞半径等于会遇船舶的安全距离。安全距离取决于相对速度和转向率,见文献^[8]。

2)避碰区(collision cone,CC)：

3)避碰角：

4)碰撞时间：τ=|L_UD|/|v_UO|,即当相对速度处于碰撞区时，USV与障碍物的碰撞时间。

5)碰撞危险度：ρ=(a×DCPA)²+(b×TCPA)²，其中a,b分别表示DCPA和TCPA的权值。碰撞危险度通过DCPA和TCPA加权确定。

6)规划时间：T=w₁ρ+w₂/τ，即规划时间与碰撞危险度成正比，与碰撞时间成反比。因为受到USV本体运动性能约束，其转角加速度和纵向加速度都有限制，所以碰撞危险度越大，碰撞时间越短，规划时间就越短，否则无法及时进行航速和方向的规划。

从上述避障法的原理可以看出，要使USV不与障碍物发生碰撞，其相对速度v_UO必须产生一个转角，使其脱离碰撞区。从图 1可以看出它有2个方向可以选择，当向左转时，即使相对速度产生Δγ_UO^R的转动，向右转时，使相对速度产生Δγ_UO^L的转动。这个转动是由规划时间内相对速度矢量Δv_UO的变化产生的，式(1)可以看出。因此，将相对速度矢量的增量Δv_UO作为待规划的变量。这样就形成了USV的避碰模型：

速度避障法解决的是局部实时避碰的问题，但容易陷入局部最小值，所以在对USV进行在线路径规划的时候，必须加入全局规划器。在基本LP中，各约束条件之间形成“与”的逻辑关系，对于具有复杂逻辑关系的优化问题不能直接求解。用MILP方法解决动态环境下的路径规划问题，可以很容易地把机器人本体动力学、运动学以及环境不确定性等约束考虑进去，并且能解决障碍物转向的“或”的逻辑问题，得到一条关于目标函数最优且满足所有约束条件的最优路径。由于MILP要求目标函数和约束均为线性，因此将这些约束和目标函数线性化。

1)第1类目标函数：距离收敛。

USV通过调整速度大小和方向，每规划一个时间间隔都使下一时刻USV离目标越来越近，即

2)第2类目标函数：速度最优。

将相对目标的速度分解为指向目标的分量V_C和垂直目标的分量V_T。要想使到达障碍物的速度最优，只需最大化V_C和最小化V_T即可。即

因此推理公式(3)转化为

综上所述可得线性规划器的目标函数:

1)加速度的约束：－Δ_max≤Δv_j≤Δ_max。

2)速度的约束： －V_max≤Δv_j+v_j≤V_max。

因为线性规划只有一个上下限，所以速度变量的上下限整合为

根据上一节对速度避障法的避碰策略可知：

式(5)确保在同一规划时间段[t_k,t_k+Δt]之内，有且仅有1组(前2个或后2个)约束成立。此处，为了遵守COLREGs规则，当右交叉相遇和对遇时，e_i=0,f_i=1,式(5)中前2不等式被激活，后2个不等式失效；当左交叉相遇时，e_i=1,f_i=0，前2个不等式失效，后2个不等式被激活。

总结上述体系结构、并融合COLREGs的在线路径规划流程图如图 2。具体的算法流程如下：

1)通过APAR雷达获得自身的经纬度和航向，通过AIS接收器获得对方船舶的距离和航向。

2)通过公式计算出每艘船舶与USV之间的最近会遇时间TCPA和最近会遇距离DCPA。

3)通过危险度判断模型判断是否处于危险状态。如果危险则开启避碰行为进行第4)步，否则不实施避碰行为。

4)进行船舶与USV的会遇局面划分及行为决策。

5)对本体运动学、动力学、探测范围、障碍物约束和目标函数进行建模。

6)将约束的建模加入MILP规划器中，由规划器可获得这个规划间隔时间里USV期望的转向角及速度。

7)将期望的航向角和速度输入到航行控制器中，通过航行控制器输出发动机的转速和舵角作用于USV实际模型中，使USV能达到期望的航向和航速。

8)检测是否达到目标，如果达到则结束，否则进行下一个规划周期，重复步骤1)~8)，直到目标到达才结束。

图 2 在线路径规划流程 Fig. 2 Flowchart of online path planning

图选项

为了说明此规划方法的有效性，采用实际的USV动力学模型进行实验仿真，采用的USV模型是Ribcraft模型^[10]，其回转速度为15.8 °/s，采用PID控制器对其进行航行控制。下面给出对遇局面和交叉局面的仿真实验结果，如图 3和4。图 3所示为对遇局面，其初始环境具体参数见表 1。图 4所示为右交叉局面，其初始环境具体参数见表 2。

仿真开发环境在MATLAB环境下。在仿真中用大圆代表USV，小圆代表运动障碍物，最上面的圆代表目标点。USV通过雷达进行探测，最大探测距离为30 nm。图 3 (a)和4(a)图表示初始状态，图 3 (b)和4(b)表示USV进行实时在线路径规划，每一个时间段规划出最优的速度矢量，图 3 (c)和4(c)表示最终到达目标点，图 3 (d)和4(d)表示在规划中的规划(期望)与USV实际的比较。图 3 (b)和4(b)中，USV在此种情况下右转向避碰，符合COLREGs规则的规定。通过圆圈稀疏可知当危险解除，规划时间变短，并符合COLREGs规定的从后方绕过障碍物的规定。

图 3 对遇局面态势 Fig. 3 Situation of head-on

图选项

图 4 交叉局面态势 Fig. 4 Situation of cross

图选项

本文通过对在线路径规划算法进行实验仿真可知，新算法在遵守COLREGs规则的同时，能获得较好的规划路径。然而本文并没有对最优化与COREGs规则存在的矛盾进行进一步讨论，因为在选择航向的过程中，最优化的结果有可能与COLREGs规则冲突，在这种情况下，需要引进一个选择算法，遵循优先级的高低来选择每种会遇局面最合适的规划。