基于定制内点法的多无人机协同轨迹规划

王 祝 徐广通 龙 腾

多无人机(Unmanned aerial vehicle,UAV)协同通过功能互补和能力叠加等方式可以提高任务完成效能,已成为无人机应用的重要发展趋势[1],协同轨迹规划(Cooperative trajectory planning,CTP)则是系统关键技术之一.

针对轨迹规划存在的非线性、约束多、变量维度高等计算复杂难题,近年来序列凸优化(Sequential convex programming,SCP)方法受到广泛关注[2].SCP 利用凸优化算法具有迭代计算上界和多项式时间复杂度[3]的特点,通过求解一系列凸优化子问题[4],可快速获得原非凸问题的局部最优解,其优势已在不同轨迹规划领域得到验证.相比基于采样的轨迹规划方法,SCP 具有更强的高维求解能力且无需进行光滑处理[5-6];相比伪谱法,SCP 能够在不降低优化性能的条件下极大地提高求解效率[7-8];相比启发式算法,SCP 具有更好的寻优和收敛能力[9-10].

在协同轨迹规划研究方面,文献[11] 首次将SCP 应用于四旋翼飞行器协同轨迹规划;文献[12]进一步提出增量SCP 以提高算法收敛速度;Morgan 等[13]提出分散SCP 以快速生成集群重构轨迹,并基于四旋翼完成了实验验证;针对固定翼无人机,文献[14]提出一种罚函数SCP 方法实现了多机协同轨迹规划;文献[15]提出一种解耦SCP 方法,通过并行求解降低了算法耗时随无人机数量的增长程度.

虽然SCP 相比于传统轨迹规划方法具有效率优势,但固定翼无人机编队轨迹规划仍需数十秒[14-16],难以满足动态规划需求.对此,本文在SCP 基础上,从子问题求解性能角度开展研究,通过定制凸优化算法降低子问题优化耗时,从而提高规划效率.

凸优化子问题一般采用SeDuMi[17]和Mosek[18]等工具包求解,但工具包为保证稳定性需增加额外运算成本,同时难以针对具体问题设计高效求解过程.已有的内点法定制研究表明,通过算法定制能够显著提高凸优化效率.Dueri 等[19]对火箭回收轨迹规划进行内点法定制,实现了轨迹实时规划,并完成了实验验证.Xu 等[20]通过定制稀疏内点法,将单个四旋翼轨迹规划耗时控制在1 s 以内.但是,定制算法不具有广泛通用性,例如针对火箭或四旋翼轨迹规划的定制内点法无法直接应用于固定翼无人机.根据文献调研,尚无针对单架或多架固定翼无人机轨迹规划的定制凸优化算法.

固定翼无人机的动力学呈非线性,同时多无人机系统还需考虑机间协同约束,这些因素都增加了轨迹规划复杂度.对此,本文在解耦SCP[15]的架构下,研究子问题求解的定制内点法,通过减少单次凸优化耗时,进一步提升多无人机协同轨迹规划效率.

1 问题建模

多无人机协同轨迹规划需要根据每架无人机的起始点和目标点,考虑运动方程、终端条件、边界条件、避障和避碰等约束,以最小化时间或能量等为性能指标,为所有无人机生成可行的飞行轨迹.

本文以时间最短为目标,将无人机协同轨迹规划建模为下述最优控制问题P1[15]:

其中,下标i表示无人机编号;下标0 表示变量初始时刻值;下标f表示变量终端时刻值;N为无人机数量;t为飞行时间;si表示无人机i的状态量且si=(xi,yi,hi,vi,χi,γi)T;smax和smin为状态量上下限;ui表示无人机i的控制量且ui=(nx,i,ny,i,nz,i)T;umax和umin为控制量上下限;pobs,m和rm为障碍m的位置和半径;M表示障碍数量;C=[I2×2,04×4];R为机间避碰安全距离.

图1 给出了多无人机协同轨迹规划问题及部分参数的示意图.

图1 多无人机协同轨迹规划问题示意图Fig.1 Illustration of multi-UAV cooperative trajectory planning problems

问题P1 中各子式的意义可参见文献[15-16].其中,运动方程=f(si,ui,t)的表达式[15]如式(2)所示.其中,(x,y)为水平位置;h为高度;v为速度;χ为航向角;γ为航迹倾角;g为重力加速度值;nx,ny,nz分别为无人机在切向、水平、铅垂方向的过载.

2 解耦SCP 方法

针对式(1)所述的最优控制问题,文献[15]给出了一种解耦SCP 求解架构.本文在该架构的基础上,聚焦凸优化子问题的定制内点法研究.考虑文章完整性,本节对解耦SCP 方法进行简述.

