基于互補(bǔ)濾波的汽車姿態(tài)數(shù)據(jù)采集系統(tǒng)研究*

張新宇

(遼寧省交通高等?？茖W(xué)校軌道交通工程系,遼寧 沈陽 110122)

隨著汽車行業(yè)的快速發(fā)展,汽車監(jiān)控系統(tǒng)越來越受到重視,汽車監(jiān)控系統(tǒng)一方面可以監(jiān)督駕駛員的駕駛行為,另一方面,也可以在發(fā)生事故后,進(jìn)行事故回放,判定事故責(zé)任,保護(hù)駕駛?cè)藛T的合法權(quán)利。

近年來,廣泛應(yīng)用于無人機(jī)領(lǐng)域的MEMS 傳感器,由于其體積小、成本低、集成度高、精度高,逐步在汽車監(jiān)控領(lǐng)域得到了應(yīng)用。但是MEMS 傳感器由于自身的特點(diǎn),存在難以克服的噪聲誤差,因此,必須采取相應(yīng)的算法,來消除噪聲誤差。劉軍等[1]中使用六軸傳感器,實(shí)現(xiàn)汽車運(yùn)動(dòng)姿態(tài)測量,采用卡爾曼濾波算法消除誤差,由于重力加速度的存在,加速度計(jì)無法修正陀螺儀的航向角,卡爾曼濾波算法比較復(fù)雜。王京等[2]中使用三軸加速度計(jì),采用改進(jìn)的遺傳神經(jīng)網(wǎng)絡(luò)算法消除誤差,實(shí)現(xiàn)汽車姿態(tài)測量,但是三軸加速度計(jì)無法計(jì)算航向角,算法也比較復(fù)雜。曹景偉等[3]使用陀螺儀和加速度計(jì),利用卡爾曼濾波算法將MEMS 陀螺儀和加速度計(jì)輸出的數(shù)據(jù)進(jìn)行濾波融合。但沒有使用磁強(qiáng)計(jì)數(shù)據(jù),無法修正航向角,無法解算出準(zhǔn)確的全方位姿態(tài)角。且算法比較復(fù)雜。李超[4]使用陀螺儀、加速度計(jì)和磁強(qiáng)計(jì),主要研究線性加速度的干擾,誤差方程采用卡爾曼濾波算法。算法比較復(fù)雜。王贇贇[5]使用三軸加速度數(shù)據(jù)、三軸角速度數(shù)據(jù)和三軸地磁數(shù)據(jù),采用一種基于粒子濾波的條件式姿態(tài)解算算法,獲得姿態(tài)角,采用非線性擴(kuò)展卡爾曼濾波進(jìn)行數(shù)據(jù)融合。算法比較復(fù)雜。

因此設(shè)計(jì)了一種基于互補(bǔ)濾波的汽車姿態(tài)數(shù)據(jù)采集系統(tǒng),采用互補(bǔ)濾波數(shù)據(jù)融合算法,解算出全方位的姿態(tài)數(shù)據(jù),為監(jiān)控汽車的運(yùn)行狀態(tài)提供一種新的手段。

1 慣性導(dǎo)航原理

慣性導(dǎo)航是一種自主式定位方法,它是利用加速度計(jì)、陀螺儀和磁力計(jì)實(shí)時(shí)測量物體的線運(yùn)動(dòng)、角運(yùn)動(dòng)以及其他外界的運(yùn)動(dòng)信息,根據(jù)運(yùn)動(dòng)特性和慣性微分方程,并在給定初始條件下,推算運(yùn)動(dòng)物體的位置、速度和運(yùn)動(dòng)姿態(tài)信息。

運(yùn)動(dòng)姿態(tài)的表示方法有方向余弦、四元數(shù)和歐拉角。歐拉角在求解姿態(tài)時(shí)存在奇點(diǎn),不能用于全姿態(tài)的解算；方向余弦可用于全姿態(tài)的解算,但是計(jì)算量較大,不能滿足實(shí)時(shí)性要求。四元數(shù)計(jì)算量小,無奇點(diǎn),可以滿足汽車運(yùn)動(dòng)過程中姿態(tài)的實(shí)時(shí)解算。因此采用四元數(shù)來進(jìn)行姿態(tài)解算。

