CN111578931B - 基于在线滚动时域估计的高动态飞行器自主姿态估计方法 - Google Patents

Abstract

本发明属于飞行器姿态估计滤波技术领域，为了解决滚动时域估计方法估计姿态时实时性差、在线应用困难的问题，提出了一种适合于高动态飞行器的陀螺/三轴磁强计在线滚动时域自主姿态估计方法。首先，建立基于陀螺/三轴磁强计的滚动时域自主姿态估计模型；然后，标称模型预测量测值，由预测的量测值计算理想滚动时域估计问题的解；最后，在得到实际三轴磁强计量测值之后，由非线性规划灵敏度公式快速计算出滚动时域估计问题的修正解。

Description

基于在线滚动时域估计的高动态飞行器自主姿态估计方法

技术领域

本发明属于飞行器姿态估计滤波技术领域，更具体地，本发明涉及一种基于在线滚动时域估计的高动态飞行器自主姿态估计方法。

背景技术

全球卫星定位***是一种全天候、全球覆盖、三维定速定时高精度导航定位***，自应用以来取得了巨大的成就，发展一种不需要数据链通信、可靠的自主导航方式已成为亟待解决的问题。

陀螺仪作为一种自主导航方式，不需要发射和接收信号，隐蔽性好、短期精度高、可提供全天候连续实时导航信息，已成为导航***的必选项。但同时具有积累误差，需要其他辅助信息及时修正的缺点。而其他辅助的自主导航方式也有各自的瓶颈，如视觉导航、地形导航在跨海飞行时均有海面地形特征不明显，组合滤波易发散的缺点；重力场变化及其微小，传感器难以测量且造价昂贵，重力导航难以实现；星光导航受天气影响较大，可靠性较低。

地磁场是一个矢量场，是地球固有资源，变化丰富，为航海、航空、航天提供了天然坐标系。利用磁探测技术测量载体坐标系下地磁场的各个信号分量，并与地磁模型进行比对，通过计算可以获得飞行器的空间姿态。其中，三轴磁强计(Three Axis Magnetometer，TAM)是目前常用的矢量磁测量传感器。由于它们成本低、低功耗、可靠性高、抗电子战干扰能力强、并且具有抗高过载能力和误差不随时间积累等优点，已引起了高度关注和广泛研究。

考虑到陀螺仪和磁强计互补的特点，可同时在飞行器上安装提供角速度测量值的陀螺仪和提供磁场向量观测值的TAM来估计飞行器姿态。这两种传感器组合后有更大的优势：惯保持了纯惯性导航设备全天候、连续导航的特点；被动测量，有很好的隐蔽性，避免通信时暴露和受到敌方诱骗、干扰；***精度高，能够为载体提供姿态、航向和位置等多维度高精度导航信息，在军事、安保等领域有很大的应用潜力。

通过陀螺仪、TAM的模型可以建立由四元数表达的非线性姿态估计***。对于线性***的状态估计卡尔曼滤波已经能够很好地解决，但是在非线性***中，传统基于扩展卡尔曼(Extended Kalman Filtre，EKF)和无迹卡尔曼(Uncensted Kalman Filter，UKF)的滤波方法在求解非线性问题时存在不能考虑四元数归一化约束、非显示求解、只能得到次优结果、精度不高、计算复杂甚至导致滤波发散等问题。常用的滚动时域估计(MovingHorizon Estimation，MHE)方法，通过将估计问题转化成由代价函数描述的优化问题进行求解，这样就可以显式地表示约束，***地求解。通常这些约束也更能反映实际情况，因此可以提供更多信息，缩小估计器的搜索空间，提高搜索效率，通过时域内包含更多数据来减少对先验概率密度函数和错误初始化的敏感性。

但同时它的缺陷是必须得到动态优化问题的在线解。到目前为止，由于计算量大，MHE的在线运行仍然是一个挑战，这个问题也在很大程度上限制了它的实际应用，特别是对高动态飞行器难以满足其实时性要求。对于这个问题，在模型预测控制中的处理方法为把计算任务分为准备阶段和反馈阶段两个部分。根据该思想，本发明利用陀螺更新速率远高于TAM的特点，将其拓展到MHE问题中。首先利用姿态估计***模型和陀螺数据预测量测值，在此基础上求解MHE问题，然后在获得真实TAM量测值之后，借用非线性规划(NonlinearProgramming，NLP)灵敏度的概念，快速修正MHE之前的解。通过这种方式，根据传感器更新速率不同划分计算单元，减少在线等待时间以期满足高动态实时在线应用场景。

