CN102865881A - 一种惯性测量单元的快速标定方法 - Google Patents

Abstract

本发明涉及一种惯性测量单元的快速标定方法。该方法可以在不使用任何外部设备的条件下，仅通过用户手持转动IMU遍历各方向的运动，即可在短时间内较精确地标定出陀螺零偏、比例因子、加速度计零偏和比例因子共十二个误差系数。本发明具有无硬件成本、高效率、简单易行的特点，且能保证一定的标定精度。本发明特别适合于对中低精度IMU的现场快速标定，有效解决微机械IMU参数的环境敏感性问题，促进MEMS惯性器件的推广应用。

Description

一种惯性测量单元的快速标定方法

技术领域

本发明涉及微机电系统技术领域，尤其是一种惯性测量单元的快速标定方法。

背景技术

近年来随着微机电系统MEMS(Micro-Electro Mechanical Systems)技术的发展而产生的MEMS IMU(惯性测量单元)具有成本低(大批量时)、尺寸小、重量轻、功耗低、可靠性高等优点，使其广泛应用于人们生活的各方各面。越来越多的消费类产品中装入了三轴陀螺和加速度计，从而构成了MEMS IMU，用户可以选择他们感兴趣的应用，如游戏、多媒体、个人导航等。

但是，MEMS传感器性能(尤其是零偏和比例因子)随着使用环境(尤其是温度)变化大仍是一个明显问题。这个问题可以通过标定来解决，即通过标定来确定传感器的误差参数，并在IMU使用过程中进行补偿。

传统的标定方法大多依赖专业的标定设备，或耗时较长，难以满足针对消费类产品不依赖外部设备、快速、简单易行的要求。

发明内容

本发明的技术解决问题是：提供一种对惯性测量单元的快速标定的方法，该方法可以在没有任何外部设备、基准的情况下对惯性测量单元进行标定。使用该方法对惯性传感器的零偏和比例因子进行标定，提高其使用精度。

本发明的技术解决方案为一种惯性测量单元的快速标定方法，包括以下步骤：

步骤1，在惯性测量系统开机预热后，判断是否已知惯性测量单元的初始水平姿态角，是则直接进入步骤3，否则进入步骤2；

步骤2，将惯性测量单元保持一段时间的静态，根据静态时段的加速度计测量信息和陀螺测量信息，近似计算惯性测量单元的初始水平姿态角；

步骤3，任意设定惯性测量单元的初始航向姿态角；

步骤4，将惯性测量单元绕其测量中心旋转，并基于已知的初始水平姿态角或步骤2所得初始水平姿态角及步骤3所得初始航向姿态角，通过标定解算得到待估的陀螺参数和加速度计参数；计算时将惯性测量单元的位置变化量以及速度变化量均设定为零，分别作为伪位置观测信息和伪速度观测信息。

而且，步骤1中将惯性测量单元保持一段时间的静态和步骤2中将惯性测量单元绕其测量中心旋转，采用手工操作实现或者其它机器设备操作实现。

而且，步骤4采用卡尔曼滤波标定方法进行标定计算，具体实现方式如下，步骤4.1，对卡尔曼滤波标定方法的整个标定计算过程进行建模，得到标定模型；基于已知的初始水平姿态角或步骤2所得初始水平姿态角及步骤3所得初始航向姿态角设置标定模型中相关参数的初值，进行标定算法初始化；

步骤4.2，将惯性测量单元绕其测量中心旋转，同时采用标定模型进行实时数据处理；

步骤4.3，判断待估IMU参数是否已收敛至相应的预设精度，是则进入步骤4.4，否则继续执行步骤4.2；

步骤4.4，标定动作完成，直接进入步骤4.5，或者对数据进行一次反向平滑后进入步骤4.5；

步骤4.5，标定完成，获得待估的陀螺参数和加速度计参数。

本发明具有如下技术效果：

1.本标定方法不需要任何外部设备或装置来辅助标定。对标定动作也没有严格要求，手持IMU绕IMU测量中心采用充足的无精度要求的旋转运动即可完成；另提供一些动作指导，可以进一步提高标定效率和精度；

2.该标定方法可以在短时间内较精确地同时标定出加速度计和陀螺的零偏和比例因子，整个过程可以在30秒左右完成；若按照本方法中提供的动作指导，可进一步缩短时间；

3.本标定方法利用了GPS(全球导航卫星系统)/INS(惯性导航系统)组合导航算法来对中低精度的IMU传感器零偏和比例因子进行标定。使用伪观测信息取代GPS测量信息来完成对参数的估计。

4.伪观测信息的提出是本标定方法的关键。伪观测信息包括伪位置观测和伪速度观测。二者的思想是认为IMU的位置和线速度的变化范围是有限的。可同时使用伪位置和伪速度信息作为量测信息，也可只取其一。位置和速度的变化范围在系统的量测误差矩阵中体现，既可根据实际操作情况进行设置，也可由程序自适应地完成。伪位置和伪速度观测信息的采用省去了大多数标定方法对IMU在各个方向上静止一段时间的要求，提高了标定效率和标定操作的方便性。除了上述伪位置和伪速度观测信息外，本发明也可采用标定过程中IMU的其它运动特征作为伪观测量。

5.在本发明的基础上给出了一系列操作指导思想。用户并不一定需要严格按照这些指导思想进行操作，但按照这些指导思想进行操作可显著提高标定效率。这些指导思想是基于各种动作下对各个传感器参数进行估计的可观测性分析给出的。

