CN109323697A - 一种针对室内机器人任意点启动时粒子快速收敛的方法 - Google Patents

Abstract

本发明提供一种针对室内机器人任意点启动时粒子快速收敛的方法，属于室内移动机器人定位领域。该方法利用Cartographer算法构造的先验地图，对先验地图构成的图像进行灰度处理，再利用图像梯度算法提取出灰度图边框，得到先验地图的轮廓。机器人启动时，采集第一次采样得到的激光数据，以图像形式将激光数据保存。利用线性近邻knn搜索算法将激光轮廓图像与先验地图的轮廓进行匹配，匹配程度大于设定阈值的区域设定为候选区。匹配后得到局部候选区列表，在初始化粒子时，遍历候选区，在候选区以数理统计特性进行粒子的初始化。粒子在候选区域进行初始化时，不再使用随机化朝向的方式，而是根据激光数据轮廓对粒子的朝向进行约束。

Description

一种针对室内机器人任意点启动时粒子快速收敛的方法

技术领域

本发明属于室内移动机器人定位技术领域，涉及一种针对室内机器人任意点启动时粒子快速收敛的方法。

背景技术

定位是室内机器人导航的必要条件，是机器人环境交互的核心技术。室内机器人定位最常用的方法为粒子滤波算法，粒子滤波算法通过非参数化的蒙特卡洛(MonteCarlo)模拟方法来实现递推贝叶斯滤波，蒙特卡洛方法通过初始化粒子群、模拟粒子群、计算粒子评分、粒子群重采样推算室内机器人位置，其位置符合高斯概率分布。粒子滤波方法适用于任何能用状态空间模型描述的非线性***，例如室内动态环境。

现有的粒子滤波算法以激光作为测距传感器，机器人任意点启动时，其位置可能处于室内环境的任意处，传统的粒子滤波方法在先验地图的全局范围内分布代表机器人位姿的粒子，通过机器人自身运动(旋转或直线运动)进行观测数据的更新，利用粒子滤波算法对代表机器人位姿的粒子进行评分和重采样，使粒子收敛，全局的粒子最终收敛为一簇，该簇粒子的方差和均值均小于某一个阈值，利用粒子簇的均值可以推算出机器人的位姿(位置和朝向)。由于先验地图为机器人可能出现的位置的全集合，利用传统粒子滤波算法实现定位时，若先验地图代表的真实环境面积较大，分布的粒子就较多，利用粒子滤波算法进行粒子重采样非常消耗计算资源，并且耗时较大。因此，找到一种使机器人在先验地图中任意点启动时能够局部分布粒子，以此使粒子快速收敛的方法很有必要。

文献(Thrun S,Fox D,Burgard W,et al.Robust Monte Carlo localizationfor mobile robots[J].Artificial Intelligence,2001,128(1):99-141.)提出一种修改粒子滤波生成粒子的算法，该算法颠覆常规粒子滤波位姿采样过程，使用双粒子滤波猜想方法进行粒子采样。文献中算法使用最新传感器测量数据进行粒子样本的生成，使用里程计数据评估样本位姿与机器人实际位姿的一致性。其性能优于常规的粒子滤波器，使粒子的重要性权重分配更为准确，也相对提高了粒子的收敛速率。该算法对传感器的精度要求过高，普通测距传感器无法在高频率数据采集情况下保持高精度的数据特性。

文献(ZHANG Heng,FAN XiaoPing,QU ZhiHua,等.Mobile Robot Adaptive MonteCarlo Localization Based on Multiple Hypothesis Tracking 2007,33(9):941-946.)提出一种自适应动态粒子聚类算法对常规粒子滤波算法进行优化。该算法适时的对粒子根据在空间上的相似性进行聚类，每簇粒子可根据均值表示出一个机器人位姿假设。在迭代过程中，每簇粒子对应一个独立的粒子滤波算法，直到满足设定的判定条件时，再从全局抽取等权重的粒子进行聚类。该方法不再对全局范围内的粒子权重进行归一化处理，局部范围粒子的收敛速率快于全局范围粒子的收敛速率。然而粒子簇群数没有限制，若先验地图包含自由区域面积过大，初始化分布粒子数量较为庞大，则***需要维持的粒子滤波器数目也较为庞大。算法时间复杂度过高，占用计算资源过大。

