CN110375738B - 一种融合惯性测量单元的单目同步定位与建图位姿解算方法 - Google Patents

Abstract

本发明涉及机器人感知领域，特别是一种融合惯性测量单元的单目同步定位与建图位姿解算方法。其特征是：至少包含以下步骤：步骤1)获取机器人视觉信息，依据机器人视觉信息和机器人惯性信息确定机器人位姿；步骤2)依据步骤1)给出机器人视觉信息坐标系C相对于世界坐标系W上的平移分量；步骤3)给出视觉与惯性在后端的误差项，优化目标函数构造及位姿求解。本发明具有良好的鲁棒性，建图精度和点云质量，可以求解更高精度的位姿，从而应用于在未知环境中行走的机器人时，能够获得精准的定位，使机器人在后续自动导航过程中可以实现精准壁障、精确规划最优路线等功能。

Description

一种融合惯性测量单元的单目同步定位与建图位姿解算方法

技术领域

本发明涉及机器人感知领域，特别是一种融合惯性测量单元的单目同步定位与建图位姿解算方法。

背景技术

在未知环境状态下移动机器人感知技术，其依据的核心为同步定位与地图构建SLAM(Simultaneous Localization and Mapping,SLAM)，它是实现机器人自主移动的关键。视觉SLAM从匹配原理上分为直接法与间接法两类。其中，基于间接法的主流方法是RaúlMur-Artal和J.M.M.Montiel提出的ORB-SLAM，在PTAM的基础上提出了初始化算法自动计算初始位姿，提取高速的ORB(Oriented FAST and Rotated BRIEF，ORB)图像特征点并使用G2O(General Graph Optimization，G2O)库进行全局误差最小化，使其更适应于大规模场景；添加了回环检测线程，引进本质图的概念来加速闭环校正过程。

基于直接法的是Engel J在2016年提出的DSO(Direct Sparse Odometry，DSO)算法，添加了光度标定模型，直接采用像素点构造光度误差项参与优化，与直接法相比极大地简化了运算量，且应用于弱纹理环境具有高鲁棒性。

在视觉惯性融合算法中，目前主要的融合方法为紧耦合的基于滤波和基于优化两类。基于滤波的有rovio算法，将IMU与图像帧和点云一起进行估计，并提出特征块辅助EKF过程，解决参与优化点的不确定性，预测过程估计块位置，在更新步骤中使用特征块求解投影后点强度差值，更新优化状态量。

对于基于优化的视觉惯性系统中，Okvis算法基于关键帧进行耦合优化减少了计算量，通过IMU偏差项以完全概率的方式与路标重投影误差构造目标函数进行共同非线性优化，边缘化旧状态以限制复杂性。

发明内容

本发明的目的在于提供了一种具有运动性、快速性、高精度、误差小的单目同步定位与建图的机器人位姿解算方法。

本发明的技术方案如下：一种融合惯性测量单元的单目同步定位与建图的高精度位姿解算方法，其特征是：至少包含以下步骤：

步骤1)获取机器人视觉信息，依据机器人视觉信息和机器人惯性信息确定机器人位姿；

步骤2)依据步骤1)给出机器人视觉信息坐标系C相对于世界坐标系W上的平移分量；

步骤3)给出视觉与惯性在后端的误差项，优化目标函数构造及位姿求解。

所述的步骤1)包括以下步骤：

步骤1.1前端初始位姿估计，特别是去畸变帧间位姿估计：

步骤1.1，通过机器人视觉传感器，获取机器人视觉图像信息，对当前图像帧的像素点p_i投影得到对应点p_j，投影公式：

p_j表示像素点p_i投影后对应像素点，p_i表示当前图像帧中的第i个像素点，K表示相机内参矩阵，r²＝u²+v²表示像到光轴中心的距离，即像素点p_i在像素坐标系上横轴变量与纵轴变量的平方和，r⁴表示r²的平方，(·)^-1表示取逆操作，exp(·)表示取指数操作，ξ_k,k+1表示k到k+1时刻位姿的李代数形式，(·)^{^}表示将六维向量转变成四维矩阵的从向量到矩阵操作。

步骤1.2对步骤1.1)的投影公式进行非线性优化后得到位姿的更新量Δξ，利用更新量对步骤1.1.1中位姿的李代数形式ξ_k,k+1进行迭代更新：ξ_{k,k+1_update}←ξ_{k,k+1_cur}+Δξ，得到更新后的ξ_{k,k+1_update}就是前端初始位姿；

步骤1.3，通过机器人惯性传感器获取机器人惯性信息，对机器人惯性信息的实际测量值进行预积分：

表示IMU坐标系相对于世界坐标系上的从第i时刻到第k时刻的旋转分量，

表示IMU坐标系相对于世界坐标系上的从第i时刻到第k时刻的速度分量，

表示IMU坐标系相对于世界坐标系上的从第i时刻到第k时刻的平移分量。

表示IMU第i次采样得到的真实角速度测量值，

表示IMU第i次采样得到的真实加速度测量值，g_W表示重力矢量，

分别表示第i时刻时的陀螺仪偏差、加速度计偏差，η^gd、η^ad分别表示陀螺仪、加速度计的离散化噪声。

所述的步骤2包括以下步骤：

步骤2.1根据步骤1.3的预积分所得的旋转分量、速度分量、平移分量以及惯性测量单元(IMU)模块固有偏差b^g、b^a，定义k时刻下相机采集的图像帧状态x_k对应的优化变量为：