6.算法可以用最优或次优(可根据用户兴趣选取)估计方法来完成。既可以用卡尔曼滤波，也可以是最小二乘(或加权最小二乘)或是其他估计手段。

7.本标定方法中体现出加速度和陀螺相互标定的思路。即：可以将加速度计组合和陀螺组合视为两个系统，不但加速度计测量信息可以被用于标定和修正陀螺参数，陀螺测量信息也可以用来标定和修正加速度计参数。

8.若使用卡尔曼滤波或递推最小二乘作为估计工具，则在旋转操作的过程中，已经不断地采用卡尔曼滤波算法实时地对加速度计和陀螺的零偏和比例因子进行估计，并不断反馈修正。旋转动作完成后，即已经完成标定，不需要后处理；若在旋转动作完成后对数据进行一次反向平滑处理，可进一步提高结果精度。若选用其他估计手段，则可在用户做完动作后的短时间内，解算出被估IMU参数。

9.本发明中提到的惯性测量单元(IMU)不限于低精度的MEMS IMU，还包括其它不同精度的惯性传感器(即陀螺和加速度计)组成的IMU；除了常见的三轴陀螺和三轴加速度计构成的IMU之外，还包括带有冗余配置的IMU和不完整轴线配置的IMU，甚至是单轴惯性传感器。

10.本发明中的标定方法不限于手工标定方法，还包括采用其它机器设备(如转台)产生标定运动的标定。

附图说明

图1是本发明实施例的流程图。

具体实施方式

本发明保持IMU一小段时间(数秒钟即可)的静态或准静态以用于惯性导航系统的初始对准，控制IMU绕其测量中心(或近似绕其测量中心)在空间内旋转，选择估计方法并针对该方法进行建模，使用本发明中提出的伪位置或伪速度信息作为量测信息来进行估计计算，提供IMU参数，按标定模型对后续陀螺仪和加速度计的直接测量值进行补偿后IMU即可进入正常工作状态。本发明所提到的所有IMU操作既可以手工进行，也可以采用其它机器设备(如转台)产生标定运动。本发明用于标定惯性测量单元(IMU)。本发明中提到的惯性测量单元不限于低精度的MEMS IMU，还包括其它不同精度的惯性传感器(即陀螺和加速度计)组成的IMU；除了常见的三轴陀螺和三轴加速度计构成的IMU之外，还包括带有冗余配置的IMU和不完整轴线配置的IMU，甚至是单轴惯性传感器。同时包括含有其他类型传感器的多传感器组合导航系统。

参见图1，实施例具体步骤如下：

步骤1，在惯性测量系统开机预热后，判断是否已知惯性测量单元的初始水平姿态角，是则直接进入步骤3，否则进入步骤2。

本领域的姿态角包括水平姿态角和航向姿态角，具体实施时，也可以直接判断是否已知惯性测量单元的初始姿态角，是则直接进入步骤4，否则进入步骤2。

已知值的初始水平姿态角或初始姿态角采用精确值或近似值均可。

步骤2，将惯性测量单元保持一段时间的静态，根据静态时段的加速度计测量信息和陀螺测量信息，近似计算惯性测量单元的初始水平姿态角。本发明不要求严格保持静态，因此将惯性测量单元保持一段时间的准静态也是可以的。

实施例在惯性测量系统开机预热后将IMU固定，  保持一段时间(建议2-3秒钟即可)的静态或准静态(即大体上静止，允许由于操作造成的IMU振动、晃动等动态)，用于惯性导航系统的初始对准。根据静态或准静态时段的加速度计及陀螺信息，求取IMU的初始姿态角。由加速度计输出信息可确定水平姿态角。这里选取公式roll＝sign(f_z)sin^-1(f_y/g)，pitch＝-sign(f_z)sin^-1(f_x/g)来分别确定横滚角roll和俯仰角pitch。这里f_x、f_y及f_z分别为x、y及z轴方向的比力输出，g为本地重力加速度值，sign(·)为符号函数。这里的比例值即可取加速度计某一时间点的输出信息，也可使用某些时段的加速度计信息的平均值。

除该步骤中所述方法外，任何可以确定IMU初始水平姿态角的方法均可。若是已经知道IMU的近似水平姿态角，则可以跳过步骤2。

步骤3，任意设定惯性测量单元的初始航向姿态角。

本发明中，对初始航向姿态角没有任何要求，也不需要求取，在算法中随意设定一个值即可(如零度)，可以由程序自行随机生成；若已知航向姿态角，同样也可以直接采用；也可由陀螺输出信息计算出初始航向姿态角。

步骤4，将惯性测量单元绕其测量中心旋转，并基于已知的初始水平姿态角或步骤2所得初始水平姿态角及步骤3所得初始航向姿态角，通过标定解算得到待估的陀螺参数(即误差系数，包括零偏和比例因子)和加速度计参数(即误差系数，包括零偏和比例因子)。所得陀螺参数和加速度计参数，即待估传感器参数，用于在惯性测量单元工作时补偿惯性测量单元直接得到的加速度计测量信息和陀螺测量信息。在本步骤完成后，IMU即可进入正常工作状态。

控制IMU绕其测量中心(或近似绕其测量中心)在空间内旋转时，本发明对旋转动作没有严格要求，不要求准确的旋转角度(如旋转90度)或指向(如某轴朝上)，采用充足的无精度要求的旋转运动即可完成。旋转动作完成即代表整套IMU操作部分即已完成。只要能使IMU遍历各种姿态，即可相应地保证标定精度。

