CN111562797A - 确保收敛的无人机飞行时间最优实时轨迹优化方法 - Google Patents

Abstract

本发明公开的确保收敛的无人机飞行时间最优实时轨迹优化方法，属于轨迹优化领域。本发明在考虑重力作用、速度和加速度因素下建立无人机运动学模型，并建立三维无量纲运动方程。根据无人机避障飞行的具体要求建立速度和控制量的约束条件，选取时间最小作为优化目标建立无人机飞行轨迹规划最优控制问题P0。随后将问题P0中的非线性动力学变换为线性动力学，得到固定初始和末端时间轨迹优化问题P1。通过凸松弛把P1问题松弛为近似凸优化问题P2，提高无人机实时求解的鲁棒性和稳健性。将问题P2离散形成二阶锥规划问题P3，通过有限次地迭代求解二阶锥规划问题P3所得最优解即为无人机飞行时间最优轨迹。本发明能够进一步提升无人机的任务反应能力。

Description

确保收敛的无人机飞行时间最优实时轨迹优化方法

技术领域

本发明属于轨迹优化领域，涉及无人机飞行时间最优的实时轨迹规划方法，尤其涉及一种基于在线凸优化技术且保证收敛的飞行时间最优实时避障轨迹优化方法。

背景技术

在过去几年中，作为空中机器人平台，无人机技术惠及人类生产生活的各个方面，而轨迹规划在无人机遂行载荷输送、目标搜索、环境监测、农业植保等任务中有关键作用。无人机执行大范围飞行任务需要提前规划飞行轨迹，为了尽可能发挥无人机的飞行性能以提高无人机执行任务的敏捷性，快速规划出具有最小飞行时间的飞行轨迹显示出突出重要的技术地位。

为了实时地解决时间最优轨迹规划相应的最优控制问题，以获得飞行需要的任务轨迹。盲目通过暴力的启发式算法难以满足敏捷性无人机对轨迹优化环节的实时性要求；一般的非线性规划算法求解这个非凸问题并不总是可行的；基于状态迭代的序列优化或者序列凸优化方法求解此类问题具有实时应用的潜力，但是无法在理论上确保优化迭代过程的大范围收敛性，因此这类算法的求解效率难以在理论上的得到保证，而直接基于非凸规划算法求解时间最优轨迹优化问题虽然具有可行性，但是求解效率低下同样难以满足实时性要求；Dubins路径规划方法能够给出二维平面运动且速度不可变条件下的时间最优飞行路径，此类方法具有很好的实时应用价值，但是无法处理三维运动且速度可变的情况，基于样条拼接的路径规划方法难以生成兼顾时间最优且速度可变的轨迹。此外，当前采用机器学习方法设计的最小时间轨迹规划算法，难以处理小样本轨迹或者未建模样本轨迹的情况，受困于黑箱模型其求解过程往往难以解释，无法充分验证在线应用时的飞行时间最优性。

发明内容

本发明公开的确保收敛的无人机飞行时间最优实时轨迹优化方法要解决的技术问题是：通过在线优化求解满足飞行速度、加速度和避障约束的时间最优轨迹，为此，建立一个以无人机三个方向加速度作为控制量的最优控制问题，并且包含加速度控制量和速度等状态量的约束以反映无人机的实际飞行能力。将原非凸非线性的优化问题松弛为一个二阶锥规划问题，通过迭代求解一系列二阶锥规划问题收敛到原问题的解并得到加速度和速度方向的最优变化策略，通过所述松弛方法确保轨迹优化方法的收敛性，进而提高无人机实时求解的鲁棒性和稳健性，此外，采用本发明求解的时间最优飞行轨迹能够进一步提升无人机的任务反应能力。所述无人机包括四旋翼、无人固定翼飞机以及无人垂直起降飞机等。

本发明的目的是通过下述技术方案实现的：

本发明公开的确保收敛的无人机飞行时间最优实时轨迹优化方法，在考虑重力作用、速度和加速度因素下建立无人机运动学模型，并对无人机运动学进行变换并量归一化，建立三维无量纲运动方程。根据无人机避障飞行的具体要求建立速度和控制量的约束条件，给出障碍的三维椭球和椭圆圆柱的描述，选取时间最小作为优化目标建立无人机飞行轨迹规划最优控制问题P0。随后将原非凸最优控制问题P0中的非线性动力学变换为线性动力学，得到固定初始和末端时间轨迹优化问题P1。通过凸松弛将P1问题中存在或者引入的非凸约束转化为凸约束，进而把P1问题松弛为近似凸优化问题P2，所述松弛方法确能够保证迭代优化求解过程的收敛性，进而提高无人机实时求解的鲁棒性和稳健性。最后在归一化区间上用(N+1)个离散点将近似凸优化问题P2离散形成二阶锥规划问题P3，通过有限次地迭代求解二阶锥规划问题P3所得最优解即为无人机飞行时间最优轨迹。

本发明公开的确保收敛的无人机飞行时间最优实时轨迹优化方法，包括如下步骤：

步骤一：考虑重力作用、速度和加速度因素建立无人机运动学模型，并对无人机运动学进行变换并量归一化，建立三维无量纲运动方程。

考虑重力作用、速度和加速度因素建立无人机运动学模型，并对无人机运动学进行变换并量归一化，无人机三维避障的无量纲运动方程表示为：

其中，r＝[x，y，z]^T是无人机的空间位置，z是高度，x，y是水平面正交方向的坐标；V是无人机速度；a＝[a_x a_y a_z]^T是无人机机动过载，g＝[0 0 g]^T是重力加速度常矢量。在式(1)中，距离变量[x，y，z]^T用初始和末端位置的欧式距离L₀来归一化，速度用

归一化。时间用

归一化。

步骤二：根据无人机避障飞行的具体要求建立速度和控制量的约束条件，给出障碍的三维椭球和椭圆圆柱的描述，选取时间最小作为优化目标建立无人机飞行轨迹规划最优控制问题P0。

步骤2.1：根据无人机避障飞行的具体要求建立速度和控制量的约束条件，所述约束条件包括初始和末端约束、速度约束、加速度约束、姿态约束、障碍约束。

步骤2.1.1：初始和末端约束为:

其中r₀＝[x₀,y₀,z₀]^T,r_f＝[x_f,y_f,z_f]^T是初始和末端位置；V₀＝[v_x0,v_y0,v_z0]^T和V_f＝[v_xf,v_yf,v_zf]^T是初始和末端位置。此外，考虑到无人机常用速度大小和角度来描述速度矢量，如此，用飞行路径角Ψ和航向角φ给出速度的端点条件：

