一种基于单目视觉里程计和IMU融合的定位方法

本发明属于移动机器人技术领域，具体涉及一种基于单目视觉里程计和imu融合的定位方法。

背景技术：

多传感器融合是机器人定位与导航中未来发展的方向之一，而多传感器融合中，imu与视觉信息的融合又是十分重要的部分。由于单目相机具有成本较低且体积较小的优点，在机器人视觉定位中被广泛使用。但是，单目相机无法获取深度信息，所以缺乏尺度信息，纯视觉定位系统在动态环境下鲁棒性差，定位精度不高；同时，使用滤波思想进行imu数据和单目视觉里程计数据的融合，复杂噪声环境会对融合数据产生不可抗拒的干扰，所以亟需提供一种方法以解决这些问题。

技术实现要素：

本发明的目的是为了解决定位过程中单目视觉在动态物体影响下定位效果和鲁棒性差的问题，同时解决imu累积误差的问题和降低在复杂噪声环境下噪声野值带来数据融合的误差问题，提出了一种基于单目视觉里程计和imu融合的定位方法。

本发明的技术方案是：一种基于单目视觉里程计和imu融合的定位方法包括以下步骤：

s1：利用单目相机采集图像信息并传输至机器人主控系统，利用机器人主控系统对图像进行特征点处理；

s2：利用底盘控制系统获取imu的三轴加速度和三轴角速度，进行处理得到imu数据，并传输至机器人主控系统；

s3：根据底盘控制系统传输的imu数据，利用机器人主控系统进行特征点匹配，得到视觉里程计数据；

s4：利用最大相关熵卡尔曼滤波法，对imu数据和视觉里程计数据进行融合，得到融合后的机器人位姿；

s5：利用dbow2算法对发生回环的机器人位姿进行重定位，完成基于单目视觉里程计和imu融合的定位。

本发明的有益效果是：本发明的定位方法使用滤波思想进行多传感器数据的融合来进一步优化定位信息，将imu与单目视觉里程计结合起来，克服相互的缺点，保留各自的优点，提高了单目视觉里程计定位在动态环境下视觉定位的鲁棒性，解决了imu累计误差不能消除的问题；同时，通过采用基于最大相关熵卡尔曼滤波算法对imu数据和视觉里程计数据进行融合，提高了定位系统在复杂噪声环境下工作的定位精度。在地图纹理变化迅速，出现场景单一，图像特征点少的情况下，能够有效地调整特征点处理机制，提高特征点匹配机制的准确。

进一步地，步骤s1包括以下子步骤：

s11：利用单目相机采集图像信息并传输至机器人主控系统，利用机器人主控系统提取图像信息的orb特征点；

s12：判断orb特征点是否大于设定阈值，若是则进行四叉树均匀化再进入步骤s3，否则直接进入步骤s3。

上述进一步方案的有益效果是：在本发明中，在视觉定位系统中，特征点的提取与处理至关重要。特征点的提取与处理会直接影响后续特征点匹配的精度和速度，从而影响整个定位系统的运行速度和定位精度。对图像信息进行特征点的处理，更好地适应地图纹理丰富度的变化，增强鲁棒性。优化的特征点处理方法可以更应对：1.在地图纹理度较低的情况下，直接进行特征点匹配，确保存在足够的特征点进行后续单目相机的位姿解算；2.在地图纹理度高的情况下，进行特征点均匀化，有效避免特征点聚集，无用特征点繁多，提高特征点匹配精度。

进一步地，步骤s2包括以下子步骤：

s21：利用底盘控制系统获取imu的三轴加速度α和三轴角速度w；

s22：对三轴加速度α和三轴角速度w进行低通滤波处理；

s23：在离散周期内，对低通滤波处理后的三轴加速度α和三轴角速度w分别进行一次积分，得到三轴相对速度v和三轴旋转信息r，对三轴加速度α进行二次积分，得到三轴平移信息t；

s24：三轴旋转信息r和三轴平移信息t作为imu数据，并将低通滤波处理后的三轴加速度α、三轴角速度w、三轴相对速度v和imu数据传输至机器人主控系统。

上述进一步方案的有益效果是：在本发明中，对三轴加速度和角速度进行低通滤波，可以降低复杂环境中噪声野值带来的影响。

