基于对偶四元数的惯性/卫星/天文组合导航系统改进联邦滤波方法

闵艳玲， 熊智， 邢丽， 刘建业， 殷德全

(南京航空航天大学 自动化学院, 江苏 南京 211100)

0 引言

捷联惯性导航系统不需要接收任何外界信息，仅依靠陀螺和加速度计的测量输出即可解算出载体的速度、位置、姿态等运动参数，具有抗干扰能力强、隐蔽性好、导航信息完整和数据更新率高等优点，但惯性解算属于积分解算，因此其导航误差随时间累积，难以满足运动载体长时间高精度的导航需求[1]。

卫星导航系统定位精度高，导航误差不随时间累积，但其信号易受干扰，且在有遮挡的环境中容易发生信号丢失或导航误差变大[2]。天文导航系统利用天文敏感器测得天体的方位信息，从而得到运动载体的姿态参数，具有自主性强、姿态测量精度高、误差不随时间累积的优点，但也易受到环境干扰[3]。根据惯性导航系统、卫星导航系统、天文导航系统的特点进行有效组合，构成惯性/卫星/天文组合导航系统，可对惯性导航系统中的各误差项进行补偿，从而实现高精度导航。目前，惯性/卫星/天文组合导航系统已成为中远程弹道导航、高空长航时飞行器、空天飞行器中最有效的高性能导航手段[3]。

文献[4]在姿态组合过程中，将天文导航输出的机体相对于惯性坐标系的姿态转化为相对于地理坐标系的姿态，转化时耦合了惯性导航系统的位置误差，影响状态量估计精度，未能最大限度地使用天文导航所提供的高精度姿态信息；文献[5]针对上述转化过程中的耦合误差问题，首先在惯性系中利用天文导航输出对陀螺误差进行估计和补偿，再在地理系下进行位置组合，其中姿态组合滤波器采用了开环结构，长时间运行存在发散风险；文献[2]和文献[4]采用联邦滤波器进行惯性/卫星/天文组合导航系统的信息融合，主滤波器与各子滤波器状态保持一致，因此惯性/天文子滤波器中包含了不可观测状态量；文献[6]提出了基于对偶四元数的惯性/天文组合导航算法，但同样忽略了状态的可观测性问题。

针对上述问题，本文提出了基于对偶四元数的惯性/卫星/天文组合、导航系统改进联邦滤波方法，利用基于奇异值分解的可观测性分析方法建立子滤波器，有效地减少滤波计算量。基于对偶四元数的降维惯性/天文组合方法，直接利用天文信息进行惯性系姿态组合，再将惯性系姿态转换为地理系姿态。相较于传统惯性/天文采用地理系姿态组合的方法，对偶四元数惯性/天文组合方法受惯性导航位置误差影响较小，具有更高的组合精度。因此，基于对偶四元数的惯性/卫星/天文改进联邦滤波方法可以有效地提高惯性/卫星/天文组合导航系统性能，并为其工程化应用提供参考。

1 对偶四元数惯性导航误差模型

1.1 坐标系定义

e表示地球坐标系，原点在地心，一个轴与地轴重合，其他两个轴在赤道面。

i表示惯性坐标系，0时刻与地球坐标系重合。

b表示载体坐标系，即捷联惯性器件的坐标系，原点在加速度计三元组的质心，3个坐标轴分别与加速度计敏感轴重合，构成右手正交系。

t表示推力速度坐标系，与载体坐标系平行，从地心到坐标原点的向量等于推力速度。

g表示引力速度坐标系，与地球坐标系平行，从地心到坐标原点的向量等于引力速度。

u表示位置坐标系，与地球坐标系平行，从地心到坐标原点的向量等于载体的位置向量。

1.2 误差模型

对偶四元数定义为

(1)

式中：q和q′分别为对偶四元数的标量和对偶部分，都是普通四元数；ε2=0，但ε≠0.

对偶四元数的加性误差定义为

(2)

式中：qo为对偶四元数计算值或量测值；q为真实值。

陀螺误差δωbib和加速度计误差δfb均建模为常值误差与高斯白噪声随机误差的线性组合：

(3)

式中：δωbibc为陀螺常值漂移；δωbibs为陀螺随机白噪声；δfbc为加速度计常值误差；δfbs为加速度计随机白噪声。

