基于多源信息的飞行姿态估计方法

辛琪，史忠科

（西北工业大学自动化学院，陕西西安 710072）

引言

飞行姿态是导航制导的关键因素，其检测精度决定了大型无人机的安全性能。为了实现对可安装多种小型测量设备的大型无人机的姿态进行高精度估计，客观上要求采用多源姿态融合估计方法实现。文献［1-3］给出了采用三天线GPS的姿态测量方法，但精度受GPS天线间基线长度的影响较大，且易受外界干扰。文献［4］给出了基于图像的姿态测量方法，但仅适合于近地点的载体姿态测量。文献［1，5-7］给出了基于加速度计和磁强度计的测量方法，但仅适合于平稳飞行时的姿态测量。文献［8］给出了基于加速度计、磁强度计和空速的姿态测量方法，但由于空速对误差的补偿不足，导致精度不高。多源信息融合的最常用方法是将多个测量作为向量观测，采用高维混合扩展卡尔曼滤波（EKF）法进行姿态估计［9］，但该方法需要解算高维矩阵的逆，增加了计算的复杂度和滤波误差。

为了实现对任意飞行状态下姿态的精确估计，本文首先对平稳飞行的姿态测量方法进行修正，给出了可适应机动飞行的四种姿态测量方法，再采用基于混合EKF的马尔柯夫估计方法对四种姿态测量进行融合估计。

1 基于混合EKF的姿态估计方法

飞机姿态具有多种表达方式，包括欧拉角、欧拉轴/旋转角、四元数、方向余弦矩阵、罗德里格参数等，其中欧拉角因具有明确的物理意义、最少参数的表达以及无约束的解算，使得欧拉角成为最经典的姿态表达方法。对于任意的角度α，记cα=cosα，sα=sinα，式（1）给出了采用欧拉角描述的飞行姿态动态方程［1］:

式中，p，q，r为载体在x，y，z轴上对应的角速度，可通过陀螺敏感；φ，θ，ψ为定义在北东地（NED）坐标系下的姿态，分别为滚转角、俯仰角和方位角。

采用陀螺测量的角速度常包含一定的误差，如角速度常值偏差和高斯白噪声等，pt=pm+Δp+wp（pt为p的真值；pm为p的测量值；Δp为pm引入的常值偏差；wp为pm引入的高斯白噪声）。为了对姿态进行精确估计，需同时考虑对角速度常值漂移误差的估计。取估计状态为x=［φθψΔpΔqΔr］T，则估计状态的线性化模型为:

姿态的辅助测量方法很多，对于任何一种全姿态测量，其规范化模型可表达为:

式中，H=［E303］；vk为测量噪声，服从N（03×1，R）分布，R=E［vkvkT］。

由于欧拉角动态方程是非线性连续时间方程，而测量方程是离散时间方程，因而需要采用混合EKF方法对状态进行估计，步骤如下:

（2）采用四级四阶Runge-Kutta算法对状态预测值，k－1和状态预测方差阵Pk，k－1进行更新。状态更新方程和状态预测方差阵的更新方程为:

（3）计算增益矩阵Kk、状态矩阵和状态方差矩阵Pk:

2 多源姿态融合估计

2.1 问题的提出

大型无人机上可安装多种姿态测量装置，为了充分利用各种测量结果对姿态进行修正，可采用基于混合EKF的姿态估计方法进行姿态估计。该方法只需将基于混合EKF的姿态估计方法中的姿态测量矩阵、噪声方差阵改为式（7）即可。但与基于单姿态测量采用混合EKF的姿态估计方法相比，该方法在实现多源姿态信息融合估计时存在求解高维矩阵的逆的问题，导致计算效率和滤波精度下降。

为了保证计算效率和滤波精度，需要对将多个姿态测量作为向量观测的高维混合EKF姿态估计方法进行改进。

2.2 基本思路

如图1所示，本文提出了采用马尔柯夫估计和混合EKF相结合的方法对多种姿态观测信息进行融合。

图1 多源姿态融合估计结构图