x_k＝[_Wp_k,_Wv_k,_Wq_k,b_g,b_a,s]

x_k表示k时刻时图像帧状态优化变量，_Wp_k表示在世界坐标系中在k时刻时的平移分量；_Wv_k表示在世界坐标系中在k时刻时的速度分量；_Wq_k表示在世界坐标系中在k时刻时的旋转分量；b_g表示陀螺仪偏差；b_a表示加速度计偏差；s表示尺度因子，使视觉光度误差项与惯性误差项中的位姿信息保持尺度一致性。

步骤2.2根据步骤1.1.1的视觉投影公式和步骤1.1.3的惯性测量单元(IMU)预积分中的各个变量，定义融合惯性测量单元后的全局优化状态变量为

x＝[x₁,x₂,...,x_m,k₁,k₂,k_c,ρ₁,...,ρ_n]

x_k表示待求解的全局优化状态变量，x₁,x₂,...,x_m表示有m个图像帧状态优化变量，k₁,k₂表示径向畸变参数，k_c表示单目相机内参矩阵，ρ₁,...,ρ_n表示n个不同像素点对应的逆深度。

步骤2.3对步骤1.1.3的惯性测量单元(IMU)预积分中的旋转分量、平移分量与步骤1.1.1的视觉投影公式中位姿的旋转分量、平移分量，定义尺度因子s，使得惯性测量单元(IMU)预积分中的旋转分量、平移分量与视觉视觉投影公式中位姿的旋转分量、平移分量之间满足：

s·_WCp＝_WBq_WBp-_WCq·_CBq·_CBp

s表示定义的尺度因子，_WCp表示相机坐标系C相对于世界坐标系W上的平移分量，_WBq表示IMU坐标系B相对于世界坐标系W上的旋转分量，_WBp表示IMU坐标系B相对于世界坐标系W上的平移分量，_WCq表示相机坐标系C相对于世界坐标系W上的旋转量，_CBq表示IMU坐标系B相对于相机坐标系C上的旋转分量，_CBp表示IMU坐标系B相对于相机坐标系C上的平移分量。

所述的步骤3)包括以下步骤：

步骤3.1给出视觉的光度误差项

步骤3.1.1给出视觉的光度误差项，对第k帧图像上的像素点p_i与投影后第k+1帧上的对应点p_j构造光度误差项：

I_k(c_i)表示在第k时刻时的像素点p_i对应的灰度值，I_k+1(p_j)表示在第k+1时刻时的像素点p_j对应的灰度值。

步骤3.1.2对光度误差项进行最大后验估计得到后端位姿优化目标函数的视觉分量：

步骤3.2给出惯性测量单元(IMU)的误差项：

步骤3.2.1利用惯性测量单元(IMU)采样测量值进行预积分获得的理想测量值与实际测量值来给出惯性测量单元(IMU)的误差项：

其中，

表示[k,k+1]区间内关于IMU测量值预积分的残差项，包括旋转差值

速度差值

位移差值

陀螺仪偏差差值

加速度计偏差差值

五部分。

表示从k时刻到i时刻对IMU采样值的实际预积分值。

步骤3.2.2对惯性测量单元(IMU)的误差项进行最大后验估计并转换为最小二乘问题得到后端位姿优化目标函数的IMU惯性分量：

其中∑_k,k+1是关于IMU预积分误差项中噪声项的协方差矩阵。∑_k,k+1的构成过程如下：

a)假定预积分后噪声项为:

表示对[k,k+1]区间内IMU测量值预积分之后的总噪声，δφ_k,k+1、δv_k,k+1、δp_k,k+1分别表示步骤1.1.3中旋转分量、速度分量、平移分量的噪声分量。

b)将各个噪声项线性展开:

c)将b)中的噪声项简写成：

d)根据

与

的分布，可以得到预积分总噪声的分布，即IMU预积分信息矩阵为：

其中，∑_k,i+n-1为上一时刻预积分的信息矩阵，Σ_η为IMU模块固有噪声的信息矩阵,A_k,i+n-1表示第一个矩阵，(·)^T表示取转置操作，∑_k,i+n-1表示噪声项

的信息矩阵，B_i+n-1表示第二个矩阵，Σ_η表示噪声项

的信息矩阵。

步骤3.3优化目标函数求解

步骤3.3.1构建后端位姿优化的目标函数

θ^*表示待求解的全局优化状态变量的目标函数，r₀表示先验信息。

步骤3.3.2使用优化目标函数求解位姿

(1)步骤3.1.1中的视觉光度误差项的雅可比矩阵求解：对视觉误差项中关于全局优化状态变量x包含的每一个变量求偏导，得到视觉光度误差项对位姿、地图点逆深度、相机内参以及畸变的雅可比矩阵。

表示视觉光度误差项对图像帧状态x_k对应的优化变量的雅可比矩阵，

表示视觉光度误差项对旋转分量的雅可比矩阵，u_||表示像素坐标系上的像素点坐标的归一化向量，

分别表示视觉光度误差项对地图点逆深度、相机内参以及畸变的雅可比矩阵。

(2)IMU惯性误差项的雅可比矩阵求解：对k、k+1时刻分别求IMU惯性误差项中图像帧状态x_k对应的优化变量的雅可比矩阵。

其中，每行代表

对单个优化状态x_k中的各变量的偏导。

步骤3.2.1中的关于IMU测量值预积分的误差项，即旋转差值

速度差值

位移差值

对预积分后才进行更新的偏差项求偏导如下：