综上所述，找到一种室内机器人在室内场景中任意点启动时，能够合理初始化粒子，使粒子能够快速准确收敛到机器人实际准确位置的方法显得十分重要。

发明内容

针对机器人任意点启动时粒子收敛速率慢的问题，本发明针对粒子初始化提出一种局部初始化粒子的方法。该方法利用Cartographer算法构造的先验地图作为机器人导航定位时的观测依据。对先验地图构成的图像进行灰度处理，再利用图像梯度算法提取出灰度图边框，得到先验地图的轮廓。机器人启动时，采集第一次采样得到的激光数据，以图像形式将激光数据保存。提取出激光数据轮廓特征，包括扫描得到的角点、切点、拐点等环境信息。利用线性近邻knn(K-Nearest Neighbors)搜索算法将激光轮廓图像与先验地图的轮廓进行匹配，匹配程度大于设定阈值的区域设定为候选区。匹配后得到局部候选区列表，在初始化粒子时，遍历候选区，在候选区以一定数理统计特性进行粒子的初始化。

粒子在候选区域进行初始化时，不再使用随机化朝向的方式，而是根据激光数据轮廓对粒子的朝向进行约束。由激光数据轮廓大致推算出机器人真实位姿中的朝向信息，推算出的朝向可存在多个，以列表形式进行存储。分布粒子时遍历该列表，对列表中每一个朝向信息，初始化一定数量的粒子，使这些粒子的朝向满足以列表中的朝向为均值、以特定阈值为方差的高斯分布。因此，候选区域存在多个符合高斯分布的粒子群。

本发明的技术方案：

一种针对室内机器人任意点启动时粒子快速收敛的方法，步骤如下：

步骤一、构建先验地图

利用构图算法构造室内二维先验地图，所述的构图算法包括hector算法、Gmapping算法或Cartographer算法，优选Cartographer算法进行先验地图的构建。Cartographer算法在构造先验地图过程中，通过闭环检测消除了构图过程中里程计带来的误差。以子图作为闭环检测的基本单元，一定数量的激光观测数据构成子图，基于其已有的激光数据将一次观测数据***子图的最佳位置，生成一个子图后进行局部回环，所有的子图局部回环完成后再进行全局回环。Cartographer算法构造的先验地图针对相似环境具有很高的精度，构造先验地图效果如图1。

步骤二、提取先验地图轮廓与关键特征点

(2.1)对步骤一得到的先验地图进行灰度处理，即将先验地图图像由RGB图像转换为灰度图像。RGB图像每个像素点由R、G、B三个通道分量组成，灰度图像每个像素点值为单通道分量Y。将RGB图像转为灰度图像的公式如下：

Y＝α*R+β*G+γ*B (1)

其中，R、G、B表示图像三个通道的值，α、β、γ表示三个通道值在灰度图像像素值中的权重，Y表示灰度图像的像素值。

(2.2)将先验地图转换的灰度图像后，得到一个二维离散函数f(x,y)，其中(x,y)表示灰度图像中像素点的坐标，f(x,y)表示该坐标的像素值。求该二维离散函数在x方向和y方向上的梯度[G_x,G_y]^T：

利用差分求解公式(2)，得到图像梯度表达公式为：

利用公式(3)及图像梯度法提取出先验地图图像的轮廓(如图2所示)。

(2.3)利用公式(1)、(2)、(3)提取出先验地图图像轮廓以后，采用SIFT(Scale-invariant feature transform)算法对先验地图的轮廓图图像进行显式特征提取，提取出轮廓图的关键特征点。提取出的显式特征可以提高匹配的精度。提取特征步骤如下：