由于使用陀螺儀、加速度計(jì)和磁力計(jì)單獨(dú)計(jì)算姿態(tài)信息,都有各自存在的無法規(guī)避的物理缺陷:使用陀螺儀測量值計(jì)算,積分過程中容易產(chǎn)生較大的累積誤差,使用加速度計(jì)測量值計(jì)算,無法分離加速度測量值中的重力分量與運(yùn)動(dòng)分量,使用磁力計(jì)測量時(shí),容易受到環(huán)境中磁場干擾的影響,這3 種誤差在使用過程中是無法預(yù)先消除的,所以就必須要使用相應(yīng)的數(shù)據(jù)處理方式來最大限度上消除單一傳感器產(chǎn)生的誤差。

陀螺儀和加速度計(jì)的姿態(tài)信息成互補(bǔ)性,可以采用濾波的方法進(jìn)行加權(quán)平均,取得最好的估計(jì)結(jié)果。常用的方法有卡爾曼濾波和互補(bǔ)濾波。

卡爾曼濾波[6]是一種對系統(tǒng)狀態(tài)進(jìn)行遞推最優(yōu)估計(jì)的算法,它使用觀測信息以及前一時(shí)刻的最優(yōu)估計(jì)值作為系統(tǒng)輸入,來估計(jì)該時(shí)刻的狀態(tài)量。卡爾曼濾波的權(quán)重有一個(gè)逐步收斂的過程,計(jì)算量較大,而且需要對待測變量的誤差模型有一個(gè)準(zhǔn)確的估計(jì)。

互補(bǔ)濾波法[7]是根據(jù)不同傳感器的各自特性,即加速度計(jì)/磁力計(jì)具有高頻噪聲(需要低通濾波),陀螺儀具有低頻噪聲(需要高通濾波),通過高通或低通濾波器來濾波,然后再進(jìn)行組合來進(jìn)行數(shù)據(jù)的融合?；パa(bǔ)濾波的權(quán)重固定,不需要對誤差模型做過多估計(jì),計(jì)算量較小,因此采取互補(bǔ)濾波算法。

2 系統(tǒng)總體設(shè)計(jì)

汽車姿態(tài)數(shù)據(jù)采集系統(tǒng)分為車載單元和上位機(jī)單元。車載單元主要由嵌入式處理器、運(yùn)動(dòng)傳感器、北斗模塊和存儲模塊組成。處理器將運(yùn)動(dòng)傳感器采集到的九軸數(shù)據(jù)進(jìn)行融合解算,得到有效的姿態(tài)信息,結(jié)合北斗模塊定位信息和時(shí)間信息,以固定格式保存到存儲模塊中。

3 系統(tǒng)硬件設(shè)計(jì)

本系統(tǒng)采用基于ARM Cortex -M4 內(nèi)核的STM32F407ZGT6 高性能處理器,其時(shí)鐘頻率達(dá)到168 MHz,功耗低,具有更快的模數(shù)轉(zhuǎn)換速度、更強(qiáng)大的輸入/輸出端口。傳感器采用六軸MPU6050 和三軸磁力計(jì) HMC5883L。定位模塊采用北斗ATGM336H。存儲模塊用來把數(shù)據(jù)保存到U 盤中。如圖1 所示。

圖1 系統(tǒng)硬件框圖

4 汽車姿態(tài)數(shù)據(jù)解算

慣性器件都有各自的缺點(diǎn)和局限性,單一使用任何傳感器,都無法得到有效準(zhǔn)確的姿態(tài)信息。因此,需要利用多個(gè)傳感器之間存在性質(zhì)互補(bǔ)的特點(diǎn),將多個(gè)傳感器數(shù)據(jù)融合到一個(gè)單一的、最優(yōu)的姿態(tài)估計(jì)中[8],從而得到最準(zhǔn)確的姿態(tài)數(shù)據(jù)。

數(shù)據(jù)解算流程圖如圖2 所示。

圖2 程序設(shè)計(jì)流程圖

4.1 九軸數(shù)據(jù)校準(zhǔn)

三軸加速度計(jì)、三軸陀螺儀、三軸磁力計(jì)的讀數(shù)都會有偏移的現(xiàn)象,數(shù)據(jù)并不是圍繞著靜止的工作點(diǎn)擺動(dòng),因此,有必要校準(zhǔn)九軸數(shù)據(jù)[9]。

