无人机、无人机的测距滤波方法及基于该方法的测距方法
技术领域
本发明涉及无人机技术领域,无人机、无人机的测距滤波方法及基于该方法的测距方法。
背景技术
无人机要实现低空尤其是近地面的自主飞行,除了要知道无人机当前的海拔高度外,还需知道无人机相对于地面的高度。无人机的海拔高度信息一般通过气压计、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:根据初始距离缓存队列和初始速度对当前测量的距离进行滤波,以得到无人机的实际飞行距离。
需要说明的是,本发明实施例的基于无人机的测距滤波方法的测距方法是基于本发明上述实施例的无人机的测距滤波方法的测距方法实现的,因此,该基于无人机的测距滤波方法的测距方法的具体实现方式与本发明实施例的无人机的测距滤波方法的具体实现方式类似,具体请参见对无人机的测距滤波方法部分的描述,为了减少冗余,此处不做赘述。
综上,根据本发明实施例的基于无人机的测距滤波方法的测距方法,首先确定声呐传感器测量的初始距离,对初始距离求导,得到无人机当前的移动速度,并且对连续的移动速度求方差,通过判断方差大小确定当前测量的距离是否有效,若当前测量的距离满足条件,则认为当前测量的距离有效,且更新初始距离数据;若当前测量的距离不满足条件,则预测一个距离作为新的当前测量的距离,且不更新初始距离数据。本发明能够滤除无人机环境下声呐传感器的测量噪声,滤波效果好,且无相位延时,提高了声呐传感器数据测量的准确性和稳定性。
在本发明的描述中,需要理解的是,术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”、“顺时针”、“逆时针”、“轴向”、“径向”、“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明和简化描述,而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明的限制。
此外,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中,“多个”的含义是至少两个,例如两个,三个等,除非另有明确具体的限定。
在本发明中,除非另有明确的规定和限定,术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解,例如,可以是固定连接,也可以是可拆卸连接,或成一体;可以是机械连接,也可以是电连接;可以是直接相连,也可以通过中间媒介间接相连,可以是两个元件内部的连通或两个元件的相互作用关系,除非另有明确的限定。对于本领域的普通技术人员而言,可以根据具体情况理解上述术语在本发明中的具体含义。
在本发明中,除非另有明确的规定和限定,第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触,或第一和第二特征通过中间媒介间接接触。而且,第一特征在第二特征“之上”、“上方”和“上面”可是第一特征在第二特征正上方或斜上方,或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”可以是第一特征在第二特征正下方或斜下方,或仅仅表示第一特征水平高度小于第二特征。
在本说明书的描述中,参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中,对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且,描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外,在不相互矛盾的情况下,本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
尽管上面已经示出和描述了本发明的实施例,可以理解的是,上述实施例是示例性的,不能理解为对本发明的限制,本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。