2008年3月
自动化学报
ACTAAUTOMATICASINICA
V o1.34,No.3
March,2008
SLAM问题中机器人定位误差分析与控制
季秀才郑志强张辉
摘要移动机器人I—J步定位与建问题(Simultaneouslocalizationandmapping,SLAM)是机器人能否布未知环境中实
现完全自主的关键问题之一.其中，机器人定位估计对于保持地的一致性非常重要.文分析J,SLAM问题中机器人定位 误差的收敛特性.分析表明随着机器人的运动，机器人定位误差总体上逐渐增大；完全未知环境中无法预测机器人定位误差
的上限.根据论分析，文提出J，一种控制机器人定位误差单位距离上增长速度的算法.该算法通过搜索获得满足定位误
差限制的最佳的机器人运动速度，从控制机器人定位误差的增长.
关键词SLAM问题，移动机器人，定位差，误差摔制
中图分类号TP242 AnalysisandControlofRObotPositionErrorinSLAM JIXiu—CaiZHENGZhi—QiangZHANGHui AbstractThesimultaneouslocalizationandmapping(SLAM)problemisoneofthekeypro blemsformobilerobotsto becompletelyautonomousinunknownenvironments.Therobotpositionestimationisver yimportanttotheconsistency
ofSLAM.Thispaperpresentsananalysisoftheconvergencepropertiesoftherobotposition error.Itisfoundthatas
awhole,therobotpositionerrorincreaseswiththemovementoftherobot.Basedontheoreti calanalysis,anal
gorithm isdesignedtocontroltheincreasingspeedoftherobotpositionerror.Thisalgorithmsearche sforthebestspeedofthe robotthatsatisfiesthelimitationtotherobotpositionerrorincreasing. KeywordsSLAM,mobilerobots,positionerror,errorcontrol
可靠导航是移动机器人实现白主的先决条件之
一
.
它包括同答:”我在哪里?”,”我要去哪里?”和
“我怎样到达那里?”这二个问题[1--2】.移动机器人
的同步定位和地创建问题fSimultaneouslocal—FFU净化单元
izationandm即building,SLAM)要求机器人住
一
个完全未知的环境中从一个未知的位置出发，利
同时根据己建立的地图同步确定自己的位置[0】从
而全面地同答”我在哪里?”这一问题.SLAM技术
具有重要的理论和应用价值，被很多学者认为是移
动机器人实现真正自主的关键，甚至称其为自主移
动机器人界的”圣杯(Holygrail)”[5】.随着移动机
器人应用环境日趋复杂和非结构化，其已成为急需
解决的重要性基础性难题，日益受到关注.
目前,SLAM问题的解决方法大致分为两类:
基于概率估计的方法和非概率估计的方法.其中基
于概率模型的方法占主导地位[6--10】.SLAM问题
主动防御
中机器人定位和建两者是相辅相生，不断迭代的
过程.机器人由自身的定位信息估计观测到的环境
路标的位置，然后根据环境路标的估计校正机器人
收稿同期2007—06一O8收修改稿F1期2007—09—22 ReceivedJune8,2007;inrevisedformSeptember22,2007
1.国防科技人学机电工程与自动化学院自动控制系长沙410073 1.DepartmentofAutomaticControl,CollegeofMechatron- icsEngineeringandAutomation,NationalUniversityofDelense Technology,Changsha410073
DoI:10.3724/SP.J.1004.2008.00323
定位，二者的估算是高度相关的，任何一方都无法
独立获取.机器人的运动和传感器观测中都存在不
可避免的噪卢，另外估计理论基本上是”估计一校
正”过程，这两点是SLAM方法中普遍应用概率估
计技术的根本原冈.基于概率估计的SLAM方法在
很恰当地表示SLAM问题的同时也揭示了SLAM
问题的一些本质特性.Smith等人f_6,n0]研究了
SLAM中地估计的收敛特性.Mourikis等人[13】
应用黎卡提方程推导出了理想情况卜机器人定位和
建估计误差的一个上界.这些研究表明SLAM问
题有解存在，可以建立环境地.
SLAM问题包括地建立和机器人定位两个方
面.机器人定位精度决定了后续建的精度和一致
性[5】‟所以有必要专门分析机器人定位误差的变化
规律.本文基于Kalman滤波的SLAM算法分析了
SLAM问题中机器人定位误差的收敛特性.根据理
论分析本文提出了一种限制机器人定位误差增长速
度的运动控制算法.该算法假定机器人的运动误差
的大小与其运动速度成正比，通过二分法搜索获
得满足要求的最佳运动速度，从而通过调整机器人
对路标的观测次数控制其定位误差在单位距离上的变化率.仿真实验验证了本文的理论分析和算法.
1SLAM问题及算法
1描述了SLAM问题的基本场景[141：移动
pstl324自动化学报34卷
机器人从一个未知的位置出发，在一个分布着人
造或者自然路标(或称为特征)的未知环境中运动.
机器人根据已知的运动学模型或者里程计信息推
定自己的位置fDeadreckoning)，同时利用自身安
装的传感器测量其附近的路标相对于自己的位置. SLAM技术的目标就是在实现机器人全局定位的同时，根据观测到的环境路标递增地建立机器人的导
航地图，地图中所描述的即为所观测到的路标.本文
沿用文献f51中描述SLAM问题的数学框架和符号. 离散时间k=1,2,…;,(k)表示机器人在k时刻
的定位；表示第i个路标的位置；系统的状态由机
器人的定位和所有路标的位置组成，表示为
植物提取x(k)=(南),p,…,p]T(1)
其中Ⅳ表示路标的个数.
图1SLAM场景示意图【】
Fig.1AscenarioofSLAM[
基于Kalman滤波的SLAM算法采用线性方
程描述系统的运动模型和路标的观测.
1.1系统运动模型
用如下离散线性方程描述机器人的运动模型
(南+1)=(南)(南)+t‟(南)+(南)(2)
其中,(南)为机器人状态转移矩阵,t‟(南)为(k,
k+1]时间段的控制输入向量,(南)为时序无关
的服从零均值协方差矩阵Q(南)的正态随机噪声向量，即(南)一N(O,Q(南)).假定环境中的路标为
静止物体，即
(南+1)=(南)=
型胞苷酸
(+1)
p1
pⅣHFo(k)0●:0
(...+(...+( (4)
如图1所示，根据几何关系可以得到如下的系
统观测模型
(南)=H,x(k)+t(南)=
月pi一月(南)+t(南),
其中，为传感器对于第i个路标的观测输出,(南)
为时序无关的服从零均值协方差矩阵R(南)的正态
随机观测噪声向量，即(南)一N(0,R(南))，皿称
为观测矩阵.
皿=[一风,,0…0,,0…0](7)
1.3基于Kalman滤波的SLAM算法
基于Kalman滤波的SLAM算法应用Kalman
滤波估计机器人和路标的位置状态，分为以下三个
循环递推的过程:
1)预测.首先根据k时刻系统状态x(k)的估
计值金(南lk)和控制输入t‟(南)，利用系统的运动模型,
预测系统在k+1时刻的状态金(南+1lk)，第i个路
标相对于机器人的观测i(南+1l)和状态估计的协
方差矩阵
金(南+llk)=F(k)~(klk)+t‟(南)
2t(南+llk)=金(南+llk)(8)
P(k+llk)=F(k)P(klk)FT(七)+Q(k)
2)观测.接下来，机器人获得了第i个路标的
实际观测(南+1)，其满足式(6).结合(南+llk),
获得由观测所得的新息,
(南+1)=(南+1)一(南+llk)(9)
荸荠去皮机
…
以及该新息的协方差矩阵
IU,
.
(南+1)=凰(南)P(南+llk)日(南)+R(南+1)
综合式(1)一(3)，可用如下方程描述系统的运动模(10)
×
,l●●●●●●/
00
一一..一
3期季秀才等:SLAM问题中机器人定位误差分析与控制325 31更新.根据预测和观测，应如卜公式更新
系统在k+1时刻的状态估计及相应的协方差矩阵
金(+1lk+1)：金(+1l)+(k+1)vi(k+1)(11)
P(k+1lk+1):P(k+1l)一,
(+1)S(k+1)r(+1),
其中,Wi(k+1)=P(k+ilk)月(+1)称为
增益矩阵.
基丁Kalman滤波的SLAM算法随着机器人
的不断运动循环执行上述二个过程，递推估计机器
人和路标的位置，从而实现了机器人的白定位递
增建立环境地.上述三个步骤中变量的协方差矩
阵体现了算法状态估计的好坏，所以协方差矩阵的
计算非常重要.本文通过分析协方差矩阵随时间变
化的情况研究机器人定位估计的收敛特性.
2机器人定位误差的收敛性分析
2.1预备知识
设矩阵,B∈R均为()正定的，如果
V≠0,∈R,T>TBx，则称人于等
于B，记为B.显然(半)正定矩阵火于零矩阵.
正定矩阵的卜述性质对于本文的后续分析非常重要. 性质1.如果>B，则一<B一.
性质2.如果B,!J!IJdetAdetB，仅当
A=B时，等号成立.
性质3.设矩阵,B∈R正定，且B,
如果矩阵C∈R满秩，则CCT>CBCT.反
之，若,B∈R正定,C∈R满秩，如果
CACTCBCT，贝UAB.
性质4.如果,B∈R为半正定矩阵，则
存在可逆矩阵C∈R，使得—CH】CT,B
=
C，其中，皿=diag(a1,…,an),H2=
diag(b1,…,bn);ai,b0(i=1,2,…钆).
引理1.当机器人某一时刻观测到多个路标时,
无论是将所有的观测看作一个整体进行状态更颗, 还是根据各个路标观测逐一更新，状态估计的结果
相同，而且与更新顺序无关.
冈为任意时刻路标的观测相互独立，根据联邦
滤波器原理[15】很容易证明引理1的结论.
引理2.机器人定位估计在k+1时刻的更新仅
与k时刻的定位估计和k+1时刻观测到的路标有关，而与+1时刻未观测到的路标无关.
文献[7]给出了相似结论的证明.为了分析机器
人定位误差的收敛特性，根据引理2，本文只考虑当
前时刻机器人观测到的路标.
阵中的H啦=I和H只=I.取
c—Hm1:
(13)
满足上述假设的SLAM问题称为线性一高斯SLAMfLG—SLAM)[引.虽然LG—SLAM比较简单，但是由于基于Kalman滤波的SLAM算法可以
获得该问题的最小方差无偏估计，所以对该问题收
敛特性的研究能够使我们了解一般的SLAM问题
的收敛性质.
系统状态的协方差矩阵可以表示为分块矩阵形
式
P二
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议