CN105223575B - 无人机、无人机的测距滤波方法及基于该方法的测距方法 - Google Patents

无人机、无人机的测距滤波方法及基于该方法的测距方法 Download PDF

Info

Publication number
CN105223575B
CN105223575B CN201510698196.0A CN201510698196A CN105223575B CN 105223575 B CN105223575 B CN 105223575B CN 201510698196 A CN201510698196 A CN 201510698196A CN 105223575 B CN105223575 B CN 105223575B
Authority
CN
China
Prior art keywords
distance
unmanned plane
buffer queue
initial
variance
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201510698196.0A
Other languages
English (en)
Other versions
CN105223575A (zh
Inventor
陈有生
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangzhou Xaircraft Technology Co Ltd
Original Assignee
Guangzhou Xaircraft Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guangzhou Xaircraft Technology Co Ltd filed Critical Guangzhou Xaircraft Technology Co Ltd
Priority to CN201510698196.0A priority Critical patent/CN105223575B/zh
Publication of CN105223575A publication Critical patent/CN105223575A/zh
Priority to EP16856909.3A priority patent/EP3367127B1/en
Priority to PCT/CN2016/102726 priority patent/WO2017067478A1/zh
Priority to JP2017535766A priority patent/JP6522761B2/ja
Priority to AU2016342884A priority patent/AU2016342884B2/en
Priority to KR1020177017019A priority patent/KR102000378B1/ko
Priority to ES16856909T priority patent/ES2907520T3/es
Priority to PL16856909T priority patent/PL3367127T3/pl
Application granted granted Critical
Publication of CN105223575B publication Critical patent/CN105223575B/zh
Priority to US15/610,763 priority patent/US10488513B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/50—Systems of measurement, based on relative movement of the target
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/06—Systems determining the position data of a target
    • G01S15/08—Systems for measuring distance only
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64C—AEROPLANES; HELICOPTERS
    • B64C39/00—Aircraft not otherwise provided for
    • B64C39/02—Aircraft not otherwise provided for characterised by special use
    • B64C39/024—Aircraft not otherwise provided for characterised by special use of the remote controlled vehicle type, i.e. RPV
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64D—EQUIPMENT FOR FITTING IN OR TO AIRCRAFT; FLIGHT SUITS; PARACHUTES; ARRANGEMENTS OR MOUNTING OF POWER PLANTS OR PROPULSION TRANSMISSIONS IN AIRCRAFT
    • B64D45/00—Aircraft indicators or protectors not otherwise provided for
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/02—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems using reflection of acoustic waves
    • G01S15/50—Systems of measurement, based on relative movement of the target
    • G01S15/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
    • G01S15/60—Velocity or trajectory determination systems; Sense-of-movement determination systems wherein the transmitter and receiver are mounted on the moving object, e.g. for determining ground speed, drift angle, ground track
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/88—Sonar systems specially adapted for specific applications
    • G01S15/93—Sonar systems specially adapted for specific applications for anti-collision purposes
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/52—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S15/00
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64U—UNMANNED AERIAL VEHICLES [UAV]; EQUIPMENT THEREFOR
    • B64U2201/00—UAVs characterised by their flight controls
    • B64U2201/10—UAVs characterised by their flight controls autonomous, i.e. by navigating independently from ground or air stations, e.g. by using inertial navigation systems [INS]

Abstract

本发明提出一种无人机的测距滤波方法，基于无人机移动过程中速度连续变化的特点，对声呐传感器测量的距离求导，得到无人机的当前移动速度，并对连续速度求方差，通过判断方差的大小确定当前测量的距离是否有效。该方法的总体流程为：首先确定声呐传感器测量的初始距离，其次利用确定的初始距离对新测得的距离进行判断，如果新测得的距离满足条件，则判定新测得的距离有效，且更新初始距离数据，如果新测得的距离不满足条件，则预测一个距离作为本次距离，且不更新初始距离数据。本发明的方法能够有效滤除声呐传感器的测量噪声，提高声呐传感器测量数据的准确性和稳定性。本发明还提出了一种无人机及基于无人机的测距滤波方法的测距方法。

Description

无人机、无人机的测距滤波方法及基于该方法的测距方法
技术领域
本发明涉及无人机技术领域，无人机、无人机的测距滤波方法及基于该方法的测距方法。
背景技术
无人机要实现低空尤其是近地面的自主飞行，除了要知道无人机当前的海拔高度外，还需知道无人机相对于地面的高度。无人机的海拔高度信息一般通过气压计、GPS等测量得到，无人机相对于地面高度，可以使用声呐测距、激光测距、微波雷达测距、以及机器视觉测量方法等方式得到。
激光测距方案容易受到光线的影响且价格成本较高。机器视觉的测距方案算法较为复杂且也容易受到光线的影响。而声呐测距不受光线影响，可全天候使用，且价格成本低廉，系统复杂性低。现有应用声呐测距环境与在无人机上使用的环境截然不同，例如安装在移动的机器人上，或者安装在固定的空间内等等，在此类环境下声呐测传感器测得的数据本身就较为准确，不需要进行复杂的滤波操作。
而无人机环境与其他环境有截然不同的特点，例如：无人机螺旋桨高速转动引起的机身高频振动；螺旋桨转动引起的气流扰动；无人机在飞行过程中的姿态快速反复倾斜变化；螺旋桨在高速转动过程中引起的电源不稳定；无人机对地测距时，地面环境较为复杂等。这些复杂的情况都会给无人机机载声呐测距引入较为严重噪声，甚至会使得测距失败，因此必须针对无人机本身的特点设计适合无人机环境应用的测距滤波算法。
发明内容
本发明旨在至少在一定程度上解决上述相关技术中的技术问题之一。
为此，本发明的一个目的在于提出一种无人机的测距滤波方法，该方法能够滤除无人机环境下声呐传感器的测量噪声，滤波效果好，且无相位延时，提高了声呐传感器数据测量的准确性和稳定性。
本发明的另一个目的在于提供一种无人机。
本发明的第三个目的在于提出一种基于无人机的测距滤波方法的测距方法。
为了实现上述目的，本发明第一方面的实施例提出了一种无人机的测距滤波方法，包括以下步骤：S1：确定无人机的初始距离缓存队列和初始移动速度缓存队列，包括：S11：对利用声呐测距方法在预设时间内测量的连续的N个距离进行求导，以得到所述无人机的N-1个移动速度；S12：求取所述N-1个移动速度的方差；S13：判断所述方差是否小于或等于第一预设值；S14：如果是，则将所述N个距离组成初始距离缓存队列，并将所述N-1个移动速度组成初始移动速度缓存队列；S2：根据初始距离缓存队列和初始速度对当前测量的距离进行滤波，以得到所述无人机的实际飞行距离，包括：S21：将所述初始距离缓存队列中第一个测量得到的距离移出队列，并根据剩余的N-1个距离和当前测量的距离执行所述S11至S12；S22：判断当前求取的方差是否小于或等于第二预设值；S23：如果是，则用所述当前测量的距离替换所述初始距离缓存队列中第一个测量得到的距离，以更新所述初始距离缓存队列，并将所述当前测量的距离作为所述无人机的实际飞行距离。
根据本发明实施例的无人机的测距滤波方法，首先确定声呐传感器测量的初始距离，对初始距离求导，得到无人机当前的移动速度，并且对连续的移动速度求方差，通过判断方差大小确定当前测量的距离是否有效，若当前测量的距离满足条件，则认为当前测量的距离有效，且更新初始距离数据；若当前测量的距离不满足条件，则预测一个距离作为新的当前测量的距离，且不更新初始距离数据。本方法能够滤除无人机环境下声呐传感器的测量噪声，滤波效果好，且无相位延时，提高了声呐传感器数据测量的准确性和稳定性。
另外，根据本发明上述实施例的无人机的测距滤波方法还可以具有如下附加的技术特征：
在一些示例中，在所述步骤S22之后，还包括：S24：如果所述当前求取的方差大于第二预设值，则将所述初始距离缓存队列中第N个测量得到的距离移出队列，并根据剩余的N-1个距离和当前测量的距离执行所述S11和S12，并判断得到的速度方差是否小于或等于第二预设值；S25：如果得到的速度方差小于或等于第二预设值，则用所述当前测量的距离替换所述初始距离缓存队列中第N个测量得到的距离，以更新所述初始距离缓存队列，并将所述当前测量的距离作为所述无人机的实际飞行距离。
在一些示例中，在所述S24之后，还包括：S26：如果得到的速度方差大于所述第二预设值，则将所述当前测量的距离加入新的缓存队列中，并在所述新的缓存队列达到N个距离时，对所述新的缓存队列求导，并求得对应N-1个移动速度的方差，如果所述新的缓存队列对应的N-1个移动速度的方差小于或等于所述第二预设值，则用所述新的缓存队列替换所述初始距离缓存队列，并将所述当前测量的距离作为所述无人机的实际飞行距离。
在一些示例中，在所述S26之后，还包括：S27：如果所述新的缓存队列对应的N-1个移动速度的方差大于所述第二预设值，则忽略所述当前测量的距离，并利用上一次测量的距离和速度得到所述无人机的当前位置估计值，并作为所述无人机的实际飞行距离，其中，所述无人机的当前位置估计值通过如下公式计算：
d_new＝d_pre+v_pre*t，
其中，d_new为所述无人机的当前位置估计值，d_pre为上一次测量的距离，v_pre为上一次测量的速度，t为时间。
，其中，d_new为所述无人机的当前位置估计值，d_pre为上一次测量的距离，v_pre为上一次测量的速度，t为时间。在一些示例中，还包括：当利用声呐测距方法连续测量距离失败的次数大于预定次数，或者连续测量得到的噪声个数大于预定个数，则清空所述初始距离缓存队列，并重新确定无人机的初始距离缓存队列和初始移动速度缓存队列。
在一些示例中，所述第一预设值根据所述无人机的最大加速度参数确定，具体为：
T1<(a*t)2，
其中，T1为所述第一预设值，a为所述无人机的最大加速度，t为所述预设时间。
在一些示例中，所述第二预设值为所述第一预设值的两倍。
在一些示例中，在所述S11之前，还包括：利用声呐测距方法测量得到连续的M个距离，其中，所述M大于所述N；从所述M个距离中提取N个最大距离，并根据所述N个最大距离确定无人机的初始距离缓存队列和初始移动速度缓存队列。
在一些示例中，在所述S13之后，还包括：如果所述方差大于所述第一预设值，则将所述初始距离缓存队列中第一个测量得到的距离移出队列，并将最新测量的距离移入所述初始距离缓存队列，直至所述方差小于或等于所述第一预设值。
本发明第二方面的实施例还提供了一种无人机，包括：声呐传感器，设置在所述无人机上用于测距；测量模块，所述测量模块用于确定无人机的初始距离缓存队列和初始移动速度缓存队列，具体包括：所述测量模块用于对所述声纳传感器在预设时间内测量的连续的N个距离进行求导，以得到所述无人机的N-1个移动速度，并求取所述N-1个移动速度的方差，并判断所述方差是否小于或等于第一预设值，以及在所述方差小于或等于所述第一预设值时，将所述N个距离组成初始距离缓存队列，并将所述N-1个移动速度组成初始移动速度缓存队列；滤波模块，所述滤波模块用于根据初始距离缓存队列和初始速度对当前测量的距离进行滤波，以得到所述无人机的实际飞行距离，具体包括：所述滤波模块将所述初始距离缓存队列中第一个测量得到的距离移出队列，并对剩余的N-1个距离和当前测量的距离进行求导，得到N-1个移动速度，并求取所述N-1个移动速度的第一方差，并判断所述第一方差是否小于或等于第二预设值，并在所述第一方差小于或等于第二预设值时，用所述当前测量的距离替换所述初始距离缓存队列中第一个测量得到的距离，以更新所述初始距离缓存队列，并将所述当前测量的距离作为所述无人机的实际飞行距离。
根据本发明实施例的无人机，首先确定声呐传感器测量的初始距离，对初始距离求导，得到无人机当前的移动速度，并且对连续的移动速度求方差，通过判断方差大小确定当前测量的距离是否有效，若当前测量的距离满足条件，则认为当前测量的距离有效，且更新初始距离数据；若当前测量的距离不满足条件，则预测一个距离作为新的当前测量的距离，且不更新初始距离数据。本发明能够滤除无人机环境下声呐传感器的测量噪声，滤波效果好，且无相位延时，提高了声呐传感器数据测量的准确性和稳定性。
另外，根据本发明上述实施例的无人机还可以具有如下附加的技术特征：
在一些示例中，所述滤波模块还用于在所述第一方差大于第二预设值时，将所述初始距离缓存队列中第N个测量得到的距离移出队列，并根据剩余的N-1个距离和当前测量的距离求取第二方差，并在所述二方差小于或等于所述第二预设值时，用所述当前测量的距离替换所述初始距离缓存队列中第N个测量得到的距离，以更新所述初始距离缓存队列，并将所述当前测量的距离作为所述无人机的实际飞行距离。
在一些示例中，所述滤波模块还用于在所述第二方差大于所述第二预设值时，将所述当前测量的距离加入新的缓存队列中，并在所述新的缓存队列达到N个距离时，对所述新的缓存队列求导，并求得对应N-1个移动速度的方差，并在所述新的缓存队列对应的N-1个移动速度的方差小于或等于所述第二预设值时，用所述新的缓存队列替换所述初始距离缓存队列，并将所述当前测量的距离作为所述无人机的实际飞行距离。
在一些示例中，所述滤波模块还用于在所述新的缓存队列对应的N-1个移动速度的方差大于所述第二预设值时，忽略所述当前测量的距离，并利用上一次测量的距离和速度得到所述无人机的当前位置估计值，并作为所述无人机的实际飞行距离，其中，所述无人机的当前位置估计值通过如下公式计算：
d_new＝d_pre+v_pre*t，
其中，d_new为所述无人机的当前位置估计值，d_pre为上一次测量的距离，v_pre为上一次测量的速度，t为时间。
在一些示例中，所述测量模块还用于在利用声呐测距方法连续测量距离失败的次数大于预定次数，或者连续测量得到的噪声个数大于预定个数时，清空所述初始距离缓存队列，并重新确定无人机的初始距离缓存队列和初始移动速度缓存队列。
在一些示例中，所述第一预设值根据所述无人机的最大加速度参数确定，具体为：
T1<(a*t)2，
其中，T1为所述第一预设值，a为所述无人机的最大加速度，t为所述预设时间。
在一些示例中，所述第二预设值为所述第一预设值的两倍。
在一些示例中，所述测量模块还用于利用声呐测距方法测量得到连续的M个距离，其中，所述M大于所述N，并从所述M个距离中提取N个最大距离，并根据所述N个最大距离确定无人机的初始距离缓存队列和初始移动速度缓存队列。
在一些示例中，所述滤波模块还用于在所述测量模块得到的所述方差大于所述第一预设值时，将所述初始距离缓存队列中第一个测量得到的距离移出队列，并将最新测量的距离移入所述初始距离缓存队列，直至所述方差小于或等于所述第一预设值。
本发明第三方面的实施例还提出了一种基于无人机的测距滤波方法的测距方法，包括以下步骤：通过所述无人机的声纳传感器在预设时间内获取连续的M个距离，并从所述M个距离中提取N个最大距离，其中，所述M大于所述N；根据所述连续的N个距离确定所述无人机的初始距离缓存队列和初始移动速度缓存队列；根据所述初始距离缓存队列和初始速度对当前测量的距离进行滤波，以得到所述无人机的实际飞行距离。
根据本发明实施例的基于无人机的测距滤波方法的测距方法，首先确定声呐传感器测量的初始距离，对初始距离求导，得到无人机当前的移动速度，并且对连续的移动速度求方差，通过判断方差大小确定当前测量的距离是否有效，若当前测量的距离满足条件，则认为当前测量的距离有效，且更新初始距离数据；若当前测量的距离不满足条件，则预测一个距离作为新的当前测量的距离，且不更新初始距离数据。本发明能够滤除无人机环境下声呐传感器的测量噪声，滤波效果好，且无相位延时，提高了声呐传感器数据测量的准确性和稳定性。
本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：
图1是根据本发明一个实施例的无人机的测距滤波方法的总体流程图；
图2是根据本发明一个实施例的无人机的测距滤波方法的详细流程图；
图3是根据本发明一个具体实施例的初始距离缓存队列和初始移动速度缓存队列的确定方法示意图；
图4是根据本发明一个实施例的无人机相对于地面距离示意图；
图5是根据本发明一个具体实施例的对当前测量的距离进行滤波的方法的流程图；
图6是根据本发明一个实施例的无人机的结构框图；以及
图7是根据本发明一个实施例的基于无人机的测距滤波方法的测距方法。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
以下结合附图描述根据本发明实施例的无人机的测距滤波方法的测距方法、无人机及基于无人机的测距滤波方法的测距方法的测距方法。
图1是根据本发明一个实施例的无人机的测距滤波方法的总体流程图。如图1所示，本发明的方法主要基于无人机移动过程中速度连续变化(如无人机在升高、下降或水平移动过程中速度连续变化)的特点，对声呐传感器测量的距离信息求导，得到无人机当前的移动速度，并且对连续速度求方差，通过判断方差的大小来决定当前测量的距离是否有效。该方法的总体框架为：首先确定初始测量的距离，其次利用确定的初始距离对新测得的距离进行判断，若新测得的距离满足条件，则认为新测得的距离有效，且更新初始距离数据；若新测得距离不满足条件，则预测一个距离作为本次距离，且不更新初始距离数据。若连续多次测量失败或测量得到的数据均不满足条件，则重新确定初始距离。需要说明的是，本发明中所描述的测距都是指声呐传感器安装在无人机的底部，测量无人机相对于地面的距离，但是本发明实施例的方法不局限于测量对地的距离，只要在无人机上用声呐测距，本发明实施例的方法均适用。
图2是根据本发明一个实施例的无人机的测距滤波方法的详细流程图。如图2所示，该方法具体包括以下步骤：
步骤S1：确定无人机的初始距离缓存队列和初始移动速度缓存队列。具体包括以下步骤：
步骤S11：对利用声呐测距方法在预设时间内测量的连续的N个距离进行求导，以得到无人机的N-1个移动速度。
步骤S12：求取N-1个移动速度的方差。
步骤S13：判断该方差是否小于或等于第一预设值。
步骤S14：如果是，则将N个距离组成初始距离缓存队列，并将N-1个移动速度组成初始移动速度缓存队列。
进一步地，在步骤S13之后，还包括：如果方差大于第一预设值，则将初始距离缓存队列中第一个测量得到的距离移出队列，并将最新测量的距离移入初始距离缓存队列，直至方差小于或等于第一预设值。
作为具体的实施例，以下结合图3对确定无人机的初始距离缓存队列和初始移动速度缓存队列的过程进行进一步地描述。
实施例1
结合图3所示，在本实施例中，N例如为5。即首先测得按时间顺序排列的连续5次距离数据组成一个队列，其中，5次测量中若有测量失败，则忽略掉测量失败的数据，在预设时间内累计够5次测量的距离数据，若在预设时间内未累积够5次测量的数据，则删除时间较早的数据，直到累积够5次测量的数据为止。然后，对5次测量的距离进行求导，得出对应的4个移动速度，并对这4个移动速度求方差。由于无人机在移动过程中速度始终是连续变化的，也就是说无人机的移动速度不会有阶跃性的突变，所以无人机在短时间内速度的方差必定趋近于零。若求得连续4次移动速度方差小于或等于一个第一预设值T1，则认为当前5次测量中没有噪声，且把当前的5个距离作为后续滤波的初始距离缓存队列，当前的4个速度作为后续滤波的初始移动速度缓存队列。若求得连续4次移动速度方差大于第一预设值T1，则认为当前5次测量数据中存在噪声，则从初始距离缓存队列中移除时间最早(最先测量得到)的1个数据，且加入1个新测量的数据到初始距离缓存队列中得到新组成的距离缓存队列，对新组成的距离缓存队列中的5个距离数据求导得到4个速度，判断这4个速度的方差与第一预设值T1，直到得到的速度方差小于或等于T1为止。
进一步地，在本发明的一个实施例中，在步骤S11之前，还包括：利用声呐测距方法测量得到连续的M个距离，其中，M大于N；然后，从M个距离中提取N个最大距离，并根据N个最大距离确定无人机的初始距离缓存队列和初始移动速度缓存队列。以下结合图3，以具体的实施例对该过程进行详细描述。
实施例2
具体地说，声呐测距的原理是发射端发射一簇特定频率的波，发射波遇到障碍物反射到接收端，接收端通过计算从发射到接收到反射波之间的时间差计算得到距离信息。在测量过程中若噪声出现在回波之前，且噪声的频率与发射波的频率相同，则此次测量数据有可能为噪声；若噪声出现在回波之后，由于接收端收到真正的回波，噪声对测量没有干扰；如果噪声与回波重叠，或相互叠加，则可能导致回波的频率与发射的频率不同，从而使得测量失败，另外由于距离太远、无人机倾斜角度太大、或者反射面吸收发射波，都会导致接收端接收不到回波，从而使得测量失败。在实际应用过程中，噪声在绝大多数情况下都出现在回波之前，使得测量的距离在大多数情况下都比实际距离偏小。若在测量过程中没有真正的回波被接收到，只存在与发射波频率相同，且数量相似的噪声，则测量的距离有可能比实际距离偏大，也可能偏小，但此类情况在实际应用中较少出现。综上所述，在声呐测量数据实际情况中，噪声一般都比实际距离偏小，但是无论噪声偏大还是偏小，只要在测量数据中夹杂着噪声，其速度的方差都较大。基于以上分析和实际测试的情况，由于噪声在绝大多数情况都比实际测量距离偏小，所以在确定初始距离时，若测得连续10(即M)次距离数据中利用实施例1中描述的方法均不能确定出初始距离和速度，说明在10次测量中未有连续的5个正确距离数据，则在10个数据中选取5个最大的数据，对选取的5(即N)个最大的数据做与实施例1中描述的方法相同的处理来确定初始距离缓存队列和初始移动速度缓存队列。
综上，确定初始距离缓存队列和初始移动速度缓存队列的方法有两种，分别为实施例1和实施例2中所描述的两种方法。其中，两种方法的主要区别在于：实施例1中描述的方法是对连续的5次测量距离数据进行分析，且5个距离数据不能夹杂噪声，才能确定出初始距离缓存队列和初始移动速度缓存队列。实施例2中描述的方法在10次距离数据中选取5个最大的距离数据进行分析，5个最大的数据若不存在噪声就可确定初始距离缓存队列和初始移动速度缓存队列，对这5个数据没有连续性要求。这两种方法同时使用，只要其中一种方法满足条件就可以确定初始距离缓存队列和初始移动速度缓存队列。从实际应用中来看，实施例1中描述的方法主要适用于测量距离较短的环境，实施例2中描述的方法主要适用于测量距离较远的环境。这是由于在距离较近时，测量得到的噪声较少，数据连续性较好且测量失败几率较小；距离较远时测量得到的噪声较大，数据连续性较差，且可能出现连续的噪声或者测量失败的情况。
步骤S2：根据初始距离缓存队列和初始移动速度缓存队列对当前测量的距离进行滤波。具体包括以下步骤：
步骤S21：将初始距离缓存队列中最先测量得到的距离出列，并根据剩余的N-1个距离和当前测量的距离执行步骤S11至步骤S12。
步骤S22：判断当前求取的方差是否小于或等于第二预设值。
步骤S23：如果是，也即当前求取的方差小于或等于第二预设值，则用当前测量的距离替换初始距离缓存队列中第一个测量得到的距离，以更新初始距离缓存队列，并将当前测量的距离作为无人机的实际飞行距离。
其中，在本发明的一个实施例中，在步骤S22之后，还包括：
步骤S24：如果当前求取的方差大于第二预设值，则将初始距离缓存队列中第N个测量得到的距离移出队列，并根据剩余的N-1个距离和当前测量的距离执行步骤S11和步骤S12，并判断得到的速度方差是否小于或等于第二预设值。
步骤S25：如果步骤S24中得到的速度方差小于或等于第二预设值，则用当前测量的距离替换初始距离缓存队列中第N个测量得到的距离，以更新初始距离缓存队列，并将当前测量的距离作为无人机的实际飞行距离。
进一步地，在步骤S24之后，例如还包括：
步骤S26：如果得到的速度方差大于第二预设值，则将当前测量的距离加入新的缓存队列中，并在新的缓存队列达到N个距离时，对新的缓存队列求导，并求得对应N-1个移动速度的方差，如果新的缓存队列对应的N-1个移动速度的方差小于或等于第二预设值，则用新的缓存队列替换初始距离缓存队列，并将当前测量的距离作为无人机的实际飞行距离。
在步骤S26之后，例如还包括：
步骤S27：如果新的缓存队列对应的N-1个移动速度的方差大于第二预设值，则忽略当前测量的距离，并利用上一次测量的距离和速度得到无人机的当前位置估计值，并将该当前位置估计值作为无人机的实际距离。其中，在一些示例中，例如通过如下公式计算无人机的当前位置估计值，该公式为：
d_new＝d_pre+v_pre*t，
其中，d_new为无人机的当前位置估计值，d_pre为上一次测量的距离，v_pre为上一次测量的速度，t为时间。
为了便于理解本发明，以下结合图4和图5，以具体的实施例对本发明上述实施例对当前测量的距离进行滤波的过程进行更为详细的描述。
实施例3
具体地说，结合图5所示，在实施例1和实施例2中已确定初始距离缓存队列和初始移动速度缓存队列，假定确定初始距离缓存队列为D，D由按时间顺序排列的5个历史测量距离D[1]…D[5]组成。每次新测量得到一个距离d后，将初始距离缓存队列中的D[1]，也就是最先测量得到的缓存数据(距离)出列，新测得的距离d入列组成新的队列，对新的队列中5个数据进行求导，计算出对应的4个速度，然后计算4个速度的方差，若方差小于或等于第二预设值T2，则认为当前新测得的数据d有效，d就为无人机当前的距离，且更新缓存队列D。若4个速度的方差大于第二预设值T2，则存在以下三种可能：
1.当前距离为真实距离，上一次测的距离可能为噪声，且此噪声与真实的距离非常接近，导致计算出的速度方差略小于第二预设值T2，从而使得本次计算出的速度方差稍大于第二预设值T2。
2.无人机测得距离实际存在阶跃式的突变，例如图4所示，由于初始距离缓存队列中的距离和当前距离本身存在较大的差，所以计算出速度的方差必定大于第二预设值T2。
3.当前测得的距离为噪声，计算出来的速度方差大于第二预设值T2。
针对以上三种不同情况做不同处理：第一种情况中，由于可能是上一次测量的数据为噪声，也就是初始距离缓存队列中D[5]可能为噪声，所以将初始距离缓存队列中的D[5]出列，由缓存数据D[1],D[2],D[3]D[4]和当前的距离d组成新的队列，并计算此队列速度的方差，若计算得到的速度方差小于第二预设值T2，则在初始距离缓存队列中用当前的距离d替换缓存D[5]的值，以更新初始距离缓存队列，且无人机当前的距离为d。若计算得到速度方差仍然大于第二预设值T2，则有可能为第二种情况，例如图4所示，无人机由图4中的位置A移动到位置B，或者由位置B移动到位置A，此时由于实际距离的突变，将新测得的距离d加入到初始距离缓存队列D中计算出来的速度方差始终会大于第二预设值T2。因此，将此时测得的数据d加入到一个新的缓存队列L中，并且由上一次测的距离和上一次的速度估算一个新的位置作为无人机本次的位置值，具体计算公式如下：
d_new＝d_pre+v_pre*t (1)，
其中，d_new无人机本次的位置值，d_pre前一次距离，v_pre为前一次速度，t为时间。
进一步地，如果新的缓存队列L数据达到5个，对新的缓存队列L中的数据进行求导，计算出对应速度方差，如果方差小于第二预设值T2，则将新的缓存队列L的数据复制到原初始距离缓存队列D中，接受本次测量的距离为无人机当前的距离。若新的缓存队列L中数据计算得到的速度方差与原初始距离缓存队列D中计算得到速度的方差均大于第二预设值T2，则为第三种情况，也即当前测得的距离数据为噪声，则忽略掉本次测量的值，利用前一次测量的距离和速度估算出一个新的距离作为本次的距离，例如上述公式(1)所示。原初始距离缓存队列D中的数据不更新，新的缓存队列L中的L[1]出队，当前测得的数据d入队，此时新的缓存队列，就相当于重新确定初始位置的过程。一旦有数据加入到原初始距离缓存队列D中，计算得到的速度方差小于第二预设值T2，则清空新的缓存队列L，具体算法流程图例如图5所示。
在本发明的一个实施例中，该方法例如还包括：当利用声呐测距方法连续测量距离失败的次数大于预定次数，或者连续测量得到的噪声个数大于预定个数，则清空初始距离缓存队列，并重新确定无人机的初始距离缓存队列和初始移动速度缓存队列。结合图5所示，预定次数例如为但不限于20次。也就是说，当声呐传感器连续20次都测量失败，或者连续测得的噪声个数超过预定个数时，则清空原初始距离缓存队列D的数据，此时需要依据实施例1和实施例2所描述的方法重新确定初始距离缓存队列和初始移动速度缓存队列。
在本发明上述的实施例中，涉及到两个速度方差的阈值T1和T2，其中第一预设值T1为确定初始位置和初始移动速度的速度方差阈值，第二预设值T2为依据初始距离对当前测得距离滤波时用到的阈值。由于本方法中初始位置和初始移动速度的确定对后续的滤波非常重要，只有在初始位置和速度确定正确的情况下，后续的滤波算法才有效，因此选取适合的第一预设值T1非常的重要。而在实际的飞行过程中，无人机的高度可能会频繁变化，或者地面会有高低起伏，会导致无人机短时间内的速度方差会有波动，但此波动都在一定范围内。在本发明的一个实施例中，例如，第一预设值根据无人机的最大加速度参数确定，具体为：
T1<(a*t)2，
其中，T1为第一预设值，a为无人机的最大加速度，t为所述预设时间。
进一步地，第二预设值T2例如为第一预设值T1的两倍。
综上，根据本发明实施例的无人机的测距滤波方法，首先确定声呐传感器测量的初始距离，对初始距离求导，得到无人机当前的移动速度，并且对连续的移动速度求方差，通过判断方差的大小来确定当前测量的距离是否有效，若当前测量的距离满足条件，则认为当前测量的距离有效，且更新初始距离数据；若当前测量的距离不满足条件，则预测一个距离作为新的当前测量的距离，且不更新初始距离数据。本方法能够滤除无人机环境下声呐传感器的测量噪声，滤波效果好，且无相位延时，提高了声呐传感器数据测量的准确性和稳定性。
本发明的进一步实施例还提供了一种无人机。
图6是根据本发明一个实施例的无人机的结构框图。如图6所示，该无人机100包括：声呐传感器110、测量模块120和滤波模块130。
其中，声呐传感器110设置在无人机上用于测距。
测量模块120用于确定无人机的初始距离缓存队列和初始移动速度缓存队列，具体包括：测量模块120用于对声纳传感器在预设时间内测量的连续的N个距离进行求导，以得到无人机的N-1个移动速度，并求取N-1个移动速度的方差，并判断方差是否小于或等于第一预设值，以及在方差小于或等于第一预设值时，将N个距离组成初始距离缓存队列，并将N-1个移动速度组成初始移动速度缓存队列。
在本发明的一个实施例中，测量模块120还用于在声纳传感器110连续测量距离失败的次数大于预定次数，或者连续测量得到的噪声个数大于预定个数时，清空初始距离缓存队列，并重新确定无人机的初始距离缓存队列和初始移动速度缓存队列。
进一步地，在一些示例中，测量模块120例如还用于利用声呐传感器110测量得到连续的M个距离，其中，M大于N，并从M个距离中提取N个最大距离，并根据N个最大距离确定无人机的初始距离缓存队列和初始移动速度缓存队列。
滤波模块130用于根据初始距离缓存队列和初始速度对当前测量的距离进行滤波，以得到无人机的实际飞行距离，具体包括：滤波模块130将初始距离缓存队列中第一个测量得到的距离移出队列，并对剩余的N-1个距离和当前测量的距离进行求导，得到N-1个移动速度，并求取N-1个移动速度的第一方差，并判断第一方差是否小于或等于第二预设值，并在第一方差小于或等于第二预设值时，用当前测量的距离替换初始距离缓存队列中第一个测量得到的距离，以更新初始距离缓存队列，并将当前测量的距离作为无人机的实际飞行距离。
进一步地，在本发明的一个实施例中，滤波模块130还用于在第一方差大于第二预设值时，将初始距离缓存队列中第N个测量得到的距离移出队列，并根据剩余的N-1个距离和当前测量的距离求取第二方差，并在二方差小于或等于第二预设值时，用当前测量的距离替换初始距离缓存队列中第N个测量得到的距离，以更新初始距离缓存队列，并将当前测量的距离作为无人机的实际飞行距离。
进一步地，滤波模块130例如还用于在第二方差大于第二预设值时，将当前测量的距离加入新的缓存队列中，并在新的缓存队列达到N个距离时，对新的缓存队列求导，并求得对应N-1个移动速度的方差，并在新的缓存队列对应的N-1个移动速度的方差小于或等于第二预设值时，用新的缓存队列替换初始距离缓存队列，并将当前测量的距离作为无人机的实际飞行距离。
进一步地，滤波模块130例如还用于在新的缓存队列对应的N-1个移动速度的方差大于第二预设值时，忽略当前测量的距离，并利用上一次测量的距离和速度得到无人机的当前位置估计值，并作为无人机的实际飞行距离，其中，
无人机的当前位置估计值通过如下公式计算：
d_new＝d_pre+v_pre*t，
其中，d_new为无人机的当前位置估计值，d_pre为上一次测量的距离，v_pre为上一次测量的速度，t为时间。
进一步地，滤波模块130还用于在测量模块120得到的方差大于第一预设值时，将初始距离缓存队列中第一个测量得到的距离移出队列，并将最新测量的距离移入初始距离缓存队列，直至方差小于或等于第一预设值。
在本发明的一个实施例中，上述的第一预设值例如根据无人机的最大加速度参数确定，具体为：
T1<(a*t)2，
其中，T1为第一预设值，a为无人机的最大加速度，t为预设时间。
进一步地，第二预设值T2例如为第一预设值T1的两倍。
需要说明的是，本发明实施例的无人机的具体实现方式与本发明实施例的无人机的测距滤波方法的具体实现方式类似，具体请参见方法部分的描述，为了减少冗余，此处不做赘述。
综上，根据本发明实施例的无人机，首先确定声呐传感器测量的初始距离，对初始距离求导，得到无人机当前的移动速度，并且对连续的移动速度求方差，通过判断方差大小确定当前测量的距离是否有效，若当前测量的距离满足条件，则认为当前测量的距离有效，且更新初始距离数据；若当前测量的距离不满足条件，则预测一个距离作为新的当前测量的距离，且不更新初始距离数据。本发明能够滤除无人机环境下声呐传感器的测量噪声，滤波效果好，且无相位延时，提高了声呐传感器数据测量的准确性和稳定性。
本发明的进一步实施例还提出了一种基于本发明上述实施例所描述的无人机的测距滤波方法的测距方法。
图7为根据本发明一个实施例的基于无人机的测距滤波方法的测距方法的流程图。如图7所示，该方法包括以下步骤：
步骤S101：通过无人机的声纳传感器在预设时间内获取连续的M个距离，并从M个距离中提取N个最大距离，其中，M大于N。
步骤S102：根据连续的N个距离确定无人机的初始距离缓存队列和初始移动速度缓存队列。
步骤S103：根据初始距离缓存队列和初始速度对当前测量的距离进行滤波，以得到无人机的实际飞行距离。
需要说明的是，本发明实施例的基于无人机的测距滤波方法的测距方法是基于本发明上述实施例的无人机的测距滤波方法的测距方法实现的，因此，该基于无人机的测距滤波方法的测距方法的具体实现方式与本发明实施例的无人机的测距滤波方法的具体实现方式类似，具体请参见对无人机的测距滤波方法部分的描述，为了减少冗余，此处不做赘述。
综上，根据本发明实施例的基于无人机的测距滤波方法的测距方法，首先确定声呐传感器测量的初始距离，对初始距离求导，得到无人机当前的移动速度，并且对连续的移动速度求方差，通过判断方差大小确定当前测量的距离是否有效，若当前测量的距离满足条件，则认为当前测量的距离有效，且更新初始距离数据；若当前测量的距离不满足条件，则预测一个距离作为新的当前测量的距离，且不更新初始距离数据。本发明能够滤除无人机环境下声呐传感器的测量噪声，滤波效果好，且无相位延时，提高了声呐传感器数据测量的准确性和稳定性。
在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”、“顺时针”、“逆时针”、“轴向”、“径向”、“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
在本发明中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。而且，第一特征在第二特征“之上”、“上方”和“上面”可是第一特征在第二特征正上方或斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”可以是第一特征在第二特征正下方或斜下方，或仅仅表示第一特征水平高度小于第二特征。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (20)

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)2，
其中，T1为所述第一预设值，a为所述无人机的最大加速度，t为所述预设时间。
7.根据权利要求6所述的无人机的测距滤波方法，其特征在于，所述第二预设值为所述第一预设值的两倍。
8.根据权利要求1所述的无人机的测距滤波方法，其特征在于，在所述S11之前，还包括：
利用声呐测距方法测量得到连续的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所述的无人机的测距滤波装置，其特征在于，所述第一预设值根据所述无人机的最大加速度参数确定，具体为：
T1<(a*t)2，
其中，T1为所述第一预设值，a为所述无人机的最大加速度，t为所述预设时间。
16.根据权利要求15所述的无人机的测距滤波装置，其特征在于，所述第二预设值为所述第一预设值的两倍。
17.根据权利要求10所述的无人机的测距滤波装置，其特征在于，所述测量模块还用于利用所述声呐传感器测量得到连续的M个距离，其中，所述M大于所述N，并从所述M个距离中提取N个最大距离，并根据所述N个最大距离确定无人机的初始距离缓存队列和初始移动速度缓存队列。
18.根据权利要求10所述的无人机的测距滤波装置，其特征在于，所述滤波模块还用于在所述测量模块得到的所述方差大于所述第一预设值时，将所述初始距离缓存队列中第一个测量得到的距离移出队列，并将最新测量的距离移入所述初始距离缓存队列，直至所述方差小于或等于所述第一预设值。
19.一种无人机，其特征在于，包括如权利要求10-18任一项所述的无人机的测距滤波装置。
20.一种基于权利要求1所述的无人机的测距滤波方法的测距方法，其特征在于，包括以下步骤：
通过所述无人机的声呐传感器在预设时间内获取连续的M个距离，并从所述M个距离中提取N个最大距离，其中，所述M大于所述N；
根据所述连续的N个距离确定所述无人机的初始距离缓存队列和初始移动速度缓存队列；
根据所述初始距离缓存队列和初始速度对当前测量的距离进行滤波，以得到所述无人机的实际飞行距离。
CN201510698196.0A 2015-10-22 2015-10-22 无人机、无人机的测距滤波方法及基于该方法的测距方法 Active CN105223575B (zh)

