基于全过程约束的四旋翼实时轨迹规划及降落控制方法

1.本发明涉及四旋翼飞行器制导控制领域，尤其涉及飞行器 的降落追踪控制，具体涉及一种基基于全过程约束的四旋翼实 时轨迹规划及降落控制方法。


背景技术：

2.四旋翼飞行器由于其灵活性和快速部署在工业中越来越 受欢迎，并在诸如拓扑和农业航空摄影、搜索救援行动和测绘 等应用中显示了其实用性。但由于四旋翼飞行器的有效载荷和 飞行时间是有限的，难以满足城市快递、森林救援和海上探测 等任务的需求，所以一些研究人员设计出飞行器与移动平台协 同进行任务的方案，以最大化任务能力。飞行器任务过程中， 首先搭载部分任务载荷从移动平台起飞，完成该阶段任务后， 再返回移动平台进行能源补充，并搭载下一部分任务载荷。其 中，依据任务目标，移动平台可以是卡车、船舰和飞机等海陆 空平台。
3.为了在规定时间内有效完成任务，需要四旋翼在整个任务 过程中实现稳定可靠飞行，尤其降落过程对四旋翼的控制能力 提出了极高的要求。因为降落平台在快速移动，而且环境非常 复杂，比如高楼林立的城市和树木丛生的森林，所以平台状态 没有规则地快速变化，导致四旋翼在降落过程中发生碰撞和倾 覆等事故。
4.此外，移动平台在不同环境下的运动状态不同，在城市道 路中比较平稳；在树林中，随地形的变化，车辆会产生不同程 度的倾斜；在海面上，舰艇平台会随着水面波动而上下起伏。 由于四旋翼的最终目标是降落在移动平台上，所以四旋翼的终 端状态约束需要匹配移动平台的运动状态。另外，由于移动平 台在持续移动，需要依据平台状态的变化，实时重规划出新的 降落轨迹，以满足全过程约束。而常规路径规划算法在考虑全 过程约束的同时，难以满足实时性要求，无法在任务过程中实 现轨迹重规划。
5.现有技术中的降落跟踪算法中，还需要降落平台在移动过 程中考虑降落飞行器的具体工况，适当减速或者保持匀速运 动，尽量保持平稳等等，即降落平台的正常工作状态受到影响， 需要以接收旋翼飞行器降落作为主要任务，使其正常的工作状 态受到一定的影响。
6.由于上述原因，本发明人对现有的飞行器降落控制方法做 了深入研究，以期待设计出一种能够解决上述问题的新的飞行 器降落控制方法。


技术实现要素：

