混合式惯导系统是一种新型惯导系统，其吸收了平台式和捷联式惯导系统的各自优点，并且将隔离载体角运动的物理平台、捷联姿态算法与旋转调制抑制误差效应这三者集于一体。该系统主要着眼于高速和高动态运载器对高精度惯导提出的新需求，不仅能大幅度提高导航定位精度，实现快速精确自对准，还可实现装机条件下的自标定以及明显降低购置和维护成本^[1]。
惯性系统误差标定补偿是提高系统精度的有效手段，针对混合式惯导系统三轴全姿态物理平台和系统装机自标定的特点，选择运用连续自标定方法进行混合式惯导系统的误差自标定。连续自标定是Jackson^[2]于1973年提出的一种动态标定方法。其基本原理如下：惯性平台在外力矩的作用下以角速度ω_c(称为加矩角速度)转动，在地球自转角速度、加矩角速度以及重力加速度的激励下，加速度计输出中包含有陀螺仪误差、加速度计误差、安装误差和平台对准误差等全部误差信息。以加速度计输出为观测量，以平台对准误差方程为动力学模型，采用最优滤波算法估计平台误差系数和对准误差，并用估计结果对平台模型进行补偿与调整^[3]。相比于传统的多位置自标定，连续自标定中惯性平台转动的每一个采样点都相当于多位置自标定中的一个位置，因此连续自标定方法能够以比多位置自标定更高的精度同时标定出惯性平台的全部误差系数。
传统的连续自标定针对平台式惯导系统，主要有基于失准角和基于框架角的2种自标定模型。文献[4]系统地介绍了平台连续自标定的失准角模型、可观测性分析、参数辨识等内容。文献[5-6]针对大失准角情况下传统的失准角误差模型会产生较大误差的问题，建立了框架角误差模型。文献[7-8]分析了平台连续自标定中各误差系数的可观测性，并针对安装误差可观测性低这一问题，分别建立了以加速度计输入轴和陀螺仪输入轴为基准的平台坐标系，减少了安装误差项，使所有的安装误差变得可观，提高了标定精度。
考虑到混合式惯导系统采用捷联姿态算法，而捷联姿态算法中使用姿态四元数来描述物体转动，四元数表示能够避免传统的欧拉角表示中大量的三角运算带来的舍入误差，从而提高计算速度和标定精度^[9]。以四元数为基础的四元数卡尔曼滤波(Quaternion Kalman Filter，QKF)在惯性导航系统的姿态估计^[10]和自对准^[11]问题都已有一定应用。因此本文在混合式惯导系统框架角约束方程的基础上，利用姿态四元数代替欧拉角描述混合式惯导系统中三轴物理平台的转动，建立一种混合式惯导系统姿态四元数连续自标定模型，并且针对这一模型的特点，提出了一种基于奇异值分解的四元数无迹卡尔曼滤波(Unscented Kalman Filter based on Singular Value Decomposition Quaternion，SVD-QUKF)来标定混合式惯导系统误差系数。通过仿真和试验比较了基于姿态四元数模型的SVD-QUKF算法和基于框架角模型的UKF算法的标定结果，证明本文提出的自标定模型和标定算法在计算速度和标定精度上较传统方法都有一定优势。
1 惯性平台姿态四元数与框架角之间的转换关系 首先给出混合式惯导系统中的坐标系定义，混合式惯导系统采用隔离载体角运动的三轴物理平台，物理平台通过3个框架轴固联在基座上，其中惯性平台固联在台体轴上，台体轴固联在内环轴，内环轴固联在外环轴，外环轴固联在基座上，由此构成具有三自由度的稳定平台。三轴物理平台由3个陀螺仪与3个加速度计组成，如图 1所示。定义如下坐标系：

图 1 三轴物理平台组成 Fig. 1 Three-axis physical platform geometry

图选项