发明内容

本发明的目的在于突破传统滚动时域估计方法估计姿态时实时性差、在线应用困难的问题，提出一种基于在线滚动时域估计的高动态飞行器自主姿态估计方法，利用传感器更新速率不同的特点，减小姿态估计时的在线等待时间。其中解决的主要问题包括：

(1)卫星定位***隐蔽性差、易受敌方电子战干扰，不能实现自主运行；

(2)经典卡尔曼滤波算法不能以显式求解四元数归一化约束；

(3)传统的滚动时域估计方法计算量大，难以在线运行、满足实时性要求；

(4)KKT矩阵为非带对角结构，由于维数大，占用存储空间多，求逆运算时计算量大。

本发明所述的基于在线滚动时域估计的高动态飞行器自主姿态估计方法，其特征在于包括以下技术措施：

步骤一、将当前时刻的陀螺数据带入离散化后的姿态动力学模型，求得下一时刻的姿态四元数，并带入由三轴磁强计建立的无扰动的量测方程，来预测下一时刻的三轴磁强计量测值，将预测的量测值、过去一段时间已知的量测、窗口第一点的状态以及它的状态协方差定义为一个扩展数据向量；

步骤二、将扩展数据向量带入滚动时域估计模型，并利用高斯牛顿迭代法对其进行求解，从而得到KKT***，在KKT***等式两边乘以KKT矩阵的逆，得到近似的姿态估计值；

步骤三、利用滚动时域估计模型，将姿态估计问题转化成非线性规划问题，并根据拉格朗日定理将其表示成关于未知状态的隐函数，利用隐函数定理计算得到灵敏度矩阵；

步骤四、将下一时刻三轴磁强计的真实量测值、近似姿态估计值以及灵敏度矩阵带入解的一阶估计模型，求得修正后的姿态估计值。

对比现有技术，本技术方案所述的基于在线滚动时域估计的高动态飞行器自主姿态估计方法，有益效果在于：

(1)构建基于陀螺/三轴磁强计的滚动时域自主姿态计算模型，实现了***的自主运行，避免了通信链路受干扰的问题；

(2)解决了经典卡尔曼框架下滤波算法不能考虑四元数归一化约束的问题；

(3)减少了传统滚动时域估计方法计算量大，存在延时，难以在线运行的问题，提高了方法实时性；

(4)KKT转化为带对角结构以后，采用稀疏矩阵的方式记录，占用存储空间小，且求逆时计算简便。

附图说明

附图1是基于在线滚动时域估计的高动态飞行器自主姿态估计方法实施流程图；附图2是滚动时域估计方法基本原理图；

附图3是非带对角结构的KKT矩阵结构图；

附图4是具有带对角结构的KKT矩阵结构图。

具体实施方式

为了解决滚动时域估计方法估计姿态时实时性差、在线应用困难的问题，提出了一种适合于高动态飞行器的陀螺/三轴磁强计在线滚动时域自主姿态估计方法。首先，建立基于陀螺/三轴磁强计的滚动时域自主姿态估计模型；然后，标称模型预测量测值，由预测的量测值计算理想MHE问题的解；最后，在得到实际三轴磁强计量测值之后，由NLP灵敏度公式快速计算出MHE问题的修正解。

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

(1)扩展数据向量的构建

①状态方程的建立

假设姿态四元数向量q为

其中，e＝[e_x e_y e_z]^T为主旋转轴，α表示参考坐标系到载体系旋转的角度。通过该方法使四元数完全并且唯一地描述刚体在参考坐标系下的方向。

此时，姿态矩阵A(有时称为姿态余弦矩阵)用四元数可表示为

其中，矩阵[q_1:3×]为差乘反对称矩阵；I_3×3是3×3的单位矩阵。

