采用自适应无迹卡尔曼滤波的卫星姿态确定

肖 磊 ，王绍举 ，常 琳 ，周美丽

（1. 中国科学院长春光学精密机械与物理研究所，吉林长春130033；2. 中国科学院大学，北京 100049；3. 中国科学院天基动态快速光学成像技术重点实验室，吉林长春130033）

1 引 言

姿态参数是卫星正常运行的关键参数［1-2］，其测量精度直接影响着卫星的工作性能。基于星敏和陀螺组成的卫星姿态确定系统是卫星姿态确定中较为广泛应用的方案之一［3］。针对卫星姿态确定的非线性问题，拓展卡尔曼滤波（Extended Kalman Filter，EKF）及其改进算法被广泛应用于姿态确定的工程实践中［4-5］。文献［6］采用投影法确定时不变观测矩阵，用恒增益矩阵代替雅可比矩阵，大大减少了算法的计算量，提高了算法的实时性。文献［7］设计了基于新息的函数，制订高低阶数近似切换策略，改善了估计精度和计算负担二者对立的处境。文献［8］将RTS 平滑技术和EKF 算法相结合，应用到遥感卫星定姿的后处理中，在传感器精度不高的情况下，算法仍可以取得较好的姿态确定结果。

但EKF 本质上是将非线性问题线性化近似处理，这导致滤波精度的下降甚至滤波发散，具有局限性。而无迹卡尔曼滤波（Unscented Kalman Filter，UKF）克服了 EKF 的缺陷，拥有更高的估计精度和稳定性［9］。在系统存在模型参数估计不准和量测不良的问题时，上述滤波方法［6-8］的精度和鲁棒性较差，因此研究人员更倾向于使用改进的UKF 算法。文献［10］结合残差协方差匹配原理和最陡梯度下降算法，提出一种鲁棒自适应UKF 算法，并应用于空间三维定位跟踪上，具有良好的跟踪精度和关于模型参数不准的鲁棒性。文献［11］提出一种基于自适应UKF 的皮纳卫星姿态确定算法，以欧拉角作为状态矢量，对系统噪声和量测噪声的协方差矩阵进行自适应调整。文献［12］提出一种修正的自适应Masreliez-Martin UKF 算法，应用于卫星姿态确定，有效地提高了姿态确定精度。

本文在标准UKF 的基础上，结合残差协方差匹配原理和滤波发散判别准则，引入自适应因子矩阵，推导了自适应无迹卡尔曼滤波器（Adaptive Unscented Kalman Filter，AUKF）的卫星定姿算法，并通过卫星姿态测量仿真实验验证了该算法在模型参数估计准确和参数估计不准的情况下的高精度及强鲁棒性。

2 卫星姿态确定系统模型

2.1 陀螺量测模型

陀螺仪的功能是测量卫星的三轴角速度，输出卫星本体系相对于参考坐标系的角速度。假设陀螺测量的坐标系与卫星本体坐标系重合，其测量模型如下［13］：

式中：ωg(t)为陀螺的实际量测输出值；ω(t)为卫星三轴相对参考坐标系的真实角速度；b0为陀螺的常值漂移；d(t)为角速率随机游走，ng(t)为角度随机游走，d(t)，ng(t)二者之和构成陀螺随机误差；ng(t)，nd(t)为不相关的零均值高斯白噪声，满足：

2.2 系统状态方程

以四元数作为姿态参数描述卫星姿态，有：

卫星的四元数姿态运动学方程描述如下：

式中ω=[ωx ωy ωz]T，是卫星三轴相对参考坐标系转动角速度在本体系中的投影，且有：

由于陀螺测量的星体角速度中含有随机误差，采用星敏补偿陀螺漂移时只能得到估计的和。而四元数存在约束性原则，若直接将它作为状态变量处理，在计算过程中容易出现方差矩阵奇异的问题，因此采用误差四元数作为变量。定义真实四元数q和估计四元数之间的误差四元数为 Δq=[Δq1Δq2Δq3Δq4]T。采用增量式表示四元数误差时，有：

真实角速度和估计角速度之间的差值为：

对式（6）求导并将式（7）代入，有［14］：

由式（8）可以看出，误差四元数降阶为3 个独立变量，从而避免了因系统变量冗余而导致的系统协方差矩阵奇异问题。取误差四元数矢量部分和陀螺常值漂移误差Δb作为系统状态 变 量 ，即构 成 系 统 状 态方程：

2.3 系统量测方程

假设星敏测量的坐标系与卫星本体坐标系重合，且安装误差矩阵为单位阵。则星敏量测模型可表示为：

