留言板

尊敬的读者、作者、审稿人, 关于本刊的投稿、审稿、编辑和出版的任何问题, 您可以本页添加留言。我们将尽快给您答复。谢谢您的支持!

姓名
邮箱
手机号码
标题
留言内容
验证码

基于精确罚函数的无人艇航迹规划和自动避障算法

李清亮 李彬 孙国皓 崔星 毛新涛

李清亮, 李彬, 孙国皓, 等. 基于精确罚函数的无人艇航迹规划和自动避障算法[J]. 中国舰船研究, 2021, 16(1): 1–7 doi:  10.19693/j.issn.1673-3185.02209
引用本文: 李清亮, 李彬, 孙国皓, 等. 基于精确罚函数的无人艇航迹规划和自动避障算法[J]. 中国舰船研究, 2021, 16(1): 1–7 doi:  10.19693/j.issn.1673-3185.02209
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): 1–7 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): 1–7 doi:  10.19693/j.issn.1673-3185.02209

基于精确罚函数的无人艇航迹规划和自动避障算法

doi: 10.19693/j.issn.1673-3185.02209
基金项目: 空间智能控制技术国防重点实验室2018年度基金资助项目(KGJZDSYS-2018-03)
详细信息
    作者简介:

    李清亮,男,1998年生,硕士生。研究方向:智能体轨迹规划。E-mail:liqingliang@stu.scu.edu.cn

    李彬,男,1983年生,博士,研究员。研究方向:航空航天中的控制问题,无人系统自主控制,物理层保密通信,信能同传技术,数值优化。E-mail:bin.li@scu.edu.cn

    孙国皓,男,1990年生,博士,助理研究员。研究方向:分布式组网雷达信号处理,机载/星载雷达信号处理,智能感知技术。E-mail:sghsjw2005@126.com

    通讯作者:

    李彬,

    孙国皓

  • 中图分类号: U664.82

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

  • 摘要:   目的  对于已知多个障碍物的局部水域,如何规划安全高效的无人艇运动航迹,是当前的研究热点。  方法  首先,采用简洁有效的圆形包络面和凸四边形包络面处理障碍物区域,并将避障问题转化为时间最优控制问题中的状态不等式约束;然后,利用控制参数化和时间尺度变换,将时间最优控制问题转化为最优参数选择问题;最后,对于由多个障碍物带来的多个连续状态不等式约束,采用精确罚函数法将所有的状态约束都附加到目标函数中,从而构建适用于任何有效优化技术予以求解的非线性优化问题。  结果  数值仿真结果表明,该算法所规划的航迹能成功规避水域中的所有障碍物,同时符合无人艇的运动特性。  结论  研究成果可为无人艇航迹规划的避障问题提供参考。
  • 图  1  无人艇运动学坐标系

    Figure  1.  Kinematic coordinate system of USV

    图  2  障碍物区域的包络图

    Figure  2.  Envelope diagram of obstacle area

    图  3  圆形包络面建模

    Figure  3.  Circular envelope modeling

    图  4  凸四边形包络面建模

    Figure  4.  Convex quadrilateral envelope modeling

    图  5  时间尺度变换

    Figure  5.  Time scale transformation

    图  6  控制参数化

    Figure  6.  Control parameterization

    图  7  无人艇的航迹图

    Figure  7.  Track map of USV

    图  8  无人艇位置随时间的变化图

    Figure  8.  Diagram of USV position changing with time

    图  9  无人艇速度随时间的变化曲线

    Figure  9.  Diagram of USV speed changing with time

    图  10  输入控制信号

    Figure  10.  Input control signal

    表  1  仿真数据

    Table  1.   Simulated data

    术语名称数值及内容
    最大加速度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}$
    下载: 导出CSV
  • 张树凯, 刘正江, 张显库, 等. 无人船艇的发展及展望[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).
    张树凯, 刘正江, 蔡垚, 等. 无人船艇航线自动生成研究现状及展望[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).
    李文华, 张君彦, 林珊颖, 等. 水面自主船舶技术发展路径[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).
    张彩虹, 胡彦军. 无人船远距离目标追踪与自动避障算法优化[J]. 舰船科学技术, 2019, 41(16): 22–24.

    ZHANG C H, HU Y J. Optimization of long-range target tracking and automatic obstacle avoidance algorithms for unmanned vehicles[J]. Ship Science and Technology, 2019, 41(16): 22–24 (in Chinese).
    余必秀, 初秀民, 柳晨光, 等. 基于改进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).
    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
    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
    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
    欧阳子路, 王鸿东, 王检耀, 等. 基于改进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).
    舒宗玉. 基于多目标混合粒子群算法的无人船全局路径规划[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).
    邱晨, 周海峰, 王荣杰, 等. 改进蚁群算法的无人救生船航迹规划[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).
    沈海青. 基于强化学习的无人船舶避碰导航及控制[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).
    魏新勇. 水面无人艇自主局部避障系统关键技术研究[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).
    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
    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
    TEO K L, GOH C J, WONG K H. A unified computational approach to optimal control problems[M]. Essex: Longman Scientific and Technical, 1991.
    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
  • [1] 祝亢, 黄珍, 王绪明2021: 基于深度强化学习的智能船舶航迹跟踪控制 , 中国舰船研究. doi: 10.19693/j.issn.1673-3185.01940
    [2] 吴文涛, 彭周华, 王丹, 刘陆, 姜继洲, 任帅2021: 基于扩张状态观测器的双桨推进无人艇抗干扰目标跟踪控制 , 中国舰船研究. doi: 10.19693/j.issn.1673-3185.01665
    [3] 彭周华, 吴文涛, 王丹, 刘陆2021: 多无人艇集群协同控制研究进展与未来趋势 , 中国舰船研究. doi: 10.19693/j.issn.1673-3185.01923
    [4] 杨忠凯, 仲伟波, 冯友兵, 孙彬2021: 基于改进的视线导引算法与自抗扰航向控制器的无人艇航迹控制 , 中国舰船研究. doi: 10.19693/j.issn.1673-3185.01842
    [5] 胡剑, 杨建军, 蔡文涛2021: 某型无人艇综合健康管理系统分析 , 中国舰船研究. doi: 10.19693/j.issn.1673-3185.01976
    [6] 楼建坤, 王鸿东, 王检耀, 易宏2021: 基于机器学习的实海域无人艇避碰算法智能演进方法初探 , 中国舰船研究. doi: 10.19693/j.issn.1673-3185.02116
    [7] 路春晖, 张博, 付振楷, 尹美琳, 张嘉琪2021: 搜索算法在USV航迹规划中的应用 , 中国舰船研究. doi: 10.19693/j.issn.1673-3185.01604
    [8] 侯岳奇, 陶浩, 龚俊斌, 梁晓龙, 张诺2021: 多约束条件下无人艇和无人机集群协同航迹规划 , 中国舰船研究. doi: 10.19693/j.issn.1673-3185.02091
    [9] 李贺, 王宁, 薛皓原2020: 水面无人艇领航—跟随固定时间编队控制 , 中国舰船研究, 15, 111-118. doi: 10.19693/j.issn.1673-3185.01755
    [10] 王文晋, 王冠学, 李奔, 徐国华2020: 大尺度高速水下无人艇控制系统设计与试验验证 , 中国舰船研究, 15, 95-103. doi: 10.19693/j.issn.1673-3185.01759
    [11] 欧阳子路, 王鸿东, 黄一, 杨楷文, 易宏2020: 基于改进RRT算法的无人艇编队路径规划技术 , 中国舰船研究, 15, 18-24. doi: 10.19693/j.issn.1673-3185.01639
    [12] 陈于涛, 曹诗杰, 曾凡明2020: 一种用于无人艇目标跟踪的实时Q学习算法 , 中国舰船研究. doi: 10.19693/j.issn.1673-3185.01763
    [13] 张子昌, 林莉, 徐雪峰, 赵光2019: 基于模型预测控制的潜艇水平面航迹控制算法 , 中国舰船研究, 14, 172-178. doi: 10.19693/j.issn.1673-3185.01335
    [14] 向伦凯, 李晓, 蒋云, 杨咏林2019: 无人水面艇推进系统模型辨识 , 中国舰船研究, 14, 7-11,22. doi: 10.19693/j.issn.1673-3185.01200
    [15] 胡建章, 唐国元, 王建军, 解德2019: 基于自适应反步滑模的水面无人艇集群控制 , 中国舰船研究, 14, 1-7. doi: 10.19693/j.issn.1673-3185.01521
    [16] 张晓东, 刘世亮, 刘宇, 胡晓芳, 高超2018: 无人水面艇收放技术发展趋势探讨 , 中国舰船研究, 13, 50-57. doi: 10.19693/j.issn.1673-3185.01258
    [17] 薛彩霞, 袁伟, 俞孟蕻, 樊冀生2016: 遗传粒子群优化算法在船舶动力定位控制中的应用 , 中国舰船研究, 11, 111-115. doi: 10.3969/j.issn.1673-3185.2016.04.016
    [18] 严浙平, 段海璞2015: UUV航迹跟踪的双闭环Terminal滑模控制 , 中国舰船研究, 10, 112-117,142. doi: 10.3969/j.issn.1673-3185.2015.04.017
    [19] 曹诗杰, 曾凡明, 陈于涛2015: 无人水面艇航向航速协同控制方法 , 中国舰船研究, 10, 74-80. doi: 10.3969/j.issn.1673-3185.2015.06.011
    [20] 陈铭, 朱齐丹, 刘志林, 于瑞亭2010: 基于不同航行状态无人艇的水动力模型研究 , 中国舰船研究, 5, 1-5. doi: 10.3969/j.issn.1673-3185.2010.06.001
  • 加载中
