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

本发明专利技术涉及一种基于惯性测量单元的动态场景SLAM方法，包括以下步骤：采集IMU数据：获得t时刻的加速度计测量的瞬时加速度值

A dynamic scene slam method based on IMU

The invention relates to a dynamic scene slam method based on an inertial measurement unit, which comprises the following steps: collecting IMU data: obtaining the instantaneous acceleration value measured by the accelerometer at time t

【技术实现步骤摘要】
一种基于惯性测量单元的动态场景SLAM方法
本专利技术属于机器人视觉导航
，尤其是一种基于惯性测量单元的动态场景SLAM方法。
技术介绍
SLAM(simultaneouslocalizationandmapping)指基于视觉的即时定位与地图构建，该技术能够使未获得环境先验信息的机器人在运动状态下，通过视觉传感器的数据信息完成建图任务，是机器人学的热门方向，目前SLAM多应用于机器人导航、AR、VR与自动驾驶等领域，并推动了一批相关技术的发展。按照机器人使用传感器的不同，如激光雷达或者相机(单目、双目)，SLAM技术分为激光SLAM和视觉SLAM。激光SLAM发展较早，技术较为成熟；视觉SLAM出现较晚，但是具有成本低，图像含有丰富信息等优点，逐渐成为主流并具有潜力的SLAM方法。视觉SLAM技术相对于其他定位技术，精度较高，但是，其依赖环境的特征信息，在环境纹理缺失或者动态场景下容易产生误匹配、稳定性较差。惯性测量元件(IMU，InertialMeasurementUnit)虽然可以获得传感器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数据进行预积分处理：其中：是陀螺仪到旋转矩阵的雅各比矩阵展开式，为速度的雅各比矩阵展开式；δbg，δba是陀螺仪和加速度计的变化量。进一步，所述步骤4的具体实现方法为：设当前的关键帧为k，上一时刻关键帧为k-1，则构建优化方程和优化变量：优化变量相对于恒速模型下通过图优化进行位姿估计，增加了IMU的速度、陀螺仪零偏、加速度零偏：系统的目标损失函数：式中，∑k∈LEproj(k)是当前帧对应的局部地图点的的视觉重投影误差，在关键帧k，∑k∈LEproj(k)定义为：其中，∑k-1是关键帧的信息矩阵，Xw是在相机坐标下的地图点，Eimu(k-1，k)是IMU的误差项，ρ()是Huber鲁棒代价函数，yk-1是重投影到当前帧图像上的特征点；IMU的误差项Eimu(k-1，k)：其中，∑I是IMU预积分的信息矩阵，∑R是IMU的零偏随机游走的信息矩阵；通过高斯牛顿法对上述优化模型进行求解，联合IMU的预积分值和作为优化的初始值，融合IMU的旋转矩阵R、速度V、平移向量T对相机的位姿进行矫正。进一步，所述步骤5的具体实现方法为：考虑状态变量和运动方程：X＝[x1x2…xky1y2…yj]将相机的姿态和位置构建拓扑图，该拓扑图由许多顶点及连接顶点的若干条边构成，优化变量用顶点表示，误差项用边表示，表示为式G{V，E}，相机的六自由度姿态V表示为：表示姿态的变换关系E表示为：构建优化函数：e(xi，yj，ri，j)＝ri，j-g(xi，yj)上式中，xi表示i时刻的相机位姿，在图优化中称为位姿节点；yj表示j时刻相机观测到的路标，称为路标节点；ri，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坐标系到世界坐标系通过旋转和平移变本文档来自技高网...

【技术保护点】
1.一种基于惯性测量单元的动态场景SLAM方法，其特征在于包括以下步骤：/n步骤1、采集IMU数据：获得t时刻的加速度计测量的瞬时加速度值

【技术特征摘要】
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数据进行预积分处理：









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


3.根据权利要求1所述的一种基于惯性测量单元的动态场景SLAM方法，其特征在于：所述步骤4的具体实现方法为：
设当前的关键帧为k，上一时刻关键帧为k-1，则构建优化方程和优化变量：
优化变量
相对于恒速模型下通过图优化进行位姿估计，增加了IMU...

【专利技术属性】
技术研发人员：张建畅，白石，万媛，张小俊，
申请(专利权)人：河北工业大学，
类型：发明
国别省市：天津;12