一种无人机上TOF距离传感器的测距方法及系统与流程

本发明属于距离测量技术领域，尤其涉及一种无人机上tof距离传感器的测距方法及系统。

背景技术：

随着无人机技术的快速发展，无人机中室内定位技术已越来越重要了。近十多年以来，为了解决室内定位“最后一公里”的问题，各科研机构以及创业公司在室内定位方面展开了大量的研究，旨在找到一套良好的室内定位解决方案。

现有的定位系统主要有四种：

第一种：基于gps或者北斗导航定位系统，其主要应用在室外定位领域。该定位系统不适合应用于室内，其室内定位误差很大。

第二种：基于wifi的定位系统，其便利性极强。但由于该系统的信号在传播过程中容易受到折射、反射、非视距因素以及2.4ghz等高频段信号的干扰，同时该信号存在信号死角，因此容易造成定位不准确的问题。

第三种：基于蓝牙的定位系统，定位相对准确，基于蓝牙的三角定位法，定位精准。但此系统需要手机蓝牙模块，还需要在室内部署蓝牙基站，导致定位成本较高。

第四种：超宽带室内定位系统，采用超宽带脉冲信号，通过预先设置的传感器对信号标签进行追踪分析，定位精度高。但该定位系统，需要在室内布置用于定位的传感器，成本高，定位范围有限，可移植性差。

技术实现要素：

本发明实施例所要解决的技术问题在于提供一种无人机上tof距离传感器的测距方法及系统，旨在解决现有技术中tof距离传感器的测量精度不高的问题。

本发明实施例第一方面提供了一种无人机上tof距离传感器的测距方法，所述方法包括：

通过tof距离传感器测量无人机与目标物体之间的距离，获取测量距离数据；

根据滑动模型对所述测量距离数据进行滤波，得到滤波距离数据；

获取所述无人机的姿态数据，并根据所述姿态数据对所述滤波距离数据进行校正，得到校正距离数据。

本发明实施例第二方面提供了一种无人机上tof距离传感器的测距系统，所述系统包括：

数据测量模块，用于通过tof距离传感器测量无人机与目标物体之间的距离，获取测量距离数据；

数据滤波模块，用于根据滑动模型对所述测量距离数据进行滤波，得到滤波距离数据；

数据校正模块，用于获取所述无人机的姿态数据，并根据所述姿态数据对所述滤波距离数据进行校正，得到校正距离数据。

从上述本发明实施例可知，本发明通过tof距离传感器测量无人机与目标物体之间的距离，获取测量距离数据，根据滑动模型对测量距离数据进行滤波，得到滤波距离数据，获取无人机的姿态数据，并根据姿态数据对滤波距离数据进行校正，得到校正距离数据。相较于现有技术，本发明能够显著提高tof距离传感器的测量精度。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

附图1是本发明第一实施例提供的无人机上tof距离传感器的测距方法的实现流程示意图；

附图2是本发明第二实施例提供的无人机上tof距离传感器的测距系统的结构示意图；

附图3是本发明第一实施例提供的滤波方法的流程图；

附图4是本发明第一实施例提供的校正方法的示意图。

具体实施方式

为使得本发明实施例的发明目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而非全部实施例。基于本发明中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅附图1，附图1为本发明第一实施例提供的无人机上tof距离传感器的测距方法的实现流程示意图。如附图1所示，该方法主要包括以下步骤：

s101、通过tof距离传感器测量无人机与目标物体之间的距离，获取测量距离数据；

本发明实施例通过无人机上配置的一个或多个飞行时间(timeofflight，tof)距离传感器，实时测量场景中的物体与无人机之间的距离，获取测量距离数据。

其中，该目标物体可指场景中任一无人机周围的物体。

s102、根据滑动模型对测量距离数据进行滤波，得到滤波距离数据；

进一步地，该步骤之前还包括：建立测量距离数据与滑动模型的对应关系。

对每个测量距离数据的数值都设置对应的滑动模型，并建立对应关系。

例如，当测量距离数据的数值小于x1时，对应第一滑动模型；当测量距离数据的数值大于或等于x1且小于x2时，对应第二滑动模型；当测量距离数据的数值大于x2时，对应第三滑动模型(其中x1＜x2)。

