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

Abstract

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

Description

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

技术领域

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

背景技术

随着移动机器人应用范围的逐渐扩展，如何使机器人自主认知未知环境成为近年来机器人和人工智能领域中的一个研究热点。机器人自主认知未知环境的方式之一就是构建未知环境的模型或者地图，即由机器人根据传感器测量信息自主构建其所在环境的空间模型或地图(参见“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_α·(β/θ)².利用坐标变换，确定锥形区域中各个栅格在局部占用栅格地图中的对应位置，利用公式

$\log \frac{p}{1-p}=\log \frac{p^{\′}}{1-p^{\′}}+\log \frac{f}{1-f}---\left(3\right)$

计算局部占用栅格地图中对应栅格的被占用概率，其中p′为该栅格的原被占用概率，p是所求的更新后的被占用概率。

在该实施例子中，对每个线段特征用[c，θ，l，P，gt，conf，FLine]六项参数描述，记为L。其中c，θ为线段所在直线的参数，直线方程为

                       xcosθ+ysinθ+c=0.                                    (4)c为原点到直线的距离，θ为直线的法线与坐标系x轴的夹角。l为线段长度，P＝(x_c，y_c)^T为线段中心点位置(T表示转置)。gt为时间戳属性，记录线段生成时间。conf为线段的可信度属性。FLine为布尔量，用于表示线段是否为拐角的构成线段，如果是则为真，否则为假。

在该实施例子的局部线段特征地图构建中，首先对测距传感器所获得的障碍物点数据集合应用哈夫变换直线拟合法初步确定那些点数据属于哪条线段。具体为按一定的离散度构建二维投票网格，其中一维为直线参数c的离散取值，一维为直线参数θ的离散取值。对于每个障碍物点(x_i，y_i)，都有一组参数为(c_j，θ_j)的直线经过该点，从而为相应网格投票。通过投票计数，可以确定各个障碍物点属于哪条线段以及该线段的参数描述。

然而由于测量误差，测距传感器所测得的障碍物点并不是准确的共线，在哈夫变化中也很难选择一个合适的离散网格尺寸，对于一组应该共线的点往往会生成多于一条的拟合线段。因此在实施例子的局部线段特征地图构建中，在哈夫变换直线拟合之后，对所得到线段集合中的线段进行两两同线性判断。当线段L₁和线段L₂满足如下同线性判断规则时，认为两条线段是同线的，进行线段上点集合的合并。

1)同向性，要求|θ₁-θ₂|≤Δθ，其中Δθ为阀值；

2)共线性，要求‖c₁|-|c₂‖≤ΔC， $d=|x_{c_1}\cos {\mathrm{\θ}}_2+y_{c_1}\sin {\mathrm{\θ}}_2+c_2|\≤\mathrm{\ΔD}$ ，其中ΔC,ΔD为阀值。即原点到线段所在直线的距离差小于阀值ΔC，同时一条线段中心点到另一条线段的垂直距离小于阀值ΔD。

3)重和性，要求 ${(x_{c_1}-x_{c_2})}^2+{(y_{c_1}-y_{c_2})}^2\≤{(l_1/2+l_2/2)}^2$ ，即线段中心点之间的欧几里得距离小于等于线段半场之和。

通过同线性判断合并大致重合的线段点集合后，再利用最小二乘直线拟合法求取更为精确的线段参数c，θ，l和p。参数gt为当前时间，参数conf＝1。当线段与另一线段的夹角在90度左右且相交交点在线段上时，FLine被置为真。

在该实施例子中，根据测距传感器数据、航位推测传感器数据、上一周期对移动机器人的位姿估计和前期构建得到的全局线段特征地图和全局占用栅格地图，估计当前周期移动机器人的位姿，其步骤如图3所示。在该实施例子中，以移动机器人初始位姿为原点、以移动机器人初始方向为X轴构造全局地图坐标系。航位推测传感器数据和移动机器人位姿估计均为全局地图坐标系中的数据。由此，机器人在全局占用栅格地图和全局线段特征地图中的初始位姿为(0，0，0)。

