CN111156998B - 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 - Google Patents

Abstract

本发明公开了一种基于RGB‑D相机与IMU信息融合的移动机器人定位方法，包括下述步骤：(1)建立针孔相机模型；(2)建立IMU测量模型；(3)结构光相机深度计算与基于特征点匹配的位姿变换计算；(4)IMU预积分姿态解算以及IMU坐标系与相机坐标系的转换；(5)RGB‑D数据与IMU数据融合过程以及相机位姿优化过程，最终获得精准的定位姿态。本发明使用RGB‑D相机与IMU的传感器组合的定位，很好的利用了IMU在短时间的快速运动具有较好的状态估计的特性，而相机具有静态条件下基本不漂移的特性，从而使定位系统具有较好的静态特性和动态特性，使机器人能适应低速运动场合和高速运动场合。

Description

一种基于RGB-D相机与IMU信息融合的移动机器人定位方法

技术领域

本发明属于智能移动机器人感知领域，具体涉及一种基于RGB-D相机与IMU信息融合的定位方法。

背景技术

近几十年，随着科技的不断进步，生产生活中自动化智能化的程度不断提升，移动机器人由于其本身具有能独立完成某项任务的能力，今后必然将在人类生产生活中发挥巨大的作用。例如太空探索，海洋探测，工业矿井探测等这些由人类去完成危险程度很大甚至无法完成的任务，有了机器人的参与都将更好更快更安全地去完成这些危险任务。智能移动机器人的自主移动需要不断获取自身的位姿信息，为移动机器人的导航与路径规划提供相应的定位信息。

目前可知的常用于移动机器人定位的传感器包括GPS系统、激光雷达、视觉传感器、惯性传感器等。使用GPS差分定位的系统可以提供较为精准的全球定位信息，但是在室内环境以及室外存在遮挡的地方由于无法接受GPS卫星信号而导致定位失败。其次，即使使用差分GPS定位，其定位精度也仅有分米级别。使用多线激光雷达的定位系统通过获取点云数据并使用ICP等算法可以获得精准的位姿，但是多线激光雷达的价格较为昂贵且体积较大。使用惯性传感器也可以用于机器人位姿解算，其中高精度的惯性测量单元通过积分可以得到较为精准的位姿，但是成本较高，而廉价的MENS惯性测量单元由于数据漂移较为严重，直接积分解算位姿很容易导致位姿的发散，很难用于机器人定位。使用立体视觉的方法，如双目相机或者RGB-D相机利用视觉SLAM算法可以获得精准的定位信息，视觉定位的方法静态性能较好，但是当移动机器人快速移动时易。

发明内容

本发明的主要目的在于克服现有技术的缺点与不足，提供一种基于RGB-D相机与IMU信息融合的定位方法，根据该定位方法可计算出较为精准机器人的位置与姿态，且该定位系统具有较强的鲁棒性。

为了达到上述目的，本发明采用以下技术方案：

一种基于RGB-D相机与IMU信息融合的移动机器人定位方法，包括下述步骤：

(1)建立针孔相机模型，所述针孔相机模型用于明确实际采集的二维图像点与对应的空间中三维点的相机坐标的数学关系；

(2)建立IMU测量模型，确定IMU测量数据与状态参数速度、位姿、偏差的关系；

(3)基于针孔相机模型可知二维相机坐标计算三维相机坐标的数学关系，基于结构光相机深度计算与基于特征点匹配的位姿变换计算，利用相机求解空间点深度信息的原理，从而恢复出空间点的三维信息，对于相机不同时刻的图像帧，假设在已知图像帧间多组匹配的三维空间点的情况下通过匹配点的信息求解两帧图像间的坐标变换K，t，从而迭代求解机器人的定位信息；

(4)IMU预积分姿态解算以及IMU坐标系与相机坐标系的转换，IMU通过本身的测量计算出IMU中心在世界坐标的位姿，通过已知的IMU与相机的坐标变换，可以求解出初步的相机位姿，并以此来预测相机位姿，加速特征点匹配；

(5)通过特征点匹配可求解出机器人位姿，由IMU测量解算出的IMU位姿可以加速特征点匹配，在匹配的基础上构建重投影误差进一步优化机器人位姿，最终获得精准的定位姿态。

