[发明专利]无人机悬停震荡判断方法及无人机视觉高度修正方法

说明书

本发明公开了无人机悬停震荡判断方法及无人机视觉高度修正方法。包括如下步骤：S1判断无人机是否处于震荡状态；主要通过开源飞控PX4中EKF算法获得pos_test_ratio标志数据；通过对该标志数据所限定的阈值进行判断震荡情况；S2对无人机震荡进行初步调整；设定调整阈值通过扩展卡尔曼滤波器对处于震荡状态的无人机进行调节，在调整阈值内无人机任然震荡则进入步骤S3进行动态修正高度削弱震荡；S3动态修正估计高度减少震荡；设定高度和次数阈值，进行逐步调整，直至视觉震荡消失。本发明中可以在不依赖外部对地高度传感器的情况下，仅根据无人机IMU预测航迹和实际下视视觉位置反馈来校正无人机当前对地高度，自动调整各个参数，优化控制效果，方法易懂，实施简单。

技术领域

本发明涉及无人机定位领域，具体涉及一种无人机悬停震荡判断方法及无人机视觉高度修正方法。

背景技术

目前，各类无人飞行器(包括多旋翼、垂直起降固定翼等机型，以下统称为：无人机)在空中飞行时普遍使用GPS或北斗定位信息实现悬停和航路中飞行，当缺失GPS/北斗定位信号时，无人机便难以精确悬停和航路飞行。此时，依赖外部视觉信息为无人机提供导航信息是在这种情况下一种可行的解决方案，外部视觉信息辅助无人机悬停时往往依赖下视相机中采集到的图像像素位移、无人机机身姿态和相机对地高度等信息计算无人机对地位移，从而帮助无人机进行稳定悬停。

如果相机标定之后，在无人机平动情况下其对地位移就与真高和特征点像素位移呈线性关系，具体如图1所示。

此时，对地位移的具体计算方法为：Δs＝Δ_pixel*H*k；其中：

而如果此时无人机不仅存在平动，且存在姿态变化，则需要根据当前姿态计算补偿由于姿态变化带来的特征点位移Δl，具体如图2所示。

此时，位姿补偿量的计算方式如下：

其中各个符号所表示的意义如下：

包括平动与旋转运动的对地位移计算公式为：

可以从上述分析中得出：这种计算方法较为依赖对地高度H的估计，若无人机飞行航路始终处于对地等高的航线上则可以正常使用气压高度作为对地高度，但如果无人机悬停位置下方出现沟壑或者建筑等高低起伏的地形或者无人机无法获取准确的对地高度H时，就会使无人机计算对地位移和位姿补偿不准确，例如：飞机水平位置变化产生像素位移，但估计高度比真高偏低，使用该像素位移*估计高度必定会小于真实的对地位移。当观测量不准确时，使用经典PID控制或其他控制方法时会直接影响误差的计算，从而影响控制器的输出以及悬停的精度和稳定性。

发明内容

本发明的一个目的是提供一种无人机悬停震荡判断方法，可以解决上述技术问题中的一个或是多个。

一种无人机悬停震荡判断方法，包括如下步骤：

S1设定一段时间内IMU航迹推算得到的位置认为是真实位置和噪声结合所得到的结果，即：P_{k_est}＝P_k-1+ΔP≈P_real+P_noise，其中P_{k_est}是K时刻IMU航迹推算得到的无人机位置、P_{k_vis}是K时刻视觉观测的无人机位置、P_real是K时刻无人机的真实位置、P_noise是K时刻无人机的视觉观测噪声；P_k-1是K-1时刻无人机的水平位置；ΔP是K-1到K时刻无人机的水平位置变化量；

S2通过开源飞控PX4中EKF算法获得pos_test_ratio标志数据；

S3将步骤S2中所获得的pos_test_ratio标志数据在T时间内、按照频率F记入ringbuffer缓冲区；