技术领域

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

背景技术

近年来随着微机电系统 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， 在惯性测量系统开机预热后， 判断是否已知惯性测量单元的初始水平姿态角 ， 是则直接进入步骤 3， 否则进入步骤 2。

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

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

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

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

除该步骤中所述方法外， 任何可以确定 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年， 卡尔曼滤波与组合导航原理， 西北工业大学出版社。

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

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

式中^ 和 δ 分别为比力和角速度误差向量； 和 依次为加速度计和陀螺的零 偏； 。 和 分别为加速度计和陀螺的比例因子误差； 和 ^b _ib 分别为真实比力和角速 度。 \^和\^为加速度计和陀螺的伪噪声项。 符号 ^(ν)表示由向量 v = [v _x v _y v _z 了中元 素组成的对角阵：

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

Ψ -(ω^ + ω^) χ ψ - C _b d&' «5。= -丄 b _a +w _fc 。 （7) b = _~ -b +w, (8)

(9)

= -Ss +w (10)

(4)〜(6)式为对导航误差参数的建模。 这里仅选用各状态和物理量投影在 c系 （计算坐标 系） 的情况进行分析， 实际也可投影到任何坐标系， 且分析类似。 其中 ^^，^^和^)/分别为载体位置 (纬度、 经度、 高程)误差、 速度误差以及姿态角旋转 矢量误差在 c系（计算坐标系）中的投影； 和 依次为加速度计和陀螺的零偏； 。和 分别为加速度计和陀螺的比例因子误差； Si ^c 、 δΥ、 ψ b _a , Ss _a , <¾ _g 依次为各量的 时间微分；

为 c系中比力向量； （^和 ω 分别为地球自转角速度和 c系相对 e系的旋转角速度在 c系中投影； _{W sa} R r 分别代表各传感器误差的相关时间； _Wfca ， w _bg , w _M 及 为 驱动白噪声； 和 C _fc "分别为 b系 （载体坐标系） 到 p系 （平台坐标系） 和 b系到 n系 （导 航坐标系） 的方向余弦矩阵； 符号 X 表示两向量的叉乘。

(5)式中 F _vr 可表示为：

F _vr =[-g/(R _M +h) - _g /(R _N +h) 2g/(R + h)] (11) 其中 ^«和 依次为地球子午圈和卯酉圈曲率半径； R = ^； g和 h 分别为本地 重力值和高程。

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

卡尔曼滤波的量测信息可以选用伪位置约束或 伪速度约束， 对应的量测模型可依次写为： z _r =r ^c -r ^c = Sr ^c +n _r (12) 其中 f ^c = constant 且 Ϋ ^ε = 0。 这里 τ _Γ 和 分别为卡尔曼滤波位置和速度量测向量； 和 ^为卡尔曼滤波状态方程预 测的位置和速度； 和 为伪位置和伪速度观测向量； Λ· ^ε 和^^为状态向量中的位置误差 和速度误差； 和 η _ν 为量测噪声。 若选用伪位置和伪速度信息共同约束， 则 (12)和 (13)均用做量测方程。 若仅用伪位置或伪 速度信息， 则仅选用 (12)或 (13)作为量测方程。

特别地， 仅选用伪速度信息时， 也可以从卡尔曼滤波器状态向量中去掉位置误 差， 并从 状态方程中去掉 (4)式， 以及 (5)式中的 F _v i ^项。 此外， 考虑到本发明的使用对象及操作情况 （IMU位置近似不变， 速度近似为零、 且针 对中低精度 IMU) , 也可对状态方程进行一些简化。

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

δΥ = -(2ω + ( : _c ) χ ^v ^e + f ^e χ ψ + C _b ^p Sf ^b (14)

Ψ = ~( ^ω ί + «L) ^x Ψ - C _b ⁿ d& ^b _ib (15) 设置卡尔曼滤波相关参数。 参照标定对象内置 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 Smoothin ) 算法为例， 完整的 RTS平滑方程如下： 其中 Λ为平滑增益矩阵：