使用三角函数反求欧拉角，所述欧拉角即飞行路径角和航向角，所述路径角和航向角表示为速度分量变量的函数，如公式(4)所示：

步骤2.1.2：速度约束如公式(5)所示：

V(t)＝||V||≤V_max (5)

步骤2.1.3：加速度约束：在无人机轨迹规划问题中，考虑以过载约束表示无人机的机动性能，最大允许过载为a_max，则：

a(t)＝||a||≤a_max (6)

步骤2.1.4：姿态约束：无人机尤其多旋翼无人机飞行过程中，其考虑到载荷的安全性，总升力方向与地面的角度将有一个限制，所述限制通过三个方向过载构成的二阶锥表示：

步骤2.1.5：障碍约束被推广成如公式(8)所示的凹函数：

gⁱ(x,y,z)≤0, i＝1,2,...m+n (8)

椭球或者椭圆柱约束，分别表达如下：

其中，

和

分别表示障碍的球心和椭圆柱中心。

和

分别表示障碍的球心和椭圆柱中心的半径。由解析几何知，

是椭圆形区域的形状；

是椭圆柱区域的形状。对于椭圆柱，

是椭圆柱的中心轴上的单位矢量，

表示n的叉乘矩阵表示为：

如公式(9)-(10)所示的障碍约束是非凸约束。

步骤2.2：选取时间最小作为优化目标建立无人机飞行轨迹规划最优控制问题P0。

时间自由问题的优化目标是最小化飞行时间，表示成以下积分形式的目标函数：

然后导出无人机飞行轨迹规划最优控制问题P0如下：

满足：公式(1),(2)-(8) (13)

最优控制问题P0是非凸的，因为等式(1)中的动力学包含非线性的时间自由因素，并且避障区域的约束进一步加重条件。用一般非线性规划求解器求解所述一个非凸问题是费时的。为此，后续步骤三、四将非凸最优控制问题P0转换为凸优化问题，从而使得如此耗时的问题变得更轻且易于实现。

步骤三：把原非凸最优控制问题P0中的非线性动力学变换为线性动力学，得到固定初始和末端时间轨迹优化问题P1。

原非凸最优控制问题P0是一个时间自由非凸最优控制问题。为高效可靠地解决原非凸最优控制问题P0，将原非凸最优控制问题P0转化为固定时间最优控制问题OCP，并将固定时间最优控制问题OCP凸化为二阶锥规划SOCP问题，其中目标函数是线性的，约束条件是线性的或二阶锥。

时间自由最优控制问题OCP通过增加一个参数转化为时间自由问题。原非凸最优控制问题P0中，初始时间是固定过的，末端时间是自由的。然后将原非凸最优控制问题P0转化为具有固定初始时间和固定结束时间的优化问题P1，实现方法如下：

首先，将时间参数更改为：

设置t₀＝0。根据式(14)，微分得：

公式(1)所示的运动学方程变为：

定义新的速度状态量：

通过新的速度状态量重写运动方程(16)为运动方程(18)：

其中t₁是公式(14)中定义的末端时间，

是公式(17)定义的新速度状态量。

为将方程(18)变成线性动力学，定义：

无人机线性运动方程为：

上述线性系统控制量为

和时间参数t₁,t₂。式(2)表示的初始和末端约束变为：

考虑速度约束，新状态量

的约束描述下为：

考虑加速度约束(6)，新控制量

的约束描述下为：

无人机姿态约束在新控制量

下表示为：

优化目标函数等价于：

根据以上处理，原始原非凸最优控制问题P0转化为固定初始时间和固定结束时间的优化问题P1：

P₁:min J＝t₁ (27)

使得：

原始原非凸最优控制问题P0转化为固定初始时间和固定结束时间的优化问题P1后，除飞行障碍约束外，只有(33)是非凸约束。

在步骤三中，将原始原非凸最优控制问题P0的运动学转换成具有二重积分形式的线性系统。在固定初始时间和固定结束时间的优化问题P1后中，时间间隔是固定的。但是P1问题仍然是非凸的，因为除线性初始约束和末端约束，其余约束是非凸的。在步骤四中，将通过凸化处理将P1转化为凸优化问题。

步骤四：通过凸松弛将P1问题中存在或者引入的非凸约束转化为凸约束，进而把P1问题松弛为近似凸优化问题P2，所述松弛方法确能够保证迭代优化求解过程的收敛性，进而提高无人机实时求解的鲁棒性和稳健性。

固定初始时间和固定结束时间的优化问题P1的约束方程(33)-(34)都是非凸的，首先，处理非凸状态约束方程(33)，将等号“＝”改变为等号“≥”，使其变成二阶旋转锥约束：

将约束方程(33)改变为二阶旋转锥约束(35)实际上扩大可行集的空间。为确保松弛的等价性，必须保证最优解存在于二阶旋转锥约束(35)的边界上。然而，二阶旋转锥约束(35)并不能保证是活跃的，因此，如果仅仅松弛(33)，优化问题P1最优解也并不是符合原问题的最优解，甚至算不上一个可行解。通过正则化方法在得到飞行时间最优解的同时保证二阶旋转锥约束(35)是活跃的，即通过更改目标函数J’，将目标函数取为优化问题P1可行域的一个支撑超平面，最小化飞行时间等价于最小化支撑超平面的截距，目标函数J′表达为：

J′＝t₂-σt₁ (36)

其中σ≥0为待定的正则参数，考虑目标函数(36)和二阶旋转锥约束(35)构成的无障轨迹规划模型如公式(37)至(43)所示：

使得：

中有关松弛约束t₁,t₂的可行集表示为：

中包含t₁和t₂的其余约束均是凸的，可行集合为：

因此，无障碍无人机轨迹规划问题

关于t₁和t₂的可行集为

给定支撑斜率σ，最优化问题

的目标函数(36)给出一个关于t₁和t₂在

上的支撑直线，其中截距为J′。

不考虑障碍约束的路径优化问题为

路径优化问题

通过删除原始原非凸最优控制问题P₀中的障碍约束(34)获到。非凸路径优化问题

的可行解包含于凸优化问题

的可行解中，并且位于

的边界上。寻找非凸路径优化问题

问题的最优解，等价于寻找凸集

在右边界上t₁最小的支撑点。支撑直线在所述支撑点上可能存在多个可行的支撑斜率σ，只需要找一个可行的支撑斜率σ即可。寻找可行支撑斜率σ的策略如下：

对于无障碍约束的三维轨迹规划问题，搜寻可行σ得到最优飞行时间。只需要1-3次迭代求解

问题便可得到

的最优解。具体方法如下：

