邹子理,孙骞,黄雨杰,李一兵
1. 哈尔滨工程大学 信息与通信工程学院,黑龙江 哈尔滨 150001
2. 哈尔滨工程大学 先进船舶通信与信息技术工业和信息化部重点实验室,黑龙江 哈尔滨 150001
水面无人艇(unmaned surface vehicle,USV)是一种通过遥控或自主方式在水面作业的智能化小型平台,广泛用于情报搜集、海上救援、反潜等任务,对于维护我国海上安全与海洋权益具有重大意义。无人艇实时路径规划是水面无人艇研究的重要内容之一。路径规划算法可以分为全局规划和局部规划两部分,其中经典的全局路径规划方法包括A*算法、Dijkstra 算法、快速探索随机树(rapidly-exploring random trees,RRT)、栅格法等;局部规划包括人工势场法(artificial potential field,APF)、动态窗口法(dynamic window approach,DWA)等[1]。全局规划根据已知的全局环境信息,使用各种寻优算法获取全局最优路径,其能够在任意不规则障碍物环境中计算出全局最优解,但是存在计算量大、搜索效率低的缺陷,且对突发情况应对能力不足,所以单纯的全局规划算法难以应用于探索环境未知的海域。局部规划根据智能体传感器采集的实时障碍物与位置信息,获取到目标点的局部最优路径,优点是实时性强、计算量小,能够实现对于突发事件的快速响应,但是局部规划容易陷入局部最优解,降低USV 执行任务的效率。
APF 由Khabit 于1986 年首次提出,其基本思想是仿照静电场中电势和电势场的概念,在目标点与障碍物位置处分别构造引力场Uatt与斥力场Urep,其中,引力势场将吸引USV 向目标点移动而斥力势场帮助USV 实现障碍物规避。在势场中,USV 视为一个质点,其在当前位置的合力作用下移动一个指定单位步长,到达下一位置后实时更新势场,进行下一步路径规划。APF 的显著优点之一是实时性强,能较好地应对目标的变化与环境中的动态障碍物。但是在复杂地形多障碍物环境下若不同障碍物对USV 施加的斥力与目标点引力的合力为0 时,USV 将陷入局部最优解无法脱离导致路径规划失败或目标点不可达等问题。同时APF 生成的规划路径曲率变化较大,路径不平滑,对USV 机动性提出挑战[2]。
针对传统APF 中存在的目标点不可达、无效碰撞等问题,通过对势场函数进行修正[3],基于碰撞锥的分层人工势场[4]等方法可以加以改进。同时近年来许多工作在进行USV 路径规划时考虑了国际海上避碰规则(International Regulations for Preventing Collisions at Sea,COLREGS)的要求[5−9],但大多数是根据规则对斥力场函数分类讨论,且对保持USV 编队的规则情形较少关注。COLREGS规定了存在碰撞风险时应采取的机动类型,当USV 在其他船只附近航行时必须遵守COLREGS,以便USV 安全避开其他海上智能体,同时给其他运动船只的决策提供可靠参考。
本文首先对经典人工势场法的障碍物形状和斥力函数进行改进以平滑规划路径,以避障过程路径消耗为指标研究了基于改进人工势场法的多无人艇群静态避障策略,提出一种进行艇群会遇态势判断的艇群国际海上避碰规则(swarm-COLREGS,sCOLREGS),并结合改进人工势场法提出符合sCOLREGS 的偏置障碍物区域多USV动态避障策略。最后通过仿真验证改进APF 能有效平滑规划路径,同时证明了在特定环境下分散编队避障将显著降低路程消耗,所提出的偏置障碍区域算法能够在保证实时性与安全性的情况下遵守国际海上避碰规则。
在经典斥力场中,场函数是距离倒数的平方,这使得经典APF 算法中的斥力场函数曲线升降过快,斥力大小在短距离内显著变化,导致规划路径曲率变化过大,对USV 转向机动性提出挑战,同时造成额外的路径消耗。
为平滑避障路径,提高避障效率,本节通过增加缓冲区域使避障区域沿无人艇速度方向扩展改进斥力场形状,同时设计平滑修正系数改进斥力场函数。
无人艇使用质点模型建模,其运动状态仅受虚拟势场力及艇间通信影响,并将执行任务环境抽象为二维平面,不考虑海浪、海流等因素的干扰。记障碍物在平面中占据的空间为点集Vobs。
将环境平面划分为一系列适当精度的正方形栅格 αi,若栅格属于某一障碍物(岛屿、船只等),将其赋值为1,否则为0。当划分的栅格足够小时,可近似将小栅格视为平面中的一点(x,y),二值障碍物地图如图1 所示。
图1 二值障碍物栅格地图
考虑二维平面n艘无人艇集合S(n)={Si=(xi,yi)|i=1,2,···,n}, 记第i艘无人艇Si到目标(goal)点g(xgoal,ygoal)的距离为则目标点对Si产生的引力场Uatt为
式中 ε为对应的引力相关正比例系数。则引力为
式中ni,g为Si指向目标点的单位矢量。记Si到平面上一障碍物(obs)点αi(xobs,yobs)距离为
改进后的斥力场由障碍物椭圆、禁止区、过渡区3 部分组成,如图2 所示。其中 ρobs为不规则障碍物最小圆覆盖半径,表征障碍物大小; ρ0为预设禁止距离,若USV 进入此区域将无法成功避碰; ρ1为过渡区半径,当USV 距障碍物为 ρ1时开始避碰,且从 ρ1降至 ρ0,斥力由0 以指数速度增至最大值。
图2 改进人工势场法斥力分区
本文以ρobs为短轴长度、 ρ0为长轴长度、USV 与障碍物中心连线为长轴方向构造改进的椭圆形斥力场,通过对斥力场形状进行改进,可以使无人艇较经典方法提前进行避障,提高航行安全性,同时也使路径更为平滑,减小转向角。
改进后斥力场函数的数学表达式为
式中 λ为斥力场改进系数,定义如下
式中: ηs为比例系数, γ为负实数,m为大于1 的实数,ρi1=|ρ1−ρ(i,obs)|, ρ01=|ρ1−ρ0|,由于ρ(i,obs)>ρ0, 故在过渡区ρi1<|ρ1−ρ0|=ρ01, 指数系数γ(1/−1/)<0,由指数函数的特点知在(−∞,0]上 λ由0 随ρ(i,obs)增速变化至 ηs,且变化较为缓慢,更容易控制。通过求解斥力场的梯度可以得到USV 所受斥力为
故无人艇在(xi,yi)处所受的虚拟势场合力为
合势力场形状如图3 所示。
图3 改进人工势场法合势力场形状
USV 在执行任务过程中,可能会遇到大小尺寸不同的障碍物。USV 编队在复杂地形环境中作业时,障碍物规避问题成为USV 编队需要面对的一大挑战。由于复杂环境下障碍物数量与分布随机,为保证安全性USV 编队需要低速通过,故此时对于航线长度的要求高于规划路径平稳性,以降低避障过程能量消耗,提高USV 编队续航能力[10]。本节首先介绍编队控制方法,在此基础上研究当障碍物与编队大小相当时以编队避障总路程为评价指标的一种编队变换避障策略。
多无人艇编队采用leader-follower 编队形式[11],以一虚拟领航者为中心,编队成员均匀对称分布于半径为r的圆周。设编队成员数为N的艇群为S(N)={Si|i=0,1,···,N},其中S0为编队虚拟领航者(Leader)。定义加权有向图G=(V,E)表示编队通信拓扑结构,其中节点集V=S(N)表示编队成员,边集E={eij=(Si,S j)}表示编队成员i、j间是否存在通信关系,对应的eij取值为1、0。定义邻接矩阵
式中:当eij=1时,aij=1;否则aij=0。本文编队拓扑图如图4 所示。
图4 编队通信拓扑结构有向图
定义入度矩阵
定义有向图G的拉普拉斯矩阵为
式中L是半正定阵。
编队一阶线性连续控制方程为
式中:xi为状态量,ui为如下线性输入量。
使用Lyapunov 函数V=xTx/2研究此系统稳定性。对状态量求导,并利用L的半正定性可得
考虑=0的情形。注意到图G联通,即存在生成树,故L有且仅有一个0 特征值,且其特征向量为1 向量[12],即
由L的半正定性知xTLx=0与Lx=0n同解,所以x=x01n(x0∈R),此时诸状态量达到一致,且仅与L及初状态有关。故当状态量不一致时由<0状态量将逐步收敛至x0(=0),系统稳定性得到证明。
离散时间情形下编队一阶控制方程为
编队中跟随者(follower)控制算法为
式中:ε >0为常数,N(i)为节点i的邻居节点集合,dij为节点i、j间距离。
Leader 控制算法为第1 节所导的APF 合力,即
如图5 所示,设障碍物最小覆盖圆大小为R,单艇避碰过程中到障碍物圆的最小许可距离为D,当虚拟领航者S0距离障碍物中心小于D0(>R+D+r)时,开始避障程序。
图5 避障开始与结束位置示意
记Si从开始避障位置相对障碍物中心驶过的角度为 θ,当0°<θ <90°时,USV 处于避障阶段。当θ=90°时,结束避障。
若避障时USV 编队保持结构刚性,不进行编队变换,按照虚拟领航者所受人工势场力进行路径规划,整体从障碍物一侧绕过,这一策略的示意如图6 所示。
图6 策略1 示意
避障总路程与最小许可距离D有关,设此函数关系为f(D)。
在上述策略中,由编队队形的对称性,当艇群成员数j为偶数时,在虚拟领航者两侧的一对无人艇行驶的总路程可以等效为虚拟领航者行驶总路程的2 倍,故所有编队成员避障总路程为虚拟领航者的2×j/2=j倍;当j为奇数时,设第1 艘无人艇在虚拟领航者正前方,则此艘艇行驶总路程与虚拟领航者相等,对其余j−1(偶数)艘无人艇,化归为上述偶数情形,故所有编队成员避障总路程为虚拟领航者的1+(j−1)=j倍。编队成员数量为奇数和偶数时的队形对称等效示意如图7 所示。
图7 USV 编队对称性分析
综合2 种情形,无人艇编队避障总路程消耗为
在不考虑编队变换的情况下,USV 编队将保持当前结构从障碍物短侧绕行,这种策略不仅导致USV 编队的总体能量消耗提升,不利于USV 编队进行长时间作业,而且会增大与紧邻障碍物碰撞的风险,降低USV 航行的安全性。
下面考虑编队分散的避障策略。当虚拟领航者S0距离障碍物中心小于D0时,开始避障程序。此时编队队形不再保持,各艇按照距离障碍物最近距离,由小到大开始进入避障程序,目标点为引力点,障碍物及其他艇为斥力点形成的人工势场中进行路径规划,如图8 所示。
图8 编队分散避障策略示意
上述策略中无人艇编队避障总路程消耗为
将f(D+r)在D处进行泰勒展开,得
当R足够大时,避碰段曲线f近似为二次曲线,可认为f(i)(D)=0,i≥3。
由式(1)可知2 种策略的路程比仅与编队半径r、障碍物覆盖圆半径R(f与R有关)以及安全距离D相关。对改进APF 椭圆形斥力场,记安全距离为 ρ0,对1/4 椭圆弧长,有如下Ramanujan 近似公式[13]:
式中λ=(ρ0−R)/(ρ0+R+2D)。对变量D求偏导,有λ′=−2λ2/(ρ0−R) ,λ′′=8λ3/(ρ0−R)2。
由于(ρ0−R)≪(ρ0+R+2D),所以λ ≈0,故
所以
略去 λ的高阶无穷小项,得
综上所述,在改进椭圆形人工势场情形下,当编队大小r与障碍物大小R在同一数量级时,避障总路程之比d1/d2将会显著大于1,r与R的比值越大,2 种策略的路程比也越大,此时采用第2 种策略将大大降低USV 编队的能量消耗,提升编队的续航能力。
本节考虑艇群与动态障碍物(如船只)会遇的局面。通过速度障碍法判断碰撞风险,按照艇群避碰需求对COLREGS 对遇、右交叉、左交叉3 种会遇态势进行修正,提出艇群国际海上避碰规则,提出一种基于改进APF 的偏置障碍区域避碰方法。
速度障碍法(velocity obstacle,VO)常用于无人艇紧急避碰中[14]。假设无人艇当前的位置为PS i,速度为vS i。将障碍物抽象为一圆形区域,障碍物中心位置为Pobs,速度为vobs,无人艇与障碍物圆的2 条切线分别为T1、T2。如图9 所示,两者的相对速度为vuo=vS i−vobs,若max{〈vuo,T1〉,〈vuo,T2〉}≤〈T1,T2〉成立,则认为可能碰撞,此时无人艇速度vS i称为一个速度障碍。所有速度障碍组成的集合可以表示为
图9 速度障碍法示意
当速度vS i∈VVO时,为保证航行的安全性USV 优先执行避障任务;当vS i∉VVO时认为此时刻无碰撞风险,USV 保持原方向与速度继续航行。
自1972 年提出以来,国际海上避碰规则一直是航海实践中会遇态势判断与避碰策略选择的重要依据。对于两艇会遇局面下的COLREGS 遵守,已有较多的工作[15−16]。然而当USV 通信距离受限,需要在保持编队的情况下进行动态避障时,上述方法将难以在保证安全性的前提下严格遵守COLREGS。本节根据艇群避碰特点,提出适用于艇群与动态障碍物会遇的sCOLREGS。
首先介绍COLREGS 对两单艇会遇态势与避碰责任的规定。如图10 所示,定义会遇角φi∈(0°,360°)为本船与障碍船质心连线与本船速度方向所成角[17]。在有碰撞风险的情况下,依照国际海上避碰规则指导,以α1=5°、α1=112.5°为阈值,按 φi的大小及船速比将本船采取的避障策略分为表1 中的5 种情况,对应的策略示意如图11 所示。
表1 会遇态势判断表
图10 COLREGS 会遇态势判断
图11 COLREGS 避碰策略示意
对于艇群与动态障碍物会遇态势判断,由于一方从单艇膨胀为艇群,上述规则不能直接应用。为此,分别以USV 编队虚拟leaderS0(编队方)与动态障碍船Sobs(单艇方)为参考,定义如下判断准则sCOLREGS。
对于编队方,定义 α−前向区域为编队沿速度方向前进时覆盖的区域,其中 α为区域与速度方向顺时针夹角。图12 中射线l1、l2形成的区域即为此时编队的 0°−前向区域。
图12 sCOLREGS 编队方会遇态势判断
记编队的α1−前向区域与(−α1)−前向区域的边界l4、l6所夹区域为 Ω1,如图12(a)所示。编队的α1−前向区域与 α2前向区域的边界l4、l8所夹区域为Ω2,如图12(b)所示。当Sobs∈Ω1时,认为此时相当于COLREGS 中的对遇局面;当Sobs∈Ω2时,认为此时相当于COLREGS 中的右交叉局面;当Sobs∈时,认为此时相当于COLREGS 中的左交叉局面。
对于单艇方,以区域编队成员最大累计数为指标进行会遇态势划分。以Sobs为 中心将平面按与速度夹角为(360°−α1,360°)∪(0°,α1),(α1,α2),(α2,360°−α1)分为Ω1、Ω2、Ω3共3 部分,如图13 所示。
图13 sCOLREGS 单艇方会遇态势判断
设编队成员为S j(j=1,2,···,n),定义区域 Ωi编队成员累计数为σi(i=1,2,3)。
区域编队成员最大累计数为σ=max{σ1,σ2,σ3}。若σ=σ1,认为此时相当于COLREGS 中的对遇局面;若σ=σ2,认为此时相当于COLREGS 中的右交叉局面;若σ=σ3,认为此时相当于COLREGS中的左交叉局面;以图13 为例说明,此时σ1=1,σ2=3,σ3=0,σ=max{1,3,0}=3=σ2,认为此时为右交叉会遇态势,本船采取右转避让策略。
高速航行的无人艇进行避碰,对避碰路径的生成速率及实时性有较高要求。考虑到APF 具有实时性且运算量较小的特点,本节提出一种满足sCOLREGS 的偏置障碍区域改进APF 实时动态避碰方法。
设艇群在宽阔海域执行任务,艇载雷达感知范围为ρmax,当有动态障碍物与艇群成员最近距离小于ρmax时,USV 编队将执行避障程序。
设动态障碍物速度为vobs,此时通过速度障碍法进行避碰判断。若vobs∈VVO,则认为有碰撞风险,进行下一步程序,否则认为无碰撞风险,保速直行,同时重复速度障碍法避碰判断直至动态障碍物与所有艇群成员最近距离大于ρmax。
当碰撞风险存在时,USV 根据sCOLREGS 判断会遇态势,若我方为非避让船,则保速直行,同时重复速度障碍法避碰判断直至动态障碍物与艇群成员最近距离大于ρmax。若我方为责任船,则按照本节接下来提出的实时人工势场策略进行动态避障,同时进行速度障碍法判断碰撞风险,直至动态障碍物与艇群成员最近距离大于ρmax。避障策略框架如图14 所示。
图14 动态避障策略流程
图15 以右交叉会遇局面为例展示了基于改进人工势场法的无人艇动态避碰策略。图15 中无人艇Si位置为P(xi,yi),速度为vS i。障碍船位置为Q(xobs,yobs), 速度为vobs,两船间距d为‖(xi,yi)−(xobs,yobs)‖2, 会遇角φi∈(−5°,112.5°) ,vS i∈VVO,vS i/vobs<0.95,故属于右交叉情形,且本船为避让船。
图15 偏置障碍物区域示意
为实现国际海上避碰规则规定的右转避让,USV 执行动态避碰策略构造左偏置斥力区域。以P为圆心、d为半径将障碍物所在点Q旋转至速度方向上的D点,以PD为斜边作直角三角形OPD,其中∠OPD=α−φi, α为临界角度112.5°。
设障碍船半径为r=vobs·dt,代表单位时间后动态障碍物的可能位置范围。以O为中心、m·IOP·nOP为长轴、n·r·nOD为短轴建立前文所提及改进椭圆人工斥力场,其中m、n为长短轴系数,nOP、nOD为 OP、 OD方向单位向量。
φi越小,即动态障碍物位置越接近本船正前方时,椭圆斥力区域越偏离运动方向,右转幅度越小。反之,障碍物越偏离本船正前方时(此时碰撞概率较大),椭圆区域越接近运动方向,本船右转幅度较大,符合实际避碰需求。
为使APF 符合COLREGS 的约束,常见的工作是对斥力场形状、函数根据不同会遇态势进行分类讨论,得出的结果较为复杂,本节通过将斥力场位置进行变换实现对COLREGS 遵守的方法较为简便。同时,多数工作并没有考虑基于编队保持的COLREGS 遵守[18],本节提出的sCOLREGS考虑了艇群的编队尺寸,较传统COLREGS 更符合海上USV 编队动态避障的安全需求。本节依据改进APF,在满足动态避障的实时性的同时,考虑了对国际海上避碰规则的遵守,具有较高的安全性与求解规划路径速度。
使用科学计算平台对本文所提出的改进型人工势场路径规划、编队分散避碰策略、遵守国际海上避碰规则的多艇动态避碰策略进行仿真分析,证明提出算法的可行性。
本小节对1.2 节提出的分散编队避障策略进行软件仿真验证,仿真程序参数如下设置:地图大小20 n mile×20 n mile,r=2 n mile,ρobs=1.5 n mile,ρ0=2 n mile,ρ1=3 n mile,ε=100,ηs=300,γ=10,无人艇速度上限vmax=2 n mile/h,加速度上限amax=0.5 n mile/h, 角速度上限ωmax=5°/s, 出发点坐标(0 n mile,0 n mile),目标点坐标(16 nmile,17 n mile),障碍物中心坐标(9 n mile,9 n mile)。2 种策略的仿真航迹如图16 所示,避障过程中的路径消耗、最大转向角、距障碍物最近距离如表2 所示,关键理论参数的实验结果如表3 所示。
表2 策略参数对比
表3 关键理论参数实验结果
图16 改进APF 静态避障策略航迹图
由仿真设置,编队半径r与实际最近距离D的比值为1.408,具有较大的距离裕度,能保证无人艇群的避障安全。图16 显示本文改进的人工势场法可以安全地实现避障,且路径较为平滑。
由表2 知策略2 分散避障的最大转向角略大于策略1,在复杂环境下USV 编队速度较低,对航迹长度的要求大于对转向角的要求。
本仿真情景中r/R=1.33接近1,可以认为无人艇编队大小与障碍物大小相当。此时策略1(保持编队)的路程消耗明显大于策略2(分散编队),由图16 仿真航迹及2.2 节分析,所减少的路径消耗主要来自编队外侧无人艇。表3 显示d1/d2与1+2r/(ρ0+R+2D)较为接近,符合2.2 节推导的理论式,艇群分散避障策略的建模分析得到仿真验证。
本小节对第3 节提出的基于偏置APF 的sCOLREGS 遵守策略进行仿真验证。在一次航行任务中依次设置对遇、右交叉、左交叉3 种海上会遇态势,并在对遇、左交叉局面中设置动态干扰艇以检验算法对多移动障碍物的有效性。仿真参数设置为:r=2 n mile,m=2.5,n=1.5,无人艇速度上限vmax、加速度上限amax、角速度上限ωmax与4.1 节一致。艇群初始中心(0 n mile,0 n mile),地图大小50 n mile×50 n mile,地标位置参数设置如表4 所示。规划生成的全局航迹如图17(a)所示,对遇、右交叉、左交叉会遇态势的放大局部航迹分别如图17(b)、图17(c)、图17(d)所示。全程USV 编队成员与障碍艇距离如图18 所示。
表4 地标位置参数设置
图17 偏置APF-sCOLREGS仿真航迹
图18 (sCOLREGS)USV 编队与障碍艇距离曲线
在仿真结果图17 中,处于对遇局面时,USV 编队与动态障碍船均为责任船,均采取右转避让措施,如图17(b)所示;处于右交叉局面时,USV 编队为责任船,采取右转避让措施,动态障碍船非责任船,保速直行,如图17(c)所示;处于左交叉局面时,动态障碍船为责任船,采取右转避让措施,USV 编队非责任船,保速直行,如图17(d)所示。故第3 节提出的基于改进APF 的偏置障碍区域sCOLREGS 遵守方法能完全符合国际海上避碰规则条例,可以实现USV 集群海面保持编队进行动态避障。此外,在对遇、左交叉局面中设置的动态干扰艇并没有改变无人艇群根据sCOLREGS 选择的避障策略,说明所提出的方法具有一定的抗干扰能力,可以处理多移动障碍物局面。
图18 中从左至右分别为全程航行过程中USV 编队与对遇障碍艇、右交叉障碍艇、左交叉障碍艇的距离曲线。由图18 可知障碍物与USV 编队具有较充足的距离裕度,可以保证USV 编队动态避碰的安全性,同时为突发情况提供了缓冲区域。此距离裕度主要是因为sCOLREGS对单艇会遇的COLREGS 进行了物理范围的扩充,考虑了编队的几何尺寸。若不考虑sCOLREGS规则,在对遇情形下由于原COLERGS 的阈值角度为 5°,难以完全避开艇群,往往会导致碰撞的发生,如图19、图20 所示。
图19 COLREGS 航迹
图20 (COLREGS)USV 编队与障碍艇距离曲线
图21 为避障过程中USV 编队航向角变化曲线。本文所提出的算法航向角极差约为 37°,对USV 的转向机动性能要求较低。由于航向角变化较小,生成的航迹曲线较为平滑,既避免了尖锐的转向,也节省了额外的路径消耗,提高了USV 编队执行任务的续航能力。图21 中可观察到航向角在刚开始变化时有一段维持过程,这是因为对遇与右交叉态势中偏置APF 障碍区域的转向夹角(α−φi)较大,障碍物椭圆区域接近USV 编队正左方,对USV 编队右转偏向斥力场较小,从而转向不明显。这一过程为平滑航迹做出了贡献。
本文针对传统人工势场法路径不平滑的问题,首先对斥力场形状与函数进行修正,针对复杂环境下多无人艇群避障时的额外路径消耗,提出一种分散避碰策略,同时结合艇群会遇态势判断特点,提出艇群国际海上避碰规则,结合改进人工势场法偏置斥力场区域实现动态避障。实验结果表明,本文提出的分散策略在设定情境中避障总路程显著小于维持编队策略,提高了编队的续航能力。
本文提出的动态避障策略,在对遇、右交叉、左交叉3 种常见海上会遇态势中以较大的距离裕度进行符合规则的动态避障,编队转向角度范围较小,规划航迹平滑,能在确保实时性的同时较好地遵守国际海上避碰规则的相关条例。