CN103900574A - 一种基于迭代容积卡尔曼滤波姿态估计方法 - Google Patents

Abstract

本发明公开了一种基于迭代容积卡尔曼滤波姿态估计方法，包括以下几个步骤：步骤一：采集陀螺与星敏感器的输出数据作为量测量；步骤二：确定状态向量和量测向量；步骤三：在k-1时刻利用迭代容积卡尔曼滤波估计出k时刻的误差四元数矢量部分以及陀螺漂移误差；步骤四：对于k时刻的估计求出四元数估计及陀螺漂移估计，对姿态及陀螺漂移进行校正，得到修正后k时刻姿态及陀螺漂移；步骤五：姿态估计非线性离散系统的运行时间为M，若k＝M，则输出姿态及陀螺漂移的结果；若k＜M，则重复步骤三至步骤四，直至姿态估计系统运行结束。本发明具有估计精度高、计算量少的优点。

Description

一种基于迭代容积卡尔曼滤波姿态估计方法

技术领域

本发明属于利用非线性滤波技术及迭代理论进行姿态估计的技术领域，特别涉及一种基于迭代容积卡尔曼滤波的姿态估计方法。

背景技术

飞行器姿态估计的精度直接影响飞行器姿态控制的精度，因此，姿态估计是姿态控制的关键技术之一。姿态估计方法主要分为两类：确定性算法和状态估计法。确定性算法的优点在于可以根据某一时刻的一组量测信息直接求解出该时刻下飞行器的姿态矩阵，算法简单，物理意义明确，但是需要知道当前时刻的至少两个独立的观测矢量，即准确的量测信息，而在实际应用中由于其他因素的干扰很难保证观测矢量的绝对准确。与这类方法相反，状态估计法是以状态空间模型为基础的递推滤波算法，可以将一些非姿态参数作为状态进行估计，一定程度上克服了不确定因素对姿态估计精度的影响，能够提高姿态估计的精度。

陀螺与星敏感器组成的非线性姿态估计系统由于测姿精度高，被广泛的用于飞行器中。欧拉角、修正罗德里格斯参数、方向余弦、四元数等是飞行器的主要姿态描述参数。扩展卡尔曼滤波（Extended Kalman Filter,MEKF）由于其结构简单、易于实现等优点被广泛应用于姿态估计中。但是，EKF存在一定的局限性：1）由模型线性化引入的截断误差会导致滤波精度下降，且需要计算较为复杂的雅克比矩阵；2）当初始状态误差较大或系统模型非线性程度较高时，会严重影响滤波精度甚至导致滤波发散。

发明内容

本发明的目的是为了提高姿态估计的精度，设计了一种基于迭代容积卡尔曼滤波姿态估计方法。

本发明是通过以下技术方案实现的：

一种基于迭代容积卡尔曼滤波姿态估计方法，其特征在于，包括以下几个步骤：

步骤一：采集飞行器运动过程中陀螺与星敏感器的输出数据作为量测量；

步骤二：确定系统的状态向量和量测向量；

步骤2.1建立飞行器姿态估计系统的非线性误差四元数方程，确定系统的状态向量；

非线性误差四元数方程为：

$\stackrel{\·}{x}=\left[\begin{array}{c}\mathrm{\Δ}\stackrel{\·}{\mathrm{\ρ}}\\ \mathrm{\Δ}\stackrel{\·}{\mathrm{\β}}\end{array}\right]=\left[\begin{array}{c}-[\widehat{\mathrm{\ω}}\×]\mathrm{\Δ\ρ}+\frac{1}{2}[(\mathrm{\Δ\β}+{\mathrm{\η}}_v)\×]\mathrm{\Δ\ρ}-\frac{1}{2}\mathrm{\Δe}(\mathrm{\Δ\β}+{\mathrm{\η}}_v)\\ {\mathrm{\η}}_u\end{array}\right],\mathrm{\Δe}=\sqrt{1-{\left|\right|\mathrm{\Δ\ρ}\left|\right|}_2^2}$

其中：

为陀螺角速度的估计值，Δe为误差四元数的标量部分，η_v为陀螺仪的测量噪声，η_u为陀螺漂移的测量噪声，[ω×]表示由向量ω的分量构成的反对称矩阵，

$[\mathrm{\ω}\×]=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_3& {\mathrm{\ω}}_2\\ {\mathrm{\ω}}_3& 0& -{\mathrm{\ω}}_1\\ -{\mathrm{\ω}}_2& {\mathrm{\ω}}_1& 0\end{array}\right];$

状态向量x由误差四元数矢量部分Δρ与陀螺漂移误差Δβ组成，x＝[Δρ^T,Δβ^T]^T；

步骤2.2确定系统的量测向量；

飞行器姿态矩阵为：

$A\left(q\right)=(q_4^2-{\mathrm{\ρ}}^T\mathrm{\ρ})I_{3\×3}+2{\mathrm{\ρ\ρ}}^T-2q_4[\mathrm{\ρ}\×]$

其中：q＝[q₁,q₂,q₃,q₄]＝[ρ^T,q₄]为姿态四元数，ρ为四元数向量部分，

星敏感器的输出为：

