海流环境下基于改进D*算法的AUV 动态路径规划①

李世奇 孙 兵 朱蟋蟋

(上海海事大学 智能海事搜救与水下机器人上海工程技术研究中心 上海 201306)

0 引言

自治水下机器人(autonomous underwater vehicle,AUV)是一种依靠自主导航、决策系统航行并完成任务的海洋探索与研究工具。为了使AUV 在复杂的海洋环境中能够安全有效地到达预定区域,合适的路径规划算法是不可或缺的[1-3]。AUV 在运行过程中,海流作为能量流会在其行进过程中产生很大的影响,当遇到强海流时甚至会危害到任务的顺利完成。由于AUV 自带能源有限,在大范围海洋环境航行中必须考虑海流对其能耗的影响,使AUV 尽量保持顺流行驶并在减少逆流和侧流的情况下充分利用海流以减少能耗[4]。文献[5]采用了量子粒子群优化算法进行路径规划,考虑了海流和障碍物的影响,但其优化目标是航行时间,没有考虑能量消耗。文献[6]利用稀疏A*算法对AUV 进行了全局路径规划,将航行总距离作为优化目标,并建立了相应的约束条件,但同样没有考虑能耗代价。文献[7]将遗传算法与粒子群优化算法进行结合,使AUV 在考虑海流因素航行时减少耗能,但其存在计算时间长且计算量大的问题[8]。而D*算法[9-11]具有速度快、寻路优等特点,在AUV 的路径规划领域中得到了广泛的应用。

另外,路径规划通常都会把机器人当作质点处理,而AUV 自身体积较大,一般小型AUV 身长是1～2 m 左右,按照规划路径行驶可能会与障碍物边缘碰撞,因此更安全地避障也是AUV 路径规划需要注意的问题。文献[12]将支持向量机(support vector machine,SVM)应用到D*算法中,保证无人车规划路径的安全与平滑,但此方法不适用于障碍物密集的环境。文献[13]在D*算法中加入碰撞因子,应用于无人机室内路径规划,使规划路径更安全,但该方法不能解决路径偏向角大的问题,且仅考虑到二维环境。文献[14]将粒子群优化算法与D*算法进行结合来解决路径规划中的门限状态,但没有考虑机器人的性能约束和三维航迹规划问题。

本文针对上述问题提出了一种基于D*算法的改进算法,主要贡献如下。

(1)建立海流影响能耗代价模型,使得AUV 能充分利用海流,减少能耗。

(2)添加障碍物威胁约束,使得规划出的路径尽量躲避障碍物边缘,以保证AUV 行驶更安全。

(3)采用三次均匀B 样条曲线拟合对规划的路径进行平滑处理,使生成的路径更加符合AUV 的行驶条件。

1 改进D*路径规划算法

AUV 在复杂的海洋环境中运行时,影响其航行的因素有很多,其中海流的影响是很大的。特别当AUV 进行远距离航行时,能耗问题就变得尤为显著。海流是海水较大规模相对稳定的非周期性流动,随季节、气候、海域、地形、深度变化而变化,是时间和空间的复杂函数,目前很难用精确的数学表达式描述其运动规律[15-16]。但在有限的海域及时段内,海流的流速和流向都是比较稳定的,因此本文研究主要考虑海流定常流动和对水下机器人的定常干扰。

1.1 海流模型

建立海流的数学模式时需要知道速度势和流函数的概念。对于函数方程:

其中σ 即为速度势,φ 即为流函数。于是可得:

进一步可得如下Cauchy-Rieman 方程:

结合式(5):

最后可得:

同理可得:

以上方程均是基于二维平面的,同理可得出三维方程:

在二维恒定流场中,如果速度平行x 轴,则速度势和流函数为

方向平行于x 轴,速度大小为1 m/s 的流场仿真模型如图1 所示。

图1 平行于x 轴的定常海流模型

类似地,在三维环境中平行于z 轴的定常流场仿真模型如图2 所示。

