无人飞行器室内定位数据处理方法及装置与流程

本发明涉及无人飞行器技术领域，特别是涉及一种无人飞行器室内定位数据处理方法及装置。

背景技术：

无人机是一种有动力、可控制、能携带多种任务设备、执行多种任务并能重复使用的飞行器。能够利用无线遥控设备和自身的控制装置进行控制的不载人飞行器，例如无人直升机、无人固定翼机、无人伞翼机等等。该无人机可以用于挂载拍摄装置，用于航拍、测绘、侦查等等。

目前，随着无人机应用越来越普及，出现了更多航飞速度更快、飞行高度更高、更加操作灵活、体积更小的无人飞行器。而随着飞行速度及飞行高度的不断增加，加上飞行器本体越来越小，对飞行器本身的性能要求也越来越高，具体的，飞行器飞行速度及高度的提升，将导致飞行器根据操控要求实现悬停的难度提高；小型飞行器在室内或空间较为隐蔽的地方飞行时由于无法实现卫星定位或卫星定位精度差，而无法实现定点悬停的问题，上述问题将导致无人飞行器存在一定的安全隐患，同时对操控者而言，也无法体验到良好的飞行操控效果。

全球卫星定位系统（gps）的应用是基于卫星发射信号给定位端，当定位端同时收到4颗以上的卫星信号后，再根据相关的定位算法算出其当前所处位置的三维坐标、速度和时间等。然而，这种定位原理的前提是要能接收到卫星信号，这就将gps模块的使用限制在室外且能够接收到良好的卫星信号的环境下。在室内等卫星信号不好的一些环境下，我们无法接收到符合要求的卫星信号，或者是完全接收不到信号，并且这个信号产生的gps位置信息的误差变得非常大，几乎无法使用。

然而，在实际的应用中，无人机通常需要工作在卫星信号不好的环境下，如室内环境等。所以，在正常gps信号无法满足需求的情况下，有必要研究和开发出一套行之有效的室内定位系统。

对比文件1（cn104932523a）公开一种无人飞行器的定位方法，该方法通过无人飞行器上的摄像头获取视频流图像信息，根据解析视频图像信息得到特征点信息以及高度信息和姿态信息，得到飞行器的漂移方向和漂移距离，融合成视频流定位信息，再通过获取卫星定位信号，将卫星定位信号和视频流定位信号进行融合处理后，得到精度的定位信息。

当前，已经有一些关于无人机室内定位技术的专利，如光流定位技术、惯性设备与超生波测距相结合定位技术、摄像头图像结合雷达避障系统定位技术以及摄像头图像对比定位技术等。尽管这些方法在一定程度上可以较好地用于无人机室内定位，但仍存在一些局限性。如使用光流或摄像头图像定位技术，不仅成本较高，而且还需要复杂的图像处理算法；而使用惯性测量器件等，则定位精度会受到一定的限制；同时，采用雷达避障则需要高精度的雷达扫描系统，价格昂贵、结构复杂。

技术实现要素：

本发明正是基于以上一个或多个问题，提供一种无人飞行器室内定位数据处理方法及装置，用以解决现有技术中存在的无人飞行器定位成本高昂以及定位数据不准确的问题。

本发明提供一种无人飞行器室内定位数据处理方法，包括以下步骤：

s01依据室内不同位置接收的至少三个超声波信号计算出所述无人飞行器的定位信息；

s02依据所述无人飞行器在预设时间内的连续n次历史定位信息以及相应的历史预测位置信息，计算所述无人飞行器在空间坐标x、y、z轴上的分量的各历史定位信息与相应的历史预测位置信息之间的实际偏差值以及所述历史预测位置信息的历史预测偏差值，其中n为大于等于4的整数；

s03依据所述实际偏差值，获取当前时刻所述无人飞行器在空间坐标x、y、z轴上的分量的预测模糊值；

s04依据所述历史预测偏差值与预测模糊值，计算出噪声偏差；

s05依据所述预测模糊值以及所述噪声偏差计算出当前时刻的真实定位值的加权系数；

