1.本发明涉及移动机器人定位领域，特别是涉及一种基于激光雷达可估位姿的移动机器人位置重识别方法。
背景技术：2.位置重识别技术是移动机器人全局定位中非常重要的部分，其能保障机器人所构建的地图是全局一致的。全局定位在没有任何先验信息的情况下估计当前机器人的位置信息。常见的做法是将全局定位分为两个部分，首先是位置重识别，即将当前的机器人观测与已行进地点的观测进行匹配，获取机器人可能的候选地点，其次是进行位姿估计，即通过当前观测与候选地点观测进行较为精确的位姿估计，从而计算当前机器人的位姿。
3.近年来激光雷达硬件的不断迭代推动了基于激光的地点位置重识别方法的发展。这类方法不依赖于相机采集的图像，故对环境的变化鲁棒，不容易受到光照以及时间季节变化的影响。这类方法目前虽然能够在部分限制场景下提供比较好的结果，但是它们大多没有考虑到全局定位的最终目的，即给出机器人的位姿。大多方法仅考虑给出候选地点，但给出候选地点后，根据两帧点云进一步估计位姿时可能会出错。这是因为常用的点云位姿估计的方法对于初值的要求较高。在复杂情况下虽然位置重识别模块给出了地点，但由于初值较差可能导致后续位姿估计错误，导致定位结果的错误。
技术实现要素：4.本发明的目的在于解决现有技术中存在的问题，并提出一种能够兼顾位姿估计的基于激光雷达可估位姿的移动机器人位置重识别方法，以期实现对环境变化鲁棒的全局定位。
5.为实现上述发明目的，本发明具体采用的技术方案如下：
6.一种基于激光雷达可估位姿的移动机器人位置重识别方法，其包括以下步骤：
7.s1、由激光雷达实时采集移动机器人的激光雷达数据，每行进预设距离后对该段行进距离内的激光雷达数据进行处理，形成当前位置点的多通道特征鸟瞰图；
8.s2、将所述多通道特征鸟瞰图逐通道进行拉东变换，生成关联至当前位置点的多通道正弦图，且每个通道的正弦图中，纵轴变化代表激光的旋转，横轴变化代表不同旋转角度上点云的平移；对所述多通道正弦图中每一通道的横轴进行一维傅里叶变换，将变换得到各通道的频谱的幅值谱作为当前位置点的地点描述子；
9.s3、遍历地图数据库中每一个已关联多通道正弦图的已知位置点，针对当前位置点的地点描述子与每个已知位置点的地点描述子中每一个通道的幅值谱进行互相关操作，在地图数据库中检索与当前位置点的地点描述子相关性最大的若干个候选位置点，并估计得到当前位置点与每个候选位置点之间的平移和旋转；
10.s4、针对每个候选位置点，将当前位置点与该候选位置点之间的平移和旋转应用于调整该候选位置点的激光点云，使该候选位置点的激光点云对齐至当前位置点的激光点
云，得到该候选位置点的修正激光点云；然后基于每个候选位置点的修正激光点云进行位姿估计和优化。
11.本发明相比现有技术，其优点在于：
12.1.本发明的移动机器人位置重识别方法，不依赖相机采集的图像，实现了对环境变化鲁棒，不容易受到光照以及时间季节变化的影响。
13.2.本发明可以在不同采样稀疏情况下达到一致的效果，可以减少地图存储
14.3.本发明可以同时估计位姿，大大提高了全局定位的准确性。
15.4.本发明不依赖深度学习，可以不改变配置的情况下用于不同场景。
附图说明
16.图1为基于激光雷达信息的可估位姿的移动机器人位置重识别方法的流程图。
17.图2为本发明实施例中获得幅值谱的流程图。
18.图3为本发明实施例特征鸟瞰图转化为正弦图的示意图。
19.图4为本发明实施例特征正弦图平移和旋转特性的示意图。
20.图5为本发明实施例正弦图转化为幅值谱的示意图。
具体实施方式
21.为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图对本发明的具体实施方式做详细的说明。在下面的描述中阐述了很多具体细节以便于充分理解本发明。但是本发明能够以很多不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似改进，因此本发明不受下面公开的具体实施例的限制。本发明各个实施例中的技术特征在没有相互冲突的前提下，均可进行相应组合。
22.在本发明的一个较佳实施例中，提供了一种基于激光雷达可估位姿的移动机器人位置重识别方法，该方法包括以下步骤：
23.s1、由激光雷达实时采集移动机器人的激光雷达数据，每行进预设距离后对该段行进距离内的激光雷达数据进行处理，形成当前位置点的多通道特征鸟瞰图。
24.需要说明的是，移动机器人上通过激光雷达采集激光雷达数据属于现有技术，对此不再赘述。移动机器人在行进过程中，可根据里程计信息判断自身的行进距离，每当行进一定距离后即可将这一段行进距离内的激光雷达数据处理成当前位置点的多通道特征鸟瞰图。具体设定的距离值可以根据实际进行调整，一般应《100m。作为本发明实施例的一种较佳方式，可设置根据移动机器人的里程计信息，每行进20m的距离对激光雷达数据处理形成多通道特征鸟瞰图。
25.作为本发明实施例的一种较佳方式，在上述步骤s1中，对激光雷达数据进行处理形成当前位置点的多通道特征鸟瞰图的具体子步骤如下：
26.s11、遍历激光雷达数据中的每一个点，计算以当前遍历点为中心的局部点云的若干个特征值，形成当前遍历点的多通道特征，并对各特征进行归一化。
27.需要说明的是，激光雷达数据中每一个点的多通道特征是通过对其局部点云进行特征值计算得到的，每一个特征值作为一个通道特征。多通道特征中具体选用的特征值类型可以根据实际进行选择，可包括迹、线性性、平面性、球度性、曲率变化、特征熵、各项异性
以及全方差中的部分或全部特征。优选的，可采用这8个特征中的全部。另外，一个点的局部点云中，具体点数量k可根据实际进行调整，优选采用离该点最近的k＝20～40个点构建局部点云，进一步优选为k＝30个点。
28.由于激光雷达数据的点云规模通常较大，在cpu上计算耗时很长，因此作为本发明实施例的一种较佳方式，可采用gpu并行地对每个激光点云进行特征计算，可以提高计算效率，达到实时性。
29.s12、根据激光雷达数据中每一个点的平面坐标，将对应的多通道特征存储到鸟瞰视角栅格的对应栅格点中，若有多个点投影到相同栅格点中，则仅存储每个通道特征中的最大值。
30.需要说明的是，鸟瞰视角栅格是一个鸟瞰状态下的平面栅格，栅格中的每一个网格视为一个栅格点，地图中的每一个xy平面坐标都可以对应至一个栅格点中。每一个栅格点中均存储有一组多通道特征，因此当多个点的xy坐标相同时会存储至同一个栅格点中，此时对于每一个通道特征需要取所有点中该通道特征的最大值，然后在栅格点中存储该最大值，其余点的通道特征直接舍弃。
31.s13、将鸟瞰视角栅格按通道划分，每个通道图像中的像素值为对应的通道特征值，从而形成多通道特征鸟瞰图。
32.经步骤s1的计算，移动机器人在同一地点的旋转变化在多通道特征鸟瞰图中表现为纵向平移变化。在同一地点附近的平移则表现为横向平移。
33.s2、将所述多通道特征鸟瞰图逐通道进行拉东变换(randon变换)，生成关联至当前位置点的多通道正弦图，且每个通道的正弦图中，纵轴变化反映了激光的旋转，横轴变化则表示了不同旋转角度上点云的平移；对所述多通道正弦图中每一通道的横轴进行一维傅里叶变换，将变换得到各通道的频谱的幅值谱作为当前位置点的地点描述子。
34.因此作为本发明实施例的一种较佳方式，上述步骤s2的具体实现子步骤如下：
35.s21、将当前位置点的多通道特征鸟瞰图的每一通道进行拉东变换，生成当前位置点的多通道正弦图；
36.s22、将多通道正弦图的每一通道图像通过横轴的一维快速傅里叶变换转换为频域表示，取变换得到的频谱的幅度谱作为当前位置点的地点描述子。
37.需说明的是，幅度谱即幅值谱。在一示例性的实施例中，如图2所示，先按照s1得到通道特征鸟瞰图，再将多通道特征鸟瞰图逐层进行拉东变换，生成的多通道正弦图表示中的其中一个通道的正弦图如图3所示。对于每个通道的正弦图，纵轴变化反映了激光的旋转，横轴则表示了不同旋转角度上点云的平移，该性质如图4所示。得到多通道正弦图后，对其每一通道的横轴进行一维傅里叶变换，将得到的频谱的幅值谱作为地点描述子，幅值谱如图5所示；
38.由于快速傅里叶变换的特性，多通道正弦图中的横向平移将并不会体现在频谱的幅度谱中，故得到的地点描述子具有平移不变性，即在同一地点的附近的描述子是一致的，可以基于该特征对地点之间的平移和旋转进行估计。
39.s3、遍历地图数据库中每一个已关联多通道正弦图的已知位置点，针对当前位置点的地点描述子与每个已知位置点的地点描述子中每一个通道的幅值谱进行互相关操作，在地图数据库中检索与当前位置点的地点描述子相关性最大的若干个候选位置点，并估计
得到当前位置点与每个候选位置点之间的平移和旋转。
40.作为本发明实施例的一种较佳方式，上述步骤s3的具体实现子步骤如下：
41.s31、针对地图数据库中每一个已知位置点关联的多通道正弦图，对其中每一通道的横轴进行一维快速傅里叶变换，将其转换为频域表示，得到各通道的频谱的幅值谱，作为已知位置点的地点描述子；然后对当前位置点的幅值谱和每个已知位置点的幅值谱逐通道进行互相关计算，将多通道的相关性谱相加得到最终的单通道相关性谱；当前位置点与每一个已知位置点都得到一个单通道相关性谱后，找到每个单通道相关性谱中的相关性最大值，选择相关性最大值最大的若干已知位置点作为候选地点，每个候选地点的单通道相关性谱中相关性最大值所在纵坐标作为该候选地点与当前位置点之间的相对旋转。
42.需要说明的是，相关性谱中，横坐标代表傅里叶变换中的采样频率，纵坐标代表相对旋转，相关性谱中的每个点的值代表了一个采样频率下相对旋转的相关性。
43.需要说明的是，地图数据库是一个用于存储移动机器人所需的数字地图的数据库，在构建该地图数据库时，移动机器人可在地图区域内探索，不同位置点处均可按照前述s1和s2所描述的流程计算并关联该位置点对应的多通道正弦图。地图数据库是逐步构建的，随着移动机器人在区域内不断通过激光扫描进行探索，整个区域内的位置点会不断关联自身的多通道正弦图。本发明中将已经过激光扫描且关联了多通道正弦图的位置点称为已知位置点。
44.需要说明的是，每次选择的候选地点的数量k需要根据实际进行调整，地图数据库中的所有已知位置点都可以得到一个单通道相关性谱中的相关性最大值，对这些相关性最大值进行降序排列，选择前k个相关性最大值所对应的已知位置点即可作为k个候选地点。
45.s32、对于每个候选地点，将该候选地点与当前位置点之间的相对旋转补偿到该候选地点对应的多通道正弦图上，使当前位置点和候选地点的多通道正弦图之间只有平移变换而不存在旋转变换，即只存在各通道正弦图上横向平移的变化。
46.s33、针对每个候选地点，对经过相对旋转补偿的多通道正弦图中像素行进行一维互相关操作，计算两行像素之间由横纵平移量耦合而成的耦合平移量，将多通道正弦图上的每一行根据拉东变换的原理组成一个二元一次方程，且二元一次方程中的两个自变量为当前位置点与每个候选位置点之间的横向平移量和纵向平移量，因变量是耦合平移量。
47.需注意的是，多通道正弦图的耦合平移量可以通过一维互相关操作求得，其由真实位置的横纵平移量耦合而成。正弦图上的每一行可以组成一个包含真实位置的横纵平移的二元一次方程，该二元一次方程的具体形式可根据拉东变换的原理确定。在本发明实施例中，上述二元一次方程的形式可以表示如下：
48.xcos(θi)+ysin(θi)＝τi49.式中：θi表示第i行相似像素在拉东变换中对应的旋转角；x,y分别为待求的当前位置点与每个候选位置点之间的横向平移量和纵向平移量，τi为第i行由一维互相关操作求得的两行像素之间的耦合平移量。
50.由于多通道正弦图的每一行都可以建立一个二元一次方程，因此整个多通道正弦图的所有二元一次方程组成一个超定方程，通过奇异值分解(svd)方法求解该超定方程，即可求得当前位置点与每个候选位置点之间点云的横向平移量和纵向平移量。
51.s4、针对每个候选位置点，将当前位置点与该候选位置点之间的平移和旋转应用
于调整该候选位置点的激光点云，使该候选位置点的激光点云对齐至当前位置点的激光点云，得到该候选位置点的修正激光点云；然后基于每个候选位置点的修正激光点云进行更加精细的位姿估计和优化。
52.当获得该候选位置点的修正激光点云后，可根据现有技术中任意可行的位姿估计和优化方法对移动机器人的当前位置进行估计和优化。
53.作为本发明实施例的一种较佳方式，上述步骤s4中，基于每个候选位置点的修正激光点云进行位姿估计和优化时，先基于每个候选位置点的修正激光点云利用迭代最近点算法(iterative closest point,icp)进行精细位姿估计，并判断是否收敛，如果收敛则将估计结果用于进行后续的位姿优化。在一实施例中，此处的位姿优化所采用的方法可采用g2o图优化算法。
54.需要说明的是，在进行精细位姿估计时，若位姿估计结果收敛则将当前位置与地图中该候选位置进行数据关联，为后续位姿优化提供帮助；但若位姿估计结果不收敛，则表明移动机器人当前位置并不在地图中，可先跳过当前位置点的位姿估计和优化，但该位置点的多通道正弦图依然可关联存储于地图数据库中。
55.作为本发明实施例的一种较佳方式，在移动机器人行进过程中，可以在完成上述s1～s4步骤后，实时将每一个位置点对应生成的多通道正弦图存储到地图数据库中，用于下一次候选地点检索和相对位姿估计和优化。由此，地图数据库中的已知位置点会逐渐增加，候选地点检索和相对位姿估计优化性能会逐步提升。
56.综上，本发明利用拉东变换将旋转和平移变化转换到正弦图的两个轴的平移变化，基于频谱的幅度谱对于平移的不变性以及两张图像进行频谱互相关计算，求解图像的平移的性质。将平移不变性用于产生位置描述子从而进行位置重识别的候选匹配，互相关的计算则可与拉东变换一起求解相对旋转和平移；考虑时刻变换的环境，采用多通道特征鸟瞰图的表示以提高对激光点云中局部特征表征能力。
57.以上所述的实施例只是本发明的一种较佳的方案，然其并非用以限制本发明。有关技术领域的普通技术人员，在不脱离本发明的精神和范围的情况下，还可以做出各种变化和变型。因此凡采取等同替换或等效变换的方式所获得的技术方案，均落在本发明的保护范围内。
技术特征：1.一种基于激光雷达可估位姿的移动机器人位置重识别方法，其特征在于，包括以下步骤：s1、由激光雷达实时采集移动机器人的激光雷达数据，每行进预设距离后对该段行进距离内的激光雷达数据进行处理，形成当前位置点的多通道特征鸟瞰图；s2、将所述多通道特征鸟瞰图逐通道进行拉东变换，生成关联至当前位置点的多通道正弦图，且每个通道的正弦图中，纵轴变化代表激光的旋转，横轴变化代表不同旋转角度上点云的平移；对所述多通道正弦图中每一通道的横轴进行一维傅里叶变换，将变换得到各通道的频谱的幅值谱作为当前位置点的地点描述子；s3、遍历地图数据库中每一个已关联多通道正弦图的已知位置点，针对当前位置点的地点描述子与每个已知位置点的地点描述子中每一个通道的幅值谱进行互相关操作，在地图数据库中检索与当前位置点的地点描述子相关性最大的若干个候选位置点，并估计得到当前位置点与每个候选位置点之间的平移和旋转；s4、针对每个候选位置点，将当前位置点与该候选位置点之间的平移和旋转应用于调整该候选位置点的激光点云，使该候选位置点的激光点云对齐至当前位置点的激光点云，得到该候选位置点的修正激光点云；然后基于每个候选位置点的修正激光点云进行位姿估计和优化。2.如权利要求1所述的基于激光雷达可估位姿的移动机器人位置重识别方法，其特征在于，所述移动机器人根据里程计信息判断自身的行进距离。3.如权利要求1所述的基于激光雷达可估位姿的移动机器人位置重识别方法，其特征在于，所述步骤s1中，对激光雷达数据进行处理形成当前位置点的多通道特征鸟瞰图的具体步骤如下：s11、遍历激光雷达数据中的每一个点，计算以当前遍历点为中心的局部点云的若干个特征值，形成当前遍历点的多通道特征，并对各特征进行归一化；s12、根据激光雷达数据中每一个点的平面坐标，将对应的多通道特征存储到鸟瞰视角栅格的对应栅格点中，若有多个点投影到相同栅格点中，则仅存储每个通道特征中的最大值；s13、将鸟瞰视角栅格按通道划分，每个通道图像中的像素值为对应的通道特征值，从而形成多通道特征鸟瞰图。4.如权利要求1所述的基于激光雷达可估位姿的移动机器人位置重识别方法，其特征在于，所述多通道特征包括迹、线性性、平面性、球度性、曲率变化、特征熵、各项异性以及全方差中的部分或全部特征。5.如权利要求1所述的基于激光雷达可估位姿的移动机器人位置重识别方法，其特征在于，所述步骤s2的具体实现步骤如下：s21、将当前位置点的多通道特征鸟瞰图的每一通道进行拉东变换，生成当前位置点的多通道正弦图；s22、将多通道正弦图的每一通道图像通过横轴的一维快速傅里叶变换转换为频域表示，取变换得到的频谱的幅度谱作为当前位置点的地点描述子。6.如权利要求1所述的基于激光雷达可估位姿的移动机器人位置重识别方法，其特征在于，所述步骤s3的具体实现步骤如下：
s31、针对地图数据库中每一个已知位置点关联的多通道正弦图，对其中每一通道的横轴进行一维快速傅里叶变换，将其转换为频域表示，得到各通道的频谱的幅值谱，作为已知位置点的地点描述子；然后对当前位置点的幅值谱和每个已知位置点的幅值谱逐通道进行互相关计算，将多通道的相关性谱相加得到最终的单通道相关性谱；当前位置点与每一个已知位置点都得到一个单通道相关性谱后，找到每个单通道相关性谱中的相关性最大值，选择相关性最大值最大的若干已知位置点作为候选地点，每个候选地点的单通道相关性谱中相关性最大值所在纵坐标作为该候选地点与当前位置点之间的相对旋转；s32、对于每个候选地点，将该候选地点与当前位置点之间的相对旋转补偿到该候选地点对应的多通道正弦图上，使当前位置点和候选地点的多通道正弦图之间只有平移变换而不存在旋转变换；s33、针对每个候选地点，对经过相对旋转补偿的多通道正弦图中像素行进行一维互相关操作，计算两行像素之间由横纵平移量耦合而成的耦合平移量，将多通道正弦图上的每一行根据拉东变换的原理组成一个二元一次方程，且二元一次方程中的两个自变量为当前位置点与每个候选位置点之间的横向平移量和纵向平移量，因变量是耦合平移量；整个多通道正弦图的所有二元一次方程组成一个超定方程，通过奇异值分解方法求得当前位置点与每个候选位置点之间点云的横向平移量和纵向平移量。7.如权利要求1所述的基于激光雷达可估位姿的移动机器人位置重识别方法，其特征在于，所述二元一次方程的形式为：xcos(θ
i
)+ysin(θ
i
)＝τ
i
式中：θ
i
表示第i行相似像素在拉东变换中对应的旋转角；x,y分别为待求的当前位置点与每个候选位置点之间的横向平移量和纵向平移量，τ
i
为第i行由一维互相关操作求得的两行像素之间的耦合平移量。8.如权利要求1所述的基于激光雷达可估位姿的移动机器人位置重识别方法，其特征在于，所述步骤s4中，基于每个候选位置点的修正激光点云进行位姿估计和优化时，先基于每个候选位置点的修正激光点云利用迭代最近点算法进行位姿估计，并判断是否收敛，如果收敛则将估计结果用于进行后续的位姿优化。9.如权利要求8所述的基于激光雷达可估位姿的移动机器人位置重识别方法，其特征在于，所述位姿优化所采用的方法为g2o图优化。10.如权利要求1所述的基于激光雷达可估位姿的移动机器人位置重识别方法，其特征在于，在移动机器人行进过程中，实时将每一个位置点对应生成的多通道正弦图存储到地图数据库中，用于下一次候选地点检索和相对位姿估计。
技术总结本发明公开了一种基于激光雷达可估位姿的移动机器人位置重识别方法。本发明利用拉东变换将旋转和平移变化转换到正弦图的两个轴的平移变化，基于频谱的幅度谱对于平移的不变性以及两张图像进行频谱互相关计算，求解图像的平移的性质；将平移不变性用于产生位置描述子从而进行位置重识别的候选匹配，互相关的计算则可与拉东变换一起求解相对旋转和平移。本发明考虑时刻变换的环境，采用多通道特征鸟瞰图表示，可以提高对激光点云中局部特征表征能力。力。力。
技术研发人员：王越 许学成 芦莎 熊蓉
受保护的技术使用者：浙江大学
技术研发日：2022.07.13
技术公布日：2022/11/1