1)将步骤(2.2)得到的图像轮廓作为二维观测数据Image(x,y)，并生成二维观测数据矩阵；

2)采用高斯函数对观测数据进行降采样处理生成图像金字塔，其中图像金字塔的层数确定为：

O＝[log(min(H,W))]-2 (4)

其中，H、W表示二维观测数据矩阵的行数和列数；

根据得到的图像金字塔，生成差分图像金字塔：

D(x,y)＝L(N+1)-L(N) (5)

其中，L(N+1)表示第N+1层降采样的图像，L(N)表示第N层降采样的图像。

3)由差分图像金字塔确定局部极值点，具体为：遍历差分图像金字塔的第二层到倒数第二层，分别比较每个像素点与周围像素点像素值的大小，当该像素点像素值大于或小于它的图像域和尺度域的所有相邻点时，即为极值点。

4)由差分图像金字塔中得到的局部极值点回至图像金字塔中，确定极值点的主要方向。局部极值点对应于图像金字塔中的局部区域即为要提取的关键点，分析图像金字塔中局部区域的方向直方图，方向直方图的峰值则代表了该关键点邻域梯度的方向，以方向直方图中最大值作为该关键点的主方向。

以上步骤提取出的先验地图的轮廓的特征点如图3。

步骤三、处理第一帧激光数据

对于第一次采集的激光数据，激光点相对于激光观测器越远的距离，激光点越稀疏，如图4。在图像轮廓匹配时，对稀疏的激光点进行噪声处理；遍历激光点云，对于激光点云中的一个激光点P_g，当其与最近激光点的距离大于所有的激光点的平均距离时，将激光点P_g设定为孤点，将所有的孤点剔除(如图5)。将第一帧激光保存为图像格式，并按照步骤二所述方式提取第一帧激光图像的轮廓与关键特征点；

步骤四、采用线性最近邻knn搜索算法匹配激光轮廓与先验地图轮廓

(4.1)将激光轮廓图像作为训练集T{l₁,l₂,l₃,l₄...}，将先验地图轮廓图像作为查询集P{p₁,p₂,p₃,p₄....}。

(4.2)将训练集中的特征点与查询集中的特征点进行交叉匹配，即形成特征点集合{(l₁,p₁),(l₁,p₂),(l₁,p₃),(l₁,p₄),(l₂,p₁),(l₂,p₂)....}；该匹配形成的特征点对数庞大，也无法定位出激光轮廓对应于先验地图中的位置。

(4.3)将特征点集合进行过滤：对于每个训练集中的特征点l(x₁,y₁)，在查询集中选择K个与此特征点最相似的特征点p(x₁,y₁),p(x₂,y₂)....p(x_k,y_k)，当前k-1个点与第k点的欧式距离比率大于给定阈值，则前k-1个点为正确的匹配。此匹配算法使用的特征点为步骤(2.3)中提取出的SIFT特征点。使用该特征点进行匹配可消除激光轮廓尺度相对于先验地图轮廓尺度缩放、亮度变换、角度旋转带来的噪声。如图6，经过线性最近邻knn搜索算法，可获得先验地图中与激光轮廓特征点匹配的特征点集合。

(4.4)计算训练集与查询集中所匹配特征点间的距离之比，获得激光轮廓图相对于先验地图的尺度缩放率K，根据尺度缩放率K与激光轮廓图的尺寸确定分布先验粒子的区域。取激光轮廓图的宽和高分别为W_j、H_j，选择包含特征点长和宽分别为W_j*K和H_j*K的区域作为分布先验粒子的区域。

步骤五、优化初始化粒子朝向

初始化粒子时，不再全局分布粒子，只在激光轮廓线匹配得到的先验地图的候选区分布粒子。结合激光传感器扫描角度范围为钝角的特性，机器人的真实位姿必然位于激光轮廓线内。因此分布粒子时，只在激光轮廓线包含的区域内分布粒子。