Α, = Ρ„Φ[ (Ρ, _+1| , ) ^_1 (18) k = N - l, N - 2 .., Q , N为总的量测更新次数。 Φ为离散卡尔曼滤波系统的状态转移矩阵； 为状态向量； P为状态参数的方差协方差矩阵。 下标 表示利用量测向量 z z ₂ ， . . . , Z, , 根据其数学模型求定第 时刻的状态向量或方差协方差矩阵的最佳估值 。 步骤 4.5， 标定完成， 获得待估的陀螺参数和加速度计参数。

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

具体分析如下：

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

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

SV = f " X _ψ " + δΐ" (19)

Υ=-δ& (20) b _a =0 (21) b,= 0 (22) δΚ= 0 (23) ^s _g = 0 (24) 其中 t"、 ψ" b _a Ss _a , <¾ _g 依次为速度误差、 姿态角旋转矢量误差、 加速度计零 偏、 陀螺零偏、 加速度计比例因子误差、 陀螺比例因子误差在在 n系中投影的时间微分； ψ" 为姿态角旋转矢量误差； Γ为 n系中比力向量。 δΓ和 δω 分别为比力向量和角速度向量误差在 η系中的投影： δϊ ^π =b" + diag (f ⁿ )Ss ⁿ _a + w" (25) δ^Ι =b" _g + diag(a ⁿ _nh )d^ ⁿ _g + w" _g (26) K和 b" _g 依次为加速度计和陀螺的零偏在 n系中的投影； 和 δ 分别为加速度计和陀 螺的比例因子误差在 η系中的投影； Γ和 分别为比力和 b系相对 η系的角速度在 η系中 的投影。 和 为 η系中加速度计和陀螺的伪噪声项。 符号 'ί^(ν)表示由向量 V中元素组 成的对角阵。

由于 IMU没有明显的位移， 这里忽略了 η系相对 i系 （惯性坐标系） 的角速度 i 。 将 （24) 和 （25)分别代入 （19) 和 （20) ， 可得：

SV =f" χψ" +b" + diag(f ⁿ )Ss ⁿ _a (27)