1) 框架坐标系(K)：X_K、Y_K、Z_K三轴分别指向混合式惯导系统的外框架轴、内框架轴、台体轴，三者构成右手正交坐标系。
2) 平台坐标系(p)：以加速度计敏感轴为基准定义，取三轴物理平台几何中心为原点，X_p轴与X加速度计敏感轴平行，Y_p轴平行于X和Y加速度计敏感轴所确定的平面，并与X_p轴垂直，Z_p轴与X_p轴以及Y_p轴构成右手正交坐标系。
根据惯性平台台体和3个框架轴之间的固联关系可知，无论3个框架轴如何转动，都可以通过绕Z_K—Y_K—X_K轴的顺序使平台台体转回原位置，对于p系来说，就是绕Y_p—X_p—Z_p轴的顺序转回原位置。若假设惯性平台绕X_K、Y_K和Z_K轴转动角度分别为θ_x、θ_y和θ_z，则对于p系来说，就是依次绕Z_p、X_p和Y_p轴分别转动θ_x、θ_y和θ_z。根据四元数定义，描述惯性平台转动的姿态四元数为

(1)

同时，姿态四元数到框架角的转换关系为

(2)

2 混合式惯导系统连续自标定模型 2.1 混合式惯导系统框架角约束方程 混合式惯导系统在地面标定时处于稳定和稳定加旋转2种工作模式^[1]，此时混合式惯导系统的工作原理与平台式惯导系统相同，因此可以采用平台式惯导系统的框架角约束方程。利用框架角约束方程建立自标定模型能够避免失准角模型中的小角度假设，从而能够适用于大失准角的情况。假设惯性平台绕X_K、Y_K和Z_K框架轴转动的框架角分别为θ_x、θ_y和θ_z，则其框架角约束方程为^[6]

(3)

式中：T为框架角约束矩阵；ω_Ip^p为p系相对于惯性坐标系(I)的旋转角速度。两者表达式分别为

(4)

其中：，Δ_oi和Δ_si(i=x, y, z)为陀螺仪安装误差系数；ω_Inⁿ=[0 ??ω_IEcos L ??ω_IEsin L]^T，ω_IE为地球自转角速度值，L为当地纬度；ω_g^p为平台漂移，由陀螺仪误差引起的，混合式惯导系统所用的光纤陀螺仪具有很好的线性度，二次项误差基本可以忽略不计^[12]，只需要考虑其零偏误差和随机误差，所以有

(5)

式中：k_g0i(i=x, y, z)为陀螺仪零次项误差系数；ε_gi为陀螺仪量测噪声。
2.2 加速度计输出方程 加速度计的输出与加速度计三轴敏感的比力有关，加速度计三轴上的比力为

(6)

式中：C_p^ai为加速度计安装误差矩阵，根据p系的定义，有C_p^gx=I_3×3,，η_pz、η_oy和η_oz为加速度计安装误差系数；g_n为地球重力加速度在地理坐标系(n)的投影，g_n=[0 ??－g₀??0]^T，g₀为地球重力加速度值；C_n^p为地理坐标系(n)到p系的转换矩阵，有

(7)

则加速度计输出方程为^[13]

(8)

式中：k_aji(j=0, 1;i=x, y, z)为加速度计误差系数；ε_a为加速度计量测噪声；C_mn(m, n=1, 2, 3)为矩阵C_n^p的元素。
以框架角约束方程为系统方程，以加速度计输出和框架角传感器输出为系统量测，结合式(3)~式(5)和式(8)，则可以得到基于框架角的连续自标定模型(以下简称为框架角模型)为

(9)

式中：v_θ=[v_x??v_y??v_z]^T为框架角传感器的量测噪声；w为系统噪声; v为量测噪声。从框架角模型的表达式可以看出，当cos θ_y=0时，框架角模型将出现奇异，这就是利用欧拉角描述框架转动时可能出现的“框架自锁”现象，而采用姿态四元数描述框架转动则可以避免这一现象。
2.3 四元数连续自标定模型 对式(1)进行求导，整理可得四元数微分与框架角微分之间的关系为

(10)

式中：

将式(9)中的代入式(10)，并运用式(2)的框架角和四元数之间的转换关系可得

(11)

