張新宇

(遼寧省交通高等專科學(xué)校軌道交通工程系,遼寧 沈陽(yáng) 110122)

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

近年來(lái),廣泛應(yīng)用于無(wú)人機(jī)領(lǐng)域的MEMS 傳感器,由于其體積小、成本低、集成度高、精度高,逐步在汽車監(jiān)控領(lǐng)域得到了應(yīng)用。但是MEMS 傳感器由于自身的特點(diǎn),存在難以克服的噪聲誤差,因此,必須采取相應(yīng)的算法,來(lái)消除噪聲誤差。劉軍等[1]中使用六軸傳感器,實(shí)現(xiàn)汽車運(yùn)動(dòng)姿態(tài)測(cè)量,采用卡爾曼濾波算法消除誤差,由于重力加速度的存在,加速度計(jì)無(wú)法修正陀螺儀的航向角,卡爾曼濾波算法比較復(fù)雜。王京等[2]中使用三軸加速度計(jì),采用改進(jìn)的遺傳神經(jīng)網(wǎng)絡(luò)算法消除誤差,實(shí)現(xiàn)汽車姿態(tài)測(cè)量,但是三軸加速度計(jì)無(wú)法計(jì)算航向角,算法也比較復(fù)雜。曹景偉等[3]使用陀螺儀和加速度計(jì),利用卡爾曼濾波算法將MEMS 陀螺儀和加速度計(jì)輸出的數(shù)據(jù)進(jìn)行濾波融合。但沒(méi)有使用磁強(qiáng)計(jì)數(shù)據(jù),無(wú)法修正航向角,無(wú)法解算出準(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í)測(cè)量物體的線運(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ì)算量小,無(wú)奇點(diǎn),可以滿足汽車運(yùn)動(dòng)過(guò)程中姿態(tài)的實(shí)時(shí)解算。因此采用四元數(shù)來(lái)進(jìn)行姿態(tài)解算。

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

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

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

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

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

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

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。存儲(chǔ)模塊用來(lái)把數(shù)據(jù)保存到U 盤中。如圖1 所示。

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

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

慣性器件都有各自的缺點(diǎn)和局限性,單一使用任何傳感器,都無(wú)法得到有效準(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ù)都會(huì)有偏移的現(xiàn)象,數(shù)據(jù)并不是圍繞著靜止的工作點(diǎn)擺動(dòng),因此,有必要校準(zhǔn)九軸數(shù)據(jù)[9]。

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

通過(guò)計(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的一半)。

通過(guò)計(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),理論上會(huì)形成一個(gè)圓,而實(shí)際上卻是一個(gè)橢圓,并且中心點(diǎn)不在原點(diǎn),此時(shí)使用航向角公式算出的角度是不準(zhǔn)的,如果想用航向角公式的話,需要把這個(gè)偏移的橢圓校正成中心點(diǎn)在原點(diǎn)的正圓。校準(zhǔn)的輸出可以根據(jù)下面的方法來(lái)計(jì)算:

通過(guò)計(jì)算機(jī)串口收集磁力計(jì)被旋轉(zhuǎn)360°后的數(shù)據(jù)。對(duì)采集到的數(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)解算

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

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

通過(guò)互補(bǔ)濾波將陀螺儀和加速度計(jì)的優(yōu)點(diǎn)結(jié)合起來(lái)[14],可以在低頻段和高頻段得到較好的信號(hào),有效提高姿態(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ì)測(cè)得的三軸加速度ab在b系中的矩陣表示為:

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

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

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

PI 控制的公式為:

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

只要誤差存在,PI 控制器就會(huì)持續(xù)作用[15],直到誤差為0?？刂频男ЧQ于P 和I 的值,分別對(duì)應(yīng)于比例控制和積分控制的參數(shù)。PI 控制器參數(shù)獲取的具體方法為,先設(shè)定積分控制系數(shù)Ki為0,將系統(tǒng)變?yōu)榧儽壤{(diào)節(jié),然后將比例控制系數(shù)Kp值由零開(kāi)始逐步增大,直到系統(tǒng)出現(xiàn)振蕩,再反過(guò)來(lái)調(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ù)通過(guò)四元數(shù)微分方程轉(zhuǎn)化為四元數(shù)輸出。

式中:T為采樣周期。

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

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

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

4.3 航向角修正

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

無(wú)論ψ和θ如何轉(zhuǎn)動(dòng),OF都是位于xOy平面內(nèi)的,利用公式sin(θ)OF就可以得到F點(diǎn)在Z軸的變化,從而通過(guò)等比計(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)漂移,簡(jiǎn)單易行；采用互補(bǔ)濾波法,合理調(diào)整PI 參數(shù),進(jìn)行數(shù)據(jù)融合,較之卡爾曼濾波,計(jì)算量小,更適合工程實(shí)現(xiàn)；針對(duì)加速度計(jì)無(wú)法修正航向角的問(wèn)題,采用加速度計(jì)修正磁力計(jì)的水平問(wèn)題,再用磁力計(jì)修正航向角。彌補(bǔ)了六軸傳感器無(wú)法修正航向角的不足,全方位修正了姿態(tài)角。經(jīng)過(guò)實(shí)際車輛測(cè)試,能夠真實(shí)地反映出汽車的運(yùn)行狀態(tài)。為監(jiān)控汽車的運(yùn)行狀態(tài)提供了一種更簡(jiǎn)單、更實(shí)用的手段。