基于深度视觉和惯性测量的移动设备自主定位方法

1.本发明涉及以深度视觉和惯性测量对移动设备进行自主定位的方法，尤其涉及一种基于滑动窗口策略的多源信息紧耦合方法。


背景技术：

2.为了确保无人机、机器人、自动驾驶车辆等可自主移动设备能够安全可靠地完成任务，基于多源数据融合的移动设备自主定位技术已成为近年来的热门话题。该技术最早提出于 1986年的国际机器人与自动化大会，提出了创建一致的概率地图问题及其理论框架和计算问题是机器人领域需要解决的基本问题。移动设备自主定位技术可以理解为将一个可移动设备放置于未知环境中，是否有方法使其通过搭载的传感器观测数据估计自身位置和姿态。移动设备所处的环境决定了自主定位功能实现所要搭载的传感器类型，gps、雷达等传感设备可以用于移动设备在开阔的室外环境下的精确定位。但是对于室内移动设备，视觉、惯性测量传感器因其成本较低，占用空间小，获取环境信息较为全面等优点更具优势。在数据融合方面，早期的主流方案均基于滤波算法实现移动设备的位置和姿态更新，但是基于马尔可夫性的滤波方法无法将当前时刻与历史时刻的移动设备状态进行关联，且滤波方法需要保存状态量的均值和方差等参数，在大范围环境下的状态数据存储量极大。目前的大多数移动设备自主定位技术均基于非线性优化理论，具体实现过程涉及几何学、图论和概率估计的交叉，且已经具备了成熟的实现框架。
3.本发明实现了移动设备在室内环境下基于深度视觉和惯性测量的自主定位，同时在位姿估计的理论框架以及技术实现上展开探索，最终开发了一套便携、实用的移动设备室内自主定位系统，可以通过手持的方式进行场景扫描并快速准确地估计设备位姿。自主定位系统可划分为两部分，分别是用来获取深度视觉和惯性测量值的可移动数据采集端和实现多源数据紧耦合算法的自主定位终端。数据采集端包括彩色相机、深度相机、惯性测量单元(imu)、数据预处理模块和无线通信模块，三种传感器设备在移动设备扫描室内环境过程中采集彩色图像、深度图像和移动设备的运动加速度和角速度，数据预处理模块用来实现传感器的标定任务，无线通信模块将数据采集端的多源数据传输至自主定位终端。自主定位终端用来实现基于滑动窗口策略的多源信息紧耦合方法，实时估计移动设备在每个离散时刻的位姿状态。


技术实现要素：