s06依据所述加权系数、当前时刻的预测位置信息以及当前时刻所述无人飞行器的测量位置信息，计算出所述无人飞行器当前时刻的所述真实定位值。

优选地，在步骤s02中，所述实际偏差值和预测偏差值是基于速度预测的均方根平滑滤波方法获得，具体步骤如下：

s021选择时间窗口tw，所述tw有以下两种选取方式：当预测速度大于所述速度预定值vset时，令a取vm的最大值，tw的数值等于a/vset²的值；当预测速度小于等于所述速度预定值vset时，令a取vm的最小值，tw的数值等于a*vset²的值，其中，设定所述无人飞行器的速度预定值为vset，预设平滑滤波处理的时间窗口为tw，vm为所述无人飞行器的速度预设值范围。

优选地，所述步骤s02进一步包括：

s022所述时间窗口tw内连续获得n次所述无人飞行器的实际飞行速度，根据第n时刻所述无人飞行器的俯仰角、偏航角、横滚角求出所述无人飞行器在x、y、z三个方向的飞行速度实际值；

s023依据所述时间窗口tw内连续获得的前n-1次历史飞行速度实际值，获得飞行速度平均值以及自所述时间窗口tw内开始至第n-1时刻的总飞行时间，得到前n-1时刻的位置实际值dn-1；

s024将第n-1个时刻的飞行速度与第n时刻与第n-1时刻的时间差值相乘得到对第n时刻位置变化量，与所述前n-1时刻的位置实际值dn-1相加，得到第n时刻的预测位置值。

优选地，步骤s05中所述真实定位值的加权系数的公式表述为：kn=gn²/(gn²+an²)，其中kn为加权系数，gn为高斯噪声，an为前n-1次历史位置实际偏差值的算术平均值。

优选地，步骤06计算出所述真实定位值按照如下公式计算：

tn=pn*kn+dn*(1-kn)；

其中，dn为第n时刻的位置测量值，pn为第n时刻的位置预测值，tn为第n时刻融合位置测量值和位置预测值获得的真实定位值，用于控制所述无人飞行器飞行。

优选地，第n时刻的位置偏差值sn：

sn=((1-kn)*sn-1²)^1/2，

其中，sn表示第n时刻的位置偏差值，sn-1表示第n-1时刻的位置偏差值。

本发明还提供一种无人飞行器室内定位数据处理装置，所述无人飞行器室内定位数据处理装置包括：

定位计算单元，用于依据室内不同位置接收的至少三个超声波信号计算出所述无人飞行器的定位信息；

偏差计算单元，用于依据所述无人飞行器在预设时间内的连续n次历史定位信息以及相应的历史预测位置信息，计算所述无人飞行器在空间坐标x、y、z轴上的分量的各历史定位信息与相应的历史预测位置信息之间的实际偏差值以及所述历史预测位置信息的历史预测偏差值，其中n为大于等于4的整数；

预测模糊值获取单元，用于依据所述实际偏差值，获取当前时刻所述无人飞行器在空间坐标x、y、z轴上的分量的预测模糊值；

噪声偏差计算单元，用于依据所述历史预测偏差值与预测模糊值，计算出噪声偏差；

加权系数计算单元，用于依据所述预测模糊值以及所述噪声偏差计算出当前时刻的真实定位值的加权系数；

真实定位值计算单元，用于依据所述加权系数、当前时刻的预测位置信息以及当前时刻所述无人飞行器的测量位置信息，计算出所述无人飞行器当前时刻的所述真实定位值。

优选地，所述偏差计算单元的所述实际偏差值和预测偏差值是基于速度预测的均方根平滑滤波方法获得，所述偏差计算单元还包括：

时间窗口选择单元，用于选择时间窗口tw，所述tw有以下两种选取方式：当预测速度大于所述速度预定值vset时，令a取vm的最大值，tw的数值等于a/vset²的值；当预测速度小于等于所述速度预定值vset时，令a取vm的最小值，tw的数值等于a*vset²的值，其中，设定所述无人飞行器的速度预定值为vset，预设平滑滤波处理的时间窗口为tw，vm为所述无人飞行器的速度预设值范围。

