應(yīng)用MEMS陀螺儀和加速度計的汽車運(yùn)動姿態(tài)測量

曹景偉，朱寶全

(東北林業(yè)大學(xué) 交通學(xué)院， 哈爾濱 150040)

改革開放以來，我國科學(xué)技術(shù)得到迅速發(fā)展，人民生活水平逐步提升，汽車已經(jīng)成為人們?nèi)粘Ｉ钪胁豢苫蛉钡拇焦ぞ?。汽車保有量的逐年攀升?dǎo)致道路交通事故頻發(fā)，汽車安全性已經(jīng)成為人們?nèi)找骊P(guān)注的焦點(diǎn)。汽車安全技術(shù)已經(jīng)成為當(dāng)今汽車系統(tǒng)主要的研究方向之一。汽車安全技術(shù)需要諸多精確的汽車行駛狀態(tài)參數(shù)，以此來確定汽車是否處于一種相對安全的運(yùn)行狀態(tài)[1-3]，因此汽車在各種工況下的運(yùn)動姿態(tài)測量對于保障汽車的安全性顯得尤為重要。

慣性技術(shù)產(chǎn)生于20世紀(jì)初，到目前為止已有百余年研究歷史。傳統(tǒng)上一般采用機(jī)械轉(zhuǎn)子陀螺構(gòu)成機(jī)械平臺對汽車運(yùn)動姿態(tài)進(jìn)行實(shí)時測量，但是由于測量平臺體積較大、安裝不便、運(yùn)行不穩(wěn)定，極大地影響了實(shí)驗(yàn)數(shù)據(jù)的采集和結(jié)論的判定[4]。近年來，MEMS(micro-electro-mechanical system)傳感器技術(shù)得到迅猛發(fā)展，因其具有體積小、成本低、能耗低、功率高、靈敏度高等諸多優(yōu)點(diǎn)，在汽車運(yùn)行狀態(tài)測量方面得到了廣泛應(yīng)用[5-7]。

汽車運(yùn)動姿態(tài)解算方法包括歐拉角法、方向余弦法和四元數(shù)法等。在使用歐拉角法求解姿態(tài)微分方程時，如果俯仰角低于90°，計算結(jié)果較為準(zhǔn)確，如果俯仰角接近90°，方程會出現(xiàn)退化現(xiàn)象[8-10]。方向余弦法可以避免歐拉角法方程退化的現(xiàn)象發(fā)生，但在求解過程中要計算9個聯(lián)立微分方程，計算量較大，且實(shí)時解算比較困難，在工程上并不實(shí)用[11]。意大利比薩大學(xué)的Angelo M.Savatin[12]提出基于四元數(shù)的擴(kuò)展卡爾曼濾波算法，同時在求解過程中引入自適應(yīng)的測量噪聲協(xié)方差來減少線性加速度和周圍局部磁場的干擾，較好地提高了姿態(tài)測量精度。

本文針對汽車實(shí)際運(yùn)行環(huán)境，運(yùn)用集MEMS陀螺儀和加速度計為一體的MPU-6050傳感器模塊進(jìn)行汽車運(yùn)動姿態(tài)測量，為研究汽車在不同運(yùn)動姿態(tài)下的運(yùn)動規(guī)律、提高汽車行駛安全性提供一定的參考依據(jù)。

1 姿態(tài)測量的基本原理

汽車運(yùn)行姿態(tài)測量主要利用MEMS傳感器技術(shù)，通過集MEMS陀螺儀和加速度計為一體的MPU-6050傳感器模塊采集運(yùn)行姿態(tài)數(shù)據(jù)。實(shí)驗(yàn)時首先將MPU-6050傳感器模塊安裝在一個立方體小盒中，同時將其固定于汽車質(zhì)心位置處，并保證汽車與MPU-6050傳感器模塊固結(jié)于同一坐標(biāo)軸上。MPU-6050傳感器模塊將采集到的數(shù)據(jù)經(jīng)坐標(biāo)變換和姿態(tài)解算后，以加速度包、角速度包和角度包3個數(shù)據(jù)包的形式傳至上位機(jī)，然后通過顯示器實(shí)時顯示三軸加速度、角速度和角度的曲線變化。汽車在不同運(yùn)動姿態(tài)下的姿態(tài)數(shù)據(jù)可以隨時記錄，形成固定文件并保存在存儲模塊中，可以隨時打開并導(dǎo)入Matlab中，方便日后的分析與整理。

2 姿態(tài)測量所需硬件