图2 平行于z 轴的定常海流模型

在海流情况下,不能简单地通过AUV 航行的总路程来衡量规划算法的效果。因为在无海流环境中,最短路径等价于最小能量消耗,但是在海流环境下,通过文献[17]定义一个海流能量函数来衡量路径规划算法的效果。定义Xi-1Xi是连接Xi与Xi-1的路径,di代表Xi-1Xi的长度,ei是沿着Xi-1Xi方向的单位向量,V 是AUV 的额定速度。先定义如下速度向量:

式中vi(x,y) 是(x,y) 处的AUV 速度向量,vc(x,y) 是(x,y) 处的海流速度向量,以此为基础定义AUV 航行的海流能量函数,用于评估AUV 路径规划的性能。

其中,Ji表示Xi-1Xi段的能量消耗,J 表示整个AUV航行的总能量消耗,m 是AUV 所规划路径的段数。

1.2 海流影响能耗代价模型

海上大风浪航行的船舶为增强适航性和安全性都会开足马力以最大航速逆流行驶,因为这样可以加大螺旋桨效率,增强舵的水动力性能,提升船舶操纵性水平。

同样,如果AUV 的最大速度与海流相比有较大余量,逆流航行会保持较为稳定的艏向角,从而减小事故发生的可能性;但如果作业任务要求AUV 做较长距离的航行时,若海流大小适中,在满足AUV 操纵性的基础上则可以充分利用海流,顺流行驶以减少能耗。

当海流的方向不同于AUV 首尾方向,即AUV侧流行驶时,海流力会对AUV 产生转艏力矩;另外当AUV 受到流压时,自身将朝流向漂移,从而产生阻止其漂移的水的反作用力,水阻力也给AUV 以转艏力矩。对于这种由海流产生的转艏作用,AUV 需要消耗大量的能量来抵抗其产生的力和力矩,同时还要保持既定航向。当AUV 首尾线方向与海流方向垂直时,AUV 便很难按照预定轨迹航行。因此在进行路径规划时,应尽量减少横向海流力的作用,以此节约AUV 运行所需的能耗。

综上,在AUV 远距离航行时,按照顺流最好、顶流次之、侧向流最差的原则进行路径规划可以充分利用海流节约能耗。

根据以上海流对AUV 的能耗影响可建立代价模型,应用到D*算法中。在D*算法的代价函数上添加能耗代价项,如式(12)所示。

其中,G(s) 表示从起始点到当前栅格的实际代价,H(s) 为启发函数,它表示当前栅格到目标点的估计代价,δ(s) 表示栅格i 和j 间的海流影响权重。

因为假定AUV 行驶在速度大小均匀的海流中,所以只需考虑海流的方向信息。海流影响因子设计是根据AUV 运动方向与海流方向的夹角大小确定的,在二维环境中如图3 所示,在三维环境中如图4 所示。则定义栅格i 和j 间的海流影响权重δi,j(s) 如下:

图3 海流方向与AUV 运动方向夹角示意图

图4 海流方向与AUV 运动方向夹角示意图

其中,a 为一个常量因子,可由实际的海洋环境确定,用于调节δi,j(s) 大小范围使其适合代价函数公式。a 的值越大,表示海流能耗代价项占总代价项的比重越大,规划出的路径相对越顺流。本文仿真部分a 的值统一设定为1。δk为根据海流方向与AUV运动方向的夹角| w1 -w2| 制定一个用来近似表示海流对AUV 能耗影响的权重。根据顺流最好、顶流次之、侧向流最差的原则定义δ1=0,δ2=2,δ3=4,δ4=3,δ5=1。

同理,在三维水下环境中,栅格i 和j 间的海流影响权重δi,j(s) 可设置为

根据顺流最好、顶流次之、侧向流最差的原则定义δ1=0,δ2=2,δ3=2.3,δ4=4,δ5=3.3,δ6=3,δ7=1。