进一步地，步骤s3包括以下子步骤：

s31：根据底盘控制系统传输的imu数据，计算固定频率δt下的三轴相对位移δt和三轴相对旋转δr；

s32：根据三轴相对位移δt和三轴相对旋转δr，利用2d-2d算法进行特征点匹配；

s33：设置图像关键帧和图像跟踪固定大小滑动窗口；

s34：对匹配后的特征点建立重投影误差代价函数，并计算最小重投影误差，进行全局优化，得到图像关键帧之间的相对平移数据t^v和相对旋转数据r^v，并将相对平移数据t^v和相对旋转数据r^v作为视觉里程计数据。

上述进一步方案的有益效果是：在本发明中，根据优化的匹配机制进行特征点的匹配。建立图像跟踪固定大小滑动窗口，用于维持计算量；视觉里程计根据前后两帧计算的相对位移和相对旋转受到图像数据的直接影响(动态环境和场景复杂度)，具有不稳定性和精度较低的缺点。根据imu的数据，计算相对位移和相对旋转，来代替视觉里程计解算的数据，能够提高特征点匹配精度和系统鲁棒性。

进一步地，步骤s31中，三轴相对位移δt的计算公式为：

δt＝vδt

其中，δt表示固定频率，v表示三轴相对速度；

三轴相对旋转δr的计算公式为：

δr＝wδt

其中，w表示三轴角速度；

步骤s32中，进行特征点匹配的方法为：根据三轴相对位移δt和三轴相对旋转δr，计算同一特征点在当前帧投影的像素坐标p2，并根据同一特征点在当前帧投影的像素坐标p2在设定匹配搜索范围阈值th内进行特征点匹配，图像投影像素坐标p2的计算公式为：

p2^tfp1＝0

其中，f＝k^-tek^-1，e＝δt^δr，k表示相机内参，e表示本质矩阵，f表示基础矩阵，p1表示同一特征点在上一时刻帧投影的像素坐标；

步骤s33中，设置图像关键帧的方法为：将图像信息的第一帧设定为图像关键帧，若当前图像跟踪上一个图像关键帧的特征点数小于设定阈值，则把当前帧设定为新的图像关键帧；否则不设定。

进一步地，步骤s34中，建立重投影误差代价函数的计算公式为：

δξ＝ui-kexp(ξ)pi

其中，δξ表示重投影误差，i表示当前帧匹配后的第i个特征点，ui表示第i个特征点的像素平面坐标，exp(·)表示指数运算，k表示相机内参，ξ表示三轴相对位移δt和三轴相对旋转δr的外积，pi表示第i个特征点的特征点空间坐标；

最小重投影误差ξ^*的计算公式为：

其中，表示最小重投影误差值运算，n表示图像关键帧数量；

进行全局优化的代价函数f的计算公式为：

其中，eij表示第i个特征点在第j关键帧图像中的重投影误差，m表示当前帧匹配后的特征点数量。

上述进一步方案的有益效果是：在本发明中，通过对极几何、三维重建和p3p等算法，建立重投影误差代价函数；最小重投影误差是对所有当前帧匹配后的特征点进行重投影误差的2范数进行最优化求解，最终解算出优化后的图像关键帧之间的相对平移和相对旋转数据(即视觉里程计数据)，为后续滤波器融合提供观测信息。

进一步地，步骤s4包括以下子步骤：

s41：构建系统状态方程和系统观测方程；

s42：在系统状态方程和系统观测方程中，利用最大相关熵卡尔曼滤波法，对imu数据和视觉里程计数据进行融合，得到融合后的机器人位姿。

进一步地，步骤s41中，系统状态方程的表达式为：

x(k+1)＝ax(k)+bu(k)+v(k+1)

x＝[tx,ty,tz,rx,ry,rz,vx,vy,vz,wx,wy,wz]^t

u＝[αx,αy,αz]^t