优选地，所述偏差计算单元进一步包括：

飞行速度实际值获取模块，用于所述时间窗口tw内连续获得n次所述无人飞行器的实际飞行速度，根据第n时刻所述无人飞行器的俯仰角、偏航角、横滚角求出所述无人飞行器在x、y、z三个方向的飞行速度实际值；

位置实际值获取模块，用于依据所述时间窗口tw内连续获得的前n-1次历史飞行速度实际值，获得飞行速度平均值以及自所述时间窗口tw内开始至第n-1时刻的总飞行时间，获取前n-1时刻的位置实际值dn-1；

预测位置值获取模块，用于将第n-1个时刻的飞行速度与第n时刻与第n-1时刻的时间差值相乘得到对第n时刻位置变化量，与所述前n-1时刻的位置实际值dn-1相加，获取第n时刻的预测位置值。

优选地，所述加权系数计算单元中所述真实定位值的加权系数的公式表述为：kn=gn²/(gn²+an²)，其中kn为加权系数，gn为高斯噪声，an为前n-1次历史位置实际偏差值的算术平均值；

所述真实定位值计算单元计算出所述真实定位值按照如下公式计算：

tn=pn*kn+dn*(1-kn)；

其中，dn为第n时刻的位置测量值，pn为第n时刻的位置预测值，tn为第n时刻融合位置测量值和位置预测值获得的真实定位值，用于控制所述无人飞行器飞行。

本发明还提供一种无人飞行器室内定位数据处理装置，所述无人飞行器上设有超声波发射器，所述室内不同位置至少分布有三个超声波接收器，所述超声波接收器接收所述超声波发射器发射的超声波信号，所述无人飞行器还包括：无人飞行器室内定位数据处理装置，所述无人飞行器室内定位数据处理装置进一步包括：处理器和存储器，所述处理器调用所述存储器存储的程序指令实现以下步骤：

s01依据室内不同位置接收的至少三个超声波信号计算出所述无人飞行器的定位信息；

s02依据所述无人飞行器在预设时间内的连续n次历史定位信息以及相应的历史预测位置信息，计算所述无人飞行器在空间坐标x、y、z轴上的分量的各历史定位信息与相应的历史预测位置信息之间的实际偏差值以及所述历史预测位置信息的历史预测偏差值，其中n为大于等于4的整数；

s03依据所述实际偏差值，获取当前时刻所述无人飞行器在空间坐标x、y、z轴上的分量的预测模糊值；

s04依据所述历史预测偏差值与预测模糊值，计算出噪声偏差；

s05依据所述预测模糊值以及所述噪声偏差计算出当前时刻的真实定位值的加权系数；

s06依据所述加权系数、当前时刻的预测位置信息以及当前时刻所述无人飞行器的测量位置信息，计算出所述无人飞行器当前时刻的所述真实定位值。

本发明提供的无人飞行器室内定位数据处理方法及装置，不仅简化了数据处理复杂度，而且在低成本的定位设计下实现无人飞行器的准确定位。

附图说明

图1是本发明实施方式一的无人飞行器室内定位数据处理方法的流程示意图；

图2是本发明实施方式二的无人飞行器室内定位数据处理装置的结构示意图；

图3是本发明实施方式三的无人飞行器的结构示意图。

具体实施方式

下面结合附图和实施例对本发明进行详细说明。需要说明的是，如果不冲突，本发明实施例以及实施例中的各个特征可以相互结合，均在本发明的保护范围之内。

实施方式一

请参见图1，本发明提供一种无人飞行器室内定位数据处理方法，包括以下步骤：

s01依据室内不同位置接收的至少三个超声波信号计算出所述无人飞行器的定位信息；

s02依据所述无人飞行器在预设时间内的连续n次历史定位信息以及相应的历史预测位置信息，计算所述无人飞行器在空间坐标x、y、z轴上的分量的各历史定位信息与相应的历史预测位置信息之间的实际偏差值以及所述历史预测位置信息的历史预测偏差值，其中n取大于等于4的整数；