标定动作时间以保证所有待估IMU参数收敛至相应程度为准，建议半分钟左右即可。实际操作中既可以进行时间充分的操作以保证标定精度；也可以根据本发明中各参数信息的提示确定标定动作是否已经充分。比如，在标定过程中，实时地通过卡尔曼滤波器提供的信息(比如被估IMU参数的标准差)来判断标定的完成进度。结合相应产品的精度需求，设定标定结果门限值。若系统提供的信息显示已达到该精度要求，则提示用户标定已完成，可停止标定动作。

本发明同时基于各种动作下对各个传感器参数进行估计的可观测性分析给出了一系列操作指导思想。用户并不一定需要严格按照这些指导思想进行操作，但按照这些指导思想进行操作可显著提高标定效率。

本发明的标定数据处理既可以在标定动作进行过程中实时进行，也可以在标定操作完成后进行事后处理。算法可以用最优或次优(可根据具体情况选取)估计方法来完成。既可以用卡尔曼滤波，也可以是最小二乘(或加权最小二乘)或是其他估计手段。

若采用实时进行标定数据处理的方案，则可采用卡尔曼滤波或递推最小二乘作为估计工具，则在标定操作的过程中，不断地对加速度计和陀螺的零偏和比例因子进行实时估计，并不断反馈修正。旋转动作完成后，即已经完成标定，不需要后处理。在标定过程中，实时地通过卡尔曼滤波器提供的信息(比如被估IMU参数的标准差)来判断标定的完成进度。结合相应产品的精度需求，设定标定结果门限值。若系统提供的信息显示已达到该精度要求，则提示用户标定已完成，可停止标定动作。在卡尔曼滤波计算完成后，也可使用滤波结果，结合整段标定数据，进行一次反向平滑。将反向平滑的结果和正向滤波的结果加权平均，可得到更为精确的传感器参数估计结果。

若采用事后处理，可在用户做完动作后的短时间内，解算出被估IMU参数。可选用其他估计手段，比如最小二乘(或加权最小二乘)。

可以预先针对本发明中所选用的估计方法进行建模，得到相应的标定模型。以卡尔曼滤波器为例，状态参数包含导航状态(位置、速度、姿态角)或其中部分导航参数、惯性传感器参数以及其他参数(比如其他传感器提供的辅助信息)。惯性传感器参数可同时包含传感器的零偏、比例因子；也可仅包含零偏或比例因子。所有状态参数均可采用误差形式或非误差形式。可使用任何符合实际情况的随机过程对待估惯性传感器参数进行建模，如随机常数、随机游走、一阶高斯马尔可夫等。

标定计算时，由于基于惯性测量单元绕其测量中心旋转时，要求IMU测量中心位置变化范围有限，本发明在解算程序中将IMU的位置变化量以及速度变化量均设定为零，以分别作为伪位置观测信息和伪速度观测信息。实际中可能的位置和速度变化范围用量测噪声的形式给出。量测误差矩阵设置由程序自适应地完成，也可根据实际操作情况进行设置。伪位置和伪速度观测信息的采用省去了大多数标定方法对IMU在各个方向上静止一段时间的要求，提高了标定效率和标定操作的方便性。

既可同时使用伪位置和伪速度信息作为量测信息，也可只取其一。除了上述伪位置和伪速度观测信息外，本发明也可采用标定过程中IMU的其它运动特征作为伪观测量。上述伪观测信息不仅可以独立使用，也可与其他标定方法结合起来使用。若是所标定对象为多传感器组合导航系统，也可使用其他传感器提供的信息作为量测信息。由于伪观测是有一定的噪声范围的，使用时可采用量测噪声来体现观测值可能的不准确程度，比如量测噪声的形式给出可能的位置和速度变化范围。

本标定方法中体现出加速度和陀螺相互标定的思路。即：可以将加速度计组合和陀螺组合视为两个系统，不但加速度计测量信息可以被用于标定和修正陀螺参数，陀螺测量信息也可以用来标定和修正加速度计参数。

详细步骤说明如下。主要结合卡尔曼滤波方法进行说明，最小二乘则一定程度上类似。

步骤4.1，对卡尔曼滤波标定方法的整个标定计算过程进行建模，得到标定模型。建模后，进行标定算法初始化(设置滤波过程中用到的各状态参数的初值和矩阵的初始状态)，与具体选择的卡尔曼滤波方法有关，可由采用软件技术实现自动赋值。可以选用各种卡尔曼滤波，这里以采用增广卡尔曼滤波(augmented Kalman filter，AKF)的情况为例，其他卡尔曼滤波方法类似。

滤波过程中用到的各状态参数，可采用向量形式，记为卡尔曼滤波状态向量x。建议卡尔曼滤波里的待估状态选用误差形式。实施例的卡尔曼滤波状态向量x的状态参数中包含导航误差状态参数(包括位置误差、速度误差及姿态角误差，实际使用中也可舍掉其中某些状态)以及传感器误差参数(即待估陀螺和加速度计参数的误差，包括加速度计及陀螺的零偏及比例因子的误差)。

一般的，进行卡尔曼滤波前，可给卡尔曼滤波状态向量x的状态参数赋初值，对各状态参数的方差构成的方差协方差阵、系统的状态噪声矩阵和量测噪声矩阵进行初始设置。卡尔曼滤波状态向量x的状态参数初值建议全设成0。各导航误差状态参数的方差构成的方差协方差阵代表的各状态参数的误差程度，其中各元素可按相应的实际情况设。待估传感器参数相应的元素建议参照传感器的性能指标来设。状态噪声矩阵参照传感器的性能指标来设。量测噪声矩阵代表了实际中IMU的位置、速度变化和实施例采用的伪位置观测信息和伪速度观测信息(0)可能有多大差异。可以通过软件技术事先的自动数据处理过程：设置一个一般化的量测噪声阵进行一段时间的滤波计算，参照这段时间的滤波结果来设定量测噪声，再对整个滤波过程中的数据进行滤波计算，得到量测噪声矩阵的初值。卡尔曼滤波方法的具体实现可参见相关文献：秦永元等，1998年，卡尔曼滤波与组合导航原理，西北工业大学出版社。