$z_k^i=A\left(q\left(t_k\right)\right){\stackrel{\→}{r}}^i+v_k^i=A(\mathrm{\δq}\⊗\hat{q}){\stackrel{\→}{r}}^i+v_k^i=A\left(\mathrm{\δq}\right)A\left(\hat{q}\right){\stackrel{\→}{r}}^i+v_k^i$

其中：A(q(t_k))为在t_k时刻真实的姿态矩阵，

为星敏感器的参考向量，

为零均值的高斯白噪声，i为取不同参考矢量所对应的数字，

系统的量测向量为：

$z_k=\left[\begin{array}{c}{z}_k^1\\ z_k^2\\ z_k^3\end{array}\right]=\left[\begin{array}{c}A\left(\mathrm{\δq}\right)A\left(\hat{q}\right){\stackrel{\→}{r}}^1\\ A\left(\mathrm{\δq}\right)A\left(\hat{q}\right){\stackrel{\→}{r}}^2\\ A\left(\mathrm{\δq}\right)A\left(\hat{q}\right){\stackrel{\→}{r}}^3\end{array}\right]+\left[\begin{array}{c}{v}_k^1\\ v_k^2\\ v_k^3\end{array}\right]=h\left(x_k\right)+v_k$

其中：h(x_k)为与状态有关的非线性函数，v_k均值为零、方差为R_k的高斯白噪声；

步骤三：在k-1时刻利用迭代容积卡尔曼滤波估计出k时刻的误差四元数矢量部分以及陀螺漂移误差；

将系统的状态向量x_k和量测向量z_k离散化，得到：

$\left\{\begin{array}{c}{x}_k=f(x_{k-1},\widehat{\mathrm{\ω}})+w_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.$

其中：f(·)和h(·)为系统非线性状态函数和量测函数，系统噪声w_k-1与量测噪声v_k分别是均值为零，协方差为Q_k-1和R_k的互不相关的高斯白噪声，

步骤3.1求取容积点；

容积点与容积点的权值：

${\mathrm{\ϵ}}_j=\sqrt{\frac{m}{2}}{\left[1\right]}_j,{\mathrm{\ω}}_j=\frac{1}{m},j=1,...,m$

式中：m为容积点总数，且m＝2n，ε_j为第j个容积点，e＝[1,0,...,0]^T，符号[1]表示对e的元素进行全排列和改变元素符号所产生的点集，[1]_j表示点集中[1]的第j个点；ω_j为对应第j点的权值；

步骤3.2对迭代容积卡尔曼滤波器进行时间更新；

为k-1时刻的状态估计值，P_k-1为方差，容积点及容积点经状态方程的传递值为：

${\mathrm{\χ}}_{j,k-1}={\hat{x}}_{k-1}+S_{k-1}{\mathrm{\ϵ}}_j,j=\mathrm{1,2},...,2n$

${\mathrm{\χ}}_{j,k}^{*}=f\left({\mathrm{\χ}}_{j,k-1}\right)$

其中P_k-1＝S_k-1(S_k-1)^T，

一步预测的状态和协方差阵为：

$x_k^{-}=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\ω}}_j{\mathrm{\χ}}_{j,k}^{*}$

$P_k^{-}=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\ω}}_j{\mathrm{\χ}}_{j,k}^{*}{\left({\mathrm{\χ}}_{j,k}^{*}\right)}^T-x_k^{-}{\left(x_k^{-}\right)}^T+Q_{k-1};$

步骤3.3进行量测更新，确定k时刻的状态估计值和状态协方差阵；

容积点的迭代更新函数：

$J\left({\mathrm{\χ}}_j\right)=\arg \min {\left|\right|{\mathrm{\χ}}_j-{\mathrm{\χ}}_{j,k}^{-}\left|\right|}_{{\left(P_k^{-}\right)}^{-1}}^2+{\left|\right|z-h\left({\mathrm{\χ}}_j\right)\left|\right|}_{R^{-1}}^2,j=\mathrm{1,2},...,2n$

其中：z为量测值，χ_j为需要求的迭代容积点，为已知的容积点，

量测向量线性化近似的协方差和方差为：

$P_{\mathrm{xz}}^i\≈P_k^{-}{\left(H_j^i\right)}^T,P_{\mathrm{zz}}^i\≈H_j^i{P}_k^{-}{\left(H_j^i\right)}^T+R_k$

容积点的迭代更新函数可以求得：

${\mathrm{\χ}}_j^{i+1}={\mathrm{\χ}}_{j,k}^{-}+P_{\mathrm{xz}}^i{\left(P_{\mathrm{zz}}^i\right)}^{-1}[z-h\left({\mathrm{\χ}}_j^i\right)-{\left(P_{\mathrm{xz}}^i\right)}^T{\left(P_k^{-}\right)}^{-1}({\mathrm{\χ}}_{j,k}^{-}-{\mathrm{\χ}}_j^i)],j=1,...,2n,$

将一步预测的状态与量测噪声进行扩维，则有

以及h^a(x_k,v_k)＝h(x_k)+v_k，

求取扩维后的容积点：

${\mathrm{\χ}}_{j,k}^{-}={\hat{x}}_k^0+S_k^0{\mathrm{\ϵ}}_j,j=\mathrm{1,2},...,2(n+p)$

式中： $P_k^0=S_k^0{\left(S_k^0\right)}^T,$

以

和

为初始值进行迭代，i＝1,...,N_max，第i次迭代的容积点和方差分别为

和

