李 哲, 吳 珂, 黃瓊丹

(1.西安郵電大學(xué) 電子工程學(xué)院,陜西 西安 710121；2.西安郵電大學(xué) 通信與信息工程學(xué)院,陜西 西安 710121)

0 引 言

小巧廉價(jià)的微慣性測(cè)量單元(micro inertial measurement unit,MIMU)廣泛應(yīng)用在無(wú)人機(jī)、智能手機(jī)和機(jī)器人中,提供了載體姿態(tài)和位置等信息。但由電子陀螺儀、加速度計(jì)和磁力計(jì)組成的MIMU存在難以估計(jì)誤差來(lái)源,如軸向偏差、比例因子和時(shí)變偏移量[1,2]。因此,在使用MIMU時(shí)會(huì)伴隨陀螺儀漂移、噪聲和磁性物體干擾等問(wèn)題,MIMU無(wú)法直接獲得精準(zhǔn)的姿態(tài)角。

姿態(tài)估計(jì)方法主要有互補(bǔ)濾波和卡爾曼濾波算法。文獻(xiàn)[3]使用互補(bǔ)濾波算法融合加速度和角速度數(shù)據(jù)輸出姿態(tài)角,但在動(dòng)態(tài)系統(tǒng)中存在濾波比例系數(shù)難以調(diào)整的問(wèn)題。文獻(xiàn)[4～6]使用卡爾曼濾波融合全球定位系統(tǒng)(GPS)數(shù)據(jù),雖提高了輸出姿態(tài)角精度,但存在GPS系統(tǒng)在室內(nèi)空間接收不到信號(hào),集成在小模塊中成本高等問(wèn)題。文獻(xiàn)[7]使用梯度下降算法結(jié)合卡爾曼濾波融合陀螺儀、加速度計(jì)和磁力計(jì)來(lái)估計(jì)姿態(tài)角。文獻(xiàn)[8]提出基于比例—積分—微分(proportional-integral-differential,PID)自適應(yīng)卡爾曼的慣導(dǎo)姿態(tài)算法,通過(guò)帶有自適應(yīng)的系數(shù)的濾波方程來(lái)解算姿態(tài)角。文獻(xiàn)[7,8]中的算法僅僅消除了陀螺儀誤差,仍存在加速度計(jì)和磁力計(jì)易受噪聲和磁場(chǎng)干擾的問(wèn)題。

針對(duì)上述算法抗干擾能力弱,動(dòng)態(tài)修正誤差困難等問(wèn)題。本文提出一種基于空洞卷積神經(jīng)網(wǎng)絡(luò)(dilated convolution neural network,DCNN)的MIMU姿態(tài)估計(jì)算法實(shí)現(xiàn)高精度的姿態(tài)估計(jì)。

1 算法整體框架

算法整體工作方案如圖1。

圖1 整體工作框架

2 四元數(shù)姿態(tài)角更新

2.1 建立姿態(tài)坐標(biāo)

如圖2選取合適的坐標(biāo)系,導(dǎo)航坐標(biāo)系選用“東、北、天”坐標(biāo)系(n系),載體坐標(biāo)系選用“前,左,上”坐標(biāo)系(b系)。

圖2 載體坐標(biāo)系與導(dǎo)航坐標(biāo)系之間角度轉(zhuǎn)換

(1)

2.2 陀螺儀姿態(tài)角解算

在俯仰角θ=±90°時(shí),對(duì)式(1)的歐拉角計(jì)算會(huì)出現(xiàn)奇異點(diǎn)問(wèn)題[9],導(dǎo)致數(shù)值不穩(wěn)定,所以，歐拉角法算法不能適用于全姿態(tài)工作。四元數(shù)法基于工程應(yīng)用,具有計(jì)算量低、便于實(shí)現(xiàn)、精度高的優(yōu)點(diǎn),可在全姿態(tài)下工作。四元數(shù)表示的變換矩陣如下

(2)

同時(shí),四元數(shù)向量滿足微分方程

(3)

采用四階龍格庫(kù)塔法求解微分方程,計(jì)算四元數(shù)的更新值

(4)

為準(zhǔn)確表示姿態(tài)信息,進(jìn)一步將四元數(shù)單位化

(5)

根據(jù)式(2)～式(5)可得姿態(tài)角

(6)

2.3 加速度計(jì)和磁力計(jì)姿態(tài)解算

(7)

磁力計(jì)用于水平方向航向角的計(jì)算,當(dāng)載體位于任意姿態(tài)時(shí),可得地磁場(chǎng)沿x軸和y軸的分量mx和my為

青海漢話大多是將[tala]置于時(shí)間名詞之后、動(dòng)詞之前，表示動(dòng)作行為的“終止”，或者行為動(dòng)作達(dá)到的“界限”，蒙古語(yǔ)則是將[tala]接綴在動(dòng)詞詞干上，同樣表示行為的“終止”或行為延續(xù)結(jié)束的“界限”。例如：

(8)

由式(8)得初始磁航向角ψm=arctan(my/mx),在載體處于任意姿態(tài)時(shí),初始磁航向角的基礎(chǔ)上加上磁偏角Δψ(真北和磁北的夾角)即為動(dòng)態(tài)的航向角

ψ=ψm+Δψ

(9)

3 基于空洞卷積神經(jīng)網(wǎng)絡(luò)的陀螺儀修正模型

3.1 低成本MIMU輸出模型

(10)

(11)

(12)

包含用于校正信號(hào)的信息：軸偏差(矩陣Mω≈I3,Ma≈I3)；比例因子(對(duì)角矩陣Sω≈I3,Sa≈I3)；以及MIMU測(cè)量的線性加速度(矩陣A≈03×3)。