四元数动力学方程可表示为

其中，ω＝[ω_x,ω_y,ω_z]^T为载体坐标系下的角速度。该四元数动力学方程在任意旋转时都不会出现奇点。

载体上通过陀螺仪来测量角速度。陀螺仪模型可表示为

ω^m(t)＝ω(t)+β(t)+η_v(t)， (4)

其中，ω^m为陀螺角速度测量值，η_v是陀螺的零均值高斯白噪声，陀螺的偏差β建模为随机游走

其中，δ(t)为Dirac delta函数，

为噪声方差。

为了计算机求解必须将模型离散化，式(3)离散化后可表示为

其中，Δt为陀螺采样周期。由于ω有三个分量而q有四个，因此q必须满足归一化约束||q||＝1以定义ω和q之间的关系。

对于漂移偏差，前向欧拉离散化方法为

β_k+1＝β_k+η_u,k (8)

此时，可将状态方程(7)简写为

x_k+1＝f_k+1,k(x_k,u_k)+w_k,k＝0,1,… (9)

其中，

是待估计的姿态四元数q，u_k是已知输入，过程噪声为

过程噪声方差为Q_k。

②观测方程的建立

在每个离散时刻k的TAM量测模型为

B_m,k＝A(q_k)H_k+v_k (10)

其中，H_k为参考坐标系下的地磁场向量；B_m,k为载体坐标系下的地磁场量测向量，量测噪声v_k为零均值的高斯白噪声，方差为R_k。此时，可将观测方程简写为

y_k+1＝h_k+1(x_k+1)+v_k+1,k＝0,1,… (11)

其中，

为量测值，

R_k分别为量测噪声和量测噪声方差。

通过这种方式，构建了完整的陀螺/TAM自主姿态估计***模型。

③带有四元数等式约束的MHE姿态估计***的建立

通过陀螺和TAM建立姿态估计***模型之后，就需要考虑如何能根据传感器的数据得到飞行器当前最优姿态估计。经典卡尔曼滤波框架下，非线性姿态估计***存在不能显式求解四元数归一化约束、精度不高、计算复杂甚至导致滤波发散等问题。而MHE除了考虑约束之外，和广为人知的次优非线性状态估计相比，就精度和鲁棒性而言，非线性MHE具有更好的性能，因此本发明采用基于MHE框架的姿态估计方法。

MHE只考虑当前时刻以前固定数量的数据，同时将历史数据对估计的影响用一种方法来近似描述，则每次参与优化的数据量不变，这种方法实现起来就像一个滚动的观测“窗”。这个“窗”包含过去有限的量测值，“窗”的最后一个输出为当前状态估计。设“窗”口长度为L(i＝1,2,…,L)，其中“窗”口最后的一个点(i＝L)代表现在时刻k。为了方便，下文在符号标记时省略了k，如状态x_ik表示k时刻“窗”口的第i个点，简计为x_i。由于四元数姿态估计只包含四元数归一化等式约束，本发明只建立包含等式约束的MHE，可表示为

s.t.

x_i+1＝f_k+1,k(x_i,u_i)+w_i,for i＝1,…,L-1, (12b)

y_i+1＝h_k+1(x_i+1)+v_i+1,for i＝1,…,L-1, (12c)

l_i(x_i)＝0,for i＝1,…,L. (12d)

其中，在时域内的第一个时刻点为

方差为P₁ ⁰。代价函数(12a)中的第一项为到达代价。值得注意的是i＝1时刻的量测值并没有包含在代价函数J中，因为假定从量测中得到的所有信息已经包含在先验估计

中。等式约束由式(12d)给出。至此即已建立基于陀螺/TAM的MHE姿态估计***模型。

定义MHE问题的数据向量为

(2)近似的姿态估计值的计算

式(12a)可以写为

s.t.

l_i(x_i)＝0,for i＝1,…,L。 (14b)

其中，Γ(x₁)和

为(12a)中相应的项。

这样基于陀螺/TAM的MHE姿态估计就转化为了一个NLP优化问题。现在的任务即为在包含L个历史信息的“窗”口中寻找使代价函数J取得极小值时的最优状态序列

并从该序列中提取***当前状态