s03依据所述实际偏差值，获取当前时刻所述无人飞行器在空间坐标x、y、z轴上的分量的预测模糊值；这里的预测模糊值是指预测的数值还处于不确定；

s04依据所述历史预测偏差值与预测模糊值，计算出噪声偏差；

s05依据所述预测模糊值以及所述噪声偏差计算出当前时刻的真实定位值的加权系数；

s06依据所述加权系数、当前时刻的预测位置信息以及当前时刻所述无人飞行器的测量位置信息，计算出所述无人飞行器当前时刻的所述真实定位值。

本发明提供的无人飞行器室内定位数据处理方法，不仅简化了数据处理复杂度，而且在低成本的定位设计下实现无人飞行器的准确定位。

优选地，在步骤s02中，所述实际偏差值和预测偏差值是基于速度预测的均方根平滑滤波方法获得，具体步骤如下：

s021选择时间窗口tw，所述tw有以下两种选取方式：当预测速度大于所述速度预定值vset时，令a取vm的最大值，tw的数值等于a/vset²的值；当预测速度小于等于所述速度预定值vset时，令a取vm的最小值，tw的数值等于a*vset²的值，其中，设定所述无人飞行器的速度预定值为vset，预设平滑滤波处理的时间窗口为tw，vm为所述无人飞行器的速度预设值范围。这里的可以是将vset设定为1m/s。

优选地，所述步骤s02进一步包括：

s022所述时间窗口tw内连续获得n次所述无人飞行器的实际飞行速度，根据第n时刻所述无人飞行器的俯仰角、偏航角、横滚角求出所述无人飞行器在x、y、z三个方向的飞行速度实际值；

s023依据所述时间窗口tw内连续获得的前n-1次历史飞行速度实际值，获得飞行速度平均值以及自所述时间窗口tw内开始至第n-1时刻的总飞行时间，得到前n-1时刻的位置实际值dn-1；

s024将第n-1个时刻的飞行速度与第n时刻与第n-1时刻的时间差值相乘得到对第n时刻位置变化量，与所述前n-1时刻的位置实际值dn-1相加，得到第n时刻的预测位置值。

优选地，步骤s05中所述真实定位值的加权系数的公式表述为：kn=gn²/(gn²+an²)，其中kn为加权系数，gn为高斯噪声，an为前n-1次历史位置实际偏差值的算术平均值。

优选地，步骤06计算出所述真实定位值按照如下公式计算：

tn=pn*kn+dn*(1-kn)；

其中，dn为第n时刻的位置测量值，pn为第n时刻的位置预测值，tn为第n时刻融合位置测量值和位置预测值获得的真实定位值，用于控制所述无人飞行器飞行。

优选地，第n时刻的位置偏差值sn：

sn=((1-kn)*sn-1²)^1/2，

其中，sn表示第n时刻的位置偏差值，sn-1表示第n-1时刻的位置偏差值。

在一个具体实施例中，在定位计算时使用了一种基于速度预测的均方根平滑滤波方法。

由于超声波在实际的使用中会受到各种干扰，如自身回波、外界的超声波或被物体吸收导致收超声波迅速衰减到无法检测。因此，在处理计算出的数据时，本发明采用平滑滤波的方法滤除干扰因子，使得到的坐标数据可以更好地接近实际的坐标位置。平滑滤波的方法可以有很多种，本发明优选使用复合平滑滤波算法，设定无人飞行器在空间内运动时的速度vxn(n取大于等于4的整数)，vyn(n取大于等于4的整数)，我们可以通过历史速度计算得出来，本实施例中，我们假设时间窗口内共采样到5个速度数据。我们可以通过对前4次的历史速度数据，按照下面的公式用来预测下一次的速度（基于物体的速度是一个连续的物理量，不会出现微观上的跳跃这一原理，并且其加速度也不会）：

x轴向的速度：vxn=(vxn-1+vxn-2+vxn-3+vxn-4)/4；

