基于深度相机和IMU组合的室内移动终端定位方法与流程

基于深度相机和imu组合的室内移动终端定位方法
技术领域
1.本发明属于通信技术领域，更进一步涉及同步定位与建图技术领域中的一种基于深度相机和惯性测量单元imu(inertial measurement unit)组合的室内移动终端定位方法。本发明可用于对室内环境下自主移动终端的定位及跟踪，实时显示终端的位姿和运动轨迹。


背景技术：

2.同步定位与建图技术(simultaneous localization and mapping，slam)是指移动终端仅依靠自身传感器获取实时位姿信息的技术。其广泛应用于虚拟现实、自动驾驶、位置服务等与人类生活息息相关的新兴科技领域。目前，针对室外移动终端的定位技术(如gps定位)已经非常成熟，定位技术市场需求的重心逐渐转移到gps无法正常使用的室内环境中。统计表明，室内自主移动终端数量在近几年呈现爆发式增长，因此针对室内环境下定位服务的前景非常广阔。但是，就目前室内定位技术的发展状况来看，由于室内通常存在活动空间狭小、周围环境复杂等问题，且移动终端所搭载的传感器具有价格、体积、质量等制约因素，无法满足人们对室内定位的要求，在一定程度上影响了室内定位技术的进一步发展。
3.哈尔滨工业大学在其申请的专利文献“基于logo的双目视觉室内定位方法”(申请号：201610546034.x，申请公布号：cn106228538a)中提出了一种基于logo的双目视觉室内定位方法，用于室内移动目标的定位。该方法具体流程如下：首先，建立logo图像采集的视觉地图数据库，标定双目相机中左、右摄像头的内参数和外参数。然后，使用双目相机拍摄logo图像，利用logo图像视觉信息特征与visual map数据库图像的视觉信息特征进行匹配，保留正确匹配点并剔除误匹配点，计算出顶点坐标。最后，求得相机在世界坐标系下的坐标，实现定位功能。该方法虽然能够实现定位，但是，该方法仍然存在的不足之处是，在定位前需要建立周围环境的视觉地图数据库，导致在未知环境下无法实现室内移动目标的定位。
4.四川北控聚慧物联网科技有限公司在其申请的专利文献中“一种基于wifi的室内定位方法”(申请号：202110274155.4，申请公布号：cn112689237a)中提出了一种基于wifi信号的室内定位方法，其具体流程如下：首先，在室内选取多个参照点，在每一个参照点位置上采集每个wifi信号源的wifi信号强度。然后，对每个信号源信号进行降噪处理，得到每一参照点位置的信号强度特征值。最后，将移动接收设备当前位置的信号强度特征值与每一参照点位置的信号强度特征值做对比，计算特征值相似度，将相似度最高的参照位置点设定为移动接收设备的位置。该方法虽然能够实现定位，但是，该方法仍然存在的不足之处是，这种定位方法需要在室内安装多个提供wifi信号源的辅助设备，无法摆脱移动接收设备对外部设备的依赖，部署复杂，成本高。


技术实现要素：

