一种基于因子图的IMU误差在线标定方法与流程

本发明涉及组合导航领域，尤其涉及了一种基于因子图的imu误差在线标定方法。

背景技术：

惯性导航系统由于其自主性强、连续性好、稳定性高的优点，以其为主的组合导航系统一直是导航技术领域的研究热点。然而惯性导航系统其本质上属于航位推算系统，存在误差随时间积累的问题。影响导航精度的误差源有很多种，而惯性器件自身的误差如：零偏、标度因数误差、安装误差等对导航精度的影响非常大，需要进行精确的标定。

为了得到imu(inertialmeasurementunit惯性测量单元)的各种误差系数，目前主要的方法有两种：

方法1：利用实验室的高精度双轴转台进行离线标定，将imu静置在高精度双轴转台上，并对转台施加不同的控制输入，同时采集一段时间的imu数据，后续通过数据处理手段可得出imu的各项误差。

方法2：利用传统的惯性预积分方法进行在线标定，仅可得出imu的零偏误差。

然而方法1存在的缺点是imu的各项误差由于随机启动不确定性与标定本身存在的误差使得离线标定结果与实际使用imu时的误差存在偏差，对实时高精度导航带来不利的影响；方法2存在的缺点是对imu器件误差的考虑较单一该方法通常只能标定出零偏这一项imu误差，而忽略了标度因数误差与安装误差对导航精度的影响，因此同样会对高精度导航带来不利影响。综上所述，现有技术中imu误差标定方法适用性较弱，无法完成对imu标度因数误差与安装误差的在线标定，从而影响了系统精度。

技术实现要素：

本发明提供一种基于因子图的imu误差在线标定方法，能够联合惯性残差与其它导航传感器提供的残差，优化求解载体导航信息与imu误差，提高实时高精度导航的精度，且成本低、适用性强。

为达到上述目的，本发明采用如下技术方案：

一种基于因子图的imu误差在线标定方法，包括：

s1、采集惯性传感器中加速度计和陀螺仪的测量值，并采用所述测量值对imu(inertialmeasurementunit惯性测量单元)进行预积分传播。

s2、利用所述测量值计算所述预积分误差传递方程。

s3、采集并利用其他导航传感器，即视觉图像传感器、gps接收机、激光雷达的测量值求解惯性预积分误差，并联合其它导航传感器提供的误差项优化求解载体导航信息与imu误差，循环执行s1-s3。

进一步的，在所述s1中，所述预积分传播方法包括：

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

其中，ma1、ma2、ma3分别表示加速度计的x、y、z轴的误差系数，mω1、mω2、mω3分别表示陀螺仪的x、y、z轴的误差系数，表示加速度计x轴与y轴的安装误差，表示加速度计x轴与z轴的安装误差，表示加速度计y轴与z轴的安装误差；表示陀螺仪x轴与y轴的安装误差，表示陀螺仪x轴与z轴的安装误差，表示陀螺仪y轴与z轴的安装误差；

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

其中kax、kay、kaz分别表示加速度计的三个轴向的标度因数误差；kωx、kωy、kωz分别表示陀螺仪的三个轴向的标度因数误差；

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

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

进一步的，所述s2包括：

s21、计算系统的误差状态转移矩阵fi与噪声状态转移矩阵gi：

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

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

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

ai+1＝fiai+gin

pi+1＝fipifi^t+giqgi^t

其中ti时刻的误差传递方程为ai，ti时刻的雅可比矩阵为ji^bk，ti时刻的协方差为pi；

其中，n为噪声，表示ti时刻载体的位置预积分误差状态，表示ti时刻载体的角度预积分误差状态，表示ti时刻载体的速度预积分误差状态，分别表示ti时刻载体的加速度计、陀螺仪的零偏误差状态，分别表示ti时刻载体的误差系数矩阵的误差状态；表示ti时刻陀螺仪与加速度计的白噪声，表示ti+1时刻陀螺仪与加速度计的白噪声，表示加速度计零偏白噪声，表示陀螺仪零偏白噪声；为初始时刻系统状态的雅可比矩阵，且有q为噪声n的协方差矩阵，初始时刻有

