李昌明，梅 莉，秦东兴

（1.电子科技大学，四川 成都 611731；2.克拉玛依职业技术学院，新疆 独山子 833600）

0 引 言

由于传感器都存在着自身缺陷，获取的信息量有限，单一传感器信号难以保证输入信息的准确性、可靠性及充足性。针对这个问题，该文采用扩展卡尔曼滤波进行多传感信息融合，扩展卡尔曼滤波器（EKF）具有收敛速度快、存储量小的优点[1]。其中里程计、陀螺仪、激光雷达为主要传感器，将相对定位[2]与绝对定位[3]进行组合，里程计与陀螺仪的融合滤波作为机器人运动学模型；通过激光雷达获取环境特征，建立其模型作为机器位置观测模型。运动模型与观测模型进行结合，利用扩展卡尔曼滤波对环境特征进行跟踪，最终实现机器人的精确定位。

1 系统工作原理

图1为设计的扩展卡尔曼滤波机器人定位系统的原理图。由图可知，该系统的过程为一个递推过程。第一步位置预测或动作更新，把高斯误差运动模型[4]直接应用到机器人所测量的里程计与陀螺仪的行走，获取里程计和陀螺仪的数据，通过里程计和陀螺仪的估计融合，产生机器人的预测位置。然后根据预测位置，在环境地图数据库中找到与之匹配的预测观测值，即预测激光雷达将提取到的环境特征及特征的位置信息。在匹配过程中，机器人将预测的观测值与激光雷达的实际观测值进行匹配，找出最佳匹配。最后应用扩展卡尔曼滤波将最佳匹配所提供的信息进行数据融合，更新估计机器人的信任度状态，得到机器人位置的最佳估计，最终实现机器人基于环境地图定位的精确定位[5-6]。

图1 系统工作原理图

2 传感器模型

2.1 里程计模型

建立机器人运动二维坐标地图，移动机器人的姿态可以表示为

式中：x，y——机器人相对坐标原点的X和Y方向运动距离；

θ——机器人自身姿态相对于X轴角度。

差动驱动机器人的运动模型[7]如图2所示，在极短固定采样时间Δt内p到p′机器人位姿改变量（Δx，Δy，Δθ）为

式中：Δsr、Δsl——左右轮行走的距离；

b——两差动驱动轮之间的间距。

图2 差动驱动机器人的运动学模型

由此，更新过的位置p′为

式（6）为里程表位置更新的基本方程。在增量运动（Δsr，Δsl）中，存在传感器不确定性积分误差和近似运动模型带来的误差。由误差传播理论，里程表位置估计的协方差矩阵Σp′为

假定初始点协方差矩阵Σp已知，运动增量（Δsr，Δsl）协方差矩阵为

式中：kr、kl——误差常数，代表驱动电机、轮子和地面交互的非确定性参数，其具体值应由实验确定。

利用式（6）可以计算2个雅可比矩阵Fp和FΔr1。

2.2 陀螺仪模型

陀螺仪是一种惯性器件，用于测量其载体的转动角速度和转动角度，其输出经积分得到机器人的位姿角度变化量。因此，使用陀螺仪时首先给定一个基准方向，机器人运动中的姿态将由自身角度变化量的不断累加得到。一定时间Ts内更新过的姿态角 θ′为

式中：θ——未更新时的姿态角；

ωi——陀螺仪的输出角速度。

该文假定陀螺仪的测量值服从高斯白噪声分布，对零输入时建立拟合误差模型，确定模型参数，最后求出方差Q（K）。

2.3 激光雷达模型

如图3所示，在机器人传感器所处的极坐标中，产生n个测量点xRi=（ρRi，θRi），假定距离信息ρR和扫描角度信息θR的测量误差受高斯概率密度曲线的约束，方差分别为常量（σρ，σθ），且彼此独立。给定测量点（ρR，θR）计算相应的欧氏坐标为

xR=ρRcosθR， yR=ρRsinθR

给定一条直线方程为

特定点xRi=（ρRi，θRi）到直线之间的正交距离dRi为

图3 最小平方意义的估计直线及环境坐标W到机器人坐标框架R位置表示

将属于同一条直线的数据点归并成类，并采用最小二乘法进行直线拟合，最后得出拟合直线参数αR和 γR。

激光雷达传感器测量的不确定性将影响所提取直线的不确定性。用A和E分别表示随机输出变量αR和γR，系统输出协方差矩阵为

式中：CX——给定2n×2n输入协方差矩阵，即

Fρθ——雅可比。