该方法首先将每路测量作为独立观测，采用混合EKF滤波建立了四路状态估计，再采用马尔柯夫估计对四路状态估计的结果进行综合，给出状态和状态方差阵的最优估计。其滤波步骤如下:

（1）对四路观测分别采用基于混合EKF的姿态估计方法进行滤波，给出状态和状态方差阵。

（2）采用马尔柯夫估计对四路状态更新的结果进行综合，式（8）给出了状态估计和状态方差阵的表达:

为了采用该方法对任意飞行状态下的姿态进行精确估计，需要对常规的仅适合平稳飞行的姿态测量方法进行部分修正。

2.3 姿态辅助测量方法

2.3.1 基于加速度、磁强度和地速时间导数的姿态测量方法

式（9）给出了飞机对地速度的动力学方程:

当飞机水平时，其方位角为飞机机体y，x轴上测得的地磁强度分量Hny和Hnx之比的反正切值。而当飞机存在倾斜角时，将地理坐标系绕地向旋转ψ之后的坐标系记为投影坐标系，则将地磁强度向量向投影坐标系上投影后，可得方位角的表达式如下:

式中，［HbxHbyHbz］为机体上测得的地磁强度。

2.3.2 基于加速度、磁强度、空速的姿态测量方法

当GPS的天线被遮蔽或GPS信号受到外界干扰时，GPS的观测值极不可靠，常利用空速测量来弥补这一不足，飞机速度方程为:

再利用式（11）确定航向。

2.3.3 基于图像的近地点姿态测量方法

无人机上一般都装有摄像头，可采用基于机场跑道的姿态确定方法给出无人机在近地点的姿态。图2给出了基于机场跑道的姿态确定原理结构图。

图2 基于图像的姿态辅助测量原理图

将摄像头安装在云台上，调整云台转角，使机场跑道的着陆线P1P3与跑道线P1P2和P3P4总是处于锁定状态，采用图像处理算法确定消失点P和跑道特征点P1和P3的图像坐标位置，再根据成像原理，给出摄像机坐标系中P，P1，P3的图像坐标，结合P1P3⊥OcP给出P1点相对于P3点的尺度因子k，从而给出摄像机坐标系的三轴单位向量:

式中，f为摄像机的焦距；（u，v），（u1，v1），（u3，v3）分别为P点、P1点、P3点的图像坐标；尺度因子k为（f2+uu3+vv3）/（f2+uu1+vv1）。则地理坐标系到摄像机坐标系的方向余弦矩阵为:

姿态矩阵为:

2.3.4 基于多天线GPS的姿态测量方法

大型无人机体积较大，具有安装多天线GPS的能力。基于多天线GPS姿态测量方法的关键是求解Wabba问题［10］，即通过最优化指标寻找公用旋转变换，从而确定两个坐标系间的关系。图3为基于三天线GPS的姿态测量方法示意图。

图3 基于三天线GPS的姿态测量方法

通过 GPS可确定H点坐标（xh，yh，zh），F点坐标（xf，yf，zf）。通过OH与地理坐标系的空间关系可确定俯仰姿态和航向:

3 仿真分析

3.1 仿真实验环境设置

按照式（19）配置载体的欧拉角动态:

式中，φ0=40°；θ0=0.5°；ψ0=110 － 15 sin（0.2）U（t10），U（tτ）为单位阶跃函数，tτ=t－τ。陀螺的常值漂移误差为2（°）/s，陀螺噪声标准差为0.05（°）/s。姿态测量方法1的测量误差为2°，方法2的测量误差为3.5°，方法3的测量误差为3°，方法4的测量误差为2.5°。状态初值为零向量，初始噪声方差阵为mE3，m取为25。T=0.1 s，姿态更新周期和滤波周期均为0.1 s，试验100 s。

3.2 仿真结果及分析

图4为本文方法与基于未经校准的角速度姿态解算方法的对比。图中，“未校准”为采用未扣除常值偏差的角速度解算的姿态；“真实”为导航级姿态仪表的输出值；“本文方法”为基于多源测量信息的姿态融合估计结果。

