CN111337943B - 一种基于视觉引导激光重定位的移动机器人定位方法 - Google Patents

Abstract

本发明涉及一种基于视觉引导激光重定位的移动机器人定位方法，包括以下步骤：根据视觉特征地图对机器人的位置进行初始化定位，并将其映射到激光地图上；采用自适应粒子滤波方法，根据激光扫描匹配的结果获取机器人在激光地图上的精准定位；判断在自适应粒子滤波方法的定位过程中定位的粒子方差是否超过设定的阈值，若是，利用视觉特征地图进行视觉重定位，输出机器人的定位结果，并对当前的粒子进行重新初始化，即错误恢复；若否，输出机器人的定位结果；与现有技术相比，本发明可使得机器人能在初始化时或“被绑架”后，依靠视觉特征地图的重定位功能，快速恢复精准的定位，从而保证了定位的稳定可靠。

Description

一种基于视觉引导激光重定位的移动机器人定位方法

技术领域

本发明涉及移动机器人自主定位领域，尤其是涉及一种基于视觉引导激光重定位的移动机器人定位方法。

背景技术

定位技术是机器人自主移动的基础，是赋予机器人感知和行动能力的关键。随着机器人技术的发展和广泛应用，越来越多的工业搬运机器人和巡检机器人需要在无人化的工厂里工作，而定位技术则是其执行任务的基础。

传统的定位方法中，绝对定位主要采用导航信标、标识和卫星导航技术等进行定位，但信标等的建设和维护成本较高，GPS只能用于室外。相对定位主要采用轮式编码器、惯性测量单元和激光雷达等，轮式编码器和惯性测量单元都是测量一些间接的物理量，高精度的激光雷达价格过于昂贵，这些因素给自主定位的移动机器人的商业化、进入日常生活带来了不小的挑战和困难。近几年来，以视觉传感器为中心的视觉SLAM技术得到了明显的转变和突破。多种基于单目、双目和深度相机的理论方法被提出，其中，相机相比于激光雷达虽然精度较差、噪声大，但是成本低，能够采集彩色图像信息，因而更加适合在普通的办公、家庭等室内场景使用。

然而目前主流的方法中，机器人定位多采用激光雷达方案或视觉方案。基于激光雷达的方法可以在结构化的环境中保持良好的定位，对高速运动不敏感，但其难以初始化且错误恢复较难。当前基于激光定位技术的无人搬运车辆往往需要固定工位，在设定好的工位进行初始化而无法灵活地在任意地方开始工作。另外，一旦其定位发生，由于工厂环境结构的对称性及相似性，要恢复到正确的位置往往需要很长时间的更新和恢复，而大大影响了生产效率，更有可能造成安全隐患。基于视觉的定位方法则可以快速初始化，但其不能应对快速运动和旋转，且对环境特征和光线要求很高，如若环境特征不能一直维持，也很容易丢失定位。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于视觉引导激光重定位的移动机器人定位方法。

本发明的目的可以通过以下技术方案来实现：

一种基于视觉引导激光重定位的移动机器人定位方法，包括以下步骤：

S1、根据视觉特征地图对机器人的位置进行初始化定位，并将其映射到激光地图上。

S2、采用自适应粒子滤波方法，根据激光扫描匹配的结果获取机器人在激光地图上的精准定位；

S3、判断在自适应粒子滤波方法的定位过程中定位的粒子方差是否超过设定的阈值，若是，则执行步骤S4；若否，则执行步骤S5；

S4、利用视觉特征地图进行视觉重定位，输出机器人的定位结果，并对当前的粒子进行重新初始化，即错误恢复；

S5、输出机器人的定位结果。

进一步地，所述的步骤S1中，根据视觉特征地图对机器人进行定位具体包括以下步骤：

S11、提取图像ORB特征，使用DBoW2模型计算当前帧的BoW词袋向量，生成一个表征向量v；

S12、挑选视觉特征地图上的备选关键帧，计算备选关键帧与当前帧的BoW相似性得分，相似性标准采用L1范数距离，返回所有满足设定得分的关键帧；

S13、逐个比较当前帧和候选关键帧的匹配特征点，得到匹配关系；

S14、根据匹配关系，对每个备选关键帧轮流执行EPnP算法，使用RANSAC算法迭代求解当前帧的位姿，即在视觉特征地图中的位置。

