[发明专利]时间最优快速三维避障路径规划方法

权利要求书

1.时间最优快速三维避障路径规划方法，其特征在于：包括如下步骤，

步骤一：对无人机进行运动学建模并量纲归一化，建立三维无量纲运动方程；

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

步骤三：把原无人机避障路径时间最优控制问题P₀中的非线性动力学变换为线性动力学，将原P₀问题转化为问题P₁；

时间自由最优控制问题能够通过增加一个参数转化为固定时间的最优控制问题；在原P₀问题中，初始时间是固定的，末端时间是自由的；将问题P₀转化为具有固定初始时间和固定结束时间的最优控制问题；

步骤四：通过凸松弛将问题P₁中存在或者引入的非凸约束转化为凸约束，进而把P₁问题松弛为凸优化问题P₂；

步骤五：在[t₀,t_f]上用(N+1)个离散点将问题P₂离散形成二阶锥规划问题P₃；所述(N+1)个离散点即{t₀,...,t_N}；

步骤六：迭代求解步骤五得到的二阶锥规划问题P₃，在每次迭代中，首先计算P₃中的依赖参数y^(k)，然后再次求解P₃问题，得到一个新的解，用于更新下一次迭代中的参数；重复这个过程，直到当前的解与上一步的解一致，即实现通过协调飞行时间和飞行速度方向以实现时间最优的避障路径规划，通过所优化的最优的避障路径飞行轨迹提升无人机执行任务的反应能力；

步骤一具体实现方法为，对无人机进行运动学建模，并量纲归一化，无人机三维避障的无量纲运动方程表示为：

其中，[x，y，z]^T是无人机的空间位置，z是高度，x，y是水平面正交方向的坐标；Vc是无人机速度，为已知量；ψ和φ分别为飞行路径角和航向角；在公式(1)中，除了ψ和φ以外，距离变量[x，y，z]^T用初始和末端位置的欧式距离L₀来归一化，速度用V_c归一化；时间和比冲均用L₀/V_c归一化；

步骤二具体实现方法为，对于公式(1)中的控制量为飞行路径角ψ和和航向角φ；在无人机路径规划问题中，考虑以加速度约束来表示无人机的机动性能；各个方向的加速度分量表示为：

对于公式(1)所述的运动学模型，总加速度表示为：

由于速度为已知，所以绝对加速度在速度的法平面上；除了符合运动学模型之外，在具体的飞行任务中满足的约束还包括:

初始和末端约束：

其中χ₀＝[x₀,y₀,z₀]^T,χ_f＝[x_f,y_f,z_f]^T是初始和末端位置；

加速度约束：定义最大允许加速度为，则：

障碍约束：障碍约束被推广成凹函数如下：

当障碍约束简化为圆柱形障碍约束时，障碍约束如公式(7)所示：

当障碍约束简化为球形障碍约束时，障碍约束如公式(8)所示：

其中，和分别表示障碍的球心和圆柱中心；和分别表示球和圆柱的半径；和是椭圆形区域的预定义半轴/半轴；圆柱形障碍约束和球形障碍约束是非凸约束；

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

然后导出无人机避障路径时间最优控制问题如下：

满足方程:(1),(4)-(5),(6) (11)

无人机避障路径时间最优控制问题是非凸的，因为公式(1)中的动力学包含强非线性因子的三角函数，并且避障区域的约束进一步加重非线性因素；用一般非线性规划求解器求解所述非凸问题是费时的；为此，将非凸问题P₀转换为凸优化问题，从而使得如此耗时的非凸问题变得更轻且易于实现；

步骤三具体实现方法为，首先将原P₀问题中非线性动力学变换为线性动力学模型；时间参数更改为：

设置t₀＝0；根据上式，微分得：

通过公式(12)、(13)，将公式(1)所示的运动学模型变换为：

公式(4)表示的初始和末端约束变为：

对于公式(14)和公式(5)，是欧拉角和角速度的非线性函数；因为凸优化要求所有等式约束都是线性的，所有不等式约束都是凸的；因此，需将公式(14)转换成线性运动学模型；当使用速度向量v＝[v_x,v_y,v_z]^T，而不使用公式(14)和公式(5)中出现的欧拉角，因此，具有如下优先：①能够防止奇异性；②由于采用向量表示法，能够将公式(14)、(15)所述的模型转化为线性运动学模型：

v_x:＝t_fV_ccosψcosφ；v_y:＝t_fV_ccosψsinφv_z:＝t_fV_csinψ (16)

上式新的变量v必须满足:

然后，给出关于新输入的加速度约束；其中一个加速度分量是时间τ的函数，因此，加速度分量能够替换为如下形式:

上式中出现的考虑公式(14)，得到：

所以加速度的分量成为以τ作为新自变量的函数：

将上述公式(21)、(22)代入公式(2)；法向加速度相对于τ将表示如下:

将公式(5)替换为关于新自变量τ的函数，所述函数能够线性化成圆锥凸约束；定义：

v′_x:＝u_x；v′_y:＝u_y；v′_z:＝u_z (23)

将公式(23)代入公式(22)，法向加速度表示为：

公式(5)变换为如下表达：

根据公式(23)，公式(14)重新表述为以下双积分形式：

()’表示相对于τ的微分，公式(26)简写为：

X′＝AX+Bu (27)

其中X:＝[x,y,z,v_x,v_y,v_z]^T，

至此公式(1)已被转换成具有新状态的固定区间线性运动学模型；

对于公式(27)、初始和末端约束为：

以上是一系列等式线性约束，其中V:＝[V_x,V_y,V_z]^T是速度矢量；根据公式(1)知：

优化目标函数等价于：

至此，原无人机避障路径时间最优控制问题P₀转化为问题P₁：

P₁:min J＝t_f (31)

s.t. X′＝AX+Bu,τ∈[0,1] (32)

使用三角函数反求欧拉角，所述欧拉角即指飞行路径角和航向角，能够表示为速度分量变量的函数，分别如下：

上述方程的非奇异条件是v_x≠0,v_z≤t_fV_c，即使v_x＝0，仍然能够通过速度矢量v定义合适的欧拉角；此外，当使用欧拉角直接表示加速度约束时，复杂的表达式不利于优化问题的求解。