无人船(unmanned surface vehicle, USV)作为一种海上无人移动平台, 其小体积、大负载、高机动和低成本的优势, 使得其可以在无人侦察、环境监测、水深绘图、海洋学研究等一系列海洋任务中发挥重要的作用。随着传感器、电子集成、计算机等相关技术的发展, 近十几年来无人船受到了越来越多的关注^[1-4]。然而, 当前无人船的自主化水平无法满足绝大部分的应用需求, 严重制约了无人船的应用和发展^[1]。

避碰路径规划是无人船导航、制导与控制(navigation, guidance and control, NGC)系统的重要部分。根据障碍物信息是否已知, 路径规划一般包括全局路径规划和局部路径规划。作为无人船研究的核心问题之一, 路径规划一定程度上代表了无人船自主化水平的程度, 尤其是在未知海洋环境下的局部避碰路径规划能力。在无人船局部路径规划方面, Soltan等^[5]提出了一种结合轨迹规划和实时跟踪控制的方法。耿以才等^[6]针对Dijkstra算法缺点, 结合无人船航行环境, 提出了一个动态网络环境下的实时路径模型, 并构造Dijkstra双桶算法, 设计实现了无人船实时最短路径算法。Tang等^[7]则提出了一种包含方向稳态模型和平动速度模型的局部反射避碰算法用于高速无人船。

速度障碍(velocity obstacles, VO)方法最初是一种用于机器人路径规划的方法, Fiorini和Shiller^[8]介绍了该算法的基本理论, 该方法及其扩展方法, 如概率速度障碍法^[9], 归一化速度障碍法^[10]等被广泛应用于机器人局部避碰路径规划应用, 在无人船局部路径规划方面也同样存在很多相应的研究和应用。例如, Kuwata等^[11]考虑COLREGs规则提出了基于速度障碍法的自主运动规划算法。Larson等^[12]利用速度障碍法确定安全的速度范围用于运动障碍物的避碰。这类算法通常同时改变无人船的航速和航向, 然而, 在某些特定的应用场合, 如环境监测、海底地形测绘等, 可能希望无人船保持一个稳定的航速, 一方面可使无人船推进系统在稳定经济航速下提高续航能力, 另一方面可以得到更稳定的测量数据。此时可通过转向控制实现无人船的避碰, 在绝大多数情况下基本都可以实现。

全局路径规划通常以关键位点构成规划路径进行导航, 为使得局部避碰路径规划可以更有效地与全局路径规划兼容, 提出基于位点导航的算法。有限状态机(finite-state machine, FSM)是一种表示有限个状态以及在这些状态之间的转移和动作等行为的数学模型, 具有离散、有限的特征, 在硬件电路设计和软件工程中广泛应用。将该思想应用于导航算法设计, 可使算法更易于理解、维护和后期扩展。因此, 本文基于FSM模型提出了一种以位点导航为主要状态的局部转向避碰路径规划算法, 并利用仿真实验进行了验证。

1 环境模型及危险评估模型

路径规划的主要任务是在有障碍物的环境内, 按照一定的评价标准, 寻找一条从起始状态到达目标状态的无碰撞路径^[13]。无人船局部路径规划基于环境模型实现, 不同的算法通常需要选择合适的环境模型。

1.1 环境模型构建 1.1.1 坐标系组

本文中建立了一组坐标系用于环境模型构建, 包括绝对坐标系OXY, 相对坐标系oxy以及极坐标系ox。OXY选择测地学直角坐标系, X和Y轴正方向分别为正北和正东向; oxy以实时无人船中心$\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} $为原点, 分别以无人船航向和右舷方向为X和Y轴正方向; ox与oxy是同一的, 即ox的原点、X轴均与oxy一致。三个坐标系下角度值均以X轴正方向为0, 并随顺时针方向增加。

1.1.2 等效障碍物

障碍物探测系统是无人船避碰系统的重要组成部分, 目前复杂海洋环境下准确的障碍物探测仍是无人船避碰技术的一大难点, 但随着传感器及相关探测技术的发展, 无人船障碍物探测能力也在不断提高。本文假设基于多传感器融合探测技术可以探测得到满足精度需求的障碍物特征, 包括障碍物相对无人船的运动速度和方向, 相对方位角β, 有效探测视角φ, 相对距离D_LOS等参数, 如图 1所示。此时可将障碍物近似等效为圆形障碍物, 其半径R_Obs为

$ {R_{{\text{Obs}}}} = {D_{{\text{LOS}}}}\sin \left( {\frac{\varphi }{2}} \right)/\left( {1-\sin \left( {\frac{\varphi }{2}} \right)} \right) $

(1)

若障碍物比较“狭长”, 则将其等效成多个相连或重叠的圆形障碍物。具体的障碍物探测技术实现不在本文讨论范围内。

1.1.3 速度变量

