一种动态环境下无人艇局部路径规划方法

容 煜 何正伟 王森杰

(武汉理工大学航运学院 武汉 430063)

0 引 言

水面无人艇(unmanned surface vehicle, USV)路径规划是无人艇应用中的一个关键问题[1].然而,真实航行环境复杂多变,动态环境[2]不仅包括岛屿、海岸线、锚地等静态碍航因素,还包括运动船舶、漂浮物等动态目标.因此,需要路径规划系统实时理解动态环境并为无人艇提供自动避碰路径.

全局路径规划是指根据水域环境设计出一条起点到终点最优路径,虽然解决了路线可达问题,但是仅考虑了静态障碍物.因此需要局部路径规划技术通过传感器获取附近障碍物信息,实时修正当前航线.当前局部路径技术根据方法原理主要分为传统算法、速度障碍域、动态窗口法、强化学习四种类型.传统算法是改进常用的路径规划算法用于满足船舶避碰的需求.Fuat等[3]针对狭窄水带提出了一种动态环境下的人工势场法,分析了船舶对遇和追越两种避让情况的处理方式.Wang等[4]改进蚁群算法提出了满足《国际海上避碰规则》的蚁群算法.速度障碍法是通过计算最近会遇点、最近会遇距离和最近会遇时间来进行碰撞区域建模和避碰的方法.Xia等[5]提出了一种非线性有限时间速度障碍法,通过高斯混合模型和高斯回归算法预测动态障碍物的轨迹,能够处理非线性运动的障碍物.动态窗口法(dynamic window approach,DWA)是一种对无人艇运动状态参数采样生成一系列轨迹,然后利用评价函数得到最优路径的方法.该方法将轨迹规划和轨迹避碰进行了分开建模,因此易与其它算法结合,如文献[2]改进DWA算法,在DWA的评价函数中增加了考虑海况的权重系数,在海况级别变高时,通过降低速度的权重并增加距离的权重来确保USV的安全性.基于深度强化学习的规划方法是将代理机器人放在环境中自由探索,通过设置与环境交互的奖励,让网络模型端到端地学会自主避碰.封佳祥等[6]基于Q-learning算法对无人艇路径规划任务进行了建模,该算法实现了在简单环境下的无人艇自主避碰问题.

现有方法主要研究在满足算法实时性的前提下如何实现无人艇的动态避碰问题,其重心放在了避碰时机和安全性上,没有考虑动态障碍物不确定性和实时变化的问题,导致研究结果存在局限性.

文中针对动态障碍物不确定性的问题,构建轨迹障碍域模型对局部窗口内多条船舶未来航行范围同时进行估计,并考虑《避碰规则》提出改进的动态窗口算法,高效地求解局部避碰路径,并设计真实水域环境验证该局部路径规划方法的适应性和实用性.

1 问题描述与分析

假设无人艇可以准确地、实时地获取环境信息,并且路径规划系统输出路径可以得到运动控制系统良好执行.

图1为局部路径规划场景,由图1可知:无人艇在视野范围内发现有动态障碍物,局部路径规划系统开始计算与分析,在计算出的障碍物未来轨迹范围不影响无人艇安全航行时,在线局部路径规划模块的输出路径和全局规划的路径重合,实现按照既定路线行驶,在存在碰撞风险时,在线局部路径规划系统在局部区域内重新规划路径完成动态避碰,并在冲突结束后,重新回到全局路线上,实现复航.

图1 局部路径规划场景示意图

因此局部路径规划算法需要实现:

1) 在线局部路径规划算法仅在出现动态障碍物时修改路径,否则按照全局路径航行.

2) 能够对动态障碍物的轨迹进行精准估计.

3) 无人艇在动态环境中能自动避开障碍物的前提下,选择最优路径完成复航.

2 基于改进动态窗口的局部路径规划方法

文中提出了图2的改进动态窗口算法(improved dynamic window algorithm, IDWA),利用轨迹采样预测一系列无人艇局部路径,为了解决规划需要满足无人艇操纵性能、航行安全的问题,采用无人艇性能约束对采样轨迹的最大速度、最大回旋半径、加速度等属性鉴别,删除超出操纵性能的轨迹,采用无人艇碰撞约束用于检查规划轨迹是否侵入它船的安全领域.过滤得到可行轨迹后,改进动态窗口算法为了实现无人艇的动态避碰与复航,设计了碰撞风险函数,实时计算路径的动态碰撞风险,添加《避碰规则》代价用于解决避碰复杂性的问题,采用全局路径跟随代价对规划路径的偏航程度和航速进行评价.

图2 IDWA的局部路径规划方法框架图

由于动态障碍物的位置是动态变化的,无法直接确定动态障碍物的未来的位置,因此给路径的避碰决策带来困难.为了解决这个问题,文中提出轨迹障碍域模型(trajectory obstacle area,TOA)用于实时对非线性运动的动态障碍物进行精准估计.

2.1 环境模型

