考虑噪声特性未知的多传感器姿态融合方法

王 硕，宋申民，于 浛

（哈尔滨工业大学 控制理论与制导技术研究中心，哈尔滨 150001）

考虑噪声特性未知的多传感器姿态融合方法

王 硕，宋申民，于 浛

（哈尔滨工业大学 控制理论与制导技术研究中心，哈尔滨 150001）

研究了噪声特性和互协方差未知情况下多传感器系统的协方差交叉融合估计问题，其中噪声特性未知是指系统的状态噪声和量测噪声的相关性和真实方差均是未知的。首先，每个子系统根据最新更新时刻的测量信息，选择相应的CKF估计器，从而得到各子系统在每一时刻的一个局部估计。其次，以矩阵加权线性最小方差为最优融合准则，提出了一种快速连续的协方差交叉(SCI)融合策略，将多维优化问题简化到对几个一维非线性代价函数的优化，得到最优融合估计。再次，在子系统的估计中采用误差四元数和误差修正罗德里格斯参数相互切换的方法来避免四元数的规范约束和误差修正罗德里格斯参数奇异的发生。最后，通过一个卫星姿态估计的例子验证了所提方法的有效性。

CKF估计；信息融合；协方差交叉；四元数；罗德里格斯参数

随着航天器和空间系统复杂性的不断加大，需要对航天器的姿态进行更加准确和鲁棒的估计。对每个航天器的功能需求正在增加，而航天器本身的尺寸却越来越小，且硬件之间是相互独立的，这就导致系统的计算能力受到很大限制。这样的系统常常被称是操作响应空间(ORS)。ORS系统的主要目标是为了航天器使用模块化的类型来建设加速任务概念推出的时间。这种模块化的系统，定制的接口和软件必须管理传送到中央处理器的所有数据。然而，潜在的危险情况是数据只传播一部分，并不是全部。处理器是一个大型的分布式系统。由于数据不能遍布每个处理器进行运算，从这个角度上说估计结果是次优的。在理想的情况下，有可能把所有的次优估计通过某种方式结合起来而处理所有数据，从而得到最优解。这样做的一个主要缺陷是有多个估计，而每个估计之间的相关性是无法确定的。

为了克服此数据融合问题，一个简单而有效的方法是采用协方差交叉(CI)算法[1-2]。CI算法通过考虑误差空间的交叉来克服未知的相关性问题，而获得一个保守但却一致的融合结果。文献[3]表明，CI算法可以被视为一种优化问题的解决方案，使得单个估计和融合估计之间的权重误差最小。而对多传感器数据通过批处理获得鲁棒的CI融合估计，鲁棒CI融合的获得是基于多维非线性代价函数的优化，这需要更大的计算负担[1,4]。在特殊情况下，鲁棒 CI融合的一个快速近似解析解在文献[4]中获得。为了减少复杂性和计算负担，通过连续处理，基于连续的两传感器CI融合容积卡尔曼滤波器，提出了一个快速连续的协方差交叉(SCI)容积卡尔曼融合算法。这是一个快速递归的两个传感器CI融合，是把多维优化问题减少到对几个一维非线性代价函数的优化。而一维优化问题则可以通过黄金节分割点方法或斐波那契法简单的解决。

在局部估计中采用四元数作为姿态的参数化描述，给出了一些解决办法，解决四元数的CI方程，同时保持约束性。然而，这种情况下状态向量融合包括多个四元数，文献[3]的方法不能得到广义的 CI解决方案，因为状态必须服从多个二次等式约束。Nebelecky等人在文献[5]中给出了一个解决办法，使用迭代的Newton-Raphson方法来确定最优融合估计。解决四元数 CI融合问题的主要困难在于保持四元数的规范约束。避免这个问题最为直接的方法是使用三维的姿态参数描述，如罗德里格斯参数和修正罗德里格斯参数[6-8]。自1965年Wahba问题提出以来，四元数就一直是姿态参数化首选的描述方式[9]。然而，近些年修正罗德里格斯参数(MRPs)也已经被广泛应用于航天器的姿态估计和控制当中。Karlgaar等在文献中给出了经典的修正罗德里格斯参数[10]，这些参数是由四元数到三维超平面立体投影的结果。需要指出的是，罗德里格斯参数和MRPs参数是对称的球面参数的一部分。而所有的三维参数描述姿态又存在特定的奇异现象，这使问题变得更复杂了。解决这个问题的简单有效方法是采用四元数和三维参数化表示相互切换进行姿态的估计。这种方法在非线性估计技术中普遍使用，如容积卡尔曼滤波(CKF)和粒子滤波(PF)[11-15]。其中，CKF在对非线性系统的估计中表现尤为突出，在数值稳定性上明显优于UKF，且滤波精度接近PF。