算法1：无障碍约束飞行时间最优三维轨迹规划算法：

输入量：迭代容差，轨迹规划初始和末端状态，过载约束以及姿态约束；

输出量：最小飞行时间；飞行路径、速度以及过载等过程量。

1)设置k＝0；σ^[0]＝0，求解最优控制问题P1得到

2)判断

是否成立，如果成立则执行步骤4)，否则继续步骤3)；

3)更新参数：

令k＝k+1，求解最优控制问题P1得到

并返回步骤2)；

4)返回求解结果：得到最优飞行总时间

位置r＝r^[k]，速度

过载

以上算法中，最关键的是对支撑函数斜率σ的更新。即

如此将获得一个从凸集

内部逐次逼近的解。后续的数值案例可以看出，这种逼近是很快速的。备注：对于无障碍轨迹规划，只要原问题有解，算法1总是能够收敛的。因为问题

实际上给出一个共轭函数，所述共轭函数描述正则参数和

目标函数最优值之间的关系，当正则参数σ＝[0,∞)时，问题

的解包含于以σ为参数的凸优化问题

中。当正则参数以σ＝0开始按照算法1进行迭代求解时，支撑点将从凸集

内部逐步逼近最优解。

处理障碍约束：为此处理障碍约束需要一个初始路径，但是并不需要一个十分精确的路径，所述路径甚至不需要任何“光滑”特性。定义[x^(k),y^(k),z^(k)]^T是第k次迭代的解。对于一个给定的带有障碍的三维空间，给定一个穿越方式，将给出基于所述穿越方式下的最优飞行时间。此外，在每次迭代过程中，一旦到达凸集

边界，即获得一个满足原始原非凸最优控制问题P0约束的可行解。

通过在[x^(k),y^(k),z^(k)]^T处进行线性化椭圆或椭圆柱面函数处理实现凸化障碍约束方程(8)，然而当初始点[x^(k),y^(k),z^(k)]^T在障碍约束内部时，直接线性化将产生一个消极的局部约束，因此采用投影线性化处理避免所述情况，同时也能进一步提高迭代效率，对于椭圆球和椭圆柱分别进行投影线性化得到线性约束如(44)-(45)所示：

其中：

r^(k)＝[x^(k),y^(k),z^(k)]^T，

和

分投影时的平移量，具体地：

为保证线性化的合理性，对[x,y,z]^T上的置信域如下

|r(τ)-r^(k)(τ)|≤δ_r (46)

其中δ_r是用户定义的信赖域半径。

基于上述公式(44)-(46)处理，在凸优化问题

中加入线性化的障碍约束，得到近似最优凸优化问题P2为：

P₂:min J′＝t₂-σt₁ (47)

使得：

即通过松弛方法将非凸优化问题P1转化为近似凸优化问题P2，保证迭代优化求解过程的收敛性，进而提高无人机实时求解的鲁棒性和稳健性。

步骤五：在归一化区间上用(N+1)个离散点将近似凸优化问题P2离散形成二阶锥规划问题P3。

步骤五：在归一化区间[0,1]上用(N+1)个离散点{τ₀,...,τ_N}，将近似凸优化问题P2离散形成二阶锥规划问题P3。

P₃:min l^[k]Ty (56)

使得：F(y^(k))y＝g(y^(k)) (57)

其中，y∈Rⁿ是所有离散点上状态量{x(t_i)}_i＝0,...,N和控制量{u(t_i)}_i＝0,...,N组成的优化变量，约束系数F∈R^m×n,g∈R^m,

p_i∈Rⁿ依赖于依赖参数y^(k)，特别是第k步下的r^(k)，优化目标中的系数l^[k]依赖于σ^[k]。从近似凸优化问题P2知，二阶锥规划问题P3中的式(57)来自于近似凸优化问题P2的等式约束，二阶锥规划问题P3中的式(58)来自于问题P2的不等式约束，包括线性不等式约束和二阶锥约束。

步骤六：迭代求解步骤五得到的二阶锥规划问题P3，在每次迭代中，给定正则参数和二阶锥规划问题P3中的参考路径，然后求解二阶锥规划问题P3问题，得到一个新的解，用于更新下一次迭代中的参考路径，返回步骤五迭代，直到当前的解与上一步的解一致后更新正则参数，再次返回步骤五迭代，直至所述时间参数凸松弛约束活跃，所得最优解即为优化得到的确保收敛的无人机飞行时间最优实时轨迹。

迭代求解步骤五得到的二阶锥规划问题P3，在每次迭代中，给定正则参数σ和二阶锥规划问题P3中的依赖参数y^(k)，然后求解二阶锥规划问题P3问题，得到一个新的解，用于更新下一次迭代中参数，返回步骤五迭代，直到当前的解与上一步的解一致后更新正则参数，再次返回步骤五迭代，直至所述时间参数凸松弛约束活跃，所得最优解即为优化得到的确保收敛的无人机飞行时间最优实时轨迹。

算法2：障碍约束下飞行时间最优三维轨迹规划算法：

输入量：迭代容差，参考路径、轨迹规划初始和末端状态，过载约束以及姿态约束；

输出量：最小飞行时间；飞行路径、速度以及过载等过程量。

1)设置k＝0，选择初始状态剖面r⁽⁰⁾＝[x⁽⁰⁾y⁽⁰⁾z⁽⁰⁾]^T剖面和初始正则参量σ⁽⁰⁾＝0，可用于构造y⁽⁰⁾和l⁽⁰⁾。

2)在(k+1)步(k≥0)，计算P₃问题中的依赖参数y^(k)，特别地，依赖于r^(k)。然后，求解问题P3获得一个解.

3)检查下列收敛停止条件是否满足：

其中是∈_r∈R³用户定义的用于满足收敛性的足够小容差。如果条件(59)式满足，则转到程序4；否则用y^(k)代替y^(k+1)，设置k＝k+1并且转到程序2.

4)检查下列收敛停止条件是否满足：

其中是∈_t∈R用户定义的用于满足收敛性的足够小容差。如果条件(60)式满足，则转到程序5；否则用

更新正则参数，设置k＝k+1并且转到程序2。

5)序列求解过程收敛，得到

是原问题P0的解，停止。

备注：问题P3中的初始参数r^(k)需要选择，前面已经说明选择不同的初始路径r^(k)将决定不同的越障方式。本发明提供的数值案例中，选择为初始位置到末端位置之间的直线，尽管初始路径选择的较为粗略，实施例可以看到，序列求解过程仍然以较快的速度收敛。迭代求解问题P3的过程中，时间参数

