CN112068586A - 一种空间时间联合优化的四旋翼无人飞行器轨迹规划方法 - Google Patents

Abstract

本发明公开了一种空间时间联合优化的四旋翼无人飞行器轨迹规划方法，对前端路径搜索得到离散路径点，采用梯形时间分配法给各段路径分配时间间隔，通过无约束多项式系数优化得到初始轨迹路径；基于初始轨迹的持续时间代价与能量消耗代价，在考虑约束的情况下对时间间隔进行比例调节，求解最优调节比例并更新所有轨迹段的时间间隔；之后交替进行无约束多项式系数优化与比例调节时间间隔两个环节，当轨迹代价趋于收敛时结束循环步骤；对整个轨迹路径进行整体比例调节，确保整条轨迹满足约束条件，得到最终优化轨迹；利用该多项式轨迹时间离散得到的路径点来控制飞行器跟随轨迹。本发明不仅实现了轨迹的空间‑时间联合优化，同时大幅提升计算速率。

Description

一种空间时间联合优化的四旋翼无人飞行器轨迹规划方法

技术领域

本发明属于四旋翼无人飞行器导航轨迹规划技术领域，具体涉及一种空间时间联合优化的四旋翼无人飞行器轨迹规划方法。

背景技术

最近十几年，随着各种新型技术的发展，如集成电路、微机电***、新型控制理论与方法等，四旋翼无人机获得了人们的研究兴趣。四旋翼无人机体积小、运动灵活、具有垂直起降功能、可在空中悬停，拥有着无可替代的优势。此外，通信技术、控制原理、SLAM算法、路径规划算法等的完善，使得四旋翼无人机的性能指标与各项能力逐渐提升，除了军事领域应用外，四旋翼无人机的应用已扩展到抗震救灾、农业植保、影视拍摄等众多领域。

四旋翼无人飞行器路径规划一般可分为前端路径搜索与后端轨迹规划两部分。针对四旋翼后端轨迹规划问题，现有基于分段多项式轨迹，建模最小化轨迹snap积分的QP问题，并推导多项式轨迹最优边界条件的闭式解，其重点集中在了轨迹空间优化部分；还有前端动力学约束下的路径搜索联合后端B样条曲线优化的方法，并通过调整非均匀B样条曲线的节点长度来保证轨迹的动力学约束，不足之处在于忽略了轨迹时间段的优化，经过节点长度调节之后的轨迹已无法保证最优性；还有基于分段多项式，通过循环交替进行带约束的多项式边界条件优化与带约束的多项式时间间隔优化，首次提出并证明了空间-时间联合最优轨迹生成方法。但是由于两个带约束的优化问题求解复杂，以及方法的循环优化结构，使得轨迹生成的速率减慢。

综上所述，尽管轨迹规划领域已经存在多种解决方案，但是仍缺少实时快速的空间-时间联合优化的轨迹生成方法。

发明内容

本发明所要解决的技术问题在于针对上述现有技术中的不足，提供一种空间时间联合优化的四旋翼无人飞行器轨迹规划方法，基于分段多项式模型，解耦多项式系数优化与多项式时间间隔优化，并重新设计多项式时间间隔优化方法，不仅实现了轨迹的空间-时间联合优化，同时大幅提升计算速率。

本发明采用以下技术方案：

一种空间时间联合优化的四旋翼无人飞行器轨迹规划方法，包括以下步骤：

S1、对前端路径搜索得到离散路径点，采用梯形时间分配法给各段路径分配时间间隔T_i，通过无约束多项式系数优化得到初始轨迹路径；

S2、对步骤S1得到的初始轨迹路径进行分段处理，基于初始轨迹的持续时间代价与能量消耗代价，在考虑约束的情况下对时间间隔T_i进行比例调节，求解最优调节比例并更新所有轨迹段的时间间隔；

S3、交替进行无约束多项式系数优化与比例调节时间间隔两个环节，当轨迹代价趋于收敛时结束循环步骤；

S4、对整个轨迹路径进行整体比例调节，确保整条轨迹满足约束条件，得到最终优化轨迹，利用该多项式轨迹时间离散得到的路径点来控制飞行器跟随轨迹。

