基于位姿预测和轨迹优化的无人机引导降落方法及无人机

1.本发明涉及无人机的导航和运动规划领域，尤其是涉及一种基于位姿预测和轨迹优化的四旋翼无人机视觉引导降落方法。


背景技术：

2.随着科学技术的不断发展，多旋翼无人机已经广泛应用于农业、测绘、物流、消防、救援等领域，目前，随着无人机相关技术的不断升级和应用场景的不断扩展，无人机已经进入了快速发展期：传感器、控制系统、通信等技术不断升级，让实现更加复杂的任务成为可能。而无人机和无人车之间的协同作业可以实现更高效、更精准的任务完成。无人机可以提供高空视角和快速的移动能力，适用于需要大范围监测和快速响应的任务，如灾害监测、交通监管等；而无人车则可以提供更稳定、更精细的操作能力，适用于需要精细操作和长时间持续工作的任务，如物流配送、环境清理等。在协同作业中，无人机可以先进行快速的勘测和监测，将数据传输给无人车进行进一步的处理和操作。例如，在物流配送中，无人机可以先进行货物的快速运输和目标地点的勘测，将数据传输给无人车进行精细的操作和货物的最终配送。在环境清理中，无人机可以先进行空中监测和快速清理，将数据传输给无人车进行地面清理和处理。因此，让无人机能够平稳、安全的降落在无人车上实现车机协同作业就成为高质量完成任务的关键。
3.目前已经有一些关于无人机自主降落的研究，申请号为202011237125.8的专利，基于视觉锚点的无人机降落位姿滤波估计方法及系统，通过构建无人机在降落过程中的空间位姿构建卡尔曼滤波模型，但并没有对目标降落小车的空间位姿进行未来预测，当出现目标物短暂遮挡和运动误差时不能很好的降落到小车上，影响无人机的降落精度；申请号为202111452204.5的专利，设计了基于视觉的无人机降落方法，将无人机的降落过程分为返航、中高空、低空、近距离四部分，返航部分通过gps信号引导，其余几个部分通过多个标识符的不同和kcf算法来实现无人机的跟踪和降落，但并没有对无人机的着降曲线进行优化处理。


技术实现要素：

