[发明专利]一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法

摘要

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

主权项

1.一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，其特征在于，其步骤如下：步骤一、建立飞行器导航系统的非线性状态方程在目标坐标系下，利用惯性测量单元提供的飞行器的加速度和角速度建立飞行器的动力学模型，其状态方程为其中，r＝[r_x,r_y,r_z]^T和v＝[v_x,v_y,v_z]^T分别是飞行器的位置和速度，X、Y、Z是目标系的坐标轴；q＝[q₀ q₁ q₂ q₃]^T是姿态四元数；a＝[a_x,a_y,a_z]^T和ω＝[ω_x,ω_y,ω_z]^T分别是陀螺的加速度和角速度；g是重力加速度；是从体坐标系到目标坐标系的系数转换矩阵，系数转换矩阵为：四元数矩阵Ω(ω)为：状态向量为x＝[r,v,ba,bω,q]T，则其状态方程为其中，和分别为加速度计和陀螺仪的输出向量，a和ω分别为陀螺真实的加速度和角速度，b_a和b_ω分别为加速度的偏差和角速度的偏差，ξ_a和ξ_ω分别为加速度和角速度的零均值高斯白噪声，η_a和η_ω分别为加速度和角速度的漂移偏差的高斯白噪声，系数矩阵Θ_q为步骤二：建立飞行器导航系统的非线性量测方程将惯性测量单元的速度和加速度，测距测速仪测得的高度和三轴速度和两个导航信标的距离作为量测值引入到量测模型，则量测方程为：z＝[rz vc R1 R2]T+ζ＝h(x)+ζ                (6)其中，其中，Rm表示飞行器与导航信标的距离，rb是信标的位置坐标，ζR,m是测距的噪声，ζR,m为零均值高斯白噪声；步骤三：离散化状态方程和量测方程将状态方程和量测方程进行离散化，得到离散的飞行器的状态方程和量测方程：xk＝f(xk‑1)+wk‑1                       (9)zk＝h(xk)+ζk                        (10)其中，k时刻的状态变量x_k具有范数约束w_k和ζ_k是相互独立的零均值高斯白噪声序列，且w_k和ζ_k的方差分别为Q_k和R_k，且满足其中，δkj为Kroneckerδ函数，当k＝j时，δkj＝1；当k≠j时，δkj＝0；步骤四：将离散后状态方程和量测方程采用范数约束容积卡尔曼滤波，输出飞行器的姿态1.对离散的状态方程和量测方程的状态和误差方差阵分别进行初始化：2.设第k‑1步t_k‑1时刻的状态估计值和误差方差阵分别为和则第k步t_k时刻的容积点为：其中，上标“+”表示其后验估计，为误差方差阵的平方根，满足ξ_i为2n个容积点的第i个值，其取值集合为其中，ei，i＝1,2,…,n，为单位向量，其第i个值为1，其余元素均为零，即ei＝[0,…,0,1,0,…,0]T；经过非线性函数传递后的容积点为；则其相应的先验状态估计和方差分别为式中，上标“－”表示变量的先验估计；3.进行量测方程的更新：首先，由先验状态估计和方差重新计算容积点：然后，计算经过非线性量测函数传递的容积点为：其次，计算量测先验估计值及先验估计值的方差、先验估计值协方差：4.计算范数约束的卡尔曼增益：首先，计算无约束的卡尔曼增益和无约束的最优状态估计值其中，然后，计算具有范数约束的卡尔曼增益Kk：其中，5.计算状态估计值和相应的方差阵：基于范数约束的后验状态估计值为方差阵为式中，通过步骤四中步骤2‑5的4步循环迭代，得到飞行器的实时状态估计值包含探测器的位置速度和姿态