錢鐳源, 張建強(qiáng)，許國(guó)強(qiáng), 曹文濤

(1.運(yùn)城職業(yè)技術(shù)大學(xué) 智能制造與數(shù)智礦山學(xué)院，山西 運(yùn)城 044000；2.陽(yáng)煤豐喜肥業(yè)(集團(tuán))有限責(zé)任公司，山西 運(yùn)城 044000)

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

實(shí)際應(yīng)用中，濾波算法常會(huì)出現(xiàn)誤差協(xié)方差矩陣非正定導(dǎo)致運(yùn)行終止的問(wèn)題，平方根濾波和奇異值分解是解決這一問(wèn)題最常用的方法[8-10]。但平方根濾波在每次循環(huán)運(yùn)算時(shí)，均進(jìn)行三角分解，造成算法誤差積累；而奇異值分解能夠?qū)?fù)雜矩陣以特征值與特征向量乘積的形式表示，避免開(kāi)方運(yùn)算，具有更高的魯棒性[11]。2018年，秦康等人為了解決CKF的非局部采樣問(wèn)題，對(duì)采樣點(diǎn)進(jìn)行正交變換，提出了一種濾波性能優(yōu)于CKF的正交變換容積卡爾曼濾波(Transformed Cubature Kalman Filter，TCKF)方法[12]。

為提高TCKF算法的濾波精度和姿態(tài)測(cè)量系統(tǒng)的性能，本文將奇異值分解引入到TCKF算法中，將其應(yīng)用于九軸姿態(tài)測(cè)量系統(tǒng)，提出了基于奇異值分解的改進(jìn)TCKF姿態(tài)估計(jì)算法。該算法在對(duì)姿態(tài)數(shù)據(jù)進(jìn)行融合的同時(shí)，對(duì)TCKF的誤差協(xié)方差矩陣進(jìn)行SVD分解，代替原有的Cholesky分解，保證融合過(guò)程中協(xié)方差矩陣始終正定，提高姿態(tài)估計(jì)的穩(wěn)定性和精度。

1 數(shù)據(jù)融合方案及數(shù)學(xué)模型

1.1 數(shù)據(jù)融合方案

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

圖1 姿態(tài)數(shù)據(jù)融合方案

1.2 姿態(tài)測(cè)量系統(tǒng)數(shù)學(xué)模型

由圖1可知，姿態(tài)測(cè)量系統(tǒng)的數(shù)學(xué)模型可簡(jiǎn)化為：

(1)

式中，xk為k時(shí)刻的狀態(tài)向量，即姿態(tài)四元數(shù)；f(·)為系統(tǒng)k-1時(shí)刻到k時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣；wk-1為k-1時(shí)刻的過(guò)程噪聲，其協(xié)方差矩陣為Qk-1；zk為系統(tǒng)k時(shí)刻的量測(cè)向量；h(·)為k時(shí)刻的量測(cè)矩陣；vk為系統(tǒng)k時(shí)刻的量測(cè)噪聲，其協(xié)方差矩陣為Rk。

采用捷聯(lián)姿態(tài)解算得到的姿態(tài)四元數(shù)作為系統(tǒng)狀態(tài)量，即：

xk=[q0q1q2q3]

(2)

式中，q0、q1、q2和q3為姿態(tài)四元數(shù)，其運(yùn)動(dòng)學(xué)微分方程為：

(3)

根據(jù)四階角增量法，系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣為：

(4)

系統(tǒng)量測(cè)為：

z=[θaccφaccΨ]

(5)

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

(6)

其中，ax、ay和az分別為載體水平或勻速時(shí)三軸加速度計(jì)的測(cè)量結(jié)果。

Ψ由以下公式計(jì)算得到：

Ψ=Ψm+λ

(7)

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

根據(jù)量測(cè)值θacc、φacc、Ψ與狀態(tài)向量

[q0q1q2q3]之間的關(guān)系[13]，系統(tǒng)量測(cè)矩陣為：

(8)

2 ASVDTCKF算法設(shè)計(jì)

2.1 TCKF算法流程

(1)根據(jù)三階球面-徑向容積準(zhǔn)則，計(jì)算CKF的采樣點(diǎn)ξj和權(quán)值ωj：

(9)

式中，n為系統(tǒng)狀態(tài)向量維數(shù)；j=1,2,…,2n；[1]2n為2n維單位球面與各坐標(biāo)軸的交點(diǎn)，

則采樣點(diǎn)集可進(jìn)一步表示成：

(11)

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

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

(12)

(13)

(14)

(15)

式中，r=1,2,…,n/2，當(dāng)n為奇數(shù)時(shí)，Λn,j=(-1)j。

(3)基于狀態(tài)估計(jì)的采樣點(diǎn)計(jì)算：

(16)

