[发明专利]一种基于因子图的IMU误差在线标定方法

权利要求书

1.一种基于因子图的IMU误差在线标定方法，其特征在于，包括：S1、采集惯性传感器中加速度计和陀螺仪的测量值，并采用所述测量值对IMU(Inertial measurement unit惯性测量单元)进行预积分传播，预积分传播方法包括：

S11、利用所述测量值计算加速度计与陀螺仪的误差系数矩阵，加速度计与陀螺仪在t时刻的误差系数矩阵分别为：

其中，M_a1、M_a2、M_a3分别表示所述加速度计的X、Y、Z轴的误差系数，M_ω1、M_ω2、M_ω3分别表示所述陀螺仪的X、Y、Z轴的误差系数，表示加速度计X轴与Y轴的安装误差，表示加速度计X轴与Z轴的安装误差，表示加速度计Y轴与Z轴的安装误差；表示陀螺仪X轴与Y轴的安装误差，表示陀螺仪X轴与Z轴的安装误差，表示陀螺仪Y轴与Z轴的安装误差；

所述误差系数矩阵中对角线位置上的元素按如下矩阵进行计算：

其中K_ax、K_ay、K_az分别表示加速度计的三个轴向的标度因数误差；K_ωx、K_ωy、K_ωz分别表示陀螺仪的三个轴向的标度因数误差；

S12、针对两相邻的IMU采样时刻t_i与t_i+1，计算t_i+1时刻惯性预积分的值，计算公式为：

其中，b_k表示初始t_k时刻的机体坐标系，分别表示相邻两惯性帧i与i+1的采样时刻t_i与t_i+1时所述加速度计的测量值，分别表示t_i与t_i+1时刻所述陀螺仪的测量值，δt表示惯性传感器的采样周期；分别表示t_i+1时刻，载体的位置预积分值、速度预积分值与角度预积分值，分别表示t_i时刻，载体的位置预积分值、速度预积分值与角度预积分值，为单位四元数；R(γ)表示将四元数转化为旋转矩阵，即表示将四元数所表示的t_i时刻载体角度预积分值转化为旋转矩阵，即表示将四元数所表示的t_i+1时刻载体角度预积分值转化为旋转矩阵；分别表示t_i时刻加速度与陀螺仪的零偏，分别表示t_i+1时刻加速度与陀螺仪的零偏，其导数为白噪声；分别表示在t_i时刻加速度与陀螺仪的误差系数矩阵，分别表示在t_i+1时刻加速度与陀螺仪的误差系数矩阵；

S2、利用所述测量值计算预积分误差传递方程，具体步骤包括：

S21、计算系统的误差状态转移矩阵F_i与噪声状态转移矩阵G_i：

其中，表示将四元数所表示的t_i时刻载体角度预积分值转化为旋转矩阵，表示将四元数所表示的t_i+1时刻载体角度预积分值转化为旋转矩阵，I为单位矩阵，δt表示惯性传感器的采样周期，所述误差状态转移矩阵F_i与噪声状态转移矩阵G_i中的部分未具体表示的元素f_mn、g_mn，其中m、n的取值为正整数，具体如下所示：

其中，分别表示在t_i时刻加速度与陀螺仪的误差系数矩阵，分别表示t_i时刻加速度与陀螺仪的零偏，a_i与a_i+1分别表示t_i时刻与t_i+1时刻所述加速度计的输出值，ω_i与ω_i+1分别表示t_i时刻与t_i+1时刻所述陀螺仪的输出值；

S22、计算相邻两个IMU采样时刻t_i与t_i+1，系统的误差传递方程、雅可比矩阵与协方差，t_i+1时刻的误差传递方程、雅可比矩阵与协方差分别为：

A_i+1＝F_iA_i+G_iN

P_i+1＝F_iP_iF_i^T+G_iQG_i^T

其中t_i时刻的误差传递方程为A_i，t_i时刻的雅可比矩阵为J_i^bk，t_i时刻的协方差为P_i；

其中，N为噪声，表示t_i时刻载体的位置预积分误差状态，表示t_i时刻载体的角度预积分误差状态，表示t_i时刻载体的速度预积分误差状态，分别表示t_i时刻载体的加速度计、陀螺仪的零偏误差状态，分别表示t_i时刻载体的误差系数矩阵的误差状态；表示t_i时刻陀螺仪与加速度计的白噪声，表示t_i+1时刻陀螺仪与加速度计的白噪声，表示加速度计零偏白噪声，表示陀螺仪零偏白噪声；

S3、采集并利用视觉图像传感器、GPS接收机、激光雷达的测量值求解惯性预积分误差，并联合所述视觉图像传感器、GPS接收机、激光雷达、惯性传感器以外的导航传感器提供的误差项优化求解载体导航信息与IMU误差，循环执行S1-S3，其中，所述载体导航信息与IMU误差的求解方法为：

采集所述视觉图像传感器、GPS接收机、激光雷达的测量值S(k)，将当前t_k+1时刻的机体系标记为b_k+1，在t_k时刻到t_k+1时刻间，载体一共采集到l个IMU数据，补偿过误差后的载体的位置预积分值、速度预积分值与角度预积分值的计算公式如下：

