移动机器人室内重定位方法、计算机装置及产品

1.本发明涉及机器人定位技术领域，特别是一种移动机器人室内重定位方法、计算机装置及产品。


背景技术：

2.移动机器人和agv的自主导航作业在社会生产生活中发挥着重要作用，广泛地应用于工业、农业、服务业中的各种场景。
3.室外定位可以通过组合导航提供的定位信息解决，移动机器人室内重定位是移动机器人室内导航的重点与难点，由于室内场景无法接受卫星信号，所以需要人工标定参照物或者采用主动式定位。虽然基于wlan、蓝牙、uwb或二维码等参照信息的室内定位方法已经能够保证良好的定位效果，但是此类方法需要对设置的参照信息进行位置标定，如果室内场景较大，所需的工作量也是巨大的，而且移动机器人在导航过程中有可能出现无法观测参考信息的情况，导致重定位无法完成。主动式定位方案中，基于视觉和激光雷达的重定位方案相对成熟，但是基于视觉的重定位受环境光线影响较大，而且精度低于基于激光雷达的定位方案。基于激光雷达点云特征匹配的重定位方法精度高，而且不受环境光的影响可靠性高，是一种可靠的重定位定位解决方案，所以选用激光雷达作为移动机器人室内重定位依赖的主动式传感器，通过移动机器人当前获取的点云数据与先验地图进行匹配来实现重定位。
4.现有的基于点云数据的的重定位方法本质上是一种基于局部地图与先验全局地图的匹配定位方法。
5.根据地图的表征形式不同，主要有基于代价栅格地图的重定位和基于3维点云地图的重定位。通过让移动机器人在无准确定位信息的条件下进行环境探索构建局部代价栅格地图或点云地图，并通过局部地图与建立的先验全局地图进行匹配，来获取移动机器人在室内地图中的位置，从而完成重定位。但由于代价栅格地图只包含二维环境信息，所以基于代价栅格地图的匹配重定位在环境特征较少或单一的情况下成功率较低；三维点云地图虽然包含了更多的环境特征，但是点云数据的处理量也随之变大，需要消耗更多的计算资源，而且常用的匹配算法，如icp、ndt等在全局地图规模较大时无法达到一个好的匹配效果。同时，无论是基于代价栅格地图的重定位方法还是基于三维点云地图的重定位方法，都要求移动机器人在没有全局定位信息的情况下进行一定范围内的环境探索，通过构建移动机器人当前所处位置周边的局部地图来实现全局配准与重定位，移动机器人在缺少定位信息条件下进行探索存在着较大的未知性与风险性，而且有效地图的构建效率无法得到保证。本发明在多线激光雷达点云数据中提取拓扑语义特征和几何特征，并与先验的语义-几何特征地图进行一个二阶段的匹配，能够实现移动机器人在无需自由探索的条件下完成室内场景的快速重定位。
6.现有技术还通过传感器获取环境中特征的距离值来构建环境边界信息，并结合粒子滤波来进行重定位，此类方法虽然在某些环境中也能实现全局重定位，但是该方法在特
征相似的环境中重定位的失误率较大，而且对于动态物体较为密集的环境，其稳定性也会受到影响。


技术实现要素：

