基于扩展卡尔曼滤波的移动机器人定位技术

基于扩展卡尔曼滤波的轮式移动机器人定位技术

李昌明1梅莉2秦东兴1

（1.电子科技大学机械电子工程学院，四川成都611731; 2.克拉玛依职业技术学院电

子与电气工程系，新疆独山子833600 ）

摘要：提出了一种将相对定位与绝对定位相结合的移动机器人多传感融合定位方法。采用码盘、陀螺仪进行机器人相对定位，采用激光雷达进行机器人绝对定位。建立环境地图、传感器及机器人运动模型，以扩展卡尔曼滤波作为多传感器融合技术，建立多传感器信息融合模型，实现了移动机器人的精确定位。

关键词：轮式移动机器人定位多传感融合扩展卡尔曼滤波

Localization of wheeled mobile robot based on Extended Kalman Filtering

Chang-ming LI1,Li MEI2,Dong -xing QIN1

(1.College of Mechanical and Electronic Engineering ,University of Electronic Science and

Technology of China,Chengdu, Sichuan Province, 611731;

2.Department of Electrical and Electronic Engineering,Karamay Vocational&Technical

College,Dushanzi ,Xinjiang Province ,833600)

Abstract: A mobile robot localization method with Relative positioning combined with absolute orientation is presented. The code salver and gyroscope are used for robot relative positioning, and the laser radar is used to detect the robot absolute orientation. Environmental map, sensors and the robot motion is modeled, and the multi-sensor information fusion model is established with Extended Kalman Filtering as multi-sensor data fusion technology to realize the precise

orientation of mobile robot.

Keywords: wheeled mobile robot, Positioning, multi-sensor, EKF.

1 引言

随着科学技术的发展，移动机器人技术取得了巨大的发展，但随着机器人工作环境与任务的日趋复杂，人们无论从功能还是性能上都提出了更高的要求，传统机器人正向智能机器发展。机器人的智能化给机器人的导航提出了新的挑战，作为机器人的导航的基础，移动机器的精确定位技术受到人们的极大关注。同时作为移动机器人的一个重要分支，轮式机器人因其结构简单，便于控制，执行效率高等优点，受到人们的广泛研究及应用，轮式机器人的定位技术已成为机器人研究人员研究的热点。

由于传感器都存在着自身缺陷,获取的信息量有限，单一传感器信号难以保证输入信息的准确性、可靠性及充足性，因此机器人依靠单一传感器信息不能精确定位。多传感器信息的输入将有效解决信息量不足问题，多个传感器可提供同一位置的冗余信息和互补信息，通过对所有传感器信息的合理融合，机器人将获得自身的位置信息即达到精确定位目的。多传感器融合技术便是针对一个系统中多个传感信息处理的新研究方向。

本文提出具有特征提取的扩展卡尔曼滤波定位方法，以里程计、陀螺仪、激光雷达为主要传感器，将相对定位与绝对定位进行组合，里程计与陀螺仪的融合滤波作为机器人运动学模型。激光雷达获取环境特征，对机器人进行绝对定位，建立其模型作为机器位置观测模型。运动模型与观测模型进行结合，利用扩展卡尔曼滤波对环境特征进行跟踪，最终实现机器人的精确定位。

P(x,y,?)

(x,y,?)p 'θθ's ?θ?X Y

R X R Y R

X R Y 图2 差动驱动机器人的运动学模型估计融合陀螺仪估计融合位置预测估计融合

环境地图（数据库）

观测预测匹配

实际观测值(激光雷达)预测的

观测值匹配预测与实际观测值提取的特征图1 系统工作原理图2 系统工作原理

图1为设计的扩展卡尔曼滤波机器人定位

系统的原理图。由图可知，该系统的过程为一个递推过程。第一步位置预测或动作更新，

把高斯误差运动模型直接应用到机器人所测

量的里程计与陀螺仪的行走，获取里程计和陀螺仪的数据，通过里程计和陀螺仪的估计融合，产生机器人的预测位置。然后根据预测位置，在环境地图数据库中找到与之匹配

