CN113885546A - 无人机悬停震荡判断方法及无人机视觉高度修正方法 - Google Patents

Abstract

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

Description

无人机悬停震荡判断方法及无人机视觉高度修正方法

技术领域

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

背景技术

目前，各类无人飞行器(包括多旋翼、垂直起降固定翼等机型，以下统称为：无人机)在空中飞行时普遍使用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缓冲区；

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

进一步的：所述步骤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航迹推算得到的无人机位置。

进一步的：步骤S3中的T时间是60S、频率F是0.5S。

本发明的另一个目的是提供一种无人机视觉高度修正方法，可以解决上述技术问题中的一个或是多个。

为了达到上述目的，本发明提出的技术方案如下：

一种无人机视觉高度修正方法，包括如下步骤：

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

(11)设定一段时间内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时刻无人机的水平位置变化量；

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

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

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

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

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

(22)设定调整次数阈值N1；在调整次数阈值N1时无人机任然震荡则进入步骤(3)进行动态修正高度削弱震荡。

进一步的：步骤(22)中所述的N1＝1。

进一步的：步骤(21)中的具体是在将原先视觉数据观测噪声误差调整到1.5倍时，视觉数据观测噪声带来的最大误差不超过1m。

(3)动态修正估计高度减少震荡

(31)设定修正高度阈值H；设定修正高度阈值H的次数N2；

(32)按照修正高度阈值H逐次调节，每次调节之后重复步骤(1)—步骤(2)，直至视觉震荡消失。

进一步的：所述步骤(12)中pos_test_ratio标志的计算方法为：

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

进一步的：步骤(13)中的T时间是60S、频率F是0.5S。

进一步的：所述步骤(3)中的修正高度阈值H＝1m；修正高度阈值H的次数N2＝20次。

本发明的技术效果是：

本发明能够快速稳定由于无人机对地高度估计不准确导致的悬停位置震荡情况。

本发明中可以在不依赖外部对地高度传感器的情况下，仅根据无人机IMU预测航迹和实际下视视觉位置反馈来校正无人机当前对地高度，即基于负反馈的方式自动调整各个参数，优化控制效果，方法易懂，实施简单。

该方法不仅能够辅助无人机修正对地高度，还能够快速稳定由于无人机对地高度估计不准确导致的悬停位置震荡情况。

附图说明

构成本申请的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

在附图中：

图1是视觉特征点像素位移与无人机对地位移的关系。

图2是姿态补偿特征点像素位移示意图。

图3是ringbuffer数据存储示意图。

图4是位置震荡判断流程示意图。

图5是EKF噪声调整流程示意图。

图6是动态高度修正流程示意图。

具体实施方式

下面将结合附图以及具体实施例来详细说明本发明，其中的示意性实施例以及说明仅用来解释本发明，但并不作为对本发明的不当限定。

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施方式例如能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

为了便于描述，在这里可以使用空间相对术语，如“在……之上”、“在……上方”、“在……上表面”、“上面的”等，用来描述如在图中所示的一个器件或特征与其他器件或特征的空间位置关系。应当理解的是，空间相对术语旨在包含除了器件在图中所描述的方位之外的在使用或操作中的不同方位。例如，如果附图中的器件被倒置，则描述为“在其他器件或构造上方”或“在其他器件或构造之上”的器件之后将被定位为“在其他器件或构造下方”或“在其他器件或构造之下”。因而，示例性术语“在……上方”可以包括“在……上方”和“在……下方”两种方位。该器件也可以其他不同方式定位(旋转90度或处于其他方位)，并且对这里所使用的空间相对描述作出相应解释。

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

S1设定一段时间内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时刻无人机的水平位置变化量；

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

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

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

本判断无人机震荡状态的方法简单，60S内判断一次，与EKF相关性好,判断准确、快速。在实际使用无人时，使用本方法快速判断无人机震荡之后，可以使用任意方式对无人机状态进行调整，以让其悬停状态稳定。

一种无人机视觉高度修正方法，包括如下步骤：

如图4所示，(1)判断无人机是否处于震荡状态；

(11)在IMU精度较高且静差较小的情况下，假定去除所有可同时干扰IMU位置预测和视觉位置观测的外部干扰因素外(如：风速变化，外部接触等)，设定一段时间内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时刻无人机的水平位置变化量；

(12)通过开源飞控PX4中EKF算法获得pos_test_ratio标志数据；所述步骤(12)中pos_test_ratio标志的计算方法为：

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

在这里选择的是pos_test_ratio标志数据作为震荡的判定，而该标志数据在在PX4的算法中，该标志主要用于帮助EKF估计器判断是否接收外部观测，其通过计算方差和新息来表征预测数据和观测数据之间的差异情况，使用innov_gate进行归一化处理，当预测数据接近观测数据时，该值会接近0，当预测与观测相差较大时，该值会接近1，当预测与观测之间超过允许误差时，该值超过1且EKF拒绝观测该数据。

而在本步骤中，采用这个标志作为震荡判定的主要原因是由于：发明人发现无人机实际悬停中出现震荡后其误差大小与视觉高度的估计误差有一定相关性，且该震荡的周期较长，通常是在40s至90s之间，由于pos_test_ratio标志数据和飞控代码中的EKF算法在同一周期内计算的，刷新频率高，且在位置出现震荡的波峰和波谷处有更加明显的特征，不但符合震荡周期的时间，准确性也较高，具有快速、准确的效果；所以更适合作为检测震荡的特征使用。