根据步骤三的激光轮廓，简化粒子朝向的分布，使大部分粒子朝向接近真实位姿朝向的方向。如示意图7，分布粒子步骤如下：

1)分别连接激光轮廓线开口端点，生成n条线段。

2)分别作n条线线段的垂直平分线。

3)分布粒子时以n条垂直平分线指向激光轮廓的朝向为主。主要分布为n个高斯分布，高斯分布的均值分别为n条垂直平分线的朝向，方差为设定值。

步骤六、粒子的收敛

(6.1)重要性权值分配与位姿更新：每个粒子都被分配一个重要性权值，机器人初始化分布粒子的权重相同，并进行归一化处理。根据机器人的运动更新，粒子会根据里程计累计数据进行位姿更新。

(6.2)粒子权重更新与重采样：根据观测数据，更新每一个粒子的权重，并根据粒子的权重进行重采样，使粒子收敛到方差更小的高斯后验分布。随着迭代过程的进行，机器人的位姿能根据更精准的后验分布推测出。

本发明的有益效果：本发明在机器人启动时，不再全局分布粒子，而选择激光轮廓与先验地图轮廓匹配后，将表示机器人位姿的粒子分布在候选区域，并且大部分粒子朝向以机器人真实朝向为约束，从而在全局定位过程中，使粒子能快速收敛到机器人真实位置附近。

附图说明

图1为先验地图静态层示意图。

图2为先验地图静态层轮廓图。

图3为先验地图静态层轮廓图提取出的特征点。

图4为原始激光数据示意图。

图5为过滤杂点的激光数据示意图。

图6为激光轮廓图与先验地图轮廓匹配效果图。

图7为粒子朝向分布策略示意图。

具体实施方式

以下结合技术方案和附图详细叙述本发明的具体实施方式。

本方案选择测距范围为0.06m-30m的二维激光(Hokuyo UTM-30LX)作为激光测距传感器。该激光的二维平面扫描角度范围为270°，具有0.25°的角度分辨率，即270°范围内，每0.25°取一个样本点，即每一帧激光数据有1080个激光点，频率为40赫兹。

一种针对室内机器人任意点启动时粒子快速收敛的方法，步骤如下：

步骤一、构建先验地图

利用构图算法构造室内二维先验地图，选Cartographer算法进行先验地图的构建。场景选择相似程度较高的具有双闭环的走廊(如图1和图2所示)。运动控制载体选择两轮驱动式先锋3移动机器人，利用Cartographer算法构建已知环境的二维先验地图，先验地图只取静态层。

Cartographer算法在构造先验地图过程中，通过闭环检测消除了构图过程中里程计带来的误差。以子图作为闭环检测的基本单元，一定数量的激光观测数据构成子图，基于其已有的激光数据将一次观测数据***子图的最佳位置，生成一个子图后进行局部回环，所有的子图局部回环完成后再进行全局回环。Cartographer算法构造的先验地图针对相似环境具有很高的精度，构造先验地图效果如图1。

步骤二、提取先验地图轮廓与关键特征点

(2.1)对步骤一得到的先验地图进行灰度处理，即将先验地图图像由RGB图像转换为灰度图像。RGB图像每个像素点由R、G、B三个通道分量组成，灰度图像每个像素点值为单通道分量Y。将RGB图像转为灰度图像的公式如下：

Y＝α*R+β*G+γ*B (1)

其中，R、G、B表示图像三个通道的值，α、β、γ表示三个通道值在灰度图像像素值中的权重，Y表示灰度图像的像素值。

(2.2)将先验地图转换的灰度图像后，得到一个二维离散函数f(x,y)，其中(x,y)表示灰度图像中像素点的坐标，f(x,y)表示该坐标的像素值。求该二维离散函数在x方向和y方向上的梯度[G_x,G_y]^T：

利用差分求解公式(2)，得到图像梯度表达公式为：

利用公式(3)及图像梯度法提取出先验地图图像的轮廓(如图2所示)。