作为优选的技术方案，步骤(1)中，使用MyntEye-D相机，根据其视角参数与畸变特性，可将其建模成针孔模型，以下简称针孔相机模型，针孔相机模型可表述为，在相机的视野范围内，三维空间中的一点P，其反射光线必通过相机光心，并在二维相机平面上成像为点P′，且P与P′符合如下数学转换关系：

其中P′表示像素坐标，

是像素坐标的齐次坐标表示形式；P是P′对应的三维空间点，

是在当前相机坐标系下的相机坐标，K是相机内参矩阵，K中的f_x,f_y,c_x,c_y是与相机焦距和相机中心位置相关的参数。

作为优选的技术方案，步骤(2)中，所述IMU测量模型如下：

其中上标g表示陀螺仪，上标a表示加速度计，上标w表示在世界坐标系中，上标b表示IMU体坐标系，故有

表示为加速度计陀螺仪的测量值，a^b,w^b表示为加速度计陀螺仪的真实值，g^w表示重力向量在世界坐标系中的表示，q_bw表示IMU位姿旋转变换的四元数表示，n^g,n^a表示陀螺仪加速度计测量的高斯白噪声，b^g,b^a表示陀螺仪加速度计的偏差，它们被建模成随机游走，其对时间的导数服从正态分布。

作为优选的技术方案，步骤(3)中，结构光相机深度计算与基于特征点匹配相机坐标变换的计算方法具体为：

所述对于结构光深度相机得到深度信息的原理公式为：

其中f为相机焦距，u_L,u_R为图像点在左右相机平面上的横轴的成像坐标轴，b为左右相机光心的距离即基线大小，z为待求点深度大小，即到相机平面的距离，结构相机中深度的计算实际由相机自身计算并输出结果，故深度z为已知信息；

所述基于特征点匹配的位姿变换计算具体为：

其中P表示空间点的世界坐标，p₁,p₂表示空间点投影到两个平面上的像素坐标，即匹配的特征点，z₁,z₂表示空间点到两个平面上的深度，K表示相机内参矩阵，R,t表示图像帧之间的旋转变换和平移变换，即通过多组匹配点求解图像帧间的旋转R和平移t。

作为优选的技术方案，步骤(4)中，由IMU测量值解算IMU世界坐标的方法，具体为：

其中

为i,j时刻相对世界坐标系的位置，V_j ^w,V_i ^w为i,j时刻相对世界坐标系的速度，

为i,j时刻相对世界坐标系的角度的四元数表示，中使用

和

为i时刻的陀螺仪和加速度计的真实值，Δt为两相邻IMU数据间的时间间隔。

作为优选的技术方案，步骤(5)中，相机重投影误差e_i的最终定义为：

其中

为当前帧像素坐标的齐次坐标表示，[X_i,Y_i,Z_i,1]^T为三维空间点世界坐标的齐次坐标表示方法，K为相机内参，

为最终需要优化的当前帧的世界坐标变化，通过对残差构造最小二乘并对

进行优化，其代价函数ε^*为：

最终使用Levenberg-Marquadt算法对位姿状态

进行优化。

本发明与现有技术相比，具有如下优点和有益效果：

1、本发明采用松耦合方式结合RGB-D图像数据与IMU测量数据，即IMU通过积分得到6自由度位姿，RGB-D通过特征点匹配然后通过位姿变换计算出位姿，在此过程中使用IMU计算出位姿预测相机当前时刻地位姿，通过反向投影，加速特征点匹配。匹配之后通过BA优化即可得到当前时刻准确的位姿，下一次IMU积分的起始位姿都基于当前时刻相机优化的位姿。因此这样很好的利用了IMU在短时间的快速运动具有较好的状态估计的特性，而相机具有静态条件下基本不漂移的特性，从而使定位系统具有较好的静态特性和动态特性，使机器人能适应低速运动场合和高速运动场合。

2、本发明在传感器融合的算法过程中，使用IMU数据预测当前状态的相机位姿，可以加速相机特征点的匹配，使算法更加具有实时性。

