CN110455301A - 一种基于惯性测量单元的动态场景slam方法 - Google Patents

Abstract

本发明涉及一种基于惯性测量单元的动态场景SLAM方法，包括以下步骤：采集IMU数据：获得t时刻的加速度计测量的瞬时加速度值陀螺仪测量的瞬时角速度和双目相机数据N(t)；通过瞬时加速度值分析系统加速度在一定时间内变化值γ_a，并与预设阈值γ_k进行对比，当γ_a＞γ_k时，则融合IMU数据，矫正系统位姿，当γ_a≤γ_k时，则处理相机数据。本发明设计合理，在保证系统解算速度的前提下，提高了SLAM系统的鲁棒性，并建立精准地图，有效克服了动态场景、剧烈运动对SLAM系统的影响。

Description

一种基于惯性测量单元的动态场景SLAM方法

技术领域

本发明属于机器人视觉导航技术领域，尤其是一种基于惯性测量单元的动态场景SLAM方法。

背景技术

SLAM(simultaneous localization and mapping)指基于视觉的即时定位与地图构建，该技术能够使未获得环境先验信息的机器人在运动状态下，通过视觉传感器的数据信息完成建图任务，是机器人学的热门方向，目前SLAM多应用于机器人导航、AR、VR与自动驾驶等领域，并推动了一批相关技术的发展。

按照机器人使用传感器的不同，如激光雷达或者相机(单目、双目)，SLAM技术分为激光SLAM和视觉SLAM。激光SLAM发展较早，技术较为成熟；视觉SLAM出现较晚，但是具有成本低，图像含有丰富信息等优点，逐渐成为主流并具有潜力的SLAM方法。

视觉SLAM技术相对于其他定位技术，精度较高，但是，其依赖环境的特征信息，在环境纹理缺失或者动态场景下容易产生误匹配、稳定性较差。惯性测量元件(IMU，InertialMeasurement Unit)虽然可以获得传感器x、y、z三轴的角速度和加速度，测量不受外界因素影响，但是在每一时刻产生的误差会随着时间累计发生漂移现象。如何将上述技术结合在一起，在保证系统计算量的同时有效提高SLAM算法的鲁棒性，适应动态场景下的剧烈运动并实现动态场景SLAM功能是目前迫切需要解决的问题。

发明内容

本发明的目的在于克服现有技术的不足，提出一种设计合理、性能稳定且安全可靠的基于惯性测量单元的动态场景SLAM方法。

本发明解决其技术问题是采取以下技术方案实现的：

一种基于惯性测量单元的动态场景SLAM方法，包括以下步骤：

步骤1、采集IMU数据：获得t时刻的加速度计测量的瞬时加速度值陀螺仪测量的瞬时角速度和双目相机数据N(t)；

步骤2、通过瞬时加速度值分析系统加速度在一定时间内变化值γ_a，并与预设阈值γ_k进行对比，当γ_a＞γ_k时，执行步骤3、4，当γ_a≤γ_k时，执行步骤5；

步骤3、对获得的IMU数据进行预积分处理，使两个传感器数据在测量模型中的频率保持一致；

步骤4、通过IMU提供平移向量T和旋转矩阵R预测相机位姿，使用初始模型追寻初始化后的两帧图像，通过融合IMU的旋转矩阵R、速度V、平移向量T对相机的位姿进行矫正，更新地图并跳转回步骤1；

步骤5：使用前一帧的速度和相对位姿估计当前时刻的位姿，通过视觉重投影误差构造损失函数，使用图优化的方法进行位姿估计，更新地图并跳转回步骤1。

进一步，所述步骤3采用如下预积分测量模型，对IMU数据进行预积分处理：

其中：是陀螺仪到旋转矩阵的雅各比矩阵展开式，为速度的雅各比矩阵展开式；δb^g，δb^a是陀螺仪和加速度计的变化量。

进一步，所述步骤4的具体实现方法为：

设当前的关键帧为k，上一时刻关键帧为k-1，则构建优化方程和优化变量：

优化变量

相对于恒速模型下通过图优化进行位姿估计，增加了IMU的速度、陀螺仪零偏、加速度零偏：

系统的目标损失函数：

式中，∑_k∈LE_proj(k)是当前帧对应的局部地图点的的视觉重投影误差，

在关键帧k，∑_k∈LE_proj(k)定义为：

其中，∑_k-1是关键帧的信息矩阵，X_w是在相机坐标下的地图点，E_imu(k-1，k)是IMU的误差项，ρ( )是Huber鲁棒代价函数，y^k-1是重投影到当前帧图像上的特征点；

IMU的误差项E_imu(k-1，k)：

