亚星游戏官网-www.yaxin868.com

专利名称：：一种基于esoqpf和ukf主从滤波的微小卫星姿态确定方法
技术领域：
：本发明涉及一种基于ESOQPF(EstimatoroftheQuaternionParticleFilter,四元数粒子滤波估计器)和UKF(UnscentedKalmanFilter，Unscented卡尔曼滤波)主从滤波的微小卫星姿态确定方法，适用于有陀螺配置模式下，基于矢量观测的微小卫星姿态确定。本发明属于航天器姿态确定领域。
背景技术：
：姿态确定是微小卫星系统技术中的重要问题之一，其精度和实时性是影响姿态控制系统精度的重要因素。姿态确定的精度和实时性不仅取决于姿态敏感器的性能，还与姿态确定方法有关。姿态确定方法分为确定性算法和状态估计法两类。常见的确定性算法有q_method、QUEST(QuaternionEstimator,四元数估计器)、ES0Q(EstimatoroftheOptimalQuaternion，最优四元数估计器)等。常见的基于状态估计的姿态确定方法有KF(KalmanFilter，卡尔曼滤波)、EKF(ExtendedKalmanFilter，扩展卡尔曼滤波)、UKF和PF(ParticleFilter，粒子滤波)等。其中，UKF采用U变换来处理状态方程和观测方程的非线性问题，不需要计算Jacobian矩阵，计算的状态均值和方差可以精确到三阶，但UKF对于具有较强非线性、非高斯系统的状态估计就达不到期望的效果。针对上述不足，出现了PF滤波方法。PF是一种利用随机样本或粒子来表示系统状态变量的后验概率分布的递推贝叶斯滤波方法，将PF应用于姿态确定，估计精度高。但PF存在计算量大从而使定姿过程长的缺点，为了达到要求的估计精度，需要上百个甚至上千个粒子，且随着状态维数的增加，计算量会剧增，严重影响姿态确定的实时性。近几年，已有学者在提高姿态确定实时性方面进行了研究。其中，YaakovOshman提出了GA-QPF(GeneticsAlgorithm-QuaternionParticleFilter，遗传算法-四元数粒子滤波)算法，该算法将姿态四元数与陀螺漂移分开估计，利用QPF(QuaternionParticleFilter，四元数粒子滤波)算法估计四元数，GA(GeneticsAlgorithm，遗传算法)算法估计陀螺漂移，进而降低了粒子滤波状态维数，提高实时性。其中，QPF算法直接利用加权四元数粒子集进行时间更新和观测更新处理，并结合q_method计算姿态四元数。但是qjiiethod估计姿态四元数时需要求出Davenport(达文波特)矩阵的所有特征值，计算量大，从而定姿过程长。
发明内容本发明的技术解决问题是(1)克服了基于粒子滤波的定姿方法计算量大从而定姿过程长，姿态确定实时性差的不足，利用ESOQPF和UKF进行主从滤波，将姿态四元数与陀螺漂移分开估计，在保证姿态确定精度的同时提高实时性；(2)克服了QPF在估计姿态四元数时采用q_method算法引起的计算量大从而定姿过程长的不足，ESOQPF在估计姿态四元数时采用ES0Q2算法，进一步降低计算量，缩短了定姿过程。本发明的技术解决方案是基于ES0QPF和UKF主从滤波的微小卫星姿态确定方法，将ES0QPF作为主滤波器估计姿态四元数，UKF作为从滤波器估计陀螺漂移，即姿态四元数与陀螺漂移分开估计。但是，主从滤波器之间又存在相互联系主滤波器估计第k时刻姿态四元数时，要利用从滤波器第k-1时刻估计的陀螺漂移；从滤波器估计第k时刻陀螺漂移时，要利用主滤波器第k-1时刻估计的姿态四元数。此外，ES0QPF主滤波器估计姿态四元数时采用一种计算量小的确定性算法ES0Q2算法，进一步提高姿态确定的实时性。本发明的具体步骤如下(1)建立微小卫星系统状态空间模型a.姿态四元数运动学方程qk+1=Q(wk)qk式中，qk和qk+1分别为第k时刻和第k+1时刻卫星本体坐标系相对于参考坐标系的姿态四元数，gA=[qkTq4kf&和q4k分别为四元数的矢量部分和标量部分；《k为第k时刻本体坐标系相对参考坐标系姿态角速度。假设在采样周期At内《k保持不变，则rn、cos(0.5||^||A)/3><3-[^X]￥kO.{cok)=T,丨丨丨丨、式中，I3X3*3X3单位阵；V.7；[￥kX]^；…的反对称矩阵。wb.陀螺数学模型cbk=cok+pk+]v’k，3k+1=3k+nu,k式中，钱为陀螺的测量输出值；《k为真实的姿态角速度4k和3k+1分别为第k时刻和第k+i时刻的陀螺漂移；nv,k与nu,k是互不相关的零均值高斯白噪声，nv,k和nu,k的方差分别为ov2与ou2。c.矢量观测模型bk=A(qk)rk+8bk式中，bk为观测矢量；rk为参考矢量；Sbk为测量噪声，且Sbk服从概率分布Sbk~psbt；A(qk)为参考坐标系到卫星本体坐标系的姿态转移矩阵。(2)基于步骤⑴建立的系统状态空间模型，进行滤波初始化滤波初始化要基于步骤(1)建立的系统状态空间模型，包括ES0QPF主滤波器初始化和UKF从滤波器初始化两部分。ES0QPF主滤波器的初始化主要是初始化N个粒子及粒子权值；UKF从滤波器的初始化主要完成陀螺漂移估计值和估计误差方差阵的初始化。(3)以姿态四元数为状态变量，基于步骤(1)中的四元数运动学方程和矢量观测模型建立主滤波器的状态空间模型，利用ES0QPF估计姿态四元数。ES0QPF主滤波器估计姿态四元数的基本步骤包括从第k-1时刻到第k时刻的时间更新，测量更新即粒子的权值更新，计算Davenport矩阵K，利用ES0Q2算法估计第k时刻姿态四元数么。此外，主滤波器估计第k时刻姿态四元数么时用到从滤波器第k-1时刻估计出的陀螺漂移；^,。(4)四元数转换成姿态角根据四元数与姿态角之间的关系，将步骤(3)估计出的姿态四元数么转换为姿态四元数对应的姿态角横滚角外，俯仰角ek，偏航角Vk。(5)以陀螺漂移为状态变量，基于步骤(1)中的陀螺漂移数学模型和矢量观测模型建立从滤波器的状态空间模型，利用UKF估计陀螺漂移。UKF从滤波器估计陀螺漂移的基本步骤包括计算Sigma点，计算Sigma点权值，时间更新，测量更新。此外，从滤波器估计第k时刻陀螺漂移及时用到主滤波器第k-1时刻估计出的姿态四元数么。(6)当有新的观测值时，重复步骤(3)(5)进行姿态确定。本发明的原理是微小卫星姿态确定的主要任务是利用星上的姿态敏感器测量所得到的信息，经过适当的处理，求得固连于卫星本体的坐标系相对于空间参考坐标系的姿态。由于陀螺存在漂移，无法将陀螺的输出值直接用于姿态估计，所以在四元数估计过程中要考虑陀螺漂移。本发明原理步骤是(1)建立微小卫星系统状态空间模型；(2)滤波初始化；(3)以姿态四元数为状态变量，将ES0QPF作为主滤波器估计姿态四元数；(4)将估计出来的四元数转换为姿态角；(5)以陀螺漂移为状态变量，将UKF作为从滤波器估计陀螺漂移。此外，主从滤波器之间又存在相互联系，主滤波器估计第k时刻姿态四元数时，要利用从滤波器第k-1时刻估计的陀螺漂移；从滤波器估计第k时刻陀螺漂移时，要利用主滤波器第k-1时刻估计的姿态四元数。本发明与现有技术相比的优点在于本发明涉及一种基于ES0QPF和UKF主从滤波的微小卫星姿态确定方法，在保证姿态估计精度的同时，从两个方面降低了计算量，缩短了定姿过程。(1)从整体上讲，将ES0QPF作为主滤波器估计姿态四元数，UKF作为从滤波器估计陀螺漂移，即姿态四元数与陀螺漂移分开估计，避免了粒子滤波状态维数高而引起计算量大从而定姿过程长的问题，在保证姿态确定精度的同时提高姿态确定方法的实时性；(2)对于主滤波器来讲，ES0QPF估计姿态四元数时，将QPF算法与ES0Q2算法相结合，采用ES0Q2算法直接计算Davenport矩阵的最大特征值所对应的特征向量，克服了QPF中采用q_method计算Davenport矩阵所有特征值引起的计算量大从而定姿过程长的不足，进一步提高姿态确定的实时性。图1为本发明的实现流程图；图2为ES0QPF与UKF主从滤波器在第k时刻的计算流程图。具体实施例方式如图1所示，本发明采用ES0QPF和UKF主从滤波确定微小卫星姿态，具体步骤如下1.建立微小卫星系统状态空间模型a.姿态运动学方程姿态四元数运动学方程的离散形式为qk+1=Q(cok)qk(1)式中，qk和qk+1分别为第k时刻和第k+1时刻卫星本体坐标系相对于参考坐标系的姿态四元数，&=[qkTq4k]T&和q4k分别为四元数的矢量部分和标量部分；《k是本体坐标系相对参考坐标系姿态角速度。假设在采样周期At内《k保持不变，则rn、「cos(0.5吟A,)4<3-^]QK)=；,式中，I3X3*3X3单位阵；V.‘；；[￥kX]^；…的反对称矩阵。Wb.陀螺数学模型陀螺的数学模型采用下式描述&k^(Dk+Pk+T]vk，旦k+i=旦k+nuk(2)式中，而为陀螺的测量输出值；《k为真实的姿态角速度4k和3k+1分别为第k时刻和第k+i时刻的陀螺漂移；nv,k与nu,k是互不相关的零均值高斯白噪声，nv,k和nu,k的方差分别为ov2与ou2。c.矢量观测模型bk=A(qk)rk+8bk式中，bk为测量矢量；rk为参考矢量；Sbk为测量噪声，且Sbk服从概率分布Sbk~psh；A(qk)为参考坐标系到卫星本体坐标系的姿态转移矩阵，表示为A{qk)=[(q,kf-qkrqk]hx3+2qkqkT-2qik[qkx](3)式中，[^x]为四元数矢量部分&的反对称矩阵。2.基于步骤(1)建立的系统状态空间模型，进行滤波初始化滤波初始化要基于步骤(1)建立的系统状态空间模型，包括ES0QPF主滤波器的初始化和UKF从滤波器的初始化两部分。a.ES0QPF主滤波器初始化四元数粒子初始化为qQ(i)，i=1，…，N；各个粒子的权值初始化为WQ(i)=1/N，i=1，…，N;N为粒子个数。b.UKF从滤波器初始化=E[J%]’Pfio=E[{P0-成)(爲-A)"](4)式中，0^和&分别为初始时刻陀螺漂移的真值和估计值；&。为初始估计误差方差阵；E[*]表示数学期望。3.ES0QPF估计姿态四元数以姿态四元数为状态变量，基于步骤(1)中的四元数运动学方程和矢量观测模型建立主滤波器的状态空间模型状态方程&⑶观测方程bk=A(qk)rk+6bk(6)由于陀螺存在漂移，其输出值不能直接用于姿态估计，所以在四元数估计过程中要将陀螺漂移估计出来。实际计算过程中，主滤波器估计第k时刻的姿态四元数时要利用从滤波器第k-1时刻估计的陀螺漂移。ES0QPF主滤波器(1)估计姿态四元数基本步骤如下a.QPF算法计算Davenport矩阵K利用第k_l时刻的四元数粒子qnG)、归一化的粒子权值和第k时刻的观测值bk计算Davenport矩阵K。1)时间更新根据式(5)由第k_l时刻的四元数粒子c^^i)求第k时刻各粒子的预测值2)测量更新测量更新即粒子权值更新，利用第k_l时刻的归一化的粒子权值和第k时刻的观测值bk计算第k时刻的粒子权值Wk(i)。Wk(i)ccpbM{bk\qk^(i)Wk-X(})=Psh(bk-A{qk{k_x(i))rk)Wk,x(i)(7)式中，i=1，…，N；A▲仇!“)为‘的似然概率；;X汍-氺知—々))。)为h-成么|^(0)灿勺概率。将粒子权值Wk⑴归一化Wk(0=Wk(0/…,N(8)4)计算Davenport矩阵KBk+Bkr~I3xMBk)z(9)其中，代矻(10)/=1厶z=(11)Bk{3,2)~Bk(2,3)丛(1,3)-丛(3,1)Bk(2,l)-Bk(l,2)_式中，tr(Bk)为矩阵Bk的迹。b.利用ES0Q2估计姿态四元数么第k时刻四元数估计值么为矩阵K最大特征值所对应的特征向量，即Kqk=AmJk1)求矩阵K的最大特征值入(12)(13)式中，b=-2tr(Bk)+tr(adj(Bk+BkT))-zTz；d=det(K)；adj(Bk+BkT)为矩阵(Bk+BkT)的伴随矩阵；tr(adj(Bk+BkT))为矩阵adj(Bk+BkT)的迹；det⑷为矩阵K的行列式c2)求第k时刻姿态四元数估计值么姿态四元数与欧拉轴/角的关系q=[eTsin(0/2)cos(0/2)]T式中，e为旋转轴矢量，O为绕旋转轴的转角(将式(14)代入式(12)并消去①得[(tr(Bk)-入隱)S-zzT]e=Me=0(14)(15)9式中，S=811+8111-(廿低)+人_)13><3，由上式可知，矩阵1为对称阵，即mmrm、,m=M=[^！m2w3]=ymhmsmymzmc(16)由式(15)可知，旋转轴e与矩阵M的各行/列向量都正交，所以可通过两向量叉乘求取旋转轴e。因此，旋转轴e的取值有三种情况e}=m2xm3=[mbmc-mz2mymz-mxmcmxmz-mymj7e2=fh3xmx(17)=[mymz-mxmcmamc-my2mjny-mzmafe3=Wjxin2=[mxm2-mymbmxmy-mzmamamb-mz2f式(17)中的ei(i=1，2，3)是相互平行的，但模值不同，选取4=maX{||e,’||}f=1。则ek最大特征值对应的特征向量为qk=Tzek(18)将^归一化得第k时刻姿态四元数估计值么为qk=qj^k(19)4.四元数转换成姿态角根据四元数与姿态角之间的关系，将步骤(3)估计出的姿态四元数么转换为姿态四元数对应的姿态角横滚角約，俯仰角ek，偏航角Vk。5.UKF估计陀螺漂移由于陀螺存在漂移，无法将陀螺的输出值直接用于姿态估计，所以在四元数估计过程中要将陀螺漂移估计出来。以陀螺漂移为状态变量，基于步骤(1)中的陀螺漂移数学模型和矢量观测模型建立从滤波器的状态空间模型。状态方程3k=3H+nu,H(20)观测方程bk=A、qk、rk+Sbk(21)=A(Q(3k_,—jBk_x)qk_])rk+5bk由状态空间模型可知状态方程为线性，观测方程为非线性。从滤波器估计第k时刻陀螺漂移时要利用主滤波器第k_l时刻估计的姿态四元数。假设状态方程和观测方程的噪声方差阵分别为Q和R，UKF从滤波器(2)估计陀螺漂移的基本步骤为a计算Sigma点Xo,k-\=Pk-\x刺=1”(22)式中，x。,^，Xi.H，x^H分别是戎_,对应的第0个、第i个、第i+n个Sigma点，及q对应的Sigma点共有2n+l个别为第k_l时刻陀螺漂移的估计值和估计误差方差阵；n为状态维数，这里n=3；T为合成比例参数；将估计误差方差阵分解，(D,表示A的第i列。^AA10113]b.计算Sigma点对应的权值0114]0118]0128]0129]0130]W0=T/(n+r)W,=l/[2(w+r)]i=l,...,nWl+n=l/[2(n+r)](23)0115]式中，W^，W^Wh分别是第0个、第i个、第i+n个Sigma点对应的权值c0116]c.时间更新0117]第k时刻各Sigma点的预测值,kk-l=Xi=0,1,…，2n(24)0119]第k时刻陀螺漂移的预测均值爲和预测误差方差阵々t分别为0120]p-k==P,+Q,=oAA-丨^0121]式中，为第k-l时刻陀螺漂移的预测误差方差阵。0122]第k时刻各Sigma点对应的测量预测值0123]=~Xi,k\k-i)Qk-\)rk‘Z=0，l”..，20124]测量的预测均值&为aIn0125]b￥=L_々)(25)(26)(27)0126]d.测量更新0127]测量的预测误差方差阵和测量与状态变量之间的协方差阵分别为pbA=-h)(V-丨(0-+Ri=0(28)(29)增frff.矩o3阵c=PpkbkPbkbk0131]则第k时刻陀螺漂移的估计值&和陀螺漂移估计误差方差阵々t分别为0132]Pk=P-k+Kk(bk-b-k)(31)权利要求一种基于ESOQPF和UKF主从滤波的微小卫星姿态确定方法，其特征在于包括以下步骤(1)建立微小卫星系统状态空间模型微小卫星系统状态空间模型包括姿态四元数运动学方程、陀螺数学模型、矢量观测模型；a.姿态四元数运动学方程qk+1＝Ω(ωk)qk式中，qk和qk+1分别为第k时刻和第k+1时刻卫星本体坐标系相对于参考坐标系的姿态四元数，和q4k分别为四元数的矢量部分和标量部分；ωk为第k时刻本体坐标系相对参考坐标系的姿态角速度。假设在采样周期Δt内ωk保持不变，则<mrow><mi>&Omega;</mi><mrow><mo>(</mo><msub><mi>&omega;</mi><mi>k</mi></msub><mo>)</mo></mrow><mo>=</mo><msub><mfencedopen='['close=']'><mtable><mtr><mtd><mi>cos</mi><mrow><mo>(</mo><mn>0.5</mn><mo>|</mo><mo>|</mo><msub><mi>&omega;</mi><mi>k</mi></msub><mo>|</mo><mo>|</mo><mi>&Delta;t</mi><mo>)</mo></mrow><msub><mi>I</mi><mrow><mn>3</mn><mo>&times;</mo><mn>3</mn></mrow></msub><mo>-</mo><mo>[</mo><msub><mi>&psi;</mi><mi>k</mi></msub><mo>&times;</mo><mo>]</mo></mtd><mtd><msub><mi>&psi;</mi><mi>k</mi></msub></mtd></mtr><mtr><mtd><mo>-</mo><msubsup><mi>&psi;</mi><mi>k</mi><mi>T</mi></msubsup></mtd><mtd><mi>cos</mi><mrow><mo>(</mo><mn>0.5</mn><mo>|</mo><mo>|</mo><msub><mi>&omega;</mi><mi>k</mi></msub><mo>|</mo><mo>|</mo><mi>&Delta;t</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mrow><mn>4</mn><mo>&times;</mo><mn>4</mn></mrow></msub></mrow>式中，I3×3为3×3的单位阵；[ψk×]为ψk的反对称矩阵；b.陀螺数学模型βk+1＝βk+ηu，k式中，为陀螺的测量输出值；ωk为真实的姿态角速度；βk和βk+1分别为第k时刻和第k+1时刻的陀螺漂移；ηv，k与ηu，k是互不相关的零均值高斯白噪声，ηv，k和ηu，k的方差分别为σv2与σu2；c.矢量观测模型bk＝A(qk)rk+δbk式中，bk为观测矢量；rk为参考矢量；δbk为测量噪声，且δbk服从概率分布A(qk)为参考坐标系到卫星本体坐标系的姿态转移矩阵；(2)基于步骤(1)建立的系统状态空间模型，进行滤波初始化滤波初始化要基于步骤(1)建立的系统状态空间模型，包括ESOQPF主滤波器初始化和UKF从滤波器初始化两部分。ESOQPF主滤波器的初始化主要是初始化N个粒子及粒子权值；UKF从滤波器的初始化是对陀螺漂移估计值和估计误差方差阵进行初始化；(3)以姿态四元数为状态变量，基于步骤(1)中的四元数运动学方程和矢量观测模型建立主滤波器的状态空间模型，利用ESOQPF主滤波器估计第k时刻的姿态四元数此外，主滤波器估计第k时刻姿态四元数时，要利用从滤波器第k1时刻估计的陀螺漂移(4)四元数转换为姿态角根据四元数与姿态角之间的关系，将步骤(3)估计出的姿态四元数转换为姿态四元数对应的姿态角横滚角俯仰角θk，偏航角ψk；(5)以陀螺漂移为状态变量，基于步骤(1)中的陀螺漂移数学模型和矢量观测模型建立从滤波器的状态空间模型，利用UKF从滤波器估计第k时刻的陀螺漂移此外，从滤波器估计第k时刻陀螺漂移时，要利用主滤波器第k1时刻估计的姿态四元数(6)当有新的观测值时，重复步骤(3)～(5)进行姿态确定，得到姿态角估计值和陀螺漂移估计值。FSA00000270286200011.tif,FSA00000270286200012.tif,FSA00000270286200014.tif,FSA00000270286200015.tif,FSA00000270286200016.tif,FSA00000270286200017.tif,FSA00000270286200018.tif,FSA00000270286200021.tif,FSA00000270286200022.tif,FSA00000270286200023.tif,FSA00000270286200024.tif,FSA00000270286200025.tif,FSA00000270286200026.tif,FSA00000270286200027.tif,FSA00000270286200028.tif2.根据权利要求1所述的一种基于ES0QPF和UKF主从滤波的微小卫星姿态确定方法，其特征在于所述步骤(3)中ES0QPF主滤波器将QPF算法与ES0Q2算法相结合来估计姿态四元数；ES0QPF主滤波器首先利用QPF算法计算出Davenport矩阵，然后利用ES0Q2算法直接计算出该矩阵的最大特征值，最后计算出最大特征值对应的特征向量，即姿态四元数；具体实现步骤如下(1)QPF算法计算Davenport矩阵K利用第k-1时刻的四元数粒子Qh(i)、归一化的粒子权值巧乂/)、陀螺漂移估计值&q和第k时刻的观测值bk计算Davenport矩阵K；a.时间更新根据姿态四元数运动学方程，由第k-1时刻的四元数粒子qk—Ji)求各粒子第k时刻的预测值知i=l,...,N-,b.测量更新测量更新即粒子权值更新，利用第k-1时刻的归一化的粒子权值和第k时刻的观测值bk计算第k时刻的各粒子权值Wk(i)为■xPbih队^HW^ii)=Psbk(bk-A{qk^{i))rkW^(i)式中，i=1，...，N;A▲汍1%-,(0)为么丨w(0的似然概率；Pshk(bk-A{qk]k_{{i))rk)为氏-成的概率；将上式各粒子权值Wk(i)归一化为Wk{i)=Wk(i)/fjWk{i),i^\,...,N/；=1c.计算Davenport矩阵KKABk+BkT-I^Xx{Bk)z‘其中，(么M(/))/=1'Bk{3,2)-Bk(2,3)战(1,3)-代(3,1)Bk(2,l)-Bk(l,2)_式中，tr(Bk)为矩阵Bk的迹；(2)ES0Q2算法估计姿态四元数么a.求矩阵K的最大特征值入.；lmax=去(^2^/d-b+式中，b=-2tr(Bk)+tr(adj(Bk+BkT))-zTz；d=det(K)；adj(Bk+BkT)为矩阵(Bk+BkT)的伴随矩阵；tr(adj(Bk+BkT))为矩阵adj(Bk+BkT)的迹；det(K)为矩阵K的行列式；b.求姿态四元数估计值也矩阵K最大特征值对应的特征向量么为全文摘要本发明涉及一种基于ESOQPF和UKF主从滤波的微小卫星姿态确定方法。首先建立微小卫星系统状态空间模型，进行姿态确定滤波初始化；然后以姿态四元数为状态变量，将ESOQPF作为主滤波器估计姿态四元数，将估计出来的四元数转换为相应的姿态角；以陀螺漂移为状态变量，将UKF作为从滤波器估计陀螺漂移。本发明在保证姿态估计精度的同时，从两个方面降低了计算量，缩短了定姿过程。一方面，利用ESOQPF和UKF进行主从滤波，即将姿态四元数与陀螺漂移分开估计；另一方面，ESOQPF估计姿态四元数时将QPF算法与ESOQ2算法相结合，利用QPF算法计算出Davenport矩阵，然后利用ESOQ2算法直接计算出该矩阵最大特征值所对应的特征向量。该方法适用于有陀螺配置模式下，基于矢量观测的微小卫星姿态确定。文档编号G01C1/00GK101982732SQ201010281990公开日2011年3月2日申请日期2010年9月14日优先权日2010年9月14日发明者全伟,崔培玲,张会娟,房建成,郭雷申请人:北京航空航天大学