不同坐标系下, 速度变量的速度值相等, 方向值不完全一致。设oxy下无人船、障碍物及两者相互运动的速度变量分别记为$\left( {{\upsilon _{\text{U}}}, {\theta _{\text{U}}}} \right)$, $\left( {{\upsilon _{\text{O}}}, {\theta _{\text{O}}}} \right)$和$\left( {{\upsilon _{{\text{UO}}}}, {\theta _{{\text{UO}}}}} \right)$, 其中, ${\theta _{\text{U}}} = 0$。在ox下, 上述速度变量可分别表示为速度矢量${\mathit{\boldsymbol{V}}_{\text{U}}} = {\upsilon _{\text{U}}}{{\text{e}}^{{\text{i}}{\theta _{\text{U}}}}}$, ${\mathit{\boldsymbol{V}}_{\text{O}}} = {\upsilon _{\text{O}}}{{\text{e}}^{{\text{i}}{\theta _{\text{O}}}}}$和${\mathit{\boldsymbol{V}}_{{\text{UO}}}} = {\upsilon _{{\text{UO}}}}{{\text{e}}^{{\text{i}}{\theta _{{\text{UO}}}}}}$。在OXY下, 速度变量则分别记为$\left( {{\upsilon _{\text{U}}}, {\theta _{{\text{abs, U}}}}} \right)$, $\left( {{\upsilon _{\text{O}}}, {\theta _{{\text{abs, O}}}}} \right)$和$\left( {{\upsilon _{{\text{UO}}}}, {\theta _{{\text{abs, UO}}}}} \right)$, 其中, ${\theta _{{\text{abs}}, {\text{U}}}} = \psi $为无人船航向。设正常情况下任意角度值均在全区间$U = \left( {-\pi, \pi } \right]$范围内, 则${\theta _{{\text{abs, O}}}}$与${\theta _{\text{O}}}$以及${\theta _{{\text{abs}}, {\text{UO}}}}$与${\theta _{{\text{UO}}}}$分别满足${\theta _{{\text{abs}}, {\text{O}}}} = {F_{{\text{TriC}}}}\left( {{\theta _{\text{O}}} + \psi } \right)$, ${\theta _{{\text{abs}}, {\text{UO}}}} = {F_{{\text{TriC}}}}\left( {{\theta _{{\text{UO}}}} + \psi } \right)$。这里, F_TriC(θ)表示取以2π为周期的θ值对应的U范围内的值。

任意T_n采样时刻, 无人船航速ν_U和航向ψ可通过导航系统和电子罗盘获取, 相对运动速度ν_UO和方向θ_UO基于障碍物探测系统获取。由于${\mathit{\boldsymbol{V}}_{\text{O}}} = {\mathit{\boldsymbol{V}}_{\text{U}}}-{\mathit{\boldsymbol{V}}_{{\text{UO}}}}$, 故障碍物运动速度值ν_O和运动方向θ_O分别为

$ {\upsilon _{\text{O}}} = \sqrt {\upsilon _{\text{U}}^2 + \upsilon _{{\text{UO}}}^2 - 2{\upsilon _{\text{U}}}{\upsilon _{{\text{UO}}}}\cos {\theta _{{\text{UO}}}}} $

(2)

$ {\theta _{\text{O}}} = {F_{\text{atan} }}\left( {\text{Re} \left( {{\boldsymbol{V}_\text{O}}} \right), \text{Im} \left( {{\boldsymbol{V}_\text{O}}} \right)} \right) $

(3)

式中, 函数${F_{{\text{atan}}}}\left( {x, y} \right)$表示U范围内角度值y/x的反正切函数, 特别地, 当x=0, y>0时, 函数值取$\frac{\pi }{2}$; 当x=0, y < 0时, 函数值取$-\frac{\pi }{2}$; 当x=y=0时, 函数值取0。${\mathop{\rm Re}\nolimits} \left( \mathit{\boldsymbol{V}} \right)$和${\mathop{\rm Im}\nolimits} \left( \mathit{\boldsymbol{V}} \right)$分别表示速度矢量V的实部和虚部。

1.2 危险评估模型

通过危险评估对一个可能发生的碰撞进行及时地警告可以帮助无人船进行更有效的避碰操作。基于相关研究成果^[14-15]并考虑无人船系统受到的干扰, 本文提出了一个简单的危险评估模型。如图 2所示, 该模型利用障碍物边界B_O, 禁止边界B_P, 警告边界B_W和安全边界B_S共四个边界(均可用B_x或B_y边界表示)将障碍物周边划分为障碍区域S_O, 禁止区域S_P, 警告区域S_W, 安全区域S_S和无碰区域S_NoC。假设无人船和障碍物运动保持不变, 则可利用最近点(closest point of approach, CPA)方法确定无人船和障碍物之间的最近点。若CPA位于S_P内, 无人船会驶入S_P, 无人船几乎无法避免碰撞; 相似地, 若CPA位于S_W内, 无人船极易与障碍物发生碰撞, 但通过避碰操作基本可以避免碰撞; 若CPA位于S_S内, 无人船基本不会与障碍物发生碰撞, 但需要实时关注; 若CPA位于S_NoC内, 无人船可以确保航行安全。

设B_x和B_y边界半径分别为R_x和R_y, 半径差$\Delta {R_{x-y}}$可经验确定。由于${R_{\rm{O}}} = {R_{{\rm{Obs}}}} + {R_{{\rm{USV}}}}$, 其中, R_USV为无人船尺寸, 则R_P, R_W和R_S可依次确定。记从$\mathord{\buildrel{\lower3pt\hbox{$\scriptscriptstyle\frown$}} \over C} $出发并与边界B_x相切的两条射线分别为${l_{{\rm{L}}, \mathord{\buildrel{\lower3pt\hbox{$\scriptscriptstyle\frown$}} \over C}, x}}$和${l_{{\rm{R}}, \mathord{\buildrel{\lower3pt\hbox{$\scriptscriptstyle\frown$}} \over C}, x}}$, 两条射线夹角的一半为边界偏角$\Delta {\beta _x}$, 表示为

$ \Delta {\beta _x} = {\sin ^{-1}}\left( {\frac{{{R_x}}}{{{D_{{\rm{UO}}}}}}} \right), {R_x} < {D_{{\rm{UO}}}} $

(4)

其中, ${D_{{\rm{UO}}}} = {D_{{\rm{LOS}}}} + {R_{{\rm{Obs}}}}$表示$\mathord{\buildrel{\lower3pt\hbox{$\scriptscriptstyle\frown$}} \over C} $与障碍物中心之间的距离。在oxy下${l_{{\rm{L}}, \mathord{\buildrel{\lower3pt\hbox{$\scriptscriptstyle\frown$}} \over C}, x}}$和${l_{{\rm{R}}, \mathord{\buildrel{\lower3pt\hbox{$\scriptscriptstyle\frown$}} \over C}, x}}$之间的角度区间为边界区间${\phi _x}$。若不考虑无人船后侧的障碍物, 则${\phi _x}$可表示为

