基于奇異值分解的改進TCKF姿態估計算法*

錢鐳源, 張建強，許國強, 曹文濤

(1.運城職業技術大學 智能制造與數智礦山學院，山西 運城 044000；2.陽煤豐喜肥業(集團)有限責任公司，山西 運城 044000)

微電機系統(Micro-Electro-Mechanical Systems，MEMS)在慣性器件上的應用推動了捷聯慣性導航系統(Strapdown Inertial Navigation System，SINS)的發展[1]，使SINS被廣泛用于煤礦、水下勘探等場合[2-3]。但MEMS-SINS本身受加速度計、陀螺儀和磁力計等慣性器件自身特性的影響[4]，無法對姿態角信息進行精確解算[5]。針對這一問題，科研人員進行了諸多相關研究，GENG Jijun等人提出了一種基于四元數解算的自適應容積卡爾曼濾波(Adaptive Cubature Kalman Filter，ACKF)算法[6]，有效提高了姿態角的估計精度；Chiella Antonio C B等人提出了一種基于四元數的魯棒自適應無跡卡爾曼濾波(Quaternion Robust Adaptive Unscented Kalman Filter，QRAUKF)算法，解決了MEMS傳感器姿態精度下降的問題[7]。

實際應用中，濾波算法常會出現誤差協方差矩陣非正定導致運行終止的問題，平方根濾波和奇異值分解是解決這一問題最常用的方法[8-10]。但平方根濾波在每次循環運算時，均進行三角分解，造成算法誤差積累；而奇異值分解能夠將復雜矩陣以特征值與特征向量乘積的形式表示，避免開方運算，具有更高的魯棒性[11]。2018年，秦康等人為了解決CKF的非局部采樣問題，對采樣點進行正交變換，提出了一種濾波性能優于CKF的正交變換容積卡爾曼濾波(Transformed Cubature Kalman Filter，TCKF)方法[12]。

為提高TCKF算法的濾波精度和姿態測量系統的性能，本文將奇異值分解引入到TCKF算法中，將其應用于九軸姿態測量系統，提出了基于奇異值分解的改進TCKF姿態估計算法。該算法在對姿態數據進行融合的同時，對TCKF的誤差協方差矩陣進行SVD分解，代替原有的Cholesky分解，保證融合過程中協方差矩陣始終正定，提高姿態估計的穩定性和精度。

1 數據融合方案及數學模型

1.1 數據融合方案

為了保證系統輸出持續可靠的姿態信息，本文設計了一種基于奇異值分解的改進TCKF姿態數據融合方案，如圖1所示。該方案以捷聯姿態解算得到的姿態四元數作為系統狀態，以加速度計計算的俯仰角和橫滾角、磁力計計算的航向角作為系統量測，采用ASVDTCKF算法對測量數據進行融合，估計姿態四元數，進而得到準確的姿態角信息。其中，姿態四元數的初值由加速度計和磁力計的測量數據計算得到。

圖1 姿態數據融合方案

1.2 姿態測量系統數學模型

由圖1可知，姿態測量系統的數學模型可簡化為：

(1)

式中，xk為k時刻的狀態向量，即姿態四元數；f(·)為系統k-1時刻到k時刻的狀態轉移矩陣；wk-1為k-1時刻的過程噪聲，其協方差矩陣為Qk-1；zk為系統k時刻的量測向量；h(·)為k時刻的量測矩陣；vk為系統k時刻的量測噪聲，其協方差矩陣為Rk。

采用捷聯姿態解算得到的姿態四元數作為系統狀態量，即：

xk=[q0q1q2q3]

(2)

式中，q0、q1、q2和q3為姿態四元數，其運動學微分方程為：

(3)

根據四階角增量法，系統的狀態轉移矩陣為：

(4)

系統量測為：

z=[θaccφaccΨ]

(5)

式中，θacc和φacc分別由以下公式計算得到：

(6)

其中，ax、ay和az分別為載體水平或勻速時三軸加速度計的測量結果。

Ψ由以下公式計算得到：

Ψ=Ψm+λ

(7)

式中，Ψm為磁航向角；λ為載體真實航向角與磁航向角之間存在的磁偏角。

根據量測值θacc、φacc、Ψ與狀態向量

[q0q1q2q3]之間的關系[13]，系統量測矩陣為：

(8)

2 ASVDTCKF算法設計

2.1 TCKF算法流程

(1)根據三階球面-徑向容積準則，計算CKF的采樣點ξj和權值ωj：

(9)

式中，n為系統狀態向量維數；j=1,2,…,2n；[1]2n為2n維單位球面與各坐標軸的交點，

則采樣點集可進一步表示成：

(11)

(2)對CKF采樣點ξj進行正交變換，得到TCKF的采樣點λj，即[12]：

Bξ=[λ1|λ2|…|λ2n]

(12)

(13)

(14)

(15)

式中，r=1,2,…,n/2，當n為奇數時，Λn,j=(-1)j。

(3)基于狀態估計的采樣點計算：

(16)

(17)

(4)狀態預測：

(18)

式中，L=2n。

(5)計算狀態預測誤差的協方差矩陣：

(19)

(6)計算基于狀態預測的采樣點：

(20)

(21)

(7)量測預測：

(22)

(8)計算新息的協方差矩陣：

(23)

(9)計算狀態向量與量測向量之間的互協方差矩陣：

(24)

(10)計算卡爾曼濾波增益：

