IEKF滤波在移动机器人定位中的应用

强敏利，张万绪

（西北大学 信息科学与技术学院，陕西 西安 710127）

随着电子技术和计算机技术的快速发展及机器人定位精度的不断提高，智能移动机器人的应用越来越广泛。现已广泛应用于工业生产、海空探索、军事、家庭和一些服务行业。

精确的定位是移动机器人安全、有效地完成任务的关键和前提[1]。现有的大多数定位方法是通过机器自身携带的传感器实时感知自身所处环境的位置和周围信息，并不断修正自身状态，然后在有障碍物的环境中有效地完成任务。

卡尔曼滤波[2]是由一系列递归数学公式描述的。它们提供了一种高效可计算的方法来估计过程的状态，并使估计均方误差最小，应用广泛且功能强大。但如果模型或系统噪声特性估计与实际不符，则会降低滤波器的精度甚至导致滤波器发散。为了解决此问题，本文结合外部环境特征信息并在测量更新阶段多次迭代滤波估计值，减小定位误差，提高系统稳定性。

1 系统模型

本文的研究对象是一种三轮智能移动机器人，该机器人的其中一轮为万向轮，另外两轮为驱动轮。两个驱动轮上装有光电码盘。利用驱动轮的码盘数据，可以计算机器人的动态位置信息。机器人的正前方装有激光雷达测距传感器，可以采集已知环境中的路标信息。准确的系统模型直接影响着机器人的定位精度。因此，本文首先根据机器人信息建立了坐标系统模型、机器人运动模型和传感器观测模型。

1.1 坐标系统

定位是对机器人的位姿进行估计，用三个变量[xx(k),xy(k),xθ(k)]T表示，其中(xx(k),xy(k))表示机器人的位置，xθ(k)表示机器人车体的方向。为了确定机器人在平面中的位置，建立了二维平面全局坐标系，如图1所示。坐标原点为O，取在机器人正前方测距传感器的中心位置，并把原点作为机器人的起始位置。X轴的正方向作为机器人开始运动时的正前方，即以 X轴作为 0°，顺时针方向为负，逆时针方向为正，夹角范围是-180°～180°。假设机器人从O点运动到P点，相对初始位置的偏移角度为α，则机器人在二维平面中的位姿可描述为[x,y,α]T,其中(x,y)是 P点的坐标，表示机器人在 P点的位置，α是机器人车体相对全局坐标X轴的夹角,表示机器人在P点的航向角。

图1 机器人的坐标模型

1.2 运动模型

机器人的运动模型即位姿估计模型，是根据内部传感器里程计初步估计出机器人当前的位姿。理想条件下，机器人的运动模型应该准确地描述机器人的运动，得到机器人的状态变化过程。然而，用有限的参数进行系统建模不可能完全表达系统的动态变化过程，同时传感器数据和车体的运动都带有噪声，给机器人的运动模型带来了不确定性。因此，要完整地描述机器人的运动，必须采用一个高度复杂的非线性函数，这为定位算法的实现带来了难度。可以用一个离散时间差分方程[3]表示：

其中，f(x(k-1))是系统的状态转移函数，一般是非线性的，u(k)是输入控制指令，q(k)是过程噪声，用来表示机器人运动过程中，传感器的误差漂移、轮子的滑动和系统建模等的误差。实际应用中，通常采用一个简化的运动模型来近似，如式(2):

其中，x(k-1)表示机器人位姿；△T为传感器采样时间；v为机器人运动速度；△θ为转角(gamma)；WB为两驱动轮的轴距，x(k)是机器人的新位姿。

1.3 观测模型

根据机器人运动模型估计的位姿，只是实际位姿的一个粗略的估计，由于轮子打滑等原因存在一定误差，而且随着机器人移动时间的增加，误差将越来越大，最终移动机器人将偏离自己的轨迹。为了修正误差，移动机器人需要利用激光雷达传感器，对路标进行观测，修正自身的位姿。

