多圆碟形水下滑翔机的双层协调三维路径规划

王浩亮,卢丽宇,王 丹,李文华,刘 陆,王安青

(1.大连海事大学轮机工程学院,辽宁大连 116026;2.大连海事大学船舶电气工程学院,辽宁大连 116026)

1 引言

近年来水下滑翔机(autonomous underwater glider,AUG)被广泛应用于一些特定的水下作业任务,如海洋监测、海底搜索以及海底地形探测等[1].在海流和碍航物影响下,为了使AUG能在复杂海洋环境中安全到达预定海域,适合的路径规划算法是非常必要的.因此,作为运动控制的上层算法[2-4],路径规划一直是AUG研究领域的热点问题.

在单水下航行器路径规划方面,文献[5]以能量消耗为优化目标,采用遗传算法实现了水下航行器的路径规划.文献[6]采用两步链式算法研究了基于能耗最优的AUG路径规划问题.文献[7]采用改进的粒子群优化算法,实现了以能量消耗为优化目标的圆碟形AUG路径规划.文献[8-9]分别采用A*算法和自主启发式神经网络算法对水下航行器路径规划中的避障问题进行了研究.文献[10]提出一种基于区间优化的最优时间路径规划算法,增强了路径规划器的鲁棒性并解决了生成路径不可行问题.文献[11]提出一种以期望时间为优化指标的概率多目标搜索算法,可显著缩短路径规划时目标搜索的期望时间.文献[12]采用基于异步多线程近端优化策略的路径规划方法,研究了海流影响下的路径规划问题.文献[13]采用基于仿生神经网络的最优路径规划方法,研究了海流和水下碍航物影响下的最短路径规划.在水下航行器集群路径规划方面,文献[14-15]分别采用改进波前算法和动态规划算法研究了基于时间最优的集群路径规划.文献[16]采用偏差最小一致性算法实现了航行终点同时到达目标下的集群路径规划.文献[17]采用蚁群优化算法和差异进化算法,研究了不同目标任务下的最短路径规划.文献[18]采用改进的模块化A*算法,研究了基于能耗最优的集群路径规划.然而,以上集群路径规划方法仅解决了水平面内水下航行器的路径规划问题.

在分析上述文献的基础上,本文采用基于量子行为的自适应粒子群优化方法(quantum-behaved adaptive particle swarm optimization,QAPSO)和人工势场法(artificial potential field,APF)构建了双层协调结构,用于解决多圆碟形AUG在海流干扰和水下碍航物影响下的三维路径规划问题.首先,建立了圆碟形AUG的航行时间模型,设计了三维路径规划的优化目标.然后,构建了基于三维离散空间的全局路径规划和基于APF方法的局部路径规划,保证了不同优先级的AUG在无碰撞情况下安全滑翔.最后,基于双层协调路径规划结构,采用QAPSO方法实现了时间最优目标下多圆碟形AUG的三维路径规划.

与现有水下航行器路径规划研究结果相比,所提三维集群路径规划方法的优点如下:相比与文献[5-13]提出的路径规划仅适用于单水下航行器,本文提出了适用于多AUG的路径规划方法.相比与文献[14-18]致力于二维水平面的集群路径规划,所提路径规划方法能够满足AUG集群开展三维海洋观测的路径规划需求,且具有避障避碰功能,解决了多AUG之间以及AUG与碍航物之间的碰撞问题.相比与经典进化算法中的遗传算法(genetic algorithm,GA)[19]和粒子群算法(particle swarm optimization,PSO)[20]容易提前收敛而陷入局部最优,QAPSO求解能力更强,更容易获得全局最优路径.

2 时间最优下的多AUG路径规划结构

2.1 采用双层协调结构的多AUG路径规划

为研究圆碟形AUG的路径规划,首先需要建立一个合适的坐标系来描述AUG的位置与运动状态.海洋环境下的惯性坐标系{I}如图1所示.

图1 海洋环境下的固定坐标系Fig.1 Fixed coordinate system in ocean environment

在海洋环境中可将AUG简化为一个质点,其中AUG起点为PS=(xS,yS,zS),终点为PD=(xD,yD,zD),当前位置为(x,y,z),AUG对水航速为V,对地航速为VG,海流速度为Vc,ψ,Ψ和ψc分别为V,VG和Vc在XEOEYE平面的投影与XE轴的夹角,θ,Θ和θc分别为V,VG和Vc与其在XEOEYE平面投影的夹角,由此可得