的预测观测值，即预测激光雷达将提取到的

环境特征及特征的位置信息。在匹配过程中，机器人将预测的观测值与激光雷达的实际观

测值进行匹配，找出最佳匹配。最后应用扩

展卡尔曼滤波将最佳匹配所提供的信息进行数据融合，更新估计中机器人的信任度状态，即得到机器人位置的最佳估计。

本文采用的是差动驱动轮式机器人作为实验平台，在驱动轮的电机上安装编码器作为里程计，附加导向传感器陀螺仪，读出两者数据来推算获得机器人的位置信息。因为机器人运动中集成了两传感器测量误差，所以位置误差随时间累加。机器人上安装的外感受传感器激光雷达对运动过程中的位置误差的消除起着关键作用。系统工作过程中激光雷达外部环境与地图的匹配，获得了机器人的绝对定位信息，利用绝对定位信息不断修正了机器人自身的误差量，从而系统克服了仅信任里程计或陀螺仪定位带来的越来越大的累积误差，从而实现机器人长时间的精确定位。

3 传感器模型

3.1 里程计模型

里程计作为相对定位的有效传感器已在轮式机器人运动定位领域中得到了广泛应用，它根据安装在机器人两个驱动轮上的光学编码器来检测车轮在一定时间内转过的弧度，来推算机器人该时间段内姿态的改变。一般而言，移动机器人的姿态可以用下面这个向量表示： ??????????=θy x p （3-1）差动驱动机器人的运动模型如图2所示，在极短

固定采样时间t ?内机器人由p 移动到位置p '，

姿态的改变量可以由编码器反回量的积分予以

估计，机器人的轨迹可以用直线表示，因此p 到

p '机器人位姿改变量（x ? ，y ?，θ? ）为：

