CN113029127A - 基于分布式循环结构的飞行器自主姿态估计方法 - Google Patents

Abstract

本发明为了解决基于微机械陀螺/TAM姿态估计方法在线运行困难，精度较低的问题，提出了一种基于分布式循环结构的飞行器自主姿态估计方法。首先，建立自主姿态估计滤波器并将微机械陀螺和无校正误差的三轴磁强计数据代入，求得当前时刻飞行器的姿态；然后，利用已求解的当前时刻姿态矩阵建立TAM校正***；最后，将TAM校正***输出后无误差的数值重新输入姿态估计***，对下一时刻的姿态进行估计。

Description

基于分布式循环结构的飞行器自主姿态估计方法

技术领域

本发明属于飞行器姿态估计领域，涉及一种基于分布式循环结构的飞行器自主姿态估计方法。

背景技术

全球卫星定位***自应用以来取得了巨大的成就。但是随着反制技术的发展，其隐蔽性差、易受敌方电子战干扰的突出缺点也在多场局部战争中显露无疑。因此为了能进一步适应当前战争的形式，发展一种不需要数据链通信、可靠的自主导航方式已成为亟待解决的问题。微机械陀螺仪工作过程中不需要发射和接收信号，隐蔽性好、短期精度高、可提供全天候连续实时导航信息，已成为导航***的必选项。但同时具有积累误差，需要其他辅助信息及时修正。三轴磁强计(Three Axis Magnetometer，TAM)是目前常用的矢量磁测量传感器。与微机械陀螺仪组合后有更大的优势，惯保持了纯惯性导航设备全天候、连续导航的特点；被动测量，有很好的隐蔽性，避免通信时暴露和受到敌方诱骗、干扰；***精度高，能够为载体提供姿态、航向和位置等多维度高精度导航信息，在军事、安保等领域有很大的应用潜力。

但是，由于这两种传感器精度相对较低，如何获得较高的姿态精度是需要考虑的问题。TAM实际工作环境和线下测试时有很大区别，极端的温度梯度、机械压力、复杂的电磁环境等均对传感器的状态有较大影响，对姿态精度起着重要的作用。一些实际案例也证实了对TAM进行在线校正以确保获得一定精度的姿态确定结果来说是非常必要的。但对于在线校正问题，以上方法是将参数化成额外增加的状态。这样一个全阶滤波器的实现会有以下几个方面的困难：(1)状态向量的维数较大，会给实时操作带来计算负担；(2)状态包含快速变化项和缓变项，滤波器参数很难调节；(3)由于TAM误差校正参数较好的可观测性，所以量测更新能够更好的调节它，这将导致尺度因素和偏差对量测噪声较敏感，而姿态则反应较慢。可见，如何在姿态估计的同时更好地在线校正TAM误差参数是当前需要解决的一个关键问题。

针对现有微机械陀螺/TAM姿态估计***的不足，本发明提出一种基于分布式循环结构的飞行器自主姿态估计新方法。该方法将姿态估计和传感器校正分为两部分，通过这种分布式双循环的结构降低***维数，提高姿态估计精度。

发明内容

本发明的目的在于突破微机械陀螺以及TAM精度较低，TAM易受电磁环境干扰，在线估计状态维数较多的问题，提出一种基于分布式循环结构的飞行器自主姿态估计新方法。其中要解决的问题包括：

(1)状态向量的维数较大，会给实时操作带来计算负担；

(2)状态包含快速变化项和缓变项，滤波器参数很难调节；

(3)由于TAM误差校正参数较好的可观测性，所以量测更新能够更好的调节它，这将导致尺度因素和偏差对量测噪声较敏感，而姿态则反应较慢。

本发明所述的基于分布式循环结构的飞行器自主姿态估计方法，其特征在于包括以下技术措施：

步骤一：利用离散四元数动力学方程建立飞行器姿态估计***状态方程和三轴磁强计测量模型建立姿态估计***观测方程，从而完整构建飞行器姿态估计***，并求解该姿态估计***，从而得到当前时刻的姿态四元数、姿态转换矩阵、三轴姿态，利用当前时刻姿态矩阵将当地地磁场矢量理论值转化到载体坐标系下，将转化到载体坐标系下的当地地磁场矢量值与校正后的三轴磁强计测量值做差，得到地磁场残差向量；

步骤二：利用加入测量噪声的残差向量构建三轴磁强计校正***观测方程，以及由尺度因素误差和零偏两个误差参数作为***状态构建的三轴磁强计校正***状态方程，由此构建完整的三轴磁强计校正***，并利用UKF滤波器估计该***当前时刻的状态，也即误差参数；

步骤三：利用估计出的当前时刻的误差参数校正三轴磁强计测量值，并将校正好的测量值代入飞行器姿态估计***，重新估计三轴姿态。

