基于贝塞尔曲线转接的无人机轨迹平滑方法.pdf

本发明提供了一种基于贝塞尔曲线转接的无人机轨迹平滑方法，包括：根据已知直线航线段构造贝塞尔转接函数，快速建立满足曲率连续的飞行轨迹几何特征；以直线航线段的长度以及允许的最大轨迹平滑误差作为约束，建立并求解各贝塞尔曲线平滑转接长度的最优化问题；确定最大速度、加速度以及跃度约束，基于贝塞尔函数的性质确定转接段的最大飞行速度；对所有直线段进行S型运动规划，确定各直线段加减速时间；迭代搜索并规划各段飞行速度，保证运动学相容性；进行实时插补，完成飞行轨迹生成。本发明能够在保证计算效率的前提下，大幅提升无人机的飞行性能。

1.一种基于贝塞尔曲线转接的无人机轨迹平滑方法，其特征在于，包括如下步骤：
步骤1：根据已知直线航线段构造贝塞尔转接函数，建立满足曲率连续的飞行轨迹几何
特征；
步骤2：以直线航线段长度以及允许的最大轨迹平滑误差作为约束，建立并求解各贝塞
尔曲线平滑转接参数的最优化问题；
步骤3：确定最大速度、加速度以及跃度约束，基于贝塞尔函数的性质确定转接段的最
大飞行速度；
步骤4：对所有直线段进行S型运动规划，确定各直线段加减速时间；
步骤5：迭代搜索并规划各段飞行速度，保证运动学相容性；
步骤6：对飞行轨迹进行实时插补，得到参考飞行轨迹。
2.根据权利要求1所述的基于贝塞尔曲线转接的无人机轨迹平滑方法，其特征在于，所
述步骤1包括：已知航点P1，P2，P3组成的直线轨迹，针对航点P2构造如下的贝塞尔转接函数：
$B_1\left(u\right)=\underset{i=0}{\overset{3}{\Σ}}\left(\begin{array}{c}3\\ i\end{array}\right)B_{1i}{u}^i{(1-u)}^{3-i}$
$B_2\left(u\right)=\underset{i=0}{\overset{3}{\mathrm{\Σ}}}\left(\begin{array}{c}3\\ 3-i\end{array}\right)B_{2\left(3-i\right)}{u}^i{(1-u)}^{3-i}$
其中：
$c_1=2(\sqrt{6}-1)/5,c_2=(c_1+4)(c_1+1),c_3=(c_1+4)/(c_2+6),\mathrm{\η}=6c_{3}cos\mathrm{\β}/(c_1+4),$

