基于盒粒子滤波的低成本皮纳卫星高效姿态确定算法

侯晓磊，张聪哲，刘 勇，潘 泉，李毅兰

(西北工业大学自动化学院，西安 710072)

0 引 言

皮纳卫星是近年来兴起的一种新概念微小卫星[1]，质量通常在0.1～10 kg之间，其具有发射方式灵活、功能密度高、研制周期短、成本低等优点。

姿态确定系统是卫星飞行控制系统中的重要组成部分，其定姿精度直接影响卫星运行的稳定性甚至决定任务成败。目前，卫星姿态确定系统多采用光电太阳敏感器及磁强计作为主要传感器，并已在大卫星上得到了应用，如COMPASS-1，AAUSa1[2]，同时，近年来MEMS、MOEMS等先进加工技术的发展实现了传感器的微小型化，诸如MEMS微型陀螺和微型加速度计，以及MOEMS太阳敏感器和三轴微型磁强计等[3-4]传感器开始在皮纳卫星上得到广泛的应用。然而，微型传感器受到体积、测量原理等方面的限制，导致其量测性能相对较弱。同时，皮纳卫星的设计存在体积、功耗、成本、运算能力和存储空间等方面的约束。同时，为满足卫星姿态控制精度，皮纳卫星对姿态确定算法以及卫星的运算能力提出了较高的要求[5]。

目前卫星姿态确定算法主要包括两大类，即静态确定性算法和状态空间估计算法。TRIAD法[8]是一种典型的静态确定性算法，在无需姿态的先验知识的情况下，通过有姿态敏感器测量值构建的两个非平行矢量测量信息确定卫星的姿态矩阵[6]，算法简单且计算量较小，但其姿态确定结果易受测量精度影响导致较大误差，并且至少需要两个矢量才能确定姿态，其应用范围有一定的局限性[7]；卫星姿态确定的状态空间估计算法，是利用传感器量测的历史信息，结合航天器姿态运动模型，得到统计意义下的最优姿态估计[6-7,9]。在早期工程应用领域中主要采用扩展卡尔曼滤波(EKF)[10-11]，但由于航天器姿态观测的噪声类型复杂、状态量分布各异，导致EKF滤波器设计复杂且可能无法完成姿态估计，后来逐渐发展出可在任意分布下实现非线性近似的粒子滤波(PF)方法[12-13]。

皮纳卫星姿态确定过程是高非线性的，基于PF的高精度姿态确定方法计算复杂度高，皮纳卫星的CPU无法提供高效的计算能力，而基础的EKF方法精度不够。在某些实际应用中，为了保证结果的精确性，粒子滤波所采用的重要性重采样(SIR)方法可能需要数千个粒子。于是为了降低采样粒子个数，Abdllah等[14]通过将持续蒙特卡罗(SMC)和区间分析相结合，提出了以描述状态空间中占据一个体积小且可控制的非零矩形区域的盒粒子的概念，并在此基础上提出了盒粒子滤波(Box particle filter,BPF)[15-16]。BPF滤波器通过区间的概念有效地减少了粒子数量、降低了计算的复杂度，并且较适合分布式过滤。一般BPF滤波器只需几个或者几十个盒粒子即可达到与粒子滤波相同或者相近的精度。

本文基于区间理论，提出了区间化皮纳卫星姿态描述方法，并建立了姿态敏感器的数学模型，又在此基础上，推导出基于BPF的皮纳卫星姿态确定算法，通过数字仿真校验了算法的有效性，同时探究了不同参数设置对滤波器参数影响。

1 卫星姿态区间化描述

1.1 卫星姿态区间化描述

在许多应用中，区间理论是一个很好的处理非白噪声和偏差的方法。本文提出一个涉及区间数据移动定位的PF策略。使用区间数据可以有效降低PF算法所使用的粒子的数量，从而提高计算效率。

在利用区间理论描述卫星姿态时，在状态空间Rn内，定义区间[x]，[x]是空间Rn内封闭连续集合，则盒[x]可表示为区间的笛卡尔积，即：

