改进自适应抗差容积卡尔曼滤波多源室内定位

李 鹏，荣冬成，向宇翔，凌智琛，夏 珺

(湘潭大学自动化与电子信息学院，湖南湘潭 411100)

0 引言

在组合导航系统中，高精度的滤波算法对导航定位的解算精度具有重要的影响。扩展卡尔曼滤波(Extended Kalman Filter，EKF)虽然可以用于非线性系统滤波，但是其线性化环节会引入高截断误差，且Jacobi矩阵的计算会增加运算的难度，在实际工程应用中效果不佳。无迹卡尔曼滤波(Unscented Kalman Filter，UKF)克服了EKF的局限性，基于无味变换(Unscented Transformation， UT)构建了一系列Sigma点，以逼近状态向量的后验概率密度函数，实现简单且精度远高于EKF；但是UKF在系统状态量发生突变的情况下鲁棒性较差，其精度容易受到影响。容积卡尔曼滤波(Cubature Kalman Filter，CKF)利用三阶球面径向容积准则，对概率密度函数进行近似，相较于以上的滤波方法，CKF的精度和稳定性都有所提高，为解决非线性和噪声不确定问题提供了一个新的起点。

以上方法均为非线性系统滤波常用的方法，但普遍存在跟踪能力不强和自适应能力差的问题。在系统受到观测值异常或者状态量突变的影响时，滤波器容易产生精度下降的问题，甚至出现滤波发散的情况。文献[6]提出了自适应CKF方法，能够有效增强系统的跟踪能力，系统的观测值异常或状态量突变问题也得到了很好地解决。文献[8]提出了一种抗差方法，能够有效减弱波动较大的数据对于滤波器稳定性的影响。

针对滤波跟踪能力不强和自适应能力差的问题，提出了一种改进自适应抗差CKF算法，在自适应修正的基础上使用抗差方法可以更好地减弱异常观测值的影响，结合奇异值分解(Singular Value Decomposition，SVD)方法，可以使得滤波器更好地运行，改善了滤波效果，提高了滤波器的稳定性。

1 系统状态方程和观测方程

1.1 超宽带测量方程

超宽带(Ultra-Wide Band，UWB)测量方程可表示为

=()+

(1)

其中，=[,,…,]；=[,,]；(·)为观测方程；=[,1,…,,]是零均值高斯测量误差噪声，其协方差矩阵为。

1.2 惯性导航系统测量方程

惯性导航系统(Inertial Navigation System，INS)测量模型，通过惯性测量单元(Interial Measurement Unit, IMU)传感器的测量角速度和测量加速度获取目标位姿信息，测量模型如下

=()+

(2)

1.3 UWB/INS融合定位方程

非线性系统模型如下

(3)

2 改进自适应抗差CKF算法

2.1 新息自适应修正

新息是实际值与量测预测值之差，能比较直观地反映出预测值偏离实际状态的趋势与程度。新息一般用于对系统进行判定，并决定是否需要修正以及如何修正。

图1 自适应修正判决门限

(4)

修正系数()的选取需要遵循适度修正原则，避免过度修正。主要考虑两方面：一是修正系数与系统量测精度有关；二是与新息+1的维度有关。如新息中包含位置信息，位置信息对应的修正系数()可以选取稍大一些，不应超过0.01;速度的修正可以通过位置变化传递，则可以选取稍小些。

(5)

当新息小于判决门限时，则系统不要修正，按照CKF程序正常滤波即可。

2.2 抗差因子

在自适应修正的基础上，引入抗差因子，减小异常观测值对CKF过程的干扰。当观测信息精度很高时，需加大观测值在状态估计中的权值；反之，当观测信息误差偏大时，需降低观测值在状态估计中的权重。

在系统得到新息之后，将新息进行标准化得到抗差

=,+1,+1

(6)

其中，,+1为新息的第个分量；,+1为其标准差。

类似IGGIII等价权函数模型，对观测抗差值进行分类，分为3个等级进行筛选，分别对应三种不同的抗差因子

(7)

式中：、为常值，通常选取=15～20，=30～85；为标准化抗差。需要注意的是，抗差因子不能设置为0，否则可能会影响观测向量协方差矩阵的迭代更新。

根据观测抗差值引入抗差因子，对观测噪声协方差阵进行修正，即

(8)