在目前的多海洋航行器路径规划方法中,采用全局规划算法很难应对局部路径的实时协调,而倾向于局部规划的方法又难以形成优质的全局路径.针对这一问题,本文提出一种基于全局与局部双层协调的多AUG路径规划结构,如图2所示,全局路径规划层采用QAPSO方法,根据给定的优化目标为每个AUG规划出一条满足操纵性要求的路径,同时该路径避免了与碍航物的碰撞,局部路径协调层根据多AUG之间的避碰要求,采用APF方法来解决AUG间的碰撞问题.

图2 双层协调多AUG路径规划流程图Fig.2 Flow diagram of multi-AUG path planning under double-layer coordination

双层协调结构可以被概括为:首先采用QAPSO方法进行全局路径规划,然后根据全局路径规划结果对全局路径组中的各条路径进行预测,当发现AUG之间存在碰撞趋势时,提取碰撞点周围的环境信息以及AUG在全局路径上的运动信息进行局部路径协调,从而避免碰撞发生.

2.2 AUG航行时间模型

由于AUG的额定航速和海流速度处在同一量级,海流将对AUG的航行时间产生较大影响.一般而言,在一定范围内洋流强度在不同深度处相差不大,可视为不同深度具有同一环流分布,因此本文将三维洋流模型简化为若干个二维洋流模型的分层叠加,采用分层建模的方法构建三维海流场.为模拟AUG水下航行时的洋流情况,本文采用若干个粘性Lamb涡叠加来建立海洋环流模型,Lamb涡可表示为

其中:为二维的空间区域,为涡旋的中心位置,D为涡旋强度,δ为涡旋半径.

在实际执行海洋任务时,经常需要AUG以最短的航行时间到达目标点,因此需要建立考虑海洋环境的AUG航行时间模型.由于路径上的海流分布不均,因此仅计算某节点处的速度或计算路径上所有点的速度都是不够合理的[15],所以本文以海流栅格为单位采用插值法将全局路径细化,航行时间的计算方法论述如下.

如图3所示,Vn为AUG在Pn点处的对水航速且假设其为一个恒定值|Vn|=U*,Vcn为Pn点处的海流速度,VGn为AUG在Pn点处指向下一点的对地速度.显然,可以通过调整AUG的姿态角改变Vn的方向,并与海流作用Vcn叠加为合成速度VGn,因此,|VGn|的求解可通过余弦定理求得行,此时将一个很小的正数作为|VGn|的解,以此可得出滑翔在该路径上所花费的时间接近无穷大,因此该路径不是可选的最优路径.对于出现两个正数解的情况,则选择较大的一个作为|VGn|的解,因为较大的速度可获得较短的航行时间.同理可得|VGn+1|,将|VGn,n+1|=(|VGn|+|VGn+1|)/2作为航行在路径Pn Pn+1上的速度,则在此路径段的航行时间可表示为

图3 航行速度示意图Fig.3 Diagram of sailing speed

其中|Sn,n+1|为路径点Pn到点Pn+1的水平距离.

综上所述,AUG整个航程时间之和为

其中N为插值后细化的路径点总数.

3 双层协调结构下的多AUG路径规划方法

3.1 基于QAPSO的全局路径规划方法

3.1.1 基于三维离散空间的全局路径规划构建

传统基于惯性坐标系的栅格法仅能选择相邻8个方向的栅格,限制了路径选择的最优性[21].本文将水下三维空间离散化,建立基于环境信息的离散空间模型.将路径起点和终点的连线作为横坐标,通过变换惯性坐标系OE-XEYEZE来表示离散空间,用于AUG的路径规划.

图4 水平面离散建模示意图Fig.4 Diagram of horizontal discrete modeling

采用路径解耦方法规划出的水平面最优路径点集合和纵平面最优路径点集合在轴上具有相同的坐标,因此可以将Pxy和Pxz中的路径点按照轴上坐标一致的原则合成三维最优路径点集合Pxyz={PS,P1,P2,···,PD},由此建立三维离散空间模型,将集合Pxyz中各节点依次相连形成的路径即为三维空间的最优路径.

