考慮噪聲特性未知的多傳感器姿態融合方法

王 碩，宋申民，于 浛

（哈爾濱工業大學 控制理論與制導技術研究中心，哈爾濱 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