在步骤S61中，根据航位推测传感器数据和上一周期对移动机器人的位姿估计，利用移动机器人运动模型，预估当前周期移动机器人的位姿。记当前周期为t，t-1周期航位推测传感器读数为 $O_{t-1}={(x_{O_{t-1}},y_{O_{t-1}},z_{O_{t-1}})}^T$ ，t周期航位推测传感器读数为 $O_t={(x_{O_t},y_{O_t},z_{O_t})}^T$ ，t-1周期移动机器人的位姿估计为X_t-1＝(x_t-1,y_t-1,z_t-1)^T。可以假设t-1周期到t周期期间机器人的运动近似为匀速运动，其运动速度为D＝(dx，dy，dθ)^T，其中dx，dy分别为x，y方向的线速度，dθ为角速度，其计算公式如下：

$\left\{\begin{array}{c}\mathrm{dx}=(x_{O_t}-x_{O_t})\cos {\mathrm{\θ}}_{O_{t-1}}+(y_{O_t}-y_{O_{t-1}})\sin {\mathrm{\θ}}_{O_{t-1}}\\ \mathrm{dy}=-(x_{O_t}-x_{O_{t-1}})\sin {\mathrm{\θ}}_{O_{t-1}}+(y_{O_t}-y_{O_{t-1}})\cos {\mathrm{\θ}}_{O_{t-1}}\\ \mathrm{d\θ}={\mathrm{\θ}}_{O_t}-{\mathrm{\θ}}_{O_{t-1}}\end{array}.\right.---\left(5\right)$

根据该移动速度和t-1周期移动机器人的位姿估计，可以得到t周期移动机器人位姿的预估 $X_{t_0}={(x_{t_0},y_{t_0},z_{t_0})}^T$ ，其计算公式如下：

$\left\{\begin{array}{c}{x}_{t_0}=x_{t-1}+\mathrm{dx}\cos {\mathrm{\θ}}_{t-1}-\mathrm{dy}\sin {\mathrm{\θ}}_{t-1}\\ y_{t_0}=y_{t-1}+\mathrm{dx}\sin {\mathrm{\θ}}_{t-1}+\mathrm{dy}\cos {\mathrm{\θ}}_{t-1}\·\\ {\mathrm{\θ}}_{t_0}={\mathrm{\θ}}_{t-1}+\mathrm{d\θ}\end{array}\right.---\left(6\right)$

在步骤S62中，根据预估位姿 $X_{t_0}={(x_{t_0},y_{t_0},z_{t_0})}^T$ 从全局线段特征地图中取出落在测距传感器扫描范围内的线段特征，获得预计当前可见的线段特征。在该实施例子中，端点之一落在测距仪测量范围之内的线段特征为预计当前可见的线段特征，所有预计当前可见的线段特征构成集合г。

在该实施例子中，选择局部地图坐标系为匹配坐标系，因此在步骤S63中将预计当前可见的线段特征从全局地图坐标系转换到局部地图坐标系中，即将集合г中的线段特征转换到局部地图坐标系中。记集合г中的线段为L_W：{c_W，θ_W，l_W，P_W，gt_W，conf_W}，转换到局部地图坐标系中的线段为L_R：{c_R，θ_R，l_R，P_R，gt_R，conf_R}，转换方程为

$\left\{\begin{array}{c}\left[\begin{array}{c}{x}_{c_R}\\ y_{c_R}\end{array}\right]=\left[\begin{array}{cc}\cos {\mathrm{\θ}}_{t0}& \sin {\mathrm{\θ}}_{t0}\\ -\sin {\mathrm{\θ}}_{t0}& \cos {\mathrm{\θ}}_{t0}\end{array}\right]\left[\begin{array}{c}{x}_{c_W}-x_{t0}\\ y_{c_W}-y_{t0}\end{array}\right]\\ {\mathrm{\θ}}_R={\mathrm{\θ}}_W-{\mathrm{\θ}}_{t0}\\ c_R=-(x_{c_R}\cos {\mathrm{\θ}}_R+x_{c_R}\sin {\mathrm{\θ}}_R)\\ l_R=l_W,{\mathrm{gt}}_R={\mathrm{gt}}_W,{\mathrm{conf}}_R={\mathrm{conf}}_W\end{array}.---\left(7\right)\right.$

转换得到的线段特征构成集合г′。

在步骤S64中，通过寻找测距传感器数据与预计当前可见线段特征的最佳相合，计算位姿校正偏移量。图4给出了实现步骤。

在步骤S641中，为每个测距传感器测得的障碍物点数据(x_i，y_i)在г′中寻找与之距离最近的线段特征。在该实施例子中，要求障碍物数据点到最近线段特征的距离小于一定的阀值，如果满足条件，就将该线段特征称为该数据点的匹配线段，将该数据点称为匹配点。所有匹配点构成集合v。

步骤S641的匹配方式容易引起不当匹配，这类不当匹配将影响所计算得到的位姿校正偏移量的精确度，需要予以去除。在步骤S642中，进行不当匹配判断，并将判断为不当匹配的点从集合v中删除。不当匹配判断的一个实施例子是：记某一匹配线段为L_p，将集合v中以L_p为匹配线段的匹配点按顺时针或逆时针顺序连接起来，记为集合Z，Z＝{v_i，i＝1，…，k}，v_i＝(x_i，y_i)^T，k是匹配点数。利用最小二乘直线拟和法对Z中的点进行线段拟合，记拟合得到的线段为L_q。计算点到拟合线段的距离和d，

$d=\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{d}_i=\underset{i=1}{\overset{k}{\mathrm{\Σ}}}(x_i\cos {\mathrm{\θ}}_q+y_i\sin {\mathrm{\θ}}_q+c_q).---\left(8\right)$

如果|θ_p-θ_q|＞阀值，即匹配点拟和得到的线段与匹配线段角度差较大时，认为匹配错误，在集合v中删除集合Z中的所有点。如果|θ_p-θ_q|≤阀值，而d＞阀值，则认为Z中的部分点存在误匹配。为了去除这些误匹配，对Z中的每个点进行如下判断：

1)如果v_i是当前局部线段特征地图中某线段L_o的拟合点，则转(3)，否则转(2)；

2)在Z中取v_i的前一个匹配点v_i-1和后一个匹配点v_i+1，分别求v_i与这两个点的连线长度和角度，

θ₁＝∠v_iv_i-1，

θ₂＝∠v_iv_i+1，Δθ₁＝|θ_p-θ₁|，Δθ₂＝|θ_p-θ₂|.如果d₁或d₂大于一定值，且Δθ₁和Δθ₁均小于定值，则是正确匹配，否则为误匹配。

3)如果局部线段L_o与匹配线段L_p满足同线性要求，则是正确匹配，否则是误匹配。