式中：T_q为矩阵T的四元数表示。
考虑框架角传感器的量测噪声，将θ_i+v_i(i=x, y, z)代入式(1)可得带有噪声干扰的四元数为

(12)

同理可得

(13)

因此，包含噪声的姿态四元数量测方程为

(14)

式中：

C_nq^p为矩阵C_n^p的四元数表示：

同时用C_nq^p代替式(8)中的C_n^p，则式(9)中的观测方程可以写为

(15)

结合式(11)和式(15)，可得混合式惯导系统的四元数连续自标定模型(以下简称为四元数模型)为

(16)

混合式惯导系统采用捷联姿态算法，而捷联姿态算法中使用姿态四元数来描述物体转动，四元数模型正好满足这一要求。同时，四元数表示能够避免传统的欧拉角表示中大量的三角运算带来的舍入误差，提高计算速度和标定精度，并且四元数表示不存在奇异点，因此能够避免惯性平台连续转动过程中可能出现的“框架自锁”现象。
3 基于奇异值分解的四元数无迹卡尔曼滤波 可以看出，式(16)的四元数模型是一个强非线性的系统方程，因此选择UKF对其进行状态估计和参数辨识。但是有2个问题需要解决：①在UKF滤波过程中，由于量测噪声、截断误差和状态模型扰动等因素的影响，状态协方差矩阵P容易变得非正定，然而在UKF中利用Cholesky分解获取σ样本点时，要求P必须正定；②在将欧拉角表示转换为四元数表示之后，原来3维的系统噪声和量测噪声都变成了4维噪声，系统噪声方差阵Q和量测噪声方差阵R的形式也随之发生变化，需要重新进行推导。
3.1 奇异值分解 针对第一个问题，本文选择奇异值分解代替Cholesky分解来获取σ样本点。因为奇异值分解不要求被分解矩阵为正定，同时，如果被分解矩阵是正定的，那么奇异值分解和Cholesky分解的结果是相同的^[14]。奇异值分解的结果为

(17)

式中：P∈R^m×n，m≥n；Λ∈R^m×n；U∈R^m×m；V∈R^n×n；S=diag(s₁, s₂, …, s_r)，s₁≥s₂≥…≥s_r≥0为矩阵P的r个奇异值，r=rank(P)。
3.2 系统噪声方差阵和量测噪声方差阵的推导 根据系统噪声方差阵Q和量测噪声方差阵R的定义，有Q=E[ww^T]和R=E[vv^T]。根据2.3节中的推导可知，在将欧拉角表示转换为四元数表示之后，系统噪声和量测噪声变为w_q= 和，则四元数表示的系统噪声方差阵Q_q和量测噪声方差阵R_q的表达式有如下推导：

(18)

式中：Q_q, R_q∈R^4×4。
3.3 SVD-QUKF算法步骤 在将欧拉角表示转换为四元数表示之后，系统噪声w_q和量测噪声v_q由加性噪声变为了非加性噪声，此时需要将其扩展为系统状态量^[15]。此外，待标定的惯导系统误差系数也扩展为系统状态量，则扩展后的系统状态量为

(19)

式中：q=[q₀??q₁??q₂??q₃]^T；k_g=[k_g0x ??k_g0y??k_g0z]^T；η=[η_pz??η_oy??η_oz]^T；Δ=[Δ_sx ??Δ_sy ??Δ_sz??Δ_ox ??Δ_oy ??Δ_oz]^T；k_a=[k_a0x ??k_a0y ??k_a0z ??k_a1x ??k_a1y??k_a1z]^T；。
状态协方差矩阵P为

(20)

对式(16)进行离散化，则可得到SVD-QUKF的滤波方程为

(21)

式中：ΔT=0.2 s为离散时间间隔。
则SVD-QUKF的算法步骤如下：
1) 滤波初值

(22)

(23)

2) 计算σ样本点

(24)

式中：U和S为式(17)中状态协方差矩阵P的SVD分解结果；。
3) 时间更新

(25)

式中: , m=2；, i=1, 2, …, 2L。
4) 量测更新

(26)

5) 归一化
在每一次迭代之后，都需要对姿态四元数进行归一化以保证其为标准四元数^[16]，即