y轴向的速度：vyn=(vyn-1+vyn-2+vyn-3+vyn-4)/4；

设dt为两次采样之间的间隔；

在后续的算法中需要使用上一次的x轴坐标值（dxn-1）和y轴坐标值（dyn-1），则预测的x轴坐标值和y轴坐标值px和py分别表达为：

px=dxn-1+vxn*dt；

py=dyn-1+vyn*dt；

而采样得出的当前的x轴坐标值和y轴坐标值分别为dx和dy。x、y轴向的预测值与测量值之间的偏差∆x和∆y的表达式为：

∆x=px-dx；

∆y=py–dy；

当我们通过采样计算出来的坐标值与预测值近似或相等时，我们可以直接使用这个采样计算的坐标值，但是在实际应用中，不可避免的出现噪声的干扰，这就需要采用一种新的算法进行数据的修正。我们就可以采用一种线性数据回归融合的方法进行处理，即使用相应的加权系数kn对这两个数据进行数据融合得到最终的坐标数据。

基本的数据融合算法为:

预测值pn与测量值dn之间的偏差用∆n表示，对前四次的偏差值求平均值，作为本次测量数值的预测模糊值（或者预测不确定值），用an表示：

an=(∆n-4+∆n-3+∆n-2+∆n-1)/4

用sn-1表示距离n最近的一次预测偏差，求得an与sn-1的均方根，表示高斯噪声的偏差gn：

gn=(an*an+sn-1*sn-1)^1/2。

进而得到加权系数kn、本次的预测偏差为sn和最终融合数值tn分别为：

kn=gn²/(gn²+an²)；

sn=((1-kn)*sn-1²)^1/2；

tn=pn*kn+dn*(1-kn)。

此外，要注意的是，若n取值为0、1、2、3时，此时不适合采用上述方法，但也可依据本发明的思想来做，当n=0，只有实际测量值d0。

当n=1时，∆1=p1-d1；这里预测值p1依据d0获得。

当n=2时，∆2=p2-d2；

当n=3时，∆3=p3-d3；

当n=4时，∆4=p4-d4，s4=(∆1+∆2+∆3)/3；

当n=5时，∆5=p5-d5，a5=(∆1+∆2+∆3+∆4)/4；

高斯噪声g5=(a5²+s4²)^1/2，

加权系数k5=g5²/(a5²+g5²)，

s5=((1-k5)s4²)^1/2。

以此类推，n>=6时,

an=(∆n-4+∆n-3+∆n-2+∆n-1)/4

gn=(an*an+sn-1*sn-1)^1/2

kn=gn²/(gn²+an²)，

sn=((1-kn)*sn-1)^1/2；

tn=pn*kn+dn*(1-kn)。

实施方式二

如图2所示，本发明还提供一种无人飞行器室内定位数据处理装置，所述无人飞行器室内定位数据处理装置包括：

定位计算单元10，用于依据室内不同位置接收的至少三个超声波信号计算出所述无人飞行器的定位信息；

偏差计算单元20，用于依据所述无人飞行器在预设时间内的连续n次历史定位信息以及相应的历史预测位置信息，计算所述无人飞行器在空间坐标x、y、z轴上的分量的各历史定位信息与相应的历史预测位置信息之间的实际偏差值以及所述历史预测位置信息的历史预测偏差值，其中n为大于等于4的整数；

预测模糊值获取单元30，用于依据所述实际偏差值，获取当前时刻所述无人飞行器在空间坐标x、y、z轴上的分量的预测模糊值；

噪声偏差计算单元40，用于依据所述历史预测偏差值与预测模糊值，计算出噪声偏差；

加权系数计算单元50，用于依据所述预测模糊值以及所述噪声偏差计算出当前时刻的真实定位值的加权系数；

真实定位值计算单元60，用于依据所述加权系数、当前时刻的预测位置信息以及当前时刻所述无人飞行器的测量位置信息，计算出所述无人飞行器当前时刻的所述真实定位值。