图4 仿真1:姿态算法对比

结果表明，不对陀螺进行校准，则会因角速度常值偏差导致姿态解算的发散。本文方法在估计姿态的同时，对角速度的常值偏差进行了估计，并在姿态更新过程中扣除了角速度常值偏差对姿态的影响，再进行姿态的时间更新和测量更新。仿真结果表明本文提出的多源姿态融合估计方法可保证姿态估计过程的收敛性。

图5 仿真2:姿态误差对比

图5给出了采用本文方法和基于单个测量采用混合EKF的姿态估计误差的对比结果。本文方法是四种基于单个测量采用混合EKF的姿态估计结果的马尔柯夫估计，具有原理上最小的滤波误差。试验结果表明，本文方法估计的姿态误差一致优于基于单个观测采用混合EKF的估计结果。

图6和图7对本文方法和基于多个姿态测量的高维混合EKF方法的姿态估计误差进行了对比。

图6 仿真3:姿态误差对比

图7 仿真3:角速度常值偏差估计对比

可以看出，本文方法由于避免了对高维矩阵的求逆运算，降低了滤波增益矩阵的误差，因而估计精度优于基于多个姿态测量的高维混合EKF方法。

表1给出了六种估计方法的均方误差，其中方法0是本文方法，方法5是高维混合EKF方法，方法1～方法4顺序对应基于单个测量的方法。由上述分析知，本文方法的滤波精度高于基于多个姿态测量的高维混合EKF，这两种方法的估计精度高于任何一种基于单个观测的混合EKF。

表1 六种估计方法的均方误差

4 结束语

本文首先对混合EKF估计方法进行了分析，提出了基于多个测量的高维混合EKF估计方法存在解算高维矩阵逆的问题。针对该问题，给出了对四种基于单个测量的混合EKF的状态估计采用马尔柯夫估计进行综合的方法，该方法可精确估计出角速度的常值偏差，保证滤波的收敛性；同时该方法的滤波结果优于任何一种基于单个观测的混合EKF的滤波结果和基于多个测量的高维EKF滤波结果。

［1］Gebre-Egziabher D，Hayward R C，Powell J D.Design of multi-sensor attitude determination systems［J］.IEEE Transactions on Aerospace and Electronic Systems，2004，40（2）:627-649.

［2］Gebre-Egziabher D，Hayward R C，Powell J D.A low cost GPS/inertial attitude heading reference system（AHRS）for general aviation applications［C］//IEEE Position Location and Navigation Symposium.Palm Springs:IEEE，1998:518-525.

［3］Cohen C E.Attitude determination using GPS ［D］.Palo Alto:Stanford University，1992.

［4］赵昊昱.基于视觉的无人机着陆导航［D］.武汉:华中科技大学，2006.

［5］Gebre-Egziabher D，Elkaim G H，Powell，J D，et al.A gyro-free quaternion-based attitude determination system suitable for implementation using low cost sensors［C］//IEEE Position Location and Navigation Symposium.San Diego:IEEE，2000:185-192.

［6］付旭，周兆英，熊沈蜀，等.基于EKF的多MEMS传感器姿态测量系统［J］.清华大学学报（自然科学版），2006，46（11）:1857-1859，1863.

［7］王松，田波，战榆莉，等.基于修正 EKF的微小型飞行器姿态估计［J］.高技术通讯，2011，21（6）:612-618.

［8］LIU Cheng，ZHOU Zhaoying，FU Xu.Attitude determination for MAVs using a Kalman filter［J］.Tsinghua Science and Technology，2008，13（5）:593-597.

［9］Dan Simon.Optimal state estimation:Kalman，H∞，and nonlinear approaches［M］.First Edition.New Jersey:John Wiley ＆ Sons，Inc Publication，2006:403-407.

［10］Wahba G.Problem 65-1:a least squares estimate of satellite attitude［J］.SIAM Review，1965，7（3）:409-409.