汽車運(yùn)行姿態(tài)測量所用到的主要硬件包括MPU-6050傳感器模塊、數(shù)據(jù)處理單元、顯示器、存儲模塊以及車載直流電源等。MPU-6050傳感器模塊內(nèi)部集三軸MEMS陀螺儀和三軸加速計為一體，可以實(shí)時采集汽車運(yùn)動時的三軸加速度、角速度和角度。數(shù)據(jù)處理單元將采集到的數(shù)據(jù)進(jìn)行坐標(biāo)變換和姿態(tài)解算處理，以加速度包、角速度包和角度包的形式傳至上位機(jī)。顯示器內(nèi)置上位機(jī)，將數(shù)據(jù)處理單元發(fā)送過來的3個數(shù)據(jù)包順序輸出，生成可以實(shí)時顯示的隨時間不斷變化的動態(tài)曲線。上位機(jī)可以隨時開始記錄所需要的數(shù)據(jù)，并在數(shù)據(jù)采集結(jié)束之后停止記錄，數(shù)據(jù)被保存在存儲模塊中，可以隨時打開已存數(shù)據(jù)，便于進(jìn)一步的整理與分析。

MPU-6050傳感器模塊將三軸MEMS陀螺儀、三軸加速計與單一傳感器模塊整合在一起，同時融合九軸運(yùn)動感測演算技術(shù)的六軸運(yùn)動處理組件，其內(nèi)部自帶電壓穩(wěn)定電路，可以兼容3.3 V/5 V的嵌入式系統(tǒng)，連接便捷。傳感器模塊采用先進(jìn)的數(shù)字濾波技術(shù)，配合動態(tài)卡爾曼濾波算法，能有效降低測量噪聲，可以實(shí)現(xiàn)動態(tài)準(zhǔn)確輸出，測量精度可以精確到0.01°，可靠性高，具有良好的綜合性能。

本研究使用的MPU-6050傳感器模塊簡圖如圖1所示，技術(shù)參數(shù)如表1所示。

圖1 MPU-6050傳感器模塊簡圖

表1 MPU-6050傳感器模塊技術(shù)參數(shù)

MPU-6050傳感器模塊在3個方向上分別采用了一個AD轉(zhuǎn)換器，將角度的變化信息轉(zhuǎn)換為數(shù)字量進(jìn)行輸出，同時3個方向的角速度值通過積分得到角度值。如圖2所示，MEMS陀螺儀與CPU之間采用I2C時序通信，其基準(zhǔn)電壓通過上拉電阻接3.3 V電源。此外，MEMS陀螺儀具有轉(zhuǎn)換完畢的信號線，該信號線與CPU的外部中斷口相連，CPU可以通過中斷方式或者查詢方式讀取陀螺儀的狀態(tài)。

圖2 陀螺儀電路原理

3 姿態(tài)角的測量

3.1 坐標(biāo)系與姿態(tài)角

載體導(dǎo)航坐標(biāo)系OXYZ和載體平臺坐標(biāo)系Oxyz是汽車運(yùn)動姿態(tài)測量中重要的2個坐標(biāo)參考系，如圖3所示。載體導(dǎo)航坐標(biāo)系OXYZ的原點(diǎn)隨汽車重心移動，同時其與地球慣性坐標(biāo)系的Z軸保持平行；載體平臺坐標(biāo)系通常固定于車身，當(dāng)車身姿態(tài)發(fā)生變化時，3軸向隨即發(fā)生改變。汽車運(yùn)動姿態(tài)測量實(shí)際上就是對載體平臺坐標(biāo)系Oxyz相對于載體導(dǎo)航坐標(biāo)系OXYZ的角度偏移量進(jìn)行測量。姿態(tài)角一般包括3個：俯仰角α(z軸與OXZ平面的夾角)、橫滾角β(x軸與OXY平面的夾角)、航向角γ(y軸與OYZ平面的夾角)。

圖3 坐標(biāo)系與姿態(tài)角

剛體在三維空間的轉(zhuǎn)動用歐拉角進(jìn)行表示，坐標(biāo)參考系可以按照先后順序，以繞著轉(zhuǎn)動軸轉(zhuǎn)動的形式從最初的參考系到三維空間內(nèi)任意的參考系。所謂的載體平臺的空間姿態(tài)實(shí)質(zhì)上為平臺從起始位置依次繞Z軸、X軸、Y軸做基本旋轉(zhuǎn)后到下一位置的復(fù)合結(jié)果，如圖4所示。根據(jù)需要，平臺初始位置所在的坐標(biāo)系為O-X0Y0Z0，當(dāng)運(yùn)動到任意位置時所在的坐標(biāo)系為O?-X?Y?Z?，平臺的任意運(yùn)動可以分解為平臺繞X、Y、Z軸的基本旋轉(zhuǎn)，順序依次為：Z0軸旋轉(zhuǎn)到O′-X′Y′Z′,繞X0軸旋轉(zhuǎn)到O″-X″Y″Z″，繞Y0軸旋轉(zhuǎn)到O?-X?Y?Z?。