进一步地，所述的步骤S14中，EPnP算法的步骤包括：

A1、选取世界坐标下的四个控制点坐标为：

C_w＝[0 0 0 1]^T，[1 0 0 1]^T，[0 1 0 1]^T，[0 0 1 1]^T；

A2、通过n个3D点在摄像头平面的投影关系，以及与这四个控制点的权重关系，构建一个12×12方阵，求得其零空间特征向量，得到虚拟控制点的摄像头平面坐标，然后使用正交投影迭代变换即可求出摄像头位姿；

A3、通过EPnP算法求得四对点下的封闭解后，将该解作为非线性优化的初值，优化提高精度。

进一步地，所述的步骤S2中，自适应粒子滤波的方法具体包括以下步骤：

S21、获取里程计和激光传感器的数据；

S22、通过粒子分布设定机器人位置的后验概率分布，设t时刻的N个粒子为

S23、根据里程计移动模型和激光传感器模型更新粒子

S24、计算粒子的权重并且按权重进行自适应重采样；

S25、输出粒子的加权值，作为机器人的当前位姿。

进一步地，所述的里程计移动模型的表达式为：

其中，p_rot1为在机器人位置在世界坐标系下的旋转误差分布，p_rot2为机器人的自身旋转误差分布，p_trans为机器人移动的位移误差分布，u_t为里程计提供的数据。

进一步地，所述的激光传感器模型的表达式为：

其中，z_hit、z_rand、z_max为权重，p_hit为测量误差，p_rand为观测随机噪声，p_max为最大测量距离似然，z_t为激光传感器提供的数据。

进一步地，所述的步骤S2中，将视觉特征地图中定位的位置映射到激光地图上，具体包括：将机器人在激光地图下的位姿估计为地图间齐次变换矩阵与使用视觉特征地图重定位后获取位姿的乘积。

进一步地，自适应粒子滤波方法的定位过程中定位的粒子方差的计算表达式为：

其中，

表示定位的粒子集合。

进一步地，视觉特征地图根据ORB-SLAM2方法建立，建立的视觉特征地图中包含特征点和关键帧的信息；激光地图根据cartographer方法建立；建立视觉特征地图和激光地图时选择相同的初始点。

与现有技术相比，本发明具有以下优点：

1、本发明有机结合了激光匹配定位算法和视觉定位算法，可使得机器人能在“初始化”或“被绑架”(意为突然被搬到某地，也就是机器人非自发地被移动到某个地方)后，依靠视觉特征地图的重定位功能，快速恢复精准的定位，从而保证了定位的稳定可靠；本发明比传统激光匹配定位算法更加抗干扰，机器人被绑架后可以快速恢复自身定位；同时，本发明比传统基于特征查找的视觉定位算法更加鲁棒，不会受剧烈运动的影响，特征点的丢失不影响激光匹配端的定位。

2、本发明不对环境特征产生过多的依赖，仅在粒子失效时利用视觉特征进行错误恢复，使得机器人的定位效果也更加稳定。应用方面，也不再依赖于固定工位的设计，可以使得机器人在任意地方初始化，短期定位漂移后也能快速恢复，更加适用于更加复杂环境中精准稳定的定位。

3、本发明同时建立视觉特征地图和激光地图，提供两种位置表达，从环境中提取出了更加稳定而丰富的定位信息，提高了定位的精度。

附图说明

图1为本实施例的定位系统示意图。

图2为本实施例的流程示意图。

图3为视觉重定位算法流程示意图。

图4为激光定位算法流程示意图。

图5为实施例的定位设备结构示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

如图1所示，本实施例中机器人采用的定位系统包括激光传感器、摄像头和里程计，以及分别连接激光传感器、摄像头和里程计的一个定位模块。基于视觉引导激光重定位的移动机器人定位方法在定位模块中执行用于实时输出机器人的位置。

如图5所示，机器人搭载了摄像头，激光雷达和轮式里程计。定义世界坐标系为建立激光地图和视觉特征地图时所使用的坐标系，摄像头坐标系原点位于机器人上摄像头的安装位置，设定以摄像头的前方为x轴，左侧为y轴，垂直于地面的为z轴。定义激光传感器坐标系和里程计坐标系重合，其原点位于机器人模型底盘上激光传感器的安装位置，前方为x轴，左侧为y轴，垂直于地面的为z轴。