文献[6-7]对基于加性对偶四元数的捷联惯性导航算法误差方程进行了详细推导，此处不再赘述。

推力对偶四元数qit=qit+εq′it刻画了推力速度坐标系t相对于惯性坐标系i的一般性刚体运动，其误差方程为

(4)

式中：fb和ωbib分别是加速度计和陀螺输出值；∘为四元数乘子。

引力对偶四元数qig=qig+εq′ig刻画了引力速度坐标系g相对于惯性坐标系i的一般性刚体运动，其误差方程为

(5)

式中：ωeie为地球自转角速度。

位置对偶四元数qiu=qiu+εq′iu刻画了位置坐标系u相对于惯性坐标系i的一般性刚体运动，其误差方程为

(6)

(7)

式中：q*iu、q*it、q*ig分别为qiu、qit、qig的共轭四元数。

1.3 状态方程

为了方便进行四元数计算，δωbibc、δfbc均表示为标量部分为0的四元数，将其矢量部分分别表示为δΩbibc、δFbc并扩充到状态量中，可将文献[6]中的24维状态量简化到22维；δωbibs、δfbs同样也可将为0的标量部分剔除，仅将其矢量部分δΩbibs、δFbs作为噪声向量，则噪声向量由8维简化到6维，状态方程也可相应地进行简化。

由(3)式～(6)式，可建立如下状态方程：

(8)

式中：X∈R22×1为状态向量，

(9)

F∈R22×22为状态转移矩阵；G∈R22×6为噪声驱动阵；w∈R6×1为噪声向量，

(10)

当四元数写成4维向量q=[s,vT]T时，s为其标量部分，向量部分v=[v1v2v3]，四元数乘法[7]可以表示为

(11)

两个矩阵[q]+和[q]-定义为

(12)

式中：I3为3维单位矩阵；[v×]为向量v生成的反对称矩阵，

(13)

为简化矩阵表示，设存在矩阵T，M(T)表示去除T矩阵第1列后的矩阵。

因此，(8)式中的F阵可表示为

(14)

(8)式中的G阵可表示为

(15)

1.4 量测方程

1.4.1 惯性/卫星子导航系统

根据对偶四元数捷联惯性导航算法的特点，选择惯性坐标系i中的速度误差和地球坐标系e中的位置误差作为量测向量。对偶四元数算法解算得到的惯性系速度Vi及地球系位置Re为

(16)

(17)

由于惯性系在1.1节中定义为0时刻的地球系，即δqig(0)=δqiu(0)=0，且由文献[7]可知：

(18)

δVi=2(δq′it∘q*it-q′it∘q*it∘δqit∘q*it+
δq′ig∘q*ig)，

(20)

(21)

由于卫星导航系统所测得的速度和位置均为地球坐标系参数，因此需利用卫星的速度测量信息VeGNSS计算载体的惯性系速度ViGNSS. 由速度合成定理及坐标系旋转可得

(22)

式中：Cie表示地球系到惯性系的坐标旋转矩阵；Vie表示地球系与惯性系之间的相对速度，可通过地球自转角速度和卫星导航系统测得的载体位置简单计算得到。由于地球自转角速度较小，且卫星导航系统的位置测量误差也是小量，因此可以忽略Vie的误差。在此条件下，设δVeGNSS为卫星导航系统的地球系速度测量误差，则卫星导航系统的惯性系速度测量误差δViGNSS=CieδVeGNSS. 地球自转角速度的四元数表示为ωeie=[000ωie]，利用标量ωie计算可得Cie，

(23)

由于对偶四元数惯性导航算法解算得到的惯性系速度Vi及地球系位置Re均为标量部分为0的四元数，因此只取其矢量部分与卫星导航系统测量值的差值作为量测量。由(20)式、(21)式可得惯性、卫星子导航系统量测方程为

(24)

式中：Z1∈R6×1为量测向量；H1∈R6×22为系统量测矩阵；V1∈R6×1为系统量测噪声向量，

(25)

nV∈R3×1为卫星导航系统计算得到的惯性系速度量测噪声，nR∈R3×1为卫星导航系统的位置量测噪声。

设U(T)表示去除矩阵T的第1行，则系统量测矩阵H1可表示为

