基于廣義羅德里格參數的恒星相機和陀螺聯合測姿方法

尹 瀟，柴洪洲，杜禎強，向民志，石明琛

（信息工程大學地理空間信息學院，鄭州 450001）

航天器控制的重要前提是確定航天器的軌道和姿態，而其中姿態信息對于航天任務的完成至關重要。因此，相關學者對姿態測定的傳感器及算法進行了深入研究[1-2]。特別地，隨著我國測繪衛星將配備恒星相機，利用恒星相機和陀螺聯合測姿成為一種相對應用廣泛的方法[3]。該方法兼顧恒星相機的高精度姿態四元數信息和陀螺的高頻角速度信息，可實現高精度高頻率的姿態獲取。

在進行恒星相機和陀螺數據融合時，多采用四元數進行狀態傳遞，然后采用三個姿態角誤差進行測量更新[4]。當姿態擾動誤差較小時，利用姿態角誤差進行測量更新，并對更新獲得的四元數進行強制歸一的方法適用性較好[5]。但當狀態初值不準確時，四元數強制歸一會引起估計的誤差。

因此，文獻[6]提出一種基于廣義羅德里格參數的誤差四元數更新方法，其通過四元數的乘法特性，保持了更新后的四元數的歸一化性質。另外，與經典Gibbs 向量和改進的羅德里格參數(Modified Rodrigues Parameters,MRPs)相比，廣義羅德里格參數不存在180 °和360 °的奇異現象[7]?？紤]文獻[6]采用三軸磁強計且沒有給出具體的算例，有必要進一步分析，不同初始姿態精度時，廣義羅德里格參數對于恒星相機測姿的影響。

另外，在進行姿態估計時，常采用擴展卡爾曼濾波(Extended Kalman Filter,EKF)，其算法簡單且易于實現，但該算法忽略了高階信息，較適合于弱非線性的系統。對于強非線性和非高斯環境，常采用采樣趨近的方法逼近后驗概率密度分布，經典的算法有文獻[8]提出的無跡卡爾曼濾波(Unscented Kalman Filter,UKF) 和文獻[9]提出的粒子濾波(Particle Filter,PF)。但PF 計算量較大，且容易出現粒子退化，較難滿足導航實時性的需求[10-11]。為此，在狀態初值不準確時，有必要研究采用UKF 進行姿態估計[12]。特別地，對于恒星相機的非線性測量方程，利用UKF 進行測量更新優勢較為明顯。另外，相較文獻[12]，有必要分析不同縮放參數對于恒星相機和陀螺聯合測姿的影響，確定基于廣義羅德里格參數采樣的策略。

為此，本文首先給出廣義羅德里格參數的構造方法以及其與四元數的關系，并詳細介紹其在四元數歸一化中的應用。然后，介紹基于陀螺角速度的運動學方程以及基于恒星相機的測量方程，并給出UKF 的詳細流程。最后將本文算法應用于旋轉航天器的姿態估計，并與EKF 結果比較，驗證算法的有效性。

1 廣義羅德里格參數

在描述航天器姿態時，常采用的方式包括歐拉角、方向余弦和四元數。其中，歐拉角在接近90 °時，會出現退化現象，僅適合于姿態變化不大的情況[13]。方向余弦避免了退化現象，但方向余弦包含九個未知量，計算量大，難以滿足實時性的要求。因此，多選用四元數表達姿態信息[14]：

由于利用四個數表達三維姿態，四元數滿足以下的約束條件：

式(2)也稱為歸一化條件。利用四元數表達姿態矩陣，表達為：

其中，

I 為單位陣，［ρ×］為向量ρ的反對稱矩陣，即：

由四元數表示的姿態運動學方程為：

式中，ω為經過陀螺零漂改正的測量角速度向量。在高采樣的情況下，可近似認為ω在相鄰時刻的方向固定不變，則姿態運動學方程可離散為：

其中，

式中，為修正后的四元數，δα為測量更新估計得到的三個姿態角擾動誤差。式(6)修正后的四元數在大的初始狀態偏差時仍然需要進行強制歸一化，即：

為此，本文采用廣義羅德里格參數建立誤差運動學方程，其表達式為[6]：

式中，δ p為羅德里格參數，其為三維向量；f為比例因子；a為常數，通常取0 ～ 1。當a= 0,f= 1時，式(8)為Gibbs 向量，其存在180 °的奇異；當a= 1,f=1時，式(8)為MRPs，其存在360 °的奇異。因此，本文取a= 1,f= 2 （a+ 1）計算式(8)，其不存在奇異，稱為廣義羅德里格參數，其與誤差四元數的關系為：

當由測量更新計算出δ p的估值后，即可以采用(9)式計算出誤差四元數，再利用四元數乘法修正式(5)，即：

式中，?表示四元數乘法。由于連續旋轉不會改變向量的模，因此四元數乘法保持了四元數歸一化特性。因此，采用廣義羅德里格參數建立運動學方程，然后轉換為誤差四元數進行乘法修正可有效避免強制歸一化引起的誤差。

2 基于恒星相機和陀螺的UKF 姿態確定

2.1 姿態運動學方程

式(5)給出離散的姿態運動學方程，其由陀螺測量的角速度進行更新。陀螺的測量模型一般表示為[15]：

式中，為陀螺測量的角速度；β為陀螺零漂，為待求參數；nv和nu分別為測量白噪聲和隨機游走噪聲，二者滿足均值為零的高斯分布，且二者獨立，方差分別為σv2和σu2。

2.2 恒星相機測量方程

利用固定在航天器上的相機拍攝恒星照片，獲取恒星像點坐標bi與其慣性系坐標的共線方程，即可求解航天器的姿態。因此，可以建立如下的測量方程：

其中，