其中，x(k+1)表示当前时刻的状态向量，a表示系统状态矩阵，x(k)表示上一时刻的状态向量，b表示输入矩阵，u(k)表示输入向量，v(k+1)表示当前噪声，tx表示imu解算的x轴位移量，ty表示imu解算的y轴位移量，tz表示imu解算的z轴位移量，rx表示imu解算的x轴旋转量，ry表示imu解算的y轴旋转量，rz表示imu解算的z轴旋转量，vx表示当前机器人x轴速度，vy表示当前机器人y轴速度，vz表示当前机器人z轴速度，wx表示当前机器人x轴角速度，wy表示当前机器人y轴角速度，wz表示当前机器人z轴角速度，αx表示当前机器人x轴加速度，αy表示当前机器人y轴加速度，αz表示当前机器人z轴加速度，δt表示固定频率；

步骤s41中，系统观测方程的表达式为：

y(k+1)＝hx(k+1)+w(k+1)

其中，y(k+1)表示当前观测真实值，h表示观测矩阵，w(k+1)表示当前观测噪声，表示相机解算的世界坐标系下的x轴位移量，表示相机解算的世界坐标系下的y轴位移量，表示相机解算的世界坐标系下的z轴位移量，表示相机解算的世界坐标系下的x轴旋转量，表示相机解算的世界坐标系下的y轴旋转量，表示相机解算的世界坐标系下的z轴旋转量。

进一步地，步骤s5包括以下子步骤：

s51：提取图像关键帧的100个orb特征点；

s52：根据每个orb特征点的描述子，利用dbow2算法计算当前帧与之前图像关键帧的相似度大小s(a,b)；

s53：根据相似度大小s(a,b)判断是否发生回环，若是则进入步骤s54，否则返回步骤s4；

s54：将当前帧的orb特征点与回环检测到的帧进行orb描述子匹配；

s55：将描述子匹配得到的匹配点进行ransac误匹配剔除；

s56：对ransac误匹配剔除后的匹配点，利用3d-3d算法计算当前帧在世界坐标系中相对位置的优化函数，完成基于单目视觉里程计和imu融合的定位。

上述进一步方案的有益效果是：在本发明中，随着机器人的运动，机器人的路径还是会与实际路径发生一定的偏移，所以需要使用回环检测模块降低累积误差对机器人路径的影响。进行回环检测后，机器人路径重新进行了调整，得到更加精准的最终结果。若发生回环，根据回环候选帧进行位置计算，并纠正回环中其他关键帧的位置。

进一步地，步骤s52中，当前帧与之前图像关键帧的相似度大小s(a,b)的计算公式为：

其中，w表示图像关键帧的描述子数量，a表示关键帧特征点描述子集合，b表示当前帧特征点描述子集合；

步骤s53中，若相似度大小s(a,b)大于设定阈值，则发生回环，否则未发生回环；

步骤s54中，进行orb描述子匹配的方法为对应描述子的汉明距离；

步骤s56中，当前帧在世界坐标系中相对位置的优化函数的计算公式为：

其中，tp表示当前帧在世界坐标系下的相对位移，rp表示前帧在世界坐标系下的相对旋转，i为当前帧与图像关键帧的第i个匹配特征点，pi表示当前帧的第i个匹配点，pi′为与pi对应的关键帧中的匹配点，n为当前帧与图像关键帧匹配成功的特征点数量。

上述进一步方案的有益效果是：在本发明中，当图像关键帧和当前帧特征点的描述子完全相同时，将得到1；完全相反时(a为0的地方b为1)得到0。这样就定义了两个描述向量的相似性，也就定义了图像之间的相似程度。寻求最优的tp和rp使得优化函数误差最小，即为解算出的相对位姿数据。

附图说明

图1为基于单目视觉里程计和imu融合的定位方法的流程图；

图2为定位系统的结构示意图；

图3为特征点匹配机制原理示意图；

图4为回环检测示意图。

具体实施方式

下面结合附图对本发明的实施例作进一步的说明。

在描述本发明的具体实施例之前，为使本发明的方案更加清楚完整，首先对本发明中出现的缩略语和关键术语定义进行说明：

imu：惯性测量单元(英文：inertialmeasurementunit，简称imu)是测量物体三轴姿态角(或角速率)以及加速度的装置。

2d-2d算法：对极约束算法。

3d-3d算法：英文：iterativeclosestpoint，简称icp，指迭代最近点算法。

如图1所示，本发明提供了一种基于单目视觉里程计和imu融合的定位方法，包括以下步骤：

