姿态估计是载体姿态控制的关键问题，一般是通过载体捷联惯导系统(strap-down inertial navigation system,SINS)姿态模型和辅助观测信息利用高精度估计算法获得载体姿态估计结果，载体姿态估计精度会直接影响系统姿态、速度及位置量的计算精度。目前SINS姿态实时解算方法主要有旋转矢量法、欧拉法、四元数法以及修正罗德里格参数法等^[1]，其中四元数法应用最为广泛。但四元数必须进行归一化约束变换，该步骤限制了非线性滤波算法的应用。对此，Markley采用乘性扩展卡尔曼(MEKF)算法处理^[2]，但是MEKF算法高阶截断误差使其无法获得理想计算结果；Crassidis等提出了USQUE算法^{[3, 4]}来避免四元数归一化约束。近年来，载体姿态的四元数卡尔曼滤波(quaternion Kalman filtering,QKF)算法获得得到广泛应用^[5]，QKF算法采用四元数伪观测模型，观测噪声与四元数向量是乘性关系，递推更新计算中四元数向量被强制归一化获得单位四元数向量。

目前针对非线性系统应用的采样逼近卡尔曼算法获得快速发展，主要有无迹卡尔曼滤波(unscented Kalman filter,UKF)算法^[6]、中心差分滤波(central divided Kalman filter,CDKF)算法^[7]以及类似的二阶插值滤波(SDDF)算法^[8]和Gauss-Hermite滤波(GHKF)算法^[9]等。GHKF算法复杂，计算负担重；UKF算法在高维场合应用中易于发散；CDKF和SDDF算法数值计算稳定性较差。设计高精度稳定数值计算的最优滤波算法成为载体姿态估计的关键问题,而基于球面径向容积积分逼近卡尔曼滤波算法的计算精度可以达到三阶^{[10, 11]}以上，计算稳定性明显优于UKF与CDKF算法。

本文基于SINS姿态估计高精度和数值计算稳定性要求提出了一种SINS四元数姿态估计的球面径向容积卡尔曼(SRC-KF)模型算法，设计了新型四元数观测方程。姿态估计方程和观测方程具有相同的噪声项表达式。利用SINS/CCD姿态仿真平台开展模型算法验证研究。仿真结果表明了SINS的SRC-KF对准模型算法的有效性及计算优势。

在高斯域内考虑函数F(X)数值积分：

式中,n维状态向量X∈Rⁿ，利用极坐标系中的方向向量Y和球径r表示X为X=rY，其中Y^TY=1，且有X^TX=r²，r∈[0,∞)，向量Y定义了单位超球面U_n，则有：

定义球面积分S(r)=∫_UnF(rY)dσ(Y)，重新整理式(1)为：

按照Gauss-Hermite数值积分，取m_l个高斯求积点获得径向积分逼近计算：

同样，取m_s个求积点获得球面积分逼近：

从而获得球面径向容积积分逼近计算：

若取加权指数函数ω₁(X)和概率密度分布加权函数ω₂(X)有：ω₁(X)=exp[－X^TX],ω₂(X)=N(X;η,Σ),且，那么，有：

取m_l=1，m_s=2n，可获得三阶球面径向数值逼近计算：

其中,，则2n个求积点为：

式中，e_i表示第i列单位向量，其第i个元素为1，有容积逼近求积式：

式中，ω_i为权值系数，ω_i=1/2n。

考虑非线性系统离散化模型方程：

式中,X_k∈R_n和Z_k∈R_m表示系统状态向量和量测向量；W_k和V_k是相互独立的过程噪声和观测噪声，具有W_k~N(0,Q_k)，V_k~N(0,R_k)分布。若观测向量序列Z_k={Z₁,Z₂,…,Z_k}，系统状态向量和观测向量后验密度函数服从高斯分布。

若k时刻系统状态向量估计为X_k，估计方差矩阵为P_k，则对其实施乔里斯基分解：

利用式(3)的求积点构造状态向量和过程噪声的采样点及其权值为：

其中，i=1,2,…,2n。

那么{ζ_X,i,k;ζ_W,i,k}经由式(5)状态函数映射后获得的采样点更新值为：

则系统状态向量的预测均值为：

系统状态向量预测方差为：

对系统状态向量预测方差和观测噪声方差矩阵同时展开乔里斯基分解，获得乔里斯基因子P_k+1|k=S_k+1|kS_k+1|k^T，R_k+1=S_V,k+1|kS_V,k+1|k^T，则可以构造采样点为：

经由式(5)观测方程映射获得采样点更新值：

那么观测向量预测值为：

其预测方差矩阵为：

同时状态向量和观测向量交叉协方差为：

那么，由

可以获得系统状态向量k+1时刻的估计值及其方差。

四元数是表示载体姿态旋转的一类特殊四维矢量，定义四元数为Θ≡［ϑ₀,ϑ^T]^T，其中 =[ϑ₁,ϑ₂,ϑ₃]^T为向量分量，ϑ ₀为实数分量，其运算要求必须满足规范化限制，即有：

那么由四元数表示的姿态矩阵

式中，；I_3×3是单位矩阵；〈ϑ ×〉为斜对称矩阵^[7]。

根据四元数微分方程，结合式(6)进一步得到SINS系统姿态四元数微分方程：

式中,。

SINS系统姿态四元数微分方程中的角速率由载体坐标系中三轴速率陀螺测量得到，陀螺测量得到的角速率方程为：

式中,ω(t)为陀螺量测输出角速率向量；α(t)为陀螺随时间的漂移量；W_μ(t)和W_u(t)分别为陀螺测量噪声和陀螺漂移的互不相关的高斯随机白噪声，满足W_u(t)~(0,ρ_μ²)和W_u(t)~(0,ρ_u²)，从而SINS姿态四元数微分方程为：

