一种基于单目的鲁棒性视觉惯性紧耦合定位方法技术

本发明专利技术公开了一种基于单目的鲁棒性视觉惯性紧耦合定位方法，步骤包括：通过相机采集视觉数据，通过IMU单元采集惯性数据；进行IMU预积分，并得到IMU先验值；将所述IMU先验值代入视觉惯性联合初始化模型，完成参数的初始化；在所述参数的初始化所需时间内，利用连续关键帧之间的变换矩阵计算运动信息，带入视觉惯性融合定位框架的后端模块，实现紧耦合定位；参数的初始化完成后，转而将所述参数带入视觉惯性融合定位模型，计算得到运动信息，带入视觉惯性融合定位框架的后端模块，实现紧耦合定位。使用本发明专利技术提出的方法，初始化时间可缩短至10秒内，相比传统的基于ORB_SLAM2的单目视觉惯性定位系统，定位精度可提高约30%。

A Robust Visual Inertial Tightly Coupled Location Method Based on Single Purpose

【技术实现步骤摘要】
一种基于单目的鲁棒性视觉惯性紧耦合定位方法
本专利技术涉及一种单目鲁棒性视觉惯性紧耦合定位方法，属于SLAM(SimultaneousLocalizationandMapping，同步定位与建图)领域。
技术介绍
随着微型飞行器自主飞行、自动驾驶、虚拟现实及增强现实等技术的迅速发展，实现高精度、强鲁棒性定位是完成移动智能体自主导航、探索未知区域等既定任务的重要前提。将视觉传感器和惯性测量单元(IMU)融合，可构建出精度更高、鲁棒性更强的视觉惯性融合定位系统(VIO)。传统的视觉惯性融合定位框架包括前端和后端两个模块，前端通过IMU和图像估算相邻图像间相机的运动，后端接受前端不同时刻估算的相机运动信息，对其进行局部与全局优化，得到全局一致的轨迹。现有的VIO包括OKVIS、基于ORB_SLAM2的单目视觉惯性融合定位系统、VINS。OKVIS框架(LeuteneggerS,FurgaleP,RabaudV,etal.Keyframe-basedvisual-inertialslamusingnonlinearoptimization[J].ProceedingsofRobotisScienceandSystems(RSS)2013,2013.)提出了视觉惯性融合定位框架的前端模型，该模型实现了视觉和惯性数据的紧耦合，但未恢复出系统尺度和重力加速度等数据，同时不含预积分框架，因此其定位精度和鲁棒性较差；基于ORB_SLAM2(Mur-ArtalR,TardósJD.Visual-inertialmonocularSLAMwithmapreuse[J].IEEERoboticsandAutomationLetters,2017,2(2):796-803.)的单目视觉惯性定位系统，其预积分模型采用流型预积分算法，视觉惯性初始化模型需约15秒，在该阶段无法实现实时鲁棒性定位，同时其跟踪模型采用简单的匀速模型，使相机剧烈运动时无法正确初始化，甚至跟踪失败，最终导致定位效果不佳；VINs(QinT,LiP,ShenS.Vins-mono:Arobustandversatilemonocularvisual-inertialstateestimator[J].IEEETransactionsonRobotics,2018,34(4):1004-1020.)初始化模型相对较快，但并未对加速度计偏置进行标定，同时初始化精度略低，采用传统的参考帧跟踪模型限制了其定位精度。
技术实现思路
本专利技术所要解决的技术问题是：针对上述现有技术，提出一种包含IMU加权预积分、快速联合初始化及视觉IMU辅助跟踪的面向单目视觉惯性紧耦合定位的方法，提升视觉惯性定位的精度、实时性和鲁棒性。本专利技术为解决上述技术问题采用以下技术方案：本专利技术提出一种基于单目的鲁棒性视觉惯性紧耦合定位方法，步骤包括：步骤一、通过相机采集视觉数据，通过IMU单元采集惯性数据；步骤二、IMU预积分：利用IMU加权预积分模型同步所述视觉数据和所述惯性数据，进行IMU预积分，并得到IMU先验值；步骤三、初始化：将所述IMU先验值代入视觉惯性联合初始化模型，完成参数的初始化；步骤四、辅助跟踪定位：在步骤三所述参数的初始化所需时间内，利用连续关键帧之间的变换矩阵计算运动信息，代入视觉惯性融合定位框架的后端模块，实现紧耦合定位；步骤五、跟踪定位：步骤三所述参数的初始化完成后，转而将所述参数带入视觉惯性融合定位模型，计算得到运动信息，带入视觉惯性融合定位框架的后端模块，实现紧耦合定位。如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，进一步地，步骤二所述IMU预积分包括如下具体步骤：步骤2.1、计算加权IMU测量值，得到IMU坐标系下的角速度测量值和加速度测量值t时刻典型的IMU测量值方程表示为：其中wB(t)和aW(t)分别是IMU坐标系下的角速度真实值和世界坐标系下加速度真实值，bg(t)和ba(t)分别是陀螺仪和加速度计的偏置，ηg(t)和ηa(t)分别是陀螺仪和加速度计的高斯噪声，gW是世界坐标系下的重力加速度，是t时刻世界坐标向IMU坐标旋转的转置；t到t+Δt时刻之间真实的加权IMU测量值表示为：wB(t,t+Δt)＝c1(t)wB(t)+c2(t)wB(t+Δt)aB(t,t+Δt)＝c3(t)aB(t)+c4(t)aB(t+Δt)(2)其中，aB(t)为t时刻IMU坐标系下的加速度；加权系数c1(t),c2(t),c3(t),c4(t)具体表示为：c1(t)＝wB(t)/(wB(t)+wB(t+Δt))c2(t)＝wB(t+Δt)/(wB(t)+wB(t+Δt))c3(t)＝aB(t)/(aB(t)+aB(t+Δt))c4(t)＝aB(t+Δt)/(aB(t)+aB(t+Δt))(3)步骤2.2、IMU坐标系下进行加权预积分，求IMU先验值，建立高精度的加权预积分模型：由t时刻的状态积分得到当前时刻t+Δt的状态，如式(4)所示：其中RWB(t+Δt)是t+Δt时刻下世界坐标系相对于IMU坐标系的旋转,vW(t+Δt)、pW(t+Δt)分别世界坐标系下的速度和平移；RWB(t)是t时刻下世界坐标系相对于IMU坐标系的旋转,vW(t)、pW(t)分别是世界坐标系下的速度和平移；wW(τ)、aW(τ)和vW(τ)分别为世界坐标系下的角速度变量、加速度变量、速度变量；将式(4)两边同乘将世界坐标系下的旋转、速度及平移三个状态量转换到IMU相对坐标系下；将相邻两次IMU测量之间的角速度、加速度及旋转视为定值，进行数值积分得到IMU坐标系下的加权预积分公式(5)：其中，在IMU坐标系下，ΔRij为i,j时刻的旋转增量，ΔRik为i,k时刻的旋转增量；Ri为i时刻的旋转，Rj为j时刻的旋转，Rk为k时刻的旋转；为k时刻加速度测量值，为时刻角速度测量值，为k时刻陀螺仪的偏置，为k时刻加速度计的偏置；为k时刻陀螺仪的高斯噪声，为k时刻加速度计的高斯噪声；Δvij为i,j时刻的速度增量，Δvik为i,k时刻的速度增量；vi为i时刻的速度,vj为j时刻的速度；g为重力加速度；Δtij为i,j时刻的时间增量，Δpij为i,j时刻的平移增量；pi为i时刻的平移，pj为j时刻的平移；分别考虑i,j时刻间的预积分数值受偏置及噪声的影响：先假设偏置不变，仅讨论噪声的影响，再讨论偏置更新的影响，最后由式(6)得到IMU先验值：其中是旋转对陀螺仪偏置的雅可比矩阵，为i时刻陀螺仪的偏置，为i时刻加速度计的偏置，为i时刻无噪声情况下陀螺仪的偏置，为i时刻无噪声情况下加速度计的偏置，是速度对陀螺仪偏置的雅可比矩阵，是速度对加速度计偏置的雅可比矩阵，和分别是i,j时刻陀螺仪和加速度计偏置的变化量，为,j时刻的速度增量测量值，为,j时刻无噪声情况下的旋转增量测量值，为i,j时刻无噪声情况下的旋转增量，bg为陀螺仪偏置，ba为加速度计偏置；(·)∧表示将向量转换成矩阵；如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，进一步地，步骤三所述参数的初始化包括如下具体步骤：步骤3.1、估计优化的陀螺仪偏置，估计方向修正的重力加速度gw：分别从陀螺仪测量值和视觉测量值得到旋转，将相对旋转的差异最小化得到优本文档来自技高网...

