噪声相关情况下的强跟踪滤波器在飞行器姿态确定中的应用

郑均辉，张国平

噪声相关情况下的强跟踪滤波器在飞行器姿态确定中的应用

郑均辉，张国平

针对飞行器姿态估计系统问题，标准 EKF 存在不能克服测量噪声和过程噪声相关和抗干扰能力差的缺陷。为此，提出了一种噪声相关的强跟踪滤波器算法，该算法通过引入噪声相关矩阵和自适应因子来调整状态协方差矩阵和增益矩阵，可以较好地克服干扰和噪声相关的情况。通过实验仿真结果表明算法的有效性。

飞行器姿态估计；强跟踪滤波器；噪声相关；估计精度

0 引言

飞行器姿态确定系统是飞行器姿态控制系统的重要组成部分，其精度直接影响姿态控制系统的精度，飞行器姿态通常采用滤波的方法从概率统计角度最优的估计出飞行器的姿态。由于飞行器姿态系统是一个非线性系统结构，因此，需要用非线性滤波方法才能较好地得到系统状态变量的最优估计。

EKF方法作为最早提出的非线性滤波方法之一，在工程上有着广泛的应用，文献[1]将EKF用于两种MRPs误差模型下的飞行器姿态确定。文献[2]将EKF和噪声估计器相结合用于对地三轴稳定的飞行器进行姿态估计。但是，上述都没有考虑噪声相关的情况和系统存在噪声干扰的情况。而飞行器的实际运行空间环境是错综复杂的，状态系统和测量系统常常会受到未知的干扰，而且，测量噪声和过程噪声也不相互独立，这会使得EKF的估计性能下降。针对未知干扰问题，周东华等提出了强跟踪滤波器（STF），STF具有较好的关于模型不确定性鲁棒性和较强的关于突变状态的跟踪能力。但是，文献[3]并未讨论噪声相关情况下的强跟踪滤波情况。

为此，针对上述问题，本文提出了一种噪声相关情况下的强跟踪滤波算法，将该算法应用与飞行器姿态估计系统中进行仿真，仿真结果表明该算法相对于EKF和传统的强跟踪滤波器, 能够较好的解决状态系统和测量系统受到干扰以及测量噪声和过程噪声相关的情况，具有更好的估计精度和适应性。

1 系统状态方程及测量方程

由MRPs表示的姿态运动学方程为公式（2）：

式中，I3为单位矩阵，为星体的三轴角速度，的斜单位对称矩阵。

从MRPs到四元数的转化关系通过下式实现，即公式（3）：

在没有陀螺提供角速度的情况下，需要利用飞行器的姿态动力学方程为公式（4）：

姿态测量方案采用基于星敏感器的双矢量观测，离散观测方程为公式（6）：

2 噪声相关的强跟踪滤波算法

将公式（5）和（6）组成的飞行器姿态确定系统用4阶龙格库塔法离散，得到如下非线性离散系统公式（8）：

公式（8）中，xk为系统状态矢量，uk为控制输入向量，zk 为测量矢量，wk 和vk分别为状态系统高斯白噪声和测量系统高斯白噪声，它们的统计特性满足[6]公式（9）：

本文要解决的问题为针对飞行器姿态确定系统（8）具有公式（9）所示的噪声统计特性设计强跟踪滤波器使其满足下列条件[7]公式（10）：

证明：具体证明步骤可参考文献[3]和[7]，这里只给出不同之处如公式（12）：

根据公式（9）可得公式（13）：

将公式（13）代入公式（12）即可得公式（11），证闭。

定理2 针对公式（8）和（9）所示的飞行器姿态确定系统，有如下强跟踪滤波器算法为公式（14）～（25）：

证明：根据最优滤波准则，定理2中的公式（14）、（15）、（16）和（17）显然成立。这里只需证明公式（18）和公式（19）即可。

根据协方差矩阵的定义可知公式（26）：

和公式（28）：

由公式（26）、（27）和（28）可得公式（29）：

将公式（30）代入公式（29）即可得公式（19），证闭。对于引入自适应调节因子后，滤波器满足公式（10）的性能指标的证明可参考文献[3]和文献[7]以及结合定理1很容易得出。值得说明的是本文将引入的是两个自适应调节因子矩阵，这样可以保证协方差矩阵的正定性和对称性，增强滤波算法的稳定性。

3 实验仿真结果

为了验证本文提出的自适应UKF算法的有效性，本文将其应用与飞行器姿态估计系统当中，仿真参数设置为：

状态初始参数

状态估计初始参数