基于SCP 的无人机轨迹规划步骤可概括为:1)采用配点法将最优控制问题转化为非凸优化问题;2)利用约束凸化构建一系列凸优化子问题;3)通过迭代求解凸优化子问题,获得非凸优化问题的解,即最优控制问题的数值近似解.

在SCP 基础上,解耦SCP 进一步针对协同规划问题,利用轨迹冻结思想和协调约束机制,将关于多机轨迹的一个优化问题解耦为一组优化问题,解耦后每个问题的优化变量仅为一架无人机的轨迹,且解耦问题支持并行求解,提高了协同轨迹规划效率.解耦SCP 方法的求解架构如图2 所示.

图2 解耦SCP 方法架构Fig.2 Frame of decoupled SCP method

针对式(1)所述的问题P1,利用梯形积分和约束凸化,并引入罚函数和信赖域,可构建解耦SCP方法中需多无人机并行迭代求解的凸优化子问题P2[15],如式(3)所示.

3 子问题最优性条件

为了开展内点法定制研究,本节首先构建子问题的一种等价描述形式,然后基于最优化理论构建该形式下的结果最优性条件.

3.1 凸优化子问题的等价描述形式

为了方便表述和推导,将问题P2 罚函数中的所有等式约束统一表示为式(13),包括运动约束式(5)以及终端约束式(6)和式(7),其中zi简记为z.

将不等式约束统一表示为式(14),包括边界约束式(8)～(10)、避障约束式(11)和避碰约束式(12),即

由于凸优化子问题P2 的约束式(6)～(12)均具有线性特征,因此上述表述是可行的.在此约束表示形式下,凸优化子问题P2 可表示为子问题P3

其中,下标l表示取矩阵或向量第l行,且

则凸优化子问题可进一步等价表示为下述仅含有不等式约束的线性规划子问题P4.

其中,变量X维度nX和约束数量nA为

3.2 凸优化子问题的最优性条件

针对式(20)所示的不等式约束线性规划问题P4,其拉格朗日函数L(X,λ)可表示为

根据凸优化理论的Karush-Kuhn-Tucker 条件[3],子问题P4 的最优性条件可表示为

引入辅助变量s=-(A·X-b),可将上述最优性条件等价表示为

针对式(26)给出的子问题最优性条件,利用数值算法对该非线性方程组进行迭代求解,即可获得问题P4 的最优解,即凸优化子问题P2 的最优解.

4 内点法定制

本节针对凸优化子问题的最优性条件方程组,定制高效的内点法以降低子问题优化的计算复杂度,进而提高原协同轨迹规划问题的求解效率.

4.1 预测-校正原对偶内点法框架

本文定制内点法的框架沿用文献[21]提出的预测-校正原对偶内点法,其流程如图3 所示,包括初始化、预测步、校正步、搜索点更新和收敛判断等.

图3 原对偶内点法计算框架Fig.3 Frame of primal-dual interior-point method

在预测-校正原对偶内点法的每次迭代中,搜索点的更新需考虑预测步和校正步.其中,预测步通过求解牛顿系统计算仿射方向和步长;校正步则利用仿射搜索信息对牛顿系统进行高阶修正,然后求解获得校正搜索方向和步长,以实现沿中心路径快速搜索最优解.其中,在确定搜索方向后,搜索步长h根据Wolfe 准则计算得到.

中心路径为严格可行点构成的一条曲线[21],其定义为

其中,τ为中心路径参数.F(X,λ,s)可根据式(26)表示为

其中,Λ=diag{λ},S=diag{s},e=[1,1,···,1]T.

4.2 原对偶搜索方向计算公式

在预测-校正原对偶内点法中,搜索方向ΔX由预测步搜索方向 ΔXaff和中心校正方向ΔXcen构成,如式(29)所示[21].

其中,上标aff 表示预测步的参数取值,上标cen 表示校正步的参数取值.ΔXaff和 ΔXcen的计算方法如下所述.

1)预测步搜索方向

在预测步,直接利用牛顿法计算迭代方向,即计算函数F(X,λ,s)的雅可比矩阵J(X,λ,s),构建非线性方程组的牛顿系统,具体为

其中,下标q表示内点法中当前的迭代步数.

因此,预测步的搜索方向 ΔXaff可根据式(31)计算得到:

其中,Δλ和 Δs表示拉格朗日乘子λ和辅助变量s对应的搜索方向;右端项的对偶残差rd,原残差rp和互补残差rsλ可将第q次迭代步的值(Xq,λq,sq)代入式(28)计算得到:

2)校正步搜索方向

预测搜索方向是对非线性系统线性化计算得到的仿射方向,为了使迭代能够贴近中心路径和加快迭代收敛,增加校正步以修正仿射搜索方向.

由于式(26)的前两项均为仿射方程,因此校正步仅需对最优条件的第3 项进行校正,校正后的互补残差可表示为

其中,σ∈(0,1)为中心校正因子,µ为互补条件均值,其表达式为

基于校正后的残差值,校正步的搜索方向ΔXcen可根据式(35)进行计算:

4.3 原对偶搜索方向快速计算方法

为了获得原对偶搜索方向,预测步和校正步需要分别求解式(31)和式(35)所示的非线性方程组,本节针对方程特点,定制搜索方向的快速计算方法.

1)逐次消元求解搜索方向

若直接对方程组的左端矩阵进行求逆,由于矩阵非对称且不正定,迭代过程中容易发生矩阵奇异,导致内点法迭代难以收敛.

对此,本研究通过逐次消元推导,定制非线性方程组的求解步骤,避免求解过程出现非对称矩阵求逆运算,而且变量逐次求解可以降低计算复杂性.

针对预测步搜索方向的计算,首先根据式(31)的第3 行,推导可得

将式(36)与式(31)第2 行结合,消去 Δsaff,可得

再将式(37)与式(31)第1 行结合,消去 Δλaff可得

通过上述推导,将根据式(31)对变量同时求解的方式转换为依次根据式(38)、(37)和(36)求解ΔXaff、Δλaff和 Δsaff.

采用相同的消元方式,可以得到校正步方向的计算公式,即

2)基于LDL 分解降低计算复杂度

依据前述的消元逐次求解方法,原对偶搜索方向的计算量主要在的求逆运算.由于S和 Λ 均为对角矩阵,所以S-1·Λ 为对角矩阵.结合式(22)中A的表达式,可将S-1·Λ 表示为

其中,

对于多无人机协同轨迹规划问题,由式(5)～(12)可知,矩阵Aeq、Ain和Atr均为稀疏矩阵,其中Aeq为分块双对角与列向量组合的形式,如式(43)所示.

Ain中的避障约束、避碰约束、状态边界约束以及信赖域约束系数矩阵Atr均为分块对角矩阵,例如信赖域约束的左端项系数矩阵形式如式(44)所示.

同时,考虑到矩阵D1,D2,···,D5均为对角矩阵,因此,为稀疏的对称矩阵.对此,可以采用稀疏矩阵的LDL 分解[22],即

其中,P为排列矩阵,L为单位下三角矩阵,D为分块对角阵.则式(38)可以转化为

从而,可将求解搜索方向 ΔXaff转化为求解一系列低计算成本的方程,计算过程如式(47)所示.而且计算校正搜索方向 ΔXcen时,可用同样的方法降低计算成本.

为了说明定制方法计算搜索方向的优势,以浮点运算次数为统计指标,对下述3 种方式的计算复杂度进行定量分析: 方法I (即定制方法)通过LDL分解计算 ΔX,再依次计算 Δλ和 Δs;方法II 以求逆方式求解式(38),再计算 Δλ和 Δs;方法III 直接以求逆方式求解式(31).3 种方法的计算成本如表1 所示,其中,nX和nA的表达式见式(23).

表1 原对偶搜索方向计算复杂度Table 1 Computation complexity for primal-dual search direction

图4 给出了3 种搜索方向生成方法的计算复杂度随变量维度nX和约束数量nA的变化情况.由表1和图4 可见,本文提出的定制方法通过消元逐次计算(比较方法III 和方法II)和LDL 分解(比较方法II 和方法I)减少了原对偶搜索方向的计算复杂度,从而可以降低子问题优化成本.

图4 原对偶搜索方向计算复杂度变化图Fig.4 Variations of computation complexity for primal-dual search direction

另外,上述定制算法利用数值特征降低子问题的计算成本,并不会改变原对偶内点法或SCP 协同轨迹规划的收敛性,其收敛性分析可参考文献[4]和文献[23-24].

5 仿真实验

本节通过数值仿真实验,在解耦SCP 协同轨迹规划架构下,验证定制内点法的有效性,并将定制内点法与一般内点法进行对比,验证定制内点法的性能优势.

5.1 仿真环境与想定

数值仿真实验在Windows7 中的MATLAB 2016a 下进行,硬件环境为Intel Core i5-2310@2.90 GHz 处理器和4 GB 内存的PC 机.凸优化子问题分别利用定制内点法和SeDuMi[17]进行对比求解.SeDuMi 是广泛应用于MATLAB 环境的一种凸优化工具包,其中采用了基本的预测-校正原对偶内点法,因此将其作为对比算法以验证本文方法的性能.