3.、本发明在实际应用场景中使用结构光深度惯导相机，可使算法能运行在室内外等更加复杂的场景。

附图说明

图1为IMU信息与RGB-D信息融合流程图。

图2为针孔相机投影模型示意图。

图3为像素坐标表示示意图。

图4为双目结构光相机深度计算原理示意图。

图5为相机信息与IMU信息融合示意图。

图6为所使用的相机IMU坐标示意图。

图7为前后图像对空间某一特征点匹配示意图。

图8为相机重投影误差示意图。

图9为MyntEye-D相机实际场景定位测试结果。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例

如图1所示，本实施例基于RGB-D相机与IMU融合的移动机器人定位的方法，具体实时步骤如下所述：

步骤(1)：建立针孔相机模型。

考虑如图2所示的针孔相机模型，其中

平面为成像平面，

平面为相机平面。其中xyz为相机坐标，相机坐标中的z轴垂直于相机平面，xy轴分别平行于相机的两个边框并与z轴构成右手坐标系。平面坐标系x′y′为与xy轴分别平行。o为相机光心，o′为通过光心与z轴平行的直线与成像平面α的交点，oo′为焦距，其大小为f。假设三维空间中的一点P在相机坐标系下的坐标为(X,Y,Z)，该点投影到成像平面上为点P′,点P′在x′y′下的坐标为(X′,Y′),由小孔成像中的相似三角形，可知：

空间中的点投影到像素平面上的成像点在读入处理器中用像素坐标表示。如图3所示，其中以图2中像素成像平面的点M为原点(0,0)，坐标轴的方向与相机坐标的xy轴平行。由相机成像的原理可知：

其中mn代表像素坐标在实际成像平面上与成像点物理尺度的比例，一般情况下αβ是相等的，不然成像会有尺寸畸变。将小孔成像比例式带入其中，即有：

令f_x＝αf,f_y＝βf,即有：

将像素坐标表示成齐次坐标的形式，与相机坐标的关系式为：

这里的3×3的矩阵K被称为相机的内参矩阵，在相机出厂后便固定不变，可以通过一定的标定手段获得，如张定友棋盘格标定方法。

步骤(2)：建立IMU测量模型。

IMU即惯性测量单元，主要由两部分组成，一部分为陀螺仪，能测量三个坐标方向的角速度，一部分叫加速度计，可测量三个方向的加速度。在IMU的计算中，我们一般将东北天坐标系设置为世界坐标系，将机器人的起点设置为世界坐标系的原点，即IMU的测量模型如下所示：

其中上标g表示陀螺仪，上标a表示加速度计。上标w表示在世界坐标系中，上标b表示IMU体坐标系。故有

表示为加速度计陀螺仪的测量值。a^b,w^b表示为加速度计陀螺仪的真实值。g^w表示重力向量在世界坐标系中的表示，q_bw表示IMU位姿旋转变换的四元数表示。n^g,n^a表示陀螺仪加速度计测量的高斯白噪声。b^g,b^a表示陀螺仪加速度计的偏差，它们被建模成随机游走，即其对时间的导数服从正态分布。对于一个IMU系统，其高斯白噪声和随机游走中的相关参数可以通过相关的方法标定出来，例如Allen方差标定法。

步骤(3)：描述结构光相机深度计算与基于特征点匹配的位姿变换计算。

本发明中使用的是双目结构光深度相机，其深度计算原理和普通双目一样都是通过三角化计算深度。结构光的作用是通过特殊编码方式将特定结构的光斑投射到物体上，可加速双目特征点的匹配，在纹理不明显的场景也可通过结构光的光斑来进行匹配。具体的计算方式如图4所示。双目相机的左右相机的相机坐标轴一般重合，仅横轴上相差基线长度。假设一空间点P在双目相机的左目和右目上成像为点P_L和P_R。O_L和O_R为左右光圈中心，f为焦距，u_L,u_R为左右相机平面上的横轴的成像坐标轴，b为左右相机光心的距离即基线大小，该点深度大小为z，即到相机平面的距离。由相关三角形关系可得：

即有深度z为：