如图2所示，本实施例提供了一种基于视觉引导激光重定位的移动机器人定位方法，包括以下步骤：

步骤S1、根据视觉特征地图对机器人的位置进行初始化定位，并将其映射到激光地图上。

步骤S2、采用自适应粒子滤波方法，根据激光扫描匹配的结果获取机器人在激光地图上的精准定位。

步骤S3、判断在自适应粒子滤波方法的定位过程中定位的粒子方差是否超过设定的阈值，若是，则执行步骤S4；若否，则执行步骤S5。

步骤S4、利用视觉特征地图进行视觉重定位，输出机器人的定位结果，并对当前的粒子进行重新初始化，即错误恢复。

步骤S5、输出机器人的定位结果。

根据上述的方法，本实施例可具体分为二个部分进行描述：

一、视觉特征重定位与错误恢复

本实施例采用的视觉特征为ORB特征，其具有良好的旋转不变性，且计算较快。如图3所示，在步骤S1和步骤S4中，视觉定位均为首先提取图像ORB特征，然后匹配视觉特征地图，解算机器人位置。

需要注意的是，本实施例中激光地图根据google开源的cartographer方法建立，视觉特征地图根据ORB-SLAM2方法建立。设激光地图坐标系为m₀，视觉特征地图坐标系为m₁，坐标系是在开始建立地图时所设定的，若建立地图时选择的初始点相同，则两坐标系重合。另外，建立的视觉特征地图中包含了特征点和关键帧的信息。

获取视觉特征地图坐标系下的位姿方法如下：

步骤S11、提取图像ORB特征，使用DBoW2模型计算当前帧的BoW(Bag of Words)词袋向量，生成一个表征向量v。

步骤S12、挑选视觉特征地图上的备选关键帧，计算备选关键帧与当前帧的BoW相似性得分，相似性标准采用L1范数距离，返回所有满足设定得分(本实施例中采用最高得分的75％)的关键帧；

定义两向量间的L1范数距离如下：

其中，v₁，v₂分别代表备选关键帧和当前帧的表征向

步骤S13、逐个比较当前帧和候选关键帧的匹配特征点，得到匹配关系。

步骤S14、根据匹配关系，对每个备选关键帧轮流执行EPnP算法，使用RANSAC算法迭代求解当前帧的位姿，即在视觉特征地图中的位置。

其中，EPnP算法的步骤包括：

步骤A1、选取世界坐标系下的四个控制点坐标为：

C_w＝[0 0 0 1]^T，[1 0 0 1]^T，[0 1 0 1]^T，[0 0 1 1]^T；

步骤A2、通过n个3D点在摄像头平面的投影关系，以及与这四个控制点的权重关系，构建一个12×12方阵，求得其零空间特征向量，得到虚拟控制点的摄像头平面坐标，然后使用正交投影迭代变换即可求出摄像头位姿；

步骤A3、通过EPnP算法求得四对点下的封闭解后，将该解作为非线性优化的初值，优化提高精度。

在得到视觉特征地图下的位置后，要执行步骤S2，对机器人位姿进行错误恢复，就需要根据地图之间的坐标转换把当前位置映射到激光地图中。具体包括：将机器人在激光地图下的位姿估计为地图间齐次变换矩阵与使用视觉特征地图重定位后获取位姿的乘积。

设使用视觉特征地图重定位后获取的机器人位姿为x|_m1，设m₁到m₀之间的齐次变换矩阵为

则其在激光地图下的位姿可估计为：

其中，m₁表示视觉特征地图坐标系，m₀表示激光地图坐标系。

二、激光特征定位过程

本部分旨在解决利用激光传感器数据和轮式里程计数据获取定位结果的问题，其流程如图4所示，采用自适应粒子滤波的方法，在获取了里程计和激光的数据后，由粒子滤波器更新，最后融合粒子信息得到机器人的位置。

设机器人位置在激光地图下的表示x_t|_m0(x,y,θ)，里程计提的数据U＝{u₀,u₁,u₂,···,u_t}，激光传感器提供测量数据Z＝{z₀,z₁,z₂,···,z_t}；

自适应粒子滤波的方法具体包括以下步骤：