7.本发明所要解决的技术问题是，针对现有技术不足，提供一种移动机器人室内重定位方法、计算机装置及产品，即便是相似环境也能通过某个细微的几何特征的不同来构建不同的拓扑结构实现准确的匹配和重定位，对有动态障碍物的环境有较强的鲁棒性。
8.为解决上述技术问题，本发明所采用的技术方案是：一种移动机器人室内重定位方法，包括以下步骤：
9.s1、移动机器人位姿丢失或重启后，对室内工作场景建图时保存的关键帧点云与关键帧位姿进行基于多维特征描述子相似度分析的几何特征、拓扑语义特征提取，并构建全局几何-拓扑语义特征地图；
10.s2、根据移动机器人位姿丢失位置感知的点云数据构建局部拓扑语义地图，将局部拓扑语义地图与全局几何-拓扑语义特征地图进行基于三角拓扑结构的相似度分析，根据相似度分析结果得到匹配的三角拓扑结构，完成拓扑语义特征的匹配；
11.s3、对多对已知匹配关系的三角拓扑结构进行多组位姿估计，通过一个置信度评分机制对筛选出的三角拓扑结构对进行评分，选出评分最高的三角拓扑结构对用于位姿估计，通过求解一个已知点的匹配关系的非线性优化问题，使移动机器人在任意位置启动时在无需随机游走建图匹配的条件下，获取当前位置到全局坐标系的位姿粗变换。
12.本发明通过构建先验全局几何-拓扑语义地图与局部拓扑语义地图匹配进行重定位，无需对工作场景进行复杂的标定工作，可操作性更强性强。
13.本发明通过与先验全局几何-拓扑语义地图匹配进行重定位，无需在室内大规模地安置信号收发设备或标志物，在大规模环境下实施更加便捷、低成本。通过两阶段特征分割并构建拓扑语义地图，降低了环境信息表达的数据量级，使重定位过程更加轻量化、更迅速，根据拓扑语义地图中的三角拓扑结构的相似度分析来实现节点的匹配，通过很好地利用特征之间的相对位置关系，实现了无需初始阶段随机游走的重定位，而且根据特征之间相对关系的不变性，克服了环境噪声点的干扰，并且对于环境特征相似、单一的场景仍然能通过特征相对关系的分析察觉出环境特征的细微变化并完成重定位。最后通过已知匹配关系的几何特征的位姿估计，补偿了单纯依赖于拓扑语义特征进行位姿估计带来的误差，提高了重定位的精度。
14.为了进一步提高定位准确性，本发明还包括：
15.s4、根据已知匹配关系的几何特征，构建目标函数解算移动机器人到全局坐标系的位姿精变换，实现移动机器人丢失位姿后的室内重定位。
16.步骤s1的具体实现过程包括：
17.1)控制搭载多线激光雷达的移动机器人以小于第一设定阈值的速度对室内工作场景进行遍历采集点云数据，进行实时建图定位，当移动机器人当前位置相较上一个关键帧保存位置的位移大于第二设定阈值时，保存当前的关键帧点云、关键帧位姿；
18.2)对保存的关键帧点云进行基于多维特征描述子相似度分析的两阶段特征分割，得到边缘特征、平面特征以及点云簇特征，并将分割出的特征等效为一个拓扑节点；
19.3)将关键帧点云中基于局部点云平滑度计算提取的几何特征点云与步骤2)得到的关键帧拓扑节点图进行融合，得到包含几何信息和拓扑语义信息的关键帧，基于关键帧位姿对关键帧进行拼接，得到全局几何-拓扑语义地图mg。
20.本发明通过一个两阶段特征分割，获取独立的边缘、平面以及点云簇特征，并将其等效为拓扑节点的表达形式，减少了数据处理的量级，给特征赋予了语义标签，能够使移动机器人在不需要随机游走的条件下在大规模室内场景丢失位姿后完成快速的粗定位。同时在地图中添加原有的几何特征信息，保证移动机器人能根据几何特征完成最后重定位的位姿精估计，进一步提高了重定位的精度。
21.步骤2)中，对保存的关键帧点云进行基于多维特征描述子相似度分析的两阶段特征分割的具体实现过程包括：
22.a)对关键帧点云p，提取出边缘特征fe、平面特征f
p
以及点云簇序列cluster；
23.b)对fe和f
p
中的每个点构建多维特征向量，将所述多维特征向量作为描述子，构建边缘特征描述子ve{v
e1
,v
e2
,v
e3
}和平面特征描述子v
p
{v
p1
,v
p2
,v
p3
}；其中，v
e1
包含边缘特征点的空间位置信息，即三维坐标(x,y,z)，v
e2
包含边缘特征点所在直线的参数拟合信息(ae,be,ce,de)，边缘特征点所在直线参数方程为：
24.aex bey cez de＝0；
25.v
e3
包含边缘特征点所在直线在水平地面投影点信息(x
pj
,y
pj
)；v
p1
包含平面特征点局部平面的法向信息v
p2
包含平面特征点所在平面在水平地面投影直线的参数信息(a
p
,b
p
,c
p
,d
p
)，投影直线参数方程为：
26.a
p
x b
p
y c
p
z d
p
＝0；
27.v
e3
包含平面特征点所在平面在水平地面投影线段的中点二维坐标(x
lpm
,y
lpm
)；
28.c)对fe中的点基于v
e1
进行欧式聚类，对得到的每一个聚类依次基于v
e2
和v
e3
进行欧式聚类，最终得到a个聚类分割，即精分割后的独立边缘特征序列e＝{e1,e2,...,ea}；对f
p
中的点首先基于v
p1
进行欧式聚类，对得到的每一个聚类依次基于v
p2
和v
p3
进行欧式聚类，得到b个聚类，即独立平面特征序列w＝{w1,w2,...,wb}，其中a为精分割后独立边缘特征的数量，b为精分割后的独立平面特征的数量；
29.d)对e＝{e1,e2,...,ea}中每一个精分割后的独立边缘特征，选取独立边缘特征在水平地面的投影点作为表征边缘特征的拓扑节点，对包含n个边缘特征点的独立边缘特征p
ie
{{x1,y1,z1},{x2,y2,z2},...,{xn,yn,zn}}，其对应的拓扑节点为对w＝{w1,w2,...,wb}中每一个精分割后的独立平面特征，提取平面上包含所有平面特征点的最小矩形，投影到水平面上，将得到的有限线段的中点作为表征平面特征的拓扑节点；
30.对包含m个平面特征点的独立平面特征p
ip
{{x1,y1,z1},{x2,y2,z2},...,{xm,ym,zm}}，将所有平面特征点投影到水平面，找到投影直线的边界点edge1(x1,y1)和edge2(x2,y2)，对应的拓扑节点为
31.取点云簇特征序列中每一个特征元素的质心在水平地面的投影作为表征当前簇点云的拓扑节点，对包含k个边缘特征点的独立边缘特征p
ic
{{x1,y1,z1},{x2,y2,z2},...,{xk,yk,zk}}，对应的拓扑节点为：
32.e)对每一个独立的边缘、平面、点云簇聚类都采用拓扑节点坐标计算方法(即上述步骤c)～d)的过程)进行计算，最终得到全局拓扑语义地图mg。
33.本发明通过构建针对边缘、平面特征的特征描述子，并对每个特征点的描述子进行聚类分析，在原有的几何、平面特征的基础上将其进一步地分割成独立的特征表达，并分割后的特征等效为拓扑节点，在保证信息丰富度的情况下，降低了环境特征描述的数据量。
34.步骤s2中，将局部拓扑语义地图与全局几何-拓扑语义特征地图进行基于三角拓扑结构的相似度分析，根据相似度分析结果得到匹配的三角拓扑结构的具体实现过程包括：
35.a)从全局几何-拓扑语义特征地图mg{s
ge
,s
gp
,s
gc
}和局部拓扑语义地图m
l
{s
le
,s
lp
,s
lc
}中提取相似的拓扑边，其中，设mg和m
l
中的拓扑边的长度差值小于第三阈值，则认为mg和m
l
相似；s
ge
为全局拓扑语义地图中由边缘特征构成的拓扑节点，s
gp
为全局拓扑语义地图中由平面特征构成的拓扑节点，s
gc
为全局拓扑语义地图中由点云簇特征构成的拓扑节点，s
le
为局部拓扑语义地图中由边缘特征构成的拓扑节点；s
lp
为局部拓扑语义地图中由平面特征构成的拓扑节点，s
lc
为局部拓扑语义地图中由点云簇特征构成的拓扑节点；
36.b)对c
ge
中的拓扑节点按照该节点与地图中心连线与地图坐标系x轴所成夹角大小进行排序；对s
le
中的拓扑节点按照该节点与激光雷达坐标系原点连线与激光雷达坐标系x轴所成夹角大小进行排序；
37.c)计算排序后相邻节点对应的拓扑边的长度，合并mg和m
l
中的拓扑边集合，重新按照拓扑边的长度进行排序，将每条拓扑边长度与相邻的拓扑边长度做差，若拓扑边长度差值小于第四阈值，且两条拓扑边分别属于mg和m
l
，则认为找到两条相似的拓扑边，进而构建4个待验证的三角拓扑结构；
38.d)对平面特征拓扑节点和点云簇拓扑节点，进行步骤b)和步骤c)的操作，得到对应特征类下的三角拓扑结构。
39.本发明采用对拓扑节点进行先排序后构建拓扑边，然后对拓扑边进行相似度分析来筛选相似的拓扑边构建三角拓扑结构，通过先排序大大缩小了相似度分析时的搜索范围，节约了时间成本，通过先进行相似度分析后构建拓扑三角，提高了所构建拓扑三角结构对成功匹配的概率，能够使算法快速收敛。
40.在相似的拓扑边中选取拓扑节点进行三角拓扑结对构建并完成拓扑语义特征的匹配的具体实现过程包括：
41.i)构建三角特征结构描述子v
tri
{v
tri1
,v
tri2
}，其中v
tri1
{len1,len2,len3}包含三角特征结构的边长信息，其中len1,len2,len3为三角特征结构的三边边长，包含三角特征结构的方向信息，为三角特征结构的三边的方向向量；
42.ii)根据三角特征描述子对全局-几何拓扑语义地图与局部拓扑语义地图中的三角拓扑结构进行相似性分析，若排列组合得到的9个边长差中存在3个以上边长差小于第五阈值，排列组合得到的9个夹角中存在3个以上夹角小于第六阈值，则认为两个三角拓扑结构相似，匹配成功；
43.iii)设存在一对匹配成功的三角拓扑结构tri_g(tria,trib,tric)，tri_l(trid,trie,trif)，其中tri_g是全局拓扑语义地图中的三角拓扑结构，tri_l是移动机器人位姿丢
失位置的局部拓扑语义地图中的三角拓扑结构；若边长满足匹配关系：
[0044][0045]
则通过
[0046]
得到拓扑节点tria对应拓扑节点trie，拓扑节点trib对应拓扑节点trif，拓扑节点c对应拓扑节点trid；
[0047]
iv)基于步骤i)～iii)得到三类匹配的拓扑节点对，包括边缘拓扑节点对me、平面拓扑节点对mp以及点云簇拓扑节点对mc，其中me＝{{ge_node1,le_node1},{ge_node2,le_node2},
…
,{ge_node
x
,le_node
x
}}，mp＝{{gp_node1,lp_node1},{gp_node2,lp_node2},
…
,{gp_nodey,lp_nodey}}，mc＝{{gc_node1,lc_node1},{gc_node2,lc_node2},
…
,{gc_nodez,lc_nodez}}，x、y、z分别是匹配成功的边缘、平面、点云簇拓扑节点数量。
[0048]
本发明通过构建三角特征结构描述子，能够快速、准确地分析所构建的三角拓扑结构是否匹配成功，并进一步地获取拓扑节点之间的一一对应关系，基于所得到了拓扑节点的一一映射关系，为优化求解位姿变换关系的过程添加了更强的约束，求解的位姿变换精度更高。
[0049]
步骤s3的具体实现过程包括：
[0050]
根据匹配的三角拓扑结构得到匹配好的拓扑节点对：其中p1,p2,p3为全局几何-拓扑语义特征地图中某个拓扑三角结构中的拓扑节点，p
′1,p
′2,p
′3为局部拓扑语义特征地图中某个拓扑三角结构中的拓扑节点；
[0051]
构建最小二乘问题，最小二乘问题表示为：
[0052][0053]
n＝3，r、t为使目标函数j取极小值时的旋转矩阵和平移向量；
[0054]
计算上述最小二乘问题，得到u组位姿变换关系t
sets
＝{t1,t2,
…
,tu}，其中t1,t2,
…
,tu由求解的每一组旋转矩阵和平移向量组成。；
[0055]
对求得的u组位姿变换关系t
sets
＝{t1,t2,
…
,tu}进行基于变量(x,y,yaw)的欧式聚类，其中x为位姿变换中x方向上的平移量，y为位姿变换中y方向上的平移量，yaw为绕z轴的旋转角度，得到u’个聚类类别；
[0056]
对每个聚类类别进行置信度计算，其中聚类体积系数设为kv，聚类质量系数设为kq；
[0057]
置信度分数最高的聚类包含q个匹配三角拓扑结构，基于三角拓扑结构推理得到q’对匹配拓扑节点，对所述q’对匹配拓扑节点中的边缘特征拓扑节点，进行一个已知点的匹配关系的非线性优化问题求解，将匹配的平面特征拓扑节点和点云簇拓扑节点作为位姿
约束，得到重定位所需的粗变换t
vague
＝{r
vague
,t
vague
}，其中r
vague
为重定位粗定位位姿变换中的旋转矩阵，t
vague
为重定位粗定位位姿变换中的平移向量。
[0058]
本发明通过一个对求解位姿变换关系的置信度评分机制，去除了可能存在的错误匹配的三角拓扑结构，提高了位姿估计的精度和鲁棒性。
[0059]
步骤s4的具体实现过程包括：
[0060]
根据置信度高的q’对匹配的拓扑节点，索引对应的q’对匹配的特征点云，设q’对匹配的特征点云中包含ne对匹配的边缘特征点云：
[0061]
对第i对匹配的边缘特征点云{egi,eli}中eli的边缘特征点edgea，在egi中找到其最邻近的两个点edgeb、edgec，计算edgea到直线edgebedgec的距离d∈；
[0062]
设q’对匹配的特征点云中包含n
p
对匹配的平面特征点云：
[0063][0064]
对第r对匹配的平面特征点云{pgr,plr}中plr的边缘特征点planara，在pgr中找到其最邻近的三个点planarb、planarc、planard，计算planara到planarb、planarc、planard所在平面的距离dh；
[0065]
构建最小二乘问题，求使目标函数达到极小的r’，其中
[0066][0067]
m1为eli中平面特征点数量；
[0068][0069]
其中m2为plr中平面特征点数量；
[0070]
基于g2o库计算非线性优化问题，得到重定位过程所需的精确位姿变换t
acc
＝{r
acc
,t
acc
}，其中r
acc
＝r’r
vague
,t
acc
＝r’t
vague
t’。
[0071]
在完成重定位的位姿粗估计后，在有良好初值条件的情况下，基于已知匹配关系的几何特征进行重定位过程的精确估计，能够很好地补偿基于拓扑语义特征匹配位姿估计的误差，获得移动机器人精确位姿信息。
[0072]
本发明还提供了一种计算机装置，包括存储器、处理器及存储在存储器上的计算机程序；所述处理器执行所述计算机程序，以实现本发明方法的步骤。
[0073]
本发明还提供了一种计算机程序产品，包括计算机程序/指令；该计算机程序/指令被处理器执行时实现本发明所述方法的步骤。
[0074]
与现有技术相比，本发明所具有的有益效果为：
[0075]
1)基于蓝牙、wifi、二维码或uwb的重定位方法需要事先对传感器进行标定工作，工作复杂且标定结果对重定位效果影响很大，本发明通过构建先验全局几何-拓扑语义地图与局部拓扑语义地图匹配进行重定位，无需对工作场景进行复杂的标定工作，可操作性更强性强。而且对于大型场景，基于蓝牙、wifi或uwb的重定位方法需要布置大量的传感器，使用成本更高，而基于激光雷达的匹配定位方法成本不受场景大小的限制；
[0076]
2)基于2d栅格地图的重定位的方法需要移动机器人在无定位信息的条件下进行
随机游走进行局部地图的构建才能完成重定位，而某些工作场景中随机游走具有一定的风险性，且耗时较长。本方法通过对当前位置拓扑语义地图与全局拓扑语义地图中的三角拓扑结构进行相似度分析，找到拓扑节点的对应关系并进行位姿估计，实现无需随机游走的快速重定位。
[0077]
3)基于3d激光slam或视觉slam的重定位方法在大范围工作场景中进行建图匹配由于数据稠密，所以对算力要求较高，而且需要通过分割全局地图进行分块匹配定位，耗时较长。本方法通过将工作场景中的几何特征和点云簇特征分割并表征为二维拓扑节点的形式，大大降低了运算量，并通过拓扑节点之间相对关系的利用弥补了普通拓扑地图包含特征较少的弊端。
[0078]
4)在基于几何特征的重定位精估计过程中，通过拓扑节点之间的匹配关系能够获取原有几何特征之间的一一对应关系，相较loam系列基于最近点进行的匹配与位姿估计的方法，本方法的匹配与位姿估计所构建的目标函数可理解性更强，且避免了噪声点对匹配的干扰。
[0079]
5)本发明方法构建包含拓扑节点相对位置关系的拓扑结构作为特征，即便是相似环境也能通过某个细微的几何特征的不同来构建不同的拓扑结构来实现准确的匹配和重定位，并行对有动态障碍物的环境有较强的鲁棒性。
附图说明
[0080]
图1为重定位方法总框图；
[0081]
图2为全局几何-拓扑语义地图构建示意图；
[0082]
图3为三角拓扑结构构建与匹配示意图；
[0083]
图4为基于已知匹配关系的两阶段位姿估计示意图；
[0084]
图5为地下停车场全局点云地图；
[0085]
图6为地下停车场几何地图(边缘、平面特征地图)；
[0086]
图7为地下停车场拓扑节点地图；
[0087]
图8为局部拓扑语义地图中的三角拓扑结构示意图；
[0088]
图9(a)和图9(b)为多线激光雷达的坐标系示意图；图9(a)激光雷达侧视图；图9(b)激光雷达俯视图；
[0089]
图10为全局匹配重定位示意图(其中灰色为全局拓扑节点，白色为待匹配的局部拓扑节点)；
[0090]
图11为基于icp的全局匹配定位示意图((白色点云为重定位后的点云)；
[0091]
图12为基于ndt的全局匹配定位示意图(白色点云为重定位后的点云)；
[0092]
图13为基于本发明方法的全局重定位结果示意图(白色点云为重定位后的点云)；
[0093]
图14(a)和图14(b)为减少局部拓扑节点特征数量的全局重定位结果图，图14(a)为4个局部拓扑节点特征数量的全局重定位结果，图14(b)为10个局部拓扑节点特征数量的全局重定位结果；
[0094]
图15为不同局部地图拓扑节点数量对应的全局重定位误差(其中圆点虚线、长实线、点划线、长虚线分别为局部地图包含5、10、15、20个拓扑节点的实验结果)；
[0095]
图16为不同局部地图拓噪声点数量对应的全局重定位误差(其中圆点虚线、长虚
线、长实线、点划线分别为局部地图包含30、20、10、5个噪声点的实验结果)。
具体实施方式
[0096]
如图1所示，本发明实施例包括以下步骤：
[0097]
步骤1、移动机器人位姿丢失或重启后，需要在任意位置基于多线激光雷达感知的环境信息对位姿进行重新估计，首先对室内工作场景建图时保存的关键帧点云与关键帧位姿进行基于多维特征描述子相似度分析的几何特征、拓扑语义特征提取，并构建全局几何-拓扑语义特征地图；
[0098]
步骤2、根据移动机器人位姿丢失位置感知点云数据构建局部拓扑语义地图，与全局拓扑语义地图进行基于三角拓扑结构的相似度分析，根据相似度分析结果得到匹配的三角拓扑结构，进而完成拓扑语义特征的匹配；
[0099]
步骤3、对求解得到多对已知匹配关系的三角拓扑结构进行多组位姿估计，通过一种对位姿估计结果的评分机制获取置信度高的三角拓扑结构对，通过求解一个已知点的匹配关系的非线性优化问题，使移动机器人在任意位置启动时能在无需随机游走建图匹配的条件下，获取一个当前位置到全局坐标系的位姿粗变换。
[0100]
步骤4、根据已知匹配关系的几何特征，构建几何约束性更准确的目标函数来解算移动机器人到全局坐标系的位姿精变换，本方法通过一个两阶段的位姿估计实现了移动机器人丢失位姿后的室内重定位。
[0101]
全局重定位的总框架如图1所示。
[0102]
各步骤具体实现过程如下：
[0103]
步骤1、对室内工作场景建图时保存的关键帧点云与关键帧位姿进行基于多维特征描述子相似度分析的几何特征、拓扑语义特征提取，并构建全局几何-拓扑语义特征地图，全局几何-拓扑语义特征地图的构建流程如图2所示；
[0104]
步骤1.1、对室内工作场景进行实时定位建图，并保存关键帧点云与关键帧位姿；
[0105]
步骤1.1.1、控制搭载多线激光雷达的移动机器人遍历室内工作场景，行进过程中基于lego-loam进行实时定位建图；
[0106]
步骤1.1.2、当移动机器人位移距离大于设置的第一阈值时保存当前帧点云作为关键帧点云，并记录由lego-loam建图定位系统提供的位姿作为关键帧位姿；
[0107]
步骤1.2、对保存的关键帧点云进行几何特征和拓扑语义特征提取；
[0108]
步骤1.2.1、对关键帧点云p，先基于lego-loam中的图像投影与特征提取模块初步提取出边缘特征fe、平面特征f
p
以及点云簇序列
[0109]
cluster＝{cluster1,cluster2,...,clusterm}；
[0110]
其中m为分割出来的独立点云簇数量；
[0111]
步骤1.2.2、对fe和f
p
中的每个点构建多维特征向量作为描述子，构建边缘特征描述子：
[0112]ve
{v
e1
,v
e2
,v
e3
}；
[0113]
边缘特征描述子包含3个维度的特征信息，v
e1
包含点的该边缘特征点的空间位置信息，即三维坐标(x,y,z)，v
e2
包含该边缘特征点所在直线的参数拟合信息(ae,be,ce,de)，边缘特征点所在直线参数方程为：
[0114]ae
x bey cez de＝0；
[0115]ve3
包含该边缘特征点所在直线在水平地面投影点信息(x
pj
,y
pj
)。
[0116]
构建平面特征描述子
[0117]vp
{v
p1
,v
p2
,v
p3
}；
[0118]
平面特征描述子包含3个维度的特征信息，v
p1
包含该平面特征点局部平面的法向信息v
p2
包含该平面特征点所在平面在水平地面投影直线的参数信息(a
p
,b
p
,c
p
,d
p
)，投影直线参数方程为：
[0119]ap
x b
p
y c
p
z d
p
＝0；
[0120]ve3
包含该平面特征点所在平面在水平地面投影线段的中点二维坐标(x
lpm
,y
lpm
)。
[0121]
步骤1.2.3、对fe和f
p
中的每个点的特征描述子，利用欧式聚类算法进行聚类操作，对fe中的点首先基于v
e1
进行欧式聚类，对得到的每一个聚类依次基于v
e2
和v
e3
进行欧式聚类，最终得到a个聚类分割，即精分割后的独立边缘特征序列e＝{e1,e2,...,ea}；
[0122]
对f
p
中的点首先基于v
p1
进行欧式聚类，对得到的每一个聚类依次基于v
p2
和v
p3
进行欧式聚类，得到b个聚类，即独立平面特征序列w＝{w1,w2,...,wb}，其中a为精分割后独立边缘特征的数量，b为精分割后的独立平面特征的数量。
[0123]
步骤1.2.4、选取独立边缘特征在水平地面的投影点作为表征边缘特征的拓扑节点，对包含n个边缘特征点的独立边缘特征p
ie
{{x1,y1,z1},{x2,y2,z2},...,{xn,yn,zn}}，其对应的拓扑节点为：
[0124][0125]
对w＝{w1,w2,...,wb}中每一个精分割后的独立平面特征，提取平面上包含所有平面特征点的最小矩形，投影到水平面上，将得到的有限线段的中点作为表征平面特征的拓扑节点。对包含m个平面特征点的独立平面特征p
ip
{{x1,y1,z1},{x2,y2,z2},...,{xm,ym,zm}}，将所有平面特征点投影到水平面，找到投影直线的边界点edge1(x1,y1)和edge2(x2,y2)，其对应的拓扑节点为：
[0126][0127]
选取点云簇特征序列中每一个特征元素的质心在水平地面的投影作为表征该簇点云的拓扑节点，对包含k个边缘特征点的独立边缘特征p
ic
{{x1,y1,z1},{x2,y2,z2},...,{xk,yk,zk}}，其对应的拓扑节点为
[0128][0129]
对每一个独立的边缘、平面、点云簇聚类都采样上述的拓扑节点坐标计算方法进行计算，最终得到全局拓扑语义地图mg；
[0130]
步骤2、基于移动机器人当前位置感知点云数据构建局部拓扑语义地图，与全局拓扑语义地图进行基于三角拓扑结构的相似度分析，根据相似度分析结果得到匹配的三角拓扑结构，进而完成拓扑语义特征的匹配，三角拓扑结构构建与匹配流程如图3所示；
[0131]
步骤2.1、在移动机器人位姿丢失，或重启后无法获取当前位姿时，读取当前移动机器人所处位置激光雷达扫描到的点云数据，根据步骤s1中的拓扑语义节点提取方法进行
拓扑语义地图构建，得到局部拓扑语义地图m
l
，此局部拓扑语义地图包含位姿丢失位置的环境特征信息，与全局拓扑语义地图共同作为重定位过程的输入；
[0132]
步骤2.2、对全局拓扑语义地图mg和局部拓扑语义地图m
l
的每一类拓扑节点进行三角拓扑结构构建，根据移动机器人位姿丢失位置局部拓扑语义地图中拓扑节点的分布构建包含节点间相对位置关系的三角拓扑结构；
[0133]
步骤2.2.1、从全局几何-拓扑语义特征地图mg{s
ge
,s
gp
,s
gc
}和局部拓扑语义地图m
l
{s
le
,s
lp
,s
lc
}中提取相似的拓扑边，其中，设mg和m
l
中的拓扑边的长度差值小于第三阈值，则认为mg和m
l
相似；s
ge
为全局拓扑语义地图中由边缘特征构成的拓扑节点，s
gp
为全局拓扑语义地图中由平面特征构成的拓扑节点，s
gc
为全局拓扑语义地图中由点云簇特征构成的拓扑节点，s
le
为局部拓扑语义地图中由边缘特征构成的拓扑节点；s
lp
为局部拓扑语义地图中由平面特征构成的拓扑节点，为局部拓扑语义地图中由点云簇特征构成的拓扑节点；
[0134]
步骤2.2.2、对s
ge
中的拓扑节点按照该节点与地图中心连线与地图坐标系x轴所成夹角大小进行排序，对s
le
中的拓扑节点按照该节点与激光雷达坐标系原点连线与激光雷达坐标系x轴所成夹角大小进行排序；
[0135]
步骤2.2.3、计算排序后相邻节点对应的拓扑边的长度。将mg和m
l
中的拓扑边重新按照拓扑边的长度进行排序，每条拓扑边与相邻的拓扑边做长度差，拓扑边长度差值小于第四阈值，且两条拓扑边分别属于mg和m
l
，则认为找到两条相似的拓扑边，进而构建4个待验证的三角拓扑结构；
[0136]
步骤2.2.4、同理，对平面特征拓扑节点和点云簇拓扑节点按照步骤2.2.2、步骤2.2.3进行处理，得到该特征类下的三角拓扑结构；
[0137]
步骤2.3、对mg和m
l
中筛选出来的初始相似置信度高的三角拓扑结构进行相似度分析，如果相似度评分高于第三阈值则认为这两个三角拓扑结构匹配成功，并据此进一步得到位姿丢失位置的拓扑节点与全局拓扑节点之间的对应关系的具体实现过程包括：
[0138]
步骤2.3.1、构建三角特征结构描述子v
tri
{v
tri1
,v
tri2
}，其中v
tri1
{len1,len2,len3}包含三角特征结构的边长信息，其中len1,len2,len3为三角特征结构的三边边长，包含三角特征结构的方向信息，为三角特征结构的三边的方向向量；
[0139]
步骤2.3.2、根据三角特征描述子对全局-几何拓扑语义地图与局部拓扑语义地图中的三角拓扑结构进行相似性分析，若排列组合得到的9个边长差中存在3个以上边长差小于第五阈值，排列组合得到的9个夹角中存在3个以上夹角小于第六阈值，则认为两个三角拓扑结构相似，匹配成功。
[0140]
步骤2.3.3、根据匹配成功的三角拓扑结构对，进一步分析得到具体的拓扑节点匹配关系。假设存在一对匹配成功的三角拓扑结构tri_g(tria,trib,tric)，tri_l(trid,trie,trif)，其中tri_g是全局拓扑语义地图中的三角拓扑结构，tri_l是移动机器人位姿丢失位置的局部拓扑语义地图中的三角拓扑结构。
[0141]
如果边长满足匹配关系：
[0142]
[0143]
则通过
[0144]
得到拓扑节点tria对应拓扑节点trie，拓扑节点trib对应拓扑节点trif，拓扑节点c对应拓扑节点trid；
[0145]
步骤2.3.4、基于步骤2.3.1、步骤2.3.2、步骤2.3.3所述方法得到三类匹配的拓扑节点对，包括边缘拓扑节点对me、平面拓扑节点对mp以及点云簇拓扑节点对mc，其中me＝{{ge_node1,le_node1},{ge_node2,le_node2},
…
,{ge_node
x
,le_node
x
}}，mp＝{{gp_node1,lp_node1},{gp_node2,lp_node2},
…
,{gp_nodey,lp_nodey}}，mc＝{{gc_node1,lc_node1},{gc_node2,lc_node2},
…
,{gc_nodez,lc_nodez}}，x、y、z分别是匹配成功的边缘、平面、点云簇拓扑节点数量。
[0146]
步骤3、基于求解的多对已知匹配关系的三角拓扑结构进行多组位姿估计，通过一种对位姿估计结果的评分机制获取置信度高的三角拓扑结构对，求解一个已知点的匹配关系的非线性优化问题得到移动机器人在当前位置到全局坐标系的位姿粗变换。最后基于一个已知匹配关系的几何特征，构建几何约束性更准确的目标函数来解算移动机器人到全局坐标系的位姿精变换，通过一个两阶段的位姿估计实现了无需自由探索的移动机器人室内静态重定位，基于已知匹配关系的两阶段位姿估流程如图4所示；
[0147]
步骤3.1、求解每一对匹配成功的三角拓扑结构对应的位姿变换关系；
[0148]
步骤3.1.1、基于步骤2得到u对匹配成功的三角拓扑结构，每一对三角拓扑结构中包括3对匹配成功的拓扑节点对
[0149][0150]
其中p1,p2,p3为全局几何-拓扑语义特征地图中某个拓扑三角结构中的拓扑节点，p
′1,p
′2,p
′3为局部拓扑语义特征地图中某个拓扑三角结构中的拓扑节点；
[0151]
步骤3.1.2、找一个欧式变换r，t使得：
[0152][0153]
其中node_num在这里取1，2，3。定义第node_num对点的误差项：
[0154]enode_num
＝p
node_num-(rp
′
node_num
t)；
[0155]
步骤3.1.3、构建最小二乘问题，使求误差平方和达到极小的r，t：
[0156][0157]
其中r、t为使目标函数j取极小值时的旋转矩阵和平移向量；
[0158]
步骤3.1.3、对u对匹配三角拓扑结构求解最小二乘问题，得到u组位姿变换关系t
sets
＝{t1,t2,
…
,tu}，其中t1,t2,
…
,tu由求解的每一组旋转矩阵和平移向量组成。，每一组变换关系都表征着当前位置到全局地图位姿粗变换的一种可能，为了避免由噪声节点产生的干扰，需要对重定位的有效位姿粗变换关系进行筛选；
[0159]
步骤3.2、对u对位姿变换关系进行投票，选出置信度高的q对三角拓扑结构；
[0160]
步骤3.2.1、基于有效假设：筛选出的成功匹配的三角拓扑结构对的有效数量远远
大于(至少是大于)误匹配三角拓扑结构数量；
[0161]
步骤3.2.2、对求得u组位姿变换关系t
sets
＝{t1,t2,
…
,tu}进行基于变量(x,y,yaw)的欧式聚类，得到u’个聚类类别；
[0162]
步骤3.2.3、对每个聚类类别进行置信度计算，其中聚类体积系数设为kv，聚类质量系数设为kq。
[0163]
聚类质量系数kq与聚类元素的离散程度相关，聚类元素的离散程度越高，聚类质量越差。聚类元素的离散程度相关变量e1为平移变量的方差，聚类元素的离散程度相关变量e2为旋转变量的方差。聚类质量系数
[0164][0165]
聚类可信度计算方法为：
[0166]
q＝kq×kv
×
n；
[0167]
按照上述计算方法对每个聚类进行置信度计算，选取置信度分数最高的聚类元素作为重定位粗变换的计算依据。
[0168]
步骤3.2.4、置信度分数最高的聚类包含q个匹配三角拓扑结构，基于匹配的三角拓扑结构推理得到q’对匹配拓扑节点，对这q’对匹配的拓扑节点中的边缘特征拓扑节点，求解一个已知点的匹配关系的非线性优化问题，得到位姿估计，将匹配的平面特征拓扑节点和点云簇拓扑节点作为位姿约束，得到重定位所需的粗变换t
vague
＝{r
vague
,t
vague
}，通过这个位姿粗变换，能让任意位置发生位姿丢失的移动机器人获取一个在全局地图中相对准确的位姿估计；
[0169]
步骤3.3、根据3.2求得的重定位粗变换t
vague
对局部几何特征地图进行坐标变换，然后对粗变换后的局部几何特征地图与全局几何特征地图进行基于已知几何特征匹配关系的非线性优化问题求解，得到的重定位所需的精变换t
acc
＝{r
acc
,t
acc
}；
[0170]
步骤3.3.1、根据步骤s2中最终得到置信度高的q’对匹配的拓扑节点，索引对应的
[0171]
q’对匹配的特征点云，假设q’对匹配的特征点云中包含ne对匹配的边缘特征点云：
[0172]
{{eg1,el1},{eg2,el2},...,{egi,eli},...,{eg
ne
,el
ne
}}；
[0173]
其中egi表示第i对匹配的边缘特征点云中属于全局先验地图中的边缘特征，eli表示第i对匹配的边缘特征点云中属于当前位置局部地图中的边缘特征。
[0174]
假设q’对匹配的特征点云中包含n
p
对匹配的平面特征点云：
[0175][0176]
其中pgr表示第r对匹配的平面特征点云中属于全局先验地图中的平面特征，plr表示第r对匹配的平面特征点云中属于当前位置局部地图中的平面特征。
[0177]
步骤3.3.2、对第i对匹配的边缘特征点云{egi,eli}中eli的边缘特征点edgea，在egi中找到其最邻近的两个点edgeb、edgec，计算edgea到直线edgebedgec的距离d∈
[0178][0179]
其中为edgea、edgeb、edgec点的坐标。
[0180]
对第r对匹配的平面特征点云{pgr,plr}中plr的边缘特征点planara，在pgr中找到其最邻近的三个点planarb、planarc、planard，计算planara到planarb、planarc、planard所在平面的距离dh
[0181][0182]
其中为planara、planarb、planarc、planard点的坐标。
[0183]
步骤3.3.3、构造目标函数求解重定位所需的精确位姿变换t
acc
＝{r
acc
,t
acc
}。
[0184]
基于边缘特征匹配关系的目标函数φ1有
[0185][0186]
m1为eli中平面特征点数量；
[0187]
基于平面特征匹配关系的目标函数φ2有
[0188][0189]
其中m2为plr中平面特征点数量；
[0190]
构造总目标函数
[0191]
φ＝φ1 φ2；
[0192]
构建最小二乘问题，使求目标函数达到极小的r’，t’：
[0193][0194]
基于g2o库求解非线性优化问题，得到使目标函数取极小的r、t，即重定位过程所需的精确位姿变换t
acc
＝{r
acc
,t
acc
}，，其中r
acc
＝r’r
vague
,t
acc
＝r’t
vague
t’。
[0195]
至此，重定位所需的二阶位姿变换t
vague
＝{r
vague
,t
vague
}以及t
acc
＝{r
acc
,t
acc
}求解完成，通过这个二阶的位姿变换关系可以实现移动机器人在室内任意丢失相对地图坐标系的位姿时，重新获取自身在地图坐标系中的位置，而无需让机器人重新回到建图原点，避免了传统位姿估计方法对于初始位姿固定的局限性。
[0196]
在仿真环境中生成一个100m*100m范围内包含100个拓扑节点的全局拓扑地图，并在全局拓扑地图截取5、10、15、20个不等的拓扑节点，并进行10组数值不同的平移旋转操作，作为待匹配的局部拓扑地图，即得到全局地图中10个不同位置的局部拓扑语义地图进行全局重定位测试。
[0197]
以截取15个拓扑节点进行10组数值不同的平移旋转操作，作为待匹配的局部拓扑地图为例，如图10所示，其中灰色点云为全局拓扑地图，白色部分为待重定位的局部拓扑地图。
[0198]
icp、ndt针对稀疏、初值条件差的拓扑节点图完全无法进行配准，如图11、图12所示，图11为基于icp的重定位结果，图12为基于ndt的重定位结果，可以看到配准解算的位姿变换无法将局部拓扑地图正确地转换到全局坐标系，而本发明方法可以很好地完成配准，如图13所示，且重定位误差能控制在10cm以内。
[0199]
减少局部地图拓扑特征数量的全局重定位测试，将原来的15个拓扑节点，减少到
10个和4个进行测试，包含4个局部地图拓扑特征的全局重定位结果如图14(a)所示，包含10个局部地图拓扑特征的全局重定位结果如图14(b)所示，可以看出即便局部拓扑特征数量很少的情况，本发明也能完成全局重定位。
[0200]
选取最具代表性的一组测试，即局部地图拓扑节点数量最少，选取4个局部地图拓扑节点，噪声点最多的一组，选取30个噪声点。记录10组重定位测试位置真值位姿、重定位粗配准后的位姿以及重定位误差在表1中，可以发现利用本发明方法，重定位能够顺利完成，且误差控制在10cm以内。
[0201]
表1仿真环境中拓扑地图基于三角拓扑结构相似度分析进行匹配与重定位结果
[0202]
测试序号真值位姿(m,m)重定位位姿(m,m)误差(m)1(12.36，23.67)(12.32，23.62)0.0642(53.54，81.98)(53.53，82)0.0223(23.19，74.23)(23.17，74.23)0.024(38.89，90.77)(38.85，90.79)0.0455(47.90，49.12)(47.91，49.13)0.0146(28.87，82.45)(28.85，82.47)0.0287(8.98，67.32)(8.98，67.3)0.028(34.55，88.31)(34.56，88.29)0.0219(90.91，8.93)(90.88，8.91)0.03710(77.13，7.53)(77.11，7.50)0.036