本文利用激光雷达测距传感器的观测信息来修正机器人自身的位置。机器人在运动过程中，利用测距传感器扫描周围环境，获得的观测量z(k)是周围环境路标相对于传感器的距离和方向，然后根据先验的地图信息确定自身的实际位置。在定位问题的状态空间描述中可以表示为：

其中，h(·)是测量函数，r(k)是观测噪声，表示运动和观测中的不确定性。

图 2表示移动机器人在位置[xx(k),xy(k),xθ(k)]T时，用激光雷达观测到第i个路(xi,yi)时机器人与路标的位置信息。其中，d表示路标与机器人的相对距离,α-xθ(k)表示移动机器人在[xx(k),xy(k),xθ(k)]T时，路标距离机器人方向的角度。

图2 机器人与路标位置对应关系

激光雷达返回的路标观测信息为机器人与路标的相对位置信息(d,g)，d为路标相对机器人的距离，g为路标相对机器人的夹角，则观测模型可以表示为：

式(4)中的观测模型是由第i个路标的位置坐标(xi,yi)和机器人位姿[xx(k),xy(k),xθ(k)]T计算得到第 i个路标特征相对机器人的距离和机器人前进方向的夹角。

2 IEKF滤波定位算法

机器人从已知位置出发，利用里程计及惯性导航仪可以保持对自身运动的跟踪。由于里程计存在误差，移动一段距离后，机器人对其位置会变得不确定。机器人应用迭代扩展卡尔曼滤波定位算法可以实现相对环境地图定位，消除不确定性，否则，将会偏离预计路径。迭代扩展卡尔曼滤波[4](IEKF)是非线性系统进行参数估计和数据融合[5]常用的算法，用泰勒级数将非线性方程进行线性化，然后再遵循经典的卡尔曼滤波算法对系统状态进行估计。具体的融合定位结构框图如图3所示。

图3 多信息融合定位结构图

IEKF算法是以EKF算法为基础，可以归纳为状态预测、观测与匹配、迭代与更新三个步骤。

首先，进行状态预测。机器人初始时刻的位姿已知，假设k-1时刻获得的状态估计值为,利用式(2)系统运动模型获得k时刻的状态估计值(k)。然后分别计算系统模型对状态和过程噪声的雅克比矩阵，预测误差的协方差P(k)-，完成状态的预测过程。

最后，利用观测值迭代更新预测值。先分别计算观测模型的雅克比矩阵 Hi(k)，卡尔曼增益 Wi(k)，再利用Hi(k)和Wi(k)更新误差协方差矩阵。其次计算观测变量的残余vi(k)，更新状态。

综上所述，IEKF滤波定位的具体步骤可以表示如下：

(1)给定已知的环境路标与实际机器人路径，建立数据库。

(2)初始化机器人位姿与协方差矩阵。假设机器人从原点开始运动，则：

根据式(2)得具体位姿预测形式如下：

误差协方差预测：

其中，Gv是f对x偏导的雅克比矩阵：

Gu是f对q的偏导的雅克比矩阵：

Q为过程噪声的协方差矩阵。

(4)获取观测信息。根据观测方程(3)得到环境信息。首先根据预测的状态信息x^-(k)以及已知的地图路标信息得到预测的观测值z^(k)，然后利用激光雷达观测环境中的路标得到实际的观测值z(k)并进行特征匹配。

(5)状态更新。根据观测值修正预测的机器人位姿。具体步骤如下：

②For i=1:N

计算观测模型的雅克比矩阵：

计算卡尔曼增益：

更新误差协方差矩阵：

计算观测变量的残余：

更新状态：

③ 将迭代结果作为k时刻的状态更新值。

其中：“^”代表估计，“-”代表先验，“+”代表后验，N 为迭代次数。

3 仿真实验及结果分析

这一部分对IEKF算法进行实验验证，并与传统的EKF算法进行比较。