Priority Applications (9)

Application Number Priority Date Filing Date Title
CN201510698196.0A CN105223575B (zh) 2015-10-22 2015-10-22 无人机、无人机的测距滤波方法及基于该方法的测距方法
AU2016342884A AU2016342884B2 (en) 2015-10-22 2016-10-20 Unmanned aerial vehicle, method and apparatus for filtering in ranging of the same, and ranging method
PCT/CN2016/102726 WO2017067478A1 (zh) 2015-10-22 2016-10-20 无人机及其测距滤波装置、方法及基于该方法的测距方法
JP2017535766A JP6522761B2 (ja) 2015-10-22 2016-10-20 無人航空機及びその距離測定フィルター装置、方法及び当該方法に基づいた距離測定方法
EP16856909.3A EP3367127B1 (en) 2015-10-22 2016-10-20 Unmanned aerial vehicle (uav) and distance measuring and filtering device and method thereof and distance measurement method based on same
KR1020177017019A KR102000378B1 (ko) 2015-10-22 2016-10-20 무인기 및 무인기의 거리 측정 필터링 장치, 방법 및 상기 방법에 기반한 거리 측정 방법
ES16856909T ES2907520T3 (es) 2015-10-22 2016-10-20 Vehículo aéreo no tripulado (UAV) y dispositivo de medición de distancia y filtrado y su procedimiento y procedimiento de medición de distancia basado en el mismo
PL16856909T PL3367127T3 (pl) 2015-10-22 2016-10-20 Bezzałogowy statek powietrzny (uav) i jego urządzenie oraz sposób do pomiaru i filtrowania odległości oraz bazujący na nim sposób pomiaru odległości
US15/610,763 US10488513B2 (en) 2015-10-22 2017-06-01 Unmanned aerial vehicle, method and apparatus for filtering in ranging of the same, and ranging method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510698196.0A CN105223575B (zh) 2015-10-22 2015-10-22 无人机、无人机的测距滤波方法及基于该方法的测距方法