(3)将上述的雅可比矩阵构造为全局雅可比矩阵J，对步骤3.2.1中的d)所述的IMU预积分信息矩阵分别对步骤1.2中的全局优化变量中的每个变量求雅可比矩阵J，对步骤3.3.1中的目标函数f(x)使用高斯牛顿法J^TJΔx＝-J^Tf(x)求解，得到关于全局状态变量x的更新量Δx。利用更新量对步骤1.2中全局状态变量x中的每个变量进行迭代更新：x_update←x_cur+Δx，得到更新后的x_update中的图像帧状态x_{k_update}即包含当前时刻解算的位姿信息，即x_{k_update}中的旋转分量q和平移分量p。

从以下几个方面进行位姿的精度对比：

1)融合惯性测量单元之后求解的位姿与真实位姿值之间求绝对轨迹误差，未融合惯性测量单元求解的位姿与真实位姿值之间求绝对轨迹误，将两个绝对轨迹误差进行误差值大小的比较；

2)融合惯性测量单元之后求解的位姿与真实位姿值之间求相对轨迹误差，未融合惯性测量单元求解的位姿与真实位姿值之间求相对轨迹误，将两个相对轨迹误差进行误差值大小的比较；

3)融合惯性测量单元之后求解的位姿与真实位姿值之间求尺度误差漂移，未融合惯性测量单元求解的位姿与真实位姿值之间求尺度误差漂移，将两个尺度误差漂移进行误差值大小的比较；

4)融合惯性测量单元之后求解的位姿与真实位姿值之间求平移分量误差漂移，未融合惯性测量单元求解的位姿与真实位姿值之间求平移分量误差漂移，将两个平移分量误差漂移进行误差值大小的比较；

5)融合惯性测量单元之后求解的位姿与真实位姿值之间求绝对轨迹误差，融合惯性测量单元的其他方法求解的位姿与真实位姿值之间求绝对轨迹误，将两个绝对轨迹误差进行误差值大小的比较。

步骤中符号定义

与现有的技术相比，本发明的有益效果具体体现在：

1)与视觉SLAM相比，不需要刻意依赖上一帧地图点，不会因为地图扩展性太慢导致跟丢，不会因为累计误差导致对后续的位姿信息定位不准确。

2)与目前紧耦合的基于滤波和基于优化两种主要融合方法相比，对算法误差的处理更加完善，预测过程和更新优化量更准确。

3)纠正视觉位姿误差累积问题，修正视觉位姿尺度，纠正IMU模块存在的偏差漂移问题，实时获取高精度位姿信息，具有良好的鲁棒性，建图精度和点云质量方面均得到了改善。

4)可以求解更高精度的位姿，从而应用于在未知环境中行走的机器人时，能够获得精准的定位，使机器人在后续自动导航过程中可以实现精准壁障、精确规划最优路线等功能。

下边结合实施例附图对本发明作进一步说明：

附图说明

图1是视觉惯性SLAM尺度因子影响图；

图2是先验信息用于优化过程因子图；

图3(a)是表示本算法估计值与真实值之间的APE；

图3(b)是表示本算法估计值与真实值之间的RPE；

图3(c)是融合IMU之前DSO的估计值与真实值之间的APE误差曲线图；

图3(d)是融合IMU之前DSO的估计值与真实值之间的RPE误差曲线图；

图4(a)的综合轨迹图表示由本算法位姿轨迹、真实值和单目位姿轨迹组成的轨迹图；

图4(b)是本算法APE对应在轨迹曲线上的误差项细节图；

图4(c)是本算法RPE对应在轨迹曲线上的误差项细节图；

图4(d)是仅单目APE对应在轨迹上的误差项细节图；

图4(e)是仅单目RPE对应在轨迹上的误差项细节图；

图5(a)是MH_05数据集的真实值与本算法位姿估计值关于平移向量在x、y、z三个轴上的拟合曲线；

图5(b)是融合IMU之前的VO以及本算法在x、y、z三个轴上的平移分量漂移量示意图；

图6是VIO\VO里程计与真值关于尺度漂移示意图；

图7(a)是未融合IMU的单目构建的地图；

图7(b)是本算法构建的地图。

具体实施方式

实施例一

一种融合IMU的单目SLAM高精度位姿解算方法，至少包含以下步骤：

步骤1)视觉与惯性在前端的初始位姿估计

1.1前端初始位姿估计

在前端对图像帧间初始位姿估计过程中，本文提出了去畸变帧间位姿估计算法，于帧间匹配过程加入畸变参数并构建光度误差函数求解视觉初始位姿。假定存在两个图像帧：当前帧和参考帧。对当前帧的像素点p_i利用投影公式得到对应点p_j：

p_j表示像素点p_i投影后对应像素点，p_i表示当前图像帧中的第i个像素点，K表示相机内参矩阵，r²＝u²+v²表示像到光轴中心的距离，即像素点p_i在像素坐标系上横轴变量与纵轴变量的平方和，r⁴表示r²的平方，(·)^-1表示取逆操作，exp(·)表示取指数操作，ξ_k,k+1表示k到k+1时刻位姿的李代数形式，(·)^{^}表示将六维向量转变成四维矩阵的从向量到矩阵操作。

步骤1.1.2对投影公式进行非线性优化后得到位姿的更新量Δξ，利用更新量对步骤1.1.1中位姿的李代数形式ξ_k,k+1进行迭代更新：ξ_{k,k+1_update}←ξ_{k,k+1_cur}+Δξ，得到更新后的ξ_{k,k+1_update}就是前端初始位姿。