有关的收敛判据(60)能够很容易得到满足，如此以来，后续的迭代过程中得到的解均是一个满足运动学约束的可行解。最优飞行时间的迭代提升通过不断调整障碍约束来实现。

还包括步骤七：利用步骤一至步骤六进行无人机飞行时间最优实时轨迹规划，应用步骤四保证迭代优化求解的收敛性，进而提高实时轨迹规划求解过程的鲁棒性和稳健性，此外，通过对障碍区域的建模，将无障碍时间最优轨迹规划方法拓展到避障时间最优轨迹规划方法，由于优化模型充分利用速度和加速度等约束提高无人机路径飞行性能，进一步减少无人机的避障飞行时间，从而提高无人机的执行任务的反应能力。

有益效果：

1.针对无人机飞行时间最优实时轨迹规划问题，本发明公开的确保收敛的无人机飞行时间最优实时轨迹优化方法，通过构造保证收敛的序列二阶锥规划得到一个具有有限时间复杂度的优化方法，提高实时轨迹规划求解过程的鲁棒性和稳健性，在轨迹规划中同时存在障碍约束和非线性运动学约束下实现飞行时间最优控制。

2.针对无人机飞行时间最优实时轨迹规划问题，本发明公开的确保收敛的无人机飞行时间最优实时轨迹优化方法，通过对障碍区域的建模，将无障碍时间最优轨迹规划方法拓展到避障时间最优轨迹规划方法，由于在优化模型中充分利用加速度控制的约束提高无人机路径飞行性能，进一步减少无人机的避障飞行时间，从而提高无人机的执行任务的敏捷性。

3.针对无人机飞行时间最优实时轨迹规划问题，本发明公开的确保收敛的无人机飞行时间最优实时轨迹优化方法，不需要提供最优时间猜测值，并且迭代优化求解过程可以很快收敛至飞行轨迹的可行解，之后每次优化所得可行解的飞行时间能够得到不断提升并收敛至最优解。由于凸优化算法的快速性，显著提高本发明在线应用的潜力。因此，本发明能够为无人机机载计算机进行飞行时间最优避障轨迹规划提供支撑。

附图说明

图1为确保收敛的无人机飞行时间最优实时轨迹优化方法流程图；

图2为本发明步骤二中无人机运动参量说明图，其中图2(a)为运动几何示意图；图2(b)为飞行姿态约束示意图；

图3为本发明步骤四中非凸约束的凸松弛图；

图4为本发明步骤四中优化算法的时间参数可行域图解；

图5为本发明实施例中无障碍轨迹规划任务下无人机飞行路径与过载矢量图；

图6为本发明实施例中无障碍轨迹规划任务下的状态随时间变化历程，其中图6(a)为路径角变化历程，图6(b)为航向角变化历程；图6(c)为速度变量历程；图6(d)为加速度变量历程；

图7为本发明实施例中有障碍轨迹规划任务下三维路径迭代过程；

图8为本发明施例中有障碍路径规划任务下无人机飞行路径与过载矢量图；

图9为本发明实施例中有障碍轨迹规划任务下的状态随时间变化历程，其中图9(a)为路径角变化历程，图9(b)为航向角变化历程；图9(c)为速度变量历程；图9(d)为加速度变量历程；

图10为本发明实施例中有障碍轨迹规划任务下路径点与障碍表面的最小距离变化历程；

图11为本发明实施例中中有障碍轨迹规划任务下时间参数收敛历程；

图12为本发明实施例中中有障碍轨迹规划任务下状态参数迭代误差历程。

具体实施方式

为更好的说明本发明的目的和优点，下面结合附图和实例对发明内容做进一步说明。

实施例：

如图1所示，本实施例公开的确保收敛的无人机飞行时间最优实时轨迹优化方法，具体实现步骤如下：

步骤1：无人机运动学建模，无人机三维避障的无量纲运动方程表示为：

其中，r＝[x，y，z]^T是无人机的空间位置，z是高度，x，y是水平面正交方向的坐标；V是无人机速度；a＝[a_x a_y a_z]^T是无人机机动过载，g＝[0 0 g]^T是重力加速度常矢量。在式(1)中，距离变量[x，y，z]^T用初始和末端位置的欧式距离L₀来归一化，速度用

归一化。时间用

归一化。

步骤2：建立避障轨迹规划最优控制问题模型：

除符合运动学之外，在一个具体的飞行任务中满足的约束包括:

2.1.初始和末端约束:

其中r₀＝[x₀,y₀,z₀]^T,r_f＝[x_f,y_f,z_f]^T是初始和末端位置；V₀＝[v_x0,v_y0,v_z0]^T和V_f＝[v_xf,v_yf,v_zf]^T是初始和末端位置。此外，考虑到无人机常用速度大小和角度来描述速度矢量，如图2(a)所示，如此，可以用飞行路径角和航向角给出速度的端点条件：

也可以很容易地使用三角函数来反求飞行路径角和航向角。它可以表示为速度分量变量的函数，分别如下

2.2.速度约束：

V(t)＝||V||≤V_max (5)

2.3.加速度约束：在无人机轨迹规划问题中，考虑以过载约束来表示无人机的机动性能，假设最大允许过载为a_max，则：

a(t)＝||a||≤a_max (6)

2.4.姿态约束：无人机尤其多旋翼无人机飞行过程中，其考虑到载荷的安全性，总升力方向与地面的角度将有一个限制，如图2(b)所示，这个限制可以通过三个方向过载构成的二阶锥表示：

2.5.障碍约束：假设它可以被推广成一个凹函数如下：

gⁱ(x,y,z)≤0, i＝1,2,…m+n (8)

例如，椭球或者椭圆柱约束，分别表达如下：

其中，

和

分别表示障碍的球心和椭圆柱中心。

和

表示它们的半径。由解析几何可知，

和

分别定义是椭圆形区域的形状。对于椭圆柱，假设

是椭圆柱的中心轴上的单位矢量，

表示n的叉乘矩阵表示为：

注意到以上是非凸约束。

选取飞行总时间最小作为优化目标，表示成以下积分形式的目标函数：

然后导出最优控制问题如下：

满足：公式(1),(2)-(8) (13)

该问题是非凸的，因为等式(1)中的动力学包含非线性的时间自由因素，并且避障区域的约束进一步加重条件。用一般非线性规划求解器求解这样一个非凸问题是费时的。为此，将展示如何将非凸问题P0转换为凸优化问题，从而使得如此耗时的问题变得更轻且易于实现。

步骤3：把原问题P0中的非线性动力学变换为线性动力学，得到原P0问题的近似优化问题P1：