1.3 障碍物威胁约束模型

基于传统D*算法规划出的路径存在紧贴障碍物边缘的问题,考虑到AUV 自身大小,这种情况下对AUV 安全航行是有一定威胁的,因此在D*算法的代价函数上添加障碍物威胁约束[18-19],改进后的D*算法代价值计算方法如式(15)所示。

其中,H(s) 表示启发函数,它表示当前栅格到目标点的估计代价,G(s) 表示从起始点到当前栅格发生的实际代价,W(s) 表示周围障碍物对当前栅格产生的威胁代价。

障碍物对AUV 航行的威胁是一个复杂的问题,本文将其做了简化,如图5 所示,不同形状的黑色图形为障碍物,灰色栅格代表水下机器人的位置。

图5 障碍物威胁模型图

以栅格中心点为中心,以AUV 当前位置到障碍物的距离为自变量d,建立威胁约束模型如式(16)所示。

其中,rmax为最大效用半径,当d 大于rmax这个距离,障碍物对栅格不再产生威胁作用;rmin为最小限制半径,当d 到达距离障碍物距离为这个值时,对它产生最大限制。当d 在两个距离之间,产生的代价与距离呈现负指数关系,k 为常系数。

关于常系数k 对路径规划的影响,k 的值越大,表示障碍物威胁项占总代价项的比重越大。其物理意义是,障碍物威胁部分约束影响力越大,规划出的路径越相对远离障碍物边缘。本文仿真部分k 的值统一设定为3。

1.4 B 样条曲线平滑

AUV 行驶过程中,当规划好的路径是折线段时,由于转向角度的限制,AUV 可能无法行驶。而且转角会导致航行路径长度的增加,造成能量浪费,如果将规划出的折线路径作平滑处理,则更能适应AUV 的水下航行。本文采用三次均匀B 样条曲线[20-21]对规划后的折线进行平滑处理,基于三次均匀B 样条曲线拟合模型如图6 所示。

图6 拟合曲线图

给定m+n +1 个平面或空间顶点Pi(i=0,1,…,m+n),称n 次参数曲线段:

为第k 段n 次B 样条曲线段(k=0,1,…,m),这些曲线段的全体称为n 次B 样条曲线,其中基函数Gi,n(t) 定义为

取n=3,则有三次B 样条曲线的基函数如下:

三次B 样条曲线段P0,3(t) 为

1.5 改进后算法的实现

由于改进D*算法是基于原D*算法的,因此,改进D*算法涉及到的大部分符号与功能函数和原D*算法相同。下面主要介绍Current_cost()和Danger_cost()2 个功能函数。

Current_cost()是海流影响能耗代价函数,在路径规划过程中,计算所要拓展栅格由于海流影响而产生的能耗代价值,按照顺流最好、顶流次之、侧向流最差的原则进行取值。

Danger_cost()是障碍物威胁代价函数,在路径规划过程中,计算所要拓展栅格由于障碍物的威胁影响而付出的代价值。它的值与到最近障碍物边界的距离有关,距离越近,付出的代价越大,反之,则越小。

由于这2 个函数主要是在Process_state2()中调用,因此在图7 中介绍Process_state2()的算法流程。

图7 Process_ state2()函数流程图

2 仿真及结果分析

本文通过栅格法将环境地图离散化,将环境中包含障碍物的区域设置为障碍栅格,其他的设置为自由栅格,仿真图中的五角星表示目标点,菱形表示起始点,灰色栅格是静态障碍物,黑色栅格是动态障碍物,点划线是未出现动态障碍物之前规划的路径,带有“x”标记的虚线是重规划路径,实线是将折线平滑后的结果。在Matlab 2016a 中针对二维障碍物海流环境和三维障碍物海流环境分别进行全局路径规划仿真实验,以验证结果的有效性。

2.1 相关工作仿真对比