对比现有技术，本发明所述的基于分布式循环结构的飞行器自主姿态估计方法，有益效果在于：

1)将整体***分为姿态估计***和传感器校正***，降低了每个滤波器的维数，减少了计算量和实时操作的负担；

2)将整体***分为姿态估计***和传感器校正***，使得快速变化项和缓变项能够分开，易于参数调节；

3)在姿态估计的同时，能够校正TAM误差参数，降低机载电磁环境对TAM测量的影响，提高***精度。

4)采用微机械陀螺和TAM构建起组合姿态估计***，在显著降低成本的同时，实现自主姿态估计，提高***隐蔽性、抗诱骗的能力。

附图说明

附图1是基于分布式循环结构的飞行器自主姿态估计方法步骤流程图；

附图2是分布式循环***结构图。

具体实施方式

为了解决基于微机械陀螺/TAM姿态估计方法在线运行困难，精度较低的问题，提出了一种基于分布式循环结构的飞行器自主姿态估计方法。首先，建立自主姿态估计滤波器并将微机械陀螺和无校正误差的三轴磁强计数据代入，求得当前时刻飞行器的姿态；然后，利用已求解的当前时刻姿态矩阵建立TAM校正***；最后，将TAM校正***输出后无误差的数值重新输入姿态估计***，对下一时刻的姿态进行估计。

以下结合说明书附图1对本发明做进一步的详细描述。参照说明书附图1，本发明的处理流程分以下步骤：

(1)当前时刻姿态四元数的一步预测

定义姿态四元数q为一四维向量：

q≡[q₀ ρ^T]^T，

其中ρ≡[q₁ q₂ q₃]^T为向量部分；q₀为标量部分，有

由四元数q表示的姿态矩阵A(·)如下：

其中，I_3×3为3×3的单位矩阵，<ρ×>为斜对称矩阵。

四元数微分方程为

本文采用光纤陀螺提取角速度ω，其输出模型可以表示为

ω＝ω-b-η_a (2)

其中，ω_g为实际陀螺输出，ω为真实值，b为陀螺漂移，η_a、η_b分别是均值为0，方差为

和

的高斯白噪声。

方程(1)、(2)、(3)可采用一阶龙格库塔法进行离散化。

(2)姿态估计***观测方程的构建

在载体坐标系下的磁场向量B_k和地理坐标系下磁场向量H_k的关系为

其中，

为当前k时刻的姿态四元数一步预测值，A(·)为从地理坐标系到载体坐标系的姿态转换矩阵，B_k为在载体坐标系下的三轴磁强计测量值，H_k为地理坐标系下磁场向量值，v_k为测量噪声，

(3)基于滚动时域估计的飞行器姿态估计

通过陀螺和TAM建立姿态估计***模型之后，就需要考虑如何能根据传感器的数据得到飞行器当前最优姿态估计。考虑到四元数的归一化性质，本发明建立了了带有等式约束的滚动时域估计模型。它的基本思想为，只考虑当前时刻以前固定数量的数据，同时将历史数据对估计的影响用一种方法来近似描述，则每次参与优化的数据量不变，这种方法实现起来就像一个滚动的观测“窗口”。这个“窗”包含过去有限的量测值，“窗”的最后一个输出为当前状态估计。

窗口最后的一个点(i＝L)代表现在时刻k，为方便起见，表示窗口中的某时刻时，直接采用L2，…，L表示。带等式约束的完整MHE问题可表示为

x_i+1＝f_i+1，i(x_i，u_i)+w_i，for i＝1，…，L-1 (5)

y_i+1＝h_i+1(x_i+1)+v_i+1，for i＝1，…，L-1 (6)

l_i(x_i)＝0，for i＝1，…，L (7)

其中，x_i是待估计的状态。式(5)描述了在离散情况下的状态递推过程。在该递推方程中u_i是已知输入，它的过程噪声为w_i，过程噪声方差为Q_i。方程(6)为量测方程，y_i为量测值，v_i、R_i分别为量测噪声和量测噪声方差。在时域内状态

和方差

是对历史信息的总结。式(8)中c_nt，k为代价函数，它描述了到达代价。值得注意的是i＝1时刻的量测值并没有包含在代价函数中。假定从量测中得到的所有信息已经包含在先验估计

中。等式约束l_i由式(7)给出。MHE问题的输出为x_L：时域“窗”中最后一个时刻的估计状态向量。

这就是一个带权重的最小二乘问题，在该问题中权重就是方差阵的逆：

采用二范数记法：

把方程(5)、方程(6)、方程(7)带入到代价函数(4)中，得到下式：

s.t.

l_i(x_i)＝0，for i＝1，…，L