同时通过“窗”的移动不断更新当前时刻的状态。

在下一时刻，当得到新的量测y_k+1时，可以通过丢掉旧的测量值y_k，纳入y_k+1，来使测量值向量向前移动一个采样时刻，获得下一时刻的量测向量{y₃,…,y_L+1}。此外，先验状态估计通过定义

进行更新，状态方差通常采用EKF的更新公式进行更新。得到新的数据向量η(k+1)之后，就可以求解新的MHE问题U_L(η(k+1))，从而得到下一时刻的状态

具体原理建附图2。

在实际操作过程中，由于自由度数量巨大以及病态方程导致的非凸性使得MHE问题U_L(η(k+1))计算复杂，无法直接求解。在建立了基于陀螺/TAM的姿态估计MHE问题的代价函数U_L(η)的基础上，本发明进一步对其求解方法进行设计。

①基于高斯-牛顿迭代的一步求解法

由于现已将状态估计问题转化成了非线性规划问题，这时可以采用数值优化的方法求解。定义参数p＝η(·)，则问题U_L(p)的拉格朗日函数可写为

其中，

是拉格朗日乘子向量。

此时，对该非线性最优化问题，我们采用高斯牛顿迭代法，通过沿着当前轨迹线性化KKT条件来计算搜索方向

来求解。最终可得到线性的KKT***

其中，上式左边第一项即为KKT矩阵，并表示成如下形式

根据代价函数(12)计算可得到

其中，

A_a＝[-1,0,…,0]， (28)

最终，该优化问题的解是

该优化问题的解只需要一次矩阵因式分解，即式(31)。此时，单步MHE问题的求解被限制在一步之内，那么计算时间就是有限和可预测的，满足了实时性的初步要求。

②KKT矩阵的带对角化

此外，从式(18)、(19)、(22)、(24)、(25)、(27)、(28)、(30)可以看出矩阵H_ssin和矩阵A_eq具有带对角结构。然而，从式(17)可以看出需要因式分解的矩阵K不具备带对角结构，如附图3所示。如果，我们将变量的记录方式从

变为

那么，矩阵K就会出现带对角结构，如附图4所示。

具有带对角结构的矩阵K可采用稀疏矩阵记录。计算机运行时因式分解附图4的稀疏矩阵K要比常规矩阵记录方法速度快的多。因此，本发明采用式(33)的变量记录方式。在计算机运行时采用MATLAB语言中的稀疏矩阵的方式存储KKT矩阵。

这样，通过构造具有带对角结构的高斯牛顿迭代法即可求得该MHE姿态估计问题的解。然而，按这种方式运行，需要等到获得陀螺和TAM全部的信息以后才能再求解整个问题。事实上由于状态维数多，求解过程复杂，由必然会因计算量庞大而产生延迟，对于在线应用带来一定的困难，特别是高动态飞行器将无法满足实时需求。因此，下面进一步消除在线等待时间，提高实时性。

(3)KKT***灵敏度矩阵的计算

在陀螺/TAM自主姿态估计***中，陀螺更新速率远大于TAM，并且它们的更新时间并不同步，因此本发明考虑利用这一特点，将姿态估计计算分为两部分。首先由陀螺数据和***方程预测量测值，根据上文设计的MHE求解方法得到近似解，然后在获得真实TAM量测值后快速修正该近似解，从而大幅缩减在线等待时间。

根据函数L即式(15)可知，所有的原始变量以及乘子变成了关于p的隐函数：

其中，解向量定义为

并且式(34)有优化解s^*(p,L)。

现在研究扰动|p-p₀|对一个给定标称解s^*(p₀,L)邻域的影响。首先

假设(NLP灵敏度假设)

·f(·)、E(·)、Γ(·)对于所有变量二阶连续可微。同时，这些函数和它的微分有界；

·U_L(p₀)的标称解s^*(p₀,L)满足线性独立约束条件(LICQ)，充分二阶条件(sufficient second order conditions，SSOC)和严格的互补松弛性条件。

SSOC要求在点s^*(p₀,L)计算的拉格朗日函数L的Hessian矩阵在任何可行的方向上都是正定的。由于该假设的存在，解向量s^*(p,L)在标称解s^*(p₀,L)的邻域内是连续可微函数并且

