CN109839953A - 基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法 - Google Patents

Abstract

本发明公开了一种基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，涉及无人机导航技术领域，包括步骤：输入三维空间中的航点，最大速度，最大加速度，最大跃度以及允许航点最大误差；以直线航线段长度以及允许的最大轨迹平滑误差作为约束，建立并求解各贝塞尔曲线平滑转接参数的最优化问题；利用完全二叉树数据结构与动力学约束对曲线段速度进行规划；根据曲线段速度规划对直线段进行速度规划；对整个飞行轨迹进行实时插补，得到参考飞行轨迹。本发明调整了路径规划的参数，优化飞行速度从而缩短飞行时间；高效地进行速度规划，保证了实时计算的进行。

Description

基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法

技术领域

本发明涉及无人机导航技术领域，尤其涉及一种基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法。

背景技术

在无人机自主飞行的路径规划过程中，利用贝塞尔曲线进行航点之间的连接以及转接的方法已被广泛使用。但是现有的方法依旧存在着诸多局限，比如：规划过程没有充分考虑无人机的动力学特性；规划出的路径不合理、时间效率低下；路径规划的算力成本高、时间长；速度规划方法效率低下，不符合实时计算的要求等问题。

因此，本领域的技术人员致力于开发一种基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，调整了路径规划的参数，优化飞行速度从而缩短飞行时间；高效地进行速度规划，保证实时计算的进行。

发明内容

有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是如何调整路径规划的参数，优化飞行速度从而缩短飞行时间，高效地进行速度规划，保证实时计算的进行。

为实现上述目的，本发明提供了一种基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，所述方法包括以下步骤：

步骤1、输入三维空间中的航点，最大速度，最大加速度，最大跃度以及允许航点最大误差；

步骤2、以直线航线段长度以及允许的最大轨迹平滑误差作为约束，建立并求解各贝塞尔曲线平滑转接参数的最优化问题；

步骤3、利用完全二叉树数据结构与动力学约束对曲线段速度进行规划；

步骤4、根据曲线段速度规划对直线段进行速度规划；

步骤5、对整个飞行轨迹进行实时插补，得到参考飞行轨迹。

进一步地，所述步骤2包括以下步骤：

步骤2.1、对于每个由航点组成的直线轨迹，构造两段贝塞尔曲线实现转接；

步骤2.2、根据给定的曲线平滑误差ò_max与动力学约束，构造线性规划最优化问题，求解得到各段贝塞尔曲线的转接长度d。

进一步地，所述步骤3包括以下步骤：

步骤3.1、对于给定的曲线平滑误差ò_max以及最大加速度A_max，确定贝塞尔曲线段的最大飞行速度V_cm；

步骤3.2、将所有曲线段速度设为该曲线段的最大飞行速度；

步骤3.3、检测所有曲线段对，寻找违反动力学约束的曲线段对，其中速度较低的曲线段序号按照速度排序***完全二叉树；

步骤3.4、按照速度从小到大的顺序遍历二叉树，每次将相邻的两个曲线段的速度降到不违反动力学约束的极限情况，同时检测被降速的曲线段是否发生了新的动力学冲突，如果是，新的动力学冲突对中速度较低的曲线段序号要***完全二叉树；

步骤3.5、重复步骤3.4直到没有新的动力学冲突发生为止，完成曲线段速度规划。

进一步地，所述步骤2.1包括，针对由航点P₂与其相邻航点P₁，P₃组成的直线轨迹，在顶点处P₂构造如下的两段贝塞尔转接函数：

其中：

式中：P₁表示第一个航点，P₂表示第二个航点，P₃表示第三个航点，B₁(u)表示第一段贝塞尔曲线，B_1i表示第一段贝塞尔曲线的第i个控制点，u表示贝塞尔曲线参数，B₂(u)表示第二段贝塞尔曲线，B_2(3-i)表示第一段贝塞尔曲线的第3-i个控制点，T₁表示上式中沿的单位向量，d表示贝塞尔曲线转接长度，η表示上式中确定的曲线设计参数，u_d表示由所确定的单位向量，T₂表示由所确定的单位向量，β为T₁与u_d之间的夹角。

