基于激光测距传感器的家庭机器人导航仿真

题.
关键词: 家庭机器人; 激光测距; 同时定位和地图生成; 人工路标; 扩展卡尔曼滤波
中图分类号: TP24
文献标识码: A
文章编号: 0367- 6234( 2004) 07- 0902- 03
Navigating simulation of house robot based on laserfinder
在通常的室内环境下, 自主移动机器人的应 用依然面临很多挑战性问题[ 1] . 定位问题就是其 中之一. 在室内环境下广泛应用于机器人导航的 超声波传感器的缺点是其角度分辨率太低( 约为 25b左右) 、旁波较大, 需要消耗过多的数据后处理 时间[ 2] . 文献[ 3] 使用全维立体视觉作为外部传 感器, 通过锥形镜和摄像机, 生成移动机器人周围 环境的全景图. 这种方法由于其计算的复杂性和 缺乏距离测量的动态变化, 使其应用室内受到一 定的限制. 毫米波雷达技术[ 4] 在室内机器人的障 碍检测、地图生成和导航方面具有很大的潜力, 但 缺点是成本太高. 激光测距器[ 2] 由于在距离范围 和方向上具有较高的精确度, 已经成为室内机器
机器人的状态为 X = xv, x l T,
x v = ( x , y , H, C, U1, U2, U3) I R7, x l = ( x 1, y 1, ,, x n, yn) I R2@ n .
其中 x v 和 x l 分别是机器人和人工路标的状态. 由
第7期
蔡则苏, 等: 基于激光测距传感器的家庭机器人导航仿真
# 903 #
于人工路标是静态的, 所以系统的动态模型扩展 为
x v( k + 1) = f ( x v( k ) ) , x l( k + 1) = x l ( k ) .
扩展系统的雅克比矩阵为
5F 5X
=
5f 5xv
5T
5 =
I
J1 5T
5 ,
I
J 1 I R7@ 7, 5 I R 7@ 2n, I I R2n@ 2n.
P ( k + 1 | k ) = ¨f P ( k | k ) ¨f T + Q( k) .
图 3 基于激光测距器的室内地图
3) 观察量. 激光测距器测量的路标特征值为 z ( k + 1) = [ zri , z iB] T .
路标的特征预测值为:
^z ( k + 1) = h( z ( k + 1 | k ) , x^ ( k + 1 | k) ) . 计算的新息为:
5) 更新. EKF 的增益更新为 W( k + 1) = P( k + 1 | k) ¨hT S- 1( k + 1) , 机器人的状态为
x ( k + 1 | k + 1) = x^ ( k + 1 | k ) + W( k + 1) v( k
+ 1) , 协方差矩阵更新如下:
P( k + 1 | k + 1) = P( k + 1 | k ) W( k + 1) S( k + 1) WT ( k + 1) .
[ 4] CLAK S, DURRANT- WHYTE H. Autonomous land vehicle navigation using millimeter wave radar[ J] . Proc of the IEEE International conference of Robotic and Automation. 1998, 4: 3697- 3702.
Abstract: An algorithm for simultaneous location and map building is presented by integrating the scanning data from a t ime- o-f flight laser and the odometer readings from a mobile robot. Based on an extended Kalman filter, the continuous position updat ing for the robot is given by filtering the outliers and matching the sensor data and the artifical landmarks. Simulation results show that the method performs navigat ion very well in an indoor environment. Key words: house robot; laser scanning; simultaneous localization and map building( SLAM) ; artificial landmark; extended kalman filter( EKF)
[ 2] GUIVANT J, NEBOT E, BAIKER S. Autonomous navigation and map building using laser range sensors in outdoor appl-i cation[ J] . Journal of Robotic Systems, 2000, 10( 17) : 5655 83.
1 同时定位和地图生成
同时定位和地图生成( Simulatneous localization and map building, SLAM) [ 5] 问题可以 通过传感的 融合来解决, 特征或路标的估计位置就变成机器 人状态的一部分. 机器人从一个具有给定不确定 性的位置开始运动, 逐渐获得相对其位置的环境 特征测量值, 并用这些测量值增量地创建和保持 导航地图以及在这个地图中定位.
通过激光测距器所获得的距离和方向信息都
是相对于机器人位置的, 因此观察量方程是机器 人状态和人工路标的位置状态的函数:
z
i r
=
hr ( X ) =
( x - xi) 2 + ( y - yi) 2 ,
z
i B
=
hB( X ) =
a tan
y - yi x - xi
-
C.
图 1 为家庭机器人运动的室内环境. 图 2 为
图 5 路标观察和路径跟踪
于传统航位推算法的路径、基于 EKF 的融合算法 估计的路径及路标的观测位置. 可以看出, 传统 的航位推算法的误差随时间而增加, 在机器人移 动一定距离后, 航位推算的路径偏离欲跟踪路径 较大. 基于 EKF 的融合算法, 通过激光测距器对 外部的路标进行测量, 并用路标的位置和方向来 修正跟踪误差, 形成的估计路径在整个机器人运 动过程中, 与欲跟踪的路径基本重合, 但当运动
仿真 SICK LMS 200 激光测距器所形成的室内地
图. 为了简化特征提取的计算复杂度, 采用了人工 路标法[ 6] , 分别设置了 12 个路标点 B1~ B12. 激光
测距器的配置是 180b扫描视野, 角度分辨率为 1b,
最大感知范围为 8 m, 通过 RS232 接口以 19200 波
特率获取环境的信息. 图 3 给出了基于激光测量
第 36 卷 第 7 期 2 0 0 4 年7 月
哈尔滨工业大学学报 JOURNAL OF HARBIN INSTITUTE OF TECHNOLOGY
Vol1 36 No17 July, 2004
基于激光测距传感器的家庭机器人导航仿真
蔡则苏, 洪炳 , 周浦城
( 哈尔滨工业大学 计算机科学与技术学院, 黑龙江 哈尔滨 150001, E- mail: caizesu@ hit. edu. cn)
v( k + 1) = z ( k + 1) - ^z ( k + 1) , 新息的协方差矩阵为 S( k + 1) = ¨hP( k + 1 | k ) ¨hT + R( k + 1) .
4) 匹配. 对于每一次测量, 由于测量的不确 定性, 需要对每次测量进行校验来决定是否匹配.
v( k + 1) S( k + 1) vT ( k + 1) [ G , G 为一常数, 若上式成立, 则测量值匹配, 否则不 匹配并舍弃.
收稿日期: 2004- 04- 02. 基金项目: 国家高技术研究发展计划资助项目( 2002AA735041) . 作者简介: 蔡则苏( 1967- ) , 男, 博士研究生, 讲师;
洪炳 ( 1937- ) , 男, 教授, 博士生导师.
人定位和地图生成中最有吸引力的传感器之一. 本文采用基于激光扫描和人工路标的方法, 来解 决室内环境下的机器人导航问题.
摘 要: 通过将激光测距器的扫描数据和来自家庭机 器人的 里程计结 合在一 起, 给出了同 时定位 和地图 生
成的算法. 基于扩展卡尔曼滤波算法, 滤去不必要的数据, 直接将传感数据和环境特征相匹配, 融合激光扫 描
数据和人工路标, 给出了机器人连续位置更新的方法. 仿真结果表明该方法较好地解决了室内机器人导航 问
的地图, 图中采用了网格占据几率来描述激光数
据的关联性.
图 1 家庭机器人运动的室内环境
图 2 基于人工路标的室内地图创建
2 基于扩展卡尔曼滤波的导航算法
使用 扩展卡尔 曼滤波算 法( Extended Kalman Filter, EKF) [ 1, 7] 可以较好地融合里程计数据和激 光测距数据. EKF 算法使用平均值和协方差矩阵
6) 返回第二步递归执行上述 2 ~ 5 步.
# 904 #
哈尔滨工业大学学报
第 36 卷
3 仿真结果
图4 给出了在室内环境下通过设定人工路 标, 采用扩展卡尔曼滤波算法融合激光测距数据 和里程计数据, 完成的机器人在室内环境下的导 航. 图 5 分别给出了移动机器人欲跟踪的路径、基
图 4 基于激光测距器的机器人导航
路径中路标稀少时, EKF 不能有效融合路标数据, 使得路径跟踪的误差较大.
4结论
基于激光测距传感器的 EKF 导 航算法可以 有效地提高家庭机器人定位的精度, 能够有效地 完成机器人的路径跟踪.
参考文献:
[ 1] JENSFELT P, CHRISTENSEN H I. Pose tracking using laser scanning and minimalistic environmental models [ J] . IEEE transactions on robotics and automation, 2001, 2( 17) : 138 - 147.
假定机器人在室内二维平面内运动状态平均值是描述路标的维数距离通过激光测距器所获得的距离和方向信息都和方向角ekf算法如下初始化移动机器人的广义状态矢量x0误差协方差矩阵p0是相对于机器人位置的因此观察量方程是机器1020301010人状态和人工路标的位置状态的函数仿真sicklms200激光测距器所形成的室内地为了简化特征提取的计算复杂度采用了人工路标法分别设置了12个路标点b1b12激光测距器的配置是180扫描视野角度分辨率为通过rs232接口以19200给出了基于激光测量的地图图中采用了网格占据几率来描述激光数据的关联性
将 SLAM 的后验值表述为高维、多变量高斯参数.
平均值描述了机器人和路标的最有可能的状态,
协方差矩阵表明了所有状态变量对之间的关联.
假定机器人在室内二维平面内运动, 状态平均值
是 2N + 7维, 其中 N 是所采用的路标个数, 7 是描
述机器人位姿的维数, 2 是描述路标的维数( 距离
和方向角) . EKF 算法如下:
1) 初始化移动机器人的广义状态矢量 x^ 0 和
误差协方差矩阵 P0.
x^
0
=
[ U20, U30, x 10, y 10,
,, xn0, y n0] T, P0 = N0 NT0 .
2) 预测
x ( k + 1 | k) = f ( x ( k) , u ( k) ) ,
CAI Ze- su, HONG Bing- rong, ZHOU Pu- cheng
( School of Computer Science and Technology , Harbin Institute of Technology, Harbin 150001, China, E- mail: caizesu2hit. edu. cn)
[ 3] DROCOUT C, DELAHOCHE L , PEGARD C, et al . Mobile robot localization based on an omnidirectional stereoscopic v-i sion perception system[ J] . Proc. of the 1999 IEEE Conference on Robotics and Automation, 1999, 2: 1322- 1328.