三軸陀螺儀的校準(zhǔn)是相對簡單的工作,只要找到擺動(dòng)的數(shù)據(jù)圍繞的中心點(diǎn)。在靜止?fàn)顟B(tài)時(shí),陀螺儀的X軸、Y軸和Z軸讀數(shù)理論上應(yīng)為0,但往往會存在偏移。

通過計(jì)算機(jī)串口讀取一段靜止時(shí)的三軸陀螺儀讀數(shù),將這三組讀數(shù)用統(tǒng)計(jì)均值的方式獲取偏移量,每次的讀數(shù)都減去偏移量就可以得到校準(zhǔn)后的數(shù)據(jù)了。

加速度計(jì)的校準(zhǔn)需要將設(shè)備置于水平、堅(jiān)固的平面上。加速度計(jì)的X軸和Y軸的理論值應(yīng)為0,加速度計(jì)的Z軸的理論值應(yīng)為-16 384(假設(shè)加速度計(jì)設(shè)定為2 gn,g的加速度的讀數(shù)應(yīng)為最大值-32 768的一半)。

通過計(jì)算機(jī)串口讀取一段時(shí)間的三軸加速度數(shù)值,由于加速度計(jì)的X軸和Y軸的理論值應(yīng)為0,這2 個(gè)讀數(shù)偏移量可以用統(tǒng)計(jì)均值的方法求得。加速度計(jì)的Z軸統(tǒng)計(jì)偏移量時(shí),每次讀數(shù)都需要加上16 384,再進(jìn)行統(tǒng)計(jì)均值計(jì)算。之后每次都減去偏移量就可得到比較準(zhǔn)確的三軸加速度值。

磁力計(jì)的校準(zhǔn)[10-12],如果磁力計(jì)平放著繞Z軸旋轉(zhuǎn)一周,讀回的X軸和Y軸2 個(gè)軸的數(shù)據(jù)組成的點(diǎn),理論上會形成一個(gè)圓,而實(shí)際上卻是一個(gè)橢圓,并且中心點(diǎn)不在原點(diǎn),此時(shí)使用航向角公式算出的角度是不準(zhǔn)的,如果想用航向角公式的話,需要把這個(gè)偏移的橢圓校正成中心點(diǎn)在原點(diǎn)的正圓。校準(zhǔn)的輸出可以根據(jù)下面的方法來計(jì)算:

通過計(jì)算機(jī)串口收集磁力計(jì)被旋轉(zhuǎn)360°后的數(shù)據(jù)。對采集到的數(shù)據(jù)進(jìn)行分析,找到磁力計(jì)X軸、Y軸以及Z軸的最大值和最小值,xmax,xmin,ymax,ymin,zmax,zmin。

于是可以得到X軸的偏移值為:

因?yàn)橹恍枰较?所以可以設(shè)X軸的比例系數(shù)xgain為1。

Y軸的偏移值為:

Y軸的比例系數(shù)可以由下式計(jì)算。

Z軸的偏移值為:

Z軸的比例系數(shù)可以由下式計(jì)算。

磁力計(jì)可以由下式校準(zhǔn)。

4.2 九軸姿態(tài)解算

陀螺儀可獲得角速度,通過積分可獲得角度,但隨時(shí)間累積,積分漂移誤差會越來越大,陀螺儀響應(yīng)速度快,表現(xiàn)為低頻特性,誤差隨時(shí)間累積。

加速度計(jì)對載體的加速度敏感,動(dòng)態(tài)響應(yīng)較慢,表現(xiàn)為高頻特性,測量傾斜角時(shí),短時(shí)間誤差較大[13]。

通過互補(bǔ)濾波將陀螺儀和加速度計(jì)的優(yōu)點(diǎn)結(jié)合起來[14],可以在低頻段和高頻段得到較好的信號,有效提高姿態(tài)輸出精度。

標(biāo)準(zhǔn)重力加速度gb從導(dǎo)航坐標(biāo)系n系轉(zhuǎn)換到載體坐標(biāo)系b系中的四元數(shù)表示的矩陣為:

式中:vX、vY、vZ是標(biāo)準(zhǔn)重力加速度gb的X軸、Y軸、Z軸分量。q0、q1、q2、q3是四元數(shù)中的實(shí)數(shù)分量。

加速度計(jì)測得的三軸加速度ab在b系中的矩陣表示為:

式中:ax、ay、az是三軸加速度ab在X軸、Y軸、Z軸分量。

gb和ab都是b系的向量,經(jīng)過歸一化處理后,模長為1,gb和ab向量叉乘的大小僅與sinθ有關(guān),當(dāng)角度很小時(shí),叉乘結(jié)果可以近似于角度成正比。這個(gè)結(jié)果可以作為給陀螺儀的校正補(bǔ)償值。公式為:

利用PI 控制器進(jìn)行互補(bǔ)濾波處理,消除偏移誤差。

PI 控制的公式為:

式中:ω是陀螺儀輸出的角速度,是陀螺儀的校正補(bǔ)償值。

只要誤差存在,PI 控制器就會持續(xù)作用[15],直到誤差為0?？刂频男ЧQ于P 和I 的值,分別對應(yīng)于比例控制和積分控制的參數(shù)。PI 控制器參數(shù)獲取的具體方法為,先設(shè)定積分控制系數(shù)Ki為0,將系統(tǒng)變?yōu)榧儽壤{(diào)節(jié),然后將比例控制系數(shù)Kp值由零開始逐步增大,直到系統(tǒng)出現(xiàn)振蕩,再反過來調(diào)節(jié)將Kp值逐漸減小,直到系統(tǒng)振蕩消失,記錄當(dāng)前的比例控制系數(shù)Kp值；當(dāng)比例控制系數(shù)值確定后,設(shè)定一個(gè)比較大的積分時(shí)間常數(shù)Ti,逐漸減小積分時(shí)間常數(shù),直到系統(tǒng)出現(xiàn)振蕩,再增大積分時(shí)間常數(shù)Ti,直到系統(tǒng)振蕩消失,記錄當(dāng)前的積分時(shí)間常數(shù)Ti值,積分控制系數(shù)Ki就等于Kp除以Ti。

將補(bǔ)償值加到三軸陀螺儀輸出的數(shù)據(jù)上進(jìn)行數(shù)據(jù)修正、融合[16]。

將融合后的陀螺儀數(shù)據(jù)通過四元數(shù)微分方程轉(zhuǎn)化為四元數(shù)輸出。

式中:T為采樣周期。

將得到的四元數(shù)轉(zhuǎn)化為姿態(tài)角。

互補(bǔ)濾波解算法流程如圖3 所示:

圖3 互補(bǔ)濾波解算法流程圖

4.3 航向角修正

上面的互補(bǔ)濾波法因?yàn)闆]有參考量,無法計(jì)算出當(dāng)前的航向角γ的絕對角度,只能得到航向角γ的變化量,也就是陀螺儀的Z軸的角速度?？梢酝ㄟ^對陀螺儀Z軸角速度進(jìn)行積分,來計(jì)算當(dāng)前航向角γ。由于測量精度以及溫度影響的問題,并且,隨著時(shí)間的推移,計(jì)算值會產(chǎn)生累積誤差,長時(shí)間后就完全失去了意義。

加速度計(jì)可以取得與地球重力相關(guān)的橫滾和俯仰方向的姿態(tài)信息,可以用來修正陀螺儀的橫滾角和俯仰角,但是加速度計(jì)無法測量重力方向(即Z軸方向)的旋轉(zhuǎn),無法計(jì)算出航向角,無法修正陀螺儀的航向角。因此需要加入磁力計(jì)HMC5883L 來解決這個(gè)問題,得到3 個(gè)軸的準(zhǔn)確姿態(tài)信息。

水平放置時(shí)航向角可由下式得到:

式中:MY和MX是磁力計(jì)的Y軸和X軸分量。

單獨(dú)使用磁力計(jì)時(shí),無法保證磁力計(jì)水平放置,導(dǎo)致測量的航向角有較大誤差,通常利用加速度計(jì)來修正磁力計(jì)的傾斜角。

非水平放置時(shí)通過MPU6050 獲得俯仰角(ψ)和翻滾角(θ)對航向角進(jìn)行傾斜角補(bǔ)償,公式如下:

非水平放置時(shí)航向角公式為:

5 試驗(yàn)驗(yàn)證