在集合v中删除判断为误匹配的点。由步骤S42得到匹配点集合v′。

在步骤S643中，对匹配点集合v′中的各个匹配点v_i＝(x_i，y_i)^T定义匹配权值w_i。权值计算公式为

$w_i=(t-{\mathrm{gt}}_p)/(t-\underset{i=1,\·\·\·,m}{\min }\left({\mathrm{gt}}_i\right))+{\mathrm{Fline}}_p+{\mathrm{conf}}_p+\mathrm{LineDot}\×\min (1,l_p/l_o)---\left(9\right)$

公式中L_p为v_i对应的匹配线段特征，LineDot为布尔量，用于表示点v_i是否为当前局部线段特征地图中某线段特征的拟合点，如果是则为真，并记该局部线段特征为L_o。公式中的第一项表示匹配线段生成时间越早，则匹配权值越高，因为线段特征随着地图的更新被不断地修正；第二项表示当匹配线段为拐角的构成线段时匹配权值较高；第三项说明匹配线可信度越高，匹配权值越高；第四项表示当匹配点是某局部线段的拟合点时，其匹配权值高于未能拟合到任何局部线段中的匹配点的匹配权值，且局部线段长度小于或等于匹配线段长度时，其权值高于局部线段长度大于匹配线段长度的权值，因为后者说明匹配线段将被修正。