具体的，步骤S1中，基于分段多项式表示四旋翼无人飞行器轨迹，每段无人机的三维轨迹表示为：

P(t)＝Aβ(t),t∈[0,T]

其中，A∈R^3×(N+1)，为多项式系数矩阵，N为多项式阶数，β(t)＝(t^N...t²,t,1)^T为多项式基函数，T为对应段多项式的时间间隔。

具体的，步骤S2中，设某段初始轨迹为p_i(t)，计算代价函数，对于每段多项式轨迹，比例调节T_i为kT_i，求解在满足约束条件v(t)≤v_max,a(t)≤a_max下的最优调整比例best_k，依据求得的最优比例best_k更新多项式时间间隔。

进一步的，最优比例best_k为：

其中，k_tight为每段轨迹时间间隔调整的临界比例，cost_k为轨迹持续时间代价，cost_d为轨迹能量消耗代价。

更进一步的，本发明的特点还在于，按比例调节T_i为kT_i后，对应的轨迹代价函数为：

cost_t'＝kcost_t

其中，cost_t'为比例调整之后的轨迹持续时间代价，cost_t为轨迹持续时间代价，cost_d'为比例调整之后的轨迹能量消耗代价，cost_d为轨迹能量消耗代价。

具体的，步骤S3中，无约束多项式系数优化环节保持当前多项式时间间隔不变，在不考虑最大速度和最大加速度的约束求解令cost_d最小的多项式系数，优化问题表达成QP问题求解，推导对应的闭式最优解。

具体的，步骤S4中，当循环进行带约束的、最优比例时间间隔调节环节与无约束多项式系数优化环节至代价函数的减小收敛到规定阈值，进行整体比例调节环节，使约束条件v(t)≤v_max,a(t)≤a_max在整个轨迹上都得到满足。

进一步的，某段轨迹进行整体时间比例调节的过程为：

p_i(t)＝a₅t⁵+a₄t⁴+a₃t³+a₂t²+a₁t+a₀,t∈[0,T_i]

其中，轨迹p_i'(t)为轨迹p_i(t)依据整体调节比例kf进行更新后的轨迹。

更进一步的，本发明的特点还在于，整体调节比例kf为：

kf＝scale,if scale>1

kf＝1,if scale≤1

其中，v_all_max为整条轨迹最大速度，a_all_max为整条轨迹最大加速度。

更进一步的，本发明的特点还在于：

与现有技术相比，本发明至少具有以下有益效果：

本发明一种空间时间联合优化的四旋翼无人飞行器轨迹规划方法，通过无约束多项式系数优化及带约束最优比例时间间隔调节，实现基于分段多项式轨迹的空间-时间联合优化；同时对无约束多项式系数优化及带约束最优比例时间调节两个模块均推导出闭式解，大幅提升了计算速率。

进一步的，基于分段多项式模型，可以闭式求解轨迹的微分与积分，容易进行极值的搜索，方便三个维度轨迹的解耦，保证轨迹的平滑性。

进一步的，基于当前轨迹的持续时间代价和能量消耗代价推导最优时间调整比例的闭式解，在满足约束条件下优化轨迹代价。

进一步的，设置轨迹代价函数为

同时优化轨迹时间代价与轨迹能量代价，列出比例时间调整之后轨迹代价的变化。

进一步的，通过无约束多项式系数优化环节对当前轨迹进行多项式系数优化，继续优化轨迹代价，同时保证两段轨迹连接处的光滑性。

进一步的，循环进行带约束的最优比例时间间隔调节环节与无约束多项式系数优化环节，迭代优化轨迹代价至轨迹代价收敛。

进一步的，基于约束条件推导整体调节比例，保证轨迹满足约束条件。

进一步的，对于整体比例调节环节，仅需修改多项式系数与各轨迹段的时间间隔即可完成。

综上所述，本发明不仅实现了轨迹的空间-时间联合优化，同时大幅提升计算速率。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为本发明轨迹规划方法流程图；

图2为本发明与对比方法的路径代价对比图；

图3为6段轨迹优化效果图；

图4为随机地图中路径规划效果对比图，其中，(a)为本发明方法轨迹优化结果和现有开源代码轨迹优化结果对比图，(b)为两种轨迹的细节放大对比图。