实验所用的环境区域是100 m×100 m的室内环境(假设障碍物静止不动)。机器人运行路径的关键点(waypoint)和路标 (landmark)均人为指定，实验中随机设定了7个路径关键点和8个路标，如图3所示。移动机器人的运动速度v=2.0 m/s,角速度 rate g=20×π/180 rad/s。激光测距传感器的最大观测距离为30 m,观测角度范围为-30°～30°。 控制噪声为 σv=0.6 m/s,σg=1.0×π/180 rad，观测噪声为 σR=0.1 m/s,σB=1.0×π/180 rad， 机器人两个驱动轮之间的间距WB=0.5 m，传感器采样时间△T=0.025 s。

假设机器人从原点[0,0,0]T开始运动,分别应用EKF和IEKF两种算法进行实验，结果如图4所示。

图4 机器人定位结果

图中分别为机器人实际的路径、EKF算法和IEKF算法估计路径，“*”为设定的路标 (landmark)，“+”代表 EKF 算法和IEKF算法估计的路标，“o”代表路径关键点 (waypoint)。

从图4可以看出，在刚开始的一段时间，两种算法的状态估计和特征点估计较理想，但随着时间的推移，IEKF所估计的状态能较好地与真实状态吻合，效果最为理想。表1给出了两种不同算法分别在X轴方向、Y轴方向和方位角的均方误差。可以明显地看出，IEKF算法的定位精度优于EKF算法。

表1 两种滤波算法在各个方向的均方误差比较

IEKF算法与传统的EKF算法不同之处在于状态更新阶段，在状态更新阶段IEKF算法采用多步迭代，多次对观测方程线性化，重复整个滤波过程，最终达到最优的状态估计。 图5为改变更新阶段的迭代次数，其他参数设置不变产生的定位曲线与真实曲线的对比。

图5 不同迭代次数产生的定位曲线

可以看到，总体上不同迭代次数的IEKF算法定位曲线与真实曲线基本吻合，定位结果比较理想。随着迭代次数的增加，定位精度更加精确，具体反映在表2中。

表2 不同迭代次数在各个方向的均方误差比较

众所周知，迭代次数越多，计算量越大，时间越长。因此，实际机器人定位中，针对实际硬件条件，可以选取合适的迭代次数提高算法的定位精度，增强算法的收敛稳定性。

由两次实验结果可以看出，IEKF算法估计的路标特征和机器人路径与实际基本相符。从实验所得的误差数据可以看出，相对于机器人自身的空间大小，误差在可接受的范围之内，表明该定位系统定位精度较高，具有较高的可靠性。

本文针对机器人定位精度问题，在传统卡尔曼滤波的基础上，提出一种迭代扩展卡尔曼滤波算法。该算法相对于传统卡尔曼滤波算法，在状态更新阶段采用多次迭代更新，减少了系统线性化时所带来的误差，并将该算法与多传感器信息融合技术结合应用于有路标的机器人定位。模型简单、存储量小。实验结果表明该算法在保证实时性的同时较大地提高了定位精度，能够满足机器人高精度及可靠性等方面的要求。

[1]丁伟，孙华，曾建辉.基于多传感器信息融合的移动机器人导航综述[J].传感器与微系统，2006,25(7):1-3.

[2]石杏喜，赵春霞.基于概率的移动机器人SLAM算法框架[J].计算机工程，2010,36(2)：31-32.

[3]曾健平,王保同，谢海情.自主移动机器人定位系统Kalman滤波算法改进[J].计算机应用研究，2011,28(5):1710-1712.

[4]李良群，姬红兵，罗军辉.迭代扩展卡尔曼粒子滤波器[J].西安电子科技大学学报，2007,34(2)：233-238.

[5]陈小宁，黄玉清，杨佳.多传感器信息融合在移动机器人定位中的应用[J].传感器与微系统，2008，27(6)：110-113.

[6]邹智荣，蔡自兴，陈白帆.移动机器人SLAM中一种混合数据关联方法[J].小型微型计算机系统，2011,32(7)：1341-1343.