进一步地，所述步骤2.2包括，对于给定的曲线平滑误差ò_max，贝塞尔曲线转接长度需满足如下条件：

进一步地，所述步骤2.2包括，对于每一贝塞尔转接函数，确定最大曲率κ_max的计算公式为：

进一步地，所述步骤2.2包括，对于有n+2个航点的飞行任务，共有n+1个直线航段，记每条航线段长度为l_i，构建如下形式的转接线段长度最优问题：

s.t.d_i≤c₄òcscβ_i

d_i≤min(l_i,l_i-1)×0.45

d₁≤l₀×0.5

d_n≤l_n+1×0.5

d_i≥0

ξ≥0

式中：ξ表示最小曲率半径，λ为设计参数，λ设为0.5，d_i表示第i段曲线的转接长度，β_i表示第i段航线中T₁与u_d之间的夹角，d_i-1表示第i-1段曲线的转接长度，l_i-1表示第i段航线的直线长度，l₀表示第1段航线的直线长度，c₅＝(c₁+4)/(54c₃)为中间变量。

进一步地，所述步骤3.1包括，对于给定的曲线平滑误差ò_max以及最大加速度A_max，确定贝塞尔曲线段的最大飞行速度V_cm，计算公式如下：

进一步地，所述步骤4包括，根据曲线段速度规划对直线段进行速度规划，具体公式如下：

式中：s(t)表示曲线路径，F_k表示第k曲线段速度规划，k的取值范围为1～n(n为总航线段数)，ΔV表示曲线段起止点速度差，t_e表示加减速时间，t表示时间，v(t)表示飞行速度，a(t)表示飞行加速度，j(t)表示飞行跃度；

所述加减速时间t_e需满足以下条件限制：

式中：表示受限加速度情况下所允许最大转接时间，表示受限跃度情况下所允许最大转接时间，J_max表示所允许最大跃度。

进一步地，所述步骤5中的实时插补公式如下：

式中：C_l(t_k,i)表示所生成的轨迹时变方程，P_(k-1)0、P_k0分别表示直线段的两个端点，s(iT_s)表示步骤4得到的i个T_s周期内直线段轨迹，T_s表示插补周期；

对于贝塞尔曲线，由于无人机为匀速飞行，采用以下实时插补方式：

式中：Δs＝F_kT_s，Δu为每次插补的目标参变量增量，L_B为贝塞尔曲线长度。

本发明的有益技术效果为：

1、调整路径规划的参数，优化飞行速度从而缩短飞行时间；

2、高效地进行速度规划，保证实时计算。

以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。

附图说明

图1是本发明的一个较佳实施例的流程图；

图2是现有技术的全程路径规划示意图；

图3是现有技术的路径规划局部示意图；

图4是现有技术的速度规划示意图；

图5是本发明的一个较佳实施例的路径规划局部示意图；

图6是本发明的一个较佳实施例的速度规划示意图；

图7是本发明的一个较佳实施例的速度变化极限示意图。

具体实施方式

以下参考说明书附图介绍本发明的多个优选实施例，使其技术内容更加清楚和便于理解。本发明可以通过许多不同形式的实施例来得以体现，本发明的保护范围并非仅限于文中提到的实施例。

如图1所示，本发明提供的基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，包括如下步骤：

步骤1：输入三维空间中的航点，动力学约束(最大速度，最大加速度，最大跃度)与允许航点最大误差；

步骤2：以直线航线段长度以及允许的最大轨迹平滑误差作为约束，建立并求解各贝塞尔曲线平滑转接参数的最优化问题；

步骤3：利用完全二叉树数据结构与动力学约束对曲线段速度进行规划；

步骤4：根据曲线段速度规划对直线段进行速度规划；

步骤5：对飞行轨迹进行实时插补，得到参考飞行轨迹。

具体实施方法如下：

对于每个航点，都由两段贝塞尔曲线进行顺滑。比如针对由航点P₂与其相邻航点P₁，P₃组成的直线轨迹，在顶点P₂处由以下两段贝塞尔曲线实现转接。

