基于正弦函數擬合的高動態捷聯慣導姿態更新算法

路永樂，楊 杰，孫 旗，羅 毅，肖 軒，劉 宇

（重慶郵電大學 智能傳感技術與微系統重慶市高校工程研究中心，重慶 400065）

捷聯慣性導航系統（Strapdown Inertial Navigation System,SINS）以數學平臺為導航平臺，使用姿態矩陣描述數學平臺相對載體坐標系的關系[1]，具有高可靠性、使用靈活的優點[2]，其核心為捷聯慣導姿態更新算法[3]。然而，由于剛體有限旋轉的不可交換性，姿態更新中不可避免地引入不可交換性誤差[4]。高動態環境下，這種誤差更加明顯[5]。

1971 年Bortz[6]提出等效旋轉矢量算法以補償不可交換性誤差，隨后各學者基于此開展了廣泛的研究。根據研究方法的不同可以分為兩類[7]：一是以多項式角運動為假設前提，基于泰勒級數展開或最小二乘估計進行算法系數求取的方法：Miller[8]在純圓錐運動條件下推導了三子樣圓錐補償算法；Savage 等[9]基于多項式角運動假設和最小二乘估計提出了顯式頻率整形圓錐補償算法；王茂松等[10]同樣以多項式角運動假設為基礎提出了高階等效旋轉矢量姿態更新算法。二是數值迭代的方法：嚴恭敏等[11]基于多項式迭代提出了等效旋轉矢量精確解方法；武元新[12]基于函數迭代提出了三階旋轉矢量微分方程數值解方法。

然而，利用有限次的多項式表示角運動是一種近似擬合[12]，而數值迭代類算法具有更高精度的同時意味著所需迭代時間更長[13]，同時一些傳統算法推導過程中忽略了Bortz 方程三階及以上項的影響，導致實際使用精度達不到理論效果，特別是在高動態環境下，有時高子樣算法精度反而不如低子樣算法[11]。本文采用第一類研究方法的思想進行誤差補償算法設計。同時，為了更好地擬合載體運動角速度，進一步提升高動態環境下姿態解算精度，本文以正弦函數代替多項式對載體運動角運動進行擬合，基于此提出了一種基于正弦函數擬合的高動態捷聯慣導姿態更新算法，并在圓錐運動與大角速率轉動并存的條件下，對本文算法進行仿真驗證。

1 基于等效旋轉矢量的姿態更新算法設計方法

等效旋轉矢量算法是在高動態環境下實現高精度姿態更新的常用方法[3]，其利用姿態更新周期內的角增量構建等效旋轉矢量，通過優化算法系數，實現不可交換性誤差補償。

等效旋轉矢量算法對不可交換性誤差進行補償的理論基礎為等效旋轉矢量微分方程，即Bortz 方程[6]，工程上常用近似形式為：

其中，Φ為等效旋轉矢量；ω為載體運動角速度；稱為不可交換性誤差。

相較于四元數姿態更新法，等效旋轉矢量法進行姿態更新時需要先求取等效旋轉矢量，再計算姿態變化四元數，隨后才能進行四元數更新。

1.1 構建等效旋轉矢量

在實際應用中，姿態更新周期常為毫秒級。以數據傳輸頻率200 Hz 計算，四元數法姿態更新周期為5 ms，對應二子樣等效旋轉矢量法姿態更新周期為10 ms。由于姿態更新周期短，將等效旋轉矢量用角增量代替，即：Φ≈Δθ。于是，式(1)進一步改寫為：

以式(2)為理論基礎，在姿態更新周期內以不同階次的多項式對載體角速度進行擬合，得到不同的等效旋轉矢量。例如，令角速度為非零常數，即零次多項式ω(t)=a(0≤t≤h)，其中a為常數向量，h=tk+1-tk表示一個姿態更新周期，tk、tk+1分別表示k、k+1時刻。此時，得到單子樣算法的等效旋轉矢量[8]為：

其中，Δθ表示 [tk,tk+1]內的角增量。

1.2 構造姿態變化四元數

從角位置A0到角位置A1的旋轉四元數記為：

其中，Q表示旋轉四元數；u表示單位向量；θ表示旋轉角度。

又有四元數更新方程[8]為：

其中，Q(tk+1)、Q(tk)分別為tk+1、tk時刻導航坐標系至載體坐標系的旋轉四元數；q(h)為tk至tk+1時刻的載體坐標系旋轉四元數。

一個姿態更新周期內，旋轉角度為旋轉矢量模值Φ，因此，q(h)由旋轉矢量表示為式(6)，并稱為[tk,tk+1]時間段內的姿態變化四元數[7]。

2 基于正弦函數擬合的等效旋轉矢量算法

逼近理論的基本問題是用一系列簡單函數來逼近（擬合）一個復雜或是沒有解析式的函數[14]，這些簡單函數通常為一組正交基[15]。例如，Legendre 正交基：1,x,(3x2-1)/2,(5x3-3x)/2…；Chebyshev 正交基：1,cosx,cos2x,cos3x… ；Fourier 正交基 ：1,sinx,cosx,sin2x,cos2x…。正交基的特點為任意兩個不同的基，其內積為 0。本文使用正交基(sinx,sin 2x,sin3x…) 對載體角速度進行擬合。