(26)

1.4.2 惯性/天文子导航系统

天文敏感器的输出信息为载体坐标系b相对于惯性系i的旋转四元数qcns，对偶四元数捷联惯性导航算法解算得到的qit为推力速度坐标系t相对于惯性坐标系i的旋转四元数。从坐标系的定义可知，推力坐标系t与载体坐标系b平行。因此不需要进行转换，即可将二者差值作为量测量。

惯性/天文子导航系统的量测方程为

(27)

式中：Z2∈R4×1为量测向量；V2∈R4×1为系统量测噪声向量；H2∈R4×22为系统量测矩阵，

(28)

2 基于可观测性分析的改进联邦滤波器设计

2.1 基于奇异值分解的可观测性分析方法

由1.3节可知，全状态量的维数为22，组合导航系统一般采用卡尔曼滤波器进行信息融合，其计算时间主要由系统状态维数n和量测维数m决定，每一个滤波周期的计算量与(n3+mn2)呈正比[8]。如果系统的维数较高，会增加滤波器计算量，导致导航系统的实时性变差。因此在保证导航精度的前提下，提高系统的实时性，具有重要的研究意义。

假设某动态系统进行分段线性处理后，各时间段内的系统状态转移矩阵为Φj(j=1,2,…)，量测矩阵为Hj. 对该动态系统，设在第j个时间段内的可观测性矩阵为Nj，则

(29)

设时间段个数为λ，则总时间内系统的可观测性矩阵N(l×n维)为

(30)

对其进行奇异值分解得

(31)

式中：U=[u1u2…ul]；V=[v1v2…vn]为正交矩阵；S=[Λr×r0

00]，Λr×r=diag(σ1σ2…σr)，r、σi(i=1,2,…,r)分别为N的秩和非零奇异值。

奇异值σi是与其对应的右奇异向量vi中取得最大绝对值的状态所对应的奇异值[9]。奇异值的个数即矩阵N的秩r，决定了可观测状态量的个数。

2.2 子滤波器状态变量可观测性分析

由于不同的机动情况对状态量的观测性会有所影响，因此分别选择了匀速机动、加速机动、横滚机动、俯仰机动、航向机动这5种常见的机动方式对各状态量的可观测性情况进行分析。每次仿真时间为5 s，时间间隔为0.02 s，各机动方式的仿真参数如表1所示。

表1 机动参数设置

根据上述仿真条件进行基于奇异值分解的可观测性分析，发现在5种机动情况下，惯性/卫星子导航系统中可观测性矩阵的秩均为22，但不同的机动情况下各状态量的奇异值相对大小不同。这表明惯性/卫星子导航系统对所有状态均具有可观测性，但不同机动条件下各状态量的可观测度不同。由于本文主要关注状态量是否是可观测的，因此惯性/卫星子滤波器不需进行降维。

惯性/天文子导航系统在5种机动情况下可观测性矩阵的秩均为7，可观测的7个状态量包括四元数δqit以及三维矢量δΩbibc，但同样在不同机动条件下各状态量的可观测度不同。由此可知，可对惯性/天文子滤波器进行降维。

2.3 降维惯性/天文子滤波器

2.3.1 状态方程

降维惯性/天文子导航系统的状态方程为

(32)

式中：X2∈R7×1为状态向量，

(33)

F2∈R7×7为状态转移矩阵，

(34)

G2∈R7×3为噪声驱动阵，

(35)

w2=δΩbibs为3维噪声向量。

2.3.2 量测方程

降维后，惯性/天文子导航系统的量测方程为

(36)

式中：量测向量Z2与量测噪声向量V2未发生变化，仅由于状态变量变为X2，导致系统量测矩阵由H2变为H3，H3∈R4×7，

(37)

2.3.3 计算量对比

滤波计算量与(n3+mn2)呈正比，表2为降维前后的计算量对比。

表2 惯性/天文子滤波器计算量对比

由表2可知，与文献[6]中滤波器的计算量相比，降维后的计算量仅为其4.28%，计算量得到大量简化。

2.4 改进联邦滤波器设计

