[发明专利]无人机、无人机的测距滤波方法及基于该方法的测距方法

权利要求书

1.一种无人机的测距滤波方法，其特征在于，包括以下步骤：

S1：确定无人机的初始距离缓存队列和初始移动速度缓存队列，包括：

S11：对利用声呐测距方法在预设时间内测量的连续的N个距离进行求导，以得到所述无人机的N-1个移动速度；

S12：求取所述N-1个移动速度的方差；

S13：判断所述方差是否小于或等于第一预设值；

S14：如果是，则将所述N个距离组成初始距离缓存队列，并将所述N-1个移动速度组成初始移动速度缓存队列；

S2：根据初始距离缓存队列和初始移动速度缓存队列对当前测量的距离进行滤波，以得到所述无人机的实际飞行距离，包括：

S21：将所述初始距离缓存队列中第一个测量得到的距离移出队列，并根据剩余的N-1个距离和当前测量的距离执行所述S11至S12；

S22：判断当前求取的方差是否小于或等于第二预设值；

S23：如果是，则用所述当前测量的距离替换所述初始距离缓存队列中第一个测量得到的距离，以更新所述初始距离缓存队列，并将所述当前测量的距离作为所述无人机的实际飞行距离。

2.根据权利要求1所述的无人机的测距滤波方法，其特征在于，在所述步骤S22之后，还包括：

S24：如果所述当前求取的方差大于第二预设值，则将所述初始距离缓存队列中第N个测量得到的距离移出队列，并根据剩余的N-1个距离和当前测量的距离执行所述S11和S12，并判断得到的速度方差是否小于或等于第二预设值；

S25：如果得到的速度方差小于或等于第二预设值，则用所述当前测量的距离替换所述初始距离缓存队列中第N个测量得到的距离，以更新所述初始距离缓存队列，并将所述当前测量的距离作为所述无人机的实际飞行距离。

3.根据权利要求2所述的无人机的测距滤波方法，其特征在于，在所述S24之后，还包括：

S26：如果得到的速度方差大于所述第二预设值，则将所述当前测量的距离加入新的缓存队列中，并在所述新的缓存队列达到N个距离时，对所述新的缓存队列求导，并求得对应N-1个移动速度的方差，如果所述新的缓存队列对应的N-1个移动速度的方差小于或等于所述第二预设值，则用所述新的缓存队列替换所述初始距离缓存队列，并将所述当前测量的距离作为所述无人机的实际飞行距离。

4.根据权利要求3所述的无人机的测距滤波方法，其特征在于，在所述S26之后，还包括：

S27：如果所述新的缓存队列对应的N-1个移动速度的方差大于所述第二预设值，则忽略所述当前测量的距离，并利用上一次测量的距离和速度得到所述无人机的当前位置估计值，并作为所述无人机的实际飞行距离，其中，

所述无人机的当前位置估计值通过如下公式计算：

d_new＝d_pre+v_pre*t，

其中，d_new为所述无人机的当前位置估计值，d_pre为上一次测量的距离，v_pre为上一次测量的速度，t为时间。

5.根据权利要求1所述的无人机的测距滤波方法，其特征在于，还包括：

当利用声呐测距方法连续测量距离失败的次数大于预定次数，或者连续测量得到的噪声个数大于预定个数，则清空所述初始距离缓存队列，并重新确定无人机的初始距离缓存队列和初始移动速度缓存队列。

6.根据权利要求1所述的无人机的测距滤波方法，其特征在于，所述第一预设值根据所述无人机的最大加速度参数确定，具体为：

T1<(a*t)²，

其中，T1为所述第一预设值，a为所述无人机的最大加速度，t为所述预设时间。

7.根据权利要求6所述的无人机的测距滤波方法，其特征在于，所述第二预设值为所述第一预设值的两倍。

8.根据权利要求1所述的无人机的测距滤波方法，其特征在于，在所述S11之前，还包括：

利用声呐测距方法测量得到连续的M个距离，其中，所述M大于所述N；

从所述M个距离中提取N个最大距离，并根据所述N个最大距离确定无人机的初始距离缓存队列和初始移动速度缓存队列。