（模式识别与智能系统专业论文）移动机器人slam中的数据关联算法研究.pdf

ab s t r a c t ab s t r a c t d a t a a s s o c i a t i o n o f s i m u l t a n e o u s l o c a l i z a t i o n a n d m a p p i n g f o r m o b i l e r o b o t i s t h e p r o c e s s o f rel a t i n g c u r r e n t o b s e r v a t i o n s w i t h t h e l a n d m a r k s ( f e a t u re s ) i n c l u d e d i n t h e m a p . wi t h t h e c o n t i n u o u s l y d e e p re s e a r c h o f s l a m , t h e re s e a r c h o f d a t a a s s o c i a t i o n a l g o r i t h m s i n s l a m h a s b e c o m e a h o t p r o b l e m . t h e re s e a r c h o f e ffi c i e n t , r o b u s t d a t a a s s o c i a t i o n a l g o r i t h m s h a s a g r e a t m e a n i n g o n s l a m p r o b l e m o f m o b i l e r o b o t . 玩t h i s t h e s i s , t h r e e p a r t s o f w o r k a r e p r o p o s e d f o r d a t a a s s o c i a t i o n a l g o r i t h m s b a s e d o n e k f ( e x t e n d e d k a l ma n f i l t e r ) s l a m. f i r s t , c o m b i n i n g w it h t h e i n v e s t i g a t i o n o f t h e c u r r e n t r e s e a r c h s i t u a t i o n o f d a t a a s s o c i a t i o n a l g o r i t h m s , s i m u l a t i o n res u l t s a r e g i v e n f o r c o mp a r i s o n a n d a n a l y s i s f o r s o m e p o p u l a r m e t h o d s a n d t h e d e fi c i e n c y c a n b e f o u n d . s e c o n d , t h e c l a s s i c a l d a t a a s s o c i a t i o n a l g o r i t h m - n n ( n e a r e s t n e i g h b o u r ) m e t h o d u s e s s t a t i c t h r e s h o l d t o d a t a a s s o c i a t i o n , b u t i t d o e s n t c o n s i d e r t h e d y n a m i c p r o p e r ty i n s l a m p r o c e s s . c o m b i n i n g t h e d y n a m i c p r o p e r ty , a m e t h o d w i t h d y n a m i c t h r e s h o l d i s p r o p o s e d i n t h i s t h e s i s f o r i m p r o v e m e n t o f t h e p r e v i o u s s t a t i c w a y . a c c o r d i n g t o t h e u n c e r ta i n t y o f l o c a l i z a t i o n a n d m a p p i n g i n s l a m p r o c e s s , w e s e t t h e t h re s h o l d d y n a m i c a l l y o n l i n e i n o r d e r t o i n c r e a s e t h e a c c u r a c y o f l o c a l i z a t i o n a n d m a p p i n g o f r o b o t . m e a n w h i l e , t h e c o n c e p t o f d y n a m i c t h re s h o l d i s f u r th e r e x t e n d e d t o t h e p r e p r o c e s s 吨 o f a s s o c i a t e d i n f o r m a t i o n . t h i r d , 勿 a n a l y z i n g t h e e s s e n c e o f d a t a a s s o c i a ti o n p r o b l e m , i t i s c o n v e rt e d i n t o a c o m b i n e d o p t i mi z a ti o n p r o b l e m . t h e c o m p l e x i t y o f a s s o c i a ti o n a l g o r i t h ms i s r e d u c e d 勿 t h e p r e p r o c e s s i n g o f d y n a m i c t h re s h o l d . f i n a l l y , s i m u l a ti o n a n n e a l m e t h o d i s u s e d f o r s o l v i n g t h e p r o b l e m t o o b t a i n f in e e x p e r i m e n t a l r e s u l t s . ke y wo r d s : s i m u l t a n e o u s l o c a l i z a t i o n a n d ma p p i n g , d a t a a s s o c i a t i o n , e x t e n d e d k a l m a n f i lt e r , d y n a mi c t h re s h o l d , s i m u l a ti o n a n n e a l 南开大学学位论文版权使用授权书 本人完全了 解南开大学关于收集、保存、使用学位论文的规定， 同意如下各项内 容:按照学校要求提交学位论文的印刷本和电子版 本:学校有权保存学位论文的印 刷本和电子版，并采用影印、缩印、 扫描、 数字化或其它手段保存论文; 学校有权提供目 录检索以及提供 本学位论文全文或者部分的阅览服务; 学校有权按有关规定向国家有 关部门 或者机构送交论文的复印件和电 子版; 在不以赢利为目 的的前 提下，学校可以 适当复制论文的部分或全部内 容用于学术活动。 学 位 论 文 作 者 签 “ : j a j ,47 年 上 月 日 经指导教师同意， 本学位论文属于保密， 在 本授权书。 年解密后适用 指导教师签名:学位论文作者签名: 1 4 * 解密时间:年月日 各密级的最长保密年限及书写格式规定如下: 内部 5 年 ( 最长5 年， 可少于5 年) 秘密1 0 年 ( 最长1 0 年，可少于 1 0 年) 机密2 0 年 ( 最长2 0 年，可少于2 0 年) 南开大学学位论文原创性声明 本人郑重声明: 所呈交的学位论文， 是本人在导师指导下， 进行 研究工作所取得的成果。 除文中己 经注明引 用的内 容外， 本学位论文 的研究成果不包含任何他人创作的、 已 公开发表或者没有公开发表的 作品的内容。对本论文所涉及的研究工作做出贡献的其他个人和集 体， 均己 在文中以明确方式标明。 本学位论文原创性声明的法律责任 由本人承担。 学位论文作者签名: 0 -,l 年 犷月 2 言 日 第一章 绪论 第一章绪论 随着社会的发展 和科技的进步，机器人已经 成为 人类从事 各项工 作的得力 助手。 移动机器人作为机器人学的一个重要分支，由于具 有可移动性，可以代 替人到各种极限环境中执行探测、操作等任务。随着人类活动范围的不断扩展， 移动机器人在航天探测、海洋开发、军事侦察、 危险品处理等领域显 现出广 泛 的应用前景。而这些领域都存在着很大程度的不 确定性， 有的甚至是完全未知 的新环境， 因此 研究未知环境中 移动机器人的自 主导航问题，对提高 移动机 器 人的实用性至关重要，并有着广泛的经济和社会价值。 研究背景 1 . 1 . 1 s l a y 的提出 智能移动 机器人是一种在复杂的环境下工作的具有自 规划、自 组织、自 适 应能力的机器 人w 。在移动机 器人的 相关研究中，自 主导航是其核 心技术之一， 也是实现真正的智能化和完全自 主移动的关键技术。自 主导航研究的目 标就是 在没有人干 预的情况下使机器人有目 的地移动并完 成特定任务， 进行特定操作. 机器人通过携带的传感器提供的 数据， 获得外部环境信息， 实现自 主定位，判 定自身状态，规划并执行下一步动作。 在已 有的关于移动机器人自 主导航的研究中，大部分都是针对结构化的环 境进行的， 此时环境地图 信息是先验的， 因此导航过程主要包括路径规划、多 传感信息的 融合、定位和路径跟踪等简单步骤。 但是随着移动机器人应用领域 的不断扩展，非结构化环境特别是完全未知环境下的导航更成为移动机器人领 域倍受关注的热点问题。众所周知，定位是机器人导航的基础。定位是确定移 动机器人在二维( 三维) 工作环境中 相对于全局坐标的 位置及其自 身的 姿态，在 已 知环境中， 移动机器人的 路径规 划、 定位、 跟踪和避障等 任务均依赖于先验 地图 信息。 而对于完全未知环境， 机器人的 主要导航任务就是 进入环境通过传 感信息建立环境地图。由 于事先没 有环境地图，因 此难以 使用地图 信息 进行定 位: 另一方面， 要建立环境地图， 机器人必须能 够准确定 位，即定 位与 地图 创 建相互依赖、 相互决定，从 而形成 “ 鸡生蛋、 蛋生 鸡”的问 题a 。因 此， 如 何借 第一章 绪论 助于二者之间的相 互制约， 使二 者同时 在线完成就成为当前移 动机器人导航的 热点问题 同时定位与地图创建 ( s i m u l t a n e o u s l o c a l i z a t i o n a n d m a p p i n g o r c o n c u r r e n t m a p p 吨 a n d l o c a l i z a t i o n ) ，即s l a m或c m l问 题. 在移动机器人导航领域，s l a m 已 被公 认为能够真正实现机器 人自 主 完成 导航任务的关 键技术，它在 星球探测、 军事 侦察、灾难救助和探险 等领域有着 巨大的应用潜力。 1 . 1 . 2 s l a w的研究方法 移动机器人同时定位与 建图是 对环境创建 一个基于特征的地图并 使用该 地 图 获取机器人位置估计的过程。机器人 从一 个未知的地方开始运动， 并且陆 标 ( 特征) 的位置是未知的。通过对陆标的 相对观测信息，机器人同时 计算出自 身 的估计位置以 及陆标的 估计位置. 周而复始的，机器人可以 对陆标建 立起完整 的地图并依靠该地图不断的对自 身进行定位。 机器人的位置和环境 特征的 位置 都可以 通过机器人和环境 特征的 相对关系 来估 计a 7 在 s l a m中，机器人在 未知环境中从一 个未知位置开 始移动， 机器人利用 传感器识别未知环境中的 未知陆标或 特征标志， 然后根据机器人与陆 标或特征 标志之间的相对位置测量值和里程计的读数进行机器人定位和地图创建 ( 地图 创建的主要任务是估计环境中 陆标或特征标志以 及障碍物的 位置， 现有的地图 创建主要 针对二维平面进行 ) ， 在此过程中地图 创建以 增量式完成， 且定位与建 图 融为一体，二者同时在 线完 成， 而不是独立的两个阶段。 s l a m 归纳起来是 一个 “ 感知一 估计一校正”的 过程， 主要包括如下迭代步骤:( 1 ) 机器人根据当 前位姿 估计与地图 信息对下一步的 位姿与地图 信息 进行 预测:( 2 ) 在下 一步中通 过传感器 观测环境中的陆 标( 特征 ) ;( 3 ) 利用观测信息校正 上一 步的定 位与建图 结果。 s l a m 方法作为完全自 主导 航的 工具，已 经在移动机器人领域引 起了 广泛 的 兴趣。目 前存在许多s l a m方 法， 在这些方法中有的是用栅 格来表示 环境， 例如d p - s l a m ) 5 3 ， 而有的 方法则试图 用几何 信息 来表示地图中 的物 体16 1 171 。 然 而， 这些 方法都有很大的 局限性。 例如， d p - s l a m算法， 明确 地限 定用 特殊类 型的 观测设备。 而在那 些试图 描述环境中复 杂的 几何信息的方法中， 在高维度 或者说那些有多 种几何 特征的复杂环 境下， 计算的复杂度则非常大，实 时性非 常 差。 在a y a c h e 和f a u g e r a s 3 以 及c h a t i l a 和l a u m o n d ) 工作的 基础 上， s m i t h . 第一章 绪论 s e lf 和c h e e s e m a n 的 论 文中 101 n 首 次 提出 了 基 于 扩 展 卡 尔曼 滤 波( e k f ) 的随 机建 图方法。 近几年来， e k f 方法成为当下最为流行的s l a m工具。 与此同时，基 于 概 率 的s l a m zi ， 基 于 粒 子 滤 波 器 ( p a r ti c le fi lt e r ) 的s l a m ， 基 于 稀 疏 扩展 信 息 滤 波 器 ( s p a r s e e x te n d e d i n f o r m a t i o n f il t e r ) 的s l a m 和 基 于 集 合 理 论估 计的 s l a m s1等s l a m方 法 也 各 具 特 色。 在 本 文的 工 作中 ， 主 要 是以 扩展 卡尔 曼 滤 波为基础的。 1 . 1 . 3 s l a m 的研究难点 随 着人们对s l a m问 题研究的 不断深入， 研究 涉及的 方面越来越多，主要 包括以下几个方面. 第一，不确定性的处理。事实上， s l a m 的困难就在于系统中无处不在的 不确定性。机器人本身机械性能或未知外力( 绑架) 造成的不确定性将导致运动 估计出现误差， 观测的不确定性( 包括传感器的不确定性和数据关联的 不确定性) 将导致校正失效， 更不必说动态环境中的环境的不确定性。如何处理确定性， 尽 量减少各种客观存在的不确定性引起的误差是 s l a m 的关键， 也是各种算法的 精髓. 第二，数据关联问 题。数据关联是指利用当前传感器探测到的m个观测值 对地图中已 经存在的 n个特征进行更新时，必须明 确指出 某个观测值对应于哪 个特征，或是一个新特征或是一个噪声数据。尤其对于 e k f ( 扩展卡尔曼滤波) 来说， 数 据关联不准 确将导 致算法发散. 传统的 数据 关联 方法 n n ( n e a r e s t n e i g h b o r ) ， 方法简单， 但对 距离很 近的 两个特 征， 算法很容易失 效。 近几年 关于数据关联的研究逐渐增加，相继提出了基于联合一致性测试的数据关联方 法、基于几何关系的数据关联方法。而基于概率的方法巧妙地绕过数据关联问 题，但非常高昂的计算代价不利于大规模环境的应用。 第三，算法复杂度问题。真正完全自主的机器人要求建图与定位必须能够 在线计算，因而对计算复杂度的要求是非常苛刻的。 现在的算法最多能够运行 儿天或几个星期， 可存储和计算的环境特征达到1 0 级， 但仍然与真实世界有很 大差距。单从运算速度角度，现有计算机与人相比 虽然有差距，但这种差距呈 日 渐减小趋势。 但进行定位判断等处理时， 计算机却远远不能与人相比。原因 之一就是人脑的并行计算及分布式存储。将神经网 络技术或其它并行算法应用 于s l a m ，极有可能较大幅度提高s l a m算法在真实环境部署的可能性。 第一章 绪论 第四，地图的表示问题。地图表示要求尽量容易理解和计算，容易加入新 的 信息更新地图。e k f 算法采用平面坐标表示地图， 基于极大似然估计的 概率 算法局部使用概率格计算， 但全局仍然采用几何坐标的 平面地图。 然而用平面 坐标不能充分表示现实环境的特征， 尤其应用视觉传感器获得的环境有可能采 用更复杂、更贴近现实、更容易理解和计算的地图表示。此外，人的头脑中大 多数时候并没有完全精确的坐标形式的地图，但人仍可以完成判断方向、定位 等工作。将人工智能的知识表示理论应用于机器人的地图表示，有可能产生完 全新的 方法 并 取得重大 突 破(2 除此之外还有其它方面的问 题，例如动态环境下的s l a m、自 主导航策略、 多机器人协调、 基于视觉的s l a m等。 本文的 研究工作主要是围 绕s l a m中的 数据关联这一方面而展开的。 1 . 2 掀据关联研究现状 1 . 2 . 1致据关联概述 随着科学技术， 特别是传感器技术、 计算机技术、 信息技术等的飞速发展， 现代跟踪目 标所处的环境变得越来越复杂，现代跟踪理论也在不断的发展和完 善。而数据关联是目 标跟踪尤其是多目 标跟踪的关键技术，一直是研究的热点 问题。在本文中主要研究的是移动机器人同时定位与建图中的数据关联算法。 s l a m 中一个重要的研究问题就是数据关联。数据关联指利用当前传感器 的观测值对地图中己经建立的陆标( 特征) 进行更新时，必须确定某个观测值对 应于已 建立的 部分地图中的哪个陆标( 特征) ， 或是一个新陆标( 特征) 或是一个 噪声数据，其运算复杂度与观测数呈指数关系，且错误的数据关联可能会导致 建图发散。 迄今为止， 对于s l a m问 题的 研究工作大部分都假定先验的观测信息， 而 忽略数据关联问 题。只要环境的地图 包含一个或者更多的特征，就会出现一些 数据关联的不确定性。实际上，在考虑观测误差的情况下，即使已知在环境中 只存在一个物体，仍然存在数据关联的不确定性。造成数据关联不确定性的因 素有很多，一种情况是一个观测值匹配多个特征，另一种情况就是多个观测值 匹配一个特征，每个匹配的概率是不同的，于是造成了数据关联的不确定性。 除此之外，观测值、机器人位姿、陆标的不确定性也造成了数据关联的不确定 第一章 绪论 性。 为了 更清楚 地描述哪种类型的 不确定性对数据关联不确定 性的影响最大， 我 们 首 先 定 义 一 些 变 量: 月 表 示当 前 对 特 征i 的 位置 估 计， z 表 示当 前的 观 测 值 ( 或 者 是 表明 一 个 特 征 可能 所 在的 位 置 ) ， s , 表 示当 前 机 器 人的 估 计 位置 。 然 后， 通过以 下三个图 来阐 述这些不确定性造成数据关联的不确定性的原因。 图1 . 1 观测的不确定性示意图 图 1 . 1表示了由于观测的不确定性导致的数据关联的不确定性. 在图中， 内 部虚线椭圆 代表“ 小”的不确定性， 而外部的实线椭圆代表“ 大”的 观测不 确定性。随着观测不确定性的加大，包含了更多的特征数。也就是说，如果在 观 测 不 确定 性 小 的 情 况 下， 我 们可以 认 为， z 匹 配f , ， 但 是随 着不 确 定 性的 加 大 ， 我 们 就 需 要 考 虑z 匹 配f , . f 2 . f 3 三 个 特征中 的 哪 一 个。 因 此 观测 值 的 不 确 定 性增大意味 着需要考虑更多的 特征匹 配。由 于在许多数据关联算法中 采用数据 关联似然性来进行关联度量，所以 观测的不确定性带来的影响较大。 机器人位姿的不确定性对数据关联的影响与观测的不确定性对数据关联的 影响类似，如图1 . 2 所示。在图1 . 2中， 对于机器人来说，分不清当 前观测的 是哪个陆标，因为它完全不能确定自己 精确的位置或者说它朝向哪里。 在这里， 椭圆表示机器人当前位置的不确定性，两条对称的虚线表示朝向的不确定性。 第一章 绪论 由 于 机 器 人 位置 和 朝向 的 不 确定 性 ， 因 此 对 于 观 测z 可以 匹 配 给f , 或 者f 2 e 图1 . 2 机器人位姿的不确定性示意图 图1 . 3陆标不确定性示意图 从概率的角度来看，陆标的不确定性导致了在匹配的过程中， 将一个观测 匹配给多个陆标，如图1 . 3 所示。 在图1 . 3中，椭圆表示陆标( 特征) 的不确定 第一章 绪论 性， 而 对 于 观 测z 同 时 处 在f , 和f z 的 不 确 定 性 范 围 内 ， 因 此 将z 匹 配 给f , 的 概 率 与匹 配 给f z 的 概 率 基 本 上 是 相同 的， 于 是 造 成了 数 据 关 联的 不 确 定 性。 1 . 2 . 2数据关联研究 方法 近几十年来，人们对于数据关联方法问 题进行了 大量的研究， 提出了 许多 数据关联方法。 1 9 7 1 年， s in g e r 提出 了 最 近 邻 法nneuel ， 用目 标 和 观 测 之间 的 统 计 距离 作 为 关联判决准则，在环境特征稀疏的情况下，性能较好。但当特征密集，某个特 征有效判定区域内出 现多 个观测时， 性能迅速下降。 最近邻法计算量小，易于 实现， 在工程上有着广泛应用。 1 9 7 4 年b a r - s h a l o m提出 了 概 率 数 据关 联 算 法 ( p d a ) 0 8 03 . p d a 利用 需 要 更新的目 标确认域内的所有观测， 可以得到很好的关联性能。 p d a的计算量和 存储量与标准的k a h n a n 滤波器相当， 因此， 一经提出， 很快受到广泛关注。 p d a 的缺点是仅适用于单目 标和分散孤立的多目 标环境。为了 适应密集多目 标环境， b a r - s h a l o m在p d a f ( 概率数据关联滤波) 的基础上，于 1 9 8 0 年提出了联合概率 数据关联算法 ( j p d a ) ， 该 方 法至今 被 认为是一 种比 较完 善的 算法。 j p d a定 义了 联合事件，引入了“ 聚”的概念， 通过计算联合事件的联合概率，计算观测与 目 标之间关联的边缘概率。 但是 j p d a 的计算量随着目 标数的增加而成指数增 长，使 p d a的实时应用成为难题。 c .l .m o re fi e l d 提出了0 - 1 整数规划法o .，它计算每一个所谓联合事件的 后 验概率，选择概率最大的联合事件进行关联。m o re fi e l d将这一过程对应于数学 上早已 研究过的整数规划方法，在变换过程中，假设每一联合事件的先验概率 相等。这样的 假设结果使得变换方便， 但没有充分利用先验信息。 d .b .r e i d提出 多假设计算(2 1) ，即 计算每个假设的 后验概率， 通过删除后验 概率小的假设和合并近似的假设来确认关联。多假设法的不足是过多的依赖于 一些先验信息。另外，还有其它的多种数据关联算法，如聚类方法， v i t e r b i 数 据关联( v d a ) 技术， 模糊数据关联( f d a ) 技术。 1 . 3 问题的提出 通过对多种数据关联方法的分析比 较可以看出，多数方法对当前时刻所获 得的观测值先进行预处理操作，针对所要匹配的陆标 “ 筛选”出符合一定条件 第一章 绪论 的候选观测值集合，这样在进行数据关联算法的时候可以减少运算的复杂度， 更加适应s l a m中 对数据关联实时 性的要求。 在数据关联方法中 通用的“ 筛选” 规则是根据m a h a l a n o b i s 距离，选择与要匹 配的陆标的m a h a l a n o b i s 距离满足一 定的闽 值范围的 观测值. 但是多数的 方法在 选择的时 候采用的是静态阂值。 然 而在同时定位与建图的过程中，移动机器人自 身定位和已 建图中的陆标的不确 定信息每个时刻都是不同的，也就是说在进行 “ 筛选”的时候没有考虑系统状 态的不确定性因素。因此根据 s l a m中系统协方差矩阵提供的不确定信息，采 用动态阂值的观点，提出一种基于动态阐值的信息预处理方法。 简单来讲， s l a m 中的数据关联就是将m个观测值对地图中已 经存在的” 个陆标进行更新时，明 确指出 某个观测值对 应于哪个陆标，或是一个新陆 标或 是一个噪声数据。 在进行数据关联匹配的时 候，多数方法采用的是似然估计， 即计算观测值和待匹配陆标估计值之间的似然度.而且，我们可以发现，数据 关联问 题实质上就是一种组合优化问 题， 所以 我们将数据关联问 题进行转化， 提出一种新的解决数据关联算法。 1 . 4 本文的主要内容和组织结构 研究鲁棒、高效的数据关联算法对于s l a m有着十分重要的意义，因为错 误的数据关联将导 致建图的 发散。 本文的 研究工作就是对主要的 几种s l a m数 据关联算法进行比 较分析，在此基础上提出 基于动态阐值的数据关联算法。并 且通过分析s l a m数据关联问题的实 质， 将数据关联问 题进行转化，然后用比 较成熟的方法解决这个问题。 本文的主要内容和组织结构如下: 第一章绪论主要介绍移动机器人同时定位与建图的概念、研究方法以及研 究的难点问 题。 然后在此基础上介绍s l a m中数据关联算法的研究现状。 第二章主要介绍本文的理论基础一移动机器人同时定位与建图及其两种重 要s l a m研究方法，即 扩展卡尔曼滤波( e k f ) 和粒子滤波( p a rt i c l e f i l t e r - p f ) a 第三章详细介绍主要的几种数据关联算法，并且通过实现相应的算法，对 各种算法的性能进行深入的比 较分析。 第四章详细叙述动态阐值概念，并在此基础上详细介绍基于动态阐值的关 联信息预处理方法以 及基于动态阐值的改进的数据关联算法，并对算法的效果 第一章 绪论 进行实验分析比较。 第五章介绍数据关联问题的转化形式，并且应用模拟退火方法进行求解。 最后给出相应的算法的性能分析。 第六章对本文进行全面的总结， 讨论本文数据关联算法存在的问 题，并对 以后的改进方向 进行了展望。 第二章 s l a m简介 第二章 s / 1 简介 2 . 1 s l a m 问题描述 2 . 1 . 1系统状态 由 于s l a m问题固有的 特点( 机 器人缺乏自 身位置和环 境的 先 验信息; 靠外 部和内 部传感器获得知识; 环境、 传感器信息及机器人 运动本身具 有不确定性) ， 各 种s l a m方法归纳起来都 是一个 “ 估计一校正” 的过 程， 必须解 决下列问 题: 1 )如何估计机器人及 环境特征 状态? 2 ) 如何校正和更新地图? 3 ) 地图如何表示? 4 )不确定性如何处理? 在 s l a m中，机器人 在环境中 运动， 依靠传感器来获得 环境的 特征信息， 系统 在 k 时刻的状 态用 递增的状态 矢量x ( k ) 来表示，系统的 状态由 机器人的 位 姿戈( k ) 和 地图 信 息戈(k ) 组 成(包 含 各 特 征 标 志 的 位置 信 息 ) 。 ( 2 . 1 ) 其中 x (卜 x , (k)x (k) _ x . (k) r x ,(k )l x , (k ) ” y , ( k ) l e , ( k ) j ( 2 . 2 ) _ _ ( x (k )1 x , ( k ) y l (k ) x . (k ) 一 1: ” 1 : 1 l x . ( k ) j x ( k ) l y . ( k ) ( 2 . 3 ) 在 这 里 ， x , (k ) , y . ( k ) 和x ( k ) , y ( k ) 分 别 代 表了 机 器人 以 及 陆 标 的 二 维 笛 卡 尔 坐 标， b , ( k ) 描 述了 机 器 人 当 前 状 态 的 方 位 角 对 于 两 轮 机 器 人 来 说， 芯( k ) 唯 一的确定了机器人当前位姿。 第 二章 s l a m简介 2 . 1 . 2机器人模型 机器人模型用来描述机器人在给定的控制输入u ( k ) 下，过去的状态 x , ( k - 1) 和 当 前的 状 态x , ( k ) 之 间 的 关 系。 x , ( k ) = f ( x , ( k - 1 ) , u ( k ) ) + v ( k ) ( 2 . 4 ) 其 中 x , (k ) 是k 时 刻 机 器 人 的 状 态， v ( k ) 是 一 个 随 机 矢 量， 用 来 描 述 机 器人 模 型 中的噪声，即表示机器人运动不确定性。在导航系统中，准确的机器人模型是 十分重要的，并且机器人的 模型是随 着机器人的结构不同而变 化的， 通常使 用 的有两轮、三轮、四轮机器人.在理想的状态下，机器人模型能够准确的描述 出机器人的运动，不确定性为 零， 但是在实际中是不可能的.由于 机器人的 运 动和传 感器都 受到噪声的 影响，因 此会影响系统 状态估计的 精度。 图 2 . 1描 述 了一个二轮的机器人。 图2 . 1 二轮机器人 示意图 相应的机器人模型为: 、 (2. ，.j (k)(k)(k) vx.气饭 -1.esesesesesesl 十 ，.j 乃t) x , ( k ) y . ( k ) 6 , ( k ) 卜 (k 一 ) + u , ( k ) - t - c o s ( b , ( k 一 ) + u 2 ( k ) = 卜( k 一 ) + u , ( k ) - t - s in (b , (k - 1) + u 2 ( k ) l 0 , ( k 一 1) + u 2 ( k ) .t 其中 u , ( k ) , u . (k ) 分 别 表 示 在k 时 刻 ， 控制 输 入 中 的 线 速 度 和 角 速 度. 第二章 s l a m简介 2 . 1 . 3陆标模型 在对 s l a m 的 介绍中， 提到陆标就是指 环境中 的特征， 能够被机器人的 传 感器所观测到。 我们用变量的形式来描述陆标，因为系统的 状态矢量的维数是 随着观测到 环境中陆 标数目 的增加而不断变化的，所以 用变量的形式更方便对 系统的 状态矢量进 行扩维。 在本文中是 针对静止的环境进行 研究， 故假设陆标 是静止的，于是陆标模型可以简单的表示为: x ,. ( k ) = x . ( k 一 1 ) ( 2 . 6 ) 如果具体到 某一个陆 标， 可以表示为: 戈( k ) 二 戈( k 一 1 ) ( 2 . 7 ) 2 . 1 . 4传感模型 在 s l a m中， 机器人依靠所携带的 传感器来获 得环境的特征信息。 我们通 常用一个非 线性函 数 来描述系统的传感模 型。 z ( k ) = h ( x ( k ) ) + w ( k ) ( 2 . 8 ) 其中z ( k ) 表示 在k 时 刻的 观测值， h ( x ( k ) ) 是个非线性的函 数， 来描述系统状态 的观测模型，w ( k ) 是一个随机矢量， 表示观 测模型中的 噪声即观测过程中的不 确定 性。 因 此根据这 个模型， 在己 知当 前的 系统状 态下， 我们可以获得k 时 刻的 观测值。 具体的 传感模型依赖于导航系统中 的传感器 类型，通常所用的 传感器 为激光和声纳，这两种都属于距离/ 方向传感器，相应的传感模型如下: f z ( k ) l z (k ) = z . m = , y . ( k ) 一 y ( k ) . 。 ， ， 、 a 川 习 旧 口吸 ， 一 口 . ( kl x , ( k ) 一 x , ( k ) : : (2.9) 2 . 2 扩展卡尔里 泣波s l a m 2 . 2 . 1扩展卡尔曼泣波s l a m 的描述 1 9 6 0 年， r e . k a l m a n 在其发表的一 篇著名的论文2 2 1 中 ， 描述了 用递归 的方法 来解决离散数据的线 性滤波问 题，这便是卡 尔曼滤 波的 雏形。简单来讲，我们 可以 把卡尔曼滤波看作 是一组数学方程式， 通过方 程式来解决过程中的 状态估 计问题。 这种方法不仅 可以对过去、现在的 状态进 行估计，而且可以 预测未来 的 状态， 所以它 在机 器人自 主或辅助导航 研究中占 有很重要的 位置2 2 7 2 41 第二章 s l a m简介 在勺a c h e 和f a u g e r a s 25 1 以 及c h a tila 和l a u m o n d (261 工 作 的 基 础 上 ， s m it h , s e l f 和 c h e e s e m a n的论文中 1o r n w 首次 提出了 基于扩展卡尔曼滤 波的 随机建图方 法， 揭开了s l a m问题研究的序 幕。 近 几年来， e k f 方法成为当 下最为流行的 s l a m工具。 s l a m 算法中，要根 据从传感器 抽取的有 用信息 对系统的 状态进行估计。 e k f 可以 通过预测、 观测、 更新三个步 骤来实 现对系统的 状态估计。 在( 2 . 4 ) ( 2 . 8 ) 中描 述了 系统状态与环境特征的关系的 两个非线性模型，其中其中v ( k ) 和w ( k ) 分别 表示机器人 运动 不确定性和传感 器观测不 确定 性， 它们都是均 值为 0的 正 态白 噪声， 如果分别 用q ( k ) 和r ( k ) 表 示两者的 协方差， 则有: e v ( k ) = e w ( k ) = 0 e v (k ) _v r (k )l = q ( k ) e w ( k ) w ( k ) = r ( k ) 由 于s m it h 等认为不确定 性是 空间关 系内 在的固有性 质， 因 此环境地图不 仅 包括机器人和环境 特征的坐标， 还包括 一个描述机器人与 环境特征 之间关 系以 及上述两种不确定性的协方差阵 p ( k i k ) = 凡( k i k ) 几( k i k ) p r ( k ik )毛( k i k ) ( 2 . 1 0 ) p ( k i k ) 的 引 入 是e k f 方 法 最 大的 特 点 ， 对 它的 计 算 是e k f 算 法 的 核 心. p ( k i k ) 随着估计校正的不 断进行而成递减趋势。 此外，p ( k i k ) 只描 述了 系统状态之间 的协方差， 而观测噪声的协方差是先验的。 若系统状态之间 的协方差较大，则 表示系统偏差较大，应使用观测值尽可能地校正，若观测噪声的协方差较大， 则表示观测数据不可靠，只使用观测值做轻微校正。 应用运 动模型( 2 . 4 ) 和观测模型( 2 . 8 ) ， 基于e k f的 建图与定位 可以归 纳为 一个循环迭 代的 估计一校正过程。 首先 通过不 含噪声项v ( k ) 的 模型( 2 . 4 ) 预测机 器人的新位置，即 x ( k + 1 i k ) = f (1 ( k i k ), . ( k ) ,0 ) (2 . 1 1 ) 之所以 不对噪声 项进行预测是因为噪声 是随机且不可预知的。 对协 方差矩阵的 预测表示为: p ( k + i i k ) = o x f - p (k i k ) -o x r f + v j- q ( k ) - 0 . f (2 . 1 2 ) 通过不含噪声项w ( k ) 的( 2 . 8 ) 预测可能观 测的环境 特征 z ( k + 1 i k ) = h (x ( k + 1 i k ) ,0 ) ( 2 . 1 3 ) 第二章 s l a m简介 其中 - u , ( k ) . t . s i n ( b , ( k l u , ( k ) - t - c o s ( 0 , ( k i 1 ，! ” 一- f vx 由 于 ( 2 . 4 ) 中 用的 是 加 性 噪 声 ， 因 此v , f = r ( 3 x 3 的 单 位阵 ) 。 然 后 计 算实 际 观 测和预 测观测间的误 差， 综合系统 协方差 计算卡尔曼滤波参 数w，并用w对前 面估计的机器人位置进行校正，最后将新观测的环境特征加入 地图。 机器人移 动过程中循环不断地估计一校正，尽量消除累积误差，得到尽可能准确的定位 信息。 更新的 过程如下: x ( k + l l k + l ) = x ( k + l l k ) + w (k + l ) - y ( k + l ) (2 . 1 4 ) p ( k + l l k + l) = p (k + l k ) 一 w (k + l ) . s , ( k + l lk ) - w t ( k + l ) (2 . 1 5 ) 其中 y (k + 1 ) = z (k + 1) - h ( x ( k + 1 i k ) ,0 ) (2 . 1 6 ) 今( k + l l k ) = v x h - p (k + 1 1 k ) v x t h + r ( k + l ) ( 2 . 1 7 ) w ( k + 1 ) = p ( k + l l k ) . v x t h -今一 ， ( k + 1 i k ) ( 2 . 1 8 ) v x h ( x ( k + 1 1 k ) , 0 ) = (v x , h , 0 . . 0 , v x h ， 0 . . 0 1 ( 2 . 1 9 ) n 9户 2. ，.1.esj 0-l 之 ( k + l l k ) 一 元 ( k + 1 1 k ) v x , h = 文( k + l k ) 一 交 ( k + 1 1 k ) 试 交 ( k + l k ) 一 丸 ( k + 1 i k ) 试 2 ( k + 1 i k ) 一 元 ( k + l l k ) 象 ( k + l k ) 一 元 ( k + 1 i k ) 试 2 .x，一内y-.荞-.(- ，一1，几y- r.es卫eel工|匕weesesesesesesesesesl v x h = d , k + l l k ) 一 y , ( k + 1 1 k ) 又( k + l l k ) 一 又 ( k + l l k ) 试 s , ( k + l l k ) 一 兔 ( k + 1 i k ) 试 2 ( 2 . 2 1 ) 对于两轮机器人， 在第一节中己 给出 相应的 运动模型 和观测模型， 陆标的状态是静止的，结合相应的陆标模型，将预测的步骤具体化， x , ( k + l ! k ) = f ( x , ( k i k ) , u ( k ) ) 并且假定 就成为: ( 2 . 2 2 ) 第二章 s l a m简介 ! k ) + u , ( k ) - t - c o s ( b , ( k i k ) + u 2 ( k ) . k ) + u , ( k ) - t - s i n ( b , ( k i k ) + u 2 ( k ) b , ( k i k ) + u 2 ( k ) . t t )1 71 (k(k .xry. r.es.j - ，.，.j k)k)k) 通.二j.1.l + (k(k(k .xry.珠 一11.esj x . ( k + l k ) = x ( k 】 k ) 于是( 2 . 1 1 ) 就转化成( 2 . 2 2 ) 和( 2 . 2 3 ) .同样的，( 2 . 1 3 ) 转化成 ( 2 . 2 3 ) ， 、 一 ，无，= 一 几( k + l l k ) 凡( k + 1 i k ) (s , ( k + 1 i k ) 一 z , ( k + l l k ) 2 + ( y , ( k + l l k ) 一 j ,( k + 1 1 k ) )2 交( k + l l k ) 一 y , ( k + l j k ) a r c (劲+ 1 亩- .x ,( k + l l k ) ) 一 b , (