一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法技术

本发明专利技术公开了一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，步骤如下：建立飞行器导航系统的状态方程；建立飞行器导航系统的非线性量测方程；离散化状态方程和量测方程；对离散后的状态方程和量测方程采用范数约束容积卡尔曼滤波算法，输出飞行器的姿态。本发明专利技术所采用的范数约束容积卡尔曼滤波，将描述飞行器姿态的四元数的范数约束引入了姿态估计中，有效地解决了姿态估计过程中不考虑范数约束的协方差奇异问题，仅增加了少许计算量，有效地修正了四元数的范数约束条件对飞行器姿态估计的影响，提高了姿态估计精度，进而提高了飞行器的导航精度，增强了导航过程的稳定性。

【技术实现步骤摘要】

本专利技术属于非线性约束状态估计和姿态估计的
，具体涉及一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，可用于提高飞行器姿态估计的精度。
技术介绍
众所周知，卡尔曼滤波得到的是线性随机估计问题的无约束最优解。但是，当状态变量或者部分状态变量满足一定的约束时，卡尔曼滤波得到的估计结果就不一定是最优的。研究状态具有约束的情况下卡尔曼滤波的估计问题是必须要解决的难题，近年来越来越引起学者们的关注。姿态估计是对飞行器导航和控制的一项关键技术，也是一种典型的非线性滤波问题。由于大多数飞行器的姿态用四元数进行描述，因此解决具有四元数描述的姿态非线性估计问题是一个热点问题。用四元数描述飞行器的姿态时，四元数变量具有范数约束，即四元数四个变量的平方和的范数为一，若不考虑此范数约束条件进行非线性滤波，得到的姿态估计的精度较差，甚至会导致协方差奇异问题。经典的扩展卡尔曼滤波方法通过时间更新和量测更新对状态进行估计，但扩展卡尔曼滤波方法是利用一阶线性近似非线性状态方程和非线性量测方程，因此滤波过程中会产生较大的误差，特别是当模型不精确的时候，甚至会导致滤波系统发散。近年来，Arasaratnam提出了一种容积卡尔曼滤波，利用一组等权值容积点解决贝叶斯滤波积分方程，从而来传播系统状态的均值和方差，具有较高的滤波精度(Arasaratnam I.,Haykin S.Cubature Kalman Filters.Automatic Control,IEEE Transactions on,2009,54(6):1254-1269.)。容积卡尔曼滤波提供了一种新的实现方式用以解决非线性估计问题。考虑到范数约束问题，本专利技术将范数约束引入到容积卡尔曼滤波中，提出具有范数约束的容积卡尔曼滤波方法，用以提高飞行器的姿态估计精度。
技术实现思路
为了改进现有的飞行器姿态估计受范数约束的影响精度较低的问题，本专利技术提出了一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，将范数约束引入到容积卡尔曼滤波中，并将其应用到飞行器的姿态估计问题中，提高了飞行器 的姿态估计精度。为了达到上述目的，本专利技术的技术方案是：一种范数约束容积卡尔曼滤波的飞行器姿态估计方法，其步骤如下：步骤一、建立飞行器导航系统的非线性状态方程在目标坐标系下，利用惯性测量单元提供的飞行器的加速度和角速度建立飞行器的动力学模型，其状态方程为其中，r＝[rx,ry,rz]T和v＝[vx,vy,vz]T分别是飞行器的位置和速度，X、Y、Z是目标系的坐标轴；q＝[q0 q1 q2 q3]T是姿态四元数；a＝[ax,ay,az]T和ω＝[ωx,ωy,ωz]T分别是陀螺的加速度和角速度；g是重力加速度；是从体坐标系到目标坐标系的系数转换矩阵，系数转换矩阵为：四元数矩阵Ω(ω)为：状态向量为x＝[r,v,ba,bω,q]T，则其状态方程为其中，和分别为加速度计和陀螺仪的输出向量，a和ω分别为陀螺真实的加速度和角速度，ba和bω分别为加速度的偏差和角速度的偏差，ξa和ξω分别为加速 度和角速度的零均值高斯白噪声，ηa和ηω分别为加速度和角速度的漂移偏差的高斯白噪声，系数矩阵为步骤二：建立飞行器导航系统的非线性量测方程将惯性测量单元的速度和加速度，测距测速仪测得的高度和三轴速度和两个导航信标的距离作为量测值引入到量测模型，则量测方程为：z＝[rz vc R1 R2]T+ζ＝h(x)+ζ (6)其中，其中，Ri表示飞行器与导航信标的距离，rb是信标的位置坐标，ζR,i是测距的噪声，ζR,i为零均值高斯白噪声；步骤三：离散化状态方程和量测方程将状态方程和量测方程进行离散化，得到离散的飞行器的状态方程和量测方程：xk＝f(xk-1)+wk-1 (9)zk＝h(xk)+ζk (10)其中，k时刻的状态变量xk具有范数约束wk和ζk是相互独立的零均值高斯白噪声序列，且wk和ζk的方差分别为Qk和Rk，且满足其中，δkj为Kroneckerδ函数，当k＝j时，δkj＝1；当k≠j时，δkj＝0；步骤四：将离散后状态方程和量测方程采用范数约束容积卡尔曼滤波，输出飞行器的姿态1.对离散的状态方程和量测方程的状态和误差方差阵分别进行初始化：2.设第k-1步tk-1时刻的状态估计值和误差方差阵分别为和则第k步tk时刻的容积点为：其中，上标“+”表示其后验估计，为误差方差距阵的平方根，满足 ξj为2n个容积点的第j个值，其取值集合为其中，ej(i＝1,2,…,n)为单位向量，其第j个值为1，其余元素均为零，即ej＝[0,…,0,1,0,…,0]T；经过非线性函数传递后的容积点为；则其相应的先验状态估计和方差分别为式中，上标“－”表示变量的先验估计；3.进行量测方程的更新：首先，由先验状态估计和方差重新计算容积点：然后，计算经过非线性量测函数传递的容积点位：其次，计算量测先验估计值及先验估计值的方差、先验估计值协方差：4.计算范数约束的卡尔曼增益：首先，计算无约束的卡尔曼增益和无约束的最优状态估计值其中，然后，计算具有范数约束的卡尔曼增益Kk：其中，5.计算状态估计值和相应的方差阵：基于范数约束的后验状态估计值为方差阵为式中，以上4步循环迭代，得到飞行器的实时状态估计值，包含探测器的位置、速度和姿态。所述步骤一中的目标坐标系是指以预定着陆点为原点，x轴指向东，y轴指 向北，z轴指向天；体坐标系是指以飞行器的重心为原点，OX轴位于飞行器参考平面内平行于飞行器轴线并指向飞行器前方，OY轴垂直于飞行器参考面并指向飞行器右方，OZ轴在参考面内垂直于XOY平面，并指向飞行器下方。所述步骤一中姿态四元数的向量q＝[q0 q1 q2 q3]T的2范数是q中各个元素平方之和再开根号：且满足范数约束||q||2＝1。本专利技术的有益效果：与标准的容积卡尔曼滤波相比，本专利技术所采用的范数约束容积卡尔曼滤波，将描述飞行器姿态的四元数的范数约束引入了姿态估计中，有效地解决了姿态估计过程中不考虑范数约束的协方差奇异问题，仅增加了少许计算量，有效地修正了四元数的范数约束条件对飞行器姿态估计的影响，提高了姿态估计精度，进而提高了飞行器的导航精度，增强了导航过程的稳定性。附图说明图1为本专利技术的流程图。图2为本专利技术中的范数约束容积卡尔曼滤波的原理图。具体实施方式下面结合附图和实施例对本专利技术作详细的说明。一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，其计算流程图如图1所示，数约束容积卡尔曼滤波算法流程图如图2所示，它包括以下四个步骤:步骤一：建立飞行器导航系统的状态方程在目标坐标系下，利用惯性测量单元提供的飞行器的加速度和角速度，建立飞行器的动力学模型，即状态方程为：其中，r＝[rx,ry,rz]T和v＝[vx,vy,vz]T分别是飞行器的位置和速度，X、Y、Z是目标系的坐标轴；q＝[q0 q1 q2 q3]T是姿态四元数，姿态四元数向量q的2范数是其中各个元素平方之和再开根号，即且满足范数约束||q||2＝1；a＝[ax,ay,az]T和ω＝[ωx,ωy,ωz]T分别是陀螺的加速度和角速度；g是重力加速度，其值为g＝[0,0,-3.69]T m/s2。是从体坐标系到目标坐标 系的系数转换矩阵，系数矩阵为：四元数矩阵Ω(ω)本文档来自技高网...

