海洋科学  2018, Vol. 42 Issue (1): 119-127   PDF    
http://dx.doi.org/10.11759/hykx20171011010

文章信息

汪栋, 张杰, 金久才, 毛兴鹏. 2018.
WANG Dong, ZHANG Jie, JIN Jiu-cai, MAO Xing-peng. 2018.
一种基于有限状态机模型的局部转向避碰路径规划算法
Local steering collision avoidance path planning algorithm based on finite state machine model
海洋科学, 42(1): 119-127
Marina Sciences, 42(1): 119-127.
http://dx.doi.org/10.11759/hykx20171011010

文章历史

收稿日期:2017-10-11
修回日期:2017-12-22
一种基于有限状态机模型的局部转向避碰路径规划算法
汪栋1,2, 张杰1,2, 金久才2, 毛兴鹏1     
1. 哈尔滨工业大学 电子与信息工程学院, 黑龙江 哈尔滨 150006;
2. 国家海洋局第一海洋研究所, 山东 青岛 266061
摘要:针对多礁石、渔船等障碍物的近海复杂环境下的一些应用, 提出了一种基于有限状态机(finite-state machine, FSM)模型的无人船(unmanned surface vehicle, USV)局部转向避碰路径规划算法。首先, 基于速度障碍法和障碍物区域分层方法, 获取无人船固定航速条件下的航向角约束解析结果。然后, 基于该约束条件及障碍物探测情况设计FSM的有限状态及执行动作和状态迁移条件, 其中, 通过转向控制实现向目标位点或缓冲位点进行导航的状态为FSM的2个重要状态。最终通过FSM的执行实现局部转向避碰路径规划。仿真结果表明提出的多障碍物避碰算法具有可行性和实用性。该方法易于改进和扩展, 且容易与当前主流的无人船控制系统结合, 有利于无人船避碰系统快速工程化的实现。
关键词无人船    局部避碰    路径规划    位点导航    有限状态机    
Local steering collision avoidance path planning algorithm based on finite state machine model
WANG Dong1,2, ZHANG Jie1,2, JIN Jiu-cai2, MAO Xing-peng1     
1. School of Electronics and Information Engineering, Harbin Institute of Technology, Harbin 150006, China;
2. First Institute of Oceanography, State Oceanic Administration, Qingdao 266061, China
Abstract: In this paper, we propose a local collision avoidance algorithm based on the finite state machine (FSM) model for the unmanned surface vehicle (USV) operating in offshore environments with many obstacles such as reefs and fishing vessels. First, we obtain the analytical results for the heading constraint with a fixed USV speed based on the velocity obstacle method and an obstacle-area-stratification method. Then, we determine the finite states, which primarily contain the goal and buffer waypoint-guidance states. This is achieved by the steering control, execution actions, and state transition conditions of the FSM model, based on the given constraint conditions and situations of obstacle detection. Finally, local steering collision avoidance path planning is realized by the execution of the FSM model. The simulation results demonstrate that the proposed multi-obstacle avoidance algorithm is feasible and practical. This method is easy to improve and expand and is easily combined with the current mainstream USV control system. As such, our proposed algorithm can facilitate the rapid engineering of the USV collision avoidance system.
Key words: unmanned surface vehicle    local avoidance    path planning    waypoint navigation    finite state machine    

无人船(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以及极坐标系oxOXY选择测地学直角坐标系, XY轴正方向分别为正北和正东向; oxy以实时无人船中心$\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} $为原点, 分别以无人船航向和右舷方向为XY轴正方向; oxoxy是同一的, 即ox的原点、X轴均与oxy一致。三个坐标系下角度值均以X轴正方向为0, 并随顺时针方向增加。

1.1.2 等效障碍物

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

图 1 等效障碍物示意图 Fig. 1 Equivalent obstacles
$ {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)$。这里, FTriC(θ)表示取以2π为周期的θ值对应的U范围内的值。