式中：rs为星敏量测输出，r为对应参考矢量，ns为量测噪声。

将星敏输出转换成四元数，可得星敏输出四元数qs为［15］：

设Nout为nout的矢量部分，令星敏输出四元数qs和估计四元数之间的偏差四元数qe为：

由式（14）可得系统量测方程为：

陀螺和星敏组合姿态确定系统的工作原理如图1 所示。

图1 组合定姿估计器的工作原理Fig.1 Working principle of combined attitude determination estimator

3 自适应UKF 算法

3.1 标准UKF 算法

设置采样时间为 Δt，对于式（9）和式（15）组成的卫星姿态确定系统，其离散化方程可由四阶龙格库塔法计算得到：

式中：Xk∈Rn和Zk∈Rm分别为k时刻的系统状态估计量和系统量测量；f和h分别为系统状态转移函数和系统量测函数；wk，vk分别为系统噪声和量测噪声，为相互独立的零均值高斯白噪声，其噪声协方差矩阵分别为Qk和Rk，可表示为：基于非线性系统的标准UKF的算法步骤如下［16］：

（1）系统参数初始化：

（2）计算k-1 时刻的 2n+1 个 Sigma 样本点及权系数：

（3）计算k时刻的一步预测模型值和预测误差协方差：

（4）计算k时刻的一步预测样本点：

（5）计算量测变量预测值：

（6）计算量测协方差和互协方差：

（7）计算增益矩阵：

（8）计算滤波值：

3.2 自适应UKF 算法

UKF 算法的执行过程需要系统噪声和量测噪声的先验信息［17］，然而量测过程中各种干扰因素的存在或者系统统计模型估计的不准确，先验信息和真实值误差过大时会增大滤波的跟踪误差，甚至引起滤波发散。标准UKF 算法将系统噪声协方差Qk和量测噪声协方差Rk等先验信息设置为常值，不能很好地应对实际情况中的变化，因此需要对先验信息做自适应处理。本文提出一种AUKF 算法，对量测噪声协方差和系统噪声协方差进行自适应调整，以获得更有效的卡尔曼增益，增强滤波器的稳定性和跟踪精度。

3.2.1 量测噪声协方差R自适应

量测量是反映系统状态变量的唯一来源。在受到干扰时，量测量的作用应该被降低以减少跟踪误差。因此在这个步骤，通过自适应调整噪声协方差Rk的数值，从而减小增益Kk，以达到减少量测量作用的目的。定义残差εk为滤波器每一步量测量实际值与预测值的差值：

KF 算法中，在滤波最优的情况下，残差协方差理论值满足［18］：

在UKF 算法中，其等价描述为：

由文献［19］可知，残差协方差的理论值可由其估计值得到：

通常，在量测量受到干扰或量测不良时，残差协方差的估计值和理论值满足：

为减小跟踪误差，引入自适应矩阵，表述如下：

式中：Sk=diag{s1s2...sm}，m为量测量的维数。结合残差协方差匹配理论，即可求得自适应矩阵：

由式（37）求得的自适应矩阵Sk很可能并非对角阵，且不能保证非负定性。为避免这种情况，修正Sk的求解方式为：

式中：Nk(i，i)，Rk(i，i)分别为矩阵Nk和Rk第i行、第i列的对角线元素；μ为可调参数，且μ≥ 1。

3.2.2 系统噪声协方差Q自适应

当系统模型参数准确时，在滤波过程中预测误差协方差Pk|k-1的范数会逐渐减小，实时观测量对状态估计的修正作用变弱。而在系统参数不准和外界干扰下，量测协方差阵可能非正定，滤波极易发散，为避免滤波发散，引入对滤波发散趋势的判断规则为：

式中：γ为可调因子（γ≥ 1，值越大判定条件越宽松）；tr( )为求取矩阵的迹。在式（39）成立时，滤波器稳定；当式（39）不成立时，滤波器有发散趋势，因此引入自适应矩阵Λk对系统噪声协方差Qk做自适应调整，使预测误差协方差阵Pk|k-1在保持正定性的基础上增大范数值，提高实时观测量对滤波的修正作用，即有：

式中：Λk=diag{Λ1Λ2...Λn}，Λk的求取方式如下：

3.2.3 AUKF 滤波算法实现

卫星姿态测量中的量测不良和系统模型建立不准会导致姿态确定精度下降。本文在标准UKF 的基础上，基于残差协方差理论提出了AUKF 算法，其工作流程如图2 所示。在对卫星进行姿态确定时，由陀螺和星敏获取角速度ωg和姿态四元数qs，处理后输入到滤波器中，滤波器先执行一次标准UKF 主程序，获得残差向量εk，然后利用式（38）和式（41）计算得到自适应矩阵因子，对量测噪声协方差Rk和系统噪声协方差Qk进行自适应更新，再按照式（18）～式（30）进行量测更新，得到状态估计量X^k和误差协方差Pk，作为下次滤波循环的初值。

