基于九軸姿態(tài)傳感器的智能汽車平面定位模塊設(shè)計(jì)與實(shí)踐

王藝輝，劉延飛，趙鵬濤，王 忠

（火箭軍工程大學(xué)，陜西 西安 710025）

在智能汽車競賽中，調(diào)試過程是提升汽車行駛速度最重要的途徑。通常情況下，在調(diào)試過程中，是通過人眼來確定車輛速度、位置等信息，而大量研究者在設(shè)計(jì)和分析智能汽車控制系統(tǒng)調(diào)試方法[1-4]，以實(shí)現(xiàn)人眼識別難以定量分析的車輛行駛狀態(tài)，評判控制參數(shù)的優(yōu)劣。因此，位置信息在智能車的調(diào)試過程中扮演著十分重要的角色。慣性導(dǎo)航是導(dǎo)航的一種重要方式，它有硬件實(shí)現(xiàn)簡單的優(yōu)點(diǎn)。本系統(tǒng)通過研究分析了MPU6050應(yīng)用于慣性導(dǎo)航的方法，并通過實(shí)驗(yàn)驗(yàn)證了通過慣性導(dǎo)航解算位置信息的可行性，以實(shí)現(xiàn)對智能車坐標(biāo)的精確定位和調(diào)試過程中的定量分析。

1 系統(tǒng)整體結(jié)構(gòu)

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

慣性導(dǎo)航技術(shù)在導(dǎo)彈導(dǎo)航制導(dǎo)領(lǐng)域具有重要的地位，而電子設(shè)備成本的降低推動(dòng)了民用慣性導(dǎo)航技術(shù)的發(fā)展。如圖1所示，平面定位系統(tǒng)主要由角度采集模塊、速度采集模塊、微處理器、坐標(biāo)顯示模塊和電源模塊等組成。MPU6050作為角度采集傳感器，是通過IIC總線直接采集角速度的，避免了對采集信息進(jìn)行多次處理和其他信號的干擾。微處理模塊對角度和速度信息進(jìn)行處理后得到平面內(nèi)坐標(biāo)，可通過液晶顯示屏進(jìn)行顯示，也可以通過藍(lán)牙將坐標(biāo)傳送給電腦進(jìn)行軌跡復(fù)現(xiàn)、儲存等操作。

圖1 系統(tǒng)總體框圖

1.2 MPU6050模塊

通過平面姿態(tài)角測量模塊測量平面姿態(tài)角，與速度進(jìn)行計(jì)算，從而實(shí)時(shí)解算出車輛相對于初始位置（MPU6050初始化處）的位置。MPU6050的角速度全格感測范圍為±250，±500，±1 000，±2 000/sec（dps），可準(zhǔn)確追蹤快速與慢速動(dòng)作，并且用戶可程式控制的加速度計(jì)全格感測范圍為±2 g、±4 g、±8 g和±16 g。產(chǎn)品傳輸可透過最高至400 kHz的IIC總線或最高達(dá)20 MH稱的SPI總線[1]。本文使用的是基于MPU6050制作的GY-521模塊，此模塊體積小、裝卸方便，方便固連在智能車上。GY-521電路原理如圖2所示。

圖2 GY-521電路原理圖

1.3 微處理模塊

主控制器選用恩智浦公司Kinetis系列的MCU K60[5-6]。該型號芯片以ARM Crotex-M4為內(nèi)核，功耗低，有DSP指令和單精度浮點(diǎn)運(yùn)算單元，提供50 MHz、72 MHz、100 MHz幾種CPU頻率，并且集成了高精度的16位ADC、12位DAC、可編程增益放大器、高速比較器和內(nèi)部參考電壓。

1.4 速度采集模塊

速度采集模塊使用的是龍邱公司生產(chǎn)的512線的增量式光電編碼器。供電電壓從3.3 V到5 V兼容，兼容了正交解碼模式。該速度采集模塊采用了光電信號的模式測速，與霍爾型傳感器相比，在靠近電機(jī)測速時(shí)極大地避免了電磁信號的干擾。

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

系統(tǒng)通過上位機(jī)與下位機(jī)的配合將智能車軌跡繪制出來。單片機(jī)初始化傳感器和藍(lán)牙，上位機(jī)初始化并配置藍(lán)牙參數(shù)。當(dāng)單片機(jī)定時(shí)器觸發(fā)中斷時(shí)，采集角速度和速度信息，解算角度和走過路程。由于到電腦處理數(shù)據(jù)的能力比較強(qiáng)，將角度信息和路程信息通過藍(lán)牙通訊發(fā)送至上位機(jī)處理可以減少解算誤差。

2.1 下位機(jī)流程設(shè)計(jì)