其中

每个拐点处的转接曲线由航点P₁，P₂，P₃与参数d唯一确定。

对于给定的曲线平滑误差ò_max，贝塞尔曲线转接长度需满足

同时，对于每一贝塞尔转接函数，最大曲率为

对于给定的曲线平滑误差ò_max以及最大加速度A_max，确定贝塞尔曲线段最大飞行速度为

对于飞行轨迹中的直线段，加/减速过程采用以下形式的运动规划

其中，t_e为加减速时间，F_k为第k曲线段飞行速度规划。加减速时间t_e需满足以下条件限制：

步骤2：为保障可靠的飞行性能，需使得所有线段的最大曲率最小。基于上述限制，对于有n+2个航点的飞行任务，共有n+1个直线航段。记每条航线段长度为l_i，构建如下形式的转接线段长度最优问题：

s.t.d_i≤c₄òcscβ_i

d_i≤min(l_i,l_i-1)×0.45

d₁≤l₀×0.5

d_n≤l_n+1×0.5

d_i≥0

ξ≥0

该最优化问题为以x＝[d₁,d₂,…,d_n,ξ]为决策变量的线性规划。

步骤3包括以下步骤：

步骤3.1：将所有曲线段速度设为该曲线段的最大速度(由航点与d算得)；

步骤3.2：检测所有曲线段对(一条直线段连接的两个曲线段)，寻找违反动力学约束的曲线段对，其中速度较低的曲线段序号按照速度排序***完全二叉树；

步骤3.3：按照速度从小到大的顺序遍历二叉树，每次将相邻的两个曲线段的速度降到不违反动力学约束的极限情况，同时检测被降速的曲线段是否发生了新的动力学冲突，如果是，新的冲突对中速度较低的拐点要***完全二叉树。

步骤4：根据每个拐点的速度与拐点之间直线段的长度，规划直线段的速度。

步骤5：对于直线段，按以下方式进行实时插补：

其中P_(k-1)0以及P_k0为直线段两端点，s(iT_s)为前述直线段轨迹规划表达式，T_s为插补周期。

对于贝塞尔曲线，由于其为匀速飞行，采用以下实时插补方式：

其中，Δs＝F_kT_s，Δu为每次插补的目标参变量增量，L_B为贝塞尔曲线长度，可按数值积分求得。

与申请号为CN201610704370.2的发明相比，步骤2中的最优化问题的线性规划的边界条件从

s.t.d_i≤c₄òcscβ_i

d_i+d_i-1≤l_i-1

d₁≤l₀

d_n≤l_n+1

d_i≥0

ξ≥0

修改为

s.t.d_i≤c₄òcscβ_i

d_i≤min(l_i,l_i-1)×0.45

d₁≤l₀×0.5

d_n≤l_n+1×0.5

d_i≥0

ξ≥0

曲线段为匀速，起点不能直接处于曲线段，需要有直线段的加速(起点时速度为0，而以v＝0的匀速通过曲线段是不可能的)。因此将起点与末尾的曲线段约束修改为：

d₁≤l₀×0.5

d_n≤l_n+1×0.5

边界约束条件更改后避免了一些无解的极端情况，有助于提高运行效率。同时，也避免了原有的约束条件会导致曲线段侵蚀以及没有直线段用于速度变换问题。

本发明一个较佳实施例中，通过原有的约束条件求得的全程路径如图2所示，图3为图2所示全程路径的右上角部分，“*”为航点。起点为(0,0)，第二拐点(0.4,-0.1)对应的拐角非常小(d极小)，因为第一拐角与第三拐角可以通过侵占第二拐角的部分来获得更多拐角空间，而规划目标会促成这一行为(因为线性规划目标包含有∑d取max)。同时第一、二、三拐角的曲线段是相连的，没有任何直线段提供加减速。这意味着第一第三拐角虽然允许更高的速度，但是因为与速度更低的第二拐角之间没有任何直线段，无法加速，因此只能以第二拐角的低速运动走完全程。具体如图4所示，横坐标为时间，纵坐标为速度，可以看到在2～9秒的时间里，机器人都在以一个极低的速度运动。这段区间对应着第一～第四拐点。

