CN111273304A - 一种融合反光柱的自然定位方法及系统 - Google Patents

Abstract

本发明适用于机器人定位技术领域，提供了一种融合反光柱的自然定位方法及系统，该方法包括：S1、加载栅格地图及反光柱地图；S2、在非精定位区域，通过粒子滤波来计算移动机器人当前的位姿，即进行移动机器人的粗定位；S3、在进入精定位区域，将上一时刻粗定位的位姿作为精定位的初始位姿，在精定位区域通过反光柱来进行精定位。仅在需要精定位的区域布置反光柱，在粗定位区域无需布置反光柱，既满足了定位精度的需求，也减小了对应用场景的依赖。

Description

一种融合反光柱的自然定位方法及系统

技术领域

本发明属于机器人定位技术领域，提供了一种融合反光柱的自然定位方法 及系统。

背景技术

随着社会的发展和技术的进步，移动机器人越来越深的介入到人类的日常 生活中，例如家庭中的清洁机器人、工厂中的搬运机器人以及餐馆中的送餐机 器人等。移动机器人想要实现上述功能必须准确的知道自身所在的位置，移动 机器人定位导航常用的传感器有磁导航、摄像机和激光雷达等。其中磁导航的 导航路径单一，不利于扩展和更改，使用的局限性较大。基于视觉的定位导航 方法受光源条件影响大，稳定性和精度较差，无法让移动机器人进行稳定、精 密的操作。基于激光雷达的定位导航方法可以分为基于反光柱的定位导航方法 和基于轮廓的定位导航方法，其中基于反光柱的定位导航方法需要在环境中布 设大量的路标，应用场景受限制较多。基于轮廓的自然定位导航方法由于路径 灵活、不需要布置人工标志，越来越广泛的应用在移动机器人定位导航中。目 前基于轮廓定位的主流方法是基于粒子滤波的栅格定位方法，即通过运动模型 对机器人位姿进行采样生成大量粒子，依据传感器的观测结果更新粒子的权重 并进行重采样，不断迭代使粒子收敛。但是由于栅格地图分辨率及雷达探测精 度的影响，基于粒子滤波的栅格地图定位方法精度较差，无法满足移动机器人 叉、放货及高精度抓取的需求。

发明内容

本发明实施例提供一种融合反光柱的自然定位方法，仅在需要精定位的区 域布置反光柱，在粗定位区域无需布置反光柱，既满足了定位精度的需求，也 减小了对应用场景的依赖。

本发明是这样实现的，一种融合反光柱的自然定位方法，所述方法具体包 括如下步骤：

S1、加载栅格地图及反光柱地图；

S2、在非精定位区域，通过粒子滤波来计算移动机器人当前的位姿，即进 行移动机器人的粗定位；

S3、在进入精定位区域，将上一时刻粗定位的位姿作为精定位的初始位姿， 在精定位区域通过反光柱来进行精定位。

进一步的，基于反光柱的定位过程具体如下：

S31、提取当前激光帧中的反光柱数量；

S32、检测提取到的反光柱数量是否大于2，若检测结果为是，则基于动态 匹配匹配出当前激光帧中反光柱在反光柱地图对应的路标点；

S33、基于反光柱在激光雷达坐标系中的局部坐标及反光柱在反光柱地图坐 标系中的全局坐标，计算出激光雷达坐标系与反光柱坐标系之间的变换矩阵及 平移向量；

S34、基于变换矩阵及平移向量来获取激光雷达在反光柱地图中的坐标。

进一步的，动态匹配过程具体如下：

S321、遍历反光柱地图中的所有路标点，计算路标点到激光雷达的距离 dist，提取满足dist_min＜dist＜dist_max的路标点，dist_min为激光雷达的最小探测距离， dist_max为激光雷达的最大探测距离；

S322、将当前局部地图中的反光柱投影到反光柱地图中，获取各反光柱的 地图投影坐标，将距地图投影坐标的距离小于matchW的路标加入对应反光柱的 匹配列中，所有反光柱的匹配列即组成了匹配列表；

S323、从局部地图中选择三个反光柱r_i、r_j、r_k组成一个三角形，在三个反 光柱对应匹配列中查找该三角形的相似三角形，相似三角形由路标R_i、R_j、R_k构 成；

S324、在匹配列表的剩余匹配列中依次查找对应反光柱的最佳匹配路标， 在依次将各剩余反光柱及对应的最佳匹配路标加入当前匹配组合时，同步当前 匹配组合的匹配数目及匹配误差，最后获取该相似三角形的匹配组合，匹配数 目及匹配器误差；

S325、遍历局部地图中剩余的所有三角形，依次基于步骤S323及步骤S324 获取对应的相似三角形及相似三角形的匹配组合，匹配数量及匹配误差，输出 匹配数量最多的匹配组合；

匹配数目mCount为当前匹配组合中的反光柱数量，匹配误差mErr为反光柱 与对应最佳匹配路标的匹配误差的累加。

进一步的，反光柱r的最佳匹配路标R的获取方法具体如下：

S3241、计算反光柱r到当前匹配组合中每个反光柱的距离d，

S3242、反光柱r的匹配列中提取一个路标，计算该路标到当前匹配组合中 每个路标的距离D，

S3243、基于距离d及对应的距离D计算反光柱r与该路标的匹配误差，若 匹配误差小于matchW，则记录该路标的匹配误差；

S3244、遍历反光柱r的匹配列中的剩余路标，基于步骤S3242及步骤S3243 依次计算反光柱r与匹配列中的剩余路标的匹配误差，获取最小的匹配误差， 若最小的匹配误差小于matchW*(mCount+1)，则最小匹配误差对应的路标即为反 光柱r的最佳匹配路标。

进一步的，当反光柱地图中的三个路标R_i、R_j、R_k满足公式(1)时，则认 定R_i、R_j、R_k三个路标组成的三角形即为反光柱r_i、r_j、r_k组成三角形的相似三 角形，得到当前匹配组合mComb＝{r_i:R_i；r_j:R_j；r_k:R_k}，匹配数目mCount＝3，匹配 误差mErr＝Δ_i,j+Δ_i,k+Δ_k,j，公式(1)具体如下：

其中，matchW为反光柱匹配允许的误差。

进一步的，所述步骤S1具体包括如下步骤：

S11、初始化粒子分布：确定移动机器人在栅格地图上的初始位姿，在初始 位姿周围产生随机粒子

S12、运动模型更新：根据里程计反馈的位移增量(Δx,Δy,Δθ)，更新每一个 粒子的位姿；

S13、激光传感器观测模型更新：根据随机粒子在全局地图中的位置，采用 似然域计算各随机粒子的权重，并归一化随机粒子的权重值

S14、进行重采样：筛选出权重值大的随机粒子，同时增加随机粒子筛选出 权重值大的随机粒子，同时增加随机粒子

S15、粒子滤波器迭代更新：重采样后，更新粒子分布，使得每个粒子的权 重相等，更新粒子簇的最大权重，若粒子的最大权重大于权重阈值，则最大权 重粒子簇的平均位置即为最佳位置，即移动机器人当前在全局地图中的位置。

本发明是这样实现的，一种融合反光柱自然定位系统，所述系统包括：

布置于精定位区域的反光柱，反光柱垂直于地面设置；

设于移动机器人上的激光雷达，激光雷达与处理器连接，处理器与存储器 连接，激光雷达扫描行驶区域的行驶环境，并将扫描数据发送至处理器，处理 器基于权利要求1至6任一权利要求所述融合反光柱的自然定位方法来进行移 动机器人的定位；存储器中存储有栅格地图及反光柱地图。

本发明提供的融合反光柱的自然定位方法具有如下有益技术效果：

1)仅在需要精定位的区域布置反光柱，在粗定位区域无需布置反光柱，既 满足了定位精度的需求，也减小了对应用场景的依赖；2)基于三角生长法进行 反光柱的匹配，提高了匹配的速度，有效的滤除了干扰反光柱，匹配的可靠性 提高。

附图说明

图1为本发明实施例提供的融合反光柱自然定位方法流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实 施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅 仅用以解释本发明，并不用于限定本发明。

将激光雷达水平放置，反光柱布置于环境中的精定位区域，反光柱垂直于 地面张贴，激光雷达的扫描线与反光柱的中点大致对齐，

图1为本发明实施例提供的融合反光柱自然定位方法流程图，该方法包括 如下步骤：

S1、加载栅格地图及反光柱地图；

混合地图包括栅格地图和反光柱地图，栅格地图是通过SLAM算法构建的， 并保存到本地进行自然导航使用的地图；反光柱地图是构建栅格地图时识别到 的反光柱，并计算得到在栅格地图坐标系中的路标点集合。

S2、在非精定位区域，通过粒子滤波计算移动机器人当前的位姿，即进行 移动机器人的粗定位，其定位方法具体包括如下步骤：

S21、初始化粒子分布：确定移动机器人在栅格地图上的初始位姿，在初始 位姿周围产生随机粒子；

S22、运动模型更新：根据里程计反馈的位移增量(Δx,Δy,Δθ)，更新每一个 粒子的位姿，对于粒子p_i(x_i,y_i,θ_i)，位姿更新如下所示：

S23、激光传感器观测模型更新：根据随机粒子在反光柱地图中的位置，采 用似然域计算各随机粒子的权重，并归一化随机粒子的权重值，在本发明实施 例中，似然域模型表示具体如下：

其中，dist表示随机粒子距最近障碍物距离，q表示测距似然，代表粒子的 权重，(x_k,y_k)为随机粒子在反光柱地图(栅格地图)的坐标，(x,y)为距离最近 的障碍物坐标，z_hit、z_randdom及z_max分别代表测距误差混合权重的不同部分，分别 代表测量噪声、无法解释的随机测量及测量失败，σ_hit为测量噪声的标准差。

S24、进行重采样：筛选出权重值大的随机粒子，同时增加随机粒子，随机 粒子以以下概率增加，概率的计算模型具体如下：

max{0.0，1.0-w_fast/w_slaw}

其中，w_slow表示短期似然平均，w_fast表示长期似然平均，其计算公式具体如 下：

其中，w_avg表示经验测量似然，α_slow表示短期平均的指数滤波器的衰减率， α_fast表示长期平均的指数滤波器的衰减率。

S25、粒子滤波器迭代更新：重采样后，更新粒子分布，使得每个粒子的权 重相等，更新粒子簇的最大权重，若粒子的最大权重大于权重阈值，则最大权 重粒子簇的平均位置即为最佳位置，即移动机器人当前在反光柱地图中的位置。

在本发明实施例中，粒子簇的权重为粒子簇中各粒子权重的总和。

S3、在进入精定位区域，将上一时刻粗定位的位姿作为精定位的初始位姿， 在精定位区域通过反光柱来进行精定位，基于反光柱的定位方法具体包括如下 步骤：

上一帧定位成功得到激光雷达位姿pose(x,y,θ)，基于反光板的定位过程具 体如下：

S31、提取当前激光帧中的反光柱数量；

S32、检测提取到的反光柱数量是否大于2，若检测结果为是，则基于动态 匹配匹配出当前激光帧中反光柱子反光柱地图中对应的路标点，

S33、基于反光柱在激光雷达坐标系中的局部坐标及反光柱在反光柱地图坐 标系中的全局坐标，计算出激光雷达坐标系与反光柱坐标系之间的变换矩阵及 平移向量；

S34、基于变换矩阵及平移向量来获取激光雷达在反光柱地图中的坐标，因 为激光雷达的局部坐标为

则激光雷达在地图中的坐标X^scan＝t，激光雷 达相对于地图坐标系的旋转角θ可由旋转矩阵R分解得到。

在本发明实施例中，动态匹配过程具体如下：

S321、探测距离约束：遍历反光柱地图中的所有路标点 R₁(X₁,Y₁),…,R_m(X_m,Y_m)，计算路标点到激光雷达的距离dist，提取满足 dist_min＜dist＜dist_max的路标点，dist_min为激光雷达的最小探测距离，dist_max为激光雷 达的最大探测距离；

S322、将当前局部地图中的反光柱投影到反光柱地图中，获取各反光柱的 地图投影坐标，将距地图投影坐标的距离小于matchW的路标加入对应反光柱的 匹配列中，所有反光柱的匹配列即组成了匹配列表，一个反光柱的匹配列中可 能存在多个路标r₁:R_i,...；...；r_n:R_j,...；

将将局部地图中的反光柱r(θ,d)投影到反光柱地图中，地图投影坐标 P_r(x_r,y_r)计算方法如下：

其中，θ为激光雷达的扫描角度，d为反光柱距激光雷达的距离。

S323、从局部地图中选择三个反光柱r_i、r_j、r_k组成一个三角形，在三个反 光柱对应匹配列中查找该三角形的相似三角形，相似三角形由路标R_i、R_j、R_k构 成，R_i、R_j、R_k分别为反光柱r_i、r_j、r_k对应匹配列中的一个路标，当三个路标R_i、R_j、R_k满足公式(1)时，则认定R_i、R_j、R_k三个路标组成的三角形即为 反光柱r_i、r_j、r_k组成三角形的相似三角形，得到当前匹配组合 mComb＝{r_i:R_i；r_j:R_j；r_k:R_k}，匹配数目mCount＝3，匹配误差mErr＝Δ_i,j+Δ_i,k+Δ_k,j，公式(1)具体如下：

其中，matchW为反光柱匹配允许的误差，d_i,j、d_i,k、d_k,j分别为反光柱r_i与反 光柱r_j在局部地图中的距离、反光柱r_i与反光柱r_k在局部地图中的距离、反光柱 r_k与反光柱r_j在局部地图中的距离，D_i,j、D_i,k、D_k,j分别为路标R_i与路标R_j在反光 柱地图中的距离、路标R_i与路标R_k在反光柱地图中的距离、路标R_k与路标R_j在 反光柱地图中的距离。

S324、在匹配列表的剩余匹配列中依次查找对应反光柱的最佳匹配路标， 依次将各剩余反光柱及对应的最佳匹配路标加入当前匹配组合，并同步当前匹 配组合的匹配数目及匹配误差，即为该相似三角形的匹配组合，匹配数目及匹 配器误差；

S325、遍历局部地图中剩余的所有三角形，依次基于步骤S323及步骤S324 获取对应的相似三角形及相似三角形的匹配组合，匹配数量及匹配误差，输出 匹配数量最多的匹配组合，匹配数目mCount为当前匹配组合中的反光柱数量， 匹配误差mErr为反光柱与对应最佳匹配路标的匹配误差的累加。

在本发明实施例中，反光柱r的最佳匹配路标R的获取方法具体如下：

S3241、计算反光柱r到当前匹配组合中每个反光柱的距离d，

S3242、反光柱r的匹配列中提取一个路标，计算该路标到当前匹配组合中 每个路标的距离D，

S3243、基于距离d及对应的距离D计算反光柱r与该路标的匹配误差，若 匹配误差小于matchW，则记录该路标的匹配误差；

S3244、遍历反光柱r的匹配列中的剩余路标，基于步骤S3242及步骤S3243 依次计算反光柱r与匹配列中的剩余路标的匹配误差，获取最小的匹配误差， 若最小的匹配误差小于matchW*(mCount+1)，则最小匹配误差对应的路标即为反 光柱r的最佳匹配路标，若最小的匹配误差大于matchW*(mCount+1)，则当前反 光柱r匹配的路标为空；

假定当前匹配组合mComb＝{r_i:R_i；r_j:R_j；r_k:R_k；r_m:R_m}，反光柱r_n与路标R_n的匹 配误差的计算方法为：

依次计算反光柱r_n到反光柱r_i、反光柱r_j、反光柱r_k、反光柱r_m的距离d_ni、 d_nj、d_nk及d_nm；依次计算路标R_n到路标R_i、路标R_j、路标R_k、路标R_m的距离D_ni、 D_nj、D_nk及D_nm，匹配误差∑Δ＝Δ_ni+Δ_nj+Δ_nk+Δ_nm，其中，Δ_n,i＝|d_n,i-D_n,i|， Δ_n,j＝|d_n,j-D_n,j|，Δ_n,k＝|d_n,k-D_n,k|，Δ_n,m＝|d_n,m-D_n,m|。

本发明还提供一种融合反光柱自然定位系统，该系统包括：

布置于精定位区域的反光柱，反光柱垂直于地面设置；设于移动机器人上 的激光雷达，激光雷达与处理器连接，处理器与存储器连接，激光雷达扫描行 驶区域(由精定位区域和非精定位区域组成)的行驶环境，并将扫描数据发送 至处理器，处理器基于上述融合反光柱的自然定位方法来进行移动机器人的定 位；存储器中存储有栅格地图及反光柱地图。

本发明提供的融合反光柱的自然定位方法具有如下有益技术效果：

1)仅在需要精定位的区域布置反光柱，在粗定位区域无需布置反光柱，既 满足了定位精度的需求，也减小了对应用场景的依赖；2)基于三角生长法进行 反光柱的匹配，提高了匹配的速度，有效的滤除了干扰反光柱，匹配的可靠性 提高。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发 明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明 的保护范围之内。

Claims (7)

1.一种融合反光柱的自然定位方法，其特征在于，所述方法具体包括如下步骤：

S1、加载栅格地图及反光柱地图；

S2、在非精定位区域，通过粒子滤波来计算移动机器人当前的位姿，即进行移动机器人的粗定位；

S3、在进入精定位区域，将上一时刻粗定位的位姿作为精定位的初始位姿，在精定位区域通过反光柱来进行精定位。

2.如权利要求1所述融合反光柱的自然定位方法，其特征在于，基于反光柱的定位过程具体如下：

S31、提取当前激光帧中的反光柱数量；

S32、检测提取到的反光柱数量是否大于2，若检测结果为是，则基于动态匹配匹配出激光帧中反光柱在反光柱地图对应的路标点；

S33、基于反光柱在激光雷达坐标系中的局部坐标及反光柱在反光柱地图坐标系中的全局坐标，计算出激光雷达坐标系与反光柱坐标系之间的变换矩阵及平移向量；

S34、基于变换矩阵及平移向量来获取激光雷达在反光柱地图中的坐标。

3.如权利要求2所述融合反光柱的自然定位方法，其特征在于，动态匹配过程具体如下：

S321、遍历反光柱地图中的所有路标点，计算路标点到激光雷达的距离dist，提取满足dist_min＜dist＜dist_max的路标点，dist_min为激光雷达的最小探测距离，dist_max为激光雷达的最大探测距离；

S322、将当前局部地图中的反光柱投影到反光柱地图中，获取各反光柱的地图投影坐标，将距地图投影坐标的距离小于matchW的路标加入对应反光柱的匹配列中，所有反光柱的匹配列即组成了匹配列表；

S323、从局部地图中选择三个反光柱r_i、r_j、r_k组成一个三角形，在三个反光柱对应匹配列中查找该三角形的相似三角形，相似三角形由路标R_i、R_j、R_k构成；

S324、在匹配列表的剩余匹配列中依次查找对应反光柱的最佳匹配路标，在依次将各剩余反光柱及对应的最佳匹配路标加入当前匹配组合时，同步当前匹配组合的匹配数目及匹配误差，最后获取该相似三角形的匹配组合，匹配数目及匹配器误差；

S325、遍历局部地图中剩余的所有三角形，依次基于步骤S323及步骤S324获取对应的相似三角形及相似三角形的匹配组合，匹配数量及匹配误差，输出匹配数量最多的匹配组合；

匹配数目mCount为当前匹配组合中的反光柱数量，匹配误差mErr为反光柱与对应最佳匹配路标的匹配误差的累加。

4.如权利要求3所述融合反光柱的自然定位方法，其特征在于，其特征在于，反光柱r的最佳匹配路标R的获取方法具体如下：

S3241、计算反光柱r到当前匹配组合中每个反光柱的距离d，

S3242、反光柱r的匹配列中提取一个路标，计算该路标到当前匹配组合中每个路标的距离D，

S3243、基于距离d及对应的距离D计算反光柱r与该路标的匹配误差，若匹配误差小于matchW，则记录该路标的匹配误差；

S3244、遍历反光柱r的匹配列中的剩余路标，基于步骤S3242及步骤S3243依次计算反光柱r与匹配列中的剩余路标的匹配误差，获取最小的匹配误差，若最小的匹配误差小于matchW*(mCount+1)，则最小匹配误差对应的路标即为反光柱r的最佳匹配路标。

5.如权利要求3或4所述所述融合反光柱的自然定位方法，其特征在于，当反光柱地图中的三个路标R_i、R_j、R_k满足公式(1)时，则认定R_i、R_j、R_k三个路标组成的三角形即为反光柱r_i、r_j、r_k组成三角形的相似三角形，得到当前匹配组合mComb＝{r_i:R_i；r_j:R_j；r_k:R_k}，匹配数目mCount＝3，匹配误差mErr＝Δ_i,j+Δ_i,k+Δ_k,j，公式(1)具体如下：

其中，matchW为反光柱匹配允许的误差。

6.如权利要求1所述融合反光柱的自然定位方法，其特征在于，所述步骤S1具体包括如下步骤：

S11、初始化粒子分布：确定移动机器人在栅格地图上的初始位姿，在初始位姿周围产生随机粒子

S12、运动模型更新：根据里程计反馈的位移增量(Δx,Δy,Δθ)，更新每一个粒子的位姿；

S13、激光传感器观测模型更新：根据随机粒子在全局地图中的位置，采用似然域计算各随机粒子的权重，并归一化随机粒子的权重值

S14、进行重采样：筛选出权重值大的随机粒子，同时增加随机粒子筛选出权重值大的随机粒子，同时增加随机粒子

S15、粒子滤波器迭代更新：重采样后，更新粒子分布，使得每个粒子的权重相等，更新粒子簇的最大权重，若粒子的最大权重大于权重阈值，则最大权重粒子簇的平均位置即为最佳位置，即移动机器人当前在全局地图中的位置。

7.一种融合反光柱自然定位系统，其特征在于，所述系统包括：

布置于精定位区域的反光柱，反光柱垂直于地面设置；

设于移动机器人上的激光雷达，激光雷达与处理器连接，处理器与存储器连接，激光雷达扫描行驶区域的行驶环境，并将扫描数据发送至处理器，处理器基于权利要求1至6任一权利要求所述融合反光柱的自然定位方法来进行移动机器人的定位；存储器中存储有栅格地图及反光柱地图。

Images