下位機(jī)的主要功能是角度、速度信息的提取，坐標(biāo)的解算和顯示?？紤]到下位機(jī)的運(yùn)算能力比較強(qiáng)，將坐標(biāo)的解算放在下位機(jī)中進(jìn)行。下位機(jī)數(shù)據(jù)采集與處理流程如圖3所示，具體為：①系統(tǒng)先進(jìn)入各模塊初始化函數(shù)，其中，IIC模塊波特率設(shè)為400 kbps，并使用IIC模塊初始化MPU6050；速度采集使用DMA觸發(fā)中斷計(jì)數(shù)，以減少測速對CPU資源的占用；初始化PIT定時(shí)器，定時(shí)時(shí)間為1 ms。②定時(shí)器觸發(fā)中斷進(jìn)入中斷處理程序PIT0_IRQHandler()，通過IIC采集角速度，并計(jì)算得出角度值。讀取DMA計(jì)數(shù)值，通過計(jì)算公式解算坐標(biāo)，并將坐標(biāo)顯示在液晶顯示屏。③當(dāng)檢測到發(fā)送按鍵按下時(shí)，將數(shù)據(jù)經(jīng)藍(lán)牙發(fā)送至上位機(jī)。

圖3 下位機(jī)數(shù)據(jù)采集與處理流程圖

2.2 陀螺儀轉(zhuǎn)角信息獲取程序

由于MPU6050與車體捷聯(lián)，令MPU6050坐標(biāo)系與圖車體坐標(biāo)系平行。此時(shí)，需要采集的角度為Z軸方向轉(zhuǎn)角。

測量轉(zhuǎn)角時(shí)，重力方向通過Z軸，智能車在水平面行駛時(shí)，Z軸與重力方向夾角恒等于0，此時(shí)利用反三角函數(shù)求解得加速度輸出角度恒等于90°。如果加速度輸出角度無法與陀螺儀輸出角度進(jìn)行卡爾曼濾波運(yùn)算，考慮使用陀螺儀測量角速度積分輸出角度。使用這種方法存在兩類較大的誤差。如圖4所示，通過除法運(yùn)算將采集數(shù)據(jù)縮放，過濾部分噪聲。在智能車行駛過程中，難以消除震動(dòng)。當(dāng)MPU6050Z軸方向受到震動(dòng)時(shí)，其他2個(gè)方向也受到震動(dòng)。在整場行駛時(shí)，X軸方向不發(fā)生轉(zhuǎn)動(dòng)，因此，X軸在受到震動(dòng)時(shí)，輸出近似為震動(dòng)噪聲。MPU6050芯片體積比較小，此時(shí)將X軸、Z軸受到的震動(dòng)視為一致的，將Z軸數(shù)據(jù)減去X軸數(shù)據(jù)可以抑制震動(dòng)噪聲。

圖4 下位機(jī)數(shù)據(jù)采集與處理流程圖

2.3 坐標(biāo)計(jì)算程序

如圖5所示，Pn與Pn+1為相鄰的2個(gè)點(diǎn)，Length為速度采集值，θ為MPU6050 Z軸相對于初始點(diǎn)轉(zhuǎn)過的角度。弧PnPn+1表示實(shí)際走過的軌跡。當(dāng)采集周期趨于0時(shí)，弧線與直線圍成的圖形面積趨于0，即弧PnPn+1可用直線PnPn+1替代[7]。此時(shí)，得坐標(biāo)計(jì)算公式為：

計(jì)算公式C語言的實(shí)現(xiàn)為：

圖5 平面坐標(biāo)系與坐標(biāo)關(guān)系

2.4 上位機(jī)流程設(shè)計(jì)

上位機(jī)主要完成接收數(shù)據(jù)、繪制軌跡、儲存和歷史記錄等工作。上位機(jī)使用Visual Studio軟件制作，具體工作流程是：①系統(tǒng)初始化，創(chuàng)建儲存路徑，讀取上次使用記錄等。②判斷串口藍(lán)牙是否打來。當(dāng)serialPort1.IsOpen==true時(shí)，說明藍(lán)牙連接成功，能與藍(lán)牙從機(jī)通訊。③當(dāng)接收數(shù)據(jù)通過“頭文件”和“負(fù)載校驗(yàn)和”檢驗(yàn)時(shí)，說明接收數(shù)據(jù)完整有效。此時(shí)，取出數(shù)據(jù)，緩存至鏈表OrbitLength和OrbitAngle中，經(jīng)過計(jì)算繪制出軌跡圖案，并將數(shù)據(jù)儲存到指定目錄。

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

本文進(jìn)行了陀螺儀原始積分角度與濾波后積分角度對比、相對誤差統(tǒng)計(jì)、軌跡實(shí)現(xiàn)3次實(shí)驗(yàn)。結(jié)果表明，系統(tǒng)計(jì)算出的坐標(biāo)與實(shí)際測量坐標(biāo)誤差滿足智能車的調(diào)試需求。

3.1 陀螺儀原始積分角度與濾波后積分角度對比