利用飞行器携带的辅助观测设备如星敏感器等可以获得精确的参考观测向量R，若在SINS载体系中得到的观测向量为B，那么由QUEST观测模型^[12]，有：

式中,B_j是一个具有观测噪声的3维观测向量；观测噪声V_j是观测传感器的误差向量，具有V_j～(0,ρ_B²)分布高斯白噪声。根据文献[5]可以得到SINS的伪观测模型方程：

式中，。则由式(10)展开k次观测获得的观测方程：

式中，G（X_k,B_k）=[G₁^RΘ,G₂^RΘ,…,G_k^RΘ]_kT；B_k=[B₁,B₂,…,B_k]^T。伪观测方程中的第一项G_jΘ是四元数向量的线性函数，其矩阵秩^[6]为2。伪观测方程中噪声项－M(Θ)V_j/2是四元数相关项，与四元数方程噪声项相似，可以方便开展观测噪声方差计算。

若考虑观测噪声与函数向量X_k相关的观测方程改为Z_k=G_e(X_k)V_k，那么状态向量均值为^[13]：

定义Z_k外积计算式：

对其两边求期望值得到：

式中,G_e=[G_e1…G_enV，式(12)可以简化为：

其中,G^*=G_e[(P_Vk+V_kV_k^T)ⓧX_kX_k^T)]G_e^T，可简化为：

其中，ⓧ表示Kronecker乘积，且对于i=1,2,…,n_V,有G_eiX_k=G_e(X_k)e_i成立，从而可得到：

那么，向量Z的噪声方差为：

若噪声向量{V_k}是具有零均值高斯白噪声，则式(13)计算可以简化为：

若四元数由k-1时刻到k时刻更新操作产生误差四元数为：

其中,Θ_i,k－1是k-1时刻更新计算的四元数，Θ _i,k|k－1为四元数的一步预测值，那么δΘ_i,k－1向量部分δϑ _i,k－1可表示为：

考虑SRC-KF算法中四元数均值计算加权系数，可以把四元数加权求均值问题转化为代价函数的极值计算问题，考虑四元数代价函数

其中，

由于Θ_k|k－1^TΘ_k|k－1=‖Θ_k|k－1‖=1，设拉格朗日算子λ，利用拉格朗日乘法构造代价函数^[7]：

那么，可以通过计算函数最小值，得到：

代价函数J最小矩阵L的最小特征值的特征向量就是所求均值四元数。

本文采用SINS/CCD姿态估计平台系统开展系统模型算法研究，光纤陀螺仪测量载体旋转角速度，利用两个光轴垂直安装的星敏感器获得星光矢量观测量。仿真初始条件设置为载体初始位置为北纬45°,东经126°，初始速度设为0，载体初始姿态角φ_x=2°,φ_y=4°，φ_z=15°，陀螺常值漂移为0.04°/h，随机漂移噪声设为0.005°/h^1/2；星敏感器测量噪声为18″，系统仿真时间t=600 s。SRC-KF、CDKF、UKF算法同属于贝叶斯最优滤波算法，但由于采用不同的采样策略，为验证SINS姿态四元数SRC-KF模型算法的有效性及其数值计算比较的优势，本文针对系统模型开展3种算法对比仿真实验，从而获得SINS的图 1的纵摇姿态失准角估计误差δφ_x，图 2的横摇姿态失准角估计误差δφ_y数据和图 3的偏航失准角估计误差δφ_z数据以及表 1的估计误差数据。

图 1 纵摇角估计误差曲线 Fig. 1 Estimation Error Curves of Pitching Angle

图选项

图 2 横摇角估计误差数据曲线 Fig. 2 Estimation Error Curves of the Rolling Angle

图选项

图 3 偏航角估计误差数据曲线 Fig. 3 Estimation Error Curves of Yawing Angle

图选项

从仿真比较数据可以看出，SINS四元数姿态估计SRC-KF模型算法的计算精度明显优于CDKF和UKF算法计算精度，小角度的纵摇角和横摇角估计得到的图 1和图 2估计误差数据中SRC-KF算法估计精度在20″之内，而CDKF和UKF算法分别为25″和30″，相比于SRC-KF算法，CDKF和UKF算法的数值计算稳定性明显降低；同样在初始偏航失准角取值为15°情况下，SRC-KF算法计算精度优于其余两种算法，且SRC-KF算法数值收敛很快，稳定性也优于其余两种算法，这说明SRC-KF算法在姿态估计计算方面具有精度优势，原因在于SRC-KF算法采用球面径向容积法可以达到三阶计算精度，而CDKF算法的斯特林插值法和UKF算法的无迹变换法的计算精度仅能达到二阶。

另外，从表 1可以看出，SRC-KF算法的迭代时间小，CDKF算法收敛时间与其相近，UKF算法计算周期较长，原因在于SRC-KF和CDKF算法仅采用2n个采样点，而UKF算法需要2n+1个Sigma采样点，且SRC-KF和CDKF算法仅有一个采样点调整参数，而UKF算法需要3个参数调整采样点权值系数；同时系统两种噪声方差采用同样计算式展开计算，该措施也会影响到算法计算效率。

基于SINS姿态估计高精度稳定数值计算要求，利用球面径向积分数值逼近法设计计算精度达到三阶的SRC-KF算法，实现提高模型算法计算精度目的；利用四元数构建一类乘性噪声的SINS姿态四元数误差模型，系统方程中噪声与四元数状态向量具有乘性关系，利用噪声分离算法可用较小计算负担获得稳定计算性能。经由SINS/CCD姿态仿真平台验证，相比于CDKF和UKF算法，SINS SRC-KF姿态模型算法具有计算精度高且计算稳定特点，在飞行器载体SINS姿态估计应用中具有适用价值。