移动机器人在未知环境中同时定位与地图构建的方法

摘要

本发明公开了一种移动机器人在未知环境中同时定位与地图构建的方法。包括如下步骤：1)根据移动机器人上所安装测距传感器获得的数据，构建当前观测到的局部占用栅格地图和局部线段特征地图；2)根据测距传感器数据、航位推测传感器数据、上一周期对移动机器人的位姿估计和前期构建得到的全局线段特征地图，估计当前周期移动机器人的位姿；3)根据对当前周期移动机器人的位姿估计、构建得到的局部线段特征地图和局部占用栅格地图，更新全局占用栅格地图和全局线段特征地图。本发明构建得到线段特征地图和占用栅格地图可供各类应用机器人用于路径规划和导航，免除人工为移动机器人输入环境地图的工作，可有效提高各类机器人对未知环境的认知能力。

说明书

技术领域

本发明涉及一种移动机器人在未知环境中同时定位与地图构建的方法和系统，它们可用于移动机器人在未知环境的任意一点出发，根据测距传感器和航位推测传感器的感知数据估计机器人位姿并构建环境地图。

背景技术

随着移动机器人应用范围的逐渐扩展，如何使机器人自主认知未知环境成为近年来机器人和人工智能领域中的一个研究热点。机器人自主认知未知环境的方式之一就是构建未知环境的模型或者地图，即由机器人根据传感器测量信息自主构建其所在环境的空间模型或地图(参见“S.Thrun.Robotic mapping：Asurvey.In G.Lakemeyer and B.Nebel，editors，Exploring Artificial Intelligence in theNew Millenium.Morgan Kaufmann，2002.”及“陈卫东，张飞.移动机器人的同步自定位与地图创建研究进展。控制理论与应用，22(3)：455-460，2005.”)。通过构建得到的环境地图，机器人可以进行任务规划、路径规划、并执行各种作业。

早期的未知环境地图构建假设机器人移动过程中的所有位姿可以由航位推测传感器精确获得，由此提出了根据测距传感器信息构建各类环境地图的方法，包括基于布尔贝叶斯滤波构建占用栅格地图的方法(参见“A.Elfes.Sonar-basedreal-world mapping and navigation.IEEE Journal of Robotics and Automation，RA-3(3)：249-265，1987.”)和基于卡尔曼滤波构建特征地图的方法(参见“K.S.Chong，L.Kleeman.Mobile-robot map building from an advanced sonar arrayand accurate odometry.International Journal of Robotics Research，18(1)：20-36，1999.”)。

然而航位推测传感器所获得的机器人位姿信息并不是确定的，相反它存在着统计上相关的测量误差，即误差会随着时间积累。但传统的机器人位姿估计，如Markov定位法和Monte Carlo定位法，总是假设地图已知(参见“D.Fox，W.Burgard，and S.Thrun.Markov localization for mobile robots in dynamicenvironments.Journal of Artificial Intelligence Research，11：391-427，1999.”和“S.Thrun，D.Fox，W.Burgard and F.Dellaert.Robust Monte Carlo localization formobile robots.Artificial Intelligence，101：99-141，2000.”)。

可见，在未知环境地图构建中，由于机器人用于感知外部环境信息和自身移动信息的传感器测量数据中都存在着不可避免的测量噪声，使得机器人定位和环境地图构建中均存在不确定性，且这些不确定性相互关联、相互影响。在根据机器人位姿将当前观测到的信息加入到所构建的地图中时，会将机器人定位的不确定性带入到所构建的地图中，使所构建的地图出现偏差；在根据当前观测与已构建地图的匹配关系估计机器人位姿时，当前观测和已构建地图中的不确定性又会使机器人定位出现偏差。因此未知环境地图构建必须要同时解决机器人定位和地图构建问题。

