缓存队列，包括: 511:对利用声呐测距方法在预设时间内测量的连续的N个距离进行求导，以得到所述无人机的N-1个移动速度； 512:求取所述N-1个移动速度的方差； 513:判断所述方差是否小于或等于第一预设值； S14:如果是，则将所述N个距离组成初始距离缓存队列，并将所述N-1个移动速度组成初始移动速度缓存队列； S2:根据初始距离缓存队列和初始移动速度缓存队列对当前测量的距离进行滤波，以得到所述无人机的实际飞行距离，包括: 521:将所述初始距离缓存队列中第一个测量得到的距离移出队列，并根据剩余的N-1个距离和当前测量的距离执行所述Sll至S12 ; 522:判断当前求取的方差是否小于或等于第二预设值； 523:如果是，则用所述当前测量的距离替换所述初始距离缓存队列中第一个测量得到的距离，以更新所述初始距离缓存队列，并将所述当前测量的距离作为所述无人机的实际飞行距离。2.根据权利要求1所述的无人机的测距滤波方法，其特征在于，在所述步骤S22之后，还包括: 524:如果所述当前求取的方差大于第二预设值，则将所述初始距离缓存队列中第N个测量得到的距离移出队列，并根据剩余的N-1个距离和当前测量的距离执行所述Sll和S12，并判断得到的速度方差是否小于或等于第二预设值； 525:如果得到的速度方差小于或等于第二预设值，则用所述当前测量的距离替换所述初始距离缓存队列中第N个测量得到的距离，以更新所述初始距离缓存队列，并将所述当前测量的距离作为所述无人机的实际飞行距离。3.根据权利要求2所述的无人机的测距滤波方法，其特征在于，在所述S24之后，还包括: 526:如果得到的速度方差大于所述第二预设值，则将所述当前测量的距离加入新的缓存队列中，并在所述新的缓存队列达到N个距离时，对所述新的缓存队列求导，并求得对应N-1个移动速度的方差，如果所述新的缓存队列对应的N-1个移动速度的方差小于或等于所述第二预设值，则用所述新的缓存队列替换所述初始距离缓存队列，并将所述当前测量的距离作为所述无人机的实际飞行距离。4.根据权利要求3所述的无人机的测距滤波方法，其特征在于，在所述S26之后，还包括: 527:如果所述新的缓存队列对应的N-1个移动速度的方差大于所述第二预设值，则忽略所述当前测量的距离，并利用上一次测量的距离和速度得到所述无人机的当前位置估计值，并作为所述无人机的实际飞行距离，其中， 所述无人机的当前位置估计值通过如下公式计算: d—new = d—pre+v—pre*t， 其中，d_new为所述无人机的当前位置估计值，d_pre为上一次测量的距离，v_pre为上一次测量的速度，t为时间。5.根据权利要求1所述的无人机的测距滤波方法，其特征在于，还包括: 当利用声呐测距方法连续测量距离失败的次数大于预定次数，或者连续测量得到的噪声个数大于预定个数，则清空所述初始距离缓存队列，并重新确定无人机的初始距离缓存队列和初始移动速度缓存队列。6.根据权利要求1所述的无人机的测距滤波方法，其特征在于，所述第一预设值根据所述无人机的最大加速度参数确定，具体为:Tl〈(a*t)2， 其中，Tl为所述第一预设值，a为所述无人机的最大加速度，t为所述预设时间。7.根据权利要求6所述的无人机的测距滤波方法，其特征在于，所述第二预设值为所述第一预设值的两倍。8.根据权利要求1所述的无人机的测距滤波方法，其特征在于，在所述Sll之前，还包括: 利用声呐测距方法测量得到连续的M个距离，其中，所述M大于所述N ; 从所述M个距离中提取N个最大距离，并根据所述N个最大距离确定无人机的初始距离缓存队列和初始移动速度缓存队列。9.根据权利要求1所述的无人机的测距滤波方法，其特征在于，在所述S13之后，还包括: 如果所述方差大于所述第一预设值，则将所述初始距离缓存队列中第一个测量得到的距离移出队列，并将最新测量的距离移入所述初始距离缓存队列，直至所述方差小于或等于所述第一预设值。10.一种无人机，其特征在于，包括: 声呐传感器，设置在所述无人机上用于测距； 测量模块，所述测量模块用于确定无人机的初始距离缓存队列和初始移动速度缓存队列，具体包括:所述测量模块用于对所述声纳传感器在预设时间内测量的连续的N个距离进行求导，以得到所述无人机的N-1个移动速度，并求取所述N-1个移动速度的方差，并判断所述方差是否小于或等于第一预设值，以及在所述方差小于或等于所述第一预设值时，将所述N个距离组成初始距离缓存队列，并将所述N-1个移动速度组成初始移动速度缓存队列； 滤波模块，所述滤波模块用于根据初始距离缓存队列和初始速度对当前测量的距离进行滤波，以得到所述无人机的实际飞行距离，具体包括:所述滤波模块将所述初始距离缓存队列中第一个测量得到的距离移出队列，并对剩余的N-1个距离和当前测量的距离进行求导，得到N-1个移动速度，并求取所述N-1个移动速度的第一方差，并判断所述第一方差是否小于或等于第二预设值，并在所述第一方差小于或等于第二预设值时，用所述当前测量的距离替换所述初始距离缓存队列中第一个测量得到的距离，以更新所述初始距离缓存队列，并将所述当前测量的距离作为所述无人机的实际飞行距离。11.根据权利要求10所述的无人机，其特征在于，所述滤波模块还用于在所述第一方差大于第二预设值时，将所述初始距离缓存队列中第N个测量得到的距离移出队列，并根据剩余的N-1个距离和当前测量的距离求取第二方差，并在所述二方差小于或等于所述第二预设值时，用所述当前测量的距离替换所述初始距离缓存队列中第N个测量得到的距离，以更新所述初始距离缓存队列，并将所述当前测量的距离作为所述无人机的实际飞行距离。12.根据权利要求11所述的无人机，其特征在于，所述滤波模块还用于在所述第二方差大于所述第二预设值时，将所述当前测量的距离加入新的缓存队列中，并在所述新的缓存队列达到N个距离时，对所述新的缓存队列求导，并求得对应N-1个移动速度的方差，并在所述新的缓存队列对应的N-1个移动速度的方差小于或等于所述第二预设值时，用所述新的缓存队列替换所述初始距离缓存队列，并将所述当前测量的距离作为所述无人机的实际飞行距离。13.根据权利要求12所述的无人机，其特征在于，所述滤波模块还用于在所述新的缓存队列对应的N-1个移动速度的方差大于所述第二预设值时，忽略所述当前测量的距离，并利用上一次测量的距离和速度得到所述无人机的当前位置估计值，并作为所述无人机的实际飞行距离，其中， 所述无人机的当前位置估计值通过如下公式计算:d—new = d—pre+v—pre*t， 其中，d_new为所述无人机的当前位置估计值，d_pre为上一次测量的距离，v_pre为上一次测量的速度，t为时间。14.根据权利要求10所述的无人机，其特征在于，所述测量模块还用于在所述声纳传感器连续测量距离失败的次数大于预定次数，或者连续测量得到的噪声个数大于预定个数时，清空所述初始距离缓存队列，并重新确定无人机的初始距离缓存队列和初始移动速度缓存队列。15.根据权利要求10所述的无人机，其特征在于，所述第一预设值根据所述无人机的最大加速度参数确定，具体为:Tl〈(a*t)2， 其中，Tl为所述第一预设值，a为所述无人机的最大加速度，t为所述预设时间。16.根据权利要求15所述的无人机，其特征在于，所述第二预设值为所述第一预设值的两倍。17.根据权利要求10所述的无人机，其特征在于，所述测量模块还用于利用所述声纳传感器测量得到连续的M个距离，其中，所述M大于所述N，并从所述M个距离中提取N个最大距离，并根据所述N个最大距离确定无人机的初始距离缓存队列和初始移动速度缓存队列。18.根据权利要求10所述的无人机，其特征在于，所述滤波模块还用于在所述测量模块得到的所述方差大于所述第一预设值时，将所述初始距离缓存队列中第一个测量得到的距离移出队列，并将最新测量的距离移入所述初始距离缓存队列，直至所述方差小于或等于所述第一预设值。19.一种基于权利要求1所述的无人机的测距滤波方法的测距方法，其特征在于，包括以下步骤: 通过所述无人机的声纳传感器在预设时间内获取连续的M个距离，并从所述M个距离中提取N个最大距离，其中，所述M大于所述N ; 根据所述连续的N个距离确定所述无人机的初始距离缓存队列和初始移动速度缓存队列； 根据所述初始距离缓存队列和初始速度对当前测量的距离进行滤波，以得到所述无人机的实际飞行距离。
【专利摘要】本发明提出一种无人机的测距滤波方法，基于无人机移动过程中速度连续变化的特点，对声呐传感器测量的距离求导，得到无人机的当前移动速度，并对连续速度求方差，通过判断方差的大小确定当前测量的距离是否有效。该方法的总体流程为：首先确定声呐传感器测量的初始距离，其次利用确定的初始距离对新测得的距离进行判断，如果新测得的距离满足条件，则判定新测得的距离有效，且更新初始距离数据，如果新测得的距离不满足条件，则预测一个距离作为本次距离，且不更新初始距离数据。本发明的方法能够有效滤除声呐传感器的测量噪声，提高声呐传感器测量数据的准确性和稳定性。本发明还提出了一种无人机及基于无人机的测距滤波方法的测距方法。
【IPC分类】G01S7/52, G01S15/08
【公开号】CN105223575
【申请号】CN201510698196
【发明人】陈有生
【申请人】广州极飞电子科技有限公司
【公开日】2016年1月6日
【申请日】2015年10月22日