將載體運動角速度用不同頻率的正弦函數之和進行擬合，具體表達式為：

其中，k1、k2…kn為系數矢量。

根據姿態更新周期內的采樣點數選擇合適的角速度函數。以一個姿態更新周期內進行兩次采樣為例，載體運動角速度表示為：

其中，a、b為系數矢量。記姿態更新周期內角增量為角速度積分：

則角速度與角增量在t=0處各階導數分別為：

對式(2)進行畢卡迭代求解，得到Bortz 方程三階畢卡解[16]為：

求取式(12)中等效旋轉矢量各階畢卡解在t=0處各階導數，并將式(10)和式(11)代入得：

對Φ(h)在t=0處泰勒展開，考慮部分泰勒展開的六階分量遠大于八階分量，且七階分量為零，因此對等效旋轉矢量進行六階泰勒展開，并將式(13)代入，得：

為了將等效旋轉矢量以角增量形式表示，求取一個姿態更新周期內兩個角增量表達式。根據式(8)和式(9)所示的角速率與角增量表達式，得：

根據余弦函數的麥克勞林展開式，將式(15)中角增量之和展開為：

對比式(14)和式(16)，可得基于正弦函數擬合的二采樣等效旋轉矢量算法的等效旋轉矢量：

其中，a、b由式(15)中角增量表達式聯立求得。

同理，可得基于正弦函數擬合的三采樣等效旋轉矢量算法的等效旋轉矢量為：

其中，d、e、f為系數矢量，表達式見式(20)。

3 圓錐運功與大角速率轉動并存環境仿真分析

3.1 等效旋轉矢量真實值

圓錐運動與角速率機動并存環境是指載體處于圓錐運動的同時，又有一個坐標軸處于常值角速率旋轉的環境，其運動角速率ω(t)見式(21)[17]。當常值角速率轉動值大于100 °/s 時，稱其為圓錐運動與大角速率轉動并存環境，該環境是一種典型的高動態環境[17]。

其中，α為圓錐運動半錐角；Ω 為圓錐運動角頻率；ω0為常值轉動角速率。

以圓錐運動與角速率機動并存環境作為仿真條件的優勢在于，其等效旋轉矢量的真實值是已知的。在此條件下，載體運動的真實姿態四元數Q(t)為：

因此，姿態變化四元數q(h)及其與等效旋轉矢量[ΦxΦyΦz]T的對應關系表示為：

分析式(23)可知，Φx、Φy為周期項，其誤差不隨時間累積，對算法精度的影響可忽略不計[18]，因此，后文僅分析非周期項Φz帶來的算法誤差。

當式(23)中的常值轉動角速率ω0較小時，可以認為q0值很小，對其中正弦函數采取一階近似[8]，而當ω0較大時，需要對正弦函數采取二階近似[17]，將二階近似值代入式(23)可得：

聯立式(23)和式(24)求得Φz的理論值為：

3.2 等效旋轉矢量估計值

圓錐誤差補償算法子樣數通常在1～4 之間[9,10]，其中單子樣算法本質上為四元數算法，其對不可交換性誤差補償程度不夠，而四子樣算法往往復雜且效果不一定優于三子樣算法[13]。因此，本文僅選擇子樣數為二和子樣數為三的相關算法進行對比分析，各種算法的名稱及縮寫如表1 所示。

表1 對比分析算法名稱及縮寫Tab.1 Comparative analysis algorithm names and abbreviations

表1 中各算法的等效旋轉矢量估計值如式(26)～式(30)所示，其中式(27)中參數a、b由式(18)求取，式(30)中參數d、e、f由式(20)求取，所有算法的角增量均由式(9)求出。

1）等效旋轉矢量二子樣圓錐優化算法（ERV2）

2）正弦函數擬合二子樣旋轉矢量算法（TRV2）

3）擴展形式頻率級數三子樣圓錐算法（FSR3）

4）擴展形式顯示頻率三子樣圓錐算法（EXP3）

5）正弦函數擬合三子樣旋轉矢量算法（TRV3）

3.3 不同參數算法誤差對比分析

由式(25)可知，等效旋轉矢量算法的誤差與半錐角a、角頻率Ω、常值角速率ω0和姿態解算周期h均有關系，因此本文就不同參數條件下算法誤差分別進行仿真對比分析。算法誤差評估方法采用Musoff[19]提出的剩余誤差評估方法，該值越小表明算法性能越好，其定義為進行補償后剩余的誤差，即：

1）仿真參數設置為：α=0.001 °～90°，Ω=2π rad/s，ω0=5.30 rad/s，h=0.02 s。