协方差矩阵

过程噪声

观测噪声

参考矢量

惯量矩阵

实验仿真分2种情况：

情况1：测量噪声和过程噪声不相关，即Sk=0 ，另外同时为了说明噪声相关的强跟踪滤波器算法的抗干扰能力，在仿真时间为150s引入状态突变如图1和图2所示：

图1 飞行器三轴姿态角速度估计误差曲线

图2 飞行器三轴姿态角度误差曲线

情况1：主要是为了比较噪声相关的强跟踪滤波器和强跟踪滤波器的估计精度，从图1和图2可以看出，在系统噪声不相关的情况下，强跟踪滤波器和本文提出的噪声相关的强跟踪滤波器的对飞行器的三轴角速度和姿态角的估计精度相差不大，而且都能够较好地克服系统外界干扰，而EKF则不能克服外界的干扰，姿态角和三轴角速度的估计精度在受到干扰的时候变化很大。这是因为强跟踪滤波器和本文提出的噪声相关的强跟踪滤波器都引入了自适应调节因子，使其能够在线调整状态增益矩阵，从而使得残差序列满足公式（8）所示的性能指标。

图3 飞行器三轴姿态角速度估计误差曲线

图4 飞行器三轴姿态角度误差曲线

4 总结

本文针对飞行器姿态确定系统中存在干扰和噪声统计特性相关的情况的姿态估计问题，标准EKF存在不能克服测量噪声和过程噪声相关和抗干扰能力差的缺陷。为此，本文提出了一种噪声相关的强跟踪滤波器算法，该算法通过引入噪声相关矩阵和自适应因子来调整状态协方差矩阵和增益矩阵，可以较好的克服干扰和噪声相关的情况。通过实验仿真结果表明本文提出的算法的有效性。

[1] 赵琳,黄晨,丁继成.基于MRPs两种误差模型的卫星姿态确定方法研究[J].计算机测量与控制,2011,9(8):1972 -1976.

[2] 赵琳,苏中华,郝勇.基于对偶数和改进EKF的敏捷卫星姿态确定算法[J].系统工程与电子技术, 2013,35(12): 2552-2558.

[3] 周东华,席裕庚,张钟俊.非线性系统次优渐消因子的扩展卡尔曼滤波[J].控制与决策,1999,5(5):1-6.

[4] Idan M. Estimation of Rodrigues Parameters From Vector Observations [J]. IEEE transactions on aerospace and electronic system, 1996, 32(2): 578-586.

[5] 张红梅,邓正隆,高玉凯. UKF在基于修正罗德里格参数的飞行器姿态确定中的应用［J］.宇航学报,2005, 26(2): 164-167．

[6] 付梦印,邓志红,张继伟.kalman滤波及其在导航系统中的应用[M].北京:科学出版社,2003.

[7] 周东华,席裕庚,张钟俊.一种带多重次优渐消因子的扩展卡尔曼滤波器[J].自动化学报,1991,17(6):689-696.据，并传递至RSU。

2 CDT-RVC方案

第3节概述了合作式数据传输核心思想。接下来分析CDT-RVC方案中的群Group形成、数据分组及传输、数据整合三个部分。

2.1 群Group形成

静态和移动群Group的形成是本文的重点。移动车辆通过寻找其他移动车辆，并构成群Group，这个过程类似于节点簇Clustering。然而，不同的是，群Group内合作车辆间数据传输存在信号干扰[10]。例如，处于同群的车辆有可能同时向静态车辆发送消息，这会引起干扰。

（1）移动群Group形成

假定移动车辆M1需找周围车辆合作，构成群。那么移动车辆Mi，能够被选为合作伙伴，纳入群内的条件就是：该车辆移动Mi与车辆M1移动方向相同，并且距移动车辆M1为两跳的通信距离。

假定移动车辆M1欲寻找n 个合作伙伴，如图2所示：

图2 移动群的形成示意图

移动车辆M1，通过转发车辆，间接地向距两跳的邻居车辆（假定为移动车辆M2）发送邀请。相应地，移动车辆M2也采用同样的方法，间接地向距两跳的邻居车辆M3发送邀请，直到有n个合作伙伴或无更多邻居节点加入群，便停止寻找。这些合作伙伴M2、M3,×××,Mn向移动车辆M1回复邀请，并记录数据传输的路由，以保证整个群内车辆是连通的。

通过上述分析可知，移动群的形成是由移动车辆个人发起，并由多个移动车辆合作完成。如图2所示，移动车辆M1向M2发送邀请，若M2自己已有k个合作伙伴的群，并且这时M2直接向M1直接传输邀请回复。如图3所示：