采用文献[15]的仿真实验算例,考虑7 架无人机所构编队的集结和重构任务,对本文提出的定制内点法进行测试验证.无人机起始点、目标点和障碍的水平位置见仿真结果图,无人机的起始点和目标点高度均设为400 m,障碍半径均设为400 m.编队飞行过程中,无人机间的防撞安全距离设为100 m.

无人机初始时刻和终端时刻的速度、航向角和航迹倾角分别设为35 m/s,0 rad 和0 rad.无人机状态量和控制量的边界约束为

针对上述编队集结和重构想定,SCP 算法相关参数设置为

5.2 仿真结果与分析

1)定制内点法有效性测试

针对设定的7 架无人机编队集结和重构任务,基于定制内点法进行轨迹规划求解,轨迹结果分别如图5 和图6 以及图7 和图8 所示.由图可知,定制内点法生成的无人机协同轨迹能够对飞行空间中的障碍进行有效规避,并实现从初始点到给定集结点或队形重构点的平滑转移.距离目标点较近的无人机进行了绕远飞行,以实现飞行时间协同.

图5 编队集结协同轨迹二维结果图Fig.5 Cooperative trajectories in 2D for formation rendezvous

图6 编队集结协同轨迹规划三维结果图Fig.6 Cooperative trajectories in 3D for formation rendezvous

图7 编队重构协同轨迹规划二维结果图Fig.7 Cooperative trajectories in 2D for formation reconfiguration

图8 编队重构协同轨迹规划三维结果图Fig.8 Cooperative trajectories in 3D for formation reconfiguration

图9 和图10 给出了编队飞行过程中无人机间最短距离的变化曲线.由图可知,任意两架无人机间的距离始终大于设定的最小安全距离,可见定制内点法生成的协同轨迹能够满足机间避碰约束,实现空间协同.

图9 编队集结任务无人机间最短距离变化曲线Fig.9 Variation curve of minimum distance between UAVs for formation rendezvous

图10 编队重构任务无人机间最短距离变化曲线Fig.10 Variation curve of minimum distance between UAVs for formation reconfiguration

因此,在解耦SCP 框架下,采用定制内点法对凸优化子问题进行求解,能够规划生成满足约束的无人机编队协同轨迹.

2)算法性能对比测试

在验证定制内点法有效性的基础上,开展算法对比仿真实验,比较定制内点法和SeDuMi 工具包中一般内点法的轨迹规划性能.针对编队集结和重构任务,编队无人机数量由1 逐渐增加至7,子问题求解分别采用定制内点法和SeDuMi,对获得的协同轨迹规划结果和计算耗时进行统计.

针对编队任务完成时间即飞行时间,定制内点法和SeDuMi 优化得到的编队集结时间如图11 所示,编队重构时间如图12 所示.不同编队规模下,SeDuMi 获得的集结时间均值为179.67 s,定制内点法为186.11 s,两者相差约3.6%;SeDuMi 获得的重构时间均值为187.34 s,定制内点法为188.67 s,两者相差约0.7%.如式(1)所示,本文以任务完成时间最短为优化目标,因此定制内点法的寻优能力与SeDuMi 基本相当.

图11 编队集结任务完成时间优化结果Fig.11 Optimized completion time for formation rendezvous

图12 编队重构任务完成时间优化结果Fig.12 Optimized completion time for formation reconfiguration

针对7 机编队集结任务,图13 和图14 给出了SeDuMi 和定制内点法规划结果对应的速度、高度和过载变化曲线.根据7 机集结平均速度统计结果,SeDuMi 为34.27 m/s,定制内点法为33.08 m/s.这与图11 中的7 机集结时间结果相符,SeDuMi 所得集结时间(232.86 s)少于定制方法(256.63 s).

图13 基于SeDuMi 的编队集结轨迹规划结果Fig.13 Trajectory planning results using SeDuMi for formation rendezvous

图14 基于定制内点法的编队集结轨迹规划结果Fig.14 Trajectory planning results using customized interior-point method for formation rendezvous

对无人机集结过程中速度和高度的标准差进行计算,再统计获得编队的整体均值,其中SeDuMi 结果为1.78 m/s 和4.33 m,定制内点法结果为1.49 m/s和2.36 m.可见定制内点法生成轨迹的状态变化稍平滑,更易于轨迹跟踪控制,但两者差距不大.

