基于贝塞尔曲线的无人机转向飞行路径规划方法与流程

1.本发明涉及无人机路径规划技术领域，具体涉及一种基于贝塞尔曲线的无人机转向飞行路径规划方法。


背景技术：

2.由于惯性强，空气阻力弱的原因，无人机无法在空中完成角度明确的变向，任何变向的动作，只要当前航向角度与目标航向角度的差值不为0，都需要通过划过一段圆弧的方式完成变向。以如下工况为例，假设无人机在变向之前的状态为匀速直线运动，此时速度是一个常数v1，加速度是0；经过90
°
变向，假设这段圆弧的速度是一个常数v2，向心加速度是一个常数a2；在变向之后，无人机同样要回到之前的状态，即速度是一个常数v1，加速度是0。在这个过程中，无人机的速度变化：从匀速直线运动的一个恒定速度v1到圆周运动的另一个恒定速度v2，再回到匀速直线运动的一个恒定速度v1。这个过程是不连续的。同理，对于无人机的加速度而言，同样是不连续的。
3.对运动中的无人机而言，一切速度和加速度的变化，都由改变马达的转速来完成，而马达的转速是由通过马达的电压和电流控制的，而电荷的运动是一个连续的过程。因此，马达转速的调整也必须是连续的。如果即使像前文中的例子那样规划无人机的变向，会产生两次速度和加速度不连续的情况，这种情况是无人机的马达无法执行的，导致无人机的稳定性变差。更何况实际应用中，无人机要面临的控制问题较复杂，加速度和速度都会随时变化。
4.针对上述问题，现有主要基于b样条基函数进行路径规划。b样条基函数是经过一系列给定点的光滑曲线，该曲线具有连续的、曲率变化均匀的特点。但在实际工业应用场景中，常有目标曲线复杂的情况，此时需要的控制点变多且需要的计算量变大。基于b样条基函数进行路径规划的方法虽然可行，但是是低效率的。另外对于b样条基函数，改变其中一个控制点，那么整条曲线都会随之改变。因此该方法同样有稳定性较差的缺点。


技术实现要素：