5.本发明的目的在于针对上述现有技术的不足，提出一种基于深度相机和imu组合的室内移动终端定位方法，旨在解决室内无法在没有先验地图的情况下定位的问题，以及定位依赖外部设备带来的部署复杂，成本高的问题。
6.实现本发明目的的思路是，搭载深度相机和imu的移动终端在未知环境中自由运动，通过纯视觉三维重建和深度图像数据，确定相机坐标系下滑动窗口内所有深度相机的位姿，利用线性方程求解出采集枢纽帧灰度图像时深度相机的重力向量，通过重力向量将深度相机的位姿调整至世界坐标系下，确定深度相机的初始位姿，通过计算深度相机的状态增量，确定深度相机的实时位姿，当检测到回环时，优化存在回环的两帧图像之间的所有灰度图像对应的深度相机位姿，利用移动终端与深度相机之间的相对位姿，得到移动终端的实时位姿，实现了移动终端的实时定位，解决了无法在没有先验地图的情况下定位的问题。本发明仅通过自身搭载的传感器实时采集数据并传入移动终端，移动终端对传入数据进行处理，计算出移动终端的实时位姿，解决了室内定位依赖外部设备带来的部署复杂，成本高的问题。
7.实现本发明目的的具体步骤如下：
8.步骤1，将传感器采集的数据传入移动终端：
9.(1a)利用移动终端上设置的深度相机实时采集彩色图像和深度图像，每帧彩色图像和深度图像的大小均为640
×
480像素，采集频率均为每秒钟30帧，将彩色图像和深度图像按时间顺序传入移动终端；
10.(1b)利用移动终端上设置的imu每秒钟采集200帧加速度数据，每秒钟采集63帧角速度数据，将加速度数据和角速度数据按时间顺序传入移动终端；
11.步骤2，利用opencv的cv::createclahe()函数对图像进行均衡化处理；
12.步骤3，选取稳定的特征点：
13.(3a)以每帧灰度图像的左上角像素点为坐标原点建立二维像素坐标系，原点的像素坐标为(0,0)，灰度图像水平向右方向为x坐标轴的正方向，灰度图像竖直向下方向为y坐标轴的正方向，灰度图像中右上角的像素坐标为(639,0)；
14.(3b)在每帧灰度图像中选取左上像素点的像素坐标为(20,15)，右上像素点的像素坐标为(620,15)，左下像素点的像素坐标为(620,465)，右下像素点的像素坐标为(20,465)的四个像素点，按区域顺序依次连接四个像素点，得到待划分网格的矩形区域；
15.(3c)在左上像素点和左下像素点之间的连线上选取(20,165)、(20,315)两个像素坐标，将该连线均分为3份，在左上像素点和右上像素点之间的连线上选取(170,15)、(320,15)和(470,15)三个像素坐标，将该连线均分为4份，将选取的像素坐标作为划分矩形网格的参考点，将整个矩形区域均匀划分为3
×
4个大小相同的网格；
16.(3d)利用cv::goodfeaturestotrack()函数，提取第一帧灰度图像中的所有特征点；
17.(3e)通过光流跟踪确定第一帧灰度图像中的每个特征点在第二帧灰度图像中的位置，将第一帧灰度图像中的每个特征点在其余图像帧中被确定相应位置的次数按从大到小排序，若同一网格中的特征点确定相应位置的次数相同，则按特征点提取的先后顺序排序，将每个网格中前7个特征点作为稳定的特征点，共选取7
×
12个稳定的特征点；
18.(3f)采用与步骤(3e)相同的方法，确定前一帧灰度图像中的每个特征点在后一帧灰度图像中的位置，并确定每个网格中特征点的排序，若出现特征点跟踪丢失的情况，在该特征点所属的网格中提取新的特征点，补全该网格中特征点的数目至7；
19.步骤4，对imu数据进行积分预处理：
20.步骤5，确定滑动窗口内深度相机带尺度因子的相对位姿：
21.(5a)从含有稳定特征点的灰度图像中，按时间顺序依次选取11帧的灰度图像，建立滑动窗口，对滑动窗口内的11帧图像进行纯视觉三维重建；
22.(5b)在滑动窗口内的前10帧灰度图像中，找出一帧灰度图像作为枢纽帧图像，枢纽帧图像与第11帧灰度图像之间的视差大于20像素，且两帧图像之间至少存在25对共视点，其中，共视点表示在至少两帧灰度图像中均存在的特征点，视差表示两帧灰度图像中所有共视点的平均像素差；
23.(5c)利用cv::triangulatepoints()和cv::solvepnp()函数，计算滑动窗口内采集枢纽帧图像时与采集其他帧灰度图像时深度相机的相对位姿；
24.步骤6，计算尺度因子的平均值：
25.(6a)在滑动窗口内，找出枢纽帧图像与枢纽帧后一帧灰度图像中的所有共视点，得到每个共视点位置坐标与采集该枢纽帧图像时深度相机位置坐标的距离d
i
；
26.(6b)从枢纽帧图像对应的深度图像中获取每个共视点中心像素的深度值d
i
；
27.(6c)按照下式，计算滑动窗口内深度相机尺度因子的平均值s：
[0028][0029]
其中，n表示枢纽帧灰度图像与枢纽帧后一帧灰度图像中的共视点的总数，σ表示求和操作，i表示枢纽帧灰度图像中共视点的序号；
[0030]
步骤7，求解深度相机的速度向量和重力向量：
[0031]
利用线性方程，计算滑动窗口内采集相邻两帧灰度图像时深度相机的速度向量和采集枢纽帧灰度图像时深度相机的重力向量，将深度相机的位姿调整至世界坐标系下；
[0032]
步骤8，计算深度相机的状态增量：
[0033]
(8a)通过紧耦合的非线性优化公式，计算视觉重投影残差和imu测量残差中的待优化变量，得到滑动窗口内采集每帧灰度图像时深度相机的最优位姿；
[0034]
(8b)利用滑动窗口内优化后的深度相机位姿，得到深度相机的状态增量，确定深度相机的实时位姿；
[0035]
步骤9，优化深度相机的位姿：
[0036]
(9a)利用fast角点检测算法，在深度相机采集的每帧灰度图像中提取至少300个图像特征点，将每帧灰度图像中所有特征点的描述子信息储存在数据库中；
[0037]
(9b)利用基于dbow2的回环检测算法，计算当前图像帧与数据库中每一帧灰度图像的相似性得分，通过相似性得分判断当前帧图像是否存在回环，得分若大于或等于0.6，则执行步骤(9c)，否则，执行步骤10；
[0038]
(9c)对存在回环的两帧灰度图像间的每帧图像所对应的深度相机位姿进行非线性优化，输出每帧优化后的深度相机位姿，利用移动终端与深度相机之间的相对位姿，得到每帧优化后的移动终端位姿；
[0039]
步骤10，输出当前深度相机的位姿，利用移动终端与深度相机之间的相对位姿，得到当前移动终端的位姿。
[0040]
与现有技术相比，本发明具有以下优点：
[0041]
第一，本发明中移动终端在未知环境中自由运动，通过纯视觉三维重建和深度图像数据，得到相机坐标系下滑动窗口内所有深度相机的位姿，通过重力向量将深度相机的位姿调整至世界坐标系下，确定深度相机的初始位姿，通过紧耦合的非线性优化，得到深度相机的状态增量，确定深度相机的实时位姿，通过深度相机与移动终端的相对位姿，确定移动终端的实时位姿，克服了现有技术中室内定位需要先验地图的缺陷，使得本发明节省了定位前需要建立视觉地图数据库所占的存储空间。
[0042]
第二，由于本发明移动终端仅通过处理自身搭载的深度相机和imu采集的数据，就可以计算出移动终端的实时位姿，克服了现有技术中需要在室内安装外部设备导致定位系统部署复杂，成本高的缺陷，本发明可以在只使用移动终端自身传感器的条件下实现定位，提高了定位系统的便携性。
附图说明
[0043]
图1是本发明的流程图；
[0044]
图2是本发明提取稳定特征点的示意图。
[0045]
具体实施方法
[0046]
下面结合附图和实施例对本发明作进一步的描述。
[0047]
参照图1对本发明的实现步骤作进一步的描述。
[0048]
步骤1，将传感器采集的数据传入移动终端。
[0049]
利用移动终端上设置的深度相机实时采集彩色图像和深度图像，每帧彩色图像和深度图像的大小均为640
×
480像素，采集频率均为每秒钟30帧，将彩色图像和深度图像按时间顺序传入移动终端。
[0050]
利用移动终端上设置的imu每秒钟采集200帧加速度数据，每秒钟采集63帧角速度数据，将加速度数据和角速度数据按时间顺序传入移动终端。
[0051]
步骤2，利用opencv的cv::createclahe()函数对图像进行均衡化处理。
[0052]
所述均衡化处理是指，将每帧彩色图像格式转换成能够被opencv识别的灰度图像格式，再利用createclahe()函数对每帧灰度图像进行均衡化处理。
[0053]
步骤3，选取稳定的特征点。
[0054]
第1步，以每帧灰度图像的左上角像素点为坐标原点建立二维像素坐标系，原点a的像素坐标为(0,0)，灰度图像水平向右方向为x坐标轴的正方向，灰度图像竖直向下方向为y坐标轴的正方向，灰度图像中右上角像素点b的像素坐标为(639,0)，灰度图像中左下角像素点c的像素坐标为(0,479)。
[0055]
第2步，在每帧灰度图像中选取左上像素点d的像素坐标为(20,15)，右上像素点e的像素坐标为(620,15)，左下像素点f的像素坐标为(620,465)，右下像素点g的像素坐标为(20,465)的四个像素点，按区域顺序依次连接四个像素点，得到待划分网格的矩形区域。
[0056]
第3步，在左上像素点d和左下像素点f之间的连线上选取k(20,165)、l(20,315)两个像素点，将该连线均分为3份，在左上像素点d和右上像素点e之间的连线上选取h(170,
15)、i(320,15)和j(470,15)三个像素点，将该连线均分为4份，将选取的像素点作为划分矩形网格的参考点，将整个矩形区域均匀划分为3
×
4个大小相同的网格。
[0057]
第4步，利用cv::goodfeaturestotrack()函数，提取第一帧灰度图像中的所有特征点。
[0058]
第5步，通过光流跟踪确定第一帧灰度图像中的每个特征点在第二帧灰度图像中的位置，将第一帧灰度图像中的每个特征点在其余图像帧中被确定相应位置的次数按从大到小排序，若同一网格中的特征点确定相应位置的次数相同，则按特征点提取的先后顺序排序，将每个网格中前7个特征点作为稳定的特征点，共选取7
×
12个稳定的特征点。
[0059]
第6步，采用与本步骤第5步相同的方法，确定前一帧灰度图像中的每个特征点在后一帧灰度图像中的位置，并确定每个网格中特征点的排序，若出现特征点跟踪丢失的情况，在该特征点所属的网格中提取新的特征点，补全该网格中特征点的数目至7。
[0060]
在特征点的圆形区域内，不能存在其他特征点的中心像素点，圆形区域的半径为10像素。
[0061]
参照图2，对本发明选取稳定特征点做进一步描述：
[0062]
图2中特征点1、特征点2、特征点3、特征点4、特征点5、特征点6和特征点7为同一网格中排序靠前的7个特征点，为稳定的特征点。
[0063]
如图2中的特征点5和特征点6，两特征点中心像素点没有在对方的圆形区域内，且两特征点的排序靠前，故均保留。
[0064]
如图2中的特征点8在网格中的排序为第8位，排序靠后，故剔除特征点8。
[0065]
如图2中的特征点7和特征点9，特征点9中心像素点在特征点7的圆形区域内，且特征点9的排序靠后，故剔除特征点9而保留特征点7。
[0066]
如图2中的特征点10，特征点中心像素点不在网格区域内，故剔除特征点10。
[0067]
步骤4，对imu数据进行下述的积分预处理。
[0068]
以移动终端上的初始位置为原点，移动终端向前延伸的方向为x轴正方向，移动终端向左延伸的方向为y轴正方向，移动终端向上延伸的方向为z轴正方向建立一个三维世界坐标系。
[0069]
在世界坐标系下，利用基于中值积分法，根据imu当前帧和后一帧的加速度和角速度数据及当前帧imu的位置、速度和姿态，获取后一帧imu的位置、速度和姿态。
[0070]
利用基于中值法的imu预积分公式，根据imu当前帧和后一帧的加速度和角速度数据，得到相邻两帧之间imu的位置增量、速度增量和姿态增量。
[0071]
步骤5，确定滑动窗口内深度相机带尺度因子的相对位姿。
[0072]
第1步，从含有稳定特征点的灰度图像中，按时间顺序依次选取11帧的灰度图像，建立滑动窗口，对滑动窗口内的11帧图像进行纯视觉三维重建。
[0073]
第2步，在滑动窗口内的前10帧灰度图像中，找出一帧灰度图像作为枢纽帧图像，枢纽帧图像与第11帧灰度图像之间的视差大于20像素，且两帧图像之间至少存在25对共视点，其中，共视点表示在至少两帧灰度图像中均存在的特征点，视差表示两帧灰度图像中所有共视点的平均像素差。
[0074]
第3步，以采集枢纽帧灰度图像时的深度相机位置为原点，深度相机向前延伸的方向为x轴正方向，深度相机向左延伸的方向为y轴正方向，深度相机向上延伸的方向为z轴正
方向建立一个三维相机坐标系。
[0075]
第4步，在相机坐标系下，利用五点法，根据枢纽帧与第11帧灰度图像中至少5对的共视点中心像素坐标，确定采集枢纽帧与第11帧灰度图像时深度相机的相对位姿，相对位姿由相对旋转矩阵和带尺度因子的相对位移向量组成。
[0076]
第5步，使用opencv的cv::triangulatepoints()函数，对枢纽帧灰度图像与第11帧灰度图像进行三角化处理，得到两帧图像间每个共视点的位置坐标。
[0077]
第6步，使用opencv的cv::solvepnp()函数，依次对从第l 1帧到第10帧灰度图像中的每帧图像进行pnp求解，得到采集第l帧灰度图像时与采集从第l 1帧到第10帧灰度图像时深度相机的相对位姿，其中，第l帧灰度图像为枢纽帧灰度图像，第l 1帧灰度图像为枢纽帧后的第一帧灰度图像。
[0078]
第7步，采用与本步骤第6步相同的办法，得到采集第l帧与第l
‑
1帧灰度图像时深度相机的相对位姿，采用与本步骤第5步相同的办法，得到第l帧与第l
‑
1帧灰度图像间每个共视点的位置坐标，其中，第l
‑
1帧灰度图像为枢纽帧前的第一帧灰度图像。
[0079]
第8步，采用与本步骤第7步相同的办法，得到采集枢纽帧灰度图像时与采集枢纽帧前的每帧灰度图像时深度相机的相对位姿。
[0080]
步骤6，计算尺度因子的平均值。
[0081]
在滑动窗口内，找出枢纽帧图像与枢纽帧后一帧灰度图像中的所有共视点，得到每个共视点位置坐标与采集该枢纽帧图像时深度相机位置坐标的距离d
i
。
[0082]
从枢纽帧图像对应的深度图像中获取每个共视点中心像素的深度值d
i
。
[0083]
按照下式，计算滑动窗口内深度相机尺度因子的平均值s：
[0084][0085]
其中，n表示枢纽帧灰度图像与枢纽帧后一帧灰度图像中的共视点的总数，σ表示求和操作，i表示枢纽帧灰度图像中共视点的序号。
[0086]
步骤7，求解深度相机的速度向量和重力向量。
[0087]
通过kalibr工具箱标定深度相机与imu之间的旋转外参和平移外参。
[0088]
利用线性方程，计算滑动窗口内采集相邻两帧灰度图像时深度相机的速度向量和采集枢纽帧灰度图像时深度相机的重力向量，将深度相机的位姿调整至世界坐标系下。
[0089]
所述线性方程如下：
[0090][0091]
其中，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加速度数据进行预积分得到的速度增量。
[0092]
所述将深度相机的位姿调整至世界坐标系的步骤如下：
[0093]
相机坐标系下，将采集枢纽帧灰度图像时深度相机的重力向量旋转至世界坐标系中的z轴，重力向量的方向与z轴负方向一致，得到从相机坐标系到世界坐标系的旋转矩阵。
[0094]
将该旋转矩阵将滑动窗口内深度相机的位姿调整至世界坐标系下，得到深度相机在世界坐标系下的初始位姿。
[0095]
步骤8，计算深度相机的状态增量：
[0096]
通过紧耦合的非线性优化公式，计算视觉重投影残差和imu测量残差中的待优化变量，得到滑动窗口内采集每帧灰度图像时深度相机的最优位姿；
[0097]
利用滑动窗口内优化后的深度相机位姿，得到深度相机的状态增量，确定深度相机的实时位姿；
[0098]
所述紧耦合的非线性优化公式如下：
[0099][0100]
其中，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帧灰度图像中找到该共视点对应的像素坐标的第一个值、第二个值。
[0101]
步骤9，优化深度相机的位姿。
[0102]
第1步，利用fast角点检测算法，在深度相机采集的每帧灰度图像中提取至少300个图像特征点，将每帧灰度图像中所有特征点的描述子信息储存在数据库中。
[0103]
第2步，利用基于dbow2的回环检测算法，计算当前图像帧与数据库中每一帧灰度图像的相似性得分，通过相似性得分判断当前帧图像是否存在回环，得分若大于或等于0.6，则执行本步骤的第3步，否则，执行步骤10。
[0104]
第3步，将存在回环的两帧图像之间的所有灰度图像对应的深度相机位姿进行非线性优化，输出所有优化后的深度相机位姿，利用移动终端与深度相机之间的相对位姿，得到所有优化后的移动终端位姿。
[0105]
步骤10，输出当前深度相机的位姿，利用移动终端与深度相机之间的相对位姿，得到当前移动终端的位姿。