CN114862953A - 一种基于视觉特征和3d激光的移动机器人重定位方法及装置 - Google Patents

Abstract

本发明公开了一种基于视觉特征和3D激光的移动机器人重定位及装置的方法，该方法首先需要进行环境建图和Apriltag标签世界坐标自动标定；在移动机器人丢失定位时，开启左、右相机检测标签功能，在检测到标签后停止移动；获取移动机器人与标签之间的相对位姿，并在一段时间内做滤波处理，保证位姿数据的稳定性；根据标定所得标签世界坐标恢复机器人所处世界坐标，实现初步重定位；以该世界坐标为中心进行定向范围搜索，不断将扫描得到的3D点云帧数据与现存子图进行匹配，实现精准重定位。从而恢复移动机器人利用多线激光雷达SLAM定位能力，保证移动机器人在实际运行过程能够以较小的代价恢复定位能力。

Description

一种基于视觉特征和3D激光的移动机器人重定位方法及装置

技术领域

本发明属于移动机器人重定位技术领域，具体是涉及一种基于视觉特征和3D激光的移动机器人重定位方法及装置。

背景技术

随着现代物流仓储行业的快速发展，利用移动机器人替代人力搬运的改革正逐步开展中，越来越多的智能仓储机器人正踏入物流行业的仓库开始承担自主搬运的任务，进一步推动物流仓库的无人化和智能化。智能仓储机器人在实际场景中执行搬运任务的核心问题便是如何根据自身所载传感器和所处环境信息进行定位，并按照定位信息对任务进行分配、规划与导航。定位信息是仓储机器人移动的基础，近些年来SLAM技术的发展使得仓储机器人的自主定位成为可能，但是仓储机器人在实际运行过程中往往会碰到丢失定位问题，具体表现为：内部或外部异常原因导致仓储机器人被迫重新启动、动态环境使得仓储机器人估计位姿信息发生错误等。同时，采用基于多线激光的3D SLAM方案代替基于单线激光的2D SLAM方案能够更大程度地利用环境信息，提升仓储机器人的重定位能力，结合视觉特征信息，能够在仅靠环境信息的基础上进一步提升重定位精度。因此，研究一种基于视觉特征的3D激光SLAM重定位方法能够更好的解决机器人重定位问题。

发明内容

本发明的目的在于提高仓储机器人重定位能力，在引入多线激光信息克服2D平面局限性的基础之上，进一步结合视觉特征信息，提高仓储机器人的重定位精度，从而本申请提出一种基于视觉特征和3D激光的移动机器人重定位方法及装置，该方法涉及单目相机、多线激光雷达和IMU传感器，单目相机用于检测Apriltag标签信息，多线激光雷达用于采集环境的点云数据，IMU用于重力方向的感知以及提供速度、角速度信息。通过对现有环境进行标定，在发生定位丢失后需要进行重定位时，检测Apriltag标签信息进行初步重定位，并在此基础上进行位姿的适当变换，并结合当前扫描帧点云数据、IMU信息与现有子图信息进行匹配，从而抉择最优变换，实现精准定位。

为达到上述目的，本发明的技术方案如下：第一方面，本发明提供了一种基于视觉特征和3D激光的移动机器人重定位方法，包括如下步骤：

S1：利用多线激光雷达和IMU传感器对实际场景进行3D层面的建图，在地图构建的过程中，获取移动机器人相对于Apriltag标签的位姿，根据移动机器人世界坐标转换得到Apriltag标签世界坐标信息，同时记录标签编号，完成对Apriltag标签的世界坐标的标定；

S2：在移动机器人丢失自身定位时，通过位于移动机器人上的左右相机不断获取移动机器人左侧与右侧图像，在任一相机的视野范围内检测到Apriltag标签后即停止移动；

S3：获取移动机器人相对Apriltag标签的若干个位姿数据，记为集合S：

其中x_i，y_i，z_i分别表示X，Y，Z轴方向上相对Apriltag标签的位移变换，

φ_i分别表示X，Y，Z轴方向上相对Apriltag标签的角度变换，n表示获取的位姿总数。

S4：按照获取的Apriltag标签编号查找S1中标定得到的标签世界坐标，记为

基于S3中得到的相对Apriltag标签的位姿数据，进行移动机器人的世界坐标推算，实现移动机器人的初步重定位，记移动机器人初步重定位结果为

