[发明专利]空间飞行器姿态运动模拟平台高精度姿态确定方法

权利要求书

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

2.根据权利要求1所述的一种空间飞行器姿态运动模拟平台高精度姿态确定方法，其特征在于，所述的数据融合滤波处理的方法如下：

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

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

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

q0=1+A11+A22+A33/2q1=(A23-A32)/4q0q2=(A31-A13)/4q0q1=(A12-A21)/4q0---(2)]]>

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

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

Q.eΔb.=-[ω×]-12Cbsg03×303×3QeΔb+-12Cbsg03×303×3Cbsgngns---(3)]]>

即

X.(t)=FX(t)+GW(t)---(4)]]>

其中

[ω×]=0-ωzωyωz0-ωx-ωyωx0---(5)]]>

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

离散化处理：

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

其中

Φk,k-1=I6×6+F·T+T22F2Γk,k-1=(I6×6·T+T22F+T36F2)G---(7)]]>

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

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

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

Zk=HkXk+CissVk---(8)]]>

其中

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；

对于一步递推预测过程，需要对状态变量中的陀螺漂移和姿态四元数分开进行讨论：

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

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

b^k|k-1=b^k-1---(10)]]>

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

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

q^.k-1=12q^k-1⊗0ω^k-1---(11)]]>

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

ω^k-1=uk-1-b^k-1---(12)]]>

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

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

q^e,k|k-1=q^k|k-1-1⊗qk---(13)]]>

步骤5：状态更新

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

Pk/k-1=Φk,k-1Pk-1Φk,k-1|T+Γk-1Qk-1Γk-1T---(14)]]>

Kk=Pk/k-1HkT(HkPk/k-1HkT+Rk)-1---(15)]]>

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

Q^e,kΔb^k=Q^e,k|k-1Δb^k|k-1+Kk·Q^e,k|k-1---(17)]]>

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

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

姿态滤波信息融合过程也需要对陀螺漂移和姿态四元数分开讨论：

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

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

b^k=b^k|k-1+Δb^k=b^k-1+Δb^k---(18)]]>

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

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

q^k=q^k|k-1⊗q^e,k---(19)]]>

q^e,k=1Q^e,kT---(20)]]>

步骤7：输出姿态参数

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

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

ω^k=uk-b^k---(21)]]>

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

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