基于运动状态估计的双无人机协同航路规划

刘发永， 向 勇， 周新志， 宁 芊， 杨秀清

(1.四川大学电子信息学院， 成都 610065；2.中国民航局第二研究所， 成都610041； 3.民航成都物流技术有限公司， 成都 611400)

1 引 言

由于任务环境动态多变、目标任务多重复杂，考虑使用两个或两个以上的无人机组成编队来执行任务能够更有效地完成目标任务.近年来，相关的研究焦点多聚焦在无人机的数据感知融合[1-3]、编队通信[4]、路径规划[5-6]等方面，并取得了丰硕的研究成果.双无人机协同是研究多无人机协同任务的基础，在某些情况下，双无人机共同执行任务会比多无人机具有更好的灵活性，隐蔽性.

双无人机协同航路规划中，需要避开雷达武器等干扰，保证任务执行的成功率，为双机优化平滑且代价[7]较低的飞行航路.文献[8]使用传统的速度矢量场进行双机协同路径规划，使用矢量场方法不但存在势场为零的陷阱，而且难以确定每个威胁速度的大小.因此，智能算法如DE、PSO、A*等被广泛应用于路径优化[9-11]，并展示了巨大潜力.文献[12]基于改进的ACO算法实现了时空约束情况下的无人机组协同路径规划，文献[13]采用A*算法和模型预测控制思想提出路径规划与轨迹规划相结合的层次化航迹规划，具有较好的实时性.文献[14]针对无人机组动态航路规划问题，利用蚁群算法预测初始航路，并结合改进的快速扩展随机数(RRT)算法修正实时航路，实验表明，能够在时间协同约束下有效绕过障碍物.文献[15]研究了双无人对警戒雷达进行协同干扰的路径规划问题.此外，双无人机在解决协同定位[16]、跟踪[17]等问题时，路径规划与之紧密相连，文献[18]提出了一种基于求解福克-普朗克方程的非线性滤波方法来解决协同目标定位问题. 文献[19]研究了仅有角度测量的双机协同机动目标跟踪定位路径规划问题.

然而，这些研究大多数在二维场景下且默认无人机组间通信保持良好.但是，在协同任务过程中，无人机组往往会受到敌方干扰，造成通信故障，使得双机信息不能随时共享，导致无人机之间不能更好地进行相互配合，无法更好发挥多无人机的优势.文献[20]提出在短时间内各个无人机到达期望位置形成链式通信中继构型与地面进行通信.文献[4]使用Ad Hoc网络来解决某些无人机节点网络故障问题.而在双无人机内部系统中，只有两架无人机，无法利用中继节点来进行通信.

目前，双无人机在受到干扰的三维空间中协同任务时，造成跟随机无法准确获得领航机的状态的情况并没有相关报道.因此，本文针对这种情况，探索双无人机在受威胁的三维空间中，跟随机(B机)保持持续跟踪领航机(A机)的方法.编队控制使用leader-follower，此外，受干扰影响，B机采用基于交互式多模型IMM(Interacting Multiple Model)算法[21]跟踪A机的位置，其中，采用匀速CV(Constant Velocity)模型[22]和不同时间常数的singer模型[23]进行运动匹配，估计A机的下一个航迹点的位置和理想点的位置，B机在当前点处的转向角约束范围内采用JADE算法[24]优化B机的下一个航迹点，本文给出了双机的每一个航迹点产生的范围约束和采用加权融合方式的适应度函数，并通过数值仿真实验，验证了方法的可行性.

2 双无人机协同路径规划

无人机在复杂环境中的路径规划问题属于多目标，多约束问题.对于A机，需要以最小的代价[7]完成目标任务.对于B机，由于受到干扰，采用交互式多模型(IMM)算法[21]不断预测A机的位置信息，并达到时空协同.A机航路为静态的全局航路，在任务执行前，与B机进行信息共享.

2.1 A机航路规划

假设A机的航线由起始点到达任务点，然后依次执行每一项任务，最后到达终点的多条航段组成，且在受到干扰时，依然保持该航线.因此，首先对A机的每一条航段进行优化，寻找一条最优的航路.

在同一航段中，每条路径由n个航迹点组成，第i条路径表示为：Li=(p1,i,p2,i,…，pn,i)，在三维空间坐标系中，pj,i=[xj,i,yj,i,zj,i]T，j=1,2,…,n.种群由NP条路径组成L={L1,L2,…,LNP}.航迹点的初始化或者进化期间应该在合理的范围内能避免同一航段的路径出现迂回现象，如图1所示.

图1 A机航迹点产生范围

随机产生的pj,i点坐标应满足不等式如下式.

(1)

