[发明专利]一种针对陀螺仪随机游走噪声的精细滤波方法与测试平台

权利要求书

1.一种针对陀螺仪随机游走噪声的精细滤波测试平台，其特征在于：测试平台包括直流稳压电源(1)、实时仿真目标机(2)、三轴速率转台(3)、三轴陀螺仪(4)、太阳敏感器(5)、太阳仿真器(6)、姿态确定模块(7)、姿态控制模块(8)以及反作用飞轮(9)；所述直流稳压电源(1)用于为三轴陀螺仪(4)、太阳敏感器(5)、姿态确定模块(7)、姿态控制模块(8)以及反作用飞轮(9)提供工作电压；所述实时仿真目标机(2)包括飞行器姿态动力学仿真机(21)与飞行器姿态运动学仿真机(22)，分别实时运算飞行器的姿态动力学与运动学模型；所述三轴速率转台(3)是飞行器的运动仿真器，用于模拟飞行器的姿态运动；所述三轴陀螺仪(4)是角速度姿态信息敏感器，敏感三轴速率转台的角速度；所述太阳敏感器(5)通过敏感太阳光方位而获得飞行器姿态信息；所述太阳仿真器(6)为太阳敏感器(5)提供模拟太阳方位；所述姿态确定模块(7)实时运行精细滤波方法，对三轴陀螺仪(4)与太阳敏感器(5)测量得到的姿态信息进行实时滤波与解算，该模块包括数字运算单元(71)、RS422/232通信单元(72)、电源管理单元(73)、复位电路(74)、扩展备用I/O口(75)、存储器(76)以及时钟单元(77)，其中RS422/232通信单元(72)、电源管理单元(73)、复位电路(74)、扩展备用I/O口(75)和存储器(76)都与数字运算单元(71)相连，数字运算单元(71)是姿态确定模块(7)的运算核心，RS422/232通信单元(72)是姿态确定模块(7)的输入输出端，接受三轴陀螺仪(4)与太阳敏感器(5)实测姿态信息，输出滤波后的姿态信息，电源管理单元(73)负责供电，复位电路(74)可恢复数字运算单元(71)的初始状态，扩展备用I/O口(75)提供可扩展的输入输出端口，存储器(76)存储数字运算单元(71)的程序与数据，时钟单元(77)为数字运算单元(71)提供时钟频率；所述姿态控制模块(8)运算姿态控制方法并解算出力矩指令，为反作用飞轮(9)提供力矩控制指令，包括了数字处理单元(81)、RS422/232串口(82)、电源(83)、复位(84)、备用串口(85)、存储单元(86)及晶振(87)，其中RS422/232串口(82)、电源(83)、复位(84)、备用串口(85)、存储单元(86)都与数字处理单元(81)相连，数字处理单元(81)是姿态控制模块(8)的运算核心，RS422/232串口(82)是姿态控制模块(8)的输入输出端，接受姿态确定模块(7)产生的滤波后姿态信息，输出指令力矩信号给反作用飞轮(9)，电源(83)负责供电，复位(84)可恢复数字处理单元(81)的初始状态，备用串口(85)提供可扩展的输入输出端口，存储单元(86)存储数字处理单元(81)的程序与数据，晶振(87)为数字处理单元(81)提供时钟频率；所述反作用飞轮(9)作为姿控系统的执行机构，在接收力矩控制指令后，将输出力矩信号传给实时仿真目标机(2)；测试平台的数据流回路如下：飞行器的期望姿态信号通过姿态控制模块(8)的RS422/232串口(82)导入系统回路，该期望姿态信号通过与姿态确定模块(7)滤波后的姿态信息进行比较，得到偏差信号；将此偏差信号传递给姿态控制模块(8)，该模块中的姿态控制算法解算出指令力矩信号，该信号经RS422/232串口(82)传送至反作用飞轮(9)；反作用飞轮(9)在接收指令力矩信号后输出执行力矩信号，执行力矩信号通过RS422/232串行数据通信接口传入实时仿真目标机(2)中的飞行器姿态动力学仿真机(21)与飞行器姿态运动学仿真机(22)；飞行器姿态动力学仿真机(21)与飞行器姿态运动学仿真机(22)分别运算飞行器姿态动力学与运动学模块，得到飞行器转动角速度，该飞行器转动角速度经RS422/232串行数据通信接口传送至三轴速率转台(3)；三轴速率转台(3)受飞行器转动角速度驱动而转动，装配在三轴速率转台(3)上的三轴陀螺仪(4)以及太阳敏感器(5)将敏感到的姿态信息经RS422/232串行数据通信接口传递给姿态确定模块(7)；姿态确定模块(7)中的精细滤波方法实时运算并将由精细滤波方法得到的姿态信息与期望姿态作比较后获得新的偏差信号，形成了测试平台的数据流回路。

