基于改进IEKF 的室内移动机器人定位算法研究

刘钰芃，王斌宇，张淞淏，杨伊帆，王晓燕

（西安建筑科技大学机电工程学院，西安 710055）

移动机器人定位技术是衡量移动机器人自主导航能力的重要指标[1]。近年来，随着国内外学者对多传感器信息融合定位技术研究的深入，多传感器信息融合定位算法得到了快速发展，包括扩展卡尔曼滤波定位[2]、无迹卡尔曼滤波定位[3]和粒子滤波定位[4]。其中，基于扩展卡尔曼滤波的多传感器信息融合定位技术在无人机探测、水下搜索和无人驾驶等领域的应用最为广泛。

扩展卡尔曼滤波可以有效解决非线性系统估计的问题，但线性化过程会带来误差。为了解决这个问题，文献[5]提出了一种基于多传感器信息融合的迭代扩展卡尔曼滤波定位算法，将每次系统测量更新得到的系统后验估计作为下一个扩展卡尔曼滤波测量更新过程的先验估计进行更新计算，提高了滤波精度。文献[6]提出了一种基于新息估计的AEKF 算法。该算法利用系统的新息矩阵，自适应调整状态噪声和观测噪声的协方差矩阵，从而抑制状态误差的增加。自适应IEKF 算法修正了噪声协方差，但不可避免地会累积计算误差，可能导致卡尔曼滤波增益失去加权调整的功能。

本文提出一种改进的IEKF 定位算法。该算法通过新息估计方法自适应调整系统的状态噪声协方差和观测噪声协方差，减小恒定噪声协方差的影响；引入遗忘因子调整误差协方差预测值，限制了卡尔曼滤波的长度，减小了旧数据的累积计算误差；最后，采用Levenberg-Marquardt（LM）方法优化IEKF 的迭代过程，以提高全局估计的收敛性。

1 系统模型

本文以两轮差分移动机器人为研究对象。系统的状态模型由里程计模型建立，观测模型由激光雷达测距传感器建立。模型图（图1）和计算公式如下。

图1 两轮差速移动机器人的状态模型

两轮差速移动机器人的模型如图1 所示。R是机器人运动的弧半径，A为左右轮轴的中心点，D为移动机器人左右轮之间的距离，Δx、Δy、Δθ分别为移动机器人位置在采样时间Δt内的横坐标、纵坐标和方向角的变化。移动机器人左右驱动轮的行驶距离为ΔSL和ΔSR。

两轮差速移动机器人在横坐标和纵坐标的位置变化如下

将系统的输入定义为U（k）=[ΔS，Δθ]T，系统在k时刻的状态向量由Xk∈[xk yk θk]T表示，则系统的状态方程可表示为

观测模型和计算过程如图2 所示。

图2 两轮差分移动机器人的测量模型

由图2 所知，激光雷达观测到的地标（xi，yi）与机器人在k时刻位置[xk yk θk]T的相对位置关系。d是地标与移动机器人的相对距离、α是地标相对移动机器人的夹角、α-θk是地标相对于机器人移动方向的角度。激光雷达返回的地标观测信息是机器人与地标之间的相对位置信息（d，α）。观测模型可以表示为

2 改进的IEKF 定位算法

2.1 迭代扩展卡尔曼滤波

本文将两轮差分移动机器人建立的状态方程和观察方程离散化后表示如下[7]

式中：Xk∈[xk yk θk]T是系统的状态向量；Zk∈[d，α]T是系统的观察向量；Φk，k-1是系统的状态转换矩阵；h（·）是测量函数；wk-1是状态噪声，并且wk-1∈N（0，Qk-1）；vk是系统观测噪声，和vk∈N（0，Rk）；wk-1并且vk是高斯白噪声，E（wk-1，vk）=0。

基于IEKF 算法的多传感器信息融合过程如下[8]

式中：是系统的一步态估计是系统的一步误差协方差预测是卡尔曼滤波的增益；是创新估计向量；是系统的后验状态估计是系统的预测值是后验误差协方差；是观察方程的雅可比矩阵；i是迭代的IEKF 编号（i=1，2，3，…，n）。

IEKF 将每个扩展卡尔曼滤波过程的后验估计作为先验估计，代入多次迭代的测量更新过程中，使测量信息得到充分利用，估计值在计算中接近实值。

2.2 基于遗忘因子的改进自适应IEKF 算法

由于计算误差的累积，系统误差协方差可能会失去其非负定性和对称性，从而导致卡尔曼滤波增益矩阵的损失。为了解决这一问题，本文在改进自适应IEKF 的基础上引入遗忘因子来调整系统误差协方差，限制卡尔曼滤波的存储长度，减少过去测量数据的内存，增加当前测量数据的应用，从而进一步提高系统的定位精度。

目前，计算遗忘因子常用的方法有2 种：一步法和简化算法。为了减少计算量，提高滤波器的估计速度，本文选取简化算法作为遗忘因子的计算方法，计算过程如下

计算遗忘因子：

式中：λk是遗忘因子；trace（·）是矩阵迹线的计算；Nk计算方法Mk如下

式中：Ck是k时刻新息协方差的理论值，计算如下

新息协方差一般用窗口法估计

即

