一种基于IMU的uwb定位异常值处理方法与流程

本发明涉及uwb定位的技术领域，尤其涉及到一种基于imu的uwb定位异常值处理方法。

背景技术：

uwb定位技术是一种使用1ghz以上频率带宽的无线载波通信技术，它不采用正弦载波，而是利用纳秒级的非正弦波窄脉冲传输数据，因此其所占的频谱范围很大，尽管使用无线通信，但其数据传输速率可以达到几百兆比特每秒以上。由于uwb具有高速的数据传输、低功耗、安全性高、定位精度高的特点被广泛应用于各个领域室内定位中，本专利uwb应用背景在于给室内无人机提供高精度的定位数据。由于室内环境的复杂以及各个uwb硬件差异性，导致uwb测距输入值的原始数据发生跳变，丢失，失去真实数据，此时即需要对这些异常数据值进行处理、填充，为uwb定位提供连续而且较为准确的测量数据值。

目前对异常值常处理的方式是删除含有异常值的数据、将异常值视为缺失值，交给缺失值处理方法来处理、用平均值来修正或者是不做处理等，然而以上这些处理异常值的方法在处理uwb定位数据过程中均有定的缺陷，例如使用均值来修正的方法，当无人机进行较大加速度飞行，此时当异常值用均值代替明显地到时数据失真而其他处理方法均为后面算法作估计带来误差影响。

技术实现要素：

本发明的目的在于克服现有技术的不足，提供一种基于imu的uwb定位异常值处理方法，以四个uwb基站进行uwb定位为例，解决现有的原始数据预处理过程中的异常值判别不准确以及异常值修正的精度地等问题，提高了测量硬件数据准确度。本发明不需要外加额外硬件设备，增强了uwb传感器在复杂环境下测量数据的稳定性，同时为后面的uwb定位解算提供有效精确的测量输出数据，提高uwb定位的稳定性，快速性以及实时性。

为实现上述目的，本发明所提供的技术方案为：

一种基于imu的uwb定位异常值处理方法，包括以下步骤：

s1.获取无人机的imu数据；

s2.将加速度计和陀螺仪进行融合，计算无人机的姿态角度；

s3.依据步骤s2得到的无人机的姿态角度，计算x、y、z轴的加速度以及△t时间内x、y、z轴上的位移，从而估计出无人机的位移值q；

s4.依据步骤s3得到的位移值q以及uwb传感器的距离数据，对uwb测量值进行异常检测；若uwb测量值异常，则进入步骤s5；

s5.对异常的uwb测量值进行数据融合，修正uwb测量值。

进一步地，所述步骤s2的具体过程如下：

估计姿态角度以及陀螺仪的漂移，即令：其中，bg,k为陀螺仪的漂移量,θ,φ分别为俯仰角，横滚角；

又由于ωb为无人机角速度，r为机体坐标系到大地坐标系的旋转矩阵，提取出第三列，即：

又

其中，减去漂移误差bg，再减去陀螺仪测量白噪声wg，漂移噪声建模可为高斯白噪声，为陀螺仪的噪声；

通过离散化后,得到：

其中，ts为采样时间；

通过简化分离出状态变量得：

简化：xk＝φk-1xk-1+γk-1wk-1；

其中：

k-1时刻的系统转移矩阵：

k-1时刻系统噪声矩阵：

k-1时刻状态向量：

k-1时刻系统噪声：观测模型：

其中：

k时刻观测矩阵：

观测噪声：

综上所述，简化后的过程模型与观测模型如下：

得到过程模型与观测模型后，进行初始化

其中，e(x0)表示零时刻的数学期望值，e[(x0-e(x0))(x0-e(x0))^t]表示计算零时刻的误差协方差；

状态估计预测：

xk|k-1＝φk-1xk-1；

误差协方差预测：

pk|k-1＝φk-1pk-1|k-1φk-1^t+γk-1qk-1γk-1^t，qk-1表示系统噪声方差矩阵；

卡尔曼增益矩阵：

其中：hk＝[o3×3-gi]，r^t为测量噪声方差矩阵；

状态估计更新：

xk|k＝xk|k-1+kk(z-hkxk|k-1)；

其中：z为观测数据；

误差协方差更新：

pk|k＝(i-kkhk)pk|k-1；

i为单位矩阵，根据状态向量xk从而得到精确的姿态角度。

进一步地，所述步骤s3中x、y、z轴加速度计算的具体过程如下：

由于又

即：其中，xk(i,j)表示xk的第i行第j列的数据，即可求得俯仰角θ和横滚角φ；由于无人机飞行速度很小，即其瞬时加速度可忽略不计，即所测加速度在g重力加速度附近，根据测量的加速度模值ab：

即得其中为机体坐标系转到大地坐标系下的旋转矩阵。

进一步地，所述步骤s3中△t时间内x、y、z轴上的位移的计算过程如下：

根据公式：

即在采样时间为△t时刻的位移：

其中vt为当前速度：vt＝(pt-pt-1)/δt为三维向量，pt为t时刻的uwb定位系统经过位置解算的位置，视前一时刻的平均速度为当前时间的速度；又由于在δt时间内趋近于零，即：

δs＝vtδt+atδt；

式中，a为通过imu解算所得的精确加速度在导航坐标系下的加速度，t为系统运行时间，根据上式可得t+δt时刻的估算位移st+δs；

其中，δs1，δs2，δs3分别为采样时间δt内导航坐标系下x，y，z轴的位移。