$ {\phi _x} = \left\{ {\left. {\left( {{\beta _{{\rm{L, }}x}}, {\beta _{{\rm{R, }}x}}} \right)} \right|{\beta _{{\rm{L, }}x}} = \beta-\Delta {\beta _x}, {\beta _{{\rm{R, }}x}} = \beta + \Delta {\beta _x}} \right\} $

(5)

满足${\beta _{{\rm{L, }}x}} \in \left( {-\pi, {\pi \mathord{\left/ {\vphantom {\pi 2}} \right. } 2}} \right)$, ${\beta _{{\rm{R, }}x}} \in \left( {-{\pi \mathord{\left/ {\vphantom {\pi 2}} \right. } 2}, \pi } \right)$。记障碍物中心到CPA点的距离为D_CPA, 则有${D_{{\rm{CPA}}}} = {D_{{\rm{UO}}}}\sin \left( {\left| {{\theta _{{\rm{UO}}}}-\beta } \right|} \right)$。若${\theta _{{\rm{UO}}}} \in {\phi _x}$, 则满足${D_{{\rm{CPA}}}} \le {R_x}$。因此, 为保证无人船能够尽可能安全地避免与障碍物发生碰撞, 当${R_{\rm{W}}} < {D_{{\rm{CPA}}}} \le {R_{\rm{S}}}$时可以考虑执行避碰操作, 当${D_{{\rm{CPA}}}} \le {R_{\rm{W}}}$时则必须执行避碰操作。

2 基于FSM的局部路径规划算法 2.1 可航行角约束

设T_n时刻无人船与障碍物存在相对运动, 保持oxy不变并将无人船航向转向$\Delta \theta $角度, 转向后无人船速度变量记为$\left( {{\upsilon _{{\rm{U}}\_\Delta \theta }}, {\theta _{{\rm{U\_}}\Delta \theta }}} \right)$或简记为$\left( {{\upsilon _{\rm{U}}}, \Delta \theta } \right)$, 相对运动速度变量记为$\left( {{\upsilon _{{\rm{UO}}\_\Delta \theta }}, {\theta _{{\rm{UO\_}}\Delta \theta }}} \right)$。若${\theta _{{\rm{U\_}}\Delta \theta }}$使得${\theta _{{\rm{UO\_}}\Delta \theta }} \notin {\phi _x}$成立, 则称${\theta _{{\rm{U\_}}\Delta \theta }}$为边界B_x约束下的可航行角, 否则称其为不可航行角。所有可航行角构成可航行角区间${\mathit{\Psi} _{{\rm{A, }}x}}$, U内其余角度区间称为不可航行角区间${\mathit{\Psi} _{{\rm{N, }}x}}$, 即有

$ {\mathit{\Psi} _{{\rm{N, }}x}} = \left\{ {\left. {{\theta _{{\rm{U\_}}\Delta \theta }}} \right|{\theta _{{\rm{U\_}}\Delta \theta }} = \Delta \theta {\rm{, }}{\theta _{{\rm{UO\_}}\Delta \theta }} \in {\phi _x}} \right\} $

(6)

$ {\mathit{\Psi} _{{\text{A}}, x}} = {\complement _U}{\mathit{\Psi} _{{\text{N}}, x}} $

(7)

在OXY下可航行角和不可航行角区间分别记为${\mathit{\Psi} _{{\text{abs}}, {\text{A}}, x}}$和${\mathit{\Psi} _{{\text{abs}}, {\text{N}}, x}}$, 满足

$ {\mathit{\Psi} _{{\rm{abs, N, }}x}} = \left\{ {\left. {{\theta _{{\rm{abs, U\_}}\Delta \theta }}} \right|{\theta _{{\rm{abs, U\_}}\Delta \theta }} = {F_{{\rm{TriC}}}}({\theta _{{\rm{U\_}}\Delta \theta }} + \psi ), {\theta _{{\rm{U\_}}\Delta \theta }} \in {\mathit{\Psi} _{{\rm{N, }}x}}} \right\} $

(8)

$ {\mathit{\Psi} _{{\rm{abs, A, }}x}} = {\complement _U}{\mathit{\Psi} _{{\rm{abs, N, }}x}} $

(9)

同样保持$ox$不变且无人船转向$\Delta \theta$角度, 无人船速度变量和相对速度变量可分别表示为${\boldsymbol{V}_{\text{U}\_\Delta \theta }} = {\upsilon _{\rm{U}}}{\text{e}^{\text{i}\Delta \theta }}$和${\boldsymbol{V}_{\text{UO}\_\Delta \theta }} = {\upsilon _{{\rm{UO\_}}\Delta \theta }}{\text{e}^{\text{i}{\theta _{{\rm{UO\_}}\Delta \theta }}}}$, 且满足${\boldsymbol{V}_{\text{UO}\_\Delta \theta }} = {\boldsymbol{V}_{\text{U}\_\Delta \theta }} - {\boldsymbol{V}_\text{O}}$, 故${\theta _{{\rm{UO\_}}\Delta \theta }}$为

$ {\theta _{{\rm{UO\_}}\Delta \theta }} = {F_{\text{atan}}}\left( {\operatorname{Re} \left( {{\boldsymbol{V}_{\text{UO}\_\Delta \theta }}} \right), \operatorname{Im} \left( {{\boldsymbol{V}_{\text{UO}\_\Delta \theta }}} \right)} \right) $

(10)

记$\delta = {{{\upsilon _{\rm{O}}}} \mathord{\left/ {\vphantom {{{\upsilon _{\rm{O}}}} {{\upsilon _{\rm{U}}}}}} \right. } {{\upsilon _{\rm{U}}}}}$, 进一步分析(10)式易知, $\Delta \theta$与${\theta _{{\rm{UO\_}}\Delta \theta }}$满足