因此对空间一个点，我们可以通过深度相机获取其相机坐标。对于运动中的相机我们可以对前后两帧中同一相机中的特征点进行匹配，通过多组相机匹配点，我们可以利用P3P,Epnp等算法，计算出两帧图像之间的位姿变换。如图7所示，其中I₂为当前图像帧，I₁为上一图像帧，o₁,o₂为对应图像的相机光心，对于空间中一点P,其在图像I₁,I₂的像素坐标为p₁,p₂,设在第一帧的坐标系下点P的坐标表示为(X,Y,Z),对空间中的两个相机位姿的关系可以由一欧式变换表示，欧式变换包含旋转变换R和平移变换t,即有如下等式成立：

z₁,z₂为空间点P在图像I₁,I₂上的深度大小，p₁,p₂均采用其次坐标表示，K为相机内参矩阵，因此通过多组匹配点即可以计算出相机I₁,I₂之间的位姿变换，通过位姿变换间的矩阵乘法运算，即可以计算出每一帧相对第一帧之间的位姿变换，即定义下的世界坐标的位姿。

步骤(4)：描述IMU预积分姿态解算以及IMU坐标系与相机坐标系转换。

由上述步骤我们可知通过匹配特征点计算位姿变换可以迭代计算出当前相机所在的世界坐标，同样由IMU积分同样可以计算出六自由度的位姿。简单地思考，角速度积分可以计算出角度，加速度积分可以计算出速度，速度积分可以得到距离。在IMU计算中我们以东北天坐标系作为世界坐标系。东北天坐标系中的z轴指向天空，与地心方向或者重力方向相反，x轴方向平行于纬线，y轴方向平行于经线。这里定义t时刻IMU体坐标相对世界坐标系中的速度，位置和角度为

在这里旋转

使用四元数进行表示，已知速度，位置和角度对时间的导数为：

其中

是t时刻相对世界坐标系的加速度，

是t时刻IMU坐标系角速度的值。

为四元数的乘法运算符。其中

根据定义可知为：

其中

是t时刻IMU坐标系加速度的值，g^w为世界坐标系中的重力向量，由导数关系可知，通过对i时刻到j时刻间的测量值积分，可得到j相对世界坐标的位置，速度和角度，如下所示：

其中Δt为i,j之间的时间间隔，

为i,j时刻相对世界坐标系的位置，V_i ^w,V_j ^w为i,j时刻相对世界坐标系的速度，

为i,j时刻相对世界坐标系的角度的四元数表示，本发明中使用欧拉积分进行离散化积分运算，具体表达式如下所示：

其中

和

为i时刻的真实值，由IMU测量方程以及欧拉积分定义可得：

其中

为i时刻的测量值，

为i时刻的bias。那么这样便可以通过IMU的积分计算IMU坐标系相对世界坐标系的位姿，若将IMU测量的位姿与相机测量的位姿进行融合，那么就必须计算相机坐标系与IMU坐标系的位姿变换，本发明中所使用MyntEye-D相机的坐标系定义如图6所示，在位姿运算中我们一般使用变换矩阵进行表示，设相机坐标系到IMU坐标系的旋转变换为R_bc，平移变换为t_bc,那么变换矩阵可定义为T_bc:

一般情况下，IMU与相机变换参数T_bc通过提前标定获得。

步骤(5)：描述RGB-D数据与IMU数据融合过程以及相机位姿优化过程。