整个标定过程所需要的模型如下：

加速度计和陀螺输出误差可分别建模为：

δf^b＝b_a+diag(f^b)δs_a+w_a    (1)

${\mathrm{\δ\ω}}_{\mathrm{ib}}^b=b_g+\mathrm{diag}\left({\mathrm{\ω}}_{\mathrm{ib}}^b\right){\mathrm{\δs}}_g+w_g---\left(2\right)$

式中δf^b和

分别为比力和角速度误差向量；b_a和b_g依次为加速度计和陀螺的零偏；δs_a和δs_g分别为加速度计和陀螺的比例因子误差；f^b和

分别为真实比力和角速度。w_a和w_g为加速度计和陀螺的伪噪声项。符号diag(v)表示由向量v＝[v_x v_y v_z]^T中元素组成的对角阵：

$\mathrm{diag}\left(v\right)=\left[\begin{array}{ccc}{v}_x& 0& 0\\ 0& v_y& 0\\ 0& 0& v_z\end{array}\right]---\left(3\right)$

建议位置初值根据当前的坐标赋初值，速度初值赋为0；水平姿态角的初值采用已知的初始水平姿态角或步骤2所得初始水平姿态角，航向姿态角采用步骤3所得初始航向姿态角；陀螺和加速度计零偏初值建议设成0；陀螺和加速度计比例因子建议设成1。本领域将水平姿态角和航向姿态角统称姿态角，若是已知初始姿态角，据此赋值给水平姿态角和航向姿态角作为初值。也就是说，实时数据处理时，第一历元用初始姿态角，后面都用前一历元求得的姿态角。

卡尔曼滤波的状态方程为

$\mathrm{\δ}{\stackrel{\·}{r}}^c=-{\mathrm{\ω}}_{\mathrm{ec}}^c\×{\mathrm{\δr}}^c+{\mathrm{\δv}}^c---\left(4\right)$

$\mathrm{\δ}{\stackrel{\·}{v}}^c=F_{\mathrm{vr}}{\mathrm{\δr}}^c-({2\mathrm{\ω}}_{\mathrm{ie}}^c+{\mathrm{\ω}}_{\mathrm{ec}}^c)\×{\mathrm{\δv}}^c+f^c\×\mathrm{\ψ}+C_b^p{\mathrm{\δf}}^b---\left(5\right)$

$\stackrel{\·}{\mathrm{\ψ}}=-({\mathrm{\ω}}_{\mathrm{ie}}^c+{\mathrm{\ω}}_{\mathrm{ec}}^c)\×\mathrm{\ψ}-C_b^n{\mathrm{\δ\ω}}_{\mathrm{ib}}^b---\left(6\right)$

${\stackrel{\·}{b}}_a=-\frac{1}{{\mathrm{\τ}}_{\mathrm{ba}}}{b}_a+w_{\mathrm{ba}}---\left(7\right)$

${\stackrel{\·}{b}}_g=-\frac{1}{{\mathrm{\τ}}_{\mathrm{bg}}}{b}_g+w_{\mathrm{bg}}---\left(8\right)$

$\mathrm{\δ}{\stackrel{\·}{s}}_a=-\frac{1}{{\mathrm{\τ}}_{\mathrm{sa}}}{\mathrm{\δs}}_a+w_{\mathrm{sa}}---\left(9\right)$

$\mathrm{\δ}{\stackrel{\·}{s}}_g=-\frac{1}{{\mathrm{\τ}}_{\mathrm{sg}}}{\mathrm{\δs}}_g+w_{\mathrm{sg}}---\left(10\right)$

(4)～(6)式为对导航误差参数的建模。这里仅选用各状态和物理量投影在c系(计算坐标系)的情况进行分析，实际也可投影到任何坐标系，且分析类似。

其中δr^c，δv^c和ψ分别为载体位置(纬度、经度、高程)误差、速度误差以及姿态角旋转矢量误差在c系(计算坐标系)中的投影；b_a和b_g依次为加速度计和陀螺的零偏；δs_a和δs_g分别为加速度计和陀螺的比例因子误差；

依次为各量的时间微分；

f^c为c系中比力向量；

和

分别为地球自转角速度和c系相对e系的旋转角速度在c系中投影；τ_ba，τ_bg，τ_sa及τ_sg分别代表各传感器误差的相关时间；w_ba，w_bg，w_sa及w_sg为驱动白噪声；和分别为b系(载体坐标系)到p系(平台坐标系)和b系到n系(导航坐标系)的方向余弦矩阵；符号×表示两向量的叉乘。

(5)式中F_vr可表示为：

F_vr＝[-g/(R_M+h)-g/(R_N+h)2g/(R+h)]            (11)

其中R_M和R_N依次为地球子午圈和卯酉圈曲率半径；g和h分别为本地重力值和高程。

(7)～(10)式为对待估传感器参数的建模。这里均选用一阶高斯马尔可夫过程建模。若采用随机常数、随机游走等随机过程来建模亦可。：