式中：P1表示第一个航点，P2表示第二个航点，P3表示第三个航点，B1(u)表示第一段贝塞
尔曲线，B1i表示第一段贝塞尔曲线的第i个控制点，u表示贝塞尔曲线参数，B2(u)表示第二
段贝塞尔曲线，B2(3-i)表示第一段贝塞尔曲线的第3-i个控制点，T1表示上式中沿的方向
向量，d表示贝塞尔曲线转接长度，η表示上式中确定的曲线设计参数，ud表示由所确
定的方向向量，T2表示由所确定的方向向量，表示由第一与第二个航点所确定的方
向向量，表示由第二与第三个航点与确定的方向向量，表示贝塞尔曲线中间控
制点所确定的向量，||·||表示二次范数运算，β为T1与ud之间的夹角，c1、c2、c3分别表示三
个中间变量。
3.根据权利要求1所述的基于贝塞尔曲线转接的无人机轨迹平滑方法，其特征在于，所
述步骤2包括：
步骤2.1：当给定的曲线平滑误差为∈max，则贝塞尔曲线转接长度需满足如下条件：
$d\≤\frac{{\∈}_{max}}{(1-c_1{c}_3-c_3)sin\mathrm{\β}}=c_4{\∈}_{max}\csc \mathrm{\β};$
对于每一贝塞尔转接函数，最大曲率κmax为：
${\mathrm{\κ}}_{max}=\frac{c_{5}sin\mathrm{\β}}{d{\cos }^2\mathrm{\β}};$
式中：c4＝1/(1-c_1c_3-c_3)，c5＝(c2+4)2/(54c3)；c4、c5均为中间变量；
步骤2.2：假设有n+2个航点的飞行任务，共有n+1个直线航段，记每条航线段长度为li，
构建如下形式的转接线段长度最优问题：
$\begin{array}{cc}min& -\mathrm{\ξ}n(1-\mathrm{\λ})-\mathrm{\λ}\underset{i=1}{\overset{n}{\Σ}}{d}_i\frac{{\cos }^2{\mathrm{\β}}_i}{c_5{\mathrm{sin\β}}_i}\end{array}$
s.t.di≤c4∈cscβi
$\mathrm{\ξ}-d_i\frac{{\cos }^2{\mathrm{\β}}_i}{c_5{\mathrm{sin\β}}_i}\≤0$
di+di-1≤li-1
d1≤l0
dn≤ln+1
di≥0
ξ≥0
式中：ξ表示最小曲率半径，λ为设计参数，λ设为0.5，di表示第i段曲线的转接长度，βi表
示第i段航线中T1与ud之间的夹角，di-1表示第i-1段曲线的转接长度，li-1表示第i段航线的
直线长度，l0表示第1段航线的直线长度。
4.根据权利要求1所述的基于贝塞尔曲线转接的无人机轨迹平滑方法，其特征在于，所
述步骤3包括：对于给定的曲线平滑误差∈max以及最大加速度Vmax，确定贝塞尔曲线段的最
大飞行速度Vcm，计算公式如下：
$V_{cm}=\max \left(\sqrt{\frac{A_{\max }}{{\mathrm{\κ}}_{\max }}},V_{\max }\right)=\max \left(\sqrt{\frac{A_{\max }d{\cos }^2\mathrm{\β}}{c_5\sin \mathrm{\β}}},V_{\max }\right);$
式中：Amax表示最大加速度，max(a,b)表示运算。
5.根据权利要求1所述的基于贝塞尔曲线转接的无人机轨迹平滑方法，其特征在于，所
述步骤4包括：对所有直线段进行S型运动规划，确定各直线段加减速时间，具体公式如下：
$\begin{array}{c}s\left(t\right)=F_{k}t+2.5\mathrm{\Δ}V\frac{t^4}{t_e^3}-3\mathrm{\Δ}V\frac{t^5}{t_e^4}+\mathrm{\Δ}V\frac{t^6}{t_e^5}\\ v\left(t\right)=F_k+10\mathrm{\Δ}V\frac{t^3}{t_e^3}-15\mathrm{\Δ}V\frac{t^4}{t_e^4}+6\mathrm{\Δ}V\frac{t^5}{t_e^5}\\ a\left(t\right)=30\mathrm{\Δ}V\frac{t^2}{t_e^3}-60\mathrm{\Δ}V\frac{t^3}{t_e^4}+30\mathrm{\Δ}V\frac{t^4}{t_e^4}\\ j\left(t\right)=60\mathrm{\Δ}V\frac{t}{t_e^3}-180\mathrm{\Δ}V\frac{t^3}{t_e^4}+120\mathrm{\Δ}V\frac{t^3}{t_e^5}\end{array};$
式中：s(t)表示曲线路径，Fk表示第k曲线段飞行速度规划，k的取值范围为1～n(n为总
航线段数)，ΔV表示曲线段起止点速度差，te表示加减速时间，t表示时间，v(t)表示飞行速
度，a(t)表示飞行加速度，j(t)表示飞行跃度。
6.根据权利要求5所述的基于贝塞尔曲线转接的无人机轨迹平滑方法，其特征在于，所
述加减速时间te需满足以下条件限制条件：
$t_e=max\left(t_e^{A_{\max }},t_e^{J_{\max }}\right)$
$t_e^{A_{\max }}=\frac{15}{8}\frac{\left|\mathrm{\Δ}V\right|}{A_{max}}$
$t_e^{J_{max}}=\sqrt{\frac{45}{8}\frac{\left|\mathrm{\Δ}V\right|}{J_{max}}}$
式中：表示受限加速度情况下所允许最大转接时间，表示受限跃度情况下所允
许最大转接时间，Jmax表示所允许最大跃度。
7.根据权利要求1所述的基于贝塞尔曲线转接的无人机轨迹平滑方法，其特征在于，所
述步骤5包括：
步骤5.1：将所有线段速度设为0，即Fi＝0，对每一段设置运动学违合变量，所述违合变
量用于判定生成轨迹是否违背运动学相容性，并初始化为qi＝0，设置速度搜索增量Δv＝
0.01m/s；
步骤5.2：若qi＜1，其中i的取值范围为1～2n，则更新该段曲线速度为Fi+Δv；如果Fi违
背动力学约束Amax，Jmax或Vcm中的任一项，则取消速度增量；执行步骤5.3；若qi＞1则执行步
骤5.4；
步骤5.3：若第i段飞行曲线的速度规划违背如下的运动学约束条件：
$l_k-(\frac{F_{2k-1}+F_{2k}}{2}{t}_{e,2k-1}+\frac{F_{2k}+F_{2k+1}}{2}{t}_{e,2k})\≥0,$
则将第i段的运动学违合变量qi的值自增1，并撤消速度增量；式中：lk表示第k段航线的
直线长度，te,2k-1表示第k段航线起始处的转接时间，te,2k表示第k段航线末尾处的转接时
间，k的取值范围为1～n，n为总航线段数；
步骤5.4：撤消相邻航线段的速度增量，且当i＞1时，令qi-1的值自增1，当i＜n时，令qi+1
的值自增1；
步骤5.5：返回执行步骤5.2，直到对于任意i值，均满足qi＞0时，执行步骤6。
8.根据权利要求1所述的基于贝塞尔曲线转接的无人机轨迹平滑方法，其特征在于，所
述步骤6中的实时插补公式如下：
$C_l\left(t_{k,i}\right)=P_{(k-1)0}+\frac{P_{k0}-P_{(k-1)0}}{\left|\right|P_{k0}-P_{(k-1)0}\left|\right|}s\left({\mathrm{iT}}_s\right)$
式中：Cl(tk,i)表示所生成的轨迹时变方程，P(k-1)0、Pk0分别表示直线段的两个端点，s
(iTs)表示步骤4得到的i个Ts周期内直线段轨迹，Ts表示插补周期；
对于贝塞尔曲线，由于无人机为匀速飞行，采用以下实时插补方式：
$\mathrm{\Δ}u=\frac{1}{L_B}\mathrm{\Δ}s$
式中：Δs＝FkTs，Δu为每次插补的目标参变量增量，LB为贝塞尔曲线长度。

