[发明专利]基于深度相机和IMU组合的室内移动终端定位方法

权利要求书

1.一种基于深度相机和IMU组合的室内移动终端定位方法，其特征在于，移动终端利用深度相机和IMU分别采集数据，使用特定规则提取每张灰度图像中的特征点，利用深度图像求解相机位姿初始化所需的尺度因子；该定位方法的步骤包括如下：

步骤1，将传感器采集的数据传入移动终端：

(1a)利用移动终端上设置的深度相机实时采集彩色图像和深度图像，每帧彩色图像和深度图像的大小均为640×480像素，采集频率均为每秒钟30帧，将彩色图像和深度图像按时间顺序传入移动终端；

(1b)利用移动终端上设置的IMU每秒钟采集200帧加速度数据，每秒钟采集63帧角速度数据，将加速度数据和角速度数据按时间顺序传入移动终端；

步骤2，利用OpenCV的cv::createCLAHE()函数对图像进行均衡化处理；

步骤3，选取稳定的特征点：

(3a)以每帧灰度图像的左上角像素点为坐标原点建立二维像素坐标系，原点的像素坐标为(0,0)，灰度图像水平向右方向为x坐标轴的正方向，灰度图像竖直向下方向为y坐标轴的正方向，灰度图像中右上角的像素坐标为(639,0)；

(3b)在每帧灰度图像中选取左上像素点的像素坐标为(20,15)，右上像素点的像素坐标为(620,15)，左下像素点的像素坐标为(620,465)，右下像素点的像素坐标为(20,465)的四个像素点，按区域顺序依次连接四个像素点，得到待划分网格的矩形区域；

(3c)在左上像素点和左下像素点之间的连线上选取(20,165)、(20,315)两个像素坐标，将该连线均分为3份，在左上像素点和右上像素点之间的连线上选取(170,15)、(320,15)和(470,15)三个像素坐标，将该连线均分为4份，将选取的像素坐标作为划分矩形网格的参考点，将整个矩形区域均匀划分为3×4个大小相同的网格；

(3d)利用cv::goodFeaturesToTrack()函数，提取第一帧灰度图像中的所有特征点；

(3e)通过光流跟踪确定第一帧灰度图像中的每个特征点在第二帧灰度图像中的位置，将第一帧灰度图像中的每个特征点在其余图像帧中被确定相应位置的次数按从大到小排序，若同一网格中的特征点确定相应位置的次数相同，则按特征点提取的先后顺序排序，将每个网格中前7个特征点作为稳定的特征点，共选取7×12个稳定的特征点；

(3f)采用与步骤(3e)相同的方法，确定前一帧灰度图像中的每个特征点在后一帧灰度图像中的位置，并确定每个网格中特征点的排序，若出现特征点跟踪丢失的情况，在该特征点所属的网格中提取新的特征点，补全该网格中特征点的数目至7；

步骤4，对IMU数据进行积分预处理：

步骤5，确定滑动窗口内深度相机带尺度因子的相对位姿：

(5a)从含有稳定特征点的灰度图像中，按时间顺序依次选取11帧的灰度图像，建立滑动窗口，对滑动窗口内的11帧图像进行纯视觉三维重建；

(5b)在滑动窗口内的前10帧灰度图像中，找出一帧灰度图像作为枢纽帧图像，枢纽帧图像与第11帧灰度图像之间的视差大于20像素，且两帧图像之间至少存在25对共视点，其中，共视点表示在至少两帧灰度图像中均存在的特征点，视差表示两帧灰度图像中所有共视点的平均像素差；

(5c)利用cv::triangulatePoints()和cv::solvePnP()函数，计算滑动窗口内采集枢纽帧图像时与采集其他帧灰度图像时深度相机的相对位姿；

步骤6，计算尺度因子的平均值：

(6a)在滑动窗口内，找出枢纽帧图像与枢纽帧后一帧灰度图像中的所有共视点，得到每个共视点位置坐标与采集该枢纽帧图像时深度相机位置坐标的距离d_i；

(6b)从枢纽帧图像对应的深度图像中获取每个共视点中心像素的深度值D_i；

(6c)按照下式，计算滑动窗口内深度相机尺度因子的平均值s：

其中，n表示枢纽帧灰度图像与枢纽帧后一帧灰度图像中的共视点的总数，Σ表示求和操作，i表示枢纽帧灰度图像中共视点的序号；

步骤7，求解深度相机的速度向量和重力向量：

利用线性方程，计算滑动窗口内采集相邻两帧灰度图像时深度相机的速度向量和采集枢纽帧灰度图像时深度相机的重力向量，将深度相机的位姿调整至世界坐标系下；

步骤8，计算深度相机的状态增量：

(8a)通过紧耦合的非线性优化公式，计算视觉重投影残差和IMU测量残差中的待优化变量，得到滑动窗口内采集每帧灰度图像时深度相机的最优位姿；

(8b)利用滑动窗口内优化后的深度相机位姿，得到深度相机的状态增量，确定深度相机的实时位姿；

步骤9，优化深度相机的位姿：

(9a)利用Fast角点检测算法，在深度相机采集的每帧灰度图像中提取至少300个图像特征点，将每帧灰度图像中所有特征点的描述子信息储存在数据库中；

(9b)利用基于DboW2的回环检测算法，计算当前图像帧与数据库中每一帧灰度图像的相似性得分，通过相似性得分判断当前帧图像是否存在回环，得分若大于或等于0.6，则执行步骤(9c)，否则，执行步骤10；

(9c)对存在回环的两帧灰度图像间的每帧图像所对应的深度相机位姿进行非线性优化，输出每帧优化后的深度相机位姿，利用移动终端与深度相机之间的相对位姿，得到每帧优化后的移动终端位姿；

步骤10，输出当前深度相机的位姿，利用移动终端与深度相机之间的相对位姿，得到当前移动终端的位姿。