第i次迭代容积点经量测方程的传递值为：

$Z_{j,k}^{i-1}=h^a\left({\mathrm{\χ}}_j^{i-1}\right)$

第i次迭代的量测预测、方差及协方差为：

${\hat{z}}_k^i=\underset{j=1}{\overset{2(n+p)}{\mathrm{\Σ}}}{\mathrm{\ω}}_j{Z}_{j,k}^{i-1}$

$P_{\mathrm{zz}}^i=\frac{1}{2(n+p)}\underset{i=1}{\overset{2(n+p)}{\mathrm{\Σ}}}{Z}_{j,k}^{i-1}{\left(Z_{j,k}^{i-1}\right)}^T-{\hat{z}}_k^i{\left({\hat{z}}_k^i\right)}^T$

$P_{\mathrm{xz}}^i=\frac{1}{2(n+p)}\underset{i=1}{\overset{2(n+p)}{\mathrm{\Σ}}}{\mathrm{\χ}}_{j,k}^{i-1}{\left(Z_{j,k}^{i-1}\right)}^T-{\hat{x}}_k^{i-1}{\left({\hat{z}}_k^i\right)}^T$

第i次迭代的容积点以及方差为：

${\mathrm{\χ}}_j^i={\mathrm{\χ}}_{j,k}^{-}+P_{\mathrm{xz}}^i{\left(P_{\mathrm{zz}}^i\right)}^{-1}[z-h^a\left({\mathrm{\χ}}_j^{i-1}\right)-{\left(P_{\mathrm{xz}}^i\right)}^T{\left(P_k^0\right)}^{-1}({\mathrm{\χ}}_{j,k}^{-}-{\mathrm{\χ}}_j^{i-1})],j=1,...,2n$

$P_k^i=P_k^0-P_{\mathrm{xz}}^i{\left(P_{\mathrm{zz}}^i\right)}^{-1}{\left(P_{\mathrm{xz}}^i\right)}^T$

通过容积点计算第i次迭代的状态估计为：

${\hat{x}}_k^i=\underset{j=1}{\overset{2(n+p)}{\mathrm{\Σ}}}{\mathrm{\ω}}_j{\mathrm{\χ}}_j^i$

迭代终止条件为：

$\left|\right|{\hat{x}}_k^i(1:n)-{\hat{x}}_k^{i-1}(1:n)\left|\right|\≤\mathrm{\ϵ}$ 或i＝N_max

迭代终止时的迭代次数为N，k时刻的状态估计与方差估计为：

$x_k={\hat{x}}_k^N(1:n)$

$P_k=P_k^N(1:n,1:n);$

步骤四：对姿态及陀螺漂移进行校正；

对于k时刻的估计

利用约束条件||δq||₂＝1，求得误差四元数δq_k，求出四元数估计及陀螺漂移估计分别为

对姿态及陀螺漂移进行校正，其中

为k-1时刻估计值积分预报k时刻的值，从而得到了修正后k时刻姿态及陀螺漂移，同时确定角速度的估计值为

步骤五：姿态估计非线性离散系统的运行时间为M，若k＝M，则输出姿态及陀螺漂移的结果；若k＜M，则重复步骤三至步骤四，直至姿态估计系统运行结束。

本发明一种基于迭代容积卡尔曼滤波姿态估计方法还可以包括：

1、步骤二中，陀螺测量噪声标准差为σ_v＝0.05°/h，陀螺漂移噪声标准差为

输出频率为100Hz，星敏感器测量噪声标准差为σ_s＝18″，输出频率为1Hz，初始陀螺漂移β＝[1 1 1]^T°/h，初始姿态角误差设定为[0.5 0.5 15]^T°。

2、步骤三中，滤波的初始姿态与陀螺漂移的估计值均设为零，初始方差阵分别设为(0.2°)²I_3×3和(1.2°/h)²I_3×3，N_max＝3。

本发明的有益效果：

（1）本发明采用容积数值积分理论近似非线性函数的均值与方差，不需要对非线性函数进行线性化，避免了雅克比矩阵的计算，提高了估计精度；

（2）本发明在量测更新中，用扩维理论解决每步迭代后存在状态与量测噪声相关的问题，并采用新的容积点迭代策略，直接进行容积点的迭代，避免了传统迭代策略中容积点都是通过高斯近似以及平方根运算的局限，降低了扩维带来的计算量。

附图说明

图1是本发明与现有滤波方法的一次独立实验横滚角姿态误差结果对比图；

图2是本发明与现有滤波方法的一次独立实验俯仰角姿态误差结果对比图；

图3是本发明与现有滤波方法的一次独立实验偏航角姿态误差结果对比图；

图4是本发明的方法流程图；

图5是本发明的飞行器运动轨迹。

具体实施方式

下面结合附图对本发明进行详细说明。