本文联邦滤波器中的主滤波器采用22维全状态变量，惯性/卫星子导航系统中由于状态全部可观，因此与主滤波器保持一致，状态方程与量测方程如1.3节、1.4节所示。惯性/天文子导航系统仅使用部分状态变量，状态方程与量测方程如2.3节所示。本文采用了有重置的联邦滤波结构，利用全局估计值对对偶四元数惯导系统进行闭环反馈修正，结构图如图1所示。

主滤波器全局状态空间到惯性/卫星子滤波器局部状态空间的映射矩阵T1=I22，到惯性/天文子滤波器局部状态空间的映射矩阵T2为

(38)

全局融合估计值[10]可表示为

(39)

信息分配过程为

(40)

式中：βi=12；k表示当前时刻。

3 仿真验证

3.1 仿真条件设置

各导航传感器参数设置如表3所示，惯性导航解算周期为0.02 s，滤波周期为1 s，仿真时长为800 s，导航坐标系为“北天东”地理坐标系。

表3 传感器参数设置

3.2 基于对偶四元数的惯性/卫星/天文组合导航系统改进联邦滤波仿真验证

3.2.1 方法有效性验证

1)动态航迹设计

为了验证所设计的基于对偶四元数的惯性/卫星/天文改进联邦滤波方法性能，设计了一组姿态、速度变化的动态航迹，该航迹主要用于验证算法的有效性。

横滚角(°)、航向角(°)、俯仰角(°)和机头速度vb(m/s)随时间t的变化规律为：γ=ψ=θ=-0.000 18t2+0.18t，vb=-0.000 8t2+1.5t.

2)有效性仿真验证

根据3.1节中的仿真设置条件，对基于对偶四元数的惯性/卫星/天文改进联邦滤波进行了仿真分析，导航误差曲线如图2所示。由于当各子滤波器与主滤波器维数相同时，其全局滤波和集中式卡尔曼滤波等价[11]，因此还对基于对偶四元数的惯性/卫星/天文集中式卡尔曼滤波进行了导航仿真验证，其导航仿真误差曲线如图3所示。

观察图2及表4中改进联邦滤波方法的导航误差统计可知，改进后的联邦滤波方法可获得高精度的导航信息，为飞行器提供可靠的位置、速度、姿态数据。对比图2、图3以及表4中改进联邦滤波与集中式卡尔曼滤波导航误差的统计数据可以发现，改进联邦滤波的精度略低于集中式卡尔曼滤波，但是差别很小，误差位于同一数量级。这是由于改进联邦滤波中惯性/天文子滤波器维数与主滤波器不一致，因此损失了部分信息，属于次优滤波[11]。但是从表2中的计算量对比可知，改进联邦滤波方法的计算量得到大量的简化，鉴于改进后联邦滤波器的实时性优势，较小的精度损失是可以承受的。

传统的惯性/卫星/天文联邦滤波将地理系位置、地理系速度、地理系下定义的平台误差角、加速度计误差和陀螺误差作为状态量，且惯性/卫星与惯性/天文子滤波器状态方程保持一致。本文中基于对偶四元数的降维惯性/天文子滤波器直接利用天文信息进行组合，不需要将其转化到地理系。由于在卫星导航系统完好的情况下，惯性导航位置误差较小，基于对偶四元数的降维惯性/天文组合优势不明显。为体现本文中惯性/天文组合方法的优势，考虑卫星导航系统由于故障等原因已进行隔离，仅余惯性/天文子滤波器的情况。在3.1节的仿真条件下，将基于对偶四元数的降维惯性/天文组合导航方法与传统惯性/天文组合导航方法进行对比，导航误差对比曲线如图4所示，陀螺常值漂移估计曲线如图5、图6所示。

图4中虚线表示传统惯性/天文组合导航误差，实线表示基于对偶四元数的降维惯性/天文组合导航误差。从表5可知，传统惯性/天文组合模式姿态精度优于纯惯性导航，但是速度和位置精度却较低。这是由于在传统惯性/天文组合模式中位置误差、速度误差、加速度计误差均作为状态量进行卡尔曼滤波估计，但这些状态量可观测性较低或不具有可观测性，将这些状态量反馈并进行惯性导航误差修正，反而可能会降低惯性导航位置、速度精度。基于对偶四元数的降维惯性/天文组合相比于纯惯性导航在姿态、速度、位置(三维矢量和)上均有所提升。该组合模式中仅包含可观的状态量，且直接与天文姿态进行组合，因而具有较好的姿态精度，从而获得了更好的位置和速度精度，具有更优的导航性能，但是由于位置、速度误差缺少修正，因此位置和速度仍呈发散趋势。

