基于空洞卷積神經網絡的姿態估計算法*

李 哲, 吳 珂, 黃瓊丹

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

0 引 言

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

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

針對上述算法抗干擾能力弱,動態修正誤差困難等問題。本文提出一種基于空洞卷積神經網絡(dilated convolution neural network,DCNN)的MIMU姿態估計算法實現高精度的姿態估計。

1 算法整體框架

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

圖1 整體工作框架

2 四元數姿態角更新

2.1 建立姿態坐標

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

圖2 載體坐標系與導航坐標系之間角度轉換

(1)

2.2 陀螺儀姿態角解算

在俯仰角θ=±90°時,對式(1)的歐拉角計算會出現奇異點問題[9],導致數值不穩定,所以，歐拉角法算法不能適用于全姿態工作。四元數法基于工程應用,具有計算量低、便于實現、精度高的優點,可在全姿態下工作。四元數表示的變換矩陣如下

(2)

同時,四元數向量滿足微分方程

(3)

采用四階龍格庫塔法求解微分方程,計算四元數的更新值

(4)

為準確表示姿態信息,進一步將四元數單位化

(5)

根據式(2)～式(5)可得姿態角

(6)

2.3 加速度計和磁力計姿態解算

(7)

磁力計用于水平方向航向角的計算,當載體位于任意姿態時,可得地磁場沿x軸和y軸的分量mx和my為

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

(8)

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

ψ=ψm+Δψ

(9)

3 基于空洞卷積神經網絡的陀螺儀修正模型

3.1 低成本MIMU輸出模型

(10)

(11)

(12)

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

3.2 DCNN的設計

表1 卷積塊參數值

圖3 DCNN

4 擴展卡爾曼濾波融合數據

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

(13)

式中k為時刻值,xk為狀態值,Zk為測量值,Φ為狀態轉移矩陣,Γ為噪聲驅動矩陣,H為測量轉移矩陣,Wk-1為系統噪聲驅動矩陣,Vk為測量噪聲矩陣,滿足均值為零,方差陣分別是不相關白噪聲Q和R。綜上本文設計的EKF流程圖如圖4所示。

圖4 EKF算法流程

5 實驗驗證與結果分析

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

圖5 算法驗證平臺

5.1 靜止狀態測試

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

5.2 運動狀態測試

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

圖6 實驗測試結果

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

表2 三種算法姿態角均方誤差值

6 結 論

針對低成本MIMU融合姿態角精度不高和其易受噪聲和磁性物體干擾的問題,本文提出了基于DCNN的MIMU姿態估計算法：該算法通過DCNN來補償陀螺儀的誤差和漂移,使用基于四元數的四階龍格庫塔法更新陀螺儀姿態角,避免了出現奇異點問題,同時利用EKF算法融合加速度計和磁力計的姿態估計值,得到最優姿態角和初始姿態角。通過實驗驗證表明,新算法有效補償了陀螺儀的漂移和誤差,能穩定高精度的輸出姿態角。