修改约束条件为d_i≤min(l_i,l_i-1)×0.45后，路径如图5所示，对应的速度如图6所示。相比原有方案，速度提高了4秒，在本例中节省了近50％的时间(从先前2-9秒的部分来看)，提高了轨迹的运行效率。

速度规划方法如图7所示，横坐标为t，纵坐标为V。图7为曲线段之间速度变化的极限情况，在极限情况下，无人机离开拐点0后，在不违反动力学约束的前提下保持加速，飞完拐点0到1之间的直线段长度后恰好到达拐点1的速度。如果拐点间直线距离比最短距离长，无人机可以继续加速到更高的速度再降速到拐点1速度。也就是说只要保证曲线段之间速度差小于极限情况，直线段均可规划，因此可以先规划拐点段速度再规划直线段速度。在已知V₀与直线段长度的情况下，V_1max可算得。

上侧为优化规划参数的改动，下侧为完全不同的速度规划的改动。

对曲线段速度规划步骤如下：

步骤a，将所有曲线段速度设为改该曲线段的最大速度(由航点与d算得)；

步骤b，检测所有曲线段对(一条直线段连接的两个曲线段)，将所有违反动力学约束的“曲线段对”中速度较低的曲线段序号按照速度排序(使用完全二叉树，复杂度为log(N))；

步骤c，按照速度从小到大的顺序遍历二叉树，每次将相邻的两个曲线段的速度降到不违反动力学约束的极限情况，同时检测被降速的曲线段是否发生了新的动力学冲突，如果是，要***完全二叉树(不影响升序遍历)；

重复步骤c直到结束，便完成曲线段速度规划。

步骤c举例如下：

遍历到第k个曲线段，速度为2m/s，相邻直线段k与直线段k+1长度均为1m，根据设定的动力学约束，算得k-1与k+1曲线段的允许最大速度为3m/s，而当前设定k+1曲线段速度为6m/s。于是将k+1曲线段的速度降为3m/s，检查k+2曲线段的速度为7m/s，k+1曲线段与k+2曲线段之间的直线段长度仅有2m不能满足3→7m/s的加速，因此将k+1序号按照其速度3m/s***完全二叉树。

利用完全二叉树数据结构与动力学约束，将原来的算法复杂度从O(N*n)提高到O(N*log(N))。

其中：N为路径点的数量，n为原算法中的最大迭代次数。

最大迭代次数＝最大速度÷迭代步长

迭代步长取决于精度，比如要求小数点后2位则步长为0.01，实际运算中，N<<n。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (10)

1.一种基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，其特征在于，所述方法包括以下步骤：

步骤1、输入三维空间中的航点，最大速度，最大加速度，最大跃度以及允许航点最大误差；

步骤2、以直线航线段长度以及允许的最大轨迹平滑误差作为约束，建立并求解各贝塞尔曲线平滑转接参数的最优化问题；

步骤3、利用完全二叉树数据结构与动力学约束对曲线段速度进行规划；

步骤4、根据曲线段速度规划对直线段进行速度规划；

步骤5、对整个飞行轨迹进行实时插补，得到参考飞行轨迹。

2.如权利要求1所述的基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，其特征在于，所述步骤2包括以下步骤：

步骤2.1、对于每个由航点组成的直线轨迹，构造两段贝塞尔曲线实现转接；

步骤2.2、根据给定的曲线平滑误差ò_max与动力学约束，构造线性规划最优化问题，求解得到各段贝塞尔曲线的转接长度d。

3.如权利要求1所述的基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，其特征在于，所述步骤3包括以下步骤：

步骤3.1、对于给定的曲线平滑误差ò_max以及最大加速度A_max，确定贝塞尔曲线段的最大飞行速度V_cm；

步骤3.2、将所有曲线段速度设为该曲线段的最大飞行速度；

步骤3.3、检测所有曲线段对，寻找违反动力学约束的曲线段对，其中速度较低的曲线段序号按照速度排序***完全二叉树；