$ \sin (\Delta \theta - {\theta _{{\rm{UO\_}}\Delta \theta }}) = \delta \sin ({\theta _{\rm{O}}} - {\theta _{{\rm{UO\_}}\Delta \theta }}) $

(11)

随着$\Delta \theta $的连续变化, ${\mathit{\boldsymbol{V}}_{{\text{UO}}\_\Delta \theta }}$矢端曲线形成图 3所示的圆形轨迹。基于矢端曲线与射线${l_{{\text{L}}, \overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C}, x}}$和${l_{{\text{R}}, \overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C}, x}}$的分布关系可将其划分为6大类情形^[16], 通过无人船和障碍物运动信息及给定的${\theta _{{\text{UO}}\_\Delta \theta }}$可直接获得情形分类条件, 并基于(10)式—(11)式进一步确定${\mathit{\Psi} _{{\text{N}}, x}}$和${\mathit{\Psi} _{{\text{A}}, x}}$。${\mathit{\Psi} _{{\text{N}}, x}}$即为B_x边界条件下的可航行角约束。不同情形下具体的矢端曲线与两条射线关系图例可见文献[16], 图 4列举了其中两种典型情形下的图例。

记$\Delta \theta $的周期延拓值为$\Delta {\theta _{\rm{e}}} = \Delta \theta + 2k\pi, k = \pm 1$, 间接量$\varsigma = \delta \sin ({\theta _{\rm{O}}} - {\theta _{{\rm{UO\_\Delta }}\theta }})$。若$\varsigma \leqslant 1$, 则由(11)式可得$\Delta {\theta _{\rm{e}}}$的可能解为$\Delta {\theta _{{\rm{e, }}i}} = {F_{\Delta {\theta _{{\rm{e}}, i}}}}({\theta _{{\rm{UO\_}}\Delta \theta }}, \varsigma ), i = 1, 2$, 其中

$ {F_{\Delta {\theta _{{\rm{e}}, 1}}}}(\theta, \varsigma ) = \theta + {\sin ^{ - 1}}\varsigma $

(12)