(1)

其中，[xi]表示空间R内的封闭连续子集。则卫星姿态四元数可以区间化表示为：

[q]=[η]×[ε1]×[ε2]×[ε3]

(2)

根据四元数的约束条件，

[q]⊂[-1,1]4

(3)

令[q]=[ql,qh]，其中，[ql]为[q]下界，[qh]为[q]上界。根据欧拉角定义，姿态角速度区间化表述为：

[ω]=[ωx]×[ωy]×[ωz]

(4)

1.2 卫星数学模型区间化描述

(5)

根据区间分析理论，定义f的区间化表示为[f]，同样一个区间[x]函数[f]可以表示为[f]([x])，则姿态区间[q]的姿态运动学方程为：

(6)

(7)

同样得到区间化的动力学方程为:

(8)

由此得到了卫星姿态以及动力学模型的区间化描述，并基于此描述提出了改进的BPF姿态确定方法。

2 改进BPF姿态确定算法

本文针对皮纳卫星所装备传感器的特点，提出了将双矢量算法与BPF相结合的改进BPF姿态确定算法，图1为其算法流程图。

图1 改进BPF姿态确定算法流程图

改进BPF姿态确定算法首先通过双矢量算法对敏感器量测进行预处理，将解算出的姿态四元数作为量测值输入到BPF中，以降低敏感器噪声对估计精度的影响，从而获得更加精确的卫星姿态数据。

2.1 区间化描述

1)状态方程

选择区间化四元数与区间化角速度作为状态量。则：

[ε3]×[ωx]×[ωy]×[ωz]

(9)

定义[F]([X])为非线性区间化状态函数，即

(10)

结合式(9)与式(10)可以得到:

(11)

定义lkl和lkh分别表示第k时刻状态量区间的下界和上界。且有：

(12)

由于姿态运动学曲线变化平缓，当区间选择较小时，定义在区间内的状态转移函数基本为线性，由此可以得到：

[Xk]=[F]([Xk-1])=[lkl,lkh]=[min(F(l(k-1)l),

F(l(k-1)h)),max(F(l(k-1)l),F(l(k-1)h))]

(13)

采用四阶Runge-Kutta方法求解微分方程得到下一时刻的状态，得到:

(14)

2) 量测方程

当姿态测量仅使用磁强计和太阳敏感器时，量测量为磁强计和太阳敏感器的输出。但是这种方法在每一步滤波时会将得到的姿态旋转矩阵进行坐标转换，从而增加了计算量。为此，采用双矢量方法对量测进行预处理，降低量测维度，减少计算量，从而得到三维量测为:

[Z]=[ε]=[ε1]×[ε2]×[ε3]

(15)

同样，定义zkl和zkh分别表示第k步量测量区间的下界和上界。由此得到系统量测方程为：

(16)

2.2 从PF到BPF的姿态确定算法过程推导

1)盒粒子初始化

(17)

2)状态转移

在此步骤中，状态量盒粒子会根据状态方程进行更新。在这个过程中粒子滤波与盒粒子滤波之间的显著区别是对噪声的处理。在盒粒子滤波过程中，会根据有界误差模型对建模和量测误差进行处理[16]。

BPF的状态转移方程可以表示为：

(18)

又根据式(12)、式(13)，状态方程可以写成

(19)

3)状态更新

BPF的状态更新包括以下步骤：预测、残差计算、似然函数求解、盒式粒子滤波收缩、权重更新、归一化及状态估计过程[7]。

(1)预测

在预测过程，粒子滤波和BPF都是通过量测方程来推算第k步的预测值。BPF的预测方程可以写为：

(20)

(21)

(2)残差

