遵循COLREGs的USV多阶段自适应ph-Radau伪谱避障轨迹规划

戴怀志 蒲红红

障碍物规避策略对无人水面航行器(Unmanned Surface Vehicle,USV)顺利完成自主航行与作业十分重要,而为实现有效且协调的避障,将USV作为洋流中航行船舶的一种,遵循国际海上避碰规则公约(International Regulations for Preventing Collisions at Sea,COLREGs)是必要的[1].

针对USV避障问题,多数学者均采用遵循COLREGs的速度避障法[2−4].文献[2]针对未知动态环境,线性化处理相对速度,并将其作为约束条件整合到混合整数线性规划器中,选择多目标函数作为优化函数,提出一种遵循COLREGs的在线路径规划算法.文献[3]则利用动态障碍物与USV的相对运动状态,提出一种遵循COLREGs的粒子群优化(Particle Swarm Optimization,PSO)障碍物规避策略,实时解算规避障碍物的最优航行和航速改变量.而文献[4]在遵循COLREGs的前提下,静态和动态障碍物的规避问题,提出基于PSO的动、静态障碍物规避分层路径规划方法.

然而,上述文献中USV避障策略无法从USV运动模型出发且未充分考虑航行机动性能限制等约束条件,难以保证所规划路径的可行性,本质上归属于路径规划范畴.而基于伪谱法的轨迹优化策略是一类活跃在航天器领域的轨迹规划数值解法,以最优控制理论为通用框架,可方便地加入各种约束条件,且求解精度高,收敛速度快.此外,航天器轨迹优化策略可分为伪谱法和多阶段伪谱法[5−7]求解两类轨迹优化策略.文献[8]从六自由度运动模型出发,充分考虑飞行机动性能限制和三维空间静态障碍物约束,建立连续时间避障最优轨迹规划模型,给出基于Gauss伪谱法的无人直升机避障轨迹规划策略.文献[9]则引入内点约束将突防轨迹分段,并综合考虑求解精度和速率,结合Radau伪谱法和自适应hp节点配置法,求解含静态禁飞区和必经航路点约束的多阶段再入飞行器轨迹快速优化问题.值得注意的是,以上基于伪谱法的航天器轨迹优化策略均为针对静态障碍物和静态必经路径点的处理.

因此,基于以上分析,本文针对USV对动、静态障碍物的机动避障问题,在遵循COLREGs的前提下,提出一种基于自适应ph-Radau伪谱法的USV多阶段避障轨迹规划算法.该算法充分考虑了机动性能、状态方程以及障碍物(包括动态和静态)等约束条件,能够保证所规划轨迹的可行性;COLREGs的遵守和内点约束的引入,使得USV能同时对动态和静态障碍物有效且协调地机动避让;相比Gauss伪谱法,所设计算法具备更高的求解精度和速率.

1 国际海上避碰规则公约与避让策略

1.1 国际海上避碰规则公约

目前还未有相关法律对USV的航行行为作出明确规定,但较为合理的方案就是将USV作为船舶的一种,遵循国际海事组织制定的COLREGs.

该公约第13∼15条规定了碰撞局面及相关避让规则.以USV航向为基准,根据障碍物与USV的航向角度差值,定义图1所示的碰撞局面和如下避让规则[3]:

1)追越:若障碍物与USV的航向角度差值在［0◦,45◦)和［315◦,360◦)之间,当接近危险距离时,要求USV从被超越障碍物左舷处安全通过.

2)正面相遇:若障碍物与USV的航向角度差值在(165◦,195◦]之间,当接近危险距离时,要求USV从障碍物右舷处安全通过.

3)交叉相遇:若障碍物与USV的航向角度差值在[45◦,165◦]之间,障碍物与USV 右交叉相遇,当接近危险距离时,要求USV从障碍物右舷处安全通过;若航向角度差值在(195◦,315◦)之间,障碍物与USV左交叉相遇,当接近危险距离时,要求USV从障碍物左舷处安全通过.

1.2 避让策略

当USV在携带传感器的探测范围内检测到未知的动、静态障碍物并存在碰撞危险时,依据COLREGs判定碰撞局面,在障碍物和USV保持当前位姿不变的假定条件下估算相撞时刻,设定合适的避让航路点和禁航区约束.