S5：将W₂的坐标结合多线激光点云信息进一步推算，具体为：首先对移动机器人上3D激光的当前帧点云数据进行体素滤波和噪声去除，将当前移动机器人所处空间切分成固定边长的立方格，形成栅格空间，依据3D激光扫描得到的点云数据中的点是否在立方格内将立方格的中心值设为1和0，立方格内其余点的值则通过相邻立方格的中心值来计算，值的大小表示障碍物的概率；然后以坐标W₂为中心，以参数r_s为半径并结合S4中W₂相对于W₁的角度信息，进行定向范围搜索。对当前帧点云数据按照相等间隔的位移和角度进行3D层面的变换，并依据变换后的点在栅格空间中的坐标获取概率值，计算概率值累加和，以累加和为最大值时对应的间隔变换后的位姿数据作为精准重定位的结果，即

进一步地，步骤S1具体包括如下步骤：

S11：将移动机器人开始建图的点作为世界坐标系的原点，在移动过程中，将移动机器人上的3D激光扫描得到的点云数据与之前扫描过程中已获得点云数据进行匹配，采用子图集策略，以N帧点云数据建立子图，并以每N帧移动机器人位姿构造节点，在移动机器人不断移动的过程中，不断计算节点间的变换过程约束以及当前帧与相邻节点间的约束，将约束作为节点间的边，并进行非线性优化，将其转化为图优化问题，构建优化函数如下：

上式中，

分别表示当前扫描帧具有约束关系的子图集合和已获取的扫描帧的位姿集合，n₁，n₂分别表示与当前扫描帧具有约束关系的子图和在当前扫描帧之前扫描帧的数量，Δ_ij，Ω_ij分别表示扫描帧j相对子图i的相对位姿和协方差矩阵，E(·)则表示关于

Ω_ij，Δ_ij的非线性误差函数，采用Levenberg-Marquardt算法求解最优子图位姿p^m和最优扫描帧位姿p^s，结合移动机器人初始位姿转换为当前时刻下的移动机器人世界坐标；

S12：在构建地图的过程中，左右相机不断获取图像进行检测，当检测到Apriltag标签时，用0和1区分左右相机记录的数据，记录下此刻检测到的标签号和根据移动机器人世界坐标转换得到Apriltag标签的世界坐标并保存以便后续使用。

进一步地，步骤S4具体包括如下步骤：

S41：按照实际移动机器人移动过程中左右相机检测到标签与否分类，情形可分为两类，一类是单侧，另一类是双侧；单侧是指只有左相机或者右相机检测到标签，则进行步骤S42，双侧则是指左右相机均检测到标签，则进行步骤S43。

S42：在单侧情况下，当相机范围内检测到多个标签时，记检测到的标签编号为d_i，i＝1，…，m，其中m表示检测标签数量；根据标签编号查找对应标定所得标签世界坐标为

取此时检测所得移动机器人相对标签的位姿数据中的相对位移变换信息

角度信息

和

得到移动机器人初步的世界坐标为

角度

S43：在双侧情况下，记左侧检测所得标签编号为l_i，i＝1，…，m_l，其中m_l表示左侧检测标签数量，右侧为r_j，j＝1，…，m_r，其中m_r表示右侧检测标签数量，根据标签编号查找对应标签世界坐标为

取此时检测所得移动机器人相对标签的位姿数据中的相对位移变换信息

角度信息

和

得到移动机器人初步的世界坐标为

角度

进一步地，步骤S5具体包括如下步骤：

S51：以S4得到的移动机器人世界坐标

为球心，参数r_s为半径，角度

和

建立球锥，参数r_s可根据实际环境进行自主设置；在球锥范围内按照设定的若干个等间隔

对3D激光当前帧扫描得到的点云数据进行3D空间上的变换，记变换前的点云数据为C，变换后的点云数据为C′；

S52：利用栅格化思想，将当前移动机器人所处空间切分成固定边长的立方格，立方格中心的值表示该立方格为障碍物的概率；概率值越高则该立方格越黑，代表该位置越可能为障碍物，概率越低则表示该位置越可能为空，未知区域处的立方格设置为灰色，即概率值的0.5。

S53：根据设定的不同间隔ΔT对当前扫描点云数据C进行变换，并依据变换后的点在栅格空间中的坐标获取概率值，对当前点云数据点的概率值进行累加，将概率值的累加和最大的ΔT定义为最优变换ΔT′，概率和公式如下：