s1：利用单目相机采集图像信息并传输至机器人主控系统，利用机器人主控系统对图像进行特征点处理；

s2：利用底盘控制系统获取imu的三轴加速度和三轴角速度，进行处理得到imu数据，并传输至机器人主控系统；

s3：根据底盘控制系统传输的imu数据，利用机器人主控系统进行特征点匹配，得到视觉里程计数据；

s4：利用最大相关熵卡尔曼滤波法，对imu数据和视觉里程计数据进行融合，得到融合后的机器人位姿；

s5：利用dbow2算法对发生回环的机器人位姿进行重定位，完成基于单目视觉里程计和imu融合的定位。

在本发明实施例中，如图2所示，通过单目相机获取周围环境的图像信息并传输至机器人主控系统，由机器人主控系统对图像信息进行分析处理；通过底盘控制系统获取imu的数据并处理，将处理后的数据传输至机器人主控系统；机器人主控系统根据底盘控制系统传输的数据进行特征点匹配、信息的融合等操作，最终反馈数据至底盘操作系统，控制机器人的运动。

在本发明实施例中，如图1所示，步骤s1包括以下子步骤：

s11：利用单目相机采集图像信息并传输至机器人主控系统，利用机器人主控系统提取图像信息的orb特征点；

s12：判断orb特征点是否大于设定阈值，若是则进行四叉树均匀化再进入步骤s3，否则直接进入步骤s3。

在本发明中，在视觉定位系统中，特征点的提取与处理至关重要。特征点的提取与处理会直接影响后续特征点匹配的精度和速度，从而影响整个定位系统的运行速度和定位精度。对图像信息进行特征点的处理，更好地适应地图纹理丰富度的变化，增强鲁棒性。优化的特征点处理方法可以更应对：1.在地图纹理度较低的情况下，直接进行特征点匹配，确保存在足够的特征点进行后续单目相机的位姿解算；2.在地图纹理度高的情况下，进行特征点均匀化，有效避免特征点聚集，无用特征点繁多，提高特征点匹配精度。

如图3所示，在实施方式中，可以对raulmur-artal，j.m.m.montiel和juand.tardos提出的orb_slam算法中的特征点处理方式进行优化。该特征点处理方式是通过对提取的特征点进行四叉树均匀化，从而达到特征点均匀化的效果。

在本发明实施例中，如图1所示，步骤s2包括以下子步骤：

s21：利用底盘控制系统获取imu的三轴加速度α和三轴角速度w；

s22：对三轴加速度α和三轴角速度w进行低通滤波处理；

s23：在离散周期内，对低通滤波处理后的三轴加速度α和三轴角速度w分别进行一次积分，得到三轴相对速度v和三轴旋转信息r，对三轴加速度α进行二次积分，得到三轴平移信息t；

s24：三轴旋转信息r和三轴平移信息t作为imu数据，并将低通滤波处理后的三轴加速度α、三轴角速度w、三轴相对速度v和imu数据传输至机器人主控系统。

在本发明中，对三轴加速度和角速度进行低通滤波，可以降低复杂环境中噪声野值带来的影响。

在本发明实施例中，如图1所示，步骤s3包括以下子步骤：

s31：根据底盘控制系统传输的imu数据，计算固定频率δt下的三轴相对位移δt和三轴相对旋转δr；

s32：根据三轴相对位移δt和三轴相对旋转δr，利用2d-2d算法进行特征点匹配；

s33：设置图像关键帧和图像跟踪固定大小滑动窗口；

s34：对匹配后的特征点建立重投影误差代价函数，并计算最小重投影误差，进行全局优化，得到图像关键帧之间的相对平移数据t^v和相对旋转数据r^v，并将相对平移数据t^v和相对旋转数据r^v作为视觉里程计数据。

在本发明中，根据优化的匹配机制进行特征点的匹配。建立图像跟踪固定大小滑动窗口，用于维持计算量；视觉里程计根据前后两帧计算的相对位移和相对旋转受到图像数据的直接影响(动态环境和场景复杂度)，具有不稳定性和精度较低的缺点。根据imu的数据，计算相对位移和相对旋转，来代替视觉里程计解算的数据，能够提高特征点匹配精度和系统鲁棒性。