5.本发明的目的在于提供一种基于贝塞尔曲线的无人机转向飞行路径规划方法，以解决无人机在转向过程中产生的速度与加速度的函数不连续不可导的问题，进行转向路径规划。
6.为实现上述目的，本发明提供了一种基于贝塞尔曲线的无人机转向飞行路径规划方法，包括：
7.采集无人机实际转向飞行路径数据，根据飞行动作对实际转向飞行路径进行分类；
8.基于贝塞尔曲线对实际转向飞行路径进行规划，生成目标转向飞行路径，对实际转向飞行路径进行修正。
9.优选的，无人机的飞行动作包括直线飞行与曲线飞行，将直线飞行与曲线飞行动
作结合，确定无人机实际转向飞行路径分类为：直线飞行路径、先直线飞行后曲线飞行路径、先曲线飞行后直线飞行路径、先曲线飞行中间直线飞行后曲线飞行路径、两段连接的曲线飞行路径。
10.优选的，根据无人机目标初始航行节点与目标转向航行节点之间的距离、目标最终航行节点与目标转向航行节点之间的距离确定目标安全半径，并以目标转向航行节点为圆心，确定规划曲线的目标安全范围；根据目标初始航行节点、目标最终航行节点分别与目标转向航行节点之间的连线与目标安全范围的交点，以及目标初始航行节点、目标转向航行节点、目标最终航行节点，确定规划所需的贝塞尔曲线，生成目标转向飞行路径。
11.优选的，根据目标初始航行节点、目标最终航行节点分别与目标转向航行节点之间的连线与目标安全范围的交点，以及目标初始航行节点、目标转向航行节点、目标最终航行节点，确定规划所需的贝塞尔曲线为：
12.p(t)＝p0b
0,4
(t) p1b
1,4
(t) p2b
2,4
(t) p3b
3,4
(t) p4b
4,4
(t)
13.＝(1
‑
t)4p0 4t(1
‑
t)3p1 6t2(1
‑
t)2p2 4t3(1
‑
t)p3 t4p414.其中，p0、p1、p2、p3、p4表示规划贝塞尔曲线的控制点，b
0,4
(t)、b
1,4
(t)、b
2,4
(t)、b
3,4
(t)、b
4,4
(t)为对应的伯恩斯坦多项式。
15.优选的，若确定的规划所需的贝塞尔曲线与目标安全范围不相切，修正贝塞尔曲线，使修正后的贝塞尔曲线与目标安全范围相切。
16.优选的，若为直线飞行路径，则目标初始航行节点、目标转向航行节点、目标最终航行节点在同一水平度上，确定目标安全半径为0，目标初始航行节点、目标转向航行节点之间的距离为目标安全范围，根据目标初始航行节点、目标转向航行节点、目标最终航行节点确定的直线为相应规划的目标转向飞行路径。
17.优选的，若为先直线飞行后曲线飞行路径，则根据确定的贝塞尔曲线，确定入弧航行节点，计算目标初始航行节点与入弧航行节点之间的距离；根据目标初始航行节点与入弧航行节点之间的距离、以及贝塞尔曲线的长度，确定目标安全范围；
18.若为先曲线飞行后直线飞行路径，则根据确定的贝塞尔曲线，确定入弧航行节点，计算目标转向航行节点与入弧航行节点之间的距离；根据目标转向航行节点与入弧航行节点之间的距离、以及贝塞尔曲线的长度，确定目标安全范围；
19.若为先曲线飞行中间直线飞行后曲线飞行路径，则根据确定的贝塞尔曲线，计算两段贝塞尔曲线的入弧航行节点之间的距离；根据计算两段贝塞尔曲线的入弧航行节点之间的距离、以及贝塞尔曲线的长度，确定目标安全范围；
20.若为曲线飞行路径，则根据确定的两段贝塞尔曲线的长度，确定目标安全范围。
21.优选的，对转向飞行路径的任意航行节点，计算受下一个航行节点限制的当前航行节点的初始的最大速度；根据初速度、末速度、初始的最大速度，判断无人机的状态，修正最大目标速度，预测变速所需的目标航行路程与目标航行时间，规划速度、加速度与航行时间的关系，进行路径时间规划。
22.优选的，若初速度＝末速度＜初始的最大速度，将初速度加速到初始的最大速度所需距离与初始的最大速度减速到末速度所需距离的和，与确定的目标安全范围进行比较：
23.若初速度加速到初始的最大速度所需距离与初始的最大速度减速到末速度所需
距离的和超出目标安全范围，根据初速度与初始的最大速度对最大目标速度进行修正，使初速度加速到修正的最大目标速度所需距离与修正的最大目标速度减速到末速度所需距离的和未超出目标安全范围；
24.若初始的最大速度＜末速度＜初速度或初始的最大速度＜初速度＜末速度，将初速度减速至初始的最大速度的距离与确定的目标安全范围进行比较，若初速度减速至初始的最大速度的距离超出目标安全范围，则根据初速度与初始的最大速度对最大目标速度进行修正，使初速度减速至修正的最大目标速度所需距离未超出目标安全范围；
25.若末速度＜初始的最大速度＜初速度，将初速度减速至初始的最大速度的距离与初始的最大速度减速至末速度的距离之和、及初速度减速至末速度的距离分别与确定的目标安全范围进行比较：
26.若初速度减速至初始的最大速度的距离与初始的最大速度减速至末速度的距离之和超出目标安全范围，且初速度减速至末速度的距离未超出目标安全范围，且末速度近似0，则根据初速度与初始的最大速度对最大目标速度进行修正，使初速度减速至修正的最大目标速度的距离与修正的最大目标速度减速至末速度的距离之和未超出目标安全范围；
27.若末速度＜初速度＜初始的最大速度，将初速度加速至初始的最大速度的距离与初始的最大速度减速至末速度的距离之和、及初速度减速至末速度的距离分别与确定的目标安全范围进行比较：
28.若初速度加速至初始的最大速度的距离与初始的最大速度减速至末速度的距离之和超出目标安全范围、且初速度减速至末速度的距离未超出目标安全范围，则根据初速度与初始的最大速度对最大目标速度进行修正，使初速度加速至修正的最大目标速度的距离与修正的最大目标速度减速至末速度的距离之和未超出目标安全范围。
29.优选的，当前航行节点为转向航行节点时，若初角速度＝末角速度＜初始的最大目标角速度，若初角速度加速至初始的最大目标角速度的对应的角速度变化值与初始的最大目标角速度减速至末角速度对应的角速度变化值之和超出初始航向角与最终航向角之间的差值，则根据初角速度与初始的最大目标角速度对最大目标角速度进行修正，使初角速度加速至修正后的最大目标角速度的对应的角速度变化值与修正后的最大目标角速度减速至末角速度对应的角速度变化值之和超出初始航向角与最终航向角之间的差值。
30.本发明方法具有如下优点：
31.本发明提供了一种基于贝塞尔曲线的无人机转向飞行路径规划方法，以解决无人机在转向过程中产生的速度与加速度的函数不连续不可导的问题，进行转向路径规划。具体为，将直线飞行与曲线飞行动作结合，确定无人机实际转向飞行路径分类为：直线飞行路径、先直线飞行后曲线飞行路径、先曲线飞行后直线飞行路径、先曲线飞行中间直线飞行后曲线飞行路径、两段连接的曲线飞行路径。然后通过确定规划曲线的目标安全范围、控制点，规划贝塞尔曲线，进行向心加速度规划；然后根据初速度、末速度、初始的最大速度，判断无人机的状态，预测变速所需的目标航行路程与目标航行时间，规划速度、加速度与航行时间的关系，进行路径时间规划。该路径规划方法解决了在转向过程中产生的速度与加速度的函数不连续不可导的技术难点，具有计算量小，稳定性高的优点。
附图说明
32.图1为转角的路径规划原理；
33.图2为本发明的无人机转向路径规划方法总体原理图；
34.图3为本发明的无人机转向路径规划方法整体流程图；
35.图4为四阶贝塞尔曲线规划原理图。
具体实施方式
36.以下实施例用于说明本发明，但不用来限制本发明的范围。
37.对于无人机转向飞行路径的规划，在改变速度和方向的过程中，如图1中，使无人机自a点出发，经过b点，到达c点，最理想的条件为：使无人机自a点飞到b点，在逼近b点的过程中减速，在到达b点的时候速度降为零，从静止状态开始加速，自b点飞到c点。但该
38.理想条件的实现过程是非常低效的，2要避免先减速到零再加速的过程，因此需要在b点周围确定安全范围，只要无人机在路径上能够经过这个范围，就视为路过了b点。因此，理想的规划路径，是一条与b点确定的安全范围区域相切的弧线。因此，本实施例中考虑采用贝塞尔曲线进行路径规划，给定空间n 1个点的位置矢量p
i
,i＝0，1，2，...，n，n次贝塞尔曲线表示为：
[0039][0040]
贝塞尔函数中作为参数项的伯恩斯坦多项式表示为：
[0041][0042]
如图2、图3所示，本发明提供的基于贝塞尔曲线的无人机转向飞行路径规划方法，具体为：
[0043]
(1)采集无人机实际转向飞行路径数据，根据飞行动作对实际转向飞行路径进行分类：无人机的飞行动作包括直线飞行与曲线飞行，将直线飞行与曲线飞行动作结合，确定无人机实际转向飞行路径分类为：直线飞行路径、先直线飞行后曲线飞行路径、先曲线飞行后直线飞行路径、先曲线飞行中间直线飞行后曲线飞行路径、曲线飞行路径。
[0044]
(2)基于贝塞尔曲线对实际转向飞行路径进行规划，生成目标转向飞行路径，对实际转向飞行路径进行修正，确定目标规划路径：
[0045]
①
根据无人机目标初始航行节点与目标转向航行节点之间的距离、目标最终航行节点与目标转向航行节点之间的距离确定目标安全半径，并以目标转向航行节点为圆心，确定规划曲线的目标安全范围；根据目标初始航行节点、目标最终航行节点分别与目标转向航行节点之间的连线与目标安全范围的交点，以及目标初始航行节点、目标转向航行节点、目标最终航行节点，确定规划所需的贝塞尔曲线为：
[0046]
p(t)＝p0b
0,4
(t) p1b
1,4
(t) p2b
2,4
(t) p3b
3,4
(t) p4b
4,4
(t)
[0047]
＝(1
‑
t)4p0 4t(1
‑
t)3p1 6t2(1
‑
t)2p2 4t3(1
‑
t)p3 t4p4[0048]
其中，p0、p1、p2、p3、p4为规划贝塞尔曲线的控制点，b
0,4
(t)、b
1,4
(t)、b
2,4
(t)、b
3,4
(t)、b
4,4
(t)为对应的伯恩斯坦多项式。
[0049]
对于贝塞尔曲线的确定如图4所示，图中点o代表目标初始航行节点、点b代表目标
最终航行节点、点a代表目标转向航行节点，点m、n表示目标最终航行节点分别与目标转向航行节点之间的连线与目标安全范围的交点。对于目标安全半径，本实施例中规定为如果目标安全半径大于min(0.25oa,0.25ab)，更新安全半径为之前的一半；然后，确定点c、d的位置，使ac＝ad＝min(0.5oa,0.5ab)；确认点m、n的位置，为目标安全范围与oa、ab的交点，则确定点c、m、n、a、d为规划四阶贝塞尔曲线的五个控制点，可确定对应的贝塞尔曲线。
[0050]
②
若确定的规划所需的贝塞尔曲线与目标安全范围至少有两交点，将目标初始航行节点、目标最终航行节点分别与目标转向航行节点之间的连线与目标安全范围的交点外延，修正贝塞尔曲线，使修正后的贝塞尔曲线与目标安全范围相切。本实施例中将点m外延至现有c、m的平均值，将点n外延至现有d、n的平均值，重新根据c、m、n、a、d修正贝塞尔曲线。
[0051]
若确定的规划所需的贝塞尔曲线与目标安全范围不相切，修正贝塞尔曲线，使修正后的贝塞尔曲线与目标安全范围相切。本实施例中将点c收缩至现有c,m的平均值，将点d收缩至现有d,n的平均值，重新根据c、m、n、a、d修正贝塞尔曲线。
[0052]
③
对于目标安全范围的确定，若为直线飞行路径，则目标初始航行节点、目标转向航行节点、目标最终航行节点在同一水平度上，确定目标安全半径为0，目标初始航行节点、目标转向航行节点之间的距离为目标安全范围，根据目标初始航行节点、目标转向航行节点、目标最终航行节点确定的直线为相应规划的目标转向飞行路径。
[0053]
若为先直线飞行后曲线飞行路径，则根据确定的贝塞尔曲线，确定入弧航行节点，计算目标初始航行节点与入弧航行节点之间的距离；根据目标初始航行节点与入弧航行节点之间的距离、以及贝塞尔曲线的长度，确定目标安全范围；本实施例中具体将目标初始航行节点与入弧航行节点之间的距离 1/2贝塞尔曲线的长度，确定目标安全范围。
[0054]
若为先曲线飞行后直线飞行路径，则根据确定的贝塞尔曲线，确定入弧航行节点，计算目标转向航行节点与入弧航行节点之间的距离；根据目标转向航行节点与入弧航行节点之间的距离、以及贝塞尔曲线的长度，确定目标安全范围。本实施例中具体将目标转向航行节点与入弧航行节点之间的距离 1/2贝塞尔曲线的长度，确定目标安全范围。
[0055]
若为先曲线飞行中间直线飞行后曲线飞行路径，则根据确定的贝塞尔曲线，计算两段贝塞尔曲线的入弧航行节点之间的距离；根据计算两段贝塞尔曲线的入弧航行节点之间的距离、以及贝塞尔曲线的长度，确定目标安全范围；本实施例中具体将两段贝塞尔曲线的入弧航行节点之间的距离 1/2贝塞尔曲线的长度，确定目标安全范围。
[0056]
若为曲线飞行路径，则两段贝塞尔曲线相连，根据确定的两段贝塞尔曲线的长度，确定目标安全范围。本实施例中将两段贝塞尔曲线长度的一半相加确定目标安全范围。
[0057]
④
在路径规划的过程中，对任意航行节点，需要对末速度进行调整，使无人机在这个末速度的条件下可以顺利通过所有航行节点。因此，对转向飞行路径的任意航行节点，进一步计算受下一个航行节点限制的当前航行节点的初始的最大速度；根据初速度、末速度、初始的最大速度，判断无人机的状态，修正最大目标速度，预测变速所需的目标航行路程与目标航行时间，规划速度、加速度与航行时间的关系，进行路径时间规划。即：
[0058]
计算末速度和初速度的差，为速度的变化量：
[0059]
δv＝v
end
‑
v
start
[0060]
则位移s、速度v、加速度a、加加速度关于航行时间t的公式：
[0061][0062][0063][0064]
加速度a存在贝塞尔曲线理论计算的最大值a
target
，加加速度是一个定值计算加加速时间，即从加速度的最小值0加加速到理论的最大值a
target
，不考虑加速度的正负。
[0065][0066]
假设匀加速运动，即加速度一直保持在a
target
，假设加速时间是t0，计算最小速度变化量
[0067]
min(δv)作为比较的临界值。
[0068]
min(δv)＝a
target
*t
target
[0069]
若δv≤min(δv)，说明加速度没有一开始就保持在理论设计的最大值a
target
，因此说明存在加加速过程和减加速过程。相反，如果δv＞min(δv)，说明运动过程不仅有加加速段和减加速段，还有匀加速段。
[0070]
在最小速度变化量min(δv)的计算过程中，速度v始终按照最大值a
target
的加速度在[0,t
target
]的范围上加速，最终速度的变化量达到min(δv)，即：
[0071]
min(δv)＝a
target
t
target
[0072]
在存在加加速的过程中，速度v始终按照的加加速度在[0,t
target
]，此时加速度a是一个关于加加速度的表达式，即：
[0073][0074]
此时速度v是关于加速度a(t)在时间[0,t
target
]上的积分，加速度持续上升，直到到达最大值a
target
，此时有：
[0075][0076]
因此，说明在加加速的过程结束之后，速度v最多只能达到min(δv)的一半，因此在经过同样的减加速过程之后，速度v只能小于等于min(δv)。
[0077]
然而，无人机并不是总能够持续加加速的过程直到加速度到达最大值a
target
，因此
直接把加加速时间t0假设为t
target
是不合适的。加加速时间t0可以通过公式计算，假设这段时间内的加速度变化量是δa，即：
[0078][0079][0080][0081]
因此，存在如下几种情况以及对应的公式是：
[0082]
①
、δv＞0，δv≤min(δv)
[0083]
在加加速段，起始速度是v
start
，无人机按照规定的加加速度在时间范围[0,t0]内加速。
[0084][0085][0086]
在减加速段，
[0087][0088]
②
、δv＞0，δv＞min(δv).
[0089]
在加加速段，无人机的加加速时间t0在[0,t
target
]范围内，以加加速，直到加速度达到a
target
。因此有
[0090][0091][0092]
在匀加速段，加速时间即是速度超过min(δv)的时间，因此匀加速时间t1可求，在这段时间内的速度和距离可求。
[0093][0094]
v1＝v
start
a
target
t1[0095][0096]
在减加速段，减加速时间t2等同加加速时间t0，距离s2表示为:
[0097][0098]
③
、至于δv＜0，δv≤min(δv)与δv＜0，δv＞min(δv)时，为减速运动，加速度方向是相反的，与前两种情况雷同，本技术不在具体描述。
[0099]
若初速度＝末速度＜初始的最大速度，将初速度加速到初始的最大速度所需距离
与初始的最大速度减速到末速度所需距离的和，与确定的目标安全范围进行比较：
[0100]
若初速度加速到初始的最大速度所需距离与初始的最大速度减速到末速度所需距离的和超出目标安全范围，即无人机不能达到初始的最大速度，则根据初速度与初始的最大速度对最大目标速度进行修正，使初速度加速到修正的最大目标速度所需距离与修正的最大目标速度减速到末速度所需距离的和未超出目标安全范围；本实施例中采用二分法修正，将最大目标速度修正为初速度与初始的最大速度之和的一半。若初速度加速到初始的最大速度所需距离与初始的最大速度减速到末速度所需距离的和未超出目标安全范围，则表示无人机能够加速到初始的最大速度，并在初始的最大速度保持匀速飞行一段时间，然后进入减速阶段。
[0101]
若初始的最大速度＜末速度＜初速度或初始的最大速度＜初速度＜末速度，将初速度减速至初始的最大速度的距离与确定的目标安全范围进行比较，若初速度减速至初始的最大速度的距离超出目标安全范围，即无人机不能达到初始的最大速度，需要进行速度修正，则根据初速度与初始的最大速度对最大目标速度进行修正，使初速度减速至修正的最大目标速度所需距离未超出目标安全范围；本实施例中采用二分法修正，将最大目标速度修正为初速度与初始的最大速度之和的一半。
[0102]
若末速度＜初始的最大速度＜初速度，将初速度减速至初始的最大速度的距离与初始的最大速度减速至末速度的距离之和、及初速度减速至末速度的距离分别与确定的目标安全范围进行比较：
[0103]
若初速度减速至初始的最大速度的距离与初始的最大速度减速至末速度的距离之和超出目标安全范围，且初速度减速至末速度的距离未超出目标安全范围，且末速度近似0，则根据初速度与初始的最大速度对最大目标速度进行修正，使初速度减速至修正的最大目标速度的距离与修正的最大目标速度减速至末速度的距离之和未超出目标安全范围；本实施例中采用二分法修正，将最大目标速度修正为初速度与初始的最大速度之和的一半。
[0104]
若末速度＜初速度＜初始的最大速度，将初速度加速至初始的最大速度的距离与初始的最大速度减速至末速度的距离之和、及初速度减速至末速度的距离分别与确定的目标安全范围进行比较：
[0105]
若初速度加速至初始的最大速度的距离与初始的最大速度减速至末速度的距离之和超出目标安全范围、且初速度减速至末速度的距离未超出目标安全范围，则根据初速度与初始的最大速度对最大目标速度进行修正，使初速度加速至修正的最大目标速度的距离与修正的最大目标速度减速至末速度的距离之和未超出目标安全范围。本实施例中采用二分法修正，将最大目标速度修正为初速度与初始的最大速度之和的一半。
[0106]
当前航行节点为转向航行节点时，若初角速度＝末角速度＜初始的最大目标角速度，若初角速度加速至初始的最大目标角速度的对应的角速度变化值与初始的最大目标角速度减速至末角速度对应的角速度变化值之和超出初始航向角与最终航向角之间的差值，则根据初角速度与初始的最大目标角速度对最大目标角速度进行修正，使初角速度加速至修正后的最大目标角速度的对应的角速度变化值与修正后的最大目标角速度减速至末角速度对应的角速度变化值之和超出初始航向角与最终航向角之间的差值。
[0107]
(3)判断当前无人机状态，计算已经飞过的航行节点的距离，确定当前无人机所处
航行节点的区间位置，执行上述路径规划过程。
[0108]
综上，本发明针对无人机在转向过程中产生的速度与加速度的函数不连续不可导的问题，基于贝塞尔曲线提供了一种无人机转向飞行路径规划方法，进行转向路径规划。通过将直线飞行与曲线飞行动作结合，确定无人机实际转向飞行路径分类为：直线飞行路径、先直线飞行后曲线飞行路径、先曲线飞行后直线飞行路径、先曲线飞行中间直线飞行后曲线飞行路径、曲线飞行路径；然后通过确定规划曲线的目标安全范围、控制点，规划贝塞尔曲线，进行向心加速度规划；然后根据初速度、末速度、初始的最大速度，判断无人机的状态，预测变速所需的目标航行路程与目标航行时间，规划速度、加速度与航行时间的关系，进行路径时间规划。该路径规划方法解决了在转向过程中产生的速度与加速度的函数不连续不可导的技术难点，具有计算量小，稳定性高的优点。
[0109]
虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。