其中，i=1,2,…,NP；e为航路中某航段的单位方向向量；Mk和Mk+1为第k个航段的两个端点；g和h是该航段中均分的相邻点；o为坐标原点；αj和αj+1分别过g和h点的相邻平面；其法向量均为e.

合理的适应度函数设计是采用进化计算的关键.涉及航程、转向角、生存代价等方面优化，以及高度的约束.

1) 航程优化，Dj为相邻航迹点的距离，有

(2)

2) 相邻点之间长度分布均匀，如下式.

Cv=var(D1,D2,…,Dn-1)

(3)

(4)

4) 转向角优化，ej为相邻两个航迹点的单位方向向量，如下式.

(5)

5) 期望累积代价优化.无人机飞行过程中的5种状态分别是：未被侦测(U)、被侦测(D)、被追踪(T)、交战(E)和被击中(H)，H为吸收态，状态之间的转移符合马尔可夫过程，状态概率向量

P(t)=[pU(t),pD(t),pT(t),pE(t),pH(t)]T.

无人机的期望累积代价[25]v(t)主要由保持代价和转移代价组成，其中，保持代价b=[bU,bD,bT,bE,bH]T，而转移代价主要考虑交战(E)转移到被击中(H)时的代价cH，其余状态之间的转移为0.在时刻t的累积代价为

(6)

其中，I,J∈{U,D,T,E,H}；λIJ为由状态I到状态J的跳跃强度.航迹点在不同环境(雷达、武器、雷达武器多重威胁等)下，λIJ设置不一样.

期望累积代价与当前时刻的状态概率有关，记υ(t)=[PT(t),v(t)]T,如式(7)所示.

(7)

υ(tm)=eAm-1·(tm-tm-1)υ(tm-1)

(8)

其中，m=2,3,…,n；tm-tm-1为相邻航迹点的时间差，若无人机以匀速V飞行，则该时间与相邻航迹点的距离成正相关，即tm-tm-1=Dm-1/V，Am-1在tm-1到tm时刻之间保持不变.

因此，该条航路的期望累积代价为第n个航迹点对应的向量υ(tn)的最后一个元素值：

Cυ=υ6(tn)

(9)

综上，采用加权的方式将上述5个优化目标融合为一个适应度函数：

f(Li)=ω1Cl+ω2Cv+ω3Ch+ω4Cφ+ω5Cυ

(10)

ω为对应目标的权值，进化目标为求下列函数的最优解：

(11)

其中，hxj,i,yj,i为第j个航迹点对应的地形高度，该航迹点的z坐标值必须大于地形高度.

每条航段采用具有更快收敛速度、更强鲁棒性、以及不易陷入局部最优和自适应控制参数(交叉概率CR和缩放比例因子F)等优点的JADE[24]算法对种群L进行变异、交叉和选择操作，并设置一定的进化代数，获得每个航段的最优路径.

2.2 航迹平滑和运动状态获取

无人机的转向角受自身机械条件的约束，其最大转向角为φmax，需要进行路径平滑，如图2所示.

图2 平滑示意图Fig.2 Smooth graph

图2中，pi-1，pi和pi+1是待平滑的相邻航迹点；A为平滑的起点；B为平滑的终点.ei和ei+1是前后两段的单位方向向量，其夹角φi>φmax，Ci为圆心，Ri为半径，有

(12)

(13)

pk=A+Risin(Δθ·k)·ei+Ri(1-cos(Δθ·k))·eac，

(14)

其中，k=1,2,…N；eac向量为ACi线段的单位向量.式(14)可获得每一个平滑点的坐标.对路径中夹角大于φmax的转向角采用同样的方式进行平滑，得到实际可飞的航路.

利用交互式多模型算法进行目标跟踪时，检测算法的有效性需要了解目标的真实运动状态，假设，无人机A在未平滑的航迹点上匀速运动，在平滑的航迹点上做匀速圆周运动，速度大小均为V.

1) 未平滑的航段，例如图2中pi-1A和Bpi+1段，速度v=V·e，此时e为相邻航迹点的单位方向向量，加速度a=0.

2) 平滑的航段，例如图2中AB段，速度方向为切线方向，第k个平滑点的速度方向

(15)

速度vk=V·qk.加速度为法向加速度：ak=(V2/Ri)·epkc，epkc由pk点指向圆心的单位向量.

2.3 目标跟踪

2.1节和2.2节规划了A机的航路并获得了每个航迹点的状态(位置、速度和加速度).在受到干扰的情况下，使得B机获得A机状态时存在噪声，所以B机通过融合多个机动模型的状态估计A机的运动，典型目标跟踪离散模型.

(16)