本发明提出的基于迭代容积卡尔曼滤波的飞行器姿态估计方法是通过Matlab仿真软件进行仿真实验，与现有滤波算法的估计性能进行比较，如乘性扩展卡尔曼滤波（Multiplicative Extended Kalman Filter,MEKF）以及迭代容积卡尔曼滤波（ICKF）。仿真硬件环境均为Intel(R)Core（TM）i5-2410M CPU2.30GHz，4G RAM，Windows7操作系统。如图1到图3所示，本发明提出的IICKF算法与ICKF算法的收敛速度相当，但是它们的估计精度以及收敛速度始终高于MEKF算法，这是因为本发明的IICKF算法和ICKF算法都是以CKF算法为基础，其精度以及收敛速度都要高于EKF算法。比较本发明的IICKF算法和ICKF算法的估计精度，可以看出本发明的IICKF算法的估计精度要高于ICKF算法，这是由于ICKF在量测每步迭代后会造成状态与量测噪声相关，影响估计精度，而本发明提出的IICKF算法则是采用将噪声与状态扩维，解决了量测迭代后状态与量测噪声相关的问题，能够提高估计精度。

本发明是一种基于迭代容积卡尔曼滤波飞行器姿态估计方法，流程如图4所示，包括以下几个步骤：

步骤一：采集飞行器运动过程中陀螺与星敏感器的输出数据，并将其作为量测量；

步骤二：建立飞行器姿态估计系统的非线性误差四元数模型；

步骤2.1建立飞行器姿态估计系统的非线性误差四元数方程；

将误差四元数矢量部分Δρ与陀螺漂移误差Δβ组成状态向量，x＝[Δρ^T,Δβ^T]^T，建立误差四元数的非线性方程为：

$\stackrel{\·}{x}=\left[\begin{array}{c}\mathrm{\Δ}\stackrel{\·}{\mathrm{\ρ}}\\ \mathrm{\Δ}\stackrel{\·}{\mathrm{\β}}\end{array}\right]=\left[\begin{array}{c}-[\widehat{\mathrm{\ω}}\×]\mathrm{\Δ\ρ}+\frac{1}{2}[(\mathrm{\Δ\β}+{\mathrm{\η}}_v)\×]\mathrm{\Δ\ρ}-\frac{1}{2}\mathrm{\Δe}(\mathrm{\Δ\β}+{\mathrm{\η}}_v)\\ {\mathrm{\η}}_u\end{array}\right],\mathrm{\Δe}=\sqrt{1-{\left|\right|\mathrm{\Δ\ρ}\left|\right|}_2^2}---\left(1\right)$

式中：

为陀螺角速度的估计值；Δe为误差四元数的标量部分；η_v为陀螺仪的测量噪声；η_u为陀螺漂移的测量噪声；[ω×]表示由向量ω的分量构成的反对称矩阵；

$[\mathrm{\ω}\×]=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_3& {\mathrm{\ω}}_2\\ {\mathrm{\ω}}_3& 0& -{\mathrm{\ω}}_1\\ -{\mathrm{\ω}}_2& {\mathrm{\ω}}_1& 0\end{array}\right]$

步骤2.2建立系统量测方程；

由四元数表示的载体姿态矩阵为：

$A\left(q\right)=(q_4^2-{\mathrm{\ρ}}^T\mathrm{\ρ})I_{3\×3}+2{\mathrm{\ρ\ρ}}^T-2q_4[\mathrm{\ρ}\×]$

式中：q＝[q₁,q₂,q₃,q₄]＝[ρ^T,q₄]为姿态四元数；ρ为四元数向量部分；

星敏感器的测量模型为：

$z_k^i=A\left(q\left(t_k\right)\right){\stackrel{\→}{r}}^i+v_k^i=A(\mathrm{\δq}\⊗\hat{q}){\stackrel{\→}{r}}^i+v_k^i=A\left(\mathrm{\δq}\right)A\left(\hat{q}\right){\stackrel{\→}{r}}^i+v_k^i$

式中：

为星敏感器的输出；A(q(t_k))为在t_k时刻真实的姿态矩阵；

为星敏感器的参考向量；

为零均值的高斯白噪声；i为取不同参考矢量所对应的数字；为获得姿态信息，至少需要两个不平行参考矢量的观测值，目前工程应用中常采用三个不平行参考矢量的观测值，则非线性量测模型为：

$z_k=\left[\begin{array}{c}{z}_k^1\\ z_k^2\\ z_k^3\end{array}\right]=\left[\begin{array}{c}A\left(\mathrm{\δq}\right)A\left(\hat{q}\right){\stackrel{\→}{r}}^1\\ A\left(\mathrm{\δq}\right)A\left(\hat{q}\right){\stackrel{\→}{r}}^2\\ A\left(\mathrm{\δq}\right)A\left(\hat{q}\right){\stackrel{\→}{r}}^3\end{array}\right]+\left[\begin{array}{c}{v}_k^1\\ v_k^2\\ v_k^3\end{array}\right]=h\left(x_k\right)+v_k---\left(2\right)$

式中：z_k为扩维的量测向量；h(x_k)为与状态有关的非线性函数；v_k均值为零，方差为R_k的高斯白噪声。

步骤三：在k-1时刻利用迭代容积卡尔曼滤波估计出k时刻的误差四元数矢量部分以及陀螺漂移误差；

设采样时间为Δt，采用四阶龙格库塔法将式(1)、(2)所示的非线性连续系统离散化为：