$ {F_{\Delta {\theta _{{\rm{e}}, 2}}}}(\theta, \varsigma ) = \left\{ {\begin{array}{*{20}{l}} {\theta - {{\sin }^{ - 1}}\varsigma + \pi {\rm{, }}\varsigma \geqslant 0} \\ {\theta - {{\sin }^{ - 1}}\varsigma - \pi {\rm{, }}\varsigma < 0} \end{array}} \right. $

(13)

对于B_x边界, 若给定${\theta _{{\rm{UO\_}}\Delta \theta }} = {\beta _{{\rm{L, }}x}}$, 设$\Delta {\theta _{{{\rm{L}}_i}, x}}, i =\;\;1, 2$为(11)式可能的两个解, $\Delta {\theta _{{\rm{e, }}{{\rm{L}}_i}, x}} = \Delta {\theta _{{{\rm{L}}_i}, x}} + 2k\pi$为其周期延拓值, 间接量为${\varsigma _{{\rm{L, }}x}} = \delta \sin ({\theta _{\rm{O}}} - {\beta _{{\rm{L}}, x}})$。若$\Delta {\theta _{{{\rm{L}}_i}, x}}$存在, 则

$ \Delta {\theta _{{\text{e, }}{{\text{L}}_i}, x}} = {F_{\Delta {\theta _{{\text{e}}, i}}}}({\beta _{{\text{L}}, x}}, {\varsigma _{{\text{L, }}x}}), {\text{ }}i = 1{\text{ }}或{\text{ 2}} $

(14)

$ \Delta {\theta _{{{\text{L}}_i}, x}} = {F_{{\text{TriC}}}}(\Delta {\theta _{{\text{e, }}{{\text{L}}_i}, x}}), {\text{ }}i = 1{\text{ }}或{\text{ 2}} $

(15)

同样, 给定${\theta _{{\rm{UO\_}}\Delta \theta }} = {\beta _{{\rm{R, }}x}}$, 定义$\Delta {\theta _{{{\rm{R}}_i}, x}}$和$\Delta {\theta _{{\rm{e, }}{{\rm{R}}_i}, x}}$, 间接量${\varsigma _{{\rm{R, }}x}} = \delta \sin ({\theta _{\rm{O}}} - {\beta _{{\rm{R}}, x}})$, 若$\Delta {\theta _{{{\rm{R}}_i}, x}}$存在, 则

$ \Delta {\theta _{{\text{e, }}{{\text{R}}_i}, x}} = {F_{\Delta {\theta _{{\text{e}}, i}}}}({\beta _{{\text{R}}, x}}, {\varsigma _{{\text{R, }}x}}), {\text{ }}i = 1{\text{ }}或{\text{ 2}} $

(16)

$ \Delta {\theta _{{{\text{R}}_i}, x}} = {F_{{\text{TriC}}}}(\Delta {\theta _{{\text{e, }}{{\text{R}}_i}, x}}), {\text{ }}i = 1{\text{ }}或{\text{ 2}} $

(17)

基于上述变量, 不考虑无人船后侧障碍物, 可得6大类情形的分类条件及结果如下:

(1) 对于情形一, 分类条件为

$ \delta \leqslant 1 $

(18)

分别计算$\Delta {\theta _{{{\rm{L}}_1}, x}}$和$\Delta {\theta _{{{\rm{R}}_1}, x}}$值, 记${\kappa _1} = \min \left( {\Delta {\theta _{{{\rm{L}}_1}, x}}, \Delta {\theta _{{{\rm{R}}_1}, x}}} \right)$, ${\kappa _2} = \max \left( {\Delta {\theta _{{{\rm{L}}_1}, x}}, \Delta {\theta _{{{\rm{R}}_1}, x}}} \right)$, 并设置临时变量${\theta _{{\rm{t1}}}}$为

$ {\theta _{{\text{t1}}}} = {\left. {{F_{\text{atan}}}\left( {\operatorname{Re} \left( {{\boldsymbol{V}_{\text{UO}\_\Delta \theta }}} \right), \operatorname{Im} \left( {{\boldsymbol{V}_{\text{UO}\_\Delta \theta }}} \right)} \right)} \right|_{\Delta \theta = \frac{{{\kappa _1} + {\kappa _2}}}{2}}} $

(19)

则不可航行角区间${\mathit{\Psi} _{{\rm{N}}, x}}$为

$ {\mathit{\Psi} _{{\text{N}}, x}} = \left\{ {\begin{array}{*{20}{l}} {\left\{ {\left. \theta \right|\theta \in \left( {{\kappa _1}, {\kappa _2}} \right)} \right\}\;\;\;\;\;\;\;\;\;\;, {\text{ }}{\theta _{{\text{t1}}}} \in {\phi _x}} \\ {\left\{ {\left. \theta \right|\theta \in \left( {-{\pi}, {\kappa _1}} \right) \cup \left( {\left. {{\kappa _2}, {\pi}} \right]} \right.} \right\}, {\text{ 其他}}} \end{array}} \right. $

(20)

(2) 对于情形二和情形三, 基础条件为

$ \delta > 1, \left| {{\varsigma _{{\rm{L, }}x}}} \right| \geqslant 1, \left| {{\varsigma _{{\rm{R, }}x}}} \right| \geqslant 1 $

(21)

为进一步区分两者, 记临时变量${\theta _{{\rm{t2}}}}$为

$ {\theta _{{\rm{t2}}}} = {\left. {{F_{\text{atan}}}\left( {\operatorname{Re} \left( {{\boldsymbol{V}_{\text{UO}\_\Delta \theta }}} \right), \operatorname{Im} \left( {{\boldsymbol{V}_{\text{UO}\_\Delta \theta }}} \right)} \right)} \right|_{\Delta \theta = \frac{{\rm{ \mathsf{ π} }}}{2}}} $

(22)

若进一步满足条件

$ \min \left( {{\theta _{{\text{UO}}}}, {\theta _{{\text{t2}}}}} \right) < {\beta _{{\text{L}}, x}}{\text{ }}{\text{ }}或\max \left( {{\theta _{{\text{UO}}}}{\text{, }}{\theta _{{\text{t2}}}}} \right) > {\beta _{{\text{R}}, x}} $

(23)

则此时为情形二, ${\mathit{\Psi} _{{\rm{N}}, x}}$为

$ {\mathit{\Psi} _{{\rm{N, }}x}} = \emptyset $

(24)

否则为情形三, ${\mathit{\Psi} _{{\rm{N}}, x}}$为

$ {\mathit{\Psi} _{{\rm{N, }}x}} = U $

(25)

(3) 对于情形四, 分类条件为

$ \delta > 1, \left| {{\varsigma _{{\rm{L, }}x}}} \right| < 1, \left| {{\varsigma _{{\rm{R, }}x}}} \right| \geqslant 1 $

(26)

计算$\Delta {\theta _{{{\rm{L}}_i}, x}}, i = 1, 2$值, 记${\kappa _1} = \min \left( {\Delta {\theta _{{{\rm{L}}_1}, x}}, \Delta {\theta _{{{\rm{L}}_2}, x}}} \right)$, ${\kappa _2} = \max \left( {\Delta {\theta _{{{\rm{L}}_1}, x}}, \Delta {\theta _{{{\rm{L}}_2}, x}}} \right)$, 并设置(19)式所示的临时变量${\theta _{{\rm{t1}}}}$, 此时${\mathit{\Psi} _{{\rm{N}}, x}}$为

$ {\mathit{\Psi} _{{\text{N}}, x}} = \left\{ {\begin{array}{*{20}{l}} {\left\{ {\left. \theta \right|\theta \in \left( {{\kappa _1}, {\kappa _2}} \right)} \right\}\;\;\;\;\;\;\;\;\;\;, {\text{ }}{\theta _{{\text{t1}}}} \in {\phi _x}} \\ {\left\{ {\left. \theta \right|\theta \in \left( {-{\pi}, {\kappa _1}} \right) \cup \left( {\left. {{\kappa _2}, {\pi}} \right]} \right.} \right\}, {\text{其他}}} \end{array}} \right. $

(27)

(4) 对于情形五, 分类条件为

$ \delta > 1, \left| {{\varsigma _{{\rm{L, }}x}}} \right| \geqslant 1, \left| {{\varsigma _{{\rm{R, }}x}}} \right| < 1 $

(28)

计算$\Delta {\theta _{{{\rm{R}}_i}, x}}, i = 1, 2$, 记${\kappa _1} = \min \left( {\Delta {\theta _{{{\rm{R}}_1}, x}}, \Delta {\theta _{{{\rm{R}}_2}, x}}} \right)$, ${\kappa _2} = \max \left( {\Delta {\theta _{{{\rm{R}}_1}, x}}, \Delta {\theta _{{{\rm{R}}_2}, x}}} \right)$, 并设置(19)式所示的临时变量${\theta _{{\rm{t1}}}}$, 此时${\mathit{\Psi} _{{\rm{N}}, x}}$同样可由(27)式确定。

(5) 对于情形六, 分类条件为

$ \delta > 1, \left| {{\varsigma _{{\rm{L, }}x}}} \right| < 1, \left| {{\varsigma _{{\rm{R, }}x}}} \right| < 1 $

(29)

计算$\Delta {\theta _{{{\rm{L}}_i}, x}}, i = 1, 2$和$\Delta {\theta _{{{\rm{R}}_i}, x}}, i = 1, 2$值后排序, 并记为${\kappa _1} < {\kappa _2} < {\kappa _3} < {\kappa _4}$。设临时变量${\theta _{{\rm{t3}}}}$为

$ {\theta _{{\rm{t3}}}} = {\left. {{F_{\text{atan}}}\left( {\operatorname{Re} \left( {{\boldsymbol{V}_{\text{UO}\_\Delta \theta }}} \right), \operatorname{Im} \left( {{\boldsymbol{V}_{\text{UO}\_\Delta \theta }}} \right)} \right)} \right|_{\Delta \theta = \frac{{{\kappa _2} + {\kappa _3}}}{2}}} $

(30)

此时${\mathit{\Psi} _{{\rm{N}}, x}}$为

$ {\mathit{\Psi} _{{\text{N, }}x}} = \left\{ {\begin{array}{*{20}{l}} {\left\{ {\left. \theta \right|\theta \in \left( {-{\pi}, {\kappa _1}} \right) \cup \left( {{\kappa _2}, {\kappa _3}} \right) \cup \left( {\left. {{\kappa _4}, {\pi}} \right]} \right.} \right\}, {\theta _{{\text{t3}}}} \in {\phi _x}} \\ {\left\{ {\left. \theta \right|\theta \in \left( {{\kappa _1}, {\kappa _2}} \right) \cup \left( {{\kappa _3}, {\kappa _4}} \right)} \right\}\;\;\;\;\;\;\;\;\;\;\;\;\;, 其他} \end{array}} \right. $

(31)

当存在多个障碍物时, 记等效障碍物数量为N, 第i个障碍物对应的可航行角和不可航行角区间分别为$\mathit{\Psi} _{{\rm{A, }}x}^i$和$\mathit{\Psi} _{{\rm{N, }}x}^i$, 则${\mathit{\Psi} _{{\rm{N}}, x}}$可表示为

$ {\mathit{\Psi} _{{\rm{N}}, x}} = \left\{ {\theta \left| {\theta \in \bigcup\limits_{i = 1}^N {\mathit{\Psi} _{{\rm{N}}, x}^i} } \right.} \right\} $

(32)

相应的可航行区间${\mathit{\Psi} _{{\rm{A}}, x}}$可由(7)式确定。

2.2 位点导航模型

位点导航是当前无人船导航控制中最成熟的技术之一。将位点导航模型结合到局部避碰路径规划可有效简化无人船控制跟踪器的设计, 提高无人船的实用性。

2.2.1 最优导航角

在${\mathit{\Psi} _{{\rm{N}}, x}}$及无人船航向动态窗口^[17]约束下, 考虑将关于目标角度和障碍物方位角偏转度作为最优目标函数^[7]。考虑多目标约束优化问题, 定义最优目标函数${F_{\text{opt}}}\left( \theta \right)$为

$ {F_{\text{opt}}}\left( \theta \right) = {\omega _1}{f_{\rm{G}}}\left( \theta \right) + {\omega _2}{f_{{\rm{safe}}}}\left( \theta \right) $

(33)

式中, ${\omega _1}$和${\omega _2}$为权重系数, 满足${\omega _1} + {\omega _2} = 1$。目标和安全角度偏差度${f_{\rm{G}}}\left( \theta \right)$和${f_{{\rm{safe}}}}\left( \theta \right)$为

$ {f_{\rm{G}}}\left( \theta \right) = 1 - \frac{{\left| {{\theta _{\rm{G}}} - \theta } \right|}}{\pi } $

(34)

$ {f_{{\rm{safe}}}}\left( \theta \right) = \frac{1}{{N'}}\sum\limits_{j = 1}^{N'} {\frac{{\left| {\theta - \theta _{{\mathit{\Psi} _{{\rm{N}}, x}}}^j} \right|}}{\pi }} $

(35)

式中, ${\theta _{\rm{G}}}$为oxy下的目标位点期望角。N'为有效不可航行角区间数量。考虑角区间连续性, 对于某一障碍物可能出现包含单个或两个独立的连续不可航行角区间的常规情形以及U区间为可航行或不可航行角区间的极端情形。本文暂不考虑上述两种极端情形(前者可忽略, 后者需采取紧急措施, 正常避碰过程中很少出现), 分别设两类常规情形中的等效障碍物数量为N₁和N₂, 则$N' = {N_1} + 2{N_2}$。$\theta _{{\mathit{\Psi} _{{\rm{N}}, x}}}^j$为独立不可航行角区间中心角。

在约束条件下最大化${F_{\rm{opt}}}\left( \theta \right)$, 即可得到实时最优导航角${\theta _{{\rm{opt}}}}$。

2.2.2 目标位点及缓冲导航位点

任务开始后建立OXY, 并设OXY下目标位点P_G的坐标为$\left( {{x_{{\rm{abs, }}{P_{\rm{G}}}}}, {y_{{\rm{abs, }}{P_{\rm{G}}}}}} \right)$, 无人船中心$\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} $的位置为$\left( {{x_{{\rm{abs, }}\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} }}, {y_{{\rm{abs, }}\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} }}} \right)$, 则无人船实时目标位点期望角${\theta _{{\rm{abs, G}}}}$为

$ {\theta _{{\rm{abs, G}}}} = {F_{{\rm{atan}}}}\left( {{x_{{\rm{abs, }}{P_{\rm{G}}}}} - {x_{{\rm{abs, }}\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} }}, {y_{{\rm{abs, }}{P_{\rm{G}}}}} - {y_{{\rm{abs, }}\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} }}} \right) $

