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

(19)中华人民共和国国家知识产权局

(12)发明专利申请

(10)申请公布号 (43)申请公布日 (21)申请号 201910127933.X

(22)申请日 2019.02.19

(71)申请人 上海交通大学

地址 200240 上海市闵行区东川路800号

(72)发明人 龚畅阳　董伟　盛鑫军　朱向阳　

(74)专利代理机构 上海旭诚知识产权代理有限

公司 31220

代理人 郑立

(51)Int.Cl.

G05D 1/10(2006.01)

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

时计算的进行。权利要求书3页 说明书8页 附图3页CN 109839953 A 2019.06.04

C N 109839953

A

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 2与其相邻航点P 1，P 3组成的直线轨迹，在顶点处P 2构

造如下的两段贝塞尔转接函数：

其中：

c 2＝(c 1+4)(c 1+1)，c 3＝(c 1+4)(c 2+6)，η＝6c 3cos β/(c 1

+4)，

式中：P 1表示第一个航点，P 2表示第二个航点，P 3表示第三个航点，B 1(u)表示第一段贝塞

权　利　要　求　书1/3页2CN 109839953 A