空间飞行器姿态运动模拟平台高精度姿态确定方法

摘要

本发明的目的在于提供一种空间飞行器姿态运动模拟平台高精度姿态确定方法，采用的设备包括激光跟踪仪、智能测头、陀螺仪和两台工业控制计算机，从而搭建地面全物理仿真实验装置，该方法是一种基于扩展卡尔曼滤波的姿态确定的改进方法，通过对激光跟踪仪和陀螺的输出信息进行数据融合滤波处理，得到空间飞行器姿态运动模拟平台的姿态信息，更加符合实际工作情况，具有较高的实时性。本发明采用的测量设备安装简单，测量精度高，能够动态无接触测量空间飞行器姿态运动模拟平台的姿态角和角速率信息。

说明书

技术领域

本发明涉及测量技术，具体说就是一种空间飞行器姿态运动模拟平台高精度 姿态确定方法。

背景技术

空间飞行器姿态运动模拟平台(也称为三轴气浮台)依靠压缩空气在气浮轴 承与轴承座之间形成的气膜，使模拟台体浮起，从而实现近似无摩擦的相对运动 条件，以模拟卫星等空间飞行器在外层空间所受干扰力矩很小的力学环境。作为 空间飞行器姿态运动的通用模拟器，三轴气浮台可以进行飞行器姿态控制系统性 能的全物理仿真实验验证分析，是空间飞行器研制过程中的重要手段和方法。

空间飞行器姿态运动模拟平台在试验过程中需要通过姿态测量系统动态地 给出姿态角、角速率等姿态信息，以便完成控制闭环，由于空间飞行器姿态运动 模拟平台的特殊结构，以往用于转台测量的装置(如旋转变压器、感应同步器、 光电码盘、光栅等)不适用于空间飞行器姿态运动模拟平台的测量，需要考虑新 的测量方法和装置。并且在目前实际应用中，姿态测量系统精度的高低直接关系 到仿真试验的效果。

经检索文献发现，中国发明专利申请号：201110249979.2，专利名称为三轴 气浮台高精度姿态角测量装置及其方法，该专利在三轴气浮台台面底部四周安装 导轨、遮光帘和标志器，人工照明系统和数字CCD摄像机安装在可升降平台上。 数字CCD摄像机采集标志器的图像并传输到负责图像处理的计算机上，通过对标 志器进行亚像素定位，获取标志器的精确坐标，从而计算出标志器与数字CCD 摄像机之间的相对姿态角。但由于系统构建上的缺陷，测量精度受到限制，从而 影响其实际使用范围。

中国发明专利申请号：201310134631.8，专利名称为：三轴气浮台高精度姿 态角及角速度测量装置，该专利在三轴气浮台的仪表平台上安装智能侧头、陀螺 仪和四面棱镜，在台下安装激光跟踪仪和两台光电自准直仪，根据激光跟踪仪、 两台光电自准直仪和陀螺仪的数据直接得到姿态信息。但是该专利在数据滤波处 理部分，并没有考虑到偏差四元数模为1的约束条件，直接采用卡尔曼滤波，容 易造成误差方差阵发生奇异，导致数据发散，姿态确定失败。该专利没有考虑到 激光跟踪仪和光电自准直仪输出信息的姿态参数转换、陀螺仪量测坐标系标定等 过程。此外，将数据处理与通信部分安排在台下进行，不符合实际情况。

在文献“三轴气浮台单框伺服测角系统的研究”(发表于宇航学报，1996， 第17卷，第4期，页码：71-74)中，哈尔滨工业大学的张晓友、刘敦和北京控 制工程研究所的李继苏等提出了一种单框伺服测量方案，该系统在气浮台底座上 安装一个可以绕气浮台中心铅垂线转动的圆弧臂，并在其上安装可以移动的滑 架，通过敏感圆弧臂的转动和滑架的移动测量气浮台的姿态信息。当该系统需要 增加复杂的机械系统和敏感器系统，机构复杂，工程应用较困难，并且其精度受 到机械装置和敏感器的限制，很难达到高精度。

发明内容