现有的同时定位与地图构建方法有扩展卡尔曼滤波法、期望值最大化法、粒子滤波法。基于扩展卡尔曼滤波的同时定位与地图构建方法(参见“J.A.Castellanos，et al.The SPmap：A probabilistic framework for simultaneouslocalization and map building.IEEE Transactions on Robotics and Automation，15(5)：948-953，1999.”)中地图由一系列陆标组成，算法维护着机器人位姿和所有陆标位置的后验概率。该类方法可以实现在线地图构建，但由于采用了高斯噪声的假设，使得它要求陆标能够被充分地相互区别，陆标的缺失、陆标识别的误差和匹配错误都将导致算法的失败。基于期望值最大化的同时定位与地图构建方法(参见“W.Burgard，et al.Sonar-based mapping of large-scale mobile robotenvironments using EM.in Proc.of the International Conference on MachineLearning，Slovenia，1999，pp.67-76.”)通过不断地重复两个步骤来逐步提高地图的精确性，一个是根据给定的地图计算机器人位姿的后验概率，另一个是根据位姿期望值计算出最可能的地图。该方法可以避免由于陆标识别误差和匹配误差导致地图构建失败，但由于它在所有地图的空间中执行爬山法，因而无法实现在线地图构建。基于粒子滤波的同时定位与地图构建方法(参见“M.Montemerlo and S.Thrun.Simultaneous localization and mapping with unknowndata association using FastSLAM.in Proceedings of the 2003IEEE InternationalConference on Robotics & Automation，Taipei，Taiunn，2003，pp.1985-1991.”)是将粒子滤波和扩展卡尔曼滤波结合起来，它采用粒子滤波估计机器人的位姿，每一个粒子表示一个可能的机器人路径，同时维护着一个由该路径确定的地图，地图中陆标位置的估计采用扩展卡尔曼滤波法。该方法通过多假设解决了定位与地图构建不确定性相互影响的问题，但由于为每个粒子维护一张相应的地图，需要消耗较大的存储空间和计算资源。

中国发明专利第00816243.3号提出了一个多机器人协作系统，由一个或多个导航机器人完成环境的绘图、自身定位、功能机器人定位和功能机器人任务分配，其中导航机器人在定位自身时采用了陆标识别和推算算法。由于轮子滑动或失调的因素，推算算法的错误会随着时间累积。为了弥补这个误差，该方法在定位时结合了陆标识别。但由于它将系统中的功能机器人作为陆标，使得该方法并不适用于单个在未知环境中作业的机器人，特别是当当前观测中不存在陆标时，机器人定位将出现较大不确定性。

中国发明专利第200410100518.9号所提出的环境识别设备及方法主要面向多平面环境(即楼梯)，并没有考虑机器人定位的不确定性问题。

因此有必要开发一种既能实现准确的机器人位姿估计和地图构建、又能实时在线运行、对存储空间和计算资源要求小、并且所构建得到的地图能够提供详细的环境信息、适用于机器人导航等作业的同时定位与地图构建方法。

发明内容

本发明的目的是提供一种移动机器人在未知环境中同时定位与地图构建的方法。

移动机器人在未知环境中同时定位与地图构建的方法包括如下步骤：

1)根据移动机器人上所安装测距传感器获得的数据，构建当前观测到的局部占用栅格地图和局部线段特征地图；

2)根据测距传感器数据、航位推测传感器数据、上一周期对移动机器人的位姿估计和前期构建得到的全局线段特征地图，估计当前周期移动机器人的位姿；

3)根据对当前周期移动机器人的位姿估计、构建得到的局部线段特征地图和局部占用栅格地图，更新全局占用栅格地图和全局线段特征地图。

所述的测距传感器获得的数据是测距传感器在测距高度平面上扫描得到的环境中障碍物上各个点相对于移动机器人的距离和角度。

局部占用栅格地图和局部线段特征地图采用以当前周期移动机器人位置为原点、以当前周期移动机器人正方向为X轴的坐标系。

全局占用栅格地图和全局线段特征地图采用以第一周期移动机器人位置为原点、以第一周期移动机器人正方向为X轴的坐标系。

根据移动机器人上所安装测距传感器获得的数据，构建当前观测到的局部占用栅格地图和局部线段特征地图：它包括局部占用栅格地图的构建和局部线段特征地图的构建，其中局部占用栅格地图构建采用布尔贝叶斯滤波法，局部线段特征地图构建的步骤为：

1)对测距传感器数据，利用哈夫变换直线拟合方法得到初步拟合线段；

2)对初步拟合线段进行同向性判断、共线性判断和重合性判断，合并大致同向共线重合的线段的拟合点集合；

3)对于合并后的线段拟合点集合，利用最小二乘拟合法得到精确拟合线段；

根据测距传感器数据、航位推测传感器数据、上一周期对移动机器人的位姿估计和前期构建得到的全局线段特征地图，估计当前周期移动机器人的位姿的步骤为：

1)根据航位推测传感器数据和上一周期对移动机器人的位姿估计，利用移动机器人运动模型，预估当前周期移动机器人的位姿；

2)根据预估位姿，从全局线段特征地图中取出落在测距传感器扫描范围内的特征，获得预计当前可见的线段特征；