任意Tn采样时刻, 无人船航速ν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所示, 该模型利用障碍物边界BO, 禁止边界BP, 警告边界BW和安全边界BS共四个边界(均可用BxBy边界表示)将障碍物周边划分为障碍区域SO, 禁止区域SP, 警告区域SW, 安全区域SS和无碰区域SNoC。假设无人船和障碍物运动保持不变, 则可利用最近点(closest point of approach, CPA)方法确定无人船和障碍物之间的最近点。若CPA位于SP内, 无人船会驶入SP, 无人船几乎无法避免碰撞; 相似地, 若CPA位于SW内, 无人船极易与障碍物发生碰撞, 但通过避碰操作基本可以避免碰撞; 若CPA位于SS内, 无人船基本不会与障碍物发生碰撞, 但需要实时关注; 若CPA位于SNoC内, 无人船可以确保航行安全。

图 2 速度矢量及危险评估模型示意图 Fig. 2 Velocity vectors and risk assessment model

BxBy边界半径分别为RxRy, 半径差$\Delta {R_{x-y}}$可经验确定。由于${R_{\rm{O}}} = {R_{{\rm{Obs}}}} + {R_{{\rm{USV}}}}$, 其中, RUSV为无人船尺寸, 则RP, RWRS可依次确定。记从$\mathord{\buildrel{\lower3pt\hbox{$\scriptscriptstyle\frown$}} \over C} $出发并与边界Bx相切的两条射线分别为${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点的距离为DCPA, 则有${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 可航行角约束

Tn时刻无人船与障碍物存在相对运动, 保持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 }}$为边界Bx约束下的可航行角, 否则称其为不可航行角。所有可航行角构成可航行角区间${\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}}$即为Bx边界条件下的可航行角约束。不同情形下具体的矢端曲线与两条射线关系图例可见文献[16], 图 4列举了其中两种典型情形下的图例。

图 3 速度矢量关系及矢端曲线示意图 Fig. 3 Relationship of velocity vectors and hodograph

图 4 两种典型情形的矢端曲线与两条射线关系图例 Fig. 4 Relationships between the hodograph and two rays in two typical situations a.典型情形一图例; b.典型情形六图例 a. Situation one. b. Situation six

$\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)

对于Bx边界, 若给定${\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区间为可航行或不可航行角区间的极端情形。本文暂不考虑上述两种极端情形(前者可忽略, 后者需采取紧急措施, 正常避碰过程中很少出现), 分别设两类常规情形中的等效障碍物数量为N1N2, 则$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下目标位点PG的坐标为$\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}}}}}$可根据距离公式计算。

当无人船无法直接向PG点导航时, 由于${\theta _{{\rm{opt}}}}$基于oxy并随无人船位置和状态实时变化, 无法直接用于导航[7], 故通过增加临时位点进行间接导航的方式设计缓冲位点导航模型。设Tn时刻设置缓冲位点${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)

相似地, 可以计算OXYoxy下的实时缓冲位点期望角${\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}}}}$。基于成熟的位点导航算法, 无人船可根据避碰需求选择PG${P_{{\text{t}}, i}}$进行避碰和导航。

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

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

2.3.2 基本导航策略

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

记到达位点的距离阈值为DTh, 无人船航向与目标位点期望角所组成的角度区间为${\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用于实现局部避碰路径规划。

表 1 状态迁移基础条件 Tab. 1 Basic conditions of state transitions
基础条件 数学描述
CB1 ${D_{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} {\rm{, }}{P_{\rm{G}}}}} \leqslant {D_{{\rm{Th}}}}$
CB2 ${\mathit{\Psi} _{\rm{G}}} \cap {\mathit{\Psi} _{{\rm{N}}, {\rm{S}}}} = \emptyset$
CB3 ${D_{\overset{\lower0.5em\hbox{$\smash{\scriptscriptstyle\frown}$}}{C} {\rm{, }}{P_{{\rm{t, }}i}}}} \leqslant {D_{{\rm{Th}}}}$
CB4 $\left| {{F_{{\rm{TriC}}}}\left( {{\theta _{{\rm{opt}}}} - {\theta _{\rm{N}}}} \right)} \right| > {\theta _{{\rm{Th}}}}$
CB5 $\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|$

