Volume 16 Issue 1
Feb.  2021
Turn off MathJax
Article Contents

LI Q L, LI B, SUN G H, et al. Trajectory planning and automatic obstacle avoidance algorithm for unmanned surface vehicle based on exact penalty function[J]. Chinese Journal of Ship Research, 2021, 16(1): 89–95 doi:  10.19693/j.issn.1673-3185.02209
Citation: LI Q L, LI B, SUN G H, et al. Trajectory planning and automatic obstacle avoidance algorithm for unmanned surface vehicle based on exact penalty function[J]. Chinese Journal of Ship Research, 2021, 16(1): 89–95 doi:  10.19693/j.issn.1673-3185.02209

Trajectory planning and automatic obstacle avoidance algorithm for unmanned surface vehicle based on exact penalty function

doi: 10.19693/j.issn.1673-3185.02209
  • Received Date: 2020-11-30
  • Rev Recd Date: 2020-12-08
  • Available Online: 2021-01-26
  • Publish Date: 2021-02-07
  •   Objectives  Currently, how to plan the safe and efficient movement trajectory of an unmanned surface vehicle (USV) in local waters with multiple known obstacle positions is a research hotspot.  Methods  First, the obstacle areas are treated with simple and effective circular and convex quadrilateral envelopes, and the obstacle avoidance problem is transformed into the state inequality constraint of a time optimal control problem. The time optimal control problem is then transformed into an optimal parameter selection problem by control parameterization and time scale transformation. Finally, for multiple continuous state inequality constraints caused by multiple obstacles, the exact penalty function method is used to append all state constraints to the cost function. The final form of the problem is suitable for solving any effective optimization technique as a nonlinear optimization problem.  Results  The numerical simulation results show that the planned trajectory successfully avoids the obstacles in the water and conforms to the motion characteristics of USVs.  Conclusions  The results of this study can provide valuable references for the obstacle avoidance problem in USV trajectory planning.
  • [1] 张树凯, 刘正江, 张显库, 等. 无人船艇的发展及展望[J]. 世界海运, 2015, 38(9): 29–36.

    ZHANG S K, LIU Z J, ZHANG X K, et al. Development and prospect of unmanned ship[J]. World Shipping, 2015, 38(9): 29–36 (in Chinese).
    [2] 张树凯, 刘正江, 蔡垚, 等. 无人船艇航线自动生成研究现状及展望[J]. 中国航海, 2019, 42(3): 6–11.

    ZHANG S K, LIU Z J, CAI Y, et al. Review on automatic routeing technologies for unmanned vehicles[J]. Navigation of China, 2019, 42(3): 6–11 (in Chinese).
    [3] 李文华, 张君彦, 林珊颖, 等. 水面自主船舶技术发展路径[J]. 船舶工程, 2019, 41(7): 64–73.

    LI W H, ZHANG J Y, LIN S Y, et al. The development path of maritime autonomous surface ships technology[J]. Ship Engineering, 2019, 41(7): 64–73 (in Chinese).
    [4] 余必秀, 初秀民, 柳晨光, 等. 基于改进A*算法的无人航道测量船路径规划方法[J]. 武汉大学学报(信息科学版), 2019, 44(8): 1258–1264.

    YU B X, CHU X M, LIU C G, et al. A path planning method for unmanned waterway survey ships based on improved A* algorithm[J]. Geomatics and Information Science of Wuhan University, 2019, 44(8): 1258–1264 (in Chinese).
    [5] SONG R, LIU Y C, BUCKNALL R. Smoothed A* algorithm for practical unmanned surface vehicle path planning[J]. Applied Ocean Research, 2019, 83: 9–20. doi:  10.1016/j.apor.2018.12.001
    [6] LIU Y C, BUCKNALL R. Path planning algorithm for unmanned surface vehicle formations in a practical maritime environment[J]. Ocean Engineering, 2015, 97: 126–144. doi:  10.1016/j.oceaneng.2015.01.008
    [7] CHO Y, HAN J, KIM J, et al. Experimental validation of a velocity obstacle based collision avoidance algorithm for unmanned surface vehicles[J]. IFAC-Papers OnLine, 2019, 52(21): 329–334. doi:  10.1016/j.ifacol.2019.12.328
    [8] 欧阳子路, 王鸿东, 王检耀, 等. 基于改进Bi-RRT的无人水面艇自动避碰算法[J]. 中国舰船研究, 2019, 14(6): 8–14.

    OUYANG Z L, WANG H D, WANG J Y, et al. Automatic collision avoidance algorithm for unmanned surface vessel based on improved Bi-RRT algorithm[J]. Chinese Journal of Ship Research, 2019, 14(6): 8–14 (in Chinese).
    [9] 舒宗玉. 基于多目标混合粒子群算法的无人船全局路径规划[D]. 武汉: 武汉理工大学, 2017.

    SHU Z Y. Global path planning of unmanned surface vessel based on multi-obiective hybrid particle swarm algorithm[D]. Wuhan: Wuhan University of Technology, 2017 (in Chinese).
    [10] 邱晨, 周海峰, 王荣杰, 等. 改进蚁群算法的无人救生船航迹规划[J]. 集美大学学报(自然科学版), 2019, 24(5): 358–363.

    QIU C, ZHOU H F, WANG R J, et al. Track planning of unmanned lifeboat based on improved ant colony algorithm[J]. Journal of Jimei University (Natural Science), 2019, 24(5): 358–363 (in Chinese).
    [11] 沈海青. 基于强化学习的无人船舶避碰导航及控制[D]. 大连: 大连海事大学, 2018.

    SHEN H Q. Collision avoidance navigation and control for unmanned marine vessels based on reinforcement learning[D]. Dalian: Dalian Maritime University, 2018 (in Chinese).
    [12] 魏新勇. 水面无人艇自主局部避障系统关键技术研究[D]. 广州: 华南理工大学, 2019.

    WEI X Y. Research on key technologies of autonomous local obstacle avoidance system for unmanned surface vehicles[D]. Guangzhou: South China University of Technology, 2019 (in Chinese).
    [13] LI B, YU C J, TEO K L, et al. An exact penalty function method for continuous inequality constrained optimal control problem[J]. Journal of Optimization Theory and Applications, 2011, 151(2): 260–291. doi:  10.1007/s10957-011-9904-5
    [14] LI B, XU C, TEO K L, et al. Time optimal Zermelo's navigation problem with moving and fixed obstacles[J]. Applied Mathematics and Computation, 2013, 224: 866–875. doi:  10.1016/j.amc.2013.08.092
    [15] TEO K L, GOH C J, WONG K H. A unified computational approach to optimal control problems[M]. Essex: Longman Scientific and Technical, 1991.
    [16] YU C J, TEO K L, ZHANG L S, et al. A new exact penalty function method for continuous inequality constrained optimization problems[J]. Journal of Industrial and Management Optimization, 2010, 6(4): 895–910. doi:  10.3934/jimo.2010.6.895
  • 加载中