式中，i為觀測恒星的編號；（xi,yi）為像點坐標；f0為相機焦距；ri為恒星在慣性系的坐標，為已知值；M為待求解的姿態矩陣；Vi為觀測噪聲，近似服從高斯分布，即：

式中，σi2為測量噪聲的方差。

2.3 UKF 姿態估計

根據2.1 可知，姿態估計中的參數包括四元數和陀螺零漂，共計七個參數，即：

但在測量更新時，采用廣義羅德里格參數，即：

其中，δ p為零向量。下面給出UKF 采樣變換的方法，其主要通過構造一組Sigma 點集(2n+ 1)，以及相應的權系數Wim和Wic來近似逼近狀態分布的均值和方差[8]。權系數常用式(16)表達：

式中，n為狀態參數的個數；λ為縮放參數，一般取λ= 3-n。具體算法實現的流程總結于表1。

表1 UKF 姿態估計算法Tab.1 UKF for attitude estimation

其中，過程噪聲按照文獻[6]的方法確定，計算公式如下：

3 仿真試驗及分析

將提出的基于廣義羅德里格參數改進的UKF 姿態估計算法應用于恒星相機和陀螺的聯合測姿試驗，并與EKF 算法的結果對比。試驗中航天器的軌道周期約90 min，恒星相機的采樣頻率和陀螺的采樣頻率相同，本次試驗取1 Hz。陀螺的常值零漂噪聲σv、隨機游走σu，初始姿態q0、零漂β0及其方差P0，以及恒星相機的測量噪聲σs如表2所示。

表2 參數設置Tab.2 Configuration for technical parameters

首先按照表2給出的姿態和零漂初值進行EKF 及UKF 姿態估計，此時初始狀態精度較好，結果如圖1所示。

圖1(a) EKF 估計的姿態角誤差Fig.1 (a) Attitude errors of EKF

EKF 估計姿態時，采用式(6)強制歸一化的方法；UKF 估計姿態時，采用廣義羅德里格參數構造誤差四元數的方法。由圖1可看出，橫滾角、俯仰角的精度明顯優于偏航角，這與航天器的飛行狀態是相符的。另外，三倍中誤差邊界很好限定了姿態誤差，也表明姿態估計精度較優。對比EKF 和UKF 的估計結果，可看出二者沒有明顯的差異，表明在初始狀態準確時，UKF 并沒有優勢，采用強制歸一化引起的誤差也可以忽略。

下面給初始姿態角分別加50 °、50 °和160 °的誤差，僅采用EKF 估計，但分別采用式(6)和式(10)進行四元數更新，三維姿態誤差的比較見圖2。由圖2可以看出，采用廣義羅德里格參數構造誤差四元數的方法精度優于強制歸一化的方法。統計兩種方法的平均RMS 分別為6.79 °和6.17 °，進一步表明在初始姿態偏差較大時，采用廣義羅德里格參數構造誤差四元數的方法可有效改善強制歸一化引起的誤差。

采用同樣的初始姿態角偏差，分別采用EKF 和UKF 進行姿態估計，并皆采用式(10)進行四元數更新，重新進行一次試驗，見圖3。由圖3可以看出，在初始姿態存在較大偏差時，UKF 精度明顯優于EKF。

圖3 EKF 和UKF 的姿態角誤差比較(初始姿態偏差)Fig.3 Attitude errors comparison between EKF and UKF(Initial attitude errors)

在同樣的初始姿態角偏差基礎上，另外在偏航方向加10 °的初始零漂誤差，分別采用EKF 和UKF 進行姿態估計，并皆采用式(10)進行四元數更新，再次進行一次試驗，見圖4。由圖4可以看出，在初始姿態和零偏同時存在較大偏差時，EKF發生明顯的震蕩，無法收斂。相比EKF，UKF 仍然可以收斂至0.1 °，再次表明UKF 性能優于EKF。

圖4 EKF和UKF的姿態角誤差比較(初始姿態和零漂誤差)Fig.4 Attitude errors comparison between EKF and UKF(Initial attitude and bias errors)

上述試驗時，取縮放參數λ=- 3。但由于縮放參數影響UKF 的采樣分布[6]，本文進一步研究λ取值不同對恒星相機和陀螺聯合測姿的影響。采用如上的初始姿態和零漂誤差，試驗結果如5 所示。由圖5可以看出，λ=-3 時姿態角誤差最大，表明進行非線性變換后，狀態參數分布不再是高斯分布。更進一步采用同樣的50 次蒙特卡洛試驗，統計每次結果的RMS，如圖6所示。由圖6可以看出，λ= -3 時精度最差；λ=-2 和λ=-1 時，波動也較大；λ= 0時，大部分精度最優，但出現一次明顯的誤差；λ= 1時，穩定性和精度總體最優，這與參考文獻[6]的結論一致，再次表明在進行非線性的無跡變換后，狀態參數分布不再是高斯分布。

圖5 采用不同縮放參數的姿態角誤差Fig.5 Attitude errors using different scaling parameters

圖6 采用不同縮放參數的姿態角RMS 統計Fig.6 RMS of attitude errors using different scaling parameters

4 結 論

本文首先推導了基于廣義羅德里格參數構造誤差四元數進行乘法更新的方法，并通過試驗分析該方法對強制歸一引起誤差的改善。在此基礎上，本文給出基于UKF 的恒星相機和陀螺測姿算法流程，并與EKF比較。仿真試驗表明，在狀態初值不準確時，UKF 在收斂速度和精度上都優于EKF。并且，經過非線性無跡變換后，狀態參數的分布不再是高斯分布，縮放參數在λ= 1時估計最優。另外，UKF 計算非線性觀測方程時所需運算量大于EKF，下一步需要優化算法處理速度。