$\left\{\begin{array}{c}{x}_k=f(x_{k-1},\widehat{\mathrm{\ω}})+w_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.---\left(3\right)$

式中：x_k和z_k分别为系统n维状态向量和p维量测向量；f(·)和h(·)为系统非线性状态函数和量测函数；系统噪声w_k-1与量测噪声v_k分别是均值为零，协方差为Q_k-1和R_k的互不相关的高斯白噪声；

步骤3.1基本容积点的求取；

采用容积数值积分理论来获取基本的容积点与相应的权值：

${\mathrm{\ϵ}}_j=\sqrt{\frac{m}{2}}{\left[1\right]}_j,{\mathrm{\ω}}_j=\frac{1}{m},j=1,...,m---\left(4\right)$

式中：m为容积点总数，且m＝2n；ε_j为第j个容积点，其产生方式为：设n维单位向量e＝[1,0,...,0]^T，使用符号[1]表示对e的元素进行全排列和改变元素符号所产生的点集，称为完整全对称点集，[1]_j表示点集中[1]的第j个点；ω_j为对应点的权值；

步骤3.2利用k-1时刻的状态估计对改进的迭代容积卡尔曼滤波器进行时间更新；

设k-1时刻的状态估计值为

方差为P_k-1，容积点及容积点经状态方程的传递值为：

${\mathrm{\χ}}_{j,k-1}={\hat{x}}_{k-1}+S_{k-1}{\mathrm{\ϵ}}_j,j=\mathrm{1,2},...,2n---\left(5\right)$

${\mathrm{\χ}}_{j,k}^{*}=f\left({\mathrm{\χ}}_{j,k-1}\right)---\left(6\right)$

式中：P_k-1＝S_k-1(S_k-1)^T；

一步预测的状态和协方差阵为：

$x_k^{-}=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\ω}}_j{\mathrm{\χ}}_{j,k}^{*}---\left(7\right)$

$P_k^{-}=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\ω}}_j{\mathrm{\χ}}_{j,k}^{*}{\left({\mathrm{\χ}}_{j,k}^{*}\right)}^T-x_k^{-}{\left(x_k^{-}\right)}^T+Q_{k-1}---\left(8\right)$

步骤3.3容积点迭代策略；

根据无迹回归非线性数据一致理论，建立关于容积点的迭代更新函数：

$J\left({\mathrm{\χ}}_j\right)=\arg \min {\left|\right|{\mathrm{\χ}}_j-{\mathrm{\χ}}_{j,k}^{-}\left|\right|}_{{\left(P_k^{-}\right)}^{-1}}^2+{\left|\right|z-h\left({\mathrm{\χ}}_j\right)\left|\right|}_{R^{-1}}^2,j=\mathrm{1,2},...,2n---\left(9\right)$

式中：z为量测值；χ_j为需要求的迭代容积点；

为已知的容积点；

迭代更新函数是关于2n个容积点的独立最优问题，为了求解该问题，采用Gauss-Newton迭代理论可以求得：

${\mathrm{\χ}}_j^{i+1}={\mathrm{\χ}}_{j,k}^{-}+P_k^{-}{\left(H_j^i\right)}^T{(H_j^i{P}_k^{-}{\left(H_j^i\right)}^T+R_k)}^{-1}(z-h\left({\mathrm{\χ}}_j^i\right)-H_j^i({\mathrm{\χ}}_{j,k}^{-}-{\mathrm{\χ}}_j^i)),j=1,...,2n---\left(10\right)$

式中：量测方程线性化近似的协方差和方差为：

$P_{\mathrm{xz}}^i\≈P_k^{-}{\left(H_j^i\right)}^T,P_{\mathrm{zz}}^i\≈H_j^i{P}_k^{-}{\left(H_j^i\right)}^T+R_k$

通过引入量测方差以及协方差，来降低量测方程线性化高阶截断误差的影响，则式(10)变换为：

${\mathrm{\χ}}_j^{i+1}={\mathrm{\χ}}_{j,k}^{-}+P_{\mathrm{xz}}^i{\left(P_{\mathrm{zz}}^i\right)}^{-1}[z-h\left({\mathrm{\χ}}_j^i\right)-{\left(P_{\mathrm{xz}}^i\right)}^T{\left(P_k^{-}\right)}^{-1}({\mathrm{\χ}}_{j,k}^{-}-{\mathrm{\χ}}_j^i)],j=1,...,2n---\left(11\right)$

步骤3.4利用容积点迭代策略及扩维理论进行量测更新，确定k时刻的状态估计值和状态协方差阵；

将一步预测的状态与量测噪声进行扩维，则有

以及h^a(x_k,v_k)＝h(x_k)+v_k；

然后求取扩维后的容积点：

${\mathrm{\χ}}_{j,k}^{-}={\hat{x}}_k^0+S_k^0{\mathrm{\ϵ}}_j,j=\mathrm{1,2},...,2(n+p)---\left(12\right)$

式中： $P_k^0=S_k^0{\left(S_k^0\right)}^T;$

以

和为初始值进行迭代，记i＝1,...,N_max，第i次迭代的容积点和方差分别为

和

计算第i次迭代容积点经量测方程的传递值为：

$Z_{j,k}^{i-1}=h^a\left({\mathrm{\χ}}_j^{i-1}\right)---\left(13\right)$

计算第i次迭代的量测预测、新息方差及协方差为：

${\hat{z}}_k^i=\underset{j=1}{\overset{2(n+p)}{\mathrm{\Σ}}}{\mathrm{\ω}}_j{Z}_{j,k}^{i-1}---\left(14\right)$