【技术保护点】
1.一种基于单目的鲁棒性视觉惯性紧耦合定位方法，其特征在于，步骤包括：步骤一、通过相机采集视觉数据，通过IMU单元采集惯性数据；步骤二、IMU预积分：利用IMU加权预积分模型同步所述视觉数据和所述惯性数据，进行IMU预积分，并得到IMU先验值；步骤三、初始化：将所述IMU先验值代入视觉惯性联合初始化模型，完成参数的初始化；步骤四、辅助跟踪定位：在步骤三所述参数的初始化所需时间内，利用连续关键帧之间的变换矩阵计算运动信息，代入视觉惯性融合定位框架的后端模块，实现紧耦合定位；步骤五、跟踪定位：步骤三所述参数的初始化完成后，转而将所述参数代入视觉惯性融合定位框架的前端模块，计算得到运动信息后，代入视觉惯性融合定位框架的后端模块进行优化，实现紧耦合定位。

【技术特征摘要】
1.一种基于单目的鲁棒性视觉惯性紧耦合定位方法，其特征在于，步骤包括：步骤一、通过相机采集视觉数据，通过IMU单元采集惯性数据；步骤二、IMU预积分：利用IMU加权预积分模型同步所述视觉数据和所述惯性数据，进行IMU预积分，并得到IMU先验值；步骤三、初始化：将所述IMU先验值代入视觉惯性联合初始化模型，完成参数的初始化；步骤四、辅助跟踪定位：在步骤三所述参数的初始化所需时间内，利用连续关键帧之间的变换矩阵计算运动信息，代入视觉惯性融合定位框架的后端模块，实现紧耦合定位；步骤五、跟踪定位：步骤三所述参数的初始化完成后，转而将所述参数代入视觉惯性融合定位框架的前端模块，计算得到运动信息后，代入视觉惯性融合定位框架的后端模块进行优化，实现紧耦合定位。2.根据权利要求1所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，其特征在于，步骤二所述IMU预积分包括如下具体步骤：步骤2.1、计算加权IMU测量值，得到IMU坐标系下的角速度测量值和加速度测量值t时刻典型的IMU测量值方程表示为：其中wB(t)和aW(t)分别是IMU坐标系下的角速度真实值和世界坐标系下加速度真实值，bg(t)和ba(t)分别是陀螺仪和加速度计的偏置，ηg(t)和ηa(t)分别是陀螺仪和加速度计的高斯噪声，gW是世界坐标系下的重力加速度，是t时刻世界坐标向IMU坐标旋转的转置；t到t+Δt时刻之间真实的加权IMU测量值表示为：wB(t,t+Δt)＝c1(t)wB(t)+c2(t)wB(t+Δt)aB(t,t+Δt)＝c3(t)aB(t)+c4(t)aB(t+Δt)(2)其中，aB(t)为t时刻IMU坐标系下的加速度；加权系数c1(t),c2(t),c3(t),c4(t)具体表示为：c1(t)＝wB(t)/(wB(t)+wB(t+Δt))c2(t)＝wB(t+Δt)/(wB(t)+wB(t+Δt))c3(t)＝aB(t)/(aB(t)+aB(t+Δt))c4(t)＝aB(t+Δt)/(aB(t)+aB(t+Δt))(3)步骤2.2、IMU坐标系下进行加权预积分，求IMU先验值，建立高精度的加权预积分模型：由t时刻的状态积分得到当前时刻t+Δt的状态，如式(4)所示：其中RWB(t+Δt)是t+Δt时刻下世界坐标系相对于IMU坐标系的旋转,vW(t+Δt)、pW(t+Δt)分别世界坐标系下的速度和平移；RWB(t)是t时刻下世界坐标系相对于IMU坐标系的旋转,vW(t)、pW(t)分别是世界坐标系下的速度和平移；wW(τ)、aW(τ)和vW(τ)分别为世界坐标系下的角速度变量、加速度变量、速度变量；将式(4)两边同乘将世界坐标系下的旋转、速度及平移三个状态量转换到IMU相对坐标系下；将相邻两次IMU测量之间的角速度、加速度及旋转视为定值，进行数值积分得到IMU坐标系下的加权预积分公式(5)：其中，在IMU坐标系下，ΔRij为i,j时刻的旋转增量，ΔRik为i,k时刻的旋转增量；Ri为i时刻的旋转，Rj为j时刻的旋转，Rk为k时刻的旋转；为k时刻加速度测量值，为k时刻角速度测量值，为k时刻陀螺仪的偏置，为k时刻加速度计的偏置；为k时刻陀螺仪的高斯噪声，为k时刻加速度计的高斯噪声；Δvij为i,j时刻的速度增量，Δvik为i,k时刻的速度增量；vi为i时刻的速度,vj为j时刻的速度；g为重力加速度；Δtij为i,j时刻的时间增量，Δpij为i,j时刻的平移增量；pi为i时刻的平移，pj为j时刻的平移；分别考虑i,j时刻间的预积分数值受偏置及噪声的影响：先假设偏置不变，仅讨论噪声的影响，再讨论偏置更新的影响，最后由式(6)得到IMU先验值：其中是旋转对陀螺仪偏置的雅可比矩阵，为i时刻陀螺仪的偏置，为i时刻加速度计的偏置，为i时刻无噪声情况下陀螺仪的偏置，为i时刻无噪声情况下加速度计的偏置，是速度对陀螺仪偏置的雅可比矩阵，是速度对加速度计偏置的雅可比矩阵，和分别是i,j时刻陀螺仪和加速度计偏置的变化量，为i,j时刻的速度增量测量值，为i,j时刻无噪声情况下的旋转增量测量值，为i,j时刻无噪声情况下的旋转增量，bg为陀螺仪偏置，ba为加速度计偏置；(·)^表示将向量转换成矩阵；3.根据权利要求1所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，其特征在于，步骤三所述参数的初始化包括如下具体步骤：步骤3.1、估计优化的陀螺仪偏置，估计方向修正的重力加速度gw：分别从陀螺仪测量...

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