Ψ = _(b; + diag((o ⁿ _nh )Ss ⁿ _g ) (28) 此时的量测向量为： Z = Z _v , 其中 Z _v 为上述速度量测向量。 假定 _x „(=[( :f ( _Ψ :) ^τ (K ^T (^IY (b:„) ^T ( )Ί)是系统的一组不可观状态。以下对任 一状态 Α使用下标"， 即 表示状态 A中的不可观部分。对量测向量依次求各阶时� �微分得： z = SY _U " = 0 (29) έ = Γ X ψ= + b ⁿ _au + diag(TW = 0 (30) z = f " x (b ⁿ _gu + diag (o _nb )Ss ⁿ _gu ) = 0 (31) · = Γ x diag(m ⁿ _nh )Ss ⁿ _gu ) = 0 (32)

(4)

z = 0 (33)

(*)

z =0,k = 4,5,...,n-\ (34)

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

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

将所有 n系中的向量表示为其分量的形式， 即 =[ _ViV „ v _Eu 。 三个元素分别代表北 向、 东向及地向的分量。

贝 IJ 30)式可以写为接下来的 36M38) 式：

(37) b _aDu _gSs _aDu = (38) 从 (36) 及 （37)式可以看出， 北向和东向的加速度计零偏的不可观部分 (即 b^ 和 b _aE J 分别与东向和北向的姿态角误差不可观部分 (即 和 ^ν„)相关。 为地向加速度计比例 因子误差的不可观部分。

从 (40)可以看出， 地向姿态角旋转矢量误差 总是不可观的，因为这个状态没有在方程组 中出现。 _DU (地向加速度计零偏不可观部分） 和^ 相关。 但是， 由于对于中低精度的 加速度计而言， 其零偏较比例因子要高数个量级 (即 b _aZ3 远大于 g ^), 因此 b ^的会迅速收 敛至与 g5s _aD 相当的水平。 也就是说 b _aD 会表现为强可观。 综上， 垂直方向的加速度计误差会被估计， 进而可以估计水平姿态角误差。

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

-8(b _gNu +^ _N Ss _gNu ) = 0 (39) g(b _gEu + _gE J = Q (40) b _gDu 和 5 _SgDu 没有在 ^&¾公式中出现，意味着 向陀螺的零偏 b _Z¾ ^Btt例因子 总是 不可观的。 b _gNu 、 b _gEu 、 ^ 和 ^依次为北向陀螺零偏、 东向陀螺零偏、 北向陀螺比例因 子误差、 东向陀螺比例因子误差的不可观部分； 和《 _£ 为北向和东向旋转角速度。 若 =0， 则 b _gN (北向陀螺零偏） 可观， S _SgN (北向陀螺比例因子误差） 不可观； 相 似地， 若 ί¾=0， 则 _£ (东向陀螺零偏） 可观， 5s _gE (东向陀螺比例因子误差） 不可观。 若<¾≠0 或 <¾≠0， 则 或 _g£u 和 或 有关。 考虑到 b^远大于 且 b _gE 远大于 c _E S _SgE , 则 b^ 或 b _gE 会表现为强可观且迅速收敛至 或 c _E Ss _gE 。

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

-g(b _N ds _gNu = 0 (43) g _gEu =0 (44) 如果 ^=0， 则 ^不可观； 若是 ^≠0， 则 可观。 相似地， 的可观测性 取决于 ώ _Ε 是否为零。 和 为北向和东向旋转角速度的时间微分。 综上， 绕水平轴线的角速度变化有利于相应轴线上陀 螺比例因子的估计， 进而将增强相 应的陀螺零偏的估计。 与此同时， 可以看出， 绕垂直轴线的旋转时没有用处的。

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

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

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

用户并不一定需要严格按照这些指导思想进行 操作， 但按照这些指导思想进行操作可显 著提高标定效率。 为说明本发明效果起见， 以下提供使用本发明标定试验和结果：

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

表 1 IMU

传感器 SPAN-FSAS MTi-G

级别 Tactical Grade MEMS

采样率 200Hz 100Hz

陀螺 零偏 （Ισ ) 0.75deg/h 3600deg/h

白噪声 （ARW) 0.1 deg/Vh 3.0 deg/Vh

比例因子误差 <300ppm

(1σ )

加速度计零偏 (1σ ) lOOOmGal 2000mGal

白噪声 （VRW) <0.0005m/s ² /VHz0.002m/s ² /VHz

比例因子误差 <1000ppm 3000ppm

(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; 陀螺零偏 lOdeg/h; 陀螺比例因子误差 300ppm。 外符合精度可以代表在该模式下本发明的精 度。

表 2

、、、、、、、、、、、、、、、、、、、、 、、、、、、、、、、、、、、、、、、、、 、、、、、、、、、、、、、、、、、、、、 、、、、、、、、、、、、、、、、、、、、 、、、、、、、、、、、、、、、、、、、、 、、、、、

待估传感器加入的 内符合 外符合

误 传感 （STD) (RMS)

差参数 （单器误差

違

bax (mGal) +50000 137 396

bay (mGal) - 50000 146 312

baz (mGal) +50000 112 240

5sfax (ppm) +5000 201 289 6sfay (ppm) - 5000 234 336

Ssfaz (ppm) +5000 196 397

bgx (deg/h) +1000 6 7

bgy (deg/h) - 1000 8 10

bgz (deg/h) +1000 6 8

5sfgx (ppm) +5000 144 246

5sfgy (ppm) - 5000 109 111

6sfgz (ppm) +5000 168 、、、、、、i、!、Z.

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

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

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

、、、、、、、 表 3

、、、、、

待估传感器加入的 内符合 外符合

误 传感 （STD ) (RMS )

差参数 （单器误差

位） 值

bax (mGal) +50000 426 885

bay (mGal) - 50000 274 523

baz (mGal) +50000 273 519

Ssfax (ppm) +5000 523 555

6sfay (ppm) - 5000 406 429

Ssfaz (ppm) +5000 298 564

bgx (deg/h) +1000 14 15

bgy (deg/h) - 1000 17 23

bgz (deg/h) +1000 24 35

5sfgx (ppm) +5000 205 352

6sfgy (ppm) - 5000 59 59

Ssfgz (ppm) +5000 226 348

( 3 ) 使用本发明随意操作 MTi-G进行了 30次标定。 以作为非专业用户操作本发明的应 用实例。 表 4是模式 2 (随意操作） 下标定 MTi-G统计结果表。 第 1、 3、 4列分别表示待估参数、 其内符合和外符合精度。 第 2列这里表示待估误差的初值。

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

表 4

、、、、、、、、、、、、、、、、、、、 、、、、、、、、、、、、、、、、、、、、 、、、、、、、、、、、、、、、、、、、、 、、、、、、、、、、、、、、、、、、、、 、、、、、、、、、、、、、、、、、、、、 、、、、、、、、、、、、■

待估传感器待估传感 内符合 外符合

误 器误 （STD) (RMS)

差参数 （单差参数初

位） 值

bax (mGal) +50000 473 1364

bay (mGal) - 50000 548 759

baz (mGal) +50000 598 773

Ssfax (ppm) +5000 604 748

5sfay (ppm) - 5000 855 1040

Ssfaz (ppm) +5000 768 772

bgx (deg/h) +1000 119 140

bgy (deg/h) - 1000 52 54

bgz (deg/h) +1000 82 140

5sfgx (ppm) +5000 834 845

5sfgy (ppm) - 5000 281 447

Ssfgz (ppm) +5000 、 1122 总结

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

表 5为两种操作模式下的标定结果表。第 1列为待估传感器误差参数； 第 2列为在模式 1 (有指导的操作） 下实验达到的精度； 第 3列为在模式 2 (随意操作） 下实验达到的精度。 表 5表明， 本发明可以精确标定中低精度 IMU。 由于本发明具有无硬件成本、 高效率、 简单易行的特点， 因此特别适合于对中低精度 IMU的现场快速标定， 有效解决 MEMS IMU 参数的环境敏感性 （尤其是温度敏感性） 问题， 促进 MEMS惯性器件的推广应用。 传感器误差 有指导的 随意操作

操作 （模式 2)

(模式 1 ) 下

T 标定精度 加速度计零偏 400 mGal 900 mGal

加速度计比例

因子 400 ppm 600 ppm 陀螺零偏 i _{0 de} g/h 35 deg/h 陀螺比例因子 400 pp _m 400 ppm

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