步骤S21、获取里程计和激光传感器的数据。

步骤S22、通过粒子分布设定机器人位置的后验概率分布，初始化粒子。

定义机器人位置的后验概率分布：

bel(x_t)＝p(x_t|z_0...t,u_0...t)

这里，后验概率用粒子分布表示，设t时刻的N个粒子为

即粒子集

的分布代表了p(x_t|z_0...t,u_0...t)；

由于机器人可能具有硬件误差，因此建立激光传感器模型和里程计移动模型，再执行粒子滤波器的预测和更新。

步骤S23、根据里程计移动模型和激光传感器模型更新粒子集

首先对粒子做运动预测更新，利用里程计移动模型

机器人在上一个位置的概率

和里程计获得的数据u_t-1，计算当前时刻的机器人位置

其计算表达式如下：

其中，

x yθ分别表示机器人在激光地图坐标系下的x坐标值、y坐标值和朝向的偏角。

里程计移动模型的表达式为：

其中，p_rot1为在机器人位置在世界坐标系下的旋转误差分布，p_rot2为机器人的自身旋转误差分布，p_trans为机器人移动的位移误差分布，u_t为里程计提供的数据。

然后，采用激光传感器的数据对当前时刻的机器人位置

进行测量更新，进一步矫正定位结果。利用激光传感器模型

当前时刻的机器人位置

和归一化常数η_t，求得基于激光传感器信息提高准确度的当前位置的概率

其计算表达式为：

激光传感器模型的表达式为：

其中，z_hit、z_rand、z_max为权重，p_hit为测量误差，p_rand为观测随机噪声，p_max为最大测量距离似然，z_t为激光传感器提供的数据。

步骤S24、计算粒子的权重并且按权重进行自适应重采样。

权重计算：使用机器人上一个位置的概率

和里程计移动模型

来提取新的样本集合x_t′。使用样本x_t′中的N个粒子

里程计测量数据

和归一化常数η_p来计算权重值

按权重进行自适应重采样，权重高的粒子更有可能留下，使用样本x_t′ⁱ和w_t ⁱ来创建N个新的粒子集合

这些粒子的分布也就代表了

关于本文中的概率表达形式和位姿表达形式的解释：粒子的分布就代表了概率分布，因此在计算得出最后位姿的时候，也是得到了一个概率分布，而这个概率分布的表达形式就是粒子群，所以最后加权平均粒子求出了所需要的位姿表达。

粒子的方差按如下公式计算：

从粒子的方差是否大于设定的阈值可以判断粒子是否失效，粒子失效也就意味着激光定位模块的输出结果是不准确的，因此，需要用视觉特征定位模块对其进行重定位。若方差小于所设定的阈值，则证明粒子良好地跟随了机器人的轨迹，因此可以加权粒子的值，即从概率分布中提取得到机器人位姿：

步骤S25、输出粒子的加权值，作为机器人的当前位姿。

综上，本实施例可以采用以下算法形式实现：

1.提取图像词袋向量，在视觉特征地图中进行重定位，获取m₁中机器人的初始位姿x_0|m1；

2.初始化自适应粒子滤波的N个粒子

满足

3.根据里程计移动模型p(x_t|x_t-1,u_t)和激光传感器模型p(z_t|x_t)更新粒子

4.计算粒子的权重

并进行重采样；

5.如果(粒子的方差Σ_t大于设定的阈值)：则根据视觉特征地图进行重定位，直接输出机器人的估计位姿x_t，并将粒子重新初始化为

使得下一次自适应粒子滤波的初值准确；

6.输出粒子的加权值，作为机器人的估计位姿x_t。

本实施例基于建立的激光地图和视觉特征地图对机器人位置进行精准定位。激光地图的建立使其易于跟踪环境的结构化特征，在粒子方差小时，可以输出较精准的位置，同时其对高速旋转和快速运动不敏感。但其初始化和错误恢复的过程较困难，因此本发明提出了一种基于视觉引导激光重定位的方法。在初始化或粒子方差较大时，基于图像ORB特征对机器人位置进行视觉重定位，将视觉特征地图重定位的位置映射到激光地图上，继而维持机器人定位的长期稳定。