根据最大概率和公式进行转换，建立最小二乘化模型进行非线性优化，并引入损失函数，从而得到最优变换ΔT′。

其中，K表示t时刻扫描得到的点云数量，ρ_k表示损失函数

用于排除异常值对优化结果的影响，s表示立方格的概率值，Q表示计算概率函数，h_k表示t时刻下扫描所得点云中的第k个点在移动机器人坐标系下的坐标，ΔT表示将第k个点在移动机器人坐标系下的坐标变换为世界坐标系下坐标的变换矩阵；

S54：基于最优变换

将

变换成

作为最终精准定位的坐标。

第二方面，本发明还提供了一种基于视觉特征和3D激光的移动机器人重定位装置，包括存储器和一个或多个处理器，所述存储器中存储有可执行代码，所述处理器执行所述可执行代码时，实现所述的基于视觉特征和3D激光的移动机器人重定位方法。

第三方面，本发明还提供了一种计算机可读存储介质，其上存储有程序，所述程序被处理器执行时，实现所述的基于视觉特征和3D激光的移动机器人重定位方法。

本发明的有益效果：本发明能够有效解决移动机器人因异常而发生的定位丢失问题，通过感知视觉特征进行粗略定位，在此基础上结合3D激光信息进一步提升重定位精度，实现精准定位，使移动机器人能够在短时间内恢复对周围环境的感知能力和定位能力，从而让移动机器人能够继续完成生产任务。

附图说明

图1为本发明方法实现流程图。

图2为视觉Apriltag标签图。

图3为机器人及传感器相对位置示意图。

图4为点云变换范围示意图。

图5为栅格选取示意图。

图6为本发明一种基于视觉特征和3D激光的移动机器人重定位装置的结构图。

具体实施方式

下面结合附图说明进一步详细描述本方法的执行步骤，但本发明的保护范围不局限于以下所述。

如图1所示，本发明提出一种基于视觉特征和3D激光的移动机器人重定位方法，包括以下步骤：

本发明实施例所使用的多线激光雷达为QUANERGY M8系列16线激光雷达，左右相机采用经过标定参数后的USB相机，IMU为轮趣科技IMU，传感器的相对位置示意图如图3所示。

S1：利用多线激光雷达和IMU对实际场景进行3D层面的建图，采用实时相关性扫描匹配、非线性后端优化、分支定界加速回环检测进行3D SLAM系统的地图构建，获取移动机器人相对于Apriltag标签的位姿，根据移动机器人世界坐标转换得到Apriltag标签世界坐标信息，同时记录标签编号，完成对Apriltag标签的世界坐标的标定，Apriltag标签图如图2所示。

S2：在移动机器人丢失自身定位时，通过位于移动机器人上的左右相机不断获取移动机器人左侧与右侧图像，若是长时间内未检测到标签可小幅度进行前后移动，在任一相机的视野范围内检测到标签后即停止移动。

S3：获取相机在6秒内获取的相对位姿数据，记为集合S：

其中x_i，y_i，z_i分别表示X，Y，Z轴方向上的位移变换，

φ_i分别表示X，Z轴方向上的角度变换，n表示在6秒内获取的位姿总数。将

记为p_i，将该集合去除异常值后采用均值滤波处理得到均值

从而减少局部异常值对整体判定结果的影响。

S4：左右相机不断在实时获取的图像中检测标签，检测到标签后计算相对位姿数据，相对位姿数据包括相对位移信息和相对角度信息，相对位移信息指相机当前位置相对标签在X，Y，Z轴方向上的位移，相对角度信息则指相机当前位置相对标签在X，Y，Z轴方向上的偏转角度。

按照获取的标签编号查找步骤S1中标定得到的标签世界坐标，记为

根据实际不同的情况进行移动机器人的世界坐标推算，实现移动机器人的初步重定位，记该点为

S5：根据标签推算的位姿受光照、实际环境影响而存在一定的偏差，W₂的坐标需要结合多线激光点云信息进一步推算。首先对当前帧点云数据进行体素滤波和噪声去除，体素滤波目的在于采用特征明显的局部点云代替全局点云，降低整体需要处理的点云数据量，噪声去除的目的在于去除异常点云数据，减少对结果的不利影响。将当前移动机器人所处空间切分成固定边长的立方格，形成栅格空间，依据3D激光扫描得到的点云数据中的点是否在立方格内的情况，赋予立方格中心以不同的值，立方格中心的值表示该立方格为障碍物的概率；然后以点W₂为中心，以参数r_s为半径并结合步骤S4中W₂相对于W₁的角度信息，进行定向范围搜索。对当前帧点云数据按照相等间隔的位移和角度进行3D层面的变换，并依据变换后的点在栅格空间中的坐标获取概率值，计算概率值累加和，以累加和为最大值时对应的间隔变换后的位姿数据作为精准重定位的结果，即