基于贝塞尔曲线转接的无人机轨迹平滑方法

技术领域

本发明涉及无人机技术领域，具体地，涉及一种基于贝塞尔曲线转接的无人机轨
迹平滑方法。

背景技术

在无人机全自主飞行过程当中，合理的轨迹生成策略是保障无人机飞行性能的关
键技术之一。当前无人机自主飞行所采用轨迹生成策略呈现出学术研究与具体应用分化的
态势。一方面，学术界提出了多类基于多项式、样条拟合的全局最优飞行轨迹生成方法；另
一方面，实际自主飞行中往往仍采用简单的直线连接方法进行快速轨迹生成。上述研究与
应用方法差异显著的原因在于实时自主飞行过程中，无人机通常不太可能以数秒到数十秒
不等的计算时间代价来求取一段相对较短航程的全局最优飞行轨迹。然而，目前所广泛采
用的直线连接方式无法保证高效的自主飞行性能，甚至飞行方式本身与无人机的动力学性
能并不相容。因此，开发针对实际应用开发高计算性能的轨迹生成算法，对无人机实现高性
能自主飞行并完成相应的作业任务，具有重要的理论与现实意义。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种基于贝塞尔曲线转接的无人机
轨迹平滑方法。

根据本发明提供的基于贝塞尔曲线转接的无人机轨迹平滑方法，包括如下步骤：

步骤1：根据已知直线航线段构造贝塞尔转接函数，建立满足曲率连续的飞行轨迹
几何特征；

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

步骤3：确定最大速度、加速度以及跃度约束，基于贝塞尔函数的性质确定转接段
的最大飞行速度；

步骤4：对所有直线段进行S型运动规划，确定各直线段加减速时间；

步骤5：迭代搜索并规划各段飞行速度，保证运动学相容性；

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

优选地，所述步骤1包括：已知航点P1，P2，P3组成的直线轨迹，针对航点P2构造如下
的贝塞尔转接函数：

$B_1\left(u\right)=\underset{i=0}{\overset{3}{\Σ}}\left(\begin{array}{c}3\\ i\end{array}\right){\mathrm{\β}}_{1i}{u}^i{(1-u)}^{3-i}$

$B_2\left(u\right)=\underset{i=0}{\overset{3}{\Σ}}\left(\begin{array}{c}3\\ 3-i\end{array}\right){\mathrm{\β}}_{2(3-i)}{u}^i{(1-u)}^{3-i}$

