北斗/GPS无人飞行器动态导航中改进自适应算法*

徐　伟，李　智，，王勇军

（1.桂林电子科技大学 电子工程与自动化学院，广西 桂林541004；2.桂林航天工业学院无人机遥测重点实验室，广西 桂林541004）

北斗/GPS无人飞行器动态导航中改进自适应算法*

徐伟1，李智1，2，王勇军2

（1.桂林电子科技大学 电子工程与自动化学院，广西 桂林541004；2.桂林航天工业学院无人机遥测重点实验室，广西 桂林541004）

北斗与GPS或其他类型导航设备可构成组合导航系统，为无人飞行器提供高精度、高可靠导航信息。在处理动态导航数据采用“当前”统计模型(CS模型)并利用标准Kalman滤波方法时，系统动态噪声和观测噪声未知且时变，而且加速度上下限值不能自适应于未知运动规律的无人飞行器当前加速度，导致导航精度降低。为此，提出一种适应于飞行器运动状态的改进自适应算法，并应用于北斗/GPS动态导航系统中。实验仿真验证了该算法能有效提高组合导航系统的精度和可靠性，并能更好地自适应于无人飞行器的机动特性。

北斗；GPS；组合导航；无人飞行器；自适应算法

0　引言

北斗是中国正在实施的自主发展、独立运行的全球卫星导航系统，与目前应用最成熟的GPS或其他类型导航设备可构成组合导航系统，为工作在复杂环境中的飞行器提供可靠导航信息，而飞行器机动性未知，组合导航系统运动模型以及噪声统计特性存在误差。为了处理各种误差，文献[1]直接对 GPS接收机的输出结果进行动态滤波；文献[2]设计一种容错型联邦强跟踪滤波器并应用于COMPASS、GPS、GLONASS组合导航系统中；文献[3-4]对GPS、GLONASS、GALILEO组合导航系统提出自适应联邦Kalman滤波以及双重自适应算法。

标准 Kalman滤波是一种在正确的运动模型和噪声统计特性下对导航系统动态数据进行实时滤波的有效方法，但在实际应用中受限。Sage-Husa算法利用噪声估计器对未知且时变的噪声统计特性进行估计[5]，而北斗/ GPS组合导航系统维数较高，计算量大，一旦Q(过程噪声协方差阵)和R(量测噪声协方差阵)分别失去半正定性和正定性会导致滤波发散。简化Sage-Husa算法删去q(过程噪声均值)和 r(量测噪声均值)的计算，认为 Q稳定，并改进R估计表达式[6]。而实际情况下的Q会变化，CS模型中加速度上下限取值过大或过小，均导致系统噪声方差与实际不符，跟踪精度低。为此，本文提出一种适应飞行器运动状态的改进自适应算法，它实时估计R，且利用速度滤波、预测估计间的差值改进加速度协方差计算表达式，实现Q自适应于飞行器的机动特性，并提高跟踪能力。通过实验仿真与标准 Kalman、简化Sage-Husa算法进行了比较和分析。

1　北斗/GPS组合导航系统模型的建立

1.1联邦滤波器设计

视北斗、GPS为两个相互独立的联邦成员，并分别设计自适应子滤波器，采用容错性最好的无反馈重置联邦滤波器对各子滤波器进行数据融合，见图1。若北斗、GPS子滤波器局部状态估计和相应估计协方差阵分别为(，P11)，(，P22)，且各局部估计互不相关，即 Pij=0 (i≠j)，则全局最优估计为：

图1　北斗/GPS组合导航系统联邦滤波器

北斗、GPS子滤波器数学模型的建立方法相同，不妨以GPS为例。子滤波器建立的状态方程有12个状态变量，观测方程有由接收机输出的3个观测量，且每组状态变量和对应观测量相互独立。可分别对e，n，u(东北天坐标系)三个轴向的状态变量以及对应观测量单独滤波，以e轴模型建立进行讨论。

1.2北斗/GPS组合导航系统状态方程建立

由于飞行器是在三维空间的运动，考虑飞行器的位置、速度、加速度以及GPS在e轴方向上的总误差，采用CS模型[7]描述飞行器的运动。

状态方程为：

式中，X=[xeveaeεe]T，xe、ve、ae分别为东向位置、速度和加速度；εe为e轴方向上的总误差，等效一阶马尔可夫过程；Uk-1为加速度“当前”均值；Wk-1为系统噪声；状态转移矩阵为 Φk，k-1，系统噪声协方差阵为 Qk-1。将加速度一步预测作为“当前”加速度均值，则状态一步预测方程为：

