一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法
【专利摘要】本发明公开了一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，步骤如下：建立飞行器导航系统的状态方程；建立飞行器导航系统的非线性量测方程；离散化状态方程和量测方程；对离散后的状态方程和量测方程采用范数约束容积卡尔曼滤波算法，输出飞行器的姿态。本发明所采用的范数约束容积卡尔曼滤波，将描述飞行器姿态的四元数的范数约束引入了姿态估计中，有效地解决了姿态估计过程中不考虑范数约束的协方差奇异问题，仅增加了少许计算量，有效地修正了四元数的范数约束条件对飞行器姿态估计的影响，提高了姿态估计精度，进而提高了飞行器的导航精度，增强了导航过程的稳定性。
【专利说明】
一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法
技术领域
[0001] 本发明属于非线性约束状态估计和姿态估计的技术领域，具体涉及一种基于范数 约束容积卡尔曼滤波的飞行器姿态估计方法，可用于提高飞行器姿态估计的精度。
【背景技术】
[0002] 众所周知，卡尔曼滤波得到的是线性随机估计问题的无约束最优解。但是，当状态 变量或者部分状态变量满足一定的约束时，卡尔曼滤波得到的估计结果就不一定是最优 的。研究状态具有约束的情况下卡尔曼滤波的估计问题是必须要解决的难题，近年来越来 越引起学者们的关注。
[0003] 姿态估计是对飞行器导航和控制的一项关键技术，也是一种典型的非线性滤波问 题。由于大多数飞行器的姿态用四元数进行描述，因此解决具有四元数描述的姿态非线性 估计问题是一个热点问题。用四元数描述飞行器的姿态时，四元数变量具有范数约束，即四 元数四个变量的平方和的范数为一，若不考虑此范数约束条件进行非线性滤波，得到的姿 态估计的精度较差，甚至会导致协方差奇异问题。经典的扩展卡尔曼滤波方法通过时间更 新和量测更新对状态进行估计，但扩展卡尔曼滤波方法是利用一阶线性近似非线性状态方 程和非线性量测方程，因此滤波过程中会产生较大的误差，特别是当模型不精确的时候，甚 至会导致滤波系统发散。近年来，Arasaratnam提出了一种容积卡尔曼滤波，利用一组等权 值容积点解决贝叶斯滤波积分方程，从而来传播系统状态的均值和方差，具有较高的滤波 精度（Arasaratnam I. ,Haykin S. Cubature Kalman Filters .Automatic Control, IEEE Transactions on,2009,54(6): 1254-1269.)。容积卡尔曼滤波提供了一种新的实现方式用 以解决非线性估计问题。考虑到范数约束问题，本发明将范数约束引入到容积卡尔曼滤波 中，提出具有范数约束的容积卡尔曼滤波方法，用以提高飞行器的姿态估计精度。
【发明内容】
[0004] 为了改进现有的飞行器姿态估计受范数约束的影响精度较低的问题，本发明提出 了一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，将范数约束引入到容积卡尔 曼滤波中，并将其应用到飞行器的姿态估计问题中，提高了飞行器的姿态估计精度。
[0005] 为了达到上述目的，本发明的技术方案是:一种范数约束容积卡尔曼滤波的飞行 器姿态估计方法，其步骤如下：
[0006] 步骤一、建立飞行器导航系统的非线性状态方程
[0007] 在目标坐标系下，利用惯性测量单元提供的飞行器的加速度和角速度建立飞行器 的动力学模型，其状态方程为
[0011]其中，r= []^，1>山]1'和￥=[1，％，^]1'分别是飞行器的位置和速度，乂、丫、2是目标 系的坐标轴;q=[qo qi q2 93]?姿态四元数;a=[ax，ay，az]T和 ω=[ωχ，ωγ，ωζ]%1?^ 陀螺的加速度和角速度;g是重力加速度;(6=(cfr1是从体坐标系到目标坐标系的系数转换 矩阵，系数转换矩阵G为：
[0012]
(2)
[0013] 四元数矩阵Ω(ω)为：
[0014]
())
[0015]状态向量为义=[1￥，1^々《4]7，则其状态方程为
[0016]
(4)
[0017] 其中，S和?分别为加速度计和陀螺仪的输出向量，a和ω分别为陀螺真实的加速 度和角速度，bjPbu分别为加速度的偏差和角速度的偏差，ξ^Ρξω分别为加速度和角速度 的零均值高斯白噪声，η4Ρηω分别为加速度和角速度的漂移偏差的高斯白噪声，系数矩阵 %为
[0018]
[0019] 步骤二:建立飞行器导航系统的非线性量测方程
[0020] 将惯性测量单元的速度和加速度，测距测速仪测得的高度和三轴速度和两个导航 信标的距离作为量测值引入到量测模型，则量测方程为：
[0021] z = [rz vc Ri R2]T+G = h(x)+G (6)
[0022] 其中，
[0023]
[0024]
[0025] 其中，Ri表示飞行器与导航信标的距离，η是信标的位置坐标，（^是测距的噪声， ζ?Μ为零均值高斯白噪声；
[0026] 步骤三:离散化状态方程和量测方程
[0027] 将状态方程和量测方程进行离散化，得到离散的飞行器的状态方程和量测方程：
[0028] xk = f (xk-i)+wk-i (9)
[0029] zk = h(xk)+ik (10)
[0030] 其中，k时刻的状态变量Xk具有范数约束|七『=λ，wk和ζι<是相互独立的零均 值高斯白噪声序列，且的方差分别为Qk和Rk，且满足
[0031]
ill)
[0032] 其中，5kj为Kronecker3函数，当k = j时，5kj = 1;当k辛 j时，5kj = 0;
[0033] 步骤四：将离散后状态方程和量测方程采用范数约束容积卡尔曼滤波，输出飞行 器的姿态
[0034] 1.对离散的状态方程和量测方程的状态和误差方差阵分别进行初始化：
[0035]
[0036]
[0037] 2.设第k-Ι步tk-i时刻的状态估计值和误差方差阵分别为尤则第k步tk时刻 的容积点为：
[0038]
(14)
[0039] 其中，上标"+"表示其后验估计，^为误差方差距阵/^的平方根，满足
j为2n个容积点的第j个值，其取值集合为
[0040] 75? {，e2，…。,-巧,-? A}. (15)
[0041] 其中，ej(i = l,2,··· ,η)为单位向量，其第j个值为1，其余元素均为零，即ej = [0,…，0，1，0，···，0]τ;
[0042] 经过非线性函数传递后的容积点为；
[0043] χ；Λ = ), 7 = 12, · -, 2n (1.6)
[0044] 则其相应的先验状态估计和方差分别为
[0045]
[0046]
[0047] 式中，上标"一"表示变量的先验估计；
[0048] 3.进行量测方程的更新:首先，由先验状态估计尽和方差茗重新计算容积点：
[0049]
(19)
[0050] 然后，计算经过非线性量测函数传递的容积点位：
[0051 ]
(20)
[0052] 其次，计算量测先验估计值及先验估计值的方差、先验估计值协方差：
[0053] (21)
[0054] (22)
[0055](23) /=.1.
[0056] 4.计算范数约束的卡尔曼增益:首先，计算无约束的卡尔曼增益炙：
[0057]
(24>
[0058]和无约束的最优状态估计值
[0059]
[0060]
[0061 ] 1 、一
[0062]然后，计算具有范数约束的卡尔曼增益Kk:
[0063]
[0064]
[0065]
[0066]
[0067] 5.计算状态估计值和相应的方差阵:基于范数约束的后验状态估计值为
[0068]
[0069]
[0070]
[0071]
[0072]
[0073]以上4步循环迭代，得到飞行器的实时状态估计值，包含探测器的位置、速度和姿 ??τ 〇
[0074]所述步骤一中的目标坐标系是指以预定着陆点为原点，X轴指向东，y轴指向北，ζ 轴指向天;体坐标系是指以飞行器的重心为原点，0X轴位于飞行器参考平面内平行于飞行 器轴线并指向飞行器前方，0Y轴垂直于飞行器参考面并指向飞行器右方，0Z轴在参考面内 垂直于Χ0Υ平面，并指向飞行器下方。
[0075]所述步骤一中姿态四元数的向量q=[qo qi q2 范数是q中各个元素平方之 和再开根号
乱满足范数约束I |q| |2=1。
[0076]本发明的有益效果：与标准的容积卡尔曼滤波相比，本发明所采用的范数约束容 积卡尔曼滤波，将描述飞行器姿态的四元数的范数约束引入了姿态估计中，有效地解决了 姿态估计过程中不考虑范数约束的协方差奇异问题，仅增加了少许计算量，有效地修正了 四元数的范数约束条件对飞行器姿态估计的影响，提高了姿态估计精度，进而提高了飞行 器的导航精度，增强了导航过程的稳定性。
【附图说明】
[0077]图1为本发明的流程图。
[0078]图2为本发明中的范数约束容积卡尔曼滤波的原理图。
【具体实施方式】
[0079] 下面结合附图和实施例对本发明作详细的说明。
[0080] -种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，其计算流程图如图1 所示，数约束容积卡尔曼滤波算法流程图如图2所示，它包括以下四个步骤：
[0081] 步骤一:建立飞行器导航系统的状态方程
[0082] 在目标坐标系下，利用惯性测量单元提供的飞行器的加速度和角速度，建立飞行 器的动力学模型，即状态方程为：
[0083]
[0084]
[0085]
[0086] 其中，r= [rx，ry，rz]T和￥=[￥\,￥，，￥ 2]1'分别是飞行器的位置和速度，乂、￥、2是目标 系的坐标轴;q=[qo qi q2 q3]T是姿态四元数，姿态四元数向量q的2范数是其中各个元素平 方之和再开根号，S
.且满足范数约束I |q| |2 = l;a=[ax，ay，az]^ ω = [ ωχ, ωγ, ωζ]τ分别是陀螺的加速度和角速度；g是重力加速度，其值为8=[0,0，-3.69] T 是从体坐标系到目标坐标系的系数转换矩阵，系数矩阵C丨为：
[0087]
[0088]四元数矩阵Ω(ω)定义为
[0089]
[0090] 状态向量取为1=匕^，1^，1^，(1」|，则相应的状态方程为
[0091]
[0092] 上瓦甲，S和》甘别73那迷皮订和叱螺仪牺出的那迷皮和用迷度，a和ω分别为真 实的加速度和角速度，bjPbu分别为加速度偏差和角速度偏差，ξ^Ρξω分别为加速度和角速 度的零均值高斯白噪声，n a和η ω分别为加速度和角速度的漂移偏差的高斯白噪声，系数矩 阵％定义为
[0093]
[0094] 其中，目标坐标系是指以预定着陆点为原点，x轴指向东，y轴指向北，z轴指向天； 体坐标系是指以飞行器的重心为原点，0X轴位于飞行器参考平面内平行于飞行器轴线并指 向飞行器前方，0Y轴垂直于飞行器参考面并指向飞行器右方，0Z轴在参考面内垂直于Χ0Υ平 面，指向飞行器下方。
[0095] 步骤二:建立飞行器导航系统的非线性量测方程
[0096] 在建立量测模型时，考虑将惯性测量单元的速度和加速度、测距测速仪测得的高 度和三轴速度、两个导航信标的距离作为量测值引入到量测模型，则相应的量测方程为
[0097] z = [rz vc Ri Κ2]τ+ζ = 1ι(χ)+ζ (6)
[0098] 式中，
[0099]
[0100]
[0101] 式中，Ri表示飞行器与导航信标的距离，rb是信标的位置坐标，（^是测距的噪声， ζ?Μ为零均值高斯白噪声。
[0102] 步骤三:离散化状态方程和量测方程
[0103] 将上述式(4)的状态方程和量测方程进行离散化，得到离散的飞行器的状态方程 和量测方程：
[0104]
[0105] -________ 、--，
[0106] 式中，设此时状态变量Xk具有范数约束||x,|f ，wl^Kk为独立的零均值高斯 白噪声序列。wk和Gk的方差分别为Qk和Rk，且满足：
[0107]
(11)
[0108] 式中，为Kronecker3函数，即有当k = j时，5kj = 1;当k辛 j时，5kj = 0〇
[0109] 其中，在本发明中范数约束即为姿态四元数的约束：I |q| |2 = 1，也就是说上述算 法中的A =1 a
[0110] 步骤四：将离散后的状态方程和量测方程采用范数约束容积卡尔曼滤波，输出飞 行器的姿态
[0111] 步骤三中的离散化的式(9)状态方程和式（10)量测方程，考虑其状态满足一定的 范数约束，将其引入到容积卡尔曼滤波算法中，形成新的范数约束容积卡尔曼滤波算法。本 发明的范数约束容积卡尔曼滤波算法的原理图参见图2,实现步骤为：
[0112] 1.对离散的状态方程和量测方程的状态和误差方差阵进行初始化:赋予状态和 误差方差阵初值
[0113]
[0114]
[0115] 2.设第k-1步即时刻的状态估计值和误差方差阵分别为A和◎，在此基础之 上生成第k步即tk时刻的容积点
[0116]
(14)
[0117] 式中，"+"表示后验估计为方差匕的平方根，即满足为 2n个容积点的第j个值，其取值集合为
[0118]
(15)
[0119] 其中，ej(i = 1，2，…，n)为单位向量，其第j个值为1，其余元素均为零，即ej = [0,…，0，1，0，···，0]τ。
[0120] 经过非线性函数传递后的容积点分别为
[0121]
(16)
[0122] 则相应的状态和误差方差阵的先验状态估计和方差分别为
[0123]
[0124] -·* 7-1
[0125] 式中，上标"一"表示先验估计。
[0126] 3.进行量测更新:首先，由先验状态估计劣和方差^重新计算容积点
[0127]
[0128] 然后，计算经过非线性量测函数传递的容积点位
[0129]
[0130] 其次，计算量测先验估计值及先验估计值的的方差、协方差为
[0131]
[0132]
[0133] /-1
[0134] 4.计算范数约束的卡尔曼增益:首先，计算无约束的卡尔曼增益眾：
[0135]
(24)
[0136] 和无约束的最优状态估计值
[0137]
[0138]
[0139]
[0140]然后，计算具有范数约束的卡尔曼增益Kk:
[0141](27)
[0142] 式中，
[0143]
[0144]
[0145] 5.计算状态估计值和相应的方差阵:基于范数约束的后验状态估计值为
[0146]
[0147]
[0148]
[0149]
[0150]
[0151] 通过以上4步循环进行即可得到飞行器的实时状态估计值夂包含探测器的位置 i5、速度?和姿态4 (见流程图图1)。
[0152] 本发明通过建立飞行器导航系统的非线性状态方程和量测方程，然后利用范数约 束容积卡尔曼滤波算法将范数约束引入到容积卡尔曼滤波中，提高了飞行器的姿态估计精 度。
[0153] 以上所述，仅为本发明较佳的【具体实施方式】，但本发明的保护范围并不局限于此， 任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换， 都应涵盖在本发明的保护范围之内。
【主权项】
1. 一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，其特征在于，其步骤如 下： 步骤一、建立飞行器导航系统的非线性状态方程 在目标坐标系下，利用惯性测量单元提供的飞行器的加速度和角速度建立飞行器的动 力学模型，其状态方程为(1) 其中，r= [Γχ,Γγ,Γζ]哺v= [Vx，Vy，Vz]τ分别是飞行器的位置和速度，X、Y、Z是目标系的 坐标轴;q=[qo qi Q2 Q3]T是姿态四元数;a=[ax，ay，az]哺ω = [ωχ,ωγ,ωζ]τ分别是巧螺 的加速度和角速度；g是重力加速度；ct=(c；r是从体坐标系到目标坐标系的系数转换矩 阵，系数转换矩阵玲为：其中，6和&分别为加速度计和巧螺仪的输出向量，a和ω分别为巧螺真实的加速度和角 速度，ba和b。分别为加速度的偏差和角速度的偏差，ξ。和ξ。分别为加速度和角速度的零均值 高斯白噪声，％和η。分别为加速度和角速度的漂移偏差的高斯白噪声，系数矩阵Θ q为(5)， 步骤二:建立飞行器导航系统的非线性量测方程 将惯性测量单元的速度和加速度，测距测速仪测得的高度和Ξ轴速度和两个导航信标 的距离作为量测值引入到量测模型，则量测方程为： z = [rz V。化化]τ+ζ = Η(χ)+ζ (6) 其中，其中，Ri表示飞行器与导航信标的距离，η是信标的位置坐标，ζκ,ι是测距的噪声，ζκ,ι为 零均值高斯白噪声； 步骤Ξ:离散化状态方程和量测方程 将状态方程和量测方程进行离散化，得到离散的飞行器的状态方程和量测方程：其中，k时刻的状态变量xk具有范数约束|.Ytf =-v>t =义，冊和Ck是相互独立的零均值高 斯白噪声序列，且wk和Ck的方差分别为Qk和Rk，且满足(II) 其中，Skj为K;ronecke;rS函数，当k= j时，Skj = l;当k声 j时，Skj = 0; 步骤四：将离散后状态方程和量测方程采用范数约束容积卡尔曼滤波，输出飞行器的 姿态1. 对离散的状态方程和量测方程的状态和误差方差阵分别进行初始化：2. 设第k-1步tk-i时刻的状态估计值和误差方差阵分别为，和巧_|，则第k步tk时刻的容 积点为：(14) 其中，上标"表示其后验估计，^为误差方差距阵巧+_，的平方根，满足 柏=^/^(^/^f电为化个容积点的第^'个值，其取值集合为(15) 其中，ej(i = l，2，···，n)为单位向量，其第j个值为1，其余元素均为零，即ej = [0，···，0， 1，〇，...，〇]τ; 经过非线性函数传递后的容积点为；(峭 则其相应的先验状态估计和方差分别为式中，上标"一"表示变量的先验估计；3. 进行量测方程的更新:首先，由先验状态估计尽和方差《重新计算容积点：(19) 然后，计算经过非线性量测函数传递的容积点位： .Z巧=賊K j.;k) 口 0) 其次，计算量测先验估计值及先验估计值的方差、先验估计值协方差：W上4步循环迭代，得到飞行器的实时状态估计值?，包含探测器的位置i^速度(6和姿态 今(见流程图图1)。2. 根据权利要求1所示的基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，其特 征在于，所述步骤一中的目标坐标系是指W预定着陆点为原点，X轴指向东，y轴指向北，Z轴 指向天;体坐标系是指W飞行器的重屯、为原点，0X轴位于飞行器参考平面内平行于飞行器 轴线并指向飞行器前方，0Y轴垂直于飞行器参考面并指向飞行器右方，0Z轴在参考面内垂 直于X0Y平面，并指向飞行器下方。3. 根据权利要求1所示的基于范数约束容积卡尔曼滤波的飞行器姿态估计方法，其特 征在于，所述步骤一中姿态四元数的向量q=[q〇 qi Q2 q3]T的2范数是q中各个元素平方之 和再开根号：，且满足范数约束II q I 12= 1。
【文档编号】G01C21/20GK105973238SQ201610303633
【公开日】2016年9月28日
【申请日】2016年5月9日
【发明人】娄泰山, 贺振东, 杨小亮, 王妍, 吴青娥, 陈虎
【申请人】郑州轻工业学院