本文研究了噪声特性未知情况下多传感器系统的CKF融合估计问题，提供了一种新的SCI融合方法。将每组传感器所对应的局部估计系统建模成一个子系统。进而，每个子系统根据最新测量信息的更新时刻，选择相应的CKF估计器，从而得到各子系统在每一时刻的局部最优估计。然后根据矩阵加权最小方差融合准则得到最优的SCI融合估计。SCI融合的姿态使用误差四元数和误差修正罗德里格斯参数相互切换的估计方法获得，避免了四元数描述参数的规范约束，同时也避免了滤波过程中奇异的发生。最后，通过一个姿态估计的例子验证本文所提方法的有效性和优越性。

1 协方差交叉融合

1.1 CI融合过程

协方差交叉融合的相关估计已经取得了广泛应用，本文将其应用到航天器的姿态确定。研究中假设航天器上装有两个星敏感器，每个敏感器各自敏感测量值。每个星敏感器又都有各自的容积卡尔曼滤波处理的星测量值，两个滤波器的状态量使用同一组陀螺仪的输出值，并且呈现出了相关性，系统结构如图1所示。使用CI算法，可以更好地将两个估计进行融合，获得新的状态向量xCI和协方差PCI。

图1 CI融合结构图Fig.1 CI fusion structure

图2 CI融合的几何意义Fig.2 Geometric meaning of CI integration

图 2显示了由两个测量协方差椭圆(实心椭圆)结合成一个融合协方差椭圆(虚线椭圆)的示例。当个体之间的相关性估计精确已知时，最优协方差可以完全重建，结果得到的椭圆完全在两单个估计协方差椭圆的交点上。当相关性未知且使用CI算法进行融合时，获得的融合协方差椭圆将通过两单个协方差椭圆交叉的4个点，如图2中所示。

1.2 问题描述

考虑如下带有加性噪声的非线性连续系统：

1.3 鲁棒协方差交叉融合

这里加权参数 θi的优化过程是一个一维的优化问题，可以采用0.618法或斐波纳契法，也可以使用Matlab自带的工具箱进行运算。

由式(4)到(6)可以得到：

上述方法很好地解决了互协方差未知情况下的信息融合问题。但是在多于两个传感器时，该方法的计算负担较大。下面基于两传感器CI融合容积卡尔曼滤波器给出了一个快速连续的协方差交叉(SCI)融合容积卡尔曼滤波，这是一个快速递归两个传感器CI融合过程。

1.4 连续协方差交叉融合

为了减少在多传感器情况下的复杂性和计算负担，通过连续处理，基于(N-1)步两传感器 CI容积卡尔曼融合，给出了一个递归的 CI容积卡尔曼融合算法。(N-1)步两传感器 CI容积卡尔曼融合过程如图 3所示，其具体的(N-1)步实现如下：

该算法如图3所示。

图3 连续协方差交叉融合结构图Fig.3 Consecutive covariance intersection structure

反复使用两传感器CI融合算法(4)到(6)，SCI容积卡尔曼融合可以通过两传感器CI融合递归实现。它包括N-1次两传感器CI容积卡尔曼融合，具体数学表示为

从式(12)可以看出，这是一个一维的优化问题，可以通过黄金分割法或斐波那契法解决。因此，SCI融合算法的优化过程就转化为 N-1步的一维优化问题，这样易于实现，且增加了运算的快速性。

1.5 连续协方差交叉融合算法的精度分析

定理 1 对于互协方差Pij未知的多传感器系统(1)和(2)，SCI容积卡尔曼融合是一致的。即，对任意未知Pij有

