CN113358117B

CN113358117B - 一种利用地图的视觉惯性室内定位方法

Info

Publication number: CN113358117B
Application number: CN202110257493.7A
Authority: CN
Inventors: 任明荣; 张继统; 王普; 牟雨曼
Original assignee: Beijing University of Technology
Current assignee: Beijing University of Technology
Priority date: 2021-03-09
Filing date: 2021-03-09
Publication date: 2023-05-26
Anticipated expiration: 2041-03-09
Also published as: CN113358117A

Abstract

本发明公开了一种利用地图的视觉惯性室内定位方法，包括，针对视觉惯性里程计系统在室内定位精度较差的问题，采用了基于条件随机场的三维地图匹配算法来对视觉惯性里程计的输出进行修正。首先，建立室内三维地图的条件随机场模型，将其作为先验信息加入计算；其次，将视觉惯性里程计系统输出的位姿和轨迹信息作为观测量送入条件随机场模型中，最后，将条件随机场模型输出的最优状态点序列作为反馈值来对视觉惯性里程计系统的输出进行修正。本发明基于条件随机场的三维地图匹配算法相比二维地图匹配拓展了应用的范围。相比于以往的基于粒子滤波的地图匹配算法，在保证了精度的同时显著的减少了计算量，更加适合于实际工程应用。

Description

一种利用地图的视觉惯性室内定位方法

技术领域

本发明属于室内定位技术领域，涉及一种利用地图的视觉惯性室内定位技术。在室内地图情况已知条件下基于视觉惯性里程计(Visual-Inertial Odometry，VIO)和三维地图匹配技术来进行室内定位，利用三维地图匹配算法来增强视觉惯性里程计在室内定位的精度。

背景技术

随着物联网技术这些年的快速发展，出现了像智慧病房、无人超市、无人酒店等这些新型技术。而这些技术对室内定位有了更高的要求，因此如何室内定位精度也受到了学术界和工业界广泛的讨论和关注。

在开阔的室外环境下，高精度的差分GPS(Global Positioning System)可以提供给非常精确的实时定位，然而，GPS在室内定位中由于建筑物和墙壁的遮挡和反射，精度会大大降低，在室内GPS的表现很差。传统的室内定位多采用惯性测量单元(InertialMeasurement Unit，IMU)、Wi-Fi、超宽带技术(Ultra Wide Band，UWB)和蓝牙低功耗技术(Bluetooth Low Energy，BLE)来进行定位。然而与室外的GPS定位技术不同，这几种技术都有非常明显不足之处。惯性测量单元内部包含有加速度计和陀螺仪，可以采集行人每一时刻的加速度和角速度信息，进而积分得到速度、位置和角度信息，然而惯性测量单元在进行定位时误差会以非常高的速度进行累积，较长时间后误差就会严重的影响定位精度。甚至当行人静止不动时，误差也会累积。虽然可以使用零速检测和陀螺测漂的技术对其进行一定的修正，但在较长距离定位时效果仍然不太理想。Wi-Fi技术和超宽带技术的缺点比较相似，在使用这两者时，都需要在室内布置大量的基础设施，来对Wi-Fi和超宽带提供信号支持，因此在成本方面这两项技术需求会比较高，特别是在大型的室内环境。同时，定位精度受信号影响也比较大。对于BLE技术，今年来的确得到了不错的发展，但是它同样需要在室内搭建iBeacon设备，同时信号也会受到这些设备的影响，后期的维护也是一个比较麻烦的问题。

随着对室内定位精度要求的提高，单一的室内定位技术已经很难满足需求，因此多种定位技术进行融合成为了新的主流方向。视觉惯性里程计技术是一项著名的融合视觉定位技术和惯性测量单元定位技术来估计行人或机器人的位置和运动轨迹的方法。它融合了视觉定位技术和惯性测量单元定位技术的优点，可以获得比单视觉和单IMU更高精度和更加鲁棒的定位效果。VIO室内定位技术近几年在许多场所都进行过研究和应用。但是它的问题是，视觉定位依赖于拍摄图像的质量，当图像质量较差时，视觉技术就无法从图像中提取出足够多的有效的特征点，因此就无法进行前后两帧图像之间的有效匹配。短时间内，可以通过IMU技术来对其进行修正，但是较长时间后，IMU自身也会产生很大的误差，就会导致VIO技术定位误差增大或定位失败。