將本系統(tǒng)固定在汽車上,在汽車運(yùn)行過程中,采集汽車運(yùn)行姿態(tài)數(shù)據(jù)。上位機(jī)采用MATLAB 的GUI 功能進(jìn)行設(shè)計(jì),通過讀取系統(tǒng)中存儲的以時(shí)間命名的文本文件數(shù)據(jù),其中存儲著時(shí)間、加速度、角速度、磁向量、溫度、海拔高度、行駛速度等數(shù)據(jù)信息。在上位機(jī)界面上以曲線的形式把汽車的運(yùn)行軌跡描繪出來。經(jīng)過對比,能夠真實(shí)地反映出汽車的運(yùn)行狀態(tài)。

圖4 所示是以時(shí)間為X軸,三軸加速度為Y軸的曲線圖。

圖4 三軸加速度曲線圖

圖5 所示是以時(shí)間為X軸,三軸角速度為Y軸的曲線圖。

圖5 三軸角速度曲線圖

圖6 所示是以時(shí)間為X軸,三軸磁向量為Y軸的曲線圖。

圖6 三軸磁向量曲線圖

圖7 所示是以時(shí)間為X軸,分別以翻滾角(θ)、俯仰角(ψ)、航向角(γ)為Y軸的曲線圖。

圖7 姿態(tài)變換曲線圖

對于姿態(tài)的變換,是將解算出來的俯仰角、翻滾角和航向角通過計(jì)算轉(zhuǎn)換為在圖上顯示的點(diǎn)的變換。

首先計(jì)算航向角γ,即xOy平面中的轉(zhuǎn)動(dòng),只需要讓三角形的3 個(gè)點(diǎn)分別在下圖中顯示的大圓和小圓上即可。如圖8 所示。

圖8 姿態(tài)變換三維圖

對于俯仰角ψ,規(guī)定以坐標(biāo)(0,0,0)的點(diǎn)進(jìn)行旋轉(zhuǎn),即2 個(gè)圓的圓心,因此計(jì)算俯仰角ψ需要在xOz平面內(nèi)計(jì)算,通過公式sin(ψ)就可以計(jì)算出A、B、C3 個(gè)點(diǎn)分別在Z軸上的坐標(biāo),同時(shí)在A點(diǎn)變換后,其相對應(yīng)的X軸、Y軸變換公式是cos(ψ)。

對于翻滾角θ,需要計(jì)算B點(diǎn)和C點(diǎn)分別在Z軸上的坐標(biāo),因?yàn)檫@個(gè)三角形是繞著(0,0,0)轉(zhuǎn)的,并不是繞著BC的中點(diǎn)旋轉(zhuǎn),因而無法利用BC的長度乘以sin(θ)計(jì)算獲得。所以需要通過圓心作一條直線和BC線平行,假設(shè)其與AC交與F點(diǎn),與AB交與E點(diǎn)。如圖9 所示。

圖9 姿態(tài)變換計(jì)算三角形

無論ψ和θ如何轉(zhuǎn)動(dòng),OF都是位于xOy平面內(nèi)的,利用公式sin(θ)OF就可以得到F點(diǎn)在Z軸的變化,從而通過等比計(jì)算就可以得到C點(diǎn)在Z軸的變化,B點(diǎn)的變化和C點(diǎn)是一樣的,只是方向相反,將BC的坐標(biāo)在xOy平面利用公式cos(θ)得到,再進(jìn)行縮放就可以得到三角形的三個(gè)坐標(biāo)點(diǎn)。

6 結(jié)論

使用三軸加速度計(jì)、三軸陀螺儀、三軸磁力計(jì)采集姿態(tài)數(shù)據(jù),采用統(tǒng)計(jì)均值法,進(jìn)行九軸姿態(tài)數(shù)據(jù)校準(zhǔn),有效地解決了零點(diǎn)漂移,簡單易行；采用互補(bǔ)濾波法,合理調(diào)整PI 參數(shù),進(jìn)行數(shù)據(jù)融合,較之卡爾曼濾波,計(jì)算量小,更適合工程實(shí)現(xiàn)；針對加速度計(jì)無法修正航向角的問題,采用加速度計(jì)修正磁力計(jì)的水平問題,再用磁力計(jì)修正航向角。彌補(bǔ)了六軸傳感器無法修正航向角的不足,全方位修正了姿態(tài)角。經(jīng)過實(shí)際車輛測試,能夠真實(shí)地反映出汽車的運(yùn)行狀態(tài)。為監(jiān)控汽車的運(yùn)行狀態(tài)提供了一種更簡單、更實(shí)用的手段。