式中，θ1∈[0,1]，其解的获得是将优化目标函数最小。

引理 1 对于互协方差Pij未知的多传感器系统(1)和(2)，SCI融合的整体处理表示为

定理 2 对于互协方差Pij未知的多传感器系统(1)和(2)，的CI融合鲁棒精度高于的SCI融合精度，

证明 由式(6)可以得到：

再由式(24)有：

2 航天器姿态运动模型和矢量观测模型

2.1 姿态运动学模型

姿态运动学模型采用四元数描述可表示为

式中，ω表示角速度，矩阵Ω( ω)可用如下表示：

式中，ω=[ω1ω2ω3]T，[ω×]是ω的反对称矩阵，表示为

2.2 陀螺输出模型

假设陀螺固连在航天器上，并且陀螺的安装方向与航天器本体坐标系重合，可直接敏感航天器的角速度，则陀螺输出模型可以表示为

式中，Δt代表步长，Nv与Nu是不相关的零均值高斯白噪声。

2.3 矢量观测模型

星敏感器通过观测天体方向对照星历表来测定航天器的姿态。假设星敏感器的安装方向与航天器本体坐标系重合，则星光矢量在航天器本。体坐标系下的观测方程为

式中：r是星光矢量在惯性系下的单位矢量方向，可查询星历表获得；是惯性系到星本体坐标系的转换矩阵；v是敏感器的观测误差，这里认为是高斯白噪声。假设有m个敏感器进行观测，则在第k时刻，用四元数描述的矢量观测模型为

式中：bm和rm是第m个参考矢量分别在体系和惯性系下的分量；A(q)是姿态转移矩阵，其四元数形式的描述为[18]

展开形式是

3 CKF姿态估计

在第二节中提出的SCI解决方案是代表一个全局性的方法，唯一的假设是修正罗德里格斯参数的存在。这种方法非常适合应用在姿态是由修正罗德里格。斯参数直接估计的情况。然而，大多数的航天器应用。中姿态是采用四元数表示的。在这种情况下修正罗德。里格斯参数只是用来完成航天器姿态的数据融合。

航天器采用式(33)给出的运动学方程描述，角速度来自陀螺的输出值，系统的测量方程由式(38)表示，系统的状态向量为。本文中对单个传感器的估计过程以CKF为滤波框架，采用误差四元数和误差修正罗德里格斯参数相互切换的方法来对航天器姿态进行估计。

3.1 时间更新

为了避免采用四元数描述姿态时存在的冗余导致滤波过程中协方差阵出现奇异的状况，在此将状态向量选为为与误差四元数对应的修正罗德里格斯参数：

由容积误差四元数可以获得一步预测估计的容。积四元数点集为

陀螺的角速度估计为

获得下一步的容积四元数后，再使其转换为容积误差修正罗德里格斯参数。计算一步预测容积误差四。元数得：

将容积点误差四元数转换为容积误差修正罗德里格斯参数：

容积点误差预测均值为

3.2 量测更新

以单个星敏感器为例生成观测容积估计点为

通过以上容积点得到量测容积点均值：

求取滤波增益得：

其中协方差阵和互协方差阵分别表示如下：

再由式(50)可得误差修正罗德里格参数和陀螺漂移的量测更新为

进一步，得到容积点四元数：

协方差阵更新为

4 仿真分析

通过估计近地轨道航天器的姿态来验证SCI融合的有效性。假设航天器本体坐标系与轨道坐标系重合，。并假设航天器配有三个相同的星敏感器。角速度和角度矢量分别来自三轴陀螺仪和星敏感器的输出。星敏感器测量带有标差为2″的高斯白噪声。陀螺和星敏感器的采样周期为1 s，仿真时长是8 h。假设初始角度估计误差较大，。这里假定三轴初始角度估计误差分别为45°、45°、155°，陀螺漂移初始估计误差为零，。方差阵初始值每个星敏感器运行各自的容积卡尔曼滤波器和一个共用的陀螺仪，如图1所示。下面分别给出鲁棒CI融合和SCI融合的姿态估计结果。

4.1 鲁棒CI融合估计结果