(17)

(4)狀態(tài)預(yù)測(cè)：

(18)

式中，L=2n。

(5)計(jì)算狀態(tài)預(yù)測(cè)誤差的協(xié)方差矩陣：

(19)

(6)計(jì)算基于狀態(tài)預(yù)測(cè)的采樣點(diǎn)：

(20)

(21)

(7)量測(cè)預(yù)測(cè)：

(22)

(8)計(jì)算新息的協(xié)方差矩陣：

(23)

(9)計(jì)算狀態(tài)向量與量測(cè)向量之間的互協(xié)方差矩陣：

(24)

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

(25)

(11)狀態(tài)估計(jì)：

(26)

(12)更新協(xié)方差矩陣：

(27)

2.2 基于奇異值分解的濾波方程推導(dǎo)

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

設(shè)A是一個(gè)m×n維的實(shí)數(shù)陣，對(duì)其進(jìn)行SVD分解：

(28)

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

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

(29)

(30)

(31)

(32)

將奇異值分解濾波方程和TCKF引入圖1姿態(tài)測(cè)量系統(tǒng)中，即可得到基于奇異值分解的改進(jìn)TCKF姿態(tài)估計(jì)算法ASVDTCKF。

3 實(shí)驗(yàn)方案

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

圖2 姿態(tài)測(cè)量硬件系統(tǒng)圖

為了分析ASVDTCKF算法的性能，本文將姿態(tài)測(cè)量硬件系統(tǒng)固定于轉(zhuǎn)臺(tái)，分別進(jìn)行靜態(tài)和三軸搖擺實(shí)驗(yàn)，并將OMAP-L138采集的慣性數(shù)據(jù)保存于第三層的U盤中，接入PC機(jī)，在MATLAB中進(jìn)行姿態(tài)數(shù)據(jù)融合算法性能對(duì)比實(shí)驗(yàn)，實(shí)驗(yàn)系統(tǒng)安裝如圖3所示。

圖3 實(shí)驗(yàn)系統(tǒng)安裝圖

4 實(shí)驗(yàn)結(jié)果分析

4.1 靜態(tài)實(shí)驗(yàn)

設(shè)置轉(zhuǎn)臺(tái)處于靜止?fàn)顟B(tài)，啟動(dòng)姿態(tài)測(cè)量系統(tǒng)，持續(xù)30 s，數(shù)據(jù)采集的頻率為100 Hz。分別采用基于TCKF的姿態(tài)估計(jì)算法(ATCKF)、基于平方根濾波的TCKF姿態(tài)估計(jì)算法(ASRTCKF)和ASVDTCKF進(jìn)行數(shù)據(jù)融合。

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

表1 靜態(tài)條件下不同算法運(yùn)行情況比較

圖4 靜態(tài)環(huán)境下不同算法輸出的姿態(tài)角

表2 靜態(tài)條件下不同算法的RMSE

表3 靜態(tài)條件下不同算法運(yùn)算時(shí)間

4.2 動(dòng)態(tài)實(shí)驗(yàn)

設(shè)置轉(zhuǎn)臺(tái)處于搖擺狀態(tài)，持續(xù)30 s，搖擺幅度為20°，搖擺頻率為0.5 Hz，數(shù)據(jù)采集頻率為100 Hz。分別采用ATCKF、ASRTCKF和ASVDTCKF對(duì)搖擺狀態(tài)下的ADIS16405測(cè)量數(shù)據(jù)進(jìn)行融合。

搖擺狀態(tài)下不同算法運(yùn)行情況如表4所示，不同算法輸出的姿態(tài)角結(jié)果如圖5所示，角度RMSE如表5所示，算法運(yùn)行時(shí)間如表6所示。由表4可知，搖擺動(dòng)態(tài)環(huán)境下，ASRTCKF和ASVDTCKF運(yùn)行正常，ATCKF算法運(yùn)行至1步終止，說(shuō)明動(dòng)態(tài)條件下QR分解或SVD分解能夠使算法運(yùn)行穩(wěn)定。由圖5和表5可知，動(dòng)態(tài)條件下ASVDTCKF算法的估計(jì)精度更優(yōu)，與ASRTCKF相比，俯仰角誤差、橫滾角誤差和航向角誤差分別減小了0.89%、27.5%和7.4%。由表6可知，動(dòng)態(tài)條件下ASVDTCKF的運(yùn)算時(shí)間與ASRTCKF相比減少了20.5%，說(shuō)明ASVDTCKF算法的運(yùn)算效率更高。

表4 搖擺環(huán)境下不同算法運(yùn)行情況比較

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

表6 搖擺條件下不同算法運(yùn)算時(shí)間

圖5 搖擺環(huán)境下不同算法輸出的姿態(tài)角

5 結(jié) 論

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