4.本发明为了实现移动设备在未知环境下的自主定位，提出了一种基于滑动窗口策略的多源信息紧耦合方法，并开发了一套便携、实用的移动设备室内自主定位系统。
5.基于深度视觉与惯性测量的移动设备自主定位方法，其主要创新点在于：轻量便携的传感器数据采集端的开发、深度视觉与惯性测量紧耦合优化问题的构建、基于关键帧策略的滑动窗口算法的设计三部分。
6.所述传感器数据采集端以平板电脑为核心，基于其自身搭载的彩色相机、惯性测
量单元和外置的深度相机构成如图1所示的数据采集端硬件平台，数据预处理模块和无线通信模块分别实现传感器的标定和传感器数据在局域网中的发布。
7.所述深度视觉与惯性测量紧耦合优化问题建立了深度视觉观测和惯性测量与系统状态之间的关系，并构建了一个系统状态量统一的非线性优化问题，可以同时估计移动设备的位置、姿态、速度以及惯性测量单元的偏置。
8.所述基于关键帧策略的滑动窗口算法将图像特征点对的数量和特征点集的平均视差作为关键帧的判断标准，过滤具有较多重复信息的图像帧，并通过移除历史状态变量的方式动态更新非线性优化问题中的待优化状态变量，进而控制优化问题的求解规模，保障移动设备自主定位的实时性。
9.本发明的优点在于：
10.(1)本发明通过搭建可移动数据采集端实现室内环境的多源数据采集，并基于数据采集端实现传感器的标定和数据的无线传输。与大部分基于线缆连接的数据采集设备相比，本发明具有明显的实用性、便携性和可移植性。
11.(2)本发明提出一种基于紧耦合和非线性优化的多源数据融合方案，通过构造状态量统一的非线性优化问题同时估计移动设备的位置、姿态、速度和惯性测量单元的偏置。
12.(3)本发明实现了基于关键帧策略的滑动窗口算法，通过关键帧判断标准有效过滤了大量彩色图像和深度图像中的冗余信息，并通过滑动窗口实现了待优化状态变量的动态更新，进而提升了移动设备自主定位系统的实时性。
附图说明
13.图1为本发明给出的数据采集端硬件平台实物图片；
14.图2为本发明给出的基于深度视觉与惯性测量的移动设备自主定位方法的实现框架；
15.图3为本发明提供的多角度棋盘格图像；
16.图4为本发明提供的提取角点后的棋盘格图像；
17.图5为本发明提供的棋盘格角点重投影误差分布图；
18.图6为本发明提供的局域网下的网络通信方式示意图；
19.图7为本发明提供的移动设备自主定位方法的初始化步骤流程图；
20.图8为本发明提供的基于公开深度视觉和移动设备运动轨迹数据重建的高质量场景三维模型图；
21.图9为本发明提供的移动设备在三维空间中的运动轨迹图；
22.图10为本发明提供的三种定位方法在三组测试实验中的绝对位姿估计误差曲线图；
具体实施方式
23.下面结合附图对本发明提供的基于深度视觉与惯性测量的移动设备自主定位方法进行详细说明。
24.本发明给出的基于深度视觉与惯性测量的移动设备自主定位方法的实现框架如图2所示。按照具体实施步骤，可以将整个流程划分为三部分，分别是数据采集与传输、深度
视觉与惯性测量结果的对齐、基于滑动窗口的多源数据紧耦合算法。数据采集与传输部分的所有功能均基于ios开发，硬件设备包括第六代ipad和occipital公司推出的structure深度相机。其他两部分的算法全部基于c 开发，硬件设备为搭载linux操作系统的台式机。
25.数据采集之前需要对所有的传感器设备进行标定，相机内参标定的目的是确定空间物体表面某点的三维位置与其在图像中对应点的相互关系，并建立相机成像的几何模型，本发明使用张氏标定法对彩色相机和深度相机的内参进行标定。首先获取如图3所示的多角度棋盘格图像，并提取每张图像中完整的棋盘格角点，如图4所示。给出待标定参数的初始估计值，基于相机成像模型对空间中的棋盘格角点进行投影，并根据成像模型的角点投影坐标结果和提取的图像角点坐标建立多参数的非线性优化问题，最后通过列文伯格-马夸尔特(levenbergmarquardt，lm)算法进行迭代求解。为客观评估相机内参标定精度，将所有图像帧中的棋盘格角点投影到如图5左半部分所示的同一张图像下，不同颜色的角点表示来自不同的图像，可以发现不同图像帧中的同一角点有很好的对应关系，所有图像帧中的棋盘格角点重投影误差分布如图5右半部分所示，98％的棋盘角点重投影误差在x轴和y轴方向上都小于0.5个像素，最大误差也不超过1个像素。对于惯性测量单元，本发明假设其随机噪声服从高斯分布，并通过allan方差法对惯性测量单元的偏置和随机游走方差进行标定。
26.本发明提供的数据采集端能够实现局域网下的传感器数据无线实时传输。图片数据的实时传输对网络带宽要求较高，本发明对深度图像和彩色图像进行了压缩处理，以发布/订阅作为网络通信方式，根据相应规范设计如图6所示的数据发布和订阅过程。其中数据采集端作为数据发布节点，用来实现移动设备自主定位的台式机一端作为数据订阅节点。
27.本发明提供的如图7所示的深度视觉与惯性测量结果的对齐方案实现了移动设备自主定位的初始化过程。当惯性测量单元获得足够的激励后，开始建立相邻图像帧之间的视觉约束关系，并基于特征点之间的几何约束关系计算相邻图像帧间的移动设备相对位姿。在位姿估计合理的情况，先后执行基于滑动窗口内的sfm(structure from motion)算法和滑动窗口外的pnp(perspective-n-point)算法，得到滑动窗口内所有图像帧对应时刻的移动设备位置和姿态。计算特征点三维坐标过程中直接查询深度图像上对应点的深度值，而无需引入三角测量算法。最后基于惯性测量预积分结果和深度视觉估计结果构建非线性最小二乘问题，实现对滑动窗口内所有时刻的移动设备运动速度以及重力方向进行初始化。
28.本发明提供的基于滑动窗口的多源数据紧耦合算法充分利用每个传感器的原始数据，同时估计决定移动设备位置和姿态的所有状态变量，并通过关键帧策略和滑动窗口机制控制了算法规模，有效降低了系统的运行时间。该算法具体实施流程如下：
29.第1步，提取图像中具有一定辨识度的角点作为特征点，通过lk(lucas kanade)光流法对非首帧图像的已有特征点进行跟踪，并采用对级约束剔除误差较大的匹配特征点对。特别地，为了保证特征点数量满足跟踪算法需求以及特征点的均匀分布，本发明在特征点检测过程中引入了图像遮罩机制(mask)，即对上一帧已经存在的特征角点及其附近指定大小的邻域进行标记，新的特征角点仅能在未标记的区域进行提取；
30.第2步，构建基于深度视觉的残差函数，基于相邻图像帧中的相同特征点之间的视
觉约束可以构建关于移动设备位置和姿态变量的捆绑优化(bundle adjustment)问题，观测量是特征角点在不同图像帧上的像素坐标，则有基于深度视觉的残差函数：
[0031][0032]
其中表示某一特征点在第j帧图像中的像素坐标，表示第i帧中的同一特征点经过旋转和平移后的归一化像素坐标：
[0033][0034]
其中表示特征点在第i帧图像中的像素坐标，dd表示深度观测，和分别表示第i帧图像和第j帧图像对应时刻的移动设备相对于初始状态的位姿变换矩阵。
[0035]
第3步，本发明引入目前广泛应用于惯性测量导航的预积分算法，通过相邻图像帧之间的imu测量值计算移动设备在两个图像帧对应时间间隔内的位置变化量速度变化量和姿态变化量
[0036]
第4步，构建基于惯性测量的残差函数，待优化状态变量设为两个图像帧时刻对应的移动设备位置p
wb
、速度vw、姿态q
wb
和imu的加速度计偏置ba、陀螺仪偏置bg，观测量即第3 步计算得到的预积分值，则有残差函数：
[0037][0038]
除了旋转误差之外，其他残差表达式都是通过直接相减得到的，而旋转误差是通过计算两个四元数之间的角度误差。
[0039]
第5步，整合滑动窗口内所有上述待优化状态变量，并构造深度视觉残差函数和惯性测量残差函数的求和形式以形成新的残差函数，实现多源数据的紧耦合。
[0040]
第6步，计算残差函数对所有状态量的雅各比矩阵，深度视觉残差和惯性测量残差的协方差，最后通过ceres非线性优化库求解状态变量，完成移动设备的位姿估计。
[0041]
本发明核心代码由c 开发，并在彩色-深度-imu数据集上验证有效。为了获得基于不同运动轨迹的测试数据集，本发明根据公开的深度视觉和移动设备运动轨迹数据重建了如图 8所示的高质量场景三维模型，采用虚拟相机模型按照预设轨迹对三维模型进行彩
色和深度图像采集，同时根据设定好的运动方程仿真得到惯性测量单元的输出数据，从而得到可用于评价基于深度视觉与惯性测量的移动设备自主定位方法性能的多个数据集。
[0042]
本发明设计了三组测试实验用于评估所提方法性能，移动设备在三维空间中的运动轨迹如图9所示，其中第2组测试实验的移动设备运动轨迹在第1组的基础上添加了y轴方向的正弦运动，第3组测试实验在第1组实验的基础上增大了移动设备在每个时刻的角速度，所以空间轨迹与第1组是一致的。
[0043]
本发明提供的移动设备自主定位方法与经典的直接稀疏里程计算法(dso，directsparseodometry)和当前主流的vins-mono算法进行了对比，图10给出了三种方法在三组测试实验中的绝对位姿估计误差曲线。可以看到，本发明提出的基于深度视觉与惯性测量的定位方法的绝对轨迹误差在任一测试数据集下都是最低的，具有更小的方差和均值，表现更为稳定。例如第三组数据集包含了较大幅度的三轴旋转，会有短暂时刻的视野空白和图像模糊情况，这导致dso里程计出现了较大的绝对轨迹误差，而本发明提出的方法与vins-mono都充分利用了惯性测量数据，能够在一定程度上克服上述问题。vins-mono里程计相比，本发明融合了深度信息，解决了单目视觉存在尺度不确定性的问题。