其中：

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表示由所确定的方向向量，表示由第一与第二个航点
所确定的方向向量，表示由第二与第三个航点与确定的方向向量，表示贝塞尔曲
线中间控制点所确定的向量，||·||表示二次范数运算，β为T1与ud之间的夹角，c1、c2、c3分
别表示三个中间变量。

优选地，所述步骤2包括：

步骤2.1：当给定的曲线平滑误差为∈max，则贝塞尔曲线转接长度需满足如下条
件：

$d\≤\frac{{\∈}_{max}}{(1-c_1{c}_3-c_3)sin\mathrm{\β}}=c_4{\∈}_{max}\csc \mathrm{\β};$

对于每一贝塞尔转接函数，最大曲率κmax为：

${\mathrm{\κ}}_{max}=\frac{c_{5}sin\mathrm{\β}}{d{\cos }^2\mathrm{\β}};$

式中：c4＝1/(1-c_1c_3-c_3)，c5＝(c2+4)2/(54c3)；c4、c5均为中间变量；

步骤2.2：假设有n+2个航点的飞行任务，共有n+1个直线航段，记每条航线段长度
为li，构建如下形式的转接线段长度最优问题：

$\begin{array}{cc}min& -\mathrm{\ξ}n(1-\mathrm{\λ})-\mathrm{\λ}\underset{i=1}{\overset{n}{\Σ}}{d}_i\frac{{\cos }^2{\mathrm{\β}}_i}{c_5{\mathrm{sin\β}}_i}\end{array}$

s.t.di≤c4∈cscβi

$\mathrm{\ξ}-d_r\frac{{\cos }^2{\mathrm{\β}}_i}{c_5{\mathrm{sin\β}}_i}\≤0$

di+di-1≤li-1

d1≤l0

dn≤ln+1

di≥0

ξ≥0

式中：ξ表示最小曲率半径，λ为设计参数，λ设为0.5，di表示第i段曲线的转接长
度，βi表示第i段航线中T1与ud之间的夹角，di-1表示第i-1段曲线的转接长度，li-1表示第i段
航线的直线长度，l0表示第1段航线的直线长度。

优选地，所述步骤3包括：对于给定的曲线平滑误差∈max以及最大加速度Vmax，确定
贝塞尔曲线段的最大飞行速度Vcm，计算公式如下：

$V_{cm}=max(\sqrt{\frac{A_{\max }}{{\mathrm{\κ}}_{max}}},V_{max})=max(\sqrt{\frac{A_{max}d{\cos }^2\mathrm{\β}}{c_{5}sin\mathrm{\β}}},V_{max});$

式中：Amax表示最大加速度，max(a,b)表示运算。

优选地，所述步骤4包括：对所有直线段进行S型运动规划，确定各直线段加减速时
间，具体公式如下：

$s\left(t\right)=F_{k}t+2.5\mathrm{\Δ}V\frac{t^4}{t_e^3}-3\mathrm{\Δ}V\frac{t^5}{t_e^4}+\mathrm{\Δ}V\frac{t^6}{t_e^5}$

$\begin{array}{c}v\left(t\right)=F_k+10\mathrm{\Δ}V\frac{t^3}{t_e^3}-15\mathrm{\Δ}V\frac{t^4}{t_e^4}+6\mathrm{\Δ}V\frac{t^5}{t_e^5}\\ a\left(t\right)=30\mathrm{\Δ}V\frac{t^2}{t_e^3}-60\mathrm{\Δ}V\frac{t^3}{t_e^4}+30\mathrm{\Δ}V\frac{t^4}{t_e^4}\end{array};$

$j\left(t\right)=60\mathrm{\Δ}V\frac{t}{t_e^3}-180\mathrm{\Δ}V\frac{t^3}{t_e^4}+120\mathrm{\Δ}V\frac{t^3}{t_e^5}$

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

优选地，所述加减速时间te需满足以下条件限制条件：

$t_e=max(t_e^{A_{max}},t_e^{J_{max}})$

$t_e^{A_{\max }}=\frac{15}{8}\frac{\left|\mathrm{\Δ}V\right|}{A_{max}}$

$t_e^{J_{max}}=\sqrt{\frac{45}{8}\frac{\left|\mathrm{\Δ}V\right|}{J_{max}}}$

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