其中，∑_I是IMU预积分的信息矩阵，∑_R是IMU的零偏随机游走的信息矩阵；

通过高斯牛顿法对上述优化模型进行求解，联合IMU的预积分值和 作为优化的初始值，融合IMU的旋转矩阵R、速度V、平移向量T对相机的位姿进行矫正。

进一步，所述步骤5的具体实现方法为：

考虑状态变量和运动方程：

X＝[x₁x₂…x_ky₁y₂…y_j]

将相机的姿态和位置构建拓扑图，该拓扑图由许多顶点及连接顶点的若干条边构成，优化变量用顶点表示，误差项用边表示，表示为式G{V，E}，

相机的六自由度姿态V表示为：

表示姿态的变换关系E表示为：

构建优化函数：

e(x_i，y_j，r_i，j)＝r_i，j-g(x_i，y_j)

上式中，x_i表示i时刻的相机位姿，在图优化中称为位姿节点；y_j表示j时刻相机观测到的路标，称为路标节点；r_i，j表示位姿节点和路标节点之间的约束。

本发明的优点和积极效果是：

本发明设计合理，其通过IMU测量数据，分析系统加速度在一定时间内变化并与预设阈值进行对比，当系统运动较为平稳，依靠双目视觉可以精确、完整地跟踪系统的运动；当系统正处于剧烈运动时，在视觉SLAM中融合IMU数据，矫正系统位姿。从而构建两种视觉和惯性测量单元融合的优化模型，在保证系统解算速度的前提下，提高了SLAM系统的鲁棒性，并建立精准地图，有效克服了动态场景、剧烈运动对SLAM系统的影响。

附图说明

图1是本发明的处理流程图；

图2是本发明的IMU与世界坐标系之间的变换原理图；

图3是本发明的预积分前后的各信息节点关系示意图；

图4是本发明的位姿节点与路标节点之间的约束关系示意图。

具体实施方式

以下结合附图对本发明实施例做进一步详述。

本发明的设计思想是：通过IMU测量数据，分析系统加速度在一定时间内变化(下文用γ_a表示)，并与预设阈值(下文用γ_k表示)进行对比。当γ_a≤γ_k(加速度变化小于、等于阈值)，说明此时系统运动较为平稳，依靠双目视觉可以精确、完整地跟踪系统的运动；当γ_a＞γ_k，说明此时系统正处于剧烈运动(速度变化快，系统转向角度大)的复杂情况，此时，融合IMU数据，矫正系统位姿。通过约束算法流程构建两种观测方程，在剧烈运动情况下，系统融合IMU数据，而在平稳情况下，不需要融合IMU信息，这样在节省系统计算量的同时，从而加强了SLAM的精确性和鲁棒性。

本发明的基于惯性测量单元的动态场景SLAM方法，如图1所示，包括以下步骤：

步骤1：采集IMU数据，获得t时刻的IMU加速度计测量的瞬时加速度值陀螺仪测量的瞬时角速度和双目相机数据N(t)。

步骤2：通过IMU数据分析系统加速度在一定时间内变化(下文用γa表示)，并与预设阈值(下文用γ_k表示)进行对比，建立两种优化模型。当γ_a＞γ_k(加速度变化大于阈值)，说明此时系统正处于剧烈运动(速度变化快，系统转向角度大)的复杂情况，此时，采用步骤3、步骤4融合IMU数据，矫正系统位姿；当γ_a≤γ_k(加速度变化小于、等于阈值)，说明此时系统运动较为平稳，采用步骤5处理相机数据。

步骤3：在获得IMU数据后，进行预积分处理，使两个传感器数据在测量模型中的频率保持一致。具体方法为：

首先给出IMU坐标系和世界坐标系之间的变换关系，如图2所示。IMU坐标系到世界坐标系通过旋转和平移变换P_WB＝(R_WB，T_WB)得到，其中，用R_WB表示旋转矩阵，T_WB表示平移矩阵。

其中，a_w(t)为当前时刻瞬时加速度的真实值，ω_WB(t)为当前时刻瞬时角速度的真实值，g_w是世界坐标系下的重力加速度，b^a(t)、b^g(t)分别为加速度计和陀螺仪的测量随机偏差。η^a(t)、η^g(t)分别为加速度计和陀螺仪受到的噪声干扰。

实际测量中，IMU的测量值三轴加速度a和角速度ω通常受到高斯白噪声和零偏影响。因此，实际测量值是噪声、零偏、真实值的函数和。三者之间的函数关系为：