为了能够更为准确的跟踪A机的运动状态，更完备的机动模型才能更准确的逼近目标的运动情况.根据平滑后的运动状态采用一个CV模型和4个不同机动时间常数α的singer模型匹配A机的运动，singer模型的α设置如表1所示.

表1 4组不同时间常数的singer模型

Tab.1 Four sets of singer models with different time constants

组别αxαyαz11/601/6012111/60311141/101/101/40

表1中，组1跟踪无人机的爬升运动；组2跟踪水平方向的运动；组3跟踪空间转弯；组4兼顾水平方向和竖直方向.singer模型的F(k)和Q(k)如下.

其中，m=x,y,z，F(k)和Q(k)均为块对角矩阵，F(k)=blkdiag(Fs,Fs,Fs)，而Q(k)=blkdiag(Qs,Qs,Qs)，T(k)为采样时间，考虑到外在因素影响，前后两次的采样时间可能不同，本文使用A机经过两个连续的航迹点的行驶时间作为采样间隔时间.采样时间利用IMM对A机进行状态估计时，每个模型并行卡尔曼滤波运算.IMM框架通过递推运算估计目标的状态，每一步递推分别经过输入交互，条件滤波，模型概率更新和估计融合4个步骤完成.

2.4 B机跟随

A机和B机采用leader-follower的编队控制，A机以固定速度V飞行，而B机速度VB∈[Vmin,Vmax].为了能够协同完成目标任务，B机应尽量避开雷达武器等威胁，同时双机保持时空协同性.因此B机优化的目标为下一个满足要求的位置点.

B机的下一个位置Bnext产生方式如图3所示，当前B机的位置为Bcur，B机上一个位置Bpre，p1，p2为前后两段的单位方向向量.当前理想位置Ccur，下一个理想位置点Cnext，理想位置点处于IMM模型递推估计点的周围，e为相邻航段(起点到第一个任务点，相邻任务点之间或者任务点到终点)的单位向量.α0，α1和α2分别过Bcur，Ccur和Cnext且法向量为e的平面.dnext为Bcur到α2的距离.

图3 B机航迹点产生范围

L为步长，L∈[lmin,lmax]，若Ccur到Cnext的时间间隔为T(k)，则，lmin=VminT(k)，Bcur到α2平面的最长距离为最长母线长度：lmax=dnext/cos(arccos(eT·p1)+φ)，如果lmax>VmaxT(k)，那么，lmax=VmaxT(k).

为了保证航迹可飞，航迹点的转向角要求φ≤φmax，因此Bnext点必须落在图中锥体阴影部分内.Bnext点的坐标需要满足下式.

(17)

B机的适应度函数一方面尽量避开雷达武器威胁，其次避免与A机发生碰撞.双机位置关系如图4所示，R为无人机的探测距离，r为双机的安全距离.

图4 双机的位置关系

1) B机与A机要避免碰撞，

(18)

(19)

其中，ε为一个极小的常数；C为较大的固定值.双机不能互相检测到时，为一个常数.

2) 与雷达武器的关系，

Dt=min(Dt1,Dt2,…)

(20)

(21)

其中，tr为雷达或武器的辐射半径.

(22)

4) B机飞行高度与地形的差值Dh=zk-hxk,yk，hxk,yk为(xk,yk)点对应地形高度.B机与地形关系，

(23)

ε为一个极小的常数，防止分母为0.

综上，B机的适应度函数采取加权融合的方式：

f(Bnext_k)=w1fa+w2ft+w3fc+w4fh

(24)

进化的方向为min(f(Bnext_k)).利用JADE算法进行优化，设置最大迭代数，计算较优的Bnext点.

此外，当长时间无法获得领航机的运动状态时，IMM模型的估计可能会造成无法协同任务.当遇到此种情况时，为了保证跟随机能够尽可能跟踪领航机，只需要维持跟随机速度与领航机初始速度大小一致，并沿着起始时刻共享的全局静态航路飞行，直到再次获得领航机的运动状态信息，再进行IMM+JADE的跟踪与协同.

3 实验结果与分析

在Matlab 2016a中进行数值仿真，CPU为Intel Core i5，主频2.5 GHz.场景为700*600*500的区域内，起点(300,450,285)，终点(930,810,285)，场景内设置3个任务点，3个雷达点和2个武器点.A机飞行速度大小为10 m/s，B机速度范围为0～15 m/s,最大转向角φmax为π/12.

3.1 A机路径规划仿真结果与分析

A机在不同环境中的跳跃强度λIJ设置与文献[7]一致，保持代价b=[0,1,10,100,0]T，从交战E到被击中的转移代价cH=8000，初始状态概率P=[1,0,0,0,0]T.优化目标的权重ω=[0.2,0.2,0.2,0.2,0.2]TA机路径规划结果如图5所示.