Publications (2)

Publication Number Publication Date
CN105223575A CN105223575A (zh) 2016-01-06
CN105223575B true CN105223575B (zh) 2016-10-26

Family

ID=54992632

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510698196.0A Active CN105223575B (zh) 2015-10-22 2015-10-22 无人机、无人机的测距滤波方法及基于该方法的测距方法

Country Status (9)

Country Link
US (1) US10488513B2 (zh)
EP (1) EP3367127B1 (zh)
JP (1) JP6522761B2 (zh)
KR (1) KR102000378B1 (zh)
CN (1) CN105223575B (zh)
AU (1) AU2016342884B2 (zh)
ES (1) ES2907520T3 (zh)
PL (1) PL3367127T3 (zh)
WO (1) WO2017067478A1 (zh)

Families Citing this family (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105223575B (zh) 2015-10-22 2016-10-26 广州极飞科技有限公司 无人机、无人机的测距滤波方法及基于该方法的测距方法
US10908621B2 (en) * 2016-06-17 2021-02-02 Rakuten, Inc. Unmanned aerial vehicle control system, unmanned aerial vehicle control method, and program
CN109254279B (zh) * 2017-07-13 2021-04-06 广州极飞科技有限公司 声呐测距过程中的滤波方法及装置，执行设备
CN109254292B (zh) * 2017-07-13 2021-06-15 广州极飞科技股份有限公司 声呐测距方法及装置，执行设备
CN112504320B (zh) * 2017-08-01 2022-11-08 广州极飞科技股份有限公司 数据有效性检测的方法及装置
JP2019057185A (ja) * 2017-09-22 2019-04-11 カシオ計算機株式会社 飛行装置、飛行方法及びプログラム
CN107942333B (zh) * 2017-10-13 2020-03-10 中国南方电网有限责任公司调峰调频发电公司 测距声呐的数据处理方法
CN107907857B (zh) * 2017-10-25 2021-08-17 深圳大学 一种基于uwb的实时定位方法及定位装置
CN110389593B (zh) * 2018-04-23 2022-12-02 北京京东乾石科技有限公司 物流无人机的抛货控制方法、系统、设备和存储介质
CN110442142B (zh) * 2018-05-02 2022-12-27 北京京东尚科信息技术有限公司 速度数据处理方法、装置、电子设备及计算机可读介质
CN111190168B (zh) * 2018-11-14 2022-01-11 中国科学院声学研究所 一种侧扫声纳的姿态稳定方法
CN109630901B (zh) * 2018-11-29 2020-07-24 北京市燃气集团有限责任公司 一种中低压庭院管线异常流量识别方法及装置
JP6896962B2 (ja) * 2019-12-13 2021-06-30 エスゼット ディージェイアイ テクノロジー カンパニー リミテッドＳｚ Ｄｊｉ Ｔｅｃｈｎｏｌｏｇｙ Ｃｏ．，Ｌｔｄ 決定装置、飛行体、決定方法、及びプログラム
CN112762893B (zh) * 2021-04-06 2021-06-25 北京三快在线科技有限公司 无人机状态确定方法、装置、介质、电子设备及无人机
CN112764018A (zh) * 2021-04-08 2021-05-07 北京三快在线科技有限公司 一种测距方法、装置、存储介质及电子设备
CN113191557B (zh) * 2021-05-08 2024-02-02 李仕嘉 一种铁饼飞行距离预测方法及智能铁饼

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103365297A (zh) * 2013-06-29 2013-10-23 天津大学 基于光流的四旋翼无人机飞行控制方法
CN104567799A (zh) * 2014-11-28 2015-04-29 天津大学 基于多传感器信息融合的小型旋翼无人机高度测量方法
CA2871280A1 (en) * 2013-12-05 2015-06-05 Honeywell International Inc. Unmanned aircraft systems sense and avoid sensor fusion track initialization

Family Cites Families (27)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB8904884D0 (en) * 1989-03-03 1996-09-25 Marconi Gec Ltd Ranging systems
JPH0527031A (ja) * 1991-07-17 1993-02-05 Japan Aviation Electron Ind Ltd 高度測定装置
JP2573135Y2 (ja) * 1992-04-24 1998-05-28 株式会社明電舎 移動距離検出器
IL120787A (en) * 1997-05-05 2003-02-12 Rafael Armament Dev Authority Tracking system that includes means for early target detection
KR200257148Y1 (ko) * 2001-07-10 2001-12-24 (주)한양 3차원 위치 측정 장치
US6573791B2 (en) * 2001-09-28 2003-06-03 Koninklijke Philips Electronics N.V. Multipath nested Gm nested miller compensation
US20050114023A1 (en) * 2003-11-26 2005-05-26 Williamson Walton R. Fault-tolerant system, apparatus and method
US9534899B2 (en) * 2005-03-25 2017-01-03 Irobot Corporation Re-localization of a robot for slam
JP2007101526A (ja) * 2005-09-06 2007-04-19 Sony Corp 速度検出装置、速度検出方法、速度検出プログラム、位置検出装置、位置検出方法及び位置検出プログラム並びにナビゲーション装置
US7768873B2 (en) * 2007-03-14 2010-08-03 Lockheed Martin Corporation Combined spatial-temporal correlation SONAR
US8583915B1 (en) * 2007-05-31 2013-11-12 Bby Solutions, Inc. Security and authentication systems and methods for personalized portable devices and associated systems
US20100152933A1 (en) * 2008-12-11 2010-06-17 Honeywell International Inc. Apparatus and method for unmanned aerial vehicle ground proximity detection, landing and descent
KR101118358B1 (ko) * 2010-03-29 2012-02-28 (주)나노포인트 칼만 필터를 이용한 가속도 센서 바이어스 계산시스템
FR2961601B1 (fr) * 2010-06-22 2012-07-27 Parrot Procede d'evaluation de la vitesse horizontale d'un drone, notamment d'un drone apte au vol stationnaire autopilote
US8981995B2 (en) * 2011-06-03 2015-03-17 Microsoft Technology Licensing, Llc. Low accuracy positional data by detecting improbable samples
CN102798857B (zh) * 2012-07-27 2014-07-02 奇瑞汽车股份有限公司 测量相对速度的方法、雷达传感器以及汽车
US9321510B2 (en) * 2013-03-15 2016-04-26 Hadal, Inc. Systems and methods for deploying autonomous underwater vehicles from a ship
KR20140122002A (ko) * 2013-04-09 2014-10-17 엘지전자 주식회사 거리 측정 장치 및 동작 방법
CN103365295B (zh) * 2013-06-29 2015-09-30 天津大学 基于dsp的四旋翼无人飞行器自主悬停控制系统及方法
JP2015024705A (ja) * 2013-07-25 2015-02-05 有限会社Ｇｅｎ Ｃｏｒｐｏｒａｔｉｏｎ 小型電動ヘリコプタの自動離着陸制御方法
US9606028B2 (en) * 2014-02-14 2017-03-28 Nutech Ventures Aerial water sampler
US9710925B2 (en) * 2014-06-08 2017-07-18 The Board Of Trustees Of The Leland Stanford Junior University Robust anytime tracking combining 3D shape, color, and motion with annealed dynamic histograms
CN105336332A (zh) * 2014-07-17 2016-02-17 杜比实验室特许公司 分解音频信号
US9759561B2 (en) * 2015-01-06 2017-09-12 Trx Systems, Inc. Heading constraints in a particle filter
US9766336B2 (en) * 2015-03-16 2017-09-19 Here Global B.V. Vehicle obstruction detection
CN104820434A (zh) * 2015-03-24 2015-08-05 南京航空航天大学 一种无人机对地面运动目标的测速方法
CN105223575B (zh) * 2015-10-22 2016-10-26 广州极飞科技有限公司 无人机、无人机的测距滤波方法及基于该方法的测距方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103365297A (zh) * 2013-06-29 2013-10-23 天津大学 基于光流的四旋翼无人机飞行控制方法
CA2871280A1 (en) * 2013-12-05 2015-06-05 Honeywell International Inc. Unmanned aircraft systems sense and avoid sensor fusion track initialization
CN104567799A (zh) * 2014-11-28 2015-04-29 天津大学 基于多传感器信息融合的小型旋翼无人机高度测量方法

Also Published As

Publication number Publication date
AU2016342884A1 (en) 2017-06-29
PL3367127T3 (pl) 2022-05-02
KR102000378B1 (ko) 2019-07-15
CN105223575A (zh) 2016-01-06
US20170269207A1 (en) 2017-09-21
WO2017067478A1 (zh) 2017-04-27
EP3367127A4 (en) 2019-05-29
EP3367127B1 (en) 2022-01-19
ES2907520T3 (es) 2022-04-25
KR20170088388A (ko) 2017-08-01
JP2018511778A (ja) 2018-04-26
AU2016342884B2 (en) 2018-03-29
JP6522761B2 (ja) 2019-05-29
EP3367127A1 (en) 2018-08-29
US10488513B2 (en) 2019-11-26

Similar Documents

Publication Publication Date Title
CN105223575B (zh) 无人机、无人机的测距滤波方法及基于该方法的测距方法
CN103287358B (zh) 用于确定物体传感器未对准的方法
JP5895162B2 (ja) 交通事故検出装置及び交通事故検出方法
JP5269024B2 (ja) 路面状況推定装置および路面状況推定方法
KR100854766B1 (ko) 거리 센서를 이용한 주차 공간 검출 방법
EP3751302A1 (en) Methods and systems for acoustic machine perception for an aircraft
US20150203107A1 (en) Autonomous vehicle precipitation detection
US20160103208A1 (en) System for filtering lidar data in vehicle and method thereof
CN109444916B (zh) 一种无人驾驶可行驶区域确定装置及方法
CN111324120A (zh) 一种自动驾驶前车切入切出场景提取方法
CN104732799A (zh) 一种车位状态检测方法及装置
JP6939378B2 (ja) 車両制御装置
CN102980589A (zh) 一种通过gps速度自动计算车辆脉冲系数的方法及装置
CN106080397A (zh) 自适应巡航系统及车载设备
CN111169476B (zh) 一种运动趋势预测方法、装置、控制器及汽车
CN112396868A (zh) 碰撞预警实现方法、系统、计算机设备及存储介质
KR101756717B1 (ko) 잦은 변속을 억제하는 도로의 형상 인식을 통한 예측 변속 제어 방법
CN110458080A (zh) 一种汽车行驶中前方坑洞的预判方法及系统
CN110426714A (zh) 一种障碍物识别方法
CN202130447U (zh) 一种新型的车道线偏离检测装置
CN108254775A (zh) 车载导航系统及其实现方法
CN111959515B (zh) 一种基于视觉检测的前向目标选择方法、装置及系统
JP7215460B2 (ja) 地図システム、地図生成プログラム、記憶媒体、車両用装置およびサーバ
KR20140036025A (ko) 차선 판정 장치, 차선 판정 방법 및 차선 판정용 컴퓨터 프로그램
CN110494762A (zh) 周边监视雷达装置

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
CB02 Change of applicant information

Address after: 510000 Guangdong, Guangzhou, Tianhe District Cheng Cheng Road, No. 3A01, 1

Applicant after: Guangzhou flying Technology Co., Ltd.

Address before: 510000 Guangdong, Guangzhou, Tianhe District Cheng Cheng Road, No. 3A01, 1

Applicant before: GUANGZHOU XAIRCRAFT ELECTRONIC TECHNOLOGY CO., LTD.

COR Change of bibliographic data
C14 Grant of patent or utility model
GR01 Patent grant
CP03 Change of name, title or address
CP03 Change of name, title or address

Address after: 510000 Block C, 115 Gaopu Road, Tianhe District, Guangzhou City, Guangdong Province

Patentee after: Guangzhou Jifei Technology Co.,Ltd.

Address before: 510000 3A01, Cheng Cheng Road, 1, Guangdong, Guangzhou, Tianhe District

Patentee before: Guangzhou Xaircraft Technology Co.,Ltd.