【技术保护点】
一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，其特征在于，其步骤如下：步骤一、建立飞行器导航系统的非线性状态方程在目标坐标系下，利用惯性测量单元提供的飞行器的加速度和角速度建立飞行器的动力学模型，其状态方程为r·=vv·=Cbla+gq·=12Ω(ω)q---(1)]]>其中，r＝[rx,ry,rz]T和v＝[vx,vy,vz]T分别是飞行器的位置和速度，X、Y、Z是目标系的坐标轴；q＝[q0 q1 q2 q3]T是姿态四元数；a＝[ax,ay,az]T和ω＝[ωx,ωy,ωz]T分别是陀螺的加速度和角速度；g是重力加速度；是从体坐标系到目标坐标系的系数转换矩阵，系数转换矩阵为：Cbl=q02+q12-q22-q322(q0q3+q1q2)2(-q0q2+q1q3)2(-q0q3+q1q2)q02-q12+q22-q322(q0q1+q2q3)2(q0q2+q1q3)2(-q0q1+q2q3)q02-q12-q22+q32---(2)]]>四元数矩阵Ω(ω)为：Ω(ω)=0-ωx-ωy-ωzωx0ωz-ωyωy-ωz0ωxωzωy-ωx0---(3)]]>状态向量为x＝[r,v,ba,bω,q]T，则其状态方程为x·=vCbl(a~-ba)+g03×103×112Ω(ω~-bω)q+03×1-Cblξaηaηω-12Θqξω=f(x)+w---(4)]]>其中，和分别为加速度计和陀螺仪的输出向量，a和ω分别为陀螺真实的加速度和角速度，ba和bω分别为加速度的偏差和角速度的偏差，ξa和ξω分别为加速度和角速度的零均值高斯白噪声，ηa和ηω分别为加速度和角速度的漂移偏差的高斯白噪声，系数矩阵Θq为Θq=q1q2q3-q0q3-q2-q3-q0q1q2-q1-q0---(5);]]>步骤二：建立飞行器导航系统的非线性量测方程将惯性测量单元的速度和加速度，测距测速仪测得的高度和三轴速度和两个导航信标的距离作为量测值引入到量测模型，则量测方程为：z＝[rz vc R1 R2]T+ζ＝h(x)+ζ   (6)其中，vc=vxcvycvzcT=Clbv---(7)]]>Ri=(r-rb)T(r-rb)+ζR,i,i=1,2---(8)]]>其中，Ri表示飞行器与导航信标的距离，rb是信标的位置坐标，ζR,i是测距的噪声，ζR,i为零均值高斯白噪声；步骤三：离散化状态方程和量测方程将状态方程和量测方程进行离散化，得到离散的飞行器的状态方程和量测方程：xk＝f(xk‑1)+wk‑1   (9)zk＝h(xk)+ζk    (10)其中，k时刻的状态变量xk具有范数约束wk和ζk是相互独立的零均值高斯白噪声序列，且wk和ζk的方差分别为Qk和Rk，且满足E[wkwjT]=Qkδkj,E[ζkζjT]=Rkδkj,E[wkζjT]=0---(11)]]>其中，δkj为Kroneckerδ函数，当k＝j时，δkj＝1；当k≠j时，δkj＝0；步骤四：将离散后状态方程和量测方程采用范数约束容积卡尔曼滤波，输出飞行器的姿态1.对离散的状态方程和量测方程的状态和误差方差阵分别进行初始化：x^0=E[x0]---(12)]]>P0=E[(x0-x^0)(x0-x^0)T]---(13)]]>2.设第k‑1步tk‑1时刻的状态估计值和误差方差阵分别为和则第k步tk时刻的容积点为：χj,k-1+=Pk-1+ξj+x^k-1+,j=1,2,...,2n---(14)]]>其中，上标“+”表示其后验估计，为误差方差距阵的平方根，满足ξj为2n个容积点的第j个值，其取值集合为n{e1,e2,...,en,-e1,-e2,...,-en}---(15)]]>其中，ej(i＝1,2,…,n)为单位向量，其第j个值为1，其余元素均为零，即ej＝[0,…,0,1,0,…,0]T；经过非线性函数传递后的容积点为；χj,k*-=f(χj,k-1+),j=1,2,...,2n---(16)]]>则其相应的先验状态估计和方差分别为x^k-=12nΣj=12n&ch...