式中：

1.3北斗/GPS组合导航系统观测方程建立

系统的观测量包括接收机输出的飞行器东向、北向和天向坐标分量 xe、xn、xu，各个方向的总误差 εe、εn、εu以及量测噪声(观测方程中仅列出东向观测量)[1]，实验中经纬高度量测值单位为 m，并用量测值叠加高斯白噪声。

观测方程为：

式中，观测值 Zk=E，量测噪声 V=ωZe，量测矩阵 H= [1001]。

Rk为观测噪声协方差R的离散化形式R=。

2　算法描述

2.1简化Sage-Husa算法

在线同时估计噪声统计特性会使维数较高的组合导航系统实时性变差，认为R对滤波影响最为明显，视Q 为定值[6]。

设线性离散时间系统为：

式(6)、式(7)中Xk是n维状态向量，Zk是m维观测序列，Φk，k-1是维状态转移矩阵，Hk是 m×n维观测矩阵[8]。Wk-1和Vk是相互独立的正态白噪声序列。算法描述如下：

2.2改进自适应算法

因飞行器的飞行环境复杂，且其运动规律未知，系统干扰存在不稳定性，仅仅实时估计量测噪声不能明显获得高精度导航信息，以及实时并准确估计出系统干扰。

可以通过CS模型中加速度协方差估计出系统干扰，但是在CS模型中，当飞行器弱机动时，其当前加速度较小，与较大的加速度上下限amax差值偏大，加速度协方差偏大，导致Q偏大。可见，加速度上下限不能自适应飞行器的机动特性，滤波器跟踪能力较差。因此，本文对加速度协方差的计算进行改进。利用速度估计、速度滤波预测之间的差值改进了加速度协方差的计算表达式：

由式(9)可知，当飞行器弱机动或无机动时，其速度滤波 vk、预测估计 vk，k-1之差较小，加速度协方差较小；当飞行器强机动时，其速度滤波 vk、预测估计 vk，k-1之差值较大，加速度协方差较大[9]。因此，不管飞行器机动性如何，滤波器都能保持较好跟踪。

在进行滤波过程中，不仅能实时估计R，而且根据飞行器的机动特性计算出加速度方差，从而较准确地估计出Q。

3　实验过程以及算法仿真

3.1实验过程

实验所用的接收机为国内和芯星通公司的北斗/ GPS双模模块，将其安置在无人飞行器上，通过无线数传模块与上位机CDT软件进行通信。让无人飞行器保持一定高度在操场实时采集并存储数据，采集频率1 Hz，实验时间350 s。飞行器的初始位置为纬度25.28°，经度110.33°，初始速度为 1 m/s，初始状态协方差 P0=diag (402，1.02，0.12，402)，初始加速度为0。滤波参数如下：

3.2算法仿真分析

实验仿真曲线、数据均来自MATLAB。

实验1：改进自适应算法仿真

根据建立的北斗/GPS组合导航系统数学模型，利用标准Kalman滤波、简化Sage-Husa算法和改进自适应算法对该模型经度方向误差进行滤波估计，滤波效果如图2和图3所示。同时统计了两种算法下的经纬度、速度均方根和均值误差，如表1所示。对每种方法进行M=100次蒙特卡罗仿真，用均方根误差衡量滤波精度，每一时刻的均方误差为(dik为真值与观测值的偏差)：

表1　两种算法的滤波结果

从图2、图3的仿真曲线可知，在相同时刻，改进自适应算法得到的位置滤波效果显然要优于另外两种算法，因为飞行器采集数据过程中，其运动规律和噪声未知。标准Kalman、简化 Sage-Husa算法均未同时估计 Q和R，也没有考虑加速度上下限对状态估计精度的影响，改进自适应算法不仅估计R，而且根据飞行器的机动特性自适应调整Q，跟踪精度提升。表1的统计结果也说明了这一点。

图2　三种滤波方法的经度均方根误差

图3　三种滤波方法的经度方向误差

实验2：北斗/GPS组合导航系统计算仿真

利用无反馈重置联邦滤波器对北斗、GPS信息进行融合，子滤波器均采用改进自适应算法。将北斗/GPS组合导航系统的导航信息与实际飞行数据作差，并分别与北斗、GPS子系统的仿真结果进行对比，仿真结果见图4～图6，相关统计量见表2。

图4　GPS系统东向误差