由上述我们可知IMU和RGB-D相机都可以通过各自的方式计算出自身坐标系与世界坐标系的位姿变换，那么怎么融合这两部分的计算呢。本发明使用一种松耦合的方式，充分考虑到IMU测量短期内具有快速响应并且精准度可接受的这一特点，将IMU短期内的测量用于预测相机当前时刻的位姿，这样便可以将上一时刻的位姿通过位姿变换投影到当前图像帧上，特征点仅需要在投影点的领域内及进行特征点描述子的匹配，当计算值小于一定的阈值便可认为为匹配点，这样相对于暴力匹配算法或者K近邻算法具有更好的时间复杂度和空间复杂度，具体运行图示如图5所示：其中I_last为上一帧图像的相机位姿，为已经优化的已知的位姿；I_now为当前帧相机的真实位姿，是未知的待求得相机位姿；I_predict是在I_last的基础上结合IMU的测量值，并进行相机IMU坐标变换估计的相机位姿。由于相机的采集频率远低于IMU数据的频率，因此在两帧图像之间存在多个IMU数据，图中U₁～U_n即为IMU测量数据，本发明所使用的深度惯导相机，相机频率为30HZ，IMU频率为200HZ，因此两帧图像之间存在6～7个IMU数据值。在I_last图像上存在p₁～p_n共n个特征点，I_now存在p′₁～p′_n共n个特征点，I_last通过预测的位姿I_predict将特征点p₁～p_n反向投影至I_now上，并在阴影圆圈的领域内进行特征点描述子匹配，本发明所用的特征为ORB特征，ORB特征为二进制描述子，所用的距离度量为汉明距离，汉明距离即二进制描述子间不同比特的数目，可使用异或运算符加速运算。

对于实际的融合过程，设I_last图像帧的坐标变换为

为已知的数据。I_last到I_now之间有n个IMU数据为{U₁,U₂…U_n}。预测的相机I_predict的位姿为

其求解过程可以通过下面3个步骤计算出来：

通过匹配的特征点坐标变换之后便可以求解出当前相机较为精准的位姿

为了得到更为精准的位姿，在计算得到

之后会对所有匹配的特征点，执行一次BundleAdjustment优化，BundleAdjustment优化的过程即为相机重投影误差最小化的过程，具体示意图如图8所示：其中I_last为上一相机帧，其位姿变换

在上一次优化中已经计算出来；I_now为当前相机位姿，在前期的特征点匹配已经计算出初步的相机位姿值

通过将I_last上的特征点

投影到I_now上得到投影点

这样得到的投影点就会与原有通过匹配得到的特征点

之间就构成了误差项e。在这里定义空间中三维坐标点P_i,其在I_last上的投影点为

点反向投影至I_now上得到点

I_now上和

匹配的坐标点为

设投影点

的图像坐标为

匹配点的图像坐标

为

故相机重投影误差定义为：

令三维空间P_i特征点坐标为[X_i,Y_i,Z_i],由针孔相机模型可知

的齐次坐标为：

综上所述，相机重投影误差e_i的最终定义为：

其中

自动转换成其次坐标进行预算。最终通过对所有残差构造最小二乘并对

进行优化，具体的代价函数ε^*为：

本发明中使用Levenberg-Marquadt算法对位姿状态

进行优化。最终能获得比较精准的定位位姿。

(6)使用MyntEye-D相机在移动机器人平台上的算法测试。

通过一系列对深度惯导相机的标定操作，得到如下相机参数：

然后在TurtleBoot2移动机器人平台上进行测试，首先将MyntEye-D相机固定在机器人中心，MyntEye-D相机中已经集成了RGB-D模块与IMU模块。接着将TurtleBoot2的串口控制线连接至笔记本USB口上，MyntEye-D也与笔记本USB口连接，其中TurtleBoot2移动机器人平台由自身的电池供电，MyntEye-D相机通过USB由笔记本供电。最终笔记本上开启TurtleBoot2遥控程序与MyntEye-D相机定位程序使移动机器人在实验室2.5m*7m的矩形过道上在移动机器人上运行一周同时将定位算法计算得到的位姿保存成TUM数据集的格式(三维坐标+四元数)，最终将保存的数据在MatLab上绘制出XZ二维的平面坐标图，如图9所示。可以看出实际的轨迹基本符合实际场景的尺度，从而证明算法的可行性。实验结果表明无论在室内或室外环境中，均具有较高的定位精度和较强的鲁棒性。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (4)

1.一种基于RGB-D相机与IMU信息融合的移动机器人定位方法，其特征在于，包括下述步骤：

(1)建立针孔相机模型，所述针孔相机模型用于明确实际采集的二维图像点与对应的空间中三维点的相机坐标的数学关系；

(2)建立IMU测量模型，确定IMU测量数据与状态参数速度、位姿、偏差的关系；