图2 自适应UKF 估计器工作流程Fig.2 Work flow of adaptive UKF estimator

4 仿 真

4.1 仿真条件

表1 敏感器参数Tab.1 Sensor parameters

4.2 仿真结果及分析

图3 姿态运动真实轨迹Fig.3 True trajectories of attitude motion

卫星真实姿态运动轨迹如图3 所示。为验证算法性能，实验分别考虑了系统建模准确和不准确两种情况，选取标准UKF 算法和文献［20］提出的鲁棒自适应UKF（RAUKF）算法进行分析对比，3 个滤波器的初值设置相同。

在没有噪声干扰，系统建模准确时，3 种算法的滤波结果如图4～图6 所示。

图4 建模准确时UKF 三轴姿态估计误差Fig.4 Three-axial attitude estimation errors by UKF with accurate model

图5 建模准确时RAUKF 三轴姿态估计误差Fig.5 Three-axial attitude estimation errors by RAUKF with accurate model

图6 建模准确时AUKF 三轴姿态估计误差Fig.6 Three-axial attitude estimation errors by AUKF with accurate model

分析上述仿真结果可知，在建模准确时，传统UKF 三轴姿态估计的误差波动较大，精度控制在0.02°以内；RAUKF 三轴估计姿态的误差波动较小，偏航和滚转轴精度控制在0.01°以内，俯仰轴稳定后精度控制在0.005°以内；AUKF 三轴估计的误差波动最小，三轴精度都控制在 0.005°以内。和 RAUKF 相比，AUKF 算法在引入自适应因子时，采用多变量因子矩阵，使得各个滤波通道拥有不同的自适应调整能力，三轴精度大致相同。

在有噪声干扰时，模拟建模不准的情况下，仿真数据中系统噪声和量测噪声均方差增大为原设定值的2 倍，但滤波器的相应参数初值不变，令滤波器参数不能反映噪声的真实统计特性，3种算法的滤波结果如图7～图9 所示。

图7 建模不准时UKF 三轴姿态估计误差Fig.7 Three-axial attitude estimation errors by UKF with inaccurate model

图8 建模不准时RAUKF 三轴姿态估计误差Fig.8 Three-axial attitude estimation errors by RAUKF with inaccurate model

由图可见，在模型参数估计不准确时，3 种算法的三轴姿态估计精度都有所下降，标准UKF和RAUKF 算法的估计精度的下降幅度较大，分别在 0.05°和 0.02°以内，说明 RAUKF 的自适应能力较差，不能很好地应对建模不准的情况，但是对野值具有一定的抑制作用；而AUKF 算法精度的下降幅度较小，其滤波结果仍能快速收敛，在0.01°以内，鲁棒性较好。

为更好比较3 种算法的三轴姿态估计精度，统计三轴姿态估计的均方根误差（Root Mean Square Error，RMSE），仿 真 结 果 如 表 2 所 示 。RMSE 表达式为：

表2 三种算法的性能对比Tab.2 Performance comparison of three algorithms

由表2 可以看出，参数估计准确时，AUKF的三轴姿态角精度最高，较UKF 分别提升了70.6%，71.3%，71.9%，较RAUKF 分别提升了17.7%，14.8%，46.8%；参数估计不准时，UKF和RAUKF 算法的滤波性能迅速下降，而AUKF三轴估计误差也有增大的趋势，但下降幅度较其余两种算法较小，其三轴估计精度较两种算法分别 提 升 74.9%，75.9%，74.9% 和 30.0%，34.1%，22.4%。不过，AUKF 算法耗时也大幅增加，较UKF 平均增加37.0%，和RAUKF 耗时相当。综合来看，所提出的AUKF 算法相较于UKF 和RAUKF 算法拥有更高的精确性和鲁棒性。

5 结 论

本文根据卫星姿态确定精度高、鲁棒性强的要求，提出了一种AUKF 算法，介绍了卫星组合定姿的工作原理和滤波算法的工作流程，然后在模型参数估计准确和不准确两种情况下对UKF，RAUKF 和AUKF 3 种算法进行性能对比仿真验证。实验结果表明，在模型估计不准时，AUKF较RAUKF 三轴估计精度分别提升了30.0%，34.1%，22.4%，能够满足卫星姿态确定对精度和鲁棒性的要求。