具体实施方式

首先针对轨迹优化问题进行建模

基于分段多项式表示四旋翼无人飞行器轨迹，每段三维轨迹表示为：

P(t)＝Aβ(t),t∈[0,T]

其中，A∈R^3×(N+1)，为多项式系数矩阵，N为多项式阶数，β(t)＝(t^N...t²,t,1)^T为多项式基函数，T为该段多项式时间间隔，多项式边界条件表示为：

当多项式时间间隔T，初始边界条件d(0)，终端边界条件d(T)确定后，该段多项式轨迹便唯一确定。

设定轨迹代价函数为：

其中，m为轨迹总段数，T_i为第i段轨迹的时间间隔，积分内第一项为轨迹持续时间的代价，第二项为轨迹能量消耗代价，当j等于2时对应轨迹加速度，当j等于3时对应轨迹jerk，当j等于4时对应轨迹snap，w_t,w_d分别为两项代价的权重，轨迹优化要求在满足约束条件v(t)≤v_max,a(t)≤a_max下求解代价函数最小的轨迹。

请参阅图1，本发明一种空间时间联合优化的四旋翼无人飞行器轨迹规划方法，包括以下步骤：

S1、对前端路径搜索得到的离散路径点，采用梯形时间分配法给各段路径分配时间间隔T_i，通过无约束多项式系数优化得到初始轨迹，此时得到的轨迹由于并未考虑约束条件，故不满足最大速度与最大加速度的约束；

S2、对于初始轨迹，本发明提出一种对时间间隔T_i进行优化的闭式优化方法，对整体路径进行分段处理，基于初始轨迹的持续时间代价与能量消耗代价，在考虑约束的情况下对时间间隔T_i进行比例调节，求解最优调节比例并更新所有轨迹段的时间间隔；

带约束的最优比例时间间隔调节基于已有轨迹，分段进行时间间隔优化。设某段初始轨迹为：

p_i(t)＝a₅t⁵+a₄t⁴+a₃t³+a₂t²+a₁t+a₀,t∈[0,T_i]

此处以一维5次多项式轨迹说明本发明方法原理，j取3，计算该段轨迹的代价函数为：

cost_t＝ρT_i

其中，cost_t为轨迹持续时间代价，cost_d为轨迹能量消耗代价。

对于每段多项式轨迹，比例调节T_i为kT_i，带约束的最优比例时间间隔调节的目的是求解在满足约束条件v(t)≤v_max,a(t)≤a_max下的最优调整比例k，首先计算临界比例：

其中，v_rmax为该段轨迹实际最大速度，a_rmax为该段轨迹实际最大加速度。

当比例调节T_i为kT_i后，对应的轨迹代价函数为：

cost_t'＝kcost_t

进行近似化简得：

cost_t'＝kcost_t

则该带约束最优比例时间间隔调节问题转换为：

s.t.k＞＝k_tight

得到闭式解为：

至此完成对已有轨迹的带约束最优比例时间间隔的求解，之后依据求得的最优比例best_k更新多项式时间间隔。

S3、交替进行无约束多项式系数优化与带约束最优比例时间间隔调节两个环节，当轨迹代价趋于收敛时结束循环步骤；

无约束多项式系数优化环节基于带约束的最优比例时间间隔调整之后的轨迹，不仅求解当前时间分配下的最优多项式系数，同时解决依据最优比例进行时间间隔调整之后、在两段轨迹连接处速度与加速度不连续的问题(每段轨迹求得的最优比例不同)。

无约束多项式系数优化环节保持当前多项式时间间隔不变，求解令cost_d最小的多项式系数，此时该优化问题可以表达成QP问题求解，并且可以推导其闭式最优解，实际应用与推导参见具体实施实例。

S4、对整个路径进行整体比例调节，以确保整条轨迹满足约束条件。

当循环进行带约束的、最优比例时间间隔调节环节与无约束多项式系数优化环节至代价函数收敛到规定阈值，进行整体比例调节环节，以确保约束条件v(t)≤v_max,a(t)≤a_max在整个轨迹上都得到满足。