(2.3)利用公式(1)、(2)、(3)提取出先验地图图像轮廓以后，采用SIFT(Scale-invariant feature transform)算法对先验地图的轮廓图图像进行显式特征提取，提取出轮廓图的关键特征点，包含图像中的角点、拐点等兴趣点。提取出的显式特征可以提高匹配的精度。提取特征步骤如下：

1)将步骤(2.2)得到的图像轮廓作为二维观测数据Image(x,y)，并生成二维观测数据矩阵；

2)采用高斯函数对观测数据进行降采样处理生成图像金字塔，其中图像金字塔的层数确定为：

O＝[log(min(H,W))]-2 (4)

其中，H、W表示二维观测数据矩阵的行数和列数；

根据得到的图像金字塔，生成差分图像金字塔：

D(x,y)＝L(N+1)-L(N) (5)

其中，L(N+1)表示第N+1层降采样的图像，L(N)表示第N层降采样的图像。

3)由差分图像金字塔确定局部极值点，具体为：遍历差分图像金字塔的第二层到倒数第二层，分别比较每个像素点与周围像素点像素值的大小，当该像素点像素值大于或小于它的图像域和尺度域的所有相邻点时，即为极值点。

4)由差分图像金字塔中得到的局部极值点回至图像金字塔中，确定极值点的主要方向。局部极值点对应于图像金字塔中的局部区域即为要提取的关键点，分析图像金字塔中局部区域的方向直方图，方向直方图的峰值则代表了该关键点邻域梯度的方向，以方向直方图中最大值作为该关键点的主方向。

以上步骤提取出的先验地图的轮廓的特征点如图3。

步骤三、处理第一帧激光数据

对于第一次采集的激光数据，激光点相对于激光观测器越远的距离，激光点越稀疏，如图4。在图像轮廓匹配时，对稀疏的激光点进行噪声处理；遍历激光点云，对于激光点云中的一个激光点P_g，当其与最近激光点的距离大于所有的激光点间的平均距离时，将激光点P_g设定为孤点，将所有的孤点剔除(如图5)。将第一帧激光保存为图像格式，并按照步骤二所述方式提取第一帧激光图像的轮廓与关键特征点；

步骤四、采用线性最近邻knn搜索算法匹配激光轮廓与先验地图轮廓

(4.1)将激光轮廓图像作为训练集T{l₁,l₂,l₃,l₄...}，将先验地图轮廓图像作为查询集P{p₁,p₂,p₃,p₄....}。

(4.2)将训练集中的特征点与查询集中的特征点进行交叉匹配，即形成特征点集合{(l₁,p₁),(l₁,p₂),(l₁,p₃),(l₁,p₄),(l₂,p₁),(l₂,p₂)....}；该匹配形成的特征点对数庞大，也无法定位出激光轮廓对应于先验地图中的位置。

(4.3)将特征点集合进行过滤：对于每个训练集中的特征点l(x₁,y₁)，在查询集中选择K个与此特征点最相似的特征点p(x₁,y₁),p(x₂,y₂)....p(x_k,y_k)，当前k-1个点与第k点的欧式距离比率大于给定阈值，则前k-1个点为正确的匹配。此匹配算法使用的特征点为步骤(2.3)中提取出的SIFT特征点。使用该特征点进行匹配可消除激光轮廓尺度相对于先验地图轮廓尺度缩放、亮度变换、角度旋转带来的噪声。如图6，经过线性最近邻knn搜索算法，可获得先验地图中与激光轮廓特征点匹配的特征点集合。

(4.4)计算训练集与查询集中所匹配特征点间的距离之比，获得激光轮廓图相对于先验地图的尺度缩放率K，根据尺度缩放率K与激光轮廓图的尺寸确定分布先验粒子的区域。如图6中示意，特征点m、n分别与e、f对应，可得m、n之间的距离与e、f之间的距离之比等于激光轮廓图相对于先验地图的尺度缩放率K。取激光轮廓图的宽和高分别为W_j、H_j，选择包含特征点长和宽分别为W_j*K和H_j*K的区域作为分布先验粒子的区域。