表 2 状态迁移条件 Tab. 2 Actual conditions of state transitions
条件 数学描述(N选1)
${C_{0 \to 1}}$ 无。直接触发
${C_{1 \to 1}}$ (1)${\bar C_{{{\rm{B}}_1}}}{C_{{{\rm{B}}_{\rm{2}}}}}$
${C_{1 \to 4}}$ (1)${\bar C_{{{\rm{B}}_1}}}{\bar C_{{{\rm{B}}_{\rm{2}}}}}$
${C_{1 \to 5}}$ (1)${C_{{{\rm{B}}_1}}}$
${C_{2 \to 2}}$ (1)${\bar C_{{{\rm{B}}_1}}}{\bar C_{{{\rm{B}}_{\rm{2}}}}}{\bar C_{{{\rm{B}}_{\rm{3}}}}}{\bar C_{{{\rm{B}}_{\rm{4}}}}}{\bar C_{{{\rm{B}}_{\rm{5}}}}}$
${C_{2 \to 3}}$ (1)${\bar C_{{{\rm{B}}_1}}}{C_{{{\rm{B}}_{\rm{2}}}}}$
${C_{2 \to 4}}$ (1)${\bar C_{{{\rm{B}}_1}}}{\bar C_{{{\rm{B}}_{\rm{2}}}}}{C_{{{\rm{B}}_{\rm{3}}}}}$
(2)${\bar C_{{{\rm{B}}_1}}}{\bar C_{{{\rm{B}}_{\rm{2}}}}}{\bar C_{{{\rm{B}}_{\rm{3}}}}}{C_{{{\rm{B}}_{\rm{4}}}}}$
(3)${\bar C_{{{\rm{B}}_1}}}{\bar C_{{{\rm{B}}_{\rm{2}}}}}{\bar C_{{{\rm{B}}_{\rm{3}}}}}{\bar C_{{{\rm{B}}_{\rm{4}}}}}{C_{{{\rm{B}}_{\rm{5}}}}}$
${C_{2 \to 5}}$ (1)${C_{{{\rm{B}}_1}}}$
${C_{3 \to 1}}$ 无。直接触发
${C_{4 \to 2}}$ 无。直接触发

图 5 FSM状态迁移示意图 Fig. 5 Diagram of FMS state transitions

表 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 °/s2。位点更新时临时缓冲位点插入距离$D_{{P_{{\rm{t, }}i}}}^{{T_n}}$为200 m(小于探测范围)。假设障碍物未进入无人船探测范围内均不可见, 且无人船具有快速稳定的位点导航能力, 仿真得到如图 6所示的结果。

图 6 无人船局部避碰导航仿真 Fig. 6 Simulation of USV guidance with local collision avoidance 图中静态障碍物为Os1Os8, 红色和黄色圆分别表示BWBS边界, BS边界显示绿色表示该障碍物在无人船探测范围内。动态障碍物为Od1Od3, 品红色圆表示从无人船探测到后的障碍物BW边界轨迹, 绿色圆表示该障碍物当前位置的BW边界 The static obstacles are Os1 to Os8 with a red circle and a yellow circle which are represent BW and BS respectively, and the obstacle is detected by USV if the color of the circle of BS is green. The dynamic obstacles are Od1 to Od3, the magenta circles represent the trajectories of BW when the dynamic obstacle is detected, and the green circle of the dynamic obstacle represents the current BW of it

图 6中, T1时刻无人船开始导航, 由于没有探测到障碍物, 直接向目标位点导航, 轨迹点为青色。图中同心圆表示静态障碍物, 分别标号Os1Os8, 红色和黄色圆分别表示BWBS边界。随着无人船的航行, T2时刻探测到从东南向西北方向运动的障碍物Od1, 由于可能发生碰撞, 增加缓冲位点Pt, k1, 无人船向Pt, k1导航, 轨迹点为蓝色, 最后无人船从Od1后方驶过, 如图 6T3时刻子图所示。此时无人船仍未探测到其他障碍物, 继续向目标位点导航, 直到部分静态障碍物进入探测范围, 障碍物BS边界设置为绿色, 设置缓冲位点Pt, k2, 无人船向Pt, k2导航, 如图 6T4时刻子图所示。无人船继续航行, 在T5时刻探测到从西北向东南方向运动的障碍物Od2, 继续更新缓冲位点Pt, k3, 无人船从Od2后方驶过, 如图 6T6时刻子图所示。此后, 无人船探测到东南向西北方向运动的障碍物Od3, 此时无人船导航主要受静态障碍物影响, 更新缓冲位点Pt, k4, 无人船从Od3前方驶过, 如图 6T7时刻子图所示。最后, 无人船绕过静态障碍物后对目标位点“可视”, 直接向目标位点导航, 直至到达目标位点, 如图 6T8时刻子图所示。仿真结果表明了所提出方法的有效性。