利用地图匹配技术来提高室内定位精度一直以来都备受关注，传统的地图匹配算法多采用粒子滤波和隐马尔科夫算法来建立地图模型。粒子滤波算法的优点是对于非线性非高斯系统也可以使用，不会带来因系统线性化而产生的误差。但是缺点是，粒子滤波的计算量与粒子的数目相关，一旦粒子数目增大，计算量就会迅速增加，带来很大的计算负担。而对于隐马尔科夫算法来说，它需要假设观测量之间是相互独立的，并不适合的系统。条件随机场算法模型是2001年由Lafferty提出的，它结合了最大熵模型和隐马尔可夫模型的特点，是一种无向图模型。2014年，牛津大学的Xiao，Zhuoling等人将条件随机场模型用于室内地图匹配算法中，将它用于解决进行轨迹估计问题。相比其他算法，条件随机场模型可以捕捉观测量之间的多种约束关系，具有更加普遍的适用性和更高的准确度。

发明内容

针对上述所述问题，提供了一种利用地图的视觉惯性室内定位技术，使用三维地图匹配来增强视觉惯性里程计室内定位的精度，通过建立的条件随机场模型和VIO提供的观测量，输出在当前观测量下的最优状态点序列来对VIO输出的轨迹进行修正。实现室内高精度的行人定位。

具体步骤如下：

步骤1.地图预处理，在实验前要对地图进行处理。

步骤1.1，将图像格式的地图(PDF格式或CAJ格式)的地图转化为地图匹配算法可以使用的数字格式地图。

步骤1.2，设置状态点。

步骤1.3，去除不可到达区域的状态点。

步骤1.4，设置状态之间的传播条件，相邻的状态在转播时会有一定的限制条件。

步骤2.视觉惯性器件数据采集。

步骤2.1，将相机和IMU数据同步。

步骤2.2，采集行人运动的信息。

步骤2.3，将相机采集的数据传送给计算机。

步骤3.视觉惯性里程计算法，将采集到的数据通过视觉惯性里程计算法计算出每一时刻行人的航向、位姿和轨迹信息。

步骤3.1，采用紧耦合的视觉惯性里程计算法进行求解。

步骤3.2，将VIO的数据送入条件随机场中作为观测信息。

步骤4.行人初始位置信息初始化。

步骤4.1，首先在视觉惯性坐标系下，给出行人的初始位置。

步骤4.2，求出视觉惯性坐标系和地图坐标系之间转换关系。

步骤4.3，根据前面的行人在视觉惯性坐标系下的初始位置信息和转换关系，得出行人在地图坐标系下的初始位置。

步骤5.基于条件随机场模型的三维地图匹配算法。

步骤5.1，提取观测量，提取VIO的输出作为条件随机场模型的观测量。

步骤5.2，定义状态转移函数，状态传递函数表示观察到的连续状态之间过渡的相关程度。

步骤5.3，最优状态点序列的输出。

步骤6.反馈，将步骤5中求出的最优状态点序列作为反馈信息对VIO输出的轨迹进行修正。

与现有技术相比，本发明具有以下有益效果：

第一，实现了行人室内三维地图匹配的定位效果，与二维地图匹配相比，该技术拓展了应用的范围；第二，在三维匹配中，将二维平面匹配与高度匹配分开来进行计算，即考虑到了行人不同场所下的应用状态，方便了计算，又简化了计算量，提高了效率。第三，将视觉惯性里程计技术与三维地图匹配结合在一起进行室内定位，提高了视觉惯性里程计技术在室内定位的精度。

附图说明

图1是本发明所涉及方法的系统框架图；

图2是实验场地处理过的数字格式地图；

图3(a)是加入状态点的地图(二维)；

图3(b)是加入状态点的地图(三维)；

图4是使用的采集数据的摄像机；

图5是VIO算法的流程图；

图6(a)是未使用地图匹配的行人轨迹图(二维)；

图6(b)是加入了地图匹配的行人轨迹图(二维)；

图7(a)是未使用地图匹配的行人轨迹图(三维)；

图7(b)是加入了地图匹配的行人轨迹图(三维)；

具体实施方式

以下结合附图对本发明作进一步详细说明。

本发明是一种利用地图的视觉惯性室内定位技术，它的系统框架图如图1所示。具体包括以下步骤：

步骤1.地图预处理，在实验前要对地图进行处理。

步骤1.1，将图像格式的地图(PDF格式或CAJ格式)的地图转化为地图匹配算法可以使用的数字格式地图，转化后的数字格式地图可以在计算机中相应的地图坐标系下存储确定的坐标信息和属性地面要素和离散数据。转化后的数字格式地图如图2所示。