(1)

式中：(X?,Y?,Z?)為平臺經(jīng)坐標(biāo)系旋轉(zhuǎn)后的空間位置坐標(biāo)；(X,Y,Z)為空間起始位置坐標(biāo)。

圖4 坐標(biāo)變換

各次基本旋轉(zhuǎn)對應(yīng)的變換矩陣為

(2)

式中：α為俯仰角(°)；β為橫滾角(°)；γ為航向角(°)。

如果α、β、γ都為較小角度時，則忽略較小角度之間的高階小量，簡化旋轉(zhuǎn)矩陣為

(3)

式中：α為俯仰角(°)；β為橫滾角(°)；γ為航向角(°)。

在使用上述簡化旋轉(zhuǎn)矩陣時，定義俯仰角(pitch)為在空間內(nèi)平臺繞Y軸旋轉(zhuǎn)角度，橫滾角(roll)為在空間內(nèi)平臺繞X軸旋轉(zhuǎn)角度，航向角(yaw)為在空間內(nèi)平臺繞Z軸旋轉(zhuǎn)角度，如果平臺起始位置為水平時，則俯仰角、橫滾角和航向角為0。

對車身姿態(tài)進(jìn)行測量要分別確定俯仰角、橫滾角和航向角的大小。當(dāng)汽車處于靜止或勻速直線運(yùn)動狀態(tài)時，三軸加速度計的輸出值實(shí)際為重力加速度在3軸方向上的分量值，其傾角可以根據(jù)求解反三角函數(shù)得到。但當(dāng)汽車處于高速運(yùn)動狀態(tài)時，加速度計會受到除重力加速度以外的加速度的影響，此時如果繼續(xù)采用加速度計計算傾角則會產(chǎn)生較大的誤差，影響數(shù)據(jù)的準(zhǔn)確性。

為了解決這個問題，加入MEMS陀螺儀對當(dāng)前角速度變化進(jìn)行輔助測量，可以在瞬間實(shí)現(xiàn)對角速度變化的積分且將計算數(shù)據(jù)累加在一起，快速得到傾角。然而，由于MEMS陀螺長時間使用會造成靜態(tài)零點(diǎn)漂移誤差，使有時計算的傾角并不準(zhǔn)確，因此在實(shí)際姿態(tài)測量中要將MEMS陀螺儀和加速度計測量的數(shù)據(jù)進(jìn)行濾波融合，才能輸出準(zhǔn)確的姿態(tài)角。

3.2 MEMS陀螺儀的空間姿態(tài)測量

MPU-6050傳感器模塊內(nèi)置三軸陀螺儀L3G4200D，它能將3軸角速度動態(tài)輸出，其輸出值可以實(shí)現(xiàn)最高16位的精度，測量范圍可以實(shí)現(xiàn)最大±2 000(°)/s。

利用陀螺儀可以在平臺動態(tài)條件下實(shí)現(xiàn)積分累加計算，且測量傾角相當(dāng)準(zhǔn)確。由于陀螺儀存在靜態(tài)偏置誤差，所以在測量之前要對陀螺儀進(jìn)行零點(diǎn)偏置補(bǔ)償。

設(shè)陀螺儀在X、Y、Z三軸的輸出值分別為 GyroX、GyroY和GyroZ，零點(diǎn)偏置為GyroX_offset、GyroY_offset、GyroZ_offset,則各軸對角速度積分即為平臺繞該軸旋轉(zhuǎn)的角度，有：

(4)

(5)

(6)

式中：angleGα為陀螺儀計算俯仰角(rad)；angleGβ為陀螺儀計算橫滾角(rad)；angleGγ為陀螺儀計算航向角(rad)。

3.3 加速度計的空間姿態(tài)測量

MPU-6050傳感器模塊采用的加速度計為3軸數(shù)字輸出的加速度計ADXL345，這是一種低成本、低功耗的測量元件，可以良好地測量3軸加速度的方向和大小，且各軸的測量輸出值可以實(shí)現(xiàn)最高16位精度，測量值范圍最大可達(dá)±16g。圖5為加速度計的輸出值方向和大小示意圖。

