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

（火箭軍工程大學，陜西 西安 710025）

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

1 系統(tǒng)整體結構

1.1 系統(tǒng)總體設計

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

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

1.2 MPU6050模塊

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

圖2 GY-521電路原理圖

1.3 微處理模塊

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

1.4 速度采集模塊

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

2 程序設計

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

2.1 下位機流程設計

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

圖3 下位機數據采集與處理流程圖

2.2 陀螺儀轉角信息獲取程序

由于MPU6050與車體捷聯，令MPU6050坐標系與圖車體坐標系平行。此時，需要采集的角度為Z軸方向轉角。

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

圖4 下位機數據采集與處理流程圖

2.3 坐標計算程序

如圖5所示，Pn與Pn+1為相鄰的2個點，Length為速度采集值，θ為MPU6050 Z軸相對于初始點轉過的角度?；nPn+1表示實際走過的軌跡。當采集周期趨于0時，弧線與直線圍成的圖形面積趨于0，即弧PnPn+1可用直線PnPn+1替代[7]。此時，得坐標計算公式為：

計算公式C語言的實現為：

圖5 平面坐標系與坐標關系

2.4 上位機流程設計

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

3 實驗結果與分析

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

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

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

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

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

圖6 濾波前后數據對比

3.3 軌跡實現

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

表1 比例系數測量表

圖7 歷史軌跡對比

4 結論

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

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

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

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

［4］許可行，劉延飛，羊帆.基于自適應神經網絡的風力擺設計實現［J］.科技與創(chuàng)新，2016（12）：6-7.

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

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

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