（通信与信息系统专业论文）基于扩展卡尔曼滤波的同时定位与地图构建算法研究.pdf

基于扩展卡尔照滤波的同时定位与地图构建算法研究 摘要 移囊壤器入麴骚究涉及到饕笈控耧接本、诗算糖技零、模式谈嬲以及久工餐裁等诲 多学科，这些学科的飞速发展为移动机器人的研究带柬新的动力。随着激光测躐仪和其 它先进传感器设备在移动机器人研究中的广泛应用，移动机器人的研究越来越受到国内 钤学者的簧遮獯褪，取得了缀多豢兹成巢。 自主定位和地图构建怒移动机器人研究中的两个核心问题，是移动机器人实现自主 导航的基础。移动机器人的工作环境可分为室外环境朔室内环境两种，本文主要针对移 动壤器入在室蠹矮境下熬您辩定蕴与缝辫糖建阕题遴褥璎变。文章蘩先豢是了瓤器人同 时定位与地图构建研究中存在的问题，包括制图复杂度、数据匹配难度以及定位制图关 联度等，分析了常用的同时定位与地图构建方法。在此基础上引出了本研究的羹点，基 予专家曼滤波黪移动辊嚣入强嚣定位与缝圈稳建( s l a m ) 算法，势瓣其算法椽絮、专尔 受滤波定位等相关内容进行了介绍。本义提出了一种慕于线特征的e k f s l a m 算法并 对篡进行了详细的阐述，给出了包括机器人运动模型、观测模型的建立、数据殴配、状 惫更薮、逮嚣建立、缝鎏綮壤等方瑟翦穗荚公式，最戆黻基于e k f 懿s l a m 秀寨寒解 决移动机器入嗣时定位与地潮构建，并进行了仿真试验和进一步豹深入讨论。 荚键溺：弱辩霆经与缝霆橡建扩展专承受滤渡舀烹移韵撬器入 r e s e a r c ho ne k f b a s e dsii b mit a n e o u sl o c aiiz a tio na n dm a p pin g a i g o ti t h m a b s t r a c t t h er e s e a r c ho fm o b i l er o b o ti n v o l v e sal o to fk n o w l e d g es u c ha si n t e l l i g e n tc o n t r o l ， c o m p u t e rs c i e n c e ，p a t t e r nr e c o g n i t i o na n da r t i f i c i a li n t e l l i g e n c e w i t ht h ef a s td e v e l o p m e n to f n e w t e c h n o l o g ya n dt h ew i d eu s eo fm v a n c e ds e n s o r s ，m o b i l er o b o th a sb e c o m et h ef o c u si n t h ef i e l do f r o b o t i c sa n da u t o m a t i o n m 印b u i l d i n ga n dl o c a l i z a t i o na l et w oe s s e n t i a lt a s k sf o ra na u t o n o m o u sm o b i l er o b o t s n a v i g a t i o n t h e ya r ei m p o r t a n tf o u n d a t i o n st or e a l i z em o b i l er o b o tn a v i g a t i o ni n d e p e n d e n t l y a c c o r d i n gt ow o r k i n ge n v i r o n m e n t i tc a nb ec l a s s i f i e di n t oi n d o o rm o b i l er o b o ta n do u t d o o r m o b i l er o b o t t h i sp a p e ra i m st os t u d ys i m u l t a n e o u sl o c a l i z a t i o na n dm a p p i n g ( s l a m ) a l g o r i t h mi ni n d o o re n v i r o n m e n t t h es i m u l t a n e o u sl o c a l i z a t i o na n dm a p p i n g ( s l a m ) p r o b l e mi sf i r s t l yi n 删u c e d ，b a s e do nt h ea n a l y s i so ft h el o c a l i z a t i o np r o b l e ma n dt h em a p b u i l d i n gp r o b l e mw h i c ha r et w ok e yp o i n t si nt h en a v i g a t i o nt e c h n i q u e s ，i n c l u d i n gi t s s t r u c t u r e ，c h a r a c t e r i s t i c s ，c a t e g o r i e sa n ds oo n i ta n a l y z e ss e v e r a lc o m m o n l yu s e dm e t h o d so f t h es i m u l t a n e o u sl o c a l i z a t i o na n dm a p p i n g ( s l a m ) p r o b l e m t h e k e yp o i n tr e s e a r c ho ft h e p a p e ri ss i m u l t a n e o u sl o c a l i z a t i o na n dm a p p i n ga l g o r i t h mb a s e do ne x t e n t d e dk a l m a nf i l t e r a n dal i n e f e a t u r eb a s e ds l a ma l g o r i t h mi sw e l lp r e s e n t e di nt h i sp a p e r a l lo p e r a t i o n s r e q u i r e df o rb u i l d i n ga n dm a i n t a i n i n gt h i sm a p ，s u c ha sm o d e l s e t t i n g ，d a t aa s s o c i a t i o n ，a n d s t a t e - u p d a t i n ga r ed e s c r i b e da n df o r m u l a t e d f i n a l l y , t h ea p p r o a c ho fs l a mb a s e do ne k fm e t h o d h a sb e e np r o g r a m m e da n ds u c c e s s f u l l yt e s t e di nt h es i m u l a t i o nw o r k a n df u r t h e re x p e r i m e n ta n a l y s i ss h o w t h es i m u l t a n e o u sl o c a l i z a t i o na n dm a p p i n g a l g o r i t h m sc o n v e r g e n c e ，r o b u s t n o s sa n dc o n s i s t e n c e k e yw o r d s ：s i m u i t a n e o u sl o c a iiz a t i o na n dm a p p i n g ：e x t e n t d e dk a i m a nf ii t e r a u t o n o m o u sm o biier o b o t 独创声明 本人声明所呈交的学位论文是本人在导师指导下进行的研究工作及取得的研究成果。据 我所知，除了文中特别加以标注和致谢的地方外。论文中不包含其他人已经发表或撰写过的 研究成果也不包含未获得 l 挂；皇旦丛直基丝噩噩挂剔直明笪： 奎拦丑窒2 或其他教育机构的学位或证书使用过的材料。与我一同工作的同志对本研究所做 的任何贡献均已在论文中作了明确的说明并表示谢意。 学位论文作者签名。孑2 也荔 签字日期：砷年i 月日 学位论文版权使用授权书 本学位论文作者完全了解学校有关保留、使用学位论文的规定，有权保留并向国家有关 部门或机构送交论文的复印件和磁盘，允许论文被查阅和借阅。本人授权学校可以将学位论 文的全部或部分内容编入有关数据库进行检索，可以采用影印、缩印或扫描等复制手段保存、 汇编学位论文。( 保密的学位论文在解密后适用本授权书) 学位论文作者签名：柱娩历 导师签字： 0 时，圆弧模型方程可表述： f ( x k l k u 。) = + 等( s i n ( 幺+ 蚓“n 刚 几一等( c o s ( 岛+ 吲一引 见+ 幺 i a e , l o ( 2 - 7 ) 结合图2 一l ，我们给出该模型的基本推导如下，作角皖的角分线，其与弧峨对应的 弦垂直相交并将其平分。由图2 - 1 所示几何关系可得机器人在x 方向和y 方向的位移增 量可表示为： 舰- 2 r k s i n - 竽c o s 一+ 等) 仁s ， 觇= 2 r k s i n - 竽幽( 酢+ 等 协9 ， 经过三角公式转化并带入公式( 2 - 4 ) 就可推出公式( 2 7 ) 。 ( 2 ) 直线模型 直线模型因假设机器人在极短的时间内的运动可用直线来表示，u pl a e 。l - - o ，所以 直线模犁的方程可表述为 | 皖卜o ( 2 1 0 ) ，_-_rri 皖皖 兮m 峨峨岛 + + 坼此 r。，l i | 甜x 厂 毕j ：静聪卡尔蹙滤波的时斑位j 地| 芏i 构建辫i 上4 , t t 4 ( 该梭测报导简攀，其实际是圆弧模型的箍化影式。 零论文采用蠹线模型对举弦计数据迸荐推算，在梳器入平台隳带的软俘中所采瑙的 是改进的直线晕稷计模型。即以盥线模型为搬，同时在位骚方向角差慎的推算中使用弧 线摸裂，具体表述如下： | 墨+ z u 9 k e o s s , l 厂( 以) = l 几+ a d ks i n 见i ( 2 - 1 1 ) l最+ 只 j 2 2 激光测距仪模型 激光测距仪是一耪基于t o f m eo f f l i g h t ) 原理豹离糖度、高姆辑魔酌终都传感嚣。 l 冀德黼s i c k 公司l m s 2 0 0 为例( 如图2 2 所霹) ，具有扫描快速( 扫描周期1 3 3 m s ) 、数 据点密集和测量精度高的优点。为了减小反射光强度不均带来的不良影响，l m s 采用 二氆 化豹方法对反射走强度送纷处理，对环境光线不敏感，主要受簿磁物捞糕及其袭戆 光滑魔的影响较大。l m s 系统怒一种非接触式主动颡距系统，不需要颥设发射器和定 位标虑，其工作原理是基于对激光束飞行时间的测量( 如圈2 2 所示) 。 蓑转茨霪孛镬 图2 - 2l m s 2 0 0 型激光测距仪及工作原理 脉_ 中红终激光束被发射出去后如遇到物体则被反射嬲米，并被测躐仪的接收器记 录。激光踩冲麸发瓣弱接收的时闽与溺距仪和被测量耪落潮懿距离成形院。辣冲激光寐 被测距仪内部的个旋转光镜所偏转以便对周围环境形成个扇面扫描区域。目标物体 的轮辩线从所接收到的一系列脉冲序列束确窳。通过高速串季亍接n ( 8 0 0 k b i t s r s 4 2 2 ) 可 实时地将测量数据传送给机器人的主机进行遴一步的数掘她理。 1 5 皋十争 f 芝卡尔曼滤波的付定位j 地幽构址算法f i j 究 如图2 3 所示，实验室中移动机器人所配备的单个s i c kl m s2 0 0 型激光测距仪扫 描区域为1 8 0 。，有效测距8 m 。距离和角度分辨率分别为1 0 m m 和0 5 。1 。( 相对扫描数据 点为3 6 1 或1 8 1 个) 。扫描数据可用极坐标表示为： = ( 以，纯) ，竹= l ，n ( 2 1 2 ) 或者由直角坐标表示为： = ( _ ，以) 1 ，胛= l io ，n ( 2 1 3 ) 其中= 以c o s # ，只= 以s i n e , ，n 为扫描数据点个数。 幽2 - 3s i c kl m s 2 0 0 型激光测距仪扫描特性示意圈 本文假设测量值( 以，蛾) 受噪声干扰，它们分别服从高斯白噪声分布且互相独立，方 差分别为刃和刃时，具体取值见本文2 3 2 节对激光测距仪误差分析的讨论。由误差传 递公式可知“的协方差矩阵为： q = 匮蓦 _ 薏刃( 薏卜两o un 2 ( 两a u c ：“， q = 毕 警2 - 伽s i n 钏2 # + 手 簖 2 3 不确定性信息描述与误差分析 ( 2 1 5 ) 通过数学对现实环境进行抽象描述总是存在这样的悖论：只要数学定律涉及到对现 实的描述，它们就是不确定的；只要它们是确定的，其并不是针对现实的描述。爱因斯 坦的这句名言表明数学和现实之洲的交互的确是非常困难的。当所有前提条件都被满足 6 1j 织 2 n n 针酊2 堆1 和城卡匀：坚滤波的m 时定位。j 地脚构吐箅泣州究 的情况下，数学定律能给出精确的结果和精确定义的数据。然而现实总是难以测量，即 便能测量，也无法获得完美的数掘来对其进行复原。这就是为什么我们在研究现实问题 时要引入不确定性模型并应用于测量过程中。通过数学定律引入的不确定模型能定量的 对结果进行估计。在完全未知环境中，机器人依靠其自身携带传感器所获取的信息来建 立环境模型是移动机器人进行自主定位和导航的前提之一。所谓完全未知环境是指自主 移动机器人对环境一无所知，不存在任何先验信息，如环境形状、障碍物位置、人为设 定的参照物或人工路标等。在这种环境下，自主移动机器人必须依赖里程计、激光测距 仪等传感器来获取周围环境信息。由于传感器自身的限制，感知信息存在不同程度的不 确定性，而感知信息的不确定性必然导致所构建的环境模型也不可能是完全精确的。同 样，当依据带有不确定性的模型和感知进行决策时也必然带有不确定性，即不确定性具 有传递性。 2 3 1 里程计的局限及误差传递 里程计测距法是机器人导航中最为广泛使用的确定机器人位姿的航位推算，其价格 便宜，具有很好的短时准确性和非常高的采样速率。里程计的基本原理是对一段时间内 的运动增量信息进行集成，这也就不可避免的导致累计误差，而且误差与机器人运动距 离成比例地增加。尽管有这些局限，研究人员还是达成里程计是机器人导航系统重要组 成部分的广泛共识。里程计精度的提高将极大地化简导航任务量，而且在一些特定情况 下，里程计是机器人唯一可获取的导航信息。里程计的模型是基于车轮的旋转可以被转 化为相对对面的线性位移这一前提假设，因此其具有一定的局限性。所有误差源都可以 被分为系统误差( s y s t e m a t i ce r r o r s ) 和非系统误差( n o n s y s t e m a t i ce l t o r $ ) 两类。系统误差对 里程计误差的积累是恒定的，而非系统误差的发生一般是不可预测的，其对里程计的影 响也是随机变化的。系统误差主要包括：两驱动车轮半径的差异、车轮半径的平均值与 标称值的差异、轮间距与标称值的差异，车轮安装位罨的偏差、有限的编码器解析度和 采样频率。非系统误差主要包括：运行的路面不平整：运行中经过意外物体由于地面的 油污或者加速过度造成的车轮打滑：外部受力( 如与外部物体的碰撞) 和内部受力( 如从动 脚轮的影响) 。在一般光滑的室内地面上运行的移动机器人，系统误差对里程计的影响 较大；而在不规则的粗糙地面上，则以非系统误差对旱程计的影响为主。在我们的研究 中，机器人运行环境的地面不够平滑，所以对里程计的误差影响以非系统误差为主。我 皋于扣胜卡尔基游波1 1 勺f 九j 时定位i 地幽构矬算法圳究 们此处忽略系统误差的影响而只对非系统误差进行讨论。 里程计的非系统误差主要发生在关节空间和笛卡儿空间。差动传动的二维关节空白j 包括左右两个驱动轮，其误差包括由于车轮打滑、地面不平等影响因素：笛卡儿空间的 误差主要是由机器人受外力的作用( 如意外碰撞) 而引起的位姿被迫改变，这种误差一般 是不可预测且与诸多不确定因素有关，本文只用常值对角矩阵幺+ 表示笛卡儿空白j 的非 系统误差协方差矩阵。根据文献 4 1 n - i 知，关节空间的非系统误差主要来源于不确定输 入“。= 【a d 。，以1 7 ，其中以和以为左轮和右轮的运行距离。假设罩程计模型的输入 误差的对角协方差矩阵为巩+ l ，且误差输入与以和哝成正比，比例系数为t 和k ， 则： 。= 如i | 岱 由公式可知旱程计模型为非线性模型，根据罩程计输入对机器人状态x = ( x ，只卵7 的估计为k + * = 厂( 以p ，+ ，) 出误差传递公式可知，机器人在t + ，p 位姿下的误差协方差 矩阵更新方程为： p t 哪t = 飞f 尹t | 孓f ：七飞f pk 橐矗+ q t 。 q 一17 ) 其中只扯为上次的机器人位姿误差协方差矩阵，厂( ) 为里程计模型方程，。分别为其 对机器人位姿以k 和输入向量嘶+ 的雅可比矩阵。 幽2 - 4 机器人住相同环境中通过里稃计定位( 左) 雨l 通过独立经纬仪定位效果对照幽 缺- i 二扣艘卡尔曼滤波的时定位o j 地i 纠构让并法州究 文献【4 2 】对机器人只通过罩程计在室内环境进行导航过程中由于累计误差过大而造 成定位失败的情况进行了分析。如图2 - 4 所示，左右两图为机器人在相同环境中通过单 程计定位结果和通过与机器人及其传感器独立的精确定位工具经纬仪进行定位的结果。 后者可近似作为机器人导航过程中的精确位姿，相比较前者随着机器人移动距离和航向 角的增加，其累积误差越来越大。这种误差只能依靠激光测距仪等外部传感器进行校币。 2 3 2 激光测距仪误差分析 在文献【4 3 】中，c a n g y e 和j o h a n nb o r e n s t e i n 对s i c k 公司l m s 2 0 0 型激光测距仪的 特点和性能参数进行了详尽的介绍。虽然在此文献所做工作之前，很多基于2 d 激光扫 描测距的文章都简单的假设误差服从正态分布，但都缺乏实验证明。出于我们所使用的 激光测距仪同样为s i c k 公司l m s 2 0 0 ，所以我们主要依据上述文献内容对激光测距仪 的误差进行分析讨论。 l m s2 0 0 测距仪的性能主要受数据传输速率、漂移、物体表面的光学特性、激光光 “束入射角以及混和像素现象的影响，下面就逐一地进行讨论。 ( 1 ) 数据传输速率影响 对于快速移动的机器人，使用激光测距仪所能提供的最高传输速率5 0 0 k b p s 是十分 必要的，这需要高速r s 4 2 2 接口卡的支持。在角度解析度为r 的情况下，一次完整扫描 的数据包为3 7 2 字节长度，传送数据包的时间丌销为7 4 m s ，而一次完整扫描过程的时 间为1 3 3 m s 。实验表明，在5 0 0 k b p s 情况下会发生偶尔丢包的情况，但丢包率不超过 o 0 8 。 ( 2 ) 漂移影响 在距离激光测距仪2 m 外进行6 0 0 ，0 0 0 次完整的扫描，每1 0 0 0 次扫描为一组取一次 评价值，通过实验可以发现前1 0 0 0 0 秒的偏差特别大，所以为减少漂移的影响，文献 4 3 】 中建议进行额外的近3 小时的测距仪预热，但这在我们的实验中是不可行的，所以测距 的漂移误差必须考虑。 ( 3 ) 物体表面特性的影响 不同物体因材质的不同也造成激光反射率的差异，这也决定了激光测距仪有效测量 距离的不同。基于激光测距仪的工作原理可知有效测量距离随激光反射率的增加而增 旗乎鸯箍卡禾壁滤漩翰瓣时定 芏i 连潮搦建算注蜡究 大，物体表面反射率为2 0 时，l m s 2 0 0 的可测距离为2 0 m 左右，丽一般物体表面的反 魁搴均大于2 鹋叙舞本头懿爱瓣率在4 发套，爨以一毁罴爝8 m 淡肉豹激光数攒传为 肖效数掘。 ( 4 ) 激光入射角影响 为了验涯攀羁入惑角瓣激走测距遣袋豹影嚼，锌瓣不同角菠送行实验霹浚番穗，0 、 l o 。和2 0 。的曲线和均值相麓较小，表明在2 0 9 范围内测距的误差并不太。到3 0 。时角度误 麓较大，毽3 0 4 塑j 6 0 。时又鸯凝减套，慧黪寒说，入射惫对溅距戆缘缀蠡孽影嚷毙貉体表瑟 特性的影响要犬，最大达到1 6 r a m ，而栎准偏差相对较小。 ( 5 ) 混和像豢现象影响 激麦静混鞫稼素褒象怒缮当激光懿镶形光泰嫠始穷逶嚣令鞠躐较运懿妥弦，藏砻镀 此距离很近但表面反射率相麓很大的目标时，由于激光测距是所有锥形区域内目标点的 溉和距离，测得的距离将无物理意义。混合像素现象不是多次扫描假平均造成的，即便 纛攀次扫撵孛纛会爨凌。鬣颡像素现象辩测篷造成熬影穗主要是勃钵透缘夔检测，由予 混和像素点在测距数值上表现为前后两个测距点的平均值，对同一聪域的多次扫描完全 可以抵消混和像索的影响。幽物体表面特性影响的分析可知，纹理不同所造成的混和像 豢瑷象连可敬忽貉。嚣我，滋秘缘素瑗象对琢装裣溅鹣影螭芳不大，霹潋惑臻苓诗。 ( 6 ) 扫描补偿 由于l m s 2 0 0 激光测距仪的工作原理是由旋转电机带动反射镜宪成一次扫描的，当 凝嚣久在运动审遴行数撂袋鬃嚣，运魂会静致激毙溅遴纹豹趋蕹爨簸数蕹会发生揍夔， 即每一溺量数据点实际所对】藏的机器人位漆是不同的。数据失真的程度取决于机器人运 行速度与激光旋转镜旋转速度的比率。对激光扫描数搬补偿必须在原始数据层次进行， 嚣不是在特征艨次遴撂，因为螽者意踩饕霹特短弱入为改变。 偿凝器久运行孛鹣程箍 谈麓必须在每次扫描数据到达的同时读取里程计的假，因此必须使用时间邮戳( t i m e s t a m p ) 技术以保证两传感器数据的同步。根据l m s 2 0 0 的技术手册，在最大测躐8 m 的 揍凝下，测距豹系缓误差为壹 5 r a m ，标礁缡差+ _ s m m ，角凄误差搔参蠡岽给。夤漤溯量误 麓主要由激光的光束角和旋转电机引起，根掘技术手册l m s 2 0 0 的角度解析度为o ，2 5 。 缘会上述误差分板结果，考虑各种坯境嚣素的影响，最终测距秘角度标准蓑分别取 堆十 r 聪卡自：曼滤波的例时定位地i 射构建并泣4 , 1 f 究 吒= s m m 和= 0 2 5 。= “3 6 x 1 0 4 r a d 。 2 4 小结 里程计和激光测距仪是移动机器人通常使用的传感器。内部传感器罩程计可提供短 距离范围内机器人定位和导航所必需的位姿信息。但在长距离的运动中需要外部传感器 激光测距仪提供观测信息进行校正。本章对单程计、激光测距仪两种移动机器人常用的 传感器进行了建模工作，对移动机器人研究中的不确定信息描述问题进行了讨论，并对 里程计和激光测距仪进行了误差分析。 3 卡尔曼滤波定位 1 9 6 0 年，r e k a l m a n 4 4 1 在其发表的一篇著名的论文中，描述了用递归的方法来解 决离散数据的线性滤波问题，这便是卡尔曼滤波的雏形。简单的讲，卡尔曼滤波可以看 作是一组数学方程式，通过这些方程它提供了一种有效解决过程中的状态估计问题的方 法。该方法不仅可以对过去或现在的状态进行估计，而且可以预测未来状态，所以它在 机器人自主或辅助导航研究中占有很重要的位置。随后卡尔曼滤波不断发展，特别是在 解决非线性滤波问题的扩展卡尔曼滤波提出之后，该技术得到了更为广泛地应用与发 展。 3 1 滤波器的计算原型 卡尔曼滤波器用于估计离散时间过程的状态变量x e 孵。这个离散时间过程由以下 离散随机差分方程描述： 气2a r k l + b u k l + 一l ( 3 1 ) 定义观测变量：吼，得到量测方程 气= h x k + k ( 3 - 2 ) 随机信号和咋分别表示过程激励噪声和观测噪声，假设它们为相互独立，正态分布 罐十扩展卡尔蔓滤波的i 几j 时定位i 地幽构缱算 上研究 的白色噪声：p ( w ) 一n ( o ，q ) ，p ( v ) 一n ( o ，r ) 。实际系统中过程激励噪声协方差矩阵g 和观测噪声协方差矩阵r 可能会随每次迭代计算而变化。但在这儿我们假设它们是常 数。当控制函数一或过程激励噪声m 一。为零时，差分方程( 3 1 ) 中的n x r l 阶增益矩阵a 将 上时刻。的状态线性映射到当前时刻t 的状态。实际中a 可能随时i 日j 变化，但在这儿 假设为常数。n x l 阶矩阵b 代表可选的控制输a u 吼的增益。量测方程( 3 2 ) 中的m x r 阶矩阵h 表示状念变量对测量变量的增益。实际中h 可能随时问变化，但在这儿 假设为常数。 定义吼”( 一代表先验，、代表估计) 为在己知第k 步以前状态情况下第k 步的先 验状念估计。定义毫吼“为已知测量变量气时第k 步的后验状态估计。由此定义先验估 计误差和后验估计误差： 譬三二喜 p s ， 先验估计误差的协方差为： 巧= 町矿 ( 3 - 4 ) 后验估计误差的协方差为： 只= q q 7 ( 3 - 5 ) 根据贝叶斯理论，先验估计和加权的测量变z k 及其预测麟之差的线性组合构成了 后验状态估计毫，即 毫= 专+ k ( 乙一衄) ( 3 - 6 ) ( 3 - 6 ) 式中测量变量及其预测之差气一臌；被称为测量过程的革新或残余。残余反映了预 测值和实际值之间的不一致程度。残余为零表明二者完全吻合。( 3 6 ) 式中n ，”阶矩阵k 叫做残余的增益或混合因数，作用是使( 3 5 ) 式中的后验估计误差协方差最小。可以通过 以下步骤计算k ：首先将( 3 6 ) 式代入吒的定义式，再将吼代入( 3 5 ) 式中，求得期望后， 将( 3 5 ) 式中的只对k 求导，并使一阶导数为零从而解得k 值。k 的一种表示形式为： 培十j r 艟卡尔曼滤波的时定位o i 地幽构i i i = 法刎究 墨= p ；- 7 ( 职。日7 + r ) 一 ( 3 7 ) 由( 3 - 7 ) 式可知，观测噪声协方差r 越小，残余的增益越大k 越大。特别地，r 趋向于零 时t 有：1 。i r a + k k = 日一，另一方面，先验估计误差协方差巧越小，残余的增益k 越小 特别地，口趋向于零时，有：b l i m ，。k k = 0 。 增益x 的另一种解释是随着测量噪声协方差r 趋于零，测量变量z 。的权重越来越 大，而气的预测臌i 的权重越来越小。另一方面，随着先验估计误差协方差巧趋于零， 测量变量气的权重越来越小，而气的预测礅i 的权重越来越大。 3 2 离散卡尔曼滤波器算法 卡尔曼滤波器用反馈控制的方法估计过程状态：滤波器估计过程某一时刻的状态， 然后以( 含噪声的) 测量变量的方式获得反馈。因此卡尔曼滤波器可分为两个部分：时间 更新方程和测量更新方程。时自j 更新方程负责及时向前推算当前状态变量和误差协方差 估计的值，以便为下一个时间状态构造先验估计。测量更新方程负责反馈也就是说， 它将先验估计和新的测量变量结合以构造改进的后验估计。时间更新方程也可视为预估 方程，测量更新方程可视为校正方程。最后的估计算法成为一种具有数值解的预估一校 正算法，如图3 1 所示，时间更新方程将当前状态变量作为先验估计及时地向前投射到 测量更新方程，测量更新方程校正先验估计以获得状态的后验估计。 时间更新卜状态更新 ( 预测) 一 ( 校正) 图3 1 离散卡尔曼滤波器循环更新图 下面分别给出了时| 日j 更新方程和洲量更新方程的具体形式。 卡尔曼滤波时| 日j 更新方程： i = 4 毫一+ b u h 1 厅：彳只一彳r + q 卡尔曼滤波状念更新方程： ( 3 - 8 ) 牡十j r 艘卡尔曼滤波的i 州时定位j 地幽构迎算法州究 k 。= h t t h h t + n ? 毫= 写+ 墨( 毛一瓯) ( 3 9 ) 只= ( 卜k k h ) 巧 计算完时问更新方程和测量更新方程，整个过程再次重复。上一次计算得到的后验估计 被作为下次计算的先验估计。这种递归推算是卡尔曼滤波器最吸引人的特性之即它 比其它滤波器更容易实现：例如维纳滤波器，每次估计必须直接计算全部数据，而卡尔 曼滤波器每次只根据以前的测量变量递归计算当前的状态估计。下图将式( 3 - 8 ) 和式( 3 9 ) 结合显示了滤波器的整个操作流程。在实际使用滤波算法前，通常要通过测量采样分析 获得测量噪音协方差r 。然而过程噪音协方差q 相对来说不容易直接测得，要通过考虑 多方面的不定因素进行拟定。不管在选择上述两参数时是否有足够合理的理论依据，都 要根据具体的模型与目的对其进行调整。 幽3 2 尔曼滤波器f + 作原理幽 皋- 1 二护艟卡力：曼滤波的l 川时定位l j 地i 封构让算i 上研究 3 3 扩展卡尔曼滤波器 卡尔曼滤波器估计用线性随机差分方程描述的离散时删过程的状态变量j 吼”。但 如果被估计的过程和( 或) 观测变量与过程的关系是非线性的一些最著名和有趣的卡尔 曼滤波应用就是处理这些情况的。将期望和方差线性化的卡尔曼滤波器称作扩展卡尔曼 滤波器( e x t e n d e dk a l m a nf i l t e r ) ，简称e k f 。 同泰勒级数类似，面对非线性关系时，可以通过求过程和量测方程的偏导来线性化 并计算当前估计。我们将前面提到的公式换一种方式表示，假设过程仍具有状念向量 j ，但其状态方程已变为非线性随机差分方程的形式： x k = f ( x k 。l ，n k t w k i ) ( 3 - 1 0 ) 观测变量z e 吼“为： 气= ( ，吨) ( 3 - 1 1 ) 随机变量和咋仍代表过程激励噪声和观测噪声。差分方程( 3 - 1 0 ) 式中的非线性函数厂 将上一时刻| i 一1 的状态映射到当前时刻| i 的状态。量测方程( 3 1 1 ) 中的驱动函数和零均 值过程噪声w k 是它的参数。非线性函数厅反映了状态变量吒和观测变量气的关系： 实际中我们显然不知道每一时刻噪声叱和唯各自的值。但是，我们可以将它们假 设为零，从而估计状态向量和观测向量为： 置- - f ( - p “。，0 ) ( 3 1 2 ) 磊= ( 五，0 ) ( 3 一1 3 ) 其中，乏是过程相对前一时刻七的后验估计。根据上述两式将状态向量和观测向量线性 化： 坼墨+ 爿( h 一一毫一1 ) + 胁t 一 ( 3 - 1 4 ) 毛= 磊+ h ( 以一五) ( 3 一1 5 ) 其中，与毛分别为实际的状态向量和观测向量，曩与毛分别为根据式( 3 1 2 ) 币1 1 ( 3 一1 3 ) 堆十扣艟卡尔曼逮波的i 川时定位j 地i 兰| 构矬算法州究 得到的状态和观测向量的估计，丘是k 时刻状念向量的后验估计。 a 是，对工的偏导的雅可比矩阵： = 鼍( 和。，o ) ( 3 - 1 6 ) w 是厂对w 的偏导的雅可比矩阵： = 蔫( 和。 o ) ( 3 1 7 ) 灯是h 对工的偏导的雅可比矩阵： = 亳( 枷) ( 3 1 8 ) v 是h 对v 的偏导的雅可比矩阵： m 一- 监3 v p ( 训 ( 3 一1 9 ) 虽然上面公式中没有在4 ，w ，日，v 中加入下标k ，但它们实际上是随时间变化 的。同样可以将算法中的方程分为两大类，即时间更新方程和测量更新方程。时间更新 方程是用来通过当前状态值以及其误差协方差获得下一时刻状态的先验估计值；而测量 更新方程则是用于反馈，通过反馈回来的测量值以及先验估计来获得更为准确的后验估 计。如图3 3 示。 扩展卡尔曼滤波器的一个重要特性是卡尔曼增益甄的表达式中的雅可比矩阵巩 能正确地传递或“加权”观测信息中的有用部分。例如，如果通过 观测变量毛和状态 变量没有一一对应的关系，雅可比矩阵以便通过改变卡尔曼增益从而使得残余 气一 ( ，0 ) 中真j 下作用于状态变量的部分被加权。 壮十和+ 胜卡尔曼滤被的i 一时定位地圈构建算泣州宄 i 测堪更新( 校止) ( 1 ) 计算p 尔曼增菔 时闻更新( 预测) 墨= 只- r 一- 。r 1 , - i x r ) “ ( 1 ) 向前推算状态变鼙 ( 2 ) 由观测变量更新估计 茸= f ( f q + u k ，0 ) t = + k 。( z 。一 ( 茸，o ) ) ( 2 ) 向前推算误筹协方筹 ( 3 ) 更新误筹协方筹 巧= 4 只一。4 + q 一町 只= ( 卜- k k h i ) 巧 毫一。和只。为初始估 3 4 定位原理 图3 ，3 扩展卡尔曼滤波器工作原理 e k f 定位主要由四步组成，如图3 - 4 所示。 第一步是动作更新或状态预测，把高斯误差运动模型直接应用于机器人所测量的编 码器的行走。然后，机器人收集实际传感器的数据，并在观测步骤中提取合适的特征， 同时，根据地图中他的预测位置，机器人产生一个测量预测，它辨识机器人期待寻找的 特征和那些特征的位置。在匹配过程中，机器人要在观测期间实际提取的特征和测量预 测的期望特征之间辨识最佳的配对。最后，卡尔曼滤波器可以将所有这些匹配所提供的 信息融合，更新估计机器人状态。 摹手扩城卡岛：堑滤教靛瓣时定位i 琏鬻秘建舅法蜡宠 宴磐哟。p ( 七l k ) 状态颈溺：飘器入短姿凝箕傍方差怒簿静岛蔷 f 一移预测，鬏爨枫器人在时剡k 和控 制输入( | i ) 产嫩的运动，预测机器人在时刻七+ i 的状态，( 甜( t ) ，x ( 后) ) 一工( 七十1 降) ，位 姿误差秘方差簸簿更毅，p ( 露- 尹( 露专l 鎏) 。 观测和测墩预测：观测是在原传感器数据上执行特征提取的结果，是由单独观测 ( k + 1 ) 的集含，表示为z ( k + 1 ) ，各单独的观测可以代表一个所提取的特征，诸如条 爨线或门穰掇机器入估诗经姿量棒洚+ 1 ) 和观铡五，新豹观测预测为： 毒( 站十1 ) = 扛曼( 七陋+ 1 ) ( 3 2 0 ) 邈数趣是逮豳菇传感器之蠲豹坐标交换。 特征匹配：匹配过程是通过数据关联找到乏( k + 1 ) 的实际值。榈虑于观测所找到的 溅霪预溅，磐矮诗算掺茏誓( + 1 ) = 弓女+ 1 ) 一毒是+ 1 ) ，为了臻定测爨预澜与鼹溅粒怼应 的有效性，由数据关联的方法指定一个确认门限，进行“最佳n 配对。 估计( 状态熙新) ：接下泉根据状态预测和在时刻k + l 的观测，计算机器人状态的最 经 砉计量惫+ t l k + 1 ) ，卡尔篷增益为暂( 1 ) ，机器久德姿雯薪话诗 剐“d 端卷芝淞淼双妇 p z t ， 滞+ 1 ) + k ( 女+ 1 ) 暑( 东+ 1 ) 一l ，( 是+ 1 ) ”一7 皋十和艟卡尔壁滤波的州h 寸定位o j 地幽构建并法研究 3 5 小结 本章对卡尔曼滤波和扩展卡尔曼滤波( e k f ) 的原理进行了分析，并描述了采用扩展 卡尔曼滤波的机器人定位过程，下一章中将对基于e k f 的线特征s l a m 算法进行详细 分析。 4 基于e k f 的线特征s l a m 算法研究 4 1 算法系统模型与步骤 在基于滤波估计的s l a m 方案中，运用卡尔曼滤波对机器人位置状态和各路标特征 位置状态进行估计。机器人的位置预测是基于机器人的运动模型，路标特征的位胃预测 是基于路标特征的运动模型。基于观测方程和当前的系统状态估计，得到各环境路标特 征的预测观测。运用新息及新息方差对系统状态向量及协方差矩阵进行更新。当观测到 一个新的环境特征时，经校验确认后，就把它添加到系统状态向量中，系统状念向量的 增广基于环境路标特征增广模型。卡尔曼滤波对机器人位置和各路标特征的位置进行递 归地预测估计，并对协方差矩阵进行递归地更新替换。 4 1 1 状态空间 在时刻七，机器人的状态可以用它在环境中的位置和方向唯一地确定，所以机器人 的状态可以表示为以( 豇) 。每一个特征的状态由它在环境中的位黄确定，可以表示 置( 后) ，由这些特征点构成的环境地图就可以表示为j 0 ( 七) 。如果把所有的地图特征表 示在一起，系统状态可以简单地表示为： 聃熙 ， 其中，机器人位姿： x ，( 七) = x 。( y 。( 七) e ( 七) ( 4 - 2 1 筚- t 。静城卡笨受游竣船辅埘定 亟，地豳鞠建算法磷究 地图表示： k ( 是) =渊 麓置t 毒；： i 暑 ，浚方嫠矩簿表示为： 出，= 出铡 4 1 2 系统动态模型 ( 4 - 3 ) ( 4 4 ) 一兮系统豹动惫摸鍪磺一令露翊熬溱数接述了系绕获惫是麴毽燹纯豹。遴鬻翔一个 一阶非线性差分方程来表示； x ( k ) = i ( x ( k - o ，u ( 奄”+ w 蠡) ( 4 - 5 ) 墨( 膏) = 工( 墨( k - 0 ，u 秘) ) + ( 蠡) ( 4 - 6 ) 以( k ) = 以( k - 1 ) ( 4 - 7 ) 箕串，x ( k 表示霹刻素酌袄悉矢量，u 转) 是云知控露g 输入或者表零星程诗数据，函 数农示系统变换模型，w ( k ) 是一个随机矢量，用于表示系统动态噪声和系统建模中的 誉臻定e 萁孛麓，( k ) 是露露刻豹蓬标静状态。在本论文獗究懿s l a m 雾法孛，黪凌猗 芷 憨假设静止的。 4 ， 。3 系统观测摸型 4 1 3 1 坐标转换问题 移动讥器人在一个包含许多环境路标特征的环境中，从一个未知的位置开始，一边 行走，一边观测环境信息。机器人上安装商罩程计和激光测距雷达，晕程计用于记录机 嚣入黔运动信爨，并基于运动信息，雄髯瓤器久下一时刻懿位置鑫憝。激光测蹬嚣达藤 予对环境特征避行扫摇观测，提取自然路标，比如墙角，桌子角等。运用环境路标特征 单十和腱卡自：璺滤波的f 川时定位j 地i t l 构让臂= 法研究 的实际观测与预测观测之差对系统状态向量和系统协方差矩阵进行更新。得到机器人位 置和各环境路标特征位置的估计。 环境信息山环境特征在一个全局坐标系统中的坐标位哥来表示，机器人的位置也是 相对于这个全局坐标系的。所以选择一个全局坐标系是必要的。移动机器人在运动时， 如果移动机器人的位姿和全局坐标系不一样就需要进行多次坐标变换才能求出移动机 器人检测到的物体在全局坐标系下的位置。 车载坐标系和全局坐标系变换通常不是一个的简单的加减关系，如图4 1 所示，移 动机器人车载坐标系的x 轴与全局坐标系的x 轴有一个央角口。( ，) 是a 点在车载坐 标系下的坐标值。口为彳点与全局坐标系x 轴的交角。根据三角变换得到： 口：0 一a r c t a n 量。因此，a 点在全局坐标系下的坐标值( x ，y ) 为： 虬 x = 而+ 乏i 了s i l l 口 ( 4 8 ) y = m + 厮x c o s o 。 ( 4 - 9 ) 如果a 点的位置不是如图禾1 所示时，其在全局坐标系下的坐标( x ，】，) 将做相应的 调整。 】， 丑 d 叁地 v p , o b o t l 而 幽4 1 局部坐标与全局坐标转换 y 0 ) 蘸 + 垂域卡乐薹滤波静醚瓣定链4 ，戆糟粕建等泣剜究 4 1 3 2 观测模型 移动机器入利用激光测距雷达对周围环境进行观测，提取环境路标特征，计算其在 擎裁坐标系中的位置( 如，y o ) 。然后，基于当前的机器人位置。变换别全局坐标系中，表 承为f 菇，y 。慈稠有效豹羧攒关联援本，糖这个薪或灏豹环境特钲等恁图审存糖豹第i 伞 环境特征关联起来。运用观测到的路标特撅位置( x ，y ) 和在地图中国之对应的路标特征 戴鬟( 置，￥) 熬麓终戈震涮篷。实际上，躐溺量是一令0 囱量，瘊毅系统鼹测攘黧隽： z ( 七) = 冉( 善( 后) ) + v ( j ) = 脬。( 七) + v ( 七) ( 4 1 0 ) 芟巾，z ( k 是拜季刻的蠡预测蕊测量，v ( k ) 表示传感嚣溅量的不确定，霉戬嗣一令二维 的噪声向量表承，均值为0 。协方差为r 。矩阵峨只依赖于特定的路标信息，渐与当前 的系统状念估毒t 无关，所以系统观测模型也是一个变参的线性方程。 4 1 4 算法步骤 s l a m 算法分三个步骤处理同对定位每建图掏建阏题：状态鬏测，瑷测更毅，状态 增广。在每步，都要对系统状态向量和协方差矩阵避行更薪。 1 、状态预测 基于上瑟黪系统运动方穰，褥到滤渡嚣预测除段扩震姨态袁登浆健撂变换： 黜 _ “置x 譬k - 1 l z ( 七) j 【 。()j 一7 预测除段憨冤一部分是鏊予援器久凌态模型黥挽方蓑楚阵弱传播变换。由予锻设巧 境特征是静态的，可以对系绕的预测迸彳子简化，得到的系统协方蓑瓶阵的替换公式为： 翟k ，鼍竺， _ | 乳黟冀暑：k - 第瓯 ”警2 k - 1 l ：，i 东7 ) jl限，( 女) 气l j ) 。；l 2 、观测更新 如果把观测信息融合到系统状念估计中，首先是运用观测模型h ，基于当鲋的状态 位诗诗算颞测瓣鼹测。当馋感器测量巧缓绫悫露，褥爨环壤装短的鼹测篷，魏之为实际 祭- f - ) 艘卡矗；蹙滤波的m 时定能地幽粕建算法研究 观测，迎