步骤1.1.3对惯性测量单元(IMU)模块的实际测量值进行预积分：

表示IMU坐标系相对于世界坐标系上的从第i时刻到第k时刻的旋转分量，

表示IMU坐标系相对于世界坐标系上的从第i时刻到第k时刻的速度分量，

表示IMU坐标系相对于世界坐标系上的从第i时刻到第k时刻的平移分量。

表示IMU第i次采样得到的真实角速度测量值，

表示IMU第i次采样得到的真实加速度测量值，g_W表示重力矢量，

分别表示第i时刻时的陀螺仪偏差、加速度计偏差，η^gd、η^ad分别表示陀螺仪、加速度计的离散化噪声。

步骤2)待求解的视觉惯性变量定义

步骤2.1构造优化状态变量

已知IMU模块测量值预积分所得变量p、v、q以及固有偏差b^g、b^a，可定义k时刻下图像帧状态x_k对应的优化变量为：

x_k＝[_Wp_k,Wv_k,Wq_k,b_g,b_a,s] (2-1)

其中变量s表示尺度因子，使视觉光度误差项与惯性误差项中的位姿信息保持尺度一致性。变量p、v、q、b^g、b^a分别表示IMU预积分中的平移、速度、角度、陀螺仪偏差、加速度计偏差。全局优化状态变量的构造规则为：根据视觉与惯性紧耦合的融合特性，在后端位姿优化过程中地图点用于构造视觉帧间光度误差，因此地图点的逆深度信息是全局优化状态变量的组成参数；

步骤2.2此外，前端估计初始位姿的去畸变帧间光度误差项中包含的相机内参、畸变等参数的精度也影响后端位姿优化精度，因此本视觉惯性里程计的全局优化状态变量为：

x＝[x₁,x₂,...,x_m,k₁,k₂,k_c,ρ₁,...,ρ_n] (2-2)

式(1-2)中的x_k表示待求解的全局优化状态变量，x₁,x₂,...,x_m表示有m个图像帧状态优化变量，k₁，k₂表示图像的畸变变量，k_c表示单目相机的内参变量，ρ_i表示后端滑动窗口中未边缘化的成熟地图点的逆深度，m则代表窗口中活跃的关键帧数量。

步骤2.3尺度因子的定义

IMU模块和视觉的运动信息转换如图1所示，B、W、C分别对应IMU、世界、相机坐标系，虚线所在坐标系表示从相机坐标系到世界坐标系转换时由于尺度不确定导致的不真实尺度下的坐标系。在将B系的原点转换到W系时，通过直接转换和借助相系转换两种方案得到的T_WB存在一个尺度比例关系。两种方案获得的T_WB在理想状态下应当是一致的，但是IMU对采样值预积分后获得的变量是真实值，又由于单目视觉存在的尺度不确定性导致解算的位姿无法得到真实世界尺度，并且会随时间累积误差导致位姿的尺度信息不一致，使得借助相系转换时会转换至虚拟W系下。

图1中黑色直线条表示从当前坐标系到下一坐标的平移过程，红色弧线条表示将当前坐标系每个轴进行旋转，最终与箭头所指坐标系三个轴的朝向一致的旋转过程。为了维持IMU预积分所得位姿与视觉位姿尺度保持一致，假定存在一个尺度因子s，使得IMU采样值预积分后的q、p与视觉位姿之间有如下关系：

s·_WCp＝_WBq_WBp-_WCq·_CBq·_CBp (2-3)

其中下标代表转换过程(形如WC表示从C系转换至W系)，转换过程以IMU坐标系原点这一点进行先平移后旋转的坐标系转换，最后依据两种方案结束时坐标值的差值无限趋于零构建。

步骤3)误差项及优化目标函数构造与求解

步骤3.1给出视觉的光度误差项

优化目标函数的主要构成部分为误差项，对本文在后端位姿优化算法来说，需要将视觉与惯性误差项组建为一个整体优化目标函数才可以实现共同位姿优化过程，视觉部分基于灰度不变性原理，与前端视觉位姿估计原理一致，对第k帧图像上的像素点c_i与投影后第k+1帧上的点c_j构造光度误差项：

其中，I_k(c_i)表示在第k时刻时的像素点p_i对应的灰度值，I_k+1(p_j)表示在第k+1时刻时的像素点p_j对应的灰度值。

ξ_k,k+1表示从第k帧到第k+1帧图像所经过的位姿变换的李代数形式，(·)^表示将六维向量ξ_k,k+1转变成四维矩阵的从向量到矩阵操作。对式(3-1)进行最大后验估计，进而转换为最小二乘问题为：

步骤3.2给出惯性测量单元(IMU)的误差项：

对于IMU模块，利用IMU采样测量值进行预积分获得的理想测量值与实际测量值来构造IMU模块的误差项：

其中，

表示[k,k+1]区间内关于IMU采样值预积分的残差项，包括旋转差值

速度差值

位移差值

陀螺仪偏差差值

加速度计偏差差值

五部分。

表示从k时刻到i时刻对IMU采样值的实际预积分值。此外，对IMU测量值进行预积分的过程中b_g，b_a也有更新量。本文假设偏差在预积分的迭代过程中有小量δb_g、δb_a对当前采样时的偏差b_g和b_a进行累积，并对预积分的

和

中的偏差b_g，b_a进行更新。此处取相对位移增量

在偏差处运用泰勒一阶近似获得的

和