通过加速度推算出位移和速度，从角速度推算出旋转量，使用旋转矩阵法建立IMU的运动学模型，得到旋转量、速度和平移向量与IMU真实值(a_w(t)，ω_W(t)为)之间的积分关系：

由于IMU得到的瞬时角速度，瞬时加速度的值是离散的，所以将积分形式转为差分形式。

R_WB(t+Δt)＝R_WB(t)Exp(ω_WB(t)Δt)

V_WB(t+Δt)＝V_W(t)+a_W(t)Δt

IMU每一次测量的时间间隔Δt极短，故我们可以将_wa(t)和ω_WB(t)看作一个恒定值。利用上式中的(测量值)代替_wa(t)和ω_WB(t)这两个真实值。就可以通过计算IMU加速度和陀螺仪测量值获得速度、位移和旋转量。

其中上标η^ad中d表示的是离散(discrete)数据，是在t到t+Δt这一时间段内的测量值的高斯误差。

由上式，得到了时间间隔为Δt时的R_WB(t+Δt)，V_WB(t+Δt)，T_WB(t+Δt)变化关系。

在SLAM系统中，需要求解每个图像时刻的pose，相机的采样频率一般在几十Hz，而IMU的频率达到了几百Hz，所以我们把两个图像帧t＝i，t＝j之间的IMU观测量进行累加，中间时间间隔为j-i个Δt时间段，这样就得到了从i时刻到j时刻的两个图像帧之间的位姿、速度关系。

通过图3可以看出，虽然IMU和相机两个传感器采样频率不同，但是通过预积分的方法使两个传感器在测量模型中的频率保持一致，预积分的核心是把时刻i和j的IMU的测量信息通过积分的形式分成i和j两个时刻的相对位姿值，那么通过对两个时刻的加速度值和角速度值进行积分，可以把上式改写成如下形式。

式中，每次更新速度时，需要得到每个时刻IMU的World坐标系(以下简称W系)下的旋转，在每次更新平移向量时需要更新W系下的旋转和速度，因为IMU采样频率远远大于相机的采样频率，这样就造成了大量的冗余计算，计算复杂，解算时间更长。所以引入预积分概念计算两个关键帧t＝i，t＝j之间的旋转和平移运动，作为IMU的测量模型。这里把旋转和平移运动还有速度单独拿出不与IMU的测量数据共同积分，给出两个关键帧之间的测量模型。

(接近极限，k代指为i和j之间的任意时刻)

上式中多次出现故假设则ΔV_ij＝v_k-v_i.等式右边即为预积分量。至此，旋转、平移向量和速度量与i，j时刻的状态量R_i，T_i，V_i，R_j，T_j，V_i没有关系。只与t＝i，t＝j两个关键帧之间的噪声(η^a(t)、η^g(t))、零偏(b^a(t)、b^g(t))和IMU测量值有关。

通过上式可以看出测量方程与IMU的bias和噪声都有关系，并且关系十分复杂。因为噪声服从高斯正态分布，bias服从随机游走测量模型，所以两者处理方式不同。在这里我们假设bias为恒定值，(k＝i，i+1，i+2，…，j)先讨论噪声对测量模型产生的影响，然后对bias采用一阶泰勒展开方式，这样得到IMU的预积分测量模型：

其中：是陀螺仪到旋转矩阵的雅各比矩阵展开式，为速度的雅各比矩阵展开式；δb^g，δb^a是陀螺仪和加速度计的变化量。

步骤4：在对IMU数据进行预积分处理后，通过IMU提供平移向量T和旋转矩阵R预测相机位姿，使用初始模型追寻初始化后的两帧图像，通过融合IMU的R、V、T对相机的位姿进行矫正，更新地图并跳转回步骤1。

假设当前的关键帧为k，上一时刻关键帧为k-1，则构建优化方程和优化变量。

优化变量

相对于恒速模型下通过图优化进行位姿估计，增加了(IMU的速度、陀螺仪零偏、加速度零偏)。

系统的目标损失函数：

式中，∑_k∈LE_proj(k)是当前帧对应的局部地图点的的视觉重投影误差。

在关键帧k，∑_k∈LE_proj(k)定义为：

其中∑_k-1是关键帧的信息矩阵，X_w是在相机坐标下的地图点，E_imu(k-1，k)是IMU的误差项，ρ()是Huber鲁棒代价函数，y^k-1是重投影到当前帧图像上的特征点

IMU的误差项E_imu(k-1，k)：

其中∑_I是IMU预积分的信息矩阵，∑_R是IMU的零偏随机游走的信息矩阵。通过高斯牛顿法对上述优化模型进行求解，联合IMU的预积分值和等变量作为优化的初始值，可以得到鲁棒性和精确性更高的估计值。