为便于理解，图4和图6分别给出的是角度估计误。差和陀螺漂移估计的2-范数曲线，图5给出的是三轴角度估计误差随时间的变化曲线，图7给出的是陀螺漂移估计误差随时间的变化曲线。在仿真中可以看出，。在容积卡尔曼滤波框架下采用鲁棒CI融合估计得到的角度估计值和陀螺漂移估计值都有较好的估计精度，角度估计精度达到了0.002°以内，陀螺漂移估计精度达到了0.01 (°)/h以内，并且具有很好的收敛速度。

图4 角度估计误差2-范数Fig.4 2 - norm curve of angle estimation error

图5 三轴角度估计误差Fig.5 Three-axis angle estimation errors

图6 陀螺漂移估计误差2-范数Fig.6 2 - norm curve of gyro drift estimate error

图7 陀螺漂移估计误差Fig.7 Estimate error of gyro drift

4.2 SCI融合估计结果

应用第2节给出的SCI融合算法对三个姿态跟踪系统进行融合处理，并给出了融合结果。图8和图10分别给出的是角度估计误差和陀螺漂移估计的2-范数曲线，图9给出的是三轴角度估计误差随时间的变化曲线，图11给出的是陀螺漂移估计误差随时间变化曲线。

与鲁棒CI融合估计的仿真对比发现，SCI融合后的角度估计精度和陀螺漂移估计精度都有所提高，其中陀螺漂移估计精度提高较为明显，角度估计精度提高了10%左右，陀螺估计精度提高了20%左右。从仿真结果可知，SCI融合估计的收敛速度要明显高于鲁棒CI融合估计。

图8 角度估计误差2-范数Fig.8 2 - norm curve of angle estimation error

图9 三轴角度估计误差Fig.9 Three-axis angle estimation error

图10 陀螺漂移估计误差2-范数Fig.10 2 - norm curve of gyro drift estimation error

图11 陀螺漂移估计误差Fig.11 Gyro drift estimation error

5 结 论

本文给出了一种高效的CI融合算法。先将每组传感器所对应的局部估计系统建模成一个子系统，在融合过程中利用CKF滤波器得到各子系统的一个局部最优估计，然后使用矩阵加权线性最小方差最优融合准则获取最优融合估计。局部估计框架的状态描述采用误差修正罗德里格斯参数和误差四元数相互切换来避免四元数规范约束和奇异的发生。此外，本文提出的SCI融合算法具有良好的计算效率。把多维非线性代价函数的优化问题减少到对几个一维非线性代价函数的优化。通过与鲁棒CI融合算法进行仿真对比，。验证了本文融合方法的有效性和优越性。

（References）

[1] Uhlmann, J K. General data fusion for estimates with unknown cross covariances[C]//Proceedings of the SPIE. Orlando, FL, 1996: 536-547.

[2] Julier S J, Uhlmann J K. A non-divergent estimation algorithm in the presence of unknown correlations[C]// Proceedings of American Control Conference. Vol.4, Albuquerque, NM, June 1997: 2369-2373.

[3] Crassidis J L, Cheng Y, Nebelecky C K, Fosbury A M. Decentralized attitude estimation using a quaternion covariance intersection approach[J]. Journal of the Astronautical Sciences, 2009, 57(1-2): 113-128.

[4] Niehsen W, Gmbh R B. Information fusion based on fast covariance intersection filtering[C]//Proceedings of the 5th International Conference on Information Fusion. 2002: 901-905.

[5] Nebelecky C K, Crassidis J L, Banas W D, Cheng Y, Fosbury A M. Decentralized relative attitude estimation for three-spacecraft formation flying applications[C]// AIAA Guidance, Navigation and Control Conference. Chicago, IL, August 2009.

[6] Zhou Jiang-hua, Miao Yu-hong. A Rodrigues-vectorbased strapdown attitude algorithm[C]//IEEE Aerospace Conference. 2012: 439-517.

[7] Nebelecky, Christopher K. Attitude data fusion using a modified Rodrigues parametrization[C]//AIAA Guidance, Navigation, and Control Conference. 2010: 917-933.