步骤1.2，设置状态点，使用等大小的正方形单元格来对地图进行划分，每一个单元格的顶点是可能的隐状态，正方形的边长经过实验最终设置为0.8m，约等于一个成年人一步的距离，同时每一个隐状态都在地图坐标系中存储有相应的坐标信息。加入状态点的二维地图如图3(a)所示，加入状态点的三维地图如图3(b)所示。

步骤1.3，去除不可到达区域的状态点，根据室内的具体情况，去除那些室内不可达到区域的状态(例如，墙壁、柱子等)。这么做可以提高后续匹配的效率和准确率。

步骤1.4，设置状态之间的传播条件，相邻的状态在转播时会有一定的限制条件，当两个状态之间经过之前定义过的不可到达区域时，状态不能直接进行转移。

步骤2.视觉惯性器件数据采集，采用的采集数据仪器是Intel公司的D435i摄像机，如图4所示。它是一个内部集成了IMU的摄像头，可以同时采集图像信息和惯性信息。

步骤2.1，将相机和IMU数据同步，采用在线时间标定的方法，将相机与IMU的时钟对齐，保证采集的数据可以同步。

步骤2.2，采集行人运动的信息。将相机固定在行人身上，或行人手持。沿着试验场地进行行走，采集行人运动的信息。其中，相机采集图像信息，相机内部集成的IMU采集加速度信息和角速度信息。

步骤2.3，将相机采集的数据传送给计算机，将相机与计算机通过USB数据线连接好，将相机采集的数据传送到计算机中。用于后续的视觉惯性里程计算法计算。

步骤3.视觉惯性里程计算法，将采集到的数据通过视觉惯性里程计算法计算出每一时刻行人的航向、位姿和轨迹信息。该算法主要包含IMU预积分、初始化、基于滑动窗口的局部非线性优化、回环检测与全局位姿优化四大部分。视觉惯性里程计算法的流程图如图5所示。

步骤3.1，采用紧耦合的视觉惯性里程计算法进行求解，计算出行人运动的轨迹以及每一时刻的航向、位姿和轨迹信息。

步骤3.2，将VIO的数据送入条件随机场中作为观测信息。

步骤4.行人初始位置信息初始化，在地图坐标系下，使用视觉惯性里程计技术给出行人初始时刻的相对位置信息。

步骤4.1，首先在视觉惯性坐标系下，给出行人在视觉惯性坐标系下的初始位置。

步骤4.2，求出视觉惯性坐标系和地图坐标系之间转换关系。根据视觉惯性坐标系和地图坐标系之间的尺度、比例、旋转关系，将两个坐标系对齐，得出两个坐标系之间的旋转矩阵信息。

步骤4.3，根据前面的行人在视觉惯性坐标系下的初始位置信息和两个坐标系之间的旋转矩阵信息，得出行人在地图坐标系下的初始位置。行人的初始位置如图6、图7的红色圆点处。

步骤5.基于条件随机场模型的三维地图匹配算法，条件随机场模型是给定一组输入随机变量时，输出随机变量的条件概率分布模型。一个特殊的条件随机场模型是线性链模型，在这个模型中，输出变量被建模为一个序列，当输入为x＝(x₁，x₂…x_n)时，输出变量y＝(y₁，y₂，…y_n)有如下形式：

其中，f_k是局部状态转移函数，w_k是对应的权重，Z(x)是归一化因子，其表达式为：

建立基于条件随机场模型，包括提取观测量、定义状态转移函数、最优状态点序列的输出。将视觉惯性里程计的输出作为条件随机场模型的观测信息，利用维特比算法进行求解，最终可以得出在当前观测信息下的最大概率序列，即最优状态点序列。

步骤5.1，提取观测量，提取VIO的输出作为条件随机场模型的观测量，行人在同一楼层平面上行走时和在不同楼层之间的楼梯上行走时，会具有不同的行走特征，因为观测量的提取应该按不同情况进行划分。

在同一楼层平面行走阶段，当行人行走距离达到一定阈值时，就提取当前时刻VIO输出的信息作为一个观测量。阈值的选择是一个值得思考的问题，当阈值过大时，可能会导致精度不足，当阈值过小时，会造成计算量的增加，通过多次实验，选择阈值设置为0.8m，等于之前定义状态点时单元格的长度。

