多传感器融合实现机器人精确定位

第28卷第2期2007年2月东北大学学报(自然科学版) Journal of Northeastern University (Natural Science ) Vol 128,No. 2Feb. 2007

多传感器融合实现机器人精确定位

吴　伟1, 刘兴刚1, 王忠实2, 徐心和1

(1. 东北大学教育部流程工业综合自动化重点实验室, 辽宁沈阳　110004;

2. 沈阳大学信息工程学院, 辽宁沈阳　110044)

摘　　　要:提出采用超声波距离扫描传感器和视觉传感器数据融合技术实现室内环境复杂特征角和半平面的提取, 以便更精确地重构环境特征・利用与坐标无关的对称扰动模型建立超声波扫描的环境特征模型、改进的扩展卡尔曼滤波估计求解・以马氏距离作为特征融合判定的依据, 并且在传感器校准时, 度和消逝线的摄像机自动校准方法, , , 较为精确的二维多边形环境地图得以重建, 关　键　词:数据融合; 对称扰动模型; ; 中图分类号:TP 242　　　A () 20161204

Fusion to Precise R obot Localization

W U Wei , L IU Xi ng 2gang , W A N G Zhong 2shi , X U Xi n 2he

1

1

2

1

(1. K ey Laboratory of Process Industry Automation , Ministry of Education , Northeastern University , Shenyang

110004, China ; 2. School of Information Technology &Management Engineering , Shenyang University , Shenyang 110044, China. Corres pondent :WU Wei , E 2mail :awuwei @yahoo.com. cn )

Abstract :Based on the multi 2sensor data fusion technology , a robot with ultrasonic rangefinder and CCD camera is proposed to extract the complex characteristic corners and semiplanes in an indoor environment , so as to rebuild the relevant environmental features more accurately. Environmental features observed by ultrasonic scan are defined with symmetrical perturbation model that is regardless of coordinates , and the product of their bound matrix and relative location vector is considered as the fusion observation equation. EKF (extended Kalman filter ) is used to estimate the solution to the equation with Mahalanobis distance taken as the criterion for characteristic fusion. In addition , the CCD automatic calibration based on local intensity and vanishing line is introduced to improve the calibration accuracy of horizontal boundary points , thus enhancing greatly the corner accuracy. In such a way the 2D polynomial environmental map can be replotted to lay down a solid foundation for robot localization.

K ey w ords :data fusion ; symmetrical perturbation model ; CCD automatic calibration ; polynomial environmental map ; robot localization

将环境模型用多边形地图建模, 然后用重构的地图与环境模型进行匹配, 假设机器人配备的传感器无噪声并且所获得的信息绝对精确, 这时将机器人定位问题分成两项[1]:假设产生项和假设清除项[2]・产生项意味着机器人方向知识储备完备, 机器人航行的所有方向都已知, 清除项是用启发式清除不合理的解, 用距离函数将传感器噪声引入, 消除噪声和模型特征间的雷同之处・

机器人系统最常用的感觉网络(perception net ) 包括超声波传感器、激光传感器和视觉传感器[3]・地图匹配方法实现机器人定位是一种选择, 让机器人通过基于地图的自动搜索来自行构造地图[4], 也就是说机器人能够自动标识未探测的区域, 根据自己构建的地图导航到这些区域, 同时也不断地更新它所构建的地图・

应用多传感器融合技术一般可以解决相应的

收稿日期:2006202227

基金项目:国家自然科学基金资助项目(60475036) ; 国家教育部博士点基金资助项目([1**********]) ・作者简介:吴　伟(1969-) , 女, 辽宁昌图人, 东北大学博士研究生, 沈阳工业大学讲师; 徐心和(1940-) , 男, 河北临榆人, 东北大

学教授, 博士生导师・

162

东北大学学报(自然科学版) 　　　　　　　　　　　第28卷

表1　几何结构的特征表述

