尹 瀟，柴洪洲，杜禎強(qiáng)，向民志，石明琛

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

航天器控制的重要前提是確定航天器的軌道和姿態(tài)，而其中姿態(tài)信息對(duì)于航天任務(wù)的完成至關(guān)重要。因此，相關(guān)學(xué)者對(duì)姿態(tài)測(cè)定的傳感器及算法進(jìn)行了深入研究[1-2]。特別地，隨著我國(guó)測(cè)繪衛(wèi)星將配備恒星相機(jī)，利用恒星相機(jī)和陀螺聯(lián)合測(cè)姿成為一種相對(duì)應(yīng)用廣泛的方法[3]。該方法兼顧恒星相機(jī)的高精度姿態(tài)四元數(shù)信息和陀螺的高頻角速度信息，可實(shí)現(xiàn)高精度高頻率的姿態(tài)獲取。

在進(jìn)行恒星相機(jī)和陀螺數(shù)據(jù)融合時(shí)，多采用四元數(shù)進(jìn)行狀態(tài)傳遞，然后采用三個(gè)姿態(tài)角誤差進(jìn)行測(cè)量更新[4]。當(dāng)姿態(tài)擾動(dòng)誤差較小時(shí)，利用姿態(tài)角誤差進(jìn)行測(cè)量更新，并對(duì)更新獲得的四元數(shù)進(jìn)行強(qiáng)制歸一的方法適用性較好[5]。但當(dāng)狀態(tài)初值不準(zhǔn)確時(shí)，四元數(shù)強(qiáng)制歸一會(huì)引起估計(jì)的誤差。

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

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

為此，本文首先給出廣義羅德里格參數(shù)的構(gòu)造方法以及其與四元數(shù)的關(guān)系，并詳細(xì)介紹其在四元數(shù)歸一化中的應(yīng)用。然后，介紹基于陀螺角速度的運(yùn)動(dòng)學(xué)方程以及基于恒星相機(jī)的測(cè)量方程，并給出UKF 的詳細(xì)流程。最后將本文算法應(yīng)用于旋轉(zhuǎn)航天器的姿態(tài)估計(jì)，并與EKF 結(jié)果比較，驗(yàn)證算法的有效性。

1 廣義羅德里格參數(shù)

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

由于利用四個(gè)數(shù)表達(dá)三維姿態(tài)，四元數(shù)滿足以下的約束條件：

式(2)也稱為歸一化條件。利用四元數(shù)表達(dá)姿態(tài)矩陣，表達(dá)為：

其中，

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

由四元數(shù)表示的姿態(tài)運(yùn)動(dòng)學(xué)方程為：

式中，ω為經(jīng)過(guò)陀螺零漂改正的測(cè)量角速度向量。在高采樣的情況下，可近似認(rèn)為ω在相鄰時(shí)刻的方向固定不變，則姿態(tài)運(yùn)動(dòng)學(xué)方程可離散為：

其中，

式中，為修正后的四元數(shù)，δα為測(cè)量更新估計(jì)得到的三個(gè)姿態(tài)角擾動(dòng)誤差。式(6)修正后的四元數(shù)在大的初始狀態(tài)偏差時(shí)仍然需要進(jìn)行強(qiáng)制歸一化，即：

為此，本文采用廣義羅德里格參數(shù)建立誤差運(yùn)動(dòng)學(xué)方程，其表達(dá)式為[6]：

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

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

式中，?表示四元數(shù)乘法。由于連續(xù)旋轉(zhuǎn)不會(huì)改變向量的模，因此四元數(shù)乘法保持了四元數(shù)歸一化特性。因此，采用廣義羅德里格參數(shù)建立運(yùn)動(dòng)學(xué)方程，然后轉(zhuǎn)換為誤差四元數(shù)進(jìn)行乘法修正可有效避免強(qiáng)制歸一化引起的誤差。

2 基于恒星相機(jī)和陀螺的UKF 姿態(tài)確定

2.1 姿態(tài)運(yùn)動(dòng)學(xué)方程

式(5)給出離散的姿態(tài)運(yùn)動(dòng)學(xué)方程，其由陀螺測(cè)量的角速度進(jìn)行更新。陀螺的測(cè)量模型一般表示為[15]：

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

2.2 恒星相機(jī)測(cè)量方程

利用固定在航天器上的相機(jī)拍攝恒星照片，獲取恒星像點(diǎn)坐標(biāo)bi與其慣性系坐標(biāo)的共線方程，即可求解航天器的姿態(tài)。因此，可以建立如下的測(cè)量方程：

其中，

式中，i為觀測(cè)恒星的編號(hào)；（xi,yi）為像點(diǎn)坐標(biāo)；f0為相機(jī)焦距；ri為恒星在慣性系的坐標(biāo)，為已知值；M為待求解的姿態(tài)矩陣；Vi為觀測(cè)噪聲，近似服從高斯分布，即：

式中，σi2為測(cè)量噪聲的方差。

2.3 UKF 姿態(tài)估計(jì)

根據(jù)2.1 可知，姿態(tài)估計(jì)中的參數(shù)包括四元數(shù)和陀螺零漂，共計(jì)七個(gè)參數(shù)，即：

但在測(cè)量更新時(shí)，采用廣義羅德里格參數(shù)，即：

其中，δ p為零向量。下面給出UKF 采樣變換的方法，其主要通過(guò)構(gòu)造一組Sigma 點(diǎn)集(2n+ 1)，以及相應(yīng)的權(quán)系數(shù)Wim和Wic來(lái)近似逼近狀態(tài)分布的均值和方差[8]。權(quán)系數(shù)常用式(16)表達(dá)：

式中，n為狀態(tài)參數(shù)的個(gè)數(shù)；λ為縮放參數(shù)，一般取λ= 3-n。具體算法實(shí)現(xiàn)的流程總結(jié)于表1。

表1 UKF 姿態(tài)估計(jì)算法Tab.1 UKF for attitude estimation

其中，過(guò)程噪聲按照文獻(xiàn)[6]的方法確定，計(jì)算公式如下：

3 仿真試驗(yàn)及分析

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

表2 參數(shù)設(shè)置Tab.2 Configuration for technical parameters

首先按照表2給出的姿態(tài)和零漂初值進(jìn)行EKF 及UKF 姿態(tài)估計(jì)，此時(shí)初始狀態(tài)精度較好，結(jié)果如圖1所示。

圖1(a) EKF 估計(jì)的姿態(tài)角誤差Fig.1 (a) Attitude errors of EKF

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

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

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

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

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

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

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

圖5 采用不同縮放參數(shù)的姿態(tài)角誤差Fig.5 Attitude errors using different scaling parameters

圖6 采用不同縮放參數(shù)的姿態(tài)角RMS 統(tǒng)計(jì)Fig.6 RMS of attitude errors using different scaling parameters

4 結(jié) 論

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