[发明专利]一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法

摘要

本发明公开的一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法，属于轨迹规划技术领域。本发明实现方法如下：针对多旋翼无人机轨迹生成问题，建立包括性能指标、运动学约束、状态约束、推力约束、倾斜角约束和障碍规避约束的最优控制问题模型；基于离散化方法和逐次线性化将最优控制问题转化为二阶锥规划问题，然后对二阶锥规划问题进行迭代求解，从而生成满足复杂约束的多旋翼无人机飞行轨迹。本发明要解决的技术问题为：根据实际任务需要，基于二阶锥规划方法获得满足复杂约束的多旋翼无人机飞行轨迹，具有轨迹生成效率高、轨迹结果最优性好的优点。

主权项

一种基于二阶锥规划的多旋翼无人机飞行轨迹生成方法，其特征在于：包括如下步骤，步骤一：输入多旋翼无人机参数信息、轨迹约束信息、任务环境信息和算法参数信息；所述的多旋翼无人机参数信息包括多旋翼无人机的质量、最大推力；所述的轨迹约束信息包括初始位置、初始速度、目标位置、目标速度；所述的任务环境信息包括障碍、边界；所述的算法参数信息包括飞行时间离散区间的数量、位置收敛最大允许误差、速度收敛最大允许误差；步骤二：针对多旋翼无人机轨迹生成问题，建立包括性能指标、运动学约束、状态约束、推力约束、倾斜角约束和障碍规避约束的最优控制问题模型；多旋翼无人机轨迹生成的性能指标J的表达式如式(1)所示，通过最小化如式(1)所示的性能指标J以获得平滑的多旋翼无人机飞行轨迹；$J={\∫}_{t_0}^{t_f}{(m\·||a+g|{|}_2)}^{2}dt---\left(1\right)$其中，a为多旋翼螺旋桨推力产生的加速度，g为重力加速度，t₀表示飞行轨迹的初始时刻，t_f表示飞行轨迹的终端时刻，||·||₂表示二范数；多旋翼无人机的运动学约束表示为如公式(2)所示的线性微分方程组；$\{\begin{array}{c}\stackrel{\·}{p}\left(t\right)=v\left(t\right)\\ \stackrel{\·}{v}\left(t\right)=a\left(t\right)\end{array}---\left(2\right)$其中，t表示时间，p＝(p_x,p_y,p_z)^T表示无人机的位置，p_x、p_y、p_z分别表示无人机位置在x、y、z轴上的分量，v＝(v_x,v_y,v_z)^T表示无人机的速度，v_x、v_y、v_z分别表示无人机速度在x、y、z轴上的分量，a＝(a_x,a_y,a_z)^T表示无人机的加速度，a_x、a_y、a_z分别表示无人机加速度在x、y、z轴上的分量；多旋翼无人机的状态约束包括初始状态约束、终端状态约束和状态边界约束，分别如式(3)、(4)和(5)所示；p(t₀)＝p₀,v(t₀)＝v₀  (3)p(t_f)＝p_f,v(t_f)＝v_f  (4)p_min≤p(t)≤p_max,v_min≤v(t)≤v_max,t∈[t₀,t_f]  (5)其中，p₀表示初始位置，v₀表示初始速度，p_f表示终端位置，v_f表示终端速度，p_min表示位置下边界，p_max表示位置上边界，v_min表示速度下边界，v_max表示速度上边界；多旋翼无人机的推力约束如式(6)所示，其中F_max为所有螺旋桨共同产生的推力最大值，m为无人机质量，g表示重力加速度的大小；||(a_x(t),a_y(t),a_z(t)+g)||₂≤F_max/m  (6)多旋翼无人机的倾斜角约束表示为式(7)所示，其中α_max为允许的最大倾斜角；||(a_x(t),a_y(t),0)||₂≤tanα_max·(a_z(t)+g)  (7)多旋翼无人机障碍规避约束表示如式(8)所示，其中，p_xy＝(p_x,p_y)^T表示无人机位置p在水平面的投影，为第m个障碍的中心位置，为第m个障碍的半径，M为障碍总数量；$\left|\right|p_{xy}\left(t\right)-p_m^T\left|\right|\≥r_m^T,m=1,2,\mathrm{....},M---\left(8\right)$根据式(1)给出的性能指标和式(2)‑(8)给出的约束条件，建立针对多旋翼无人机轨迹生成问题的最优控制问题，具体如式(9)所示；步骤三：将步骤三中建立的最优控制问题参数化为最优化问题，包括对性能指标和约束条件的离散化；将飞行时间均匀离散为K个区间，所述的区间数量K为步骤一输入的算法参数之一，每个区间的时间步长为Δt＝(t_f‑t₀)/K，则多旋翼无人机的飞行轨迹可用K+1个离散点近似描述；记离散点的时刻为t_k＝t₀+k·Δt，多旋翼无人机在t_k时刻的位置、速度和加速度为p[k]＝p(t_k)，v[k]＝v(t_k)和a[k]＝a(t_k)，其中k＝0,1,…,K；根据离散化的