在本发明实施例中，如图1所示，步骤s31中，三轴相对位移δt的计算公式为：

δt＝vδt

其中，δt表示固定频率，v表示三轴相对速度；

三轴相对旋转δr的计算公式为：

δr＝wδt

其中，w表示三轴角速度；

步骤s32中，进行特征点匹配的方法为：根据三轴相对位移δt和三轴相对旋转δr，计算同一特征点在当前帧投影的像素坐标p2，并根据同一特征点在当前帧投影的像素坐标p2在设定匹配搜索范围阈值th内进行特征点匹配，图像投影像素坐标p2的计算公式为：

p2^tfp1＝0

其中，f＝k^-tek^-1，e＝δt^δr，k表示相机内参，e表示本质矩阵，f表示基础矩阵，p1表示同一特征点在上一时刻帧投影的像素坐标；

步骤s33中，设置图像关键帧的方法为：将图像信息的第一帧设定为图像关键帧，若当前图像跟踪上一个图像关键帧的特征点数小于设定阈值，则把当前帧设定为新的图像关键帧；否则不设定。

在本发明实施例中，如图1所示，步骤s34中，建立重投影误差代价函数的计算公式为：

δξ＝ui-kexp(ξ)pi

其中，δξ表示重投影误差，i表示当前帧匹配后的第i个特征点，ui表示第i个特征点的像素平面坐标，exp(·)表示指数运算，k表示相机内参，ξ表示三轴相对位移δt和三轴相对旋转δr的外积，pi表示第i个特征点的特征点空间坐标；

最小重投影误差ξ^*的计算公式为：

其中，表示最小重投影误差值运算，n表示图像关键帧数量；

进行全局优化的代价函数f的计算公式为：

其中，eij表示第i个特征点在第j关键帧图像中的重投影误差，m表示当前帧匹配后的特征点数量。

在本发明中，通过对极几何、三维重建和p3p等算法，建立重投影误差代价函数；最小重投影误差是对所有当前帧匹配后的特征点进行重投影误差的2范数进行最优化求解，最终解算出优化后的图像关键帧之间的相对平移和相对旋转数据(即视觉里程计数据)，为后续滤波器融合提供观测信息。

在本发明实施例中，如图1所示，步骤s4包括以下子步骤：

s41：构建系统状态方程和系统观测方程；

s42：在系统状态方程和系统观测方程中，利用最大相关熵卡尔曼滤波法，对imu数据和视觉里程计数据进行融合，得到融合后的机器人位姿。

在本发明实施例中，如图1所示，步骤s41中，系统状态方程的表达式为：

x(k+1)＝ax(k)+bu(k)+v(k+1)

x＝[tx,ty,tz,rx,ry,rz,vx,vy,vz,wx,wy,wz]^t

u＝[αx,αy,αz]^t

其中，x(k+1)表示当前时刻的状态向量，a表示系统状态矩阵，x(k)表示上一时刻的状态向量，b表示输入矩阵，u(k)表示输入向量，v(k+1)表示当前噪声，tx表示imu解算的x轴位移量，ty表示imu解算的y轴位移量，tz表示imu解算的z轴位移量，rx表示imu解算的x轴旋转量，ry表示imu解算的y轴旋转量，rz表示imu解算的z轴旋转量，vx表示当前机器人x轴速度，vy表示当前机器人y轴速度，vz表示当前机器人z轴速度，wx表示当前机器人x轴角速度，wy表示当前机器人y轴角速度，wz表示当前机器人z轴角速度，αx表示当前机器人x轴加速度，αy表示当前机器人y轴加速度，αz表示当前机器人z轴加速度，δt表示固定频率；

步骤s41中，系统观测方程的表达式为：

y(k+1)＝hx(k+1)+w(k+1)

其中，y(k+1)表示当前观测真实值，h表示观测矩阵，w(k+1)表示当前观测噪声，表示相机解算的世界坐标系下的x轴位移量，表示相机解算的世界坐标系下的y轴位移量，表示相机解算的世界坐标系下的z轴位移量，表示相机解算的世界坐标系下的x轴旋转量，表示相机解算的世界坐标系下的y轴旋转量，表示相机解算的世界坐标系下的z轴旋转量。