該環境下各算法隨半錐角變化的誤差仿真結果如圖1 所示。從圖1 可以看出：①半錐角小于1.38 °時，TRV2 與ERV2 精度相當，誤差均在0.1 °/h 以內；TRV3性能較好，其中在半錐角 0.77 °時誤差最小，為1.08×10-5°/h，比FSR3、EXP3 低3 個數量級。②半錐角大于1.38°時，TRV2、TRV3 表現不足；ERV2、FSR3、EXP3 三者誤差在不同半錐角下各自達到最小值，如圖1 局部放大圖所示，EXP3 在半錐角40.07 °時誤差最小，為6.89×10-7°/h；ERV2 在半錐角57.31 °時誤差最小，為5.94×10-8°/h；FSR3 在半錐角57.71 °時誤差最小，為7.13×10-7°/h。綜上分析可知，本文所提算法在小半錐角情況下表現更好。

圖1 各算法隨半錐角變化的誤差對比Fig.1 Comparison of the relative cone error of each algorithm with the half-cone angle

2 ）仿真參數設置為：α=0.5°，Ω=π rad/s～500π rad/s，ω0=5.30 rad/s，h=0.02 s。

該環境下各算法隨角頻率變化的誤差仿真結果如圖2 所示。從圖2 可以看出：①角頻率低于2.83π rad/s時，TRV2 與ERV2 精度相當，誤差均在0.1 °/h 以內；TRV3 性能較好，其中在角頻率2.26π rad/s 時誤差最小，為4.36×10-4°/h，比FSR3、EXP3 低2 個數量級。②角頻率高于2.83π rad/s 時，隨著角頻率變化，各算法分別在不同角頻率條件下達到性能最佳點。例如，角頻率 13.04π rad/s 時 ERV2 誤差最小，為1.18×10-4°/h；角頻率64.71π rad/s 時TRV2 誤差最小，為3.26×10-3°/h；角頻率67.88π rad/s 時FSR3 誤差最小，為3.90×10-3°/h；角頻率70.93π rad/s 時EXP3 誤差最小，為7.44×10-3°/h。同時，可以發現，當角頻率達到301.69π rad/s 左右時，所有算法的誤差相同（圖2 局部放大圖中五角星處）。③從整體來看，所有算法的誤差均隨圓錐運動角頻率的升高而增加，且在一定間隔后，同子樣數的算法誤差將變得相同，而每個間隔內各算法性能呈現交替變換的趨勢。對于子樣數為二的算法，該間隔為200π rad/s，對于子樣數為三的算法，該間隔為300π rad/s，如圖3 所示。綜上分析可知，本文所提算法在低頻情況下表現更好。

圖3 不同子樣數算法隨角頻率變化的誤差對比Fig.3 Comparison of the relative cone error with the angular frequency for different subsample number algorithms

3）仿真參數設置為：α=0.5°，Ω=2π rad/s，ω0=0rad/s～ 25 rad/s，h=0.02 s。

該環境下各算法隨常值角速率變化的誤差仿真結果如圖4 所示。從圖4 可以看出：①常值角速率低于3.67 rad/s 時，ERV2、FSR3 和EXP3 的誤差小于TRV2、TRV3，且 ERV2 和 EXP3 分別在常值角速率為0.81 rad/s 和 1.05 rad/s 時誤差最小，分別為1.79×10-9°/h 和2.98×10-8°/h。②當常值角速率高于3.67 rad/s、低于6.28 rad/s 時，TRV3 性能較好，其中在常值角速率4.83 rad/s 時誤差最小，為8.27×10-6°/h，比其余算法低 4 個數量級。③常值角速率高于6.28 rad/s 時，各算法的誤差幾乎相同。綜上分析可知，本文所提算法在高速常值角速率轉動情況下表現更好。

圖4 各算法隨常值角速率變化的誤差對比Fig.4 Comparison of the relative cone error of each algorithm with the constant angular rate

4）仿真參數設置為：α=0.5°，Ω=2π rad/s，ω0=5.30 rad/s，h=0.0005s～ 0.05 s 。

該環境下各算法隨姿態解算周期變化的誤差仿真結果如圖5 所示。

從圖5 可以看出：①姿態解算周期小于0.02 s 時，各算法性能幾乎相同。②姿態解算周期大于0.02 s 時，TRV3 誤差略低于其余算法，在姿態解算周期為0.05 s時TRV3 誤差為1.75 °/h，其余算法誤差均在1.88 °/h左右。③整體而言，隨著姿態解算周期增大，各算法誤差均表現出增大的趨勢，但TRV3 誤差增速略緩于其余算法。綜上分析可知，本文所提算法在姿態解算周期相同的情況下表現更好。

4 總結

降低高動態環境下姿態更新中不可交換性誤差的有效手段是使用等效旋轉矢量算法。本文依據等效旋轉矢量算法的設計方法，通過改變算法設計的前提條件，采用正弦函數正交基對載體角速度進行擬合，提出了一種基于正弦函數擬合的高動態捷聯慣導姿態更新算法。在圓錐運動與大角速率轉動并存環境下進行了仿真驗證，并與現有的三種典型的圓錐誤差補償算法進行了對比。仿真結果表明，在小半錐角低頻圓錐運動伴隨快速常值角速率轉動時，本文所提方法具有一定優勢，這表明該算法在大角速率轉動伴隨小幅圓錐運動環境下可能發揮重要的作用，為高動態環境下高精度誤差補償算法研究提供了理論參考價值。