一种星敏感器与高频角位移传感器组合定姿方法及系统的制作方法
一种星敏感器与高频角位移传感器组合定姿方法及系统的制作方法
【技术领域】
[0001] 本发明属于遥感卫星地面预处理领域，特别是设及到一种星敏感器与高频角位移 传感器组合定姿方法及系统。
【背景技术】
[0002] 高分辨率光学卫星是我国高分辨率对地观测系统发展的重要研究领域之一，未来 十年我国将发射数颗空间分辨率优于1米的光学卫星（简称亚米级光学卫星），最高空间分 辨率甚至可W达到0.1米左右。但是，更高的空间分辨率并不意味着具有更高的几何相对精 度。由于高分辨率光学卫星推扫成像频率为上万赫兹，而传统的星敏感器与巧螺组合定姿 方式输出的姿态频率为几赫兹到几十赫兹，很难满足其高精度几何处理需求。本发明针对 该问题，提出一种低频星敏感器与高频角位移传感器组合定姿方法。近年来，角位移传感器 作为一种新型姿态敏感器逐渐搭载在一系列高分辨率光学卫星上进行在轨使用，由于其可 W输出上万赫兹的角增量姿态数据，将其与低频星敏感器进行最优信息融合可W得到高频 姿态数据，W满足高分辨率光学影像高精度几何处理要求。
【发明内容】
[0003] 本发明针对当前低频姿态数据难W满足高分辨率光学卫星影像几何处理要求的 问题，提供了一种基于低频星敏感器与高频角位移传感器高精度组合定姿技术方案。
[0004] 本发明提供的技术方案为一种星敏感器与高频角位移传感器组合定姿方法，包括 W下步骤：
[000引步骤1，构建角位移传感器测量模型如下，
[0006] Q g二(1+ Δ + 八)Q +b巧g
[0007] 其中，og为角位移传感器测量得到的角速度大小，Δ表示角位移传感器安装误 差，A表示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表 示角位移传感器漂移大小，％为角位移传感器测量噪声；
[0008] 步骤2,基于多星敏感器信息融合结果构建系统量测方程，实现如下，
[0009] 设有一组正交矢量，皮祭请表示正交矢量在本体坐标系下真实值，也，底，4表示正 交矢量在惯性坐标系下的测量值，表示正交矢量在惯性坐标系下真实值；
[0010] 得至慢ii方程如下，
[0013] 其中，Zk表示观测矢量，化表示观测矩阵，Xk表示状态变量，這表示四元数最优估计 值，4(斯表示由本体到惯性系转换矩阵，Vk表示tk时刻观测噪声序列；
[0014]步骤3,根据卫星姿态运动学方程和角位移传感器测量模型构建系统状态方程，实 现如下，
[001引根据姿态运动学方程得到基于状态=?Δ^ ΔΑ7的系统状态方程如下，
[0019] 其中，义找)表示状态量微分值，X(t)表示状态量，F(t)表示状态转移矩阵微分，W (t)表示系统噪声序列，[心<]表示角速度矢量叉乘，13X3表示单位矩阵，03X3表示元素为0的 矩阵，Δ矿'表示误差四元数矢量部分，Δ护表示误差漂移；
[0020] 对W上系统状态方程进行线性化离散化得到，
[0021] Xk=巫 k,k-戍 k-i+Tk-iWk-i
[002引其中，Xk、Xk-i表示tk时刻、tk-i时刻状态变量，&k,k-i表示状态转移矩阵，Tk-i表示 系统噪声驱动阵，Wk-i表示系统噪声序列；
[0023] 步骤4,基于步骤2和步骤3分别构建的系统量测方程和系统状态方程，采用双向卡 尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。
[0024] 而且，步骤3中姿态运动学方程表示如下姿态运动学方程表示如下，
[0027] 其中，f表示四元数的微分值，q和ω分别表示卫星本体坐标系相对于惯性坐标系 的姿态四元数与Ξ轴旋转角速度，q=[qo qi Q2 93]τ，ω=(ωχ，ωγ，ωζ)。
[002引而且，步骤4实现如下，
[0029] (1)基于角位移测量信息进行姿态预测
[0030] 基于角位移测量信息在tk-i时刻对下式进行积分，得到卫星姿态四元数预测值 9kik-\，
[0031]
[00创其中，義_1表示tk-1时刻四元数最优估计值，缉_:1表示tk-1时刻角速度最优估计值； [0033]角位移漂移预测值4,的预测模型如下，
[0034]
[00巧]其中，表示tk-1时刻角位移漂移最优估计值；
[0036] 对于误差协方差阵的预测值?,,解算模型如下，
[0037]
[0038] 其中，表示tk-i时刻误差协方差阵估计值，Qk-i表示系统干扰方差阵；
[0039] (2)基于星敏感器测量值进行姿态修正
[0040] 在tk时刻，根据系统量测方程计算观测矩阵化，进一步计算滤波增益Kk，
[0041]
[0042] 其中，Pk/k-i表示误差协方差阵预测值，则相应的系统状态变量1,为，
[0043]
[0044] 卫星姿态四元数tk时刻的修正更新值糸与角位移漂移的修正更新值4分别为，
[0045]
[0046]
[0047] 其中，4么表示误差四元数估计值，么|4^表示误差漂移量估计值；
[0048] 误差协方差阵Pk的更新计算为，
[0049]
[0050] 其中，化表示星敏感器量测噪声方差阵，I表示单位矩阵，化采用修正更新后的状态 变量《^重新进行计算；
[0051] 根据上述过程（1)(2)进行初始滤波、反向滤波W及正向滤波，最终基于正向滤波 与反向滤波结果进行整体加权平差。
[0052] 本发明还提供一种星敏感器与高频角位移传感器组合定姿系统，包括W下模块：
[0053] 第一模块，用于构建角位移传感器测量模型如下，
[0054] 〇 g= (1+ Δ + 八）〇 +b~i~rig
[005引其中，wg为角位移传感器测量得到的角速度大小，Δ表示角位移传感器安装误 差，A表示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表 示角位移传感器漂移大小，％为角位移传感器测量噪声；
[0056] 第二模块，用于基于多星敏感器信息融合结果构建系统量测方程，实现如下，
[0057] 设有一组正交矢量，C，4:，C表示正交矢量在本体坐标系下真实值，4，/,;,，攻表示 正交矢量在惯性坐标系下的测量值，表示正交矢量在惯性坐标系下真实值；
[0058] 得到量测方程如下，
[0059] Zk = HkXk+Vk
[0060]
[0061] 其中，Zk表示观测矢量，化表示观测矩阵，Xk表示状态变量，?表示四元数最优估计 值，4(如表示由本体到惯性系转换矩阵，Vk表示tk时刻观测噪声序列；
[0062] 第Ξ模块，用于根据卫星姿态运动学方程和角位移传感器测量模型构建系统状态 方程，实现如下，
[0063] 根据姿态运动学方程得到基于状态Xm =[Δ3" ΔΖ/]τ的系统状态方程如下，
[0067] 其中，乂巧表示状态量微分值，X(t)表示状态量，F(t)表示状态转移矩阵微分，W (t)表示系统噪声序列，[?χ]表示角速度矢量叉乘，13X3表示单位矩阵，〇3X3表示元素为0的 矩阵，Δ矿"表示误差四元数矢量部分，Δ护表示误差漂移；
[0068] 对W上系统状态方程进行线性化离散化得到，
[0069] Xk=巫 k,k-戍k-i+ 厂 k-iWk-i
[0070] 其中，Xk、Xk-i表示tk时刻、tk-1时刻状态变量，&k,k-i表示状态转移矩阵，Tk-1表示 系统噪声驱动阵，Wk-i表示系统噪声序列；
[0071] 第四模块，用于基于第二模块和第Ξ模块分别构建的系统量测方程和系统状态方 程，采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。
[0072] 而且，第Ξ模块中姿态运动学方程表示如下姿态运动学方程表示如下，
[0075]其中，每表示四元数的微分值，q和ω分别表示卫星本体坐标系相对于惯性坐标系 的婪态四兀数与二轴旋转角速度，q二[qo qi Q2 q3]T,c〇=(c〇x,c〇y,c〇z)。
[0076]而且，第四模块实现如下，
[0077] (1)基于角位移测量信息进行姿态预测
[0078] 基于角位移测量信息在tk-i时刻对下式进行积分，得到卫星姿态四元数预测值 斯化-v'
[0079]
[0080] 其中，如_1表示tk-i时刻四元数最优估计值，毎_1表示tk-i时刻角速度最优估计值；
[0081] 角位移漂移预测值4,W的预测模型如下，
[0082]
[008引其中，心1表示tk-拥刻角位移漂移最优估计值；
[0084] 对于误差协方差阵的预测值4,W解算模型如下，
[0085]
[0086] 其中，表示tk-i时刻误差协方差阵估计值，Qk-i表示系统干扰方差阵；
[0087] (2)基于星敏感器测量值进行姿态修正
[0088] 在tk时刻，根据系统量测方程计算观测矩阵化，进一步计算滤波增益Kk，
[0089]
[0090] 其中，Pk/k-i表示误差协方差阵预测值，则相应的系统状态变量i;为，
[0091]
[0092] 卫星姿态四元数tk时刻的修正更新值矣与角位移漂移的修正更新值4分别为，
[0095] 其中，Δ矣表示误差四元数估计值，表示误差漂移量估计值；
[0096] 误差 协方差阵Pk的更新计算为，
[0097]
[0098] 其中，化表示星敏感器量测噪声方差阵，I表示单位矩阵，化采用修正更新后的状态 变量重新进行计算；
[0099] 根据上述过程（1)(2)进行初始滤波、反向滤波W及正向滤波，最终基于正向滤波 与反向滤波结果进行整体加权平差。
[0100] 本发明提供了一种低频星敏感器与高频角位移传感器组合定姿技术方案，实现了 星敏感器高精度定姿优势与角位移传感器高频率角增量输出优势互补，通过本发明提供的 技术方案可W得到高频率、高精度的卫星姿态数据，为高分辨率光学卫星高精度几何处理 奠定基础。
【附图说明】
[0101] 图1为本发明实施例的流程图。
【具体实施方式】
[0102] W下结合附图和实施例详细说明本发明技术方案。
[0103] 参见图1所示星敏感器与高频角位移传感器组合定姿技术流程图，W下针对实施 例流程中的各步骤，对本发明方法做进一步详细描述。
[0104] 步骤1，根据角位移传感器工作原理，构建合理的误差测量模型，充分考虑其测量 值中存在的各种误差源。
[0105] 角位移传感器作为一种惯性姿态传感器，可W直接输出高频的角增量数据，得到 的测量数据含义是该采样时刻相比上一采样时刻时间间隔内所产生的角增量大小。根据上 述原理得到:假设角位移传感器数据采样周期为T，某一时刻t的卫星Ξ轴角位移观测值分 另IJ为Nx，Ny，Nz，则得到该时刻t的ミ轴角速度ωχ，ωy，ωz，具体展开如下：
[0106]
(1)
[0107]由于角位移传感器在轨工作过程中存在的误差项包括安装误差、标度因数误差、 漂移误差W及测量噪声等，故进一步得到角位移传感器的误差测量模型如下：
[010引 0g= (1+Δ + 八）0+b~i~rig (2)
[0109] 其中，Qg为角位移传感器测量得到的角速度大小，Δ表示角位移传感器安装误 差，A表示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表 示角位移传感器漂移大小，屯为角位移传感器测量噪声。运里假设角位移传感器测量噪声 为高斯白噪声，即和，〇g表示测量噪声中误差。
[0110] 进一步假设角位移传感器的漂移b由白噪声驱动，表示为：
[0111] 屋=呢 （3)
[0112] 其中，?表示漂移量的微分值，rU为角位移传感器与时间相关的噪声，满足 布~游典為），〇b表不漂移噪声中误差。
[0113] 步骤2,基于多星敏感器信息融合结果构建系统量测方程。
[0114] 本发明基于多星敏感器光轴矢量观测值实现多星敏感器最优信息融合，进一步构 建系统量测方程，具体过程如下：
[0115] 假设一组正交矢量在本体坐标系、惯性坐标系下表达方式如下：
[0116]
(4)
[0117] 其中，/:，42，省表示正交矢量在本体坐标系下真实值，4乂，,，/1表示正交矢量在惯性 坐标系下的测量值，/U2,护表示正交矢量在惯性坐标系下真实值。
[0118] 进一步根据推导得到量测方程如下：
[0119] Zk = HkXk+Vk (5)
[0121] 其中，Zk表示观测矢量，化表示观测矩阵，Xk表示状态变量，f表示四元数最优估计 值，4(如表示由本体到惯性系转换矩阵，Vk表示tk时刻观测噪声序列，且满足：
[0120]
E(Vk)表示观测噪声序列平均值，表示不同时刻之间观测噪声序 列协方差，Ff表示t扣寸刻观测噪声矩阵转置，Sw表示狄克莱函数;化表示星敏感器量测噪声 方差阵。
[0122] 步骤3,基于姿态运动学方程W及角位移传感器误差测量模型构建系统的状态方 程。
[0123] 本发明根据姿态运动学方程W及角位移传感器测量模型进一步构建状态方程，具 体过程如下：
[0124] 姿态运动学方程表示如下：
[0127] 其中，f表示四元数的微分值，q和ω分别表示卫星本体坐标系相对于惯性坐标系 的姿态四元数与Ξ轴旋转角速度，q=[qo qi Q2 93]τ，ω=(ωχ，ωγ，ωζ)。
[012引进一步推导得到基于状态JTw二[Δ5' Δ,ν' |''的系统状态方程：
[0129]
(巧
[0130] 其中，乂的表示状态量微分值，X(t)表示状态量，F(t)表示状态转移矩阵微分，W (t)表示系统噪声序列，悼X]表示角速度矢量叉乘，13X3表示单位矩阵，03X3表示元素为0的 矩阵，Δ矿'表示误差四元数矢量部分，AbT表示误差漂移，W上得到的状态方程为连续动态 系统滤波方程，需要进行线性化和离散化，故进行线性化离散化得到：
[01 引]Xk=巫 k,k-戍 k-i+Tk-iWk-i (8)
[01创其中，Xk、Xk-i表示tk时刻、tk-i时刻状态变量，Φk,k-i表示状态转移矩阵，Γ k-i表示 系统噪声驱动阵，Wk-i表示系统噪声序列。
[0133] 步骤4,基于步骤2和步骤3分别构建的系统量测方程和系统状态方程，采用双向卡 尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。
[0134] 由于星敏感器输出频率为几赫兹到几十赫兹，而角位移传感器输出频率达到上万 赫兹，W往的组合定姿滤波器设计已经无法满足实际需要。本发明针对该问题提出一种双 向卡尔曼滤波与整体加权平差相结合的模型算法实现多频多种姿态敏感器最优信息融合。
[0135] (1)基于角位移测量信息进行姿态预测
[0136] 基于角位移测量信息在tk-i时刻对下式进行积分，可W得到卫星姿态四元数预测 值^5用-Γ〇
[0137]
(9)
[0138] 其中，如1表示tk-i时刻四元数最优估计值，勾-_1表示tk-i时刻角速度最优估计值。
[0139] 角位移漂移预测值的预测模型如下：
[0140]
(!0)
[0141] 其中，表示tk-i时刻角位移漂移最优估计值。
[0142] 对于误差协方差阵的预测值含W解算模型如下：
[0143]
(II)
[0144] 其中，,表示tk-i时刻误差协方差阵估计值，Qk-i表示系统干扰方差阵。
[0145] (2)基于星敏感器测量值进行姿态修正
[0146] 在tk时刻，根据步骤2量测方程计算观测矩阵化。进一步计算滤波增益Kk:
[0147]
(12)
[014引其中，Pk/k-i表示误差协方差阵预测值，则相应的系统状态变量玄，为：
[0149]
(13)
[0150] 卫星姿态四元数tk时刻的修正更新值么与角位移漂移的修正更新值&分别为：
[0151]
(14)：
[0152] 其中，表示误差四元数估计值，Δ&表示误差漂移量估计值。
[0153] 误差协方差阵Pk的更新计算为：
[0154]
(1句
[0155] 其中，化表示星敏感器量测噪声方差阵，I表示单位矩阵，化采用修正更新后的状态 变量重新进行计算，运样做的目的是使滤波过程具有更好的收敛性。在校正了卫星姿态 四元数和角位移的漂移后，状态变量需要重新设置为零。
[0156] 根据上述过程进行初始滤波、反向滤波W及正向滤波，（1)到（2)为初次滤波；（2) 至|J(1)为反向滤波;最后（1)到（2)为正向滤波，每次滤波的初始值为上次滤波的结果;最终 基于正向滤波与反向滤波结果进行整体加权平差。
[0157]
[0158] 根符号吁"表示正向滤波结果，根符号"b"表示反向滤波结果，根符号V'表示整体 加权平差结果，
[0159] 4,(&)表示tk时刻正向滤波姿态最优估计值，如表示tk时刻反向滤波姿态最优估 计值兔斯的逆，若^(巧表示tk时刻正反向滤波结果四元数误差；:#*斯=[知。知如。私《3了 ；
[0160] 表示正反向状态量滤波结果误差矩阵，sgn巧J6。)表示返回标量正负号， 炸）、A,,，作):表示tk时刻漂移正向与反向滤波最优估计值，
[0161] 与--I倘与為脚分别表示误差协方差阵正反向最优估计值的逆，卸A)表示整理 加权的误差协方差阵估计值；
[016引 觸表示正反向状态量滤波结果误差矩阵加权平均值，Δ表'W表示误差四元 数整体加权平均矢量部分结果，Δ&' (/0表示误差漂移整体加权平均结果；
[0163] 炸)表示误差四元数整体加权结果，Δ& (A)表示误差漂移整体加权结果；
[0164] 表(&)表示tk时刻四元数整体加权结果，4,似=[Δ支。从）Δ朵/炸）Δ击/脚Δ朵/(坤， /;、(又-)表示tk时刻漂移量整体加权结果。
[0165] 具体实施时，本发明所提供方法可基于软件技术实现自动运行流程，也可采用模 块化方式实现相应系统。本发明实施例还提供一种星敏感器与高频角位移传感器组合定姿 系统，包括W下模块：
[0166] 第一模块，用于构建角位移传感器测量模型如下，
[0167] Q g二(1+Δ + 八)Q+b巧g
[0168] 其中，Og为角位移传感器测量得到的角速度大小，Δ表示角位移传感器安装误 差，A表示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表 示角位移传感器漂移大小，％为角位移传感器测量噪声；
[0169] 第二模块，用于基于多星敏感器信息融合结果构建系统量测方程，实现如下，
[0170] 设有一组正交矢量，c，42，c表示正交矢量在本体坐标系下真实值，4点，4表示正 交矢量在惯性坐标系下的测量值，杳，和/f表示正交矢量在惯性坐标系下真实值；
[0171]得到量测方程如下，
[0174] 其中，Zk表示观测矢量，化表示观测矩阵，Xk表示状态变量，f表示四元数最优估计 值，省巧)表示由本体到惯性系转换矩阵，Vk表示tk时刻观测噪声序列；
[0175] 第Ξ模块，用于根据卫星姿态运动学方程和角位移传感器测量模型构建系统状态 方程，实现如下，
[0176] 根据姿态运动学方程得到基于状态JTw叫Δ?7'' Δ/，'' Γ'的系统状态方程如下，
[0180] 其中，义的表示状态量微分值，X(t)表示状态量，F(t)表示状态转移矩阵微分，W (t)表示系统噪声序列，的叫表示角速度矢量叉乘，13X3表示单位矩阵，03X3表示元素为0的 矩阵，Δ沪表示误差四元数矢量部分，Δ bT表示误差漂移；
[0181 ]对W上系统状态方程进行线性化离散化得到，
[0182] Xk=巫 k,k-戍 k-i+Tk-iWk-i
[018引其中，Xk、Xk-i表示tk时刻、tk-i时刻状态变量，&k,k-i表示状态转移矩阵，Tk-i表示 系统噪声驱动阵，Wk-i表示系统噪声序列；
[0184] 第四模块，用于基于第二模块和第Ξ模块分别构建的系统量测方程和系统状态方 程，采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。
[0185] 各模块具体实现可参见相应步骤，本发明不予寶述。
[0186] 本文中所描述的具体实例仅仅是对本发明精神作举例说明。本发明所属技术领域 的技术人员可W对所描述的具体实例做各种各样的修改或补充或采用类似的方式替代，但 并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。
【主权项】
1. 一种星敏感器与高频角位移传感器组合定姿方法，其特征在于，包括以下步骤： 步骤1，构建角位移传感器测量模型如下，其中，ω g为角位移传感器测量得到的角速度大小，△表示角位移传感器安装误差，Λ表 示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表示角位移 传感器漂移大小，％为角位移传感器测量噪声； 步骤2,基于多星敏感器信息融合结果构建系统量测方程，实现如下， 设有一组正交矢量，表示正交矢量在本体坐标系下真实值，/!",&,4表示正交矢 量在惯性坐标系下的测量值，彳,匕/；5表示正交矢量在惯性坐标系下真实值； 得到量测方程如下，其中，Zk表示观测矢量，Hk表示观测矩阵，Xk表示状态变量，I表示四元数最优估计值， 4(d表示由本体到惯性系转换矩阵，Vk表示tk时刻观测噪声序列； 步骤3，根据卫星姿态运动学方程和角位移传感器测量模型构建系统状态方程，实现如 下， 根据姿态运动学方程得到基于状态的系统状态方程如下，ff(t) = [-0.5% nb]T 其中，表不状态量微分值，X(t)表不状态量，F(t)表不状态转移矩阵微分，W(t)表 示系统噪声序列，[6χ]表示角速度矢量叉乘，I3X3表示单位矩阵，O3X 3表示元素为O的矩阵， 表示误差四元数矢量部分，△『表示误差漂移； 对以上系统状态方程进行线性化离散化得到， Xk = ?k,k-lXk-l+ Γ k-lffk-1 其中，Xk、Xk-i表示tk时刻、tk-i时刻状态变量，Φ k,k-i表示状态转移矩阵，Γ I^1表示系统噪 声驱动阵，W1^1表示系统噪声序列； 步骤4,基于步骤2和步骤3分别构建的系统量测方程和系统状态方程，采用双向卡尔曼 滤波与整体加权平差模型，实现高频姿态参数最优估计。2. 根据权利要求1所述星敏感器与高频角位移传感器组合定姿方法，其特征在于:步骤 3中姿态运动学方程表示如下姿态运动学方程表示如下，其中，4表示四元数的微分值，q和ω分别表示卫星本体坐标系相对于惯性坐标系的姿 态四元数与三轴旋转角速度，q=[qo qi q2 q3]T，w=(cox，coy，coz)。3.根据权利要求1或2所述星敏感器与高频角位移传感器组合定姿方法，其特征在于： 步骤4实现如下， (1) 基于角位移测量信息进行姿态预测 基于角位移测量信息在时刻对下式进行积分，得到卫星姿态四元数预测值i.其中，IiM表示tk-l时刻四元数最优估计值，表示tk-l时刻角速度最优估计值； 角位移漂移预测值的预测模型如下，其中，表示tk-Ι时刻角位移漂移最优估计值； 对于误差协方差阵的预测值為解算模型如下，其中，表不tk-1时刻误差协方差阵估计值，仏-1表不系统干扰方差阵； (2) 基于星敏感器测量值进行姿态修正 在tk时刻，根据系统量测方程计算观测矩阵Hk，进一步计算滤波增益Kk，其中，Pk/k-i表示误差协方差阵预测值，则相应的系统状态变量为，卫星姿态四元数tk时刻的修正更新值么与角位移漂移的修正更新值&分别为，其中，Δ#,表示误差四元数估计值，表示误差漂移量估计值； 误差协方差阵Pk的更新计算为，其中，Rk表示星敏感器量测噪声方差阵，I表示单位矩阵，Hk采用修正更新后的状态变量 14重新进行计算； 根据上述过程（1)(2)进行初始滤波、反向滤波以及正向滤波，最终基于正向滤波与反 向滤波结果进行整体加权平差。4. 一种星敏感器与高频角位移传感器组合定姿系统，其特征在于，包括以下模块： 第一模块，用于构建角位移传感器测量模型如下， og=(l+A + A ) 〇 +b+Hg 其中，ω g为角位移传感器测量得到的角速度大小，△表示角位移传感器安装误差，Λ表 示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表示角位移 传感器漂移大小，％为角位移传感器测量噪声； 第二模块，用于基于多星敏感器信息融合结果构建系统量测方程，实现如下， 设有一组正交矢量，表示正交矢量在本体坐标系下真实值，表示正交矢 量在惯性坐标系下的测量值，/,1Jf表示正交矢量在惯性坐标系下真实值； 得到量测方程如下，其中，Zk表示观测矢量，Hk表示观测矩阵，Xk表示状态变量，i表示四元数最优估计值， 4(1)表示由本体到惯性系转换矩阵，Vk表示tk时刻观测噪声序列； 第三模块，用于根据卫星姿态运动学方程和角位移传感器测量模型构建系统状态方 程，实现如下， 根据姿态运动学方程得到基于状态的系统状态方程如下，ff(t) = [-0.5% nb]T 其中，乂(〇表不状态量微分值，X(t)表不状态量，F(t)表不状态转移矩阵微分，W(t)表 示系统噪声序列，[洽X]表示角速度矢量叉乘，I3X3表示单位矩阵，O3X 3表示元素为〇的矩阵， Δ-/表示误差四元数矢量部分，△以表示误差漂移； 对以上系统状态方程进行线性化离散化得到， Xk = ?k,k-lXk-l+ Γ k-lffk-1 其中，Xk、Xk-i表示tk时刻、tk-i时刻状态变量，Φ k,k-i表示状态转移矩阵，Γ I^1表示系统噪 声驱动阵，W1^1表示系统噪声序列； 第四模块，用于基于第二模块和第三模块分别构建的系统量测方程和系统状态方程， 采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。5. 根据权利要求4所述星敏感器与高频角位移传感器组合定姿方法，其特征在于:第三 模块中姿态运动学方程表示如下姿态运动学方程表示如下，其中，4表示四元数的微分值，q和ω分别表示卫星本体坐标系相对于惯性坐标系的姿 态四元数与三轴旋转角速度，6. 根据权利要求4或5所述星敏感器与高频角位移传感器组合定姿方法，其特征在于： 第四模块实现如下， (1) 基于角位移测量信息进行姿态预测 基于角位移测量信息在时刻对下式进行积分，得到卫星姿态四元数预测值，其中，I表示tk-i时刻四元数最优估计值，表示tk-l时刻角速度最优估计值； 角位移漂移预测值的预测模型如下，其中，表示tk-l时刻角位移漂移最优估计值； 对于误差协方差阵的预测值解算模型如下，其中，时刻误差协方差阵估计值，Qk-i表不系统干扰方差阵； (2) 基于星敏感器测量值进行姿态修正 在tk时刻，根据系统量测方程计算观测矩阵Hk，进一步计算滤波增益Kk，其中，Pk/k-i表示误差协方差阵预测值，则相应的系统状态变量为，卫星姿态四元数tk时刻的修正更新值A与角位移漂移的修正更新值&分别为，其中，表示误差四元数估计值，表示误差漂移量估计值； 误差协方差阵Pk的更新计算为，其中，Rk表示星敏感器量测噪声方差阵，I表示单位矩阵，Hk采用修正更新后的状态变量 乂4重新进行计算； 根据上述过程（1)(2)进行初始滤波、反向滤波以及正向滤波，最终基于正向滤波与反 向滤波结果进行整体加权平差。
【专利摘要】本发明提供了一种星敏感器与高频角位移传感器组合定姿方法及系统，包括根据角位移传感器测量原理构建其误差测量模型，充分考虑其观测值存在的各种误差源；基于多星敏感器信息融合结果构建系统量测方程；根据卫星姿态运动学方程和角位移传感器测量模型构建系统状态方程；最后采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。本发明可以实现低频星敏感器与高频角位移传感器最优信息融合，得到高频高精度的姿态数据，从而为高分辨率光学影像高精度几何处理奠定基础。
【IPC分类】G01C21/16, G01C25/00, G01C21/20
【公开号】CN105486312
【申请号】CN201610067447
【发明人】王密, 范城城, 李德仁
【申请人】武汉大学
【公开日】2016年4月13日
【申请日】2016年1月30日
【技术领域】
[0001] 本发明属于遥感卫星地面预处理领域，特别是设及到一种星敏感器与高频角位移 传感器组合定姿方法及系统。
【背景技术】
[0002] 高分辨率光学卫星是我国高分辨率对地观测系统发展的重要研究领域之一，未来 十年我国将发射数颗空间分辨率优于1米的光学卫星（简称亚米级光学卫星），最高空间分 辨率甚至可W达到0.1米左右。但是，更高的空间分辨率并不意味着具有更高的几何相对精 度。由于高分辨率光学卫星推扫成像频率为上万赫兹，而传统的星敏感器与巧螺组合定姿 方式输出的姿态频率为几赫兹到几十赫兹，很难满足其高精度几何处理需求。本发明针对 该问题，提出一种低频星敏感器与高频角位移传感器组合定姿方法。近年来，角位移传感器 作为一种新型姿态敏感器逐渐搭载在一系列高分辨率光学卫星上进行在轨使用，由于其可 W输出上万赫兹的角增量姿态数据，将其与低频星敏感器进行最优信息融合可W得到高频 姿态数据，W满足高分辨率光学影像高精度几何处理要求。
【发明内容】
[0003] 本发明针对当前低频姿态数据难W满足高分辨率光学卫星影像几何处理要求的 问题，提供了一种基于低频星敏感器与高频角位移传感器高精度组合定姿技术方案。
[0004] 本发明提供的技术方案为一种星敏感器与高频角位移传感器组合定姿方法，包括 W下步骤：
[000引步骤1，构建角位移传感器测量模型如下，
[0006] Q g二(1+ Δ + 八)Q +b巧g
[0007] 其中，og为角位移传感器测量得到的角速度大小，Δ表示角位移传感器安装误 差，A表示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表 示角位移传感器漂移大小，％为角位移传感器测量噪声；
[0008] 步骤2,基于多星敏感器信息融合结果构建系统量测方程，实现如下，
[0009] 设有一组正交矢量，皮祭请表示正交矢量在本体坐标系下真实值，也，底，4表示正 交矢量在惯性坐标系下的测量值，表示正交矢量在惯性坐标系下真实值；
[0010] 得至慢ii方程如下，
[0013] 其中，Zk表示观测矢量，化表示观测矩阵，Xk表示状态变量，這表示四元数最优估计 值，4(斯表示由本体到惯性系转换矩阵，Vk表示tk时刻观测噪声序列；
[0014]步骤3,根据卫星姿态运动学方程和角位移传感器测量模型构建系统状态方程，实 现如下，
[001引根据姿态运动学方程得到基于状态=?Δ^ ΔΑ7的系统状态方程如下，
[0019] 其中，义找)表示状态量微分值，X(t)表示状态量，F(t)表示状态转移矩阵微分，W (t)表示系统噪声序列，[心<]表示角速度矢量叉乘，13X3表示单位矩阵，03X3表示元素为0的 矩阵，Δ矿'表示误差四元数矢量部分，Δ护表示误差漂移；
[0020] 对W上系统状态方程进行线性化离散化得到，
[0021] Xk=巫 k,k-戍 k-i+Tk-iWk-i
[002引其中，Xk、Xk-i表示tk时刻、tk-i时刻状态变量，&k,k-i表示状态转移矩阵，Tk-i表示 系统噪声驱动阵，Wk-i表示系统噪声序列；
[0023] 步骤4,基于步骤2和步骤3分别构建的系统量测方程和系统状态方程，采用双向卡 尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。
[0024] 而且，步骤3中姿态运动学方程表示如下姿态运动学方程表示如下，
[0027] 其中，f表示四元数的微分值，q和ω分别表示卫星本体坐标系相对于惯性坐标系 的姿态四元数与Ξ轴旋转角速度，q=[qo qi Q2 93]τ，ω=(ωχ，ωγ，ωζ)。
[002引而且，步骤4实现如下，
[0029] (1)基于角位移测量信息进行姿态预测
[0030] 基于角位移测量信息在tk-i时刻对下式进行积分，得到卫星姿态四元数预测值 9kik-\，
[0031]
[00创其中，義_1表示tk-1时刻四元数最优估计值，缉_:1表示tk-1时刻角速度最优估计值； [0033]角位移漂移预测值4,的预测模型如下，
[0034]
[00巧]其中，表示tk-1时刻角位移漂移最优估计值；
[0036] 对于误差协方差阵的预测值?,,解算模型如下，
[0037]
[0038] 其中，表示tk-i时刻误差协方差阵估计值，Qk-i表示系统干扰方差阵；
[0039] (2)基于星敏感器测量值进行姿态修正
[0040] 在tk时刻，根据系统量测方程计算观测矩阵化，进一步计算滤波增益Kk，
[0041]
[0042] 其中，Pk/k-i表示误差协方差阵预测值，则相应的系统状态变量1,为，
[0043]
[0044] 卫星姿态四元数tk时刻的修正更新值糸与角位移漂移的修正更新值4分别为，
[0045]
[0046]
[0047] 其中，4么表示误差四元数估计值，么|4^表示误差漂移量估计值；
[0048] 误差协方差阵Pk的更新计算为，
[0049]
[0050] 其中，化表示星敏感器量测噪声方差阵，I表示单位矩阵，化采用修正更新后的状态 变量《^重新进行计算；
[0051] 根据上述过程（1)(2)进行初始滤波、反向滤波W及正向滤波，最终基于正向滤波 与反向滤波结果进行整体加权平差。
[0052] 本发明还提供一种星敏感器与高频角位移传感器组合定姿系统，包括W下模块：
[0053] 第一模块，用于构建角位移传感器测量模型如下，
[0054] 〇 g= (1+ Δ + 八）〇 +b~i~rig
[005引其中，wg为角位移传感器测量得到的角速度大小，Δ表示角位移传感器安装误 差，A表示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表 示角位移传感器漂移大小，％为角位移传感器测量噪声；
[0056] 第二模块，用于基于多星敏感器信息融合结果构建系统量测方程，实现如下，
[0057] 设有一组正交矢量，C，4:，C表示正交矢量在本体坐标系下真实值，4，/,;,，攻表示 正交矢量在惯性坐标系下的测量值，表示正交矢量在惯性坐标系下真实值；
[0058] 得到量测方程如下，
[0059] Zk = HkXk+Vk
[0060]
[0061] 其中，Zk表示观测矢量，化表示观测矩阵，Xk表示状态变量，?表示四元数最优估计 值，4(如表示由本体到惯性系转换矩阵，Vk表示tk时刻观测噪声序列；
[0062] 第Ξ模块，用于根据卫星姿态运动学方程和角位移传感器测量模型构建系统状态 方程，实现如下，
[0063] 根据姿态运动学方程得到基于状态Xm =[Δ3" ΔΖ/]τ的系统状态方程如下，
[0067] 其中，乂巧表示状态量微分值，X(t)表示状态量，F(t)表示状态转移矩阵微分，W (t)表示系统噪声序列，[?χ]表示角速度矢量叉乘，13X3表示单位矩阵，〇3X3表示元素为0的 矩阵，Δ矿"表示误差四元数矢量部分，Δ护表示误差漂移；
[0068] 对W上系统状态方程进行线性化离散化得到，
[0069] Xk=巫 k,k-戍k-i+ 厂 k-iWk-i
[0070] 其中，Xk、Xk-i表示tk时刻、tk-1时刻状态变量，&k,k-i表示状态转移矩阵，Tk-1表示 系统噪声驱动阵，Wk-i表示系统噪声序列；
[0071] 第四模块，用于基于第二模块和第Ξ模块分别构建的系统量测方程和系统状态方 程，采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。
[0072] 而且，第Ξ模块中姿态运动学方程表示如下姿态运动学方程表示如下，
[0075]其中，每表示四元数的微分值，q和ω分别表示卫星本体坐标系相对于惯性坐标系 的婪态四兀数与二轴旋转角速度，q二[qo qi Q2 q3]T,c〇=(c〇x,c〇y,c〇z)。
[0076]而且，第四模块实现如下，
[0077] (1)基于角位移测量信息进行姿态预测
[0078] 基于角位移测量信息在tk-i时刻对下式进行积分，得到卫星姿态四元数预测值 斯化-v'
[0079]
[0080] 其中，如_1表示tk-i时刻四元数最优估计值，毎_1表示tk-i时刻角速度最优估计值；
[0081] 角位移漂移预测值4,W的预测模型如下，
[0082]
[008引其中，心1表示tk-拥刻角位移漂移最优估计值；
[0084] 对于误差协方差阵的预测值4,W解算模型如下，
[0085]
[0086] 其中，表示tk-i时刻误差协方差阵估计值，Qk-i表示系统干扰方差阵；
[0087] (2)基于星敏感器测量值进行姿态修正
[0088] 在tk时刻，根据系统量测方程计算观测矩阵化，进一步计算滤波增益Kk，
[0089]
[0090] 其中，Pk/k-i表示误差协方差阵预测值，则相应的系统状态变量i;为，
[0091]
[0092] 卫星姿态四元数tk时刻的修正更新值矣与角位移漂移的修正更新值4分别为，
[0095] 其中，Δ矣表示误差四元数估计值，表示误差漂移量估计值；
[0096] 误差 协方差阵Pk的更新计算为，
[0097]
[0098] 其中，化表示星敏感器量测噪声方差阵，I表示单位矩阵，化采用修正更新后的状态 变量重新进行计算；
[0099] 根据上述过程（1)(2)进行初始滤波、反向滤波W及正向滤波，最终基于正向滤波 与反向滤波结果进行整体加权平差。
[0100] 本发明提供了一种低频星敏感器与高频角位移传感器组合定姿技术方案，实现了 星敏感器高精度定姿优势与角位移传感器高频率角增量输出优势互补，通过本发明提供的 技术方案可W得到高频率、高精度的卫星姿态数据，为高分辨率光学卫星高精度几何处理 奠定基础。
【附图说明】
[0101] 图1为本发明实施例的流程图。
【具体实施方式】
[0102] W下结合附图和实施例详细说明本发明技术方案。
[0103] 参见图1所示星敏感器与高频角位移传感器组合定姿技术流程图，W下针对实施 例流程中的各步骤，对本发明方法做进一步详细描述。
[0104] 步骤1，根据角位移传感器工作原理，构建合理的误差测量模型，充分考虑其测量 值中存在的各种误差源。
[0105] 角位移传感器作为一种惯性姿态传感器，可W直接输出高频的角增量数据，得到 的测量数据含义是该采样时刻相比上一采样时刻时间间隔内所产生的角增量大小。根据上 述原理得到:假设角位移传感器数据采样周期为T，某一时刻t的卫星Ξ轴角位移观测值分 另IJ为Nx，Ny，Nz，则得到该时刻t的ミ轴角速度ωχ，ωy，ωz，具体展开如下：
[0106]
(1)
[0107]由于角位移传感器在轨工作过程中存在的误差项包括安装误差、标度因数误差、 漂移误差W及测量噪声等，故进一步得到角位移传感器的误差测量模型如下：
[010引 0g= (1+Δ + 八）0+b~i~rig (2)
[0109] 其中，Qg为角位移传感器测量得到的角速度大小，Δ表示角位移传感器安装误 差，A表示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表 示角位移传感器漂移大小，屯为角位移传感器测量噪声。运里假设角位移传感器测量噪声 为高斯白噪声，即和，〇g表示测量噪声中误差。
[0110] 进一步假设角位移传感器的漂移b由白噪声驱动，表示为：
[0111] 屋=呢 （3)
[0112] 其中，?表示漂移量的微分值，rU为角位移传感器与时间相关的噪声，满足 布~游典為），〇b表不漂移噪声中误差。
[0113] 步骤2,基于多星敏感器信息融合结果构建系统量测方程。
[0114] 本发明基于多星敏感器光轴矢量观测值实现多星敏感器最优信息融合，进一步构 建系统量测方程，具体过程如下：
[0115] 假设一组正交矢量在本体坐标系、惯性坐标系下表达方式如下：
[0116]
(4)
[0117] 其中，/:，42，省表示正交矢量在本体坐标系下真实值，4乂，,，/1表示正交矢量在惯性 坐标系下的测量值，/U2,护表示正交矢量在惯性坐标系下真实值。
[0118] 进一步根据推导得到量测方程如下：
[0119] Zk = HkXk+Vk (5)
[0121] 其中，Zk表示观测矢量，化表示观测矩阵，Xk表示状态变量，f表示四元数最优估计 值，4(如表示由本体到惯性系转换矩阵，Vk表示tk时刻观测噪声序列，且满足：
[0120]
E(Vk)表示观测噪声序列平均值，表示不同时刻之间观测噪声序 列协方差，Ff表示t扣寸刻观测噪声矩阵转置，Sw表示狄克莱函数;化表示星敏感器量测噪声 方差阵。
[0122] 步骤3,基于姿态运动学方程W及角位移传感器误差测量模型构建系统的状态方 程。
[0123] 本发明根据姿态运动学方程W及角位移传感器测量模型进一步构建状态方程，具 体过程如下：
[0124] 姿态运动学方程表示如下：
[0127] 其中，f表示四元数的微分值，q和ω分别表示卫星本体坐标系相对于惯性坐标系 的姿态四元数与Ξ轴旋转角速度，q=[qo qi Q2 93]τ，ω=(ωχ，ωγ，ωζ)。
[012引进一步推导得到基于状态JTw二[Δ5' Δ,ν' |''的系统状态方程：
[0129]
(巧
[0130] 其中，乂的表示状态量微分值，X(t)表示状态量，F(t)表示状态转移矩阵微分，W (t)表示系统噪声序列，悼X]表示角速度矢量叉乘，13X3表示单位矩阵，03X3表示元素为0的 矩阵，Δ矿'表示误差四元数矢量部分，AbT表示误差漂移，W上得到的状态方程为连续动态 系统滤波方程，需要进行线性化和离散化，故进行线性化离散化得到：
[01 引]Xk=巫 k,k-戍 k-i+Tk-iWk-i (8)
[01创其中，Xk、Xk-i表示tk时刻、tk-i时刻状态变量，Φk,k-i表示状态转移矩阵，Γ k-i表示 系统噪声驱动阵，Wk-i表示系统噪声序列。
[0133] 步骤4,基于步骤2和步骤3分别构建的系统量测方程和系统状态方程，采用双向卡 尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。
[0134] 由于星敏感器输出频率为几赫兹到几十赫兹，而角位移传感器输出频率达到上万 赫兹，W往的组合定姿滤波器设计已经无法满足实际需要。本发明针对该问题提出一种双 向卡尔曼滤波与整体加权平差相结合的模型算法实现多频多种姿态敏感器最优信息融合。
[0135] (1)基于角位移测量信息进行姿态预测
[0136] 基于角位移测量信息在tk-i时刻对下式进行积分，可W得到卫星姿态四元数预测 值^5用-Γ〇
[0137]
(9)
[0138] 其中，如1表示tk-i时刻四元数最优估计值，勾-_1表示tk-i时刻角速度最优估计值。
[0139] 角位移漂移预测值的预测模型如下：
[0140]
(!0)
[0141] 其中，表示tk-i时刻角位移漂移最优估计值。
[0142] 对于误差协方差阵的预测值含W解算模型如下：
[0143]
(II)
[0144] 其中，,表示tk-i时刻误差协方差阵估计值，Qk-i表示系统干扰方差阵。
[0145] (2)基于星敏感器测量值进行姿态修正
[0146] 在tk时刻，根据步骤2量测方程计算观测矩阵化。进一步计算滤波增益Kk:
[0147]
(12)
[014引其中，Pk/k-i表示误差协方差阵预测值，则相应的系统状态变量玄，为：
[0149]
(13)
[0150] 卫星姿态四元数tk时刻的修正更新值么与角位移漂移的修正更新值&分别为：
[0151]
(14)：
[0152] 其中，表示误差四元数估计值，Δ&表示误差漂移量估计值。
[0153] 误差协方差阵Pk的更新计算为：
[0154]
(1句
[0155] 其中，化表示星敏感器量测噪声方差阵，I表示单位矩阵，化采用修正更新后的状态 变量重新进行计算，运样做的目的是使滤波过程具有更好的收敛性。在校正了卫星姿态 四元数和角位移的漂移后，状态变量需要重新设置为零。
[0156] 根据上述过程进行初始滤波、反向滤波W及正向滤波，（1)到（2)为初次滤波；（2) 至|J(1)为反向滤波;最后（1)到（2)为正向滤波，每次滤波的初始值为上次滤波的结果;最终 基于正向滤波与反向滤波结果进行整体加权平差。
[0157]
[0158] 根符号吁"表示正向滤波结果，根符号"b"表示反向滤波结果，根符号V'表示整体 加权平差结果，
[0159] 4,(&)表示tk时刻正向滤波姿态最优估计值，如表示tk时刻反向滤波姿态最优估 计值兔斯的逆，若^(巧表示tk时刻正反向滤波结果四元数误差；:#*斯=[知。知如。私《3了 ；
[0160] 表示正反向状态量滤波结果误差矩阵，sgn巧J6。)表示返回标量正负号， 炸）、A,,，作):表示tk时刻漂移正向与反向滤波最优估计值，
[0161] 与--I倘与為脚分别表示误差协方差阵正反向最优估计值的逆，卸A)表示整理 加权的误差协方差阵估计值；
[016引 觸表示正反向状态量滤波结果误差矩阵加权平均值，Δ表'W表示误差四元 数整体加权平均矢量部分结果，Δ&' (/0表示误差漂移整体加权平均结果；
[0163] 炸)表示误差四元数整体加权结果，Δ& (A)表示误差漂移整体加权结果；
[0164] 表(&)表示tk时刻四元数整体加权结果，4,似=[Δ支。从）Δ朵/炸）Δ击/脚Δ朵/(坤， /;、(又-)表示tk时刻漂移量整体加权结果。
[0165] 具体实施时，本发明所提供方法可基于软件技术实现自动运行流程，也可采用模 块化方式实现相应系统。本发明实施例还提供一种星敏感器与高频角位移传感器组合定姿 系统，包括W下模块：
[0166] 第一模块，用于构建角位移传感器测量模型如下，
[0167] Q g二(1+Δ + 八)Q+b巧g
[0168] 其中，Og为角位移传感器测量得到的角速度大小，Δ表示角位移传感器安装误 差，A表示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表 示角位移传感器漂移大小，％为角位移传感器测量噪声；
[0169] 第二模块，用于基于多星敏感器信息融合结果构建系统量测方程，实现如下，
[0170] 设有一组正交矢量，c，42，c表示正交矢量在本体坐标系下真实值，4点，4表示正 交矢量在惯性坐标系下的测量值，杳，和/f表示正交矢量在惯性坐标系下真实值；
[0171]得到量测方程如下，
[0174] 其中，Zk表示观测矢量，化表示观测矩阵，Xk表示状态变量，f表示四元数最优估计 值，省巧)表示由本体到惯性系转换矩阵，Vk表示tk时刻观测噪声序列；
[0175] 第Ξ模块，用于根据卫星姿态运动学方程和角位移传感器测量模型构建系统状态 方程，实现如下，
[0176] 根据姿态运动学方程得到基于状态JTw叫Δ?7'' Δ/，'' Γ'的系统状态方程如下，
[0180] 其中，义的表示状态量微分值，X(t)表示状态量，F(t)表示状态转移矩阵微分，W (t)表示系统噪声序列，的叫表示角速度矢量叉乘，13X3表示单位矩阵，03X3表示元素为0的 矩阵，Δ沪表示误差四元数矢量部分，Δ bT表示误差漂移；
[0181 ]对W上系统状态方程进行线性化离散化得到，
[0182] Xk=巫 k,k-戍 k-i+Tk-iWk-i
[018引其中，Xk、Xk-i表示tk时刻、tk-i时刻状态变量，&k,k-i表示状态转移矩阵，Tk-i表示 系统噪声驱动阵，Wk-i表示系统噪声序列；
[0184] 第四模块，用于基于第二模块和第Ξ模块分别构建的系统量测方程和系统状态方 程，采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。
[0185] 各模块具体实现可参见相应步骤，本发明不予寶述。
[0186] 本文中所描述的具体实例仅仅是对本发明精神作举例说明。本发明所属技术领域 的技术人员可W对所描述的具体实例做各种各样的修改或补充或采用类似的方式替代，但 并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。
【主权项】
1. 一种星敏感器与高频角位移传感器组合定姿方法，其特征在于，包括以下步骤： 步骤1，构建角位移传感器测量模型如下，其中，ω g为角位移传感器测量得到的角速度大小，△表示角位移传感器安装误差，Λ表 示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表示角位移 传感器漂移大小，％为角位移传感器测量噪声； 步骤2,基于多星敏感器信息融合结果构建系统量测方程，实现如下， 设有一组正交矢量，表示正交矢量在本体坐标系下真实值，/!",&,4表示正交矢 量在惯性坐标系下的测量值，彳,匕/；5表示正交矢量在惯性坐标系下真实值； 得到量测方程如下，其中，Zk表示观测矢量，Hk表示观测矩阵，Xk表示状态变量，I表示四元数最优估计值， 4(d表示由本体到惯性系转换矩阵，Vk表示tk时刻观测噪声序列； 步骤3，根据卫星姿态运动学方程和角位移传感器测量模型构建系统状态方程，实现如 下， 根据姿态运动学方程得到基于状态的系统状态方程如下，ff(t) = [-0.5% nb]T 其中，表不状态量微分值，X(t)表不状态量，F(t)表不状态转移矩阵微分，W(t)表 示系统噪声序列，[6χ]表示角速度矢量叉乘，I3X3表示单位矩阵，O3X 3表示元素为O的矩阵， 表示误差四元数矢量部分，△『表示误差漂移； 对以上系统状态方程进行线性化离散化得到， Xk = ?k,k-lXk-l+ Γ k-lffk-1 其中，Xk、Xk-i表示tk时刻、tk-i时刻状态变量，Φ k,k-i表示状态转移矩阵，Γ I^1表示系统噪 声驱动阵，W1^1表示系统噪声序列； 步骤4,基于步骤2和步骤3分别构建的系统量测方程和系统状态方程，采用双向卡尔曼 滤波与整体加权平差模型，实现高频姿态参数最优估计。2. 根据权利要求1所述星敏感器与高频角位移传感器组合定姿方法，其特征在于:步骤 3中姿态运动学方程表示如下姿态运动学方程表示如下，其中，4表示四元数的微分值，q和ω分别表示卫星本体坐标系相对于惯性坐标系的姿 态四元数与三轴旋转角速度，q=[qo qi q2 q3]T，w=(cox，coy，coz)。3.根据权利要求1或2所述星敏感器与高频角位移传感器组合定姿方法，其特征在于： 步骤4实现如下， (1) 基于角位移测量信息进行姿态预测 基于角位移测量信息在时刻对下式进行积分，得到卫星姿态四元数预测值i.其中，IiM表示tk-l时刻四元数最优估计值，表示tk-l时刻角速度最优估计值； 角位移漂移预测值的预测模型如下，其中，表示tk-Ι时刻角位移漂移最优估计值； 对于误差协方差阵的预测值為解算模型如下，其中，表不tk-1时刻误差协方差阵估计值，仏-1表不系统干扰方差阵； (2) 基于星敏感器测量值进行姿态修正 在tk时刻，根据系统量测方程计算观测矩阵Hk，进一步计算滤波增益Kk，其中，Pk/k-i表示误差协方差阵预测值，则相应的系统状态变量为，卫星姿态四元数tk时刻的修正更新值么与角位移漂移的修正更新值&分别为，其中，Δ#,表示误差四元数估计值，表示误差漂移量估计值； 误差协方差阵Pk的更新计算为，其中，Rk表示星敏感器量测噪声方差阵，I表示单位矩阵，Hk采用修正更新后的状态变量 14重新进行计算； 根据上述过程（1)(2)进行初始滤波、反向滤波以及正向滤波，最终基于正向滤波与反 向滤波结果进行整体加权平差。4. 一种星敏感器与高频角位移传感器组合定姿系统，其特征在于，包括以下模块： 第一模块，用于构建角位移传感器测量模型如下， og=(l+A + A ) 〇 +b+Hg 其中，ω g为角位移传感器测量得到的角速度大小，△表示角位移传感器安装误差，Λ表 示角位移传感器标度因数误差，ω为卫星本体相对于惯性空间的真实角速度，b表示角位移 传感器漂移大小，％为角位移传感器测量噪声； 第二模块，用于基于多星敏感器信息融合结果构建系统量测方程，实现如下， 设有一组正交矢量，表示正交矢量在本体坐标系下真实值，表示正交矢 量在惯性坐标系下的测量值，/,1Jf表示正交矢量在惯性坐标系下真实值； 得到量测方程如下，其中，Zk表示观测矢量，Hk表示观测矩阵，Xk表示状态变量，i表示四元数最优估计值， 4(1)表示由本体到惯性系转换矩阵，Vk表示tk时刻观测噪声序列； 第三模块，用于根据卫星姿态运动学方程和角位移传感器测量模型构建系统状态方 程，实现如下， 根据姿态运动学方程得到基于状态的系统状态方程如下，ff(t) = [-0.5% nb]T 其中，乂(〇表不状态量微分值，X(t)表不状态量，F(t)表不状态转移矩阵微分，W(t)表 示系统噪声序列，[洽X]表示角速度矢量叉乘，I3X3表示单位矩阵，O3X 3表示元素为〇的矩阵， Δ-/表示误差四元数矢量部分，△以表示误差漂移； 对以上系统状态方程进行线性化离散化得到， Xk = ?k,k-lXk-l+ Γ k-lffk-1 其中，Xk、Xk-i表示tk时刻、tk-i时刻状态变量，Φ k,k-i表示状态转移矩阵，Γ I^1表示系统噪 声驱动阵，W1^1表示系统噪声序列； 第四模块，用于基于第二模块和第三模块分别构建的系统量测方程和系统状态方程， 采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。5. 根据权利要求4所述星敏感器与高频角位移传感器组合定姿方法，其特征在于:第三 模块中姿态运动学方程表示如下姿态运动学方程表示如下，其中，4表示四元数的微分值，q和ω分别表示卫星本体坐标系相对于惯性坐标系的姿 态四元数与三轴旋转角速度，6. 根据权利要求4或5所述星敏感器与高频角位移传感器组合定姿方法，其特征在于： 第四模块实现如下， (1) 基于角位移测量信息进行姿态预测 基于角位移测量信息在时刻对下式进行积分，得到卫星姿态四元数预测值，其中，I表示tk-i时刻四元数最优估计值，表示tk-l时刻角速度最优估计值； 角位移漂移预测值的预测模型如下，其中，表示tk-l时刻角位移漂移最优估计值； 对于误差协方差阵的预测值解算模型如下，其中，时刻误差协方差阵估计值，Qk-i表不系统干扰方差阵； (2) 基于星敏感器测量值进行姿态修正 在tk时刻，根据系统量测方程计算观测矩阵Hk，进一步计算滤波增益Kk，其中，Pk/k-i表示误差协方差阵预测值，则相应的系统状态变量为，卫星姿态四元数tk时刻的修正更新值A与角位移漂移的修正更新值&分别为，其中，表示误差四元数估计值，表示误差漂移量估计值； 误差协方差阵Pk的更新计算为，其中，Rk表示星敏感器量测噪声方差阵，I表示单位矩阵，Hk采用修正更新后的状态变量 乂4重新进行计算； 根据上述过程（1)(2)进行初始滤波、反向滤波以及正向滤波，最终基于正向滤波与反 向滤波结果进行整体加权平差。
【专利摘要】本发明提供了一种星敏感器与高频角位移传感器组合定姿方法及系统，包括根据角位移传感器测量原理构建其误差测量模型，充分考虑其观测值存在的各种误差源；基于多星敏感器信息融合结果构建系统量测方程；根据卫星姿态运动学方程和角位移传感器测量模型构建系统状态方程；最后采用双向卡尔曼滤波与整体加权平差模型，实现高频姿态参数最优估计。本发明可以实现低频星敏感器与高频角位移传感器最优信息融合，得到高频高精度的姿态数据，从而为高分辨率光学影像高精度几何处理奠定基础。
【IPC分类】G01C21/16, G01C25/00, G01C21/20
【公开号】CN105486312
【申请号】CN201610067447
【发明人】王密, 范城城, 李德仁
【申请人】武汉大学
【公开日】2016年4月13日
【申请日】2016年1月30日
最新回复(0)