(36)

在oxy下的实时目标位点期望角${\theta _{\rm{G}}}$为

$ {\theta _{\rm{G}}} = {F_{{\rm{TriC}}}}\left( {{\theta _{{\rm{abs, G}}}} - \psi } \right) $

(37)

$\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C}$到${P_{\rm{G}}}$的距离${D_{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} {\rm{, }}{P_{\rm{G}}}}}$可根据距离公式计算。

当无人船无法直接向P_G点导航时, 由于${\theta _{{\rm{opt}}}}$基于oxy并随无人船位置和状态实时变化, 无法直接用于导航^[7], 故通过增加临时位点进行间接导航的方式设计缓冲位点导航模型。设T_n时刻设置缓冲位点${P_{{\rm{t, }}i}}, i = 1, 2, \cdots$为导航位点, 在oxy下最优导航角记为$\theta _{{\rm{opt}}}^{{T_n}}$, 在OXY下$\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C}$坐标为$\left( {x_{{\rm{abs, }}\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} }^{{T_n}}, y_{{\rm{abs, }}\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} }^{{T_n}}} \right)$, 其与${P_{{\rm{t, }}i}}$的距离为$D_{{P_{{\rm{t, }}i}}}^{{T_n}}$, 则${P_{{\rm{t, }}i}}$在OXY下的坐标$\left( {{x_{{\rm{abs, }}{{\rm{P}}_{{\rm{t}}, i}}}}, {y_{{\rm{abs, }}{{\rm{P}}_{{\rm{t}}, i}}}}} \right)$为