式中：en是新息残差。

为了更好地利用新的测量值来修改状态估计值，避免模型误差对滤波新息值的影响，本文采用公式（22）作为遗忘因子加法，因为其可以更好地把握系统预测误差协方差的整体变化。由公式（21）可知，C^k会随着en的增大而增大，同时遗忘因子也会增大，从而提高滤波精度。

2.3 基于LM 优化的改进自适应IEKF 算法

由于IEKF 的一阶泰勒展开中的线性化误差，系统状态模型与实际测量结果不匹配。在迭代过程中，系统的误差协方差将小于实际值，这将影响估计的稳定性。针对该问题，本文采用Levenberg Marquardt（LM）方法优化了IEKF 的迭代过程，提高了IEKF 算法迭代过程的稳定性和全局收敛性。

LM 方法的主要思想是引入修正参数αi，在每次迭代中调整系统误差协方差预测矩阵，然后使用调整后的预测协方差矩阵迭代更新计算。

系统的误差协方差预测矩阵调整如下

综上所述，基于LM 优化的具有遗忘因子的改进自适应IEKF 算法的流程表示如下。

步骤1：初始化系统状态估计X^0和系统误差协方差预测矩阵P0；

步骤2通过方程（10）和（11）计算系统状态的一步预测值和一步误差协方差预测值，完成时间更新过程；

步骤3：Kk从系统状态预测值和优化的系统预测误差协方差计算卡尔曼滤波增益、新息残差向量ek和观测方程雅可比矩阵Hk，然后更新误差协方差估计和状态估计，完成测量更新过程；

步骤4：通过卡尔曼滤波增益Kk、新息残差向量ek和观测方程雅可比矩阵Hk，通过自适应调整状态噪声协方差Qk-1和测量噪声协方差Rk，分别对步骤2 和步骤3 进行调整；

步骤5：通过方程（16），（17），（20）和方程（21）从新息残差向量ek和观测方程雅可比矩阵Hk计算遗忘因子λk；然后将遗忘因子λk应用于步骤2；

步骤6：利用LM 算法优化系统误差协方差的预测值；

步骤7：重复步骤3 至步骤6，当迭代次数达到最大迭代次数时，退出迭代，输出和。

3 仿真实验与分析

3.1 仿真设计

假设在静态室内环境（障碍物保持静止）下，两轮差分移动机器人的运动速度为v=3.0 m/s、角速度为rate G=60×π/180 rad，激光雷达测距传感器的最大观测距离20 m，观察角度范围360°。状态噪声为σv=0.6 m/s，σg=1.0×π/180 rad，观测噪声σR=0.2 m/s，σB=1.0×π/180 rad，两轮差速移动机器人2 个驱动轮之间的距离D=0.5 m。总采样时间为60 s，采样间隔为0.1 s，即ΔT=0.1 s 步长。设置LM 校正参数αi=0.1，滑动窗口S=5。开展50 次蒙特卡罗实验，以采样时间内IEKF、LM-IEKF、LM-AIEKF、LM-AIEKF 和LM-FAIEKF 的估计值和实际值的RMSE（均方根误差）平均值为标准，测量算法的定位精度，RMSE 越小，定位精度越高。RMSE 平均值计算如下

3.2 仿真结果

随着定位过程的进行，状态噪声和观测噪声会受到环境因素的影响。在本实验中，前300 个仿真步长中状态噪声和观测噪声的协方差设置为原始步骤的一半，即0.5Q 和0.5R；后300 个仿真步长中的状态噪声协方差和观测噪声协方差与初始值一致，通过50 次蒙特卡罗实验验证了每种算法的收敛性和定位精度。结果如图3 所示。

图3 噪声变化情况下不同算法定位误差

图3 比较了状态噪声和观测噪声变化时IEKF、LM-IEKF、LM-AIEKF 和LM-FAIEKF 的定位均方根误差。从图3 可以看出，4 种算法的定位误差随着系统状态噪声误差和观测噪声误差的增加而增大。其中，LM-IEKF 算法比IEKF 算法具有更好的收敛性和稳定性；LM-AIEKF 算法的定位精度优于LM-IEKF 算法。50 次蒙特卡罗实验中不同算法的RMSE 平均值和平均运行时间见表1。

表1 RMSE 平均值和平均运行时间

由表1 可知，通过50 次蒙特卡罗实验，与IEKF算法相比，LM-IEKF、LM-AIEKF 和LM-FAIEKF 的定位精度分别提高了39.59%、46.57%、84.62%。本文提出的改进IEKF 算法（LM-FAIEKF）的定位精度明显优于其他3 种算法。虽然该算法的平均运行时间高于IEKF和LM-IEKF 算法，略小于LM-AIEKF 算法，但具有良好的收敛性和鲁棒性，符合预期要求。

4 结论

本文提出一种改进的室内移动机器人多传感器融合定位IEKF 算法。解决了姿态估计中恒定噪声协方差误差、前一数据累积计算误差、IEKF 算法收敛性差等问题。在2 种仿真场景中，比较了IEKF、LM-IEKF、LM-AIEKF 和LM-FAIEKF 的性能。仿真结果表明，所提算法具有较高的稳定性和定位精度，符合室内移动机器人的定位要求。