有界且唯一。

那么在这个假设前提下，式(34)即可由隐函数定理推导得

其中，K^*(p₀,L)表示问题U_L(p₀)在点s^*(p₀,L)的KKT矩阵。同时，如果标称解满足假设，那么KKT矩阵是非奇异的，这样就可以用来计算式(35)中的灵敏度矩阵

(4)修正后的姿态估计值的计算

此时，邻域问题的解可写为

该解的快速一阶估计可表示为

其中，

是s^*(p,L)的近似解。通过式(37)即可将MHE问题拆分求解。由于TAM更新速率较慢，在未获得TAM真实量测值之前，可由***方程预测量测值，通过上节建立的基于高斯牛顿迭代的MHE问题求解方法求得近似解s^*(p₀,L)以及灵敏度矩阵

一旦获得TAM量测值后，即可更新数据向量，由式(37)求得MHE问题的修正解。通过这种方式，以TAM量测值的获取时间为界，将该自主姿态估计问题的求解划分为两部分，避免获得所有传感器数据后再计算庞大的MHE问题，减小在线等待时间。

从假设和式(23)可知，

满足

其中，L_s为正常数，|·|为Euclidean范数。同时，如果采用以上估计策略，NLP灵敏度扰动为

因此近似误差为

此外，由于扰动非常小，牛顿迭代的扰动近似误差也是同阶无穷小。

Claims (3)

1.基于在线滚动时域估计的高动态飞行器自主姿态估计方法，其特征在于包括以下步骤：

步骤一、将当前时刻的陀螺数据带入离散化后的姿态动力学模型，求得下一时刻的姿态四元数，并带入由三轴磁强计建立的无扰动的量测方程，来预测下一时刻的三轴磁强计量测值，将预测的量测值、过去一段时间已知的量测值、窗口第一点的状态以及它的状态协方差定义为一个扩展数据向量；

所述离散化后的姿态动力学模型为：

其中，Δt为陀螺采样周期；

所述扩展数据向量为：

其中，η(k)为扩展数据向量；

为时域内的第一个时刻点；P₁ ⁰为方差；

步骤二、将扩展数据向量带入滚动时域估计模型，并利用高斯牛顿迭代法对其进行求解，从而得到KKT***，在KKT***等式两边乘以KKT矩阵的逆，得到近似的姿态估计值；

所述滚动时域估计模型为：

s.t.

x_i+1＝f_k+1,k(x_i,u_i)+w_i,for i＝1,…,L-1

y_i+1＝h_k+1(x_i+1)+v_i+1,for i＝1,…,L-1

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

其中，J为代价函数；

所述KKT***为：

步骤三、利用滚动时域估计模型，将姿态估计问题转化成非线性规划问题，并根据拉格朗日定理将其表示成关于未知状态的隐函数，利用隐函数定理计算得到灵敏度矩阵；

步骤四、将下一时刻三轴磁强计的真实量测值、近似姿态估计值以及灵敏度矩阵带入解的一阶估计模型，求得修正后的姿态估计值；

所述解的一阶估计模型为：

其中，

为修正后的姿态估计值；s^*(p₀，L)为近似的姿态估计值；

为灵敏度矩阵；p为包括真实量测值的扩展数据向量；p₀为包括预测的量测值的扩展数据向量。

2.根据权利要求1所述的基于在线滚动时域估计的高动态飞行器自主姿态估计方法，其特征在于步骤二中KKT矩阵的记录方式为：

KKT***中待求变量的记录顺序变为状态变量与拉格朗日乘子交替排列，即

其中，

表示窗口内第i个状态变量，i＝1，…，L，L表示窗口的长度；

表示窗口内第i个拉格朗日乘子向量，然后根据变量的记录顺序调整KKT矩阵的记录方式。

3.根据权利要求1所述的基于在线滚动时域估计的高动态飞行器自主姿态估计方法，其特征在于步骤二中KKT矩阵的记录方式为：

在计算机运行时采用MATLAB语言中的稀疏矩阵的方式存储KKT矩阵。

Images