一种基于激光雷达信息的移动机器人位置重识别方法
技术领域
本发明涉及移动机器人定位领域，特别是涉及一种基于激光雷达信息的移动机器人位置重识别方法。
背景技术
位置重识别技术是移动机器人全局定位中非常重要的部分。全局定位在没有任何先验信息的情况下估计当前机器人的位置信息。常见的做法是将全局定位分为两个部分，首先是位置重识别，即将当前的机器人观测与已行进地点的观测进行匹配，获取机器人可能的候选地点，其次是进行位姿估计，即通过当前观测与候选地点观测进行较为精确的位姿估计，从而计算当前机器人的位姿。
目前已经有许多基于视觉的位置重识别方法，这类方法依赖于相机采集的图像，故对环境的变化不鲁棒，容易受到光照以及时间季节变化的影响。随着近年来激光雷达的普及，考虑到激光对于环境变化的可靠性，基于激光雷达的位置重识别方法也逐渐涌现。这类方法能够达到一定的效果，但是它们大多没有考虑到全局定位的最终目的，即给出机器人的位姿。大多方法仅考虑给出候选地点，但给出候选地点后，根据两帧点云进一步估计位姿时可能会出错。这是因为常用的点云位姿估计的方法对于初值的要求较高。比如在十字路口，车从对向进入路口，此时虽然地点重识别模块可以给出曾经路过这个路口的指示，但是由于两帧点云存在180°的变化，可能导致后续位姿估计错误地认为当前该车是和第一次通过路口一样的朝向，导致定位结果的错误。
公开号为CN111914815A的发明公开了一种垃圾目标的机器视觉智能识别系统，包括里程计模块、目标识别模块、建图模块，该发明还公开了一种垃圾目标的机器视觉智能识别方法，包括以下步骤：计算位姿数据、识别垃圾目标、建立点云地图、建立训练集和测试集、构建改进yolov3模型、训练网络、网络测试，该发明的主要发明目的是利用机器人在自主行走的过程中的实时位姿信息，把图像中识别的垃圾目标的位置通过进行一系列的坐标变换，映射到一个实时的二维地图中，从而实现监控整个社区生活垃圾变化的情况。
公开号为CN111708042A的发明公开了一种行人轨迹预测和跟随的机器人方法及系统，包括：ZED摄像头、GPU嵌入式平台、工控机、MCU控制器、激光雷达传感器和轮式里程计；该发明融合激光雷达和摄像头，将行人轨迹预测网络与行人重识别框架结合，利用行人交互轨迹网络进行预测，实现对目标行人的主动选取最优视角进行跟随。
发明内容
本发明的目的在于提出一种能够兼顾后续位姿估计的基于激光雷达的位置重识别方法，实现对环境变化鲁棒的全局定位。
一种基于激光雷达信息的移动机器人位置重识别方法，包括以下步骤：
(1)激光雷达实时采集移动机器人的激光雷达数据，根据移动机器人的里程计信息，每行进设定的距离对激光雷达数据处理形成多通道激光雷达极坐标鸟瞰图；
(2)将步骤(1)得到的多通道激光雷达极坐标鸟瞰图输入特征提取模块，生成位置描述子；
(3)在地图数据库中检索与当前的位置描述子相似的若干个候选地点，估计当前位置与候选地点的相对朝向；
(4)根据步骤(3)得到的相对朝向调整候选地点的激光点云，使其与当前位置朝向一致，然后进行后续位姿估计；
(5)将位置描述子及其对应的频谱存储到地图数据库中，用于下一次候选地点检索和相对朝向估计。
本发明利用极坐标变换将旋转转换到平移，基于频谱对于平移的不变性以及两张图像进行频谱互相关计算，求解图像的平移的性质。将平移不变性用于产生位置描述子从而进行位置重识别的候选匹配，互相关的计算则可与极坐标变换一起求解相对旋转；考虑时刻变换的环境，采用深度学习网络来进行特征提取。
所述步骤(1)中，激光雷达数据处理的具体步骤如下：
(1-1)将激光雷达数据按高度分为若干层；
(1-2)在各个高度层中分别进行极坐标变换并划分栅格，每个栅格统计点云占用信息；
(1-3)将栅格视为像素值，进而各个高度层分别转换为鸟瞰图，若干层鸟瞰图叠加形成多通道激光雷达极坐标鸟瞰图。
所述步骤(1)利用GPU(图形处理器)的多核并行结构进行计算：在GPU框架下，点云中每个点可单独计算其坐标变换后在极坐标鸟瞰图上对应的坐标，再根据极坐标鸟瞰图上对应的坐标信息可重新组织成为激光雷达极坐标鸟瞰图；与传统的CPU(中央处理器)的串行计算方式相比，利用GPU的多核并行结构进行计算时，点云规模越大，其加速效果越明显。
经步骤(1)的计算，移动机器人在同一地点的旋转在多通道激光雷达极坐标鸟瞰图中表现为横向平移。
所述特征提取模块采用编码-解码器网络构成。
所述步骤(2)中，生成位置描述子的具体步骤如下：
(2-1)通过编码-解码器网络提取多通道激光雷达极坐标鸟瞰图中鲁棒的位置特征，得到与所述多通道激光雷达极坐标鸟瞰图大小相同的特征图，所述特征图用于形成位置描述子和计算相对朝向；
(2-2)将特征图通过快速傅里叶变换转换为频域表示，取频谱的幅度谱作为位置描述子。
由于快速傅里叶变换的特性，特征图中的平移将并不会体现在频谱的幅度谱中，故所述的位置描述子具有旋转不变性。
所述编码-解码器网络为深度学习网络，其模型训练步骤如下：
(2-1-1)制作编码-解码器网络训练使用的数据集，所述数据集包括同一轨迹不同时间段的激光雷达数据及与其相对应的GPS坐标；
(2-1-2)对于一条轨迹中的激光雷达数据按一定距离间隔采样，在其余轨迹中通过距离检索近邻位置，所述近邻位置作为位置重识别的近邻，其余位置作为反例；采样结束后，输出每个采样地点的三元组的激光数据与其绝对朝向，所述三元组为当前位置、近邻和反例；
(2-1-3)将步骤(2-1-2)输出的一个采样地点的三元组输入编码-解码器网络进行单次训练，以学习鲁棒特征；
(2-1-4)将编码-解码器网络学习到的鲁棒特征经快速傅里叶变换转换到频域，将得到当前频谱的幅度谱作为当前位置描述子；将三元组中的近邻和反例的激光数据经快速傅里叶变换转换到频域，分别得到近邻频谱和反例频谱，近邻频谱的幅度谱作为近邻位置描述子，反例频谱的幅度谱作为反例位置描述子；
(2-1-5)近邻位置描述子、反例位置描述子和当前位置描述子计算得到位置重识别损失函数；近邻频谱和当前频谱进行相位互相关计算相对朝向，得到相对朝向损失函数；
(2-1-6)将位置重识别损失函数和相对朝向损失函数结合作为整体网络损失，通过整体网络损失训练编码-解码器网络。
优选地，近邻频谱和当前频谱进行的相位互相关计算采用可微分相位相关算法，其具体方法如下：
改写传统相位互相关算法，向其加入梯度计算部分，使得网络在训练时可反向传导梯度，实现网络参数的更新；对互相关谱的最大值求取部分进行改写，使原不可求导的argmax操作，改写为计算坐标期望的softargmax操作，使得梯度传递正常。
可微分相位相关算法可在网络中进行梯度传导，监督模型学习相关特征。
所述步骤(3)中，当前位置与候选地点的相对朝向的具体估计方法如下：经过快速傅里叶变换后的频谱与候选地点的频谱进行互相关计算后得到相关性谱，相关性谱中最大值所在的横坐标即对应当前位置与候选位置的相对朝向。
所述步骤(4)中，后续位姿估计时，若位姿估计结果收敛则将当前位置与地图中该候选位置进行数据关联，为后续位姿优化校正提供帮助；若位姿估计结果不收敛，则移动机器人当前位置并不在地图中。
本发明相比现有技术，其优点在于：
1.本发明不依赖相机采集的图像，实现了对环境变化鲁棒，不容易受到光照以及时间季节变化的影响。
2.本发明兼顾后续位姿估计，防止根据两帧点云进一步估计位姿时可能产生的错误。
附图说明
图1为本发明实施例基于激光雷达信息的移动机器人位置重识别方法的流程图。
图2为本发明实施例原始激光点云转化为多通道图像的示意图。
图3为本发明实施例旋转不变的位置描述子的照片。
图4为图1所示可微分相位相关法求解的流程图。
图5为本发明实施例相关性谱以及其与相对朝向的关系的照片。
具体实施方式
如图1所示，基于激光雷达信息的移动机器人位置重识别方法，包括以下步骤：
(1)移动机器人在室外场景中移动，激光雷达实时采集其激光雷达数据，根据移动机器人的里程计信息，设定每行进3m就对当前得到的激光雷达数据进行处理，形成多通道激光雷达极坐标鸟瞰图，如图2所示，其对激光雷达数据进行处理的具体步骤如下：
(1-1)将激光雷达数据按高度分为若干层；
(1-2)在各个高度层中分别进行极坐标变换，按角度和半径划分扇形划分栅格，每个栅格统计点云占用信息；
(1-3)将栅格视为像素值，进而各个高度层分别转换为鸟瞰图，其中，鸟瞰图的宽代表栅格对应的角度，角度值为0～2π，鸟瞰图的高代表代表栅格对应半径，由上到下对应半径为0～最大半径值，鸟瞰图的每个通道则代表一个高度层，若干层鸟瞰图叠加形成多通道激光雷达极坐标鸟瞰图。
利用GPU的多核并行结构进行计算：在GPU框架下，点云中每个点可单独计算其坐标变换后在极坐标鸟瞰图上对应的坐标，再根据极坐标鸟瞰图上对应的坐标信息可重新组织成为激光雷达极坐标鸟瞰图；与传统的CPU的串行计算方式相比，利用GPU的多核并行结构进行计算时，点云规模越大，其加速效果越明显。
经步骤(1)的计算，移动机器人在同一地点的旋转在多通道激光雷达极坐标鸟瞰图中表现为横向平移。
(2)将步骤(1)得到的多通道激光雷达极坐标鸟瞰图输入编码-解码器网络，生成位置描述子，其具体步骤如下：
(2-1)通过编码-解码器网络提取多通道激光雷达极坐标鸟瞰图中鲁棒的位置特征，得到与多通道激光雷达极坐标鸟瞰图大小相同的特征图，特征图用于形成位置描述子和计算相对朝向；
(2-2)将特征图通过快速傅里叶变换转换为频域表示，取频谱的幅度谱作为位置描述子，如图3所示。
由于快速傅里叶变换的特性，特征图中的平移将并不会体现在频谱的幅度谱中，故位置描述子具有旋转不变性。
编码-解码器网络为深度学习网络，其模型训练步骤如下：
(2-1-1)制作编码-解码器网络训练使用的数据集，数据集包括同一轨迹不同时间段的激光雷达数据及与其相对应的GPS坐标；
(2-1-2)对于一条轨迹中的激光雷达数据按一定距离间隔采样，在其余轨迹中通过距离检索近邻位置，作为位置重识别的近邻，其余位置作为反例；采样结束后，输出每个采样地点的三元组的激光数据与其绝对朝向，所述三元组为当前位置、近邻和反例；
(2-1-3)将步骤(2-1-2)输出的一个采样地点的三元组输入编码-解码器网络进行单次训练，以学习鲁棒特征；
(2-1-4)将编码-解码器网络学习到的鲁棒特征经快速傅里叶变换转换到频域，将得到当前频谱的幅度谱作为当前位置描述子；将三元组中的近邻和反例的激光数据经快速傅里叶变换转换到频域，分别得到近邻频谱和反例频谱，近邻频谱的幅度谱作为近邻位置描述子，反例频谱的幅度谱作为反例位置描述子；
(2-1-5)近邻位置描述子、反例位置描述子和当前位置描述子计算得到位置重识别损失函数；近邻频谱和当前频谱进行相位互相关计算相对朝向，可微分相位相关算法，得到相对朝向损失函数，如图4所示。
可微分相位相关算法其改写传统相位互相关算法，向其加入梯度计算部分，使得网络在训练时可反向传导梯度，实现网络参数的更新；对互相关谱的最大值求取部分进行改写，使原不可求导的argmax操作，改写为计算坐标期望的softargmax操作，使得梯度传递正常。
由于常用的相位互相关算法无法直接应用于网络学习，采用可微分相位相关算法可在网络中进行梯度传导，监督模型学习相关特征。
(2-1-6)将位置重识别损失函数和相对朝向损失函数结合作为整体网络损失，通过整体网络损失训练编码-解码器网络。
(3)在地图数据库中检索与当前的位置描述子相似的若干个候选地点，并估计当前位置与候选地点的相对朝向；
(4)根据步骤(3)得到的相对朝向调整候选地点的激光点云，使其与当前位置朝向一致，然后进行后续位姿估计；
(5)将位置描述子及其对应的频谱存储到地图数据库中，用于下一次候选地点检索和相对朝向估计。
(3)在地图数据库中检索与当前位置描述子相似的若干个候选地点，估计当前位置与候选地点的相对朝向；当前位置与候选地点的相对朝向的具体估计方法如下：经过快速傅里叶变换后的频谱与候选地点的频谱进行互相关计算后得到相关性谱，相关性谱中最大值所在的横坐标即对应当前位置与候选位置的相对朝向，如图5所示。
(4)根据步骤(3)得到的相对朝向调整候选地点的激光点云，使其与当前位置朝向一致，然后进行后续位姿估计；后续位姿估计时，若位姿估计结果收敛则将当前位置与地图中该候选位置进行数据关联，为后续位姿优化校正提供帮助；若位姿估计结果不收敛，则移动机器人当前位置并不在地图中。
(5)无论移动机器人当前位置是否在地图中，均将位置描述子及其对应的频谱存储到地图数据库中，用于下一次候选地点检索和相对朝向估计。