使用BPF时，可预测的值可以比作一个真正的盒子测量，因此残差要求能够反应真实量测与预测量的接近程度，由此可以将残差定义为量测区间与预测区间的交叉点[7]。本文中，量测噪声近似于高斯分布，即为高斯白噪声，符合正态分布。由正态分布的置信度理论可以得到，在3倍标准差±σ以内，随机误差出现的概率为99.73%。再结合边界误差理论对量测建模。高斯白噪声vk区间化后为[vk]=[-3σv, 3σv]，则可以得到量测区间为：

[yk]=[yk-3σ,yk+3σ]=[ykl,ykh]

(22)

(23)

下式用以保证盒粒子满足区间特性：

(24)

(25)

则残差盒粒子的区间长度为：

(26)

(3)似然函数

对于粒子滤波，根据量测噪声的概率模型可以求出其似然函数，由于量测噪声w符合高斯分布。根据边界误差方法，很显然对于一个盒粒子，预测盒子和量测盒子没有交集的盒子应该被舍弃，仅保留那些同时包括实际量测信息与预测信息的盒子。由此，基本的盒式粒子滤波的似然函数可以设计如下[7]：

(27)

式中:q表示量测的维数，即为3。

(4)盒式粒子滤波收缩过程

盒粒子收缩过程旨在利用Waltz理论[7]，通过残差缩小状态量预测与估计区间，从而加速滤波过程，提高运行效率，使得BPF相较于PF更加快速高效。

收缩器可以定义为用于缩小CSP基础定义域的算法，从而产生新的S∈[x′]满足的盒[x′]∈[x]。根据Waltz理论，BPF收缩方程为：

(28)

式中:[B]为Waltz算法的函数表示。

(5)权重更新

(29)

而BPF和粒子滤波的单位化过程相同，都是为了保证这些盒粒子的权重之和为1，方程如下：

(30)

(6)状态估计

粒子滤波中，根据粒子的权重可以估计出系统当前时刻的状态，这个估计结果的可信度可以通过协方差矩阵来进行表述[7]。而盒粒子的估计值则是通过N维的中心粒子来计算，可以表示为：

(31)

(32)

4)重要性重采样

滤波过程中，区间收缩和概率选择的过程会使粒子多样性减小，从而导致滤波算法精度下降。解决这一问题的一般方法为重采样[7]，重采样的方法有很多种，本文采用的是重要性重采样。

其中，令：

(33)

(34)

重采样结束后，完成盒粒子更新及权重归一化：

(35)

3 仿真结果

由于本文设计的算法的输入为双矢量姿态确定结果，量测噪声非高斯，而滤波器设计中所用到的噪声模型要求为高斯分布，因此双矢量姿态确定算法的姿态误差模型的建立是整个仿真实验的先决条件[7]。根据实际需求，本文首先仿真统计了不同量测噪声下双矢量姿态确定算法的精度。统计结果如下：

(1)双矢量姿态确定的三轴精度均值基本为0。

(2)双矢量姿态确定的三轴精度标准差统计如表1所示。

表1 双矢量算法姿态确定精度统计表[7]

(1)BPF姿态确定算法与PF姿态确定算法的对比实验。

(2)量测噪声对BPF姿态确定精度的影响实验。

(3)盒粒子数量对BPF姿态确定的影响实验。

1)基准场景下BPF姿态确定算法与PF姿态确定算法的对比实验

为验证BPF姿态确定算法的有效性，如下给出了BPF姿态确定算法与PF姿态确定算法的实验对比结果。基准场景参数设计如下：

(1)BPF盒粒子数为100，PF粒子数为1000。

(2)两个滤波器的重采样阈值均为20。

(3)磁强计噪声为10-7/T，太阳敏感器噪声为0.01。根据表1中的结果，双矢量姿态确定算法估计精度的标准差为0.96。

图2给出了BPF与PF姿态确定误差对比结果。从图2可以看出，在姿态确定初期，BPF姿态确定算法相较于PF方法具有较快的收敛速度，姿态误差曲线在500 s左右进入稳态。由于BPF方法具有盒粒子收缩过程，即通过量测值与量测预测的交集反解出状态量以缩短状态量的区间，加快收敛速度，图中的跳变就是因为量测噪声突变导致反解出的状态量大幅度偏移的结果；PF姿态确定方法在700 s以后收敛速度降低直至进入稳态，且在稳态时有较好的估计精度。