x ?=s ?)2/cos(θθ?+ （3-2） y ?=s ?)2/sin(θθ?+ （3-3）

s s l r ?-?=

?θ (3-4) s ?= 2l r s s ?+? (3-5) 式中：r s ?;l s ?分别为左右轮行走的距离; b 为两差动驱动轮之间的间距。

由此，我们更新过的位置p '为：

????

????????+??+?+??????????=?????????????+=??????????'''='θθθθθθθθ)2/sin()2/cos(s s y x y x p y x p (3-6) 上式就为里程表位置更新的基本方程。在增量运动（r s ?，l s ?）中，存在传感器不确定性积分误差和近似运动模型带来的误差。我们须要建立整体位置p '的误差模型，得到里程表位置估计的协方差矩阵

∑'p ，∑'p 由下式确定。 T T p p f f f p f p rl rl ?????+?

?='∑∑∑ (3-7) 我们假定初始点协方差矩阵∑p 已知，运动增量（r s ?，l s ?）协方差矩阵为: ??

??????=?∑l l r r s k s k 00 (3-8) r k 和l k 为误差常数，代表驱动电机，轮子和地面交互的非确定性参数。r k 和l k 有具体值应由实验确定。

利用式（3-6）可以计算两个雅可比矩阵: ????

???????+??+?-=????????????=?=100)2/cos(10)2/sin(01θθθθθs s f y f x f

f F P P (3-9)

????????????????-?+?-?+?+?+?+?+?+?+?+?-?+=?=??b b b s b s b s b s F rl rl 11)2cos(2)2sin(21)2cos(2)2sin(21)2sin(2)2cos(21)2sin(2)2cos(21θθθθθθθθθθθθθθθθ (3-10)

分析完里程计的过程和误差模型，我们可以将其作为机器人运动学模型，确定控制输入u(k)的运动后，预测机器人的位置和它的不确定性。信任度状态假定为高斯，用以上p '

P(x,y,?)R X R Y α

W r

R α

R W X W Y ),(i i θρi ρi θO 图 3 最小平方意义的估计直线及环境坐

标｛W ｝到机器人坐标框架{R}位置表示和∑'p 两个参数来表征信任度状态。

3.2 陀螺仪模型

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

T i d S

?+=?+='0ωθθθθ (3-11)

其中θ为未更新时的姿态角，i ω为陀螺仪的输出角速度。

陀螺仪的主要误差分为两类：尺度误差和偏移误差。尺度误差主要是由陀螺仪的输入与输出之间存在着一定比例关系，由陀螺仪自身决定，属于有规律的漂移误差。偏移误差是由外部环境的影响，陀螺仪在零输入的情况下也会产生随机有限的输出，偏移误差属于随机漂移误差，大范围的温度变化和噪声的影响是产生陀螺仪漂移误差的主要原因。本文假定陀螺仪的测量值服从高斯白噪声分布，对零输入时建立拟合误差模型，确实模型参数，最后求出方差Q(K)。

3.3 激光雷达模型激光雷达又叫激光测距仪，它与超声的测距仪

的原理类似，是一种有源测距装置。其优点是它能够在扫描平面上对周围环境按一定的角度分辨率进行扫描测距，从而得到环境中测量点的距离信息ρ和扫描角度信息θ。

这些点所能反应的一般都为环境中的基本特征。从激光雷达中提取的是直线特征。激光雷达在机器人单一位置上测量的数目都是大于被估计的直线特征参数数目，因为传感器的测量存在误差，选用一定优化算法提取特征，使之与全部测量值之间的差异最小。

如图3所示，在机器人传感器所处的极坐标

中，产生n 个测量点i x =（i ρ，i θ），我们假定距离信息ρ和扫描角度信息θ的测量误差受高斯概率密度曲线的约束，平均值为测量值，方差分别为常量ρσ、θσ，且彼此独立。这些测量点可以估计出一条最优直线。给定测量点（ρ，θ）们计算相应的欧氏坐标为：θρcos =x ;θsin =y 。给定一条直线方程如下式：

0)cos(sin sin cos cos =--=-+r r αθραθραθρ (3-12)

特定点i x =（i ρ，i θ）到直线之间的正交距离为:

i i i d r =--)cos(αθρ (3-13) 将属于同一条直线的数据点归并成类，并采用最小二乘法进行直线拟合，最后得出拟合直线参数α和r 如下：

???? ?

?+--=∑∑∑∑∑∑)(cos 2cos sin cos 2sin arctan 2122j i j i i i j i j i i i θθρρθρθθρρθρα (3-14) ∑-=)cos(αθρi i r (3-15)

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

T X R AR AR A AR F C F C ρθρθσσσσ=??????=22 (3-16)

X C 为给定n n 22?输入协方差矩阵

???

?????=??????=)(00)(0

022i i diag diag C C C X θραρσσ (3-17) ρθF 为雅可比 ?????

???????????????????????????????=n n n n r r r r r r

F θθθρρρθαθαθαραραραρθ (21212121)

(3-18) 因此，在考虑激光雷达传感器数据误差的基础上，根据提取的直线环境特征建立了传感器模型。从传感器数据提取了的第一条直线，都有一对参数),(r α和其对应的误差协方差AR C 与其对应。该协方差就是观测方程中的观测误差方差∑i R ,。

4环境特征的预测

多传感融合中最重要的就是测量预测环境特征与观测环境特征之间的匹配。预测的机器人位置)|(?k k p

将产生期望特征i t z ,。在环境地图中，我们存储的特征是直线特征，以环境坐标系参数给出。而传感器所提取的直线特征是以机器人自身局部坐标给出，因此需要将环境坐标系[W]中的测量预测特征变换到机器人框架[R]中，变换由下式给出：

???

?????+++-+-=+))sin()|1(?)cos()|1(?()/1(?)1(?,,,,i t W W i t W W i t W W i t W i k k y k k x r k k k z ααθα (4-1) 它的雅可比i h ?下式给出：

X Y

O R

X R Y A C 图5 实验中机器人行走路线图X Y O R X R Y A C B

图4 机器人行走路径;B 点位置预测（粗实线）测量（细实线)及修正融合后的机器人位置（粗虚线）的更新估计??

????---=????????????????????????=?0si cos 100???

???,,,,,,,,i t W i t W i t i t i t i t i t i t i n r y r x r y x h ααθθααα (4-2) 故由机器人预测的位置，机器人框架下可能提取的环境特征被获取。根据扩展卡尔曼滤波可以得出以下观测方程：

)())/1(?()1(?k w k k p z h k z

i t i i ++=+ (4-3)

式中)(k w i 为传感器的观测误差。

5实验

本实验平台为ARM8962 32位ARM Cortex-M3

主控芯片,协处理器DSP 作为数据处理单元进行

控制的轮式自主机器人。根据实验模型，机器人上同时安装了内部传感器光电编码器、陀螺仪和外部环境传感器激光雷达。激光雷达采用SICK LMS200，该传感器能对?

180~0范围内进行扫描，返回环境测量点距离信息和角度信息。如图4所示，实验在走廊通道中进行，图中O 点确定为环境地图原点，在O 点上建立环境坐标系，路径规划机器人由起始位置A 移动到终点C ，行走距离20米。机器人移动速度为1m/s,采样时间控制为0.5s ，估计的初始置A ),,(θy x 为（0，0，0）。为验正算法收敛性，实际A 一定程度偏离原点。机器人移动过程中采用上述扩展卡尔曼滤波多传感融合算法进行定位。在一点B 我们给出机器人位置预测（细线）与测量（粗线），以及修正融合后得到的机器人位置更新估计（粗虚线）。机器的位置预测，测量及最新估计都存在不确定性，因此机器人的位置以椭圆表示。机器人预测与观测的特征匹配中采用Mahalanobis 距离进行判断，找出相应匹配。

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

7 结论

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

参考文献：

【1】SangJoo Kwon, Kwang Woong Yang. An Effective Kalman Filter Localization Method for Mobile Robots. Beijing ,China: Proceedings of the 2006 IEEE/RSJ International

Conference on Intelligent Robots and Systems,2006,October 9-15:1524-1529

【2】Leopoldo Jetto, et al. Development and Experimental Validation of an Adaptive Extended Kalman Filter for the Localization of Mobile Robots . IEEE Trans. Robotics and Automation,1999,15(2):219-229

【3】Lin Y Z, Huang Y M, Shi E X. Application of data fusion algorithm based on Kalman filter in mobile robot position measuring system [C]//Proceedings of the 5th World Congress on Intelligent Control and Automation. New York: IEEE Press,2004:4956-4959【4】Borenstein,J.,Everet,t H.R.,Feng,L., Navigating Mobile Robots, Systems and Techniques.Natick,MA,A.K.Peters,Ltd.,1996

【5】Borenstein,J.,Everett,H.R.,Feng,L., Where Am I ? Sensors and Methods for Mobile Robot Positioning. Ann Arbor, University of Mihigan.

【6】Arras,K.O.,Tomatis,N., Improving Robustness and Precision in Mobile Robot Localization by Using Laser Range Finding and Monocular Vision. Proceedings of 3rd European Workshop on Advanced Mobile Robots(Eurobot 99),Zurich, September 6-9,1999. 【7】Arras,K.O.,Tomatis, N.,“Feature Extraction and Scene Interpretation for Map-based Navigation and Map Building”,in Proceedings of SPIE, Mobile Robotics XII,Vol.3210,1997,42-53,.

【8】Kanayama.A stable tracking control method for an autonomous mobile robot.In:Proc IEEE Int Conf Robots. IEEE Trans on Robotics and Automation. 1996,12:47-61

作者简介：李昌明（1987——）男（汉）湖北宜昌人硕士(在读) 四川成都市高新区（西区）西源大道2006号电子科技大学机械电子工程学院精密仪器与机械专业

电话：158********，邮箱：lichangming16@https://www.360docs.net/doc/a07250896.html,邮编611731