在楼梯上行走阶段，行人涉及到了高度的变化，同时行人在楼梯上行走时，每一步都会在台阶上停留一段时间，这个时间段内行人的速度为0。因此规定当行人的高度值变化达到一个台阶的高度且速度为0时，就提取当前时刻的VIO输出的信息作为一个观测量。

其中，x_t-1(z)和x_t(z)是相邻两个时刻观测量的高度值。

步骤5.2，定义状态转移函数，状态传递函数表示观察到的连续状态之间过渡的相关程度。当一个观测值与一个状态点相匹配时，一个明显的规律是，到这个状态点的距离越小，这个观测值在这个状态点的概率就越大，反之亦然。

因此，将当前时刻VIO输出的位姿信息与上一时刻VIO输出的位姿信息之间的距离作为第一个特征来建立状态转移函数：

其中，

是两个相邻观测量之间的欧氏距离，s(y_t-1,y_t)是两个相邻状态点之间的欧氏距离，/>

是观测数据距离的方差。

当一个观测值与多个状态点之间的距离相同时，这个观测值的方位角信息就可以作为第二个考察信息。

因此，将当前时刻VIO输出的方位角信息与上一时刻VIO输出的方位角信息作为特征来建立第二个状态转移函数：

其中，

是两个相邻观测量之间的方位角信息，θ(y_t-1,y_t)是两个相邻状态点之间的方位角信息，/>

是观测数据方位角的方差。

在楼梯阶段，需要添加与高度匹配的一维状态点。行人的每一步都会对应一个真实的高度值。可以将行人每步匹配的高度值与VIO输出的高度值之间的误差作为高度匹配特征来建立状态传递函数。

因此，将这个误差信息作为特征来建立第三个状态转移函数：

其中，y_t是t时刻的真实高度值，x_t是t时刻VIO输出的高度信息值，σ_e是高度误差方差，M_t(x,y)是t时刻之前所有高度误差的均值。

步骤5.3，最优状态点序列的输出。采用维特比算法来对之前的条件随机场模型进行求解，维特比算法可以计算出在当前观测值下的最大概率的状态点序列。

维特比算法的步骤如下所示：

(1)初始化：计算所有状态第一个位置的非标准化概率，其中，m是状态的个数。

δ₁(j)＝w·F₁(y₀＝start,y₁＝j,x) j＝1,2,…m (7)

(2)递归求解：从前到后遍历每个状态，求出每个状态l＝1,2,…m在位置i＝2，3，…n处的非标准化概率的最大值，并记录下最大概率的状态点序列标签。

(3)当i＝n时，得到非标准化概率的最大值和最有状态点序列的终点。

(4)计算最终的状态点输出序列。

(5)最终，最优状态点输出序列为：

为了验证算法的有效性，进行了实验验证。实验场地包括走廊、房间和楼梯环境。二维行人轨迹如图6所示，图6(a)是为使用地图匹配的结果，图6(b)是加入了地图匹配的结果，可以看到行人轨迹被明显修正，校正了轨迹穿墙的现象，提高了精度。

三维行人轨迹如图7所示，图7(a)是未使用地图匹配的行人轨迹图(三维)；图7(b)是加入了地图匹配的行人轨迹图(三维)。可以看到图未使用地图匹配之前VIO的输出无论二维轨迹还是高度信息都有明显的误差，定位精度不准确，且出现了轨迹穿墙现象，修正后的轨迹明显改善了这些问题。精度得到了提升。实验结果显示该算法对提高VIO在室内定位的精度具有很高的准确性和有效性。

Claims

1.一种利用地图的视觉惯性室内定位方法，其特征在于：该方法具体包括以下步骤：

步骤1. 地图预处理：图像格式地图转化为数字格式地图；设置状态点；室内可到达区域判断，去除不可到达区域状态；设置状态之间传播的约束条件；

步骤2. 视觉惯性器件数据采集：采集数据所用视觉惯性器件是一个内部集成有惯性测量单元IMU的单目相机，将惯性测量单元IMU与相机对齐采集数据，通过数据线将数据传输给计算机；IMU中包含三轴陀螺仪数据和三轴加速度计数据，相机采用的是单目相机，采集图像信息；

步骤3. 视觉惯性里程计算法：包含IMU预积分、初始化、基于滑动窗口的局部非线性优化、回环检测与全局位姿优化四大部分，计算机利用步骤2中采集的数据解算出每一时刻行人的航向、位姿和轨迹信息；