步骤S644计算位姿校正偏移量Δ，Δ＝(Δ_x，Δ_yΔ_θ)^T。在实施例子中，先根据最佳匹配构建参数模型结构。最佳匹配情况是对V′中的每个匹配点v_i按位姿校正偏移量Δ进行旋转平移后，每个匹配点到对应匹配线的距离均为0。记v_i′为v_i按Δ进行旋转平移后得到的点。

${v_i}^{\′}=\left[\begin{array}{cc}\cos {\mathrm{\Δ}}_{\mathrm{\θ}}& -\sin {\mathrm{\Δ}}_{\mathrm{\θ}}\\ \sin {\mathrm{\Δ}}_{\mathrm{\θ}}& \cos {\mathrm{\Δ}}_{\mathrm{\θ}}\end{array}\right](v_i-\stackrel{-}{v})+\stackrel{-}{v}+\left[\begin{array}{c}{\mathrm{\Δ}}_x\\ {\mathrm{\Δ}}_y\end{array}\right]\≈{\mathrm{\Δ}}_{\mathrm{\θ}}\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right](v_t-\stackrel{-}{v})+v_i+\left[\begin{array}{c}{\mathrm{\Δ}}_x\\ {\mathrm{\Δ}}_y\end{array}\right].---\left(10\right)$

其中 为匹配集合V′中所有点的重心。v_i′到v_i对应匹配线L_p的距离为d_i＝r_i-u_i·v_i′，u_i为L_p的单位法线向量，r_i为匹配线上任意一点与u_i的点乘。由直线方程可得u_i＝[u_i1，u_i2]＝[cosθ_p，sinθ_p]，r_i＝-c_p.则由最佳匹配条件d_i＝0得

y_i＝(_i1，_i2，_i3)Δ，    (11)

其中

由集合V′中共有m个点，可得

Y＝ФΔ.                       (13)

其中Y是由y_i组成的m×1列向量，Ф是(_i1，_i2，_i3)组成的m×3矩阵。利用加权最小二乘估计可计算得到位姿校正偏移量Δ，

Δ＝(Ф^TWФ)^-1Ф^TWY.           (14)

其中W为m×m对角正定加权矩阵，W[i，i]为匹配点v_i的匹配权值w_i。此时匹配部分将达到最佳相合，即匹配误差协方差J_W＝(Y-ФΔ)^TW(Y-ФΔ)达到最小值。

步骤S645将每个障碍物数据点按位姿校正偏移量作平移和旋转。然后再次执行步骤S41，在步骤S41执行完成后，进行收敛判断。在该实施例子中，收敛条件为加权最小二乘估计的残差小于一定值或者稳定在一定的值上。当收敛条件满足时，所得位姿校正偏移量即为所求位姿校正偏移量；当收敛条件不满足时，回到步骤S42，如此循环往复直到收敛条件满足。

然后执行步骤S65，根据当前周期移动机器人的预估位姿 $X_{t_0}={(x_{t_0},y_{t_0},z_{t_0})}^T$ 和位姿校正偏移量Δ＝(Δ_x，Δ_y，Δ_θ)^T，计算当前周期移动机器人的位姿估计X_t＝(x_t，y_t，z_t)^T，其计算公式为：

