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

权利要求书

1.一种无人机悬停震荡判断方法，其特征在于：包括如下步骤：

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将步骤(12)中所获得的pos_test_ratio标志数据在T时间内、按照频率F记入ringbuffer缓冲区；

S4设置pos_test_ratio标志数据的阈值为A，则在T时间内pos_test_ratio标志数据超过A阈值的次数超过F1次，且持续增长趋势；则判定无人机处于震荡状态。

2.根据权利要求1所述的基于负反馈控制的无人机视觉高度修正方法，其特征在于：所述步骤S2中pos_test_ratio标志的计算方法为：

其中innov是视觉位置的革新值，innov_gate是允许的视觉位置革新值的最大误差；innov_var是视觉位置革新值的方差；innov＝P_{k_est}-P_{k_vis}；P_{k_vis}是k时刻视觉观测的无人机位置，P_{k_est}是k时刻IMU航迹推算得到的无人机位置。

3.根据权利要求1所述的基于负反馈控制的无人机视觉高度修正方法，其特征在于：步骤S3中的T时间是60S、频率F是0.5S。

4.一种无人机视觉高度修正方法，其特征在于：包括如下步骤：

(1)判断无人机是否处于震荡状态；

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

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

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

(14)设置pos_test_ratio标志数据的阈值为A，则在T时间内pos_test_ratio标志数据超过A阈值的次数超过F1次，且持续增长趋势；则进入步骤(2)进行噪音调节；

(2)对无人机震荡进行初步调整；

(21)通过扩展卡尔曼滤波器对处于震荡状态的无人机进行调节，每调节一次回到步骤(1)重新判断无人机是否处于震荡状态；

(22)设定调整次数阈值N1；削弱或消除无人机震荡。

5.根据权利要求4所述的无人机视觉高度修正方法，其特征在于：步骤(22)中所述的N1＝1。

6.根据权利要求5所述的无人机视觉高度修正方法，其特征在于：步骤(21)中的具体是在将原先视觉数据观测噪声误差调整到1.5倍时，视觉数据观测噪声带来的最大误差不超过1m。