基于深度相机和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，输出当前深度相机的位姿，利用移动终端与深度相机之间的相对位姿，得到当前移动终端的位姿。2.根据权利要求1所述的基于深度相机和imu组合的室内移动终端定位方法，其特征在于，步骤(2)中所述均衡化处理是指，将每帧彩色图像格式转换成能够被opencv识别的灰度图像格式，再利用createclahe()函数对每帧灰度图像进行均衡化处理。3.根据权利要求1所述的基于深度相机和imu组合的室内移动终端定位方法，其特征在于，步骤(4)中所述对imu数据进行积分预处理的步骤如下：第一步，以移动终端上的初始位置为原点，移动终端向前延伸的方向为x轴正方向，移动终端向左延伸的方向为y轴正方向，移动终端向上延伸的方向为z轴正方向建立一个三维世界坐标系；第二步，在世界坐标系下，利用基于中值积分法，根据imu当前帧和后一帧的加速度和角速度数据及当前帧imu的位置、速度和姿态，获取后一帧imu的位置、速度和姿态；第三步，利用基于中值法的imu预积分公式，根据imu当前帧和后一帧的加速度和角速
度数据，得到相邻两帧之间imu的位置增量、速度增量和姿态增量。4.根据权利要求1所述的基于深度相机和imu组合的室内移动终端定位方法，其特征在于，步骤(5c)中所述利用cv::triangulatepoints()和cv::solvepnp()函数，计算滑动窗口内采集枢纽帧图像时与采集其他帧灰度图像时深度相机的相对位姿的步骤如下：第一步，以采集枢纽帧灰度图像时的深度相机位置为原点，深度相机向前延伸的方向为x轴正方向，深度相机向左延伸的方向为y轴正方向，深度相机向上延伸的方向为z轴正方向建立一个三维相机坐标系；第二步，在相机坐标系下，利用五点法，根据枢纽帧与第11帧灰度图像中至少5对的共视点中心像素坐标，确定采集枢纽帧与第11帧灰度图像时深度相机的相对位姿，相对位姿由相对旋转矩阵和带尺度因子的相对位移向量组成；第三步，使用opencv的cv::triangulatepoints()函数，对枢纽帧灰度图像与第11帧灰度图像进行三角化处理，得到两帧图像间每个共视点的位置坐标；第四步，使用opencv的cv::solvepnp()函数，依次对从第l 1帧到第10帧灰度图像中的每帧图像进行pnp求解，得到采集第l帧灰度图像时与采集从第l 1帧到第10帧灰度图像时深度相机的相对位姿，其中，第l帧灰度图像为枢纽帧灰度图像，第l 1帧灰度图像为枢纽帧后的第一帧灰度图像；第五步，采用与第四步相同的办法，得到采集第l帧与第l
‑
1帧灰度图像时深度相机的相对位姿，采用与第三步相同的办法，得到第l帧与第l
‑
1帧灰度图像间每个共视点的位置坐标，其中，第l
‑
1帧灰度图像为枢纽帧前的第一帧灰度图像；第六步，采用与第五步相同的办法，得到采集枢纽帧灰度图像时与采集枢纽帧前的每帧灰度图像时深度相机的相对位姿。5.根据权利要求1所述的基于深度相机和imu组合的室内移动终端定位方法，其特征在于，步骤(7)中所述线性方程如下：其中，i表示3
×
3的单位矩阵，δt表示采集相邻两帧灰度图像的时间间隔，表示采集枢纽帧灰度图像时深度相机与第k帧灰度图像时深度相机之间相对姿态的旋转矩阵，v
k
表示采集第k帧灰度图像时深度相机的速度向量，v
k 1
表示采集第k 1帧灰度图像时深度相机的速度向量，表示在采集相邻两帧灰度图像的时间间隔内，对所有imu加速度数据进行预积分得到的位置增量，p表示深度相机与imu之间的平移外参，表示采集第k 1帧灰度图像时深度相机与采集第k帧灰度图像时深度相机之间相对姿态的旋转矩阵，p
k 1
表示采集第k 1帧灰度图像时深度相机的位置，p
k
表示采集第k帧灰度图像时深度相机的位置，g
l
表示采集枢纽帧灰度图像时深度相机的重力向量，表示在采集相邻两帧灰度图像的时间间隔内，对所有imu加速度数据进行预积分得到的速度增量。6.根据权利要求1所述的基于深度相机和imu组合的室内移动终端定位方法，其特征在于，步骤(7)中所述将深度相机的位姿调整至世界坐标系的步骤如下：
第一步，相机坐标系下，将采集枢纽帧灰度图像时深度相机的重力向量旋转至世界坐标系中的z轴，重力向量的方向与z轴负方向一致，得到从相机坐标系到世界坐标系的旋转矩阵；第二步，将该旋转矩阵将滑动窗口内深度相机的位姿调整至世界坐标系下，得到深度相机在世界坐标系下的初始位姿。7.根据权利要求1所述的基于深度相机和imu组合的室内移动终端定位方法，其特征在于，步骤(8a)中所述紧耦合的非线性优化公式如下：其中，min{
·
}表示取最小值操作，x表示滑动窗口内的所有待优化变量，b表示滑动窗口内所有imu测量残差矩阵的二范数的集合，||
·
||2表示取二范数操作，表示采集第k帧灰度图像时imu的姿态与世界坐标系坐标轴之间的旋转矩阵，分别表示采集第k帧灰度图像时、采集第k 1帧灰度图像时imu在世界坐标系下的位置，表示采集第k帧灰度图像时imu在世界坐标系下的速度，g
w
表示世界坐标系下的重力向量，表示在采集相邻两帧灰度图像的时间间隔内，对所有imu加速度数据进行预积分得到的姿态增量的四元数形式，分别表示采集第k帧灰度图像时、采集第k 1帧灰度图像时imu在世界坐标系下的姿态的四元数形式，[
·
]
e
表示取四元数虚部操作，表示采集第k 1帧灰度图像时imu在世界坐标系下的速度，表示在采集相邻两帧灰度图像的时间间隔内，对所有imu加速度数据进行预积分得到的速度增量，分别表示采集第k帧灰度图像时、采集第k 1帧灰度图像时的加速度偏置，分别表示采集第k帧灰度图像时、采集第k 1帧灰度图像时的角速度偏置，c表示滑动窗口内所有视觉重投影残差矩阵的二范数的集合，表示第l个共视点相对于采集第j帧灰度图像时深度相机的位置坐标的第一个值、第二个值、第三个值，表示滑动窗口内第l个共视点通过光流跟踪，在第j帧灰度图像中找到该共视点对应的像素坐标的第一个值、第二个值。

技术总结
本发明公开了一种基于深度相机和IMU组合的室内移动终端定位方法，其实现步骤为：通过纯视觉三维重建和深度图像数据，确定滑动窗口内采集每帧灰度图像时深度相机在相机坐标系下的位姿；通过重力向量，确定深度相机在世界坐标系下的初始位姿；通过紧耦合的非线性优化，计算状态增量，确定深度相机的实时位姿；当检测到回环时，优化深度相机的位姿；通过移动终端与深度相机之间的相对位姿，得到移动终端的实时位姿。本发明在没有先验地图的室内环境下，仅依靠自身搭载的传感器实现实时定位，提高了定位系统的便携性，节省了先验地图所占的存储空间。存储空间。存储空间。


技术研发人员：张军禹 仇原鹰 王宇纬 李静 张剑宇 程培涛 段学超
受保护的技术使用者：西安电子科技大学
技术研发日：2021.08.09
技术公布日：2021/11/4