进一步地，步骤S1具体包括如下步骤：

S11：如图3示意图所示，Apriltag标签高度以移动机器人相机安装高度上下幅度不超过10cm为准，将移动机器人开始建图的点作为世界坐标系的原点，在移动过程中，将移动机器人上的3D激光扫描得到的点云数据与之前扫描过程中已获得点云数据进行匹配，采用子图集策略，以N帧点云数据建立子图，并以每N帧位姿构造节点，在移动机器人不断移动的过程中，不断计算节点间的变换过程约束以及当前帧与相邻节点间的约束，将约束作为节点间的边，在3D SLAM系统的后端进行非线性优化，将其转化为图优化问题，构建优化函数如下：

上式中，

分别表示当前扫描帧具有约束关系的子图集合和已获取的扫描帧的位姿集合，n₁，n₂分别表示与当前扫描帧具有约束关系的子图和之前扫描帧的数量，Δ_ij，Ω_ij分别表示扫描帧j相对子图i的相对位姿和协方差矩阵，E(·)则表示

Ω_ij，Δ_ij的非线性误差函数，采用Levenberg-Marquardt算法求解最优子图位姿p^m和最优扫描帧位姿p^s，结合移动机器人初始位姿转换为当前时刻下的世界坐标。

S12：在构建地图的过程中，左右相机不断获取图像进行检测，当检测到Apriltag标签并正对该标签时，用0和1区分左右相机记录的数据，记录下此刻检测到的标签号和根据移动机器人世界坐标转换得到Apriltag标签的世界坐标并保存以便后续使用。

进一步地，步骤S3具体包括如下步骤：

S31：异常值设定为当前均值的5倍及以上，检测到异常值则抛弃该数据，对所获多个正常数据采用均值滤波处理，公式如下：

其中，

为最终获取的位姿均值数据，p_i为非异常位姿数据。

进一步地，步骤S4具体包括如下步骤：

S41：实际移动机器人移动过程中左右相机并不一定都能检测到标签，按照检测到标签与否分类，情形可分为两类，一类是单侧，另一类是双侧。单侧是指只有左相机或者右相机检测到标签，即如图3示意图所示只有左侧或者右侧墙上的标签进入单侧相机的视野范围内，双侧则是指左右相机均检测到标签，正好左右相机都能在各自墙上检测到对应标签。

S42：在单侧情况下，当相机范围内检测到多个标签时，记检测到的标签编号为d_i，i＝1，…，m，其中m表示检测标签数量，由于视野范围受限，m一般不超过5。根据标签编号查找对应标定所得世界坐标为

取此时检测所得移动机器人相对标签的位姿数据中的相对位移变换信息

角度信息

和

得到移动机器人初步的世界坐标为

角度

S43：在双侧情况下，记左侧检测所得标签编号为l_i，i＝1，…，m_l，其中m_l表示左侧检测标签数量，右侧为r_j，j＝1，…，m_r，其中m_r表示右侧检测标签数量，根据标签编号在标定信息中查找对应标签世界坐标为

取此时检测所得相对位姿数据中的相对位移变换信息

角度信息

和

得到移动机器人初步的世界坐标为

角度

进一步地，步骤S5包括如下步骤：

S51：以步骤S4得到的移动机器人世界坐标

为球心，参数r_s为半径，角度

和

建立球锥，如图4所示，变换的范围并不是整个球体范围，采用在该球锥范围内进行变换能够保证在正确方向上匹配并且极大地减少变换量，参数r_s可根据实际环境进行自主设置。在球锥范围内按照设定若干个等间隔

对当前帧扫描得到的点云数据进行3D空间上的变换，记变换前的点云数据为C，变换后的点云数据为C′。

S52：利用栅格化思想，将当前移动机器人所处空间切分成固定边长的立方格，立方格中心的值表示该立方格为障碍物的概率，计算概率值q的泛化公式如下：

q＝A*(2t³-3t²+1)+B*(-2t³+3t²)

