[发明专利]一种基于IMU的uwb定位异常值处理方法

权利要求书

1.一种基于IMU的uwb定位异常值处理方法，其特征在于，包括以下步骤：

S1.获取无人机的IMU数据；

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

S3.依据步骤S2得到的无人机的姿态角度，计算X、Y、Z轴的加速度以及采样时间T_s时间内X、Y、Z轴上的位移，从而估计出无人机的位移值Q；

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

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

其中，所述步骤S2的具体过程如下：

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

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

又

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

通过离散化后,得到：

其中，T_s为采样时间；

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

简化：X_k＝Φ_k-1X_k-1+Γ_k-1w_k-1；

其中：

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

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

k-1时刻状态向量：

k-1时刻系统噪声：

观测模型：

其中：

K时刻观测矩阵：

观测噪声：

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

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

其中，E(X₀)表示零时刻的数学期望值，E[(X₀-E(X₀))(X₀-E(X₀))^T]表示计算零时刻的误差协方差；

状态估计预测：

X_k|k-1＝Φ_k-1X_k-1；

误差协方差预测：

P_k|k-1＝Φ_k-1P_k-1|k-1Φ_k-1^T+Γ_k-1Q_k-1Γ_k-1^T，Q_k-1表示系统噪声方差矩阵；

卡尔曼增益矩阵：

其中：H_k＝[O_3×3 -gI]，RT为测量噪声方差矩阵；

状态估计更新：

X_k|k＝X_k|k-1+K_k(Z-H_kX_k|k-1)；

其中：Z为观测数据；

误差协方差更新：

P_k|k＝(I-K_kH_k)P_k|k-1；

I为单位矩阵，根据状态向量Xk从而得到精确的姿态角度；

所述步骤S3中X、Y、Z轴加速度计算的具体过程如下：

由于又

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

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

所述步骤S3中采样时间T_s时间内X、Y、Z轴上的位移的计算过程如下：

根据公式：

即在采样时间为T_s的位移：

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

Δs＝v_tT_s+atT_s；

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

其中，Δs₁，Δs₂，Δs₃分别为采样时间T_s内导航坐标系下X，Y，Z轴的位移；

所述步骤S3中无人机的位移值Q的计算公式如下：

其中，

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

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

2.根据权利要求1所述的一种基于IMU的uwb定位异常值处理方法，其特征在于，所述步骤S5修正uwb测量值的公式如下：

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