清代解《诗》以诗话的体式，相对于汉、宋传统解经模式，其文学特性彰显，有不少横溢出传统经学模式之外的灵心妙解，《诗》的文学情意被激发出来。“以诗话解《诗》”的主要特点如下。

图3 移动群的形成示意图

若kn-1，M2继续向后传递邀请，最终形成n个合作伙伴。

（2） 静态群Group形成

寻找RSU附近的Parking vehicles，并构建静态群Group。Parking vehicles（假定S1）寻找邻近的其余Parking vehicles建立静态群Group。邻近的其余Parking vehicles（假定S2）能够成为合作伙伴，并纳入静态群Group的条件就是：S2是停靠在路边，并且与S1相距两跳的距离。之所以要相距两跳距离[11]，是为了防止干扰。

在形成静态群Group中，首先由RSU向邻近的Parking vehicles广播邀请，接收到邀请的车辆，继续广播。若车辆数量较多时，势必产生广播风暴问题。为此，采用定时器优先广播原则。

静态Parking vehicles收到来自RSU的邀请消息后，进入等待状态，并设置定时器，当定时器计时完毕后，并且没有收到其他节点重播消息，即自己的定时器是最先计时完毕，再重播邀请消息。定时器的运行时间正比于离RSU的距离，如公式（1）：

其中，rsi为车辆si到RSU的距离。T为最长的等待时间。

从公式（1）可知，离RSU最远的Parking Vehicles率先计时完毕，具有最高的重播优先权。

每个车辆都保存转发邀请记录。最终，静态车辆连接至以RSU为根的树[12]。在tree中的节点的父节点也称为上流节点（Upstream node）。假定，RSU发送的邀请消息内有一区域，该区域有于记录消息转发的次数h 。最初，h=0，每当邀请消息被转发一次，h就加一，并存于邀请消息内。通过这种方式，收到邀请消息的静态车辆知道距RSU的有几跳的距离。

RSU先发送邀请消息，转发节点收到后，便转发至静态车辆s1，以同样的方式，静态车辆s1向后继车辆重播邀请消息，如图4所示：

图4 静态群的形成示意图

2.2 数据分组及传输

由于在移动群Group或静态群Group中的合作伙伴均相隔两跳的距离，每个合作伙伴均能够在没有任何干扰的情况下传输数据。当经过RSU或静态车辆身边，移动车辆便向其传输数据[12]。

2.3 数据整合

将数据分成向几组传输后，接收端需要还原数据，即数据整合。处于同群的静态合作伙伴将接收到的数据组向RSU传输，处于同群的移动合作伙伴将接收的数据组向移动群的领头车辆传输。

最简单的数据整合方案就是one-by-one原则，如图5所示：

图5 one-by-one原则的数据整合过程示意图

最左边的合作者将数据传输至群的转发者，随后，转发者再将数据传输至第二合作者。接收数据后，第二合作者将接收的数据加上自己的数据一起传输转发者，一直按此方法进行，直到数据传输到目的节点。这种方式不存在任何竞争以及碰撞，因为任何时间只有一个节点在传输数据。

如图5所知，最初合作伙伴M1、M2、M3均存有一个数据组。依据one-by-one的数据整合原则，第一轮，M3将自己的数据组传输至转发者F1，再由F1转发至合作伙伴M2。此时，M2获取两个数据组。第二轮，M2将两个数据组传输至转发者F2，转发者F2再转发至M1。第三轮，M1将收到两个数据组加上自己拥有的数据组，共三个数据组，一并传输至目的节点。通过这种“接力”的方式，整合数据。

3 性能分析

在分析线-线（Line-to-Line）合作模型之前，先分析基本的点对点传输，如图6所示：

图6 点对点模型中的传输距离

假定无线传输范围为r米，移动车辆与静态车辆的垂直距离为b ，因此，两个移动车辆能保持通信的最长距离为2l，且。假定移动车辆的平均速度为无线信道的带宽为Bbyte/s。那么，移动车辆与静态车辆间能够传输的最大数据为

点对线模型如图7所示：

图7 点对线模型传输距离

假定静态车辆均匀相隔两跳的通信距离。两个连续静态车辆间的距离最大为2r 。移动车辆与静态车辆间的距离为2l 。因此，n 个成员的静态群传输的最远距离为相应地，传输的数据总量等于

若移动群内有m个成员，每个成员传输相同的数据量。因此，对于线-线模型情况内传输的总数据等于相对点对点情况，传输的数据量提高了