3)将预计当前可见的线段特征从全局地图坐标系转换到局部地图坐标系中；

4)通过寻找测距传感器数据与预计当前可见线段特征的最佳相合，计算位姿校正偏移量；

5)根据当前周期移动机器人的预估位姿和位姿校正偏移量，计算当前周期移动机器人的位姿估计。

根据航位推测传感器数据和上一周期对移动机器人的位姿估计，利用移动机器人运动模型，预估当前周期移动机器人的位姿：包括如下步骤，

1)根据当前周期的航位推测传感器数据和上一周期的航位推测传感器数据，计算这期间移动机器人的近似移动速度；

2)根据计算得到的移动机器人的近似移动速度和上一周期对移动机器人的估计位姿，计算得到当前周期移动机器人的位姿预估。

通过寻找测距传感器数据与预计当前可见线段特征的最佳相合，计算位姿校正偏移量：包括如下步骤，

1)采用点到线段特征距离最短的匹配方法，寻找测距传感器数据与预计当前可见线段特征的匹配关系；

2)从与同一线段特征相匹配的数据点关系着手，对上述匹配关系进行判断，删除不当匹配；

3)从匹配线段特征生成时间、匹配线段特征的可信度、匹配线段是否为拐角组成线段、匹配线段长度、以及匹配数据是否为某一局部线段特征的拟合点五个方面进行比例量化定义匹配权值；

4)根据最佳匹配是数据点到匹配线段特征距离为零的条件构建匹配部分最佳相合的参数模型结构，利用加权最小二乘参数估计法位姿校正偏移量；

5)将测距传感器数据按位姿校正偏移量作平移和旋转；

6)重复步骤1)-5)，直到满足加权最小二乘参数估计的残差小于一定值或者稳定的收敛条件。

根据对当前周期移动机器人的位姿估计、构建得到的局部线段特征地图和局部占用栅格地图，更新全局占用栅格地图和全局线段特征地图：包括全局占用栅格地图更新和全局线段特征地图更新，其中全局占用栅格地图更新采用布尔贝叶斯滤波法，全局线段特征地图更新包括如下步骤：

1)对于局部线段特征地图中的线段特征，在全局线段特征地图中寻找是否存在一个全局线段特征与该局部线段特征满足同向性、共线性和重合性要求；

2)如果存在，则合并线段拟合点，利用最小二乘直线拟合法重新拟合得到线段，替换原全局线段特征；

3)如果不存在，则将局部线段特征加入到全局线段特征地图中。

4)利用全局占用栅格地图计算全局线段特征地图中线段特征的可信度。

本发明提出的移动机器人在未知环境中同时定位与地图构建的方法解决了未知环境地图构建中机器人位姿的不确定性与地图构建的不确定性相互关联、相互影响的问题，所占存储空间和计算资源较少，可以实现在线未知环境地图构建，所构建得到全局线段特征地图和全局占用栅格地图适用于各类应用机器人进行路径规划和导航。

附图说明

图1是实施移动机器人在未知环境中同时定位与地图构建的软件流程图；

图2是在局部占用栅格地图构建中对每个障碍物点数据所构造的锥形图；

图3是本发明方法中估计当前周期移动机器人位姿的操作流程图；

图4是本发明方法中计算位姿校正偏移量的操作流程图；

图5是利用本发明方法中的局部占用栅格地图构建方法和局部线段特征地图构建方法实际绘制局部占用栅格地图和局部线段特征地图的一个实例；

图5(a)是测距传感器获得障碍物点数据的一个实例；

图5(b)是利用利用本发明方法中的局部占用栅格地图构建方法，对图5(a)中的数据实例构建得到的一个局部占用栅格地图，其中黑色表示被占用区域，白色表示空闲区域，灰色表示不确定区域；

图5(c)是利用利用本发明方法中的局部线段特征地图构建方法，对图5(a)中的数据实例构建得到的一个局部线段特征地图；

图6是利用本发明方法中的局部占用栅格地图构建方法和局部线段特征地图构建方法实际绘制局部占用栅格地图和局部线段特征地图的另一个实例；

图6(a)是测距传感器获得障碍物点数据的另一个实例；

图6(b)是利用利用本发明方法中的局部占用栅格地图构建方法，对图6(a)中的数据实例构建得到的一个局部占用栅格地图，其中黑色表示被占用区域，白色表示空闲区域，灰色表示不确定区域；

