system: [SYSTEM]

user: 2. 高度滤波 下面分三部分阐述高度滤波，依次为姿态解算、GPS与气压计观测量融合、高度滤波。 2.1. 纸型 四元数可以表征一个坐标系或一个矢量相对某一个坐标系的旋转，计算不存在“奇点” [ 4 ] ，运算简单，所以本文采用四元数进行姿态解算。四元数微分方程采用一阶龙格-库塔进行解算，因为姿态解算不是本文研究方向，所以此处只列出公式，具体推导过程不再展示。 一阶龙格–库塔解算四元数 [ 5 ] ： q 0 ( t + T ) = q 0 ( t ) + T 2 [ − w x ( t ) q 1 ( t ) − w y ( t ) q 2 ( t ) − w z ( t ) q 3 ( t ) ] (1) q 1 ( t + T ) = q 1 ( t ) + T 2 [ w x ( t ) q 0 ( t ) + w z ( t ) q 2 ( t ) − w y ( t ) q 3 ( t ) ] (2) q 2 ( t + T ) = q 2 ( t ) + T 2 [ w y ( t ) q 0 ( t ) − w z ( t ) q 1 ( t ) + w x ( t ) q 3 ( t ) ] (3) q 3 ( t + T ) = q 3 ( t ) + T 2 [ w z ( t ) q 0 ( t ) + w y ( t ) q 1 ( t ) − w x ( t ) q 2 ( t ) ] (4) 其中 q 0 ( t ) ， q 1 ( t ) ， q 2 ( t ) ， q 3 ( t ) 为t时刻的四元数， q → ( t ) = [ q 0 ( t ) ， q 1 ( t ) ， q 2 ( t ) ， q 3 ( t ) ] T ， w x ( t ) ， w y ( t ) ， w z ( t ) 分别为机体坐标系下 x ， y ， z 轴的角速度值，亦即陀螺仪的输出。 解算出四元数后，可以从四元数中提取由机体坐标系到导航系的旋转矩阵： C b n = [ q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) q 0 2 + q 2 2 − q 1 2 − q 3 2 2 ( q 2 q 3 − q 0 q 1 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 0 2 + q 3 2 − q 1 2 − q 2 2 ] (5) 同时可得由导航系到机体坐标系的旋转矩阵 C n b = ( C b n ) T (6) 由于陀螺仪漂移，若没有其它传感器数据进行校正，单纯用陀螺仪数据解算得到的姿态会慢慢漂移，此时通过姿态矩阵转换至导航系的加速度积分得到的速度和位置漂移会越来越快。所以，需要进行姿态校正。目前文献普遍用加速度计对姿态进行水平校正，用磁力计进行偏航角校正，滤波算法有互补滤波 [ 6 ] [ 7 ] 、卡尔曼滤波 [ 8 ] [ 9 ] 等。因为加速度计测量加速度由重力加速度及运动加速度组成，所以当采用加速度计对水平姿态进行校正时，解算得到的姿态会被运动加速度污染。综上，无论单纯使用陀螺仪解算姿态或是采用加速度计进行校正，积分得到的高度均需要进行滤波。本文采用互补滤波算法 [ 6 ] 用加速度计及磁力计对姿态进行校正。 然后将加速度计测量载体坐标系下的加速度转换至导航系下，得到导航系下的加速度。 a n → = C b n ∗ a b → (7) 其中 C b n 为姿态解算得到的旋转矩阵， a b → 为载体坐标系下的加速度值，亦即加速度计输出值， a n → 为导航系下的加速度值。 2.2. GPS与气压计高度测量值融合 将GPS高度测量值与气压计高度值融合后的高度作为观测量。 H ^ = K H H 1 + ( 1 − K H ) H 2 (8) 上式中 H 1 为GPS观测高度， H 2 为气压计观测高度， H ^ 为GPS与气压计观测高度融合后的高度值，作为滤波抑制惯导高度发散的观测量。其中 K H 为比例因子，表征GPS高度测量值与气压计测量值在观测量中所占的比例大小。 GPS卫星数与三维位置精度因子pDOP是决定GPS定位精度高低的关键因素。可用GPS卫星为4个时，能够实现初步定位，但是此时精度较差，具体表现为，当载体静止不动时，测量得到的高度值在几十米甚至是一百多米的范围内变动，此时的GPS数据只能确定载体在地球中的粗略位置，无法实现精确定位。特别是对于多旋翼无人机这样活动范围较小的载体，需要更为精确的定为。GPS卫星相对于用户的几何分布对GPS解算得到的位置精度有很大影响，在GPS导航和定位中，使用几何精度因子来衡量可用卫星的空间几何分布对定位精度的影响。其中pDOP三维位置精度因子为GPS几何精度因子中的重要一个，其值越小，代表GPS解算得到的位置信息精度越高，当小于3时表明有效定位。GPS卫星数与pDOP也存在一定关联，因为可用的卫星数越多，往往卫星几何位置越能够满足要求，即GPS卫星数越多，pDOP越小。 根据上述分析，设GPS搜到的卫星数与其定位精度成正比，pDOP与其定位精度成反比，因此可将比例系数 K H 的计算公式设为： K H = n 14 ∗ 1.1 x (9) 其中， n 为GPS搜到的卫星数， x 为pDOP三维位置精度因子。 由 K H 值计算公式知当搜到的卫星数越多，且pDOP三维位置精度因子越小时， K H 值越大，越接近1，即作为观测量GPS高度值所占比例越大。因为当GPS信号良好时，其高度测量值不受大风等环境影响，数据稳定性比气压计要高，所以此时更加信任GPS数据；当搜到的卫星数较少，且pDOP三维位置精度因子较大时， K H 越小，越接近0，此时GPS信号较差，误差太大，观测量更加信任气压计数据。因此，加入比例因子 K H 后，能够在GPS高度与气压计高度任何一方出现问题时，保证滤波后的高度稳定收敛不发散。 因为GPS输出高度值为WGS-84坐标系下高度值，气压计计算高度值为气压高度，两者为不同基准，但都表示垂直方向上的距离变化，所以需要转换至同一基准下的高度值，然后再进行高度融合。 2.3. GPS与气压计高度测量值融合 采用互补滤波对惯导垂直方向位置、速度及加速度进行滤波。 首先计算高度偏差： Δ H ( k ) = H ^ ( k ) − H ∗ ( k − n ) (10) 其中， H ∗ ( k − n ) 为距当前 k 时刻 n 个滤波周期前的滤波高度历史值。因为GPS与气压计传感器观测高度存在延迟，所以为了在时间上保持计算同步，将当前的观测高度与 n 个滤波周期前的滤波高度作差值求取高度偏差。 Δ H ( k ) 为观测高度与滤波高度的偏差值。 加速度，速度及高度修正值计算： a c ( k ) = a c ( k − 1 ) + Δ H ( k ) K a Δ T ( k ) (11) v c ( k ) = v c ( k − 1 ) + Δ H ( k ) K v Δ T ( k ) (12) p c ( k ) = p c ( k − 1 ) + Δ H ( k ) K p Δ T ( k ) (13) 其中 a c ( k ) 、 v c ( k ) 及 p c ( k ) 分别为第 k 时刻的加速度、速度和高度修正值。 Δ T ( k ) 为第 k 个滤波周期与第 k − 1 个滤波周期之间的间隔时间。 K a 、 K v 及 K p 分别为加速度、速度及高度修正值的计算比例系数，其计算方式为： K a = 1 / K 3 (14) K v = 1 / K 2 (15) K p = 1 / K (16) 其中 K 可取2~10之间的整数值，可以根据滤波效果具体调整。 然后，进行加速度校正： a ∗ ( k ) = a n ( k ) + a c ( k ) (17) 其中 a ∗ ( k ) 为校正后的加速度值， a n ( k ) 为加速度计输出转换至导航系下的垂直方向加速度值。 进行高度计算及高度校正： Δ v ( k ) = ( a ∗ ( k ) + a ∗ ( k − 1 ) ) Δ T ( k ) / 2 (18) H o ( k ) = H o ( k − 1 ) + [ v ∗ ( k − 1 ) + ( v ∗ ( k − 1 ) + Δ v ( k ) / 2 ) ] Δ T ( k ) (19) H ∗ ( k ) = H o ( k ) + p c ( k ) (20) 其中 Δ v ( k ) 为第 k − 1 时刻至 k 时刻的速度增量， H ∗ ( k ) 为最终滤波后的高度， H o ( k ) 为中间计算未经滤波后的高度， v ∗ ( k − 1 ) 为 k − 1 时刻的滤波速度。 计算速度及进行速度校正： v o ( k ) = v o ( k − 1 ) + Δ v ( k ) (21) v ∗ ( k ) = v o ( k ) + v c ( k ) (22) 其中 v ∗ ( k ) 为滤波后的速度， v o ( k ) 为第 k 时刻未经校正的速度计算值。