基于三维激光雷达的城市轨道交通巡检机器人定位方法与流程

1.本发明属于机器人自主导航相关技术领域，更具体地，涉及一种基于三维激光雷达的城市轨道交通巡检机器人定位方法。


背景技术：

2.智能巡检机器人由于其相比于人工作业的显著技术优势被广泛应用于军事、工业和民用各个领域。轨道交通车辆巡检现场的特点是：几乎无法使用gps定位信息，要求高精度定点作业，并且需要环境的高度信息，因此，三维激光雷达成为了轨道交通车辆巡检机器人最重要的传感器之一。基于三维激光雷达的巡检机器人自主导航技术一般拆分为两部分：建图和定位，其中，定位算法的鲁棒性和精度将直接影响巡检任务是否能够成功完成及完成的准确度。目前基于三维激光雷达的巡检机器人主流定位算法的思路是：首先加载建图过程中已经建好的地图点云，并进行降采样操作，随后不断对机器人运动过程中扫描的点云进行特征提取和降采样，得到当前帧的特征点云，然后将当前帧的特征点云与地图点云进行配准，基于点云配准算法迭代计算出当前帧特征点云与地图点云的相对位置关系，从而得到当前帧的定位信息。这种思路的优势在于把建图和定位两个步骤进行了解耦，建图过程只用提供一张点云地图作为定位过程的输入，除此之外定位过程是独立于建图过程的。这种思路的问题在于牺牲了定位精度和实时性，由于在定位时除地图外对环境没有其他任何先验知识，在进行点云配准的时候也就没有一个良好的初值，容易陷入局部最优从而损失定位精度，还会导致迭代时间更长，实时性差的问题。因此亟需设计一种精度更高、实时性更好的城市轨道机器人定位方法。


技术实现要素：