这样就将状态估计问题转化成了非线性规划(NPL)问题，可以用数值优化的方法求解，代入陀螺数据，以及经过校正的三轴磁强计的测量值B′_m，k，能够解得当前时刻的四元数最优估计值、姿态矩阵以及飞行器三轴姿态。姿态估计过程以磁强计测量周期ΔT进行更新。

(4)三轴磁强计测量残差的求解

制约磁强计精度的各种误差因素会引起姿态估计误差，因此必须在线估计传感器的参数以排除传感器误差的影响。传感器校正过程是通过使用姿态估计阶段的残差来估计k时刻的尺度因素s_k和零偏d_k。误差参数定义为Θ_k：

Θ_k＝[s_x，k s_y，k s_z，k d_x，k d_y，k d_z，k]^T

令

为滤波后的四元数最优估计值，B_m，k为磁强计在k时刻的测量值。假设离散区间内角速率保持恒定，该时刻的磁场三分量测量误差向量Δy_k即为当地地磁场向量在机体坐标系下的理论值

与经过误差校正的三轴磁强计测量值

之差

(5)三轴磁强计误差参数的估计

将Δy_k积累一定数量之后就可以用来校正传感器。令n为一个循环中积累的量测值数量，那么量测方程可以写为

z_i＝Δy_i+r_i，i＝1，…，n (9)

其中，z_i是在一个循环内积累的第i个时刻的误差，Δy_i为地磁场残差向量，r_i是相应的噪声，它对姿态估计误差也有影响。

在离散形式下，传感器校正***的状态方程为

其中，b_i是一个过程噪声项，用来表示尺度因素和零偏的变化量。对于通过式(9)和式(10)建立的传感器校正***可以用采用UKF来完成滤波。

在上式中，尺度因数s_i和零偏d_i这两个估计量可以用校正过的地面测试数据初始化，并且在载体运行阶段通过传感器校正过程进行修正，最终得到当时时刻的最优估计值

(6)三轴磁强计测量值的校正

当获得了最优估计值

之后，即可对三轴磁强计的测量值进行校正，方法如下：

得到校正后的三轴磁强计测量值B′_k，m。将该值代入姿态估计***循环，可以估计飞行器姿态。

Claims (5)

1.基于分布式循环结构的飞行器自主姿态估计方法，其特征在于，包括以下步骤：

步骤一：利用离散四元数动力学方程建立飞行器姿态估计***状态方程和三轴磁强计测量模型建立姿态估计***观测方程，从而完整构建飞行器姿态估计***，并求解该姿态估计***，从而得到当前时刻的姿态四元数、姿态转换矩阵、三轴姿态，利用当前时刻姿态矩阵将当地地磁场矢量理论值转化到载体坐标系下，将转化到载体坐标系下的当地地磁场矢量值与校正后的三轴磁强计测量值做差，得到地磁场残差向量；

步骤二：利用加入测量噪声的残差向量构建三轴磁强计校正***观测方程，以及由尺度因素误差和零偏两个误差参数作为***状态构建的三轴磁强计校正***状态方程，由此构建完整的三轴磁强计校正***，并利用UKF滤波器估计该***当前时刻的状态，也即误差参数；

步骤三：利用估计出的当前时刻的误差参数校正三轴磁强计测量值，并将校正好的测量值代入飞行器姿态估计***，重新估计三轴姿态。

2.根据权利要求1所述的基于分布式循环结构的飞行器自主姿态估计方法，其特征在于，步骤一中姿态估计***观测方程构建方法为：

其中，

为当前k时刻的姿态四元数一步预测值，A(·)为从地理坐标系到载体坐标系的姿态转换矩阵，B_k为在载体坐标系下的三轴磁强计测量值，H_k为地理坐标系下磁场向量值，v_k为测量噪声。

3.根据权利要求1所述的基于分布式循环结构的飞行器自主姿态估计方法，其特征在于，步骤一中飞行器姿态估计***求解方法为：

将离散的飞行器姿态估计***代入滚动时域估计模型，采用高斯-牛顿迭代法并结合MEMS速率陀螺和校正好的三轴磁强计测量值求解该滚动时域估计模型，从而得到当前时刻的姿态四元数、姿态转换矩阵、三轴姿态。

4.根据权利要求1所述的基于分布式循环结构的飞行器自主姿态估计方法，其特征在于，步骤二中三轴磁强计校正***观测方程构建方法为：

令n为一个循环中积累的量测值数量，那么将观测方程写为

z_i＝Δy_i+r_i,i＝1,…,n

其中，z_i是在一个循环内积累的第i个时刻的误差，Δy_i为地磁场残差向量，r_i是相应的噪声。

5.根据权利要求1所述的基于分布式循环结构的飞行器自主姿态估计方法，其特征在于，步骤二中三轴磁强计校正***状态方程构建方法为：

其中，s_i为尺度因素，d_i为零偏，b_i是一个过程噪声项，用来表示尺度因素和零偏的变化量。

Images