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

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

Trajectory Planning and Velocity Planning Method Based on Bessel Curve Transfer Smoothing

The invention discloses a method of trajectory planning and velocity planning based on Bessel curve transition smoothing, which relates to the field of UAV navigation technology, including steps: inputting the navigation point in three-dimensional space, maximum speed, maximum acceleration, maximum jump and maximum error of allowable navigation point; establishing and seeking the maximum trajectory smoothing error constrained by the length of straight line segment and allowable maximum error of trajectory smoothing. Solve the optimization problem of smooth transition parameters of Bessel curves; use complete binary tree data structure and dynamic constraints to plan the speed of curve segments; plan the speed of straight line segments according to the speed planning of curve segments; interpolate the whole flight trajectory in real time to get the reference flight trajectory. The invention adjusts the parameters of the path planning, optimizes the flight speed so as to shorten the flight time, carries out the speed planning efficiently, and ensures the real-time calculation.

【技术实现步骤摘要】
基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法
本专利技术涉及无人机导航
，尤其涉及一种基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法。
技术介绍
在无人机自主飞行的路径规划过程中，利用贝塞尔曲线进行航点之间的连接以及转接的方法已被广泛使用。但是现有的方法依旧存在着诸多局限，比如：规划过程没有充分考虑无人机的动力学特性；规划出的路径不合理、时间效率低下；路径规划的算力成本高、时间长；速度规划方法效率低下，不符合实时计算的要求等问题。因此，本领域的技术人员致力于开发一种基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，调整了路径规划的参数，优化飞行速度从而缩短飞行时间；高效地进行速度规划，保证实时计算的进行。
技术实现思路
有鉴于现有技术的上述缺陷，本专利技术所要解决的技术问题是如何调整路径规划的参数，优化飞行速度从而缩短飞行时间，高效地进行速度规划，保证实时计算的进行。为实现上述目的，本专利技术提供了一种基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，所述方法包括以下步骤：步骤1、输入三维空间中的航点，最大速度，最大加速度，最大跃度以及允许航点最大误差；步骤2、以直线航线段长度以及允许的最大轨迹平滑误差作为约束，建立并求解各贝塞尔曲线平滑转接参数的最优化问题；步骤3、利用完全二叉树数据结构与动力学约束对曲线段速度进行规划；步骤4、根据曲线段速度规划对直线段进行速度规划；步骤5、对整个飞行轨迹进行实时插补，得到参考飞行轨迹。进一步地，所述步骤2包括以下步骤：步骤2.1、对于每个由航点组成的直线轨迹，构造两段贝塞尔曲线实现转接；步骤2.2、根据给定的曲线平滑误差òmax与动力学约束，构造线性规划最优化问题，求解得到各段贝塞尔曲线的转接长度d。进一步地，所述步骤3包括以下步骤：步骤3.1、对于给定的曲线平滑误差òmax以及最大加速度Amax，确定贝塞尔曲线段的最大飞行速度Vcm；步骤3.2、将所有曲线段速度设为该曲线段的最大飞行速度；步骤3.3、检测所有曲线段对，寻找违反动力学约束的曲线段对，其中速度较低的曲线段序号按照速度排序插入完全二叉树；步骤3.4、按照速度从小到大的顺序遍历二叉树，每次将相邻的两个曲线段的速度降到不违反动力学约束的极限情况，同时检测被降速的曲线段是否发生了新的动力学冲突，如果是，新的动力学冲突对中速度较低的曲线段序号要插入完全二叉树；步骤3.5、重复步骤3.4直到没有新的动力学冲突发生为止，完成曲线段速度规划。进一步地，所述步骤2.1包括，针对由航点P2与其相邻航点P1，P3组成的直线轨迹，在顶点处P2构造如下的两段贝塞尔转接函数：其中：式中：P1表示第一个航点，P2表示第二个航点，P3表示第三个航点，B1(u)表示第一段贝塞尔曲线，B1i表示第一段贝塞尔曲线的第i个控制点，u表示贝塞尔曲线参数，B2(u)表示第二段贝塞尔曲线，B2(3-i)表示第一段贝塞尔曲线的第3-i个控制点，T1表示上式中沿的单位向量，d表示贝塞尔曲线转接长度，η表示上式中确定的曲线设计参数，ud表示由所确定的单位向量，T2表示由所确定的单位向量，β为T1与ud之间的夹角。进一步地，所述步骤2.2包括，对于给定的曲线平滑误差òmax，贝塞尔曲线转接长度需满足如下条件：进一步地，所述步骤2.2包括，对于每一贝塞尔转接函数，确定最大曲率κmax的计算公式为：进一步地，所述步骤2.2包括，对于有n+2个航点的飞行任务，共有n+1个直线航段，记每条航线段长度为li，构建如下形式的转接线段长度最优问题：s.t.di≤c4òcscβidi≤min(li,li-1)×0.45d1≤l0×0.5dn≤ln+1×0.5di≥0ξ≥0式中：ξ表示最小曲率半径，λ为设计参数，λ设为0.5，di表示第i段曲线的转接长度，βi表示第i段航线中T1与ud之间的夹角，di-1表示第i-1段曲线的转接长度，li-1表示第i段航线的直线长度，l0表示第1段航线的直线长度，c5＝(c1+4)/(54c3)为中间变量。进一步地，所述步骤3.1包括，对于给定的曲线平滑误差òmax以及最大加速度Amax，确定贝塞尔曲线段的最大飞行速度Vcm，计算公式如下：进一步地，所述步骤4包括，根据曲线段速度规划对直线段进行速度规划，具体公式如下：式中：s(t)表示曲线路径，Fk表示第k曲线段速度规划，k的取值范围为1～n(n为总航线段数)，ΔV表示曲线段起止点速度差，te表示加减速时间，t表示时间，v(t)表示飞行速度，a(t)表示飞行加速度，j(t)表示飞行跃度；所述加减速时间te需满足以下条件限制：式中：表示受限加速度情况下所允许最大转接时间，表示受限跃度情况下所允许最大转接时间，Jmax表示所允许最大跃度。进一步地，所述步骤5中的实时插补公式如下：式中：Cl(tk,i)表示所生成的轨迹时变方程，P(k-1)0、Pk0分别表示直线段的两个端点，s(iTs)表示步骤4得到的i个Ts周期内直线段轨迹，Ts表示插补周期；对于贝塞尔曲线，由于无人机为匀速飞行，采用以下实时插补方式：式中：Δs＝FkTs，Δu为每次插补的目标参变量增量，LB为贝塞尔曲线长度。本专利技术的有益技术效果为：1、调整路径规划的参数，优化飞行速度从而缩短飞行时间；2、高效地进行速度规划，保证实时计算。以下将结合附图对本专利技术的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本专利技术的目的、特征和效果。附图说明图1是本专利技术的一个较佳实施例的流程图；图2是现有技术的全程路径规划示意图；图3是现有技术的路径规划局部示意图；图4是现有技术的速度规划示意图；图5是本专利技术的一个较佳实施例的路径规划局部示意图；图6是本专利技术的一个较佳实施例的速度规划示意图；图7是本专利技术的一个较佳实施例的速度变化极限示意图。具体实施方式以下参考说明书附图介绍本专利技术的多个优选实施例，使其
技术实现思路
更加清楚和便于理解。本专利技术可以通过许多不同形式的实施例来得以体现，本专利技术的保护范围并非仅限于文中提到的实施例。如图1所示，本专利技术提供的基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，包括如下步骤：步骤1：输入三维空间中的航点，动力学约束(最大速度，最大加速度，最大跃度)与允许航点最大误差；步骤2：以直线航线段长度以及允许的最大轨迹平滑误差作为约束，建立并求解各贝塞尔曲线平滑转接参数的最优化问题；步骤3：利用完全二叉树数据结构与动力学约束对曲线段速度进行规划；步骤4：根据曲线段速度规划对直线段进行速度规划；步骤5：对飞行轨迹进行实时插补，得到参考飞行轨迹。具体实施方法如下：对于每个航点，都由两段贝塞尔曲线进行顺滑。比如针对由航点P2与其相邻航点P1，P3组成的直线轨迹，在顶点P2处由以下两段贝塞尔曲线实现转接。其中每个拐点处的转接曲线由航点P1，P2，P3与参数d唯一确定。对于给定的曲线平滑误差òmax，贝塞尔曲线转接长度需满足同时，对于每一贝塞尔转接函数，最大曲率为对于给定的曲线平滑误差òmax以及最大加速度Amax，确定贝塞尔曲线段最大飞行速度为对于飞行轨迹中的直线段，加/减速过程采用以下形式的运动规划其中，te为加减速时间，Fk为第k曲线段飞行速度规划。加减速时间te需满足以下条件限制：步骤2：为保障可靠的飞行性能，需使得所有线段的最本文档来自技高网...