构造关于偏差b_g，b_a的误差项。对IMU模块误差项进行最大后验估计并转换为最小二乘问题：

式(3-4)便是组成目标函数的IMU惯性分量，其中∑_k,k+1是关于IMU预积分误差项中噪声项的协方差矩阵。此处假定预积分后的噪声项为：

三个噪声由IMU模块固有噪声η_g、η_a和预积分噪声δφ_k,i、δv_k,i等变量构成，并且与固有噪声呈非线性关系。由于固有噪声服从高斯分布，首先对每项进行一阶近似，可以获得噪声

的各噪声分量的分布呈线性，然后对各分量δφ_k,k+1，δv_k,k+1，δp_k,k+1进行线性展开，得到

的噪声构成如下：

此处可以将式(3-6)简写为：

根据

与

的分布，可以得到预积分总噪声的分布，即IMU预积分信息矩阵为：

其中，∑_k,i+n-1为上一时刻预积分的信息矩阵，Σ_η为IMU模块固有噪声的信息矩阵。

步骤3.3优化目标函数求解

经过上述对视觉和IMU模块分别进行误差项分析及优化项构建后，后端位姿优化的目标函数可以定义为：

优化目标函数的第一项为先验信息，它是与优化状态相关的信息，在之前时刻进行后端位姿优化后得到的。在后端滑窗位姿优化过程中，选取包含信息多于窗口中其他帧的第四帧优化信息作为先验。添加了先验的优化因子图如下图2所示。

灰色方块代表先验因子，用于给第k时刻优化时提供优化信息，它来源于之前时刻IMU和视觉共同优化的结果；紫色方块代表IMU预积分因子，对两个时刻之间的数据进行预积分；黄色方块代表偏差因子，需要注意的是偏差在预积分结束后进行更新；橙色方块表示尺度因子、内参、畸变和逆深度等多参数与各时刻的位姿信息进行联合优化的视觉因子。该图展示了在后端优化中各变量之间的关联。

基于式(3-2)优化状态变量的定义以及视觉和IMU模块误差项的构造，在对目标函数进行优化求解时可以细化到分别对视觉误差项和IMU模块误差项中各优化变量的求解。对式(3-1)的视觉误差项中每个变量求偏导得到视觉部分的雅可比矩阵如下：

对于全局优化状态变量x中的每个分量求偏导如下：

式(3-11)分别表示视觉误差项对相机内参、地图点逆深度以及畸变的雅可比矩阵，IMU模块的残差

涉及到预积分各分量和偏差等参数，并且存在两个时刻下的变量，因此需要对每个时刻分别求各优化状态变量的雅可比矩阵：

其中，每行代表

对单个优化状态x_k中的各变量的偏导。由于偏差在区间[k,k+1]内不更新，因此只求k时刻关于偏差项的雅可比矩阵，但是IMU在区间[k,k+1]内进行预积分时IMU偏差项也以小量δb_g、δb_a的形式对当前采样时的偏差b_g，b_a进行累积，因此式(3-3)中的

对预积分后才进行更新的偏差项求偏导如下：

在对式(3-9)中视觉和惯性误差项分别求式(2-2)中每个变量的雅可比矩阵后，利用视觉惯性方法中所述方法将本文中的雅可比矩阵构造为全局雅可比矩阵J，对J、式(2-2)全局状态变量x和式(3-9)目标函数f(x)使用高斯牛顿法J^TJΔx＝-J^Tf(x)求解，得到关于全局状态变量x的更新量Δx。利用更新量对式(2-2)中的每个变量进行迭代更新：x_update←x_cur+Δx，更新后的x_update中即包含当前时刻解算的位姿信息。

步骤4)实验结果与分析

步骤4.1精度分析依据标准

在本算法对实验结果精度分析上，我们将融合IMU前后的轨迹估计值与地面真实值进行对比，由于最终输出的位姿部分代表了轨迹信息，因此当系统运行所得轨迹与地面真实值之间的误差足够小时，即达到了本系统的高精度的指标。误差描述主要采取两种指标：绝对轨迹误差和相对位姿误差。

对于绝对轨迹误差(ATE)，又称为绝对位姿误差(APE)，原理为将估计值与真实值之间对轨迹进行绝对距离评估。两者以一定的标准对齐，然后计算偏差：

其中，F_i表示i时刻对应的整体绝对轨迹误差，P_i代表本算法后端优化的位姿信息，由式(4-1)中对应的旋转p_i、平移q_i组成。Q_i代表与P_i对应的数据集真实值位姿信息。同时，根据上述误差公式可以计算所有时间上的均方根误差：

上式是以F_i的平移分量q_i参与的计算。

相对位姿误差(RPE)不像APE强调整体上的一致性，而是注重于估计值与真实值之间局部范围内的差异。因此可以将RPE定义为：

其中，Δ表示比对的局部范围的大小，默认为1。根据E_i我们亦可以计算关于RPE的均方根误差：

步骤4.2位姿解算精度对比