考虑避障的轨迹规划问题是一个时间自由非凸最优控制问题。为高效可靠地解决这个问题，需要理解并处理它。本节旨在将P0转化为固定时间OCP(最优控制问题)，并将固定OCP凸化为二阶锥规划(SOCP)问题，其中目标函数是线性的，约束条件是线性的或二阶锥。

通常，时间自由最优控制问题可以通过增加一个参数转化为时间自由问题。假设P0中，初始时间是固定过的，末端时间是自由的。然后将问题P0转化为具有固定初始时间和固定结束时间的问题。首先，将时间参数更改为：

不失一般性，设置t₀＝0。根据上式，微分得：

新的运动学方程变为：

定义新的速度状态量：

重写运动方程(16)：

其中t₁是公式(14)中定义的末端时间，

是公式(17)定义的新速度状态量。

为将上述方程变成线性动力学，定义：

无人机线性运动方程为：

上述线性系统控制量为

和时间参数t₁,t₂。式(2)表示的初始和末端约束变为：

考虑速度约束，新状态量

的约束描述下为：

考虑加速度约束(6)，新控制量

的约束描述下为：

无人机姿态约束在新控制量下表示为：

优化目标函数等价于：

根据以上处理，原始最优问题P0转化为：

P₁:min J＝t₁ (27)

使得：

优化问题P0中除飞行障碍约束外，只有(33)是非凸约束。

在步骤3中，将原始问题的运动学转换成具有二重积分形式的线性系统。在新问题P1中，时间间隔是固定的。但是P1仍然是非凸的，因为除线性初始约束和末端约束，其余约束是非凸的。在下一步骤中，将通过一些凸化技术将P1转化为凸优化问题。

步骤4：通过凸松弛将P1问题中存在或者引入的非凸约束转化为凸约束，进而把P1问题松弛为凸优化问题P2：

显然，约束方程(33)-(34)都是非凸的。在这一小节中，将讨论它们的凸化。

首先，关注非凸状态约束方程(33)，通过将等号“＝”改变为等号“≥”，使其变成二阶旋转锥约束：

这是典型的凸约束，这个松弛过程的情况如图3所示。约束的改变实际上扩大可行集的空间。为确保松弛的等价性，必须保证最优解存在于约束方程的边界上。然而，参数约束(35)并不能保证是活跃的，这意味着如果仅仅松弛(33)，其最优解也并不是符合原问题的最优解，甚至算不上一个可行解。因此本发明提出一种正则化方法，在得到飞行时间最优解的同时保证约束(35)是活跃的。主要技术是更改目标函数，将目标函数取为原问题可行域的一个支撑超平面，最小化飞行时间等价于最小化支撑超平面的截距，具体表达为：

J′＝t₂-σt₁ (36)

其中σ≥0为待定的正则参数，考虑目标函数(36)和凸约束(35)构成的无障轨迹规划模型：

使得：

设

中有关松弛约束t₁,t₂的可行集表示为：

中包含t₁和t₂的其余约束均是凸的，他们的交集依然是凸的，设为可行集合：

假设

如图4所示，如此以来无障碍无人机轨迹规划问题

关于t₁和t₂的可行集为

给定σ，最优化问题

的目标函数(36)给出一个关于t₁和t₂在

上的支撑直线，其中截距为J′。

下面处理障碍约束：为此本发明需要一个初始路径，但是并不需要一个十分精确的路径，这个路径甚至不需要任何“光滑”特性。不失一般性，假设[x^(k),y^(k),z^(k)]^T是第k次迭代的解。对于一个给定的带有障碍的三维空间，给定一个穿越方式，本发明的算法将给出基于特定穿越方式下的最优飞行时间。此外，本发明在每次迭代过程中，一旦到达凸集

边界，也就获得一个满足原问题约束的可行解。这一点，对于快速轨迹规划来说，是一个十分有益的特点。

首先，本发明通过在[x^(k),y^(k),z^(k)]^T处进行线性化椭圆或柱面函数来凸化方程(8)，然而当初始点例如[x^(k),y^(k),z^(k)]^T在约束内部时，直接线性化将产生一个消极的局部约束，因此本发明采用投影线性化的方法来避免这种情况，同时也能进一步提高算法的迭代效率，对于椭圆球和椭圆柱分别进行投影线性化得到线性约束如下:

其中：

r^(k)＝[x^(k),y^(k),z^(k)]^T，

和

分投影时的平移量，具体地：

为保证线性化的合理性，对[x,y,z]^T上的置信域如下

|r(τ)-r^(k)(τ)|≤δ_r (46)

其中δ_r是用户定义的信赖域半径。

基于上述处理，在凸优化问题

中加入线性化的障碍约束，得到最优控制问题P2为：

P₂:min J′＝t₂-σt₁ (47)

使得：

步骤5：在[0,1]上用(N+1)个离散点(即{τ₀,...,τ_N})将问题P2离散形成如下二阶锥规划问题的形式：

P₃:min l^[k]Ty (56)

使得：F(y^(k))y＝g(y^(k)) (57)

其中，y∈Rⁿ是所有离散点上状态量{x(t_i)}_i＝0,...,N和控制量{u(t_i)}_i＝0,...,N组成的优化变量，约束系数F∈R^m×n,g∈R^m,

p_i∈Rⁿ依赖于y^(k)，特别是第k步下的r^(k)，优化目标中的系数l^[k]依赖于σ^[k]。从问题P2中可以看出。注意到，问题P3中的式(57)来自于问题P2的等式约束，问题P3中的式(58)来自于问题P2的不等式约束，包括线性不等式约束和二阶锥约束。

步骤6：迭代求解步骤五得到的二阶锥规划问题P3，在每次迭代中，给定正则参数σ和二阶锥规划问题P3中的依赖参数y^[k]，然后求解二阶锥规划问题P3问题，得到一个新的解，用于更新下一次迭代中参数，返回步骤五迭代，直到当前的解与上一步的解一致后更新正则参数，再次返回步骤五迭代，直至所述时间参数凸松弛约束活跃，所得最优解即为优化得到的确保收敛的无人机飞行时间最优实时轨迹。具体如下：

输入量：迭代容差，参考路径、轨迹规划初始和末端状态，过载约束以及姿态约束；

输出量：最小飞行时间；飞行路径、速度以及过载等过程量。

1)设置k＝0，选择初始状态剖面r⁽⁰⁾＝[x⁽⁰⁾ y⁽⁰⁾ z⁽⁰⁾]^T剖面和初始正则参量σ⁽⁰⁾＝0，可用于构造y⁽⁰⁾和l⁽⁰⁾。