优选地，所述步骤5包括：

步骤5.1：将所有线段速度设为0，即Fi＝0，对每一段设置运动学违合变量，所述违
合变量用于判定生成轨迹是否违背运动学相容性，并初始化为qi＝0，设置速度搜索增量Δ
v＝0.01m/s；

步骤5.2：若qi＜1，其中i的取值范围为1～2n，则更新该段曲线速度为Fi+Δv；如果
Fi违背动力学约束Amax，Jmax或Vcm中的任一项，则取消速度增量；执行步骤5.3；若qi＞1则执
行步骤5.4；

步骤5.3：若第i段飞行曲线的速度规划违背如下的运动学约束条件：

$l_k-(\frac{F_{2k-1}+F_{2k}}{2}{t}_{e,2k-1}+\frac{F_{2k}+F_{2k+1}}{2}{t}_{e,2k})\≥0,$

则将第i段的运动学违合变量qi的值自增1，并撤消速度增量；式中：lk表示第k段航
线的直线长度，te,2k-1表示第k段航线起始处的转接时间，te,2k表示第k段航线末尾处的转接
时间，k的取值范围为1～n，n为总航线段数；

步骤5.4：撤消相邻航线段的速度增量，且当i＞1时，令qi-1的值自增1，当i＜n时，
令qi+1的值自增1；

步骤5.5：返回执行步骤5.2，直到对于任意i值，均满足qi＞0时，执行步骤6。

优选地，所述步骤6中的实时插补公式如下：

$C_l\left(t_{k,i}\right)=P_{(k-1)0}+\frac{P_{k0}-P_{(k-1)0}}{\left|\right|P_{k0}-P_{(k-1)0}\left|\right|}s\left({\mathrm{iT}}_s\right)$

式中：Cl(tk,i)表示所生成的轨迹时变方程，P(k-1)0、Pk0分别表示直线段的两个端
点，s(iTs)表示步骤4得到的i个Ts周期内直线段轨迹，Ts表示插补周期；

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

$\mathrm{\Δ}u=\frac{1}{L_B}\mathrm{\Δ}s$

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

与现有技术相比，本发明具有如下的有益效果：

本发明提供的基于贝塞尔曲线转接的无人机轨迹平滑方法，依据多旋翼无人机的
动力学特性，采用贝塞尔曲线进行直线段间平滑连接以保障曲率连续，然后采用S型运动规
划以保障无人机飞行性能，能够在保证计算效率的前提下，尽可能提升无人机的飞行性能，
实现高性能自主飞行。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术
人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术
人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明
的保护范围。

根据本发明提供的基于贝塞尔曲线转接的无人机轨迹平滑方法，包括如下步骤：

步骤S1：针对由航点P1，P2，P3组成的直线轨迹，在顶点P2处实现如下形式的贝塞尔
曲线转接

$B_1\left(u\right)=\underset{i=0}{\overset{3}{\Σ}}\left(\begin{array}{c}3\\ i\end{array}\right)B_{1i}{u}^i{(1-u)}^{3-i},B_2\left(u\right)=\underset{i=0}{\overset{3}{\Σ}}\left(\begin{array}{c}3\\ 3-i\end{array}\right)B_{2(3-i)}{u}^i{(1-u)}^{3-i}---\left(1\right)$

其中

$\begin{array}{c}{B}_{10}=P_2-T_{1}d\\ B_{11}=P_2-T_1(1-c_1{c}_3)d\\ B_{12}=P_2-T_1(1-c_1{c}_3-c_3)d\\ B_{13}=B_{12}+{\mathrm{\ηdu}}_d\end{array},\begin{array}{c}{B}_{20}=P_2-T_{2}d\\ B_{21}=P_2-T_2(1-c_1{c}_3)d\\ B_{22}=P_2-T_2(1-c_1{c}_3-c_3)d\\ B_{23}=B_{22}+{\mathrm{\ηdu}}_d\end{array}$

且有c2＝(c1+4)(c1+1)，c3＝(c1+4)/(c2+6)，η＝6c3cosβ/(c1+4)，

步骤S2：对于给定的曲线平滑误差∈max，贝塞尔曲线转接长度需满足

$d\≤\frac{{\∈}_{max}}{(1-c_1{c}_3-c_3)sin\mathrm{\β}}=c_4{\∈}_{max}\csc \mathrm{\β}---\left(2\right)$

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