(3)基于针孔相机模型计算二维相机坐标，建立二维相机坐标与三维相机坐标的数学关系，基于结构光相机深度计算与基于特征点匹配的位姿变换计算，利用相机求解空间点深度信息的原理，从而恢复出空间点的三维信息，对于相机不同时刻的图像帧，假设在已知图像帧间多组匹配的三维空间点的情况下通过匹配点的信息求解两帧图像间的坐标变换K，t，从而迭代求解机器人的定位信息；

步骤(3)中，结构光相机深度计算与基于特征点匹配相机坐标变换的计算方法具体为：

所述对于结构光深度相机得到深度信息的原理公式为：

其中f为相机焦距，u_L,u_R为图像点在左右相机平面上的横轴的成像坐标轴，b为左右相机光心的距离即基线大小，z为待求点深度大小，即到相机平面的距离，结构相机中深度的计算实际由相机自身计算并输出结果，故深度z为已知信息；

所述基于特征点匹配的位姿变换计算具体为：

其中P表示空间点的世界坐标，p₁,p₂表示空间点投影到两个平面上的像素坐标，即匹配的特征点，z₁,z₂表示空间点到两个平面上的深度，K表示相机内参矩阵，R,t表示图像帧之间的旋转变换和平移变换，即通过多组匹配点求解图像帧间的旋转R和平移t；

(4)IMU预积分姿态解算以及IMU坐标系与相机坐标系的转换，IMU通过本身的测量计算出IMU中心在世界坐标的位姿，通过已知的IMU与相机的坐标变换，求解出初步的相机位姿，并以此来预测相机位姿，加速特征点匹配；

(5)通过特征点匹配求解出机器人位姿，由IMU测量解算出的IMU位姿加速特征点匹配，在匹配的基础上构建重投影误差进一步优化机器人位姿，最终获得精准的定位姿态；

步骤(5)中，相机重投影误差e_i的最终定义为：

其中

为当前帧像素坐标的齐次坐标表示，[X_i，Y_i，Z_i，1]^T为三维空间点世界坐标的齐次坐标表示方法，K为相机内参，

为最终需要优化的当前帧的世界坐标变化，通过对残差构造最小二乘并对

进行优化，其代价函数ε^*为：

最终使用Levenberg-Marquadt算法对位姿状态

进行优化。

2.根据权利要求1所述基于RGB-D相机与IMU信息融合的移动机器人定位方法，其特征在于，步骤(1)中，使用MyntEye-D相机，根据其视角参数与畸变特性，将其建模成针孔模型，以下简称针孔相机模型，针孔相机模型表述为，在相机的视野范围内，三维空间中的一点P，其反射光线必通过相机光心，并在二维相机平面上成像为点P′，且P与P′符合如下数学转换关系：

其中P′表示像素坐标，

是像素坐标的齐次坐标表示形式；P是P′对应的三维空间点，

是在当前相机坐标系下的相机坐标，K是相机内参矩阵，K中的f_x，f_y，c_x，c_y是与相机焦距和相机中心位置相关的参数。

3.根据权利要求1所述基于RGB-D相机与IMU信息融合的移动机器人定位方法，其特征在于，步骤(2)中，所述IMU测量模型如下：

其中上标g表示陀螺仪，上标a表示加速度计，上标w表示在世界坐标系中，上标b表示IMU体坐标系，故有

表示为加速度计陀螺仪的测量值，a^b，w^b表示为加速度计陀螺仪的真实值，g^w表示重力向量在世界坐标系中的表示，q_bw表示IMU位姿旋转变换的四元数表示，n^g，n^a表示陀螺仪加速度计测量的高斯白噪声，b^g，b^a表示陀螺仪加速度计的偏差，它们被建模成随机游走，其对时间的导数服从正态分布。

4.根据权利要求1所述基于RGB-D相机与IMU信息融合的移动机器人定位方法，其特征在于，步骤(4)中，由IMU测量值解算IMU世界坐标的方法，具体为：

其中

为i,j时刻相对世界坐标系的位置，V_j ^w,V_i ^w为i,j时刻相对世界坐标系的速度，

为i,j时刻相对世界坐标系的角度的四元数表示，中使用

和

为i时刻的陀螺仪和加速度计的真实值，Δt为两相邻IMU数据间的时间间隔。

Images