2)在(k+1)步(k≥0)，计算P3问题中的依赖参数y^(k)，特别地，依赖于r^(k)。然后，求解问题P3获得一个解.

3)检查下列收敛停止条件是否满足：

其中是∈_r∈R³用户定义的用于满足收敛性的足够小容差。如果条件(59)式满足，则转到程序4；否则用y^(k)代替y^(k+1)，设置k＝k+1并且转到程序2.

4)检查下列收敛停止条件是否满足：

其中是∈_t∈R用户定义的用于满足收敛性的足够小容差。如果条件(60)式满足，则转到程序5；否则用

更新正则参数，设置k＝k+1并且转到程序2.

5)序列求解过程收敛，得到

的就是原问题P0的解，停止。

在以上步骤中，已经看到，求解非线性最优控制问题以获得最小时间运动路径相当于依次求解相应的凸优化问题。在本节中，通过数值例子说明所提出方法的有效性。数值模拟中使用的飞行器模型参数是r₀,r_f,V₀,V_f。最大加速度约束为20m/s²。最大速度约束设置为40m/s。姿态约束设置为45deg，其他初始和末端约束条件如表1所示。P2中的信赖域约束的参数设置为：

δ_r＝10e^-1[x_f-x₀,y_f-y₀,z_f-z₀]^T (61)

收敛停止准则设置为：

ε_r＝1.0e^-4[x_f-x₀,y_f-y₀,z_f-z₀]^T,ε_t＝1.0e^-4 (62)

运行求解软件MOSEK的桌面电脑配置为Intel Core i7-33703.40GHz，迭代求解SOCP问题的离散点的数目为101(n＝100)，在接下来的两个小节中，将首先设置一个无障碍轨迹规划案例，展示算法的快速收敛效果。然后，还将展示避障如何影响控制量加速度，这些加速度用来控制速度大小和方向以避免碰撞的同时尽可能的节省时间消耗。

实施例A：无障碍轨迹规划

这一小节旨在说明通过所提出方法的计算快速性。设计一个无障碍的情况，初始和末端条件如表一所示。本案例中，只需要一次计算凸优化便可以得到问题的解。具体只需要0.1-0.2秒就能解决SOCP问题P3。因为不存在障碍约束，不需要进行路径迭代，只须按照算法1所示步骤进行求解。本案例中，最优解正好位于σ＝0对应的支撑点，其他状态量将显示在图6中。

表一无避障约束条件下三维轨迹规划的初始和末端条件

最优飞行时间为19.57s。从图5-6中可以看出，飞行路径端点和初末角度均满足约束条件。无障碍飞行时间最优路径如图5所示，其中箭头实线为加速度方向矢量。对应的速度和加速度曲线如图6(c)所示。从中可看出，最大速度为40m/s，并且历经加速和减速两个阶段。对应于加速度曲线中的前后两个阶段，更具体的，飞行器在0s-2.57s以大于重力加速度的机动过载进行加速；2.57s-13.64s飞行器以平衡重力加速度的机动过载进行匀速直线运动；在13.64s-19.57s对应的减速阶段，飞行器在15.22s-17s进行无动力的自由抛体运动，主要目的是充分利用重力完成转弯，尽可能的节省飞行时间。

实施例B：考虑障碍的三维轨迹规划

在这一实例中，考虑避障约束的轨迹规划任务。作为比较，在无障碍任务的路径过程中加入两个椭圆柱障碍，障碍物椭圆柱的参数如表二所示。最小飞行时间为20.437秒。图7-10中绘制的数值解显示，对于这个避障路径任务来说，初始、终端和加速度约束都得到满足。图7中绘出各次迭代过程的路径，其中初始路径为连接初末点的直线段，最优避障路径为粗实线。

表二障碍约束椭圆柱参数

表三给出8次迭代的收敛误差，其中关于时间参数的误差以及图11说明：从第二至第八次得到的解关于时间参数的松弛约束是活跃的。由此可见，第二次到最终的迭代解均满足运动学和障碍约束。过载分量与大小变化历程如图9所示，中间“突出”部分处于避障机动飞行的过程，图10表示路径点与障碍表面的最小距离，可以看出，最优路径接触障碍物的边界。

表三避障三维轨迹规划求解收敛步骤

以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.确保收敛的无人机飞行时间最优实时轨迹优化方法，其特征在于：包括如下步骤：

步骤一：考虑重力作用、速度和加速度因素建立无人机运动学模型，并对无人机运动学进行变换并量归一化，建立三维无量纲运动方程；

步骤二：根据无人机避障飞行的具体要求建立速度和控制量的约束条件，给出障碍的三维椭球和椭圆圆柱的描述，选取时间最小作为优化目标建立无人机飞行轨迹规划最优控制问题P0；

步骤三：把原非凸最优控制问题P0中的非线性动力学变换为线性动力学，得到固定初始和末端时间轨迹优化问题P1；

步骤四：通过凸松弛将P1问题中存在或者引入的非凸约束转化为凸约束，进而把P1问题松弛为近似凸优化问题P2，所述松弛方法确能够保证迭代优化求解过程的收敛性，进而提高无人机实时求解的鲁棒性和稳健性；

步骤五：在归一化区间上用(N+1)个离散点将近似凸优化问题P2离散形成二阶锥规划问题P3；

步骤六：迭代求解步骤五得到的二阶锥规划问题P3，在每次迭代中，给定正则参数和二阶锥规划问题P3中的参考路径，然后求解二阶锥规划问题P3问题，得到一个新的解，用于更新下一次迭代中的参考路径，返回步骤五迭代，直到当前的解与上一步的解一致后更新正则参数，再次返回步骤五迭代，直至所述时间参数凸松弛约束活跃，所得最优解即为优化得到的确保收敛的无人机飞行时间最优实时轨迹。

2.如权利要求1所述的确保收敛的无人机飞行时间最优实时轨迹优化方法，其特征在于：还包括步骤七，利用步骤一至步骤六进行无人机飞行时间最优实时轨迹规划，应用步骤四保证迭代优化求解的收敛性，进而提高实时轨迹规划求解过程的鲁棒性和稳健性，此外，通过对障碍区域的建模，将无障碍时间最优轨迹规划方法拓展到避障时间最优轨迹规划方法，由于优化模型充分利用速度和加速度等约束提高无人机路径飞行性能，进一步减少无人机的避障飞行时间，从而提高无人机的执行任务的反应能力。

3.如权利要求1或2所述的确保收敛的无人机飞行时间最优实时轨迹优化方法，其特征在于：步骤一实现方法为，