3.针对现有技术的以上缺陷或改进需求，本发明提供了一种基于三维激光雷达的城市轨道交通巡检机器人定位方法，通过将定位过程和建图过程相结合，能够实现轨道交通巡检任务的高精度定位。
4.为实现上述目的，按照本发明的一个方面，提供了一种基于三维激光雷达的城市轨道交通巡检机器人定位方法，所述方法包括：s1：采用三维激光雷达遍历待检测轨道交通车辆巡检现场，获取轨道交通车辆巡检现场的三维点云数据；s2：采用lego-loam技术基于所述三维点云数据建立特征点云地图，保存关键帧特征点云，计算关键帧位姿，并采用scan context技术提取关键帧的scd特征；关键帧的scd特征、关键帧特征点云和关键帧位姿，分别作为历史关键帧scd、历史关键帧特征点云和历史关键帧位姿；s3：定位过程中，采用scan context技术提取当前帧scd，并分别对历史关键帧scd和当前帧scd进行特征提取，选择与当前帧scd的特征向量距离最小的多个历史关键帧作为候选历史关键帧，随后对候选历史关键帧scd和当前帧scd对应列向量的余弦距离进行累加归一化获得两者距离，并将距离最小的历史关键帧scd对应的历史关键帧位姿作为当前帧的初始位姿估计；s4：采用lego-loam中的特征提取技术对当前帧进行特征提取得到当前帧的特征点云，获得该当前帧对应
的历史关键帧的邻域点云，而后以所述初始位姿估计为初始值，对当前帧的特征点云和历史关键帧的邻域点云进行icp配准，进而获得当前帧的精确位姿。
5.优选的，步骤s2中，采用如下方式获得关键帧对应的位姿：s21：将每一时刻的三维点云数据投影至row
×
col的距离图像中，并获取三维点云数据中每个点的粗糙度，其中，row为激光雷达每转一圈在水平方向上发射出的激光束数，col为激光雷达的线数；s22：将所述距离图像分割为多个子图像，对于每个子图像中的点，将粗糙度大于预设粗糙度的点设置为边缘特征点，将粗糙度小于预设粗糙度的点设置为平面特征点；s23：在每个子图像的每行提取第一预设数量个具有最大粗糙度的边缘特征点和第二预设数量个具有最小粗糙度的平面特征点，分别组成弱边缘特征点集和弱平面特征点集，统称为弱特征点；同理，在每个子图像的每行提取第三预设数量个具有最大粗糙度的边缘特征点和第四预设数量个具有最小粗糙度的平面特征点，分别组成强边缘特征点集和强平面特征点集，统称为强特征点，其中，第三预设数量小于第一预设数量，第四预设数量小于第二预设数量；s24：采用lom模块分别计算当前帧的强边缘特征点和强平面特征点分别与前一帧的弱边缘特征点和弱平面特征点的特征距离，该特征距离为当前帧位姿的函数，采用lego-loam中的两步l-m优化算法使该特征距离函数最小化，得到所述lom模块输出的最优位姿；同时，lmm模块求解当前帧弱特征点与局部地图的特征距离，采用l-m优化算法使该特征距离函数最小化，得到所述lmm模块输出的最优位姿，其中，局部地图由当前帧前k帧的弱特征点组合而成；s25：若lmm模块输出的最优位姿和lom模块输出的最优位姿均已获得，则最终的关键帧位姿为lmm模块输出的最优位姿；若仅获得lom模块输出的最优位姿则最终的关键帧位姿为lom模块输出的最优位姿。
6.优选的，步骤s2中，将所有关键帧的弱特征点均投影至世界坐标系下则得到关键帧特征点云，其中，世界坐标系为激光雷达采集第一帧三维点云数据时对应的坐标系。
7.优选的，步骤s2中，采用如下方式获得关键帧的scd：
8.s21’：将所述三维点云数据按周向划分为ns个扇形区，并按径向划分为nr个圆环，因此扇形区和圆环的交集组成nr×ns
个块；
9.s22’：将每个块中z坐标最大的点的z值代表该块，进而将每一帧激光雷达采集的三维点云数据表示为nr×ns
的矩阵，也即一个scd。
10.优选的，步骤s22’中将每个块中z坐标最大的点的z值代表该块具体包括：
11.将块中的三维点云数据通过如下公式映射为标量：
[0012][0013]
其中，为第i个圆环和第j个扇形区的交集块中的三维点云数据集；p为交集块中的三维点云数据，φ为块编码函数，z为三维点云数据的z轴坐标。
[0014]
优选的，步骤s3中分别对历史关键帧scd的特征和当前帧scd的特征进行提取具体包括：
[0015]
采用如下公式分别从历史关键帧scd和当前帧scd提取一个nr维的向量，也即历史关键帧圆环特征向量和当前帧圆环特征向量：
[0016]
[0017]
其中，ri为第i个圆环，ψ为圆环中三维点云的块的数量比圆环包含的块的总数量，
[0018]
优选的，步骤s3中，选择与当前帧scd的特征向量距离最小的多个历史关键帧作为候选历史关键帧，随后对候选历史关键帧scd和当前帧scd对应列向量的余弦距离进行累加归一化获得两者距离，并将距离最小的历史关键帧scd对应的历史关键帧位姿作为当前帧的初始位姿估计具体为：
[0019]
s31：采用历史关键帧圆环特征向量构建kd树，然后基于kd树采用广度优先，搜索出与当前帧圆环特征向量距离最小的前n个历史关键帧scd作为候选历史关键帧scd；
[0020]
s32：对候选历史关键帧scd和当前帧scd对应列向量的余弦距离进行累加归一化获得两者距离，具体公式为：
[0021][0022]
其中，i
t
为当前帧scd，i
t
为候选历史关键帧scd，为当前帧scd中的第j列，为候选历史关键帧scd中的第j列。
[0023]
优选的，为消除视角对距离计算的影响，步骤s32还包括：
[0024]
将候选历史关键帧scd的列进行循环移动，采用如下公式获得循环移动的最佳列数n*：
[0025][0026]
其中，为将候选历史关键帧scd的所有列向右移动n列，[ns]为集合{1，2，
…
，ns}；
[0027]
则最终候选历史关键帧scd和当前帧scd的距离为：
[0028][0029]
其中，为i
t
的所有列向右移动n*列后的候选历史关键帧scd。
[0030]
优选的，步骤s2还包括对距离图像中点分为地面点云和非地面点云，并对非地面点云分割为多个点云簇，保留大于预设点数的点云簇，并记这些点云簇为分割点云；提取弱特征点时，最大粗糙度的边缘特征点不属于所述地面点云，最小粗糙度的平面特征点属于地面点云或分割点云；提取强特征点时，最大粗糙度的边缘特征点不属于所述地面点云，最小粗糙度的平面特征点只属于地面点云。
[0031]
优选的，步骤s22中将所述距离图像均匀分割为多个子图像。
[0032]
总体而言，通过本发明所构思的以上技术方案与现有技术相比，本发明提供的基于三维激光雷达的城市轨道交通巡检机器人定位方法具有如下
[0033]
有益效果：
[0034]
1.本技术在建图过程中对关键帧进行特征提取，并保存关键帧的特征信息，定位时对当前帧进行特征提取，将当前帧与建图过程中对应的关键帧进行匹配，将建图过程中
对应的关键帧的位姿作为当前帧的初始位姿估计，再利用点云配准算法将当前帧点云和地图点云进行配准，得到当前帧的精确位姿。
[0035]
2.本技术通过将候选关键帧scd的列进行循环移动获得最佳列数进而消除视角对距离计算的影响。
[0036]
3.采用两个模块计算最优位姿，既保证了最优位姿获取的可靠性，又可以保证最优位姿的准确性。
[0037]
4.本技术将建图过程和定位过程相结合，进而可以让建图过程为定位过程提供更多的先验信息，使得定位时能够更快搜索到相似的历史关键帧附近，采用“粗略定位+精细定位”两步定位法，实现更高的定位精度。
附图说明
[0038]
图1是本技术基于三维激光雷达的城市轨道交通巡检机器人定位方法的步骤图；
[0039]
图2是本技术基于lego-loam和scan context技术获取关键帧的scd、关键帧特征点云和关键帧特征点云位姿的流程图；
[0040]
图3是本技术获取关键帧位姿的详细流程图；
[0041]
图4是本技术对非地面点云进行分割的原理图；
[0042]
图5中的(a)为基于ndt的传统定位方法的误差示意图，(b)为本技术提出的新定位方法的误差示意图。
具体实施方式
[0043]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
[0044]
请参阅图1，本发明提供了一种基于三维激光雷达的城市轨道交通巡检机器人定位方法，该方法包括如下步骤s1～s4。
[0045]
s1：采用三维激光雷达遍历待检测轨道交通车辆巡检现场，获取轨道交通车辆巡检现场的三维点云数据。
[0046]
s2：采用lego-loam技术基于所述三维点云数据建立特征点云地图，保存关键帧特征点云，计算关键帧位姿，并采用scan context技术提取关键帧的scd特征；关键帧的scd特征、关键帧特征点云和关键帧位姿，分别作为历史关键帧scd、历史关键帧特征点云和历史关键帧位姿。
[0047]
基于lego-loam(lightweight and ground-optimized lidar odometry and mapping，针对地面优化的轻量化激光雷达里程计和建图)对三维激光雷达扫描得到的三维点云数据进行处理建立特征点云地图，在建立点云地图的过程中，不断提取关键帧的scd(scan context descriptor，扫描帧上下文描述子)，同时保存关键帧对应的特征点云和位姿，作为地图信息的一部分，如图2所示。
[0048]
具体的，如图3所示，采用如下步骤s21～s24获得关键帧对应的位姿：
[0049]
s21：将每一时刻的三维点云数据投影至row
×
col的距离图像中，并获取三维点云
数据中每个点的粗糙度，其中，row为激光雷达每转一圈在水平方向上发射出的激光束数，col为激光雷达的线数。
[0050]
假设p
t
＝{p1，p2，
…
，pe，
…
，pn}为在t时刻三维激光雷达采集的三维点云数据，lego-loam首先将p
t
投影至一张row
×
col的距离图像中，其中，row为激光雷达每转一圈在水平方向上发射处的激光束数，col为激光雷达的线数，该值是由激光雷达的发射频率和转速决定的。
[0051]
设s是距离图像中与pe同一行的部分点的集合，包含了pe的左侧k个点和右侧k个点，从而可以计算出点pe的粗糙度c：
[0052][0053]
其中，像素值re为距离图像中点pe到三维激光雷达的距离，rf为距离图像中的点pf对应的像素值。
[0054]
s22：将所述距离图像分割为多个子图像，对于每个子图像中的点，将粗糙度大于预设粗糙度的点设置为边缘特征点，将粗糙度小于预设粗糙度的点设置为平面特征点。
[0055]
优选为将距离图像均分为多个子图像，分别对每个子图像进行相同的特征提取操作。
[0056]
设定预设粗糙度c
th
，将粗糙度大于c
th
的点标记为边缘特征点(边缘点)，将粗糙度小于c
th
的点记为平面特征点(平面点)。接着采用步骤s23提取弱特征点和强特征点。
[0057]
s23：在每个子图像的每行提取第一预设数量个具有最大粗糙度的边缘特征点和第二预设数量个具有最小粗糙度的平面特征点，分别组成弱边缘特征点集和弱平面特征点集，统称为弱特征点；同理，在每个子图像的每行提取第三预设数量个具有最大粗糙度的边缘特征点和第四预设数量个具有最小粗糙度的平面特征点，分别组成强边缘特征点集和强平面特征点集，统称为强特征点，其中，第三预设数量小于第一预设数量，第四预设数量小于第二预设数量。
[0058]
提取弱特征点方法如下：
[0059]
在每个子图像的每行中提取个具有最大粗糙度的边缘点，和个具有最小粗糙度的平面点，此过程提取到的弱边缘点和弱平面点集合分别记为和
[0060]
接着提取强特征点方法如下：
[0061]
该步骤中在每个子图像的每行中提取个具有最大粗糙度的边缘点和个具有最小粗糙度的平面点，此过程提取到的强边缘点和强平面点集合分别为fe和f
p
，显然有
[0062]
进一步优选的方案中，该步骤还包括对距离图像中点分为地面点云和非地面点云，并对非地面点云分割为多个点云簇，保留大于预设点数的点云簇，并记这些点云簇为分割点云提取弱特征点时，最大粗糙度的边缘特征点不属于所述地面点云最小粗糙度的平面特征点属于地面点云或分割点云提取强特征点时，最大粗糙
度的边缘特征点不属于所述地面点云最小粗糙度的平面特征点只属于地面点云
[0063]
可以采用下述方法筛选出地面点云：
[0064]
步骤一：将三维激光雷达扫描得到的360
°
点云沿周向均分为n个扇形区，每个扇形区又均分为m个切块；
[0065]
步骤二：将所有点投影到切块中，从三维坐标(x，y，z)降为二维坐标(d，z)，其中，
[0066]
步骤三：对同一扇形区的所有非空切块，取出每个切块中具有最小z值的点，根据这些点的二维坐标(d，z)使用最小二乘法拟合直线z＝kd+b，则该直线为这个扇形块内的地平面线；
[0067]
步骤四：对该扇形区内的任意点(d0，z0)，代入该直线z
′
＝kd0+b得到z
′
，如果|z
′‑
z0|＞z
th
(z
th
为阈值)，则认为该点不是地面点，否则是地面点。
[0068]
根据以上步骤，依次对当前帧扫描到的所有点进行判断，即可将距离图像中的地面点云筛选出来。
[0069]
可以采用如下方法对非地面点云进行分割：
[0070]
如下图4所示，oa和ob为两道相邻的激光雷达光束，a，b均为物体上的点，o为激光发射点，a为离o更远的点，b为离o更近的点，设oa为y轴，ob为x轴，β为ab与y轴的夹角，α为ob与y轴的夹角，d1，d2分别为oa和ob的长度，则β的计算公式为：
[0071][0072]
一般而言，如果a，b为同一物体上的两个点，则β的值会比较大，设置一个阈值θ
th
，当β＞θ
th
时，认为a，b是同一物体上的两个点，归为同一点云簇，否则不属于一个点云簇。
[0073]
s24：采用lom模块分别计算当前帧的强边缘特征点和强平面特征点分别与前一帧的弱边缘特征点和弱平面特征点的特征距离，该特征距离为当前帧位姿的函数，采用lego-loam中的两步l-m优化算法使该特征距离函数最小化，得到所述lom模块输出的最优位姿；同时，lmm模块求解当前帧弱特征点与局部地图的特征距离，采用l-m优化算法使该特征距离函数最小化，得到所述lmm模块输出的最优位姿，其中，局部地图由当前帧前k帧的弱特征点组合而成。
[0074]
首先采用lom(lidar odometry module，激光雷达里程计模块)模块以10hz的频率执行边缘点和地面点的帧间匹配，输出当前帧激光雷达位姿。具体步骤为：将当前帧的强边缘特征点和强平面特征点分别与前一帧的弱边缘特征点和弱平面特征点进行特征匹配，特征匹配过程中需要计算特征距离，其定义与loam(lidar odometry and mapping，激光雷达里程计与建图)算法中特征距离的定义相同，其可以表示为当前帧位姿的函数，然后以该函数为目标函数，以当前帧位姿为优化变量，使用lego-loam中提出的两步l-m(levenberg-marquardt，列文伯格-马奎特)优化方法解出最优的当前帧位姿，这是激光雷达里程计模块输出的最优位姿，记为
[0075]
同时，lmm(lidar mapping module，激光雷达建图模块)模块以2hz的频率执行当前帧弱特征点与局部地图的特征距离，采用l-m优化算法求解该特征距离得到所述lmm模块输出的最优位姿，其中，局部地图由当前帧前k帧的弱特征点组合而成：
[0076][0077]
同样的，以特征匹配过程中计算的特征距离函数为目标函数，以当前帧位姿为优化变量，使用l-m法优化得到更准确的当前帧位姿，这是激光雷达建图模块输出的最优位姿，记为
[0078]
s25：若lmm模块输出的最优位姿和lom模块输出的最优位姿均已获得，则最终的关键帧位姿为lmm模块输出的最优位姿；若仅获得lom模块输出的最优位姿则最终的关键帧位姿为lom模块输出的最优位姿。
[0079]
通过上述步骤s24获得两种当前位姿，一个是由激光雷达里程计模块以10hz频率发布的另一个是由激光雷达建图模块以2hz发布的本技术最终输出的当前帧位姿t
t
的发布频率为10hz，如果发布t
t
时已更新，则否则
[0080]
其中，定义世界坐标系{w}为激光雷达采集第一帧三维点云数据时对应的坐标系则当前帧位姿可以表示为当前帧激光雷达坐标系{l
t
}相对于世界坐标系{w}的位姿，这个相对位姿可以由一个六维向量来表示：t
t
＝[t
x
，ty，tz，θ
x
，θy，θz]
t
，其中，t
x
，ty，tz为沿{w}的x，y，z轴的位移量，θ
x
，θy，θz是绕{w}的轴x，y，z的旋转角。
[0081]
关键帧对应的特征点云采用如下方式获得：
[0082]
将所有关键帧的弱特征点均投影至世界坐标系{w}下则得到关键帧对应的特征点云，其中，世界坐标系{w}为激光雷达采集第一帧三维点云数据时对应的坐标系
[0083]
采用如下方式获得关键帧的scd：
[0084]
s21’：将所述三维点云数据按周向划分为ns个扇形区，并按径向划分为nr个圆环，因此扇形区和圆环的交集组成nr×ns
个块；
[0085]
s22’：将每个块中z坐标最大的点的z值代表该块，进而将每一帧激光雷达采集的三维点云数据表示为nr×ns
的矩阵，也即一个scd。
[0086]
将每个块中z坐标最大的点的z值代表该块公式表示为：
[0087]
将块中的三维点云数据通过如下公式映射为标量：
[0088][0089]
其中，为第i个圆环和第j个扇形区的交集块中的三维点云数据集；p为交集块中的三维点云数据，φ为块编码函数，z为三维点云数据的z轴坐标。
[0090]
将每一帧激光雷达采集的三维点云数据表示为nr×ns
的矩阵，也即一个scd，公式
表示为：
[0091][0092]
s3：定位过程中，并分别对历史关键帧scd和当前帧scd进行特征提取，选择与当前帧scd的特征向量距离最小的多个历史关键帧作为候选历史关键帧，随后对候选历史关键帧scd和当前帧scd对应列向量的余弦距离进行累加归一化获得两者距离，并将距离最小的历史关键帧scd对应的历史关键帧位姿作为当前帧的初始位姿估计。
[0093]
具体步骤为，采用如下公式分别从历史关键帧scd和当前帧scd提取一个nr维的向量，也即历史关键帧圆环特征向量和当前帧圆环特征向量：
[0094][0095]
其中，ri为第i个圆环，ψ为圆环中三维点云的块的数量比圆环包含的块的总数量，
[0096]
选择与当前帧scd的特征最接近的多个历史关键帧作为候选历史关键帧，随后对历史关键帧scd和当前帧scd对应列向量的余弦距离进行累加归一化获得两者距离，并将距离最小的历史关键帧scd对应的历史关键帧位姿作为当前帧的初始位姿估计具体为：
[0097]
s31：采用历史关键帧圆环特征向量构建kd树(k dimensional tree)，然后基于kd树采用广度优先搜索出与当前帧圆环特征向量k
t
距离(向量的欧式距离)最小的前n个历史关键帧scd作为候选历史关键帧scd。
[0098]
s32：对候选历史关键帧scd和当前帧scd对应列向量的余弦距离进行累加归一化获得两者距离，具体公式为：
[0099][0100]
其中，i
t
为当前帧scd，i
t
为候选历史关键帧scd，为当前帧的scd中的第j列，为候选历史关键帧scd中的第j列。
[0101]
为消除视角对距离计算的影响，步骤s32还包括：
[0102]
将候选历史关键帧scd的列进行循环移动，采用如下公式获得循环移动的最佳列数n*：
[0103][0104]
其中，为将候选历史关键帧scd的所有列向右移动n列，[ns]为集合{1，2，
…
，ns}；
[0105]
则最终候选历史关键帧scd和当前帧scd的距离为：
[0106][0107]
其中，为i
t
的所有列向右移动n*列后的候选历史关键帧scd。
[0108]
选择与当前帧scd距离最小的历史关键帧作为与当前帧匹配的历史关键帧，将该历史关键帧的位姿t
t
作为当前帧的初始位姿估计即
[0109]
s4：采用lego-loam中的特征提取技术对当前帧进行特征提取得到当前帧的特征点云，获得该当前帧对应的历史关键帧的邻域点云，而后以所述初始位姿估计为初始值，对当前帧的特征点云和历史关键帧的邻域点云进行icp配准，进而获得当前帧的精确位姿。
[0110]
对当前帧点云p
t
进行步骤s2中的特征提取和降采样操作，得到当前帧的弱特征点云记与当前帧scd匹配的历史关键帧的弱特征点云为然后求出该历史关键帧的邻域点云，即前后各k帧弱特征点云的集合：
[0111][0112]
然后以当前帧的初始位姿估计为初始值，对与n
t
进行icp点云配准，得到当前帧的精确位姿
[0113]
实施例
[0114]
本实施例采用16线及以上的三维激光雷达，其高精度定位方法具体的实施例步骤如下所示：
[0115]
步骤1，在智能巡检机器人上搭载16线或以上的三维激光雷达，要求激光雷达安装在车顶部，四周没有被遮挡。巡检机器人开机后，将其手动操控到巡检起点，到达巡检起点后，启动建图算法程序，激光雷达开始将扫描到的三维点云数据传输给建图算法。
[0116]
步骤2：建立轨道交通巡检现场的特征点云地图，并在建立点云地图的过程中保存关键帧的scd、特征点云及位姿。在这个过程中，机器人将以0.8m/s的平均速度沿巡检路线行走，最终回到巡检起点，完成对巡检场景的感知，形成巡检现场的地图。下面具体描述建图过程：建图算法收到激光雷达输入的三维点云信息后，开始对逐帧的点云进行处理。以vlp-16激光雷达为例，假设p
t
＝{p1，p2，
…
，pn}是在t时刻vlp-16采集到的点云，lego-loam首先将p
t
投影到一张1800
×
16的距离图像中，对距离图像进行逐列评估将地面点云筛选出来，随后对地面点云以外的点云进行分割操作，将点云分为多个点云簇，并只保留大于30个点的点云簇，记这些点云簇的集合为分割点云设s是在距离图像中与pi同一行的部分点的集合，包含了pi的左侧5个点和右侧5个点。则由公式(1)可以计算出距离图像中每个点的粗糙度c。
[0117]
随后把距离图像均分为6个子图像，分别对每个子图像进行相同的特征提取操作。设定阈值为0.1，粗糙度大于0.1的点标记为边缘特征点(边缘点)，小于0.1的点标记为平面特征点(平面点)。首先提取弱特征点，在每个子图像的每行中提取出个具有最大粗糙度的边缘点(不属于)和个具有最小粗糙度的平面点(可以属于也可以属于)，此过程中提取到的弱边缘点和弱平面点集合分别记为和再提取强特征点，这次在每个子图像的每行中提取出个具有最大粗糙度的边缘点(不属于
)和个具有最小粗糙度的平面点(只属于)，此过程中提取到的强边缘点和强平面点集合分别记为和
[0118]
定义世界坐标系{w}为第一帧的激光雷达坐标系则当前帧位姿可表示为当前帧激光雷达坐标系{l
t
}相对于世界坐标系{w}的位姿，这个相对位姿可以由一个六维向量来表示：t
t
＝[t
x
，ty，tz，θ
x
，θy，θz]
t
，其中t
x
，ty，tz是沿{w}的x，y，z轴的位移量，θ
x
，θy，θz是绕{w}的x，y，z轴的旋转角。
[0119]
首先由激光雷达里程计模块(lidar odometry module)以10hz的频率执行边缘点和平面点的帧间匹配，输出当前帧激光雷达位姿(后面简称为当前帧位姿)。将当前帧的强边缘点和强平面点分别与前一帧的弱边缘点和弱平面点进行特征匹配，特征匹配过程中需要计算特征距离，其定义与loam中特征距离的定义是相同的，它可以表示为当前帧位姿的函数，然后以该函数为目标函数，以当前帧位姿为优化变量，用lego-loam中提出的两步l-m(levenberg-marquardt，列文伯格-马奎特)优化方法解出激光雷达里程计模块输出的最优位姿
[0120]
同时，激光雷达建图模块(lidar mapping module)以2hz的频率执行当前帧弱特征点与局部地图的特征匹配，输出更精确的当前帧位姿。当前帧的弱特征点与局部地图的特征匹配方法与loam激光雷达建图模块的特征匹配方法相同，其中局部地图由前50帧的弱特征点组合而成：
[0121][0122]
同样地，以特征匹配过程中计算的特征距离函数为目标函数，以当前帧位姿为优化变量，使用l-m法优化得到激光雷达建图模块输出的最优位姿
[0123]
算法最终输出的当前帧位姿t
t
的发布频率是10hz，如果发布t
t
时已更新，则取否则t
t
的物理意义是全局坐标系{w}到当前帧的点云坐标系{l
t
}的位姿变换，因此对当前帧的弱特征点云做的逆坐标变换即可将其坐标转换到全局坐标系{w}下，将所有关键帧的弱特征点都投影到{w}下后，即得到了最终的特征点云地图，将其保存为本地文件。
[0124]
具体实施过程中，扫描帧上下文描述子scd的定义如下：将空间按周向划分为20个扇形区(sector)，按径向划分为60个圆环(ring)，一个圆环和一个扇形区的交集称为一个块(bin)，所以一共有20
×
60个块，设表示第i个圆环和第j个扇形区的交集块中的点云集合，通过块编码函数来将该块内的点云映射成一个标量，块编码函数的定义见式(4)，因此可以将一帧激光雷达点云表示为一个20
×
60的矩阵，称为一个scd：
[0125]
[0126]
求得关键帧的scd后，与关键帧特征点云和位姿一起保存为本地文件。
[0127]
步骤3，在机器人完成对巡检现场的感知后，得到了巡检现场的地图，包括特征点云地图和关键帧scd、特征点云、位姿等信息。当再次从巡检起点出发，正式开始巡检任务时，就启动定位算法程序，在描述这部分时，将建图时历史关键帧的时间戳用大写t来表示，定位时当前帧的时间戳用小写t来表示，以作区分。
[0128]
首先加载本地特征点云地图文件，使用边长为0.2m的体素网格对地图点云进行降采样；随后加载历史关键帧scd文件，对scd进行特征提取，提取出圆环特征向量(ring key)，圆环特征向量是从scd中提取出的一个20维的向量：
[0129]
k＝(ψ(r1)，
…
，ψ(r
20
))
ꢀꢀꢀꢀ
(13)
[0130]
其中ri表示scd中的第i行(即第i个圆环)，映射ψ用于计算圆环的占据比，即圆环中含有扫描点的块的数量比圆环包含的块的总数量：
[0131][0132]
对当前帧点云提取scd与建图过程中对关键帧提取scd的原理是相同的，提取出当前帧scd后，开始搜索最接近的历史关键帧scd，这个过程包括两步：第一步，用历史关键帧的圆环特征向量构建一颗kd树(kdimensional tree)，然后基于kd树采用广度优先搜索出与当前帧圆环特征
[0133]
向量k
t
距离(向量的欧式距离)最小的前10个scd作为候选历史关键帧scd；第二步，对于这10个候选历史关键帧scd，分别计算它们与当前scd的距离，设某个候选历史关键帧scd为i
t
，当前帧scd为i
t
，两者之间的距离定义见公式(7)。
[0134]
因为scd的第一列总是激光雷达的第一束激光返回的扫描点，所以机器人在相同位置不同视角下得到的scd是不同的，它们的差异仅仅在于列的排列顺序。为了消除视角对距离计算准确性的影响，需要把i
t
的列进行循环移动，表示把i
t
的所有列向右移动n列，最后n列变为前面n列，然后计算i
t
与的距离以确定最佳的n*，使得最小，记[ns]表示集合{1，2，
…
，60}，n*的计算方法见式(8)，最终i
t
与i
t
的距离定义见式(9)；选择与当前帧scd距离最小的候选帧作为与当前帧匹配的历史关键帧，将该历史关键帧的位姿t
t
作为当前帧的初始位姿估计即
[0135]
对当前帧点云p
t
进行步骤1中相同的特征提取和降采样操作，得到当前帧的弱特征点云记与当前帧scd匹配的历史关键帧的弱特征点云为然后求出该历史关键帧的邻域点云，即前后各25帧弱特征点云的集合：
[0136][0137]
然后以当前帧的初始位姿估计为初始值，对与n
t
进行icp点云配准，得到当前帧的精确位姿
[0138]
经实验验证，如图5所示，本发明提出的基于三维激光雷达的城市轨道交通车辆智
能巡检机器人高精度定位方法具有较高的定位精度和较强的定位鲁棒性。
[0139]
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种基于三维激光雷达的城市轨道交通巡检机器人定位方法，其特征在于，所述方法包括：s1：采用三维激光雷达遍历待检测轨道交通车辆巡检现场，获取轨道交通车辆巡检现场的三维点云数据；s2：采用lego-loam技术基于所述三维点云数据建立特征点云地图，保存关键帧特征点云，计算关键帧位姿，并采用scan context技术提取关键帧的scd特征；关键帧的scd特征、关键帧特征点云和关键帧位姿，分别作为历史关键帧scd、历史关键帧特征点云和历史关键帧位姿；s3：定位过程中，采用scan context技术提取当前帧scd，并分别对历史关键帧scd和当前帧scd进行特征提取，选择与当前帧scd的特征向量距离最小的多个历史关键帧作为候选历史关键帧，随后对候选历史关键帧scd和当前帧scd对应列向量的余弦距离进行累加归一化获得两者距离，并将距离最小的历史关键帧scd对应的历史关键帧位姿作为当前帧的初始位姿估计；s4：采用lego-loam中的特征提取技术对当前帧进行特征提取得到当前帧的特征点云，获得该当前帧对应的历史关键帧的邻域点云，而后以所述初始位姿估计为初始值，对当前帧的特征点云和历史关键帧的邻域点云进行icp配准，进而获得当前帧的精确位姿。2.根据权利要求1所述的方法，其特征在于，步骤s2中，采用如下方式获得关键帧位姿：s21：将每一时刻的三维点云数据投影至row
×
col的距离图像中，并获取三维点云数据中每个点的粗糙度，其中，row为激光雷达每转一圈在水平方向上发射出的激光束数，col为激光雷达的线数；s22：将所述距离图像分割为多个子图像，对于每个子图像中的点，将粗糙度大于预设粗糙度的点设置为边缘特征点，将粗糙度小于预设粗糙度的点设置为平面特征点；s23：在每个子图像的每行提取第一预设数量个具有最大粗糙度的边缘特征点和第二预设数量个具有最小粗糙度的平面特征点，分别组成弱边缘特征点集和弱平面特征点集，统称为弱特征点；同理，在每个子图像的每行提取第三预设数量个具有最大粗糙度的边缘特征点和第四预设数量个具有最小粗糙度的平面特征点，分别组成强边缘特征点集和强平面特征点集，统称为强特征点，其中，第三预设数量小于第一预设数量，第四预设数量小于第二预设数量；s24：采用lom模块分别计算当前帧的强边缘特征点和强平面特征点分别与前一帧的弱边缘特征点和弱平面特征点的特征距离，该特征距离为当前帧位姿的函数，采用lego-loam中的两步l-m优化算法使该特征距离函数最小化，得到所述lom模块输出的最优位姿；同时，lmm模块求解当前帧弱特征点与局部地图的特征距离，采用l-m优化算法使该特征距离函数最小化，得到所述lmm模块输出的最优位姿，其中，局部地图由当前帧前k帧的弱特征点组合而成；s25：若lmm模块输出的最优位姿和lom模块输出的最优位姿均已获得，则最终的关键帧位姿为lmm模块输出的最优位姿；若仅获得lom模块输出的最优位姿则最终的关键帧位姿为lom模块输出的最优位姿。3.根据权利要求2所述的方法，其特征在于，步骤s2中，将所有关键帧的弱特征点均投影至世界坐标系下则得到关键帧特征点云，其中，世界坐标系为激光雷达采集第一帧三维
点云数据时对应的激光雷达坐标系。4.根据权利要求1或3所述的方法，其特征在于，步骤s2中，采用如下方式获得关键帧的scd：s21’：将所述三维点云数据按周向划分为ns个扇形区，并按径向划分为n
r
个圆环，因此扇形区和圆环的交集组成n
r
×
n
s
个块；s22’：将每个块中z坐标最大的点的z值代表该块，进而将每一帧激光雷达采集的三维点云数据表示为n
r
×
n
s
的矩阵，也即一个scd。5.根据权利要求4所述的方法，其特征在于，步骤s22’中将每个块中z坐标最大的点的z值代表该块具体包括：将块中的三维点云数据通过如下公式映射为标量：其中，为第i个圆环和第j个扇形区的交集块中的三维点云数据集；p为交集块中的三维点云数据，φ为块编码函数，z为三维点云数据的z轴坐标。6.根据权利要求4的方法，其特征在于，步骤s3中分别对历史关键帧scd和当前帧scd进行特征提取具体包括：采用如下公式分别从历史关键帧scd和当前帧scd提取一个n
r
维的向量，也即历史关键帧圆环特征向量和当前帧圆环特征向量：其中，r
i
为第i个圆环，ψ为圆环中含有三维点云的块的数量比圆环中块的总数量，7.根据权利要求6所述的方法，其特征在于，步骤s3中，选择与当前帧scd的特征距离最小的多个历史关键帧作为候选历史关键帧，随后对候选历史关键帧scd和当前帧scd对应列向量的余弦距离进行累加归一化获得两者距离，并将距离最小的历史关键帧scd对应的历史关键帧位姿作为当前帧的初始位姿估计具体为：s31：采用历史关键帧圆环特征向量构建kd树，然后基于kd树采用广度优先搜索，搜索出与当前帧圆环特征向量距离最小的前n个历史关键帧scd作为候选历史关键帧scd；s32：对候选历史关键帧scd和当前帧scd对应列向量的余弦距离进行累加归一化获得两者距离，具体公式为：其中，i
t
为当前帧scd，i
t
为候选历史关键帧scd，为当前帧scd中的第j列，为候选历史关键帧scd中的第j列。8.根据权利要求7所述的方法，其特征在于，为消除视角对距离计算的影响，步骤s32还包括：将候选历史关键帧scd的列进行循环移动，采用如下公式获得循环移动的最佳列数n*：
其中，为将候选历史关键帧scd的所有列向右移动n列，[n
s
]为集合{1，2，
…
，n
s
}；则最终候选历史关键帧scd和当前帧scd的距离为：其中，为i
t
的所有列向右移动n*列后的候选历史关键帧scd。9.根据权利要求2所述的方法，其特征在于，步骤s2还包括对距离图像中点分为地面点云和非地面点云，并对非地面点云分割为多个点云簇，保留大于预设点数的点云簇，并记这些点云簇为分割点云；提取弱特征点时，最大粗糙度的边缘特征点不属于所述地面点云，最小粗糙度的平面特征点属于地面点云或分割点云；提取强特征点时，最大粗糙度的边缘特征点不属于所述地面点云，最小粗糙度的平面特征点只属于地面点云。10.根据权利要求2所述的方法，其特征在于，步骤s22中将所述距离图像均匀分割为多个子图像。

技术总结
本发明属于机器人自主导航相关技术领域，其公开了一种基于三维激光雷达的城市轨道交通巡检机器人定位方法，该方法包括：采用LeGO-LOAM技术建立特征点云地图，保存历史关键帧特征点云，计算历史关键帧位姿，并提取历史关键帧SCD特征；定位过程中，选择与当前帧SCD的特征最接近的多个历史关键帧作为候选历史关键帧，选取距离最小的候选历史关键的位姿作为当前帧的初始位姿估计。以初始位姿估计为初始值，对当前帧的特征点云和对应历史关键帧的邻域点云进行ICP配准，获得当前帧的精确位姿。本申请通过把定位过程中的实时点云特征和建图过程中的历史点云特征进行特征关联，实现轨道交通巡检任务的高精度定位。交通巡检任务的高精度定位。交通巡检任务的高精度定位。


技术研发人员：司言 左玉东 喻蝶 白丹辉 鲍柏仲 杨震 段暕 龙胡 詹小斌 史铁林
受保护的技术使用者：中铁第四勘察设计院集团有限公司
技术研发日：2023.05.12
技术公布日：2023/9/6