其中,避让航路点可看作是为了提高海事交通安全性,即有效且协调地安全避障,让USV遵循COLREGs而设定的必经地理位置.而为避免避让策略的复杂性,仅在动态障碍物规避时设置避让航路点即可.此外,为保证USV在安全避让障碍物后能“优雅”且快速地返回原始机动航行路线,需限制避让航路点处的其他条件(如时间、姿态),避让航路点约束可设置如下:

式中,tl为要求USV到达避让航路点处的估算时刻,即时间约束;(xl,yl,ψl)为第l个避让航路点的位置和航向;ψlmin和ψlmax为航向ψl的边界值;lN为避让航路点数目.

为保障避让策略的可靠性,将未知静态障碍物看作是一类特殊的动态障碍物并考虑避让过程中仍存在碰撞危险,要求USV始终与障碍物保持一定的安全距离,即设置禁航区约束.将障碍物近似为一个速度和航向均为常值,半径为Rj的圆域,j=1,2,···jN,jN为障碍物数目.假设初始时刻t0=0圆心坐标为(xj0,yj0),移动速度为Vj,航向为ψj,t时刻的障碍物可描述为

则禁航区约束为

2 避障最优轨迹规划模型

2.1 约束条件

1)状态方程约束

水平面三自由度欠驱动USV运动模型,即状态方程[10]约束如下:

式中,m11=m−X˙u,m22=m−Y˙v,m33=Iz−N˙r;C13=−m22v,C23=m11u;d11=−Xu,d22=−Xv,d33=−Nr;τ=[Tu0Tr]T为控制输入向量,τu为前进推力,τr为偏航力矩.注意到,式(4)中仅存在作用在纵荡和艏摇两个运动自由度上的控制力/力矩τu和τr,横荡运动自由度上不存在控制力/力矩,系统的控制输入少于运动自由度个数,故该三自由度USV为典型的欠驱动系统.

2)机动性能约束

为保证所使用运动模型的可靠性,根据航行器的机动性能对状态变量作如下约束:

式中,umax、vmax和ψmax均为非负常值.此外,考虑到控制变量受其执行机构性能与姿态控制系统的限制,为避免USV在航行过程中失控,对其前进推力和偏航力矩作如下约束:

式中,Tumax和Trmax均为非负常值.

3)禁航区约束

遵循COLREGs设置禁航区和避让航路点约束,其中禁航区约束为式(3).

4)边界条件约束

初始时刻t0=0的USV位姿为x x x(t0),指定终端时刻tf的避障结束位置(xf,yf)为终端目标,即

为提高避障可靠性,引入终端目标处的其他条件约束(如速度、姿态),即

式中,ufmin、ufmax、ψfmin和ψfmax均为常值.

2.2 性能指标

考虑到大洋中自主航行的USV携带能量有限,且能量补给困难,可选择已到达终端目标处的能耗最少为性能指标,即

当然,依据USV的具体作业与任务需求,该性能指标也可选择为航行时间最短、航程最短等.

2.3 多阶段避障最优轨迹规划模型

将避障轨迹在避让航路点处分段,并引入内点约束,将避让航路点转化为内点约束,进而可借鉴多阶段伪谱法轨迹优化策略,建立USV多阶段迹规划模型.

给定避让航路点数目lN,将原始时间区域I=[t0,tf]划分为P段,P=lN+1,阶段Ip=[tp−1,tp],p=1,2,···,P满足:且于是,含有避让航路点约束的USV避障轨迹规划问题可描述为多阶段轨迹规划问题,即寻找控制变量u u u(p)(t)∈Rnu,最小化Bolza型性能指标:

满足约束条件:

本文避让航路点约束式(1)转化为如下内点约束:

式中,ψl满足ψlmin≤ψl≤ψlmax.

3 自适应ph-Radau伪谱法

在阶段Ip=[tp−1,tp],p=1,2,···,P内,以伪谱法,如Gauss、Radau伪谱法离散化求解策略,可将连续时间最优控制式(11)∼式(14)转化为含有带有代数方程约束的大规模非线性规划(Nonlinear Porgram,NLP)问题,借助序列二次规划等数值方法,可获得原连续时间最优控制问题的近似数值解[11−12].

然而,多数实际问题均为非光滑问题,固定网格个数和配点数配置方式的伪谱法无法捕捉非光滑最优控制问题的状态或控制的不连续性;且求解精度与全局插值多项式维数(即配点数)有关,配点数的增加在提高求解精度的同时也增加计算代价[13].h法和p法是两典型的网格细化算法,可有效解决NLP维度高与求解速率低的矛盾问题.因此,本文在Radau伪谱法的基础上,结合自适应网格划分与节点个数分配[14−15],即基于自适应ph-Rddau伪谱法求解多阶段避障轨迹规划问题.

