[发明专利]一种基于视觉引导激光重定位的移动机器人定位方法

权利要求书

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方法建立；建立视觉特征地图和激光地图时选择相同的初始点。