7.为了克服上述问题，本发明人进行了锐意研究，设计出一 种基于全过程约束的四旋翼实时轨迹规划及降落控制方法，该 方法中，通过通信系统获得降落平台的运动信息，即速度位置 和加速度等信息，通过其携带的传感器获得其自身的运动信 息，即可获得一组相对距离和相对速度信息，该信息相对来说 误差较大，适用于距离降落平台较远的情况，当距离较近时， 通过视觉传感器直接获得相对距离和相对速度信息，其精度较 高；进而获得估计的降落时间，基于预测时域，将所述降落时 间分为固定份数，如20份，每份之间
的临界点即为采样点，每 份时间长度即为采样时间长度，估计出飞行器在每个采用点处 的状态向量，并据此控制飞行器沿着估计轨迹飞行，到达第一 个采样点后，重复上述步骤，重新估计轨迹，再次控制飞行器 到达第一个采样点；直至飞行器到达降落平台，完成飞行器降 落，从而完成本发明。
8.具体来说，本发明的目的在于提供以一种基于全过程约束 的四旋翼实时轨迹规划及降落控制方法，该方法包括如下步骤：
9.步骤一：获得四旋翼飞行器与降落平台的相对距离pr和相 对速度信息vr；
10.步骤二：估计降落所需的时间，基于预测时域n获得采样 时间长度；
11.步骤三：确定四旋翼飞行器当前的行为模式，规划参考轨 迹；
12.步骤四：通过控制器控制四旋翼飞行器跟踪参考轨迹；
13.步骤五：到达第一个采样点时，重复步骤1，并判断四旋翼 飞行器与降落平台的相对距离pr是否小于或等于预定阈值，若 不小于或等于预定阈值，则重复步骤二、步骤三、步骤四、步 骤五，直至四旋翼飞行器与降落平台的相对距离pr小于或等于 预定阈值，四旋翼飞行器停机，落到降落平台上。
14.其中，在步骤一中，通过通信系统接收降落平台传递出的 降落平台的位置和速度信息，通过四旋翼飞行器上搭载的传感 器获得四旋翼飞行器的位置和速度信息，从而获得四旋翼飞行 器与降落平台的相对距离pr和相对速度信息vr；
15.或者，通过四旋翼飞行器上搭载的视觉传感器捕获到降落 平台，直接获得四旋翼飞行器与降落平台的相对距离pr和相对 速度信息vr。
16.其中，在步骤二中，降落所需的时间ta通过下式(1)获得：
[0017][0018]
其中，pr表示相对距离矢量，||pr||表示相对距离幅值，即 无人机到目标的相对距离长度；
[0019]er
表示相对距离矢量的方向，即从无人机指向目标的单位 矢量，通过下式(1.1)获得：
[0020][0021]
其中，在步骤二中，将所述降落所需的时间ta平均分为n 段，每ta/n时间即为一个采样时间长度。
[0022]
其中，当所述四旋翼飞行器通过通信系统和传感器获得四 旋翼飞行器与降落平台的相对距离pr和相对速度信息vr时，四 旋翼飞行器当前的行为模式为搜索模式；
[0023]
当所述四旋翼飞行器通过视觉传感器获得四旋翼飞行器与 降落平台的相对距离pr和相对速度信息vr时，四旋翼飞行器当 前的行为模式为着陆模式。
[0024]
其中，所述参考轨迹包括n个状态向量，每个状态向量都包 括坐标位置、飞行器到达该坐标位置时的速度和飞行器到达该 坐标位置时的加速度。
[0025]
其中，所述规划参考轨迹的过程包括，使得目标函数最小，
[0026]
当行为模式为搜索模式时，所述目标函数为下式(2)，
[0027]
[0028]
当行为模式为着陆模式时，所述目标函数为下式(3)，
[0029][0030]
其中，j表示目标函数，i表示第i个采样点，xa(i)表示第i个 采样点的四旋翼飞行器与降落平台的相对距离，表示x(i) 的转置；ta表示降落所需的时间；qa和wa都表示权重系数；
[0031]
x
l
表示四旋翼飞行器与降落平台的相对距离pr和相对速度vr组成的矩阵，即x
l
＝[pr，vr]
t
；表示x
l
的转置；x
l
(i)表示在 第i个采样点的四旋翼飞行器与降落平台的相对距离pr和相对 速度vr组成的矩阵；
[0032]
u(i)表示第i个采样点的控制量，u
t
(i)表示u(i)的转置；
[0033]al
表示四旋翼飞行器水平面内的加速度，即a
l
＝[a
x
，ay]
t
； 表示终端时刻n的加速度矢量，能够保证无人机在终端降 落过程中平稳性；
[0034]ql
、r
l
、w
l
都表示系数矩阵。
[0035]
其中，在所述规划参考轨迹的过程包括，基于下式(4.1)、 (4.2)和(4.3)使得目标函数最小，
[0036]
x(k+1)＝ax(k)+bu(k)
ꢀꢀꢀ
(4.1)
[0037]
x(0)＝x0，x(n)＝xf，z(k+1)＞h
p
(4.2)
[0038]
|a
p
(k+1)|≤a
p，max
，|u
p
(k)|≤u
p，max
，p＝x，y，z
ꢀꢀꢀ
(4.3)
[0039]
其中，x(k+1)表示预测获得的第k+1时刻的状态向量，即 第k+1个采样点状态向量；k＝0，1，...，n-1；
[0040]
x(k)表示第k时刻的状态向量，即第k个采样点状态向量；
[0041]
a和b都表示系数矩阵；
[0042]
u(k)表示第k个采样点的控制量；
[0043]
x(0)表示当前优化时域中，初始时刻的无人机状态，x0表示 当前时刻的无人机真实状态；x(n)表示当前优化时域中，终端 时刻的无人机状态，xf表示该优化时域中，终端时刻无人机所 需满足的终端状态约束；z(k+1)表示k+1时刻的无人机高度，h
p
表示降落平台的高度；
[0044]
|a
p
(k+1)|表示第k+1个采样点的加速度，|u
p
(k)|表示第k个 采样点的控制量；a
p，max
表示飞行器能够提供的最大加速度，u
p，max
表示飞行器能够提供的最大控制量。
[0045]
其中，在步骤四中，在到达第一个采样点前，当滚动优化 的采样周期ta比控制器的控制周期小0.05s时，对第一个采样点 以前的轨迹做差值处理，并根据差值处理结果控制四旋翼飞行 器飞行。
[0046]
其中，在步骤四中，通过下式(5)对轨迹做差值处理：
[0047][0048]
其中，x(t)表示t时刻无人机的状态，x(k)表示k时刻的无 人机状态，x(k+1)表示k+1时刻无人机的状态，，t(k)表示离散 化时间k所对应的连续时间t(k)，t(k+1)表示离散化
时间k+1 所对应的连续时间t(k+1)，t表示连续时间中的t时刻。
[0049]
本发明所具有的有益效果包括：
[0050]
(1)根据本发明提供的基于全过程约束的四旋翼实时轨 迹规划及降落控制方法能够在降落平台正常机动的情况下降 落到该平台上，无需平台有针对性地等待或者匀速机动；
[0051]
(2)根据本发明提供的基于全过程约束的四旋翼实时轨 迹规划及降落控制方法能够在快速机动的情况下，持续追踪移 动平台，在接近平台时通过视觉传感器自动捕获平台，整体降 落过程可实现自动化；
[0052]
(3)根据本发明提供的基于全过程约束的四旋翼实时轨 迹规划及降落控制方法，把模型预测控制方法应用在轨迹规划 任务中，通过设置合理的预测时域，在兼顾追踪准确性的情况 下降低运算量，可以满足轨迹重规划的实时性要求，实现四旋 翼快速准确平稳地降落在移动平台上；
[0053]
(4)根据本发明提供的基于全过程约束的四旋翼实时轨 迹规划及降落控制方法，增强了四旋翼和海陆空移动平台协同 任务的能力，另外，也可以在仿真环境中引入风场等外界干扰， 并把本文的开源飞控算法替换为个人研究的飞控算法，从而用 自主降落任务的完成度检验控制算法的可靠性、鲁棒性和抗干 扰能力。
附图说明
[0054]
图1示出根据本发明一种优选实施方式的基于全过程约束 的四旋翼实时轨迹规划及降落控制方法整体逻辑图；
[0055]
图2示出根据本发明实施例中四旋翼飞行器与降落平台的 路线轨迹。
具体实施方式
[0056]
下面通过附图和实施例对本发明进一步详细说明。通过这 些说明，本发明的特点和优点将变得更为清楚明确。
[0057]
在这里专用的词“示例性”意为“用作例子、实施例或说 明性”。这里作为“示例性”所说明的任何实施例不必解释为 优于或好于其它实施例。尽管在附图中示出了实施例的各种方 面，但是除非特别指出，不必按比例绘制附图。
[0058]
根据本发明提供的基于全过程约束的四旋翼实时轨迹规 划及降落控制方法，如图1中所示，该方法包括如下步骤：
[0059]
步骤一，获得四旋翼飞行器与降落平台的相对距离pr和 相对速度信息vr；
[0060]
其中，当飞行器开始执行降落任务时，通过通信系统接收 降落平台传递出的降落平台的位置和速度信息，所述通信系统 为飞行器上搭载的设备，如雷达等，其能够接收移动平台实时 传递出的移动平台的位置、速度和加速度信息；在所述飞行器 上还搭载有传感器，通过传感器获得四旋翼飞行器的位置和速 度信息，所述传感器包括卫星信号接收系统和惯导系统；基于 移动平台的运动信息和飞行器的运动信息，获得四旋翼飞行器 与降落平台的相对距离pr和相对速度信息vr；
[0061]
飞行器在接收上述信息的同时，还启动视觉传感器，通过 视觉传感器实时寻找捕捉移动平台，当视觉传感器发现并锁定 移动平台后，通过四旋翼飞行器上搭载的视觉传感
器捕获到降 落平台，直接获得四旋翼飞行器与降落平台的相对距离pr和相 对速度信息vr。当视觉传感器捕获到目标后，仅通过视觉传感 器获得的信息执行飞行器的制导控制工作。
[0062]
步骤二：估计降落所需的时间，基于预测时域n获得采 样时间长度；
[0063]
优选地，所述降落所需的时间ta通过下式(1)获得：
[0064][0065]
其中，pr表示相对距离矢量，||pr||表示相对距离幅值， 即无人机到目标的相对距离长度；
[0066]er
表示相对距离矢量的方向，即从无人机指向目标的单位 矢量，通过下式(1.1)获得：
[0067][0068]vr
·er
表示相对速度在视线上的投影，所述视线是指飞行 器与降落平台的连线。
[0069]
在获得降落所需的时间后，将该降落所需的时间平均分n 段/份，其中的n为预测时域，每ta/n时间即为一个采样时间 长度，每份之间的临界点即为采样点，每份时间长度即为采样 时间长度。
[0070]
本技术中，所述预测时域为固定值，该值越大则规划路径 上的采样点越多，轨迹越精确，但相应地，计算量增加，对硬 件设备要求越高，容易导致计算延迟，该值越小，则采样点越 少，轨迹越粗糙，容易丢失目标。本技术优选地将所述预测时 域设置为20；本发明人发现当该预测时域设置为20时四旋翼飞 行器能够准确地追踪到降落平台，并且运算量适中，能够满足 实时控制解算的需求。
[0071]
步骤三：确定四旋翼飞行器当前的行为模式，规划参考轨 迹；
[0072]
优选地，当所述四旋翼飞行器通过通信系统和传感器获得 四旋翼飞行器与降落平台的相对距离pr和相对速度信息vr时， 四旋翼飞行器当前的行为模式为搜索模式；
[0073]
当所述四旋翼飞行器通过视觉传感器获得四旋翼飞行器 与降落平台的相对距离pr和相对速度信息vr时，四旋翼飞行器 当前的行为模式为着陆模式。
[0074]
优选地，所述参考轨迹包括n个状态向量，每个状态向量 都包括坐标位置、飞行器到达该坐标位置时的速度和飞行器到 达该坐标位置时的加速度。
[0075]
状态向量x＝[x
x
,y,z,v
x
,vy,vz,a
x
,ay,az]
t
[0076]
x
x
，y，z分别表示大地坐标系中的三维位置坐标，其中，x
x
表 示大地坐标系中x轴方向的坐标，y表示大地坐标系中y轴方 向的坐标，z表示大地坐标系中z轴方向的坐标；v
x
，vy，vz分别 表示速度在大地坐标系中的三个方向的分量，a
x
，ay，az分别表 示加速度在大地坐标系中的三个方向的分量。
[0077]
优选地，规划参考轨迹的过程包括，使得目标函数最小，
[0078]
当行为模式为搜索模式时，所述目标函数为下式(2)，
[0079][0080]
当行为模式为着陆模式时，所述目标函数为下式(3)，
[0081][0082]
其中，j表示目标函数，i表示第i个采样点，xa(i)表示第i个 采样点的四旋翼飞行器与降落平台的相对距离，表示x(i) 的转置；ta表示降落所需的时间；qa和wa都表示权重系数；
[0083]
x
l
表示四旋翼飞行器与降落平台的相对距离pr和相对速度 vr组成的矩阵，即x
l
＝[pr，vr]
t
；表示x
l
的转置；x
l
(i)表示在 第i个采样点的四旋翼飞行器与降落平台的相对距离pr和相对 速度vr组成的矩阵；
[0084]
u(i)表示第i个采样点的控制量，u
t
(i)表示u(i)的转置；
[0085]al
表示四旋翼飞行器水平面内的加速度，即a
l
＝[a
x
，ay]
t
； 表示终端时刻n的加速度矢量，能够保证无人机在终端降 落过程中平稳性；
[0086]ql
、r
l
、w
l
都表示系数矩阵，其取值为
[0087]ql
＝[0.1，0，0，0，0，0； 0，0.1，0，0，0，0； 0，0，0.1，0，0，0； 0，0，0，0.1，0，0； 0，0，0，0，0.1，0； 0，0，0，0，0，0.1]，
[0088]rl
＝[1，0，0； 0，1，0； 0，0，1]，
[0089]wl
＝[0.1，0； 0，0.1]。
[0090]
优选地，在所述规划参考轨迹的过程中，基于下式(4.1)、(4.2)和(4.3)的约束使得目标函数最小，
[0091]
x(k+1)＝ax(k)+bu(k)
ꢀꢀꢀ
(4.1)
[0092]
x(0)＝x0，x(n)＝xf，z(k+1)＞h
p
ꢀꢀꢀ
(4.2)
[0093]
|a
p
(k+1)|≤a
p，max
，|u
p
(k)|≤u
p，max
，p＝x，y，z
ꢀꢀꢀ
(4.3)
[0094]
其中，x(k+1)表示预测获得的第k+1时刻的状态向量，即 第k+1个采样点状态向量；k＝0，1，...，n-1；
[0095]
x(k)表示第k时刻的状态向量，即第k个采样点状态向量；
[0096]
a和b都表示系数矩阵，其取值为
[0097]
a＝[ta/n，0，0，ta/n，0，0，(ta/n)/2，0，0； 0，ta/n，0，0，ta/n，0，0，(ta/n)/2，0； 0，0，ta/n，0，0，ta/n，0，0，(ta/n)/2； 0，0，0，ta/n，0，0，ta/n，0，0； 0，0，0，0，ta/n，0，0，ta/n，0； 0，0，0，0，0，ta/n，0，0，ta/n； 0，0，0，0，0，0，ta/n，0，0； 0，0，0，0，0，0，0，ta/n，0； 0，0，0，0，0，0，0，0，ta/n]，
[0098]
b＝[ta/n，0，0； 0，ta/n，0； 0，0，ta/n； (ta/n)/2，0，0； 0，(ta/n)/2，0； 0，0，(ta/n)/2； (ta/n)/3，0，0； 0，(ta/n)/3，0； 0，0，(ta/n)/3]
[0099]
其中，ta/n是指步骤二中计算的采样时间长度；
[0100]
u(k)表示第k个采样点的控制量；
[0101]
x(0)表示当前优化时域中，初始时刻的无人机状态，x0表示 当前时刻的无人机真实状态；x(n)表示当前优化时域中，终端 时刻的无人机状态，xf表示该优化时域中，终端时刻无人机所 需满足的终端状态约束；z(k+1)表示k+1时刻的无人机高度，h
p
表示降落平台的高度；
[0102]
|a
p
(k+1)|表示第k+1个采样点的加速度，|u
p
(k)|表示第k 个采样点的控制量；a
p，max
表示飞行器能够提供的最大加速度， u
p，max
表示飞行器能够提供的最大控制量。
[0103]
本技术中的规划参考轨迹即为解算获得n个状态向量，并 且在实际控制过程中，仅仅应用了第一个状态向量，在到达第 一个状态向量时，需要重新规划参考轨迹。
[0104]
步骤四：通过控制器控制四旋翼飞行器跟踪参考轨迹；
[0105]
优选地，在到达第一个采样点前，当滚动优化的采样周期 ta比控制器的控制周期小0.05s时，对第一个采样点以前的轨 迹做差值处理，并根据差值处理结果控制四旋翼飞行器飞行。 优选地，滚动优化的采样周期是指优化过程中，相邻迭代步之 间的时间间隔，控制器的控制周期是指四旋翼控制过程中，相 邻控制输入之间的时间间隔。
[0106]
优选地，通过下式(5)对轨迹做差值处理：
[0107][0108]
其中，x(t)表示t时刻无人机的状态，x(k)表示k时刻的无 人机状态，x(k+1)表示k+1时刻无人机的状态，t(k)表示离散 化时间k所对应的连续时间t(k)，t(k+1)表示离散化时间k+1 所对应的连续时间t(k+1)，t表示连续时间中的t时刻。
[0109]
通过所述差值处理能够获得更匹配控制器性能的轨迹，从 而保证控制的性能，使无人机安全平稳地跟踪轨迹，快速精准 地降落在移动平台上。
[0110]
在一个优选的实施方式中，所述控制器采用开源的px4飞 控算法对四旋翼飞行器进行控制。
[0111]
步骤五：到达第一个采样点时，重复步骤一，并判断四旋 翼飞行器与降落平台的相对距离pr是否小于或等于预定阈 值，若不小于或等于预定阈值，则重复步骤二、步骤三、步骤 四、步骤五，直至四旋翼飞行器与降落平台的相对距离pr小 于或等于预定阈值，四旋翼飞行器停机，落到降落平台上。
[0112]
其中，所述预定阈值根据实际情况选择设置，如将预定阈 值设置为降落平台直径尺寸的三分之一等。在实际工作过程 中，上述步骤五中的重复过程会持续多次，随着飞行器与降落 平台之间逐渐接近，重复的频率越来越高。本技术中，飞行器 的位置坐标是指飞行器质心的坐标，降落平台的位置坐标是指 降落平台台面的几何中心。
[0113]
实施例：
[0114]
降落平台的初始位置为(10,0，0)、初始速度为(3,0,0)、 初始加速度为(0,0,0)，其移动轨迹如图2中的虚线所示，
[0115]
四旋翼飞行器的初始位置为(0,2,3)、初始速度为 (5,0,0)、初始加速度为(0,0,0)，其移动轨迹如图2中的实 现所示；
[0116]
其中，四旋翼飞行器在与降落平台之间的距离缩小到2m 时，四旋翼飞行器的行为模式由搜索模式调整为着陆模式；
[0117]
在四旋翼飞行器追踪降落平台的过程中，具体包括如下过 程：
[0118]
步骤1，实时获得四旋翼飞行器与降落平台的相对距离pr和相对速度信息vr；
[0119]
步骤2，通过下式(1)估计降落所需的时间，基于预测时 域n＝20获得采样时间长度；
[0120]
[0121]
步骤3，确定四旋翼飞行器当前的行为模式，规划参考轨 迹；所述规划参考轨迹的过程包括，使得目标函数最小，
[0122]
当行为模式为搜索模式时，所述目标函数为下式(2)，
[0123][0124]
当行为模式为着陆模式时，所述目标函数为下式(3)，
[0125][0126]
在所述规划参考轨迹的过程中，基于下式(4.1)、(4.2) 和(4.3)的约束使得目标函数最小，
[0127]
x(k+1)＝ax(k)+bu(k)
ꢀꢀꢀ
(4.1)
[0128]
x(0)＝x0，x(n)＝xf，z(k+1)＞h
p
ꢀꢀꢀ
(4.2)
[0129]
|a
p
(k+1)|≤a
p，max
，|u
p
(k)|≤u
p，max
，p＝x，y，z
ꢀꢀꢀ
(4.3)
[0130]
步骤4：通过控制器控制四旋翼飞行器跟踪参考轨迹；
[0131]
步骤5，到达第一个采样点时，重复步骤1，并判断四旋翼 飞行器与降落平台的相对距离pr是否小于或等于0.5米，若不 小于或等于0.5米，则重复步骤2、步骤3、步骤4、步骤5，直 至四旋翼飞行器与降落平台的相对距离pr小于或等于0.5米， 四旋翼飞行器停机，落到降落平台上，四旋翼飞行器降落作业 结束。
[0132]
从图2可看出，四旋翼飞行器沿着实时规划出的平滑轨迹， 快速平稳精准地降落在移动平台上，降落过程总耗时4.6s。
[0133]
以上结合了优选的实施方式对本发明进行了说明，不过这 些实施方式仅是范例性的，仅起到说明性的作用。在此基础上， 可以对本发明进行多种替换和改进，这些均落入本发明的保护 范围内。