基于九軸姿態傳感器的智能汽車平面定位模塊設計與實踐

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

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

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

1 系統整體結構

1.1 系統總體設計

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

圖1 系統總體框圖

1.2 MPU6050模塊

通過平面姿態角測量模塊測量平面姿態角，與速度進行計算，從而實時解算出車輛相對于初始位置（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 程序設計

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

2.1 下位機流程設計

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

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

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

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

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

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

2.3 坐標計算程序

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

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

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

2.4 上位機流程設計

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

3 實驗結果與分析

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

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

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

3.2 相對誤差統計

定義比例系數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次行車路線圖繪制在同一界面時，播放智能車行駛軌跡點，一方面，可以對比智能車路徑的優劣；另一方面，可以通過對比軌跡點評判2次行車在某一路段的行車速度。如圖7所示，這些線條分別為2條不同歷史軌跡的對比情況。

表1 比例系數測量表

圖7 歷史軌跡對比

4 結論

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

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

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

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

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

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

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

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