卡尔曼滤波的量测信息可以选用伪位置约束或伪速度约束，对应的量测模型可依次写为：

$z_r={\hat{r}}^c-{\tilde{r}}^c={\mathrm{\δr}}^c+n_r---\left(12\right)$

$z_v={\hat{v}}^c-{\tilde{v}}^c={\mathrm{\δv}}^c+n_v---\left(13\right)$

其中 ${\tilde{r}}^c=\mathrm{cons}\tan t$ 且 ${\tilde{v}}^c=0.$

这里z_r和z_v分别为卡尔曼滤波位置和速度量测向量；和为卡尔曼滤波状态方程预测的位置和速度；

和

为伪位置和伪速度观测向量；δr^c和δv^c为状态向量中的位置误差和速度误差；n_r和n_v为量测噪声。

若选用伪位置和伪速度信息共同约束，则(12)和(13)均用做量测方程。若仅用伪位置或伪速度信息，则仅选用(12)或(13)作为量测方程。

特别地，仅选用伪速度信息时，也可以从卡尔曼滤波器状态向量中去掉位置误差，并从状态方程中去掉(4)式，以及(5)式中的F_vrδr^c项。

此外，考虑到本发明的使用对象及操作情况(IMU位置近似不变，速度近似为零、且针对中低精度IMU)，也可对状态方程进行一些简化。

(5)、(6)式可简化为：

$\mathrm{\δ}{\stackrel{\·}{v}}^c=-({2\mathrm{\ω}}_{\mathrm{ie}}^c+{\mathrm{\ω}}_{\mathrm{ec}}^c)\×{\mathrm{\δv}}^c+f^c\×\mathrm{\ψ}+C_b^p{\mathrm{\δf}}^b---\left(14\right)$

$\stackrel{\·}{\mathrm{\ψ}}=-({\mathrm{\ω}}_{\mathrm{ie}}^c+{\mathrm{\ω}}_{\mathrm{ec}}^c)\×\mathrm{\ψ}-C_b^n{\mathrm{\δ\ω}}_{\mathrm{ib}}^b---\left(15\right)$

设置卡尔曼滤波相关参数。参照标定对象内置IMU的传感器性能参数来设定滤波状态误差方差阵(Q)，以及初始状态向量(x)及初始状态方差阵(P)中与待估参数(加速度计和陀螺的零偏和比例因子)有关项。参照实际标定动作情况，来设定量测误差方差阵(R)，以及x和P初值中与导航误差参数(位置、速度、姿态角误差)有关项。IMU位置或速度可能的变化范围在量测误差方差阵R中体现。

若使用最小二乘作为估计方式，则相应地对平差过程建模。可将待估传感器参数作为待估参数；伪位置、伪速度信息可用作量测信息或限制条件，实际操作中可能的位置和速度变化范围在协方差矩阵中给出。

步骤4.2，将惯性测量单元绕其测量中心旋转，同时采用标定模型进行实时数据处理。本发明在旋转过程中，即由卡尔曼滤波器不断进行状态预测以及量测更新，实时地对加速度计和陀螺的零偏和比例因子进行估计，并不断反馈修正。就是说，每一次滤波，会估计一组误差状态参数。然后对应地用这组参数值修正当前位置、速度、姿态角和传感器参数的值。这些值再作为系数参与下一次滤波计算。

若使用最小二乘作为估计手段，则在该步骤中不进行数据处理，储存各时刻的传感器输出即可。

步骤4.3，根据卡尔曼滤波中提供的指标信息，判断参数收敛是否已收敛至相应的预设精度，是则进入步骤4.4，否则继续执行步骤4.2。若步骤4.4在标定动作完成后使用反向平滑，则判断是否已收敛至相应程度的预设精度为A，否则预设精度为B，A大于B。即可适当放宽对目标收敛程度的要求。

若使用最小二乘作为估计手段，则在标定操作过程中无法获取指标信息，则需要旋转足够的时间，来保证标定精度。使用最小二乘，则必要的操作时间为C，使用卡尔曼滤波，必要的操作时间为D，C小于D。即是用最小二乘只需要较少的操作时间，即可保证与卡尔曼滤波相同的精度。

步骤4.4，标定动作完成，直接进入步骤4.5，或者对数据进行一次反向平滑后进入步骤4.5。

在滤波计算完成后进行反向平滑，可进一步提高参数估计精度或缩短标定时间。若需使用反向平滑，需将各时刻系统的状态参数量及其方差协方差阵保存下来。滤波过程中保存的数据在反向平滑算法中被利用，通过最大似然方法估计新系统的状态量。通过对正向滤波和反向平滑的结果加权可尽量保持结果的连续型和平滑性。

以固定时间间隔平滑(Fixed Interval Smoothing)算法为例，完整的RTS平滑方程如下：

${\hat{x}}_{k|N}={\hat{x}}_{k|k}+A_k({\hat{x}}_{k+1|N}-{\hat{x}}_{k+1|k})---\left(16\right)$

$P_{k|N}=P_{k|k}+A_k(P_{k|N}-P_{k+1|k}){A_k}^T---\left(17\right)$

其中A_k为平滑增益矩阵：

$A_k=P_{k|k}{\mathrm{\Φ}}_k^T{\left(P_{k+1|k}\right)}^{-1}---\left(18\right)$

k＝N-1，N-2，...，0，N为总的量测更新次数。Φ为离散卡尔曼滤波系统的状态转移矩阵；

为状态向量；P为状态参数的方差协方差矩阵。  下标j|i表示利用量测向量z₁，z₂，...，z_i，根据其数学模型求定第t_j时刻的状态向量或方差协方差矩阵的最佳估值。