【技术保护点】
1.一种基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，其特征在于，所述方法包括以下步骤：步骤1、输入三维空间中的航点，最大速度，最大加速度，最大跃度以及允许航点最大误差；步骤2、以直线航线段长度以及允许的最大轨迹平滑误差作为约束，建立并求解各贝塞尔曲线平滑转接参数的最优化问题；步骤3、利用完全二叉树数据结构与动力学约束对曲线段速度进行规划；步骤4、根据曲线段速度规划对直线段进行速度规划；步骤5、对整个飞行轨迹进行实时插补，得到参考飞行轨迹。

【技术特征摘要】
1.一种基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，其特征在于，所述方法包括以下步骤：步骤1、输入三维空间中的航点，最大速度，最大加速度，最大跃度以及允许航点最大误差；步骤2、以直线航线段长度以及允许的最大轨迹平滑误差作为约束，建立并求解各贝塞尔曲线平滑转接参数的最优化问题；步骤3、利用完全二叉树数据结构与动力学约束对曲线段速度进行规划；步骤4、根据曲线段速度规划对直线段进行速度规划；步骤5、对整个飞行轨迹进行实时插补，得到参考飞行轨迹。2.如权利要求1所述的基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，其特征在于，所述步骤2包括以下步骤：步骤2.1、对于每个由航点组成的直线轨迹，构造两段贝塞尔曲线实现转接；步骤2.2、根据给定的曲线平滑误差òmax与动力学约束，构造线性规划最优化问题，求解得到各段贝塞尔曲线的转接长度d。3.如权利要求1所述的基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，其特征在于，所述步骤3包括以下步骤：步骤3.1、对于给定的曲线平滑误差òmax以及最大加速度Amax，确定贝塞尔曲线段的最大飞行速度Vcm；步骤3.2、将所有曲线段速度设为该曲线段的最大飞行速度；步骤3.3、检测所有曲线段对，寻找违反动力学约束的曲线段对，其中速度较低的曲线段序号按照速度排序插入完全二叉树；步骤3.4、按照速度从小到大的顺序遍历二叉树，每次将相邻的两个曲线段的速度降到不违反动力学约束的极限情况，同时检测被降速的曲线段是否发生了新的动力学冲突，如果是，新的动力学冲突对中速度较低的曲线段序号要插入完全二叉树；步骤3.5、重复步骤3.4直到没有新的动力学冲突发生为止，完成曲线段速度规划。4.如权利要求2所述的基于贝塞尔曲线转接平滑的轨迹规划与速度规划方法，其特征在于，所述步骤2.1包括，针对由航点P2与其相邻航点P1，P3组成的直线轨迹，在顶点处P2构造如下的两段贝塞尔转接函数：其中：c2＝(c1+4)(c1+1)，c3＝(c1+4)(c2+6)，η＝6c3cosβ/(c1+4)，式中：P1表示第一个航点，P2表示第二个航点，P3表示第三个航点，B1(u)表示第一段贝塞尔曲线，B1i表示第一段贝塞尔曲线的第i个控制点，u表示贝塞尔曲线参数，B2(u)表示第二段贝塞尔曲线，B2(3-i)表示第一段贝塞尔曲线的第3-i个控制点，T1表示上式中沿的单位向量，d表示贝塞尔曲线转接长度，η表示上式中确定的曲线设计参数，ud表示由所确定的单位向量，T2表示由所确定的单位向量，...

【专利技术属性】
技术研发人员：龚畅阳，董伟，盛鑫军，朱向阳，
申请(专利权)人：上海交通大学，
类型：发明
国别省市：上海,31