4.本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于位姿预测和轨迹优化的无人机引导降落方法及无人机。
5.本发明的目的可以通过以下技术方案来实现：
6.作为本发明的第一方面，提供一种基于位姿预测和轨迹优化的无人机引导降落方法，所述方法基于无人机的飞行高度和与目标降落物的距离将无人机的引导降落分为远距离引导，中距离跟踪和近距离着降；
7.其中，所述近距离降落过程的具体步骤如下：
8.当无人机在中距离跟踪过程中着降角度达到降落阈值，无人机进入预着降状态；
9.无人机进入预着降状态，获取当前目标降落物的运动状态，通过卡尔曼滤波算法
预测目标降落物运动状态；
10.将预测的目标降落物的运动过程与设定的偏航运动角比较进而判断目标降落物的运动状态是否稳定，并判断无人机是否能够进入着降状态；
11.进入着降状态后，保持着降角度不变并根据当前无人机的姿态和动力学结构构建初始降落轨迹；
12.在降落过程中通过贝塞尔曲线不断优化初始降落曲线，找到最优的降落轨迹；
13.无人机识别最终标志物，基于最优的降落轨迹降落到目标降落物上。
14.进一步的，所述远距离引导过程具体为：无人机获得目标降落物的相对位置，并根据定位导航系统导航飞行至目标物附近。
15.进一步的，所述中距离跟踪过程具体为：当无人机飞到目标物附近后，通过无人机的视觉相机捕捉目标物的图像信息，通过目标检测核相关滤波kcf算法对目标降落物进行框选跟踪。
16.进一步的，所述目标降落物设有用于检测识别的二维码；
17.当无人机的视觉相机检测到二维码信息后，将获取到的图像信息输入到目标的检测kcf算法中实现基于相机视觉的目标跟踪，具体步骤如下：
18.选择一个预先设计好的目标模板表示目标降落物，所述目标模板通过方向梯度直方图特征和颜色直方图特征构建；
19.在视觉相机的每一帧图像中，kcf算法将目标模板与当前帧的图像进行相关运算得到相应图，相应图中响应值最高的位置即为目标降落物的当前位置；
20.无人机实时监测并输出目标降落物的位置，无人机通过pid控制算法调整自身的飞行姿态和速度，实现对目标降落物的追踪。
21.进一步的，所述无人机在进行kcf算法框选追踪的过程中，不断计算并判断摄像头的视线角是否达到降落阈值；如果达到降落阈值，则进入预着降状态。
22.进一步的，当着降角度达到降落阈值后，无人机通过相机的多帧连续识别，结合卡尔曼滤波来判断目标降落物接下来的运动情况，并进一步判断无人机是否可以进入降落状态，通过卡尔曼滤波预测目标降落物到运动状态具体步骤如下：
23.根据目标降落物过程中的视觉锚点测量情况，构造目标降落物的位姿估计卡尔曼滤波模型，所述卡尔曼滤波模型包括系统状态预测方程和系统观测方程；
24.建立目标降落物的位置表示方程；
25.通过卡尔曼滤波模型估计目标降落物的未来运动轨迹。
26.进一步的，所述目标降落物的位置表示为线性方程：
27.x(k+1)＝ax(k)+gw(k)
28.x(k+1)＝ax(k)+gw(k)
29.z(k+1)＝hx(k+1)+v(k+1)
30.其中，k为离散时间；x(k)为k时刻系统状态；a为状态转移矩阵；z(k)为k时刻对应状态的观测值；h为观测矩阵；g为噪声矩阵；w(k)为白噪声；v(k)观测噪声；
31.规定w(k)和v(k)的均值为0，q、r为协方差矩阵；所述卡尔曼滤波器表述如下：
32.状态预测：
33.x(k+1|k)＝ax(k|k)
34.协方差矩阵预测：
35.p(k+1|k)＝ap(k|k)a
t
+gqg
t
36.状态更新：
37.x(k+1|k+1)＝x(k+1|k)+k(k+1)[z(k+1)-hx(k+1|k)]
[0038]
卡尔曼增益矩阵：
[0039]
k(k+1)＝p(k+1|k)h
t
[hp(k+1|k)h
t
+r]-1
[0040]
协方差矩阵更新：
[0041]
p(k+1|k+1)＝[i
n-k(k+1)h]p(k+1|k)
[0042]
其中，x(k+1|k)为k+1时刻的后验状态估计值，x(k|k)为k时刻的后验状态估计值，p(k+1|k)为k时刻的先验估计协方差(x(k+1|x)的协方差)，p(k+1|k+1)为k+1时刻的后验估计协方差，p(k|k)为k时刻的后验估计协方差，k(k+1)为滤波增益矩阵也就是卡尔曼增益，z(k+1)为k+1时刻对应状态的观测值，in为n阶单位矩阵。
[0043]
进一步的，将得到的目标降落物的预测轨迹使用贝塞尔曲线进行动力学和运动学约束，得到无人机降落的视觉引导曲线，具体步骤如下：
[0044]
根据得到的预测轨迹点建立贝塞尔曲线，在预测的轨迹中定义并选择起点和终点，设置控制点并建立贝塞尔曲线：
[0045]
构建问题约束，包括连续性约束、端点约束和动力学约束；
[0046]
使用贝塞尔曲线公式计算出曲线上的点的坐标；
[0047]
通过软约束对贝塞尔曲线的进一步优化；
[0048]
得到符合动力学约束的降落飞行曲线。
[0049]
进一步的，所述通过软约束对贝塞尔曲线的进一步优化，构建优化目标函数如下：
[0050]
j＝λ1js+λ2jc+λ3jd[0051]
其中，表示光滑代价，用minimum snap形式最小化加速度，fu(t)为参数化轨迹，x,y,z为三个维度上生成的轨迹点，t为轨迹生成周期，表示k阶导数；
[0052]
表示碰撞代价，将对轨迹ds的积分使用每一小段的速度乘以时间dt来代替，c(p(t))表示沿着轨迹点的距离惩罚，c(p(tk))表示每个事件段上的距离惩罚，v(t)表示x、y、z三个方向的速度合并后的无人机的真实速度，t/dt表示将时间t按dt时间间隔分割；
[0053]
jd，表示动力学代价，用于惩罚速度和加速度超出限制的部分；λ1、λ2、λ3，为权重优化系数。
[0054]
作为本发明的第二方面，提供一种无人机，所述无人机在降落时执行如上任意一项所述的降落方法。
[0055]
与现有技术相比，本发明具有以下有益效果：
[0056]
1)本发明在传统的使用单目视觉进行追踪的基础上加入了卡尔曼滤波预测目标
降落物的未来移动轨迹，对于目标降落物被一定程度上遮挡导致无法准确识别降落路径和轨迹的情况有很好的预测效果。
[0057]
2)除此之外，增加了对角度约束下的降落曲线的贝塞尔优化，通过贝塞尔曲线的优化，使得下降曲线的抖动和震动减小，降落曲线的轨迹也更加符合无人机的安全性约束和动力学约束，更有利于视觉降落时的轨迹跟踪的实现。本发明能够提升多旋翼无人机在视觉引导降落上的稳定性和准确性。
附图说明
[0058]
图1为本发明基于滤波估计算法的无人机视觉引导降落系统的流程图；
[0059]
图2为车辆尾部设置的用于无人机识别的二维码阵列信息；
[0060]
图3为卡尔曼滤波实现的小车轨迹预测与误差分析曲线(x位置信息)；
[0061]
图4为卡尔曼滤波实现的小车轨迹预测与误差分析曲线(y位置信息)；
[0062]
图5为卡尔曼滤波实现的小车轨迹预测与误差分析曲线(y误差信息)；
[0063]
图6为θ角度约束下的无人机远、中、近距离着降示意图；
[0064]
图7为经过贝塞尔曲线优化后的无人机着降曲线。
[0065]
具体实施方式
[0066]
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
[0067]
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于位姿预测与轨迹优化的无人机视觉引导降落方法，用以将执行完飞行任务的无人机精准降落到移动目标物上。
[0068]
具体的，本发明设计了一种基于卡尔曼滤波算法的位姿预测方法以及基于贝塞尔曲线的轨迹优化算法，并基于前两种方法设计了一种基于角度约束的无人机自主降落方法，具体步骤如下：
[0069]
s1：针对无人机的实际飞行高度和距离目标降落物的实际距离将无人机的引导降落分为远距离引导，中距离跟踪和近距离着降；
[0070]
s2：远距离引导过程，无人机在返航时通过记载数传系统和通讯系统获得目标降落物的相对位置，并根据gps导航系统导航飞行至目标物附近；
[0071]
s3：中距离跟踪过程，当无人机飞到目标物附近后，通过无人机的单目视觉相机和双目视觉相机捕捉目标物的图像信息，通过无人机的目标检测kcf滤波算法对目标降落物进行框选跟踪；
[0072]
s4：近距离降落过程，当无人机在跟踪过程中着降角度达到降落阈值时，无人机进入预着降状态；
[0073]
s5：无人机进入预着降状态，通过无人机的单目视觉相机的多帧获取当前目标降落物的运动状态，通过卡尔曼滤波算法预测目标降落物运动状态；
[0074]
s6：将预测的目标降落物的运动过程与设定的偏航运动角比较进而判断目标降落
物的运动状态是否稳定，并判断无人机是否能够进入着降状态；
[0075]
s7：进入着降状态后，通过单目视觉相机识别保持相机与目标降落物之间的着降角度不变，进而得到当前情况下的初始降落轨迹；
[0076]
s8：在无人机的降落过程中通过贝塞尔曲线不断优化降落曲线，减少无人机在降落过程中调整姿态产生的震荡，找到最优的无人机降落轨迹；
[0077]
s9：无人机识别最终标志物并实现在目标降落物上的精准降落。
[0078]
进一步的，所述目标物为一般车辆，在车辆的尾部设有用于检测识别的二维码信息；
[0079]
所述二维码信息是由多组二维码组成的二维码阵列，每个二维码呈白底黑码，每个二维码均具有自己独立的id，包括4个较小的二维码a、b、c、d在阵列的四周，一个较大的二维码e在阵列的中心，一个较小的二维码f处于较大二维码的中心；
[0080]
进一步的，所述目标降落小车用于无人机在远距离通过gps定位系统在远距离跟踪；
[0081]
所述二维码a、b、c、d用于无人机在中距离引导降落时使用，当无人机的单目相机检测到二维码信息后，将获取到的图像信息输入到目标的检测kcf算法中实现基于单目相机视觉的目标跟踪算法；
[0082]
步骤一，无人机机载视觉选择一个预先设计好的目标模板来表示小车，通过hog特征和颜色直方图等特征子来构建目标模板；
[0083]
步骤二，在单目视觉相机的每一帧图像中，kcf算法将目标模板与当前帧的图像进行相关运算得到相应图，相应图中响应值最高的位置即为小车的当前位置；
[0084]
步骤三，无人机根据记载模块实时监测并输出小车的位置，无人机通过pid控制算法调整自身的飞行姿态和速度，实现对小车的追踪。
[0085]
进一步的，无人机在使用kcf算法框选追踪的过程中，会不断通过记载视觉导引信号的解算来判断摄像头的视线角是否达到降落阈值。如果达到降落阈值，则进入预着降状态；
[0086]
进一步的，当着降角度达到降落阈值后，通过单目相机的多帧连续识别，结合卡尔曼滤波来判断小车接下来的运动情况，并进一步判断无人机是否可以进入降落状态，通过卡尔曼滤波预测目标降落小车到运动状态步骤：
[0087]
步骤一，根据目标降落小车过程中的视觉锚点测量情况，构造目标降落小车的位姿估计卡尔曼滤波模型；其中模型包括系统状态预测方程和系统观测方程；
[0088]
步骤二，识别视觉锚点的过程实际上是对单帧图像的处理，属于离散数据处理，可以将标识物识别过程看作线性离散控制，卡尔曼滤波能够通过递推有效的对其进行预测，这里我们将目标降落小车的位置表示为线性方程：
[0089]
x(k+1)＝ax(k)+gw(k)
[0090]
x(k+1)＝ax(k)+gw(k)
[0091]
z(k+1)＝hx(k+1)+v(k+1)
[0092]
其中，k为离散时间；x(k)为k时刻系统状态；a为状态转移矩阵；z(k)为k时刻对应状态的观测值；h为观测矩阵；g为噪声矩阵；w(k)为白噪声；v(k)观测噪声。
[0093]
步骤三，规定w(k)和v(k)的均值为0，协方差矩阵分别为q、r。基于状态预测方程，
通过上一时刻预测下一时刻系统状态，假定现在的系统状态为k，根据系统的模型，基于系统上一时刻状态预测出当前状态。
[0094]
卡尔曼滤波器表述如下：
[0095]
(1)状态预测：
[0096]
x(k+1|k)＝ax(k|k)
[0097]
(2)协方差矩阵预测：
[0098]
p(k+1|k)＝ap(k|k)a
t
+gqg
t
[0099]
(3)状态更新：
[0100]
x(k+1|k+1)＝x(k+1|k)+k(k+1)[z(k+1)-hx(k+1|k)]
[0101]
(4)卡尔曼增益矩阵：
[0102]
k(k+1)＝p(k+1|k)h
t
[hp(k+1|k)h
t
+r]-1
[0103]
(5)协方差矩阵更新：
[0104]
p(k+1|k+1)＝[i
n-k(k+1)h]p(k+1|k)
[0105]
其中，in为n阶数单位矩阵，x(k+1|k)为k+1时刻的后验状态估计值，x(k|k)为k时刻的后验状态估计值，p(k+1|k)为k时刻的先验估计协方差(x(k+1|x)的协方差)，p(k+1|k+1)为k+1时刻的后验估计协方差，p(k|k)为k时刻的后验估计协方差，k(k+1)为滤波增益矩阵也就是卡尔曼增益，z(k+1)为k+1时刻对应状态的观测值。
[0106]
步骤四，根据本文中小车的运动学方程可知，本专利中目标降落小车的状态向量为：
[0107][0108]
其中，x(k)，y(k)表示目标降落小车在x，y方向上的位置信息。
[0109]
由于视觉识别过程仅能观察到目标降落小车的位置信息，因此观测向量为：
[0110]
z(k)＝[x(k) y(k)]
t
[0111]
则可知观测矩阵为：
[0112][0113]
步骤五，确定状态方程之后，将卡尔曼滤波用于估计目标降落小车的未来运动轨迹。
[0114]
进一步的，根据得到的目标降落小车的预测轨迹，对角度约束下的无人机降落轨迹使用贝塞尔曲线进行动力学和运动学约束，得到适合无人机降落的视觉引导曲线，具体方式如下：
[0115]
步骤一，无人机保持相机与目标降落物之间的着降角度不变，并根据与目标降落物之间的位姿偏差和距离得到预计降落轨迹曲线，然后根据得到的预测轨迹点建立贝塞尔曲线，在预测的轨迹中定义并选择起点和终点，其中每4个点设置一个控制点建立贝塞尔曲线；
[0116][0117]
其中，0《t《1，pi为贝塞尔曲线的参数
[0118]
步骤二，构建问题约束，包括连续性约束、端点约束和动力学约束三部分，具体如下：
[0119]
(1)端点约束：
[0120][0121]
(2)连续性约束：
[0122][0123]
(3)动力学约束：
[0124][0125][0126]
其中，表示第j段第l阶曲线的第i个控制点的加速度，s表示控制点的位置矩阵，用于确定曲线上点的位置，d表示曲线的导数矩阵，n表示无人机的加速度限制，c表示曲线上控制点的曲率。
[0127]
步骤三，使用上述贝塞尔曲线公式计算出曲线上的点的坐标；
[0128]
步骤四，通过软约束将得到的贝塞尔曲线进行进一步优化，具体方式如下：
[0129]
传统的硬约束优化如下：
[0130]
等式约束：
[0131]gi
(x)＝ci[0132]
不等式约束：
[0133]hj
(x)≥dj[0134]
其中，gi(x)是优化函数需要满足的边界条件，hj(x)是优化函数需要满足的区域条件。
[0135]
但由于硬约束方法在基于飞行走廊的情况下生成的轨迹仍然存在较大的碰撞风险，同时对测量噪声也比较敏感，所以本发明采用软约束进行优化，入加权惩罚项：
[0136]
min f(s)+λ1g(x)+λ2h(x)
[0137]
其中，gi(x)是优化函数需要满足的边界条件，hj(x)是优化函数需要满足的区域条件，λ1、λ2表示权重优化系数。
[0138]
建立优化问题：
[0139]
参数化轨迹：
[0140][0141]
上式将带有多个节点的轨迹使用分段多项式进行描述，其中每一段轨迹使用m个时间间隔的n阶分段多项式函数表达，p
nj
为第j段曲线的第n个控制点。
[0142]
构建目标函数：
[0143]
j＝λ1js+λ2jc+λ3jd[0144]
其中，表示光滑代价，用minimum snap形式最小化加速度，fu(t)为参数化轨迹，x,y,z为三个维度上生成的轨迹点，t为轨迹生成周期，表示k阶导数。
[0145]
表示碰撞代价，将对轨迹ds的积分使用每一小段的速度乘以时间dt来代替，c(p(t))表示沿着轨迹点的距离惩罚，c(p(tk))表示每个事件段上的距离惩罚，v(t)表示x、y、z三个方向的速度合并后的无人机的真实速度即三轴速度平方和的算术平方根，t/dt表示将时间t按dt时间间隔分割。
[0146]
jd，表示动力学代价，惩罚速度和加速度超出限制的部分。
[0147]
λ1、λ2、λ3，表示权重优化系数。
[0148]
步骤五，通过贝塞尔曲线优化得到符合动力学约束的降落飞行曲线
[0149]
进一步的，无人机通过角度约束、卡尔曼滤波和贝塞尔曲线优化后得到一条适合无人机进行着降的降落曲线。
[0150]
与现有技术相比，本发明具有以下有益效果：
[0151]
本发明在传统的使用单目视觉进行追踪的基础上加入了卡尔曼滤波预测目标降落物的未来移动轨迹，对于目标降落物被一定程度上遮挡导致无法准确识别降落路径和轨迹的情况有很好的预测效果；除此之外，增加了对角度约束下的降落曲线的贝塞尔优化，通过贝塞尔曲线的优化，使得下降曲线的抖动和震动减小，降落曲线的轨迹也更加符合无人机的安全性约束和动力学约束，更有利于视觉降落时的轨迹跟踪的实现。本发明对提升多旋翼无人机在视觉引导降落上的稳定性和准确性上具有重要的意义。
[0152]
实施例2
[0153]
作为本发明的另一种实施方式，本发明还提供一种多旋翼无人机，该无人机上搭载有单目视觉摄像机、机载执行器等设备。当无人机进行降落时，执行如上实施例所述的基于卡尔曼滤波位姿预测与贝塞尔曲线轨迹优化的无人机自主降落方法。
[0154]
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思做出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