其中,自适应ph配置方法为:当任意网格区间的最大相对误差,k=1,2,···,K大于设定的误差门限值ε时,自适应进行网格划分或增加Legendre Gauss Radau(LGR)节点个数.

1)误差判定准则

假设在阶段Ip的第k个网格区间[tk−1,tk]内,Nk个 LGR 配点和末端点=1构成了该网格区间的插值节点,而连续时间最优控制式(11)∼式(14)及其状态变量、控制变量的Radau伪谱法离散化过程见文献[12−14].考虑到增加配点个数可提高求解精度,因而设定为网格区间[tk−1,tk]内的Mk个LGR配点,且Mk=Nk+1,=−1,=1.取Mk个点作为误差判定准则的采样点,则近似状态‡变量·和近似控‡制变·量在采样点上可表示为、,i=1,2,···,Mk.以‡ 为插·值节点,‡得到·近似状态变量和控制变量和,则状态方程在LGR配点=1,2,···,Mk上可近似为

式 中,=j=1,2,···,Mk为网格区间[tk−1,tk]的Mk×Mk维LGR积分权重矩阵.则第i个状态在采样点上的绝对误差和相对误差可表示为

网格区间[tk−1,tk]中最大相对误差为

2)自适应ph网格配置方法

令Nmin和Nmax分别为每个网格区间内的最小和最大LGR配点个数.假定网格区间[tk−1,tk]内LGR配点个数为Nk,Nk≤Nmax,最大相对误差大于误差门限值ε,则令

增加配点个数,并取整为

当进行网格细化时,要求细化后所有网格区间的配点个数之和与一致,且每个网格区间的配点个数均为Nmin,即将网格区间[tk−1,tk]进行Bq等分,等分子区间配点个数之和为BqNmin.等分子区间的个数Bq取

值得注意的是,在该网格更新策略中,无论是增加配点个数还是进一步网格细化,配点总数均为˜Nq.该网格更新策略可称为自适应ph网格更新策略,即先p后h的网格更新策略,当区间的配点个数超过最大配点个数Nmax时,才对网格进行细化处理.而结合Radau伪谱法与自适应ph网格更新策略的自适应ph-Radau伪谱法具体迭代策略可参见文献[15].

4 仿真实验与分析

采用挪威科技大学试验模型船CyberShipΠ[16]模型进行仿真实验,验证所设计算法的可行性与有效性,即是否能遵循COLREGs制定的避让航路点和禁航区约束,成功规避动、静态障碍物的同时,也能兼顾算法的求解精度与速率.

4.1 避让航路点约束验证实验

选择静态和动态障碍物各一个进行避障实例验证.为提高避障安全度,对静、动态障碍物进行膨胀处理.障碍物运动参数如表1所示.

表1 障碍物运动参数

遵循COLREGs判定存在碰撞危险,且与动态障碍物的碰撞局面为正面相遇,避让航路点数目lN=1,障碍物数目jN=2.其中,估算避让时刻为tl=40 s,避让航路点约束为

而带入障碍物运动参数表1和式(2)∼式(3),即可获得禁航区约束.

此外,给定USV的边界条件约束:

USV的机动性能约束:

给定自适应ph-Radau伪谱法离散化求解策略中的参数:

式中,Mmax为最大迭代次数;K0和N0分别表示阶段内网格区间初始个数和网格区间内LGR配点个数;Nmin和Nmax分别为网格区间内最小和最大配点个数.