$ \left\{ {\begin{array}{*{20}{l}} {{x_{{\rm{abs, }}{{\rm{P}}_{{\rm{t}}, i}}}} = x_{{\rm{abs, }}\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} }^{{T_n}} + D_{{{\rm{P}}_{{\rm{t}}, i}}}^{{T_n}}\cos \left( {\theta _{{\rm{opt}}}^{{T_n}} + {\theta _{{\rm{abs, U}}}}} \right)} \\ {{y_{{\rm{abs, }}{{\rm{P}}_{{\rm{t}}, i}}}} = y_{{\rm{abs, }}\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} }^{{T_n}} + D_{{{\rm{P}}_{{\rm{t}}, i}}}^{{T_n}}\sin \left( {\theta _{{\rm{opt}}}^{{T_n}} + {\theta _{{\rm{abs, U}}}}} \right)} \end{array}} \right. $

(38)

相似地, 可以计算OXY和oxy下的实时缓冲位点期望角${\theta _{{\rm{abs, N}}}}$和${\theta _{\rm{N}}}$以及无人船到${P_{{\text{t}}, i}}$的实时距离${D_{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} {\rm{, }}{P_{{\rm{t, }}i}}}}$。基于成熟的位点导航算法, 无人船可根据避碰需求选择P_G或${P_{{\text{t}}, i}}$进行避碰和导航。

2.3 局部避碰路径规划算法 2.3.1 FSM基本要素及运行流程

本文中设计的FSM包含现态、条件、动作和次态4个要素。现态即指当前所处状态, 如位点导航状态; 条件用于判定下一周期迁移的状态, 包括同一状态的自迁移或迁移到新的状态; 动作主要指相关的导航操作, 非必需; 次态相对现态而言, 即为下一状态, 可能为原状态或新状态, 一旦被激活就转为现态。当确定有限的状态以及执行动作和条件后, FSM从初始状态开始执行动作, 并根据条件不断进行状态迁移直至进入最终态。FSM虽然为有限状态, 但只要未进入最终状态即可一直执行。该思想与无人船局部避碰导航过程非常契合。

2.3.2 基本导航策略

根据无人船自身运动状态、障碍物分布和运动情况等, 提出适用于无人船局部避碰的基本导航策略。

记到达位点的距离阈值为D_Th, 无人船航向与目标位点期望角所组成的角度区间为${\mathit{\Psi} _{\rm{G}}}$, 在oxy下其值为

$ {\mathit{\Psi} _{\rm{G}}} = \left\{ {\begin{array}{*{20}{l}} {\left[{0, {\theta _{\rm{G}}}} \right], {\theta _{\rm{G}}} \geqslant 0} \\ {\left[{{\theta _{\rm{G}}}, 0} \right], {\theta _{\rm{G}}} < 0} \end{array}} \right. $

(39)

实时θ_opt与θ_G或θ_N的角度偏差阈值为θ_Th, θ_opt和θ_N与θ_G的角度偏差分别为Δθ_opt-G和Δθ_N-G。基本导航策略如下:

(1) 当${D_{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} {\rm{, }}{P_{\rm{G}}}}} \leqslant {D_{{\rm{Th}}}}$时, 即认为无人船到达${P_{\rm{G}}}$。否则考虑策略(2);

(2) 若${\mathit{\Psi} _{\rm{G}}}$内无不可航行角区间, 即${\mathit{\Psi} _{\rm{G}}} \cap {\mathit{\Psi} _{{\rm{N}}, {\rm{S}}}} = $ $\emptyset $, 则无人船以${P_{\rm{G}}}$作为导航位点。否则考虑策略(3);

(3) 无人船向${P_{{\rm{t, }}i}}$导航, 若到达${P_{{\rm{t, }}i}}$, 即满足${D_{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} {\rm{, }}{P_{{\rm{t, }}i}}}} \leqslant {D_{{\rm{Th}}}}$, 则增加${P_{{\rm{t, }}i + 1}}$作为导航位点。否则考虑策略(4);

(4) 无人船向${P_{{\rm{t, }}i}}$导航, 若实时${\theta _{{\rm{opt}}}}$与${\theta _{\rm{N}}}$的角度偏差超过阈值${\theta _{{\rm{Th}}}}$, 即$\left| {{F_{{\rm{TriC}}}}\left( {{\theta _{{\rm{opt}}}} - {\theta _{\rm{N}}}} \right)} \right| > {\theta _{{\rm{Th}}}}$, 则增加${P_{{\rm{t, }}i + 1}}$作为导航位点。否则考虑策略(5);

(5) 无人船向${P_{{\rm{t, }}i}}$导航, 若$\Delta {\theta _{{\rm{N - G}}}} > \Delta {\theta _{{\rm{opt - G}}}}$, 即$\left| {{F_{{\rm{TriC}}}}\left( {{\theta _{\rm{N}}} - {\theta _{\rm{G}}}} \right)} \right| > \left| {{F_{{\rm{TriC}}}}\left( {{\theta _{{\rm{opt}}}} - {\theta _{\rm{G}}}} \right)} \right|$, 则增加${P_{{\rm{t, }}i + 1}}$作为导航位点。否则考虑策略(6);