技术特征：
1.一种基于位姿预测和轨迹优化的无人机引导降落方法，其特征在于，所述方法基于无人机的飞行高度和与目标降落物的距离将无人机的引导降落分为远距离引导，中距离跟踪和近距离着降；其中，所述近距离降落过程的具体步骤如下：当无人机在中距离跟踪过程中着降角度达到降落阈值，无人机进入预着降状态；无人机进入预着降状态，获取当前目标降落物的运动状态，通过卡尔曼滤波算法预测目标降落物运动状态；将预测的目标降落物的运动过程与设定的偏航运动角比较进而判断目标降落物的运动状态是否稳定，并判断无人机是否能够进入着降状态；进入着降状态后，保持着降角度不变并根据当前无人机的姿态和动力学结构构建初始降落轨迹；在降落过程中通过贝塞尔曲线不断优化初始降落曲线，找到最优的降落轨迹；无人机识别最终标志物，基于最优的降落轨迹降落到目标降落物上。2.根据权利要求1所述的一种基于位姿预测和轨迹优化的无人机引导降落方法，其特征在于，所述远距离引导过程具体为：无人机获得目标降落物的相对位置，并根据定位导航系统导航飞行至目标物附近。3.根据权利要求1所述的一种基于位姿预测和轨迹优化的无人机引导降落方法，其特征在于，所述中距离跟踪过程具体为：当无人机飞到目标物附近后，通过无人机的视觉相机捕捉目标物的图像信息，通过目标检测核相关滤波kcf算法对目标降落物进行框选跟踪。4.根据权利要求3所述的一种基于位姿预测和轨迹优化的无人机引导降落方法，其特征在于，所述目标降落物设有用于检测识别的二维码；当无人机的视觉相机检测到二维码信息后，将获取到的图像信息输入到目标的检测kcf算法中实现基于相机视觉的目标跟踪，具体步骤如下：选择一个预先设计好的目标模板表示目标降落物，所述目标模板通过方向梯度直方图特征和颜色直方图特征构建；在视觉相机的每一帧图像中，kcf算法将目标模板与当前帧的图像进行相关运算得到相应图，相应图中响应值最高的位置即为目标降落物的当前位置；无人机实时监测并输出目标降落物的位置，无人机通过pid控制算法调整自身的飞行姿态和速度，实现对目标降落物的追踪。5.根据权利要求4所述的一种基于位姿预测和轨迹优化的无人机引导降落方法，其特征在于，所述无人机在进行kcf算法框选追踪的过程中，不断计算并判断摄像头的视线角是否达到降落阈值；如果达到降落阈值，则进入预着降状态。6.根据权利要求1所述的一种基于位姿预测和轨迹优化的无人机引导降落方法，其特征在于，当着降角度达到降落阈值后，无人机通过相机的多帧连续识别，结合卡尔曼滤波来判断目标降落物接下来的运动情况，并进一步判断无人机是否可以进入降落状态，通过卡尔曼滤波预测目标降落物到运动状态具体步骤如下：根据目标降落物过程中的视觉锚点测量情况，构造目标降落物的位姿估计卡尔曼滤波模型，所述卡尔曼滤波模型包括系统状态预测方程和系统观测方程；建立目标降落物的位置表示方程；
通过卡尔曼滤波模型估计目标降落物的未来运动轨迹。7.根据权利要求6所述的一种基于位姿预测和轨迹优化的无人机引导降落方法，其特征在于，所述目标降落物的位置表示为线性方程：x(k+1)＝ax(k)+gw(k)x(k+1)＝ax(k)+gw(k)z(k+1)＝hx(k+1)+v(k+1)其中，k为离散时间；x(k)为k时刻系统状态；a为状态转移矩阵；z(k)为k时刻对应状态的观测值；h为观测矩阵；g为噪声矩阵；w(k)为白噪声；v(k)观测噪声；规定w(k)和v(k)的均值为0，q、r为协方差矩阵；所述卡尔曼滤波器表述如下：状态预测：x(k+1|k)＝ax(k|k)协方差矩阵预测：p(k+1|k)＝ap(k|k)a
t
+gqg
t
状态更新：x(k+1|k+1)＝x(k+1|k)+k(k+1)[z(k+1)-hx(k+1|k)]卡尔曼增益矩阵：k(k+1)＝p(k+1|k)h
t
[hp(k+1|k)h
t
+r]-1
协方差矩阵更新：p(k+1|k+1)＝[i
n-k(k+1)h]p(k+1|k)其中，x(k+1|k)为k+1时刻的后验状态估计值，x(k|k)为k时刻的后验状态估计值，p(k+1|k)为k时刻的先验估计协方差(x(k+1|x)的协方差)，p(k+1|k+1)为k+1时刻的后验估计协方差，p(k|k)为k时刻的后验估计协方差，k(k+1)为滤波增益矩阵也就是卡尔曼增益，z(k+1)为k+1时刻对应状态的观测值，i
n
为n阶单位矩阵。8.根据权利要求1所述的一种基于位姿预测和轨迹优化的无人机引导降落方法，其特征在于，将得到的目标降落物的预测轨迹使用贝塞尔曲线进行动力学和运动学约束，得到无人机降落的视觉引导曲线，具体步骤如下：根据得到的预测轨迹点建立贝塞尔曲线，在预测的轨迹中定义并选择起点和终点，设置控制点并建立贝塞尔曲线：构建问题约束，包括连续性约束、端点约束和动力学约束；使用贝塞尔曲线公式计算出曲线上的点的坐标；通过软约束对贝塞尔曲线的进一步优化；得到符合动力学约束的降落飞行曲线。9.根据权利要求1所述的一种基于位姿预测和轨迹优化的无人机引导降落方法，其特征在于，所述通过软约束对贝塞尔曲线的进一步优化，构建优化目标函数如下：j＝λ1j
s
+λ2j
c
+λ3j
d
其中，表示光滑代价，用minimum snap形式最小化加速度，f
u
(t)为参数化轨迹，x,y,z为三个维度上生成的轨迹点，t为轨迹生成周期，表示k阶导
数；表示碰撞代价，将对轨迹ds的积分使用每一小段的速度乘以时间dt来代替，c(p(t))表示沿着轨迹点的距离惩罚，c(p(t
k
))表示每个事件段上的距离惩罚，v(t)表示x、y、z三个方向的速度合并后的无人机的真实速度，t/dt表示将时间t按dt时间间隔分割；j
d
，表示动力学代价，用于惩罚速度和加速度超出限制的部分；λ1、λ2、λ3，为权重优化系数。10.一种无人机，其特征在于，所述无人机在降落时执行如权利要求1-9任意一项所述的降落方法。

技术总结
本发明涉及一种基于位姿预测和轨迹优化的无人机引导降落方法及无人机，所述方法在近距离降落过程的具体步骤如下：当无人机在中距离跟踪过程中着降角度达到降落阈值，进入预着降状态；获取当前目标降落物的运动状态，通过卡尔曼滤波算法预测目标降落物运动状态；将预测的目标降落物的运动过程与设定的偏航运动角比较，判断目标降落物的运动状态是否稳定以及无人机是否进入着降状态；进入着降状态后，保持着降角度不变进行着降，根据当前的姿态和动力学结构构建初始降落轨迹；通过贝塞尔曲线优化初始降落曲线，找到最优降落轨迹；无人机基于最优降落轨迹降落到目标降落物上。与现有技术相比，本发明提升了多旋翼无人机在视觉引导降落上的稳定性和准确性。导降落上的稳定性和准确性。导降落上的稳定性和准确性。


技术研发人员：苗中华 胡博 修贤超 朴胜杰 李楠 何创新 李云辉
受保护的技术使用者：上海大学
技术研发日：2023.06.09
技术公布日：2023/8/23