步骤3.4、按照速度从小到大的顺序遍历二叉树，每次将相邻的两个曲线段的速度降到不违反动力学约束的极限情况，同时检测被降速的曲线段是否发生了新的动力学冲突，如果是，新的动力学冲突对中速度较低的曲线段序号要***完全二叉树；

步骤3.5、重复步骤3.4直到没有新的动力学冲突发生为止，完成曲线段速度规划。

4.如权利要求2所述的基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，其特征在于，所述步骤2.1包括，针对由航点P₂与其相邻航点P₁，P₃组成的直线轨迹，在顶点处P₂构造如下的两段贝塞尔转接函数：

其中：

c₂＝(c₁+4)(c₁+1)，c₃＝(c₁+4)(c₂+6)，η＝6c₃cosβ/(c₁+4)，

式中：P₁表示第一个航点，P₂表示第二个航点，P₃表示第三个航点，B₁(u)表示第一段贝塞尔曲线，B_1i表示第一段贝塞尔曲线的第i个控制点，u表示贝塞尔曲线参数，B₂(u)表示第二段贝塞尔曲线，B_2(3-i)表示第一段贝塞尔曲线的第3-i个控制点，T₁表示上式中沿的单位向量，d表示贝塞尔曲线转接长度，η表示上式中确定的曲线设计参数，u_d表示由所确定的单位向量，T₂表示由所确定的单位向量，β为T₁与u_d之间的夹角。

5.如权利要求2所述的基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，其特征在于，所述步骤2.2包括，对于给定的曲线平滑误差ò_max，贝塞尔曲线转接长度需满足如下条件：

6.如权利要求2所述的基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，其特征在于，所述步骤2.2包括，对于每一贝塞尔转接函数，确定最大曲率κ_max的计算公式为：

7.如权利要求2所述的基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，其特征在于，所述步骤2.2包括，对于有n+2个航点的飞行任务，共有n+1个直线航段，记每条航线段长度为l_i，构建如下形式的转接线段长度最优问题：

s.t.d_i≤c₄òcscβ_i

d_i≤min(l_i,l_i-1)×0.45

d₁≤l₀×0.5

d_n≤l_n+1×0.5

d_i≥0

ξ≥0

式中：ξ表示最小曲率半径，λ为设计参数，d_i表示第i段曲线的转接长度，β_i表示第i段航线中T₁与u_d之间的夹角，d_i-1表示第i-1段曲线的转接长度，l_i-1表示第i段航线的直线长度，l₀表示第1段航线的直线长度，c₅＝(c₁+4)/(54c₃)为中间变量。

8.如权利要求3所述的基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，其特征在于，所述步骤3.1包括，对于给定的曲线平滑误差ò_max以及最大加速度A_max，确定贝塞尔曲线段的最大飞行速度V_cm，计算公式如下：

9.如权利要求1所述的基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，其特征在于，所述步骤4包括，根据曲线段速度规划对直线段进行速度规划，具体公式如下：

式中：s(t)表示曲线路径，F_k表示第k曲线段速度规划，k的取值范围为1～n(n为总航线段数)，ΔV表示曲线段起止点速度差，t_e表示加减速时间，t表示时间，v(t)表示飞行速度，a(t)表示飞行加速度，j(t)表示飞行跃度；

所述加减速时间t_e需满足以下条件限制：

式中：表示受限加速度情况下所允许最大转接时间，表示受限跃度情况下所允许最大转接时间，J_max表示所允许最大跃度。

10.如权利要求1所述的基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，其特征在于，所述步骤5中的实时插补公式如下：

式中：C_l(t_k,i)表示所生成的轨迹时变方程，P_(k-1)0、P_k0分别表示直线段的两个端点，s(iT_s)表示步骤4得到的i个T_s周期内直线段轨迹，T_s表示插补周期；

对于贝塞尔曲线，由于无人机为匀速飞行，采用以下实时插补方式：

式中：Δs＝F_kT_s，Δu为每次插补的目标参变量增量，L_B为贝塞尔曲线长度。