通讯作者: 陈斌, bchen63@163.com
  • 1. 

    沈阳化工大学材料科学与工程学院 沈阳 110142

  1. 本站搜索
  2. 百度学术搜索
  3. 万方数据库搜索
  4. CNKI搜索

Figures(10)  / Tables(1)

Article Metrics

Article views(28) PDF downloads(17) Cited by()

Proportional views
Related

Trajectory planning and automatic obstacle avoidance algorithm for unmanned surface vehicle based on exact penalty function

doi: 10.19693/j.issn.1673-3185.02209

Abstract:   Objectives  Currently, how to plan the safe and efficient movement trajectory of an unmanned surface vehicle (USV) in local waters with multiple known obstacle positions is a research hotspot.  Methods  First, the obstacle areas are treated with simple and effective circular and convex quadrilateral envelopes, and the obstacle avoidance problem is transformed into the state inequality constraint of a time optimal control problem. The time optimal control problem is then transformed into an optimal parameter selection problem by control parameterization and time scale transformation. Finally, for multiple continuous state inequality constraints caused by multiple obstacles, the exact penalty function method is used to append all state constraints to the cost function. The final form of the problem is suitable for solving any effective optimization technique as a nonlinear optimization problem.  Results  The numerical simulation results show that the planned trajectory successfully avoids the obstacles in the water and conforms to the motion characteristics of USVs.  Conclusions  The results of this study can provide valuable references for the obstacle avoidance problem in USV trajectory planning.

