[发明专利]一种基于单目的鲁棒性视觉惯性紧耦合定位方法

权利要求书

1.一种基于单目的鲁棒性视觉惯性紧耦合定位方法，其特征在于，步骤包括：

步骤一、通过相机采集视觉数据，通过IMU单元采集惯性数据；

步骤二、IMU预积分：利用IMU加权预积分模型同步所述视觉数据和所述惯性数据，进行IMU预积分，并得到IMU先验值；

步骤三、初始化：将所述IMU先验值代入视觉惯性联合初始化模型，完成参数的初始化；

步骤四、辅助跟踪定位：在步骤三所述参数的初始化所需时间内，利用连续关键帧之间的变换矩阵计算运动信息，代入视觉惯性融合定位框架的后端模块，实现紧耦合定位；

步骤五、跟踪定位：步骤三所述参数的初始化完成后，转而将所述参数代入视觉惯性融合定位框架的前端模块，计算得到运动信息后，代入视觉惯性融合定位框架的后端模块进行优化，实现紧耦合定位；

步骤五所述跟踪定位具体地包括：

由步骤三得到参数初始化的结果，所述参数包括方向修正的重力加速度g_w，t_j时刻帧的速度v_j，t_j时刻帧对应的旋转矩阵R_j，优化后尺度s；

对于三个连续的关键帧F1、F2和F3，有：

其中：v₂，v₃分别为关键帧F2和F3的速度，t₂，t₃分别为关键帧F2和F3的时刻，g_w为方向修正的重力加速度，R₂为关键帧F2的旋转矩阵，a₂为关键帧F2的加速度，为关键帧F2的加速度计偏置，为加速度计噪声；又有改进的关键帧F3对应的变换矩阵T₃：

其中，是相机和IMU之间的旋转外参，R₁、R₂和R₃分别为关键帧F1、F2和F3的旋转矩阵，ΔR₂₃为关键帧F2与关键帧F3间的相对旋转；

最终将改进的T₃和v₃联合作为初始状态代入视觉惯性融合定位框架的后端模块。

2.根据权利要求1所述的一种基于单目的鲁棒性视觉惯性紧耦合定位方法，其特征在于，步骤二所述IMU预积分包括如下具体步骤：

步骤2.1、计算加权IMU测量值，得到IMU坐标系下的角速度测量值和加速度测量值

t时刻典型的IMU测量值方程表示为：

其中w_B(t)和a_W(t)分别是IMU坐标系下的角速度真实值和世界坐标系下加速度真实值，b^g(t)和b^a(t)分别是陀螺仪和加速度计的偏置，η^g(t)和η^a(t)分别是陀螺仪和加速度计的高斯噪声，g^W是世界坐标系下的重力加速度，是t时刻世界坐标向IMU坐标旋转的转置；

t到t+Δt时刻之间真实的加权IMU测量值表示为：

w_B(t，t+Δt)＝c₁(t)w_B(t)+c₂(t)w_B(t+Δt)

a_B(t，t+Δt)＝c₃(t)a_B(t)+c₄(t)a_B(t+Δt) (2)

其中，a_B(t)为t时刻IMU坐标系下的加速度；加权系数c₁(t)，c₂(t)，c₃(t)，c₄(t)具体表示为：

c₁(t)＝w_B(t)/(w_B(t)+w_B(t+Δt))

c₂(t)＝w_B(t+Δt)/(w_B(t)+w_B(t+Δt))

c₃(t)＝a_B(t)/(a_B(t)+a_B(t+Δt))

c₄(t)＝a_B(t+Δt)/(a_B(t)+a_B(t+Δt)) (3)

步骤2.2、IMU坐标系下进行加权预积分，求IMU先验值，建立高精度的加权预积分模型：

由t时刻的状态积分得到当前时刻t+Δt的状态，如式(4)所示：

其中R_WB(t+Δt)是t+Δt时刻下世界坐标系相对于IMU坐标系的旋转，v_W(t+Δt)、p_W(t+Δt)分别世界坐标系下的速度和平移；R_WB(t)是t时刻下世界坐标系相对于IMU坐标系的旋转，v_W(t)、p_W(t)分别是世界坐标系下的速度和平移；w_W(τ)、a_W(τ)和v_W(τ)分别为世界坐标系下的角速度变量、加速度变量、速度变量；

将式(4)两边同乘将世界坐标系下的旋转、速度及平移三个状态量转换到IMU相对坐标系下；将相邻两次IMU测量之间的角速度、加速度及旋转视为定值，进行数值积分得到IMU坐标系下的加权预积分公式(5)：

其中，在IMU坐标系下，ΔR_ij为i，j时刻的旋转增量，ΔR_ik为i，k时刻的旋转增量；R_i为i时刻的旋转，R_j为j时刻的旋转，R_k为k时刻的旋转；为k时刻加速度测量值，为k时刻角速度测量值，为k时刻陀螺仪的偏置，为k时刻加速度计的偏置；为k时刻陀螺仪的高斯噪声，为k时刻加速度计的高斯噪声；Δv_ij为i，j时刻的速度增量，Δv_ik为i，k时刻的速度增量；v_i为i时刻的速度，v_j为j时刻的速度；g为重力加速度；Δt_ij为i，j时刻的时间增量，Δp_ij为i，j时刻的平移增量；p_i为i时刻的平移，p_j为j时刻的平移；

分别考虑i，j时刻间的预积分数值受偏置及噪声的影响：先假设偏置不变，仅讨论噪声的影响，再讨论偏置更新的影响，最后由式(6)得到IMU先验值：

其中是旋转对陀螺仪偏置的雅可比矩阵，为i时刻陀螺仪的偏置，为i时刻加速度计的偏置，为i时刻无噪声情况下陀螺仪的偏置，为i时刻无噪声情况下加速度计的偏置，是速度对陀螺仪偏置的雅可比矩阵，是速度对加速度计偏置的雅可比矩阵，和分别是i，j时刻陀螺仪和加速度计偏置的变化量，为i，j时刻的速度增量测量值，为i，j时刻无噪声情况下的旋转增量测量值，为i，j时刻无噪声情况下的旋转增量，b^g为陀螺仪偏置，b^a为加速度计偏置；(·)^表示将向量转换成矩阵。