[发明专利]一种基于IMU预积分的视觉惯性里程计方法

权利要求书

1.一种基于IMU预积分的视觉惯性里程计方法，其特征在于，包括如下步骤；

(1)搭建视觉惯性里程计系统：定义固连在载体上的IMU参考坐标系为I系；定义固连在载体上的相机参考坐标系为C系；以初始时刻相机位置为世界坐标系原点，右前上为XYZ轴建立世界坐标系，即G系；定义载体坐标系与I系重合；载体运动过程中采集相机传感器和IMU传感器数据；

(2)构建系统状态向量和误差向量，并建立IMU运动方程和误差状态方程；

(3)构建离散化系统协方差矩阵，并由IMU测量执行零阶四元数积分计算当前时刻IMU的旋转，执行IMU预积分算法计算当前时刻IMU的位置以及速度；

(4)当系统接收到新的图像数据后，对系统状态向量增广，并对系统协方差矩阵增广；

(5)由相机观测构建系统观测模型；

(6)执行扩展卡尔曼滤波更新获得载体位姿；

步骤(2)中，构建系统状态向量和误差向量，并建立IMU运动方程和误差状态方程的具体步骤如下：

IMU状态向量定义为：

式中，表示从G系到I系的旋转单位四元数向量；^Gv_I和^Gp_I表示载体在世界坐标系中的速度和位置的三维向量；b_g和b_a表示IMU中陀螺仪和加速度计的三维偏置向量；和^Ip_C表示相机参考系和IMU参考系之间的旋转四元数和平移向量；

误差四元数定义为：

且

式中，δq表示误差四元数；表示四元数估计值；表示小角度旋转的三维向量；

IMU误差状态向量定义为：

式中，表示小角度旋转量；表示陀螺仪偏置误差；表示世界坐标系下载体速度误差；表示加速度计偏置误差；表示世界坐标系下载体的位置误差；表示IMU参考系和相机参考系之间的小角度旋转；表示IMU参考系和相机参考系之间的位置误差；

系统误差向量定义为：

式中，每个相机误差状态定义为：

式中，和分别表示第i个相机在世界坐标系下的小角度旋转误差和位置误差；

连续时间IMU观测状态传递方程如下：

式中，表示为IMU角速度测量估计值，w_m表示为IMU角速度测量值，表示为陀螺仪偏置的估计值；表示为IMU加速度测量估计值，a_m表示为IMU加速度测量值，表示加速度计偏置的估计值；C(·)表示旋转四元数对应的旋转矩阵；其中定义如下：

式中，其中分别为陀螺仪在载体固定坐标系采集的x轴、y轴和z轴方向上的角速度数据的估计值；T表示矩阵转置；

基于IMU运动方程，IMU误差状态方程如下：

式中，F表示为系统矩阵；G表示系统观测噪声矩阵；其中，n_I定义如下：

n_I^T＝[n_g^Tn_wg^Tn_a^Tn_wa^T]^T

式中，n_g和n_a表示陀螺仪和加速度计测量的高斯白噪声矩阵，n_wg和n_wa表示陀螺仪和加速度计测量偏置的随机游走；其中F和G定义分别如下:

步骤(3)中，构建离散化系统协方差矩阵，并由IMU测量执行零阶四元数积分计算当前时刻IMU的旋转，执行IMU预积分算法计算当前时刻IMU的位置以及速度的具体步骤如下：

离散系统的状态转移矩阵如下：

式中，Φ_k表示k时刻状态转移矩阵；

离散系统的噪声协方差矩阵如下：

式中，Q_k表示离散系统噪声协方差矩阵；其中Q表示连续系统噪声协方差矩阵，方程如下：

Q＝E[n_In_I^T]

建立IMU状态协方差矩阵方程如下：

式中，表示k时刻IMU状态协方差矩阵；表示k+1时刻增广的IMU状态协方差矩阵；

k时刻IMU和相机系统协方差矩阵方程如下：

式中，P_k|k表示k时刻IMU和相机系统协方差矩阵；表示k时刻IMU和相机之间的协方差关联矩阵；表示k时刻相机状态协方差矩阵；

利用k时刻IMU和相机系统协方差矩阵方程建立k+1时刻IMU和相机系统协方差矩阵如下：

根据k时刻到k+1时刻的IMU测量，使用零阶四元数积分更新旋转四元数方程如下：

式中，表示k+1时刻旋转四元数；表示k时刻的旋转四元数；Δt表示时间间隔，Δt方程如下：

Δt＝t_k+1-t_k

根据k时刻到k+1时刻的IMU测量，使用IMU预积分更新载体速度和位置方程如下：

式中，^Gv_k+1表示k+1时刻载体在世界坐标系下的速度；^Gv_k表示k时刻载体在世界坐标系下的速度；表示k时刻载体坐标系到世界坐标系的旋转矩阵；表示τ时刻载体系到k时刻载体系的旋转矩阵；n_a表示IMU测量噪声；^Gp_k+1表示k+1时刻载体在世界坐标系下的位置；^Gp_k表示k时刻载体在世界坐标系下的位置；

根据上式方程，记：

式中，^kα_k+1和^kβ_k+1表示k+1时刻的预积分项，其方程如下：

式中，δt表示IMU测量时间间隔，方程如下：

Δt＝t_τ+1-t_τ

步骤(4)中，当系统接收到新的图像数据后，对系统状态向量增广，并对系统协方差矩阵增广具体步骤如下：

当相机拍摄到一张新图像时，由此时IMU状态计算相机状态的方程如下：

式中，表示世界坐标系到此时刻相机坐标系的旋转四元数；表示世界坐标系到此时刻相机坐标系的平移矢量；和表示相机和IMU之间的外参数；

将此时的相机位姿填入系统状态向量后面，增广系统状态量；相应增广系统协方差矩阵方程如下：

式中，N为系统状态向量中相机状态数量；J为雅可比矩阵，方程如下：

J＝(J_I 0_6×6N)

其中，

步骤(5)中，由相机观测构建系统观测模型具体步骤如下：

假设当前时刻相机观测到空间中单个特征点f_j,测量方程为：

式中，表示第i个相机观测到第j个特征点的像素坐标；和分别为特征点j在第i个相机中的三维坐标分量；

由测量方程构建观测残差并线性化方程如下：

式中，表示第i个相机的观测残差；表示第i个相机观测的估计值；表示第i个相机测量噪声；和分别为关于和的雅可比矩阵；

对特征点f_j的所有相机观测值在维度上叠加后，构建系统观测残差方程如下：

式中，r^j表示叠加后的观测残差；n^j表示叠加后的系统观测噪声，和分别为叠加后的雅可比矩阵；

对上式方程左乘的左零空间矩阵V^T,得：

式中，

步骤(6)中，执行扩展卡尔曼滤波更新获得当前载体位姿具体步骤如下：

由系统观测方程和系统状态方程，执行扩展卡尔曼滤波方程如下：

式中，K表示卡尔曼增益；ΔX表示系统状态更新量；P_k+1|k+1表示k+1时刻系统协方差矩阵；R_n表示噪声协方差矩阵，由系统状态更新量可以获得当前机器人的系统状态X，从而得到载体位姿。