文章快速检索  
  高级检索
实时避碰的无人水面机器人在线路径规划方法
冷静1,2, 刘健1, 徐红丽1
1. 中国科学院 沈阳自动化研究所, 辽宁 沈阳 110016;
2. 中国科学院大学, 北京 100049
基金项目:中国科学院重点部署项目子课题(KGFZD-125-014).     
摘要:针对动态环境下,无人水面机器人(USV)必须遵守国际海上避碰规则公约且实时在线路径规划的难题,提出一种无人水面机器人在动态环境下进行在线路径规划的方法.此方法将国际海上避碰规则公约融入速度避障法,将速度避障法中的相对速度线性化,使其能作为约束融入混合整数线性规划器.同时将USV的本体动力学约束与环境约束结合,采用多目标函数作为优化函数,根据任务的要求选择距离优化函数、速度优化函数和遵守COLREGs目标函数等多个优化目标函数.最后,通过对USV在2种会遇情况规划路径的仿真实验分析,验证了此规划算法的有效性.
关键词无人水面机器人     在线路径规划     速度避障法     国际海上避碰规则公约     混合整数线性规划     动态环境    
Online path planning of an unmanned surface vehicle for real-time collision avoidance
LENG Jing1,2 , LIU Jiang1, XU Hongli1
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.
Key words: unmanned surface vehicle (USV)     online path planning     velocity obstacle     Convention on the International Regulations for Preventing Collisions at Sea (COLREGs)     mixed-integer linear programming (MILP)     dynamic environment    

无人水面机器人(unmanned surface vehicle,USV)在海上航行时需要具备自主规避障碍的能力,在线路径规划提供了一种远程障碍规避的手段。在线路径规划是指基于雷达获得的船舶、岛屿等障碍信息,实时规划自身的速度和航向以产生避开障碍、到达目标的最优或次最优路径(无障碍物路径)。根据对环境的获知程度,路径规划分为全局路径规划和局部路径规划,在线路径规划是一种局部路径规划,因为部分环境是未知的。

无人水面机器人在海洋中在线路径规划的特点是:1)海洋环境具有动态、突发性、不可预测性。2)USV具有6个自由度、偏航、前进、横移,也会出现横倾、纵倾,体积小、速度快、灵活性高、瞬时的失误可能造成无法弥补的损失等运动特性。3)USV预先定义好的航线和驾驶规则。常用的在线路径规划算法采用局部避障算法结合全局规划算法,如障碍物边界追踪结合融入A*搜索算法[1]。然而这种方法并没有考虑国际海上避碰规则公约(convention on the international regulations for preventing collisions at sea,COLREGs)[2]。COLREGs指出了当存在碰撞的危险时,应该采取的避碰行为。当USV与其他船舶相遇时,其导航算法必须遵守COLREGs,才能安全躲避其他船舶,这样其他船舶上的驾驶人就能预测它所采取的安全行为。许多学者提出了遵守COLREGs的导航方法,如模糊逻辑[3]、区间优化[4]。然而这些算法对多个COLREGs规则的应用不是很好,而且在实时性需求方面并不能达到理想。Yoshiaki等[5]将COLREGs规定的可行速度区与速度障碍区同时作为约束条件,以交叉检测的碰撞时间和下个路径点的期望速度作为代价函数,在速度-航向(v-θ)空间搜索最好的速度矢量。这种方法能快速有效地避开障碍物,然而他们也指出,使用的速度避障法只是一种局部规划器,要想实现远程距离的路径规划必须加入全局规划器。COLREGs本身就包含有避碰行动信息,与速度避障法能非常好的融合,同时使速度避障法更好地运用于USV与船舶会遇的避碰行动中,使避碰行动完全合理合法。祖迪等[6]采用MILP (mixed-integer linear programming)进行移动机器人的路径规划,将移动机器人的本体动力学和速度避障法角度约束结合,取得比较好的效果。

本文对USV在动态环境下进行在线路径规划进行研究,采用基于速度避障法的局部规划器规避障碍,基于MILP的全局规划器实现距离时间的优化,使规划的路径既能遵守COLREGs规则,安全地避开障碍,同时获得全局路径优化。