路径规划需要避开静态障碍物.通过栅格化导航区域的离线地图,构建环境模型,根据无人艇的视野范围决定地图的分辨率,因此,无人艇在环境模型中被视为一个栅格点.栅格化的地图由二维数组构成,网格的值为255表示可行区域,0表示障碍物区域,可以被保存用于判断路径是否与障碍物发生碰撞.

2.2 轨迹采样

轨迹采样用于获取多组预测的局部路径,将笛卡尔坐标系的位置转换到Frenet坐标系中.Moritz等[7]将轨迹采样过程分解为

(1)

式中:s(t)为无人艇从起始点沿着全局路径行驶的弧长;d(t)为无人艇当前位置偏离全局路径的距离;(α0,α1,…,α5)为s(t)多项式待求解系数;(β0,β1,…,β5)为d(t)多项式待求解系数.

(2)

同样的,可以得到关于dt的方程组,因此可以通过求解方程组(2)得到st和dt的方程系数解.通过求解过程发现,给定无人艇的当前状态作为初始状态参数.

(3)

可以通过采样一系列无人艇的目标状态

(4)

来求取方程得到无人艇未来的一系列轨迹点.每次采样tmax为最大预测时间,ttick为时间刻度,预测每个ttick时的无人艇轨迹,tmax时间内无人艇轨迹点集合代表一组采样轨迹,根据采样区域大小Wmax与刻度s可以得到Wmax/s组轨迹.

2.3 轨迹障碍域模型

图3为轨迹障碍域模型的网络图,模型使用GRU网络对船舶样本轨迹序列X进行建模解决多步长连续预测的问题,利用全连接层神经网络实现群体船舶相对运动信息的提取,多条轨迹在经过编码器和解码器的双层神经网络结构后可以实现多条轨迹时空特征和全局交通态势特征的融合Y′.鉴别器用于鉴别生成轨迹X+Y′和真实轨迹X+Y的真假.TOA作为一种非线性预测模型,适用于会遇局面下船舶未来航行区域的推断.

图3 轨迹障碍域模型网络结构

2.4 约束项

算法经过轨迹采样之后,需要设计约束项对结果进行过滤,筛选出满足无人艇操作性能、无碰撞的可行路径.设置了无人艇性能约束项和无人艇碰撞约束项来满足以上需求:

1)Pfilter1为USV性能约束.

Pfilter1(L)={li|li∈L,φv(li)≤

(5)

式中:L为采样轨迹集合;li为第组采样轨迹;φv(li)为跟随轨迹li的需求速度;φc(li)为跟随轨迹li的需求曲率;φa(li)为跟随轨迹li的需求加速度;Vmax为USV的最大航行速度;R为USV最小回旋半径;amax为USV的最大加速度.

2) 船舶领域是保障船舶安全的最小领域[8],因此文中将无人艇安全领域内存在障碍物的采样轨迹点均视为碰撞轨迹.四元船舶领域边界[9]到船体中心的距离为r,由前后左右四个方向的半径决定.Cfilter2为USV碰撞约束.

Cfilter2(L)={li|li∈L,φd(li)≥r}

(6)

式中:φd(li)为USV按照轨迹li航行时与目标船的距离,需要轨迹障碍域模型对目标船的航行范围进行预测并计算欧氏距离得到.

2.5 代价函数

1) 在四元船舶领域模型的基础上,考虑障碍物距离领域边界越远越安全,因此将轨迹li的碰撞风险J1定义为所有障碍物与无人艇领域边界距离的倒数和.

(7)

2) 全局路径跟随代价函数J2为实现无人艇避碰后复航的关键函数.定义采样轨迹与全局路线的偏差程度Δd为

(8)

定义采样轨迹速度与期望航速的偏差程度Δv为

(9)

J2(li)=Δd+Δv

(10)

式中:Δd给予偏航越大的路径更高的代价,保证规划轨迹尽量沿着全局航线行驶.Δv给予与期望速度偏差越大的路径更高的代价,保证规划轨迹尽量保持期望航速行驶,避碰时首先考虑转向来进行避碰,仅靠转向无法避碰时才通过改变航速来实现避碰,更加符合实际需求.

3) 《避碰规则》代价函数是实现规划路径满足《避碰规则》约束的关键.首先人为定义代价项J3为

(11)

(12)

2.6 评价模型

本文为了适用于船舶冲突局面,设计了四种综合代价函数.

1) 对遇当USV处于对遇局面时,双方都有避让责任,双方应该右转从双方的左舷经过.因此路径的总代价函数应该包含J3项,对不满足规则的路径进行惩罚.

2) 右舷交叉当USV处于小角度右舷交叉相遇局面和大角度右舷交叉相遇局面时,USV作为避让船,有义务右转从目标船的船尾经过或直线减速.因此路径的总代价函数应该包含J3项,对不满足规则的路径进行惩罚.

3) 追越当USV处于左舷追越局面和右舷追越局面时,USV作为让路船需要根据风险决定从目标左舷还是右舷追越,此时代价项J1和J2就可以满足避碰要求,因此不需要添加J3即J3=0.