设整条轨迹最大速度为v_all_max，整条轨迹最大加速度为a_all_max，则整体调节比例kf为：

kf＝scale,if scale>1

kf＝1,if scale≤1

对分段多项式进行整体时间比例调节仅需要更新各段多项式时间间隔，同时根据整体调节比例kf更新多项式系数，由于各段多项式的调整比例都相同，所以进行调整之后的多项式满足在连接处速度与加速度连续。某段轨迹进行整体时间比例调节的过程为：

p_i(t)＝a₅t⁵+a₄t⁴+a₃t³+a₂t²+a₁t+a₀,t∈[0,T_i]

其中，轨迹p_i'(t)为轨迹p_i(t)依据整体调节比例kf进行更新后的轨迹。

在边长为40的正方体内随机采样60个点作为前端路径搜索结果，对比本发明方法与先进路径规划方法性能，对比结果如表1所示。

表1.本发明方法与先进路径规划方法性能对比

设置本发明方法参数为j＝3,w_t＝1024,w_d＝20，对于随机生成的60个三维路径点采用本方法与“Wang Z，Zhou X，Xu C，et al.Alternating Minimization BasedTrajectory Generation for Quadrotor Aggressive Flight[J].2020”中基于分段多项式的空间-时间联合最优轨迹生成方法进行轨迹优化，记录20次运行结果，对比最终轨迹代价如图2所示，其中，虚线为对比方法的结果，实线为本发明方法结果。

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中的描述和所示的本发明实施例的组件可以通过各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例

在边长为40的正方体内随机采样6个三维坐标作为路径点，设置初始速度、初始加速度、初始jerk、终端速度、终端加速度、终端jerk均为零，约束条件为：

v_max＝7m/s,a_max＝4m/s²

代价函数参数m＝6,j＝3,w_t＝1024,w_d＝50。

当利用梯形时间分配法分配时间间隔后，无约束多项式系数优化环节推导步骤如下，仍以一维情况做原理进行说明：

某段单轴五阶多项式轨迹的cost_d为：

则对于整段轨迹的优化问题转化为：

其中，等式约束条件包括初始时刻与终端时刻零状态约束，路径点约束以及两段轨迹连接处的速度，加速度连续约束。通过变换矩阵M将系数矩阵A转换为边界条件，再通过构造变换矩阵C^T将边界条件分解为已固定的边界条件d_F和自由边界条件d_P，上述等式约束条件可以通过构造变换矩阵C^T体现，则优化问题转换为最小化cost_d：

通过cost_d对d_p求导，以确定最优的自由边界条件

之后通过变换矩阵恢复最优多项式系数为：

至此完成一次完整的无最大速度、最大加速度约束的多项式系数优化环节，获得初始轨迹。再通过循环进行最优比例调节时间间隔和无约束多项式系数优化两个环节至轨迹代价趋于收敛。最后进行整体比例调整时，由于之前优化步骤中分段最优比例调节时间间隔环节中考虑了最大速度与最大加速度约束，故在轨迹代价趋于收敛时，最大速度约束与最大加速度约束会达到紧约束或略微超出规定值，所以整体比例调整对于整个轨迹代价函数的影响几乎可以忽略。某次6段轨迹优化结果如图3。

对于完整的路径规划进行仿真：首先建立随机地图，如图4所示，在设定初始点三维坐标与终点三维坐标后，利用RRT*方法得到初始路径点，并对该初始路径点进行简化操作之后得到白色离散路径点。之后采用本方法进行轨迹优化，同时利用“Wang Z,Zhou X,XuC,et al.Alternating Minimization Based Trajectory Generation for QuadrotorAggressive Flight[J].2020.”文中开源代码进行轨迹生成。可以看出，在该仿真过程中，由RRT*方法得到的前端路径点之间距离相对较短，两种方法得到的实际轨迹基本一致，但本发明方法在速率上大约提升10倍左右。

综上所述，本发明一种空间时间联合优化的四旋翼无人飞行器轨迹规划方法，基于分段多项式模型，通过无约束多项式系数优化与时间间隔最优比例调节两个环节对轨迹代价函数进行优化，通过整体比例调节保证轨迹满足约束，不仅实现了轨迹的空间-时间联合优化，同时大幅提升计算速率。