在本发明实施例中，如图4所示，步骤s5包括以下子步骤：

s51：提取图像关键帧的100个orb特征点；

s52：根据每个orb特征点的描述子，利用dbow2算法计算当前帧与之前图像关键帧的相似度大小s(a,b)；

s53：根据相似度大小s(a,b)判断是否发生回环，若是则进入步骤s54，否则返回步骤s4；

s54：将当前帧的orb特征点与回环检测到的帧进行orb描述子匹配；

s55：将描述子匹配得到的匹配点进行ransac误匹配剔除；

s56：对ransac误匹配剔除后的匹配点，利用3d-3d算法计算当前帧在世界坐标系中相对位置的优化函数，完成基于单目视觉里程计和imu融合的定位。

在本发明中，随着机器人的运动，机器人的路径还是会与实际路径发生一定的偏移，所以需要使用回环检测模块降低累积误差对机器人路径的影响。进行回环检测后，机器人路径重新进行了调整，得到更加精准的最终结果。若发生回环，根据回环候选帧进行位置计算，并纠正回环中其他关键帧的位置。

在实施方式中，本发明以badongchen等人提出的maximumcorrentropykalmanfilter(mckf算法)作为本申请的多传感器融合的融合算法。该滤波器算法无论是在高斯噪声环境下，还是非高斯噪声环境下，都能够快速地进行数据的融合，有效地降低复杂环境下噪声野值带来的定位误差。

在本发明实施例中，如图1所示，步骤s52中，当前帧与之前图像关键帧的相似度大小s(a,b)的计算公式为：

其中，w表示图像关键帧的描述子数量，a表示关键帧特征点描述子集合，b表示当前帧特征点描述子集合；

步骤s53中，若相似度大小s(a,b)大于设定阈值，则发生回环，否则未发生回环；

步骤s54中，进行orb描述子匹配的方法为对应描述子的汉明距离；

步骤s56中，当前帧在世界坐标系中相对位置的优化函数的计算公式为：

其中，tp表示当前帧在世界坐标系下的相对位移，rp表示前帧在世界坐标系下的相对旋转，i为当前帧与图像关键帧的第i个匹配特征点，pi表示当前帧的第i个匹配点，pi′为与pi对应的关键帧中的匹配点，n为当前帧与图像关键帧匹配成功的特征点数量。

在本发明中，当图像关键帧和当前帧特征点的描述子完全相同时，将得到1；完全相反时(a为0的地方b为1)得到0。这样就定义了两个描述向量的相似性，也就定义了图像之间的相似程度。寻求最优的tp和rp使得优化函数误差最小，即为解算出的相对位姿数据。

本发明的工作原理及过程为：本发明提供了一种单目视觉和imu融合定位方案，包括：首先，在ros系统中获取当前环境的图像数据，并对所述图像进行特征点的处理；其次，通过底盘控制芯片获取并处理imu的数据，最后传输数据至ros系统；再次，根据底盘控制芯片上传的数据，进行特征点的匹配；然后，使用基于最大相关熵卡尔曼滤波算法(mckf)对imu的数据和单目视觉里程计解算出的数据进行融合；最后对局部地图进行回环检测。本发明通过使用滤波思想进行多传感器数据的融合来进一步优化定位信息，将imu与单目视觉里程计结合起来，克服相互的缺点，保留各自的优点，能够克服定位过程中单目视觉在动态物体影响下定位效果和鲁棒性差的问题，同时解决imu累积误差的问题和降低在复杂噪声环境数据融合的误差。

本发明的有益效果为：本发明的定位方法使用滤波思想进行多传感器数据的融合来进一步优化定位信息，将imu与单目视觉里程计结合起来，克服相互的缺点，保留各自的优点，提高了单目视觉里程计定位在动态环境下视觉定位的鲁棒性，解决了imu累计误差不能消除的问题；同时，通过采用基于最大相关熵卡尔曼滤波算法对imu数据和视觉里程计数据进行融合，提高了定位系统在复杂噪声环境下工作的定位精度。在地图纹理变化迅速，出现场景单一，图像特征点少的情况下，能够有效地调整特征点处理机制，提高特征点匹配机制的准确。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。