本发明公开的时间最优快速三维避障路径规划方法，尤其涉及无人机避障路径规划方法，属于无人机路径规划领域。本发明公开的时间最优快速三维避障路径规划方法，通过考虑最大加速度以及障碍约束条件，建立一个以各方向加速度为控制量包含飞行时间和的无人最优控制模型；然后将原非凸非线性的优化问题松弛为一个二阶锥规划问题；最后通过迭代求解一系列二阶锥规划问题得到原问题的解并得到速度方向的最优变化策略，即通过协调飞行时间和飞行速度方向以实现时间最优的避障路径规划，能够实现避障路径和相关控制量的在线规划，且能够通过所优化的时间更优的飞行轨迹进一步提升无人机的任务反应能力。
时间最优快速三维避障路径规划方法
技术领域
[0001] 本发明属于无人机路径规划领域，尤其涉及无人机避障路径规划方法，尤其涉及一种基于在线凸优化的时间最优快速三维避障路径规划方法。
背景技术
[0002] 在过去几年中，无人机技术渗透到生产生活的各个方面，而路径规划在无人机遂行监测、有效载荷输送、农业植物保护、目标搜索等任务中有关键作用。
[0003] 时间最优的路径规划问题是个典型的优化问题，为了提高无人机执行任务的灵活性和快速性，需要实时地解决相应的最优控制模型，以获得耗时最小的飞行路径。盲目通过暴力的方法(例如非线性规划算法)求解这个具有非凸特性的问题往往并不可行，因为这类方法的收敛性和求解效率得不到保证。
发明内容
[0004] 本发明公开的时间最优快速三维避障路径规划方法要解决的技术问题是：提供一种基于凸优化的无人机避障时间最优路径规划方法，通过协调飞行时间和飞行速度方向以实现时间最优的避障路径规划，能够实现避障路径和相关控制量的在线规划，且能够通过所优化的时间更优的飞行轨迹进一步提升无人机的任务反应能力。
[0005] 针对避障路径规划问题，本发明的贡献在于通过改造原非凸问题将其转化为一个二阶锥规划(SOCP)问题，其中目标函数是线性的，其他所有的约束是线性或二阶锥约束。需要注意的是，本发明对涉及障碍约束和时间自由的高度非线性约束和运动方程进行处理。此外，通过数值计算验证所采用的松弛技术的有效性。所述前期工作对采用凸优化算法(多项式时间复杂度)求解原本难以求解的问题具有重要意义。因此，本发明能够实现最优时间避障路径的在线规划。
[0006] 本发明的目的是通过下述技术方案实现的。
[0007] 本发明公开的时间最优快速三维避障路径规划方法，通过考虑最大加速度以及障碍约束条件，建立一个以各方向加速度为控制量包含飞行时间和的无人最优控制模型；然后将原非凸非线性的优化问题松弛为一个二阶锥规划问题；最后通过迭代求解一系列二阶锥规划问题得到原问题的解并得到速度方向的最优变化策略，即通过协调飞行时间和飞行速度方向以实现时间最优的避障路径规划，能够实现避障路径和相关控制量的在线规划，且能够通过所优化的时间更优的飞行轨迹进一步提升无人机的任务反应能力。
[0008] 本发明公开的时间最优快速三维避障路径规划方法，包括如下步骤：
[0009] 步骤一：对无人机进行运动学建模并量纲归一化，建立三维无量纲运动方程；
[0010] 步骤一具体实现方法为，对无人机进行运动学建模，并量纲归一化，无人机三维避障的无量纲运动方程表示为：
[0011]
[0012] 其中，[x，y，z]T是无人机的空间位置，z是高度，x，y是水平面正交方向的坐标；Vc是无人机速度，为已知量；ψ和φ分别为飞行路径角和航向角。在式(1)中，除了ψ和φ以外，距离变量[x，y，z]T用初始和末端位置的欧式距离L0来归一化，速度用Vc归一化。时间和比冲均用L0/Vc归一化。
[0013] 步骤二：根据无人机避障飞行的具体要求建立速度和控制量的约束条件，给出障碍的三维球和圆柱描述，选取时间最小作为优化目标，建立无人机避障路径时间最优控制问题P0；
[0014] 步骤二具体实现方法为：
[0015] 对于式(1)中的控制量为飞行路径角ψ和和航向角φ。在无人机路径规划问题中，考虑以加速度约束来表示无人机的机动性能。各个方向的加速度分量表示为：
[0016]
[0017] 对于式(56)所述的运动学模型，总加速度表示为：
[0018]
[0019] 由于速度为已知，所以绝对加速度在速度的法平面上。除了符合运动学模型之外，在具体的飞行任务中满足的约束还包括:
[0020] 初始和末端约束:
[0021]
[0022] 其中χ0＝[x0,y0,z0]T,χf＝[xf,yf,zf]T是初始和末端位置。
[0023] 加速度约束：定义最大允许加速度为，则：
[0024]
[0025] 障碍约束：障碍约束被推广成凹函数如下：
[0026]
[0027] 当障碍约束简化为圆柱形障碍约束时，障碍约束如公式(7)所示：
[0028]
[0029] 当障碍约束简化为球形障碍约束时，障碍约束如公式(8)所示：
[0030]
[0031] 其中， 和 分别表示障碍的球心和圆柱中心。 和 分别表示球和圆柱的半径。 和 是椭圆形区域的预定义半轴/半轴。圆柱形障碍约束和球形障碍约束是非凸约束。
[0032] 时间自由问题的优化目标是最小化飞行时间，因此，时间自由优化问题有以下积分形式的目标函数：
[0033]
[0034] 然后导出无人机避障路径时间最优控制问题如下：
[0035]
[0036] s.t.Eqs.(1),(4)-(5),(6) (11)
[0037] 无人机避障路径时间最优控制问题是非凸的，因为等式(56)中的动力学包含强非线性因子的三角函数，并且避障区域的约束进一步加重非线性因素。用一般非线性规划求解器求解所述非凸问题是费时的。为此，将非凸问题P0转换为凸优化问题，从而使得如此耗时的非凸问题变得更轻且易于实现。
[0038] 步骤三：把原无人机避障路径时间最优控制问题P0中的非线性动力学变换为线性动力学，将原P0问题转化为问题P1。
[0039] 时间自由最优控制问题能够通过增加一个参数转化为固定时间的最优控制问题。在原P0问题中，初始时间是固定的，末端时间是自由的。将问题P0转化为具有固定初始时间和固定结束时间的最优控制问题。
[0040] 首先将原P0问题中非线性动力学变换为线性动力学模型。时间参数更改为：
[0041]
[0042] 设置t0＝0。根据上式，微分得：
[0043]
[0044] 通过公式(12)、(13)，将公式(1)所示的运动学模型变换为：
[0045]
[0046] 约束方程(59)表示的初始和末端约束变为：
[0047]
[0048] 对于非线性运动学模型(14)和非线性加速度约束(5)，是欧拉角和角速度的非线性函数。因为凸优化要求所有等式约束都是线性的，所有不等式约束都是凸的。因此，需将非线性运动学模型(14)转换成线性运动学模型。当使用速度向量v＝[vx,vy,vz]T，而不使用非线性运动学模型(14)和非线性加速度约束(5)中出现的欧拉角，因此，具有如下优先：①能够防止奇异性；②由于采用向量表示法，能够将公式(14)、(15)所述的模型转化为线性运动学模型：
[0049] vx:＝tfVccosψcosφ；vy:＝tfVccosψsinφvz:＝tfVcsinψ (16)[0050] 上式新的变量v必须满足:
[0051]
[0052] 然后，给出关于新输入的加速度约束。其中一个加速度分量是时间τ的函数，因袭，加速度分量能够替换为如下形式:
[0053]
[0054] 上式中出现的 考虑模型(69)，得到：
[0055]
[0056] 所以加速度的分量成为以τ作为新自变量的函数：
[0057]
[0058]
[0059] 将上述方程式(21)、(22)代入方程(57)。法向加速度相对于τ将表示如下:
[0060]
[0061] 将加速度约束(5)替换为关于新自变量τ的函数，所述函数能够线性化成圆锥凸约束。
[0062] 定义：
[0063] v′x:＝ux；v′y:＝uy；v′z:＝uz (23)
[0064] 将式(78)代入式(77)，法向加速度表示为：
[0065]
[0066] 加速度约束(60)变换为如下表达：
[0067]
[0068] 根据新的定义(78)，运动学方程(69)重新表述为以下双积分形式：
[0069]
[0070] ()’表示相对于τ的微分，方程(26)简写为：
[0071] X′＝AX+Bu (27)
[0072] 其中X:＝[x,y,z,vx,vy,vz]T，
[0073]
[0074] 至此非线性运动学模型(56)已被转换成具有新状态的固定区间线性运动学模型。
[0075] 对于线性模型(82)、初始和末端约束为：
[0076]
[0077] 以上是一系列等式线性约束，其中V:＝[Vx,Vy,Vz]T是速度矢量。根据运动学模型(56)知：
[0078]
[0079] 优化目标函数等价于：
[0080]
[0081] 至此，原无人机避障路径时间最优控制问题P0转化为问题P1：
[0082] P1:min J＝tf (31)
[0083] s.t. X′＝AX+Bu,τ∈[0,1] (32)
[0084]
[0085]
[0086]
[0087]
[0088] 使用三角函数反求欧拉角，所述欧拉角即指飞行路径角和航向角，能够表示为速度分量变量的函数，分别如下：
[0089]
[0090] 上述方程的非奇异条件是vx≠0,vz≤tfVc，即使vx＝0，仍然能够通过速度矢量v定义合适的欧拉角。此外，当使用欧拉角直接表示加速度约束时，复杂的表达式不利于优化问题的求解。
[0091] 在步骤三中，将原无人机避障路径时间最优控制问题P0的运动学模型转换成具有二重积分形式的线性模型。在新问题P1中，时间间隔是固定的。但是新问题P1仍然是非凸的，因为除了线性初始约束和末端约束，约束方程(34)、(35)和(36)是非凸的。在步骤四中，通过凸化处理将P1转化为凸优化问题。
[0092] 步骤四：通过凸松弛将问题P1中存在或者引入的非凸约束转化为凸约束，进而把P1问题松弛为凸优化问题P2；
[0093] 由于步骤三中约束方程(89)-(91)都是非凸的。定义[x(k),y(k),z(k)]T是第k次迭代的解。
[0094] 通过在[x(k),y(k),z(k)]T处线性化椭圆或柱面函数来凸化方程(61):
[0095]
[0096] 其中χ(k) ＝[x(k) ,y (k) ,z(k)] T , 和为保证线性化的合理性，在第k次迭代解[x(k),y(k),z(k)]T上加入置信
域约束如下：
[0097] |χ(τ)-χ(k)(τ)|≤δχ (39)
[0098] 其中δχ是用户定义的信赖域半径。
[0099] 对于非凸状态约束方程(72)，通过将等号“＝”改变为“≤”，变成二阶圆锥约束：
[0100]
[0101] 二阶圆锥约束是典型的凸约束，非凸状态约束方程(72)改变为约束方程(40)能够扩大可行集的空间。为了确保松弛的等价性，必须保证最优解存在于约束方程(40)的边界上。约束方程(40)最优解总是位于圆锥体的曲面上。因此松弛方法是有效的。
[0102] 对于非凸控制约束(80)，通过在 处线性化右项 表示为如公式(41)所示的二阶圆锥约束：
[0103]
[0104] 关于上式的置信域约束表示为：
[0105]
[0106] 其中 是关于时间参数tf的用户定义的信赖域半径。如果在迭代过程中 非常接近最优值 非凸约束方程(80)和凸约束(96)之间几乎没有区别。
[0107] 至此，最优控制问题P1能够凸化为问题P2：
[0108] P2:min J＝tf (43)
[0109] s.t. X′＝AX+Bu,τ∈[0,1] (44)
[0110]
[0111] |χ(τ)-χ(k)(τ)|≤δχ (46)
[0112]
[0113]
[0114]
[0115]
[0116] 步骤五：在[t0,tf]上用(N+1)个离散点将问题P2离散形成二阶锥规划问题P3；所述(N+1)个离散点即{t0,…,tN}。
[0117] 在[t0,tf]上用(N+1)个离散点将问题P2离散形成如下二阶锥规划问题(SOCP)的形式：
[0118] P3:min lTy (51)
[0119] s.t. F(y(k))y＝g(y(k)) (52)
[0120]
[0121] 其中，y∈Rn是所有离散点上状态量{x(ti)}i＝0,...,N和控制量{u(ti)}i＝0,...,N组成的优化变量，约束系数F∈Rm×n,g∈Rm, pi∈Rn依赖于y(k)，特别依赖于第k步下的x(k)和 问题P3中的式(107)来自于问题P2的等式约束，问题P3中的式(108)来自于问题P2的不等式约束，包括线性不等式约束(100)-(101)和二阶锥约束(103)-(104)。
[0122] 步骤六：迭代求解步骤五得到的二阶锥规划问题P3，在每次迭代中，首先计算P3中的依赖参数y(k)，然后再次求解P3问题，得到一个新的解，用于更新下一次迭代中的参数。重复这个过程，直到当前的解与上一步的解一致，即实现通过协调飞行时间和飞行速度方向以实现时间最优的避障路径规划，通过所优化的最优的避障路径飞行轨迹提升无人机执行任务的反应能力。
[0123] 步骤6.1：设置k＝0，选择初始状态剖面χ(0)＝[x(0) y(0) z(0)]T剖面和初始时间参量 用于构造y(0)。
[0124] 步骤6.2：在k+1步(k≥0)，计算问题P3中的依赖参数y(k)，求解问题P3获得一个解记为
[0125] 检查如公式(54)、(55)所述的收敛停止条件是否满足：
[0126]
[0127]
[0128] 其中是∈χ∈R3, 用户定义的用于满足收敛性的足够小容差。如果条件(54)-(55)式满足，则转到步骤6.4；然后，用y(k)代替y(k+1)，设置k＝k+1并且转到步骤6.2。
[0129] 步骤6.4：直到当前的解与上一步的解一致，序列求解过程收敛，得到即是原问题P0的解，停止迭代，即实现通过协调飞行时间和飞行速度方向以实现时间最优的避障路径规划，通过所优化的最优的避障路径飞行轨迹提升无人机执行任务的反应能力。
[0130] 有益效果：
[0131] 1.本发明公开的时间最优快速三维避障路径规划方法，利用序列二阶锥规划得到一个具有有限时间复杂度的优化计算方法，能够通过协调飞行时间和飞行速度方向以实现时间最优的路径规划。
[0132] 2.本发明公开的时间最优快速三维避障路径规划方法，由于在优化模型中以前时间最小作为优化性能指标，能够减少无人机的避障飞行时间，从而提升无人机执行避障飞行任务的反应能力。
[0133] 3.本发明公开的时间最优快速三维避障路径规划方法，具有计算量小，计算快速的特点，能够用于实现无人机机载计算机进行实时避障路径规划。
附图说明
[0134] 图1是本发明的一种最优快速三维避障路径规划方法算法流程图；
[0135] 图2是步骤一的三维避障路径规划运动几何图；
[0136] 图3(a)是本步骤三的非凸速度状态量约束的松弛示例图；
[0137] 图3(b)是本步骤三的非凸控制量约束的松弛示例图；
[0138] 图4是本发明实施例的Dubins曲线路径图；
[0139] 图5是实施例A的Dubins任务下路径和几何关系的数值解；
[0140] 图6是实施例A的Dubins任务下3次迭代中状态的连续解；
[0141] 图7是实施例A的Dubins任务中最优时间前3次迭代中的序列解；
[0142] 图8是实施例A的Dubins任务下的速度分量和速度大小变化历程图；
[0143] 图9是实施例A的Dubins任务下的航向角和航向角速度变化历程图；
[0144] 图10是实施例A的Dubins任务下加速度变化历程图；
[0145] 图11是实施例B的三维避障最小飞行时间路径规划的数值计算结果图；
[0146] 图12是实施例B的三维避障无人机(质点)与障碍物表面之间的距离变化历程图；
[0147] 图13是实施例B的三维避障最小飞行时间的最优速度分量剖面图；
[0148] 图14是实施例B的三维避障最小飞行时间最优速度大小变化历程图；
[0149] 图15是实施例B的三维避障最小飞行时间路径角度和路径角速度历程图；
[0150] 图16是实施例B的三维避障最小飞行时间航向角度和路径角速度历程图；
[0151] 图17是实施例B的三维避障最小飞行时间飞行过载历程图。
具体实施方式
[0152] 为了更好的说明本发明的目的和优点，下面结合附图和实例对发明内容做进一步说明。
[0153] 实施例1
[0154] 本实施例公开的一种考虑障碍约束的时间最优路径规划的方法，具体步骤如下：
[0155] 步骤一：无人机运动学建模。基于图1所示，无人机三维避障的无量纲运动方程表示为：
[0156]
[0157] 其中，[x，y，z]T是无人机的空间位置，z是高度，x，y是水平面正交方向的坐标；Vc是无人机速度，假定为已知量；ψ和φ分别为飞行路径角和航向角。在式(1)中，除了ψ和φ以外，距离变量[x，y，z]T用初始和末端位置的欧式距离L0来归一化，速度用Vc归一化。时间和比冲均用L0/Vc归一化。
[0158] 步骤2：建立避障路径规划最优控制问题模型：
[0159] 式(1)中的控制量为飞行路径角ψ和和航向角φ。在无人机路径规划问题中，本发明考虑以加速度约束来表示无人机的机动性能。各个方向的加速度表示为：
[0160]
[0161] 总加速度为：
[0162]
[0163] 除了符合运动学之外，在一个具体的飞行任务中满足的约束还包括:
[0164] 1.初始和末端约束:
[0165]
[0166] 其中χ0＝[x0,y0,z0]T,χf＝[xf,yf,zf]T是初始和末端位置。
[0167] 2.加速度约束：最大允许加速度为amax，则：
[0168]
[0169] 3.障碍约束：障碍约束可以被推广成一个凹函数如下：
[0170]
[0171] 具体表示为椭圆柱或者椭球约束，表达如下：
[0172]
[0173]
[0174] 其中， 和 分别表示障碍的球心和圆柱中心。 和 表示椭球和椭圆柱的半径。 和 是椭圆形区域的预定义半轴/半轴。以上是非凸约束。
[0175] 时间自由问题的优化目标是最小化飞行时间。因此有以下积分形式的目标函数：
[0176]
[0177] 然后导出最优控制问题如下：
[0178]
[0179] s.t.Eqs.(1),(4)-(5),(6) (66)
[0180] 所述问题是非凸的，因为等式(56)中的动力学包含强非线性因子的三角函数，并且避障区域的约束进一步加重了条件。用一般非线性规划求解器求解这样一个非凸问题是费时的。为此，将展示如何将非凸问题P0转换为凸优化问题，从而使得如此耗时的问题变得更轻且易于实现。
[0181] 步骤3：把原问题P0中的非线性动力学变换为线性动力学，得到原P0问题的近似优化问题P1：
[0182] 首先，本实施例将时间参数更改为：
[0183]
[0184] 设置t0＝0。根据上式，微分得：
[0185]
[0186] 运动学方程变为：
[0187]
[0188] 方程(59)表示的初始和末端约束变为：
[0189]
[0190] 定义新的速度变量为：
[0191] vx:＝tfVccosψcosφ；vy:＝tfVccosψsinφvz:＝tfVcsinψ (71)[0192] 上式意味着新的变量v必须满足:
[0193]
[0194] 然后，给出关于新输入的加速度约束。其中一个加速度分量(相对于τ)替换为如下形式:
[0195]
[0196] 上式中出现的 考虑方程(69)得到：
[0197]
[0198] 加速度v的分量成为相对于τ作为新自变量的函数，例如：
[0199]
[0200] 同样得到另外两个分量的表达式：
[0201]
[0202] 将上述方程式代入方程(57)。法向加速度相对于τ将表示如下:
[0203]
[0204] 上述方程的非奇异条件是vy≠0,vx≤Vc，注意即使vy＝0，仍然可以通过速度矢量v定义合适的欧拉角。此外，当使用欧拉角直接表示加速度约束时，复杂的表达式不利于优化问题的求解。接下来，将给出一个关于加速度约束的新表达式，该表达式能够用于线性化成圆锥约束。
[0205] 定义：
[0206] v′x:＝ux；v′y:＝uy；v′z:＝uz (78)
[0207] 将式(78)代入式(77)，法向加速度表示为：
[0208]
[0209] 加速度约束(60)取代为如下表达：
[0210]
[0211] 根据新的定义(78)，运动学方程(69)重新表述为以下双积分形式，其中算符()’表示相对于τ的微分：
[0212]
[0213] 以上方程简写为：
[0214] X′＝AX+Bu (82)
[0215] 其中X:＝[x,y,z,vx,vy,vz]T，
[0216]
[0217] 至此非线性运动学方程(56)已被转换成具有新状态的固定区间线性系统。对于线性系统(82)、初始和末端约束为：
[0218]
[0219] 以上是等式线性约束，其中V:＝[Vx,Vy,Vz]T是速度矢量。根据方程(56)可知：
[0220]
[0221] 优化目标函数等价于：
[0222]
[0223] 根据以上处理，原始最优问题P0转化为：
[0224] P1:min J＝tf (86)
[0225] s.t. X′＝AX+Bu,τ∈[0,1] (87)
[0226]
[0227]
[0228]
[0229]
[0230] 此外，使用三角函数来反求欧拉角(飞行路径角和航向角)。它表示为速度分量变量的函数，分别如下
[0231]
[0232] 在步骤3中，将原始问题的运动学转换成具有二重积分形式的线性系统。在新问题P1中，时间间隔是固定的。但是P1仍然是非凸的，因为除了线性初始约束和末端约束，约束是非凸的。下步骤中，将通过凸化技术将P1转化为凸优化问题。
[0233] 步骤4：通过凸松弛将P1问题中存在或者引入的非凸约束转化为凸约束，进而把P1问题松弛为凸优化问题P2：
[0234] 显然，约束方程(89)-(91)都是非凸的。下面将讨论它们的凸化方法。首先给出[x(k),y(k),z(k)]T为第k次迭代的解。通过在[x(k),y(k),z(k)]T处线性化椭圆或柱面函数来凸化方程(61):
[0235]
[0236] 其中χ(k) ＝[x(k) ,y (k) ,z(k)] T , 和为了保证线性化的合理性，对[x,y,z]T上的置信域如下
[0237] |χ(τ)-χ(k)(τ)|≤δχ (94)
[0238] 其中δχ是用户定义的信赖域半径。
[0239] 其次，再关注非凸状态约束方程(72)，通过简单地将等号“＝”改变为等号“≤”，变成二阶圆锥约束：
[0240]
[0241] 以上不等式是典型的凸约束，松弛过程的二维情况如图3(a)所示。约束的改变实际上扩大了可行集的空间。为了确保松弛的等价性，必须保证最优解存在于方程的边界上。而约束(95)在随后的数值案例中被观察到是活跃的，这说明最优解总是位于圆锥体的曲面上。因此松弛方法是有效的。
[0242] 对于非凸控制约束(80)的凸松弛，其二维的情况如图3(b)所示，通过在 处线性化右项 表示为如下的二阶圆锥约束：
[0243]
[0244] 关于上式的置信域约束表示为：
[0245]
[0246] 其中 是关于时间的用户定义的信赖域半径。如果在迭代过程中 非常接近最优值 那么非凸约束方程(80)和凸约束(96)之间几乎没有区别。
[0247] 基于上述处理，最优控制问题P1凸化为：
[0248] P2:min J＝tf (98)
[0249] s.t. X′＝AX+Bu,τ∈[0,1] (99)
[0250]
[0251] |χ(τ)-χ(k)(τ)|≤δχ (101)
[0252]
[0253]
[0254]
[0255]
[0256] 步骤5：在[t0,tf]上用(N+1)个离散点(即{t0,…,tN})将问题P2离散形成如下二阶锥规划问题的形式：
[0257] P3:min lTy (106)
[0258] s.t. F(y(k))y＝g(y(k)) (107)
[0259]
[0260] 其中，y∈Rn是所有离散点上状态量{x(ti)}i＝0,…,N和控制量{u(ti)}i＝0,…,N组成的优化变量，约束系数F∈Rm×n,g∈Rm, pi∈Rn依赖于y(k)，特别是第k步下的x(k)和 问题P3中的式(107)来自于问题P2的等式约束，问题P3中的式(108)来自于问题P2的不等式约束，包括线性不等式约束(100)-(101)和二阶锥约束(103)-(104)。
[0261] 步骤6：迭代求解步骤五得到的二阶锥规划问题P3，在每次迭代中，首先计算P3中的依赖参数y(k)，然后再次求解P3问题，得到一个新的解，用于更新下一次迭代中的参数。重复这个过程，直到当前的解与上一步的解一致，具体过程如下：
[0262] 步骤6.1：设置k＝0，选择初始状态剖面χ(0)＝[x(0) y(0) z(0)]T剖面和初始时间参量 可用于构造y(0)。
[0263] 步骤6.2：在(k+1)步(k≥0)，计算P3问题中的依赖参数y(k)【特别地，依赖于χ(k)和】。然后，求解问题P3获得一个解记为
[0264] 步骤6.3：检查下列收敛停止条件是否满足：
[0265]
[0266]
[0267] 其中是∈χ∈R3 and 用户定义的用于满足收敛性的足够小容差。如果条件(54)-(55)式满足，则转到程序4；然后，用y(k)代替y(k+1)，设置k＝k+1并且转到程序2.[0268] 步骤6.4：序列求解过程收敛，得到 的就是原问题P0的解，停止。
[0269] 问题P3中的初始参数χ(k)和 需要选择，选择不同的初始χ(k)将决定不同的越障方式。本发明提供的数值案例中，每个值简单地选择为以时间为自变量的从初值值到期望(或者估计)终端值之间的线性函数， 设置为初末欧式距离与速度的商。尽管选择的较为粗略，实施例可以看到，序列求解过程任然以很快的速度收敛。
[0270] 通过步骤六求解非线性最优控制问题以获得最小时间运动路径相当于依次求解相应的凸优化问题。通过数值例子说明所提出方法的有效性和最优特性。数值模拟中使用的飞行器模型参数是x,y,z,ψ0,ψf,φ0,φf和amax。速度为10m/s。在P2中，信赖域约束的参数设置为：
[0271]
[0272] 收敛停止准则设置为：
[0273]
[0274] 运行求解软件MOSEK的桌面电脑配置为Intel Core i7-3370 3.40GHz，迭代求解SOCP问题的离散点的数目为101(n＝100)，在接下来的两实施例中，将首先设置一个Dubins实施例，即二维路径规划问题的实施例A，说明本实施例的收敛性以及基于SOCP方法的时间最优路径的效果。然后在实施例B中展示三维带障碍约束的情况下，本发明如何快速计算得到飞行时间最优的避障路径以及相应的控制加速度，所述输入和加速度可以用来控制速度以最短路径和轨规定的运动约束到达目标并避免碰撞。
[0275] 实施例A无障碍路径规划
[0276] 针对特殊的二维情况，即z＝0的Dubins问题，初始和结束条件在表1中有详细说明。为了方便起见，最大转弯半径设置为120m(最大曲率为1/120)，amax＝Vc*1/120＝0.83m/2
s。收敛解在3次迭代中获得，每次迭代只需要0.01-0.02秒就能解决SOCP问题P3。因为值z和ψ总是零(与数值解一致)，所以在二维问题下不会显示在图中。
[0277] 表一无避障约束条件下二维路径规划的初始和末端条件
[0278]
[0279] Dubins任务下的的计算收敛过程如图6和图7。能够看出，在第二次迭代后，路径和飞行时间的连续解在图中的尺度中几乎看不到区别，表2中列出的每次迭代之间的误差也能够证实这个结果。最重要的是，凸约束(48)总是有效的，能够保证变换后的非凸约束(34)总能得到满足。然后通过反三角关系来获得飞行路径角、航向角和它们的速率(参见等式37)。本发明所提出方法的快速收敛主要是因为在凸化过程中保留了原始非线性运动学中的一些非线性。
[0280] 表二 Dubins曲线求解收敛步骤
[0281]
[0282] 图5-10中绘制的二维解清楚地表明，在这种情况下，路径可以到达目标，并且满足了对终端航向角的要求。加速度约束也得到满足，如图10所示。在这个例子中可以预见的是，最优加速度大小满足“Bang-Bang”控制。表明时间最优的飞行路径能够满足图4所示的Dubins几何结构，最优飞行时间为59.356s，与图5所示的数值优化结果完全相同。以此，能够得出，本发明作为一种数值方法，在特殊情况即Dubins案例下，与几何最优解是一致的。进而验证了本发明的第一个有益效果：具有时间最优性。应该指出的是，三维Dubins曲线难以给出解析或几何解，求解一般的优化问题以获得三维Dubins曲线并不像求解本发明中的凸优化问题那么简单，当路径规划问题中存在回避区域约束时，即考虑障碍时的三维最优路径规划采用本发明的方法将更加方便。
[0283] 实施例B考虑障碍的三维路径规划
[0284] 在本案例中，设置一个具有两个避障约束的路径规划任务。作为比较，本案例也给2
出同样端点条件和过载约束下没有避障约束下的解。最大加速度设定为0.8m/s ，初始和终止条件见表3。障碍物球的半径为80m，以[250、220、280]m为中心，障碍物圆柱体的半径为
60m，其中心线在[100、150、0]m处穿过x-y平面有和没有避障限制，最小飞行时间分别为
71.41秒和70.34秒。图11-17中绘制的收敛解显示，初始、终端和加速度约束都得到满足。图
11中绘出了具有和不具有避障约束的最佳路径。在图12中，顶部表示球体表面和路径上的点之间的距离，底部表示圆柱体表面和路径上的点之间的距离。可以看出，回避区约束都得到满足，最优路径接触障碍物的边界。
[0285] 表三 避障约束条件下三维路径规划的初始和末端条件
[0286]
[0287] 表4所示的收敛过程表示前3次迭代中的快速下降趋势。图13所示的速度分量可以反求飞行路径角和航向角，这在图15-16中可以清楚地看到，最优速度的大小与图14所示的Vc一致。意味着凸约束(40)在具有避障约束的任务中总是有效的。所有这些都为本发明提出的方法在三维避障规划方面的有效性提供了有力的支持。更重要的是，即使存在避障约束，它对于非凸约束的凸化也是有效的。图17是考虑障碍的三维路径规划的加速度历程，从中可以看出，最短飞行时间要求下，飞行过载总是“Bang-Bang”的形式。最后，不难解释出现在加速度曲线中间的“尖峰”，即最优路径有一段经过障碍物的表面。说明对于影响最优飞行路径的障碍，最优路径将尽可能的通过接触障碍区域的边界来节省飞行时间。因此，考虑避障的三维路径规划案例表明本发明能够能够减少无人机的避障飞行时间，从而提升无人机执行避障飞行任务的反应能力。
[0288] 表四 避障三维路径规划求解收敛步骤
[0289]
[0290] 综合案例A和案例B来看，每次凸优化计算所花费的时间是很小的，只有100多毫秒。由于算法的迭代过程具有快速收敛的特点，求解一个路径规划任务的整体时间消耗也会很少，即能够验证本发明作为一个计算效率高的路径规划算法具有实时在线计算的能力，能够运用在无人机机载计算机在线路径规划的技术场景中。
[0291] 以上所述的具体描述，对发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