LI Q L, LI B, SUN G H, et al. Trajectory planning and automatic obstacle avoidance algorithm for unmanned surface vehicle based on exact penalty function[J]. Chinese Journal of Ship Research, 2021, 16(1): 89–95 doi:  10.19693/j.issn.1673-3185.02209
Citation: LI Q L, LI B, SUN G H, et al. Trajectory planning and automatic obstacle avoidance algorithm for unmanned surface vehicle based on exact penalty function[J]. Chinese Journal of Ship Research, 2021, 16(1): 89–95 doi:  10.19693/j.issn.1673-3185.02209
    • 近年来,无人驾驶受到日益广泛的关注,在飞行器、智能车、舰船等领域得以快速发展。智能化是未来舰船的发展趋势之一[1]。由于无人艇的制造成本低、制造周期短、环境适应能力强、人力成本低,其在海洋资源勘测、航道测量、环境监测、水域清洁、军事作战等领域具备良好的应用前景。如何快速生成无人艇在复杂环境下的避障航线,是其自主化发展的关键技术之一[2]

      李文华等[3]总结了在工业4.0背景下世界范围内船舶自主化航行技术的主要发展路径,分析了无人艇航迹规划、智能避障等关键技术。余必秀等[4]通过增加代价函数从而改进了传统的A*算法,可用于规划无人航道测量船的运动路径。Song等[5]提出了平滑的A*算法来减少冗余的路径点,从而可以提供更连续的路径。Liu等[6]提出了一种基于快速步进(fast marching,FM)方法的无人艇信息导航路径规划算法,具有计算速度快、计算复杂度低等特点,该算法覆盖了2个区域(航行区域和避碰区域),可以保证规划轨迹不违反任何禁区。Cho等[7]在改进现有视线(line-of-sight,LOS)制导算法和速度障碍(velocity obstacle,VO)制导算法的基础上,提出了航点跟踪与避碰算法:即在路径点转换过程中引入额外的控制方案以提高跟踪控制的稳定性,并对传统的VO算法进行改进,从而有效解决了遇到障碍物时的位置不确定性问题,同时在真实海洋环境下验证了算法的有效性。欧阳子路等[8]将双向搜索树(bidirectional rapidly-exploring random trees,Bi-RRT)算法与速度障碍法相结合,提出了基于改进Bi-RRT的无人艇自动避碰算法,并采用并行延伸扩展的2颗搜索树提高了算法的实时性。受人工智能技术的影响,很多智能算法已在无人艇路径规划领域得以运用,如粒子群算法[9]、蚁群算法[10]、强化学习[11]、人工神经网络[12]等。舒宗玉[9]提出了多目标优化混合粒子群路径规划算法,实现了无人艇路径长度、路径平滑性和安全性的多目标优化。邱晨等[10]基于改进的蚁群算法,规划了无人救生艇的最短无碰撞安全路径。沈海青[11]将A*并行决策动态避障算法与基于深度Q学习的智能避碰算法相结合,为无人艇提供了多层避碰导航。魏新勇[12]利用卷积神经网络技术识别附近水域的障碍物,并结合模糊数学理论构建了危险系数指标,提出了远程航迹重规划和近程反应式避障方法。

      障碍物的存在为航迹规划带来了巨大的挑战,其中障碍物处理的难易程度直接决定了航迹优化问题的复杂程度,而现有文献对障碍物的处理方法普遍比较复杂。为此,本文拟采用圆形包络面和凸四边形包络面对障碍物进行包络处理,并将避障问题转化为在笛卡尔坐标系下的不等式约束问题;通过引入精确罚函数,以简化处理多个障碍物带来的多个强制约束问题。同时,鉴于现有研究成果在航迹规划方面鲜有考虑无人艇的运动特性,本文拟基于无人艇的运动方程,采用控制参数化方法求解时间最优的控制问题;通过结合控制参数化方法与精确罚函数方法,较好地处理无人艇的航迹规划和自动避障问题,并采用仿真验证本文方法的有效性。

    • 为简化处理,本文将无人艇视为仅规划其运动航迹的质点,其运动学坐标系如图1所示。图中,$\theta $为无人艇的航向角,V为无人艇合速度矢量的模长,${V_x}$${V_y}$分别为V沿x轴和y轴的速度分量。设定无人艇在$t$时刻的位置坐标为$\left( {x(t),y(t)} \right)$,状态量为${{X}}(t) = {[ { {x(t)}\;\;\;{y(t)}\;\;\;{\theta (t)}\;\;\;{V(t)} } ]^{\rm{T}}}$,控制量为${{u}}{\rm{(}}t{\rm{)}} = {[ {\gamma (t)}\;\;\;{a(t)} ]^{\rm{T}}}$,则无人艇的动态方程(变量上方的符号“·”表示关于时间的一阶导数)为

      $$\left\{ \begin{split} & \dot x(t) = V(t)\cos \theta (t) \\& \dot y(t) = V(t)\sin \theta (t) \\& \dot \theta (t) = \gamma (t) \\& \dot V(t) = a(t) \end{split} \right.$$ (1)

      式中:V(t)为无人艇在$t$时刻的合速度矢量的模长;θ(t)为无人艇在$t$时刻的航向角;$\gamma (t)$为无人艇在$t$时刻$\theta $的角速度;$a(t)$为无人艇在$t$时刻$V$的加速度。

      为便于描述,式(1)可以简记为函数f

      Figure 1.  Kinematic coordinate system of USV

      $${\dot{{ X}}}(t) = f\left( {t,{{X}}(t),{{u}}(t)} \right)$$ (2)
    • 在实际航行环境中,障碍物的形状大多不规则,如果对障碍进行精确建模,势必会显著增加建模的复杂程度和工作量。为简化处理,本文将采用圆形包络面和凸四边形包络面来对障碍物进行包络覆盖。对于长宽比较小的不规则障碍物,可以采用圆形包络面处理,如图2(a)图2(b)所示。对于长宽比过大的细长型障碍物,如果仍然采用圆形包络面处理,会造成较大的可通航区域浪费(图2(c)),不利于航迹规划。为此,对于细长型障碍物,宜采用凸四边形包络面进行处理,如图2(d)所示。由障碍物包络面和标准禁航区构成的不可航行区域,在下文统称为禁航区(禁止无人艇航行的区域)。

      Figure 2.  Envelope diagram of obstacle area

    • 对于长宽比较小的不规则障碍物,可以采用圆形包络面以简化建模的复杂度。将障碍物的最长端连线作为包络圆的直径,而直径中点作为包络圆的圆心,如图3所示,圆心记作$\left( {{x_i},{y_i}} \right)$,半径记作${r_i}$,其中$i$为障碍物序号。为保证无人艇的安全性,需引入安全阈值$\rho $,其值为船身长度的3倍。将增加了安全阈值的包络圆作为覆盖障碍物的禁航区域,即图3中的虚线圆,其半径${R_i} = {r_i} + \rho $。为使无人艇规避障碍物,则其位置应满足:

      Figure 3.  Circular envelope modeling

      $${\left( {x(t) - {x_i}} \right)^2} + {\left( {y(t) - {y_i}} \right)^2} \geqslant {R_i}^2$$ (3)
    • 对于长宽比较大的不规则障碍物,如果继续采用圆形包络面进行建模处理,会浪费大量的可航行区域,增加无人艇的航行距离,甚至出现无可航行区域情况。为此,宜采用凸四边形包络面进行建模处理。如图4所示,凸四边形包络面由4条相交的直线lj,1lj,2lj,3lj,4$j$为障碍物序号)围合而成。图4中的实线凸四边形为障碍物的最小面积包络面,虚线凸四边形为考虑了安全阈值$\rho $之后的包络面。本文将采用斜截式直线方程对lj,1lj,2lj,3lj,4进行建模,设定kj,1kj,2kj,3kj,4为4条边的斜率,bj,1bj,2bj,3bj,4为4条虚线边对应直线在$y$轴上的截距。

      Figure 4.  Convex quadrilateral envelope modeling

      为使无人艇规避凸四边形包络面,其位置应满足下列4个不等式之一:

      $$\left\{ \begin{aligned} & {y(t) - {k_{j,1}}x(t) - {b_{j,1}} \leqslant 0} \\ & {y(t) - {k_{j,2}}x(t) - {b_{j,2}} \geqslant 0} \\ & {y(t) - {k_{j,3}}x(t) - {b_{j,3}} \leqslant 0} \\ & {y(t) - {k_{j,4}}x(t) - {b_{j,4}} \geqslant 0} \end{aligned}\right.$$ (4)

      当某边的斜率不存在时,所在边的不等式约束形式即变为对$x(t)$的约束。为简化约束形式,式(4)可以转化为

      $$\prod\limits_{{\textit{z}} = 1}^4 {\left( {\min \left( {0,{{\left( { - 1} \right)}^{\textit{z}}}\left( {y(t) - {k_{j,{\textit{z}}}}x(t) - {b_{j,{\textit{z}}}}} \right)} \right)} \right)} = 0$$ (5)

      式中:$ {\rm{min}}(\cdot)$为取最小值函数;z=1,2,3,4,为凸四边形包络面的直线序号。

    • 航迹规划的目标是:已知无人艇的初始状态,在考虑禁航区域、最小/最大航行速度和终点位置约束的前提下,实现运动总时间的极小化,该问题模型(P1)为:

      $$ \begin{split} & ({\rm{P1}}):\mathop {\min }\limits_{{{u}}(t),T} \;T\\& {\rm{s}}{\rm{. t}}{\rm{.\; C0}}:|\gamma (t)| \leqslant {\gamma _{{\rm{max}}}},\;|a(t)| \leqslant {a_{{\rm{max}}}},\;t \in [0,\;T]\\& \;\;\;\;\;\;{\rm{C1: }}\dot {{X}}(t) = f\left( {t,\;{{X}}(t),\;{{u}}(t)} \right),\;\;t \in [0,T]\\& \;\;\;\;\;\;{\rm{ C2}}:{{X}}(0) = {{{X}}_0}\\& \;\;\;\;\;\;{\rm{ C3}}:x(T) = {x_T}\\& \;\;\;\;\;\;{\rm{ C4}}:y(T) = {y_T}\\& \;\;\;\;\;\;{\rm{ C5}}: 0 \leqslant V(t) \leqslant {V_{\max }},\;t \in [0,\;T]\\& \;\;\;\;\;\;{\rm{ C6}}: {\left( {x(t) - {x_i}} \right)^2} + {\left( {y(t) - {y_i}} \right)^2} \geqslant {R_i}^2,\\&\;\;\;\;\;\;\qquad t \in [0,\;T],\;\;i = 1,\;2, \cdots ,{N_i}\\& \;\;\;\;\;\;{\rm{ C7}}:\displaystyle\prod\limits_{{\textit{z}} = 1}^4 {\left( {\min \left( {0,{{\left( { - 1} \right)}^{\textit{z}}}\left( {y(t) - {k_{j,{\textit{z}}}}x(t) - {b_{j,{\textit{z}}}}} \right)} \right)} \right)} = 0,\\&\;\;\;\;\;\;\;\qquad t \in [0,\;T],\;\;j = 1,\;2, \cdots ,{N_j}\\[-10pt] \end{split}$$ (6)

      式中:C0为控制量约束;C1为无人艇运动学方程约束;C2为无人艇的初始状态;C3,C4为终端位置约束;C5为无人艇的航行速度约束;C6,C7分别为圆形禁航区和凸四边形禁航区;$T$为无人船的最大运动时间;γmax为最大转向角速度;amax为最大航行加速度;X(0)为0时刻的无人艇状态;X0为无人艇的初始状态;${x_T}$${y_T}$为无人艇的终端位置坐标;${V_{{\rm{max}}}}$是无人艇的最大航行速率;${N_i}$${N_j}$分别为圆形包络禁航区和凸四边形包络禁航区的障碍物最大数量。时间域$\left[ {0,\;T} \right]$是一个变化域,而约束C5~C7中均包含每个时刻的状态约束,这实际上将进一步产生无数个约束,所以给问题处理带来了巨大的挑战。

    • 本文将采用时间尺度变换和控制参数化方法来处理上述优化问题。如图5所示,时间尺度变换将可变时间域$t \in [0,\;T]$变换为固定域$s \in [0,\;1]$,其中s为转换后的时间刻度。由文献[13-14]可知

      Figure 5.  Time scale transformation

      $$\frac{{{{\rm{d}}}t}}{{{{\rm{d}}}s}} = \tan \psi = T$$ (7)

      式中,$\psi $为时间尺度转换角。

      根据式(2),可将式(7)转换为

      $${\dot{{ X}}}(s) = \frac{{{{\rm{d}}}{{X}}}}{{{{\rm{d}}}s}} = \frac{{{{\rm{d}}}{{X}}}}{{{{\rm{d}}}t}}\frac{{{{\rm{d}}}t}}{{{{\rm{d}}}s}} = f\left( {t,{{X}}(s),{{u}}(s)} \right) \cdot T$$ (8)

      为便于优化处理,本文将连续控制量在$s \in [0,\;1]$内等分为K个区间,从而产生K+1个节点$\{ 0,{s_1}, {s_2}, \cdots , {s_K} \}$,其中sK =1。分段参数化的结果如图6所示,其函数表达式为[15]

      $${u_n}(s) = \sum\limits_{m = 1}^K {{\sigma _{n,m}}{\Gamma _{({s_{K - 1}},{s_K})}}(s)} $$ (9)

      式中:${u_n}(s)$为控制量,其中n=1, 2,分别为无人艇的航向角速度和航行加速度;${\sigma _{n,m}}$为需要优化的控制参数,其中m=1, 2,···, K,为控制区间的序号;${\Gamma _{({s_{K - 1}},{s_K})}}(s)$为取值开关函数。

      其中

      $${\Gamma _{({s_{K - 1}},{s_K})}}(s) = \left\{ \begin{aligned} & 1,\;\;s \in ({s_{K - 1}},{s_K}) \\& 0,\;\;s \notin ({s_{K - 1}},{s_K}) \end{aligned} \right.$$ (10)

      Figure 6.  Control parameterization

      因此,式(6)可以改写为问题模型(P2):

      $$ \begin{split} ({\rm{P2}}):\;&\mathop {\min }\limits_{{{u}}(s),T} \;T\\ {\rm{s}}{\rm{.t}}. \;\;{\rm{S0}}:\;&|\gamma (s)| \leqslant {\gamma _{{\rm{max}}}},\;|a(s)| \leqslant {a_{{\rm{max}}}},\;\;s \in [0,1]\\ {\rm{ S1}}:\; &\dot {{X}}(s) = f\left( {s,\;{{X}}(s),\;{{u}}(s)} \right) \cdot T,\;\;s \in [0,1]\\ {\rm{ S2}}:\;&{{X}}(0) = {{{X}}_0}\\ {\rm{ S3}}:\;&x(1) = {x_T}\\ {\rm{ S4}}:\;&y(1) = {y_T}\\ {\rm{ S5}}:\;& 0 \leqslant V(s) \leqslant {V_{\max }},\;s \in [0,1]\\ {\rm{ S6}}:\; &{\left( {x(s) - {x_i}} \right)^2} + {\left( {y(s) - {y_i}} \right)^2} \geqslant {R_i}^2,\\& s \in [0,\;1],\;\;i = 1,\;2, \cdots ,{N_i}\\ {\rm{ S7}}:\;&\prod\limits_{z = 1}^4 {\left( {\min \left( {0,{{\left( { - 1} \right)}^z}\left( {y(s) - {k_{j,z}}x(s) - {b_{j,z}}} \right)} \right)} \right)} = 0,\\& s \in [0,\;1],\;\;j = 1,\;2, \cdots ,{N_j}\\[-11pt] \end{split}$$ (11)

      式中:S0~S7为约束C0~C7的改写形式;x(1),y(1)为s=1时刻的位置状态。

    • (P2)中含有终端等式约束S3和S4以及连续状态不等式约束S5~S7,这就为问题求解带来了极大的挑战。为此,本文采用精确罚函数[13, 16]来处理约束S3~S7,以便将(P2)转化为仅含控制约束和初始状态约束的优化问题,其优化目标函数J为:

      $$ \begin{split} & \;\;\;\;\qquad \underset{{{u}}(s),T, \epsilon }{{\rm{min}}}J=\theta \left({{X}}(1),T, \epsilon \right)+\\& {\displaystyle {\int }_{0}^{1}H\left({{X}}(s),{{u}}(s),{{\lambda}} (s),T, \epsilon \right)-{{{\lambda}} }^{\rm{T}}(s)\dot{{{X}}}(s)}{\rm{ d}}s\\&\qquad\qquad\qquad {\rm{s}}{\rm{. t}}{\rm{.\; S0}},{\rm{ S2}}\\&\qquad\qquad\qquad {\rm{S8}}:0< \epsilon \end{split}$$ (12)

      其中

      $$ \theta \left({{X}}(1),T, \epsilon \right)=T+{ \epsilon }^{-\alpha }\left({\left(x(1)-{x}_{T}\right)}^{2}+{\left(y(1)-{y}_{T}\right)}^{2}\right)+\delta { \epsilon }^{\beta }$$ (13)

      哈密顿函数为:

      $$ \begin{split} &\qquad\qquad H\left({{X}}(s),{{u}}(s),{{\lambda}} (s),T, \epsilon \right)=\\&\phi \left({{X}}(s),{{u}}(s),T, \epsilon \right)+{{{\lambda}} }^{\rm{T}}(s)f\left(s,{{X}}(s),{{u}}(s)\right)\cdot T \end{split}$$ (14)

      其中

      $$ \begin{array}{c}\phi ({{X}}(s),{{u}}(s),T, \epsilon )={ \epsilon }^{-\alpha }({\rm{max}}{\{0,V(s)-{V}_{\rm{max}}-{ \epsilon }^{\gamma }W\}}^{2}+\\{\rm{min}}{\{0,V(s)+{ \epsilon }^{\gamma }W\}}^{2})+{ \epsilon }^{-\alpha }\displaystyle \sum _{i=1}^{{N}_{i}}{\rm{max}}\{0,{R}_{i}{}^{2}-{(x(s)-{x}_{i})}^{2}-\\{(y(s)-{y}_{i})}^{2}-{ \epsilon }^{\gamma }{W}_{i}\}^{2}+{ \epsilon }^{-\alpha }\displaystyle \sum _{j=1}^{{N}_{j}}\kappa \displaystyle \prod _{{\textit{z}}=1}^{4}({\rm{min}}(0,{(-1)}^{{\textit{z}}}(y(s)-\\ {k}_{j,{\textit{z}}}x(s)-{b}_{j,{\textit{z}}})+{ \epsilon }^{\gamma }{W}_{j})^{2})+\delta { \epsilon }^{\beta }\\[-10pt]\end{array}$$ (15)

      拉格朗日乘子向量${{\lambda }}(s)$需满足下列方程:

      $$ \begin{split} & \frac{{\rm{d}}{{{\lambda}} }^{\rm{T}}(s)}{{\rm{d}}s}=-\frac{\partial H\left({{X}}(s),{{u}}(s),{{\lambda}} (s),T, \epsilon \right)}{\partial {{X}}(s)}\\&\qquad\quad {{{\lambda}} }^{\rm{T}}(1)=\frac{\partial \theta \left({{X}}(1),T, \epsilon \right)}{\partial {{X}}(1)} \end{split}$$ (16)

      以上式中:$ \epsilon $为惩罚参数;X(1)为无人艇的终端状态;$\alpha > 1,\;\beta > 2,\;\gamma > 2,\;\delta ,\;{W_i},\;{W_j}$均为正实数参数;$ { \epsilon }^{-\alpha }$为惩罚权重;$ \delta { \epsilon }^{\beta }$为惩罚因子;$ { \epsilon }^{\gamma }W$为松弛因子,其中W为松弛因子的权重;$\kappa $为缩放因子。

      对凸四边形包络面而言,当无人艇违反凸四边形包络面的约束时,必将同时违反凸四边形包络面4条边的约束,经过4次累乘后,其惩罚值将远大于违反圆形包络面的惩罚值。因此,本文采用了缩放因子$\kappa $来对凸四边形包络面约束进行缩放处理。

      在优化初期,由于存在大量违反约束的情况,为了放宽对约束的惩罚程度,可以增加$ \epsilon $以增加惩罚因子,从而减小惩罚权重并增加松弛因子;随着优化的进行,违反约束的情况将逐渐减少,可以减小$ \epsilon $来减小惩罚因子,从而增加惩罚权重并减小松弛因子,最终进一步减少违反约束的情况。当$ \epsilon $趋近0时,惩罚权重将趋于无穷大,松弛因子趋于0,惩罚因子趋于0,此时,即可认为优化结束后的目标函数J完全满足了约束条件。

      需注意的是,$\delta $在理论上为正无穷才能使优化过程中的$ \epsilon $趋近于0。在实际计算中,$ \epsilon $小于一定的阈值即可认为其足够小,因此可将$\delta $设置为一个较大的常数值。对于松弛因子权重W,可以根据不同惩罚项的松弛程度来调整具体数值;关于其他参数的设置,仅需满足理论要求的约束即可[16]

      式(12)是一个标准的最优参数选择问题,为了将其转化为常规的非线性优化问题,需获得目标函数$J$的梯度方程[15],即

      $$ \begin{split} & \frac{\partial J}{\partial {{u}}}=\frac{\partial \theta \left({{X}}(1),T, \epsilon \right)}{\partial {{u}}}+{\displaystyle {\int }_{0}^{1}\frac{\partial H\left({{X}}(s),{{u}}(s),{{\lambda}} (s),T, \epsilon \right)}{\partial {{u}}}}{\rm{d}}s\\& \frac{\partial J}{\partial T}=\frac{\partial \theta \left({{X}}(1),T, \epsilon \right)}{\partial T}+{\displaystyle {\int }_{0}^{1}\frac{\partial H\left({{X}}(s),{{u}}(s),{{\lambda}} (s),T, \epsilon \right)}{\partial T}}{\rm{d}}s\\& \frac{\partial J}{\partial \epsilon }=\frac{\partial \theta \left({{X}}(1),T, \epsilon \right)}{\partial \epsilon }+{\displaystyle {\int }_{0}^{1}\frac{\partial H\left({{X}}(s),{{u}}(s),{{\lambda}} (s),T, \epsilon \right)}{\partial \epsilon }}{\rm{d}}s \end{split}$$ (17)

      基于式(17),即可将式(12)转化为一个无状态约束的最优参数选择问题,且其适用于任何有效的分级优化器进行求解。

    • 为了验证本文算法的可行性,设定无人艇的初始状态$ {{X}}({0})={[0,\;0,\;0.75,\;5]}^{\rm{T}}$,终点位置为$\left( {{x_T},\;{y_T}} \right) = \left( {500,\;500} \right)$,最大速率${V_{{\rm{max}}}} = 8\;{\rm{ m/s}}$;罚函数的相关参数为:$\alpha {\rm{ = }}1.5,\;\beta {\rm{ = }}3,\;\gamma {\rm{ = }}3,\;\delta = {10^8}$$\kappa = {10^{ - 4}}$Wi=0.3,其中i=1,2,3,4,5,6;Wj=0.3,其中j=1,2,其他参数如表1所示。

      术语名称数值及内容
      最大加速度amax/(m·s−2) 2
      最大角速度γmax/(rad·s−1) 0.1
      安全阈值ρ/m 8
      圆形禁航区1/m $\left( {{x_1},{y_1}} \right) = \left( {452,436} \right),{R_1} = 34\;$
      圆形禁航区2/m $\left( {{x_2},{y_2}} \right) = \left( {88,120} \right),{R_2} = 26\;$
      圆形禁航区3/m $ \left({x}_{3},{y}_{3}\right)=\left(358,78\right),{R}_{3}=34\;$
      圆形禁航区4/m $\left( {{x_4},{y_4}} \right) = \left( {497,306} \right),{R_4} = 45$
      圆形禁航区5/m $\left( {{x_5},{y_5}} \right) = \left( {524,155} \right),{R_5} = 53\;$
      凸四边形禁航区1/m $ \begin{array}{l}\left({k}_{1,1},{k}_{1,2},{k}_{1,3},{k}_{1,4}\right)=\left(-0.35,-0.30,0.79,1.90\right)\\ \left({b}_{1,1},{b}_{1,2},{b}_{1,3},{b}_{1,4}\right)=\left(310.37,469.00,71.86,143.13\right)\end{array}$
      凸四边形禁航区2/m $\begin{array}{l}\left({k}_{2,1},{k}_{2,2},{k}_{2,3},{k}_{2,4}\right)=\left(-0.56,-0.15,0.69,1.52\right)\\ \left({b}_{2,1},{b}_{2,2},{b}_{2,3},{b}_{2,4}\right)=\left(162.00,248.88,-83.99,-158.95\right)\end{array}$

      Table 1.  Simulated data

      为了验证包络面建模对禁航区域的普适性,本文在仿真中设置了不同半径的圆形包络面和凸四边形包络面,其Matlab仿真结果如图7所示(蓝色填充区域为实际禁航区,虚线包络区为考虑安全阈值后的禁航区)。根据仿真结果,无人艇从初始位置出发,可以完全规避禁航区域,其理想航迹的总时长为100.6 s。从图7可以看出,为了极小化航行时间,航迹与1个禁航区存在相切的情况,在实际航行过程中,某些不确定性因素的影响可能使无人艇存在碰撞危险。不过,本文设置了安全阈值,可以确保航行过程中无人艇与实际禁航区的安全距离。图8所示为无人艇位置随时间的变化曲线。

      Figure 7.  Track map of USV

      Figure 8.  Diagram of USV position changing with time

      图9图10所示为无人艇的运动速度和控制量随时间的变化曲线,其仿真结果满足相关约束条件。从图9可以看出,为了实现极小化的航行时间,无人艇在80 s之前从初始状态加速到了最大航行速度,并维持在最大速度附近;在80 s之后,由于无人艇存在大角度的航向调整,为了极小化时间,需在转弯半径和转弯速度之间进行权衡,虽然较小的转弯半径可以缩短转弯航程,但需同时减小航行速度。从仿真结果可以看出,为了极小化航行时间,该无人艇的航向角速度正向满舵且航行速度下降,从而匹配了最佳的转弯半径。可见,该仿真结果验证了本文算法的有效性。

      Figure 9.  Diagram of USV speed changing with time

      Figure 10.  Input control signal

    • 本文采用圆形和凸四边形包络面处理不规则障碍物,将规避不规则障碍物的问题转化为笛卡尔坐标系下的不等式约束条件,从而将无人艇避障航迹规划问题构建为含有连续状态不等式约束和终端约束的时间最优控制问题。通过控制参数化和时间尺度变换,将时间最优控制问题进一步转化为了最优参数选择问题;同时,利用精确罚函数方法,将连续状态不等式约束和终端约束构建为约束惩罚函数并添加到目标函数中,最终转化为一个无状态约束的最优参数选择问题,其适用于任何有效的分级优化技术进行求解。因此,本文所提出的算法可以有效处理无人艇航迹规划中的避障问题。

Reference (16)

Catalog

    /

    DownLoad:  Full-Size Img  PowerPoint
    Return
    Return