考虑重力作用、速度和加速度因素建立无人机运动学模型，并对无人机运动学进行变换并量归一化，无人机三维避障的无量纲运动方程表示为：

其中，r＝[x，y，z]^T是无人机的空间位置，z是高度，x，y是水平面正交方向的坐标；V是无人机速度；a＝[a_x a_y a_z]^T是无人机机动过载，g＝[0 0 g]^T是重力加速度常矢量；在式(1)中，距离变量[x，y，z]^T用初始和末端位置的欧式距离L₀来归一化，速度用

归一化；时间用

归一化。

4.如权利要求3所述的确保收敛的无人机飞行时间最优实时轨迹优化方法，其特征在于：步骤二实现方法为，

步骤2.1：根据无人机避障飞行的具体要求建立速度和控制量的约束条件，所述约束条件包括初始和末端约束、速度约束、加速度约束、姿态约束、障碍约束；

步骤2.1.1：初始和末端约束为:

其中r₀＝[x₀,y₀,z₀]^T,r_f＝[x_f,y_f,z_f]^T是初始和末端位置；V₀＝[v_x0,v_y0,v_z0]^T和V_f＝[v_xf,v_yf,v_zf]^T是初始和末端位置；此外，考虑到无人机常用速度大小和角度来描述速度矢量，如此，用飞行路径角Ψ和航向角φ给出速度的端点条件：

使用三角函数反求欧拉角，所述欧拉角即飞行路径角和航向角，所述路径角和航向角表示为速度分量变量的函数，如公式(4)所示：

步骤2.1.2：速度约束如公式(5)所示：

V(t)＝||V||≤V_max (5)

步骤2.1.3：加速度约束：在无人机轨迹规划问题中，考虑以过载约束表示无人机的机动性能，最大允许过载为a_max，则：

a(t)＝||a||≤a_max (6)

步骤2.1.4：姿态约束：无人机尤其多旋翼无人机飞行过程中，其考虑到载荷的安全性，

总升力方向与地面的角度将有一个限制，所述限制通过三个方向过载构成的二阶锥表示：

步骤2.1.5：障碍约束被推广成如公式(8)所示的凹函数：

gⁱ(x,y,z)≤0,i＝1,2,…m+n (8)

椭球或者椭圆柱约束，分别表达如下：

其中，

和

分别表示障碍的球心和椭圆柱中心；

和

分别表示障碍的球心和椭圆柱中心的半径；由解析几何知，

是椭圆形区域的形状；

是椭圆柱区域的形状；对于椭圆柱，

是椭圆柱的中心轴上的单位矢量，

表示n的叉乘矩阵表示为：

如公式(9)-(10)所示的障碍约束是非凸约束；

步骤2.2：选取时间最小作为优化目标建立无人机飞行轨迹规划最优控制问题P0；

时间自由问题的优化目标是最小化飞行时间，表示成以下积分形式的目标函数：

然后导出无人机飞行轨迹规划最优控制问题P0如下：

满足：公式(1),(2)-(8)(13)

最优控制问题P0是非凸的，因为等式(1)中的动力学包含非线性的时间自由因素，并且避障区域的约束进一步加重条件；用一般非线性规划求解器求解所述一个非凸问题是费时的；为此，后续步骤三、四将非凸最优控制问题P0转换为凸优化问题，从而使得如此耗时的问题变得更轻且易于实现。

5.如权利要求4所述的确保收敛的无人机飞行时间最优实时轨迹优化方法，其特征在于：步骤三实现方法为，

原非凸最优控制问题P0是一个时间自由非凸最优控制问题；为高效可靠地解决原非凸最优控制问题P0，将原非凸最优控制问题P0转化为固定时间最优控制问题OCP，并将固定时间最优控制问题OCP凸化为二阶锥规划SOCP问题，其中目标函数是线性的，约束条件是线性的或二阶锥；

时间自由最优控制问题OCP通过增加一个参数转化为时间自由问题；原非凸最优控制问题P0中，初始时间是固定过的，末端时间是自由的；然后将原非凸最优控制问题P0转化为具有固定初始时间和固定结束时间的优化问题P1，实现方法如下：

首先，将时间参数更改为：

设置t₀＝0；根据式(14)，微分得：

公式(1)所示的运动学方程变为：

定义新的速度状态量：

通过新的速度状态量重写运动方程(16)为运动方程(18)：

其中t₁是公式(14)中定义的末端时间，

是公式(17)定义的新速度状态量；

为将方程(18)变成线性动力学，定义：

无人机线性运动方程为：

上述线性系统控制量为

和时间参数t₁,t₂；式(2)表示的初始和末端约束变为：

考虑速度约束，新状态量

的约束描述下为：

考虑加速度约束(6)，新控制量

的约束描述下为：

无人机姿态约束在新控制量

下表示为：

优化目标函数等价于：

根据以上处理，原始原非凸最优控制问题P0转化为固定初始时间和固定结束时间的优化问题P1：

P₁:min J＝t₁ (27)

使得：

原始原非凸最优控制问题P0转化为固定初始时间和固定结束时间的优化问题P1后，除飞行障碍约束外，只有(33)是非凸约束。

6.如权利要求5所述的确保收敛的无人机飞行时间最优实时轨迹优化方法，其特征在于：步骤四实现方法为，

固定初始时间和固定结束时间的优化问题P1的约束方程(33)-(34)都是非凸的，首先，处理非凸状态约束方程(33)，将等号“＝”改变为等号“≥”，使其变成二阶旋转锥约束：

将约束方程(33)改变为二阶旋转锥约束(35)实际上扩大可行集的空间；为确保松弛的等价性，必须保证最优解存在于二阶旋转锥约束(35)的边界上；然而，二阶旋转锥约束(35)并不能保证是活跃的，因此，如果仅仅松弛(33)，优化问题P1最优解也并不是符合原问题的最优解，甚至算不上一个可行解；通过正则化方法在得到飞行时间最优解的同时保证二阶旋转锥约束(35)是活跃的，即通过更改目标函数J’，将目标函数取为优化问题P1可行域的一个支撑超平面，最小化飞行时间等价于最小化支撑超平面的截距，目标函数J′表达为：

J′＝t₂-σt₁ (36)

其中σ≥0为待定的正则参数，考虑目标函数(36)和二阶旋转锥约束(35)构成的无障轨迹规划模型如公式(37)至(43)所示：

使得：

中有关松弛约束t₁,t₂的可行集表示为：

中包含t₁和t₂的其余约束均是凸的，可行集合为：

因此，无障碍无人机轨迹规划问题

关于t₁和t₂的可行集为

给定支撑斜率σ，最优化问题

的目标函数(36)给出一个关于t₁和t₂在