则，根据滑动模型对测量距离数据进行滤波，得到滤波距离数据，包括：

选取与测量距离数据对应的滑动模型作为目标滑动模型；

通过目标滑动模型对测量距离数据进行数据滤波，得到滤波距离数据。

参阅图3，具体方法如下：

系统中包含n个滤波器f1到fn，t(x)为切换函数，用来进行滑动模型的选择，即t(x)切换的每一函数，都对应一种滑动模型。

我们对于t(x)的选取依赖于测量距离数据，通过测量距离数据的值将t(x)切换成对应的函数，即确定目标滑动模型。对于tof传感器，当测量距离越远，数据噪声越大，则需要使用更平滑的滤波器进行数据滤波。这里的滤波器可以使用滑动平均滤波器，也可以根据实际情况选择使用其它滤波器。

根据测量距离数据的值将t(x)切换成对应的函数，该函数为每个滤波器的输出值提供一个权值，将各滤波器的权值与对应的滤波器的输出值的乘积进行加和，得到滤波距离数据。其中，滤波器的输出值是滤波器对测量距离数据进行滤波，得到的值。具体算法如下：

其中，yt为滤波距离数据，f(x)为滤波器的输出值，t(x)为各滤波器输出值的权值，t1(x)+t2(x)+…+tn(x)＝1。

进一步地，f(x)可以作为某一预置滤波器的输出值，也可将某一f(x)的值设为当前时刻的测量距离数据，还可将某一f(x)的值设为上一时刻的滤波距离数据。

进一步地，下面为使用一个滤波器进行滤波的情况，将f1(x)作为该滤波器的输出值，将f2(x)设为当前时刻的测量距离数据，则算法公式为：

将当前时刻的测量距离数据与上一时刻的滤波距离数据进行比较；

若当前时刻的测量距离数据与上一时刻的滤波距离数据的差的绝对值小于预置数值，则将滤波器滤波处理后的数据作为当前时刻的滤波距离数据进行输出，即令上式中t1(x)等于1，t2(x)等于0。

若当前时刻的测量距离数据与上一时刻的滤波距离数据的差的绝对值大于或等于预置数值，则将当前时刻的测量距离数据作为当前时刻的滤波距离数据进行输出，即令上式中t1(x)等于0，t2(x)等于1。

进一步地，本实施例还可使用两个滑动平均滤波器，m点滑动平均滤波器和n点滑动平均滤波器(m<n)。将f1(x)设为当前时刻的测量距离数据，将f2(x)作为m点滑动平均滤波器的输出值，将f3(x)作为n点滑动平均滤波器的输出值，将f4(x)设为上一时刻的滤波距离数据，则算法公式为：

1.将当前时刻的测量距离数据与上一时刻的滤波距离数据进行比较，若当前时刻的测量距离数据与上一时刻的滤波距离数据的差的绝对值大于阈值f(xt)时，直接输出当前时刻的测量距离数据，即令上式中t1(x)等于1，t2(x)等于0，t3(x)等于0，t4(x)等于0，其中阈值f(xt)＝a1xt+b1，t为当前时刻。

2.若当前时刻的测量距离数据与上一时刻的滤波距离数据的差的绝对值小于或等于阈值f(xt)时，进行m点和n点(m<n)的滑动平均滤波。

3.根据判定结果进行输出，输出结果有三种情况，分别是输出m点结果、输出n点结果和输出上一时刻的滤波距离数据。具体判定过程如下：

1)若max(ant-z，ant-z+1，…，ant)-min(ant-z，ant-z+1，…，ant)>g(xt)，则输出m点平均滤波器滤波处理后的数据，即令上式中t1(x)等于0，t2(x)等于1，t3(x)等于0，t4(x)等于0，其中阈值g(xt)＝a2xt+b2，an为n点滑动平均滤波器滤波处理后的数据，max(ant-z，ant-z+1，…，ant)表示从ant-z到ant这几个不同时刻的输出量中的最大值，min(ant-z，ant-z+1，…，ant)表示从ant-z到ant这几个不同时刻的输出量中的最小值。