${\mathrm{\κ}}_{max}=\frac{c_{5}sin\mathrm{\β}}{d{\cos }^2\mathrm{\β}}---\left(3\right)$

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

$\begin{array}{cc}min& -\mathrm{\ξ}n(1-\mathrm{\λ})-\mathrm{\λ}\underset{i=1}{\overset{n}{\Σ}}{d}_i\frac{{\cos }^2{\mathrm{\β}}_i}{c_5{\mathrm{sin\β}}_i}\end{array}$

$\begin{array}{cc}s.t.& d_i\≤c_4\∈{\mathrm{csc\β}}_i\\ & \mathrm{\ξ}-d_i\frac{{\cos }^2{\mathrm{\β}}_i}{c_5{\mathrm{sin\β}}_i}\≤0\\ & d_i+d_{i+1}\≤l_{i-1}\\ & d_1\≤l_0\\ & d_n\≤l_{n+1}\\ & d_i\≥0\\ & \mathrm{\ξ}\≥0\end{array}---\left(4\right)$

该最优化问题为以x＝[d1,d2,…,dn,ξ]为决策变量的线性规划。

步骤S3：对于给定的曲线平滑误差∈max以及最大加速度Vmax，确定贝塞尔曲线段最
大飞行速度为

$V_{cm}=max(\sqrt{\frac{A_{max}}{{\mathrm{\κ}}_{max}}},V_{max})=max(\sqrt{\frac{L_{max}d{\cos }^2\mathrm{\β}}{c_{5}sin\mathrm{\β}}},V_{max})---\left(5\right)$

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

$\begin{array}{c}s\left(t\right)=F_{k}t+2.5\mathrm{\Δ}V\frac{t^4}{t_e^3}-3\mathrm{\Δ}V\frac{t^5}{t_e^4}+\mathrm{\Δ}V\frac{t^6}{t_e^5}\\ v\left(t\right)=F_k+10\mathrm{\Δ}V\frac{t^3}{t_e^3}-15\mathrm{\Δ}V\frac{t^4}{t_e^4}+6\mathrm{\Δ}V\frac{t^5}{t_e^5}\\ a\left(t\right)=30\mathrm{\Δ}V\frac{t^2}{t_e^3}-60\mathrm{\Δ}V\frac{t^3}{t_e^4}+30\mathrm{\Δ}V\frac{t^4}{t_e^4}\\ j\left(t\right)=30\mathrm{\Δ}V\frac{t}{t_e^3}-180\mathrm{\Δ}V\frac{t^3}{t_e^4}+120\mathrm{\Δ}V\frac{t^3}{t_e^5}\end{array}---\left(6\right)$

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

$t_e=max(t_e^{A_{max}},t_e^{J_{max}}),(t_e^{A_{max}}=\frac{15}{8}\frac{\left|\mathrm{\Δ}V\right|}{A_{\max }},t_e^{J_{max}}=\sqrt{\frac{45}{8}\frac{\left|\mathrm{\Δ}V\right|}{J_{max}}})---\left(7\right)$

步骤S5：按以下方法搜索各直线以及转接曲线段最大飞行速度：

a)初始化所有线段速度为0，即Fi＝0，对每一段设置运动学违合变量并初始化为qi
＝0，设置速度搜索增量Δv＝0.01m/s.

b)从i＝1到i＝2n，若qi＜1，则增加该段曲线速度Fi＝Fi+Δv。如果Fi违背动力学
约束Amax，Jmax或Vcm，则取消上述速度增量。

c)若第i段飞行曲线的速度规划违背以下运动学约束条件
则将第i段的运动学违合变量增加qi＝qi+1，并撤
消前述速度增量。

d)如果q_i＞1，则同时撤消相邻航线段速度增量，且当i＞1时，令q_{i-1}＝q_{i-
1}+1，当i＜n时，令q_{i+1}＝q_{i+1}+1。

e)重复步骤b)到d)，直到对于任意i，满足qi＞0。

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

$C_l\left(t_{k,i}\right)=P_{(k-1)0}+\frac{P_{k0}-P_{(k-1)0}}{\left|\right|P_{k0}-P_{(k-1)0}\left|\right|}s\left({\mathrm{iT}}_s\right)$

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

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

$\mathrm{\Δ}u=\frac{1}{L_B}\mathrm{\Δ}s$

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

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述
特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影
响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相
互组合。