一种室内环境下机器人同时定位与地图构建方法

维普资讯 http://www.cqvip.com ２００７年第７期　中图分类号：ＴＰ２４２　文献标识码：Ａ　文章编号：１００９—２５５２（２０ｃｒ７）Ｏｒ７一ＯＯＯ４—０４　一种室内环境下机器人同时定位与地图构建方法　申丽曼，蔡自兴　（中南大学信息科学与工程学院，长沙４１００８３）　摘要：介绍了一种移动机器人室内环境下同时定位与地图构建方法，给出了整个系统的结构　框图。将从激光传感信息中提取出的直线特征作为主要的环境描述特征，用ＥＫＦ算法更新机器　人位姿和直线特征的估计值，用一种新的数据关联方法以实现地图的最小描述。直线特征提取　采改进的ＩＥＰＦ算法，能够快速地从数据点中得到较为精确的直线参数。实验证明整个系统具有　良好的性能。　’　关键词：移动机器人；同时定位与地图构建；扩展Ｋａｌｍａｎ滤波；ＩＥＰＦ　Ａｎ　ａｐｐｒｏａｃｈ　ｆｏｒ　ｍｏｂｉｌｅ　ｒｏｂｏｔ　ＳＬＡＭ　ｉｎ　ｉｎｄｏｏｒ　ｅｎｖｉｒｏｎｍｅｎｔ　ＳＨＥＮ　Ｕ－ｍａｎ，ＣＡＩ　Ｚｉ－ｘｉｎｇ　（Ｓｃｈｏｏｌ　ｏｆ　Ｉｎｆｏｒｍａｔｉｏｎ　Ｓｃｉｅｎｃｅ　ａｎｄ　Ｔｅｃｈｎｏｌｏｇｙ，Ｃｅｎｔｒａｌ　Ｓｏｕｔｈ　Ｕｎｉｖｅｒｓｉｔｙ，Ｃｈａｎｇｓｈａ　４１００８３，Ｃｈｉｎａ）　Ａｂｓｔｒａｃｔ：１１１ｉｓ　ａｒｔｉｃｌｅ　ｐｒｏｐｏｓｅｓ　ａｎ　ａｐｐｒｏａｃｈ　ｆｏｒ　ｍｏｂｉｌｅ　ｒｏｂｏｔ　ＳＬＡＭ　ｉｎ　ｉｎｄｏｏｒ　ｅｎｖｉｒｏｎｍｅｎｔ．Ｔｈｅ　ｗｈｏｌｅ　ｓｙｓｔｅｍ　ｆｒａｍｅ　ｉｓ　ｓｈｏｗｅｄ　ｉｎ　ｉｔ．Ｔｈｅ　ｌｉｎｅ　ｆｅａｔｕｒｅ　ｅｘｔｒａｃｔｅｄ　ｆｒｏｍ　ｌａｓｅｒ　ｄａｔａ　ｓａ　ｔｈｅ　ｍａｉｎ　ｆｅａｔｕｒｅ　ｔｏ　ｄｅｓｃｒｉｂｅ　ｈｔｅ　ｇｅｏｍｅｔｒｉｃａｌ　ｍａｐ　ｉｓ　ｃｈｏｓｅｎ．Ｉｔ　ｕｓｅｓ　ＥＫＦ　ａｌｇｏｒｉｔｈｍ　ｔｏ　ｕｐｄａｔｅ　ｔｈｅ　ｅｓｔｉｍａｔｉｏｎ　ｏｆ　ｔｈｅ　ｒｏｂｏｔ’Ｓ　ｐｏｓｅ　ａｎｄ　ｔｈｅ　ｐａｒａｍｅｔｅｒ　ｏｆ　ｔｈｅ　ｌｉｎｅ　ｆｅａｔｕｒｅ，ａｎｄ　ａｌｓｏ　ｐｒｏｐｏｓｅｓ　ａｎ　ｎｅｗ　ａｐｐｒｏａｃｈ　ｆｏｒ　ｄａｔａ　ａｓｓｏｃｉａｔｉｏｎ　ｔｏ　ｍｉｎｉｍｉｚｅ　ｔｈｅ　ｃｏｍｐｕｔａｔｉｏｎ　ｃｏｍｐｌｅｘｉｔｙ．　Ａｎ　ａｄｖａｎｃｅｄ　ＩＥＰＦ　ａｌｇｏｒｉｔｈｍ　ｉｓ　ｃｈｏｓｅｎ　ｆｏｒ　ｔｈｅ　ｌｉｎｅ　ｆｅａｔｕｒｅ　ｅｘｔｒａｃｔｉｏｎ．Ｔｈｅ　ｅｘｐｅｒｉｍｅｎｔｓ　ｄｅｍｏｎｓｔｒａｔｅ　ｔｈｅ　ｅｆｆｅｃ－　ｔｉｖｅｎｅｓｓ　ｏｆｔｈｅ　ｐｒｏｐｏｓｅｄ　ＳＬＡＭ　ｔｅｃｈｎｉｑｕｅ．　・　Ｋｅｙ　ｗｏｒｄｓ：ｍｏｂｉｌｅ　ｒｏｂｏｔ；ｓｉｍｕｌｔａｎｅｏｕｓ　ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　ｍａｐｐｉｎｇ；ｅｘｔｅｎｄ　Ｋａｌｍａｎ　ｆｉｌｔｅｒ；ＩＥＰＦ　Ｏ　前言　复杂度高的主要原因。目前的很多研究也是围绕降　移动机器人同时定位与地图创建问题是自主机　低ＥＫＦ的计算量来进行的。Ｌｅｏｎａｒｄ和Ｆｅｄｅｒ提出一　器人导航中的一个重要研究课题。它是让机器人在　种分离随机地图（ＤｓＭ）的方法来解决这个问题…，　未知的环境中，从一个未知的位置出发，增量式地创　Ｄｉｓｓａｎａｙａｋｅ等提出了一种删除无用路标的方法以减　建地图，同时利用地图信息进行机器人自定位。现　少计算量＿１　Ｊ。这几种方法实质上都是用减少环境特　在已经有很多学者提出了ＳＬＡＭ的解决方法，这些　征的数量来降低计算复杂度。本文以减少地图中的　方法的主要区别在于环境表示方式和不确定性描述　重叠特征来降低计算量。为了对环境进行最小描　方式两方面。大多数定位和地图构建方法将环境特　述，采用一种更具约束力和有效性的数据关联算法。　征看成静态的路标点，在室内环境中，环境特征多为　由于机器人ＳＬＡＭ的ＥＫＦ预测和更新过程很多论文　直线，相对于点特征，直线特征信息量大，数量少。　中都有大致介绍，本文就不再赘述，下面将着重介绍　更有利于机器人定位和降低计算复杂度，故本文选　地图处理中数据关联及融合。　择直线特征为环境描述的主要特征。　ＥＫＦ是目前ＳＬＡＭ问题所采用的最普遍的方　收稿日期：２００６—１１—０６　法，由于状态协方差矩阵的所有元素在每次观测后　基金项目：国防基础科研基金重点项目（Ａ１４２００６０１５９）　作者简介：申丽曼（１９８２一），女，中南大学信息科学与工程学院模式　都需要更新，当机器人探测范围增大，计算量随着观　识别与智能系统专业硕士，主要研究方向为未知环境下　测到的环境特征的增加而急剧增加，这是ＥＫＦ计算　机器人环境建模及定位。　一４——　维普资讯 http://www.cqvip.com １　改进的ＩＥＰＦ直线特征提取　ＩＥＰＦ是一种传统的直线提取算法，它利用拆分　的思想将点集划分成不同的集合。相比直线回归　（Ｌａ）、ｈｏｕｇｈ变换、ＥＭ等方法，ＩＥＰＦ具有速度快，计　算量低等优点，所以在时时性要求很高的机器人定　位系统中，Ⅲ＝ＰＦ算法是较好的选择。该算法获取的　直线参数多用最小二乘直线拟和法实现，当传感器　误差和噪声较大时，直线提取的精确性难以保证。　本论文将模糊聚类算法加入其中，提出了一种改进　的ＩＥＰＦ算法。　．　１．１模糊聚类算法、　直线提取算法的目标是找出一条直线，使构成直　线的数据点到该直线的距离平方和最小，假设有Ｊ７ｖ　个数据点可以提取出Ｍ条直线。则可得到以下函数：　Ｍ　Ｎ　Ｄ（ｚ，　，Ｐ）＝∑∑　；ｄ　（暂，　，ｌｉ）　（１）　ｉ＝１　＝１　其中，ｚ　代表第ｉ条直线的极坐标参数（ｒ　，仇），　；为　点集Ｐ中第　个点到第ｉ条直线　的权值，它满足以　下条件：　∈［０，１］，对于所有的直线有０＜∑　＜Ｎ。』＝１　　肼　，　对于所有的点有∑　＝ｌｏｄ（暂，竹，ｌｉ）为第　个点到　ｉ＝１　第ｉ条直线　的距离：ｄ２（ｘ￣，　，ｚ　）＝Ｒ　（巧，竹，ｆ１）＋　Ｚ　（　，，ｙ『，Ｚ　），由极坐标中点到直线的距离公式可得：　Ｒ　（巧，竹，ｚ　）＝（　—ｘｉｃｏｓ（仇）一ｙｊｓｉｎ（仇））　。其中　Ｚ　（　，，ｙ『，　）为于直线距离很远的点的惩罚函数。　对（１）式采用拉格郎日乘数法计算∞　以及（ｒ　，　），可得：　１　ｋ＝ｌ　ｄ　２（ｘｊ，ｙｊ，ｌｋ）　＝　ｃｏｓ（仇）＋多　ｓｉｎ（仇）　’ａｒｃｔａｎ（ｌ熹］　ｊ　其中：　∑　；　∑　；　　・＝　一一，Ｙ；＝　～　∑　∑　；　＝∑　；（巧一　；）　，　＝∑　；（　一ｙ　）　＝∑　；（巧一互　）（　一Ｙ—ｉ）　（１９　为第　点到第ｉ条直线的权值，ｄ为点到直线的距　离，（　仇）为第ｉ条直线的极坐标参数，（巧，　）为　第　点的坐标。　１．２　ＡＩＥＰＦ算法步骤　　‘ｓｔｅｐｌ初始化：用二乘法计算初始点集Ｐ中所有　点匹配的直线Ｚ，将其加入￡。　ｓｔｅｐ２对于　中每条直线ｚ　，ｉ＝１，…，／７，¨计算　￡（属于该直线的数据点到该直线距离的平方和），　若￡＞　一，则用直线数为２的模糊聚类算法将　拆　分为两条直线　和　；用公式（２）计算直线参数。　用　代替ｚ　加入　，并将　加入　的尾部；计算点　集中的点到　和　的距离，找出距离较小的直　线，将卿加入该直线对应的点集中。　ｓｔｅｐ３若　中至少有一条直线ｚ　，ｉ＝１，…，ｎ　有　色＞　一，转到ｓｔｅｐ２。　２　基于直线特征的ＳＬＡＭ　２．１　问题描述　设系统在ｋ时刻的状态向量为　（ｋ），它由机器　人位姿向量ＸＲ（　）和地图中存在的直线特征的　（ｋ），ｉ＝１，…，嘶组成（嘶为地图中直线的数目）。　其中：　蜀（ｋ）＝［　（ｋ），Ｙ　（ｋ），　（ｋ）］　，　（ｋ）＝［　，仇］　，ｉ＝１，…，ｍｒ　系统动态模型：　（ｋ＋１）＝Ｆ（ｋ）（　（ｋ），　（ｋ＋１））＋Ｗ（ｋ＋１）　ｚ（ｋ）＝厂（ｋ）（　（ｋ））＋　（ｋ）　其中，１１，（ｋ）为ｋ时刻机器人的里程计信息，Ｗ（ｋ）为　影响里程计信息的噪声，１３（尼）为影响直线参数计算　的噪声。ｆ（ｋ）为系统对直线的观测模型：　觚㈤，ｌｉ，＝－ｒ．＋ｘｎ　（ｋ　或：　（　（后），ｚ；）：【　—　Ｒ　ｋ　）　ｃｏｓ￣　ｔｉ－ＹＲ　—（后）尼　ｓｉｎ　］　基于直线特征的ＳＬＡＭ问题的目标是给定初始　估计状态向量　（０），根据系统的动态模型计算ｋ时　刻机器人位姿和直线参数的估计值，即系统状态　（　）的估计　（ｋ）。系统框图如图１所示。　维普资讯 http://www.cqvip.com Ｌ　一＞Ａｄｄ（ｏｊ）；　ｇｏｔｏＯ；　其中，权∞　，　，　分别为０　与Ｚ　的△　，ｄ，ｒ的方差　的倒数，　和　均为一个临时的存储队列，Ｅ为一　个花费函数。　若ｚ　和Ｏｊ满足以上条件，则判定为完全匹配。就　要用　和ｏｉ的参数对全局中的线段　进行更新。若　没有与估计量匹配的直线，并不直接将其作为新特　征加入地图，而是将其暂存在一个临时队列中进行　特征跟踪，若连续３个ＳＬＡＭ样本时间内都没有找到　匹配的直线，才将其作为新特征加入地图。　图１　ＳＬＡＭ系统框图　新加入地图的直线根据机器人当前位姿估计初　２．２数据关联与地图更新　始参数：　在移动机器人地图构建的过程中，有两种情况　ｔ＋。（ｋ　Ｉ　）：　川）（ｋ　Ｉ　），　‰＋１）（ｋ　Ｉ　）］　＝　需要进行数据融合：一种情况是机器人在自身坐标　日（　（ｋ　Ｉ　ｋ），　ｋ））　系下得到的观测量转换到全局坐标下对全局地图进　一ｌＤ（ｋ）＋互Ｒ（ｋ　Ｉ　ｋ）ｃｏｓ（￣（ｋ）＋　行更新时，对同一物体表面的全局特征描述和局部　】＿　（ｋ　Ｉ　ｋ）一７ｃ）＋　Ｒ（ｋ　Ｉ　ｋ）ｓｉｎ　特征描述也需要进行关联，合并，以便进行地图更新　（　（ｋ）＋　（ｋ　Ｉ　ｋ）一７ｃ）　（ｋ）＋　或：　以及系统协方差更新等操作；另一种情况是很小的　精度容易导致对同一物体表面的多条线段描述，为　（ｋ　Ｉ　ｋ）一７ｆ　—了在保证线段精度的条件下对环境进行最小描述，　ｌＤ（ｋ）＋互Ｒ（ｋ　Ｉ　ｋ）ｃｏｓ（￣（ｋ）一　需要对相邻线段进行合并处理。　】＝　（ｋ　Ｉ　ｋ））＋　（ｋ　Ｉ　ｋ）ｓｉｎ　对于第一种情况，全局地图中的线段ｆｌ和局部　（　（ｋ）一　（ｋ　Ｉ　ｋ））　（ｋ）一　估计量Ｏｊ存在以下三个关联量：角度差△　，垂直距　（ｋ　Ｉ　ｋ）　离ｄ以及与０　的重叠率ｒ。以往的判别法是设定这　其中，日为新加入直线的初始估计函数。（１Ｄ（ｋ），　三个关联量的阈值，在阈值以内就判定直线为匹配，　（ｋ））为所提取直线在机器人坐标下的极坐标方　将其直接插入地图。但是阈值的确定非常困难，阈　程。根据机器人和新加入直线的相对位置更新协方　值过大可能造成一个估计量与多条直线匹配的情　差矩阵：　况。阈值过小又可能将估计量误判为新特征加入地　　，『－ｐｏ（ｋ　Ｉ　ｋ）　（ｋ　Ｉ　ｋ）Ｖ，　１　＋－图。本文采用以下方法来解决这个问题：　（　。　）　【　（　Ｉ　）　（　Ｉ　），　＋　（　，　Ｊ　①ｆｏｒ（ｉ＝１　ｔｏ　ｎ）（ｎ　ｉｓ　ｔｈｅ　ｎｕｍｂｅｒ　ｏｆａｌｌ　ｌｉｎｅｓ　ｉｎ　ｔｈｅ　其中，＾＝８Ｈ／ＳＸｏ，　＝８Ｈ／ａＺｏ　ｇｌｏｂａｌ　ｍａｐ）　第二种情况的处理对象是全局地图中的直线特　ｉｆ（　＝三三ｔｈ一△　＆ｄ　ｔｈ—ｄ＆＆ｒ　ｔｈ—ｒ）　征，尽管以上数据关联方法能尽量小的减少地图中　ｆｏｒｌ　重叠特征出现，但是以下两种情况还需考虑：①当某　Ａｄｄ（１　）　平面长度很大，而机器人又未能对该物体进行完全　②ｆｏｒ　ａｌｌ　ｌｉｎｅｓ　ｉｎ　Ｌ　探测，代表同一平面的直线特征之间可能出现脱节。　ｉｆ（Ｅ（△（ｐ，ｄ，ｒ）＝　△　＋（ｏＳ＋（Ｏｒ　ｆｏｒ　ｌ，ｉｓ　②当机器人对一个特征重复探测时可能出现代表同　ｈｔｅ　ｌｏｗｅｓｔ　ｏｆ　ａｌ１）　一平面的两条不同的直线。在实验中采用周期性地　ｕｐｄａｔｅ（１ｉ）ｗｉｈｔ　ｐａｒａｍｅｔｅｒｓ　ｏｆ　ｏｊ；　对系统状态向量进行检测来避免这几种状况的发　③ｆｏｒ（ｊ＝０　ｔｏ　ｍ）（ｍ　ｉｓ　ｔｈｅ　ｎｕｍｂｅｒ　ｏｆ　ｔｈｅ　ｎｅｗ　ｌｉｎｅｓ　生，时间周期选择为两个ＳＬＡＭ样本时间２ＡＴ，当存　ｉｎ　ｌｏｃａｌ　ｍａｐ）　　．在两直线角度和距离均小于给定的阈值就将它们都　ｉ　Ｌ＝０）　加入可疑特征队列，Ｊ…　…。再用Ｍａｈａｌａｎｏｂｉｓ距离　——６——　　’维普资讯 http://www.cqvip.com 判定直线是否匹配，若　ｚ　是匹配的，则利用他们　之间的距离和ＥＫＦ更新过程来更新ｆ　的参数，并且　将ｚ　从地图中删除。　３　买验　本文实验以美国ＡｃｔｉｖＭｅｄｉａ公司的Ｐｉｏｎｅｅｒ２一　Ｄｘｅ型移动机器人为实验平台，该平台装备有德国　ＳＩＫＣ公司ＬＭＳ２００型激光测距仪，ＣＣＤ图象传感器、　里程计和全方位碰撞传感器。机器人系统具有良好　的可扩展性和可靠性，为开展自主机器人的各种应　用研究提供了适宜的研究平台和有力的技术支持。　３．１系统模型　图２实验环境结构　Ｒ＝Ｖｃｏｓ（０Ｒ＋　）ｒ　Ｒ（ｋ＋１）］　Ｒ＝Ｖｓｉｎ（０Ｒ＋　），Ｉ　ＹＲ（ｋ＋１）ｌ—　Ｒ＝Ｖｓｉｎ２／Ｌ　Ｌ　（ｋ＋１）Ｊ　ｒ　Ｒ（ｋ）＋ＡＴＶ（ｋ）ｃｏｓ（０Ｒ（ｋ）＋　（ｋ））］　Ｉ　ｙＲ（ｋ）＋ＡＴＶ（ｋ）ｓｉｎ（　（居）＋　（　））Ｊ，　Ｌ　（ｋ）＋ＡＴＶ（ｋ）ｓｉｎ（　（ｋ））／　ｊ　『　（ｋ＋１）１　『　（ｋ）１　Ｌ　Ｙ　（ｋ＋１）Ｊ　Ｌ　Ｙ　（ｋ）Ｊ．　其中，　为机器人行走速度；　为机器人前轮的旋转速　图３实验结果　度；Ｌ为机器人机身的长度；ＡＴ为ＳＬＡＭ样本时间。　３．２实验结果及分析　４　结束语　本文介绍了一个室内环境下基于直线特征同时　选取的实验环境为一个简单的室内结构化环　境，如图２所示，灰色方框为机器人探测的环境中的　物体，机器人以０．４ｍ／ｓ的速度行走。总的行走距离　定位与地图构建系统。提出一种新的直线特征提取　方法ＡＩＥＰＦ和一种有效的数据关联方法以使构建的　地图能对环境最小描述。实验数据证明该方法的有　为１５ｍ。数据关联中选择的阈值分别为ｔｈ一△　＝　５。，ｔｈ—效性。下一步的工作即将单个机器人定位和建图扩　展到用多个机器人对大范围环境进行协作定位和地　图构建，其中包括机器人之间的协作策略以及多个　子地图融合成全局地图等方面的研究。　参考文献：　［１］Ｌｅｏｎａ￣Ｊ　Ｊ，ＦｅｄｅｒＨ　Ｊ　Ｓ．Ａ　ｃｍ￣ｍｆｉｏｎｌａｌｙ　ｅｆｉｆｃｉｅｎｔ　ｆｏｒｌ孵一　ｓｃａｌｅ　ｃｏｎｏＪｒｒｅｎｔ　ｍａｐｐｉｎｇ　ａｎｄ　ｌｏｃａｌｉｚａｉｔｏｎ［Ｃ］／／Ｋｏｄｉｔｓｃｈｅｋ　Ｄ，Ｈｏｌｌｅｒ—　ｂａｃｈ　Ｊ（ｅｄｓ），Ｎｉｎｔｈ　Ｉｎｔｅｍａｔ．Ｓｙｎｒｐｏｓｉｕｍ　ｉｎ　Ｒｏｂｏｔｉｃｓ　Ｒｅｓｅａｒｃｈ，　Ｓｎｏｗｂｉｒｄ，ＵＴ，Ｓｐｒｉｎｇｅｒ，２０００－＂１６９—１７６．　ｄ＝３ｃｍ，ｔｈ—ｒ＝０．０２。图３为实验效果图，图　中褐色方点是由里程计估计的机器人路径，灰色圆　点是由ＳＬＡＭ算法得到的机器人路径，由图４中可　以看出，机器人在第一次转弯时，由里程计得到的机　器人位置开始明显偏离实际位置，最终里程计的误　差累计达到４ｍ，从机器人最后的位置和方位误差看　出，利用ＳＬＡＭ算法对里程计误差进行补偿的误差　分别小于０．０７ｍ和０．５度。　［２］ＤｉｓｓａｎａｙａｋｅＭ　ＷＭＧ，Ｗｉｌｌｉａｍｓ　ＳＢ，Ｄｕｒｒａｎｔ—ＷｈｙｔｅＨ，ｅｔａ１．Ｍａｐ　ｍａｎａｇｅｍｅｎｔ　ｆｏｒ　ｅｆｉｃｉｆｅｎｔ　ｓｉｍｕｌｔａｎｅｏｕｓ　ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　ｍａｐｐｉｇ　ｎ如图３所示，ＳＬＡＭ算法所估计的路径最终构　建的地图包含了３０条直线特征，而用过去的数据关　联方法得到了４２条直线。跟踪队列中存储了１８条　（ＳＬＡＭ）［Ｊ］　Ａｕｔｏｎｏｍ．Ｒｏｂｏｔｓ　１２，２ＯＯ２：２６７—２８６　［３】Ｉｐ　Ｙ　Ｌ，ＲａｄＡｆ；，Ｃｈｏｗ　ＫＭ，ｅｔａｌ　Ｓｅｇｍｅｎｔ—ｂａｓｅｄｍａｐ　ｂｕｉｌｄｉｎｇ　ｕｓｉｇ　ｅｎｈａｎｃｎｅｄ　ａｄａｐｔｉｖｅ缸　ｃｌｕｓｔｅｒｉｎｇ　ａｌｇｏｒｉｔｈｍｆｏｒｍｏｂｉｌｅ　ｒｏｂｏｔａｐ—　直线特征，其中有１５条在之后的跟踪过程中找到了　匹配的直线，而没有误将其作为新特征加入。但是　从图中也可以看出，在每个转角处的直线还不够精　确，容易出现匹配失败造成直线重叠的情况。但总　的来说，该方法具有较好的精确性，也能满足ＳＬＡＭ　所需的时时性要求。　ｐｌｉｃａｔｉｏｎｓ［Ｊ］．Ｉｎｔｅｌｌｉｇｅｎｔ　ｏｂｏＲｔｉｃ　ｓｙｓｔｅｒ｜ｌ８　３５，２００２：２２１—２４５　［４］Ｄｉｓｓｎａａｙａｋｅ　Ｍ　Ｗ　Ｍ　Ｇ，Ｗｉｌｌｉａｍｓ　Ｓ　Ｂ，Ｄｕｒｒａｎｔ—Ｗｈｙｔｅ　Ｈ，ｅｔ　ｌａ　Ｍａｐ　ｍａｎａｇｅｍｅｎｔ　ｆｏｒ　ｅｆｆｉｃｉｅｎｔ　ｓｉｍｕｌｔａｎｅｏｕｓ　ｌｏｃａｌｉｚａｔｉｎ　ａｎｄ　ｍａｐｐｉｏｇ　ｎ（ＳＬＡＭ）Ｎ］．Ａｕｔｏｎｏｍ．Ｒｏｂｏｔｓ　１２，２００２：６７—２８２６．　［５］蔡自兴．人工智能［Ｍ］．清华大学出版社．　责任编辑：么丽苹　——　７——