图2 PF与BPF姿态误差对比

表2为姿态稳态时BPF与PF姿态估计精度与运行时间的统计结果。统计数据表明，BPF姿态确定法估计精度略低于PF方法(约为PF方法的73.3%)，但其运行时间仅为PF方法的22.9%。由此可以得到，在控制精度相近时，BPF姿态确定方法相较于传统的PF方法能够有效提高算法运行效率，减少系统能耗。

表2 姿态稳定时BPF与PF滤波结果比较

2)量测噪声对BPF姿态确定的影响实验

为了进一步校验BPF姿态确定算法的可靠性，如下仿真实验给出了不同量测噪声下BPF姿态确定估计结果。其中对比实验的磁强计噪声为10-6T，太阳敏感器噪声为0.01，根据表2中的结果，双矢量姿态确定算法估计精度的标准差为3.64，其余参数与基准场景下的滤波器参数设置相同。仿真结果如图3所示。

图3给出了量测标准差分别为0.96和3.64时的BPF姿态确定估计结果。可见，当量测噪声较大时，姿态估计精度曲线初始估计精度变差，收敛速度显著降低。这是因为当量测噪声较大时，改进BPF姿态确定算法中量测区间范围明显增大，初始估计精度随之变差。同时，通过盒粒子反解过程得到的新的状态区间也随之变大，因而收敛速度和最后的估计精度都受到了影响。

图3 不同量测噪声下BPF姿态确定估计结果

表3给出了不同量测噪声下改进BPF姿态确定算法的估计精度与运行时间。统计结果进一步表明，随着量测噪声增大，改进BPF姿态确定算法运行时间相差不大，但估计精度明显降低，即改进BPF姿态确定精度与量测噪声的大小呈负相关。

表3 不同量测噪声下BPF滤波结果比较

3)盒粒子数量对BPF姿态确定的影响实验

本节主要通过仿真实验校验盒粒子数量与BPF姿态确定精度的关系。对比实验的盒粒子数目为50。其余参数均与实验1)中的滤波器参数设置相同。仿真实验结果如图4所示。

图4 不同盒粒子数量下的BPF姿态确定估计结果

图4给出了盒粒子数目分别为50和100时的BPF姿态确定估计结果。可见，当盒粒子数目减少时，姿态估计精度曲线初始估计精度显著变差，但收敛速度较之变化不大。

表4给出了不同盒粒子数目下改进BPF姿态确定算法的估计精度与运行时间。统计结果进一步表明，粒子数目由100减少至50时，改进BPF姿态确定算法运行时间缩短了25.86%，估计精度降低了41.51%。算法运行时间虽然随着粒子数目的减少而缩短，但两者不是正比关系，这是因为初始盒粒子数目减少时，滤波过程中盒粒子区间衰减明显，因而算法因重采样次数增多而耗费了一些时间。由此得出，改进BPF姿态确定精度与盒粒子数目呈正相关，算法运行时间与盒粒子数目负相关。因此，在实际选择粒子数目时，应综合考虑精度和时间的影响，以满足实际工程问题的指标要求。

表4 不同盒粒子数目下BPF滤波结果比较

4 结 论

本文基于区间理论，提出了区间化皮纳卫星姿态描述方法，建立了姿态敏感器的数学模型。在此基础上，推导出基于改进BPF的皮纳卫星姿态确定算法，并通过数字仿真校验了算法的有效性，同时探究了不同参数设置对滤波器性能的影响。目前，该姿态确定算法应用于基于手机平台的小卫星的项目研究中，即手机卫星姿态确定系统。该算法既适用于当前课题的应用也为其他手机卫星研究提供了借鉴和经验，为今后BPF在工程项目中的应用奠定了基础。