确定采样型滤波器优化算法及其在编队导航中的应用

丛源材　吴青坡　周绍磊

摘 要： 针对基于视觉的编队导航系统的导航参数估计问题，开展了确定采样滤波器对导航参数的估计研究。根据僚机体坐标系下的编队运动方程和视觉量测方程，建立了适用于确定采样滤波器的系统状态方程和量测方程。针对采样点不能满足姿态四元数的归一化要求，采用无约束的罗德里格参数代替四元数进行采样，选择的采样点能够满足编队导航系统的要求。采用3阶采样点的确定采样型滤波器对编队导航状态进行估计。UKF和CKF由于分别存在中心采样点权值过大和非局部效应而导致滤波发散，因此通过正交变换的方法对非局部效应进行修正，实现了估计误差的稳定收敛。通过仿真验证表明，提出的改进方法能够满足编队导航状态估计的要求。

关键词： 编队导航； 采样点； 状态估计； 正交变换

中图分类号： TN96?34 文献标识码： A 文章编号： 1004?373X（2015）05?0033?06

Optimization algorithm of determination sampling type filter

and its application in formation navigation

CONG Yuan?cai， WU Qing?po， ZHOU Shao?lei

（Department of Control Engineering， Naval Aeronautical and Astronautical University， Yantai 264001， China）

Abstract： To solve the problem existing in navigation parameter estimation for formation navigation system based on vision， the estimation of navigation parameter by the determine sampling type filter is studied. According to the formation motion equation and vision measuration equation in body frame of wing plane， the systematical state equation and measuration equation suitable for the determination sampling type filter are established. In consideration that the sampling point can not meet the normalization demand of attitude quaternion， the unrestraint Rodrigues parameter is used to replace quaternion. The sampling point selected by it can meet the demand of formation navigation system. The determination sampling type filter with third?order sampling point is used for the estimation of formation navigation state. Since the centre sampling point weight of UKF is oversize， and CKF has non?local effect， which may lead to filter divergence， a method of orthogonal conversion is used to correct non?local effect to realize stable convergence of evaluated error. Simulation results are given to prove the technique.

Keywords： formation navigation； sampling point； state estimation； orthogonal conversion

0 引 言

无人机编队飞行需要编队导航系统提供飞行器之间的编队位置、编队速度和编队姿态信息。目前通常采用GPS与惯性测量装置的组合来获得位置和姿态信息。由于GPS信号容易受干扰，因此对不依赖GPS信号的编队导航系统成为近年来的研究热点。基于视觉的导航系统可以取代GPS，提供编队导航以及绝对导航信息。本文以视觉编队导航为研究背景，对确定采样型滤波器在编队导航中的应用展开研究。

编队导航系统有如下几个特点，对滤波器提出了不同的要求：

（1） 系统方程中的状态包括编队姿态、编队位置、编队速度和惯性器件误差，系统状态维数为16，会出现权值为负或非局部效应。例如，采用UKF则会出现很大的负权值[ω0=-133；]而采用CKF，虽然可以避免这一情况的发生，但是由于采样点离采样中心过远，可能会存在非局部效应的问题；

（2） 状态向量中，各分量之间的数值大小差异较大，在采样时应当避免对数值较小分量的忽视；

（3） 采用四元数描述姿态，在采样时会遇到问题，由于四元数要求[q=q20+q21+q22+q23=1，]而得到的采样点中，这一条件无法满足。因此需要采用无约束的状态变量来替代四元数进行采样。

1993年，Shuster提出了一种由3个变量组成的姿态误差向量来表示姿态四元数的误差向量[1]，这3个变量可有多种表示方法，包括Gibbs向量（180°出现奇点）和修正Rodrigues参数（360°出现奇点），虽然存在奇点，但是由于是用来表示误差的，所以不会发生奇点出现的情况。本文采用无约束的Rodrigues参数来描述编队姿态[2?3]。

1 确定采样滤波器对编队导航参数的估计

1.1 编队导航运动方程

1.1.1 编队姿态运动方程

对于姿态的描述有多种描述方式，如旋转矢量、方向余弦矩阵、四元数和修正罗德里格参数。通常采用方向余弦矩阵和四元数来表示坐标系之间的转动关系，由于方向余弦矩阵有9个未知参数，而四元数只有4个，因此在姿态更新算法中采用四元数的计算量更小、精度更高[4]。当向量在不同坐标系下变换时，习惯上采用方向余弦矩阵，而方向余弦矩阵和姿态四元数之间可以相互转换，因此文中采用四元数法进行姿态更新，在坐标变换时仍然采用方向余弦矩阵来表示。

解算坐标系（b系）是指进行编队位置解算的坐标系。编队飞行中，通常选用导航坐标系或体坐标系。解算坐标系相对惯性空间的转动引起的姿态变化通过姿态运动方程来描述，可以用方向余弦矩阵或四元数来表示。

1.1.2 编队质心运动方程

编队飞行示意图如图1所示。

其中，[ηfgu]和[ηfau]为高斯白噪声。

1.3.2 量测方程

式中[Pj]为第[j]个光标的安装位置。

1.3.3 编队导航参数估计

通过式（17）得到每个采样点对应的误差Rodrigues参数，将采样点中的编队位置、编队速度和惯性器件误差部分代入系统状态方程从而得到预测更新后的采样点[χi。]

在整个滤波过程中，姿态四元数用于编队导航状态的更新，而Rodrigues参数用于获得采样点和计算均值及协方差。

2 应用举例

选取一个固定的地理坐标系[n，]给出初始经度[λ=38°，]纬度[?=-77°，]在该地理坐标系下长机和僚机的飞行轨迹为[5]：

长机位置轨迹：

3 结 论

通过构建僚机体坐标系下基于视觉量测的无人机编队导航系统模型，采用确定采样型滤波器对编队导航状态进行估计。采用姿态四元数来完成编队姿态的更新，由于四元数有归一化要求，而在滤波过程中，获得的采样点中的姿态部分并不能满足这一要求，因此需要采用无约束的参数来表示姿态。文中在获取采样点时以广义罗德里格参数来代替四元数，通过仿真对不同的采样策略下的编队导航参数的估计效果进行了分析。仿真结果表明，本文提出的改进方法能够满足编队导航状态估计的要求。

参考文献

[1] SHUSTER M D. Survey of attitude representations [J]. Journal of Astronautical Sciences， 1993， 41： 439?517.

[2] CHEON Y J， KIM J H. Unscented filtering in a unit quaternion space for spacecraft attitude estimation [C]// IEEE International Symposium on Industrial Electronics. Vigo， Spain： IEEE， 2007： 66?71.

[3] MARKLEY F L， CRASSIDIS J， CHENG Y. Nonlinear attitude filtering methods， AIAA 2005?5927 [R]. San Francisco， California， USA： AIAA， 2005.

[4] MORTENSEN R E. Strapdown guidance error analysis [J]. IEEE Transactions on Aerospace and Electroic Systems， 1974， 10（4）： 451?457.

[5] FOSBURY A M. Control and Kalman filtering for relative dynamics of a formation of uninhabited autonomous vehicles [D]. Buffalo， USA： State University of New York， 2006.

[6] 酒锐波，王彪，曹云峰.基于DSP的无人机编队视频跟踪技术[J].现代电子技术，2012，35（6）：99?101.