为了进一步验证所提出算法的有效性,加入人工势场法[22]与所提出D*算法进行对比分析。人工势场方法的核心思想是创建一个虚拟的力场,将地图中的目标设置为引力源,障碍物设置成斥力源,并通过建立引力场函数、斥力场函数来进行路径规划。人工势场方法的优点是:规划的路径一般比较平滑,算法模型实现简单。但它的缺点也很明显,那就是存在局部最优的问题,并且对于局部最优问题一直还没有一个通用高效的解决方案。二者的仿真结果分别如图8 和图9 所示。

图8 人工势场路径规划

图9 D*算法路径规划

表1 是AUV 航行中能量消耗与总路程对比,通过对比发现D*算法障碍物环境下的路径规划的总能量消耗和路径长度均小于人工势场方法。

表1 AUV 能量消耗对比

2.2 二维海流环境下的路径规划

将改进D*算法进行仿真验证,图10 为未考虑海流影响的路径规划仿真图,图11 表示的是考虑海流影响的路径规划仿真图。

通过对比图10 和图11 可以看出,改进前后的算法都能够使AUV 从起始点到达目标点,但2 种算法的路径长度与能耗如表2 所示,即考虑海流影响的路径长度比未考虑海流影响的路径长度稍长,但是由于利用了海流以及减少了侧流的情况,使得改进后的路径能耗更低。

表2 AUV 能量消耗对比

图10 未考虑海流影响路径规划

图11 考虑海流影响路径规划

将改进后的算法继续进行仿真,图12 表示的是初始路径规划完成后临时出现了动态障碍物时的路径重规划,可以看到重规划的路径能够避开动态障碍物。

图12 出现动态障碍物时的路径规划

但此时规划的路径会紧靠障碍物边缘,考虑到AUV 自身的大小,现实情况中可能会造成与障碍物碰撞的后果,图13 表示的是添加障碍物威胁约束后的仿真。

图13 添加障碍物威胁约束的路径规划

如图13 可以看出,规划出的路径距离静态障碍物和动态障碍物都有一段距离,这样就使得规划出来的路径更安全。不过此时的路径仍是由折线段组成的,考虑到水下机器人立即改变方向的不可操作性,在实际作业过程中难以实现,因此路径平滑尤为重要。图14 中的实线是既添加了障碍物威胁约束,又对路径进行平滑后的结果。

2.3 三维海流环境下的路径规划

三维海流环境下的处理方式与二维海流环境下相似,仿真结果如图15～19 及表3 所示。通过结果对比可以得出和二维环境下类似的结论,即考虑海流影响的路径长度稍长但是能耗更低。按照改进后的算法依次进行障碍物安全距离和路径平滑的改进,结果符合预期。

图15 未考虑海流影响路径规划

图16 考虑海流影响路径规划

图17 出现动态障碍物时的路径规划

图18 添加障碍物威胁项的路径规划

图19 添加障碍物威胁项、有平滑处理

通过仿真分析可知,本文所采用的路径规划算法适用于海流信息已知的离散化栅格地图,模型建立方便,避免了复杂的计算,运行时间短,提高了AUV 路径规划的效率。但是,当海流信息不确定时,需要做进一步改进设计。

3 结论

目前大多数路径规划算法对AUV 的针对性不强,具有一定的局限性。本文针对海流环境下AUV动态路径规划问题,提出了一种基于D*算法的改进设计。通过建立海流影响能耗代价模型,使AUV能够利用海流,减少能耗;加入障碍物威胁约束,使AUV 能尽量远离障碍物边缘,保证行驶安全;对规划路径作平滑处理,使生成的路径更符合AUV 的行驶条件。这是本文工作的主要创新点也是区别于其他机器人路径规划的方面,实验仿真验证了本文提出改进算法的有效性。

由于海洋环境以及作业任务的复杂化程度不断提高,单个AUV 很难高效地完成制定的任务。因此,多AUV 协同的路径规划及任务分配问题将成为未来工作的研究方向。