2.3 SVD

为了避免CKF算法中由于Cholesky分解导致的系统状态协方差矩阵出现非正定的现象，可以采用SVD方式代替传统的Cholesky分解。

SVD方法，定义如下：

假定∈×(≥)，则矩阵的SVD可以表示为

(9)

式中，∈×；∈×；∈×；=diag(,,…,)，≥≥…≥≥0。的列向量为矩阵的左奇异向量，的列向量为矩阵的右奇异向量。

2.4 改进的自适应抗差CKF算法

改进自适应抗差CKF算法步骤如下：

1)状态参数初始化

2)计算容积点

(10)

式中，为状态量的维数，为容积点集，如下所示

(11)

式中，[1]代表单位矩阵。

3)传播容积点

(12)

4)计算状态量预测值及误差协方差预测值

(13)

(14)

5)计算容积点

(15)

6)传播容积点

(16)

其中，为系统测量函数。

7)计算测量预测值

(17)

8)计算新息

(18)

9)比较新息中的对应变量与门限值，进行状态预测值修正

|+1|>

(19)

10)计算观测预测协方差

(20)

11)计算互协方差

(21)

12)计算增益更新、状态量、误差协方差

(22)

(23)

(24)

13)若式(4)执行，则需要根据式(5)估计误差协方差阵进行补偿；若不执行，则跳过式(5)。

3 实验分析

本文设计了基于UWB与INS的融合定位仿真实验，对自适应抗差CKF融合算法的鲁棒性能进行评估。硬件由DWM1000模块与IMU传感器ADIS16465组成。实验场地如图2(a)所示。

在实验环境中，4个基站高度相同均为2.05m。基站坐标为(3,0,2.05)，基站坐标为(3,3.9,2.05)，基站坐标为(0,3.9,2.05)，基站坐标为(0,0,2.05)，单位为m。UWB标签与INS实际运动轨迹如图2(b)所示，运行轨迹中设有障碍物，目的是有效验证非视距处的UWB解算误差情况。为了验证算法的有效性，采用MATLAB进行仿真分析。

(a)

如图3所示，在仿真过程中，UWB受到障碍物以及实际环境中噪声干扰的影响，其结果在障碍物附近和起点处数据稳定性较差，整体稳定性较低。

图3 UWB定位结果

如图4所示，将UWB与INS进行松耦合，结合改进自适应抗差CKF算法进行定位，在起点与障碍物处定位轨迹更加平滑、更接近实际运动轨迹。由此可以得出，该方法状态估计更稳定，系统鲁棒性更好。

图4 UWB/INS融合定位结果

同一场景下，对原始数据分别用EKF和CKF算法进行数据融合，并与改进自适应抗差CKF数据融合误差情况进行对比。传统EKF与CKF均降低了原始数据的整体误差，平滑了误差毛刺，提升了系统稳定性。如图5所示，改进自适应抗差CKF算法与传统EKF和CKF相比,进一步降低了系统整体误差值，系统在0～100s和150～300s区间内效果最明显。改进自适应抗差CKF与其他几种滤波方法相比，精度更优，系统稳定性更好。

图5 不同滤波方法数据融合误差对比

各个滤波算法误差数值分析如表1所示。可以看到，传统CKF相较于EKF均值误差降低了2.0451cm，而改进的方法较EKF均值误差降低了6.8326cm。在此基础上，所提方法最大误差值相较于其他方法均有明显降低，在系统出现异常时，减少了数值波动，进一步提升了系统稳定性。

表1 不同滤波方法误差数值情况对比表

4 结论

本文针对容积卡尔曼滤波在多源融合定位中存在跟踪能力和自适应能力差的问题，采用改进的自适应抗差CKF算法进行仿真分析，并得到以下结论：

1)仿真结果表明，该算法能够保持较高的滤波精度和数值稳定性。

2)改进的自适应抗差CKF算法相较于传统EKF算法平均误差降低了28.42%；相较于传统CKF算法平均误差降低了23.268%。加入的自适应修正和抗差因子减小了异常观测值对传统CKF的干扰，在障碍物附近处定位结果良好；用SVD代替CKF中的Cholesky分解，有效地提高了系统数值计算稳定性，增强了在粗差干扰下的鲁棒性。因此，改进的自适应抗差CKF算法提高了多源融合定位精度。