步骤5：使用前一帧的速度和相对位姿估计当前时刻的位姿，通过视觉重投影误差构造损失函数，使用图优化的方法进行位姿估计，更新地图并跳转回步骤1。

考虑状态变量和运动方程：

X＝[x₁x₂…x_ky₁y₂…y_j]

相机的姿态和位置构建拓扑图，一个图由许多顶点(Vertex)及连接顶点的若干条边构成，优化变量用顶点表示，误差项用边表示，表示为式G{V，E}，如图4所示。

V(相机的六自由度姿态)表示为

E(表示姿态的变换关系)表示为

构建优化函数：

e(x_i，y_j，r_i，j)＝r_i，j-g(x_i，y_j)

上式中，x_i表示i时刻的相机位姿，在图优化中称为位姿节点；y_j表示j时刻相机观测到的路标，称为路标节点；r_i，j表示位姿节点和路标节点之间的约束。这样将问题转化成一个非线性最小二乘问题，使用g2o(图优化求解库)求解。

本发明未述及之处适用于现有技术。

需要强调的是，本发明所述的实施例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。

Claims (4)

1.一种基于惯性测量单元的动态场景SLAM方法，其特征在于包括以下步骤：

步骤1、采集IMU数据：获得t时刻的加速度计测量的瞬时加速度值陀螺仪测量的瞬时角速度和双目相机数据N(t)；

步骤2、通过瞬时加速度值分析系统加速度在一定时间内变化值γ_a，并与预设阈值γ_k进行对比，当γ_a>γ_k时，执行步骤3和4，当γ_a≤γ_k时，执行步骤5；

步骤3、对获得的IMU数据进行预积分处理，使两个传感器数据在测量模型中的频率保持一致；

步骤4、通过IMU提供平移向量T和旋转矩阵R预测相机位姿，使用初始模型追寻初始化后的两帧图像，通过融合IMU的旋转矩阵R、速度V、平移向量T对相机的位姿进行矫正，更新地图并跳转回步骤1；

步骤5：使用前一帧的速度和相对位姿估计当前时刻的位姿，通过视觉重投影误差构造损失函数，使用图优化的方法进行位姿估计，更新地图并跳转回步骤1。

2.根据权利要求1所述的一种基于惯性测量单元的动态场景SLAM方法，其特征在于：所述步骤3采用如下预积分测量模型，对IMU数据进行预积分处理：

其中：是陀螺仪到旋转矩阵的雅各比矩阵展开式，为速度的雅各比矩阵展开式；δb^g，δb^a是陀螺仪和加速度计的变化量。

3.根据权利要求1所述的一种基于惯性测量单元的动态场景SLAM方法，其特征在于：所述步骤4的具体实现方法为：

设当前的关键帧为k，上一时刻关键帧为k-1，则构建优化方程和优化变量：

优化变量

相对于恒速模型下通过图优化进行位姿估计，增加了IMU的速度、陀螺仪零偏、加速度零偏：

系统的目标损失函数：

式中，∑_k∈LE_proj(k)是当前帧对应的局部地图点的的视觉重投影误差，

在关键帧k，∑_k∈LE_proj(k)定义为：

其中，∑_k-1是关键帧的信息矩阵，X_w是在相机坐标下的地图点，E_imu(k-1，k)是IMU的误差项，ρ()是Huber鲁棒代价函数，y^k-1是重投影到当前帧图像上的特征点；

IMU的误差项E_imu(k-1，k)：

其中，∑_I是IMU预积分的信息矩阵，∑_R是IMU的零偏随机游走的信息矩阵；

通过高斯牛顿法对上述优化模型进行求解，联合IMU的预积分值和 作为优化的初始值，融合IMU的旋转矩阵R、速度V、平移向量T对相机的位姿进行矫正。

4.根据权利要求1所述的一种基于惯性测量单元的动态场景SLAM方法，其特征在于：所述步骤5的具体实现方法为：

考虑状态变量和运动方程：

X＝[x₁x₂…x_ky₁y₂…y_j]

将相机的姿态和位置构建拓扑图，该拓扑图由许多顶点及连接顶点的若干条边构成，优化变量用顶点表示，误差项用边表示，表示为式G{V，E}，

相机的六自由度姿态V表示为：

表示姿态的变换关系E表示为：

构建优化函数：

e(x_i，y_j，r_i，j)＝r_i，j-g(x_i，y_j)

上式中，x_i表示i时刻的相机位姿，在图优化中称为位姿节点；y_j表示j时刻相机观测到的路标，称为路标节点；r_i，j表示位姿节点和路标节点之间的约束。