步骤4.5，标定完成，获得待估的陀螺参数和加速度计参数。

基于以上步骤，可以对系统的可观测性进行一些分析。可观测性描述了系统对各待估状态的估计能力。通过分析系统的可观测性，也可以给出一系列操作指导思想。若按照这些指导思想进行操作可显著提高标定效率。

具体分析如下：

考虑到本发明中针对中低精度的IMU，且整个过程中IMU测量中心的位置变化及线速度范围非常有限，则在进行可观测性分析时，可以对状态方程进行一些简化。是否做近似不会影响可观测性分析结果。这里将可观测性分析放在n系中进行，若在其他坐标系中分析则类似。

取伪速度约束的情况为例分析。简化后的状态方程为：

$\mathrm{\δ}{\stackrel{\·}{v}}^n=f^n\×{\mathrm{\ψ}}^n+{\mathrm{\δf}}^n---\left(19\right)$

${\stackrel{\·}{\mathrm{\ψ}}}^n=-{\mathrm{\δ\ω}}_{\mathrm{ib}}^n---\left(20\right)$

${\stackrel{\·}{b}}_a=0---\left(21\right)$

${\stackrel{\·}{b}}_g=0---\left(22\right)$

$\mathrm{\δ}{\stackrel{\·}{s}}_a=0---\left(23\right)$

$\mathrm{\δ}{\stackrel{\·}{s}}_g=0---\left(24\right)$

其中依次为速度误差、姿态角旋转矢量误差、加速度计零偏、陀螺零偏、加速度计比例因子误差、陀螺比例因子误差在在n系中投影的时间微分；ψⁿ为姿态角旋转矢量误差；fⁿ为n系中比力向量。

δfⁿ和

分别为比力向量和角速度向量误差在n系中的投影：

${\mathrm{\δf}}^n=b_a^n+\mathrm{diag}\left(f^n\right){\mathrm{\δs}}_a^n+w_a^n---\left(25\right)$

${\mathrm{\δ\ω}}_{\mathrm{ib}}^n=b_g^n+\mathrm{diag}\left({\mathrm{\ω}}_{\mathrm{nb}}^n\right){\mathrm{\δs}}_g^n+w_g^n---\left(26\right)$

和

依次为加速度计和陀螺的零偏在n系中的投影；

和

分别为加速度计和陀螺的比例因子误差在n系中的投影；fⁿ和

分别为比力和b系相对n系的角速度在n系中的投影。

和

为n系中加速度计和陀螺的伪噪声项。符号diag(v)表示由向量v中元素组成的对角阵。

由于IMU没有明显的位移，这里忽略了n系相对i系(惯性坐标系)的角速度

将(24)和(25)分别代入(19)和(20)，可得：

$\mathrm{\δ}{\stackrel{\·}{v}}^n=f^n\×{\mathrm{\ψ}}^n+b_a^n+\mathrm{diag}\left(f^n\right){\mathrm{\δs}}_a^n---\left(27\right)$

$\stackrel{\·}{\mathrm{\ψ}}=-(b_g^n+\mathrm{diag}\left({\mathrm{\ω}}_{\mathrm{nb}}^n\right){\mathrm{\δs}}_g^n)---\left(28\right)$

此时的量测向量为：

z＝z_v，其中z_v为上述速度量测向量。

假定 $x_u(=\left[\begin{array}{cccccc}{\left({\mathrm{\δv}}_u^n\right)}^T& {\left({\mathrm{\ψ}}_u^n\right)}^T& {\left(b_{\mathrm{au}}^n\right)}^T& {\left({\mathrm{\δs}}_{\mathrm{au}}^n\right)}^T& {\left(b_{\mathrm{gu}}^n\right)}^T& {\left({\mathrm{\δs}}_{\mathrm{gu}}^n\right)}^T\end{array}\right])$ 是系统的一组不可观状态。以下对任一状态A使用下标u，即A_u表示状态A中的不可观部分。对量测向量依次求各阶时间微分得：

$z={\mathrm{\δv}}_u^n=0---\left(29\right)$

$\stackrel{\·}{z}=f^n\×{\mathrm{\ψ}}_u^n+b_{\mathrm{au}}^n+\mathrm{diag}\left(f^n\right){\mathrm{\δs}}_{\mathrm{au}}^n=0---\left(30\right)$

$\stackrel{\·\·}{z}=f^n\×(b_{\mathrm{gu}}^n+\mathrm{diag}\left({\mathrm{\ω}}_{\mathrm{nb}}^n\right){\mathrm{\δs}}_{\mathrm{gu}}^n)=0---\left(31\right)$

$\stackrel{\·\·\·}{z}=f^n\×\mathrm{diag}\left({\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{nb}}^n\right){\mathrm{\δs}}_{\mathrm{gu}}^n)=0---\left(32\right)$

(4)

z＝0    (33)

·

·

·

(k)

z＝0，k＝4，5，...，n-1            (34)

(29)式没有非零解，因此速度误差δvⁿ总是可观测的。

本发明中，由于IMU量测中心近乎不动，则

fⁿ＝[0  0  -g]^T                (35)

将所有n系中的向量表示为其分量的形式，即 $v_u^n={\left[\begin{array}{ccc}{v}_{\mathrm{Nu}}& v_{\mathrm{Eu}}& v_{\mathrm{Du}}\end{array}\right]}^T.$ 三个元素分别代表北向、东向及地向的分量。