针对空间飞行器姿态运动模拟平台的姿态确定问题，本发明的目的在于提供 一种空间飞行器姿态运动模拟平台高精度姿态确定方法，该方法是一种基于扩展 卡尔曼滤波的姿态确定的改进方法，通过对激光跟踪仪和陀螺的输出信息进行数 据融合滤波处理，得到空间飞行器姿态运动模拟平台的姿态信息。

本发明是这样实现的：一种空间飞行器姿态运动模拟平台高精度姿态确定方 法，采用的设备包括激光跟踪仪、智能测头、陀螺仪和两台工业控制计算机，激 光跟踪仪安装于空间飞行器姿态运动模拟平台的台下；在姿态确定之前，智能测 头安装在台下，配合激光跟踪仪标定激光跟踪仪的量测坐标系，姿态确定过程中， 智能测头固定在空间飞行器姿态运动模拟平台的仪表平台上，智能测头上具有靶 标定位系统，激光跟踪仪配合智能测头能够跟踪并确定智能测头转动的三维角度 以及移动的三维距离；陀螺仪固定在空间飞行器姿态运动模拟平台的仪表平台 上，输出空间飞行器姿态运动模拟平台的角速率信息；第一工业控制计算机安装 在空间飞行器姿态运动模拟平台的仪表平台上，进行数据处理工作；第二工业控 制计算机安装在台下，采集激光跟踪仪的输出数据，并通过无线网络，发送给第 一工业控制计算机；第一工业控制计算机接收激光跟踪仪和陀螺仪的姿态数据， 并进行数据融合滤波处理，输出空间飞行器姿态运动模拟平台的姿态角信息，同 时标定陀螺仪的漂移，实时修正陀螺仪的输出角速率。

本发明还具有如下特征：

1、如上所述的一种空间飞行器姿态运动模拟平台高精度姿态确定方法，其 中数据融合滤波处理的方法如下：

步骤1：根据姿态参数转换公式，采用312转序，利用激光跟踪仪输出的气 浮台姿态角θ、ψ计算得到气浮台体坐标系相对惯性系的坐标转换矩阵A_br：

其中，θ、ψ是激光跟踪仪输出的气浮台姿态角；

进一步计算得到姿态四元数：

$\left(\begin{array}{c}{q}_0=\sqrt{1+A_{11}+A_{22}+A_{33}}/2\\ q_1=(A_{23}-A_{32})/4q_0\\ q_2=(A_{31}-A_{13})/4q_0\\ q_1=(A_{12}-A_{21})/4q_0\end{array}\right)---\left(2\right)$

步骤2：构造系统的状态方程

选择偏差四元数的矢量部分Q_e和陀螺漂移偏差Δb组成六维状态变量X(t)， 构造系统的状态方程：

$\left(\begin{array}{c}{\stackrel{.}{Q}}_e\\ \Delta \stackrel{.}{b}\end{array}\right)=\left(\begin{array}{cc}-[\omega \times ]& -\frac{1}{2}{C}_{\mathrm{bs}}^g\\ 0_{3\times 3}& 0_{3\times 3}\end{array}\right)\left(\begin{array}{c}{Q}_e\\ \mathrm{\Delta b}\end{array}\right)+\left(\begin{array}{cc}-\frac{1}{2}{C}_{\mathrm{bs}}^g& 0_{3\times 3}\\ 0_{3\times 3}& C_{\mathrm{bs}}^g\end{array}\right)\left(\begin{array}{c}{n}_g\\ n_s\end{array}\right)---\left(3\right)$

即

$\stackrel{.}{X}\left(t\right)=\mathrm{FX}\left(t\right)+\mathrm{GW}\left(t\right)---\left(4\right)$

其中

$[\omega \times ]=\left(\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right)---\left(5\right)$

F是系统的状态矩阵，G是系统过程噪声驱动矩阵，ω是空间飞行器姿态运 动模拟平台的体坐标系相对惯性系的转动角速率在体坐标系的投影，是陀螺 的量测坐标系相对空间飞行器姿态运动模拟平台的体坐标系的坐标转换矩阵， n_g是陀螺的量测噪声，n_s是陀螺的相关漂移的驱动噪声；

离散化处理：

X_k＝Φ_k，k-1X_k-1+Γ_k-1W_k-1   (6)

其中

