[发明专利]一种基于迭代容积卡尔曼滤波姿态估计方法

摘要

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

主权项

一种基于迭代容积卡尔曼滤波姿态估计方法，其特征在于，包括以下几个步骤：步骤一：采集飞行器运动过程中陀螺与星敏感器的输出数据作为量测量；步骤二：确定系统的状态向量和量测向量；步骤2.1建立飞行器姿态估计系统的非线性误差四元数方程，确定系统的状态向量；非线性误差四元数方程为：x·=Δρ·Δβ·=-[ω^×]Δρ+12[(Δβ+ηv)×]Δρ-12Δe(Δβ+ηv)ηu,Δe=1-||Δρ||22]]>其中：为陀螺角速度的估计值，Δe为误差四元数的标量部分，ηv为陀螺仪的测量噪声，ηu为陀螺漂移的测量噪声，[ω×]表示由向量ω的分量构成的反对称矩阵，[ω×]=0-ω3ω2ω30-ω1-ω2ω10;]]>状态向量x由误差四元数矢量部分Δρ与陀螺漂移误差Δβ组成，x＝[ΔρT,ΔβT]T；步骤2.2确定系统的量测向量；飞行器姿态矩阵为：A(q)=(q42-ρTρ)I3x3+2ρρT-2q4[ρ×]]]>其中：q＝[q1,q2,q3,q4]＝[ρT,q4]为姿态四元数，ρ为四元数向量部分，星敏感器的输出为：zkI=A(q(tk))r→I+vkI=A(δq⊗q^)r→I+vkI=A(δq)A(q^)r→I+vkI]]>其中：A(q(tk))为在tk时刻真实的姿态矩阵，为星敏感器的参考向量，为零均值的高斯白噪声，I为取不同参考矢量所对应的数字，系统的量测向量为：zk=zk1zk2zk3=A(δq)A(q^)r→1A(δq)A(q^)r→2A(δq)A(q^)r→2+vk1vk2vk3=h(xk)+vk]]>其中：h(xk)为与状态有关的非线性函数，vk为均值为零、方差为Rk的高斯白噪声；步骤三：在k‑1时刻利用迭代容积卡尔曼滤波估计出k时刻的误差四元数矢量部分以及陀螺漂移误差；将系统的状态向量xk和量测向量zk离散化，得到：xk=f(xk-1,ω^)+wk-1zk=h(xk)+vk]]>其中：f(·)和h(·)为系统非线性状态函数和量测函数，系统噪声wk‑1是均值为零、协方差为Qk‑1的互不相关的高斯白噪声，量测噪声vk是均值为零、方差为Rk的互不相关的高斯白噪声；步骤3.1求取容积点；容积点与容积点的权值：ϵj=m2[1]j,ωj=1m,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时刻的状态估计值，Pk‑1为方差，容积点及容积点经状态方程的传递值为：χj,k-1=x^k-1+Sk-1ϵj,j=1,2,...,2n]]>χj,k*=f(χj,k-1)]]>其中Pk‑1＝Sk‑1(Sk‑1)T，一步预测的状态和协方差阵为：xk-=Σj=1mωjχj,k*]]>PK-=Σj=1mωjχj,k*(χj,k*)T-xk-(xk-)T+Qk-1;]]>步骤3.3进行量测更新，确定k时刻的状态估计值和状态协方差阵；容积点的迭代更新函数：J(χj)=argmin||χj-χj,k-||(Pk-)-12+||z-h(χj)||R-12,j=1,2,...,2n]]>其中：z为量测值，χj为需要求的迭代容积点，为已知的容积点，量测向量线性化近似的协方差和方差为：Pxzi≈Pk-(Hji)T,Pzzi≈HjiPk-(Hji)T+Rk]]>容积点的迭代更新函数可以求得：χji+1=χj,k-+Pxzi(Pzzi)-1[z-h(χji)-(Pxzi)T(Pk-)-1(χj,k--χji)],j=1,...,2n,]]>将一步预测的状态与量测噪声进行扩维，则有以及ha(xk,vk)＝h(xk)+vk，求取扩维后的容积点：χj,k-=x^k0+Sk0ϵj,j=1,2,...,2(n+p)]]>式中：Pk0=Sk0(Sk0)T,]]>以和为初始值进行迭代，i＝1,...,Nmax，第i次迭代的容积点和方差分别为和第i次迭代容积点经量测方程的传递值为：Zj,ki-1=ha(χji-1)]]>第i次迭代的量测预测、方差及协方差为：z^ki=Σj=12(n+p)ωjZj,ki-1]]>Pzzi=12(n+p)Σi=12(n+p)Zj,ki-1(Zj,ki-1)T-z^ki(z^ki)T]]>Pxzi=12(n+p)Σi=12(n+p)χj,ki-1(Zj,ki-1)T-x^ki-1(z^ki)T]]>第i次迭代的容积点以及方差为：χji=χj,k-+Pxzi(Pzzi)-1[z-ha(χji-1)-(Pxzi)T(Pk0)-1(χj,k--χji-1)],j=1,...,2n]]>Pki=Pk0-Pxzi(Pzzi)-1(Pxzi)T]]>通过容积点计算第i次迭代的状态估计为：x^ki=Σj=12(n+p)ωjχji]]>迭代终止条件为：迭代终止时的迭代次数为N，k时刻的状态估计与方差估计为：xk=x^kN(1:n)]]>Pk=PkN(1:n,1:n);]]>步骤四：对姿态及陀螺漂移进行校正；对于k时刻的估计利用约束条件||δq||2＝1，求得误差四元数δqk，求出四元数估计及陀螺漂移估计分别为对姿态及陀螺漂移进行校正，其中为k‑1时刻估计值积分预报k时刻的值，从而得到了修正后k时刻姿态及陀螺漂移，同时确定角速度的估计值为步骤五：姿态估计非线性离散系统的运行时间为M，若k＝M，则输出姿态及陀螺漂移的结果；若k＜M，则重复步骤三至步骤四，直至姿态估计系统运行结束。