4) 左舷交叉当USV处于被追越、小角度左舷交叉相遇局面和大角度左舷交叉相遇局面时,此时USV作为直航船没有避让义务,应保持航速航向,但是紧急局面下可以右转避碰.此时路径代价项J1和J2就可以满足避碰要求.

表1 不同会遇局面下的代价函数

综上,动态环境下局部路径规划可以被表述为一个条件排序问题,最优的局部避碰路径Lf可以被表示为代价函数最小值时的取值.

L*=Filter2(Filter1(L))

(13)

Lf=argminJ(l|l∈L*)

(14)

式中:L={l1l2…lm}为USVm组采样轨迹;Filter1为USV性能约束;Filter1为USV碰撞约束;L*为可行路径的集合.

3 实验及分析

选取舟山港岱山附近水域作为实验场景,实现了下列三种路径规划算法进行对比分析:

A*算法根据离线地图规划全局最优路径.

AD*算法(anytime dynamic A*)根据初始环境,规划出无碰撞最短路径,在障碍物的位置发生变化时,重新规划局部路径.

IDWA在线规划局部路径.轨迹采样设置无人艇当前速度为10 kn,每过20 s计算一次无人艇状态,一次采样最大预测时间为500 s,设置最大采样宽度为200 m,每次采样160条轨迹.评价模型参数k1=1,k2=1,k3=1.TOA模型训练每次batch的大小设置为32,训练迭代次数设置为1 800.轨迹序列长度设置为11,预测序列长度设置为11.采样时间间隔为10 s.

设置USV参数见表2,Vmax为最大航速,Rmax为最小回转半径,amax为最大加速度,lusv为船长.起点坐标是(122°10.439′,30°12.151′),栅格点为(261,607),位于栅格图中左侧.终点是(122°14.723′,30°11.793′),栅格点为(795,669),位于栅格图中右下侧.文中在环境中的动态障碍物信息见表3,验证避碰有效性仅测试算法能否避碰船舶A,船舶B、C用于验证IDWA算法在不同会遇局面下的有效性.

表2 无人艇综合实验参数

表3 动态障碍物运动状态参数

表4为在路径长度、转向点数量、与障碍物最小距离、计算时间共四种指标上对不同算法性能进行了统计.其中计算时间是在CPU R6-2600,32g内存电脑上执行10次算法的平均结果.

表4 AD*、改进动态窗口算法质量的评价结果

A*算法规划路径见图4.规划出一条从起点到终点的全局路径,但无法避开动态障碍物,在实验第22 min与船舶A发生了碰撞.

图4 A*实验

AD*算法规划路径见图5.无人艇根据当前水域环境规划出一条无碰撞最优路径,在第33 min无人艇与船舶A相遇,算法重新规划了避碰路线,然而,规划的路径转向点过多且曲率不平滑,避碰路径紧贴静态障碍物边界.在第40 min,无人艇抵达终点.

图5 AD*实验

IDWA见图6.全局路径为起点至终点的直线,无人艇能够有效跟随初始路径,同时处理静态和动态障碍物的复杂环境,在与它船构成冲突局面时能够及时做出符合《避碰规则》的避碰决策,并在冲突消失后复航到全局路径上,在路径中详细状态见表5.

表5 IDWA路径规划状态

图6 IDWA实验结果图

由图6可知:A*算法无法实现动态避障,AD*算法规划出最终的路径波动较为明显,最大曲率为0.785,规划的路径弯曲程度较大,在避碰过程中需要在设定的20 s内转向约45°,且与障碍物最短距离为26 m,路径安全性无法保障.改进动态窗口法规划出的路径曲率连续符合无人艇的操纵性能,为无人艇的后续运动控制和路径跟踪提供良好条件,与障碍物最短距离为128 m.在实时规划避碰路径的响应速度方面,AD*算法计算平均时间是0.62 s,改进动态窗口算法平均时间是0.28 s,所提算法在计算时间和路径质量上都优于AD*算法.同时,实验结果证明在线局部路径规划算法可以同时处理静态障碍物和动态障碍物,能够有效解决动态环境下的无人艇避碰和路径规划问题.

4 结 束 语

针对航行环境中动态障碍物不确定性和避碰复杂性的问题,文中提出了一种新的动态环境下的无人艇局部路径规划算法,构建了面向船舶实时避碰的轨迹障碍域模型,以实现局部窗口内交通态势提取,为无人艇的避碰决策和实时路径规划提供了基础.基于栅格化环境模型,引入四元船舶领域和《避碰规则》改进动态窗口算法,解决了动静态环境下路径规划一体化的问题.设计无人艇性能约束和全局路径跟随代价函数,实现了避碰后自动复航的需求.实验结果表明,用改进动态窗口算法可以有效实现不同会遇局面下的避碰与复航,规划的路径较A*、AD*算法更平滑,更符合无人艇操纵性能,实时规划的计算时间更短.后续研究考虑将风、浪、流等环境信息加入仿真环境中,结合船舶动力学模型和船舶运动学模型,以适用于更复杂的场景.