图6(c)是利用利用本发明方法中的局部线段特征地图构建方法，对图6(a)中的数据实例构建得到的一个局部线段特征地图；

图7是利用本发明方法中计算当前周期移动机器人位姿的一个实例；

图7(a)是当前周期测距传感器所获取的障碍物点数据与根据当前周期航位推测传感器数据所获取的预计当前可见线段特征的匹配情况的实例，其中黑色点是当前周期测距传感器观测到的障碍物数据点，黑色线是根据当前周期航位推测传感器数据获取得到的预计当前可见的线段特征；

图7(b)是当前周期测距传感器所获取的障碍物点数据与按照本发明方法计算得到的当前周期机器人预估位姿所获取的预计当前可见线段特征的匹配情况的实例，其中黑色点是当前周期测距传感器观测到的障碍物数据点，黑色线是按照本发明方法计算得到的当前周期机器人预估位姿所获取得到的预计当前可见的线段特征；

图7(c)是当前测距传感器所获取的障碍物点数据与按照本发明方法计算得到的当前周期机器人位姿所获取的预计当前可见线段特征的匹配情况的实例，其中黑色点是当前周期测距传感器观测到的障碍物数据点，黑色线是按照本发明方法计算得到的当前周期机器人位姿所获取得到的预计当前可见的线段特征；

图8是利用本发明方法绘制的一个全局占用栅格地图和全局线段特征地图实例；

图8(a)是对一个长为16.4m、宽为7.9m的房间利用本发明方法绘制的一个全局占用栅格地图实例。其中灰色线为航位推测传感器获得的机器人位置连线，黑色线为利用本发明方法计算得到的机器人位置连线；

图8(b)是对图8(a)中所述房间利用本发明方法绘制的一个全局线段特征地图实例。

具体实施方式

下面将参照附图，详细描述根据本发明机器人在未知环境中同时定位与地图构建的一个实施例子。在该实施例子中，机器人能够独立移动，所配备的测距传感器为激光测距仪、或声纳测距仪、或立体视觉系统、或它们的组合，所配备的航位推测传感器为里程计。

图1是实施本发明方法的软件流程图。移动机器人从未知环境中的任意一个位置、按任意一个方向启动。首先进行传感器数据采集(步骤S1)，构建得到局部占用栅格地图和局部线段特征地图(步骤S2)，通过坐标系转换，将局部占用栅格地图数据和局部线段特征地图数据转换到全局地图坐标系中，从而获得初始的全局占用栅格地图和全局线段特征地图(步骤S3)。然后机器人开始移动(步骤S4)，并间隔一定时间停止，进行传感器数据采集(步骤S5)，完成同时定位和地图构建(步骤S6)，再继续移动，如此循环反复，当机器人不再移动时，退出循环。其中同时定位和地图构建包括步骤：局部占用栅格地图和局部线段特征地图构建；机器人位姿估计和校正；全局占用栅格地图和全局线段特征地图的更新。

在实施例子中，测距传感器获得一组在测距高度平面上环境中障碍物上各点相对于机器人的距离和角度。以当前移动机器人位置为原点、以当前移动机器人正方向为X轴构造局部地图坐标系，则所检测到的各个障碍物点数据即为局部地图坐标系中的数对(r_i，α_i)，i＝1，...，n，n表示点数，r_i表示点到坐标系原点的距离，α_i表示坐标系原点到点的连线与x轴的夹角。各个障碍物点的笛卡尔坐标为

(x_i，y_i)，x_i＝r_icosα_i，y_i＝r_isinα_i.

局部占用栅格地图构建采用布尔贝叶斯滤波法。在该实施例子中，对每个障物点数据(r_i，α_i)首先构建如图2所示的锥形区域，其中d₁＝r_i-d，d₂＝r_i+d，θ为相邻障碍物点与坐标原点连线的夹角的1/2。对于该障碍物点，区域1中栅格的被占用概率计算方法为

f＝1-E_r·E_α，    (1)

其中E_r＝1-k_r·(l/d₁)²，E_a＝1-k_α·(β/θ)²，k_r，k_α为常系数，l为栅格到锥形顶点的距离，β为栅格到锥形顶点连线与锥形中心线之间的夹角。区域2中栅格的被占用概率计算方法为

f＝O_r·O_α，      (2)

其中O_r＝1-k_r·((l-r_i)/d)²，O_α＝1-k_α·(β/θ)².利用坐标变换，确定锥形区域中各个栅格在局部占用栅格地图中的对应位置，利用公式