(27)

4 仿真和试验 4.1 仿真分析 为验证本文提出的四元数模型的适用性，运用SVD-QUKF算法对式(16)的四元数模型进行了参数辨识，运用UKF算法对式(9)的框架角模型进行了参数辨识，对二者结果进行对比。混合式惯导系统三轴物理平台的转动路径如下^[17]：
1) 起始角度为θ_x=0°, θ_z=0°，以角速度1(°)/s绕Z_K轴转动π。
2) 起始角度为θ_x=0°, θ_z=π，以角速度1(°)/s绕X_K轴转动π/2。
3) 起始角度为θ_x=π/2, θ_z=π，以角速度1(°)/s绕Z_K轴转动π。
4) 起始角度为θ_x=π/2, θ_z=0°，以角速度1(°)/s绕X_K轴转动π/2。
定义系统状态量的估计误差和相对估计误差以表示标定精度：

(28)

式中：Δx为估计误差；e为相对估计误差；和x分别为系统状态量的估计值和真值。
首先，为了比较2种系统模型对框架角的估计精度，将框架角模型估计得到的框架角转换为四元数与四元数模型估计得到的四元数进行了对比，估计误差如图 2所示。

图 2 四元数模型和框架角模型的框架角估计误差曲线 Fig. 2 Estimate error curves of gimbal angle in quaternion model and gimbal angle model

图选项

从图 2可以看出，四元数模型对于框架角的估计误差小于框架角模型，所以对于框架角的估计精度，本文提出的四元数模型要优于传统框架角模型。
其次，为比较2种模型对于混合式惯导系统误差系数的标定精度，表 1给出了所有误差系数在2种系统模型下的相对估计误差和算法计算时间，图 3给出了部分误差系数的相对估计误差曲线。
表 1 误差系数相对估计误差和算法计算时间 Table 1 Computing time and relative estimate errors of error coefficients

误差系数	相对估计误差/%
	四元数模型	框架角模型
kg0x	-0.71	1.71
kg0y	0.69	2.74
kg0z	-0.62	-4.69
Δsx	-0.10	1.82
Δsy	-0.35	-1.33
Δsz	0.19	-1.26
Δox	-0.24	1.47
Δoy	-0.36	1.26
Δoz	0.57	-1.91
ka0x	-0.22	1.39
ka0y	-0.93	3.49
ka0z	-0.25	4.03
ka1x	0.031	-1.47
ka1y	-0.29	0.49
ka1z	-0.11	0.64
ηpz	0.005 5	0.46
ηoy	-0.001 7	0.20
ηoz	0.001 3	-0.023
计算时间/s	19	35

表选项

图 3 ka0i(i=x, y, z)和ηji(j=p, o; i=y, z)的相对估计误差曲线 Fig. 3 Relative estimate error curves of ka0i(i=x, y, z) and ηji(j=p, o; i=y, z)

图选项