$P_{\mathrm{zz}}^i=\frac{1}{2(n+p)}\underset{i=1}{\overset{2(n+p)}{\mathrm{\Σ}}}{Z}_{j,k}^{i-1}{\left(Z_{j,k}^{i-1}\right)}^T-{\hat{z}}_k^i{\left({\hat{z}}_k^i\right)}^T---\left(15\right)$

$P_{\mathrm{xz}}^i=\frac{1}{2(n+p)}\underset{i=1}{\overset{2(n+p)}{\mathrm{\Σ}}}{\mathrm{\χ}}_{j,k}^{i-1}{\left(Z_{j,k}^{i-1}\right)}^T-{\hat{x}}_k^{i-1}{\left({\hat{z}}_k^i\right)}^T---\left(16\right)$

计算第i次迭代的容积点以及方差为：

${\mathrm{\χ}}_j^i={\mathrm{\χ}}_{j,k}^{-}+P_{\mathrm{xz}}^i{\left(P_{\mathrm{zz}}^i\right)}^{-1}[z-h^a\left({\mathrm{\χ}}_j^{i-1}\right)-{\left(P_{\mathrm{xz}}^i\right)}^T{\left(P_k^0\right)}^{-1}({\mathrm{\χ}}_{j,k}^{-}-{\mathrm{\χ}}_j^{i-1})],j=1,...,2n---\left(17\right)$

$P_k^i=P_k^0-P_{\mathrm{xz}}^i{\left(P_{\mathrm{zz}}^i\right)}^{-1}{\left(P_{\mathrm{xz}}^i\right)}^T---\left(18\right)$

通过容积点计算第i次迭代的状态估计为：

${\hat{x}}_k^i=\underset{j=1}{\overset{2(n+p)}{\mathrm{\Σ}}}{\mathrm{\ω}}_j{\mathrm{\χ}}_j^i---\left(19\right)$

迭代终止条件为：

$\left|\right|{\hat{x}}_k^i(1:n)-{\hat{x}}_k^{i-1}(1:n)\left|\right|\≤\mathrm{\ϵ}$ 或i＝N_max           (20)

设迭代终止时的迭代次数为N，则k时刻的状态估计与方差估计为：

$x_k={\hat{x}}_k^N(1:n)---\left(21\right)$

$P_k=P_k^N(1:n,1:n)---\left(22\right)$

利用步骤3.1-3.4如此不断的循环，就能实时地估计系统的误差四元数矢量部分以及陀螺漂移误差部分。

步骤四：对于k时刻的估计

利用约束条件||δq||₂＝1，求得误差四元数δq_k，然后求出四元数估计及陀螺漂移估计分别为对姿态及陀螺漂移进行校正，其中

为k-1时刻估计值积分预报k时刻的值，从而得到了修正后k时刻姿态及陀螺漂移，同时确定角速度的估计值为

步骤五：姿态估计非线性离散系统的运行时间为M，若k＝M，则输出姿态及陀螺漂移的结果，完成姿态估计；若k＜M，表示滤波过程未完成，则重复步骤五至步骤六，直至姿态估计系统运行结束。

对本发明的有益效果说明如下：

MATLAB仿真实验，在以下的仿真条件下，对该方法进行仿真实验：

由捷联惯导工具箱生成一条飞行器的运动轨迹，如图5所示，轨迹包括滑跑、加速、爬高、转弯等；载体初始位置为北纬45°；东经126°；高度300m；初始航向角90°，初始横滚角和俯仰角都为0°；初始速度为0m/s；初始加速度4m/s；仿真时间600s；陀螺测量噪声σ_v＝0.05°/h，陀螺漂移噪声

输出频率为100Hz；星敏感器测量噪声σ_s＝18″，输出频率为1Hz；初始陀螺漂移β＝[1 1 1]^T°/h；初始姿态角误差设定为[0.5 0.5 15]^T°；初始姿态与陀螺漂移的估计值均设为零；初始方差阵分别设为(0.2°)²I_3×3和(1.2°/h)²I_3×3。

Claims (3)

1.一种基于迭代容积卡尔曼滤波姿态估计方法，其特征在于，包括以下几个步骤：

步骤一：采集飞行器运动过程中陀螺与星敏感器的输出数据作为量测量；

步骤二：确定系统的状态向量和量测向量；

步骤2.1建立飞行器姿态估计系统的非线性误差四元数方程，确定系统的状态向量；

非线性误差四元数方程为：

$\stackrel{\·}{x}=\left[\begin{array}{c}\mathrm{\Δ}\stackrel{\·}{\mathrm{\ρ}}\\ \mathrm{\Δ}\stackrel{\·}{\mathrm{\β}}\end{array}\right]=\left[\begin{array}{c}-[\widehat{\mathrm{\ω}}\×]\mathrm{\Δ\ρ}+\frac{1}{2}[(\mathrm{\Δ\β}+{\mathrm{\η}}_v)\×]\mathrm{\Δ\ρ}-\frac{1}{2}\mathrm{\Δe}(\mathrm{\Δ\β}+{\mathrm{\η}}_v)\\ {\mathrm{\η}}_u\end{array}\right],\mathrm{\Δe}=\sqrt{1-{\left|\right|\mathrm{\Δ\ρ}\left|\right|}_2^2}$

其中：