在本例中，我们以EuRoc数据集的MH_05为例，对本算法位姿估计值与真实值进行以APE和RPE为主的精度分析讨论。如图3所示，其中图中的标题代表着两条轨迹的配准标准，横轴皆代表时间戳对应的时刻值。图3(a)表示本算法估计值与真实值之间的APE，图中曲线为在不同时间戳对应下，本算法位姿估计值与真实值之间的绝对轨迹误差值连接而成，可以看出，误差值的整体波动趋于平稳并且随着时间戳的增大误差值在减小，在时间戳为80+后，误差值低于绿色线条所对应均方根误差值；图3(b)为两者的RPE，图中曲线为在不同时间戳对应下，本算法位姿估计值与真实值之间的相对轨迹误差值连接而成，相对轨迹误差表示相邻帧位姿之间的误差差异，在横轴70+处对应差异较大，但整体上帧间位姿差异趋于平稳，且绿色线条所在的整体RPE均方根误差为0.11+，误差值较小；图3(c)为融合IMU之前DSO的估计值与真实值之间的APE误差曲线图，可以看出，相较于图3(a)曲线来说，融合IMU之前的APE曲线对应的纵坐标绝对轨迹误差值远高于本算法APE值，误差较高；图3(d)为两者的RPE误差曲线图，绿色线条所在的整体RPE均方根误差为0.6+，相较于图3(b)的0.11+来说，融合IMU之前的RPE相对轨迹误差值远大于本算法RPE值，并且在横轴40+处曲线RPE值有明显的跳变，局部对位姿的估计具有不稳定性。表1给出了融合IMU前后位姿估计值与真实值关于APE和RPE精度对比的各项数据详情，VIO_APE一列对应着图3(a)，VIO_RPE一列对应着图3(b)，VO_APE一列对应着图3(c)，VO_RPE一列对应着图3(d)。

表1 MH_05的VIO与VO误差统计

表1中的RMSE一项对应着图3各图中的绿色线条，是衡量位姿估计值精度的主要指标。从图3以及表1可以得到结论如下：

(1)本算法的APE中均方根误差为0.091455，对应图3(a)绿色线条所在APE值，单目视觉里程计DSO的APE中均方根误差为6.200442，对应图3(c)绿色线条所在APE值，图3(a)中误差值的整体波动趋于平稳并且随着时间戳的增大误差值在减小，而图3(c)融合IMU之前的APE曲线对应的纵坐标绝对轨迹误差值远高于图3(a)本算法APE值，误差较高；因此本算法与融合IMU模块之前的DSO相比在全局上拥有更高的精度。

(2)本算法的RPE中均方根误差为0.111598，对应图3(b)绿色线条所在RPE值，单目视觉里程计DSO的RPE中均方根误差为0.890695，对应图3(d)绿色线条所在RPE值。对于RPE的局部轨迹比对，我们选取Δ＝1，即对相邻帧之间进行RPE分析计算。并且图3(b)RPE曲线波动表示整体上帧间位姿差异趋于平稳，而图3(d)融合IMU之前的RPE相对轨迹误差值远大于图3(b)本算法RPE值，并且在横轴40+处曲线RPE值有明显的跳变，此处有较大的漂移，局部对位姿的估计具有不稳定性。因此，本算法融合IMU之后局部精度更高，漂移更小。

通过对本算法实验结果位姿估计值进行与真实值之间的精度分析，并将其与融合IMU前进行关于精度的对比分析得到上述结论，验证了本算法在融合IMU之后的高精度性。接下来将从轨迹图以及坐标轴三个分量上对实验结果进行讨论。图4给出的是综合轨迹图和表1各项误差值对应在轨迹上与真实值之间的细节图。

图4(a)的综合轨迹图表示由本算法位姿轨迹、真实值和单目位姿轨迹组成的轨迹图，三者以同一标准进行对齐，虚线表示真实值，蓝色线条代表本算法实验位姿轨迹，绿色线条表示仅单目位姿轨迹。可以明显看出融合IMU之前的轨迹与本算法所绘轨迹之间存在尺度比例差异，本算法所绘轨迹可以高度契合真实值。此外，图(b)、(c)分别代表本算法APE、RPE对应在轨迹上的误差项细节图，图(d)、(e)分别代表仅单目APE、RPE对应在轨迹上的误差项细节图，以颜色标记估计位姿轨迹与真实值之间误差计算时误差数值大小的分布状况。从图(b)、(c)、(d)、(e)中误差值的展示可以得出本算法的误差值远小于单目里程计DSO误差值，而且图(e)中有一段深红色标记的轨迹段，体现在图3(d)中时间轴40+处有明显的离群值存在，联合表1中的VO_RPE一项中的RMSE与Max之间的差值可知，在此处单目里程计DSO对位姿的估计误差累积过大，不如本算法的局部稳定性。

空间坐标轴上的移动分量图如图5所示，图5(a)为MH_05数据集的真实值与本算法位姿估计值关于平移向量在x、y、z三个轴上的拟合曲线，虚线代表真实值，蓝色实线表示本算法实验值，从图中可以看出，两者高度拟合。图5(b)为融合IMU之前的VO以及本算法在x、y、z三个轴上的平移分量漂移量示意图，本算法不仅在数量级上优于融合IMU之前的单目VO，而且在平移分量数值波动上也趋于稳定；因此从位移分量图中体现出本算法实验数据的高精度性。

针对本文提出的尺度因子联合优化视觉和惯性位姿值的方法，图6给出了融合IMU之前与本算法实验数据与真实值之间存在的尺度因子漂移示意图。从图中可以看出，本算法在尺度上的漂移量远小于融合IMU之前的单目DSO，并且方面，本算法设计尺度因子并对其优化估计后，尺度的数值波动明显趋于平稳，因此可以说明本算法提出的尺度因子联合优化方案能够改善单目尺度不确定性。

最后，本算法将与融合IMU的主流视觉惯性方法Okvis进行APE精度的比较。上述对比过程是针对MH_05一个数据集的结果，接下来将对官方提供的EuRoC中的各数据集依次进行比对。具体数据如表2所示。