因此，在考虑激光雷达传感器数据误差的基础上，根据提取的直线环境特征建立了传感器模型。从传感器数据提取的每一条直线，都有一对参数（αW，rW）和其对应的误差协方差CAE与其对应，该协方差就是观测方程中的观测误差方差Σ

R，i。

3 环境特征的预测

多传感融合中最重要的就是测量预测环境特征与观测环境特征之间的匹配。预测的机器人位置pˆ（k|k）将产生期望特征 zt，i。在环境地图中，存储的特征是直线特征，以环境坐标系参数给出。而传感器所提取的直线特征是以机器人自身局部坐标给出，因此需要将环境坐标系W中的测量预测特征变换到机器人框架R中，变换由式（14）给出

它的雅可比▽hi为

由机器人预测的位置，机器人框架下可能提取的环境特征被获取。根据扩展卡尔曼滤波可以得出观测方程为

式中：wi（k）——传感器的观测误差。

4 扩展卡尔曼滤波器原理

卡尔曼滤波器是一种基于概率模型进行状态和参数估计的有效方法，由一系列递归数学公式描述。它们提供了一种高效可计算的方法来估计过程的状态，并使估计均方误差最小。卡尔曼滤波器应用广泛且功能强大，它可以估计信号的过去和当前状态，甚至能估计将来的状态，即使并不知道模型的确切性质。扩展卡尔曼滤波器（EKF）用于处理观测变量与过程的关系是非线性的问题，首先对其进行线性近似，再利用线性系统卡尔曼滤波器的基本方程实现状态估计。在移动机器人位姿估计中，在系统噪声和测量服从高斯白噪声分布情况下，卡尔曼滤波用带有噪声的状态议程和观测议程递推决定统计意义上的最优位姿估计。

5实验

该实验平台为ARM8962 32位ARM Corte-M3主控芯片，协处理器DSP作为数据处理单元进行控制的轮式自主机器人。根据实验模型，机器人上同时安装了内部传感器光电编码器、陀螺仪和外部环境传感器激光雷达。激光雷达采用SICK LMS200，该传感器能对0～180°范围进行扫描，返回环境测量点距离信息和角度信息。

如图4所示，实验在走廊通道中进行，建立机器人运动环境地图。在O点上建立环境坐标系，路径规划机器人由起始位置A移动到终点C，行走距离20 m。机器人移动速度为1m/s，采样时间控制为0.5 s，估计初始置 A（x，y，θ）为（0，0，0）。为验正算法收敛性，实际A一定程度偏离原点。

机器人移动过程中采用上述扩展卡尔曼滤波多传感融合算法进行定位。在B点给出机器人位置预测（细线）与测量（粗线），以及修正融合后得到的机器人位置更新估计（粗虚线）。机器的位置预测、测量及最新估计都存在不确定性，因此机器人的位置以椭圆表示。机器人预测与观测的特征匹配中采用Mahalanobis距离[8]进行判断，找出相应匹配。机器人采用线性控制算法进行路径跟踪。

图4 机器人行走路径

图5 实验中机器人行走路线图

图5给出实验中机器人完整行走路线图。机器人按要求达到目的地，每个位置的定位均方差在2 cm左右，航向角小于0.5rad。实验结果表明，上述多传感融合系统的对机器人的导航起到改进作用。

6 结束语

采用扩展卡尔曼滤波算法作为机器人移动中的定位方法，克服了以往单一传感器或多传感系统系统误差表达方式的缺点。将该定位方法应用到实际机器人导航定位当中，能有效提高机器人定位精度，提高机器人定位的可靠性，具有一定的实际应用。

[1] 何子述，夏威.现代数字信号处理及其应用[M].北京：清华大学出版社，2009：250-350.

[2]吴伟，刘兴刚，王忠实.多传感器融合实现机器人精确定位[J].东北大学学报，2007，28（2）：161-164.

[3]杨柏胜，姬红兵.基于卡尔曼滤波的被动多传器融合跟踪[J].控制与决策，2008，23（4）：460-463.

[4] 杨国胜.基于多传器融合的移动机器人定位与导航研究[D].北京：中国科学院自动化研究所，2004.

[5] 徐立.基于信息融合的机器人定位及地图创建研究[D].绵阳：西南科技大学，2009.

[6] 孙铁波，刘奎武，李宏.基于模糊控制的机器人寻线控制系统改进设计[J].电子设计工程，2009，17（11）：32-33.

[7] Siegwart R，Nourbakhsh I R.自主移动机器人导论[M].西安：西安交通大学出版社，2006：225-300.

[8] Jetto L.Development and experimental validation of an adaptive extended kalman filter for the localization of mobile robots[J].IEEE Trans.Robotics and Automation，1999，15（2）：219-229.