则(30)式可以写为接下来的(36)-(38)式：

gψ_Eu+b_aNu＝0                (36)

-gψ_Nu+b_aEu＝0               (37)

b_aDu-gδs_aDu＝0              (38)

从(36)及(37)式可以看出，北向和东向的加速度计零偏的不可观部分(即b_aNu和b_aEu)分别与东向和北向的姿态角误差不可观部分(即ψ_Eu和ψ_Nu)相关。δs_aDu为地向加速度计比例因子误差的不可观部分。

从(40)可以看出，地向姿态角旋转矢量误差ψ_D总是不可观的，因为这个状态没有在方程组中出现。b_aDu(地向加速度计零偏不可观部分)和gδs_aDu相关。但是，由于对于中低精度的加速度计而言，其零偏较比例因子要高数个量级(即b_aD远大于gδs_aD)，因此b_aD的会迅速收敛至与gδs_aD相当的水平。也就是说b_aD会表现为强可观。

综上，垂直方向的加速度计误差会被估计，进而可以估计水平姿态角误差。

相似地，(33)式可以写为(39)-(40)

-g(b_gNu+ωNδs_gNu)＝0        (39)

g(b_gEu+ωEδs_gEu)＝0         (40)

b_gDu和δs_gDu没有在这些公式中出现，意味着垂向陀螺的零偏b_gDu和比例因子δs_gDu总是不可观的。b_gNu、b_gEu、δs_gNu和δs_gEu依次为北向陀螺零偏、东向陀螺零偏、北向陀螺比例因子误差、东向陀螺比例因子误差的不可观部分；ω_N和ω_E为北向和东向旋转角速度。若ω_N＝0，则b_gN(北向陀螺零偏)可观，δs_gN(北向陀螺比例因子误差)不可观；相似地，若ω_E＝0，则b_gE(东向陀螺零偏)可观，δs_gE(东向陀螺比例因子误差)不可观。若ω_N≠ 0或ω_E≠0，则b_gNu或b_gEu和ω_Nδs_gNu或ω_Eδs_gEu有关。考虑到b_gN远大于ω_Nδs_gN且b_gE远大于ω_Eδs_gE，则b_gN或b_gE会表现为强可观且迅速收敛至ω_Nδs_gN或ω_Eδs_gE。

(34)可写为(43)-(44)式：

$-g{\stackrel{\·}{\mathrm{\ω}}}_N{\mathrm{\δs}}_{\mathrm{gNu}}=0---\left(43\right)$

$g{\stackrel{\·}{\mathrm{\ω}}}_E{\mathrm{\δs}}_{\mathrm{gEu}}=0---\left(44\right)$

如果

则δs_gN不可观；若是则δs_gN可观。  相似地，δs_gE的可观测性取决于是否为零。和为北向和东向旋转角速度的时间微分。

综上，绕水平轴线的角速度变化有利于相应轴线上陀螺比例因子的估计，进而将增强相应的陀螺零偏的估计。与此同时，可以看出，绕垂直轴线的旋转时没有用处的。

根据上述可观测性分析，可以给出一些操作指导思想：

1.尽量使IMU绕水平轴线旋转。这类动作是有效的；绕垂直轴线的旋转对标定没有帮助，是无效的。同时，可以考虑正反转以增大陀螺输出范围，增强比例因子的估计。

2.尽量使IMU遍历各种姿态。因为一个姿态下的不可观误差参数，会在另一个姿态下变为可观。

用户并不一定需要严格按照这些指导思想进行操作，但按照这些指导思想进行操作可显著提高标定效率。

为说明本发明效果起见，以下提供使用本发明标定试验和结果：

使用两款不同精度的IMU，在两种操作模式下分别对本发明的精度进行了测试。所选两款IMU分别为：NovAtel SPAN-FSAS(www.novatel.com/assets/Documents/Papers/FSAS.pdf)及Xsens MTi-G (www.xsens.com/en/general/mti-g)。SPAN-FSAS内置高端战术级IMU，MTi-G则基于典型的MEMS惯性传感器。二者的相关性能参数见表1，表1是本发明标定实验所用两款IMU相关性能参数表。

表1

选用两款IMU的目的如下：

1.要评估本发明的标定精度，需要知道惯性传感器误差参数的真值，以作为参考。理论上，这些真值可以通过在实验室使用高精度的标定方法，比如六位置法来获得。但是，对于中低精度的IMU，尤其是MEMS IMU，其误差随温度变化大，因此，不可能获得一套真值，来作为所有时刻下标定结果的参考值。这时，考虑引入相对高精度的IMU，来解决这个问题。FSAS的误差较中低精度的IMU而言要稳定几个数量级。因此，若是将FSAS在实验室中进行标定并补偿，则其可以被认为是一款理想的，无误差的IMU。用本发明对该IMU进行标定，则可以认为最后得到的传感器误差参数值均是由于本发明算法造成。在此基础上，为了进一步反映本发明标定中低精度IMU的实际情况，再人为地向上述理想IMU输出内加入典型的中低精度传感器误差，此时得到的就是一款理想的，已知传感器误差参数的中低精度IMU。

具体地，人为加入的中低精度传感器误差为：1000deg/h的陀螺零偏；50000mGal的加速度计零偏；所有传感器比例因子误差大小均为5000ppm。为了区分，各轴线所加误差正负号不同。

2.因为使用FSAS已经足以验证本发明的精度，接下来，可使用MTi-G作为一款应用实例，进一步验证本发明的可行性。