1 改进速度避障法 1.1 问题描述

本文研究的USV在线路径规划问题如下:

1)目标:1个远程静态路径点。

2)障碍物:视一定范围内的水面船舶为运动障碍物,通过传感器获得其速度、航向和位置。

3)USV运动模型:包含运动学和动力学,应用其在仿真中可实时模拟USV的运动状态。

4)假设条件:因为船体本身具有的惯性比较大,所以假设极短的规划时间Δt内,障碍物保持匀速运动,即USV相对运动障碍物的速度改变量等于USV自身的速度改变量。

由此,在线路径规划的任务即是在每个规划时间间隔搜索出最优的速度矢量增量,使其既能遵守COLREGs规则躲避运动的障碍物,同时能搜索到达目标的全局最优或次最优(无碰撞)的优化路径。

1.2 融入COLREGs规则的改进速度避障法

基于线性速度障碍物LV-Obstacle的一阶算法原理在文献[7]有详细的说明,本文在此基础上进行改进,提出基于COLREGs规则避障的改进速度避障算法。速度避障法二维原理图如图 1所示。将USV缩小为一个点U,障碍物膨胀为一个圆,圆心为点O,两者速度矢量分别为vUvO。相对速度为vUO=vU-vO。USV与障碍物中心的连线为LUO,相对速度vUOLUO的夹角为γUO,USV与障碍物圆的2条交线分别为UNUM,相对速度vUOUNUM的夹角分别为ΔγUOR和ΔγUOL,相对速度vUO延长线与障碍物圆的最短交点为D

图 1 速度避障法二维原理图 Fig. 1 Schematics of velocity obstacle
1.2.1 相关定义

1)膨胀半径:R=Vr,碰撞半径等于会遇船舶的安全距离。安全距离取决于相对速度和转向率,见文献[8]

2)避碰区(collision cone,CC):

即所有导致USV与障碍物碰撞的相对速度vUO,它本质为相对速度的速度集。

3)避碰角:

即在规划时间内,相对速度夹角的改变量。本文避碰策略是首先将相对速度增量线性化为避碰角,然后通过避碰角选择方向来脱离避碰区,最终实现避碰过程。式(1)的线性化推导流程见文献[9]

4)碰撞时间:τ=|LUD|/|vUO|,即当相对速度处于碰撞区时,USV与障碍物的碰撞时间。

5)碰撞危险度:ρ=(a×DCPA)2+(b×TCPA)2,其中a,b分别表示DCPA和TCPA的权值。碰撞危险度通过DCPA和TCPA加权确定。

6)规划时间:T=w1ρ+w2/τ,即规划时间与碰撞危险度成正比,与碰撞时间成反比。因为受到USV本体运动性能约束,其转角加速度和纵向加速度都有限制,所以碰撞危险度越大,碰撞时间越短,规划时间就越短,否则无法及时进行航速和方向的规划。

1.2.2 避碰策略

从上述避障法的原理可以看出,要使USV不与障碍物发生碰撞,其相对速度vUO必须产生一个转角,使其脱离碰撞区。从图 1可以看出它有2个方向可以选择,当向左转时,即使相对速度产生ΔγUOR的转动,向右转时,使相对速度产生ΔγUOL的转动。这个转动是由规划时间内相对速度矢量ΔvUO的变化产生的,式(1)可以看出。因此,将相对速度矢量的增量ΔvUO作为待规划的变量。这样就形成了USV的避碰模型:

2 混合线性规划方法

速度避障法解决的是局部实时避碰的问题,但容易陷入局部最小值,所以在对USV进行在线路径规划的时候,必须加入全局规划器。在基本LP中,各约束条件之间形成“与”的逻辑关系,对于具有复杂逻辑关系的优化问题不能直接求解。用MILP方法解决动态环境下的路径规划问题,可以很容易地把机器人本体动力学、运动学以及环境不确定性等约束考虑进去,并且能解决障碍物转向的“或”的逻辑问题,得到一条关于目标函数最优且满足所有约束条件的最优路径。由于MILP要求目标函数和约束均为线性,因此将这些约束和目标函数线性化。

2.1 目标函数

1)第1类目标函数:距离收敛。