在不同合作模式的数据传输情况如图8所示：

图8 在不同合作模式的数据传输

接下来，分析整合数据所耗的时间。假定每个合作者拥有x 字节数据，并且需要将其传输至最右边的合作者。利用one-to-one整合模型，总的整合时间等于数据仿真结果如图9 所示：

图9 数据整合时间随节点数的变化情况

4 总结

车联网VANET提供了车与路边设施RSU间的RVC通信，便捷了数据传输。然而，车辆的快速移动，缩短了RVC通信连接，降低了数据传输吞吐量。为此，本文提出基于合作式数据传输RVC通信CDT-RVC方案。CDT-RVC方案首先依据车辆是否移动，将车辆分为静态车辆和移动车辆两类。然后引用群通信理念，将静态车辆组建成静态群，将同向行驶的移动车辆构成移动群。当移动车辆需数据传输至RSU时，首先将数据分成几个小组，并分别传输至群内的其他成员，再由这些成员转发至它们身边的静态车辆，最终由这些静态车辆传输至RSU。同理，RSU向移动车辆传输数据与移动车辆向RSU传输过程类似。仿真结果表明，提出的CDT-RVC方案有效延长RVC连接时间，增加数据传输吞吐量。

参考文献

[1] Zeadally,S, Hunt,R., Chen,Y., Irwin, A., and Hassan, A. Vehicular Ad Hoc Networks (VANETs): Status, Results, and Challenges[J].Telecommunication Systems,2012,51 (3):2-13.

[2] 洪棒，俞立，张贵军. VANETs自适应分布式聚簇路由协议[J]. 自动化学报.2011,37(10):1197-1206.

[3] Liu N. Liu M. Chen G. and Cao J. The Sharing at Roadside:Vehicular Content Distribution Using Parked Vehicles[J]. in Proc. IEEEINFOCOM, 2012,3(6):2641-26 45.

[4] Ali G. G. M. N. and Chan E.Co-operative Data Access in Multiple Road Side Units -based Vehicular Ad Hoc Networks [C].in Proc. Australasian Telecommunication Networks and Applications Conf., 2011: 1-6.

[5] HungC.C. Chan H. and Wu. E. H. KMobility Pattern Aware Routing for Heterogeneous Vehicular Networks[C]. in Proc. Wireless Communications and Networking Conf.2008: 2200-2205.

[6] Cheng P.C. Lee K.C. Gerla M.GeoDTN+Nav: geographic DTN routing with navigator prediction urban vehicular environments[J]. MONET, 2010,15(1): 61-82.

[7] Jarupan B. and Ekici E.PROMPT: A cross-layer position-based communication protocol for delay-aware vehicular access networks[J]. Ad Hoc Networks, 2010,8 (5): 489-505.

[8] Lu N. Luan T.H. Wang M. Shen X.S. and Bai F. Capacity and delay analysis for social-proximity urban vehicular networks[J]. in Infocom 2012,67(12):1503-1511.

[9] Martelli F. Renda M.E. Resta G. and Santi P. A measurement-based study of beaconing performance in IEEE 802.11p vehicular networks[J]. .in Infocom 2012,67(12):1545-1556.

[10] Lo S.C. Lin Y. J. and Gao J. S.A Multi-Head Clustering Algorithm in Vehicular Ad Hoc Networks [J]. Intl. Journal of Computer Theory and Engineering.2013,5(2): 242-247.

[11] X. Ma, J. Zhang, X. Yin, and K. S. Trivedi. Design and analysis of a robust broadcast scheme for vanet safety-related services[J]. IEEE T.Vehicular Technology, 2012,61(1):46-61.

[12] LoS.C. Gao J.S. and Tseng C.C.A Water-Wave Broadcast Scheme for Emergency Messages in VANET[J]. Wireless Personal Communications, 2013,71(1): 217-241.

[13] TonguzO. K. Wisitpongphan N. and Bai F. Dv-cast: A distributed vehicular broadcast protocol for vehicular ad hoc networks[J].IEEE Wireless Commun., 2010,17(2): 47-57.

（收稿日期：2015.02.14）

TP311

A

2014.12.26）

1007-757X(2015)07-0050-03

河南省科技攻关基金资助项目（122102210258）

郑均辉（1981-），男，汉族，四川叙永县人，平顶山学院，讲师，硕士，研究方向：算法分析，GPS数据处理等，平顶山，467000

张国平（1980-），男，汉族，江西南昌人，平顶山学院，讲师，硕士，研究方向：算法分析，移动通信应用等，平顶山，467000