为陀螺角速度的估计值，Δe为误差四元数的标量部分，η_v为陀螺仪的测量噪声，η_u为陀螺漂移的测量噪声，[ω×]表示由向量ω的分量构成的反对称矩阵，

$[\mathrm{\ω}\×]=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_3& {\mathrm{\ω}}_2\\ {\mathrm{\ω}}_3& 0& -{\mathrm{\ω}}_1\\ -{\mathrm{\ω}}_2& {\mathrm{\ω}}_1& 0\end{array}\right];$

状态向量x由误差四元数矢量部分Δρ与陀螺漂移误差Δβ组成，x＝[Δρ^T,Δβ^T]^T；

步骤2.2确定系统的量测向量；

飞行器姿态矩阵为：

$A\left(q\right)=(q_4^2-{\mathrm{\ρ}}^T\mathrm{\ρ})I_{3\×3}+2{\mathrm{\ρ\ρ}}^T-2q_4[\mathrm{\ρ}\×]$

其中：q＝[q₁,q₂,q₃,q₄]＝[ρ^T,q₄]为姿态四元数，ρ为四元数向量部分，

星敏感器的输出为：

$z_k^i=A\left(q\left(t_k\right)\right){\stackrel{\→}{r}}^i+v_k^i=A(\mathrm{\δq}\⊗\hat{q}){\stackrel{\→}{r}}^i+v_k^i=A\left(\mathrm{\δq}\right)A\left(\hat{q}\right){\stackrel{\→}{r}}^i+v_k^i$

其中：A(q(t_k))为在t_k时刻真实的姿态矩阵，

为星敏感器的参考向量，

为零均值的高斯白噪声，i为取不同参考矢量所对应的数字，

系统的量测向量为：

$z_k=\left[\begin{array}{c}{z}_k^1\\ z_k^2\\ z_k^3\end{array}\right]=\left[\begin{array}{c}A\left(\mathrm{\δq}\right)A\left(\hat{q}\right){\stackrel{\→}{r}}^1\\ A\left(\mathrm{\δq}\right)A\left(\hat{q}\right){\stackrel{\→}{r}}^2\\ A\left(\mathrm{\δq}\right)A\left(\hat{q}\right){\stackrel{\→}{r}}^3\end{array}\right]+\left[\begin{array}{c}{v}_k^1\\ v_k^2\\ v_k^3\end{array}\right]=h\left(x_k\right)+v_k$

其中：h(x_k)为与状态有关的非线性函数，v_k均值为零、方差为R_k的高斯白噪声；

步骤三：在k-1时刻利用迭代容积卡尔曼滤波估计出k时刻的误差四元数矢量部分以及陀螺漂移误差；

将系统的状态向量x_k和量测向量z_k离散化，得到：

$\left\{\begin{array}{c}{x}_k=f(x_{k-1},\widehat{\mathrm{\ω}})+w_{k-1}\\ z_k=h\left(x_k\right)+v_k\end{array}\right.$

其中：f(·)和h(·)为系统非线性状态函数和量测函数，系统噪声w_k-1与量测噪声v_k分别是均值为零，协方差为Q_k-1和R_k的互不相关的高斯白噪声，

步骤3.1求取容积点；

容积点与容积点的权值：

${\mathrm{\ϵ}}_j=\sqrt{\frac{m}{2}}{\left[1\right]}_j,{\mathrm{\ω}}_j=\frac{1}{m},j=1,...,m$

式中：m为容积点总数，且m＝2n，ε_j为第j个容积点，e＝[1,0,...,0]^T，符号[1]表示对e的元素进行全排列和改变元素符号所产生的点集，[1]_j表示点集中[1]的第j个点；ω_j为对应第j点的权值；

步骤3.2对迭代容积卡尔曼滤波器进行时间更新；

为k-1时刻的状态估计值，P_k-1为方差，容积点及容积点经状态方程的传递值为：

${\mathrm{\χ}}_{j,k-1}={\hat{x}}_{k-1}+S_{k-1}{\mathrm{\ϵ}}_j,j=\mathrm{1,2},...,2n$

${\mathrm{\χ}}_{j,k}^{*}=f\left({\mathrm{\χ}}_{j,k-1}\right)$

其中P_k-1＝S_k-1(S_k-1)^T，

一步预测的状态和协方差阵为：

$x_k^{-}=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\ω}}_j{\mathrm{\χ}}_{j,k}^{*}$

$P_k^{-}=\underset{j=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\ω}}_j{\mathrm{\χ}}_{j,k}^{*}{\left({\mathrm{\χ}}_{j,k}^{*}\right)}^T-x_k^{-}{\left(x_k^{-}\right)}^T+Q_{k-1};$

步骤3.3进行量测更新，确定k时刻的状态估计值和状态协方差阵；

容积点的迭代更新函数：

$J\left({\mathrm{\χ}}_j\right)=\arg \min {\left|\right|{\mathrm{\χ}}_j-{\mathrm{\χ}}_{j,k}^{-}\left|\right|}_{{\left(P_k^{-}\right)}^{-1}}^2+{\left|\right|z-h\left({\mathrm{\χ}}_j\right)\left|\right|}_{R^{-1}}^2,j=\mathrm{1,2},...,2n$

其中：z为量测值，χ_j为需要求的迭代容积点，

为已知的容积点，

量测向量线性化近似的协方差和方差为：

