一种基于多线激光雷达的机器人重定位方法及系统与流程

1.本发明涉及机器人定位技术领域，具体地说是一种基于多线激光雷达的机器人重定位方法及系统。


背景技术：

2.现阶段，多线激光雷达在机器人上得到了广泛地应用，主要涉及建图、定位、环境感知等领域。其中，定位是串联地图与导航的纽带，解决机器人“我在哪儿”的问题。但在机器人开机初始与定位丢失状态下，往往需要借助外界因素或人工干预来完成重定位，此处外界因素主要指使用gps来提供全局位姿。gps的接入往往意味着更高的价格成本，而且在gps被遮挡情况下，无法为机器人提供可靠的全局位姿。


技术实现要素：

3.本发明的技术任务是针对以上不足之处，提供一种基于多线激光雷达的机器人重定位方法及系统，可在不需要外界与人工干预的情况下完成机器人的自主重定位。
4.本发明解决其技术问题所采用的技术方案是：
5.一种基于多线激光雷达的机器人重定位方法，该方法的实现过程如下：
6.1)、输入关键帧与关键位姿；
7.2)、对输入的关键帧进行数据预处理，包括体素滤波下采样与提取感兴趣区域；
8.3)、点云分割，将点云数据分为地面与非地面两部分；
9.4)、生成搜索特征，提取点云非地面点搜索特征，根据点云标签将三维点云转化为xy平面上的特征矩阵；
10.5)、遍历完所有关键帧；
11.6)、输入实时雷达与九轴imu数据，重复步骤2)、3)、4)，得到矩阵rc；
12.7)、两步搜索，先通过偏航角对已加载的关键帧特征集进行初步筛选，缩小搜索范围，再通过计算矩阵相似性的方法进行第二次搜索；
13.8)、校验，将步骤7)得到的候选关键帧与当前输入关键帧进行ndt点云配准；设置得分阈值，若ndt点云配准得分小于所述得分阈值，则校验成功；
14.9)、位姿变换，将校验成功的候选关键帧对应的关键位姿转化为4
×
4矩阵，计算重定位变换矩阵；
15.10)、将重定位变换矩阵转化为六维位姿[x,y,z,roll,pitch,yaw]作为重定位位姿输出。
[0016]
该方法首先加载预先保存的关键帧及关键位姿，生成关键帧搜索特征集合；其次输入实时点云帧及九轴imu数据，采用两步搜索的方法获取与当前帧最相似的候选关键帧；再次对候选关键帧进行校验，若校验通过，则进行坐标变换；最后输出重定位结果。可实现在不外接gps及人为干预的情况下完成机器人的自主重定位。
[0017]
优选的，所述关键帧与关键位姿，为机器人构建环境地图时所保存，通常为运动状
态下每1秒保存一次。
[0018]
优选的，所述体素滤波采用pcl点云处理库所提供的方法；
[0019]
提取感兴趣区域从距离方面进行处理，设置最小半径r
min
和最大半径r
max
，切除近处及远处点云，设点云中的点用p(x,y,z)表示，则感兴趣区域中的点满足以下条件：
[0020][0021]
优选的，所述点云分隔，使用方法为射线法，并对分割后的点云添加标签标记，地面点云标记为label1，非地面点云标记为label2。
[0022]
优选的，所述根据点云标签将三维点云转化为xy平面上的特征矩阵，设置需要划分的扇区角度分辨率sector
angle
，则可求得将整个三维点云划分的扇区个数sector
num
，sector
angle
取值为弧度制：
[0023][0024]
公式中，π表示圆周率；
[0025]
遍历三维点云，计算非地面点所属的扇区sector
id
，首先计算点云与坐标原点连线相对于x轴正方向的夹角angle，angle取值为弧度制：
[0026][0027]
然后计算sector
id
：
[0028][0029]
统计每个扇区内半径r最小的非地面点：
[0030][0031]
并对统计得到的点集进行数据重组，构建1行sector
num
列矩阵r
p
，r
p
每列表示每个扇区内半径r最小的非地面点，每列包含四个特征，分别为半径、x轴坐标、y轴坐标、标签。
[0032]
优选的，所述两步搜索，
[0033]
首先设置初步搜索范围，根据输入九轴imu的偏航角yawc，寻找关键帧对应的关键位姿偏航角yawk与yawc相差角度δyaw的候选关键帧集，偏航角取值为弧度制：
[0034]-δyaw《yaw
c-yawk《δyaw
[0035]
然后，遍历候选关键帧集，设置扇区旋转步长step，根据旋转步长将r
p
进行列平移得到r’p
，将每次列平移得到的r’p
与rc进行对比，计算相似性，对比得到相似性最小时的r
p
对应的候选关键帧，同时计算出r
p
与rc之间的相对变换矩阵r
δ
:
[0036]
rc＝r
δ
*r
p
。
[0037]
进一步的，所述校验时，将步骤7)得到的候选关键帧与当前输入关键帧进行ndt点云配准，将r
δ
作为先验变换矩阵。
[0038]
进一步的，设所述4
×
4矩阵为rk，重定位变换矩阵为r
final
，则
[0039]rfinal
＝r
δ
*rk。
[0040]
本发明还要求保护一种基于多线激光雷达的机器人重定位系统，包括关键帧与关
键位姿输入模块、实时帧输入模块、预处理模块、点云分割模块、生成搜索特征模块、实时imu输入模块、两部搜索模块、校验模块、位姿变换模块以及输出重定位位姿模块，
[0041]
该系统通过上述的基于多线激光雷达的机器人重定位方法实现机器人重定位。
[0042]
关键帧与关键位姿输入模块用于输入关键帧与关键位姿；
[0043]
实时帧输入模块和实时imu输入模块用于实时帧输入及实时雷达与九轴imu 数据输入；
[0044]
预处理模块用于对输入的关键帧进行数据预处理，包括体素滤波下采样与提取感兴趣区域；
[0045]
点云分割模块将点云数据分为地面与非地面两部分；
[0046]
生成搜索特征模块提取点云非地面点搜索特征，根据点云标签将三维点云转化为xy平面上的特征矩阵；
[0047]
两部搜索模块先通过偏航角对已加载的关键帧特征集进行初步筛选，缩小搜索范围，再通过计算矩阵相似性的方法进行第二次搜索；
[0048]
校验模块用于将两部搜索模块得到的候选关键帧与当前输入关键帧进行 ndt点云配准；设置得分阈值，若ndt点云配准得分小于所述得分阈值，则校验成功；
[0049]
位姿变换模块将校验成功的候选关键帧对应的关键位姿转化为4
×
4矩阵，计算重定位变换矩阵；
[0050]
输出重定位位姿模块用于将重定位变换矩阵转化为六维位姿 [x,y,z,roll,pitch,yaw]作为重定位位姿输出。
[0051]
本发明还要求保护一种基于多线激光雷达的机器人重定位装置，包括：至少一个存储器和至少一个处理器；
[0052]
所述至少一个存储器，用于存储机器可读程序；
[0053]
所述至少一个处理器，用于调用所述机器可读程序，执行上述的方法。
[0054]
本发明的一种基于多线激光雷达的机器人重定位方法及系统与现有技术相比，具有以下有益效果：
[0055]
本方法或系统首先加载预先保存的关键帧及关键位姿，生成关键帧搜索特征集合；其次输入实时点云帧及九轴imu数据，采用两步搜索的方法获取与当前帧最相似的候选关键帧；再次对候选关键帧进行校验，若校验通过，则进行坐标变换；最后输出重定位结果。可实现在不外接gps及人为干预的情况下完成机器人的自主重定位。
附图说明
[0056]
图1是本发明实施例提供的基于多线激光雷达的机器人重定位方法实现过程示图。
具体实施方式
[0057]
下面结合具体实施例对本发明作进一步说明。
[0058]
本发明实施例提供了一种基于多线激光雷达的机器人重定位方法，该方法的实现过程如下：
[0059]
1)、输入关键帧与关键位姿，所述关键帧与关键位姿，为机器人构建环境地图时所
保存，通常为运动状态下每1秒保存一次。
[0060]
2)、对输入的关键帧进行数据预处理，包括体素滤波下采样与提取感兴趣区域。
[0061]
所述体素滤波采用pcl点云处理库所提供的方法，体素尺寸设置为 (0.25,0.25,0.25)cm；
[0062]
提取感兴趣区域主要从距离方面进行处理，设置最小半径r
min
、最大半径 r
max
分别为5m与85米，切除近处及远处点云，设点云中的点用p(x,y,z)表示，则感兴趣区域中的点满足以下条件：
[0063][0064]
3)、点云分割，将点云数据分为地面与非地面两部分，使用方法为射线法，并对分割后的点云添加标签标记，地面点云标记为label1，label1取值为常数5；非地面点云标记为label2，label2取值为常数10。
[0065]
4)、生成搜索特征，该部分根据点云标签将三维点云转化为xy平面上的特征矩阵；具体如下：
[0066]
设置需要划分的扇区角度分辨率sector
angle
，此处将sector
angle
设置为 0.01745，则可将整个三维点云划分的扇区个数sector
num
为：
[0067][0068]
公式中，π表示圆周率；
[0069]
遍历三维点云，计算非地面点所属的扇区sector
id
，首先计算点云与坐标原点连线相对于x轴正方向的夹角angle：
[0070][0071]
接着计算sector
id
：
[0072][0073]
统计每个扇区内半径r最小的非地面点：
[0074][0075]
并对统计得到的点集进行数据重组，构建1行sector
num
列矩阵r
p
，r
p
每列表示每个扇区内半径r最小的非地面点，每列包含四个特征，分别为半径、x轴坐标、y轴坐标、标签。
[0076]
5)、遍历完所有关键帧。
[0077]
6)、输入实时雷达与九轴imu数据，重复步骤2)、3)、4)，得到矩阵rc。
[0078]
7)、两步搜索，先通过偏航角对已加载的关键帧特征集进行初步筛选，缩小搜索范围，再通过计算矩阵相似性的方法进行第二次搜索：
[0079]
首先设置初步搜索范围，根据输入九轴imu的偏航角yawc，寻找关键帧对应的关键位姿偏航角yawk与yawc相差角度δyaw的候选关键帧集，此处将δyaw 设为1.57：
[0080]-δyaw《yaw
c-yawk《δyaw
[0081]
接着，遍历候选关键帧集，设置扇区旋转步长step，根据旋转步长将r
p
进行列平移
得到r
′
p
，将每次列平移得到的r
′
p
与rc进行对比，计算相似性，对比得到相似性最小时的r
p
对应的候选关键帧，同时计算出r
p
与rc之间的相对变换矩阵r
δ
:
[0082]
rc＝r
δ
*r
p
。
[0083]
8)、校验，将步骤7)得到的候选关键帧与当前输入关键帧进行ndt点云配准，并将r
δ
作为先验变换矩阵；设置得分阈值score
th
，score
th
取值为0.5，若 ndt点云配准得分score
ndt
小于score
th
，则校验成功。
[0084]
9)、位姿变换，将校验成功的候选关键帧对应的关键位姿转化为4
×
4矩阵rk，计算重定位变换矩阵r
final
：
[0085]rfinal
＝r
δ
*rk。
[0086]
10)、将重定位变换矩阵r
final
转化为六维位姿[x,y,z,roll,pitch,yaw]作为重定位位姿输出。
[0087]
该方法首先加载预先保存的关键帧及关键位姿，生成关键帧搜索特征集合；其次输入实时点云帧及九轴imu数据，采用两步搜索的方法获取与当前帧最相似的候选关键帧；再次对候选关键帧进行校验，若校验通过，则进行坐标变换；最后输出重定位结果。可实现在不外接gps及人为干预的情况下完成机器人的自主重定位。
[0088]
本发明实施例还提供了一种基于多线激光雷达的机器人重定位系统，包括关键帧与关键位姿输入模块、实时帧输入模块、预处理模块、点云分割模块、生成搜索特征模块、实时imu输入模块、两部搜索模块、校验模块、位姿变换模块以及输出重定位位姿模块，
[0089]
关键帧与关键位姿输入模块用于输入关键帧与关键位姿；
[0090]
实时帧输入模块和实时imu输入模块用于实时帧输入及实时雷达与九轴imu 数据输入；
[0091]
预处理模块用于对输入的关键帧进行数据预处理，包括体素滤波下采样与提取感兴趣区域；
[0092]
点云分割模块将点云数据分为地面与非地面两部分；
[0093]
生成搜索特征模块提取点云非地面点搜索特征，根据点云标签将三维点云转化为xy平面上的特征矩阵；
[0094]
两部搜索模块先通过偏航角对已加载的关键帧特征集进行初步筛选，缩小搜索范围，再通过计算矩阵相似性的方法进行第二次搜索；
[0095]
校验模块用于将两部搜索模块得到的候选关键帧与当前输入关键帧进行 ndt点云配准；设置得分阈值，若ndt点云配准得分小于所述得分阈值，则校验成功；
[0096]
位姿变换模块将校验成功的候选关键帧对应的关键位姿转化为4
×
4矩阵，计算重定位变换矩阵；
[0097]
输出重定位位姿模块用于将重定位变换矩阵转化为六维位姿 [x,y,z,roll,pitch,yaw]作为重定位位姿输出。
[0098]
该系统实现机器人重定位的具体实现过程如下：
[0099]
1)、输入关键帧与关键位姿，所述关键帧与关键位姿，为机器人构建环境地图时所保存，通常为运动状态下每1秒保存一次。
[0100]
2)、对输入的关键帧进行数据预处理，包括体素滤波下采样与提取感兴趣区域。
[0101]
所述体素滤波采用pcl点云处理库所提供的方法，体素尺寸设置为 (0.25,0.25,
0.25)cm；
[0102]
提取感兴趣区域主要从距离方面进行处理，设置最小半径r
min
、最大半径 r
max
分别为5m与85米，切除近处及远处点云，设点云中的点用p(x,y,z)表示，则感兴趣区域中的点满足以下条件：
[0103][0104]
3)、点云分割，将点云数据分为地面与非地面两部分，使用方法为射线法，并对分割后的点云添加标签标记，地面点云标记为label1，label1取值为常数5；非地面点云标记为label2，label2取值为常数10。
[0105]
4)、生成搜索特征，该部分根据点云标签将三维点云转化为xy平面上的特征矩阵；具体如下：
[0106]
设置需要划分的扇区角度分辨率sector
angle
，此处将sector
angle
设置为 0.01745，则可将整个三维点云划分的扇区个数sector
num
为：
[0107][0108]
公式中，π表示圆周率；
[0109]
遍历三维点云，计算非地面点所属的扇区sector
id
，首先计算点云与坐标原点连线相对于x轴正方向的夹角angle：
[0110][0111]
接着计算sector
id
：
[0112][0113]
统计每个扇区内半径r最小的非地面点：
[0114][0115]
并对统计得到的点集进行数据重组，构建1行sector
num
列矩阵r
p
，r
p
每列表示每个扇区内半径r最小的非地面点，每列包含四个特征，分别为半径、x轴坐标、y轴坐标、标签。
[0116]
5)、遍历完所有关键帧。
[0117]
6)、输入实时雷达与九轴imu数据，重复步骤2)、3)、4)，得到矩阵rc。
[0118]
7)、两步搜索，先通过偏航角对已加载的关键帧特征集进行初步筛选，缩小搜索范围，再通过计算矩阵相似性的方法进行第二次搜索：
[0119]
首先设置初步搜索范围，根据输入九轴imu的偏航角yawc，寻找关键帧对应的关键位姿偏航角yawk与yawc相差角度δyaw的候选关键帧集，此处将δyaw 设为1.57：
[0120]-δyaw《yaw
c-yawk《δyaw
[0121]
接着，遍历候选关键帧集，设置扇区旋转步长step，根据旋转步长将r
p
进行列平移得到r
′
p
，将每次列平移得到的r
′
p
与rc进行对比，计算相似性，对比得到相似性最小时的r
p
对应的候选关键帧，同时计算出r
p
与rc之间的相对变换矩阵r
δ
:
[0122]
rc＝r
δ
*r
p
。
[0123]
8)、校验，将步骤7)得到的候选关键帧与当前输入关键帧进行ndt点云配准，并将r
δ
作为先验变换矩阵；设置得分阈值score
th
，score
th
取值为0.5，若 ndt点云配准得分score
ndt
小于score
th
，则校验成功。
[0124]
9)、位姿变换，将校验成功的候选关键帧对应的关键位姿转化为4
×
4矩阵rk，计算重定位变换矩阵r
final
：
[0125]rfinal
＝r
δ
*rk。
[0126]
10)、将重定位变换矩阵r
final
转化为六维位姿[x,y,z,roll,pitch,yaw]作为重定位位姿输出。
[0127]
本发明实施例还提供一种基于多线激光雷达的机器人重定位装置，包括：至少一个存储器和至少一个处理器；
[0128]
所述至少一个存储器，用于存储机器可读程序；
[0129]
所述至少一个处理器，用于调用所述机器可读程序，执行上述实施例中所述的基于多线激光雷达的机器人重定位方法。
[0130]
通过上面具体实施方式，所述技术领域的技术人员可容易的实现本发明。但是应当理解，本发明并不限于上述的具体实施方式。在公开的实施方式的基础上，所述技术领域的技术人员可任意组合不同的技术特征，从而实现不同的技术方案。
[0131]
除说明书所述的技术特征外，均为本专业技术人员的已知技术。