表2各数据集上的精度对比表

表2中的APE数据是RMSE项的数值，从表中可以看出，我们提出的方法在各数据集上均方根误差均小于主流的Okvis方法。

综合上述实验数据的各项分析，我们的算法在4.1小节两大精度分析指标、图4轨迹以及图5平移变量的各轴分量变化等方面与融合IMU之前的单目DSO相比都具有高精度性质，并且与主流的Okvis方法相比，亦具有高精度性。

进一步实验将讨论本算法所构建的地图与未融合IMU的算法构建的地图进行比较分析。构建的地图如图7所示，图7(a)为未融合IMU的单目构建的地图，图7(b)为本算法构建的地图。从图中可以看出，本算法在融合IMU后，构建的点云图在整体上更加规整，冗余残差点减少，在墙壁、窗户、桌面和地面等处构建的形状更加清晰，能较好地展现细节。

本发明提出了单目DSO和惯性相融合的视觉惯性里程计。在前端视觉帧初始位姿估计中添加畸变参数构建视觉光度误差函数求解视觉初始位姿；构造本算法的全局优化状态变量以减少优化时各变量的误差累积；在后端位姿优化中，提出基于紧耦合的多参数滑窗位姿优化算法，计算尺度因子使视觉误差项与IMU预积分误差项位姿信息保持尺度一致，选取特定帧对应的先验信息，构建关于先验信息、视觉误差项和惯性误差项的整体优化目标函数，对目标函数求全局状态变量的雅可比矩阵，利用优化解更新迭代全局状态变量，完成当前时刻下的高精度位姿解算。最后对本视觉惯性里程计实验数据与融合IMU之前的DSO以及当前主流视觉惯性方法进行精度上的对比分析，并对本算法在构建点云图精度上的改善进行说明。实验结果分析表明，本发明在融合IMU之后具有高精度的位姿解算结果。

Claims (4)

1.一种融合惯性测量单元的单目同步定位与建图的高精度位姿解算方法，至少包含以下步骤：

步骤1)视觉与惯性在前端的初始位姿估计；

步骤2)待求解的视觉惯性变量定义；

步骤3)视觉与惯性在后端的误差项及优化目标函数构造及位姿求解；

所述的步骤1)包括以下步骤：

步骤1.1前端初始位姿估计，特别是去畸变帧间位姿估计，至少包括：

步骤1.1.1对当前帧的像素点p_i投影得到对应点p_j，投影公式：

p_j表示像素点p_i投影后对应像素点，p_i表示当前图像帧中的第i个像素点，K表示相机内参矩阵，r²＝u²+v²表示像到光轴中心的距离，即像素点p_i在像素坐标系上横轴变量与纵轴变量的平方和，r⁴表示r²的平方，(·)^-1表示取逆操作，exp(·)表示取指数操作，ξ_k,k+1表示k到k+1时刻位姿的李代数形式，(·)^{^}表示将六维向量转变成四维矩阵的从向量到矩阵操作；

步骤1.1.2对投影公式进行非线性优化后得到位姿的更新量Δξ，利用更新量对步骤1.1.1中位姿的李代数形式ξ_k,k+1进行迭代更新：ξ_{k,k+1_update}←ξ_{k,k+1_cur}+Δξ，得到更新后的ξ_{k,k+1_update}就是前端初始位姿；

步骤1.1.3惯性测量单元用IMU表示，对惯性测量单元模块的实际测量值进行预积分：

表示IMU坐标系相对于世界坐标系上的从第i时刻到第k时刻的旋转分量，

表示IMU坐标系相对于世界坐标系上的从第i时刻到第k时刻的速度分量， _WB p _i ^k 表示IMU坐标系相对于世界坐标系上的从第i时刻到第k时刻的平移分量；

表示IMU第i次采样得到的真实角速度测量值，

表示IMU第i次采样得到的真实加速度测量值，g_W表示重力矢量，

分别表示第i时刻时的陀螺仪偏差、加速度计偏差，η^gd、η^ad分别表示陀螺仪、加速度计的离散化噪声；

所述的步骤2)包括以下步骤：

步骤2.1根据步骤1.1.3中惯性测量单元模块的测量值的预积分所得的旋转分量、速度分量、平移分量以及惯性测量单元模块固有偏差b^g、b^a，定义k时刻下相机采集的图像帧状态x_k对应的优化变量为：

x_k＝[_Wp_k,_Wv_k,_Wq_k,b_g,b_a,s]

x_k表示k时刻时图像帧状态优化变量，_Wp_k表示在世界坐标系中在k时刻时的平移分量；_Wv_k表示在世界坐标系中在k时刻时的速度分量；_Wq_k表示在世界坐标系中在k时刻时的旋转分量；b_g表示陀螺仪偏差；b_a表示加速度计偏差；s表示尺度因子，使视觉光度误差项与惯性误差项中的位姿信息保持尺度一致性；

步骤2.2根据步骤1.1.1的视觉投影公式和步骤1.1.3的惯性测量单元预积分中的各个变量，定义融合惯性测量单元后的全局优化状态变量为

x＝[x₁,x₂,...,x_m,k₁,k₂,k_c,ρ₁,...,ρ_n]

x_k表示待求解的全局优化状态变量，x₁,x₂,...,x_m表示有m个图像帧状态优化变量，k₁,k₂表示径向畸变参数，k_c表示单目相机内参矩阵，ρ₁,...,ρ_n表示n个不同像素点对应的逆深度；

