一种自适应扩展卡尔曼滤波在无人直升机导航系统中的应用

杜佳新

中国直升机设计研究所

精准的导航信息对于无人直升机稳定飞行控制起着重要的作用。针对惯性导航系统由于惯性测量元件产生的误差会随着导航解算的过程不断累积的问题，通过全球定位系统进行组合导航，以四元数法为基础建立惯性导航系统的非线性误差模型。同时针对实际系统中噪声信号统计特性不完全会干扰滤波精度而影响误差估计的问题，提出一种自适应扩展卡尔曼滤波方法，该滤波方法可以在线估计噪声特性以此保证滤波精度。仿真结果表明，该滤波方法可以提高组合导航系统的性能。

无人直升机由于具有可以在空中实现悬停、低空域飞行以及低速持续飞行的特点，在森林救火、军事侦察、环境勘探、农业种植等军用和民用领域有着广泛的应用。无人直升机可以减小自身受飞行环境的影响，满足在各种复杂环境下完成飞行任务。无人直升机通过姿态解算获取当前的姿态值，并将当前的姿态值作为反馈输入给飞控系统，再进行控制律解算。因此，无人直升机能保持稳定飞行的必要条件之一就是精确的姿态解算。

无人直升机可以通过惯性导航系统（INS）获取当前的姿态信息。惯性导航系统具有自主性的特点，通过惯性测量单元即陀螺仪和加速度计、计算机和稳定平台组成。陀螺仪测量载体相对于惯性空间的角速度，加速度计测量载体相对于惯性空间的线加速度，再通过导航解算得到载体相对于某一基准坐标系的导航参数。但是由于无人直升机在飞行时存在自身固有频率的振动以及电磁干扰，同时惯性测量元件的陀螺漂移和加速度计零偏产生的误差会随着导航解算过程不断累积，这些因素都会造成单独使用惯性导航系统去进行导航解算无法满足无人直升机稳定飞行控制的精度要求。因此，需要通过另一种导航系统对惯性导航系统（INS）量测数据进行实时修正即进行组合导航。

由于全球卫星定位系统（GPS）具有范围遍布全球、定位精度高、观测速度快以及受地域和时间的限制很小等多方面的优点，因此可以通过全球定位系统（GPS）来修正惯性导航系统（INS）在导航解算时的累积误差，这样即保留了导航解算过程的自主性又提升了定位结果的准确性和鲁棒性。组合导航系统从其本质上来看是将系统内各个传感器获取的导航信息进行综合处理，针对这一处理过程目前国内外已经有多种理论方法，如最小二乘法、加权融合法、贝叶斯估计、卡尔曼滤波以及神经网络法等多种方法。

其中，应用于导航领域最为广泛的是卡尔曼滤波法，通过递推手段可以实时解算出当前的状态量，同时还可以对下一次的状态量进行预测，但卡尔曼滤波仅适用于系统模型线性下精度较高，而针对此改进的扩展卡尔曼滤波可以将系统模型非线性的部分进行线性化处理，从而提高滤波精度。同样扩展卡尔曼滤波也不适用于所有的情况，在考虑到实际系统模型不完全准确或不能完全知悉干扰信号的噪声特性下，会使得直接使用扩展卡尔曼滤波方法进行状态估计的精度下降，甚至有可能导致滤波发散。

基于此，本文以INS/GPS组合导航作为无人直升机的导航模块，在考虑到复杂飞行环境下噪声统计特性未知的条件下，通过在线实时估计量测噪声和状态噪声的自适应扩展卡尔曼滤波来进行导航参数解算，并通过仿真验证该方法的有效性。

INS/GPS组合导航基本原理

组合导航技术是通过两种或更多导航设备来获取同一运载体的导航信息，并将获取到的导航信息进行综合处理，以此来提升导航信息的精度。以INS/GPS组合导航系统为例，GPS通过接收机获取无人直升机的速度和位置信息，INS通过陀螺仪和加速度计获取无人直升机的速度和位置信息，将两个导航子系统位置和速度信息的量测量作差值，通过设计好的滤波器来进行状态估计得到导航子系统的误差量，再将估计出的惯导系统导航参数的误差值作为反馈来校正惯导系统的导航参数，以此来提升导航参数的精度。

基于INS/GPS组合导航系统的工作原理如图1所示。整个组合导航系统的核心部分在于设计恰当的滤波器，对导航信息进行数据处理以此得到导航参数误差的最优估计，然后再根据最小均方误差的规律对惯性导航系统的导航参数进行修正，提高整个导航系统的精度，使无人直升机的飞行控制更加稳定。

图1 INS/GPS组合导航系统的工作原理。

INS/GPS组合导航系统误差模型

INS的误差状态方程

对于惯性导航系统来说，最常见的姿态解算的方法为四元数法，既便于计算又可以保证解算精度。本文就通过四元数法解算惯性导航系统的姿态矩阵，从而推导出惯性导航系统的误差模型，从而估计出惯性导航系统的误差并对其进行修正。

导航坐标系为东北地坐标系，机体坐标系为与无人直升机机体本身固联的坐标系，其中原点O为无人直升机重心位置，纵轴OX沿着机身纵轴方向且其正方向与机头朝向一致，垂向轴OZ与无人直升机的桨毂轴平行且与OX轴形成的平面在机体的对称平面，其中正方向指向机体的下方，横轴OX与XOZ平面垂直且正方向指向机体的右侧。