$\left\{\begin{array}{c}{x}_t=x_{t_0}+\mathrm{\Δ}x\cos {\mathrm{\θ}}_{t_0}-\mathrm{\Δ}y\sin {\mathrm{\θ}}_{t_0}\\ y_t=y_{t_0}+\mathrm{\Δ}x\sin {\mathrm{\θ}}_{t_0}+\mathrm{\Δ}y\cos {\mathrm{\θ}}_{t_0}\\ {\mathrm{\θ}}_t={\mathrm{\θ}}_{t_0}+\mathrm{\Δ\θ}\end{array}.---\left(15\right)\right.$

根据所得到的机器人位姿估计、局部线段特征地图和局部占用栅格地图，可以更新全局线段特征地图和全局占用栅格地图。根据所估计的机器人位姿，计算局部占用栅格地图中的各个栅格在全局占用栅格地图中所对应的位置，利用公式(3)更新该栅格的被占用概率。同样，根据所估计的机器人位姿，将局部线段特征地图中的各个线段特征转化到全局线段特征地图中，并在全局线段特征地图中寻找是否存在一个全局线段特征与该局部线段特征满足同向性、共线性和重合性要求。如果存在，则合并线段拟合点，利用最小二乘直线拟合法重新计算线段特征属性，替换原全局线段特征，如果不存在，则将该线段特征加入到全局线段特征地图中。最后利用全局占用栅格地图计算各个线段特征的可信度，计算方式为位于被占用栅格内的线段特征长度除以该线段特征的总长度。当可信度低于一定的值时，在全局线段特征地图中删除该线段特征。

Claims (9)

1.一种移动机器人在未知环境中同时定位与地图构建的方法，其特征在于，包括如下步骤：

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

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

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

2.根据权利要求1所述的一种移动机器人在未知环境中同时定位与地图构建的方法，其特征在于，所述的测距传感器获得的数据是测距传感器在测距高度平面上扫描得到的环境中障碍物上各个点相对于移动机器人的距离和角度。

3.根据权利要求1所述的一种移动机器人在未知环境中同时定位与地图构建的方法，其特征在于，所述的局部占用栅格地图和局部线段特征地图采用以当前周期移动机器人位置为原点、以当前周期移动机器人正方向为X轴的坐标系。

4.根据权利要求1所述的一种移动机器人在未知环境中同时定位与地图构建的方法，其特征在于，所述的全局占用栅格地图和全局线段特征地图采用以第一周期移动机器人位置为原点、以第一周期移动机器人正方向为X轴的坐标系。

5.根据权利要求1所述的一种移动机器人在未知环境中同时定位与地图构建的方法，其特征在于，所述的根据移动机器人上所安装测距传感器获得的数据，构建当前观测到的局部占用栅格地图和局部线段特征地图：它包括局部占用栅格地图的构建和局部线段特征地图的构建，其中局部占用栅格地图构建采用布尔贝叶斯滤波法，局部线段特征地图构建的步骤为：

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

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

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

6.根据权利要求1所述的一种移动机器人在未知环境中同时定位与地图构建的方法，其特征在于，所述的根据测距传感器数据、航位推测传感器数据、上一周期对移动机器人的位姿估计和前期构建得到的全局线段特征地图，估计当前周期移动机器人的位姿的步骤为：

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

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

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

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

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

7.根据权利要求6所述的一种移动机器人在未知环境中同时定位与地图构建的方法，其特征在于，所述根据航位推测传感器数据和上一周期对移动机器人的位姿估计，利用移动机器人运动模型，预估当前周期移动机器人的位姿：包括如下步骤，

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

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

8.根据权利要求6所述的一种移动机器人在未知环境中同时定位与地图构建的方法，其特征在于，所述通过寻找测距传感器数据与预计当前可见线段特征的最佳相合，计算位姿校正偏移量：包括如下步骤，

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

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

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

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

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

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

9.根据权利要求1所述的一种移动机器人在未知环境中同时定位与地图构建的方法，其特征在于，所述的根据对当前周期移动机器人的位姿估计、构建得到的局部线段特征地图和局部占用栅格地图，更新全局占用栅格地图和全局线段特征地图：包括全局占用栅格地图更新和全局线段特征地图更新，其中全局占用栅格地图更新采用布尔贝叶斯滤波法，全局线段特征地图更新包括如下步骤：

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

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

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

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

Images