本领域内的技术人员应明白，本申请的实施例可提供为方法、***、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (9)

1.一种空间时间联合优化的四旋翼无人飞行器轨迹规划方法，其特征在于，包括以下步骤：

S1、对前端路径搜索得到离散路径点，采用梯形时间分配法给各段路径分配时间间隔T_i，通过无约束多项式系数优化得到初始轨迹路径；

S2、对步骤S1得到的初始轨迹路径进行分段处理，基于初始轨迹的持续时间代价与能量消耗代价，在考虑约束的情况下对时间间隔T_i进行比例调节，求解最优调节比例并更新所有轨迹段的时间间隔；

S3、交替进行无约束多项式系数优化与比例调节时间间隔两个环节，当轨迹代价趋于收敛时结束循环步骤；

S4、对整个轨迹路径进行整体比例调节，确保整条轨迹满足约束条件，得到最终优化轨迹，利用该多项式轨迹时间离散得到的路径点来控制飞行器跟随轨迹。

2.根据权利要求1所述的空间时间联合优化的四旋翼无人飞行器轨迹规划方法，其特征在于，步骤S1中，基于分段多项式表示四旋翼无人飞行器轨迹，每段无人机的三维轨迹表示为：

P(t)＝Aβ(t),t∈[0,T]

其中，A∈R^3×(N+1)，为多项式系数矩阵，N为多项式阶数，β(t)＝(t^N...t²,t,1)^T为多项式基函数，T为对应段多项式的时间间隔。

3.根据权利要求1所述的空间时间联合优化的四旋翼无人飞行器轨迹规划方法，其特征在于，步骤S2中，设某段初始轨迹为p_i(t)，计算代价函数，对于每段多项式轨迹，比例调节T_i为kT_i，求解在满足约束条件v(t)≤v_max,a(t)≤a_max下的最优调整比例best_k，依据求得的最优比例best_k更新多项式时间间隔。

4.根据权利要求3所述的空间时间联合优化的四旋翼无人飞行器轨迹规划方法，其特征在于，最优比例best_k为：

其中，k_tight为每段轨迹时间间隔调整的临界比例，cost_k为轨迹持续时间代价，cost_d为轨迹能量消耗代价。

5.根据权利要求4所述的空间时间联合优化的四旋翼无人飞行器轨迹规划方法，其特征在于，按比例调节T_i为kT_i后，对应的轨迹代价函数为：

cost_t'＝kcost_t

其中，cost_t'为比例调整之后的轨迹持续时间代价，cost_t为轨迹持续时间代价，cost_d'为比例调整之后的轨迹能量消耗代价，cost_d为轨迹能量消耗代价。

6.根据权利要求1所述的空间时间联合优化的四旋翼无人飞行器轨迹规划方法，其特征在于，步骤S3中，无约束多项式系数优化环节保持当前多项式时间间隔不变，在不考虑最大速度和最大加速度的约束求解令cost_d最小的多项式系数，优化问题表达成QP问题求解，推导对应的闭式最优解。

7.根据权利要求1所述的空间时间联合优化的四旋翼无人飞行器轨迹规划方法，其特征在于，步骤S4中，当循环进行带约束的、最优比例时间间隔调节环节与无约束多项式系数优化环节至代价函数的减小收敛到规定阈值，进行整体比例调节环节，使约束条件v(t)≤v_max,a(t)≤a_max在整个轨迹上都得到满足。

8.根据权利要求7所述的空间时间联合优化的四旋翼无人飞行器轨迹规划方法，其特征在于，某段轨迹进行整体时间比例调节的过程为：

p_i(t)＝a₅t⁵+a₄t⁴+a₃t³+a₂t²+a₁t+a₀,t∈[0,T_i]

其中，轨迹p_i'(t)为轨迹p_i(t)依据整体调节比例kf进行更新后的轨迹。

9.根据权利要求8所述的空间时间联合优化的四旋翼无人飞行器轨迹规划方法，其特征在于，整体调节比例kf为：

kf＝scale,if scale>1

kf＝1,if scale≤1

其中，v_all_max为整条轨迹最大速度，a_all_max为整条轨迹最大加速度。

Images