从图4～图6可以看出，GPS、北斗系统的位置误差分别在 1 m、2 m以内，而北斗/GPS组合导航系统的位置误差在1.2 m以内，其误差处于GPS和北斗系统之间。表2的误差统计结果表明北斗/GPS组合导航系统的位置误差仅次于GPS。这是由于北斗、GPS同属于卫星导航系统，系统性能、误差特性相似，优劣相当，因此同种类型导航系统之间的信息融合效果不及于不同类型导航系统之间取长补短带来的滤波效果。然而采用无反馈重置联邦滤波器的北斗/GPS组合导航系统，容错性能最好，且计算量少，满足导航系统实时性高的要求。

图5　北斗系统东向误差

图6　北斗/GPS组合导航系统误差

表2　组合导航系统误差统计

4　结束语

本文提出一种改进自适应算法，根据飞行器的运动状态实时估计Q，并估计出R，同时融合北斗、GPS系统信息，通过与标准Kalman、简化Sage-Husa算法的仿真比较，验证改进自适应算法能明显提高组合导航系统的导航精度。将改进自适应算法应用于北斗/GPS组合导航系统中，其定位精度虽与GPS相当，但是提高了系统容错性。随着中国北斗的不断发展，与其他卫星导航系统以及不同类型导航设备(如捷联惯导)之间的组合拥有更大的应用潜力。

[1]房建成，万德钧，吴秋平，等.GPS动态滤波的新方法[J].中国惯性技术学报，1997，5(2)：4-10.

[2]张辉，吴鹏，张国柱，等.基于容错性联邦强跟踪滤波的多星座组合导航算法研究[J].全球定位系统，2012，37 (4)：12-18.

[3]吴玲，孙永荣，陈传德.多星座组合导航自适应信息融合滤波算法[J].航天控制，2010，28(6)：38-4.

[4]孙永荣，吴玲，赵伟，等.多星座组合导航自适应联合卡尔曼滤波算法研究[J].宇航学报，2009，30(5)：1879-1884.

[5]SAGE A P，HUSA G W.Adaptive filtering with unknown prior statistics[C].Proceedings of Joint Automatic Control Conference，1969：760-769.

[6]兰义华，任浩征.一种基于“当前”模型的改进卡尔曼滤波算法[J].山东大学学报(工学版)，2012，42(5)：12-17.

[7]周宏仁，敬忠良，王培德.机动目标跟踪[M].北京：国防工业出版社，1994.

[8]KALMAN R E.A new approach to linear filtering and prediction problems[J].ASME J Basic Eng，1960，82：35.

[9]沈云峰，朱海.简化的 Sage-Husa自适应滤波算法在组合导航中的应用与仿真[J].青岛大学学报，2001，16 (1)：44-47.

An improved adaptive algorithm of BD/GPS dynamic navigation for UAV

Xu Wei1，Li Zhi1，2，Wang Yongjun2
(1.School of Electronic Engineering and Automation，Guilin University of Electronic Technology，Guilin 541004，China；2.Key Laboratory of unmanned aerial vehicle telemetry，Guilin Institute of Aerospace，Guilin 541004，China)

Compass can constitute integrated navigation system which provides high-precision,highly reliable information for unmanned aerial vehicle(UAV)with GPS or other types of navigation equipments.When using current statistical model(CS model)and standard Kalman filter to deal with dynamic navigation data,dynamic noise and observation noise was unknown and time-varying, and limits of acceleration couldn't be adaptive to the current acceleration of UAV.All of these has resulted in low navigation accuracy.To solve the problem,the paper presents an improved adaptive algorithm which adapts to UAV's motion,and applies to Compass/GPS dynamic navigation system.The simulation shows that it can effectively improve the accuracy and reliability,and adapt to the maneuvering characteristics of UAV better.

Compass；GPS；integrated navigation；unmanned aerial vehicle；adaptive algorithm

V448

A

10.16157/j.issn.0258-7998.2015.10.015

国家自然科学基金(61361006)；广西自动检测技术与仪器重点实验室基金项目(YQ14203)；广西高校无人机遥测重点实验室(WRJ2015ZR02)

2015-07-15)

徐伟(1989-)，男，硕士研究生，主要研究方向：组合导航、数据融合技术。

李智(1965-)，男，博士，教授，博士生导师，主要研究方向：自动测试总线与系统、现代测试理论与技术。

中文引用格式：徐伟，李智，王勇军.北斗/GPS无人飞行器动态导航中改进自适应算法[J].电子技术应用，2015，41 (10)：58-61.

英文引用格式：Xu Wei，Li Zhi，Wang Yongjun.An improved adaptive algorithm of BD/GPS dynamic navigation for UAV[J]. Application of Electronic Technique，2015，41(10)：58-61.