[8] Keefe O, Stephen A, Hanspeter S. Shadow set considerations for modified Rodrigues parameter attitude filtering [C]//Conference Article. August 2013: 2777-2786.

[9] Wahba G. Problem 65-1: A least squares estimate of satellite attitude[J]. SIAM Review, July 1965, 7(3): 409.

[10] Karlgaard C D, Schaub H. Nonsingular attitude filtering using modified rodrigues parameters[J]. Journal of the Astronautical Sciences, 2009: 777-791.

[11] Han, Ping1, Sang WeiLin. Simplex unscented Kalman filter for aircraft attitude algorithm[J]. Journal Article, 2014: 629-633.

[12] Zhao X.M, Yang L, Hui F. Three-dimensional vehicle attitude estimation using modified invasive weed optimized particle filter[J]. International Journal of Automotive Technology, 2014: 1143-1154.

[13] 王思思, 齐国清. 有色量测噪声下的改进求容积卡尔曼滤波器[J]. 控制理论与应用, 2015, 32(1): 1-10. Wang Si-Si, Qi Guo-qing. Improved cubature Kalman filter with colored measurement noise[J]. Control Theory and Applications, 2015, 32(1): 1-10.

[14] Roh K, Park S, Park E, Choi K. A batch filter based on the unscented transformation and its applications to attitude determination[J]. Advances in Astronautical Sciences, 2007, 127(1): 19-40.

[15] Cheng Y, Crassidis J L. Particle filtering for attitude estimation using a minimum local-error representation [J]. Journal of Guidance, Control, and Dynamics, 2010, 33(4): 1305-1310.

[16] 王硕, 宋申民, 史小平, 于浛. 噪声特性未知的多传感器协方差交叉融合姿态估计[J]. 控制与决策, 2014. Wang Shuo, Song Shen-min, Shi Xiao-ping, Yu Han. The multi-sensor covariance intersection fusion attitude estimation with unknown noise characteristics[J]. Control and Decision, 2014.

[17] Julier S J, Uhlman J K. Non-divergent estimation algorithm in the presence of unknown correlations[C]// Proceedings of the IEEE American Control Conference, Albuquerque, NM, USA, 1997: 2369-2373

[18] Crassidis J L, Landis Markley F, Cheng Y. Survey of nonlinear attitude estimation methods[J]. Journal of Guidance, Control and Dynamics, 2007, 30(1): 12-28.

[19] Crassidis J, Junkins J. Optimal estimation of dynamic systems[M]. Boca Raton: Chapman & Hall, 2004: 419- 433.

Attitude fusion approach for multisensor with unknown noise characteristics

WANG Shuo, SONG Shen-min, YU Han
(Center for Control Theory and Guidance Technology, Harbin Institute of Technology, Harbin 150001, China)

A covariance intersection fusion estimation algorithm is presented for multi-sensor system with unknown covariance and noise characteristics, which means that the relativity and variance of the state noise and measurement noise are not available. Firstly, a corresponding CKF estimator is chosen for each subsystem to produce a local estimation according to the measurement data newly acquired. Secondly, based on the least matrix-weighted linear variance rule, a fast successive covariance intersection (SCI) fusion algorithm is proposed to achieve the optimal fusion estimation, which simplifies the multidimensional optimization problem into the optimization of several one-dimensional nonlinear cost functions. Thirdly, in the subsystems, a method of switching between error quaternion and error modified rodrigues parameter is used to avoid the norm constraint of quaternion and the singular problem of modified rodrigues parameter. Finally, numerical simulations are presented to demonstrate the effectiveness of the proposed algorithm.

cubature Kalman filter estimate; information fusion; covariance intersection; quaternion; rodrigues parameter

V448.2

：A

2015-06-15；

：2015-09-28

国家自然科学基金（61174037）；国家自然科学基金创新群体项目（61021002）

王硕（1984—），男，博士研究生。E-mail：wangshuo_hit@163.com

联 系 人：宋申民（1968—），男，教授，博士生导师，研究方向为非线性系统的稳定性分析、鲁棒控制、导弹制导与飞行器控制。E-mail：songshenmin@hit.edu.cn

1005-6734(2015)05-0653-09

10.13695/j.cnki.12-1222/o3.2015.05.017