在一次測量中，利用單片機(jī)同時(shí)解算出陀螺儀原始積分角度與濾波后積分角度，此時(shí)2次測量的震動(dòng)環(huán)境相同。由于輸出的角度限幅在0°～360°，所以，轉(zhuǎn)動(dòng)固定圈數(shù)后測量可以簡單讀出兩解算值。對比2個(gè)數(shù)據(jù)與標(biāo)準(zhǔn)角度的誤差，可以判定濾波算法的實(shí)際使用效果。圖6為濾波前后數(shù)據(jù)對比圖，橫坐標(biāo)為轉(zhuǎn)過圈數(shù)，縱坐標(biāo)為誤差角度。統(tǒng)計(jì)分析得，濾波后積分角度函數(shù)表達(dá)式為y＝0.420 9x－0.304 5，直接積分角度函數(shù)表達(dá)式為y＝0.470 9x－0.127 3.其中，x為轉(zhuǎn)過圈數(shù)，y為角度誤差。從圖6中可以看出，經(jīng)濾波后的角度輸出誤差比較小，并且濾波后函數(shù)表達(dá)式的k比較小。此時(shí)，隨著工作時(shí)間的延長，抑制誤差的效果越明顯。結(jié)果表明，通過濾波減小了陀螺儀的誤差。

3.2 相對誤差統(tǒng)計(jì)

定義比例系數(shù)kx和ky，kx＝（解算橫坐標(biāo)）/（測量橫坐標(biāo)），ky＝（解算縱坐標(biāo)）/（測量縱坐標(biāo)）。當(dāng)kx和ky的方差比較小時(shí)，可以將解算坐標(biāo)視為測量坐標(biāo)乘以一定系數(shù)，此時(shí)繪制的軌跡與實(shí)際軌跡近似于等比縮放。本文進(jìn)行了8組實(shí)驗(yàn)，計(jì)算各點(diǎn)kx、ky以及kx、ky的方差。比例系數(shù)測量情況如表1所示。由實(shí)驗(yàn)數(shù)據(jù)可得，舍去誤差較大的點(diǎn)計(jì)算得kx的平均值為2.759 869，方差為0.005 899；ky的平均數(shù)為2.772 692，方差為0.003 745.kx和ky都比較小，且kx≈ky。此時(shí)，解算坐標(biāo)與測量坐標(biāo)可以看作等比縮放。

圖6 濾波前后數(shù)據(jù)對比

3.3 軌跡實(shí)現(xiàn)

在調(diào)試智能車的過程中，使用了大量的上位機(jī)。當(dāng)上位機(jī)將智能車不同參數(shù)下2次行車路線圖繪制在同一界面時(shí)，播放智能車行駛軌跡點(diǎn)，一方面，可以對比智能車路徑的優(yōu)劣；另一方面，可以通過對比軌跡點(diǎn)評判2次行車在某一路段的行車速度。如圖7所示，這些線條分別為2條不同歷史軌跡的對比情況。

表1 比例系數(shù)測量表

圖7 歷史軌跡對比

4 結(jié)論

本文設(shè)計(jì)了一種平面定位系統(tǒng)，分別從下位機(jī)、上位機(jī)入手闡述了角度信息和速度信息的采集、坐標(biāo)的解算、數(shù)據(jù)傳輸、軌跡實(shí)現(xiàn)等功能流程。最后通過角度輸出統(tǒng)計(jì)、相對誤差統(tǒng)計(jì)和軌跡實(shí)現(xiàn)3組實(shí)驗(yàn)驗(yàn)證了系統(tǒng)的有效性和實(shí)用性。該系統(tǒng)主要用于智能車比賽的調(diào)試，通過多次軌跡對比，能有效評判調(diào)試參數(shù)的優(yōu)劣，有助于提高智能車的行駛速度。

［1］李臣龍，強(qiáng)俊.基于STM32和MPU6050姿態(tài)解算的研究和實(shí)現(xiàn)［J］.佳木斯大學(xué)學(xué)報(bào)（自然科學(xué)版），2017（2）：295-296.

［2］張桐銘，劉延飛，趙鵬濤，等.基于PID神經(jīng)網(wǎng)絡(luò)的智能車追逐控制系統(tǒng)研究［J］.科技與創(chuàng)新，2016（15）：13-14.

［3］劉延飛，郭鎖利，侯榮昌.卡爾曼濾波在GPS定位誤差處理中的應(yīng)用［J］.電子科技，2011，24（09）：140-142，148.

［4］許可行，劉延飛，羊帆.基于自適應(yīng)神經(jīng)網(wǎng)絡(luò)的風(fēng)力擺設(shè)計(jì)實(shí)現(xiàn)［J］.科技與創(chuàng)新，2016（12）：6-7.

［5］楊熙，蘇勇，劉屹霄.嵌入式軟件設(shè)計(jì)入門與進(jìn)階：基于Kinetis K60/K64［M］.北京：清華大學(xué)出版社，2017.

［6］王日陽，廖錦松，申柏華.輕松玩轉(zhuǎn)ARM CORTEX-M4微控制器：基于Kinetis K60［M］.北京：北京航空航天大學(xué)出版社，2014.

［7］明日科技.C#從入門到精通［M］.附光盤第3版.北京：清華大學(xué)出版社，2012.