[发明专利]非完全自由度惯性平台关键参数多位置最优估计检测方法

权利要求书

1.非完全自由度惯性平台关键参数多位置最优估计检测方法，其特征在于：包括如下步骤：

步骤一、将东北天地理坐标系中的重力加速度和地球自转角速度转换到平台坐标系中；

地速在地理坐标系中表示为

地速在平台坐标系中表示为

其中，n表示地理坐标系，p表示平台坐标系；α，β，γ分别为平台绕X，Y，Z轴的转角，M₁(α)，M₂(β)，M₃(γ)为平台绕X，Y，Z轴转动的转移矩阵；

地球自转角速度ω_e在地理坐标系中表示为

其中，ω_ex＝0， 为地理纬度；

步骤二、在完成坐标系转换后，根据误差角的动态方程、陀螺漂移误差模型建立非完全自由度惯性平台的Kalman滤波状态方程；取加速度计及角度传感器输出作为观测量并结合加速度计线性输出模型，推导Kalman滤波的观测方程；

误差角动态方程如下所示

其中，φ_x，φ_y，φ_z分别为平台绕X轴，Y轴和Z轴的误差角；ε_x，ε_y，ε_z 分别表示X陀螺，Y陀螺和Z陀螺的漂移；其具体形式如下

X陀螺仪漂移误差模型为

Y陀螺仪漂移误差模型为

Z陀螺仪漂移误差模型为

上三式中，ε_gx，ε_gy，ε_gz分别为X陀螺，Y陀螺和Z陀螺的随机漂移；k_g0i为陀螺仪零漂，k_g11i为对应的陀螺仪输入轴一次项系数，k_g12i为对应的陀螺仪自转轴一次项系数，k_g13i为对应的陀螺仪输出轴一次项系数(i表示x或y或z)；

因此得到非完全自由度惯性平台系统状态方程如下

其中φ＝[φ_x φ_y φ_z]^T，

k_g[k_g0x k_g0y k_g0z k_g11x k_g11y k_g11z k_g12x k_g12y k_g12z k_g13x k_g13y k_g13z]^T

k_a＝[k_a0x k_a0y k_a0z k_a1x k_a1y k_a1zθ_px θ_py θ_pz θ_ox θ_oy θ_oz]^T

其中k_a0i为加表零偏，k_a1i为加表刻度因数，θ_pi，θ_oi为加表绕摆轴和输出轴的安装误差；w为高斯白噪声干扰向量；

加速度计线性输出模型为： 

平台X加速度计线性输出模型

平台Y加速度计线性输出模型

平台Z加速度计线性输出模型

e_ai为加表随机误差；

则观测方程为

其中

Z_a为三轴加速度计输出，Z_an为三轴角度传感器输出，φ₀＝[α₀ β₀ γ₀]^T为不同位置对应的转角，v为6维高斯白噪声干扰向量；

步骤三、根据所选位置运用李导数方法对步骤二得到的非完全自由度惯性平台系统的可观测性矩阵进行计算，判断系统是否完全可观；

非线性系统的Lie导数定义为

其中k为微分次数；

定义向量L为 

其中n为向量X的维数，则L对X的Jacobi矩阵为

对于多位置检测，将各位置具体参数带入(14)式得到的矩阵记为M_i，则系统的可观测性矩阵为

P＝[M₁ M₂ L M_k].

若P的秩小于n则重新选取位置后计算可观测性矩阵P；若其秩为n，则系统完全可观，进而可对全部误差系数进行检测，即进行步骤四；

步骤四，将非完全自由度惯性平台系统转至选取的位置，对加表及角度传感器输出进行连续采集，利用步骤二，步骤三得到的扩展Kalman滤波系统方程对平台误差角及各误差系数进行检测。