表4 基于对偶四元数的惯性/卫星/天文导航误差均方根值统计(有效性验证)

Tab.4 RMS statistics of navigation errors of SINS/GNSS/CNS based on dual quaternions (effectiveness simulation)

导航误差参数类型改进联邦滤波集中式卡尔曼滤波纯惯性导航位置经度误差/m0.64040.5802505.5347误差纬度误差/m0.54080.537432.4197高度误差/m0.46010.449739.0454速度北向速度误差/(m·s-1)0.01200.01150.6160误差天向速度误差/(m·s-1)0.00990.00840.4861东向速度误差/(m·s-1)0.01340.01071.8631姿态俯仰角误差/(°)5.22×10-45.10×10-40.0146误差航向角误差/(°)6.60×10-46.57×10-40.0105横滚角误差/(°)6.42×10-46.30×10-40.0325

表5 惯性/天文导航误差均方根值统计(有效性验证)

观察图4以及对比表5中的误差统计数据，可以发现基于对偶四元数的降维惯性/天文组合相比于传统惯性/天文组合具有更高的姿态精度。对比图5、图6可知，对偶四元数惯性/天文组合模式具有更高的陀螺常值漂移估计精度。这是由于传统惯性/天文组合模式在将天文信息进行坐标转换时引入了惯性导航位置误差，随着惯性导航位置的发散，使得高精度的天文信息受损，天文量测信息对陀螺常值漂移的估计效果随着时间的增长逐步减弱。当惯性导航位置误差较大时，天文精度会大大降低，且由于这种天文信息精度变化导致量测噪声难以设置，会进一步降低滤波精度。相比于传统惯性/天文在地理系下进行姿态组合，对偶四元数惯性/天文实际采用了惯性系姿态组合模式，直接有效地使用高精度天文姿态量测信息，获得了较高的陀螺漂移估计和惯性系姿态误差估计精度。虽然在最终获取地理系姿态的过程中，仍然需要使用惯性导航位置，但是由于准确地对陀螺漂移进行了估计并有效修正陀螺输出，因此可以明显地提高组合精度。

3.2.2 方法应用性验证

1)空天飞行器航迹设计

考虑到惯性/卫星/天文组合导航的使用环境，为了进一步证明算法的应用性，本文以空天飞行器为载体对象，利用在轨段飞行航迹进行仿真测试，具体航迹参数设置如表6所示，轨道航迹如图7所示。

表6 空天飞行器在轨段航迹设置

2)应用性仿真验证

按照3.1节中的仿真设置条件进行仿真测试，得到基于对偶四元数的惯性/卫星/天文导航误差统计值，如表7所示。在卫星失效情况下，惯性/天文导航误差如表8所示。

表7 基于对偶四元数的惯性/卫星/天文导航误差均方根值统计(应用性验证)

对比表7中的误差统计数据可知，空天飞行器在轨飞行运动过程中，基于对偶四元数的改进联邦滤波方法与集中式卡尔曼滤波具有基本一致的精度。通过表8可知，在卫星失效的情况下，基于对偶四元数的降维惯性/天文组合相比于传统惯性/天文组合具有更高的姿态、速度、位置(三维矢量和)精度。

表8 惯性/天文导航误差均方根值统计(应用性验证)

综上所述，本文所提出的基于对偶四元数的惯性/卫星/天文改进联邦滤波算法可以在保证导航精度的同时减少计算量，且能在卫星故障或信号丢失的情况下获得相较于传统惯性/天文更高的导航精度。

4 结论