(6) 保持导航位点不变。

2.3.3 局部避碰路径规划FSM设计

分析提出的基本策略易知导航状态及其动作主要包括: (1)${P_{\rm{G}}}$导航状态${S_1}$, 无人船以${P_{\rm{G}}}$为导航位点进行导航; (2)${P_{{\rm{t, }}i}}$导航状态${S_2}$, 无人船以${P_{{\rm{t, }}i}}$为导航位点进行导航; (3)更新${P_{\rm{G}}}$状态${S_3}$, 导航位点由${P_{{\rm{t, }}i}}$更新为${P_{\rm{G}}}$; (4)更新${P_{{\rm{t, }}i + 1}}$状态${S_4}$, 导航位点由${P_{{\rm{t, }}i}}$或${P_{\rm{G}}}$更新为${P_{{\rm{t, }}i + 1}}$。此外, 设置开始状态${S_0}$, 初始化相关参数; 终止状态${S_5}$, 无人船导航完成。记${S_i} \to {S_j}$表示状态${S_i}$向${S_j}$迁移, 基础条件${C_{{{\rm{B}}_i}}}$组成状态迁移条件${C_{i \to j}}$, 分别如表 1和表 2所示。基于上述设计的位点导航状态及相应的动作和条件, 即可设计图 5所示的FSM用于实现局部避碰路径规划。

表 2中, 对于任意${C_{{{\rm{B}}_i}}}, i = 1, \cdots, 5$, 若不满足, 则以${\bar C_{{{\rm{B}}_i}}}$表示。

3 仿真结果与分析

针对提出的局部转向避碰算法, 基于Matlab进行了仿真验证。主要仿真条件如下:起始位点坐标为(0 m, 0 m), 目标位点坐标为(950 m, 930 m), 无人船航速6 m/s, 初始航向15°, 尺寸8 m; 等效静态障碍物在[50 m, 950 m]中心位置和[5 m, 55 m]有效半径范围内随机生成, 数量为8个; 等效动态障碍物在[4 m, 29 m]有效半径和[1 m/s, 10 m/s]运动速度范围内随机生成, 数量为3个。障碍物探测半径基准为300 m, 针对不同的障碍物设置40 m的随机波动。危险评估模型半径差$\Delta {R_{{\text{O}}-{\text{P}}}} = 5\;{\text{m}}$, $\Delta {R_{{\text{P}}-{\text{W}}}} = 10\;{\text{m}}$, $\Delta {R_{{\text{W}}-{\text{S}}}} = 10\;{\text{m}}$。无人船最大角速度为10 °/s, 最大角加速度为5 °/s²。位点更新时临时缓冲位点插入距离$D_{{P_{{\rm{t, }}i}}}^{{T_n}}$为200 m(小于探测范围)。假设障碍物未进入无人船探测范围内均不可见, 且无人船具有快速稳定的位点导航能力, 仿真得到如图 6所示的结果。

在图 6中, T₁时刻无人船开始导航, 由于没有探测到障碍物, 直接向目标位点导航, 轨迹点为青色。图中同心圆表示静态障碍物, 分别标号O_s1— O_s8, 红色和黄色圆分别表示B_W和B_S边界。随着无人船的航行, T₂时刻探测到从东南向西北方向运动的障碍物O_d1, 由于可能发生碰撞, 增加缓冲位点P_{t, k1}, 无人船向P_{t, k1}导航, 轨迹点为蓝色, 最后无人船从O_d1后方驶过, 如图 6中T₃时刻子图所示。此时无人船仍未探测到其他障碍物, 继续向目标位点导航, 直到部分静态障碍物进入探测范围, 障碍物B_S边界设置为绿色, 设置缓冲位点P_{t, k2}, 无人船向P_{t, k2}导航, 如图 6中T₄时刻子图所示。无人船继续航行, 在T₅时刻探测到从西北向东南方向运动的障碍物O_d2, 继续更新缓冲位点P_{t, k3}, 无人船从O_d2后方驶过, 如图 6中T₆时刻子图所示。此后, 无人船探测到东南向西北方向运动的障碍物O_d3, 此时无人船导航主要受静态障碍物影响, 更新缓冲位点P_{t, k4}, 无人船从O_d3前方驶过, 如图 6中T₇时刻子图所示。最后, 无人船绕过静态障碍物后对目标位点“可视”, 直接向目标位点导航, 直至到达目标位点, 如图 6中T₈时刻子图所示。仿真结果表明了所提出方法的有效性。

4 讨论与结论

针对近海多礁石、渔船等障碍物的复杂环境下的环境监测等应用, 本文提出一种基于有限状态机模型的局部转向避碰路径规划算法。考虑经济航速下的续航能力及测量数据的稳定性在某些特定应用的需求, 本算法以转向控制为主, 通过划分以向目标位点或缓冲位点导航为主的有限状态设计FSM, 一方面将局部路径规划与以位点序列组成的全局路径无缝结合, 另一方面算法包含FSM易于理解、维护和扩展的优点。仿真结果表明所提出的方法具有可行性和实用性。

同样, 本算法也存在不足及改进的空间。例如, 最优导航角θ_opt的结果主要由目标位点P_G方向上的障碍物所决定, 特别当无人船和障碍物中心与P_G近似处于一条直线时, 导航过程中无人船航向的扰动可能导致θ_opt关于障碍物左右两侧振荡; 本算法部分参数通过经验确定, 虽然通过参数设置可提高避碰安全性, 但通常无法保证全局最优。基于大量实际航行数据的分析可对参数进行优化; 当某个区域障碍物较密集时, 可能会出现较大转向的路径, 出现“绕远路”的情况, 若引入转向时机机制则可在一定程度上解决该问题。另外, 对算法进行国际海上避碰规则的约束也可以进一步提高安全性。