一种引导无人机精准降落控制方法与流程

1.本发明涉及无人机控制技术领域，尤其涉及一种引导无人机精准降落控制方法。


背景技术：

2.随着无人机技术的发展，无人机在各行各业中得到了更加广泛的应用，如各种演艺活动和体育赛事的航拍、新闻拍摄、农药喷洒、物资配送、城市规划与测绘。由于无人机小巧灵活，方便大范围巡查的特点，无人机在旱情、洪水、地震和森林火灾等自然灾害的评估和监测方面都发挥着巨大的作用。为了能够更好的利用无人机，对无人机的精确控制尤为重要，目前无人机在指定地点降落时，存在降落速度慢，降落过程不平稳，降低了无人机的工作效率，容易造成无人机损坏的问题。


技术实现要素：

3.本发明的目的在于提供一种引导无人机精准降落控制方法，以解决现有技术中控制无人机在指定地点降落时，存在降落速度慢，降落过程不平稳，降低了无人机的工作效率，容易造成无人机损坏的问题。
4.为实现上述目的，本发明采用如下所述的技术方案：
5.一种引导无人机精准降落控制方法，包括以下步骤：
6.第一步，建立无人机降落过程的贝塞尔运动曲线方程，根据贝塞尔曲线的控制点，控制位置随时间的变化关系曲线；
7.第二步，调节控制点位置，得到无人机每个时刻的位置及相邻时刻的时间间隔，将位置与时间的关系转换为速度与时间的关系；
8.第三步，通过对速度与时间的数据进行多项式拟合，得到连续的下发速度曲线函数；
9.第四步，根据得到的连续的下发速度曲线函数，控制无人机的下落过程。
10.在以上方案中可以优选的是，第一步所述的无人机降落过程的贝塞尔运动曲线方程具体为三阶贝塞尔追踪方程：
11.b3(t)＝(1
‑
t)3p0+3t(1
‑
t)2p1+3t2(1
‑
t)p2+t3p3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)，
12.其中，p0为控制起点、p1和p2为中间控制点，p3为控制终点；t为时间，b3(t)为三阶贝塞尔函数。
13.还可以优选的是，在所述无人机的升降速度控制中，p1控制起始阶段的缓加速运动，p2控制降落阶段的缓减速运动。
14.还可以优选的是，第二步所述的调节控制点位置，将位置与时间的关系转换为速度与时间的关系，具体步骤为，根据所述三阶贝塞尔追踪方程，所述无人机在相邻时刻的时间间隔内为匀速运动，计算得到时间与位置数据对。
15.还可以优选的是，，所述无人机在相邻位置时间间隔内做匀速运动，根据时间与位置数据对，得到速度与位置数据对，其方程式如下：
16.v
i
＝(p
(i+1)
‑
p
i
)/(t
(i+1)
‑
t
i
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)，
17.其中，v
i
为速度，p
(i+1)
和p
i
为位置，t
(i+1)
和t
i
为时间，i为时间间隔数量，i的取值范围为2到最大时间间隔数。
18.还可以优选的是，第三步所述的多项式拟合得到连续的下发速度曲线函数，通过对速度与位置数据对进行多项式拟合，得到速度曲线函数为，
19.v＝k
×
f(a
i
，p)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)，
20.其中，v为当前速度，k为比例因子，a
i
为拟合参数，p为位置比例,即p＝当前位置距离/总距离。
21.还可以优选的是，所述使用多项式拟合是对速度与位置数据对进行拟合，所述拟合步骤为，给定数据点i点为p
i
(x
i
,y
i
)，其中，x
i
和y
i
为坐标值；求近似曲线
22.y＝φ(x)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)，
23.使得近似曲线与
24.y＝f(x)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)，
25.的偏差最小；近似曲线在点p
i
处的偏差δ
i
，
26.δ
i
＝φ(x
i
)
‑
y
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)，
27.求解使偏差平方和最小的二项式方程,
28.min
φ
∑
mi＝1
δ
2i
＝∑
mi＝1
(φ(x
i
)
‑
y
j
)2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)，
29.对二项式系数求偏导，另导数为0得方程，求解；
30.其中，i为时间间隔数量，m为最大时间间隔数量。
31.还可以优选的是，第四步所述的根据得到的连续的下发速度曲线函数，控制无人机的下落过程，其具体步骤为，
32.当控制起点p0到控制终点p3的距离大于设定阈值距离，根据所述无人机的当前位置，求位置比例p，将所述位置比例p带入式(3)的曲线函数，得到当前控制周期内的下发的速度，将所述的下发的速度发送至所述无人机的飞控模块。
33.还可以优选的是，第四步所述的根据得到的连续的下发速度曲线函数，控制无人机的下落过程，其具体步骤为，
34.当控制起点p0到控制终点p3的距离小于设定阈值距离，使用设定阈值距离值代替控制起点p0到控制终点p3的距离，将所述无人机的当前速度，将所述当前速度带入当前位置与控制起点p0的距离，求得当前位置与控制起点p0位置比例，得到速度v1；将所述当前速度带入当前位置和控制终点p3距离，求得前位置与控制终点p3位置比例，得到速度v2，取v1和v2中最小速度作为下发速度，将所述的下发的速度发送至所述无人机的飞控模块。
35.本发明的引导无人机精准降落控制方法的有益效果如下：
36.本发明的引导无人机精准降落控制方法，解决了技术中控制无人机在指定地点降落时，存在降落速度慢，降落过程不平稳，降低了无人机的工作效率，容易造成无人机损坏的问题；所述方法将贝塞尔追踪方程及多项式拟合相结合，使无人机在降落过程中，起止点附近缓速，中间过程快速的速度控制，实现了无人机在指定地点能够进行快速平稳的降落，避免了碰撞，提高了无人机的工作效率，延迟了使用寿命。
附图说明
37.此处所说明的附图用来提供对本发明的进一步理解，构成本发明的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：
38.图1为本发明的引导无人机精准降落控制方法的贝塞尔位置与时间p
‑
t图。
39.图2为本发明的引导无人机精准降落控制方法的起止点间等时间间隔采集的运动航迹示意图。
40.图3为本发明的引导无人机精准降落控制方法的位置与速度关系图。
41.图4为本发明的引导无人机精准降落控制方法的无人机降落时的速度曲线图。
42.图5为本发明的引导无人机精准降落控制方法的无人机降落时的海拔高度变化曲线。
具体实施方式
43.为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明具体实施例及相应的附图对本发明技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
44.以下实施例用于说明本发明，但不用来限制本发明的范围。
45.实施例1
46.一种引导无人机精准降落控制方法，包括以下步骤：
47.第一步，建立无人机降落过程的贝塞尔运动曲线方程，根据贝塞尔曲线的控制点，控制位置随时间的变化关系曲线。
48.第二步，调节控制点位置，得到无人机每个时刻的位置及相邻时刻的时间间隔，将位置与时间的关系转换为速度与时间的关系；
49.第三步，通过对速度与时间的数据进行多项式拟合，得到连续的下发速度曲线函数；
50.第四步，根据得到的连续的下发速度曲线函数，控制无人机的下落过程。
51.本实施例的引导无人机精准降落控制方法，其方法将贝塞尔追踪方程及多项式拟合相结合，用于无人机的降落速度控制，其原理为：
52.s1，贝塞尔曲线是依据四个位置任意的点坐标绘制出的一条光滑曲线，在所述无人机等相关的速度和时间控制中，速度是随时间缓慢变化的光滑曲线；具体速度的控制方式是增加还是减少，什么时间段要求速度达到多少值，则可以通过这四个点的位置进行控制；
53.s2，贝塞尔追踪方程是以点运动的形式描述了贝塞尔曲线的形成过程，将曲线描述为了一条随连续时间而形成的点迹，其原理如下：
54.一阶贝塞尔追踪方程：假设有个人要用1单位时间从控制点p0’
点匀速走到控制点p1’
点，则随0
‑
1之间的t时刻，人所在的迹点位置m的位置方程可写为，
55.b1(t)＝(1
‑
t)p0’
+tp1’
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)，
56.其中，b1(t)为一阶贝塞尔函数；
57.二阶贝塞尔追踪方程：定义三个控制点p
0”，p
1”，p
2”，在p
0”和p
1”之间有一个人匀速
用1单位时间从p
0”走到p
1”，则t时刻该人所在的位置m,所述位置m即迹点坐标,由一阶贝塞尔追踪方程可知：
58.m0(t)＝(1
‑
t)p
0”+tp
1”ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)，
59.当在p
1”和p
2”之间也有一个人匀速行走，则其迹点坐标
60.m1(t)＝(1
‑
t)p
1”+tp
2”ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)，
61.现在同时有第三个人要从t时刻m0的位置走到m1的位置，第三个人的迹点坐标则为：
62.b2(t)＝m0(t)+[m1(t)
‑
m0(t)]t＝(1
‑
t)2p
0”+2t(1
‑
t)p
1”+t2p
2”ꢀꢀꢀ
(11)，
[0063]
测试控制第三个的坐标位置的点有三个,p
0”控制起点，p
2”控制终点，还有一个中间控制点p
1”调整中间走向；在所述无人机升降速度控制中，除了起止的控制点p
0”和p
2”外，还需要两个中间控制点，一个中间控制点控制起始阶段的缓加速，另一个中间控制点控制降落阶段的换减速运动；故此，所述无人机升降速度控制需要三阶贝塞尔追踪方程，与一阶到二阶的推导过程相似，可知三阶贝塞尔追踪方程为：
[0064]
b3(t)＝(1
‑
t)3p0+3t(1
‑
t)2p1+3t2(1
‑
t)p2+t3p3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)，
[0065]
其中，p0为控制起点、p1和p2为中间控制点，p3为控制终点；t为时间，b3(t)为三阶贝塞尔函数；
[0066]
通过调节中间控制点p1和p2的位置，对所述无人机下降过程中的理论位置进行调节，同时得到每个时刻无人机的理论位置及相邻时刻的时间间隔，可以将位置与时间p
‑
t的关系转换为速度与时间v
‑
t的关系；
[0067]
s3，多项式拟合，由于所述速度与时间v
‑
t的关系是预先设置的采样时间间隔的离散值，而实际应用中，下发速度为通过对所述的速度与时间v
‑
t的数据对进行多项式拟合，得到连续的下发速度曲线函数。
[0068]
本实施例的引导无人机精准降落控制方法，所述方法的具体实现过程：
[0069]
设定贝塞尔运动曲线，通过增添多阶贝塞尔曲线的控制点，用来控制期望的位置position随时间time的变化关系曲线，如图1所示，横坐标表示时间t,纵坐标表示无人机的期望位置，0表示起点，1表示终点；图中虚线所示的均速迹点轨迹变为点线所示的理论迹点轨迹，起点及终点附近位置变化缓慢，即斜率较小，速度变慢；中间部分位置随时间变化较快，即斜率较大。
[0070]
使用三阶贝塞尔曲线，除了起止点外0和1外，另外添加两个控制点来影响图1的斜率变化；如图1所示，共四个控制点(t，position)坐标分别为p0(0,0)，p1(0.12097,0.036443),p2(0.71313,0.94315)，p3(1,1)，p1及p2取值要满足两个条件，第一个条件为，两点斜率小于0.5，即，
[0071]
(p1.y/p1.x)<0.5；(1
‑
p1.y)/(1
‑
p1.x)<0.5；
[0072]
第二个条件为，p2>p1，即,
[0073]
p2.x>0.5>p1.x，p2.y>0.5>p1.y；
[0074]
取值可以根据无人机的下发位置与定位传感器实测位置之差进行调测：当差值较大，说明由于风速较大或其他外界原因导致无人机的控制方差较大，不可控风险较大，测试可自适应进一步降低p1及p2点斜率，放缓起止点附近速度，反之可加大斜率，提高速度。
[0075]
实施例2
[0076]
一种引导无人机精准降落控制方法，与实施例1相似，所不同的是，第一步所述的无人机降落过程的贝塞尔运动曲线方程具体为三阶贝塞尔追踪方程：
[0077]
b3(t)＝(1
‑
t)3p0+3t(1
‑
t)2p1+3t2(1
‑
t)p2+t3p3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)，
[0078]
其中，p0为控制起点、p1和p2为中间控制点，p3为控制终点；t为时间，b3(t)为三阶贝塞尔函数。
[0079]
还可以优选的是，在所述无人机的升降速度控制中，p1控制起始阶段的缓加速运动，p2控制降落阶段的缓减速运动。
[0080]
还可以优选的是，第二步所述的调节控制点位置，将位置与时间的关系转换为速度与时间的关系，具体步骤为，根据所述三阶贝塞尔追踪方程，所述无人机在相邻时刻的时间间隔内为匀速运动，计算得到时间与位置数据对。
[0081]
还可以优选的是，所述无人机在相邻位置时间间隔内做匀速运动，根据时间与位置数据对，得到速度与位置数据对，其方程式如下：
[0082]
v
i
＝(p
(i+1)
‑
p
i
)/(t
(i+1)
‑
t
i
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)，
[0083]
其中，v
i
为速度，p
(i+1)
和p
i
为位置，t
(i+1)
和t
i
为时间，i为时间间隔数量，i的取值范围为2到最大时间间隔数。
[0084]
还可以优选的是，第三步所述的多项式拟合得到连续的下发速度曲线函数，通过对速度与位置数据对进行多项式拟合，得到速度曲线函数为，
[0085]
v＝k
×
f(a
i
，p)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)，
[0086]
其中，v为当前速度，k为比例因子，a
i
为拟合参数，p为位置比例,即p＝当前位置距离/总距离。
[0087]
还可以优选的是，所述使用多项式拟合是对速度与位置数据对进行拟合，所述拟合步骤为，给定数据点i点为p
i
(x
i
,y
i
)，其中，x
i
和y
i
为坐标值；求近似曲线
[0088]
y＝φ(x)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(4)，
[0089]
使得近似曲线与
[0090]
y＝f(x)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)，
[0091]
的偏差最小；近似曲线在点p
i
处的偏差δ
i
，
[0092]
δ
i
＝φ(x
i
)
‑
y
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)，
[0093]
求解使偏差平方和最小的二项式方程,
[0094]
min
φ
∑
mi＝1
δ
2i
＝∑
mi＝1
(φ(x
i
)
‑
y
j
)2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)，
[0095]
对二项式系数求偏导，另导数为0得方程，求解；
[0096]
其中，i为时间间隔数量，m为最大时间间隔数量。
[0097]
还可以优选的是，第四步所述的根据得到的连续的下发速度曲线函数，控制无人机的下落过程，其具体步骤为，
[0098]
当控制起点p0到控制终点p3的距离大于设定阈值距离，根据所述无人机的当前位置，求位置比例p，将所述位置比例p带入式(3)的曲线函数，得到当前控制周期内的下发的速度，将所述的下发的速度发送至所述无人机的飞控模块。
[0099]
还可以优选的是，第四步所述的根据得到的连续的下发速度曲线函数，控制无人机的下落过程，其具体步骤为，
[0100]
当控制起点p0到控制终点p3的距离小于设定阈值距离，使用设定阈值距离值代替
控制起点p0到控制终点p3的距离，将所述无人机的当前速度，将所述当前速度带入当前位置与控制起点p0的距离，求得当前位置与控制起点p0位置比例，得到速度v1；将所述当前速度带入当前位置和控制终点p3距离，求得前位置与控制终点p3位置比例，得到速度v2，取v1和v2中最小速度作为下发速度，将所述的下发的速度发送至所述无人机的飞控模块。
[0101]
本实施例的引导无人机精准降落控制方法，三阶贝塞尔跟踪方程:
[0102]
b3(t)＝(1
‑
t)3p0+3t(1
‑
t)2p1+3t2(1
‑
t)p2+t3p3ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)，
[0103]
p
i
(i＝0,1,2,3)为第一步得到的输入匀速的时间t，所述t的步进时间设为t
step
，用实际的速度控制时间间隔，例如t
step
为0.02s；
[0104]
使0≤t≤1，带入三阶贝塞尔跟踪方程，
[0105]
b3(t)＝(1
‑
t)3p0(t)+3t(1
‑
t)2p1(t)+3t2(1
‑
t)p2(t)+t3p3(t)
ꢀꢀꢀ
(12)，
[0106]
可得如图1所示的贝塞尔p
‑
t图中的一系列t值，p
i
(i＝0,1,2,3)是一个二维向量，分量分别是t和p，带入其中的t分量,计为p
i
(t),；同理，当求如图1所示的p
‑
t图中的一系列p值时，需要带入p
i
(p)；由于实际下发的控制时间t是匀速的，所以，需要反过来求均匀速度t的情况下t的一系列取值，计为t'，采用二分逼近算法：先给个初始值t'，求得t值t_cal与匀速的t值t_mean进行比较，当差值delta_t小于阈值th，则找到了正确的t'，带入贝塞尔追踪方程，分别求取t和p。
[0107]
根据位置和时间数据对，如图2和图3所示，假设每个时间间隔做匀速运动，则可求取速度与位置数据对，
[0108]
v
i
＝(p
(i+1)
‑
p
i
)/(t
(i+1)
‑
t
i
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)，
[0109]
其中，v
i
为速度，p
(i+1)
和p
i
为位置，t
(i+1)
和t
i
为时间，i为时间间隔数量，i＝2...length(t)，length(t)为最大时间间隔数；
[0110]
使用多项式拟合对离散速度和位置数据对进行拟合，得，
[0111]
v＝k
×
f(a
i
，p)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0112]
其中，v为当前速度；k为比例因子，取最大速度控制；a
i
为拟合参数，i＝1,2,3,4,5；p为位置比例,即p＝当前位置距离/总距离。
[0113]
所述无人机的下落运动控制过程，如图4和图5所示，使用如下分段式控制方法，
[0114]
当控制起点p0到控制终点p3的距离大于设定阈值距离，根据所述无人机的当前位置，求位置比例p，将所述位置比例p带入式(3)的曲线函数，得到当前控制周期内的下发的速度，将所述的下发的速度发送至所述无人机的飞控模块。
[0115]
当控制起点p0到控制终点p3的距离小于设定阈值距离，使用设定阈值距离值代替控制起点p0到控制终点p3的距离，将所述无人机的当前速度，将所述当前速度带入当前位置与控制起点p0的距离，求得当前位置与控制起点p0位置比例，得到速度v1；将所述当前速度带入当前位置和控制终点p3距离，求得前位置与控制终点p3位置比例，得到速度v2，取v1和v2中最小速度作为下发速度，将所述的下发的速度发送至所述无人机的飞控模块。
[0116]
这个过程相当于掐取了速度与位置曲线的中间部分，同时k值乘以，起止点距离/阈值距离，进一步缩小。
[0117]
虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。