所有测试均没有使用任何外部设备，仅靠手工完成。为了验证不同的标定操作方式(是按照上述指导思想，还是由一个非专业的用户在没有任何指导的情况下进行操作)。在实验中按照两种模式进行：

模式1：有指导的操作。基于本发明给出的上述指导思想，每次沿一水平的IMU轴线来旋转，来增强参数的可观测性。各陀螺均有机会在近似水平时作为旋转轴，且能经历顺时针和逆时针旋转。各加速度计也均有机会近似朝上或朝下。

模式2：随意操作。用来反映在非专业用户的操作下，本发明可以达到的精度。IMU在手中的旋转完全是随意的，而不遵循任何指导思想。

标定结果如下：

(1)在本发明指导思想下使用本发明对FSAS进行了30次标定。

表2是模式1(有指导的操作)下标定SPAN-FSAS统计结果表。表中第1列表示待估传感器误差参数；第2列表示加入的误差值；第3和第4列分别代表标定结果的内符合精度和外符合精度。

从表2中看出，模式1(有指导的操作)下标定SPAN-FSAS，30次结果的内符合精度(STD，重复性)约为：加速度计零偏200mGal；加速度计比例因子误差300ppm；陀螺零偏10deg/h；陀螺比例因子误差200ppm。

外符合精度(RMS，精度)约为：加速度计零偏400mGal；加速度计比例因子误差400ppm；陀螺零偏10deg/h；陀螺比例因子误差300ppm。外符合精度可以代表在该模式下本发明的精度。

表2

(2)使用本发明随意操作FSAS进行了30次标定。

表3是模式2(随意操作)下标定SPAN-FSAS统计结果表。表中第1列表示待估传感器误差参数；第2列表示加入的误差值；第3和第4列分别代表标定结果的内符合精度和外符合精度。

从表3中看出，外符合精度(RMS，精度)约为：  加速度计零偏900mGal；加速度计比例因子误差600ppm；陀螺零偏35deg/h；陀螺比例因子误差400ppm。

表3

(3)使用本发明随意操作MTi-G进行了30次标定。以作为非专业用户操作本发明的应用实例。

表4是模式2(随意操作)下标定MTi-G统计结果表。第1、3、4列分别表示待估参数、其内符合和外符合精度。第2列这里表示待估误差的初值。

从表4中可以看出，模式2(随意操作)下标定MTi-G的外符合精度(RMS，精度)约为：加速度计零偏1400mGal；加速度计比例因子误差1100ppm；陀螺零偏140deg/h；陀螺比例因子误差1200ppm。考虑到MTi-G使用MEMS IMU，故结果误差中中还包括由温度变化造成的影响。此应用实例也印证了本发明可以用于标定中低精度的IMU。

表4

总结

本发明涉及一种快速标定惯性测量单元误差系数的方法。该方法可以在不使用任何外部设备的条件下，仅通过用户手持转动IMU遍历各方向的运动，即可在短时间(30s左右)内较精确地标定出陀螺零偏、比例因子、加速度计零偏和比例因子共十二个误差系数。

表5为两种操作模式下的标定结果表。第1列为待估传感器误差参数；第2列为在模式1(有指导的操作)下实验达到的精度；第3列为在模式2(随意操作)下实验达到的精度。

表5表明，本发明可以精确标定中低精度IMU。由于本发明具有无硬件成本、高效率、简单易行的特点，因此特别适合于对中低精度IMU的现场快速标定，有效解决MEMS IMU参数的环境敏感性(尤其是温度敏感性)问题，促进MEMS惯性器件的推广应用。

表5

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

Claims (3)

1.一种惯性测量单元的快速标定方法，其特征在于，包括以下步骤：

步骤1，在惯性测量系统开机预热后，判断是否已知惯性测量单元的初始水平姿态角，是则直接进入步骤3，否则进入步骤2；

步骤2，将惯性测量单元保持一段时间的静态，根据静态时段的加速度计测量信息和陀螺测量信息，近似计算惯性测量单元的初始水平姿态角；

步骤3，任意设定惯性测量单元的初始航向姿态角；

步骤4，将惯性测量单元绕其测量中心旋转，并基于已知的初始水平姿态角或步骤2所得初始水平姿态角及步骤3所得初始航向姿态角，通过标定解算得到待估的陀螺参数和加速度计参数；计算时将惯性测量单元的位置变化量以及速度变化量均设定为零，分别作为伪位置观测信息和伪速度观测信息。

2.根据权利要求1所述惯性测量单元的快速标定方法，其特征在于：步骤1中将惯性测量单元保持一段时间的静态和步骤2中将惯性测量单元绕其测量中心旋转，采用手工操作实现或者其它机器设备操作实现。

3.根据权利要求1或2所述惯性测量单元的快速标定方法，其特征在于：步骤4采用卡尔曼滤波标定方法进行标定解算，具体实现方式如下，

步骤4.1，对卡尔曼滤波标定方法的整个标定计算过程进行建模，得到标定模型；基于已知的初始水平姿态角或步骤2所得初始水平姿态角及步骤3所得初始航向姿态角设置标定模型中相关参数的初值，进行标定算法初始化；

步骤4.2，将惯性测量单元绕其测量中心旋转，同时采用标定模型进行实时数据处理；

步骤4.3，判断待估IMU参数是否已收敛至相应的预设精度，是则进入步骤4.4，否则继续执行步骤4.2； 

步骤4.4，标定动作完成，直接进入步骤4.5，或者对数据进行一次反向平滑后进入步骤4.5；

步骤4.5，标定完成，获得待估的陀螺参数和加速度计参数。

Images