圖5 加速度計輸出值示意圖

(7)

(8)

(9)

(10)

式中RX、RY、RZ分別為三軸歸一化的結(jié)果(m/s2)。

由本文定義并根據(jù)圖3可算得

(11)

式中α為俯仰角(°)。

(12)

式中β為橫滾角(°)。

(13)

式中γ為航向角(°)。

4 上位機(jī)顯示

4.1 上位機(jī)軟件執(zhí)行流程

系統(tǒng)軟件在整體運(yùn)行過程中的上位機(jī)軟件執(zhí)行流程如圖6所示。

圖6 上位機(jī)軟件執(zhí)行流程

在數(shù)據(jù)正式采集之前，首先要初始化配置程序，上位機(jī)中可以自動檢測并判斷串口上是否有信號，如果串口上有信號，則自動連接完成，進(jìn)行下一步的數(shù)據(jù)采集。數(shù)據(jù)正式采集時，首先要對角度的正負(fù)進(jìn)行判斷，此時要通12位二進(jìn)制的最高位進(jìn)行0和1的判斷，0表示數(shù)據(jù)為正，1表示數(shù)據(jù)為負(fù)。然后要對檢測數(shù)據(jù)是否在非線性區(qū)間進(jìn)行判斷，如果在非線性區(qū)間，要調(diào)用非線性修正程序進(jìn)行修正，同時完成存儲；如果不在非線性區(qū)間，則直接存儲。在設(shè)置程序中，設(shè)置達(dá)到50個數(shù)據(jù)進(jìn)行1次誤差處理，假若沒有達(dá)到50個數(shù)據(jù)則重復(fù)往復(fù)步驟，直至完成數(shù)據(jù)的顯示與存儲。

4.2 上位機(jī)的設(shè)置界面

打開顯示器中上位機(jī)的設(shè)置界面，如圖7所示。首先點(diǎn)擊串口配置，選擇合適的COM串口，然后點(diǎn)擊波特率，選擇115 200 bit/s作為默認(rèn)波特率。在設(shè)置選項中，包括語言、輸出速率、靜止檢測門限、帶寬、方向、Z軸角度歸零和加速度計校準(zhǔn)等諸多選擇欄，語言包括中文和英文，可自由切換。輸出速率默認(rèn)選擇1 Hz。靜止檢測門限在0.122～1.831(°)/s可供選擇，默認(rèn)選擇0.488(°)/s。帶寬在5～260 Hz可供選擇，默認(rèn)選擇21 Hz。方向包括水平和垂直，默認(rèn)選擇水平。Z軸角度歸零和加速度計校準(zhǔn)在數(shù)據(jù)采集前初始化。

圖7 上位機(jī)的設(shè)置界面

MPU-6050傳感器模塊將經(jīng)姿態(tài)解算后的數(shù)據(jù)傳至上位機(jī)，每幀數(shù)據(jù)以加速度包、角速度包和角度包的形式順序輸出。同時可自定義波特率，定義波特率為115 200 bit/s時，每隔10 ms輸出1幀數(shù)據(jù)，定義波特率為9 600 bit/s時，每隔50 ms輸出1幀數(shù)據(jù)。

4.3 上位機(jī)顯示的試驗(yàn)結(jié)果

在不同的汽車運(yùn)動姿態(tài)下，測量出的3軸加速度、角速度和角度的變化有顯著區(qū)別。為研究汽車在不同運(yùn)動姿態(tài)下的運(yùn)動規(guī)律，可以在不同的運(yùn)動姿態(tài)下記錄當(dāng)時的3軸加速度、角速度和角度的曲線變化。以實(shí)際道路實(shí)驗(yàn)為例，實(shí)驗(yàn)車輛進(jìn)行了4次加減速實(shí)驗(yàn)，且在經(jīng)過每2次加減速實(shí)驗(yàn)之后重新回到車輛起動位置。汽車當(dāng)時的運(yùn)動狀態(tài)通過ScanMaster-ELM軟件實(shí)時監(jiān)測，該軟件與汽車OBD(on board diagnostics)系統(tǒng)通過藍(lán)牙無線連接，可以實(shí)時測量汽車當(dāng)時運(yùn)動狀態(tài)下的車輛參數(shù)，如圖8所示。

圖8 當(dāng)時運(yùn)行狀態(tài)下車輛參數(shù)變化曲線界面