【技术特征摘要】
1.一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，其特征在于，其步骤如下：步骤一、建立飞行器导航系统的非线性状态方程在目标坐标系下，利用惯性测量单元提供的飞行器的加速度和角速度建立飞行器的动力学模型，其状态方程为 r · = v v · = C b l a + g q · = 1 2 Ω ( ω ) q - - - ( 1 ) ]]>其中，r＝[rx,ry,rz]T和v＝[vx,vy,vz]T分别是飞行器的位置和速度，X、Y、Z是目标系的坐标轴；q＝[q0 q1 q2 q3]T是姿态四元数；a＝[ax,ay,az]T和ω＝[ωx,ωy,ωz]T分别是陀螺的加速度和角速度；g是重力加速度；是从体坐标系到目标坐标系的系数转换矩阵，系数转换矩阵为： C b l = q 0 2 + q 1 2 - q 2 2 - q 3 2 2 ( q 0 q 3 + q 1 q 2 ) 2 ( - q 0 q 2 + q 1 q 3 ) 2 ( - q 0 q 3 + q 1 q 2 ) q 0 2 - q 1 2 + q 2 2 - q 3 2 2 ( q 0 q 1 + q 2 q 3 ) 2 ( q 0 q 2 + q 1 q 3 ) 2 ( - q 0 q 1 + q 2 q 3 ) q 0 2 - q 1 2 - q 2 2 + q 3 2 - - - ( 2 ) ]]>四元数矩阵Ω(ω)为： Ω ( ω ) = 0 - ω x - ω y - ω z ω x 0 ω z - ω y ω y - ω z 0 ω x ω z ω y - ω x 0 - - - ( 3 ) ]]>状态向量为x＝[r,v,ba,bω,q]T，则其状态方程为 x · = v ...

【专利技术属性】
技术研发人员：娄泰山，贺振东，杨小亮，王妍，吴青娥，陈虎，
申请(专利权)人：郑州轻工业学院，
类型：发明
国别省市：河南;41