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

Abstract

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

Description

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

技术领域

本发明属于非线性约束状态估计和姿态估计的技术领域，具体涉及一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，可用于提高飞行器姿态估计的精度。

背景技术

众所周知，卡尔曼滤波得到的是线性随机估计问题的无约束最优解。但是，当状态变量或者部分状态变量满足一定的约束时，卡尔曼滤波得到的估计结果就不一定是最优的。研究状态具有约束的情况下卡尔曼滤波的估计问题是必须要解决的难题，近年来越来越引起学者们的关注。

姿态估计是对飞行器导航和控制的一项关键技术，也是一种典型的非线性滤波问题。由于大多数飞行器的姿态用四元数进行描述，因此解决具有四元数描述的姿态非线性估计问题是一个热点问题。用四元数描述飞行器的姿态时，四元数变量具有范数约束，即四元数四个变量的平方和的范数为一，若不考虑此范数约束条件进行非线性滤波，得到的姿态估计的精度较差，甚至会导致协方差奇异问题。经典的扩展卡尔曼滤波方法通过时间更新和量测更新对状态进行估计，但扩展卡尔曼滤波方法是利用一阶线性近似非线性状态方程和非线性量测方程，因此滤波过程中会产生较大的误差，特别是当模型不精确的时候，甚至会导致滤波系统发散。近年来，Arasaratnam提出了一种容积卡尔曼滤波，利用一组等权值容积点解决贝叶斯滤波积分方程，从而来传播系统状态的均值和方差，具有较高的滤波精度(Arasaratnam I.,Haykin S.Cubature Kalman Filters.Automatic Control,IEEETransactions on,2009,54(6):1254-1269.)。容积卡尔曼滤波提供了一种新的实现方式用以解决非线性估计问题。考虑到范数约束问题，本发明将范数约束引入到容积卡尔曼滤波中，提出具有范数约束的容积卡尔曼滤波方法，用以提高飞行器的姿态估计精度。

发明内容

为了改进现有的飞行器姿态估计受范数约束的影响精度较低的问题，本发明提出了一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，将范数约束引入到容积卡尔曼滤波中，并将其应用到飞行器的姿态估计问题中，提高了飞行器的姿态估计精度。

为了达到上述目的，本发明的技术方案是：一种范数约束容积卡尔曼滤波的飞行器姿态估计方法，其步骤如下：

步骤一、建立飞行器导航系统的非线性状态方程

在目标坐标系下，利用惯性测量单元提供的飞行器的加速度和角速度建立飞行器的动力学模型，其状态方程为

其中，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,b_a,b_ω,q]^T，则其状态方程为

其中，和分别为加速度计和陀螺仪的输出向量，a和ω分别为陀螺真实的加速度和角速度，b_a和b_ω分别为加速度的偏差和角速度的偏差，ξ_a和ξ_ω分别为加速度和角速度的零均值高斯白噪声，η_a和η_ω分别为加速度和角速度的漂移偏差的高斯白噪声，系数矩阵Θ_q为

步骤二：建立飞行器导航系统的非线性量测方程

将惯性测量单元的速度和加速度，测距测速仪测得的高度和三轴速度和两个导航信标的距离作为量测值引入到量测模型，则量测方程为：

z＝[r_z v^c R₁ R₂]^T+ζ＝h(x)+ζ (6)

其中，

其中，R_m表示飞行器与导航信标的距离，r_b是信标的位置坐标，ζ_R,m是测距的噪声，ζ_R,m为零均值高斯白噪声；

步骤三：离散化状态方程和量测方程

将状态方程和量测方程进行离散化，得到离散的飞行器的状态方程和量测方程：

x_k＝f(x_k-1)+w_k-1 (9)

z_k＝h(x_k)+ζ_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个值，其取值集合为

其中，e_i，i＝1,2,…,n，为单位向量，其第i个值为1，其余元素均为零，即e_i＝[0,…,0,1,0,…,0]^T；

经过非线性函数传递后的容积点为；

则其相应的先验状态估计和方差分别为

式中，上标“－”表示变量的先验估计；

3.进行量测方程的更新：首先，由先验状态估计和方差重新计算容积点：

然后，计算经过非线性量测函数传递的容积点为：

其次，计算量测先验估计值及先验估计值的方差、先验估计值协方差：

4.计算范数约束的卡尔曼增益：首先，计算无约束的卡尔曼增益

和无约束的最优状态估计值

其中，

然后，计算具有范数约束的卡尔曼增益K_k：

其中，

5.计算状态估计值和相应的方差阵：基于范数约束的后验状态估计值为

方差阵为