其中，A和B为常系数，t＝0时，A＝1，B＝0；t＝1时，A＝0，B＝1，常系数A和B的确定过程如下：

对点k进行ΔT变换后，得到地图坐标记为(x_m，y_m，z_m)，如图5所示，查询3D占据栅格图(x_m，y_m，z_m)对应的栅格号(x_index，y_index，z_index)，根据栅格号计算其栅格中心(x_c，y_c，z_c)，以x_m＞x_c，y_m＞y_c，z_m＞z_c为例进行分析，记(x_c，y_c，z_c)为(x₁，y₁，z₁)，将(x_c，y_c，z_c)加上分辨率r得到(xc+r，y_c+r，z_c+r)，将其记为(x₂，y₂，z₂)，分辨率r可自主设置，在这里设置为5cm，若是x_m＜x_c，y_m＜y_c，z_m＜z_c，则(x₂，y₂，z₂)＝(x_c-r，y_c-r，z_c-r)。

根据(x₁，y₁，z₁)和(x₂，y₂，z₂)计算归一化坐标(x_n，y_n，z_n)。

(x_n，y_n，z_n)＝((x-x₁)/(x₂-x₁)，(y-y₁)/(y₂-y₁)，(z-z₁)/(z₂-z₁))

获取(x₁，y₁，z₁)所在栅格的标签号(x_index1，y_index1，z_index1)，根据标签号获取栅格概率q₁₁₁，分别加标签加1，例如：(x_index1+1，y_index1，z_index1)，获取其余7个点的标签号，根据变换后的标签号获取概率得到q₁₁₂，q₁₂₁，q₁₂₂，q₂₁₁，q₂₁₂，q₂₂₁，q₂₂₂，值为0或1，根据得到的8个概率值得到点(x_m，y_m，z_m)的概率q的具体形式：

通过概率值q的泛化公式与公式的具体形式的对比，可知常系数A和B的具体形式，概率值越高则该立方格越黑，代表该位置越可能为障碍物，概率越低则表示该位置越可能为空，未知区域处的立方格设置为灰色，即概率值的0.5。

S53：在对现有扫描空间进行栅格化并设定好概率计算方式后，对当前扫描点云数据C按照设定的不同间隔ΔT进行变换，并依据变换后的点在栅格空间中的坐标来获取概率值，对当前点云数据点进行累加，将概率值的累加和最大的ΔT定义为最优变换ΔT′，概率和公式如下：

根据最大概率和公式进行转换，建立最小二乘化模型进行非线性优化，并引入损失函数，从而得到最优变换ΔT′。

其中，K表示t时刻扫描得到的点云数量，ρ_k表示损失函数

用于排除异常值对优化结果的影响，s表示立方格的概率值，Q表示计算概率函数，h_k表示t时刻下扫描所得点云中的点k在移动机器人坐标系下的坐标，ΔT表示将点k在移动机器人坐标系下的坐标变换为世界坐标系下坐标的变换矩阵。

S54：将计算所得最高概率和的变换

记为最优变换，将

变换成

作为最终精准定位的坐标，获取该坐标后意味着移动机器人能够以较小的误差定位在丢失位姿处，保证3D SLAM系统前端能够在匹配误差范围内，可通过3D SLAM系统后端非线性优化和回环检测进行纠正，而不会出现乱匹配和定位丢失的情况。

与前述基于视觉特征和3D激光的移动机器人重定位方法的实施例相对应，本发明还提供了基于视觉特征和3D激光的移动机器人重定位装置的实施例。

参见图6，本发明实施例提供的一种基于视觉特征和3D激光的移动机器人重定位装置，包括存储器和一个或多个处理器，所述存储器中存储有可执行代码，所述处理器执行所述可执行代码时，用于实现上述实施例中的基于视觉特征和3D激光的移动机器人重定位方法。

本发明基于视觉特征和3D激光的移动机器人重定位装置的实施例可以应用在任意具备数据处理能力的设备上，该任意具备数据处理能力的设备可以为诸如计算机等设备或装置。装置实施例可以通过软件实现，也可以通过硬件或者软硬件结合的方式实现。以软件实现为例，作为一个逻辑意义上的装置，是通过其所在任意具备数据处理能力的设备的处理器将非易失性存储器中对应的计算机程序指令读取到内存中运行形成的。从硬件层面而言，如图6所示，为本发明基于视觉特征和3D激光的移动机器人重定位装置所在任意具备数据处理能力的设备的一种硬件结构图，除了图6所示的处理器、内存、网络接口、以及非易失性存储器之外，实施例中装置所在的任意具备数据处理能力的设备通常根据该任意具备数据处理能力的设备的实际功能，还可以包括其他硬件，对此不再赘述。

