单目视觉惯性定位的IMU辅助跟踪模型制造技术

本发明专利技术公开了一种单目视觉惯性定位的IMU辅助跟踪模型，包括：初始化阶段和初始化完成后的阶段；初始化阶段时，在匀速模型设定当前帧初始平移的基础上，将IMU获取的原始数据做预积分，解算得到IMU先验旋转，获得当前帧的初始位姿；初始化完成后，在提供初始位姿的基础上，加入IMU预积分的先验速度信息，由以上计算的初始状态建立跟踪模型，实现精确定位。本发明专利技术能够有效的克服剧烈运动等影响。

【技术实现步骤摘要】
单目视觉惯性定位的IMU辅助跟踪模型
本专利技术涉及视觉定位
，尤其是一种单目视觉惯性定位的IMU辅助跟踪模型。
技术介绍
单目视觉惯性紧耦合定位技术近年来发展迅速，由最初的基于卡尔曼滤波的紧耦合方法逐渐发展到基于BA(BundleAdjustment光束平差法)优化和图优化的方法。AnastasiosI.Mourikis在2007年提出了一个典型的基于扩展卡尔曼滤波的紧耦合模型MSCKF，从此开启了视觉惯性紧耦合的定位方法。与传统的SLAM框架相同，MSCKF的跟踪模型是基本的视觉参考帧模型，在跟踪情况下没有使用IMU的先验数据，因此系统极易不收敛，导致定位精度较差。国外ORB_SLAM2[10]的作者RaulMur-Artal在2017年提出了具有地图重用的视觉惯性单目SLAM技术，该算法基于2015年开源的ORB_SLAM2框架，提出了一种基于ORB_SLAM的鲁棒性视觉惯性局部BA优化模型，同时提出了一种简便的视觉惯性联合初始化方法，但该方法需要15秒左右的初始化时间，因此在初始化成功之前需要稳定的视觉前端基础，来保障初始化的稳定性和准确度，而ORB_SLAM在初始化期间采用的是简单的匀速跟踪模型，在相机剧烈运动时无法正确初始化，甚至跟踪失败。香港科大机器人研究所在2017年提出了VINS-Mono：一种稳健的单目视觉惯性状态估计器，VINS(Visual-inertialsystem)基于单目鱼眼相机和低成本的IMU传感器的六自由度状态估计，视觉方面采用的是稀疏光流法进行跟踪定位，论文中提出了一种鲁棒性的视觉惯性联合初始化过程和恢复过程，以及基于滑动窗口的紧耦合非线性优化方法，但是整个系统采用的是最基本的参考帧跟踪模型，无法克服复杂的运动环境。
技术实现思路
本专利技术所要解决的技术问题在于，提供一种单目视觉惯性定位的IMU辅助跟踪模型，能够有效的克服剧烈运动等影响。为解决上述技术问题，本专利技术提供一种单目视觉惯性定位的IMU辅助跟踪模型，包括：初始化阶段和初始化完成后的阶段；初始化阶段时，在匀速模型设定当前帧初始平移的基础上，将IMU获取的原始数据做预积分，解算得到IMU先验旋转，获得当前帧的初始位姿；初始化完成后，在提供初始位姿的基础上，加入IMU预积分的先验速度信息，由以上计算的初始状态建立跟踪模型，实现精确定位。优选的，IMU先验数据解算具体为：IMU可以输出加速度和角速度，对角速度进行积分可以获得IMU单元的相对旋转；式(1)是IMU输出的角速度和加速度，BwwB(t)和wα(t)是IMU真实的角速度和加速度数据，bg(t)和ba(t)是陀螺仪和加速度计偏置，ηg(t)和ηa(t)是陀螺仪和加速度计噪声，B代表是IMU坐标系，W代表是世界坐标系，T是矩阵的转置，t是时刻，RWB是IMU到世界坐标系下的旋转，wg是世界坐标系下的重力加速度；同时IMU的数据帧率一般远高于视觉，在视觉的k和k+1帧之间通常会有多组IMU数据，IMU预积分模型即是计算视觉k和k+1帧之间的一组IMU积分值；式(2)是IMU运动模型，是速度的倒数，是旋转的倒数，是角速度的差乘；t+Δt时刻的运动速度和旋转状态可通过积分获得：Wv(t+Δt)＝Wv(t)+Wα(t)Δt利用中值积分法，联立式(1)和式(3)可得：是t时刻的IMU坐标系下的加速度，是t时刻的IMU坐标系下的角速度，R(t)是t时刻IMU到世界坐标下的旋转，g是重力加速度，上式可以看出每次对t+Δt时刻的旋转计算都需要知道t时刻的旋转，这样当R(t)随时间变化后需要重新计算积分，这将导致计算量的增大，为了避免重复的状态传播计算，采用预积分的方式：由预积分公式可以看出i和j时刻之间的预积分数值受两个参数影响，Δvij和ΔRij是i和j时刻之间的速度增量和旋转增量分别是偏置和以及噪声和并且关系复杂，因此需要分别考虑，首先假设偏置不变，仅讨论噪声的影响，接着再讨论偏置的影响，最后由公式(6)得到IMU先验值，其中是旋转对陀螺仪偏置的雅各比矩阵，和是速度对偏置的雅各比矩阵，是和是i和j时刻陀螺仪和加速度计偏置的变化量；优选的，系统初始状态估计具体为：假设有三个连续帧，F1、F2和F3，之间的相对旋转分别是ΔR12和ΔR23，匀速跟踪模型是假设相邻帧之间的相对变换不变，ΔT12是F1帧和F2帧间的相对变换，△T23是是F2帧和F3帧间的相对变换，即：ΔT12＝ΔT23(8)T1和T2是F1帧和F2帧的转换矩阵，R1和R2是F1帧和F2帧的旋转矩阵，t2是F2的平移矩阵，t1是F1的平移矩阵，F1和F2之间的相对变换：因此F3的变换矩阵：本专利技术将IMU的先验旋转数据初始由旋转矩阵间的转换关系可知：其中RBC是相机和IMU之间的旋转外参，ΔR23是相机帧F2与相机帧F3之间的相对旋转，由IMU先验数据提供；将旋转矩阵代入到变换矩阵模型中：最终将改进的T3作为初始值代入到优化模型中；当系统完成初始化后，重力加速度和IMU速度可以准确的获得，因此在视觉优化前可以通过IMU获得整个系统的速度信息：Δt是F2和F3的时间差，v2是上一时刻的速度，最终将改进的T3和v3作为初始状态代入到优化模型中。本专利技术的有益效果为：本专利技术提出了一种单目视觉惯性定位的IMU辅助跟踪模型，详细介绍了使用IMU先验数据提供初始位姿的方法，相比传统的视觉惯性定位技术从定位精度和鲁棒性上均有所提高，可实现室内环境下0.1米以内的定位精度，为室内移动智能体自主即时高精度定位提供了可能，同时为移动手机等平台上的视觉惯性鲁棒性定位提供了基础，本专利技术可有效提高剧烈运动下定位系统的鲁棒性。附图说明图1为本专利技术的辅助跟踪模型示意图。图2为本专利技术的匀速跟踪模型示意图。图3为本专利技术与ORB_SLAM2双目比较的V1_01_easy轨迹误差图。图4为本专利技术与OKVIS双目比较的MH_03_media轨迹平面图。具体实施方式如图1所示，一种单目视觉惯性定位的IMU辅助跟踪模型，包括：初始化阶段和初始化完成后的阶段；初始化阶段时，在匀速模型设定当前帧初始平移的基础上，将IMU获取的原始数据做预积分，解算得到IMU先验旋转，获得当前帧的初始位姿；初始化完成后，在提供初始位姿的基础上，加入IMU预积分的先验速度信息，由以上计算的初始状态建立跟踪模型，实现精确定位。IMU先验数据解算具体为：IMU可以输出加速度和角速度，对角速度进行积分可以获得IMU单元的相对旋转；式(1)是IMU输出的角速度和加速度，BwWB(t)和wα(t)是IMU真实的角速度和加速度数据，bg(t)和ba(t)是陀螺仪和加速度计偏置，ηg(t)和ηa(t)是陀螺仪和加速度计噪声，B代表是IMU坐标系，W代表是世界坐标系，T是矩阵的转置，t是时刻，RWB是IMU到世界坐标系下的旋转，wg是世界坐标系下的重力加速度；同时IMU的数据帧率一般远高于视觉，在视觉的k和k+1帧之间通常会有多组IMU数据，IMU预积分模型即是计算视觉k和k+1帧之间的一组IMU积分值；式(2)是IMU运动模型，是速度的倒数，是旋转的倒数，是角速度的差乘；t+Δt时刻的运动速度和旋转状态可通过积分获得：Wv(t+Δt)＝Wv(t)+Wα(t)Δt利用中值积分法，联立式(1本文档来自技高网...

【技术保护点】
1.单目视觉惯性定位的IMU辅助跟踪模型，其特征在于，包括：初始化阶段和初始化完成后的阶段；初始化阶段时，在匀速模型设定当前帧初始平移的基础上，将IMU获取的原始数据做预积分，解算得到IMU先验旋转，获得当前帧的初始位姿；初始化完成后，在提供初始位姿的基础上，加入IMU预积分的先验速度信息，由以上计算的初始状态建立跟踪模型，实现精确定位。

【技术特征摘要】
1.单目视觉惯性定位的IMU辅助跟踪模型，其特征在于，包括：初始化阶段和初始化完成后的阶段；初始化阶段时，在匀速模型设定当前帧初始平移的基础上，将IMU获取的原始数据做预积分，解算得到IMU先验旋转，获得当前帧的初始位姿；初始化完成后，在提供初始位姿的基础上，加入IMU预积分的先验速度信息，由以上计算的初始状态建立跟踪模型，实现精确定位。2.如权利要求1所述的单目视觉惯性定位的IMU辅助跟踪模型，其特征在于，IMU先验数据解算具体为：IMU可以输出加速度和角速度，对角速度进行积分可以获得IMU单元的相对旋转；式(1)是IMU输出的角速度和加速度，BwwB(t)和wα(t)是IMU真实的角速度和加速度数据，bg(t)和ba(t)是陀螺仪和加速度计偏置，ηg(t)和ηa(t)是陀螺仪和加速度计噪声，B代表是IMU坐标系，W代表是世界坐标系，T是矩阵的转置，t是时刻，RWB是IMU到世界坐标系下的旋转，wg是世界坐标系下的重力加速度；同时IMU的数据帧率一般远高于视觉，在视觉的k和k+1帧之间通常会有多组IMU数据，IMU预积分模型即是计算视觉k和k+1帧之间的一组IMU积分值；式(2)是IMU运动模型，是速度的倒数，是旋转的倒数，是角速度的差乘；t+Δt时刻的运动速度和旋转状态可通过积分获得：wv(t+Δt)＝wv(t)+wα(t)Δt利用中值积分法，联立式(1)和式(3)可得：是t时刻的IMU坐标系下的加速度，是t时刻的IMU坐标系下的角速度，R(t)是t时刻IMU到世界坐标下的旋转，g是重力加速度，上式可以看出每次对t+Δt时刻的旋转计算都需要知道t时刻的旋转，这样当R(t)随时间变...

【专利技术属性】
技术研发人员：潘树国，王帅，曾攀，黄砺枭，
申请(专利权)人：东南大学，
类型：发明
国别省市：江苏,32