进一步的，在所述s3中，载体导航信息与imu误差的求解方法为：

采集所述其他导航传感器的测量值s(k)，将当前tk+1时刻的机体系标记为bk+1，在tk时刻到tk+1时刻间，载体一共采集到l个imu数据，补偿过误差后的载体的位置预积分值、速度预积分值与角度预积分值的计算公式如下：

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

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

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

tk+1时刻误差状态转移方程al为

其中，i的取值范围为1至l，l为imu采集数据的个数，n为噪声，fi为系统的误差状态转移矩阵，gi为噪声状态转移矩阵，gl-1为tk到tk+1时刻之第l-1个imu数据的采样时刻时噪声状态转移矩阵；

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

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

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

其中，表示tk时刻到tk+1时刻间系统的协方差矩阵，δx表示优化过程中的误差状态量，包括其他导航传感器与imu的相对平移与旋转的误差量δxs，所述其他导航传感器所提供的误差状态量δxλ，滑动窗口内n+1个量测帧的载体位置、速度、四元数、加速度计和陀螺仪零偏的误差状态量，以及包含了标度因数误差与安装误差的加速度计和陀螺仪的误差系数矩阵的误差状态量：

δx＝[δx0,δx1,…,δxn,δxs,δxλ]

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

x＝[x0,x1,…,xn,xs,xλ]

其中x0,x1,…,xn表示滑动窗口内n+1个量测帧中与载体相关的待优化状态变量，xs表示其它导航传感器与imu所构建的待优化状态变量，xλ表示其他导航传感器所提供的待优化状态变量，具体地，对于x0,x1,…,xn中的某一确定的状态量xk与xs有：

xk矩阵中的各元素分别表示滑动窗口内某个特定量测帧的载体位置、速度、四元数、加速度计和陀螺仪的零偏，以及标度因数误差与安装误差的加速度计和陀螺仪的误差系数矩阵，k的取值为0，1,2…n；表示其它导航传感器与imu的相对平移量，表示其它导航传感器与imu的相对旋转量；

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

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

将剩余待优化变量，即载体的位置速度姿态信息与imu误差信息分为四组：

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

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

同理按如下方式计算j[2]、j[3]：

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

其中，i表示单位矩阵

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

联合所述其它导航传感器提供的误差项，构建目标函数：

其中，||rp||²为边缘化的先验约束，优化中仅保留少量量测和状态，而其他测量和状态则被边缘化并转换为先验；为惯性预积分误差，b是所有imu测量的集合，为惯性预积分误差的协方差；为其他导航传感器所提供的误差项；

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

本发明的有益效果是：

本发明可以解决由于载体运动带来的机械冲击或其它因素使得imu误差发生变化，从而带来导航精度降低的问题。

与传统imu离线误差标定方法相比，本发明有以下两点主要优势：1.离线标定通常目的为验证imu的各项误差是否符合说明书指标，仅可为载体的导航定位算法提供一定的参考，无法解决由imu误差发生变化，带来导航精度降低的问题。而本发明将imu误差作为待估计的状态量，可以实时估计imu的误差并对导航算法进行补偿，从而解决该问题，提高导航精度。2.离线标定过程时间周期较长、经济投入较大、环境要求苛刻，而本发明成本低、环境适用性强。

与以卡尔曼滤波器为代表的在线标定系统相比，本发明有以下两点主要优势：1.卡尔曼滤波器仅利用当前量测信息进行解算，得到的结果仅是当前时刻的最优解；而本发明所采用的因子图优化方法中所建立的因子节点包括了系统整个时间段或者某个滑动窗口时间段内的所有信息，得到的是这整段时间段内的全局最优解，因此本发明对imu误差的估计结果更为准确。2.针对载体在实际运动过程中的非线性部分来说，卡尔曼滤波算法对非线性部分只进行一次线性化处理，线性化误差较大，并随着推算过程会产生误差的累积；但是本发明在因子图优化中所采用的高斯牛顿迭代方法，其在估计导航解时会进行多次的线性化，可以有效地减小线性化误差，因此本发明对imu误差的估计结果更为准确。

