本发明公开了一种基于单目的鲁棒性视觉惯性紧耦合定位方法，步骤包括：通过相机采集视觉数据，通过IMU单元采集惯性数据；进行IMU预积分，并得到IMU先验值；将所述IMU先验值代入视觉惯性联合初始化模型，完成参数的初始化；在所述参数的初始化所需时间内，利用连续关键帧之间的变换矩阵计算运动信息，带入视觉惯性融合定位框架的后端模块，实现紧耦合定位；参数的初始化完成后，转而将所述参数带入视觉惯性融合定位模型，计算得到运动信息，带入视觉惯性融合定位框架的后端模块，实现紧耦合定位。使用本发明提出的方法，初始化时间可缩短至10秒内，相比传统的基于ORB_SLAM2的单目视觉惯性定位系统，定位精度可提高约30%。
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：分别从陀螺仪测量值和视觉测量值得到旋转，将相对旋转的差异最小化得到优化的陀螺仪偏置bg，所述陀螺仪偏置的优化模型表示为：其中，N是关键帧的数量， 是IMU坐标系到世界坐标系的旋转矩阵， 为相机坐标系到世界坐标系的旋转矩阵，由视觉测量值计算得到， 是相机和IMU之间的旋转外参；ΔRi,i+1是两个连续关键帧的陀螺仪积分， 是重力加速度对相对旋转的雅可比矩阵；
考虑尺度s，估计从IMU坐标系到相机坐标系的变换：
其中， 为IMU坐标系到世界坐标系的平移； 为相机坐标系到世界坐标系的平移；
结合(8)和描述了两个连续关键帧的式(4)，忽略加速度计及陀螺仪的偏置，得到帧间关系：其中， 为某时刻相机坐标系到世界坐标系的平移； 为某时刻IMU坐标系在世界坐标系下的速度；Δti,i+1为i时刻与i+1时刻间的时间增量，Δpi,i+1为i时刻与i+1时刻间的位移增量；
估计重力加速度时，同时计算出不准确的尺度s，考虑连续三个关键帧间的(8)、(9)关系，速度项由相对速度表示；分离的变量s和gw表示为：其中[λ(i)β(i)]为i时刻分离出的系数矩阵；
通过求解公式(10)得到重力加速度gw和粗略尺度s；引入重力幅值G＝9.8m/s2；
考虑参考帧I，其重力方向为 且计算出的重力方向为： 如下定义旋转 和理论重力加速度gw：
通过扰动δθ优化旋转；其中， 在参考帧I中围绕x轴和y轴旋转两次进行参数化，而z轴上的旋转与重力对齐，对gw没有影响；优化旋转如下所示：其中δθxy为重力加速度修正量；
结合(9)和(13)并包含加速度计偏置ba，得到：
其中[λ(i) Φ(i) ζ(i)]为i时刻分离出的系数矩阵；
求解公式(15)得到粗略尺度s,重力加速度方向修正δθxy和加速度计偏置ba；
步骤3.2、快速恢复尺度：将速度 重力加速度gw及尺度s合为一个状态向量XI：是第i帧图像下IMU坐标系中的速度；
其中Δpi,i+1和Δvi,i+1分别是两连续关键帧的速度增量和平移增量； 为某时刻IMU在自身坐标系下的速度；
4.根据权利要求3所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，其特征在于，步骤三中，步骤3.1还包括：估计了陀螺仪偏置bg及加速度计偏置ba后，对平移、速度和旋转再次预积分，用于提高预积分值精度。
5.根据权利要求1所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，其特征在于，步骤四所述辅助跟踪定位具体地包括步骤：系统在初始化阶段使用IMU旋转数据；所述旋转数据包括：对于三个连续关键帧F1、F2及F3，相互间的相对旋转分别为ΔT12和ΔT23，且有：ΔT12＝ΔT23 (20)
其中R1、R2为F1、F2的旋转矩阵，T1、T2、T3为F1、F2及F3的变换矩阵，t1、t2、t3为F1、F2及F3的平移向量；
IMU初始的先验旋转数据由旋转矩阵间转换关系得到：
其中R3为F3的旋转矩阵， 是相机和IMU之间的旋转外参，ΔR23是关键帧F2与关键帧F3间的相对旋转，由IMU先验数据提供；
将旋转矩阵R1、R2和R3代入到变换矩阵模型中，得到改进的T3：再将改进的T3作为初始值代入视觉惯性融合定位框架的后端模块。
6.根据权利要求1所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，其特征在于，步骤五所述跟踪定位具体地包括：由步骤三得到参数初始化的结果，所述参数包括方向修正的重力加速度gw，tj时刻帧的速度vj，tj时刻帧对应的旋转矩阵Rj，优化后尺度s；
其中：v2，v3分别为关键帧F2和F3的速度，t2，t3分别为关键帧F2和F3的时刻，gw为方向修正的重力加速度，R2为关键帧F2的旋转矩阵，a2为关键帧F2的加速度， 为关键帧F2的加速度计偏置， 为加速度计噪声；又有改进的关键帧F3对应的变换矩阵T3：其中， 是相机和IMU之间的旋转外参，R1、R2和R3分别为关键帧F1、F2和F3的旋转矩阵，ΔR23为关键帧F2与关键帧F3间的相对旋转；
最终将改进的T3和v3联合作为初始状态代入视觉惯性融合定位框架的后端模块。
一种基于单目的鲁棒性视觉惯性紧耦合定位方法
技术领域
[0001] 本发明涉及一种单目鲁棒性视觉惯性紧耦合定位方法，属于SLAM(Simultaneous Localization and Mapping，同步定位与建图)领域。
背景技术
[0002] 随着微型飞行器自主飞行、自动驾驶、虚拟现实及增强现实等技术的迅速发展，实现高精度、强鲁棒性定位是完成移动智能体自主导航、探索未知区域等既定任务的重要前提。将视觉传感器和惯性测量单元(IMU)融合，可构建出精度更高、鲁棒性更强的视觉惯性融合定位系统(VIO)。
[0003] 传统的视觉惯性融合定位框架包括前端和后端两个模块，前端通过IMU和图像估算相邻图像间相机的运动，后端接受前端不同时刻估算的相机运动信息，对其进行局部与全局优化，得到全局一致的轨迹。
[0004] 现有的VIO包括OKVIS、基于ORB_SLAM2的单目视觉惯性融合定位系统、VINS。OKVIS框架(Leutenegger S,Furgale P,Rabaud V,et al.Keyframe-based visual-inertial slam using nonlinear optimization[J].Proceedings of Robotis Science and Systems(RSS)2013,2013.)提出了视觉惯性融合定位框架的前端模型，该模型实现了视觉和惯性数据的紧耦合，但未恢复出系统尺度和重力加速度等数据，同时不含预积分框架，因此其定位精度和鲁棒性较差；基于ORB_SLAM2(Mur-Artal R,Tardós J D.Visual-inertial monocular SLAM with map reuse[J].IEEE Robotics and Automation Letters,2017,2(2):796-803.)的单目视觉惯性定位系统，其预积分模型采用流型预积分算法，视觉惯性初始化模型需约15秒，在该阶段无法实现实时鲁棒性定位，同时其跟踪模型采用简单的匀速模型，使相机剧烈运动时无法正确初始化，甚至跟踪失败，最终导致定位效果不佳；VINs(Qin T,Li P,Shen S.Vins-mono:A robust and versatile monocular visual-inertial state estimator[J].IEEE Transactions on Robotics,2018,34(4):1004-1020.)初始化模型相对较快，但并未对加速度计偏置进行标定，同时初始化精度略低，采用传统的参考帧跟踪模型限制了其定位精度。
发明内容
[0005] 本发明所要解决的技术问题是：
[0006] 针对上述现有技术，提出一种包含IMU加权预积分、快速联合初始化及视觉IMU辅助跟踪的面向单目视觉惯性紧耦合定位的方法，提升视觉惯性定位的精度、实时性和鲁棒性。
[0007] 本发明为解决上述技术问题采用以下技术方案：
[0008] 本发明提出一种基于单目的鲁棒性视觉惯性紧耦合定位方法，步骤包括：
[0009] 步骤一、通过相机采集视觉数据，通过IMU单元采集惯性数据；
[0010] 步骤二、IMU预积分：利用IMU加权预积分模型同步所述视觉数据和所述惯性数据，进行IMU预积分，并得到IMU先验值；
[0011] 步骤三、初始化：将所述IMU先验值代入视觉惯性联合初始化模型，完成参数的初始化；
[0012] 步骤四、辅助跟踪定位：在步骤三所述参数的初始化所需时间内，利用连续关键帧之间的变换矩阵计算运动信息，代入视觉惯性融合定位框架的后端模块，实现紧耦合定位；
[0013] 步骤五、跟踪定位：步骤三所述参数的初始化完成后，转而将所述参数带入视觉惯性融合定位模型，计算得到运动信息，带入视觉惯性融合定位框架的后端模块，实现紧耦合定位。
[0014] 如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，进一步地，步骤二所述IMU预积分包括如下具体步骤：
[0015] 步骤2.1、计算加权IMU测量值，得到IMU坐标系下的角速度测量值 和加速度测量值
[0016] t时刻典型的IMU测量值方程表示为：
[0017]
[0018]
[0019] 其中wB(t)和aW(t)分别是IMU坐标系下的角速度真实值和世界坐标系下加速度真实值，bg(t)和ba(t)分别是陀螺仪和加速度计的偏置，ηg(t)和ηa(t)分别是陀螺仪和加速度计的高斯噪声，gW是世界坐标系下的重力加速度， 是t时刻世界坐标向IMU坐标旋转的转置；
[0020] t到t+Δt时刻之间真实的加权IMU测量值表示为：
[0021] wB(t,t+Δt)＝c1(t)wB(t)+c2(t)wB(t+Δt)
[0022] aB(t,t+Δt)＝c3(t)aB(t)+c4(t)aB(t+Δt) (2)
[0023] 其中，aB(t)为t时刻IMU坐标系下的加速度；加权系数c1(t),c2(t),c3(t),c4(t)具体表示为：
[0024] c1(t)＝wB(t)/(wB(t)+wB(t+Δt))
[0025] c2(t)＝wB(t+Δt)/(wB(t)+wB(t+Δt))
[0026] c3(t)＝aB(t)/(aB(t)+aB(t+Δt))
[0027] c4(t)＝aB(t+Δt)/(aB(t)+aB(t+Δt)) (3)
[0028] 步骤2.2、IMU坐标系下进行加权预积分，求IMU先验值，建立高精度的加权预积分模型：
[0029] 由t时刻的状态积分得到当前时刻t+Δt的状态，如式(4)所示：
[0030]
[0031] 其中RWB(t+Δt)是t+Δt时刻下世界坐标系相对于IMU坐标系的旋转,vW(t+Δt)、pW(t+Δt)分别世界坐标系下的速度和平移；RWB(t)是t时刻下世界坐标系相对于IMU坐标系的旋转,vW(t)、pW(t)分别是世界坐标系下的速度和平移；wW(τ)、aW(τ)和vW(τ)分别为世界坐标系下的角速度变量、加速度变量、速度变量；
[0032] 将式(4)两边同乘 将世界坐标系下的旋转、速度及平移三个状态量转换到IMU相对坐标系下；将相邻两次IMU测量之间的角速度、加速度及旋转视为定值，进行数值积分得到IMU坐标系下的加权预积分公式(5)：
[0033]
[0034]
[0035]
[0036] 其中，在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时刻的平移；
[0037] 分别考虑i,j时刻间的预积分数值受偏置及噪声的影响：先假设偏置不变，仅讨论噪声的影响，再讨论偏置更新的影响，最后由式(6)得到IMU先验值：
[0038]
[0039] 其中 是旋转对陀螺仪偏置的雅可比矩阵， 为i时刻陀螺仪的偏置， 为i时刻加速度计的偏置， 为i时刻无噪声情况下陀螺仪的偏置， 为i时刻无噪声情况下加速度计的偏置， 是速度对陀螺仪偏置的雅可比矩阵， 是速度对加速度计偏置的雅可比矩阵， 和 分别是i,j时刻陀螺仪和加速度计偏置的变化量， 为,j时刻的速度增量测量值， 为,j时刻无噪声情况下的旋转增量测量值， 为i,j时刻无噪声情况下的旋转增量，bg为陀螺仪偏置，ba为加速度计偏置；(·)∧表示将向量转换成矩阵；
[0040] 如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，进一步地，步骤三所述参数的初始化包括如下具体步骤：
[0041] 步骤3.1、估计优化的陀螺仪偏置，估计方向修正的重力加速度gw：
[0042] 分别从陀螺仪测量值和视觉测量值得到旋转，将相对旋转的差异最小化得到优化的陀螺仪偏置bg，所述陀螺仪偏置的优化模型表示为：
[0043]
[0044] 其中，N是关键帧的数量， 是IMU坐标系到世界坐标系的旋转矩阵，为相机坐标系到世界坐标系的旋转矩阵，由视觉测量值计算得到， 是相机和IMU之间的旋转外参；ΔRi,i+1是两个连续关键帧的陀螺仪积分， 是重力加速度对相对旋转的雅可比矩阵； 为世界坐标系下i时刻的偏置旋转矩阵；
[0045] 考虑尺度s，估计从IMU坐标系到相机坐标系的变换：
[0046]
[0047] 其中， 为IMU坐标系到世界坐标系的平移； 为相机坐标系到世界坐标系的平移； 为IMU坐标系到相机坐标系的平移；
[0048] 结合(8)和描述了两个连续关键帧的式(4)，忽略加速度计及陀螺仪的偏置，得到帧间关系：
[0049]
[0050] 其中， 为某时刻相机坐标系到世界坐标系的平移； 为某时刻IMU坐标系在世界坐标系下的速度；Δti,i+1为i时刻与i+1时刻间的时间增量，Δpi,i+1为i时刻与i+1时刻间的位移增量；
[0051] 估计重力加速度时，同时计算出不准确的尺度s，考虑连续三个关键帧间的(8)、(9)关系，速度项由相对速度表示；分离的变量γ(i)为：
[0052]
[0053] 其中[λ(i)β(i)]为i时刻分离出的系数矩阵；
[0054] 通过求解公式(10)得到重力加速度gw和粗略尺度s；引入重力幅值G＝9.8m/s2；
[0055] 考虑参考帧I，其重力方向为 且计算出的重力方向为：如下定义旋转 和理论重力加速度gw：
[0056]
[0057] 通过扰动δθ优化旋转；其中， 在参考帧I中围绕x轴和y轴旋转两次进行参数化，而z轴上的旋转与重力对齐，对gw没有影响；优化旋转如下所示：
[0058]
[0059]
[0060] 其中δθxy为重力加速度修正量；
[0061] 结合一阶近似值,理论重力加速度gw表示为：
[0062]
[0063] 其中，()×表示将三维向量转换成三维矩阵；
[0064] 结合(9)和(13)并包含加速度计偏置ba，得到：
[0065]
[0066] 对(14)进行变量分离得到：
[0067]
[0068] 其中[λ(i) Φ(i) ζ(i)]为i时刻分离出的系数矩阵；
[0069] 求解公式(15)得到粗略尺度s,重力加速度方向修正δθxy和加速度计偏置ba；
[0070] 步骤3.2、快速恢复尺度：将速度 重力加速度gw及尺度s合为一个状态向量XI：
[0071]
[0072] 是第i帧图像下IMU坐标系中的速度；
[0073] 两连续关键帧在(9)中的关系为:
[0074]
[0075] 其中Δpi,i+1和Δvi,i+1分别是两连续关键帧的速度增量和平移增量； 为某时刻IMU在自身坐标系下的速度；
[0076] 分离变量并得到对应的含噪声项 的线性测量模型：
[0077]
[0078] 其中 和 为实际观测值；
[0079]
[0080] 求解此线性最小二乘问题：
[0081]
[0082] 得到帧的速度v和优化后尺度s。
[0083] 如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，进一步地，步骤三中，步骤3.1还包括：估计了陀螺仪偏置bg及加速度计偏置ba后，对平移、速度和旋转再次预积分，用于提高预积分值精度。
[0084] 如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，进一步地，步骤四所述辅助跟踪定位具体地包括步骤：
[0085] 系统在初始化阶段使用IMU旋转数据；所述旋转数据包括：对于三个连续关键帧F1、F2及F3，相互间的相对旋转分别为ΔT12和ΔT23，且有：
[0086] ΔT12＝ΔT23 (20)
[0087] F1和F2之间的相对变换及F3的变换矩阵分别为：
[0088]
[0089]
[0090] 其中R1、R2为F1、F2的旋转矩阵，T1、T2、T3为F1、F2及F3的变换矩阵，t1、t2、t3为F1、F2及F3的平移向量；
[0091] IMU初始的先验旋转数据由旋转矩阵间转换关系得到：
[0092]
[0093] 其中R3为F3的旋转矩阵， 是相机和IMU之间的旋转外参，ΔR23是关键帧F2与关键帧F3间的相对旋转，由IMU先验数据提供；
[0094] 将旋转矩阵R1、R2和R3代入到变换矩阵模型中，得到改进的T3：
[0095]
[0096] 再将改进的T3作为初始值代入视觉惯性融合定位框架的后端模块。
[0097] 如前所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，进一步地，步骤五所述跟踪定位具体地包括：
[0098] 由步骤三得到参数初始化的结果，所述参数包括方向修正的重力加速度gw，tj时刻帧的速度vj，tj时刻帧对应的旋转矩阵Rj，优化后尺度s；
[0099] 对于三个连续的关键帧F1、F2和F3，有：
[0100]
[0101] 其中：v2，v3分别为关键帧F2和F3的速度，t2，t3分别为关键帧F2和F3的时刻，gw为方向修正的重力加速度，R2为关键帧F2的旋转矩阵，a2为关键帧F2的加速度， 为关键帧F2的加速度计偏置， 为加速度计噪声；又有改进的关键帧F3对应的变换矩阵T3：
[0102]
[0103] 其中， 是相机和IMU之间的旋转外参，R1、R2和R3分别为关键帧F1、F2和F3的旋转矩阵，ΔR23为关键帧F2与关键帧F3间的相对旋转；
[0104] 最终将改进的T3和v3联合作为初始状态代入视觉惯性融合定位框架的后端模块。
[0105] 本发明采用以上技术方案与现有技术相比，具有以下技术效果：
[0106] 本发明在IMU预积分阶段，提出了IMU加权预积分模型，为联合初始化提供IMU先验值，为传统视觉惯性融合定位框架的后端优化部分提供高精度IMU约束，提高了定位精度；
[0107] 在初始化阶段，提出了视觉惯性联合初始化模型，构建视觉惯性融合状态向量，建立联合初始化模型，为传统视觉惯性融合定位框架的后端优化部分提供绝对尺度、重力加速度及速度信息，使本发明的鲁棒性和定位精度都得到增强；
[0108] 在初始化阶段，提出了辅助跟踪模型，用于在初始化阶段替代定位，解决了视觉惯性初始化模型需约15秒，在该阶段无法实现实时鲁棒性定位的问题；
[0109] 总体而言，本发明所提出的方法用于视觉惯性定位，所述方法同步视觉和惯性测量数据，建立高精度的IMU加权预积分模型，为联合初始化和视觉跟踪模型提供帧间运动约束，提高了定位精度；构建视觉惯性融合状态向量，建立联合初始化模型，实现视觉惯性松耦合的快速联合初始化，降低了系统初始化时间；在IMU加权预积分和快速初始化方法的基础上，建立一套视觉惯性辅助跟踪模型，有效提高了系统鲁棒性。使用本发明提出的方法，初始化时间可缩短至10秒内；相比传统的基于ORB_SLAM2的单目视觉惯性定位系统，定位精度可提高约30％。
附图说明
[0110] 图1是基于单目的视觉惯性紧耦合定位方法流程图；
[0111] 图2是本方法与现有方法在MH03序列中定位轨迹平面图；
[0112] 图3是本方法与现有方法在V101序列中定位轨迹平面图；
[0113] 图4是本方法与现有方法在MH03序列中的轨迹误差曲线图；
[0114] 图5是本方法与现有方法在MH03序列中的轨迹误差小提琴图。
具体实施方式
[0115] 下面结合附图对本发明的技术方案做进一步的详细说明：
[0116] 本技术领域技术人员可以理解的是，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。
[0117] 一种基于单目的鲁棒性视觉惯性紧耦合定位方法,首先同步视觉和惯性测量数据，建立高精度的IMU加权预积分模型；其次构建视觉惯性融合状态向量，建立联合初始化模型；最后在IMU加权预积分和快速初始化方法的基础上，建立一套视觉惯性辅助跟踪模型。
[0118] 如图1，进一步的，一种基于单目的鲁棒性视觉惯性紧耦合定位方法包括如下具体步骤：
[0119] 步骤1)，同步视觉和惯性测量数据，建立高精度的IMU加权预积分模型，具体如下：
[0120] a)，计算加权IMU测量值。IMU的测量值包含由陀螺仪和加速度计分别提供的角速度和加速度，t时刻典型的测量值方程如式(1)所示。
[0121]
[0122]
[0123] 其中 和 分别为IMU坐标系下的角速度和加速度测量值，wB(t)和aW(t)分别是IMU坐标系下的角速度真实值和世界坐标系下加速度真实值，bg(t)和ba(t)分别是陀螺仪和加速度计的偏置，ηg(t)和ηa(t)分别是陀螺仪和加速度计的高斯噪声，gW是世界坐标系下的重力加速度， 是t时刻世界坐标向IMU坐标旋转的转置。
[0124] 定义t到t+Δt时刻之间真实的加权IMU测量值为这两个时刻测量值加权描述的常数：
[0125] wB(t,t+Δt)＝c1(t)wB(t)+c2(t)wB(t+Δt)
[0126] aB(t,t+Δt)＝c3(t)aB(t)+c4(t)aB(t+Δt) (2)
[0127] 其中，加权系数c1(t),c2(t),c3(t),c4(t)具体表示如式(3)所示。
[0128] c1(t)＝wB(t)/(wB(t)+wB(t+Δt))
[0129] c2(t)＝wB(t+Δt)/(wB(t)+wB(t+Δt))
[0130] c3(t)＝aB(t)/(aB(t)+aB(t+Δt))
[0131] c4(t)＝aB(t+Δt)/(aB(t)+aB(t+Δt)) (3)
[0132] b)，建立加权预积分模型。当前时刻t+Δt的状态可由t时刻的状态积分得到，如式(4)所示：
[0133]
[0134]
[0135]
[0136] 将式(4)两边同乘 可将世界坐标系下的旋转、速度及平移三个状态量转换到IMU相对坐标系下。结合式(1)中的IMU测量值及式(2)中的加权IMU测量表示，在数值积分时，由于IMU有较高的测量频率，相邻两次IMU测量之间的角速度、加速度及旋转可视为定值。故可将式(4)整理得到IMU坐标系下的加权预积分公式(5)：
[0137]
[0138]
[0139]
[0140] 由式(5)知i,j时刻间的预积分数值受偏置 和 及噪声 和 的影响，且关系复杂，故需分别考虑。先假设偏置不变，仅讨论噪声的影响，再讨论偏置更新的影响，最后由式(6)得到IMU先验值，其中 是旋转对陀螺仪偏置的雅可比矩阵， 和 是速度对偏置的雅可比矩阵， 和 是i,j时刻陀螺仪和加速度计偏置的变化量。
[0141]
[0142]
[0143]
[0144] 步骤2)，构建视觉惯性融合状态向量，建立联合初始化模型，包含如下具体步骤：
[0145] a)，估计偏置及重力加速度。由于噪声的存在，分别从陀螺仪测量值和视觉测量值得到的旋转不相等，故通过最小化相对旋转的差异来优化陀螺仪偏置，优化模型如式(7)所示：
[0146]
[0147] N是关键帧的数量， 是IMU坐标系到世界坐标系的旋转，相机坐标系到世界坐标系的旋转 由视觉测量值计算得到，ΔRi,i+1是两个连续关键帧的陀螺仪积分。
[0148] 估计从IMU坐标系到相机坐标系的变换时包含了尺度s：
[0149]
[0150] 结合(8)和描述了两个连续关键帧的式(4)，忽略加速度计及陀螺仪的偏置可得：
[0151]
[0152] 估计重力加速度时，同时计算出不准确的尺度s，考虑连续三个关键帧间的(8)、(9)关系，速度项由相对速度表示。分离的变量为：
[0153]
[0154] 至少联合四个关键帧可形成测量模型：A3(N-2)×4x4×1＝B3(N-1)×1。通过SVD求解模型得到重力加速度向量 和粗略尺度 由于重力和加速度计偏差很难区分，加入加速度计偏置可能导致病态系统[14]。在优化重力加速度和加速度偏置估计的过程中，为提高可观察性，本文引入重力幅值G＝9.8m/s2，重力加速度只需优化方向。考虑参考帧I，其重力方向为且计算出的重力方向为： 如下定义旋转 和重力向量：
[0155]
[0156] 可在参考帧I中围绕x轴和y轴旋转两次进行参数化，而z轴上的旋转与重力对齐，对gw没有影响，故可通过扰动δθ优化旋转，如下所示：
[0157]
[0158]
[0159] 结合一阶近似值,gw可表示为：
[0160]
[0161] 结合(9)和(13)并包含加速度偏置，得到：
[0162]
[0163] 式(10)进一步转化为含有尺度、重力加速度方向修正及加速度计偏置的表达式：
[0164]
[0165] b)，快速恢复尺度。将速度、重力向量及尺度合为一个状态向量：
[0166]
[0167] 是第i帧图像下IMU坐标系中的速度，两连续关键帧在(9)中的关系为:
[0168]
[0169] 分离变量并得到对应的含噪声项 的线性测量模型：
[0170]
[0171] 其中：
[0172]
[0173] 通过求解此线性最小二乘问题，可得到相机帧的速度v和优化后尺度s：
[0174]
[0175] 步骤3)，建立视觉惯性辅助跟踪模型，具体如下：
[0176] 系统的初始化阶段由于无法获取速度、重力加速度及尺度，故只能使用IMU旋转数据。匀速跟踪模型视相邻关键帧间的相对变换不变，即对于三个连续帧F1、F2及F3，相互间的相对旋转分别为ΔR12和ΔR23，且有：
[0177] ΔT12＝ΔT23 (20)
[0178] F1和F2之间的相对变换及F3的变换矩阵分别为：
[0179]
[0180]
[0181] 本文将IMU初始的先验旋转数据由旋转矩阵间转换关系得到：
[0182]
[0183] 其中 是相机和IMU之间的旋转外参，ΔR23是关键帧F2与关键帧F3间的相对旋转，由IMU先验数据提供。
[0184] 将旋转矩阵代入到变换矩阵模型中：
[0185]
[0186] 再将改进的T3作为初始值代入优化模型。当系统完成初始化后，可获得准确的重力加速度和IMU速度，因此在视觉优化前可通过IMU获得整个系统的速度信息：
[0187]
[0188] 最终将改进的T3和v3联合作为初始状态代入优化模型。
[0189] 实施例一
[0190] 本发明提出的方法理论上可应用于现有的传统的视觉惯性融合定位框架(VIO)。现有的传统的视觉惯性融合定位框架包括前端和后端两个模块，前端通过IMU和图像估算相邻图像间相机的运动，后端接受前端不同时刻估算的相机运动信息，对其进行局部与全局优化，得到全局一致的轨迹。
[0191] 现有的VIO包括OKVIS、基于ORB_SLAM2的单目视觉惯性融合定位系统、VINS。基于视觉惯性ORB_SLAM2(后文记为origin_VIO)，并采用EuRoC数据集中的九个数据序列进行测试。该数据集包含了不同的房间和工业环境中，配备有VI-Sensor双目惯性相机的无人机动态运动。其图像采集频率为20Hz，IMU采样频率为200Hz，数据集提供真实轨迹。由于是单目视觉惯性平台，故仅使用数据集中左目相机测量值。本实例使用内存8G的Intel Core i7-7000笔记本，本方法(记为robust_VIO)实验结果与origin_VIO结果作比较。
[0192] 图2为MH03序列在origin_VIO及robust_VIO中的定位轨迹平面图。由于系统基于关键帧进行定位跟踪，故定位轨迹会存在部分不连续的情况。由图可知，两个系统估计的结果几乎跟踪了实际轨迹，但robust_VIO的轨迹较于origin_VIO的轨迹更连续、稳定且偏离更小。
[0193] 图3为MH03序列在两种系统中定位的轨迹误差分析图，主要统计分析数据的离散程度和分布密度，robust_VIO误差分布主要集中在0.095m左右，而origin_VIO主要集中在0.194m附近，前者误差的中位数和四分位数都较小，且下侧的离散值表现也更优。
[0194] 为验证本文提出的快速联合初始化方法的快速性，以V101序列为例作初始化状态图，图4和图5分别为初始化尺度和加速度计偏置的变化情况。由图可知在robust_VIO中四类变量在10秒内稳定收敛，而origin_VIO需要15秒，本文方法加快了初始化速度。
[0195] 最后给出九种EuRoC数据序列在origin_VIO及robust_VIO两种系统中的定位结果。由表1可知本文定位结果优于origin_VIO的定位结果，定位精度平均提高了约30％。
[0196] 表1：两种方法在EuRoC数据集上定位结果对比
[0197]
[0198] 以上所述仅是本发明的部分实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。