上的支撑直线，其中截距为J′。

不考虑障碍约束的路径优化问题为

路径优化问题

通过删除原始原非凸最优控制问题P₀中的障碍约束(34)获到。非凸路径优化问题

的可行解包含于凸优化问题

的可行解中，并且位于

的边界上。寻找非凸路径优化问题

问题的最优解，等价于寻找凸集

在右边界上t₁最小的支撑点。支撑直线在所述支撑点上可能存在多个可行的支撑斜率σ，只需要找一个可行的支撑斜率σ即可。寻找可行支撑斜率σ的策略如下：

对于无障碍约束的三维轨迹规划问题，搜寻可行λ得到最优飞行时间；只需要1-3次迭代求解

问题便可得到

的最优解；具体方法如下：

算法1：无障碍约束飞行时间最优三维轨迹规划算法：

输入量：迭代容差，轨迹规划初始和末端状态，过载约束以及姿态约束；

输出量：最小飞行时间；飞行路径、速度以及过载等过程量；

1)设置k＝0；σ^[0]＝0，求解最优控制问题P1得到

2)判断

是否成立，如果成立则执行步骤4)，否则继续步骤3)；

3)更新参数：

令k＝k+1，求解最优控制问题P1得到

并返回步骤2)；

4)返回求解结果：得到最优飞行总时间

位置r＝r^[k]，速度

过载

以上算法中，最关键的是对支撑函数斜率σ的更新；即

如此将获得一个从凸集

内部逐次逼近的解；后续的数值案例可以看出，这种逼近是很快速的；备注：对于无障碍轨迹规划，只要原问题有解，算法1总是能够收敛的；因为问题

实际上给出一个共轭函数，所述共轭函数描述正则参数和

目标函数最优值之间的关系，当正则参数σ＝[0,∞)时，问题

的解包含于以σ为参数的凸优化问题

中；当正则参数以σ＝0开始按照算法1进行迭代求解时，支撑点将从凸集

内部逐步逼近最优解；

处理障碍约束：为此处理障碍约束需要一个初始路径，但是并不需要一个十分精确的路径，所述路径甚至不需要任何“光滑”特性；定义[x^(k),y^(k),z^(k)]^T是第k次迭代的解；对于一个给定的带有障碍的三维空间，给定一个穿越方式，将给出基于所述穿越方式下的最优飞行时间；此外，在每次迭代过程中，一旦到达凸集

边界，即获得一个满足原始原非凸最优控制问题P0约束的可行解；

通过在[x^(k),y^(k),z^(k)]^T处进行线性化椭圆或椭圆柱面函数处理实现凸化障碍约束方程(8)，然而当初始点[x^(k),y^(k),z^(k)]^T在障碍约束内部时，直接线性化将产生一个消极的局部约束，因此采用投影线性化处理避免所述情况，同时也能进一步提高迭代效率，对于椭圆球和椭圆柱分别进行投影线性化得到线性约束如(44)-(45)所示：

其中：

r^(k)＝[x^(k),y^(k),z^(k)]^T，

和

分投影时的平移量，具体地：

为保证线性化的合理性，对[x,y,z]^T上的置信域如下

|r(τ)-r^(k)(τ)|≤δ_r (46)

其中δ_r是用户定义的信赖域半径；

基于上述公式(44)-(46)处理，在凸优化问题

中加入线性化的障碍约束，得到近似最优凸优化问题P2为：

P₂:min J′＝t₂-σt₁ (47)

使得：

即通过松弛方法将非凸优化问题P1转化为近似凸优化问题P2，保证迭代优化求解过程的收敛性，进而提高无人机实时求解的鲁棒性和稳健性。

7.如权利要求6所述的确保收敛的无人机飞行时间最优实时轨迹优化方法，其特征在于：步骤五实现方法为，

在归一化区间[0,1]上用(N+1)个离散点{τ₀,…,τ_N}，将近似凸优化问题P2离散形成二阶锥规划问题P3；

P₃:min l^[k]Ty (56)

s.t.F(y^(k))y＝g(y^(k)) (57)

其中，y∈Rⁿ是所有离散点上状态量{x(t_i)}_i＝0,…,N和控制量{u(t_i)}_i＝0,...,N组成的优化变量，约束系数F∈R^m×n,g∈R^m,

p_i∈Rⁿ依赖于依赖参数y^(k)，特别是第k步下的r^(k)，优化目标中的系数l^[k]依赖于σ^[k]；从近似凸优化问题P₂知，二阶锥规划问题P3中的式(57)来自于近似凸优化问题P2的等式约束，二阶锥规划问题P3中的式(58)来自于问题P2的不等式约束，包括线性不等式约束和二阶锥约束。

8.如权利要求7所述的确保收敛的无人机飞行时间最优实时轨迹优化方法，其特征在于：步骤六实现方法为，

迭代求解步骤五得到的二阶锥规划问题P3，在每次迭代中，给定正则参数σ和二阶锥规划问题P3中的依赖参数y^(k)，然后求解二阶锥规划问题P3问题，得到一个新的解，用于更新下一次迭代中参数，返回步骤五迭代，直到当前的解与上一步的解一致后更新正则参数，再次返回步骤五迭代，直至所述时间参数凸松弛约束活跃，所得最优解即为优化得到的确保收敛的无人机飞行时间最优实时轨迹；

算法2：障碍约束下飞行时间最优三维轨迹规划算法：

输入量：迭代容差，参考路径、轨迹规划初始和末端状态，过载约束以及姿态约束；

输出量：最小飞行时间；飞行路径、速度以及过载等过程量；

1)设置k＝0，选择初始状态剖面r⁽⁰⁾＝[x⁽⁰⁾y⁽⁰⁾z⁽⁰⁾]^T剖面和初始正则参量σ⁽⁰⁾＝0，可用于构造y⁽⁰⁾和l⁽⁰⁾；

2)在(k+1)步(k≥0)，计算P₃问题中的依赖参数y^(k),特别地，依赖于r^(k)；然后，求解问题P3获得一个解.

3)检查下列收敛停止条件是否满足：

其中是∈_r∈R³用户定义的用于满足收敛性的足够小容差；如果条件(59)式满足，则转到程序4；否则用y^(k)代替y^(k+1)，设置k＝k+1并且转到程序2.

4)检查下列收敛停止条件是否满足：

其中是∈_t∈R用户定义的用于满足收敛性的足够小容差；如果条件(60)式满足，则转到程序5；

否则用

更新正则参数，设置k＝k+1并且转到程序2；

5)序列求解过程收敛，得到

是原问题P0的解，停止。

Images