姿态角融合

关于飞行体姿态测量系统的多传感器信息融合 摘要 本文研究基于飞行体姿态测量系统的多传感器信息融合。首先飞行体姿态测 量系统的设计包含多个传感器，陀螺仪用于测量角速度，捷联式加速度计用于测 量加速度，磁航向仪测量偏航角， GPS 测量速度。其次，绘制该多传感器测量模 型。然后，信息融合滤波等同于卡尔曼滤波，卡尔曼滤波是基于信息融合估算算 法，并且卡尔曼滤波是通过融合所有测量信息来大致获取飞行体姿态角和速度。 最后， 仿真结果表明关于该飞行体姿态测量系统的设计是可行的， 并且是有效的。 1.简介 信息融合估计是一个难点，难点在于怎样利用信息融合理论和优化估计理论 来估计未知参数或未知过程。在完整、不完整或没有先验信息的条件下，李提出 基于融合规则的最佳线性无偏估计(BLUE)和基于融合规则的加权最小二乘法 (WLE)(2000，[1];2003，[2])。此外，王提出了一个统一的融合模型，该模型是 基于线性最小二乘估计，并得出了一个结论， “虽然测量精度不高，但是通过增 加测量次数，可以获取更高的估计精度”(2003，[3])。随后，王和郑提出关于线 性或非线性系统的信息融合控制理念，预期轨迹、系统动态方程和理想控制策略 都被作为信息测量的控制策略，然后，最优控制问题转换成最优融合估计问题 (2007，[4]；2008，[5])。姿态测量系统是飞形体姿态控制系统的重要组成部分， 姿态测量系统在姿态控制精度方面起着决定性的作用。 Gebre-Egziabher 提出一个 低成本的姿态测量系统(2000，[6])，开发了基于欧拉角和四元数传感器融合算法 (2004，[7])。王用 GPS 和陀螺仪组成了一个中国小卫星上的微型姿态测量系统 (2002，[8])。Moore 研究一个姿态测量系统，用低等级的角速率陀螺来辅助多天 线单频率 GPS 阵来研究(2003，[9])。Lee 提出了一个惯性测量法(2004，[10])。 胡设计了一个基于无人机(UAV)高度测量系统的联邦滤波器(2008，[11])。 本文设计了一个组合姿态测量系统，该系统内有陀螺仪测量角速度信息、磁 航向计测量航向信息、 捷联式加速度计提供角加速度信息和 GPS 测量速度信息。 通过融合所有测量信息来获取姿态角的估算。 ２.信息融合估算 估计变量的测量模型表达式如下： Z i  H i xv i ,i 1,2,,N,(1) x 是估计矢量， Z i是测量矢量，Hi是映射矩阵，vi是测量噪声，且 E[v i ] 0,E[v iv T j ]  R iij 。最优估计如下： ˆ  argmin ||z i H i x||2 x R1 i1 i N （2） T1 如果  i1 H i R i H i 是非奇异的，方差估计为： N N T1  ˆ   H i R i H i  x  i1  1 N H i1 T i R i 1z i（3） 1 定义测量矢量Zi本身的信息权是 I[z i ] R i，测量矢量 Z i 在估计变量x上的信息 权为 I[z i | x] H i TR i 1H i ˆ本身的信息权为：。因此最优估计x NN T i N ˆ] I[z i | x]H I[z i ]H i H i TR i 1H i I[x i1i1i1 （4) 等式（1—4）表达了一个信息融合估计的统一线性模型。 3. 飞行体姿态测量系统的设计 微机械陀螺仪经济实惠且尺寸小。 然而， 低精度和姿态误差引起的陀螺漂移， 成为陀螺仪主要的缺点。因此，使用加速度计来修正陀螺测量误差。由于加速度 信息不能有效的修正飞行体在机动飞行时的陀螺漂移， 陀螺漂移会使估计误差随 时间积累。 基于以上的因素， 设计了一个高精度组合多传感器来估计飞行体姿态。 该飞行体姿态测量系统结构图如图 1 所示， 结构图中的磁航向计用于改善航向角 的估计精度，图中 GPS 用于获取飞行体速度信息。 图 1 飞行体姿态角测量系统 3.1 传感器测量模型 陀螺测量模型如下： w m  wbm 1 （5） w m 是测量角速度矢量，w是真实的角速度矢量，b 是陀螺漂移，m 1 是测量噪声， 并且 E[m 1] 0,Cov[m1(t),m1( )] 1 (t )。此外，陀螺漂移的动态模型可以表 达为： 2 b  b 0 m 2 （6） b 0是陀螺常数漂移矢量，m 是高斯声噪声，并且 2 2Cov[m 1(t),m2 ()]0,E[m 2 ]0,Cov[m 2 (t),m 2 ()] 2 (t ) 捷联加速度计测量模型如下： a m  a[T 2 ()]Tg m 3 （7） a m是测量加速度矢量，a a 是真实加速度矢量， T 2 () 机体惯性系统的正交余弦矩 阵， m 3是加速度计测量高斯噪声，并且 g = g =0,0, g，g 是重力加速度， T E[m 2 ] 0,Cov[m 3 (t),m 3 ()] 3 2(t ), a 11 a 12 a 13   T 2 ()   a 21 a 22 a 23 ,  a   31 a 32 a 33  a 11  coscos,  a12  cossin, a 13  sin,  a21  sinsincoscossin,  a22  sinsinsincoscos, a  sincos, 23 a 31  cossincossinsin,  a32  cossinsinsincos,  a33  coscos. 磁航向计的测量模型如下：  m v （8）  m测量航向角， 是真实航向角，v测量噪声，并且 E[v(t)] 0,Cov[v(t),v()] v 2(t ) GPS 测量模型如下： V Vm = V V + n n(9) V Vm是惯性系统中的测量速度矢量，V V 是惯性系统中的真实速度矢量，n n 是测量 高斯噪声，并且E[n(t)] 0,E[n(t),n()]n(t ) 。 3.2 测量系统状态方程 首先，飞行体动态模型如下： 2 。 T (θ θ)w wθ θ 1 （10） θ θ ,, 是相对于惯性系的姿态角矢量，,,分别代表滚转角、俯仰角和航 T 向角。 w w   wx,wy,wz   是姿态角速度矢量，T 1(θ θ) 是转换矩阵，且  1 tansin  tancos  T (θ θ)   1 0 cossin     sincos   0  coscos   根据（5）和（10）两式，得到如下式： w w ˆ  w w m b b ˆ （11） θ θ  ˆ T 1(θ θ ˆ )w w ˆ （12） θ θ ˆ 是θ θ估计值， w w ˆ 是w w估计值。 θ θ  ˆ T 1