仿真结果如图2∼图5所示.图2为在最优控制变量作用下USV最优航行轨迹变化曲线,其中,黑色方框对应初始时刻t0、避让时刻tl和终端时刻tf对应的USV具体位置;橙色实心圆对应初始时刻t0的动态障碍物具体位置,灰色实心圆分别对应避让时刻tl和终端时刻tf的动态障碍物具体位置,黑色箭头表示动态障碍物行进过程;黑色实心圆为静态障碍物.图3为禁航区变化曲线,即避障过程中USV与静、动态障碍物之间距离的时间变化曲线.注意到,在禁航区约束中取膨胀2m后的障碍物半径,而在实际绘制USV最优航行轨迹变化曲线时取原障碍物半径.图4为最优控制输入变化曲线.图5为USV最优状态变化曲线.可以看出:1)控制变量在整个避障过程中较为平滑,无较大跳变出现,USV的初始状态和终端状态也均在约束范围内,即机动性能约束条件得以满足;2)USV准确地经过了避让航路点,且禁航区约束的设置使得USV在整个航行过程中与动、静态障碍物保持一定安全距离,即避让航路点约束和禁航区约束条件得以满足;3)USV的初始状态和终端状态均在约束范围内,边界条件约束得以满足;4)由避让航路点造成的两阶段避障轨迹和各状态变量衔接处的变化均较为平滑,表明本章所设计的多阶段避障轨迹规划策略中内点约束的有效性.

此外,该自适应ph-Radau伪谱法多阶段避障轨迹规划算法得到的网格区间最大相对误差为5.2344×10−4,满足误差门限值.文献[4]中的避障优化轨迹与本文的优化轨迹差别较大,这是由于基于PSO算法优化避障轨迹过程中未从USV无人运载平台的状态方程出发,而在实际航行过程中各运动自由度存在一定的耦合关系.

4.2 禁航区约束验证实验

保持上述仿真参数不变,验证本章所设计算法是否能够有效处理制定的禁航区约束.图6给出了同时包含避让航路点约束、禁航区约束和仅包含避让航路点约束的USV航行轨迹变化曲线.图7对应两种情况的禁航区变化曲线.

其中,同时包含避让航路点和禁航区约束的情况存在禁航区变化曲线,仅包含避让航路点约束的情况不存在禁航区约束,但考虑到禁航区实质即为USV与动、静态障碍物之间的距离变化,故为了描述方便,仍用“禁航区变化曲线”描述仅包含避让航路点约束情况下USV与障碍物之间距离的变化.此外,取原障碍物半径进行USV最优航行轨迹变化曲线的绘制,而在禁航区约束中取膨胀半径为2 m后的障碍物半径.

可以看出:1)两种情况下,USV均精确通过避让航路点;2)仅考虑避让航路点约束,即忽略避让过程中存在的碰撞危险,与同时考虑避让航路点、禁航区约束的情况下,USV的最优航行轨迹是不同的.对于存在禁航区的情况,USV需要做“S”形机动绕过禁航区,以达到指定目标位置.

因此,图2∼图7的分析表明了将避让航路点转化为内点,并考虑禁航区约束的多阶段避障轨迹规划策略的可行性.

4.3 求解精度和速度验证实验

为了更好地说明本章所设计算法的有效性,给出采用Gauss伪谱法离散化求解策略和本文求解策略的USV最优航行轨迹对比,如图8所示.表2对应两种情况下的部分仿真结果,其中LG点为Gauss伪谱法离散化求解策略所需的Lagendre-Gauss离散点.为便于比较两种求解策略的求解精度,给出图8中障碍物膨胀2 m后的USV最优航迹局部放大图,如图9所示.其中,粉色区域对应障碍物的膨胀部分.

从图8、图9和表2可明显地看出:1)在离散点个数近乎相同时,Gauss伪谱离散化求解策略下的静态障碍物禁航区约束未被满足,得到的最优航行轨迹穿过了禁航区;而自适应ph-Radau伪谱法则根据设定的解的误差判定准则,自动检测到轨迹精度的满足情况,自适应增加离散点个数或者进一步网格细化.3)自适应ph-Radau伪谱法的仿真运行时间远小于Gauss伪谱法.

表2 部分仿真结果

因此,本章自适应ph-Radau伪谱法多阶段避障轨迹规划算法能够较好处理避让航路点和禁航区约束的同时,也能兼顾解的求解精度和速度.

5 结论

本文提出一种基于自适应ph-Radau伪谱法的USV多阶段避障轨迹规划算法.相比基于PSO等速度避障法,该算法从USV运动模型出发,充分考虑航行机动性能限制等约束条件,保证了所规划避障轨迹的可行性.而遵循COLREGs设定的必然航路点和禁航区约束,以及内点约束的引入,保证了USV能同时有效且协调地规避动态和静态障碍物.此外,相比Gausss伪谱法,该算法能兼顾求解精度和速率.

如何将该方法应用于外界环境,如海风、海浪、海流扰动下的USV避障轨迹规划问题可作为进一步的研究方向.