3.2 DCNN的設(shè)計(jì)

表1 卷積塊參數(shù)值

圖3 DCNN

4 擴(kuò)展卡爾曼濾波融合數(shù)據(jù)

根據(jù)擴(kuò)展卡爾曼濾波(extend Kalman filtering,EKF)理論[12],融合陀螺儀姿態(tài)角估計(jì)值：αg=[θgγgψg],和加速度計(jì)、磁力計(jì)姿態(tài)角估計(jì)值：βg=[θaγaψa],初始偏航角由磁力計(jì)確定。以經(jīng)過(guò)DCNN修正后陀螺儀姿態(tài)角估計(jì)值作為預(yù)測(cè)狀態(tài)值,將由加速度計(jì)和磁力計(jì)得到的角度作為測(cè)量值,建立的EKF狀態(tài)方程和測(cè)量方程如下

(13)

式中k為時(shí)刻值,xk為狀態(tài)值,Zk為測(cè)量值,Φ為狀態(tài)轉(zhuǎn)移矩陣,Γ為噪聲驅(qū)動(dòng)矩陣,H為測(cè)量轉(zhuǎn)移矩陣,Wk-1為系統(tǒng)噪聲驅(qū)動(dòng)矩陣,Vk為測(cè)量噪聲矩陣,滿足均值為零,方差陣分別是不相關(guān)白噪聲Q和R。綜上本文設(shè)計(jì)的EKF流程圖如圖4所示。

圖4 EKF算法流程

5 實(shí)驗(yàn)驗(yàn)證與結(jié)果分析

驗(yàn)證系統(tǒng)的整體框架如圖5所示,微處理器采用基ARM Cortex—M4 內(nèi)核的 STM32F405RGT6,帶有32位的浮點(diǎn)運(yùn)算單元,MIMU使用內(nèi)含三軸陀螺儀、三軸加速度計(jì)和三軸磁力計(jì)的MPU9250,通過(guò)I2C總線采集MPU9250的數(shù)據(jù),采樣率100 Hz,再經(jīng)過(guò)通用異步收發(fā)器(UART)串口由上位機(jī)驗(yàn)證算法有效性。在實(shí)際使用時(shí)由于傳感器存在一定固定誤差,會(huì)對(duì)其初始化校正后再使用。

圖5 算法驗(yàn)證平臺(tái)

5.1 靜止?fàn)顟B(tài)測(cè)試

實(shí)驗(yàn)開(kāi)始時(shí)先將MPU9250初始化校正,待其預(yù)熱10 min后將模塊水平靜置3 min左右,解算得到姿態(tài)角后在上位機(jī)中使用Matplotlib繪制數(shù)據(jù)顯示結(jié)果。圖6(a)為陀螺儀原始數(shù)據(jù),圖6(b)為經(jīng)過(guò)濾波解算后的姿態(tài)角。從圖6(a)可知,陀螺儀采集的原始角速度存在噪聲干擾和隨機(jī)漂移,穩(wěn)定性和精度不足。經(jīng)過(guò)濾波后的角速度圖6(b)可見(jiàn),靜止?fàn)顟B(tài)下陀螺儀的角速度穩(wěn)定在±0.05 rad/s附近,滿足算法的要求。

5.2 運(yùn)動(dòng)狀態(tài)測(cè)試

實(shí)驗(yàn)開(kāi)始使水平靜置的MPU9250慣性測(cè)量單元做緩慢無(wú)規(guī)則運(yùn)動(dòng),將傳感器采集到的數(shù)據(jù)通過(guò)UART串口傳送至上位機(jī),分別使用標(biāo)準(zhǔn)EKF算法、互補(bǔ)濾波算法和基于空洞卷積神經(jīng)網(wǎng)絡(luò)的卡爾曼濾波(DCNN-EKF)算法獲取姿態(tài)角。三種不同方法解算的載體姿態(tài)對(duì)比結(jié)果如圖6(c)所示。從實(shí)驗(yàn)對(duì)比圖中可以看出,在三種不同的濾波算法中DCNN-EKF在抑制高噪聲方面有更明顯的作用優(yōu)于其余兩種算法,且能使曲線更加平滑,可提供更精確的姿態(tài)角方向估計(jì)。

圖6 實(shí)驗(yàn)測(cè)試結(jié)果

從表2中可知,采用DCNN-EKF解算的姿態(tài)角的偏差要小于EKF和融合濾波,其有解算精度方面的優(yōu)勢(shì)。對(duì)比EKF,DCNN-EKF在航向角、俯仰角和橫滾角三方面解算精度上分別提高了45 %,57 %,40 %。

表2 三種算法姿態(tài)角均方誤差值

6 結(jié) 論

針對(duì)低成本MIMU融合姿態(tài)角精度不高和其易受噪聲和磁性物體干擾的問(wèn)題,本文提出了基于DCNN的MIMU姿態(tài)估計(jì)算法：該算法通過(guò)DCNN來(lái)補(bǔ)償陀螺儀的誤差和漂移,使用基于四元數(shù)的四階龍格庫(kù)塔法更新陀螺儀姿態(tài)角,避免了出現(xiàn)奇異點(diǎn)問(wèn)題,同時(shí)利用EKF算法融合加速度計(jì)和磁力計(jì)的姿態(tài)估計(jì)值,得到最優(yōu)姿態(tài)角和初始姿態(tài)角。通過(guò)實(shí)驗(yàn)驗(yàn)證表明,新算法有效補(bǔ)償了陀螺儀的漂移和誤差,能穩(wěn)定高精度的輸出姿態(tài)角。