上述装置中各个单元的功能和作用的实现过程具体详见上述方法中对应步骤的实现过程，在此不再赘述。

对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本发明方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

本发明实施例还提供一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现上述实施例中的基于视觉特征和3D激光的移动机器人重定位方法。

所述计算机可读存储介质可以是前述任一实施例所述的任意具备数据处理能力的设备的内部存储单元，例如硬盘或内存。所述计算机可读存储介质也可以是任意具备数据处理能力的设备的外部存储设备，例如所述设备上配备的插接式硬盘、智能存储卡(Smart Media Card，SMC)、SD卡、闪存卡(Flash Card)等。进一步的，所述计算机可读存储介质还可以既包括任意具备数据处理能力的设备的内部存储单元也包括外部存储设备。所述计算机可读存储介质用于存储所述计算机程序以及所述任意具备数据处理能力的设备所需的其他程序和数据，还可以用于暂时地存储已经输出或者将要输出的数据。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变型，这些改进和变型也应视为本发明的保护范围。

Claims (7)

1.一种基于视觉特征和3D激光的移动机器人重定位方法，其特征在于，该方法包括如下步骤：

步骤1、利用多线激光雷达和IMU传感器对实际场景进行3D层面的建图，在地图构建的过程中，获取移动机器人相对于Apriltag标签的位姿，根据移动机器人世界坐标转换得到Apriltag标签世界坐标信息，同时记录标签编号，完成对Apriltag标签的世界坐标的标定；

步骤2、在移动机器人丢失自身定位时，通过位于移动机器人上的左右相机不断获取移动机器人左侧与右侧图像，在任一相机的视野范围内检测到Apriltag标签后即停止移动；

步骤3、获取移动机器人相对Apriltag标签的若干个位姿数据，记为集合S：

其中x_i,y_i,z_i分别表示X,Y,Z轴方向上相对Apriltag标签的位移变换，

φ_i分别表示X,Z轴方向上相对Apriltag标签的角度变换，n表示获取的位姿总数；

步骤4、按照获取的Apriltag标签编号查找步骤1中标定得到的标签世界坐标，记为

基于步骤3中得到的相对Apriltag标签的位姿数据，进行移动机器人的世界坐标推算，实现移动机器人的初步重定位，记移动机器人初步重定位结果为

步骤5、将W₂的坐标结合多线激光点云信息进一步推算，具体为：首先对移动机器人上3D激光的当前帧点云数据进行体素滤波和噪声去除，将当前移动机器人所处空间切分成固定边长的立方格，形成栅格空间，依据3D激光扫描得到的点云数据中的点是否在立方格内将立方格的中心值设为1和0，立方格内其余点的值则通过相邻立方格的中心值来计算，值的大小表示障碍物的概率；然后以坐标W₂为中心，以参数r_s为半径并结合步骤4中W₂相对于W₁的角度信息，进行定向范围搜索；对当前帧点云数据按照相等间隔的位移和角度进行3D层面的变换，并依据变换后的点在栅格空间中的坐标获取概率值，计算概率值累加和，以累加和为最大值时对应的间隔变换后的位姿数据作为精准重定位的结果，即

2.根据权利要求1所述的一种基于视觉特征和3D激光的移动机器人重定位方法，其特征在于：步骤3中，将

记为p_i，将该集合去除异常值后采用均值滤波处理得到均值

减少局部异常值对整体判定结果的影响。

3.根据权利要求1所述的一种基于视觉特征和3D激光的移动机器人重定位方法，其特征在于：所述步骤1中对Apriltag标签的世界坐标的标定，具体过程为：

1-1、将移动机器人开始建图的点作为世界坐标系的原点，在移动过程中，将移动机器人上的3D激光扫描得到的点云数据与之前扫描过程中已获得点云数据进行匹配，采用子图集策略，以N帧点云数据建立子图，并以每N帧移动机器人位姿构造节点，在移动机器人不断移动的过程中，不断计算节点间的变换过程约束以及当前帧与相邻节点间的约束，将约束作为节点间的边，并进行非线性优化，将其转化为图优化问题，构建优化函数如下：