从表 1可以看出，使用SVD-QUKF的四元数模型的计算时间远少于使用UKF算法的框架角模型，这是因为SVD-QUKF算法中采用的四元数表示避免了欧拉角表示中大量的三角运算，从而减少了计算时间。在实际的惯导系统自标定中，是采用实时滤波的方法进行误差系数辨识，根据仿真时间可得，原始算法进行一个卡尔曼滤波周期需要13 ms，这已经比较接近于一般惯导系统20 ms的计算周期，不利于进行实时滤波，而本文的算法进行一个卡尔曼滤波周期所需时间为7 ms，能够保证实时性要求。同时可以看出，在相同的转动路径和仿真条件下，四元数模型对于混合式惯导系统误差系数的估计精度也要优于传统的框架角模型，特别是对于陀螺仪和加速度计的安装误差系数的标定精度提高较大。仿真结果表明，相比于传统的框架角模型，本文提出的四元数模型能够有效提高混合式惯导系统的误差系数标定精度。
4.2 试验验证 在某型平台式惯导系统上进行了试验验证。
由于硬件限制，试验中只各绕Z_K轴和X_K轴进行了一次转动：
1) 起始角度为θ_x=0°, θ_z=0°，以角速度1(°)/s绕Z_K轴转动π。
2) 起始角度为θ_x=0°, θ_z=π，以角速度1(°)/s绕X_K轴转动π。
采集加速度计和框架角传感器的输出，利用传统的框架角模型和本文提出的四元数模型进行了误差系数标定，对于框架角的估计误差如图 4所示，其均方根误差(RMSE)如表 2所示。从图 4和表 2可以看出，四元数模型对于框架角的估计精度要高于框架角模型，从而验证了其适用性。同时可以注意到，在图 4中，在绕Z_K轴转动完毕和刚开始绕X_K轴转动时，估计误差会发生较大的波动，这是因为当加速度计接近水平位置时，加速度计输出中噪声比很大，从而会造成滤波结果偏离真值，甚至有可能导致滤波结果发散，这种情况是需要避免的。具体做法是：在设置自标定程序时，根据框架角传感器的输出判断加速度计是否接近水平位置，当加速度计接近水平位置时，断开加速度计的输出采集回路，同时停止滤波，待加速度计离开水平位置一定角度后，再重新采集加速度计输出，继续进行滤波。

图 4 试验中四元数模型和框架角模型的框架角估计误差曲线 Fig. 4 Gimbal angle estimate errors of quaternion model and gimbal angle model in experiment

图选项

表 2 四元数模型和框架角模型的框架角均方根误差 Table 2 RMSE of gimbal angle of quaternion model and gimbal angle model

模型	RMSE
	q0	q1	q2	q3
四元数模型	0.059	0.047	0.083	0.062
框架角模型	0.220	0.083	0.087	0.131

表选项






5 结论 1) 针对混合式惯导系统全姿态物理平台、捷联姿态算法和系统装机自标定的特点，本文在混合式惯导系统框架角约束方程的基础上，利用姿态四元数代替欧拉角描述混合式惯导系统中三轴物理平台的转动，建立一种姿态四元数连续自标定模型。
2) 针对四元数连续自标定模型的特点，对传统的UKF算法进行改进，利用SVD代替传统UKF算法中的Cholesky分解来获取σ样本点，避免了状态协方差矩阵P不正定时Cholesky分解结果奇异的现象；同时推导了四元数表示下的系统噪声方差阵Q_q和量测噪声方差阵R_q，将3维噪声转换为了4维噪声，提出了一种SVD-QUKF算法用于标定混合式惯导系统误差系数。
3) 仿真和试验结果表明，相比于基于传统UKF算法的框架角连续自标定模型，本文提出的四元数连续自标定模型和SVD-QUKF算法能够以低于1%的相对误差标定出混合式惯导系统所有的误差系数，在标定精度和计算速度上都有一定优势，具有实用价值。

参考文献