步骤五、优化初始化粒子朝向

初始化粒子时，不再全局分布粒子，只在激光轮廓线匹配得到的先验地图的候选区分布粒子。结合激光传感器扫描角度范围为钝角的特性，机器人的真实位姿必然位于激光轮廓线内。因此分布粒子时，只在激光轮廓线包含的区域内分布粒子。

根据步骤三的激光轮廓，简化粒子朝向的分布，使大部分粒子朝向接近真实位姿朝向的方向。如示意图7，分布粒子步骤如下：

1)分别连接激光轮廓线开口端点，形成线段mn、de和kh。

2)分别作三条线段垂直平分线：O₁A、O₃C和O₂B。

3)分布粒子以O₁A、O₃C和O₂B朝向为主。主要分布为三个高斯分布，高斯分布的均值分别为O₁A、O₃C和O₂B的朝向，方差为初始化值。

步骤六、粒子的收敛

(6.1)重要性权值分配与位姿更新：每个粒子都被分配一个重要性权值，机器人初始化分布粒子的权重相同，并进行归一化处理。根据机器人的运动更新，粒子会根据里程计累计数据进行位姿更新。

(6.2)粒子权重更新与重采样：根据观测数据，更新每一个粒子的权重，并根据粒子的权重进行重采样，使粒子收敛到方差更小的高斯后验分布。随着迭代过程的进行，机器人的位姿能根据更精准的后验分布推测出。

利用线性最近邻knn搜索算法匹配激光轮廓与先验地图轮廓，其中最近邻knn搜索算法中k取2，最近邻点与此紧邻点的比值取0.6。匹配得到分布粒子区域，进行粒子更新。传统方法中全局分布粒子的数目为3000-5000个，控制机器人以0.4rad/s的角速度原地旋转，收敛时运行的角度为4π。本方法中，使用局部初始化粒子方案分布的粒子数目为200-400个，控制机器人以0.4rad/s的角速度原地旋转，收敛时运行的角度为π。

Claims (2)

1.一种针对室内机器人任意点启动时粒子快速收敛的方法，其特征在于，步骤如下：

步骤一、构建先验地图

利用构图算法构造室内二维先验地图；

步骤二、提取先验地图轮廓与关键特征点

(2.1)对步骤一得到的先验地图进行灰度处理，即将先验地图图像由RGB图像转换为灰度图像；RGB图像每个像素点由R、G、B三个通道分量组成，灰度图像每个像素值为单通道分量Y；将RGB图像转为灰度图像的公式如下：

Y＝α*R+β*G+γ*B (1)

其中，R、G、B表示图像三个通道的值，α、β、γ表示三个通道值在灰度图像像素值中的权重，Y表示灰度图像的像素值；

(2.2)将先验地图转换的灰度图像后，得到一个二维离散函数f(x,y)，其中(x,y)表示灰度图像中像素点的坐标，f(x,y)表示该坐标的像素值；求该二维离散函数在x方向和y方向上的梯度[G_x,G_y]^T：

利用差分求解公式(2)，得到图像梯度表达公式为：

利用公式(3)及图像梯度法提取出先验地图图像的轮廓；

(2.3)采用SIFT算法对先验地图的轮廓图图像进行显式特征提取，提取出轮廓图的关键特征点；具体步骤如下：

1)将步骤(2.2)得到的图像轮廓作为二维观测数据Image(x,y)，并生成二维观测数据矩阵；

2)采用高斯函数对观测数据矩阵进行降采样处理，生成图像金字塔，其中图像金字塔的层数确定为：

O＝[log(min(H,W))]-2 (4)

其中，H、W表示二维观测数据矩阵的行数和列数；

根据得到的图像金字塔，生成差分图像金字塔：

