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

权利要求书

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是与相机焦距和相机中心位置相关的参数。