图(10) / 表 (1)
计量
  • 文章访问数:  15
  • HTML全文浏览量:  9
  • PDF下载量:  1
  • 被引次数: 0
出版历程
  • 收稿日期:  2020-11-30
  • 修回日期:  2020-12-08
  • 网络出版日期:  2021-01-05

基于精确罚函数的无人艇航迹规划和自动避障算法

doi: 10.19693/j.issn.1673-3185.02209
    基金项目:  空间智能控制技术国防重点实验室2018年度基金资助项目(KGJZDSYS-2018-03)
    作者简介:

    李清亮,男,1998年生,硕士生。研究方向:智能体轨迹规划。E-mail:liqingliang@stu.scu.edu.cn

    李彬,男,1983年生,博士,研究员。研究方向:航空航天中的控制问题,无人系统自主控制,物理层保密通信,信能同传技术,数值优化。E-mail:bin.li@scu.edu.cn

    孙国皓,男,1990年生,博士,助理研究员。研究方向:分布式组网雷达信号处理,机载/星载雷达信号处理,智能感知技术。E-mail:sghsjw2005@126.com

    通讯作者: 李彬,; 孙国皓
  • 中图分类号: U664.82

摘要:   目的  对于已知多个障碍物的局部水域,如何规划安全高效的无人艇运动航迹,是当前的研究热点。  方法  首先,采用简洁有效的圆形包络面和凸四边形包络面处理障碍物区域,并将避障问题转化为时间最优控制问题中的状态不等式约束;然后,利用控制参数化和时间尺度变换,将时间最优控制问题转化为最优参数选择问题;最后,对于由多个障碍物带来的多个连续状态不等式约束,采用精确罚函数法将所有的状态约束都附加到目标函数中,从而构建适用于任何有效优化技术予以求解的非线性优化问题。  结果  数值仿真结果表明,该算法所规划的航迹能成功规避水域中的所有障碍物,同时符合无人艇的运动特性。  结论  研究成果可为无人艇航迹规划的避障问题提供参考。