与利用传统预积分方法的在线标定系统相比，本发明的主要优势为：传统利用传统预积分方法的在线标定系统对imu器件误差的考虑较单一、imu建模简单，往往仅考虑零偏这一项imu误差，对imu误差仅能做部分补偿；而本发明将imu的标度因数误差与安装误差引入至惯性预积分的过程与因子节点的构建过程中，因此本发明对imu误差的补偿更为全面，对导航精度的提升效果更为明显。

综上，本发明进一步提高了实时高精度导航的精度，且成本低、适用性强。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1是实施例的方法流程图。

具体实施方式

为使本领域技术人员更好地理解本发明的技术方案，下面结合具体实施方式对本发明作进一步详细描述。

本发明实施例提供一种基于因子图的imu(inertialmeasurementunit惯性测量单元)误差在线标定方法，流程如图1所示，包括：

步骤一：采集惯性传感器中加速度计和陀螺仪的测量值

步骤二：加速度计测量值和陀螺仪测量值对imu进行预积分传播。

1)加速度计与陀螺仪的误差系数矩阵按如下形式进行计算：

其中，ma1、ma2、ma3分别表示所述加速度计的x、y、z轴的误差系数，mω1、mω2、mω3分别表示所述陀螺仪的x、y、z轴的误差系数，表示加速度计x轴与y轴的安装误差，表示加速度计x轴与z轴的安装误差，表示加速度计y轴与z轴的安装误差；表示陀螺仪x轴与y轴的安装误差，表示陀螺仪x轴与z轴的安装误差，表示陀螺仪y轴与z轴的安装误差。

两矩阵对角线位置上的元素按如下形式进行计算：

其中kax、kay、kaz分别表示加速度计的三个轴向的标度因数误差；kωx、kωy、kωz分别表示陀螺仪的三个轴向的标度因数误差。

2)针对相邻两imu采样时刻ti与ti+1，按如下形式计算ti+1时刻惯性预积分的值：

其中，bk表示初始tk时刻的机体坐标系，分别表示相邻两惯性帧i与i+1的采样时刻ti与ti+1时加速度计的数据，分别表示ti与ti+1时刻陀螺仪的数据，δt表示惯性传感器的采样周期；分别表示ti+1时刻，载体的位置预积分值、速度预积分值与角度预积分值，分别表示ti时刻，载体的位置预积分值、速度预积分值与角度预积分值，为单位四元数；γ是用四元数表示的旋转，r(γ)表示将四元数转化为旋转矩阵，表示将四元数所表示的ti时刻载体角度预积分值转化为旋转矩阵，表示将四元数所表示的ti+1时刻载体角度预积分值转化为旋转矩阵；初始时刻，和为0，分别表示ti时刻加速度与陀螺仪的零偏，分别表示ti+1时刻加速度与陀螺仪的零偏，其导数为白噪声；分别表示在ti时刻加速度与陀螺仪的误差系数矩阵，分别表示在ti+1时刻加速度与陀螺仪的误差系数矩阵。；在相邻两imu帧间隔内保持不变。

步骤三：利用惯性传感器数据和计算惯性预积分误差传递方程。

1)系统的误差状态转移矩阵fi与噪声状态转移矩阵gi按如下形式进行计算：

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

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

2)对于相邻两个imu采样时刻ti与ti+1，系统的误差传递方程、雅可比矩阵与协方差按如下形式进行计算：

ai+1＝fiai+gin

pi+1＝fipifi^t+giqgi^t

其中ti时刻的误差传递方程为ai，ti时刻的雅可比矩阵为ji^bk，ti时刻的协方差为pi；

其中，n为噪声，表示ti时刻载体的位置预积分误差状态，表示ti时刻载体的速度预积分误差状态，表示ti时刻载体的角度预积分误差状态、分别表示ti时刻载体的加速度计、陀螺仪的零偏误差状态，分别表示ti时刻载体的误差系数矩阵的误差状态；表示ti时刻陀螺仪与加速度计的白噪声，表示ti+1时刻陀螺仪与加速度计的白噪声，表示加速度计零偏白噪声，表示陀螺仪零偏白噪声；为初始时刻系统状态的雅可比矩阵，且有q为噪声n的协方差矩阵，初始时刻有