此外,也可以根据水平面和纵平面离散建模的方法直接生成三维路径点.通常,AUG的典型运动工况为纵平面内的锯齿形运动,鉴于欠驱动圆碟形AUG的运动能力受限,其路径规划需要考虑下潜深度H和航迹角Θ.如图5所示,可将AUG的运动海域划分成垂直于轴的M个平面,该平面由Mx维与Mz维中具有交点的直线两两相交组成,记为集合xyz={Ly1×Lz1,Ly2×Lz2,···,LyM×LzM}.利用QAPSO方法,以AUG航行时间最短为优化目标求取集合xyz上的三维路径点集合Pxyz,由此可获得时间最优的三维路径.

图5 三维离散建模示意图Fig.5 Diagram of 3D discrete modeling

上述路径规划方法得到的初始数据是一些离散化的路径点,将其直接相连形成的路径可能无法满足欠驱动圆碟形AUG的运动需求,因此需要对路径进行光顺处理,本文采用Hermite插值法[22],最终得到满足AUG跟踪需求的连续光滑路径.

3.1.2 QAPSO方法

相比文献[20]中所提到的粒子群算法(particle swarm optimization,PSO),QAPSO方法去除了粒子速度参数,只保留粒子位置,其粒子位置用概率密度函数的形式表示,并且引入了自适应扩张因子的概念,扩大了算法的搜索范围,提高了算法的寻优能力.QAPSO方法的基本思想为在M维搜索域中,随机初始化一组规模为N的粒子群,粒子群中各粒子位置用概率密度函数的形式表示[23],通过迭代的方法在搜索域中寻找最优值.位于第m维的粒子i通过跟踪吸引子pim的位置,可以得到粒子i的第m维进化方程[24]

其中:为区间[0,1]上均匀分布的随机数,吸引子为粒子i在m维搜索域上的个体极值.

其中α表示收缩扩张因子.α是算法收敛至关重要的因素,在整个搜索过程中起到了权衡全局搜索和局部搜索的作用.基于粒子个体适应度值评价体系建立了自适应收缩扩张因子,以求最小值为例,收缩扩张因子更新公式为

其中XS和XD分别为路径起始点和目标点的坐标.将该项信息加入,则基于QAPSO方法的粒子进化方程为

3.1.3 具有惩罚功能的适应度函数

QAPSO方法中所有粒子都有一个被目标函数决定的适应度值,即适应度函数值[25].本文取航行时间模型为适应度函数.由于某些相邻路径点的连线会与碍航物相交而成为非法路径,若直接去掉这些路径,则可能影响算法的迭代,路径的多样性也会随之减弱,因此构造带有惩罚函数的适应度函数[26],通过附加较大的惩罚度,使路径在优化过程中自动淘汰非法路径.以粒子i在第T次迭代产生的路径点为例,粒子的适应度值为路径起始点PS至路径点PD的航行时间,具有惩罚功能的适应度函数设置如下:

3.2 基于APF的局部路径规划方法

经过全局路径规划可以获得每艘AUG从起始点到目标点的优化路径,根据相应路径可以计算出AUG经过各路径点的时刻,并以此来预测出AUG间可能存在碰撞的路径点.

如图6所示,当优先级较低的第i艘AUG出现在黄色警戒区域SjAUG时,即认为碰撞危险可能发生.局部调整的目的是当判断碰撞可能发生时采用APF方法进行局部路径协调,让优先级低的AUG提前调整航向来避让优先级高的AUG,以此来防止碰撞的发生.

图6 AUG碰撞风险示意图Fig.6 Diagram of AUG collision risk

传统APF方法在碍航物处生成斥力势场,同时在目标点处生成引力势场[27],使航行器在引力和斥力的合力作用下沿无碍航物的路径运动,在本文中则具体考虑为来自期望路径的引力和其他AUG的斥力.

斥力势场可表示为

其中:ri和rj分别表示第i艘AUG和第j艘AUG的位置,ρ(ri,rj)为两艘AUG之间的相对距离,ρ0为斥力场的影响范围,krep为斥力场强度系数,∇表示函数的梯度.引力势场可表示为

其中:r0为路径上参考位置的坐标,katt为引力场强度系数.

斥力和引力的合力为

通过斥力与引力的合成力可计算出下一位置点,直到合力将AUG拉回全局路径.

4 仿真分析

为验证所提基于双层协调结构的多圆碟形AUG三维路径规划方法的有效性,按照AUG的不同作业情形,分3种典型任务讨论海流和碍航物影响下的路径规划.整个仿真忽略AUG由于导航定位及路径跟踪控制误差所带来的实际位置偏差,将AUG对水航速设置为V=0.25m/s,滑翔角设置为Θ=35°.QAPSO和APF方法的具体参数设置如表1所示.