USV通过调整速度大小和方向,每规划一个时间间隔都使下一时刻USV离目标越来越近,即

式中:j表示维数,即将到目标的距离分解为XY轴,这样才可以用于线性规划器中。Δvj是所要规划的变量,即为每个规划时间,相对目标的速度改变量。

2)第2类目标函数:速度最优。

将相对目标的速度分解为指向目标的分量VC和垂直目标的分量VT。要想使到达障碍物的速度最优,只需最大化VC和最小化VT即可。即

所以最小化为|VT|2,即
由泰勒公式可得

因此推理公式(3)转化为

综上所述可得线性规划器的目标函数:

式中:m1m2m3m4分别代表上述4个目标函数的权值。

2.2 USV本体动力学自身约束

1)加速度的约束:-Δmax≤Δvj≤Δmax

2)速度的约束: -Vmax≤Δvj+vjVmax

因为线性规划只有一个上下限,所以速度变量的上下限整合为

角加速度的约束:-ΔγUOmax≤ΔγUO≤ΔγUOmax

2.3 障碍物约束

根据上一节对速度避障法的避碰策略可知:

式中:ei+fi=1,eifi为二进制变量(取0或1),ξ为正实数,且远大于式(5)中不等式左侧可取的数值。

式(5)确保在同一规划时间段[tk,tkt]之内,有且仅有1组(前2个或后2个)约束成立。此处,为了遵守COLREGs规则,当右交叉相遇和对遇时,ei=0,fi=1,式(5)中前2不等式被激活,后2个不等式失效;当左交叉相遇时,ei=1,fi=0,前2个不等式失效,后2个不等式被激活。

2.4 算法流程

总结上述体系结构、并融合COLREGs的在线路径规划流程图如图 2。具体的算法流程如下:

1)通过APAR雷达获得自身的经纬度和航向,通过AIS接收器获得对方船舶的距离和航向。

2)通过公式计算出每艘船舶与USV之间的最近会遇时间TCPA和最近会遇距离DCPA。

3)通过危险度判断模型判断是否处于危险状态。如果危险则开启避碰行为进行第4)步,否则不实施避碰行为。

4)进行船舶与USV的会遇局面划分及行为决策。

5)对本体运动学、动力学、探测范围、障碍物约束和目标函数进行建模。

6)将约束的建模加入MILP规划器中,由规划器可获得这个规划间隔时间里USV期望的转向角及速度。

7)将期望的航向角和速度输入到航行控制器中,通过航行控制器输出发动机的转速和舵角作用于USV实际模型中,使USV能达到期望的航向和航速。

8)检测是否达到目标,如果达到则结束,否则进行下一个规划周期,重复步骤1)~8),直到目标到达才结束。

图 2 在线路径规划流程 Fig. 2 Flowchart of online path planning
3 仿真验证

为了说明此规划方法的有效性,采用实际的USV动力学模型进行实验仿真,采用的USV模型是Ribcraft模型[10],其回转速度为15.8 °/s,采用PID控制器对其进行航行控制。下面给出对遇局面和交叉局面的仿真实验结果,如图 34图 3所示为对遇局面,其初始环境具体参数见表 1图 4所示为右交叉局面,其初始环境具体参数见表 2

仿真开发环境在MATLAB环境下。在仿真中用大圆代表USV,小圆代表运动障碍物,最上面的圆代表目标点。USV通过雷达进行探测,最大探测距离为30 nm。图 3 (a)4(a)图表示初始状态,图 3 (b)4(b)表示USV进行实时在线路径规划,每一个时间段规划出最优的速度矢量,图 3 (c)4(c)表示最终到达目标点,图 3 (d)4(d)表示在规划中的规划(期望)与USV实际的比较。图 3 (b)4(b)中,USV在此种情况下右转向避碰,符合COLREGs规则的规定。通过圆圈稀疏可知当危险解除,规划时间变短,并符合COLREGs规定的从后方绕过障碍物的规定。

表 1 对遇局面态势仿真环境 Table 1 Simulation environment of head-on situation
物体初始位置/nm初始速度/(m·s-1)半径/m
USV(0.00,0.00)(0.00,8.00)12
运动障碍(0.00,7.00)(-0.00,-5.00)10

表 2 交叉局面态势仿真环境 Table 2 Simulation environment of cross situation
物体初始位置/nm初始速度/(m·s-1)半径/m
USV(0.00,0.00)(0.01,8.00)12
运动障碍(4.00,4.00)(-5.00,-0.01)10

图 3 对遇局面态势 Fig. 3 Situation of head-on

图 4 交叉局面态势 Fig. 4 Situation of cross
4 结束语

本文通过对在线路径规划算法进行实验仿真可知,新算法在遵守COLREGs规则的同时,能获得较好的规划路径。然而本文并没有对最优化与COREGs规则存在的矛盾进行进一步讨论,因为在选择航向的过程中,最优化的结果有可能与COLREGs规则冲突,在这种情况下,需要引进一个选择算法,遵循优先级的高低来选择每种会遇局面最合适的规划。

参考文献
[1] CASALINO G, TURETTA A, SIMETTI E. A three-layered architecture for real time path planning and obstacle avoidance for surveillance USVs operating in harbour fields[C]//IEEE OCEANS 2009-Europe. Bremen, Germany, 2009: 1-8.
[2] International Maritime Organization. International regulations for prevention of collisions at sea, 1972 (COLREGs)[EB/OL]. [2014-04-25]. http://www.imo.org/About/Conventions/ListOfConventions/Pages/COLREG.aspx.
[3] PERERA L P, CARVALHO J P, SOARES C G. Autonomous guidance and navigation based on the COLREGs rules and regulations of collision avoidance [C]//Proceedings of the International Workshop “Advanced Ship Design for Pollution Prevention”. Split, Croatia, 2009: 205-216.
[4] BENJAMIN M R, CURCIO J A, LEONARD J J, et al. Navigation of unmanned marine vehicles in accordance with the rules of the road[C]//2006 IEEE International Conference on Robotics and Automation. Orlando, USA, 2006: 3581-3587.
[5] KUWATA Y, WOLF M T, ZARZHITSKY D, et al. Safe maritime navigation with COLREGS, using velocity obstacles[C]//2011 IEEE/RSJ International Conference on Intelligent Robots and Systems. San Francisco, USA, 4728-4734.
[6] FIORINI P, SHILLER Z. Motion planning in dynamic environments using velocity obstacles[J]. The International Journal of Robotics Research, 1998, 17(7): 760-772.
[7] 祖迪, 韩建达, 谈大龙. 加速度空间中基于线性规划的移动机器人路径规划方法[J]. 自动化学报, 2007, 33(10): 1036-1042. ZU Di, HAN Jianda, TAN Dalong. LP-based path planning method in acceleration space for mobile robot[J]. Acta Automatica Sinica, 2007, 33(10): 1036-1042.
[8] COLLEY B A, CURTIS R G, STOCKEL C T. Manoeuvring times, domains and arenas[J]. Journal of Navigation, 1983, 36(2): 324-328.
[9] 程大军, 刘开周. 基于MILP的AUV实时优化行为方法研究[J]. 机械设计与制造, 2012(4): 91-93. CHENG Dajun, LIU Kaizhou. Research on real-time optimization behavior of AUV based on MILP[J]. Machinery Design & Manufacture, 2012(4): 91-93.
[10] SONNENBURG C R, WOOLSEY C A. Modeling, identification, and control of an unmanned surface vehicle[J]. Journal of Field Robotics, 2013, 30(3): 371-398.
DOI:10.3969/j.issn.1673-4785.201405012
中国人工智能学会和哈尔滨工程大学联合主办。
0

文章信息

冷静, 刘健, 徐红丽
LENG Jing, LIU Jiang, XU Hongli
实时避碰的无人水面机器人在线路径规划方法
Online path planning of an unmanned surface vehicle for real-time collision avoidance
智能系统学报, 2015, 10(03): 343-348
CAAI Transactions on Intelligent Systems, 2015, 10(03): 343-348.
DOI:10.3969/j.issn.1673-4785.201405012

文章历史

收稿日期:2014-05-06
网络出版日期:2015-06-01

相关文章

工作空间