(25)

(11)狀態估計：

(26)

(12)更新協方差矩陣：

(27)

2.2 基于奇異值分解的濾波方程推導

SVD分解的基本原理如下[14]：

設A是一個m×n維的實數陣，對其進行SVD分解：

(28)

式中，U和V均為酉矩陣；M=diag(σ1,σ2,…，σr)為奇異值矩陣；r為M的秩。

本文將SVD分解引入到TCKF算法中，代替原有的Cholesky分解，提高TCKF算法的穩定性。具體改進如下：

(29)

(30)

(31)

(32)

將奇異值分解濾波方程和TCKF引入圖1姿態測量系統中，即可得到基于奇異值分解的改進TCKF姿態估計算法ASVDTCKF。

3 實驗方案

本文采用ADI公司的ADIS16405微慣性測量單元和TI公司的OMAP-L138構建了姿態測量硬件系統，如圖2所示。第一層為ADIS16405模塊和變壓器模塊，用于降壓和提供三軸加速度計、三軸陀螺儀、三軸磁力計的原始測量數據；第二層為OMAP-L138雙核微處理器及其核心板，用于解析ADIS16405提供的原始數據、時間對準和數據傳輸；第三層為電源模塊，為整個系統供電；第四層為功能拓展層。

圖2 姿態測量硬件系統圖

為了分析ASVDTCKF算法的性能，本文將姿態測量硬件系統固定于轉臺，分別進行靜態和三軸搖擺實驗，并將OMAP-L138采集的慣性數據保存于第三層的U盤中，接入PC機，在MATLAB中進行姿態數據融合算法性能對比實驗，實驗系統安裝如圖3所示。

圖3 實驗系統安裝圖

4 實驗結果分析

4.1 靜態實驗

設置轉臺處于靜止狀態，啟動姿態測量系統，持續30 s，數據采集的頻率為100 Hz。分別采用基于TCKF的姿態估計算法(ATCKF)、基于平方根濾波的TCKF姿態估計算法(ASRTCKF)和ASVDTCKF進行數據融合。

靜態環境下不同算法的運行情況如表1所示，算法輸出的姿態角如圖4所示，角度均方根誤差(RMSE)如表2所示，算法運行時間如表3所示。由表1可知，ASRTCKF和ASVDTCKF運行正常，而ATCKF因協方差矩陣非正定而異常終止，說明對協方差矩陣進行QR分解或SVD分解能夠提升算法的穩定性。由圖4和表2可知，靜態條件下ASVDTCKF輸出的俯仰角誤差、橫滾角誤差和航向角誤差與ASRTCKF相比分別減小了20.6%、2.9%和4.6%，說明ASVDTCKF與ASRTCKF相比具有更高的姿態估計精度。由表3可知，ASVDTCKF的運行時間與ASRTCKF相比減少了18.9%，說明SVD分解的運算量低于QR分解。

表1 靜態條件下不同算法運行情況比較

圖4 靜態環境下不同算法輸出的姿態角

表2 靜態條件下不同算法的RMSE

表3 靜態條件下不同算法運算時間

4.2 動態實驗

設置轉臺處于搖擺狀態，持續30 s，搖擺幅度為20°，搖擺頻率為0.5 Hz，數據采集頻率為100 Hz。分別采用ATCKF、ASRTCKF和ASVDTCKF對搖擺狀態下的ADIS16405測量數據進行融合。

搖擺狀態下不同算法運行情況如表4所示，不同算法輸出的姿態角結果如圖5所示，角度RMSE如表5所示，算法運行時間如表6所示。由表4可知，搖擺動態環境下，ASRTCKF和ASVDTCKF運行正常，ATCKF算法運行至1步終止，說明動態條件下QR分解或SVD分解能夠使算法運行穩定。由圖5和表5可知，動態條件下ASVDTCKF算法的估計精度更優，與ASRTCKF相比，俯仰角誤差、橫滾角誤差和航向角誤差分別減小了0.89%、27.5%和7.4%。由表6可知，動態條件下ASVDTCKF的運算時間與ASRTCKF相比減少了20.5%，說明ASVDTCKF算法的運算效率更高。

表4 搖擺環境下不同算法運行情況比較

表5 搖擺條件下不同算法的RMSE

表6 搖擺條件下不同算法運算時間

圖5 搖擺環境下不同算法輸出的姿態角

5 結 論

為了提高TCKF算法的濾波穩定性及MEMS器件的姿態解算精度，本文提出了一種基于奇異值分解的改進TCKF姿態估計算法。該算法對MEMS器件輸出的姿態數據進行融合，對誤差協方差矩陣進行奇異值分解，提高了姿態角信息的測量效果。將ADIS16405姿態測量硬件系統固定于轉臺，以靜態和三軸搖擺時系統輸出的原始九軸數據作為實驗數據。實驗結果表明：(1)基于奇異值分解的改進TCKF姿態估計算法與ASRTCKF在實際姿態測量中均能保證誤差協方差矩陣始終正定，與TCKF相比，具有更強的運行穩定性；(2)基于奇異值分解的改進TCKF姿態估計算法僅對誤差協方差矩陣進行一次奇異值分解運算，與進行平方根運算的ASRTCKF相比，解決了平方根三角分解造成的誤差積累問題，具有更高濾波精度和運算效率。因此，該算法在礦山、水中、隧道等復雜情況下的載體姿態測量中具有較強的硬件實現性和適用性。