$\left(\begin{array}{c}{\Phi }_{k,k-1}=I_{6\times 6}+F·T+\frac{T^2}{2}{F}^2\\ {\Gamma }_{k,k-1}=(I_{6\times 6}·T+\frac{T^2}{2}F+\frac{T^3}{6}{F}^2)G\end{array}\right)---\left(7\right)$

T为采样时间间隔，W_k-1是系统的过程噪声；

步骤3：构造系统的量测方程

选取偏差四元数的矢量部分Q_e为量测值Z_k，构造系统的量测方程：

$Z_k=H_k{X}_k+C_{\mathrm{is}}^s{V}_k---\left(8\right)$

其中

H_k＝[I_3×3  0_3×3]   (9)

H_k是量测矩阵，V_k是激光跟踪仪的量测噪声，是激光跟踪仪的量测坐标 系相对惯性坐标系的坐标转换矩阵；

步骤4：一步递推预测

在k时刻，已知k-1时刻的姿态四元数估计值陀螺漂移的估计值陀螺的量测角速率u_k-1，量测得到当前k时刻激光跟踪仪的输出四元数q_k，陀螺 仪的输出角速率u_k；

由于四元数存在模为1的约束条件，以及偏差四元数的叉乘运算关系，对于 一步递推预测过程，需要对状态变量中的陀螺漂移和姿态四元数分开进行讨论：

(4.1)陀螺仪漂移的一步递推预测值

由于陀螺漂移变化缓慢，k时刻的陀螺漂移就是k-1时刻的估计值即：

${\hat{b}}_{k|k-1}={\hat{b}}_{k-1}---\left(10\right)$

(4.2)姿态四元数的一步递推预测值

根据姿态四元数的运动学方程，得到k-1时刻姿态四元数的变化率

${\stackrel{.}{\hat{q}}}_{k-1}=\frac{1}{2}{\hat{q}}_{k-1}\otimes \left(\begin{array}{cc}0& {\hat{\omega }}_{k-1}\end{array}\right)---\left(11\right)$

其中，是k-1时刻陀螺输出角速率的修正值，即空间飞行器姿态运动模 拟平台角速率的估计值，通过利用陀螺漂移估计将陀螺输出u_k-1中的漂移 部分剔除得到：

${\hat{\omega }}_{k-1}=u_{k-1}-{\hat{b}}_{k-1}---\left(12\right)$

利用采用相关数值积分的方法，即可得到k时刻姿态四元数的一步递 推预测值

进一步的，根据偏差四元数的定义，利用姿态四元数的一步递推预测值和激光跟踪仪的输出q_k，得到偏差四元数的一步递推预测值

${\hat{q}}_{e,k|k-1}={\hat{q}}_{k|k-1}^{-1}\otimes q_k---\left(13\right)$

步骤5：状态更新

根据下式进行状态更新，得到偏差四元数矢量部分的滤波估计值和陀螺 仪(3)漂移偏差的滤波估计值

