基于ARM的整平機定位系統的研制

朱 楠， 黃 歡， 金 鑫， 秦海鵬

(江蘇師范大學 電氣工程及自動化學院， 江蘇 徐州 221000)

引言

整平機是實現混凝土平整的一類設備。隨著國內大型建筑數量的不斷增加以及裝配式建筑的興起，整平機在現代工廠、物流倉庫、商業地坪以及裝配式建筑等市場的使用量呈現大幅度上升的趨勢[1]。激光整平機是目前市面上應用較為廣泛，技術相對成熟的一類整平機，主要由傾角傳感器、激光收發器、整平機控制器、制動裝置、電源以及平地鏟組成。首先，激光發射器產生一個水平的激光掃射平面，2個激光接收器同時接收光信號，調平桿上搭載傾角傳感器，傾角傳感器和激光接收器的信號通過總線發送給控制器，由控制器作出決策，通過制動裝置控制平地鏟，驅動平地鏟直至前后、左右調平[2]。

自動航向控制和精確定位是整平機控制系統中的關鍵技術。獲取整平機的偏航信息不僅可以提高整平效率，而且可以得到2個驅動輪的轉向角，從而達到差速控制的目的要求。目前，胡宏秋等人采用GPS系統在主控器CPU224XP上支持設計了整平機的遠程控制與定位[3]，但是無法避免GPS在惡劣環境下信號易失鎖的問題。遲德霞基于視覺導航系統對水稻插秧機的路徑進行了追蹤[4]，但是對硬件要求較高。綜上，本文提出了一種基于STM32的整平機定位系統的設計方案，選擇GPS/INS組合導航作為定位方式，首先對導航算法進行仿真，然后將仿真代碼用C語言研發調試后展開實測。實驗表明，該定位系統可實現整平機的精確定位。

1 系統整體結構

1.1 設計方案

首先給出整平機定位系統的設計方案。將慣性傳感器和GPS模塊放置在平地鏟的上方，輸出的數據通過串口發送給STM32進行導航解算。磁力計輸出的數據通過I2C接口發送給STM32，與慣性傳感器進行數據融合后計算出整平機的航向角，最后將結果通過串口發送給整平機主控制器。設計方案如圖1所示。

圖1 定位系統設計方案Fig. 1 Schematic illustration of the positioning system

1.2 元器件的選型

本文選擇GPS/INS組合導航作為整平機的定位方式。由于組合導航的狀態方程是高階時變的，為了保證運行速度，必須采用高性能處理器[5]。因此，本文選擇STM32F103TB作為控制器，就因其具備著數據處理能力強、資源豐富、性價比高等優點。是32位市場上功耗最低的產品，含有2個I2C接口以及USART接口。

由于整平機在工作過程中平地鏟的振動較大，選擇慣性傳感器時應將精度和抗震性能同時考慮在內。本文選擇北微傳感的工業級高精度九軸航姿參考系統BW-AH200。其中，采用高質量的MEMS加速度計和陀螺儀，嚴格的密封設計保證在惡劣環境下仍可以準確給出平地鏟角速度以及加速度的信息。

磁力計選擇霍尼韋爾磁阻式三軸地磁傳感器HMC5983L，其內置復位和偏置驅動器可以實現自測和偏移補償，羅盤精度高，磁場范圍廣。可以通過I2C總線接口輸出載體偏離北向的角度[6]。

GPS模塊選用高精度的瑞士u-blox MAX-6Q GPS芯片,體積小、功耗低。含有50個通道的u-blox引擎，成本較低，符合本系統的要求。

2 硬件設計

2.1 GPS接口電路

本文將GPS模塊輸出的數據通過串口發送給STM32，該模塊內建GPS有源天線以提供27 DB的增益。當需要將天線外置時,可以將天線板與模塊分離。同時，可以使用延長線或者轉換線連接別的GPS天線。GPS默認的波特率是9600 bps,在上電初始化的過程中，將波特率修改為115200,從而滿足最高5次/s的帶寬更新要求。原理設計如圖2所示。

2.2 磁力計接口電路

為了得到整平機的航向角，本文將HMC5983L與BW-AH200的輸出信號進行數據融合。由于STM32F103TB和HMC5983L磁力計都含有I2C接口，因此本文將磁力計提供的數據通過I2C發送給控制器，具體運行原理如圖3所示。其中，AUX_SDA和AUX_SCL用來傳輸I2C信號。

圖2 GPS接口電路Fig. 2 GPS interface circuit

圖3 磁力計接口電路Fig. 3 Magnetometer interface circuit

3 算法設計與實現

3.1 組合導航算法仿真