传统地理系下惯性/卫星/天文组合导航系统联邦滤波中主滤波和子滤波器中均保留所有状态量，而实际天文信息对部分状态量不具有可观测性。传统惯性/天文在组合前需先将天文姿态信息转化为地理系姿态，由于估计回路中引入惯性导航位置误差，因此状态量估计精度逐步降低。针对上述问题，本文提出基于对偶四元数的惯性/卫星/天文改进联邦滤波方法，根据状态量的可观测性建立子滤波器模型，降低子滤波器维数。基于对偶四元数的惯性/天文组合模式采用惯性系姿态组合的方法，直接利用天文信息对陀螺误差和惯性系姿态误差进行准确估计，相比于传统惯性/天文组合模式具有更好的导航性能。仿真结果表明，本文所提出的方法能在减少联邦滤波计算量的同时获得与集中滤波相当的精度，卫星信号丢失情况下相比于传统惯性/天文组合精度更高，从而为惯性/卫星/天文组合模式提供了一种新的有效方法，具有较高的工程应用参考价值。

)

[1] Yang Y Q, Zhang C X, Lu J Z. Local observability analysis of star sensor installation errors in a SINS/CNS integration system for near-earth flight vehicles [J]. Sensors, 2017, 17(1):167.

[2] Deng H, LIU G B, Ehen Hao M, et al. The application of federated Kalman filtering in SINS/GPS/CNS integrated navigation system[J]. International Journal of Wireless and Microwave Technologies, 2012, 2(2): 12-19.

[3] 全伟, 刘百奇, 宫晓琳，等. 惯性/天文/卫星组合导航技术[M]. 北京：国防工业出版社, 2011:6-7.

QUAN Wei, LIU Bai-qi, GONG Xiao-lin, et al. INS/CNS/GNSS integrated navigation technology[M]. Beijing: National Defense Industry Press, 2011:6-7. (in Chinese)

[4] 张科, 刘海鹏, 李恒年,等. SINS/GPS/CNS 组合导航联邦滤波算法[J]. 中国惯性技术学报, 2013, 21(2):226-230.

ZHANG Ke, LIU Hai-peng,LI Heng-nian,et al. SINS/GPS/CNS integrated navigation federal filtering algorithm[J]. Journal of Chinese Inertial Technology, 2013, 21(2):226-230. (in Chinese)

[5] 赵慧, 熊智, 施丽娟,等. 基于惯性系下陀螺误差在线估计修正的惯性与星光组合导航方法[J]. 兵工学报, 2016, 37(12):2259-2267.

ZHAO Hui, XIONG Zhi, SHI Li-juan, et al. A SINS/STAR integrated navigation method based on online estimation of gyroscope error in inertial coordinate[J]. Acta Armamentarii, 2016, 37(12):2259-2267. (in Chinese)

[6] 钱华明, 孙龙, 黄蔚, 等. 基于加性对偶四元数的惯性/天文组合导航算法[J]. 北京航空航天大学学报, 2013, 39(6): 739-744.

QIAN Hua-ming, SUN Long,HUANG Wei,et al. SINS/CNS integrated navigation algorithms based on dual quaternion[J]. Journal of Beijing University of Aeronautics and Astronautics, 2013, 39(6): 739-744. (in Chinese)

[7] 武元新. 对偶四元数导航算法与非线性高斯滤波研究[D]. 长沙：国防科学技术大学, 2005:24-35.

WU Yuan-xin. Research on dual-quaternion navigation algorithm and nonlinear Gaussian filtering[D].Changsha: National University of Defense Technology, 2005:24-35. (in Chinese)

[8] Wang R Y, Liu W C, Bian H W, et al. Fast alignment algorithm with order-reduced filter for SINS[J]. Journal of Chinese Inertial Technology, 2016, 24(5):607-618.

[9] 朱兰伟, 张燕, 蔡春龙. 基于可观测性分析的光纤陀螺SINS/测速仪组合系统技术[J]. 中国惯性技术学报, 2011, 19(1):50-54.

ZHU Lan-wei, ZHANG Yan, CAI Chun-long. FOG SINS/speedometer integrated system technique based on observability analysis[J]. Journal of Chinese Inertial Technology, 2011, 19(1):50-54. (in Chinese)

[10] 衣晓, 何友, 关欣. 基于不同局部模型的联合滤波算法研究[J]. 中国惯性技术学报, 2002, 10(5):16-19.

YI Xiao, HE You, GUAN Xin. Federated filtering algorithm based on different local models[J]. Journal of Chinese Inertial Technology, 2002, 10(5):16-19. (in Chinese)

[11] Gu Q T, Fang J. Global optimality for generalized federated filter[J]. Acta Automatica Sinica, 2009, 35(10):1310-1316.