[1]	冯培德. 论混合式惯导系统[J]. 中国惯性技术学报, 2016, 24(3): 281-284. FENG P D. On hybrid inertial navigation systems[J]. Journal of Chinese Inertial Technology, 2016, 24(3): 281-284. (in Chinese)
[2]	JACKSON D J.Continuous calibration and alignment techniques for an all-attitude inertial platform: AIAA-73-865[R].Reston, VA: AIAA, 1973.
[3]	CAO Y, CAI H, ZHANG S F, et al. A new continuous self-calibration scheme for a gimbaled inertial measurement unit[J]. Measurement Science and Technology, 2012, 23(1): 385-394.
[4]	杨华波.惯性测量系统误差标定及分离技术研究[D].长沙: 国防科学技术大学, 2008. YANG H B.Research on error calibration and separation for inertial measurement systems[D].Changsha: National University of Defense Technology, 2008(in Chinese). http://xuewen.cnki.net/CDFD-2008098743.nh.html
[5]	曹渊, 张士峰, 杨华波, 等. 惯导平台误差快速自标定方法研究[J]. 宇航学报, 2011, 32(6): 1281-1287. CAO Y, ZHANG S F, YANG H B, et al. Research on rapid self-calibration method for inertial platform[J]. Journal of Astronautics, 2011, 32(6): 1281-1287. DOI:10.3873/j.issn.1000-1328.2011.06.012 (in Chinese)
[6]	丁智坚, 蔡洪, 杨华波, 等. 浮球式惯性平台连续翻滚自标定自对准方法[J]. 国防科技大学学报, 2015, 37(3): 148-154. DING Z J, CAI H, YANG H B, et al. Continuous self-calibration and self-alignment method for floated inertial platform[J]. Journal of National University of Defense Technology, 2015, 37(3): 148-154. (in Chinese)
[7]	许永飞, 张士峰. 惯导平台自标定中安装误差可观性分析[J]. 中国惯性技术学报, 2015, 23(5): 575-579. XU Y F, ZHANG S F. Observability analysis of IMU's misalignment angles in self-calibration for inertial platform[J]. Journal of Chinese Inertial Technology, 2015, 23(5): 575-579. (in Chinese)
[8]	连丁磊, 夏刚, 王汀, 等. 基于陀螺仪敏感轴系的平台连续翻滚自标定算法[J]. 导弹与航天运载技术, 2013(2): 24-29. LIAN D L, XIA G, WANG T, et al. Self-calibration algorithm of inertial navigation platform continuous tumble based on gyro sensitive shaft system[J]. Missiles and Space Vehicles, 2013(2): 24-29. (in Chinese)
[9]	CHOUKROUN D, BAR-ITZHACK I Y, OSHMAN Y. Novel quaternion Kalman filter[J]. IEEE Transactions on Aerospace and Electronic Systems, 2006, 42(1): 174-190. DOI:10.1109/TAES.2006.1603413
[10]	XUE L, YUAN W Z, CHANG H L, et al. Application of quaternion-based extended Kalman filter for MAV attitude estimation using MEMS sensors[J]. Nanotechnology and Precision Engineering, 2009, 7(2): 163-167.
[11]	XU X S, ZHOU F, ZHANG T, et al. Initial alignment algorithm for SINS based on quaternion adaptive Kalman filter[J]. Journal of Chinese Inertial Technology, 2016, 24(4): 454-459.
[12]	XU C X, WU G, SHI C, et al. Temperature compensation model research on fiber optic gyroscope[J]. Optical Technique, 2014, 40(6): 492-498. DOI:10.3788/GXJS
[13]	王琪, 汪立新, 秦伟伟, 等. 基于双回路扩展卡尔曼滤波的惯性平台连续自标定[J]. 宇航学报, 2017, 38(6): 621-629. WANG Q, WANG L X, QIN W W, et al. Continuous self-calibration of inertial platform based on dual extend Kalman filter[J]. Journal of Astronautics, 2017, 38(6): 621-629. (in Chinese)
[14]	谭兴龙, 王坚, 李增科. 基于SVD的改进抗差UKF算法及在组合导航中的应用[J]. 控制与决策, 2014, 29(10): 1744-1750. TAN X L, WANG J, LI Z K. SVD aided improved robust UKF algorithm and its application for integration navigation[J]. Control and Decision, 2014, 29(10): 1744-1750. (in Chinese)
[15]	LIU Y, YU A X, ZHU J B, et al. Unscented Kalman filtering in the additive noise case[J]. Science China:Technology Sciences, 2010, 53(4): 929-941. DOI:10.1007/s11431-010-0119-z
[16]	AINSCOUGH T, ZANETTI R, CHRISTIAN J, et al. Q-method extend Kalman filter[J]. Journal of Guidance, Control, and Dynamics, 2015, 38(4): 752-760. DOI:10.2514/1.G000118
[17]	杨华波, 蔡洪, 张士峰, 等. 高精度惯性平台连续自标定自对准技术[J]. 宇航学报, 2006, 27(4): 600-604. YANG H B, CAI H, ZHANG S F, et al. Continuous calibration and alignment techniques for a high precision inertial platform[J]. Journal of Astronautics, 2006, 27(4): 600-604. DOI:10.3321/j.issn:1000-1328.2006.04.006 (in Chinese)