其中，分别表示t_k时刻到t_k+1时刻间未进行误差补偿的载体位置预积分值、速度预积分值、角度预积分值，分别表示t_k时刻载体的加速度计、陀螺仪的误差系数矩阵的误差状态，分别表示t_k时刻载体的加速度计、陀螺仪的零偏误差状态；

其中，各雅可比矩阵的计算公式为：

其中，分别表示t_k时刻到t_k+1时刻间，载体的位置预积分量、速度预积分量与角度预积分量的误差状态；

t_k+1时刻误差状态转移方程A_l为

其中，i的取值范围为1至l，N为噪声，F_i为系统的误差状态转移矩阵，G_i为噪声状态转移矩阵，G_l-1为t_k到t_k+1时刻之间第l-1个IMU数据的采样时刻时噪声状态转移矩阵；

由此，惯性预积分误差计算公式为：

其中，载体的姿态用四元数表示，表示提取四元数的虚部，补偿过误差后的载体的角度预积分值，为t_k时刻世界坐标系到机体系的旋转矩阵，分别表示t_k时刻载体在世界坐标系下的位置、速度、姿态量，分别表示t_k+1时刻载体在世界坐标系下的位置、速度、姿态量，分别表示t_k时刻与t_k+1时刻加速度计的零偏值，分别表示t_k时刻与t_k+1时刻陀螺仪的零偏值，分别表示t_k时刻与t_k+1时刻加速度计的误差系数矩阵，分别表示t_k时刻与t_k+1时刻陀螺仪的误差系数矩阵；

惯性预积分误差的增量方程的计算公式为：

其中，表示t_k时刻到t_k+1时刻间系统的协方差矩阵，δX表示优化过程中的误差状态量，包括所述视觉图像传感器、GPS接收机、激光雷达的测量值与IMU的相对平移与旋转的误差量δx_s、所述视觉图像传感器、GPS接收机、激光雷达的测量值所提供的误差状态量δx_λ，滑动窗口内n+1个量测帧的载体位置、速度、四元数、加速度计和陀螺仪零偏的误差状态量，以及包含了标度因数误差与安装误差的加速度计和陀螺仪的误差系数矩阵的误差状态量：

δX＝[δx₀,δx₁,…,δx_n,δx_s,δx_λ]

表示惯性预积分误差相对待优化状态变量X的雅可比矩阵，待优化状态变量X为：

X＝[x₀,x₁,…,x_n,x_s,x_λ]

其中x₀,x₁,…,x_n表示滑动窗口内n+1个量测帧中与载体相关的待优化状态变量，x_s表示所述视觉图像传感器、GPS接收机、激光雷达、惯性传感器以外的导航传感器与IMU所构建的待优化状态变量，x_λ表示视觉图像传感器、GPS接收机、激光雷达的测量值所提供的待优化状态变量，具体地，对于x₀,x₁,…,x_n中的某一确定的状态量x_k与x_s有：

x_k矩阵中的各元素分别表示滑动窗口内某个特定量测帧的载体位置、速度、四元数、加速度计和陀螺仪的零偏，以及标度因数误差与安装误差的加速度计和陀螺仪的误差系数矩阵，k的取值为0，1,2…n；p_s^b表示所述视觉图像传感器、GPS接收机、激光雷达、惯性传感器以外的导航传感器与IMU的相对平移量，表示所述视觉图像传感器、GPS接收机、激光雷达、惯性传感器以外的导航传感器与IMU的相对旋转量；

惯性预积分误差相对待优化状态变量X的雅可比矩阵的计算方法包括：

由惯性预积分误差的表达式可知，惯性预积分误差相对待优化状态变量x_s与x_λ的雅可比矩阵为0，即：

将剩余待优化变量，即载体的位置速度姿态信息与IMU误差信息

分为四组：

将惯性预积分误差相对待优化变量的雅可比矩阵标记为J[0]、J[1]、J[2]、J[3]；

J[0]、J[1]、J[2]、J[3]的计算方式为：

分别计算矩阵中的每一项元素，最终得到J[0]、J[1]、J[2]、J[3]的数值表达形式：

其中，I表示单位矩阵

对于四元数q＝[xyzλ]＝[ωλ]，L、R表征的意义分别为左、右，[·]_L与[·]_R分别表示四元数q的左右算子，具体地可表示为：

联合所述视觉图像传感器、GPS接收机、激光雷达、惯性传感器以外的导航传感器提供的误差项，构建目标函数：

其中，||r_p||²为边缘化的先验约束，优化中仅保留少量量测和状态，而其他测量和状态则被边缘化并转换为先验；为惯性预积分误差，B是所有IMU测量的集合，为惯性预积分误差的协方差；为视觉图像传感器、GPS接收机、激光雷达的测量值所提供的误差项；

使用高斯牛顿非线性优化方法，当达到误差收敛状态时或迭代次数达到阈值时则停止优化，输出所述载体导航信息与IMU误差。