若Q=[q0,q1,q2,q3]T为INS真实的四元数，而T为通过解算后得到的计算四元数，则有=Q+δQ其中δQ为真实的姿态四元数与计算四元数的偏差值，根据惯导解算的四元数微分方程可得出δQ的具体表达式即为INS的姿态误差方程：

其中，

在式（1）～（6）中，用b代表机体坐标系，用n代表导航坐标系，用i代表惯性坐标系，姿态误差四元数表达式为δQ=[δq0,δq1,δq2,δq3]T，陀螺测量误差表达式为=[εx,εy,εz]T，将陀螺测量误差进行补偿后得到的输出其表达式为=[ωx,ωy,ωz]T，无人直升机在导航坐标系下的运动相对于惯性空间的角速度在导航坐标系下的投影为[ωN,ωE,ωD]T,ωin的误差表达式为=[δωN,δωE,δωD]T，INS的速度误差表达式为δVn=[δVN,δVE,δVD]T，INS的位置误差表达式为δP=[δL,δλ,δh]T（其中L表示无人机的纬度，λ表示无人机的经度，h表示无人机的高度），用ωie代表地球自转角速度，用RM代表沿子午圈的曲率半径，用RN代表沿卯酉圈的曲率半径。

若Vn=[VN,VE,VD]T为INS的真实速度，而[VN,VE,VD]T为经过解算后得到的速度，由比力方程分别将Vn、展开，两者作差忽略δgn可以得到INS的速度误差方程为：

其中，

其 中，εb=[εbx,εby,εbz]T表 示 陀 螺 零 偏 向 量，εr=[εrx,εry,εrz]T表示一阶马尔可夫过程噪声向量，Tr表示相关时间常数，wr=[wrx,wry,wrz]T表示驱动噪声向量，wg=[wgx,wgy,wgz]T表示陀螺观测噪声向量。 把加速度计量测误差认为是一阶马尔可夫过程，则可以得到加速度计误差Δb为：

其中，Ta表示相关时间常数，wa=[wax,way,waz]T表示驱动噪声向量。 选取INS的误差状态量为 X(t)=[δq0,δq1,δq2,δq3,δVN,δVE,δVD,δL,δλ,δh,εbx,εby,εbz,εrx,εry,εrz,Δx,Δy,Δz]T，则由式（1）、（7）、（10）、（14）、（15）、（16）组成INS误差状态方程表示为：

W(t)=[wgx,wgy,wgz,wrx,wry,wrz,wax,way,waz]T表示过程噪声向量。

INS/GPS组合导航系统观测方程

将INS与GPS分别对运载体的量测信息即速度和位置信息作差，并将其作为INS/GPS组合导航的观测量，由此可得观测方程为：

其中，MN、ME、MD表示GPS的速度误差，NN、NE、ND表示GPS的位置误差。

自适应扩展卡尔曼滤波

将INS/GPS组合导航系统状态方程（17）和观测方程（18）进行离散化处理，可得到非线性方程为：

其中，Wk和ηk为高斯噪声，有如下关系：

对于实际系统来说，可能存在着模型不完全准确或者噪声信号统计特性不完全的情况，这会使得标准卡尔曼滤波和扩展卡尔曼滤波估计精度下降甚至引起滤波发散，因此在进行滤波之前对于系统噪声方差Qk和观测噪声方差Rk的实时估计就显得至关重要，下面具体从状态预测以及状态更新两方面来阐述滤波过程。

状态预测部分由状态一步预测方程和一步预测均方误差所组成，如下。

状态一步预测方程：

一步预测均方误差：

状态更新部分由滤波增益、状态估计方程和估计均方误差所组成，如下。

滤波增益：

状态估计方程：

估计均方误差：

为了能应对在复杂环境下系统噪声方差和观测噪声方差可能会发生变化而影响滤波器的精度，现通过对噪声特性进行实时估计以此来得到状态量的最优估计值。现以k时刻为例，定义新息dk为滤波器实际量测值Zk和估计的观测值的差值，则有：

由式（27）可知，新息dk把新的观测值以及滤波器模型参数的信息全部包括进来，这样提升了在复杂环境下滤波器的抗干扰性，新息dk的实时估计方差通过开窗估计法可得：

其中，W表示滑动数据窗口长度。

残差方差为：

滤波增益为：

在式（34）两边右乘Rk可得

将式（35）代入式（32）得Rk估计值为

同理可得系统噪声方差Qk-1估计值为

仿真及分析

图2 无人直升机的速度估计误差。

图3 无人直升机的位置估计误差。

结论

本文从分析组合导航原理出发以GPS来修正INS导航参数信息，选取INS的速度误差以及位置误差等为状态量，并通过四元数法列写INS的误差方程即为状态方程；选取INS和GPS对无人直升机速度和位置量测信息的差值作为观测量并列写观测方程；针对实际系统中噪声信号统计特性不完全的情况，提出一种对噪声特性实时估计的自适应扩展卡尔曼滤波方法。

仿真结果表明，在噪声统计特性未知的情况下此种滤波方法能够有效估计出INS/GPS组合导航系统的速度以及位置误差。但本文只验证了此种自适应扩展卡尔曼滤波方法可以估计出组合导航系统的速度与位置误差，其滤波效果并未与基本扩展卡尔曼滤波进行比较，还需要通过仿真对比不同噪声特性环境下两种滤波的效果。