2.采用权利要求1所述的针对陀螺仪随机游走噪声的精细滤波测试平台，实现的精细滤波方法，其特征在于：所述精细滤波方法是一种递推算法，该方法的滤波周期包括以下步骤：首先构建含有陀螺仪随机游走噪声的状态方程与太阳敏感器的量测方程，其次针对陀螺仪的随机游走噪声设计随机游走噪声估计器，然后构造飞行器姿态滤波器，最后结合状态估计误差方程和随机游走噪声估计误差方程求取随机游走噪声估计器增益与滤波器增益，在补偿随机游走噪声的基础上进一步抑制高斯白噪声，具体步骤如下：

第一步：设计含陀螺仪随机游走噪声的状态方程与太阳敏感器的量测方程；

状态方程含陀螺仪随机游走噪声与高斯白噪声，其状态变量是用四元数形式表示的飞行器姿态角；太阳敏感器的量测方程为含有陀螺仪随机游走噪声的状态方程提供修正信息，状态方程与量测方程为Σ₁：

${\mathrm{\Σ}}_1:\left\{\begin{array}{c}\stackrel{\·}{x}\left(t\right)=A\left(t\right)x\left(t\right)+B\left(t\right)n\left(t\right)+w\left(t\right)\\ y\left(t\right)=Cx\left(t\right)+v\left(t\right)\end{array}\right.$

其中，t表示时间，x(t)＝[q₀(t) q₁(t) q₂(t) q₃(t)]^T为四元数形式表示的飞行器姿态角；为x(t)的一阶微分；n(t)＝[n_x(t) n_y(t) n_z(t)]^T为三轴陀螺仪的随机游走噪声；w(t)＝[δ₀(t) δ₁(t) δ₂(t) δ₃(t)]^T为四元数形式表示的陀螺仪高斯白噪声；y(t)＝[y₀(t) y₁(t) y₂(t) y₃(t)]^T为四元数形式表示的太阳敏感器测得的输出变量，v(t)＝[v₀(t) v₁(t) v₂(t) v₃(t)]^T为四元数形式表示的太阳敏感器高斯白噪声；矩阵C为量测矩阵，是四阶单位阵，系统状态矩阵A(t)为随机游走噪声系数矩阵B(t)为

w_x(t)、w_y(t)、w_z(t)分别为飞行器相对轨道坐标系的滚动轴角速率、俯仰轴角速率及偏航轴角速率；

陀螺仪连续型随机游走噪声满足数学模型：

$\stackrel{\·}{n}\left(t\right)=w_1\left(t\right)$

式中为n(t)的一阶微分，w₁(t)＝[σ_x(t) σ_y(t) σ_z(t)]^T为三轴陀螺仪高斯白噪声；

将含有陀螺仪随机游走噪声的状态方程与太阳敏感器的量测方程Σ₁离散化，可以得到如下模型Σ₂：

${\mathrm{\Σ}}_2:\left\{\begin{array}{c}x(k+1)=\stackrel{\‾}{A}\left(k\right)x\left(k\right)+\stackrel{\‾}{B}\left(k\right)n\left(k\right)+w\left(k\right)\\ y\left(k\right)=Cx\left(k\right)+v\left(k\right)\end{array}\right.$

其中x(k+1)为k+1时刻以四元数形式表示的状态变量，x(k)为k时刻以四元数形式表示的状态变量，n(k)为k时刻的陀螺仪随机游走噪声，w(k)为k时刻以四元数形式表示的陀螺仪高斯白噪声，y(k)为k时刻以四元数形式表示的太阳敏感器输出变量，v(k)为k时刻以四元数形式表示的太阳敏感器高斯白噪声，式中A(k)为k时刻的系统状态矩阵，B(k)为k时刻的随机游走噪声系数矩阵，I为单位阵，T为采样时间；

将陀螺仪连续型随机游走噪声的数学模型离散化为：

n(k+1)＝n(k)+w₁(k)

式中n(k+1)为k+1时刻的陀螺仪随机游走噪声，w₁(k)为k时刻的三轴陀螺仪高斯白噪声；

第二步：设计随机游走噪声估计器估计陀螺仪随机游走噪声；

对于上述陀螺仪的随机游走噪声，设计随机游走噪声估计器，构建的估计器模型如下：

式中为k+1时刻的随机游走噪声估计值，为k时刻的随机游走噪声估计值，K(k)为k时刻的估计器增益，为k时刻的太阳敏感器输出变量估计值，由公式求得，而由第三步k-1时刻的飞行器姿态滤波器递推得到；该模型给出了随机游走噪声估计值的递推方法，k+1时刻的随机游走噪声估计值由k时刻的随机游走噪声估计值递推更新得到，初始随机游走噪声估计值可设为第三步：构造抵消陀螺仪随机游走噪声的飞行器姿态滤波器；

式中为k+1时刻的状态变量估计值，为k时刻的状态变量估计值，L(k)为k时刻的飞行器姿态滤波器增益；该飞行器姿态滤波器模型给出了状态变量估计值的递推方法，k+1时刻的状态变量估计值由k时刻的状态变量估计值递推更新得到，初始状态估计值可设为

第四步：结合状态估计误差方程和随机游走噪声估计误差方程，求解估计器增益与滤波器增益；

令与通过将含陀螺仪随机游走噪声的离散化状态方程与飞行器姿态滤波器作差得到状态估计误差方程，将随机游走噪声离散化模型与随机游走噪声估计器作差得到随机游走噪声估计误差模型，如下式Σ₃：

${\mathrm{\Σ}}_3:\left\{\begin{array}{c}\tilde{x}(k+1)=\stackrel{\‾}{A}\left(k\right)\tilde{x}\left(k\right)+\stackrel{\‾}{B}\left(k\right)\tilde{n}\left(k\right)-L\left(k\right)C\tilde{x}\left(k\right)-L\left(k\right)v\left(k\right)+w\left(k\right)\\ \tilde{n}(k+1)=\tilde{n}\left(k\right)-K\left(k\right)C\tilde{x}\left(k\right)-K\left(k\right)v\left(k\right)\end{array}\right.$

可以写成如下形式：

$\left[\begin{array}{c}\tilde{x}(k+1)\\ \tilde{n}(k+1)\end{array}\right]=\left[\begin{array}{cc}\stackrel{\‾}{A}\left(k\right)-L\left(k\right)C& \stackrel{\‾}{B}\left(k\right)\\ -K\left(k\right)C& I\end{array}\right]\left[\begin{array}{c}\tilde{x}\left(k\right)\\ \tilde{n}\left(k\right)\end{array}\right]+\left[\begin{array}{cc}I& -L\left(k\right)\\ 0& -K\left(k\right)\end{array}\right]\left[\begin{array}{c}w\left(k\right)\\ v\left(k\right)\end{array}\right]$

利用MATLAB中的LMI工具箱求取随机游走噪声估计器增益K(k)与飞行器姿态滤波器增益L(k)，使得状态估计误差与随机游走噪声估计误差均能收敛；

第五步：判断终止条件，若满足，则滤波方法运行结束，若不满足，则循环调用；

终止条件为滤波方法的总运行时间，当该方法运行时间没有达到总运行时间，则更新时间，以下一个离散时刻k+1代入第一步，循环调用该滤波算法；若满足终止条件，则退出该方法。