當(dāng)汽車開始實(shí)驗(yàn)時，點(diǎn)擊上位機(jī)中的記錄選項，數(shù)據(jù)開始保存，形成文件并保存在存儲模塊中。當(dāng)數(shù)據(jù)采集結(jié)束時，點(diǎn)擊停止選項，數(shù)據(jù)停止記錄。實(shí)驗(yàn)數(shù)據(jù)文件可直接導(dǎo)入到Matlab軟件中，編譯運(yùn)行后得到當(dāng)時運(yùn)動姿態(tài)下3軸加速度、角速度、角度以及傳感器模塊溫度的曲線變化，如圖9所示。實(shí)驗(yàn)結(jié)果表明：汽車運(yùn)動姿態(tài)角度變化與汽車當(dāng)時的運(yùn)行狀態(tài)保持一致。

圖9 三軸運(yùn)動姿態(tài)角度變化曲線

圖10為實(shí)驗(yàn)所得汽車運(yùn)動姿態(tài)估計誤差曲線，可以直觀地看出估計誤差相對較小，沒有超過1°，能夠滿足汽車運(yùn)動姿態(tài)測量的基本要求。

圖10 運(yùn)動姿態(tài)估計誤差曲線

5 結(jié)束語

本文針對在汽車運(yùn)動姿態(tài)測量上存在的傳統(tǒng)實(shí)驗(yàn)平臺體積較大、安裝不便、運(yùn)行不穩(wěn)定等問題，由集MEMS陀螺儀和加速度計為一體的MPU-6050傳感器模塊采集3軸加速度、角速度和角度數(shù)據(jù)，經(jīng)濾波融合與姿態(tài)解算后，通過顯示器中的上位機(jī)界面實(shí)現(xiàn)對汽車運(yùn)動姿態(tài)測量數(shù)據(jù)的實(shí)時顯示和存儲保存。實(shí)驗(yàn)結(jié)果表明：應(yīng)用MEMS陀螺儀和加速度計的汽車運(yùn)動姿態(tài)測量精度較高，受外界影響較小，具有良好的可靠性，可為進(jìn)一步研究汽車在不同運(yùn)動姿態(tài)下的運(yùn)動規(guī)律，提高汽車行駛安全性能提供一定的參考依據(jù)。

參考文獻(xiàn)：

[1] 趙靈.基于MEMS陀螺儀的穩(wěn)定平臺關(guān)鍵技術(shù)研究[D].南京：南京理工大學(xué)，2012.

[2] 都雪靜,田雨蒙.全鋁車身電動轎車正面碰撞仿真[J].重慶理工大學(xué)學(xué)報(自然科學(xué)),2016,30(8):20-26.

[3] 趙鋒,金智林,馮博,等.追尾碰撞的車輛穩(wěn)定性分析與控制[J].重慶理工大學(xué)學(xué)報(自然科學(xué)),2016,30(5):20-26.

[4] 閆冬,管欣,高振海.基于MEMS技術(shù)的微慣性傳感器及在汽車上的應(yīng)用[J].汽車技術(shù),2006(2):1-6.

[5] 劉付強(qiáng).基于MEMS器件的捷聯(lián)姿態(tài)測量系統(tǒng)技術(shù)研究[D].哈爾濱:哈爾濱工程大學(xué),2007.

[6] 吳黎明,張力鍇,李怡凡.基于ANN和單個三軸加速度傳感器的汽車運(yùn)動姿態(tài)測量[J].傳感技術(shù)學(xué)報,2011(6):923-927.

[7] 趙玲,李文靖,李長安，等.基于ADIS16355的汽車駕駛操作信號采集系統(tǒng)[J].電子技術(shù)應(yīng)用,2012(2):37-39.

[8] 王建鋒,張紅,李平.基于MEMS器件的車輛姿態(tài)測量系統(tǒng)開發(fā)[J].中國科技論文在線,2015,10(19):2281-2286.

[9] 包瑞新.車輛姿態(tài)多傳感器檢測系統(tǒng)與信息融合算法研究[D].沈陽:沈陽農(nóng)業(yè)大學(xué),2011.

[10] 李祥云.基于多維加速度傳感器的車輛姿態(tài)測量系統(tǒng)研究與設(shè)計[D].哈爾濱:哈爾濱工業(yè)大學(xué),2015.

[11] 任亮,白國柱,周經(jīng)美等.基于MEMS的車輛姿態(tài)感知系統(tǒng)[J].計算機(jī)系統(tǒng)應(yīng)用,2014,23(8):84-89.

[12] ANGELO M S.Quaternion-Based Extended Kalman Filter for Determining Orientation by Inertial and Magnetic Sensing[J].IEEE Transactions on Biomedical Engineering,2006， 53(7)：112-119.