$P_{\mathrm{xz}}^i\≈P_k^{-}{\left(H_j^i\right)}^T,P_{\mathrm{zz}}^i\≈H_j^i{P}_k^{-}{\left(H_j^i\right)}^T+R_k$

容积点的迭代更新函数可以求得：

${\mathrm{\χ}}_j^{i+1}={\mathrm{\χ}}_{j,k}^{-}+P_{\mathrm{xz}}^i{\left(P_{\mathrm{zz}}^i\right)}^{-1}[z-h\left({\mathrm{\χ}}_j^i\right)-{\left(P_{\mathrm{xz}}^i\right)}^T{\left(P_k^{-}\right)}^{-1}({\mathrm{\χ}}_{j,k}^{-}-{\mathrm{\χ}}_j^i)],j=1,...,2n,$

将一步预测的状态与量测噪声进行扩维，则有

以及h^a(x_k,v_k)＝h(x_k)+v_k，

求取扩维后的容积点：

${\mathrm{\χ}}_{j,k}^{-}={\hat{x}}_k^0+S_k^0{\mathrm{\ϵ}}_j,j=\mathrm{1,2},...,2(n+p)$

式中： $P_k^0=S_k^0{\left(S_k^0\right)}^T,$

以

和

为初始值进行迭代，i＝1,...,N_max，第i次迭代的容积点和方差分别为和

第i次迭代容积点经量测方程的传递值为：

$Z_{j,k}^{i-1}=h^a\left({\mathrm{\χ}}_j^{i-1}\right)$

第i次迭代的量测预测、方差及协方差为：

${\hat{z}}_k^i=\underset{j=1}{\overset{2(n+p)}{\mathrm{\Σ}}}{\mathrm{\ω}}_j{Z}_{j,k}^{i-1}$

$P_{\mathrm{zz}}^i=\frac{1}{2(n+p)}\underset{i=1}{\overset{2(n+p)}{\mathrm{\Σ}}}{Z}_{j,k}^{i-1}{\left(Z_{j,k}^{i-1}\right)}^T-{\hat{z}}_k^i{\left({\hat{z}}_k^i\right)}^T$

$P_{\mathrm{xz}}^i=\frac{1}{2(n+p)}\underset{i=1}{\overset{2(n+p)}{\mathrm{\Σ}}}{\mathrm{\χ}}_{j,k}^{i-1}{\left(Z_{j,k}^{i-1}\right)}^T-{\hat{x}}_k^{i-1}{\left({\hat{z}}_k^i\right)}^T$

第i次迭代的容积点以及方差为：

${\mathrm{\χ}}_j^i={\mathrm{\χ}}_{j,k}^{-}+P_{\mathrm{xz}}^i{\left(P_{\mathrm{zz}}^i\right)}^{-1}[z-h^a\left({\mathrm{\χ}}_j^{i-1}\right)-{\left(P_{\mathrm{xz}}^i\right)}^T{\left(P_k^0\right)}^{-1}({\mathrm{\χ}}_{j,k}^{-}-{\mathrm{\χ}}_j^{i-1})],j=1,...,2n$

$P_k^i=P_k^0-P_{\mathrm{xz}}^i{\left(P_{\mathrm{zz}}^i\right)}^{-1}{\left(P_{\mathrm{xz}}^i\right)}^T$

通过容积点计算第i次迭代的状态估计为：

${\hat{x}}_k^i=\underset{j=1}{\overset{2(n+p)}{\mathrm{\Σ}}}{\mathrm{\ω}}_j{\mathrm{\χ}}_j^i$

迭代终止条件为：

$\left|\right|{\hat{x}}_k^i(1:n)-{\hat{x}}_k^{i-1}(1:n)\left|\right|\≤\mathrm{\ϵ}$ 或i＝N_max

迭代终止时的迭代次数为N，k时刻的状态估计与方差估计为：

$x_k={\hat{x}}_k^N(1:n)$

$P_k=P_k^N(1:n,1:n);$

步骤四：对姿态及陀螺漂移进行校正；

对于k时刻的估计

利用约束条件||δq||₂＝1，求得误差四元数δq_k，求出四元数估计及陀螺漂移估计分别为

对姿态及陀螺漂移进行校正，其中

为k-1时刻估计值积分预报k时刻的值，从而得到了修正后k时刻姿态及陀螺漂移，同时确定角速度的估计值为

步骤五：姿态估计非线性离散系统的运行时间为M，若k＝M，则输出姿态及陀螺漂移的结果；若k＜M，则重复步骤三至步骤四，直至姿态估计系统运行结束。

2.根据权利要求1所述的一种基于迭代容积卡尔曼滤波姿态估计方法，其特征在于：步骤二中，陀螺测量噪声标准差为σ_v＝0.05°/h，陀螺漂移噪声标准差为

输出频率为100Hz，星敏感器测量噪声标准差为σ_s＝18″，输出频率为1Hz，初始陀螺漂移β＝[1 1 1]^T°/h，初始姿态角误差设定为[0.5 0.5 15]^T°。

3.根据权利要求1或2所述的一种基于迭代容积卡尔曼滤波姿态估计方法，其特征在于：步骤三中，滤波的初始姿态与陀螺漂移的估计值均设为零，初始方差阵分别设为(0.2°)²I_3×3和(1.2°/h)²I_3×3，N_max＝3。

Images