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

摘要

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

主权项

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方向上的梯度[Gx,Gy]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)由差分图像金字塔中得到的局部极值点回至图像金字塔中，确定局部极值点的主要方向；局部极值点对应于图像金字塔中的局部区域即为要提取的关键点，分析图像金字塔中局部区域的方向直方图，方向直方图的峰值则代表了该关键点邻域梯度的方向，以方向直方图中最大值作为该关键点的主方向；步骤三、处理第一帧激光数据对于第一次采集的激光数据，在图像轮廓匹配时，对稀疏的激光点进行噪声处理；遍历激光点云，对于激光点云中的一个激光点Pg，当其与最近激光点的距离大于所有的激光点间的平均距离时，将激光点Pg设定为孤点，将所有的孤点剔除；将第一帧激光保存为图像格式，并按照步骤二所述方式提取第一帧激光图像的轮廓与关键特征点；步骤四、采用线性最近邻knn搜索算法匹配激光轮廓与先验地图轮廓(4.1)将激光轮廓图像作为训练集T{l1,l2,l3,l4...}，将先验地图轮廓图像作为查询集P{p1,p2,p3,p4....}；(4.2)将训练集中的特征点与查询集中的特征点进行交叉匹配，即形成特征点集合{(l1,p1),(l1,p2),(l1,p3),(l1,p4),(l2,p1),(l2,p2)....}；(4.3)将特征点集合进行过滤：对于每个训练集中的特征点l(x1,y1)，在查询集中选择K个与此特征点最相似的特征点p(x1,y1),p(x2,y2)....p(xk,yk)，当前k‑1个点与第k点的欧式距离比率大于给定阈值，则前k‑1个点为正确的匹配；最终获得先验地图中与激光轮廓特征点匹配的特征点集合；(4.4)计算训练集与查询集中所匹配特征点间的距离之比，获得激光轮廓图相对于先验地图的尺度缩放率K，根据尺度缩放率K与激光轮廓图的尺寸确定分布先验粒子的区域；取激光轮廓图的宽和高分别为Wj、Hj，选择包含特征点长和宽分别为Wj*K和Hj*K的区域作为分布先验粒子的区域；步骤五、优化初始化粒子朝向初始化粒子时，不再全局分布粒子，只在激光轮廓线匹配得到的先验地图的候选区分布粒子；根据步骤三的激光轮廓，简化粒子朝向的分布，使大部分粒子朝向接近真实位姿朝向的方向；分布粒子步骤如下：1)分别连接激光轮廓线开口端点，生成n条线段；2)分别作n条线线段的垂直平分线；3)分布粒子时以n条垂直平分线指向激光轮廓的朝向为主；主要分布为n个高斯分布，高斯分布的均值分别为n条垂直平分线的朝向，方差为设定值；步骤六、粒子的收敛(6.1)重要性权值分配与位姿更新：每个粒子都被分配一个重要性权值，机器人初始化分布粒子的权重相同，并进行归一化处理；根据机器人的运动更新，粒子会根据里程计累计数据进行位姿更新；(6.2)粒子权重更新与重采样：根据观测数据，更新每一个粒子的权重，并根据粒子的权重进行重采样，使粒子收敛到方差更小的高斯后验分布；随着迭代过程的进行，机器人的位姿能根据更精准的后验分布推测出。