English Abstract

李清亮, 李彬, 孙国皓, 等. 基于精确罚函数的无人艇航迹规划和自动避障算法[J]. 中国舰船研究, 2021, 16(1): 1–7 doi:  10.19693/j.issn.1673-3185.02209
引用本文: 李清亮, 李彬, 孙国皓, 等. 基于精确罚函数的无人艇航迹规划和自动避障算法[J]. 中国舰船研究, 2021, 16(1): 1–7 doi:  10.19693/j.issn.1673-3185.02209
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): 1–7 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): 1–7 doi:  10.19693/j.issn.1673-3185.02209
    • 近年来,无人驾驶受到日益广泛的关注,在飞行器、智能车、舰船等领域得以快速发展。智能化是未来舰船的发展趋势之一[1]。由于无人艇的制造成本低、制造周期短、环境适应能力强、人力成本低等,其在海洋资源勘测、航道测量、环境监测、水域清洁、军事作战等领域具备良好的应用前景。如何快速生成无人艇在复杂环境下的避障航线,是其自主化发展的关键技术之一[2]

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

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

    • 为简化处理,本文将无人艇视为仅规划其运动航迹的质点,其运动学坐标系如图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  无人艇运动学坐标系

      Figure 1.  Kinematic coordinate system of USV

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

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

      图  2  障碍物区域的包络图

      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 $。为使无人艇规避障碍物,则其位置应满足:

      $${\left( {x(t) - {x_i}} \right)^2} + {\left( {y(t) - {y_i}} \right)^2} \geqslant {R_i}^2$$ (3)

      图  3  圆形包络面建模

      Figure 3.  Circular envelope modeling

    • 对于长宽比较大的不规则障碍物,如果继续采用圆形包络面进行建模处理,会浪费大量的可航行区域,增加无人艇的航行距离,甚至出现无可航行区域情况。为此,宜采用凸四边形包络面进行建模处理。如图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$轴上的截距。

      图  4  凸四边形包络面建模

      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为转换后的时间刻度。由文献[14-15],可知

      图  5  时间尺度变换

      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个节点$\left\{ {0,{s_1},{s_2}, \cdots ,{s_K}} \right\}$,其中sK =1。分段参数化的结果如图6所示,其函数表达式为[16]

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

      图  6  控制参数化

      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,\\&\;\;\;\;\;\qquad 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,\\&\;\;\;\;\;\qquad 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,这就为问题求解带来了极大的挑战。为此,本文采用精确罚函数[14, 17]来处理约束S3~S7,用以将(P2)转化为仅含控制约束和初始状态约束的优化问题,其优化目标函数J为:

      $$ \begin{array}{c}\;\;\;\;\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\\ {\rm{s}}{\rm{. t}}{\rm{.\; S0}},{\rm{ S2}}\\ \;\;\;\; {\rm{S8}}:0< \epsilon \end{array}$$ (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}{l}\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)}\\& {{{\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,可以根据不同惩罚项的松弛程度来调整具体数值;关于其他参数的设置,仅需满足理论要求的约束即可[17]

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

      $$ \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所示。

      表 1  仿真数据

      Table 1.  Simulated data

      术语名称数值及内容
      最大加速度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}$

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

      图  7  无人艇的航迹图

      Figure 7.  Track map of USV

      图  8  无人艇位置随时间的变化图

      Figure 8.  Diagram of USV position changing with time

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

      图  9  无人艇速度随时间的变化曲线

      Figure 9.  Diagram of USV speed changing with time

      图  10  输入控制信号

      Figure 10.  Input control signal

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

参考文献 (17)

目录

    /

    返回文章
    返回