本实施例不对环境特征产生过多的依赖，仅在粒子失效时利用视觉特征进行错误恢复，使得机器人的定位效果也更加稳定。应用方面，也不再依赖于固定工位的设计，可以在任意地方初始化，短期定位漂移后也能快速恢复。本发明融合了激光定位和视觉定位的优点，可适用于更加复杂环境中精准稳定的定位。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术人员无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (6)

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

S1、根据视觉特征地图对机器人的位置进行初始化定位，并将其映射到激光地图上；

S2、采用自适应粒子滤波方法，根据激光扫描匹配的结果获取机器人在激光地图上的精准定位；自适应粒子滤波的方法具体包括以下步骤：

S21、获取里程计和激光传感器的数据；

S22、通过粒子分布设定机器人位置的后验概率分布，设t时刻的N个粒子为

其中i＝1～N，初始化粒子使其满足机器人在激光地图中的初始位置；

S23、根据里程计移动模型和激光传感器模型更新粒子

S24、计算粒子的权重并且按权重进行自适应重采样；

S25、输出粒子的加权值，作为机器人的当前位姿；

所述的里程计移动模型的表达式为：

其中，p_rot1为在机器人位置在世界坐标系下的旋转误差分布，p_rot2为机器人的自身旋转误差分布，p_trans为机器人移动的位移误差分布，u_t为里程计提供的数据；

所述的激光传感器模型的表达式为：

其中，z_hit、z_rand、z_max为权重，p_hit为测量误差，p_rand为观测随机噪声，p_max为最大测量距离似然，z_t为激光传感器提供的数据；

S3、判断在自适应粒子滤波方法的定位过程中定位的粒子方差是否超过设定的阈值，若是，则执行步骤S4；若否，则执行步骤S5；

S4、利用视觉特征地图进行视觉重定位，输出机器人的定位结果，并对当前的粒子进行重新初始化，即错误恢复；

S5、输出机器人的定位结果。

2.根据权利要求1所述的基于视觉引导激光重定位的移动机器人定位方法，其特征在于，所述的步骤S1中，根据视觉特征地图对机器人进行定位具体包括以下步骤：

S11、提取图像ORB特征，使用DBoW2模型计算当前帧的BoW词袋向量，生成一个表征向量v；

S12、挑选视觉特征地图上的备选关键帧，计算备选关键帧与当前帧的BoW相似性得分，相似性标准采用L1范数距离，返回所有满足设定得分的关键帧；

S13、逐个比较当前帧和备选关键帧的匹配特征点，得到匹配关系；

S14、根据匹配关系，对每个备选关键帧轮流执行EPnP算法，使用RANSAC算法迭代求解当前帧的位姿，即在视觉特征地图中的位置。

3.根据权利要求2所述的基于视觉引导激光重定位的移动机器人定位方法，其特征在于，所述的步骤S14中，EPnP算法的步骤包括：

A1、选取世界坐标下的四个控制点坐标为：

C_w＝[0 0 0 1]^T，[1 0 0 1]^T，[0 1 0 1]^T，[0 0 1 1]^T；

A2、通过n个3D点在摄像头平面的投影关系，以及与这四个控制点的权重关系，构建一个12×12方阵，求得其零空间特征向量，得到虚拟控制点的摄像头平面坐标，然后使用正交投影迭代变换即可求出摄像头位姿；

A3、通过EPnP算法求得四对点下的封闭解后，将该解作为非线性优化的初值，优化提高精度。

4.根据权利要求1所述的基于视觉引导激光重定位的移动机器人定位方法，其特征在于，所述的步骤S1中，将视觉特征地图中定位的位置映射到激光地图上，具体包括：将机器人在激光地图下的位姿估计为地图间齐次变换矩阵与使用视觉特征地图重定位后获取位姿的乘积。

5.根据权利要求1所述的基于视觉引导激光重定位的移动机器人定位方法，其特征在于，自适应粒子滤波方法的定位过程中定位的粒子方差的计算表达式为：

其中，

表示定位的粒子集合。

6.根据权利要求1所述的基于视觉引导激光重定位的移动机器人定位方法，其特征在于，视觉特征地图根据ORB-SLAM2方法建立，建立的视觉特征地图中包含特征点和关键帧的信息；激光地图根据cartographer方法建立；建立视觉特征地图和激光地图时选择相同的初始点。

Images