步骤2.3对步骤1.1.3的惯性测量单元预积分中的旋转分量、平移分量与步骤1.1.1的视觉投影公式中位姿的旋转分量、平移分量，定义尺度因子s，使得惯性测量单元预积分中的旋转分量、平移分量与视觉投影公式中位姿的旋转分量、平移分量之间满足：

s·_WCp＝_WBq_WBp-_WCq·_CBq·_CBp

s表示定义的尺度因子，_WCp表示相机坐标系C相对于世界坐标系W上的平移分量，_WBq表示IMU坐标系B相对于世界坐标系W上的旋转分量，_WBp表示IMU坐标系B相对于世界坐标系W上的平移分量，_WCq表示相机坐标系C相对于世界坐标系W上的旋转量，_CBq表示IMU坐标系B相对于相机坐标系C上的旋转分量，_CBp表示IMU坐标系B相对于相机坐标系C上的平移分量；

所述的步骤3)包括以下步骤：

步骤3.1给出视觉的光度误差项；

步骤3.2给出惯性测量单元的误差项；

步骤3.3优化目标函数求解。

2.根据权利要求1所述一种融合惯性测量单元的单目同步定位与建图的高精度位姿解算方法，其特征是：述的步骤3.1具体包括：

步骤3.1.1给出视觉的光度误差项，对第k帧图像上的像素点p_i与投影后第k+1帧上的对应点p_j构造光度误差项：

I_k(c_i)表示在第k时刻时的像素点c_i对应的灰度值，I_k+1(p_j)表示在第k+1时刻时的像素点p_j对应的灰度值；

步骤3.1.2对光度误差项进行最大后验估计得到后端位姿优化目标函数的视觉分量：

3.根据权利要求1所述一种融合惯性测量单元的单目同步定位与建图的高精度位姿解算方法，其特征是：所述的步骤3.2具体包括：

步骤3.2.1利用惯性测量单元采样测量值进行预积分获得的理想测量值与实际测量值来给出惯性测量单元的误差项：

其中，

表示[k,k+1]区间内关于IMU测量值预积分的残差项，包括旋转差值

速度差值

位移差值

陀螺仪偏差差值

加速度计偏差差值

五部分；

表示从i时刻到k时刻对IMU采样值的实际预积分值；

步骤3.2.2对惯性测量单元的误差项进行最大后验估计并转换为最小二乘问题得到后端位姿优化目标函数的IMU惯性分量：

其中∑_k,k+1是关于IMU预积分误差项中噪声项的协方差矩阵；∑_k,k+1的构成过程如下：

a)假定预积分后噪声项为:

表示对[k,k+1]区间内IMU测量值预积分之后的总噪声，δφ_k,k+1、δv_k,k+1、δp_k,k+1分别表示步骤1.1.3中旋转分量、速度分量、平移分量的噪声分量；

b)将各个噪声项线性展开:

c)将b)中的噪声项简写成：

d)根据

与

的分布，可以得到预积分总噪声的分布，即IMU预积分信息矩阵为：

其中，∑_k,i+n-1为上一时刻预积分的信息矩阵，Σ_η为IMU模块固有噪声的信息矩阵,A_k,i+n-1表示第一个矩阵，(·)^T表示取转置操作，∑_k,i+n-1表示噪声项

的信息矩阵，B_i+n-1表示第二个矩阵，Σ_η表示噪声项

的信息矩阵。

4.根据权利要求1所述一种融合惯性测量单元的单目同步定位与建图的高精度位姿解算方法，其特征是：所述的步骤3.3具体包括：

步骤3.3.1构建后端位姿优化的目标函数

θ^*表示待求解的全局优化状态变量的目标函数，r₀表示先验信息；

步骤3.3.2使用优化目标函数求解位姿包括：

(1)步骤3.1.1中的视觉光度误差项的雅可比矩阵求解：对视觉误差项中关于全局优化状态变量x包含的每一个变量求偏导，得到视觉光度误差项对位姿、地图点逆深度、相机内参以及畸变的雅可比矩阵；

表示视觉光度误差项对图像帧状态x_k对应的优化变量的雅可比矩阵，

表示视觉光度误差项对旋转分量的雅可比矩阵，u_||表示像素坐标系上的像素点坐标的归一化向量，

分别表示视觉光度误差项对地图点逆深度、相机内参以及畸变的雅可比矩阵；

(2)IMU惯性误差项的雅可比矩阵求解：对k、k+1时刻分别求IMU惯性误差项中图像帧状态x_k对应的优化变量的雅可比矩阵；

其中，每行代表

对单个优化状态x_k中的各变量的偏导；

步骤3.2.1中的关于IMU测量值预积分的误差项，即旋转差值

速度差值

位移差值

对预积分后才进行更新的偏差项求偏导如下：

(3)将上述的雅可比矩阵构造为全局雅可比矩阵J，对步骤3.2.1中的d)所述的IMU预积分信息矩阵分别对步骤1.2中的全局优化变量中的每个变量求雅可比矩阵J，对步骤3.3.1中的目标函数f(x)使用高斯牛顿法J^TJΔx＝-J^Tf(x)求解，得到关于全局状态变量x 的更新量Δx；利用更新量对步骤1.2中全局状态变量x中的每个变量进行迭代更新：x_update←x_cur+Δx，得到更新后的x_update中的图像帧状态x_{k_update}即包含当前时刻解算的位姿信息，即x_{k_update}中的旋转分量q和平移分量p。

Images