2)若min(ant-w，ant-w+1，…，ant)>yt-1或者max(ant-w，ant-w+1，…，ant)<yt-1或者abs(ant-yt-1)>h(xt)，则输出n点平均滤波器滤波处理后的数据，即令上式中t1(x)等于0，t2(x)等于0，t3(x)等于1，t4(x)等于0，其中阈值h(xt)＝a3xt+b3，yt-1为上一时刻的滤波距离数据，abs(ant-yt-1)表示取ant-yt-1的绝对值。

3)如果以上条件都不满足，则维持上一时刻输出值，即输出上一时刻的滤波距离数据，即令上式中t1(x)等于0，t2(x)等于0，t3(x)等于0，t4(x)等于1。

s103、获取所述无人机的姿态数据，并根据姿态数据对所述滤波距离数据进行校正，得到校正距离数据。

获取校正距离数据(如图4所示)的方法包括：

首先，以无人机机体质心作为原点，以水平面为xy轴平面建立立体坐标系。

然后，通过无人机机载的惯性测量(inertialmeasurementunit，imu)传感器获取无人机的角速度和加速度信息，通过姿态估计算法，估算出无人机的实时姿态角θ。

最后，根据估算出的无人机的实时姿态角θ，对上一步中获取的滤波距离数据d进行数据校正，得到校正距离数据，即无人机与目标物体的真实距离数据，校正距离数据＝d·cosθ。

本发明实施例提供的无人机上tof距离传感器的测距方法，通过tof距离传感器测量无人机与目标物体之间的距离，获取测量距离数据，根据滑动模型对测量距离数据进行滤波，得到滤波距离数据，获取无人机的姿态数据，并根据姿态数据对滤波距离数据进行校正，得到校正距离数据。相较于现有技术，本发明能够显著提高tof距离传感器的测量精度。

请参阅附图2，附图2是本发明第二实施例提供的无人机上tof距离传感器的测距系统的结构示意图，为了便于说明，仅示出了与本发明实施例相关的部分。附图2示例的无人机上tof距离传感器的测距系统，主要包括：建模模块201、数据测量模块202、数据滤波模块203、数据校正模块204。

建模模块201，用于建立测量距离数据与滑动模型的对应关系。

数据测量模块202，用于通过tof距离传感器测量无人机与目标物体之间的距离，获取测量距离数据。

数据滤波模块203，用于根据滑动模型对测量距离数据进行滤波，得到滤波距离数据。

其中，数据滤波模块203，具体用于选取与测量距离数据对应的滑动模型作为目标滑动模型，并通过目标滑动模型对所述测量距离数据进行数据滤波，得到滤波距离数据。

数据校正模块204，用于获取无人机的姿态数据，并根据姿态数据对滤波距离数据进行校正，得到校正距离数据。

上述各功能模块实现各自功能的具体过程，可参考前述第一实施例提供的无人机上tof距离传感器的测距方法的相关内容，此处不再赘述。

本发明实施例提供的无人机上tof距离传感器的测距系统，通过tof距离传感器测量无人机与目标物体之间的距离，获取测量距离数据，根据滑动模型对测量距离数据进行滤波，得到滤波距离数据，获取无人机的姿态数据，并根据姿态数据对滤波距离数据进行校正，得到校正距离数据。相较于现有技术，本发明能够显著提高tof距离传感器的测量精度。

需要说明的是，对于前述的各方法实施例，为了简便描述，故将其都表述为一系列的动作组合，但是本领域技术人员应该知悉，本发明并不受所描述的动作顺序的限制，因为依据本发明，某些步骤可以采用其它顺序或者同时进行。其次，本领域技术人员也应该知悉，说明书中所描述的实施例均属于优选实施例，所涉及的动作和模块并不一定都是本发明所必须的。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其它实施例的相关描述。

以上为对本发明所提供的无人机上tof距离传感器的测距方法及系统的描述，对于本领域的技术人员，依据本发明实施例的思想，在具体实施方式及应用范围上均会有改变之处，综上，本说明书内容不应理解为对本发明的限制。