上式中，

分别表示当前扫描帧具有约束关系的子图集合和已获取的扫描帧的位姿集合，n₁,n₂分别表示与当前扫描帧具有约束关系的子图和在当前扫描帧之前扫描帧的数量，Δ_ij,Ω_ij分别表示扫描帧j相对子图i的相对位姿和协方差矩阵，E(·)则表示关于

Ω_ij,Δ_ij的非线性误差函数，采用Levenberg-Marquardt算法求解最优子图位姿p^m和最优扫描帧位姿p^s，结合移动机器人初始位姿转换为当前时刻下的移动机器人世界坐标；

1-2、在构建地图的过程中，左右相机不断获取图像进行检测，当检测到Apriltag标签时，用0和1区分左右相机记录的数据，记录下此刻检测到的标签号和根据移动机器人世界坐标转换得到Apriltag标签的世界坐标并保存以便后续使用。

4.根据权利要求1所述的一种基于视觉特征和3D激光的移动机器人重定位方法，其特征在于：所述步骤4中左右相机检测标签并获取相对位姿数据来进行初步重定位，具体为：

4-1、按照实际移动机器人移动过程中左右相机检测到标签与否分类，情形可分为两类，一类是单侧，另一类是双侧；单侧是指只有左相机或者右相机检测到标签，则进行步骤4-2，双侧则是指左右相机均检测到标签，则进行步骤4-3。

4-2、在单侧情况下，当相机范围内检测到多个标签时，记检测到的标签编号为d_i,i＝1,...,m，其中m表示检测标签数量；根据标签编号查找对应标定所得标签世界坐标为

取此时检测所得移动机器人相对标签的位姿数据中的相对位移变换信息

角度信息

和

得到移动机器人初步的世界坐标为

角度

4-3、在双侧情况下，记左侧检测所得标签编号为l_i,i＝1,...,m_l，其中m_l表示左侧检测标签数量，右侧为r_j,j＝1,...,m_r，其中m_r表示右侧检测标签数量，根据标签编号查找对应标签世界坐标为

取此时检测所得移动机器人相对标签的位姿数据中的相对位移变换信息

角度信息

和

得到移动机器人初步的世界坐标为

角度

5.根据权利要求4所述的一种基于视觉特征和3D激光的移动机器人重定位方法，其特征在于：所述步骤5结合多线激光雷达点云数据匹配进行精准重定位，具体为：

5-1、以步骤4得到的移动机器人世界坐标

为球心，参数r_s为半径，角度

和

建立球锥，参数r_s可根据实际环境进行自主设置；在球锥范围内按照设定的若干个等间隔

对3D激光当前帧扫描得到的点云数据进行3D空间上的变换，记变换前的点云数据为C，变换后的点云数据为C′；

5-2、利用栅格化思想，将当前移动机器人所处空间切分成固定边长的立方格，立方格中心的值表示该立方格为障碍物的概率；概率值越高则该立方格越黑，代表该位置越可能为障碍物，概率越低则表示该位置越可能为空，未知区域处的立方格设置为灰色，即概率值的0.5。

5-3、根据设定的不同间隔ΔT对当前扫描点云数据C进行变换，并依据变换后的点在栅格空间中的坐标获取概率值，对当前点云数据点的概率值进行累加，建立最小二乘化模型进行非线性优化，并引入损失函数，从而得到最优变换ΔT′。

其中，K表示t时刻扫描得到的点云数量，ρ_k表示损失函数

用于排除异常值对优化结果的影响，s表示立方格的概率值，Q表示计算概率函数，h_k表示t时刻下扫描所得点云中的第k个点在移动机器人坐标系下的坐标，ΔT表示将第k个点在移动机器人坐标系下的坐标变换为世界坐标系下坐标的变换矩阵；

5-4、基于最优变换

将

变换成

作为最终精准定位的坐标。

6.一种基于视觉特征和3D激光的移动机器人重定位装置，包括存储器和一个或多个处理器，所述存储器中存储有可执行代码，其特征在于，所述处理器执行所述可执行代码时，实现如权利要求1-5中任一项所述的基于视觉特征和3D激光的移动机器人重定位方法。

7.一种计算机可读存储介质，其上存储有程序，其特征在于，所述程序被处理器执行时，实现如权利要求1-5中任一项所述的基于视觉特征和3D激光的移动机器人重定位方法。

Images