本发明提供的无人飞行器室内定位数据处理装置，不仅简化了数据处理复杂度，而且在低成本的定位设计下实现无人飞行器的准确定位。

优选地，所述偏差计算单元的所述实际偏差值和预测偏差值是基于速度预测的均方根平滑滤波方法获得，所述偏差计算单元还包括：

时间窗口选择单元，用于选择时间窗口tw，所述tw有以下两种选取方式：当预测速度大于所述速度预定值vset时，令a取vm的最大值，tw的数值等于a/vset²的值；当预测速度小于等于所述速度预定值vset时，令a取vm的最小值，tw的数值等于a*vset²的值，其中，设定所述无人飞行器的速度预定值为vset，预设平滑滤波处理的时间窗口为tw，vm为所述无人飞行器的速度预设值范围。

优选地，所述偏差计算单元进一步包括：

飞行速度实际值获取模块，用于所述时间窗口tw内连续获得n次所述无人飞行器的实际飞行速度，根据第n时刻所述无人飞行器的俯仰角、偏航角、横滚角求出所述无人飞行器在x、y、z三个方向的飞行速度实际值；

位置实际值获取模块，用于依据所述时间窗口tw内连续获得的前n-1次历史飞行速度实际值，获得飞行速度平均值以及自所述时间窗口tw内开始至第n-1时刻的总飞行时间，获取前n-1时刻的位置实际值dn-1；

预测位置值获取模块，用于将第n-1个时刻的飞行速度与第n时刻与第n-1时刻的时间差值相乘得到对第n时刻位置变化量，与所述前n-1时刻的位置实际值dn-1相加，获取第n时刻的预测位置值。

优选地，所述加权系数计算单元中所述真实定位值的加权系数的公式表述为：kn=gn²/(gn²+an²)，其中kn为加权系数，gn为高斯噪声，an为前n-1次历史位置实际偏差值的算术平均值；

所述真实定位值计算单元计算出所述真实定位值按照如下公式计算：

tn=pn*kn+dn*(1-kn)；

其中，dn为第n时刻的位置测量值，pn为第n时刻的位置预测值，tn为第n时刻融合位置测量值和位置预测值获得的真实定位值，用于控制所述无人飞行器飞行。

实施方式三

如图3所示，本发明还提供一种无人飞行器室内定位数据处理装置，所述无人飞行器上设有超声波发射器300，所述室内不同位置至少分布有三个超声波接收器，如超声波接收器1、超声波接收器2以及超声波接收器3、超声波接收器m。所述超声波接收器接收所述超声波发射器300发射的超声波信号，所述无人飞行器还包括：无人飞行器室内定位数据处理装置，所述无人飞行器室内定位数据处理装置进一步包括：处理器100和存储器200，所述处理器调用所述存储器存储的程序指令实现以下步骤：

s01依据室内不同位置接收的至少三个超声波信号计算出所述无人飞行器的定位信息；

s02依据所述无人飞行器在预设时间内的连续n次历史定位信息以及相应的历史预测位置信息，计算所述无人飞行器在空间坐标x、y、z轴上的分量的各历史定位信息与相应的历史预测位置信息之间的实际偏差值以及所述历史预测位置信息的历史预测偏差值，其中n为大于等于4的整数；

s03依据所述实际偏差值，获取当前时刻所述无人飞行器在空间坐标x、y、z轴上的分量的预测模糊值；

s04依据所述历史预测偏差值与预测模糊值，计算出噪声偏差；

s05依据所述预测模糊值以及所述噪声偏差计算出当前时刻的真实定位值的加权系数；

s06依据所述加权系数、当前时刻的预测位置信息以及当前时刻所述无人飞行器的测量位置信息，计算出所述无人飞行器当前时刻的所述真实定位值。

本发明提供的无人飞行器室内定位数据处理装置，不仅简化了数据处理复杂度，而且在低成本的定位设计下实现无人飞行器的准确定位。

以上对本发明所提供的一种无人飞行器室内数据处理方法及装置，进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容仅为本发明的实施方式，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。不应理解为对本发明的限制。