本文通過Matlab對組合導航算法進行仿真，組合模式選擇松組合。首先利用卡爾曼濾波對GPS、INS各自測得的速度和位置信息進行融合，量測結果為2種導航系統各自輸出導航參數的差值。然后對慣導系統的誤差進行預測，并利用預測后的最優值對慣導系統進行校正。校正方式一般分為反饋校正和輸出校正。其中，輸出校正對軟硬件的要求不高，便于在工程上創建運行[7]。因此，本次仿真中濾波器選擇輸出校正方式，濾波初始值見表1所示。

表1中，Tg、Ta分別表示加速度計和陀螺儀的白噪聲激勵時間。仿真時間為500 s，速度、位置誤差在3個方向的仿真結果如圖4所示。其中，速度誤差被限定在0.5 m/s以內，位置誤差被限定在5 m/s以內。

3.2 組合導航算法實現

這里，將本文算法通過C語言在STM32F103TB上展開實現，編譯環境為MDK-Lite Version: 4.23。將BW-AH200和GPS輸出的數據發送給STM32，速度、位置信息通過串口發送給整平機的主控制器。軟件設計流程如圖5所示。

表1 濾波初始值設置Tab. 1 Initial values of the integrated navigation algorithm

(a) 濾波后的速度誤差(a) Velocity error after filtering

(b) 濾波后的位置誤差(b) Position error after filtering圖4 導航算法仿真圖Fig. 4 Simulation chart of navigation algorithm

圖5中，初始化包括初始姿態角、初始速度和位置、狀態誤差方差陣、量測噪聲方差陣以及初始誤差方差陣等參數的設置[8]。在實際的工程應用中，將慣性元件采集到的數據在調取捷聯解算前還需進行誤差補償和初始對準。其中，誤差補償包括圓錐補償和劃槳補償。初始對準為靜基座初始對準[9]。本文并未將誤差補償和初始對準考慮在內。

圖5 組合導航算法軟件流程圖Fig. 5 Flow chart of integrated navigation algorithm

4 實驗結果分析

本次研究設計推出了一個激光整平機的可移動實驗平臺，該實驗平臺由底座、刮平板、直流24 V穩壓電源和5個電推桿集結構成。通過電推桿可實現模擬路況和刮平板的前后、左右調平。實驗平臺的外觀結構則如圖6所示。

圖6 整平機實驗平臺Fig. 6 Experimental platform of the leveling machine

由于本文沒有將誤差補償和初始對準考慮在內，因此僅將整平機在室外運動30 s以驗證系統的可靠性，位置誤差如圖7所示。

由圖7可知，實測結果與Matlab的仿真結果呈現出相同的走勢，在3個方向上的位置誤差都在5 m左右，而組合導航的定位精度為0～5 m，GPS的定位

誤差為10～15 m[10]。因此，本文設計提出的整平機定位系統在實際應用中的精度是可靠、有效的，在30 s的時間內位置誤差始終在零刻度線上下波動。

圖7 位置誤差曲線圖Fig. 7 Experimental curve of position errors

5 結束語

本文基于STM32設計開發了一套整平機定位系統，利用卡爾曼濾波對其速度、位置進行估計，實現了速度控制和精確定位,實驗結果表明系統是可靠的。本文沒有考慮時間漂移、溫度漂移對算法的影響，未來將對這些問題展開后續深入研究。

[1] 艾建軍, 楊英蕓, 高韶坤. 我國農田激光平地機的研究進展及發展趨勢[J]. 東北農業大學學報，2013，44(11):155-160.

[2] 楊偉偉. 水田激光平地機控制系統優化設計[D]. 廣州：華南農業大學，2016.

[3] 胡宏秋，張中堯. 平地機遠程控制和定位系統設計[J]. 工程機械，2008，39(2)：40-43.

[4] 遲德霞. 水稻插秧機視覺導航系統設計與路徑追蹤試驗[D]. 沈陽：沈陽農業大學，2015.

[5] 劉長龍. 基于STM32的高速列車組合導航系統的研究[D]. 成都：西南交通大學，2012.

[6] 李臣龍，強俊. 基于STM32和MPU6050姿態解算的研究與實現[J]. 佳木斯大學學報(自然科學版)，2017，35(2)：295-298，316.

[7] 李倩. GPS/INS組合導航系統研究及實現[D]. 上海：上海交通大學，2010.

[8] 王惠南. GPS導航原理與應用[M]. 北京：科學出版社，2003.

[9] 鄧正隆. 慣性技術[M]. 哈爾濱：哈爾濱工業大學出版社，2006.

[10]KAPLAN E D，HEGARTY C J. GPS原理與應用[M]. 2版. 寇艷紅，譯. 北京：電子工業出版社，2007.