式中，

通过步骤四中步骤2-5的4步循环迭代，得到飞行器的实时状态估计值，包含探测器的位置、速度和姿态。

所述步骤一中的目标坐标系是指以预定着陆点为原点，x轴指向东，y轴指向北，z轴指向天；体坐标系是指以飞行器的重心为原点，OX轴位于飞行器参考平面内平行于飞行器轴线并指向飞行器前方，OY轴垂直于飞行器参考面并指向飞行器右方，OZ轴在参考面内垂直于XOY平面，并指向飞行器下方。

所述步骤一中姿态四元数的向量q＝[q₀ q₁ q₂ q₃]^T的2范数是q中各个元素平方之和再开根号：且满足范数约束||q||₂＝1。

本发明的有益效果：与标准的容积卡尔曼滤波相比，本发明所采用的范数约束容积卡尔曼滤波，将描述飞行器姿态的四元数的范数约束引入了姿态估计中，有效地解决了姿态估计过程中不考虑范数约束的协方差奇异问题，仅增加了少许计算量，有效地修正了四元数的范数约束条件对飞行器姿态估计的影响，提高了姿态估计精度，进而提高了飞行器的导航精度，增强了导航过程的稳定性。

附图说明

图1为本发明的流程图。

图2为本发明中的范数约束容积卡尔曼滤波的原理图。

具体实施方式

下面结合附图和实施例对本发明作详细的说明。

一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，其计算流程图如图1所示，数约束容积卡尔曼滤波算法流程图如图2所示，它包括以下四个步骤:

步骤一：建立飞行器导航系统的状态方程

在目标坐标系下，利用惯性测量单元提供的飞行器的加速度和角速度，建立飞行器的动力学模型，即状态方程为：

其中，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是姿态四元数，姿态四元数向量q的2范数是其中各个元素平方之和再开根号，即且满足范数约束||q||₂＝1；a＝[a_x,a_y,a_z]^T和ω＝[ω_x,ω_y,ω_z]^T分别是陀螺的加速度和角速度；g是重力加速度，其值为g＝[0,0,-3.69]^Tm/s²。是从体坐标系到目标坐标系的系数转换矩阵，系数矩阵为：

四元数矩阵Ω(ω)定义为

状态向量取为x＝[r,v,b_a,b_ω,q]^T，则相应的状态方程为

上式中，和分别为加速度计和陀螺仪输出的加速度和角速度，a和ω分别为真实的加速度和角速度，b_a和b_ω分别为加速度偏差和角速度偏差，ξ_a和ξ_ω分别为加速度和角速度的零均值高斯白噪声，η_a和η_ω分别为加速度和角速度的漂移偏差的高斯白噪声，系数矩阵Θ_q定义为

其中，目标坐标系是指以预定着陆点为原点，x轴指向东，y轴指向北，z轴指向天；体坐标系是指以飞行器的重心为原点，OX轴位于飞行器参考平面内平行于飞行器轴线并指向飞行器前方，OY轴垂直于飞行器参考面并指向飞行器右方，OZ轴在参考面内垂直于XOY平面，指向飞行器下方。

步骤二：建立飞行器导航系统的非线性量测方程

在建立量测模型时，考虑将惯性测量单元的速度和加速度、测距测速仪测得的高度和三轴速度、两个导航信标的距离作为量测值引入到量测模型，则相应的量测方程为

z＝[r_z v^c R₁ R₂]^T+ζ＝h(x)+ζ (6)

式中，

式中，R_i表示飞行器与导航信标的距离，r_b是信标的位置坐标，ζ_R,i是测距的噪声，ζ_R,i为零均值高斯白噪声。

步骤三：离散化状态方程和量测方程

将状态方程和量测方程进行离散化，得到离散的飞行器的状态方程和量测方程：

x_k＝f(x_k-1)+w_k-1 (9)

z_k＝h(x_k)+ζ_k (10)

式中，设此时状态变量x_k具有范数约束w_k和ζ_k为独立的零均值高斯白噪声序列。w_k和ζ_k的方差分别为Q_k和R_k，且满足：

式中，δ_kj为Kroneckerδ函数，即有当k＝j时，δ_kj＝1；当k≠j时，δ_kj＝0。

其中，在本发明中范数约束即为姿态四元数的约束：||q||₂＝1，也就是说上述算法中的

步骤四：将离散后的状态方程和量测方程采用范数约束容积卡尔曼滤波，输出飞行器的姿态