Fig. 1　SPModel repre sentation of geometric entitie s 特　征点P 边界E 角C 半平面S 视觉边界V 机器人R

扰动矢量

P P =P E =

^d x P , ^d ^d y E , ^d

T

问题・一方面, 融合来自不同传感器的信息・例如

经常采用多超声波系统实现对环境的特征提取, 声纳环[5]或者菱形声纳或者一个换能器发射、两个换能器接收的特殊位置声纳传感器[6]使得对环境或者障碍的提取方法不尽相同・在某种程度上多传感器网络的位置设计影响算法

[7]

约束矩阵

1

00B E =

0B P =

T

; 另一方

011000面, 将当前传感器的观测集成到机器人航行区域可靠的先验知识里・其中最常用的方法是概率法:将机器人位置的不确定性因素用概率分布表示, 然后用卡尔曼滤波器[8]和信息滤波器融合传感器的观测数据・

B C =I 3B S =I 3B V =(001) B R =I 3

2　定位问题

观测特征F i , i ∈{1, 2, …, n}M j , {1, 2, ・, , 于是算法的鲁棒性相应提高・

假设集合H ={h 1, h 2, …, h h }, 也就是基于二维几何特征的观测假设, 机器人的位置在假设集合内, 那么可以确定机器人的位置・每个假设h i ={L Rh i , S h i }包括两个元素, 其中S h i 为模型和观测之间可能匹配的相关集合, 定义为假设的支持集合, L Rh i 为在基坐标系下估计的机器人位置信息・令P 为观测和模型配对成功的信息集合, 那么给定假设的支持集合S h i ∈P ・多传感器的观测特征与模型特征的关联属性都反映到S h i 的配对过程, 因此整个计算过程都保持支持集合的数据, 据此来判断哪一个匹配更接近真实情况・解决移动机器人位置估计问题, 首先设定合适的误差判据为最小的估计方法・

对于给定的假设集合h ={L Rh , S h }, 其中S h

是假设支持集合, L Rh 为在图1所示基坐标系W 下的机器人位置估计信息, 其约束是假设支持集合S h 里的配对集合・因此每一个配对过程都将存在一个观测方程, 由观测特征F 和模型特征M 的相对位置描述:

(2) B M F x M F =0・式中, B M F 为配对过程的约束矩阵・由于假设支持

集合S h 描述观测间的关联属性, 而相应的配对过程批数据信息集成要求保持集成信息的独立性, 即集成那些不相关信息・

机器人初始位置未知和多传感器的应用, 使得本算法提取的特征具有很好的鲁棒性和可靠性・

1　对称扰动模型

定义每个环境几何特征的位置由(x , y ,

参考几何库中的E 是与

每个几何特征相关联的坐标系, 定义它的X 轴与这些关联的几何特征形成的轴平行・若判定某一点隶属于该无限直线, 那么其坐标的X 轴必须沿

SPModel 模型里

[9]

着这条轴线・图1说明了定义的不同结构的几何特征・为表述几何特征连续对称的集合, 不同的二维几何结构的约束矩阵如表1所示・约束矩阵可以表达一个基本几何概念:一致性・同类型的两个几何实体一致性定义如下:如果A 和B 为同一几何实体, 则它们属于同一几何结构的连续对称集合, 可以用公式(1) 阐述:

(1) B A ^x AB =0・式中, B A 为几何结构间的约束矩阵, ^x AB 为位置

矢量

・

3　单目视觉信息描述

图1　几何特征

Fig. 1　Geometric feature s

如图2所示, 检测每个图像中垂直边界・V

第2期　　　　　　　　吴　伟等:多传感器融合实现机器人精确定位定义为视觉坐标系, 可以检测到所关注的视觉边界, 摄像机光学中心定义为原点, X 轴为摄像机光学中心与视觉边界的连线方向, 视觉边界位置为L CV =(^x CV , ^P V , C V , , B V , ) , 其中,

^x CV =

0　0　arctan

f

T

163

3b 校准模式下的灰度图像检测黑白之间与边界

线相应的视觉边界V k ; 从图3c 校准模式下的基于直线分割的重构, 获取与同一边界相应的交叉点P k ・

与V k 和P k 相关的方程可描述为(x M P -x M C ) sin (

(y M P -y M C ) cos (

(4)

・(3)

式中, x uf 为P 未失真中心x 坐标, C x 为失真中心

x 坐标

, f 为焦距・也有

T

^d V =(0　0　^d

可用Matlab 库的Levenberg 2Marquardt 算法求解・

5　超声波和视觉的特征配对

如图3d 所示, 下标M V (・其B M (1　0) , ^d V =(0　0　^d

Fig. 2　field

edge in SPModel

B V =(0　0　1) , ^P V =^d

B V M (Θ(^x L V B V P V ) (^x L M B M P M ) ) =B V M (ΘB V P V Θ^x L V ^x L M B M P M ) =B V M (ΘB V P V ^x V M B M P M ) =0・

h m =f m (^P V , ^P M ) =B V M ^x V M ,

H m =G m =

T

T

T

T

T

T

σ

2

4　多传感器校准

视觉的匹配校准模式[10]如图3所示, 图3a 是原始环境图像, 图3b 为识别的两个区域・其中E 1和E 2为两个白区・图中黑白两个模式的平面角度保证两个区的检测・

(5)

设估计无误差, 即^P V =0, ^P M =0, 式(5) 可简化为

9P V 9P M

T

(^P V , ^P M )

=B V M J 2 {^x V M , 0}B V , (6) =-B V M J 1 {0, ^x V M }B M ・

T

(^P V , ^P M )

给定标准α(满足融合条件的概率) , 当距离传感

χ2r , α(这里器检测的特征与视觉边界满足D 2≤

χ2r , α为阈值, 自由度数r =rank (B M V ) =1) , 此时两者相容・

6　试验结果

图3d 描述的坐标下, 由CCD 摄像机中心C

以及超声波直线分割的特征坐标间的变换矢量X M C =(150. 52mm ,0. 129mm , -1. 5692rad ) T

图3　视觉和超声波的环境分割及校准

Fig. 3　Visual and ultrasonic segmentation of environment

and coordinate calibration (a ) —采样图像; (b ) —视频分割结果;

(c ) —超声波传感器分割结果; (d ) —校准坐标示意图・

移动机器人实现定位模式校准将在CCD 摄像机和距离传感器之间完成・模式位置索引k ∈{1, 2, …, n}・由两个传感器获取的信息为:从图

可由Levenberg 2Marquardt 算法求解得到・视觉和

超声波信息为M 坐标系下的数值・

用多传感器(超声波和CCD 摄像机) 融合检测环境特征的结果如图4所示, 视觉提取的垂直边界是将长度超过155个像素(根据环境中垂直边界的特点定义像素数) 的垂线视作为角边界和门框边界・显然这是视觉的第一判断・再与超声波

164

东北大学学报(自然科学版) 　　　　　　　　　　　第28卷

[2]

Xu Z Z , Liu J L , Xiang Z Y. Scan matching based on CLS

relationships[C ]∥Proceedings of 2003IEEE International Conferences on Robotics , Intelligent System and Signal Process. Piscataway :IEEE Press , 2003:99-104.

Makarenko A A , Williams S B , Durrant 2Whyte H F. Decentralized certainty grid maps [C ]∥Proceedings of the 2003IEEE RS J International Conference on Intelligent Robots and Systems. Las Vegas :IEEE Press , 2003:3258-3263.

Bank D. A novel ultrasonic sensing system for autonomous mobile systems[J].I EEE Sensors Journal , 2002,2(6) :597-606.

Madhavan R , Durrant 2Whyte H F. 2D map 2building and localization in outdoor environments[J].Journal of Robotics Systems , 2005,22(1) :45-63.

Dissanayake M W M G , Newman P , Clark S , et al . A solution to the simultaneous localization and map building (SLAM ) ].I EEE Robotics and 2001-, 徐心和[J], -129・

(, 2Xin 2he. Study on sense net and of mobile robots[J].Robot , 2002,24(2) :126-129. )

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 Y ork :IEEE Press , 2004:4956-4959.

Castellanos A J , Montiel J M M , Neira J , et al . The spmap :a probabilistic framework for simultaneous localization and map building [J ].I EEE Transactions on Robotics &A utomation , 1999,15(5) :948-952.

Wang Z S , Wu W , Xu X H. Auto 2recognition and auto 2location of the internal corners of planar checkerboard image [C ]∥International Conference on Intelligent Computing. Berlin :Springer , 2005:473-479.

提取的边界融合, 这样使得超声波提取的直线分割端点的失真在该模型中得到补偿・其中, 角度方差由σ降低到σ

・

[3]

[4]

[5]

图4　基本感觉信息:超声波读数和视觉边界

Fig. 4　Basic sensing in formation :ultrasonic

readings and visual field

[6]

7　结　　语

, , 1°・, 使得角度精度提高了十倍, 从而确保得到较为精确的环境特征, 为最终实现机器人准确定位奠定了良好的基础・参考文献:

[1]

Y oshida T , Ohya A. Cooperative self 2position system for multiple mobile robots[C]∥Proceedings of the 2003IEEE/ASME International Conference on Advanced Intelligent Mechatronics. Piscataway :IEEE Press , 2003:223-228.

[7]

[8]

[9]

[10]

(上接第160页)

参考文献:

[1]

economic

[5][6]

model with asymmetric density dependent

migration[J].Ecological Economics , 2006,57:466-476. Murry J D. Mathematical biology [M ].2nd ed. Beijing :Beijing Book Publishing House , 1998:25-45.

Sunita G , Ra’id K N. Chaos in seasonally perturbed ratio 2dependent prey 2predator system [J ].Chaos , Solitons and Fractals , 2003,15(1) :107-118.

[7]

Clark C W.

Mathematical bioeconomics :the optimal

management of renewable resources [M ].New Y ork :Wiley , 1990:10-69.

[8][9]

Parker T S , Chua L O. Practical numerical algorithms for chaotic systems[M ].New Y ork :Springer , 1989:66-71.

刘保政, 刘德宝, 高立群・供不应求季节性商品的价格控制和生产销售决策模型[J]・东北大学学报:自然科学版,

2005,26(11) :1040-1043・

(Liu Bao 2zheng , Liu De 2bao , G ao Li 2qun. A production/sales decision model for price control of seasonal goods on tight market [J ].

Journal of Northeastern U niversity :

N at ural Science , 2005,26(11) :1040-1043. )

[2]

何泽荣, 王绵森, 王峰・一类可再生资源系统的最优动态平衡收获[J]・应用数学与力学, 2004,25(4) :433-440・(He Ze 2rong , Wang Mian 2sen , Wang Feng. Optimal

dynamical balance harvesting for a class of renewable resources system[J].A pplied M athematics and Mechanics , 2004,25(4) :433-440. )

王杰, 陈陈・电力系统中微分代数模型的非线性控制[J]・中国电机工程学报, 2001,21(8) :15-18・

(Wang Jie , Chen Chen. Nonlinear control of differential 2

algebraic model in power systems [J ].

CS EE , 2001,21(8) :15-18. )

Proceedi ngs of the

[3]Hanson F B , Ryan D. Optimal harvesting with both population and

price

dynamics [J ].

M athematical

Biosciences , 1998,148:129-146.

[10]Isidori A. Nonlinear control systems [M ].3rd ed. New Y ork :Springer , 1995:137-218.

[4]Armstrong C W , Skonhoft A. Marine reserves :a bio 2