$P_{k/k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^{|T}+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^T---\left(14\right)$

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(15\right)$

P_k＝(1-K_kHk₎P_k/k-1   (16)

$\left(\begin{array}{c}{\hat{Q}}_{e,k}\\ \Delta {\hat{b}}_k\end{array}\right)=\left(\begin{array}{c}{\hat{Q}}_{e,k|k-1}\\ \Delta {\hat{b}}_{k|k-1}\end{array}\right)+K_k·{\hat{Q}}_{e,k|k-1}---\left(17\right)$

其中Q_k是过程噪声统计矩阵，R_k是量测噪声统计矩阵， P_k/k-1是误差统计特性的一步递推预测矩阵；K_k是滤波增益 矩阵；P_k是误差方差阵；

步骤6：姿态滤波信息融合

由于四元数存在模为1的约束条件，以及偏差四元数的叉乘运算关系，姿态 滤波信息融合过程也需要对陀螺漂移和姿态四元数分开讨论：

(6.1)陀螺仪漂移的滤波估计值

陀螺漂移偏差滤波估计值与陀螺漂移一步递推预测值相加，得到陀 螺漂移的滤波估计值

${\hat{b}}_k={\hat{b}}_{k|k-1}+\Delta {\hat{b}}_k={\hat{b}}_{k-1}+\Delta {\hat{b}}_k---\left(18\right)$

(6.2)姿态四元数的滤波估计值

根据偏差四元数的定义，得到姿态四元数的滤波估计值

${\hat{q}}_k={\hat{q}}_{k|k-1}\otimes {\hat{q}}_{e,k}---\left(19\right)$

${\hat{q}}_{e,k}={\left(\begin{array}{cc}1& {\hat{Q}}_{e,k}\end{array}\right)}^T---\left(20\right)$

步骤7：输出姿态参数

(7.1)空间飞行器姿态运动模拟平台角速率

利用陀螺漂移的滤波估计值将陀螺输出角速率u_k中的漂移量刨除掉，即 得到空间飞行器姿态运动模拟平台角速率

${\hat{\omega }}_k=u_k-{\hat{b}}_k---\left(21\right)$

(7.2)空间飞行器姿态运动模拟平台姿态角θ，ψ：

根据姿态参数的转换公式，利用姿态四元数的滤波估计值可以得到空间 飞行器姿态运动模拟平台当前时刻的姿态角：

2、如上所述的一种空间飞行器姿态运动模拟平台高精度姿态确定方法，其中激 光跟踪仪的量测坐标系相对惯性坐标系的坐标转换矩阵的标定方法是：

在使用激光跟踪仪测量空间飞行器姿态运动模拟平台的姿态角时，需要已知 激光跟踪仪的量测坐标系相对于惯性坐标系之间的方位关系，即标定激光跟踪仪 的量测坐标系O_s-X_sY_sZ_s，具体方法如下：

在惯性坐标系中的原点和北东两个方向轴上测量好的固定点上分别放置三 个靶球，测量出三个靶球在激光跟踪仪量测坐标系中的坐标，从而标定出激光跟 踪仪量测坐标系相对惯性坐标系之间的方位关系

首先调整激光跟踪仪使其水平，再通过激光跟踪仪测出O_s点和北向轴上靶球 [0 y₁ 0]^T在激光跟踪仪坐标系中的坐标分别为[a b c]^T和[x y z]^T，通过 齐次坐标变换关系有：

$\left(\begin{array}{c}0\\ y1\\ 0\\ 1\end{array}\right)=\left(\begin{array}{cccc}\cos {\theta }_{\mathrm{is}}& -\sin {\theta }_{\mathrm{is}}& 0& a\\ \sin {\theta }_{\mathrm{is}}& \cos {\theta }_{\mathrm{is}}& 0& b\\ 0& 0& 1& c\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right)---\left(23\right)$

由此解得θ_is，由于激光跟踪仪水平，因此激光跟踪仪量测坐标系O_s-X_sY_sZ_s相对惯性坐标系O_i-X_iY_iZ_i之间的坐标转换矩阵为：

$C_{\mathrm{is}}^s=\left(\begin{array}{ccc}\cos {\theta }_{\mathrm{is}}& \sin {\theta }_{\mathrm{is}}& 0\\ -\sin {\theta }_{\mathrm{is}}& \cos {\theta }_{\mathrm{is}}& 0\\ 0& 0& 1\end{array}\right)---\left(24\right)$

3、如上所述的一种空间飞行器姿态运动模拟平台高精度姿态确定方法，陀 螺量测坐标系相对空间飞行器姿态运动模拟平台体坐标系的坐标转换矩阵的 标定方法，如下：

根据陀螺仪的安装和测量，得出陀螺仪量测坐标系O_gX_g轴正向与仪表平台 本体坐标系O_bX_b轴、O_bY_b轴、O_bZ_b轴正向之间的夹角α₁、α₁、α₃，O_gY_g轴正 向与仪表平台本体坐标系O_bX_b轴、O_bY_b轴、O_bZ_b轴正向之间的夹角β₁、β₂、β₃， O_gZ_g轴正向与仪表平台本体坐标系O_bX_b轴、O_bY_b轴、O_bZ_b轴正向之间的夹角γ₁、 γ₂、γ₃，则陀螺仪量测坐标系O_g-X_gY_gZ_g相对空间飞行器姿态运动模拟平台体 坐标系O_b-X_bY_bZ_b的旋转变换矩阵为：

$C_{\mathrm{bs}}^g=\left(\begin{array}{ccc}\cos {\alpha }_1& \cos {\beta }_1& \cos {\gamma }_1\\ \cos {\alpha }_2& \cos {\beta }_2& \cos {\gamma }_2\\ \cos {\alpha }_3& \cos {\beta }_3& \cos {\gamma }_3\end{array}\right)---\left(25\right).$

本发明提供一种空间飞行器姿态运动模拟平台高精度姿态确定方法，测量设 备安装简单，测量精度高，能够动态无接触测量空间飞行器姿态运动模拟平台的 姿态角和角速率信息，并同时标定陀螺仪的漂移，实时修正陀螺仪的输出角速率。 本发明搭建地面全物理仿真实验装置，在台上对激光跟踪仪和陀螺的输出信息进 行数据融合滤波处理，更加符合实际工作情况，具有较高的实时性。本发明也用 于单轴转台姿态角、角速率的精确测量。

附图说明

图1是空间飞行器姿态运动模拟平台高精度姿态确定装置的组成示意图；

图2是空间飞行器姿态运动模拟平台姿态角精度曲线图；

图3是空间飞行器姿态运动模拟平台角速率精度曲线图；

图4是标定激光跟踪仪量测坐标系的示意图；

图5是各坐标系转换关系示意图；

图6是空间飞行器姿态运动模拟平台高精度姿态确定装置的通信模式示意 图。

具体实施方式

下面结合附图举例对本发明作进一步说明。

实施例1：

结合图1，本发明一种空间飞行器姿态运动模拟平台高精度姿态确定方法， 采用的设备包括激光跟踪仪1、智能测头2、陀螺仪3和两台工业控制计算机4.5， 激光跟踪仪1安装于空间飞行器姿态运动模拟平台的台下；在姿态确定之前，智 能测头2安装在台下，配合激光跟踪仪1标定激光跟踪仪1的量测坐标系，姿态 确定过程中，智能测头2固定在空间飞行器姿态运动模拟平台的仪表平台上，智 能测头2上具有靶标定位系统，激光跟踪仪1配合智能测头2能够跟踪并确定智 能测头2转动的三维角度以及移动的三维距离；陀螺仪3固定在空间飞行器姿态 运动模拟平台的仪表平台上，输出空间飞行器姿态运动模拟平台的角速率信息； 第一工业控制计算机4安装在空间飞行器姿态运动模拟平台的仪表平台上，进行 数据处理工作；第二工业控制计算机5安装在台下，采集激光跟踪仪1的输出数 据，并通过无线网络，发送给第一工业控制计算机4；第一工业控制计算机4接 收激光跟踪仪1和陀螺仪3的姿态数据，并进行数据融合滤波处理，输出空间飞 行器姿态运动模拟平台的姿态角信息，同时标定陀螺仪3的漂移，实时修正陀螺 仪3的输出角速率。

实现步骤如下：

步骤一：打开设备电源，待各设备预热完成后进行下一步；

步骤二：标定激光跟踪仪1的量测坐标系；

步骤三：标定陀螺仪3的量测坐标系；

步骤四：搭建无线网络，实现台上台下两台工业控制计算机4.5的网络通信；

步骤五：利用激光跟踪仪1和陀螺仪3的输出数据，通过数据融合滤波软件 6得到气浮台的姿态角和角速率信息。

本发明利用激光跟踪仪1和陀螺仪3联合实现空间飞行器姿态运动模拟平台 的高精度姿态确定，其中激光跟踪仪1可以选用瑞士Leica公司AT901产品或者 美国API公司的产品，此处以Leica公司AT901产品为例，其三维姿态角度的测 量精度为3″(3σ)，测量范围达到±60°。使用时，激光跟踪仪1安装在气浮台 下的支架上，智能测头2安装在气浮台仪表平台上和台体一起运动，安装时要保 证激光跟踪仪1光轴和智能测头2靶球之间的视场需求，以保证测试系统在工作 过程中不丢失测量自由度，激光跟踪仪1根据测量数据实时计算出智能测头2 的相对姿态信息。

空间飞行器姿态运动模拟平台角速率的测量由高精度半球谐振陀螺仪实现。 将半球谐振陀螺仪垂直安装在经调平后的空间飞行器姿态运动模拟平台中心，其 敏感轴与空间飞行器姿态运动模拟平台转轴垂直。半球谐振陀螺仪的输出包含了 空间飞行器姿态运动模拟平台的常值漂移，相关漂移，量测噪声等漂移噪声，在 实际使用时，采用激光跟踪仪的姿态信息经过数据融合滤波软件修正陀螺的输出 角速率。

实施例2：

本发明提出了一种基于扩展卡尔曼滤波的空间飞行器姿态运动模拟平台姿 态确定改进方案，搭建地面全物理仿真实验装置，通过对激光跟踪仪和陀螺仪的 输出信息进行数据融合滤波处理，得到空间飞行器姿态运动模拟平台的姿态信 息，具体步骤如下：

步骤1：根据姿态参数转换公式，采用312转序，利用激光跟踪仪输出的气 浮台姿态角θ、ψ计算得到气浮台体坐标系相对惯性系的坐标转换矩阵A_br：

其中，θ、ψ是激光跟踪仪输出的气浮台姿态角；

进一步计算得到姿态四元数：

$\left(\begin{array}{c}{q}_0=\sqrt{1+A_{11}+A_{22}+A_{33}}/2\\ q_1=(A_{23}-A_{32})/4q_0\\ q_2=(A_{31}-A_{13})/4q_0\\ q_1=(A_{12}-A_{21})/4q_0\end{array}\right)---\left(27\right)$

步骤2：构造系统的状态方程

选择偏差四元数的矢量部分Q_e和陀螺漂移偏差Δb组成六维状态变量X(t)， 构造系统的状态方程：

$\left(\begin{array}{c}{\stackrel{.}{Q}}_e\\ \Delta \stackrel{.}{b}\end{array}\right)=\left(\begin{array}{cc}-[\omega \times ]& -\frac{1}{2}{C}_{\mathrm{bs}}^g\\ 0_{3\times 3}& 0_{3\times 3}\end{array}\right)\left(\begin{array}{c}{Q}_e\\ \mathrm{\Delta b}\end{array}\right)+\left(\begin{array}{cc}-\frac{1}{2}{C}_{\mathrm{bs}}^g& 0_{3\times 3}\\ 0_{3\times 3}& C_{\mathrm{bs}}^g\end{array}\right)\left(\begin{array}{c}{n}_g\\ n_s\end{array}\right)---\left(28\right)$

即

$\stackrel{.}{X}\left(t\right)=\mathrm{FX}\left(t\right)+\mathrm{GW}\left(t\right)---\left(29\right)$

其中

$[\omega \times ]=\left(\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right)---\left(30\right)$

F是系统的状态矩阵，G是系统过程噪声驱动矩阵，ω是空间飞行器姿态运 动模拟平台的体坐标系相对惯性系的转动角速率在体坐标系的投影，是陀螺 的量测坐标系相对空间飞行器姿态运动模拟平台的体坐标系的坐标转换矩阵， n_g是陀螺的量测噪声，n_s是陀螺的相关漂移的驱动噪声；

离散化处理：

X_k＝Φ_k，k-1X_k-1+Γ_k-1W_k-1   (31)

其中

$\left(\begin{array}{c}{\Phi }_{k,k-1}=I_{6\times 6}+F·T+\frac{T^2}{2}{F}^2\\ {\Gamma }_{k,k-1}=(I_{6\times 6}·T+\frac{T^2}{2}F+\frac{T^3}{6}{F}^2)G\end{array}\right)---\left(32\right)$

T为采样时间间隔，W_k-1是系统的过程噪声；

步骤3：构造系统的量测方程

选取偏差四元数的矢量部分Q_e为量测值Z_k，构造系统的量测方程：

$Z_k=H_k{X}_k+C_{\mathrm{is}}^s{V}_k---\left(33\right)$

其中

H_k＝[I_3×3  0_3×3]   (34)

H_k是量测矩阵，V_k是激光跟踪仪的量测噪声，是激光跟踪仪的量测坐标 系相对惯性坐标系的坐标转换矩阵；

步骤4：一步递推预测

在k时刻，已知k-1时刻的姿态四元数估计值陀螺漂移的估计值陀螺的量测角速率u_k-1，量测得到当前k时刻激光跟踪仪的输出四元数q_k，陀螺 仪的输出角速率u_k；

由于四元数存在模为1的约束条件，以及偏差四元数的叉乘运算关系，对于 一步递推预测过程，需要对状态变量中的陀螺漂移和姿态四元数分开进行讨论：

(4.1)陀螺仪漂移的一步递推预测值

由于陀螺漂移变化缓慢，k时刻的陀螺漂移就是k-1时刻的估计值即：

${\hat{b}}_{k|k-1}={\hat{b}}_{k-1}---\left(35\right)$

(4.2)姿态四元数的一步递推预测值

根据姿态四元数的运动学方程，得到k-1时刻姿态四元数的变化率

${\stackrel{.}{\hat{q}}}_{k-1}=\frac{1}{2}{\hat{q}}_{k-1}\otimes \left(\begin{array}{cc}0& {\hat{\omega }}_{k-1}\end{array}\right)---\left(36\right)$

其中，是k-1时刻陀螺输出角速率的修正值，即空间飞行器姿态运动模 拟平台角速率的估计值，通过利用陀螺漂移估计将陀螺输出u_k-1中的漂移 部分剔除得到：

${\hat{\omega }}_{k-1}=u_{k-1}-{\hat{b}}_{k-1}---\left(37\right)$

利用采用相关数值积分的方法，即可得到k时刻姿态四元数的一步递 推预测值

进一步的，根据偏差四元数的定义，利用姿态四元数的一步递推预测值和激光跟踪仪的输出q_k，得到偏差四元数的一步递推预测值

${\hat{q}}_{e,k|k-1}={\hat{q}}_{k|k-1}^{-1}\otimes q_k---\left(38\right)$

步骤5：状态更新

根据下式进行状态更新，得到偏差四元数矢量部分的滤波估计值和陀螺 仪(3)漂移偏差的滤波估计值

$P_{k/k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^{|T}+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^T---\left(39\right)$

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(40\right)$

P_k＝(1-K_kH_k)P_k/k-1   (41)

$\left(\begin{array}{c}{\hat{Q}}_{e,k}\\ \Delta {\hat{b}}_k\end{array}\right)=\left(\begin{array}{c}{\hat{Q}}_{e,k|k-1}\\ \Delta {\hat{b}}_{k|k-1}\end{array}\right)+K_k·{\hat{Q}}_{e,k|k-1}---\left(42\right)$

其中Q_k是过程噪声统计矩阵，R_k是量测噪声统计矩阵， P_k/k-1是误差统计特性的一步递推预测矩阵；K_k是滤波增益 矩阵；P_k是误差方差阵；

步骤6：姿态滤波信息融合

由于四元数存在模为1的约束条件，以及偏差四元数的叉乘运算关系，姿态 滤波信息融合过程也需要对陀螺漂移和姿态四元数分开讨论：

(6.1)陀螺仪漂移的滤波估计值

陀螺漂移偏差滤波估计值与陀螺漂移一步递推预测值相加，得到陀 螺漂移的滤波估计值

${\hat{b}}_k={\hat{b}}_{k|k-1}+\Delta {\hat{b}}_k={\hat{b}}_{k-1}+\Delta {\hat{b}}_k---\left(43\right)$

(6.2)姿态四元数的滤波估计值

根据偏差四元数的定义，得到姿态四元数的滤波估计值

${\hat{q}}_k={\hat{q}}_{k|k-1}\otimes {\hat{q}}_{e,k}---\left(44\right)$

${\hat{q}}_{e,k}={\left(\begin{array}{cc}1& {\hat{Q}}_{e,k}\end{array}\right)}^T---\left(45\right)$

步骤7：输出姿态参数

(7.1)空间飞行器姿态运动模拟平台角速率

利用陀螺漂移的滤波估计值将陀螺输出角速率u_k中的漂移量刨除掉，即 得到空间飞行器姿态运动模拟平台角速率

${\hat{\omega }}_k=u_k-{\hat{b}}_k---\left(46\right)$

(7.2)空间飞行器姿态运动模拟平台姿态角θ，ψ：

根据姿态参数的转换公式，利用姿态四元数的滤波估计值可以得到空间 飞行器姿态运动模拟平台当前时刻的姿态角：

结合图2-3，激光跟踪仪的测量精度为3″(3σ)，陀螺仪的常值漂移为 1.5°/h(3σ)，随机漂移为0.003°/h(3σ)，量测噪声为10″(3σ)，经数学仿真，空 间飞行器姿态运动模拟平台姿态角精度为0.05″，角速率精度为0.7″。

实施例3：

结合图4，介绍激光跟踪仪的量测坐标系O_s-X_sY_sZ_s相对惯性坐标系 O_i-X_iY_iZ_i的坐标转换矩阵的标定方法。

首先调整激光跟踪仪使其水平，再通过激光跟踪仪测出O_s点和北向轴上靶 球[0 y₁ 0]^T在激光跟踪仪坐标系中的坐标分别为[a b c]^T和[x y z]^T，通 过齐次坐标变换关系有：

$\left(\begin{array}{c}0\\ y1\\ 0\\ 1\end{array}\right)=\left(\begin{array}{cccc}\cos {\theta }_{\mathrm{is}}& -\sin {\theta }_{\mathrm{is}}& 0& a\\ \sin {\theta }_{\mathrm{is}}& \cos {\theta }_{\mathrm{is}}& 0& b\\ 0& 0& 1& c\\ 0& 0& 0& 1\end{array}\right)\left(\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right)---\left(23\right)$

由此可解得θ_is，由于激光跟踪仪水平，因此由激光跟踪仪量测坐标系 O_s-X_sY_sZ_s相对惯性坐标系O_i-X_iY_iZ_i之间的坐标转换矩阵为：

$C_{\mathrm{is}}^s=\left(\begin{array}{ccc}\cos {\theta }_{\mathrm{is}}& \sin {\theta }_{\mathrm{is}}& 0\\ -\sin {\theta }_{\mathrm{is}}& \cos {\theta }_{\mathrm{is}}& 0\\ 0& 0& 1\end{array}\right)---\left(24\right)$

实施例4：

结合图5，介绍陀螺量测坐标系O_g-X_gY_gZ_g相对空间飞行器姿态运动模拟平 台体坐标系O_b-X_bY_bZ_b的坐标转换矩阵的标定方法，如下

根据陀螺仪的安装和测量，得出陀螺仪量测坐标系O_gX_g轴正向与仪表平台 本体坐标系O_bX_b轴、O_bY_b轴、O_bZ_b轴正向之间的夹角α₁、α₂、α₃，O_gY_g轴正 向与仪表平台本体坐标系O_bX_b轴、O_bY_b轴、O_bZ_b轴正向之间的夹角β₁、β₂、β₃， O_gZ_g轴正向与仪表平台本体坐标系O_bX_b轴、O_bY_b轴、O_bZ_b轴正向之间的夹角γ₁、 γ₂、γ₃，则陀螺仪量测坐标系O_g-X_gY_gZ_g相对空间飞行器姿态运动模拟平台体 坐标系O_b-X_bY_bZ_b的旋转变换矩阵为：

$C_{\mathrm{bs}}^g=\left(\begin{array}{ccc}\cos {\alpha }_1& \cos {\beta }_1& \cos {\gamma }_1\\ \cos {\alpha }_2& \cos {\beta }_2& \cos {\gamma }_2\\ \cos {\alpha }_3& \cos {\beta }_3& \cos {\gamma }_3\end{array}\right)---\left(25\right).$

实施例5：

结合图6，介绍空间飞行器姿态运动模拟平台高精度姿态确定装置的通信模 式。

台上的工业控制计算机通过数据采样卡采集来自陀螺仪输出的角速率数据。 台下的工业控制计算机通过数据采样卡采集来自激光跟踪仪输出的姿态角数据， 并通过无线网络发送给台上的工业控制计算机。台上的工业控制计算机利用无线 网络接收来自台下的工业控制计算机的数据，并进行数据融合滤波处理，输出空 间飞行器姿态运动模拟平台的姿态角信息，同时标定陀螺仪的漂移，实时修正陀 螺仪的输出角速率。