图5航迹绕开了相应的雷达和武器，且在某些航段保持直线飞行.在三维空间中的平滑方法满足飞行要求.

对A机的路径规划结果按文献[25]的评估方法进行评估，其结果如图6所示.其中，M1，M2和M3对应图5(a)中的3个任务点，由图6可以看出，A机在飞行过程中处于被侦测、被追踪、交战和被击中的状态概率较低，在M3点附近最大，但仅接近0.3，因此，A机整个飞行过程主要处于未被侦测状态.

(a) A机航路规划

(b) 路径平滑局部示意图图5 JADE路径规划Fig.5 JADE Route Planning

图6 A机航路评估结果Fig.6 The leader UAV route assessment results

3.2 跟踪仿真结果与分析

由于存在干扰，B机获得A机状态时存在噪声，量测噪声协方差矩阵R=[1,0,0;0,1,0;0,0,1]，量测矩阵H=blkdiag([1,0,0],[0,1,0],[0,0,1])，采用CV模型和表1所示的4个不同机动时间常数α的singer模型匹配A机的运动，进行30次仿真，并统计每个位置点处滤波值和真实值在每个坐标轴上的误差均值和方差.图7为跟踪效果，图8记录了每个坐标轴上的均值和方差.

(a) 模型滤波效果 (b) 位置误差均值

图7 IMM模型跟踪结果

Fig.7 IMM model tracking results

图7模型的位置跟踪误差(同一时刻跟踪点与实际点间的欧式距离)均值范围在5 m以内，在转弯处的方差有一定的波动，这是使用singer模型模拟三维空间转弯模型造成的误差，只有更完备的机动模型集合，才能更准确的跟踪目标运动，但也会造成较大的运算量，本文的CV模型和4个singer模型能够囊括大部分A机的运动，能较好的完成跟踪，此外，为了尽可能避免碰撞，将双机的安全距离r设置大于5 m.

3.3 B机协同路径规划仿真结果与分析

双机的探测距离R=24 m，安全距离r=5 m，理想点位置与跟踪时A机的位置处于同一高度，且在A机右侧(R-r)/3+r处.其中两个常数ε=1e-8，C=1e+5；B机优化目标权重w=[1,1,1,1]T. B机利用跟踪结果进行路径规划结果如图9所示，B机航线能较好的保持在理想位置左右.

对B机规划后的路径按文献[25]进行路径评估，如图10所示，任务点M3处于雷达范围内，处于被侦测和被追踪的概率低于0.4，任务点M1处于雷达边沿，B机短时间处于被侦测状态，但图中显示其概率较小(低于0.1)，间隔时间较短，说明B机能较快的避开相应的雷达武器威胁.

利用IMM跟踪后的路径始终不是A机的实际飞行路径，所以，统计同一时刻双机实际航迹点欧式距离，运行30次取均值，如图11所示.

图9 B机航路规划Fig.9 The follower route planning

图11 双机在不同航迹点的真实距离均值Fig.11 The true distance mean of the two UAVs at different track points

图10 B机路径评估Fig.10 The follower route assessment results

图11显示，只有8.8%的双机航迹点间距超过双机探测距离24 m，并在短时间内能够回到初始理想距离，在空间上保持了双机的协同.且双机距离均大于安全距离，说明在任意时刻，双机没有发生碰撞.在航迹结束阶段，双机的距离与起始阶段基本持平，因此，时间上双机保持了协同.

4 结 论

本文研究无人机在受到敌方干扰，跟随机无法准确获得领航机状态的情况，提出结合交互式多模型算法和JADE算法为B机优化下一个航迹点.该方法采用了1个CV模型和4个不同时间常数的singer模型跟踪A机的运动，双机的航路优化使用了不同适应度函数的JADE算法，A机的整体航路采用分段优化，优化的目标是产生航路代价较小的路径，而B机跟随时优化的目标是下一个满足时空协同的航迹点.经过数值仿真实验表明，双机航迹平滑，累积代价较小，跟踪误差在5 m以内，且仅有8.8%的航迹点在探测范围之外，并能够快速回到理想位置点.该方法能够有效解决双机在受干扰情况下的协同问题.

此外，由于依赖交互式多模型算法的跟踪准确率，因此，为了能更有效的保证双机的协同和安全,未来的研究重点在以下两个方面：第一，跟踪时，建立更完备的运动模型和更优的滤波算法.第二，在动态威胁环境中，B机在某些时段与A机并未在时空上保持协同，如果A机继续按照既定航线可能长时间陷入威胁中，因此，A机也应具有动态规划的能力，并能够跟踪B机状态，协调双机航路.