第4卷第4期 智能系统学报 Vol.46.4 2009年8月 CAAI Transactions on Intelligent Systems Ag.2009 doi:10.3969/j.issn.16734785.2009.04.003 面向室内移动机器人的无迹滤波实时导航方法 霍成立，谢凡，秦世引 (北京航空航天大学自动化科学与电气工程学院，北京100191) 摘要：根据轮式移动机器人室内自主漫游的目标要求和约束条件，提出了一种新的实时导航策略，利用UKF(u scented Kalman filter)算法对置顶相机和里程计获取的移动机器人位姿信息进行在线滤波，为实现移动机器人的室内 自主漫游控制进行实时导航定位.实验结果表明，提出的导航策略和实现算法满足实时性要求，具有较高的精度，可 为进一步的实际应用提供参考。 关键词：室内自主漫游；无迹卡尔曼滤波；实时导航；移动机器人 中图分类号：TP242.6文献标识码：A文章编号：16734785(2009)04029508 A case study in real-time UKF-based navigation for indoor autonomous travel of mobile robots HUO Cheng-li,XIE Fan,QIN Shi-yin (School of Automation Science and Electrical Engineering,BeiHang University,Beijing 100083,China)) Abstract:A new real-time navigation strategy was proposed that reflects the requirements and constraints for indoor autonomous travel of wheeled mobile robots.The unscented Kalman filter(UKF)algorithm was employed to carry out on-line filtering of mobile robot positional data collected from ceiling cameras and the odometer.This allows re- al-time navigation and positioning for control of indoor autonomous travel.A series of experiments demonstrated that this navigational strategy and implementation algorithm can meet real-time high precision requirements.These re- sults make our strategy a good reference for further research that will lead to practical applications. Keywords:indoor autonomous cruise;UKF;real-time navigation;mobile robots 随着人类探索领域的深入，移动机器人在月球以及避障、避碰；但是一般的导航技术往往存在实时 漫游、行星探测2]、地质勘探、地图构建341或其他 性不高、容易产生延时、数据丢失等现象，导致任务 危险环境作业5]等方面有着广泛的应用.在导航定 的失败，因此设计一种高实时性的导航策略和方法 位方面，常规的视觉导航、无线电导航和地磁导航等 具有十分重要的意义. 技术可在各种应用背景下发挥其重要作用.现阶段， 目前，实时导航方法主要从算法和实现技术两 常用的比较有效的导航方法主要有势场法[6]、栅格 方面对传统导航方法进行改进，在导航算法方面，不 法)和视觉导航法89]，其中势场法存在着在狭窄 损失精度的前提下，采用适合于实时处理的递推导 通道中摆动的缺陷，栅格法存在环境分辨率与环境 航计算方法.在实现技术方面，利用高速传感器以及 信息存储量大的矛盾.对于视觉导航方法而言，大多 通讯网络，在获得足够信息的同时提高数据的处理 数导航算法的复杂度较高，需要的地面环境信息也 速度以达到实时性要求. 比较多，因而实时性效果也会受到影响.在移动机器 本文针对一类移动机器人及其室内自主漫游环 人自主漫游任务中，通常要求机器人具有实时获取 境，以追求实时性和准确性为导向，考虑到机器人运 周围环境信息的能力，从而实现对控制策略的切换 动模型的非线性特性，采用以无迹变换uo](unscent~. 收稿日期：200904-13. ed transformation,UT)为基础的无迹卡尔曼滤波 基金项目：国际科技合作基金资助项目(2007DFA11530):国家高技 术研究发展计划(863)基金资助项目(2006AA04Z207);国 UKF31方法，研究新型实用的实时导航算法和相 家自然科学基金资助项目(60875072)：教育部博士点基金 应的实现技术，和传统的扩展卡尔曼滤波EKF4] 资助项目(20060006018). 通信作者：蛋成立，E-mail:hchlsy@sina.com. (extented Kalman filter)相比较，UKF具有鲁棒性和
296 智能系统学报 第4卷 实时性好，便于处理非线性系统模型问题，计算精度 式中：W为系统噪声，它服从均值为0，方差为Q的 可达到三阶泰勒级数展开，特别适合于解决高阶非 正态分布.设其控制周期为△T,可以将式(1)改写 线性问题. 成离散形式： 实验结果验证了本文所提算法与实现方案的可 行性和有效性.考虑到各种移动机器人应用项目在 x(k+1) -x(k) 进行前期实验验证时以室内环境为主，本文将重点 y(+1) y() 研究一类移动机器人的室内自主漫游实时导航算法 a(k+1) -a(k) 及其实现技术， v(k)·co8a(k)1 w1(k) v(k)·sina(k) ·△T+ 1移动机器人运动模型和环境分析 02(k) (2) w(k) 03(k) 1.1移动机器人的运动模型 式中：0：(i=1,2,3)为W向量的第i个分量.利用全 轮式移动机器人(wheel mobile robot,WMR)如 局摄像机以及机器人本身的里程计作为测量传感 图1所示，机械部分主要由2个驱动轮和一个随动 器，机器人的状态量，即机器人的位置坐标以及机器 轮组成.2个驱动轮分别由2部电机驱动，如果2个 人的运动方向可以直接被测量得到，因此可以得到 电机输出的转速不同，就会使得左右两轮产生“差 系统的测量方程为 速”，实现转向运动. rx(k) Z(k)=H·X(k)+V y(k V (3) a(k) Γ100 式中：量测矩阵H=010；V=[V1V2V] L001 为系统量测噪声，它服从均值为0，方差为R的正态 分布. 图1轮式移动机器人 Fig.1 Wheel mobile robot 由于轮式移动机器人是典型的非完整动力学系 统，它的控制要求轮子与地面间的滚动接触必须满 足纯滚动、无打滑这一非完整约束的条件，因此存在 非完整约束.且随动轮仅在运动中起到支撑的作用， 因此可以忽略它对运动的影响，在该假设下，WMR 的运动模型可由图2表示.其中v和ω分别表示机 器人的平移线速度和旋转角速度，)的方向表示机 图2轮式移动机器人运动模型 器人的运动方向，，与U,分别是WMR左轮和右轮 Fig.2 Wheel mobile robot motion model 的线速度，α是机器人前进方向与世界坐标系X轴 1.2室内漫游环境 正方向的夹角，即机器人运动的航向角.这样WMR 本文所提的导航方法及其实现技术基于室内水 的状态就可以由其2个驱动轮的轴中点C在世界 平地面，能够保证机器人做纯滚动运动，另外为了保 坐标系下的位置(x,y,)及其航向a来表示，令X= 证系统的可实现性，选用光照情况良好的条件进行 (x,y,a)表示机器人状态，而u=(vw)T作为运动 拍照，通过利用室内安装置顶像机实时地获取地面 学模型中的控制输入 移动机器人的运动学方程就可以表示为 环境及轮式移动机器人的全局信息，为机器人的导 航定位提供信息.所采用的置顶相机如图3所示，相 cos a 0- sin a 0 u W (1) 机的视场范围如图4所示， 0
第4期 霍成立，等：面向室内移动机器人的无迹滤波实时导航方法 .297. 息和上一步的预测进行机器人的位姿状态估计，其 中通过对置顶相机获取的各帧图像的实时处理来获 取机器人的当前位置坐标(x,y),根据机器人的初 始位姿方向及里程计传感器，得到机器人的角速度 0与计算机器人的当前方向角. 其中每次获得导航数据主要的时间开销，在于 位姿测量过程，其耗时主要来自相机获取图像和图 图3置顶相机 像处理的时间，包括针对获取图像的预处理，以及对 Fig.3 Ceiling camera 目标的分割提取和定位计算所耗费的时间.因此以 上的导航策略与每次导航定位过程需要在保证计算 精度的同时，在时间花费上达到最低，从而确保满足 实时性要求. 为了满足室内移动机器人自主漫游控制的实时 性要求，提高整个导航处理系统的运行效率，需要从 系统的硬件配置和软件算法上进行优化设计，同时， 利用高速的无线局域网保证系统信息传递的顺畅与 图4相机的视场范围 快速 Fig.4 Vision scope of the camera 在硬件配置上，主要由3部分组成，分别为视觉 传感器、中央处理计算机以及无线局域网络.首先从 2移动机器人室内自主漫游的实时导 视觉传感器上获取的图像信息需要实时地传递到中 航策略 央处理计算机，由于传递的图像信息数据量比较大， 如图5所示，为了实现室内机器人自主漫游，利 如果用无线网络会产生较大延时：因此，本方案中视 用基于PD的控制策略来控制机器人的速度v和角 觉传感器和计算机之间采用了数据线进行直接连 速度。，要求机器人按照预定的轨迹运动.针对机器 接，大大地加快了数据传输的速度，减小了延时.另 人自主漫游控制制定了基于UKF的实时导航定位 外，由于移动机器人和中央处理计算机之间必须通 策略，其基本思想是利用机器人的当前位姿测量信 过无线方式进行通讯，因此本方案采用802.11协议 支持的实时无线局域网络保证数据传输.该无线网 UKF递推过程 络具有极高的传输速度，机器人通过网络上传里程 机器人 状 测 计 计的测量数据，同时中央处理计算机将控制指令发 态预 位 署 xk+1)） 送给机器人，从而从硬件配置上保证了系统的实时 测 v(k+1 a(k+1) 运行. 在算法上，本方案采用了UKF递推算法，在不 a位姿测金过程x 损失精度的基础上，使得每步导航递推的时间开销 里程计 置项相机 降到最低，另外，针对图像处理、目标提取、定位计算 及UKF递推算法的软件实现，都采用VC++在 Windows操作平台上实现，通过不断地改进算法、优 当前位置 化程序、提高运算速度，从而以较短的时间计算得出 机器人的估计位姿 移动机器人 自主漫游控制集 3基于UKF滤波实时导航算法及其 实现技术 图5机器人漫游的实时导航策略 Fig.5 The real-time navigation strategy of robot cruise 3.1无迹卡尔曼滤波器原理 UKF方法是以无迹变换为基础的滤波方法，所
298. 智能系统学报 第4卷 谓无迹变换是基于这样的思想，即用固定数量的参 测量方程的传播后，通过加权求和产生预测值的均 数去近似一个高斯分布比近似任意的非线性函数更 值和方差. 容易.其实现原理是在原先状态分布中按某一规则 3.2UKF的递推算法 取一些点，使这些点的均值和协方差等于原状态分 针对实验的环境和具体情况，系统噪声和观测 布的均值和协方差；将这些点代入非线性函数中， 噪声为加性噪声的特点，本文应用的是简化的UKF 相应得到非线性函数值点集，通过这些点集求取变 递推算法.该算法不需要将噪声向量扩展为状态向 换后的均值和协方差.由于这样得到的函数值没有 量，降低了状态维数，减小了UKF的计算量. 经过线性化、没有忽略其高阶项，因而由此得到的均 UKF递推算法步骤为 值和协方差的估计比EKF方法的要精确， 1)初始化状态变量及估计方差 方差 。=E[X], Po=E[(X-)(X-)]. 均值 ⊙ 2)计算状态的Sigma点 4,2=0; y=孔) X4=1+(√n+P),i=1,,店 P=APA y=fx) A'PA t 4+(√n+)P):,i=n+1,…2 式中：-1为状态变量均值附近的第i个采样点；n 为状态变量的维数；（√(n+入)Pk-1)为矩阵 实际均值 √(n+A)Pk-i的第i列；系数入=a2(n+K)-n,a 和K为待选系数. 实际方差 对高斯分布的情况，当状态变量为标量时，选 (a)线性变换 择K=2;状态变量为向量时，选择K=3-n.选择 0≤α≤1，x的作用是控制采样点的分布状态，调节 6 α以使高阶项的影响达到最小.适当调节α、K,可以 提高估计均值的精度， Sigma点g ⊙ 3)UKF的时间更新方程如式(4)： Xk-l=f代Xk-i). (4) 其中X-1=[x(k-1)y(k-1)a(k-1)]T式 =f几x) (4)为UKF的状态方程，将移动机器人离散化后的 厂加权采秤7 ⊙均值、方！ 运动方程式(2)，带入式(4)得 Γx(k/k-1)7 Xwk-1= y(/k-1) =八X-1）= a(k/k-1) Ut仿差 Γx(k-1)7 v(k-1)cos a(k-1)7 U均值⊙ y(k-1)+ (k-1)sin a(k-1)△T, b)无迹变换 α(k-1)J o(k-1) 图6线性变化与无迹变换比较 (5) Fig.6 Comparison of linearized transformation and un- -l=∑WXi, scented transformation 2n 其变换过程如图6所示，图6(a)为EKF滤波 P-l=∑W(X-l--i）· 被估计量经过线性化后，经过状态方程和测量方程 (Xk-l-k-i)T+2a-1… (6) 的传播后，得到预测值的均值和方差，图6(b)表示 式中：？为系统噪声协方差矩阵，该噪声主要来自对 了无迹变换，其过程是：由被估计量的均值和方差产 移动机器人系统建模的误差以及未知环境中的不确 生一批离散的采样点，这批采样点经过状态方程和 定干扰
第4期 霍成立，等：面向室内移动机器人的无迹滤波实时导航方法 ·299· Zk-l=H·Xk-l (7) 检测，而实时导航计算机上运行实时导航程序，实现 为量测方程，其中 UKF算法的递推和机器人位姿的估计，并同步显示 H=diag(11 1), 移动机器人的实时轨迹。 X-1=[x(k/k-1)y(k/k-1)a(/k-1)]T, 置项 机器人位置信息 结合机器人观测方程(3)，可得 Zv-I HX-I= 图 [x(/作-1)y(k/k-1)&(k/k-1)]T, 像 2n ukl=∑WrZa- (8) 否位姿 4)UKF的测量更新方程如下 ( 移动机器人 移动机器 P.s=含w(Xi-Z-2n', 图7基于UKF的移动机器人实时导航实现方案 (9) Fig.7 Realization scheme of real-time navigation program P=宫rZ-2z-2+风 of mobile robot based on UKF 具体的实现步骤如下： (10) 1)首先设置移动机器人的初始位置X(xo,y%, 式中R为观测噪声协方差矩阵，由测量传感器造成 ),并在机器人行进过程中通过无线网络发送控 的误差，可以通过进行多次实验确定其值 制指令，随时改变其速度v和角速度ω，从而使得机 根据式(9)和(10)可以得出卡尔曼滤波的增益 器人按照设定的轨迹实现在室内的自主漫游。 矩阵： 2)移动机器人在室内开始漫游时，屋顶的广角相 Ki=PeaPE (11) 机开始采集图像，每隔0.18采集一帧，通过相机的视 根据式(6)、（10)和(11)得出滤波估计协方差 频线传递到实时导航计算机的图像采集卡上，并计算 矩阵Pk: 出图像中机器人的位置形心，即机器人的位置 P&Pvk-I -KiPaKi (12) 3)从安装在移动机器人内的里程计上实时地 最后由滤波状态预测方程(13)得出机器人的估计 读取运动的速度和角速度，利用机器人自身上带有 位置： 的无线网卡，通过室内无线网络传输到实时导航计 =-1+K(Z&-2-1）. (13) 算机上，结合2)中由摄像机获得的机器人位置信 式中：W和W均为加权系数，表达式由下式决定： 息，可以获得移动机器人的完整位姿测量信息， 欧=n在入 4)在实时导航计算上，实现UKF递推算法，通 过计算机内部计时器动态获得递推时间间隔，使得 入 听=2n+灯i=1,…2m, 递推算法更加精确.利用2)、3)共同得到的移动机 器人位姿测量信息，以及本文提出的UKF实时导航 1 盼=n+A+(1-a+), 算法，计算得出机器人的估计位姿(xk,y,a). 针对本文提出的基于UKF的移动机器人实时 1 W=2n+2=1,…,2n 导航实现技术，通过对每次测量定位、导航递推所需 待选参数B≥0，是一个非负的系数项，调节B 时间的统计，对该方案的实时性进行了定量分析.独 可提高方差精度 立地进行10次导航实验，计算每次实验中导航过程 3.3基于UKF的移动机器人实时导航实现技术 所进行递推计算的次数和总耗时，进而得到每次获 该文提出的基于UKF的移动机器人实时导航 得导航数据所需平均耗时时间，如表1所示.从表中 技术实现方案如图7所示，本方案的实现设备主要 数据可知，本方案中每次得到导航数据的平均耗时 由屋顶全局摄像头、实时导航计算机以及移动机器 为87.39ms,因此在1s中内可以获取导航数据13 人组成.其中摄像机和安装在移动机器人内部的里 次左右，完全可以满足室内移动机器人自主漫游控 程计作为主要的传感器，对机器人的位姿进行实时 制的实时要求