对无人机过载的绝对值之和进行统计,该值可近似反映无人机控制能耗的大小关系.其中铅垂方向以 (n2-1)进行统计.根据统计,SeDuMi 所得协同轨迹的三轴过载绝对值之和为(3.77,33.04,0.91),定制内点法为(3.66,37.30,0.97).可见,两种方法所生成轨迹对应的控制能耗大小相当,按定制内点法结果飞行所需的控制能耗略高.

图15 和图16 分别给出了7 机编队重构想定下SeDuMi 和定制内点法规划轨迹的速度、高度和过载变化曲线.SeDuMi 生成轨迹的编队平均速度为35.71 m/s,定制内点法则为36.08 m/s.根据图12的7 机编队重构时间结果,SeDuMi 获得的重构时间(191.91 s)相应地也大于定制内点法结果(191.24 s).

图15 基于SeDuMi 的编队重构轨迹规划结果Fig.15 Trajectory planning results using SeDuMi for formation reconfiguration

图16 基于定制内点法的编队重构轨迹规划结果Fig.16 Trajectory planning results using customized interior-point method for formation reconfiguration

根据编队无人机速度标准差和高度标准差的均值统计结果,SeDuMi 为1.02 m/s 和0.36 m,定制内点法为1.64 m/s 和0.34 m.可见两种方法生成重构轨迹的平滑性相当,都变化不大,易于进行轨迹跟踪控制.

根据7 机编队重构轨迹的过载统计结果,Se-DuMi 生成轨迹对应的三轴过载绝对值之和为(1.41,13.84,0.16),定制内点法则为(2.72,18.60,0.33),可见定制内点法所得轨迹对应的控制能耗略高于SeDuMi 所得轨迹.

综合编队集结和重构的实验结果及分析,在任务完成时间最优性、轨迹可实现性和轨迹控制能耗方面,定制内点法所得轨迹结果与SeDuMi 结果基本相当,即两种方法在上述轨迹指标中相互不存在明显优势.

基于定制内点法和SeDuMi 进行编队轨迹规划的计算耗时统计结果如图17 和图18 所示.由图可见,在所有想定下,定制内点法的规划效率都优于SeDuMi.其中,对于7 机集结和重构轨迹规划问题,基于SeDuMi 的轨迹规划耗时分别为19.0 s 和13.1 s,而基于定制内点法的耗时仅为3.0 s 和2.4 s.不同规模下编队集结和编队重构规划结果的统计数据表明,基于定制内点法的轨迹规划平均耗时约为SeDuMi 求解耗时的1/7 和1/6.

图17 编队集结轨迹规划耗时Fig.17 Computation time for formation rendezvous

图18 编队重构轨迹规划耗时Fig.18 Computation time for formation reconfiguration

同时,对于上述编队集结和重构轨迹规划问题,单机解耦规划时凸优化的变量维度nX和约束数量nA约为103和2×103,结合表1给出的计算复杂度表达式,此时定制内点法在子优化问题求解时的计算复杂度约为一般内点法的1/10.但除了子问题求解,轨迹规划还需要完成子问题构建、协同约束生成等计算,这些过程的计算耗时对于定制内点法和一般内点法是基本相同的.因此,定制内点法完成协同轨迹规划的耗时与一般内点法耗时的比值会大于1/10,这与仿真实验获得的计算耗时统计结果是一致的.

另外,根据文献[15]给出的伪谱法解耦协同轨迹规划结果,上述编队集结和重构的平均计算耗时为100.7 s 和111.0 s.可见,相比于广泛应用的伪谱法,本文提出的定制内点法可显著提高轨迹规划的效率.

根据上述轨迹结果分析和计算时效性对比可知,定制内点法生成的编队轨迹结果最优性与一般内点法相当,但采用定制内点法可将协同轨迹规划的计算耗时降低一个数量级,即从十秒级降低到秒级.对于7 机编队轨迹规划问题,在解耦SCP 规划架构下,定制内点法可以在3 s 内生成安全可行的编队协同轨迹.

6 结束语

针对基于解耦SCP 的多机协同轨迹规划中需迭代求解的凸优化子问题,通过引入松弛变量构建等价形式,并利用协同轨迹规划问题的模型特征,定制子问题求解的高效内点法,以降低子问题计算耗时,进而提高协同轨迹规划效率.在计算复杂度分析的基础上,通过数值仿真对定制内点法的有效性和性能优势进行验证.仿真结果表明,在轨迹寻优能力、轨迹可实现性和轨迹控制能耗方面,定制内点法可以获得与SeDuMi 相当的轨迹结果,但定制内点法完成轨迹规划的计算耗时相比SeDuMi 可以降低一个数量级,即由十秒级降至秒级.