表1 QAPSO和APF方法参数设置Table 1 Parameters of QAPSO and APF methods

任务1:针对单个AUG,基于双层协调结构,在全局规划层采用QAPSO方法,根据时间最优的原则在航行水深H=80m工况下进行路径规划,并与经典进化算法中的GA[19]和PSO算法[20]进行比较,3种路径规划方法的仿真结果分别如图7-8和表2所示.

图7 3种方法的全局路径规划对比图Fig.7 Global path planning comparison diagrams of three algorithms

表2 AUG航行时间Table 2 AUG sailing time

由图7可知,路径起点和终点分别为(100,100,0)和(900,900,0).针对AUG航行时间的优化路径,GA,PSO和QAPSO三种方法均可实现求解,路径全段基本顺应海流方向,同时避开碍航物并且规避海流较大的区域.此外,通过图8的迭代曲线可以看出,在海流环境较复杂的情况下,GA算法最先陷入局部最优,随后PSO方法收敛陷入局部最优,QAPSO方法因其具备量子性质的优势持续寻优,直至找到更优的路径,验证了所提方法的求解能力.

图8 3种方法的时间最优迭代图Fig.8 Time-optimal iteration comparison diagrams of three algorithms

根据不同进化算法得到的航行时间如表2所示.

由表2可知,虽然3种路径规划方法均能规划出避开碍航物的水下路径,但QAPSO方法用时最短,航行时间分别约为GA和PSO算法的70%和83%.

任务2:针对两艘AUG相遇情形,采用所提路径规划方法进行三维上浮路径规划.两艘AUG中,AUG1的优先级高于AUG2.在航行水深H=90m工况下的仿真结果如图9所示.

图9 局部路径规划图Fig.9 Local path planning diagram

由图9可知,AUG1的路径起点和终点分别为(10,90,90)和(90,10,10),AUG2的路径起点和终点分别为(10,10,90)和(90,90,10),3个碍航物的坐标和半径分 别 为(10,60,30,10)、(90,50,60,10)和(70,70,35,10).从图9中可以看出,当两AUG存在碰撞风险时,在局部路径协调作用下,优先级较低的AUG2成功避开了AUG1,并且有效避开了水下碍航物,验证了所提方法的避碰和避障功能.

任务3:五艘AUG组成的集群在目标区域进行海洋作业,采用所提双层协调三维路径规划方法,根据时间最优原则进行路径规划.五艘AUG中,优先级从高到底依次为AUG1,AUG2,···,AUG5.在航行深度H=80m工况下的仿真结果如图10所示.

由图10可知,AUG1的起点和终点分别为(100,150,0)和(900,850,0),AUG2的起点和终点分别为(100,200,0)和(900,900,0),AUG3的起点和终点分别为(100,100,0)和(900,800,0),AUG4的起点和终点分别为(100,250,0)和(900,950,0),AUG5的起点和终点分别为(100,50,0)和(900,750,0).如图10所示,规划的五组路径都能够实现水下复杂地形的避障,并可以实现AUG之间的避碰功能,满足AUG的路径规划需求.其中,五艘AUG航行时间如表3所示.

图10 多AUG时间最优规划路径三维图Fig.10 3D diagram of time-optimal planning paths of AUGs

表3 AUG航行时间Table 3 AUG sailing time

将任务3中规划的5条路径进行参数化处理[28],所得满足欠驱动圆碟形AUG跟踪需求的参数化路径见附录.

5 结论

本文研究了海流干扰和水下碍航物影响下多圆碟形AUG的三维路径规划问题.提出了适用于多圆碟形AUG路径规划的双层协调结构,建立了适用于AUG的航行时间模型,为路径规划设置了合理的优化目标.在全局规划层采用QAPSO方法实现了多AUG的全局路径规划,在局部规划层采用APF方法生成了灵活避碰的局部路径,满足了AUG集群开展三维海洋观测的路径规划需求.仿真结果验证了所提基于双层协调结构的多圆碟形AUG三维路径规划方法的有效性.在未来工作中,将研究基于规划路径的AUG协同路径跟踪控制问题.

附录

任务3中规划的满足圆碟形AUG跟踪需求的5条参数化路径如下所示: