本发明适用于反光柱匹配技术领域，提供了一种反光柱的快速匹配方法，所述方法具体包括如下步骤：S1、检测是否存在上一时刻的位姿；S2、若检测结果为是，则基于静态匹配将扫描到的反光柱与全局地图中的路标进行匹配，若检测结果为否，则基于动态匹配将扫描到的反光柱与全局地图中的路标进行匹配。基于三角生长法进行反光柱的匹配，提高了匹配的速度，有效的滤除了干扰反光柱，匹配的可靠性提高。
1.一种反光柱的快速匹配方法，其特征在于，所述方法具体包括如下步骤：S1、检测是否存在上一时刻的位姿；
S2、若检测结果为否，则基于静态匹配将扫描到的反光柱与全局地图中的路标进行匹配，若检测结果为是，则基于动态匹配将扫描到的反光柱与全局地图中的路标进行匹配；
S21、计算全局距离列表及局部距离列表，全局距离列表中记录了全局地图中各路标点之间的距离，局部距离列表记录了局部地图中各反光柱之间的距离；
S22、在局部地图中选择三个反光柱ri、rj、rk组成一个三角形，在全局地图中查找该三角形的相似三角形，相似三角形由三个路标Ri、Rj、Rk组成，得到当前匹配组合mComb＝{ri:Ri；rj:Rj；rk:Rk}；
当全局地图中的三个路标Ri、Rj、Rk满足公式(1)时，则认定Ri、Rj、Rk三个路标组成的三角形即为反光柱ri、rj、rk组成三角形的相似三角形，得到当前匹配组合mComb＝{ri:Ri；rj:Rj；rk:Rk}，匹配数目mCount＝3，匹配误差mErr＝Δi,j+Δi,k+Δk,j，公式(1)具体如下：其中，matchW为反光柱匹配允许的误差，di,j、di,k、dk,j分别为反光柱ri与反光柱rj在局部地图中的距离、反光柱ri与反光柱rk在局部地图中的距离、反光柱rk与反光柱rj在局部地图中的距离；Di,j、Di,k、Dk,j分别为路标Ri与路标Rj在全局地图中的距离、路标Ri与路标Rk在全局地图中的距离、路标Rk与路标Rj在全局地图中的距离；
S23、在全局地图的剩余路标中依次查找局部地图中各剩余反光柱的最佳匹配路标，依次将各剩余反光柱及对应的最佳匹配路标加入当前匹配组合时，同步更新当前匹配组合的匹配数目及匹配误差，最后获取该相似三角形的匹配组合，匹配数目及匹配误差；
S24、遍历局部地图中的剩余三角形，依次基于步骤S22及步骤S23获取对应的相似三角形及相似三角形的匹配组合，匹配数目及匹配误差，输出匹配数目最多的匹配组合；
匹配数目mCount为当前匹配组合中的反光柱数量。
2.如权利要求1所述反光柱的快速匹配方法，其特征在于，静态匹配过程中反光柱r的最佳匹配路标R的获取方法具体如下：
S231、计算反光柱r到当前匹配组合中每个反光柱的距离d，S232、在全局地图的剩余路标中提取一个路标，计算该路标到当前匹配组合中每个路标的距离D；
S233、基于距离d及对应的距离D计算反光柱r与该路标的匹配误差，若匹配误差小于matchW，则记录该路标的匹配误差，matchW为反光柱匹配允许的误差；
S234、遍历全局地图中剩余路标，基于步骤S232及步骤S233依次获取反光柱r与全局地图中剩余路标的匹配误差，若最小匹配误差小于matchW*(mCount+1)，则最小匹配误差对应的路标即为反光柱r的最佳匹配路标。
3.如权利要求1所述反光柱的快速匹配方法，其特征在于，动态匹配过程具体如下：S31、遍历全局地图中的所有路标点，计算路标点到激光雷达的距离dist，提取满足distmin＜dist＜distmax的路标点，distmin为激光雷达的最小探测距离，distmax为激光雷达的最大探测距离；
S32、将当前局部地图中的反光柱投影到全局地图中，获取各反光柱的地图投影坐标，将距地图投影坐标的距离小于matchW的路标加入对应反光柱的匹配列中，所有反光柱的匹配列即组成了匹配列表；
S33、从局部地图中选择三个反光柱ri、rj、rk组成一个三角形，在三个反光柱对应匹配列中查找该三角形的相似三角形，相似三角形由路标Ri、Rj、Rk构成，Ri、Rj、Rk分别为反光柱ri、rj、rk对应匹配列中的一个路标，当三个路标Ri、Rj、Rk满足公式(1)时，则认定Ri、Rj、Rk三个路标组成的三角形即为反光柱ri、rj、rk组成三角形的相似三角形，得到当前匹配组合mComb＝{ri:Ri；rj:Rj；rk:Rk}，匹配数目mCount＝3，匹配误差mErr＝Δi,j+Δi,k+Δk,j，公式(1)具体如下：
其中，matchW为反光柱匹配允许的误差，di,j、di,k、dk,j分别为反光柱ri与反光柱rj在局部地图中的距离、反光柱ri与反光柱rk在局部地图中的距离、反光柱rk与反光柱rj在局部地图中的距离，Di,j、Di,k、Dk,j分别为路标Ri与路标Rj在全局地图中的距离、路标Ri与路标Rk在全局地图中的距离、路标Rk与路标Rj在全局地图中的距离；
S34、在匹配列表的剩余匹配列中依次查找对应反光柱的最佳匹配路标，在依次将各剩余反光柱及对应的最佳匹配路标加入当前匹配组合时，同步当前匹配组合的匹配数目及匹配误差，最后获取该相似三角形的匹配组合，匹配数目及匹配误差；
S35、遍历局部地图中剩余的所有三角形，依次基于步骤S33及步骤S34获取对应的相似三角形及相似三角形的匹配组合，匹配数目及匹配误差，输出匹配数目最多的匹配组合；
匹配数目mCount为当前匹配组合中的反光柱数量。
4.如权利要求3所述反光柱的快速匹配方法，其特征在于，动态匹配过程中反光柱r的最佳匹配路标R的获取方法具体如下：
S341、计算反光柱r到当前匹配组合中每个反光柱的距离d，S342、在反光柱r的匹配列中提取一个路标，计算该路标到当前匹配组合中每个路标的距离D，
S343、基于距离d及对应的距离D计算反光柱r与该路标的匹配误差，若匹配误差小于matchW，则记录该路标的匹配误差，matchW为反光柱匹配允许的误差；
S344、遍历反光柱r的匹配列中的剩余路标，基于步骤S342及步骤S343依次计算反光柱r与匹配列中的剩余路标的匹配误差，获取最小的匹配误差，若最小的匹配误差小于matchW*(mCount+1)，则最小匹配误差对应的路标即为反光柱r的最佳匹配路标。
一种反光柱的快速匹配方法
技术领域
[0001] 本发明属于反光柱匹配技术领域，提供了一种反光柱的快速匹配方法。
背景技术
[0002] 随着社会的发展和技术的进步，移动机器人越来越深的介入到人类的日常生活中，例如家庭中的清洁机器人、工厂中的搬运机器人以及餐馆中的送餐机器人等。移动机器
人想要实现上述功能必须要准确的知道自身所在的位置，即实时定位。而移动机器人实现
实时定位的先决条件是建立地图，这是移动机器人导航和实施其它智能行为的关键。移动
机器人导航常用的传感器有磁导航、摄像机和激光雷达等。其中磁导航的导航路径单一，不
利于扩展和更改，使用的局限性较大。基于视觉的导航方法受光源条件影响大，稳定性和精
度较差，无法让移动机器人进行稳定、精密的操作。基于激光雷达的导航方法可以分为基于
反光柱的导航方法和基于轮廓的导航方法，其中基于轮廓的定位方法计算量大、精度较差，
无法适应移动机器人精密操作的要求；基于反光柱的激光雷达定位方法精度高、稳定性好。
基于反光柱的激光雷达定位导航系统的关键就是反光柱匹配，能否快速、准确将扫描到的
反光柱与地图中的路标点匹配，直接关系到地图构建和定位过程的速度和可靠性；现有的
大多是基于上一帧的位姿将当前激光帧中的反光柱映射到地图中，将地图中获取距映射点
最近的路标点确定为该反光柱的匹配路标点，存在定位精度不高的问题。
发明内容
[0003] 本发明实施例提供一种反光柱的快速匹配方法，旨在提高反光柱的匹配精度。
[0004] 本发明是这样实现的，一种反光柱的快速匹配方法，所述方法具体包括如下步骤：
[0005] S1、检测是否存在上一时刻的位姿；
[0006] S2、若检测结果为是，则基于静态匹配将扫描到的反光柱与全局地图中的路标进行匹配，若检测结果为否，则基于动态匹配将扫描到的反光柱与全局地图中的路标进行匹
配。
[0007] 进一步的，静态匹配过程具体如下：
[0008] S21、计算全局距离列表及局部距离列表，全局距离列表中记录了全局地图中各路标点之间的距离，局部距离列表记录了局部地图中各反光柱之间的距离；
[0009] S22、在局部地图中选择三个反光柱ri、rj、rk组成一个三角形，在全局地图中查找该三角形的相似三角形，相似三角形由三个路标Ri、Rj、Rk组成；
[0010] S23、在全局地图的剩余路标中依次查找局部地图中各剩余反光柱的最佳匹配路标，依次将各剩余反光柱及对应的最佳匹配路标加入当前匹配组合时，同步更新当前匹配
组合的匹配数目及匹配误差，最后获取该相似三角形的匹配组合，匹配数目及匹配器误差；
[0011] S24、遍历局部地图中的剩余三角形，依次基于步骤S22及步骤S23获取对应的相似三角形及相似三角形的匹配组合，匹配数量及匹配误差，输出匹配数量最多的匹配组合；
[0012] 匹配数目mCount为当前匹配组合中的反光柱数量，匹配误差mErr为反光柱与对应最佳匹配路标的匹配误差的累加。
[0013] 进一步的，反光柱r的最佳匹配路标R的获取方法具体如下：
[0014] S231、计算反光柱r到当前匹配组合中每个反光柱的距离d，
[0015] S232、在全局地图中剩余路标中提取一个路标，计算该路标到当前匹配组合中每个路标的距离D；
[0016] S233、基于距离d及对应的距离D计算反光柱r与该路标的匹配误差，若匹配误差小于matchW，则记录该路标的匹配误差；
[0017] S234、遍历全局地图中剩余路标，依次基于步骤S232及步骤S233依次获取反光柱r与全局地图中剩余路标的匹配误差，若最小匹配误差小于 matchW*(mCount+1)，则最小匹
配误差对应的路标即为反光柱r的最佳匹配路标。
[0018] 进一步的，动态匹配过程具体如下：
[0019] S31、遍历全局地图中的所有路标点，计算路标点到激光雷达的距离dist，提取满足distmin＜dist＜distmax的路标点，distmin为激光雷达的最小探测距离，distmax为激光雷
达的最大探测距离；
[0020] S32、将当前局部地图中的反光柱投影到全局地图中，获取各反光柱的地图投影坐标，将距地图投影坐标的距离小于matchW的路标加入对应反光柱的匹配列中，所有反光柱
的匹配列即组成了匹配列表；
[0021] S33、从局部地图中选择三个反光柱ri、rj、rk组成一个三角形，在三个反光柱对应匹配列中查找该三角形的相似三角形，相似三角形由路标Ri、Rj、Rk构成；
[0022] S34、在匹配列表的剩余匹配列中依次查找对应反光柱的最佳匹配路标，在依次将各剩余反光柱及对应的最佳匹配路标加入当前匹配组合时，同步当前匹配组合的匹配数目
及匹配误差，最后获取该相似三角形的匹配组合，匹配数目及匹配器误差；
[0023] S35、遍历局部地图中剩余的所有三角形，依次基于步骤S33及步骤S34 获取对应的相似三角形及相似三角形的匹配组合，匹配数量及匹配误差，输出匹配数量最多的匹配
组合；
[0024] 匹配数目mCount为当前匹配组合中的反光柱数量，匹配误差mErr为反光柱与对应最佳匹配路标的匹配误差的累加。
[0025] 进一步的，反光柱r的最佳匹配路标R的获取方法具体如下：
[0026] S341、计算反光柱r到当前匹配组合中每个反光柱的距离d，
[0027] S342、反光柱r的匹配列中提取一个路标，计算该路标到当前匹配组合中每个路标的距离D，
[0028] S343、基于距离d及对应的距离D计算反光柱r与该路标的匹配误差，若匹配误差小于matchW，则记录该路标的匹配误差；
[0029] S344、遍历反光柱r的匹配列中的剩余路标，基于步骤S342及步骤S343 依次计算反光柱r与匹配列中的剩余路标的匹配误差，获取最小的匹配误差，若最小的匹配误差小于
matchW*(mCount+1)，则最小匹配误差对应的路标即为反光柱r的最佳匹配路标。
[0030] 进一步的，当全局地图中的三个路标Ri、Rj、Rk满足公式(1)时，则认定 Ri、Rj、Rk三个路标组成的三角形即为反光柱ri、rj、rk组成三角形的相似三角形，得到当前匹配组合
mComb＝{ri：Ri；rj：Rj；rk：Rk}，匹配数目mCount＝3，匹配误差mErr＝Δi，j+Δi，k+Δk，j，公式
(1)具体如下：
[0031]
[0032] 其中，matchW为反光柱匹配允许的误差。
[0033] 本发明提供的反光柱快速匹配方法具有如下有益技术效果：基于三角生长法进行反光柱的匹配，提高了匹配的速度，有效的滤除了干扰反光柱，匹配的可靠性提高。
附图说明
[0034] 图1为本发明实施例提供的反光柱的快速匹配方法的流程图。
具体实施方式
[0035] 为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并
不用于限定本发明。
[0036] 图1为本发明实施例提供的反光柱的快速匹配方法的流程图，该方法具体包括如下步骤：
[0037] S1、检测是否存在上一时刻的位姿；
[0038] S2、若检测结果为是，则基于静态匹配将扫描到的反光柱与全局地图中的路标点进行匹配，若检测结果为否，则基于动态匹配将扫描到的反光柱与全局地图中的路标点进
行匹配。
[0039] 在无法预测激光雷达当前位姿的情况下，需要使用静态匹配将扫描到的反光柱与地图中的路标点匹配上，已知地图中所有路标点的全局地图坐标、当前扫描到的反光柱在
激光雷达坐标系中(局部坐标系)的本地坐标，静态匹配过程具体如下：
[0040] S21、计算全局距离列表及局部距离列表，全局距离列表中记录了全局地图中各路标点之间的距离，局部距离列表记录了局部地图中各反光柱之间的距离；
[0041] 根据全局地图中的m个路标点R1(X1，Y1)，…，Rm(Xm，Ym)计算各路标点之间的距离，生成全局距离列表D1，2，…，D1，m；…；Dm，1，…，Dm，m‑1；根据激光雷达探测到的n个反光柱r1(x1，
y1)，…，rn(xn，yn)，计算反光柱之间的距离，得到局部距离列表 d1，2，…，d1，n；…；dn，1，…，
dn，n‑1
[0042] S22、在局部地图中选择三个反光柱ri、rj、rk组成一个三角形，在全局地图中查找该三角形的相似三角形，相似三角形由三个路标Ri、Rj、Rk组成；
[0043] 当全局地图中的三个路标Ri、Rj、Rk满足公式(1)时，则认定Ri、Rj、Rk三个路标组成的三角形即为反光柱ri、rj、rk组成三角形的相似三角形，得到当前匹配组合mComb＝{ri：Ri；
rj：Rj；rk：Rk}，匹配数目mCount＝3，匹配误差 mErr＝Δi，j+Δi，k+Δk，j，公式(1)具体如下：
[0044]
[0045] 其中，matchW为反光柱匹配允许的误差，di，j、di，k、dk，j分别为反光柱ri与反光柱rj在局部地图中的距离、反光柱ri与反光柱rk在局部地图中的距离、反光柱 rk与反光柱rj在
局部地图中的距离，Di，j、Di，k、Dk，j分别为路标Ri与路标Rj在全局地图中的距离、路标Ri与路
标Rk在全局地图中的距离、路标Rk与路标Rj在全局地图中的距离。
[0046] S23、在全局地图的剩余路标中依次查找各剩余反光柱的最佳匹配路标，依次将各剩余反光柱及对应的最佳匹配路标加入当前匹配组合时，同步当前匹配组合的匹配数目及
匹配误差，最后获取该相似三角形的匹配组合，匹配数目及匹配器误差；
[0047] S24、遍历局部地图中的剩余三角形，依次基于步骤S22及步骤S23获取对应的相似三角形及相似三角形的匹配组合，匹配数量及匹配误差，输出匹配数量最多的匹配组合，匹
配数目mCount为当前匹配组合中的反光柱数量，匹配误差mErr为反光柱与对应最佳匹配路
标的匹配误差的累加；
[0048] 在相似三角形匹配组合的的最大匹配数目为M，假定同时存在两个匹配数目为M的匹配组合，则输出最小匹配误差对应的匹配组合。
[0049] 在本发明实施例中，反光柱r的最佳匹配路标R的获取方法具体如下：
[0050] S231、计算反光柱r到当前匹配组合中每个反光柱的距离d，
[0051] S232、在全局地图中剩余路标中提取一个路标，计算该路标到当前匹配组合中每个路标的距离D，
[0052] S233、基于距离d及对应的距离D计算反光柱r与该路标的匹配误差，若匹配误差小于matchW，则记录该路标的匹配误差；
[0053] S234、遍历全局地图中剩余路标，依次基于步骤S232及步骤S233依次获取反光柱r与全局地图中剩余路标的匹配误差，若最小匹配误差小于 matchW*(mCount+1)，则最小匹
配误差对应的路标即为反光柱r的最佳匹配路标，若最小匹配误差大于matchW*(mCount+
1)，则当前反光柱r匹配的路标为空；
[0054] 假定当前匹配组合mComb＝{ri：Ri；rj：Rj；rk：Rk；rm：Rm}，反光柱rn与路标Rn(路标Rn为剩余路标中的一个路标)的匹配误差的计算方法为：
[0055] 依次计算反光柱rn到反光柱ri、反光柱rj、反光柱rk、反光柱rm的距离dni、 dnj、dnk及dnm；依次计算路标Rn到路标Ri、路标Rj、路标Rk、路标Rm的距离Dni、 Dnj、Dnk及Dnm，匹配误差∑
Δ＝Δni+Δnj+Δnk+Δnm，其中，Δn，i＝|dn，i‑Dn，i|，Δn，j＝|dn，j‑Dn，j|，Δn，k＝|dn，k‑Dn，k|，
Δn，m＝|dn，m‑Dn，m|。
[0056] 在本发明实施例中，上一帧定位成功得到激光雷达位姿pose(x，y，θ)后，就可以使用动态匹配来寻找激光帧中反光柱对应的路标，动态匹配过程具体如下：
[0057] S31、探测距离约束：遍历全局地图中的所有路标点R1(X1，Y1)，…，Rm(Xm，Ym)，计算路标点到激光雷达的距离dist，提取满足distmin＜dist＜distmax的路标点，distmin为激光
雷达的最小探测距离，distmax为激光雷达的最大探测距离；
[0058] S32、将当前局部地图中的反光柱投影到全局地图中，获取各反光柱的地图投影坐标，将距地图投影坐标的距离小于matchW的路标加入对应反光柱的匹配列中，所有反光柱
的匹配列即组成了匹配列表，一个反光柱的匹配列中可能存在多个路标r1：Ri，…；…；rn：
Rj，…；
[0059] 将将局部地图中的反光柱r(θ，d)投影到全局地图中，地图投影坐标Pr(xr，yr) 计算方法如下：
[0060]
[0061] 其中，θ为激光雷达的扫描角度，d为反光柱距激光雷达的距离。
[0062] S33、从局部地图中选择三个反光柱ri、rj、rk组成一个三角形，在三个反光柱对应匹配列中查找该三角形的相似三角形，相似三角形由路标Ri、Rj、Rk构成，Ri、Rj、Rk分别为反
光柱ri、rj、rk对应匹配列中的一个路标，当三个路标 Ri、Rj、Rk满足公式(1)时，则认定Ri、Rj、
Rk三个路标组成的三角形即为反光柱ri、rj、rk组成三角形的相似三角形，得到当前匹配组
合 mComb＝{ri：Ri；rj：Rj；rk：Rk}，匹配数目mCount＝3，匹配误差mErr＝Δi，j+Δi，k+Δk，j，公
式(1)具体如下：
[0063]
[0064] 其中，matchW为反光柱匹配允许的误差，di，j、di，k、dk，j分别为反光柱ri与反光柱rj在局部地图中的距离、反光柱ri与反光柱rk在局部地图中的距离、反光柱 rk与反光柱rj在
局部地图中的距离，Di，j、Di，k、Dk，j分别为路标Ri与路标Rj在全局地图中的距离、路标Ri与路
标Rk在全局地图中的距离、路标Rk与路标Rj在全局地图中的距离。
[0065] S34、在匹配列表的剩余匹配列中依次查找对应反光柱的最佳匹配路标，依次将各剩余反光柱及对应的最佳匹配路标加入当前匹配组合，并同步当前匹配组合的匹配数目及
匹配误差，即为该相似三角形的匹配组合，匹配数目及匹配器误差；
[0066] S35、遍历局部地图中剩余的所有三角形，依次基于步骤S33及步骤S34 获取对应的相似三角形及相似三角形的匹配组合，匹配数量及匹配误差，输出匹配数量最多的匹配
组合，匹配数目mCount为当前匹配组合中的反光柱数量，匹配误差mErr为反光柱与对应最
佳匹配路标的匹配误差的累加。
[0067] 在本发明实施例中，反光柱r的最佳匹配路标R的获取方法具体如下：
[0068] S341、计算反光柱r到当前匹配组合中每个反光柱的距离d，
[0069] S342、反光柱r的匹配列中提取一个路标，计算该路标到当前匹配组合中每个路标的距离D，
[0070] S343、基于距离d及对应的距离D计算反光柱r与该路标的匹配误差，若匹配误差小于matchW，则记录该路标的匹配误差；
[0071] S344、遍历反光柱r的匹配列中的剩余路标，基于步骤S342及步骤S343 依次计算反光柱r与匹配列中的剩余路标的匹配误差，获取最小的匹配误差，若最小的匹配误差小于
matchW*(mCount+1)，则最小匹配误差对应的路标即为反光柱r的最佳匹配路标，若最小的
匹配误差大于matchW*(mCount+1)，则当前反光柱r匹配的路标为空；
[0072] 假定当前匹配组合mComb＝{ri：Ri；rj：Rj；rk：Rk；rm：Rm}，反光柱rn与路标Rn的匹配误差的计算方法为：
[0073] 依次计算反光柱rn到反光柱ri、反光柱rj、反光柱rk、反光柱rm的距离dni、 dnj、dnk及dnm；依次计算路标Rn到路标Ri、路标Rj、路标Rk、路标Rm的距离Dni、 Dnj、Dnk及Dnm，匹配误差∑
Δ＝Δni+Δnj+Δnk+Δnm，其中，Δn，i＝|dn，i‑Dn，i|，Δn，j＝|dn，j‑Dn，j|，Δn，k＝|dn，k‑Dn，k|，
Δn，m＝|dn，m‑Dn，m|。
[0074] 本发明提供的反光柱快速匹配方法具有如下有益技术效果：基于三角生长法进行反光柱的匹配，提高了匹配的速度，有效的滤除了干扰反光柱，匹配的可靠性提高。
[0075] 以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