D(x,y)＝L(N+1)-L(N) (5)

其中，L(N+1)表示第N+1层降采样的图像，L(N)表示第N层降采样的图像；

3)由差分图像金字塔确定局部极值点，具体为：遍历差分图像金字塔的第二层到倒数第二层，分别比较每个像素点与周围像素点像素值的大小，当某一像素点像素值大于或小于它的图像域和尺度域的所有相邻点时，即为局部极值点；

4)由差分图像金字塔中得到的局部极值点回至图像金字塔中，确定局部极值点的主要方向；局部极值点对应于图像金字塔中的局部区域即为要提取的关键点，分析图像金字塔中局部区域的方向直方图，方向直方图的峰值则代表了该关键点邻域梯度的方向，以方向直方图中最大值作为该关键点的主方向；

步骤三、处理第一帧激光数据

对于第一次采集的激光数据，在图像轮廓匹配时，对稀疏的激光点进行噪声处理；遍历激光点云，对于激光点云中的一个激光点P_g，当其与最近激光点的距离大于所有的激光点间的平均距离时，将激光点P_g设定为孤点，将所有的孤点剔除；将第一帧激光保存为图像格式，并按照步骤二所述方式提取第一帧激光图像的轮廓与关键特征点；

步骤四、采用线性最近邻knn搜索算法匹配激光轮廓与先验地图轮廓

(4.1)将激光轮廓图像作为训练集T{l₁,l₂,l₃,l₄...}，将先验地图轮廓图像作为查询集P{p₁,p₂,p₃,p₄....}；

(4.2)将训练集中的特征点与查询集中的特征点进行交叉匹配，即形成特征点集合{(l₁,p₁),(l₁,p₂),(l₁,p₃),(l₁,p₄),(l₂,p₁),(l₂,p₂)....}；

(4.3)将特征点集合进行过滤：对于每个训练集中的特征点l(x₁,y₁)，在查询集中选择K个与此特征点最相似的特征点p(x₁,y₁),p(x₂,y₂)....p(x_k,y_k)，当前k-1个点与第k点的欧式距离比率大于给定阈值，则前k-1个点为正确的匹配；最终获得先验地图中与激光轮廓特征点匹配的特征点集合；

(4.4)计算训练集与查询集中所匹配特征点间的距离之比，获得激光轮廓图相对于先验地图的尺度缩放率K，根据尺度缩放率K与激光轮廓图的尺寸确定分布先验粒子的区域；取激光轮廓图的宽和高分别为W_j、H_j，选择包含特征点长和宽分别为W_j*K和H_j*K的区域作为分布先验粒子的区域；

步骤五、优化初始化粒子朝向

初始化粒子时，不再全局分布粒子，只在激光轮廓线匹配得到的先验地图的候选区分布粒子；根据步骤三的激光轮廓，简化粒子朝向的分布，使大部分粒子朝向接近真实位姿朝向的方向；分布粒子步骤如下：

1)分别连接激光轮廓线开口端点，生成n条线段；

2)分别作n条线线段的垂直平分线；

3)分布粒子时以n条垂直平分线指向激光轮廓的朝向为主；主要分布为n个高斯分布，高斯分布的均值分别为n条垂直平分线的朝向，方差为设定值；

步骤六、粒子的收敛

(6.1)重要性权值分配与位姿更新：每个粒子都被分配一个重要性权值，机器人初始化分布粒子的权重相同，并进行归一化处理；根据机器人的运动更新，粒子会根据里程计累计数据进行位姿更新；

(6.2)粒子权重更新与重采样：根据观测数据，更新每一个粒子的权重，并根据粒子的权重进行重采样，使粒子收敛到方差更小的高斯后验分布；随着迭代过程的进行，机器人的位姿能根据更精准的后验分布推测出。

2.根据权利要求1所述的一种针对室内机器人任意点启动时粒子快速收敛的方法，其特征在于，所述构图算法包括hector算法、Gmapping算法或Cartographer算法。