步骤四：当采集到视觉图像传感器、gps接收机、激光雷达的测量数据s(k)时，求解惯性预积分误差并联合视觉导航传感器所提供的重投影误差项优化求解载体导航信息与imu误差。

1)当采集到视觉导航传感器数据s(k)时，记该关键帧tk+1时刻机体系为bk+1，在tk时刻到tk+1时刻间，一共有l个imu数据，补偿过误差后的载体的位置预积分值、速度预积分值与角度预积分值按如下形式进行计算：

其中，分别表示tk时刻到tk+1时刻间未进行误差补偿的载体位置预积分值、速度预积分值、角度预积分值，由步骤二所给出的相邻两imu帧间的惯性预积分传播方程，从第一帧imu数据，递推计算至第l帧imu数据所得。分别表示tk时刻载体的加速度计、陀螺仪的误差系数矩阵的误差状态，分别表示tk时刻载体的加速度计、陀螺仪的零偏误差状态。

各雅可比矩阵按如下方式进行计算：

其中，分别表示tk时刻到tk+1时刻间，载体的位置预积分量、速度预积分量与角度预积分量的误差状态，由步骤三所给出的相邻两imu帧的惯性预积分的误差状态从第一帧imu数据，递推至第l帧imu数据计算得出，具体地，有：

al为tk+1时刻误差状态转移方程,i的取值范围为1至l，l为imu采集数据的个数，n为噪声，fi为系统的误差状态转移矩阵，gi为噪声状态转移矩阵，gl-1为tk到tk+1时刻之第l-1个imu数据的采样时刻时噪声状态转移矩阵；

由此，惯性预积分误差按如下形式进行计算：

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

2)惯性预积分误差的增量方程按如下方式进行计算：

其中，表示tk时刻到tk+1时刻间系统的协方差矩阵，由步骤三所给出的相邻两imu帧的协方差矩阵递推计算得出。

δx表示优化过程中的误差状态量，包括视觉导航传感器与imu的相对平移与旋转的误差量δxs，滑动窗口内m+1个视觉特征点的逆深度，n+1个量测帧的载体位置、速度、四元数、加速度计和陀螺仪零偏的误差状态量，以及包含了标度因数误差与安装误差的加速度计和陀螺仪的误差系数矩阵的误差状态量：

δx＝[δx0,δx1,…,δxn,δxs,δλ0,δλ1,...δλm]

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

x＝[x0,x1,…,xn,xs,λ0,λ1,...,λm]

其中x0,x1,…,xn表示滑动窗口内n+1个量测帧中与载体相关的待优化状态变量，xn表示滑动窗口内某个量测帧的载体位置、速度、四元数、加速度计和陀螺仪的零偏，以及包含了标度因数误差与安装误差的加速度计和陀螺仪的误差系数矩阵；xs表示视觉导航传感器与imu的相对平移与旋转量；λm表示某个视觉特征点的逆深度。

3)惯性预积分误差相对待优化状态变量x的雅可比矩阵按如下方式进行计算：

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

将剩余待优化变量分为四组：记惯性预积分误差相对待优化变量的雅可比矩阵为j[0]、j[1]、j[2]、j[3]，分别给出j[0]、j[1]、j[2]、j[3]的计算方式：

同理按如下方式计算j[2]、j[3]：

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

其中，i表示单位矩阵

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

4)联合视觉导航传感器所提供的重投影误差，按如下方式构建目标函数：

其中，||rp||²为边缘化的先验约束，优化中仅保留少量量测和状态，而其他测量和状态则被边缘化并转换为先验；为惯性预积分误差，b是所有imu测量的集合，为惯性预积分误差的协方差；为视觉导航传感器所提供的重投影误差。使用高斯牛顿非线性优化方法，当达到误差收敛状态时或迭代次数达到阈值时则停止优化，输出载体的导航信息与误差系数矩阵。

步骤五：输出载体导航信息与imu误差，并循环执行步骤一～步骤五。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。