IMU预积分：将IMU数据积分，得到当前时刻的速度、位置和旋转信息（PVQ），同时计算后端优化中将要用到的相邻帧的预积分增量以及预积分误差的雅可比矩阵和协方差矩阵；

初始化：计算出绝对尺度信息、陀螺仪偏置信息、加速度计偏置信息和重力加速度G以及每个时刻的速度；

基于滑动窗口的局部非线性优化：将边缘化误差、IMU测量误差和视觉重投影误差放在一个大的目标函数里进行优化；使用滑动窗口控制优化变量的个数；对局部的位姿进行一次优化；

回环检测与全局位姿优化：当相机经过同一个地方时，就有可能发生回环，将当前的位置与之前的位置进行比对，进而消除累积误差，同时如果检测到发生回环，也会对全局的位姿进行一次优化调整；

步骤4. 行人初始位置信息初始化：在地图坐标系下，使用视觉惯性里程计技术给出行人初始时刻的相对位置信息；

步骤5. 基于条件随机场模型的三维地图匹配算法：将步骤3中视觉惯性里程计输出的信息作为条件随机场的观测量信息，输出经地图匹配算法计算之后的最优状态点序列；三维地图匹配算法包括：提取观测量、定义状态转移函数、最优状态点序列的输出；

步骤6. 反馈：将步骤5中得到的最优状态点序列作为反馈信息对视觉惯性里程计输出的轨迹进行修正；

步骤5所述的三维地图匹配算法包括：

将视觉惯性里程计的输出作为三维地图匹配算法的观测量；

建立状态转移函数；

获取最优状态点序列；

将视觉惯性里程计的输出作为三维地图匹配算法的观测量方法包括：

在同一楼层平面行走阶段，当行人行走距离达到一定阈值时，就提取当前时刻VIO输出的信息作为一个观测量；

在楼梯上行走阶段，当行人的高度值变化达到一个台阶的高度且速度为0时，就提取当前时刻的视觉惯性里程计输出的信息作为一个观测量；

建立状态转移函数方法包括：

使用距离、方位角和高度误差的信息来建立状态转移函数；获取最优状态点序列方法包括：根据建立好的基于条件随机场的三维地图匹配算法模型和观测量，使用维特比算法来进行求解，得到在当前观测量下的最大概率状态点序列，即最优状态点序列；

步骤6中所述方法包括：

使用条件随机场模型输出的最优状态点序列来对之前视觉惯性里程计的轨迹进行修正，得到修正后的更为精确的视觉惯性里程计轨迹作为算法最终的输出。

2.根据权利要求1所述的一种利用地图的视觉惯性室内定位方法，其特征在于，步骤1中所述方法包括：

转化后的数字格式地图在计算机中相应的地图坐标系下存储确定的坐标和属性的地面要素和离散数据；

设置状态点；

去除不可到达区域的状态。

3.根据权利要求2所述的一种利用地图的视觉惯性室内定位方法，其特征在于，所述方法包括：

设置状态点：使用等大小的正方形单元格来划分地图，每个正方形的顶点是可能的隐状态，正方形的边长设置为0.8m，等于一个成年人一步的距离；同时每一个隐状态都在地图坐标系中存储有相应的坐标信息；

去除不可到达区域的约束状态：根据室内地图的具体情况，去除室内行人不可能达到区域的隐状态；设置状态之间传播的条件：相邻的状态在转播时会有一定的限制条件。

4.根据权利要求3所述的一种利用地图的视觉惯性室内定位方法，其特征在于，去除室内行人不可能达到区域的隐状态方法包括：

处于墙壁内的状态；

处于柱子内的状态；

根据实际情况，室内不可通行区域的状态；

根据已有的地图，在地图坐标系中删除这些区域的状态，防止之后的误匹配。

5.根据权利要求3所述的一种利用地图的视觉惯性室内定位方法，其特征在于，相邻的状态在转播时会有一定的约束条件包括：当两个隐状态之间经过一个之前定义过的无法到达的区域时，状态不能直接进行转移。

6.根据权利要求1所述的一种利用地图的视觉惯性室内定位方法，其特征在于，步骤4所述方法包括：行人初始位置信息初始化；利用视觉惯性里程计技术获取行人在视觉惯性坐标系下的初始位置，之后根据视觉惯性坐标系和地图坐标系之间的尺度、比例、旋转关系，将两个坐标系对齐，求出地图坐标系下行人的初始位置信息。