步骤三中的离散化的式(9)状态方程和式(10)量测方程，考虑其状态满足一定的范数约束，将其引入到容积卡尔曼滤波算法中，形成新的范数约束容积卡尔曼滤波算法。本发明的范数约束容积卡尔曼滤波算法的原理图参见图2，实现步骤为：

1.对离散的状态方程和量测方程的状态和误差方差阵进行初始化：赋予状态和误差方差阵初值

2.设第k-1步即t_k-1时刻的状态估计值和误差方差阵分别为和在此基础之上生成第k步即t_k时刻的容积点

式中，“+”表示后验估计，为方差的平方根，即满足ξ_j为2n个容积点的第j个值，其取值集合为

其中，e_j(i＝1,2,…,n)为单位向量，其第j个值为1，其余元素均为零，即e_j＝[0,…,0,1,0,…,0]^T。

经过非线性函数传递后的容积点分别为

则相应的状态和误差方差阵的先验状态估计和方差分别为

式中，上标“－”表示先验估计。

3.进行量测更新：首先，由先验状态估计和方差重新计算容积点

然后，计算经过非线性量测函数传递的容积点为：

其次，计算量测先验估计值及先验估计值的的方差、协方差为

4.计算范数约束的卡尔曼增益：首先，计算无约束的卡尔曼增益

和无约束的最优状态估计值

式中，

然后，计算具有范数约束的卡尔曼增益K_k：

式中，

5.计算状态估计值和相应的方差阵：基于范数约束的后验状态估计值为

方差阵为

式中，

通过第2-5步的4步循环进行即可得到飞行器的实时状态估计值包含探测器的位置速度和姿态

本发明通过建立飞行器导航系统的非线性状态方程和量测方程，然后利用范数约束容积卡尔曼滤波算法将范数约束引入到容积卡尔曼滤波中，提高了飞行器的姿态估计精度。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (3)

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,b_a,b_ω,q]^T，则其状态方程为

其中，和分别为加速度计和陀螺仪的输出向量，a和ω分别为陀螺真实的加速度和角速度，b_a和b_ω分别为加速度的偏差和角速度的偏差，ξ_a和ξ_ω分别为加速度和角速度的零均值高斯白噪声，η_a和η_ω分别为加速度和角速度的漂移偏差的高斯白噪声，系数矩阵Θ_q为

步骤二：建立飞行器导航系统的非线性量测方程

将惯性测量单元的速度和加速度，测距测速仪测得的高度和三轴速度和两个导航信标的距离作为量测值引入到量测模型，则量测方程为：

z＝[r_z v^c R₁ R₂]^T+ζ＝h(x)+ζ (6)

其中，

其中，R_m表示飞行器与导航信标的距离，r_b是信标的位置坐标，ζ_R,m是测距的噪声，ζ_R,m为零均值高斯白噪声；

步骤三：离散化状态方程和量测方程

将状态方程和量测方程进行离散化，得到离散的飞行器的状态方程和量测方程：

x_k＝f(x_k-1)+w_k-1 (9)

z_k＝h(x_k)+ζ_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个值，其取值集合为

其中，e_i，i＝1,2,…,n，为单位向量，其第i个值为1，其余元素均为零，即e_i＝[0,…,0,1,0,…,0]^T；

经过非线性函数传递后的容积点为；

则其相应的先验状态估计和方差分别为

式中，上标“－”表示变量的先验估计；

3.进行量测方程的更新：首先，由先验状态估计和方差重新计算容积点：

然后，计算经过非线性量测函数传递的容积点为：

其次，计算量测先验估计值及先验估计值的方差、先验估计值协方差：

4.计算范数约束的卡尔曼增益：首先，计算无约束的卡尔曼增益

和无约束的最优状态估计值

其中，

然后，计算具有范数约束的卡尔曼增益K_k：

其中，

5.计算状态估计值和相应的方差阵：基于范数约束的后验状态估计值为

方差阵为

式中，

通过步骤四中步骤2-5的4步循环迭代，得到飞行器的实时状态估计值包含探测器的位置速度和姿态

2.根据权利要求1所示的基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，其特征在于，所述步骤一中的目标坐标系是指以预定着陆点为原点，x轴指向东，y轴指向北，z轴指向天；体坐标系是指以飞行器的重心为原点，OX轴位于飞行器参考平面内平行于飞行器轴线并指向飞行器前方，OY轴垂直于飞行器参考面并指向飞行器右方，OZ轴在参考面内垂直于XOY平面，并指向飞行器下方。

3.根据权利要求1所示的基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，其特征在于，所述步骤一中姿态四元数的向量q＝[q₀ q₁ q₂ q₃]^T的2范数是q中各个元素平方之和再开根号：且满足范数约束||q||₂＝1。