进一步地，所述步骤s3中无人机的位移值q的计算公式如下：

其中，

x,y,z为上次所测的定位值，(a*1，a*2，a*3)为第*个基站的位置值。

进一步地，所述步骤s4中，检测判断异常值时，公式为其中分别为uwb的当前测量距离值和上一时刻的测量值，q为δt时间内的位移值，mean为n个测量数据的标准方差值，k为比重参数，需要根据具体环境参数调整；当满足式子则视为测量异常值。

进一步地，所述步骤s5修正uwb测量值的公式如下：

其中，std为n个测量数据的平均值。

与现有技术相比，本方案原理及优点如下：

1.本方案融合无人机自带的imu传感器来预测无人机δt时间内的位移，能很好地预测到uwb测量的距离值的波动范围，能准确地判别出数据是否为异常。

2.本方案在判断出异常值数据以及估计出无人机在δt时间内的位移的基础上，能更加准确地对异常值进行修正，更好地恢复原始数据值。

附图说明

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

图1为本发明一种基于imu的uwb定位异常值处理方法的原理流程图。

具体实施方式

下面结合具体实施例对本发明作进一步说明：

如图1所示，本实施例所述的一种基于imu的uwb定位异常值处理方法，包括以下步骤：

s1.获取无人机的imu数据；

s2.将加速度计和陀螺仪进行融合，计算无人机的姿态角度，具体如下：

估计姿态角度以及陀螺仪的漂移，即令：其中，bg,k为陀螺仪的漂移量,θ,φ分别为俯仰角，横滚角；

又由于ωb为无人机角速度，r为机体坐标系到大地坐标系的旋转矩阵，提取出第三列，即：

又

其中，减去漂移误差bg，再减去陀螺仪测量白噪声wg，漂移噪声建模可为高斯白噪声，为陀螺仪的噪声；

通过离散化后,得到：

其中，ts为采样时间；

通过简化分离出状态变量得：

简化：xk＝φk-1xk-1+γk-1wk-1；

其中：

k-1时刻的系统转移矩阵：

k-1时刻系统噪声矩阵：

k-1时刻状态向量：

k-1时刻系统噪声：

观测模型：

其中：

k时刻观测矩阵：

观测噪声：

综上所述，简化后的过程模型与观测模型如下：

得到过程模型与观测模型后，进行初始化

其中：e(x0)表示零时刻的数学期望值，e[(x0-e(x0))(x0-e(x0))^t]表示计算零时刻的误差协方差；

状态估计预测：

xk|k-1＝φk-1xk-1；

误差协方差预测：

pk|k-1＝φk-1pk-1|k-1φk-1^t+γk-1qk-1γk-1^t，qk-1表示系统噪声方差矩阵；

卡尔曼增益矩阵：

其中：hk＝[o3×3-gi]，r^t为测量噪声方差矩阵；

状态估计更新：

xk|k＝xk|k-1+kk(z-hkxk|k-1)；

其中：z为观测数据；

误差协方差更新：

pk|k＝(i-kkhk)pk|k-1；

i为单位矩阵，根据状态向量xk从而得到精确的姿态角度。

s3.依据步骤s2得到的无人机的姿态角度，计算x、y、z轴的加速度：

由于又

即：其中，xk(i,j)表示xk的第i行第j列的数据，即可求得俯仰角θ和横滚角φ；由于无人机飞行速度很小，即其瞬时加速度可忽略不计，即所测加速度在g重力加速度附近，根据测量的加速度模值ab：

即得其中为机体坐标系转到大地坐标系下的旋转矩阵。

计算△t时间内x、y、z轴上的位移：

根据公式：

即在采样时间为△t时刻的位移：

其中vt为当前速度：vt＝(pt-pt-1)/δt为三维向量，pt为t时刻的uwb定位系统经过位置解算的位置，视前一时刻的平均速度为当前时间的速度；又由于在δt时间内趋近于零，即：

δs＝vtδt+atδt；

式中，a为通过imu解算所得的精确加速度在导航坐标系下的加速度，t为系统运行时间，根据上式可得t+δt时刻的估算位移st+δs；

其中，δs1，δs2，δs3分别为采样时间δt内导航坐标系下x，y，z轴的位移。

估计出无人机的位移值q，公式如下：

其中，

x,y,z为上次所测的定位值，(a*1，a*2，a*3)为第*个基站的位置值。

s4.依据步骤s3得到的位移值q以及uwb传感器的距离数据，对uwb测量值进行异常检测；若uwb测量值异常，则进入步骤s5；

检测判断异常值时，公式为其中分别为uwb的当前测量距离值和上一时刻的测量值，q为δt时间内的位移值，mean为n个测量数据的标准方差值，k为比重参数，需要根据具体环境参数调整；当满足式子则视为测量异常值。

s5.对异常的uwb测量值进行数据融合，修正uwb测量值；修正uwb测量值的公式如下：

其中，std为n个测量数据的平均值。

本实施例以四个uwb基站进行uwb定位为例，解决了现有的原始数据预处理过程中的异常值判别不准确以及异常值修正的精度地等问题，提高了测量硬件数据准确度。而且本实施例不需要外加额外硬件设备，增强了uwb传感器在复杂环境下测量数据的稳定性，同时为后面的uwb定位解算提供有效精确的测量输出数据，提高uwb定位的稳定性，快速性以及实时性。

以上所述之实施例子只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。