第10卷第3期 智能系统学报 Vol.10 No.3 2015年6月 CAAI Transactions on Intelligent Systems Jun.2015 D0:10.3969/j.issn.1673-4785.201405012 网络出版地址：http://www.cnki.net/kcms/detail/23.1538.tp.20150601.0940.001.html 实时避碰的无人水面机器人在线路径规划方法 冷静2，刘健，徐红丽1 (1.中国科学院沈阳自动化研究所，辽宁沈阳110016：2.中国科学院大学，北京100049) 摘要：针对动态环境下，无人水面机器人(USV)必须遵守国际海上避碰规则公约且实时在线路径规划的雄题，提出一 种无人水面机器人在动态环境下进行在线路径规划的方法。此方法将国际海上避碰规则公约融入速度避障法，将速度 避障法中的相对速度线性化，使其能作为约束融人混合整数线性规划器。同时将USV的本体动力学约束与环境约束 结合，采用多目标函数作为优化函数，根据任务的要求选择距离优化函数、速度优化函数和遵守COLREGs目标函数等 多个优化目标函数。最后，通过对USV在2种会遇情况规划路径的仿真实验分析，验证了此规划算法的有效性。 关键词：无人水面机器人；在线路径规划：速度避障法：国际海上避碰规则公约：混合整数线性规划：动态环境 中图分类号：TP242文献标志码：A文章编号：1673-4785(2015)03-0343-06 中文引用格式：冷静，刘健，徐红丽.实时避碰的无人水面机器人在线路径规划方法[J].智能系统学报，2015,10(3)：343-348。 英文引用格式：LENGJing,LIU Jiang,XU Hongli..Online path planning of an unmanned surface vehicle for real-time collision a- voidance[J].CAAI Transactions on Intelligent Systems,2015,10(3):343-348. Online path planning of an unmanned surface vehicle for real-time collision avoidance LENG Jing'2,LIU Jiang',XU Hongli' (1.Shenyang Institute of Automation,Chinese Academy of Sciences,Shenyang 110016,China;2.University of Chinese Academy of Sciences,Beijing 100049,China) Abstract:A new method of online path planning is proposed in order to solve the problem of online real-time path planning of the unmanned surface vehicle(USV)in a dynamic environment by complying with the Convention on the International Regulations for Preventing Collisions at Sea (COLREGs).This method merges the International Regulations for Preventing Collisions at Sea with velocity obstacle (VO)to linearize relative speed of VO and to make VO merge with the mixed-integer linear programming (MILP)as a set of velocity constraints.The USV body dynamics constraints and environmental constraints are combined using multi-objective function as the optimization function and choosing distance,speed and abeyance COLREGS as target functions.Finally,the simulation for the path planning in the two situations of encounter is analyzed and the results show the effectiveness of this planning algorithm. Keywords:unmanned surface vehicle (USV);online path planning;velocity obstacle;Convention on the Interna- tional Regulations for Preventing Collisions at Sea (COLREGs);mixed-integer linear programming MILP);dy- namic environment 无人水面机器人(unmanned surface vehicle, USV)在海上航行时需要具备自主规避障碍的能力， 在线路径规划提供了一种远程障碍规避的手段。在 收稿日期：2014-05-06.网络出版日期：2015-06-01. 线路径规划是指基于雷达获得的船舶、岛屿等障碍 基金项目：中国科学院重点部署项目子课题(KGFZD-125-014) 通信作者：冷静.E-mail:jingleng77@163.com. 信息，实时规划自身的速度和航向以产生避开障碍
第 １０ 卷第 ３ 期 智 能 系 统 学 报 Ｖｏｌ．１０ №．３ ２０１５ 年 ６ 月 ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ Ｊｕｎ． ２０１５ ＤＯＩ：１０．３９６９ ／ ｊ．ｉｓｓｎ．１６７３⁃４７８５．２０１４０５０１２ 网络出版地址：ｈｔｔｐ： ／ ／ ｗｗｗ．ｃｎｋｉ．ｎｅｔ ／ ｋｃｍｓ／ ｄｅｔａｉｌ ／ ２３．１５３８．ｔｐ．２０１５０６０１．０９４０．００１．ｈｔｍｌ 实时避碰的无人水面机器人在线路径规划方法 冷静１，２ ，刘健１ ，徐红丽１ （１．中国科学院 沈阳自动化研究所，辽宁 沈阳 １１００１６；２．中国科学院大学，北京 １０００４９） 摘 要：针对动态环境下，无人水面机器人（ＵＳＶ）必须遵守国际海上避碰规则公约且实时在线路径规划的难题，提出一 种无人水面机器人在动态环境下进行在线路径规划的方法。 此方法将国际海上避碰规则公约融入速度避障法，将速度 避障法中的相对速度线性化，使其能作为约束融入混合整数线性规划器。 同时将 ＵＳＶ 的本体动力学约束与环境约束 结合，采用多目标函数作为优化函数，根据任务的要求选择距离优化函数、速度优化函数和遵守 ＣＯＬＲＥＧｓ 目标函数等 多个优化目标函数。 最后，通过对 ＵＳＶ 在 ２ 种会遇情况规划路径的仿真实验分析，验证了此规划算法的有效性。 关键词：无人水面机器人；在线路径规划；速度避障法；国际海上避碰规则公约；混合整数线性规划；动态环境 中图分类号：ＴＰ２４２ 文献标志码：Ａ 文章编号：１６７３⁃４７８５（２０１５）０３⁃０３４３⁃０６ 中文引用格式：冷静，刘健，徐红丽． 实时避碰的无人水面机器人在线路径规划方法［Ｊ］． 智能系统学报， ２０１５， １０（３）： ３４３⁃３４８． 英文引用格式：ＬＥＮＧ Ｊｉｎｇ， ＬＩＵ Ｊｉａｎｇ， ＸＵ Ｈｏｎｇｌｉ． Ｏｎｌｉｎｅ ｐａｔｈ ｐｌａｎｎｉｎｇ ｏｆ ａｎ ｕｎｍａｎｎｅｄ ｓｕｒｆａｃｅ ｖｅｈｉｃｌｅ ｆｏｒ ｒｅａｌ⁃ｔｉｍｅ ｃｏｌｌｉｓｉｏｎ ａ⁃ ｖｏｉｄａｎｃｅ［Ｊ］． ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ， ２０１５， １０（３）： ３４３⁃３４８． Ｏｎｌｉｎｅ ｐａｔｈ ｐｌａｎｎｉｎｇ ｏｆ ａｎ ｕｎｍａｎｎｅｄ ｓｕｒｆａｃｅ ｖｅｈｉｃｌｅ ｆｏｒ ｒｅａｌ⁃ｔｉｍｅ ｃｏｌｌｉｓｉｏｎ ａｖｏｉｄａｎｃｅ ＬＥＮＧ Ｊｉｎｇ １，２ ， ＬＩＵ Ｊｉａｎｇ １ ， ＸＵ Ｈｏｎｇｌｉ １ （１． Ｓｈｅｎｙａｎｇ Ｉｎｓｔｉｔｕｔｅ ｏｆ Ａｕｔｏｍａｔｉｏｎ， Ｃｈｉｎｅｓｅ Ａｃａｄｅｍｙ ｏｆ Ｓｃｉｅｎｃｅｓ， Ｓｈｅｎｙａｎｇ １１００１６， Ｃｈｉｎａ； ２． Ｕｎｉｖｅｒｓｉｔｙ ｏｆ Ｃｈｉｎｅｓｅ Ａｃａｄｅｍｙ ｏｆ Ｓｃｉｅｎｃｅｓ， Ｂｅｉｊｉｎｇ １０００４９， Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ａ ｎｅｗ ｍｅｔｈｏｄ ｏｆ ｏｎｌｉｎｅ ｐａｔｈ ｐｌａｎｎｉｎｇ ｉｓ ｐｒｏｐｏｓｅｄ ｉｎ ｏｒｄｅｒ ｔｏ ｓｏｌｖｅ ｔｈｅ ｐｒｏｂｌｅｍ ｏｆ ｏｎｌｉｎｅ ｒｅａｌ⁃ｔｉｍｅ ｐａｔｈ ｐｌａｎｎｉｎｇ ｏｆ ｔｈｅ ｕｎｍａｎｎｅｄ ｓｕｒｆａｃｅ ｖｅｈｉｃｌｅ （ＵＳＶ） ｉｎ ａ ｄｙｎａｍｉｃ ｅｎｖｉｒｏｎｍｅｎｔ ｂｙ ｃｏｍｐｌｙｉｎｇ ｗｉｔｈ ｔｈｅ Ｃｏｎｖｅｎｔｉｏｎ ｏｎ ｔｈｅ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｒｅｇｕｌａｔｉｏｎｓ ｆｏｒ Ｐｒｅｖｅｎｔｉｎｇ Ｃｏｌｌｉｓｉｏｎｓ ａｔ Ｓｅａ （ＣＯＬＲＥＧｓ）． Ｔｈｉｓ ｍｅｔｈｏｄ ｍｅｒｇｅｓ ｔｈｅ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｒｅｇｕｌａｔｉｏｎｓ ｆｏｒ Ｐｒｅｖｅｎｔｉｎｇ Ｃｏｌｌｉｓｉｏｎｓ ａｔ Ｓｅａ ｗｉｔｈ ｖｅｌｏｃｉｔｙ ｏｂｓｔａｃｌｅ （ＶＯ） ｔｏ ｌｉｎｅａｒｉｚｅ ｒｅｌａｔｉｖｅ ｓｐｅｅｄ ｏｆ ＶＯ ａｎｄ ｔｏ ｍａｋｅ ＶＯ ｍｅｒｇｅ ｗｉｔｈ ｔｈｅ ｍｉｘｅｄ⁃ｉｎｔｅｇｅｒ ｌｉｎｅａｒ ｐｒｏｇｒａｍｍｉｎｇ （ＭＩＬＰ） ａｓ ａ ｓｅｔ ｏｆ ｖｅｌｏｃｉｔｙ ｃｏｎｓｔｒａｉｎｔｓ． Ｔｈｅ ＵＳＶ ｂｏｄｙ ｄｙｎａｍｉｃｓ ｃｏｎｓｔｒａｉｎｔｓ ａｎｄ ｅｎｖｉｒｏｎｍｅｎｔａｌ ｃｏｎｓｔｒａｉｎｔｓ ａｒｅ ｃｏｍｂｉｎｅｄ ｕｓｉｎｇ ｍｕｌｔｉ⁃ｏｂｊｅｃｔｉｖｅ ｆｕｎｃｔｉｏｎ ａｓ ｔｈｅ ｏｐｔｉｍｉｚａｔｉｏｎ ｆｕｎｃｔｉｏｎ ａｎｄ ｃｈｏｏｓｉｎｇ ｄｉｓｔａｎｃｅ， ｓｐｅｅｄ ａｎｄ ａｂｅｙａｎｃｅ ＣＯＬＲＥＧＳ ａｓ ｔａｒｇｅｔ ｆｕｎｃｔｉｏｎｓ． Ｆｉｎａｌｌｙ， ｔｈｅ ｓｉｍｕｌａｔｉｏｎ ｆｏｒ ｔｈｅ ｐａｔｈ ｐｌａｎｎｉｎｇ ｉｎ ｔｈｅ ｔｗｏ ｓｉｔｕａｔｉｏｎｓ ｏｆ ｅｎｃｏｕｎｔｅｒ ｉｓ ａｎａｌｙｚｅｄ ａｎｄ ｔｈｅ ｒｅｓｕｌｔｓ ｓｈｏｗ ｔｈｅ ｅｆｆｅｃｔｉｖｅｎｅｓｓ ｏｆ ｔｈｉｓ ｐｌａｎｎｉｎｇ ａｌｇｏｒｉｔｈｍ． Ｋｅｙｗｏｒｄｓ：ｕｎｍａｎｎｅｄ ｓｕｒｆａｃｅ ｖｅｈｉｃｌｅ （ＵＳＶ）； ｏｎｌｉｎｅ ｐａｔｈ ｐｌａｎｎｉｎｇ； ｖｅｌｏｃｉｔｙ ｏｂｓｔａｃｌｅ； Ｃｏｎｖｅｎｔｉｏｎ ｏｎ ｔｈｅ Ｉｎｔｅｒｎａ⁃ ｔｉｏｎａｌ Ｒｅｇｕｌａｔｉｏｎｓ ｆｏｒ Ｐｒｅｖｅｎｔｉｎｇ Ｃｏｌｌｉｓｉｏｎｓ ａｔ Ｓｅａ （ＣＯＬＲＥＧｓ）； ｍｉｘｅｄ⁃ｉｎｔｅｇｅｒ ｌｉｎｅａｒ ｐｒｏｇｒａｍｍｉｎｇ （ＭＩＬＰ）； ｄｙ⁃ ｎａｍｉｃ ｅｎｖｉｒｏｎｍｅｎｔ 收稿日期：２０１４⁃０５⁃０６． 网络出版日期：２０１５⁃０６⁃０１． 基金项目：中国科学院重点部署项目子课题（ＫＧＦＺＤ⁃１２５⁃０１４）． 通信作者：冷静． Ｅ⁃ｍａｉｌ： ｊｉｎｇｌｅｎｇ７７＠ １６３．ｃｏｍ． 无人水面机器人 （ ｕｎｍａｎｎｅｄ ｓｕｒｆａｃｅ ｖｅｈｉｃｌｅ， ＵＳＶ）在海上航行时需要具备自主规避障碍的能力， 在线路径规划提供了一种远程障碍规避的手段。 在 线路径规划是指基于雷达获得的船舶、岛屿等障碍 信息，实时规划自身的速度和航向以产生避开障碍
·344. 智能系统学报 第10卷 到达目标的最优或次最优路径（无障碍物路径）。 1)目标：1个远程静态路径点。 根据对环境的获知程度，路径规划分为全局路径规 2)障碍物：视一定范围内的水面船舶为运动障 划和局部路径规划，在线路径规划是一种局部路径 碍物，通过传感器获得其速度、航向和位置。 规划，因为部分环境是未知的。 3)USV运动模型：包含运动学和动力学，应用 无人水面机器人在海洋中在线路径规划的特点 其在仿真中可实时模拟USV的运动状态。 是：1)海洋环境具有动态、突发性、不可预测性。2) 4)假设条件：因为船体本身具有的惯性比较 USV具有6个自由度、偏航、前进、横移，也会出现 大，所以假设极短的规划时间△1内，障碍物保持匀 横倾、纵倾，体积小、速度快、灵活性高、瞬时的失误 速运动，即USV相对运动障碍物的速度改变量等于 可能造成无法弥补的损失等运动特性。3)USV预 USV自身的速度改变量。 先定义好的航线和驾驶规则。常用的在线路径规划 由此，在线路径规划的任务即是在每个规划时 算法采用局部避障算法结合全局规划算法，如障碍 间间隔搜索出最优的速度矢量增量，使其既能遵守 物边界追踪结合融入A·搜索算法[山。然而这种方 COLREGs规则躲避运动的障碍物，同时能搜索到达 法并没有考虑国际海上避碰规则公约(convention 目标的全局最优或次最优（无碰撞）的优化路径。 on the international regulations for preventing collisions 1.2融入COLREGs规则的改进速度避障法 at sea,COLREGs)I。COLREGs指出了当存在碰 基于线性速度障碍物LV-Obstacle的一阶算法 撞的危险时，应该采取的避碰行为。当USV与其他 原理在文献[7]有详细的说明，本文在此基础上进 船舶相遇时，其导航算法必须遵守COLREGs,才能 行改进，提出基于COLREGs规则避障的改进速度 安全躲避其他船舶，这样其他船舶上的驾驶人就能 避障算法。速度避障法二维原理图如图1所示。将 预测它所采取的安全行为。许多学者提出了遵守 USV缩小为一个点U,障碍物膨胀为一个圆，圆心为 COLREGs的导航方法，如模糊逻辑[)、区间优化4)。 点O,两者速度矢量分别为"。和v。。相对速度为 然而这些算法对多个COLREGs规则的应用不是很 vuo=vu-vo。USV与障碍物中心的连线为Lo,相 好，而且在实时性需求方面并不能达到理想。Yo- 对速度'o与Lo的夹角为yo,USV与障碍物圆的 shiaki等[s)将COLREGs规定的可行速度区与速 2条交线分别为UN和UM,相对速度'o与UN和 度障碍区同时作为约束条件，以交叉检测的碰撞 UM的夹角分别为△yio和△yto,相对速度"o延长 时间和下个路径点的期望速度作为代价函数，在 线与障碍物圆的最短交点为D。 速度-航向(-0)空间搜索最好的速度矢量。这 种方法能快速有效地避开障碍物，然而他们也指出， 使用的速度避障法只是一种局部规划器，要想实现 远程距离的路径规划必须加人全局规划器。COL D 0 REGs本身就包含有避碰行动信息，与速度避障法 能非常好的融合，同时使速度避障法更好地运用于 M USV与船舶会遇的避碰行动中，使避碰行动完全合 理合法。祖迪等[采用MLP(mixed-integer linear programming)进行移动机器人的路径规划，将移动 机器人的本体动力学和速度避障法角度约束结合， 取得比较好的效果。 图1速度避障法二维原理图 Fig.1 Schematics of velocity obstacle 本文对USV在动态环境下进行在线路径规划 进行研究，采用基于速度避障法的局部规划器规避 1.2.1相关定义 障碍，基于MLP的全局规划器实现距离时间的优 1)膨胀半径：R=V,/y,碰撞半径等于会遇船 化，使规划的路径既能遵守COLREGs规则，安全地 舶的安全距离。安全距离取决于相对速度和转向 避开障碍，同时获得全局路径优化。 率，见文献[8]。 1 改进速度避障法 2)避碰区(collision cone,CC): 1.1问题描述 Co={"o1Lo∩0≠⑦] 本文研究的USV在线路径规划问题如下： 即所有导致USV与障碍物碰撞的相对速度'o,它 本质为相对速度的速度集
到达目标的最优或次最优路径（无障碍物路径）。 根据对环境的获知程度，路径规划分为全局路径规 划和局部路径规划，在线路径规划是一种局部路径 规划，因为部分环境是未知的。 无人水面机器人在海洋中在线路径规划的特点 是：１）海洋环境具有动态、突发性、不可预测性。 ２） ＵＳＶ 具有 ６ 个自由度、偏航、前进、横移，也会出现 横倾、纵倾，体积小、速度快、灵活性高、瞬时的失误 可能造成无法弥补的损失等运动特性。 ３） ＵＳＶ 预 先定义好的航线和驾驶规则。 常用的在线路径规划 算法采用局部避障算法结合全局规划算法，如障碍 物边界追踪结合融入 Ａ ∗ 搜索算法［１］ 。 然而这种方 法并没有考虑国际海上避碰规则公约（ ｃｏｎｖｅｎｔｉｏｎ ｏｎ ｔｈｅ ｉｎｔｅｒｎａｔｉｏｎａｌ ｒｅｇｕｌａｔｉｏｎｓ ｆｏｒ ｐｒｅｖｅｎｔｉｎｇ ｃｏｌｌｉｓｉｏｎｓ ａｔ ｓｅａ， ＣＯＬＲＥＧｓ） ［２］ 。 ＣＯＬＲＥＧｓ 指出了当存在碰 撞的危险时，应该采取的避碰行为。 当 ＵＳＶ 与其他 船舶相遇时，其导航算法必须遵守 ＣＯＬＲＥＧｓ，才能 安全躲避其他船舶，这样其他船舶上的驾驶人就能 预测它所采取的安全行为。 许多学者提出了遵守 ＣＯＬＲＥＧｓ 的导航方法，如模糊逻辑［３］ 、区间优化［４］ 。 然而这些算法对多个 ＣＯＬＲＥＧｓ 规则的应用不是很 好，而且在实时性需求方面并不能达到理想。 Ｙｏ⁃ ｓｈｉａｋｉ 等［ ５］ 将 ＣＯＬＲＥＧｓ 规定的可行速度区与速 度障碍区同时作为约束条件，以交叉检测的碰撞 时间和下个路径点的期望速度作为代价函数，在 速度－航向（ ｖ⁃θ ） 空间搜索最好的速度矢量。 这 种方法能快速有效地避开障碍物，然而他们也指出， 使用的速度避障法只是一种局部规划器，要想实现 远程距离的路径规划必须加入全局规划器。 ＣＯＬ⁃ ＲＥＧｓ 本身就包含有避碰行动信息，与速度避障法 能非常好的融合，同时使速度避障法更好地运用于 ＵＳＶ 与船舶会遇的避碰行动中，使避碰行动完全合 理合法。 祖迪等［６］ 采用 ＭＩＬＰ （ ｍｉｘｅｄ⁃ｉｎｔｅｇｅｒ ｌｉｎｅａｒ ｐｒｏｇｒａｍｍｉｎｇ）进行移动机器人的路径规划，将移动 机器人的本体动力学和速度避障法角度约束结合， 取得比较好的效果。 本文对 ＵＳＶ 在动态环境下进行在线路径规划 进行研究，采用基于速度避障法的局部规划器规避 障碍，基于 ＭＩＬＰ 的全局规划器实现距离时间的优 化，使规划的路径既能遵守 ＣＯＬＲＥＧｓ 规则，安全地 避开障碍，同时获得全局路径优化。 １ 改进速度避障法 １．１ 问题描述 本文研究的 ＵＳＶ 在线路径规划问题如下： １）目标：１ 个远程静态路径点。 ２）障碍物：视一定范围内的水面船舶为运动障 碍物，通过传感器获得其速度、航向和位置。 ３）ＵＳＶ 运动模型：包含运动学和动力学，应用 其在仿真中可实时模拟 ＵＳＶ 的运动状态。 ４）假设条件：因为船体本身具有的惯性比较 大，所以假设极短的规划时间 Δｔ 内，障碍物保持匀 速运动，即 ＵＳＶ 相对运动障碍物的速度改变量等于 ＵＳＶ 自身的速度改变量。 由此，在线路径规划的任务即是在每个规划时 间间隔搜索出最优的速度矢量增量，使其既能遵守 ＣＯＬＲＥＧｓ 规则躲避运动的障碍物，同时能搜索到达 目标的全局最优或次最优（无碰撞）的优化路径。 １．２ 融入 ＣＯＬＲＥＧｓ 规则的改进速度避障法 基于线性速度障碍物 ＬＶ⁃Ｏｂｓｔａｃｌｅ 的一阶算法 原理在文献［７］有详细的说明，本文在此基础上进 行改进，提出基于 ＣＯＬＲＥＧｓ 规则避障的改进速度 避障算法。 速度避障法二维原理图如图 １ 所示。 将 ＵＳＶ 缩小为一个点 Ｕ，障碍物膨胀为一个圆，圆心为 点 Ｏ，两者速度矢量分别为 ｖＵ 和 ｖＯ 。 相对速度为 ｖＵＯ ＝ ｖＵ － ｖＯ 。 ＵＳＶ 与障碍物中心的连线为 ＬＵＯ ，相 对速度 ｖＵＯ 与 ＬＵＯ 的夹角为 γ ＵＯ ，ＵＳＶ 与障碍物圆的 ２ 条交线分别为 ＵＮ 和 ＵＭ ，相对速度 ｖＵＯ 与 ＵＮ 和 ＵＭ 的夹角分别为 Δγ Ｒ ＵＯ 和 Δγ Ｌ ＵＯ ，相对速度 ｖＵＯ 延长 线与障碍物圆的最短交点为 Ｄ。 图 １ 速度避障法二维原理图 Ｆｉｇ． １ Ｓｃｈｅｍａｔｉｃｓ ｏｆ ｖｅｌｏｃｉｔｙ ｏｂｓｔａｃｌｅ １．２．１ 相关定义 １）膨胀半径： Ｒ ＝ Ｖｒ ／ γ ，碰撞半径等于会遇船 舶的安全距离。 安全距离取决于相对速度和转向 率，见文献［８］。 ２）避碰区（ｃｏｌｌｉｓｉｏｎ ｃｏｎｅ， ＣＣ）： ＣＵＯ ＝ ｛ｖＵＯ ｜ ＬＤＯ ∩ Ｏ ≠ ∅｝ 即所有导致 ＵＳＶ 与障碍物碰撞的相对速度 ｖＵＯ ，它 本质为相对速度的速度集。 ·３４４· 智 能 系 统 学 报 第 １０ 卷
第3期 冷静，等：实时避碰的无人水面机器人在线路径规划方法 ·345. 3)避碰角： 把机器人本体动力学、运动学以及环境不确定性等 △yuo= 约束考虑进去，并且能解决障碍物转向的“或”的逻 1 辑问题，得到一条关于目标函数最优且满足所有约 √voLoP-p 束条件的最优路径。由于MLP要求目标函数和约 (1) 束均为线性，因此将这些约束和目标函数线性化。 即在规划时间内，相对速度夹角的改变量。本文避 2.1目标函数 碰策略是首先将相对速度增量线性化为避碰角，然 1)第1类目标函数：距离收敛。 后通过避碰角选择方向来脱离避碰区，最终实现避 USV通过调整速度大小和方向，每规划一个时 碰过程。式(1)的线性化推导流程见文献[9]。 间间隔都使下一时刻USV离目标越来越近，即 4)碰撞时间：T=Lo/"o|,即当相对速度 Luc→0，min:pu-pol 处于碰撞区时，USV与障碍物的碰撞时间。 4=l-(+△y)△1，j=1,2(2) 5)碰撞危险度：p=(a×DCPA)2+ 式中j表示维数，即将到目标的距离分解为X、Y轴， (b×TCPA)2,其中a,b分别表示DCPA和TCPA的 这样才可以用于线性规划器中。△y:是所要规划的变 权值。碰撞危险度通过DCPA和TCPA加权确定。 量，即为每个规划时间，相对目标的速度改变量。 6)规划时间：T=0p+w2/ヶ，即规划时间与碰 2)第2类目标函数：速度最优。 撞危险度成正比，与碰撞时间成反比。因为受到 将相对目标的速度分解为指向目标的分量V。 USV本体运动性能约束，其转角加速度和纵向加速 和垂直目标的分量V,。要想使到达障碍物的速度 度都有限制，所以碰撞危险度越大，碰撞时间越短， 最优，只需最大化Vc和最小化V,即可。即 规划时间就越短，否则无法及时进行航速和方向的 max:Vel=|Vuc lcos Yuc 规划。 min:Vrl=Vuc lsin Yuc (3) 1.2.2避碰策略 所以最小化为|V,2,即 从上述避障法的原理可以看出，要使USV不与 min:g(Vue)=IVr2=IVucl2-Pic/IVicll Lucl2 障碍物发生碰撞，其相对速度"0必须产生一个转 由泰勒公式可得 角，使其脱离碰撞区。从图1可以看出它有2个方 向可以选择，当向左转时，即使相对速度产生△y △g(Vc)=Vg·△V+o(‖AVcI) 的转动，向右转时，使相对速度产生△y。的转动。 Vg =2Vr-2Puc/|Luc2 LuG min:g+△g 这个转动是由规划时间内相对速度矢量△"0的变 因此推理公式(3)转化为 化产生的，式(1)可以看出。因此，将相对速度矢量 的增量△o作为待规划的变量。这样就形成了 q1=g(Vc）+Vg·AVic USV的避碰模型： 9=-Vc-V.·AVc (4) 综上所述可得线性规划器的目标函数： y≤- Yw Lwo min:J=∑m,d+m91+m42 △n≤T ∑m+m+m,=1,m>0,m3>0,m>0,i=1,2 OR Yw Lw -T≤ 式中：m1、m2,m3、m4分别代表上述4个目标函数 √/"oPLoo'-P 的权值。 Aviw≤△yo 2.2USV本体动力学自身约束 1)加速度的约束：-△r≤△，≤△。 2混合线性规划方法 2)速度的约束：-Vx≤△y;+;≤Vmso 速度避障法解决的是局部实时避碰的问题，但 因为线性规划只有一个上下限，所以速度变量 容易陷入局部最小值，所以在对USV进行在线路径 的上下限整合为 规划的时候，必须加入全局规划器。在基本LP中， max{-△mr,-Vo-"g}≤△y,≤min{△s,Vt-vg} 各约束条件之间形成“与”的逻辑关系，对于具有复 角加速度的约束：-△y0≤△yuo≤△yo。 杂逻辑关系的优化问题不能直接求解。用MLP方 2.3障碍物约束 法解决动态环境下的路径规划问题，可以很容易地 根据上一节对速度避障法的避碰策略可知：
３）避碰角： ΔγＵＯ ＝ － １ ｖＵＯ ２ ＬＵＯ ２ － Ｐ ２ ＬＵＯ － Ｐ ｖＵＯ ２ ｖＵＯ é ë ê ê ù û ú ú·ΔｖＵＯ （１） 即在规划时间内，相对速度夹角的改变量。 本文避 碰策略是首先将相对速度增量线性化为避碰角，然 后通过避碰角选择方向来脱离避碰区，最终实现避 碰过程。 式（１）的线性化推导流程见文献［９］。 ４）碰撞时间： τ ＝ ＬＵＤ ／ ｖＵＯ ，即当相对速度 处于碰撞区时，ＵＳＶ 与障碍物的碰撞时间。 ５ ） 碰 撞 危 险 度： ρ ＝ （ａ × ＤＣＰＡ） ２ ＋ （ｂ × ＴＣＰＡ） ２ ，其中 ａ， ｂ 分别表示 ＤＣＰＡ 和 ＴＣＰＡ 的 权值。 碰撞危险度通过 ＤＣＰＡ 和 ＴＣＰＡ 加权确定。 ６）规划时间： Ｔ ＝ ｗ１ ρ ＋ ｗ２ ／ τ ，即规划时间与碰 撞危险度成正比，与碰撞时间成反比。 因为受到 ＵＳＶ 本体运动性能约束，其转角加速度和纵向加速 度都有限制，所以碰撞危险度越大，碰撞时间越短， 规划时间就越短，否则无法及时进行航速和方向的 规划。 １．２．２ 避碰策略 从上述避障法的原理可以看出，要使 ＵＳＶ 不与 障碍物发生碰撞，其相对速度 ｖＵＯ 必须产生一个转 角，使其脱离碰撞区。 从图 １ 可以看出它有 ２ 个方 向可以选择，当向左转时，即使相对速度产生 Δγ Ｒ ＵＯ 的转动，向右转时，使相对速度产生 Δγ Ｌ ＵＯ 的转动。 这个转动是由规划时间内相对速度矢量 ΔｖＵＯ 的变 化产生的，式（１）可以看出。 因此，将相对速度矢量 的增量 ΔｖＵＯ 作为待规划的变量。 这样就形成了 ＵＳＶ 的避碰模型： ＯＲ Δγ Ｒ ＵＯ ≤－ ｖＵＯ ＬＵＯ ｖＵＯ ２ ＬＵＯ ２ － Ｐ ２ ＬＵＯ － Ｐ ｖＵＯ ２ ｖＵＯ é ë ê ê ù û ú ú· Δｖ Ｔ ＵＯ ≤π － π ≤－ ｖＵＯ ＬＵＯ ｖＵＯ ２ ＬＵＯ ２ － Ｐ ２ ＬＵＯ － Ｐ ｖＵＯ ２ ｖＵＯ é ë ê ê ù û ú ú· Δｖ Ｔ ＵＯ ≤Δγ Ｌ ＵＯ ì î í ï ï ï ï ï ï ï ï ïï ２ 混合线性规划方法 速度避障法解决的是局部实时避碰的问题，但 容易陷入局部最小值，所以在对 ＵＳＶ 进行在线路径 规划的时候，必须加入全局规划器。 在基本 ＬＰ 中， 各约束条件之间形成“与”的逻辑关系，对于具有复 杂逻辑关系的优化问题不能直接求解。 用 ＭＩＬＰ 方 法解决动态环境下的路径规划问题，可以很容易地 把机器人本体动力学、运动学以及环境不确定性等 约束考虑进去，并且能解决障碍物转向的“或”的逻 辑问题，得到一条关于目标函数最优且满足所有约 束条件的最优路径。 由于 ＭＩＬＰ 要求目标函数和约 束均为线性，因此将这些约束和目标函数线性化。 ２．１ 目标函数 １）第 １ 类目标函数：距离收敛。 ＵＳＶ 通过调整速度大小和方向，每规划一个时 间间隔都使下一时刻 ＵＳＶ 离目标越来越近，即 ＬＵＧ → ０，ｍｉｎ： ｐＵ － ｐＯ ｄｊ ＝ ｌ ｊ － （ｖｊ ＋ Δｖｊ）Δｔ ， ｊ ＝ １， ２ （２） 式中：ｊ 表示维数，即将到目标的距离分解为 Ｘ、Ｙ 轴， 这样才可以用于线性规划器中。 Δｖｊ 是所要规划的变 量，即为每个规划时间，相对目标的速度改变量。 ２）第 ２ 类目标函数：速度最优。 将相对目标的速度分解为指向目标的分量 ＶＣ 和垂直目标的分量 ＶＴ 。 要想使到达障碍物的速度 最优，只需最大化 ＶＣ 和最小化 ＶＴ 即可。 即 ｍａｘ： ＶＣ ＝ ＶＵＧ ｃｏｓ γＵＧ ｍｉｎ： ＶＴ ＝ ＶＵＧ ｓｉｎ γＵＧ （３） 所以最小化为 ＶＴ ２ ，即 ｍｉｎ：ｇ（ＶＵＧ） ＝ ＶＴ ２ ＝ ＶＵＧ ２ － Ｐ ２ ＵＧ ／ Ｖ ２ ＵＧ ＬＵＧ ２ 由泰勒公式可得 Δｇ（ＶＵＧ ） ＝ Ñｇ·ΔＶ Ｔ ＵＧ ＋ ｏ（‖ΔＶＵＧ‖） Ñｇ ＝ ２ ＶＴ － ２ＰＵＧ ／ ＬＵＧ ２ ＬＵＧ ｍｉｎ：ｇ ＋ Δｇ 因此推理公式（３）转化为 ｑ１ ＝ ｇ（ＶＵＧ） ＋ Ñｇ·ΔＶ Ｔ ＵＧ ｑ２ ＝ － ＶＣ － ÑＶｃ·ΔＶ Ｔ ＵＧ （４） 综上所述可得线性规划器的目标函数： ｍｉｎ：Ｊ ＝ ∑ｉ ｍｉｄｉ ＋ ｍ３ ｑ１ ＋ ｍ４ ｑ２ ∑ｉ ｍｉ ＋ ｍ３ ＋ ｍ４ ＝ １，ｍｉ ＞ ０，ｍ３ ＞ ０，ｍ４ ＞ ０，ｉ ＝ １，２ 式中： ｍ１ 、 ｍ２、 ｍ３、 ｍ４ 分别代表上述 ４ 个目标函数 的权值。 ２．２ ＵＳＶ 本体动力学自身约束 １）加速度的约束： － Δｍａｘ £Δｖｊ £Δｍａｘ 。 ２）速度的约束： － Ｖｍａｘ £Δｖｊ ＋ ｖｊ £Ｖｍａｘ 。 因为线性规划只有一个上下限，所以速度变量 的上下限整合为 ｍａｘ｛ － Δｍａｘ， － Ｖ ｍａｘ Ｕ － ｖＵｊ｝ £Δｖｊ £ｍｉｎ｛Δｍａｘ，Ｖ ｍａｘ Ｕ － ｖＵｊ｝ 角加速度的约束： － Δγ ｍａｘ ＵＯ £Δγ ＵＯ £Δγ ｍａｘ ＵＯ 。 ２．３ 障碍物约束 根据上一节对速度避障法的避碰策略可知： 第 ３ 期 冷静，等：实时避碰的无人水面机器人在线路径规划方法 ·３４５·
·346. 智能系统学报 第10卷 Ywo Lvo P 进行下一个规划周期，重复步骤1)~8)，直到目标 √T'oP LvoP-p 到达才结束。 △v0≤m+e,E 开始 Vvo Luo AISAPAR MILP+VO 雷达声呐探测 规划器 √YvoLuo下-pl 计算TCPADCPA 控制方法 △vio≥△yio-e,5 N Yuo Lvo USV P <是否危险 √vuoP LvoP-p2 小 下个 本体呐 会遇局面 △iw≥-T-f话 航速探 划分决萤 航向测 Yuo Lvo P 建模 √T"oP LcoP-pL 障碍物 声呐探测 达到目标 本体运动学 △vo≤△yio+ff 约束 范围约束 动力学约束 (5) 结 式中：e:+f=1,e:和f为二进制变量（取0或 约束建模 1),专为正实数，且远大于式(5)中不等式左侧可 图2在线路径规划流程 取的数值。 Fig.2 Flowchart of online path planning 式(5)确保在同一规划时间段[，：+△t]之 内，有且仅有1组（前2个或后2个）约束成立。此 3 仿真验证 处，为了遵守COLREGs规则，当右交叉相遇和对遇 为了说明此规划方法的有效性，采用实际的 时，e:=0,f=1,式(5)中前2不等式被激活，后2 USV动力学模型进行实验仿真，采用的USV模型是 个不等式失效：当左交叉相遇时，e:=1,f=0,前2 Ribcraft模型1o,其回转速度为l5.8/s,采用PID 个不等式失效，后2个不等式被激活。 控制器对其进行航行控制。下面给出对遇局面和交 2.4算法流程 叉局面的仿真实验结果，如图3和4。图3所示为 总结上述体系结构、并融合COLREGs的在线 对遇局面，其初始环境具体参数见表1。图4所示 路径规划流程图如图2。具体的算法流程如下： 为右交叉局面，其初始环境具体参数见表2。 1)通过APAR雷达获得自身的经纬度和航向， 仿真开发环境在MATLAB环境下。在仿真中 通过AIS接收器获得对方船舶的距离和航向。 用大圆代表USV,小圆代表运动障碍物，最上面的 2)通过公式计算出每艘船舶与USV之间的最 圆代表目标点。USV通过雷达进行探测，最大探测 近会遇时间TCPA和最近会遇距离DCPA。 距离为30nm。图3(a)和4(a)图表示初始状态，图 3)通过危险度判断模型判断是否处于危险状 3(b)和4(b)表示USV进行实时在线路径规划，每 态。如果危险则开启避碰行为进行第4)步，否则不 一个时间段规划出最优的速度矢量，图3(c)和4 实施避碰行为。 (c)表示最终到达目标点，图3(d)和4(d)表示在规 4)进行船舶与USV的会遇局面划分及行为决策。 划中的规划（期望）与USV实际的比较。图3(b)和 5)对本体运动学、动力学、探测范围、障碍物约 4(b)中，USV在此种情况下右转向避碰，符合COL- 束和目标函数进行建模。 REGs规则的规定。通过圆圈稀疏可知当危险解 6)将约束的建模加入MLP规划器中，由规划 除，规划时间变短，并符合COLREGs规定的从后方 器可获得这个规划间隔时间里USV期望的转向角 绕过障碍物的规定。 及速度。 表1对遇局面态势仿真环境 7)将期望的航向角和速度输入到航行控制器 Table 1 Simulation environment of head-on situation 中，通过航行控制器输出发动机的转速和舵角作用 于USV实际模型中，使USV能达到期望的航向和 物体 初始位置/nm初始速度/八(m·s1)半径/m 航速。 USV (0.00.0.00 (0.00.8.00) 12 8)检测是否达到目标，如果达到则结束，否则 运动障碍 (0.00.7.00)】 (-0.00.-5.00) 10
－ ｖＵＯ ＬＵＯ ｖＵＯ ２ ＬＵＯ ２ － Ｐ ２ ＬＵＯ － Ｐ ｖＵＯ ２ ｖＵＯ é ë ê ê ù û ú ú· Δｖ Ｔ ＵＯ £π ＋ ｅｉ ξ － ｖＵＯ ＬＵＯ ｖＵＯ ２ ＬＵＯ ２ － Ｐ ２ ＬＵＯ － Ｐ ｖＵＯ ２ ｖＵＯ é ë ê ê ù û ú ú· Δｖ Ｔ ＵＯ ⩾ Δγ Ｒ ＵＯ － ｅｉ ξ － ｖＵＯ ＬＵＯ ｖＵＯ ２ ＬＵＯ ２ － Ｐ ２ ＬＵＯ － Ｐ ｖＵＯ ２ ｖＵＯ é ë ê ê ù û ú ú· Δｖ Ｔ ＵＯ ⩾－ π － ｆ ｉ ξ － ｖＵＯ ＬＵＯ ｖＵＯ ２ ＬＵＯ ２ － Ｐ ２ ＬＵＯ － Ｐ ｖＵＯ ２ ｖＵＯ é ë ê ê ù û ú ú· Δｖ Ｔ ＵＯ £Δγ Ｌ ＵＯ ＋ ｆ ｉ ξ ì î í ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï ï （５） 式中： ｅｉ ＋ ｆ ｉ ＝ １， ｅｉ 和 ｆ ｉ 为二进制变量（ 取 ０ 或 １） ， ξ 为正实数，且远大于式（ ５）中不等式左侧可 取的数值。 式（５）确保在同一规划时间段 ［ｔ ｋ，ｔ ｋ ＋ Δｔ］ 之 内，有且仅有 １ 组（前 ２ 个或后 ２ 个）约束成立。 此 处，为了遵守 ＣＯＬＲＥＧｓ 规则，当右交叉相遇和对遇 时， ｅｉ ＝ ０， ｆ ｉ ＝ １，式（５）中前 ２ 不等式被激活，后 ２ 个不等式失效；当左交叉相遇时， ｅｉ ＝ １， ｆ ｉ ＝ ０，前 ２ 个不等式失效，后 ２ 个不等式被激活。 ２．４ 算法流程 总结上述体系结构、并融合 ＣＯＬＲＥＧｓ 的在线 路径规划流程图如图 ２。 具体的算法流程如下： １）通过 ＡＰＡＲ 雷达获得自身的经纬度和航向， 通过 ＡＩＳ 接收器获得对方船舶的距离和航向。 ２）通过公式计算出每艘船舶与 ＵＳＶ 之间的最 近会遇时间 ＴＣＰＡ 和最近会遇距离 ＤＣＰＡ。 ３）通过危险度判断模型判断是否处于危险状 态。 如果危险则开启避碰行为进行第 ４）步，否则不 实施避碰行为。 ４）进行船舶与 ＵＳＶ 的会遇局面划分及行为决策。 ５）对本体运动学、动力学、探测范围、障碍物约 束和目标函数进行建模。 ６）将约束的建模加入 ＭＩＬＰ 规划器中，由规划 器可获得这个规划间隔时间里 ＵＳＶ 期望的转向角 及速度。 ７）将期望的航向角和速度输入到航行控制器 中，通过航行控制器输出发动机的转速和舵角作用 于 ＵＳＶ 实际模型中，使 ＵＳＶ 能达到期望的航向和 航速。 ８）检测是否达到目标，如果达到则结束，否则 进行下一个规划周期，重复步骤 １） ～ ８），直到目标 到达才结束。 图 ２ 在线路径规划流程 Ｆｉｇ． ２ Ｆｌｏｗｃｈａｒｔ ｏｆ ｏｎｌｉｎｅ ｐａｔｈ ｐｌａｎｎｉｎｇ ３ 仿真验证 为了说明此规划方法的有效性，采用实际的 ＵＳＶ 动力学模型进行实验仿真，采用的 ＵＳＶ 模型是 Ｒｉｂｃｒａｆｔ 模型［１０］ ，其回转速度为 １５．８ °／ ｓ，采用 ＰＩＤ 控制器对其进行航行控制。 下面给出对遇局面和交 叉局面的仿真实验结果，如图 ３ 和 ４。 图 ３ 所示为 对遇局面，其初始环境具体参数见表 １。 图 ４ 所示 为右交叉局面，其初始环境具体参数见表 ２。 仿真开发环境在 ＭＡＴＬＡＢ 环境下。 在仿真中 用大圆代表 ＵＳＶ，小圆代表运动障碍物，最上面的 圆代表目标点。 ＵＳＶ 通过雷达进行探测，最大探测 距离为 ３０ ｎｍ。 图 ３（ａ）和 ４（ａ）图表示初始状态，图 ３（ｂ）和 ４（ｂ）表示 ＵＳＶ 进行实时在线路径规划，每 一个时间段规划出最优的速度矢量，图 ３（ ｃ） 和 ４ （ｃ）表示最终到达目标点，图 ３（ｄ）和 ４（ｄ）表示在规 划中的规划（期望）与 ＵＳＶ 实际的比较。 图 ３（ｂ）和 ４（ｂ）中，ＵＳＶ 在此种情况下右转向避碰，符合 ＣＯＬ⁃ ＲＥＧｓ 规则的规定。 通过圆圈稀疏可知当危险解 除，规划时间变短，并符合 ＣＯＬＲＥＧｓ 规定的从后方 绕过障碍物的规定。 表 １ 对遇局面态势仿真环境 Ｔａｂｌｅ １ Ｓｉｍｕｌａｔｉｏｎ ｅｎｖｉｒｏｎｍｅｎｔ ｏｆ ｈｅａｄ⁃ｏｎ ｓｉｔｕａｔｉｏｎ 物体 初始位置／ ｎｍ 初始速度／ （ｍ·ｓ －１ ） 半径／ ｍ ＵＳＶ （０．００， ０．００） （０．００， ８．００） １２ 运动障碍 （０．００， ７．００） （－０．００， －５．００） １０ ·３４６· 智 能 系 统 学 报 第 １０ 卷
第3期 冷静，等：实时避碰的无人水面机器人在线路径规划方法 .347. 表2交叉局面态势仿真环境 目标导航点 Table 2 Simulation environment of cross situation 物体 初始位置/nm初始速度/(m·sl)半径/m 。整动船的 USV (0.00.0.00)】 (0.01,8.00) 12 预规划轨迹 运动障碍 (4.00,4.00) (-5.00.-0.01) 10 USV 10-8-6-4-20246810 1中 X海里 124 (a)初始会遇状态，发现障碍物 10 目标导航点 》 目标导航点 到8 运动船舶 运动船加 到 3 4 预设的轨迹 USV olUSV -2 0 46 10-8-6-4-20246810 XW海里 XW海里 (a)初始会遇状态，发现障碍物 (b)开始转向 12 10 ,目标导航点 8 目标导航点 USV 6 运动船们 5 运动船舶 CE 3 2 预规划轨迹 /USV 预规划轨迹 实际运动轨迹 0 2 4 XW海里 -10-8-6-4-20246810 (b)开始转向 X7海里 (c)到达目标点 目标导航点 60 10 USV 士浪的然向角 230 6 预规划轨迹 E-20 运动船舶 -40 60 X7海里 -801 e (c)到达目标点 -100 0 5 01内202方3610 tis 30 十碳资务向角 (d)规划的航向角与实际的航向角 20◆. 图4交叉局面态势 104 Fig.4 Situation of cross 09 。ne -104 结束语 -204 o -30 本文通过对在线路径规划算法进行实验仿真可 -40 50内202方303为4010 知，新算法在遵守COLREGs规则的同时，能获得较 t/s 好的规划路径。然而本文并没有对最优化与 (d)规划的航向角与实际的航向角 COREGs规则存在的矛盾进行进一步讨论，因为在 图3对遇局面态势 选择航向的过程中，最优化的结果有可能与C0L Fig.3 Situation of head-on REGs规则冲突，在这种情况下，需要引进一个选择
表 ２ 交叉局面态势仿真环境 Ｔａｂｌｅ ２ Ｓｉｍｕｌａｔｉｏｎ ｅｎｖｉｒｏｎｍｅｎｔ ｏｆ ｃｒｏｓｓ ｓｉｔｕａｔｉｏｎ 物体 初始位置／ ｎｍ 初始速度／ （ｍ·ｓ －１ ） 半径／ ｍ ＵＳＶ （０．００， ０．００） （０．０１， ８．００） １２ 运动障碍 （４．００， ４．００） （－５．００， －０．０１） １０ （ａ）初始会遇状态，发现障碍物 （ｂ）开始转向 （ｃ）到达目标点 （ｄ）规划的航向角与实际的航向角 图 ３ 对遇局面态势 Ｆｉｇ． ３ Ｓｉｔｕａｔｉｏｎ ｏｆ ｈｅａｄ⁃ｏｎ （ａ）初始会遇状态，发现障碍物 （ｂ）开始转向 （ｃ）到达目标点 （ｄ）规划的航向角与实际的航向角 图 ４ 交叉局面态势 Ｆｉｇ． ４ Ｓｉｔｕａｔｉｏｎ ｏｆ ｃｒｏｓｓ ４ 结束语 本文通过对在线路径规划算法进行实验仿真可 知，新算法在遵守 ＣＯＬＲＥＧｓ 规则的同时，能获得较 好的 规 划 路 径。 然 而 本 文 并 没 有 对 最 优 化 与 ＣＯＲＥＧｓ 规则存在的矛盾进行进一步讨论，因为在 选择航向的过程中，最优化的结果有可能与 ＣＯＬ⁃ ＲＥＧｓ 规则冲突，在这种情况下，需要引进一个选择 第 ３ 期 冷静，等：实时避碰的无人水面机器人在线路径规划方法 ·３４７·
.348. 智能系统学报 第10卷 算法，遵循优先级的高低来选择每种会遇局面最合 ZU Di,HAN Jianda,TAN Dalong.LP-based path planning 适的规划。 method in acceleration space for mobile robot[J].Acta Au- tomatica Sinica,2007,33(10):1036-1042. 参考文献： [8]COLLEY B A,CURTIS R G,STOCKEL C T.Manoeuvring [1]CASALINO G,TURETTA A,SIMETTI E.A three-layered times,domains and arenas [J].Journal of Navigation, architecture for real time path planning and obstacle avoid- 1983,36(2):324-328. ance for surveillance USVs operating in harbour fields[C]// [9]程大军，刘开周.基于MP的AUV实时优化行为方法 IEEE OCEANS 2009-Europe.Bremen,Germany,2009:1- 研究[J].机械设计与制造，2012(4)：91-93. 8. CHENG Dajun,LIU Kaizhou.Research on real-time optimi- [2]International Maritime Organization.International regulations zation behavior of AUV based on MILP[J].Machinery De- for prevention of collisions at sea,1972 (COLREGs)[EB/ sign Manufacture,2012(4):91-93. OL].[2014-04-25].http://www.imo.org/About/Conven- [10]SONNENBURG C R,WOOLSEY C A.Modeling,identifi- tions/ListOfConventions/Pages/COLREG.aspx. cation,and control of an unmanned surface vehicle[J]. [3]PERERA L P,CARVALHO J P,SOARES C G.Autono- Journal of Field Robotics,2013,30(3):371-398. mous guidance and navigation based on the COLREGs rules 作者简介： and regulations of collision avoidance [C]//Proceedings of 冷静，女，1990年生，硕士研究生 the International Workshop "Advanced Ship Design for Pol- 主要研究方向为无人水面机器人的在 lution Prevention".Split,Croatia,2009:205-216. 线路径规划。曾获中国科学院大学“三 [4]BENJAMIN M R,CURCIO J A,LEONARD JJ,et al. 好学生”和国家奖学金。 Navigation of unmanned marine vehicles in accordance with the rules of the road[C]//2006 IEEE International Confer- ence on Robotics and Automation.Orlando,USA,2006: 刘健，男，1962年生，研究员，主要 3581.3587. 研究方向为工业控制、图像跟踪、水下 [5]KUWATA Y,WOLF M T,ZARZHITSKY D,et al.Safe 机器人控制和组合导航技术，主持完成 maritime navigation with COLREGS,using velocity obsta- 国家重大项目多项。 cles[C]//2011 IEEE/RSJ International Conference on In- telligent Robots and Systems.San Francisco,USA,4728- 4734. [6]FIORINI P,SHILLER Z.Motion planning in dynamic envi- 徐红丽，女，1979年生，副研究员， 工学博士，主要研究方向为水下机器人 ronments using velocity obstacles [J].The International 控制、水下声呐图像处理、多传感器数 Journal of Robotics Research,1998,17(7):760-772. 据融合与环境建模。曾主持国家自然 [7]祖迪，韩建达，谈大龙.加速度空间中基于线性规划的 科学基金项目1项、国家“863”计划项 移动机器人路径规划方法[J].自动化学报，2007,33 目1项，并参与国家及省部级重点项目 (10)：1036-1042 多项，发表学术论文10余篇
算法，遵循优先级的高低来选择每种会遇局面最合 适的规划。 参考文献： ［１］ＣＡＳＡＬＩＮＯ Ｇ， ＴＵＲＥＴＴＡ Ａ， ＳＩＭＥＴＴＩ Ｅ． Ａ ｔｈｒｅｅ⁃ｌａｙｅｒｅｄ ａｒｃｈｉｔｅｃｔｕｒｅ ｆｏｒ ｒｅａｌ ｔｉｍｅ ｐａｔｈ ｐｌａｎｎｉｎｇ ａｎｄ ｏｂｓｔａｃｌｅ ａｖｏｉｄ⁃ ａｎｃｅ ｆｏｒ ｓｕｒｖｅｉｌｌａｎｃｅ ＵＳＶｓ ｏｐｅｒａｔｉｎｇ ｉｎ ｈａｒｂｏｕｒ ｆｉｅｌｄｓ［Ｃ］ ／ ／ ＩＥＥＥ ＯＣＥＡＮＳ ２００９－Ｅｕｒｏｐｅ． Ｂｒｅｍｅｎ， Ｇｅｒｍａｎｙ， ２００９： １⁃ ８． ［２］Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｍａｒｉｔｉｍｅ Ｏｒｇａｎｉｚａｔｉｏｎ． Ｉｎｔｅｒｎａｔｉｏｎａｌ ｒｅｇｕｌａｔｉｏｎｓ ｆｏｒ ｐｒｅｖｅｎｔｉｏｎ ｏｆ ｃｏｌｌｉｓｉｏｎｓ ａｔ ｓｅａ， １９７２ （ＣＯＬＲＥＧｓ） ［ＥＢ／ ＯＬ］． ［ ２０１４⁃０４⁃２５］． ｈｔｔｐ： ／ ／ ｗｗｗ． ｉｍｏ． ｏｒｇ ／ Ａｂｏｕｔ ／ Ｃｏｎｖｅｎ⁃ ｔｉｏｎｓ／ ＬｉｓｔＯｆＣｏｎｖｅｎｔｉｏｎｓ／ Ｐａｇｅｓ／ ＣＯＬＲＥＧ．ａｓｐｘ． ［３］ＰＥＲＥＲＡ Ｌ Ｐ， ＣＡＲＶＡＬＨＯ Ｊ Ｐ， ＳＯＡＲＥＳ Ｃ Ｇ． Ａｕｔｏｎｏ⁃ ｍｏｕｓ ｇｕｉｄａｎｃｅ ａｎｄ ｎａｖｉｇａｔｉｏｎ ｂａｓｅｄ ｏｎ ｔｈｅ ＣＯＬＲＥＧｓ ｒｕｌｅｓ ａｎｄ ｒｅｇｕｌａｔｉｏｎｓ ｏｆ ｃｏｌｌｉｓｉｏｎ ａｖｏｉｄａｎｃｅ ［Ｃ］ ／ ／ Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ ｔｈｅ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｗｏｒｋｓｈｏｐ “Ａｄｖａｎｃｅｄ Ｓｈｉｐ Ｄｅｓｉｇｎ ｆｏｒ Ｐｏｌ⁃ ｌｕｔｉｏｎ Ｐｒｅｖｅｎｔｉｏｎ”． Ｓｐｌｉｔ， Ｃｒｏａｔｉａ， ２００９： ２０５⁃２１６． ［４］ ＢＥＮＪＡＭＩＮ Ｍ Ｒ， ＣＵＲＣＩＯ Ｊ Ａ， ＬＥＯＮＡＲＤ Ｊ Ｊ， ｅｔ ａｌ． Ｎａｖｉｇａｔｉｏｎ ｏｆ ｕｎｍａｎｎｅｄ ｍａｒｉｎｅ ｖｅｈｉｃｌｅｓ ｉｎ ａｃｃｏｒｄａｎｃｅ ｗｉｔｈ ｔｈｅ ｒｕｌｅｓ ｏｆ ｔｈｅ ｒｏａｄ［Ｃ］ ／ ／ ２００６ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒ⁃ ｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ． Ｏｒｌａｎｄｏ， ＵＳＡ， ２００６： ３５８１⁃３５８７． ［５］ＫＵＷＡＴＡ Ｙ， ＷＯＬＦ Ｍ Ｔ， ＺＡＲＺＨＩＴＳＫＹ Ｄ， ｅｔ ａｌ． Ｓａｆｅ ｍａｒｉｔｉｍｅ ｎａｖｉｇａｔｉｏｎ ｗｉｔｈ ＣＯＬＲＥＧＳ， ｕｓｉｎｇ ｖｅｌｏｃｉｔｙ ｏｂｓｔａ⁃ ｃｌｅｓ［Ｃ］ ／ ／ ２０１１ ＩＥＥＥ／ ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎ⁃ ｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ． Ｓａｎ Ｆｒａｎｃｉｓｃｏ， ＵＳＡ， ４７２８⁃ ４７３４． ［６］ＦＩＯＲＩＮＩ Ｐ， ＳＨＩＬＬＥＲ Ｚ． Ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇ ｉｎ ｄｙｎａｍｉｃ ｅｎｖｉ⁃ ｒｏｎｍｅｎｔｓ ｕｓｉｎｇ ｖｅｌｏｃｉｔｙ ｏｂｓｔａｃｌｅｓ ［ Ｊ ］． Ｔｈｅ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｒｎａｌ ｏｆ Ｒｏｂｏｔｉｃｓ Ｒｅｓｅａｒｃｈ， １９９８， １７（７）： ７６０⁃７７２． ［７］祖迪， 韩建达， 谈大龙． 加速度空间中基于线性规划的 移动机器人路径规划方法［ Ｊ］． 自动化学报， ２００７， ３３ （１０）： １０３６⁃１０４２． ＺＵ Ｄｉ， ＨＡＮ Ｊｉａｎｄａ， ＴＡＮ Ｄａｌｏｎｇ． ＬＰ⁃ｂａｓｅｄ ｐａｔｈ ｐｌａｎｎｉｎｇ ｍｅｔｈｏｄ ｉｎ ａｃｃｅｌｅｒａｔｉｏｎ ｓｐａｃｅ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔ［Ｊ］． Ａｃｔａ Ａｕ⁃ ｔｏｍａｔｉｃａ Ｓｉｎｉｃａ， ２００７， ３３（１０）： １０３６⁃１０４２． ［８］ＣＯＬＬＥＹ Ｂ Ａ， ＣＵＲＴＩＳ Ｒ Ｇ， ＳＴＯＣＫＥＬ Ｃ Ｔ． Ｍａｎｏｅｕｖｒｉｎｇ ｔｉｍｅｓ， ｄｏｍａｉｎｓ ａｎｄ ａｒｅｎａｓ ［ Ｊ ］． Ｊｏｕｒｎａｌ ｏｆ Ｎａｖｉｇａｔｉｏｎ， １９８３， ３６（２）： ３２４⁃３２８． ［９］程大军， 刘开周． 基于 ＭＩＬＰ 的 ＡＵＶ 实时优化行为方法 研究［Ｊ］． 机械设计与制造， ２０１２（４）： ９１⁃９３． ＣＨＥＮＧ Ｄａｊｕｎ， ＬＩＵ Ｋａｉｚｈｏｕ． Ｒｅｓｅａｒｃｈ ｏｎ ｒｅａｌ⁃ｔｉｍｅ ｏｐｔｉｍｉ⁃ ｚａｔｉｏｎ ｂｅｈａｖｉｏｒ ｏｆ ＡＵＶ ｂａｓｅｄ ｏｎ ＭＩＬＰ［ Ｊ］． Ｍａｃｈｉｎｅｒｙ Ｄｅ⁃ ｓｉｇｎ ＆ Ｍａｎｕｆａｃｔｕｒｅ， ２０１２（４）： ９１⁃９３． ［１０］ＳＯＮＮＥＮＢＵＲＧ Ｃ Ｒ， ＷＯＯＬＳＥＹ Ｃ Ａ． Ｍｏｄｅｌｉｎｇ， ｉｄｅｎｔｉｆｉ⁃ ｃａｔｉｏｎ， ａｎｄ ｃｏｎｔｒｏｌ ｏｆ ａｎ ｕｎｍａｎｎｅｄ ｓｕｒｆａｃｅ ｖｅｈｉｃｌｅ ［ Ｊ］． Ｊｏｕｒｎａｌ ｏｆ Ｆｉｅｌｄ Ｒｏｂｏｔｉｃｓ， ２０１３， ３０（３）： ３７１⁃３９８． 作者简介： 冷静，女，１９９０ 年生，硕士研究生， 主要研究方向为无人水面机器人的在 线路径规划。 曾获中国科学院大学“三 好学生”和国家奖学金。 刘健，男，１９６２ 年生，研究员，主要 研究方向为工业控制、图像跟踪、水下 机器人控制和组合导航技术，主持完成 国家重大项目多项。 徐红丽，女，１９７９ 年生，副研究员， 工学博士，主要研究方向为水下机器人 控制、水下声呐图像处理、多传感器数 据融合与环境建模。 曾主持国家自然 科学基金项目 １ 项、国家“ ８６３” 计划项 目 １ 项，并参与国家及省部级重点项目 多项，发表学术论文 １０ 余篇。 ·３４８· 智 能 系 统 学 报 第 １０ 卷