(13)将步骤(12)中所获得的pos_test_ratio标志数据在T时间内、按照频率F记入ringbuffer(循环缓冲区)缓冲区。如图3所示。

在这里，T和F的具体值可以根据实际情况进行调整和限定，在这里根据计算和缓冲频率，步骤(13)中的T时间是60S、频率F是0.5S。这样在60S内进行一次震荡判断，进而做出调节，这样可以既可以避免频繁的调节造成的EKF融合过程中发生不收敛的情况，同时可以快速进行震荡调节，实现快速准确的要求。

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

如图5所示，(2)对无人机震荡进行初步调整；

(21)通过扩展卡尔曼滤波器对处于震荡状态的无人机进行调节，每调节一次回到步骤S1重新判断无人机是否处于震荡状态；在这里由于在T时间内只进行一次震荡的判断，因此调节过程也是在T时间内调整一次。

进一步的：所述步骤(2)中的扩展卡尔曼滤波器调整过程在此不做详细阐述，对于本领域的技术人员来说是常识。

而在本文中，使用的是PX4中的EKF算法，未对该部分代码做任何修改，而只是根据它输出的一些指标动态调整EKF的一些参数。在本方法中对EKF的其他参数和输入变量不做调整，只是根据位置震荡的检测结果对参数EKF_EVP_NOISE(在这里，EKF_EVP_NOISE与上文中的P_noise代表含义相同，而在EKF调节过程中中并非直接使用该数据，而是将该数据作为观测噪声矩阵中的一个元素进行运算，具体的运算过程属于本领域技术人员公知，在此不做进一步阐述)做动态调整，因此这个EKF_EVP_NOISE是一个人为给定的参数；参数的实际值是根据实际情况进行设定的。

在EKF中，观测噪声由R确定，通过调整R矩阵或者矩阵中的元素，可以调整EKF在估计过程中对各个传感器输出数据的置信度。

在某些实施例中，具体是调整为原先视觉数据观测噪声误差大小的1.5倍(150％)时，视觉数据观测噪声带来的最大误差不超过1m；否则循环步骤(1)-(2)直至震荡收敛或是进入步骤(3)。

(22)设定调整次数阈值N1(60S内只有一次的震荡判断，因此调整阈值在60S内是1次，优选的是N1＝1)；在调整次数阈值N1时无人机任然震荡则进入步骤(3)进行动态修正高度削弱震荡；

通过本次调整，在大部分情况下会降低无人机位置估计的误差，当位置估计器反馈给控制器的观测量准确度提高时，控制器相应调整无人机姿态进行位置控制的效果也会明显改善，由此直接提高其在空中悬停的位置稳定性。

在步骤(2)的基础上增加了步骤(3)，主要用于当无人机高度估计出现10m以上的误差时使用。由于无人机使用视觉像素位移估计对地位移，飞行高度越高，高度估计出现误差的概率和范围也会越大，实际测试中当航线高于100m时，无人机仅依赖视觉估计得到的高度误差普遍大于10m，此时使用上述方案想要使位置控制收敛就变得困难且往往需要较长的时间才能使悬停位置震荡收敛到较小的范围内，如果此时无人机附近有较高建筑物或已经续航时间不足的话很容易发生碰撞坠机或低电量迫降的事故。因此额外增加步骤S3以保证飞行安全性。

如图6所示，(3)动态修正估计高度减少震荡

(31)设定修正高度阈值H；设定修正高度阈值H的次数N2。在这里，H和N2的具体值可以根据实际情况进行调整和限定，在这里根据最大误差的设定和经验，将其中所述步骤(3)中的修正高度阈值H＝1m；修正高度阈值H的次数N2＝20次。

(32)按照修正高度阈值H逐次调节，每次调节之后重复步骤(1)—步骤(2)，直至视觉震荡消失。

本发明中的技术方案首先通过步骤(1)判断无人机在丢失GPS信号后仅依靠视觉定位的情况下是否出现位置震荡来间接判断此时无人机对地高度高度估计的误差大小，若检测到该误差较大，且这个误差影响到了无人机的悬停精度时采用步骤(2)进行初步的处理，如果结果步骤(2)之后该现象依旧未能改善时使用步骤(3)方案直接对对地高度做修正，以此间接影响水平悬停位置精度。

该方法不仅能够辅助无人机修正对地高度，还能够快速稳定由于无人机对地高度估计不准确导致的悬停位置震荡情况。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

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。

7.根据权利要求6所述的无人机视觉高度修正方法，其特征在于：还包括步骤(3)动态修正估计高度减少震荡；在调整次数阈值N1时无人机任然震荡则进入步骤(3)进行动态修正高度削弱或消除震荡；

步骤步骤(3)动态修正视觉高度

(31)设定修正高度阈值H；设定修正高度阈值H的次数N2；

(32)按照修正高度阈值H逐次调节，每次调节之后重复步骤(1)—步骤(2)，直至视觉震荡消失。

8.根据权利要求7所述的无人机视觉高度修正方法，其特征在于：所述步骤(3)中的修正高度阈值H＝1m；修正高度阈值H的次数N2＝20次。

9.根据权利要求4所述的无人机视觉高度修正方法，其特征在于：所述步骤(12)中pos_test_ratio标志的计算方法：

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

10.根据权利要求4所述的无人机视觉高度修正方法，其特征在于：步骤(13)中的T时间是60S、频率F是0.5S。

Images