4 讨论与结论

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

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

参考文献
[1]
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
[2]
Yan R J, Pang S, Sun H B, et al. Development and missions of unmanned surface vehicle[J]. Journal of Marine Science & Application, 2010, 9(4): 451-457.
[3]
柳晨光, 初秀民, 吴青, 等. USV发展现状及展望[J]. 中国造船, 2014, 55(4): 194-205.
Liu Chenguang, Chu Xiumin, Wu Qing, et al. A review and prospect of USV research[J]. Shipbuliding of China, 2014, 55(4): 194-205.
[4]
Liu Z, Zhang Y, Yu X, et al. Unmanned surface vehicles: an overview of developments and challenges[J]. Annual Reviews in Control, 2016, 41: 71-93. DOI:10.1016/j.arcontrol.2016.04.018
[5]
Soltan R A, Ashrafiuon H, Muske K R. Trajectory real-time obstacle avoidance for underactuated unmanned surface vessels[C] // American Society of Mechanical Engineers. Proceedings of ASME 2009 International Design Engineering Technical Conferences & Computers and Information in Engineering Conference. San Diego: American Society of Mechanical Engineers, 2009: 1059-1067.
[6]
耿以才, 黄立新, 陈凌珊, 等. 无人船安全目标追踪与自动避障算法[J]. 计算机测量与控制, 2015, 23(7): 2467-2469.
Geng Yicai, Huang Lixin, Chen Lingshan, et al. Safe target-tracking algorithm with obstacle avoidance for unmanned ship[J]. Computer Measurement & Control, 2015, 23(7): 2467-2469.
[7]
Tang P, Zhang R, Liu D, et al. Local reactive obstacle avoidance approach for high-speed unmanned surface vehicle[J]. Ocean Engineering, 2015, 106: 128-140. DOI:10.1016/j.oceaneng.2015.06.055
[8]
Fiorini P, Shiller Z. Motion planning in dynamic environments using velocity obstacles[J]. International Journal of Robotics Research, 1998, 17(7): 760-772. DOI:10.1177/027836499801700706
[9]
Kluge B, Prassler E. Reflective navigation: individual behaviors and group behaviors[C]//IEEE. Proceedings of the 2004 IEEE International Conference on Robotics and Automation. New Orlwans: IEEE. 2004: 4172-4177. https://ieeexplore.ieee.org/document/1308926/
[10]
Wilkie D, Berg J V D, Manocha D. Generalized velocity obstacles[C]// IEEE. IEEE/RSJ International Conference on Intelligent Robots and Systems. St. Louis: IEEE, 2009: 5573-5578.
[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, 2011, 39(1): 110-119.
[12]
Larson J, Bruch M, Ebken J. Autonomous navigation and obstacle avoidance for unmanned surface vehicles[J]. Proceedings of SPIE - The International Society for Optical Engineering, 2006, 6230: 623007-1-623007-12.
[13]
蒋新松. 机器人学导论[M]. 沈阳: 辽宁科学技术出版社, 1994: 511-516.
Jiang Xinsong. Robotics Introduction[M]. Shenyang: Liaoning Science and Technology Publishing, 1994: 511-516.
[14]
Szlapczynski R. On evolutionary computing in multi- ship trajectory planning[J]. Applied Intelligence, 2012, 37(2): 155-174. DOI:10.1007/s10489-011-0319-7
[15]
Almeida C, Franco T, Ferreira H, et al. Radar based collision detection developments on USV ROAZ Ⅱ[C]// IEEE. Oceans'09 IEEE Bremen. Bremen: IEEE, 2009: 1-6.
[16]
Wang D, Zhang J, Jin J, et al. A steering occasion strategy for local collision avoidance of an unmanned surface vehicle in a complex environment[C]// IEEE. 2017 Chinese Control Conference Proceedings of the 36th CCC. Dalian: IEEE, 2017: 6133-6138.
[17]
Fox D, Burgard W, Thrun S. The dynamic window approach to collision avoidance[J]. IEEE Robotics & Automation Magazine, 2002, 4(1): 23-33.