秦海鵬 張帥 黃歡 曹競豪

摘? 要： 研究一種基于STM32的激光整平機控制系統(tǒng)的設(shè)計方案。該系統(tǒng)由STM32微處理器、慣性傳感器（MPU6050）、電推桿驅(qū)動器等部分組成。以STM32F103處理器作為姿態(tài)解算單元，以STM32F407處理器作為控制單元，兩者之間通過RS 232接口進行通信。采用擴展卡爾曼濾波算法將MEMS陀螺儀、加速度計和磁力計的測量數(shù)據(jù)進行融合，利用四元數(shù)坐標(biāo)變換解算出所需的姿態(tài)角。搭建一套整平機實驗平臺用來驗證方法的有效性，利用PID算法實現(xiàn)整平機水平傾角的調(diào)平控制。實驗結(jié)果顯示，控制系統(tǒng)可以穩(wěn)定運行，姿態(tài)解算偏差不超過0.5°，水平控制時超調(diào)量較小，調(diào)節(jié)時間在2 s以內(nèi)。綜上所述，該系統(tǒng)測量精度高，控制穩(wěn)定性好，能夠較好地實現(xiàn)整平機的水平控制，因此，該系統(tǒng)可以提高整平精度，降低整機成本，具有很高的實用價值。

關(guān)鍵詞： 激光整平機; 混凝土整平; 數(shù)據(jù)融合; 調(diào)平控制; 控制系統(tǒng)設(shè)計; 有效性驗證

中圖分類號： TN876?34; TP202? ? ? ? ? ? ? ? ? ?文獻標(biāo)識碼： A? ? ? ? ? ? ? ? ? ? ? 文章編號： 1004?373X（2020）03?0150?04

Design of concrete laser leveling machine horizontal control system based on STM32

QIN Haipeng， ZHANG Shuai， HUANG Huan， CAO Jinghao

（School of Electrical Engineering & Automation， Jiangsu Normal University， Xuzhou 221000， China）

Abstract： A designing scheme of laser leveling control system based on STM32 is proposed. The system consists of the STM32 microprocessor， the inertial sensor （MPU6050） and the electric pushrod driver. The STM32F103 processor is used as the attitude calculation unit， and the STM32F407 processor as the control unit of the system. The two units communicate through the RS 232 interface. The measured data of MEMS gyroscope， accelerometer and magnetometer are fused by the extended Kalman filtering algorithm， and the required attitude angle is solved with the quaternion coordinate transformation. A series of leveling machine experimental platforms are built to verify the effectiveness of the method. The leveling control of horizontal angle of the leveling machine is realized by the PID （proportion?integration?differentiation） algorithm. The experimental results show that the control system can operate stably， the attitude solution deviation does not exceed 0.5°， the overshoot for horizontal control is relatively small and the adjusting time is within 2 s. In summary， the measurement accuracy of the system is high， the control is stable， and the system can better realize horizontal control of the leveling machine. Therefore， the system can improve the leveling accuracy and reduce the cost of complete machine， thus it has high practical value.

Keywords： laser leveling machine; concrete leveling; data fusion; leveling control; control system design; effectiveness verification

0? 引? 言

平地鏟的水平控制是整平機控制系統(tǒng)中的關(guān)鍵技術(shù)，角度傳感器為水平控制系統(tǒng)提供平地鏟當(dāng)前的水平傾角，而高精度的姿態(tài)角信號是保障系統(tǒng)性能的前提?；谥亓[和阻尼油結(jié)構(gòu)的傾角傳感器由于測量誤差較大，無法保證系統(tǒng)的精度[1]。隨著微機電系統(tǒng)和數(shù)據(jù)融合算法的發(fā)展，MEMS慣性傳感器由于體積小、成本低、抗干擾能力強、數(shù)據(jù)可靠性高等優(yōu)點，被廣泛應(yīng)用于慣導(dǎo)、航姿領(lǐng)域[2]。陀螺儀和加速度計在積分時間漂移和測量噪聲的影響下會產(chǎn)生較大誤差[3?4]。為了提高整平后的平整度和水平度，本文通過擴展卡爾曼濾波融合MEMS陀螺儀、加速度計和磁力計的數(shù)據(jù)，從而得到姿態(tài)角的估計值。同時，隨著整平機應(yīng)用領(lǐng)域越來越廣泛，需要實現(xiàn)較強的人機交互功能，而傳統(tǒng)的8/16位處理器無法滿足要求，因此本文采用STM32系列處理器作為姿態(tài)解算模塊和控制模塊。為了模擬整平機在實際運行過程中的各種干擾，搭建一套實驗平臺并在其上完成了控制算法的設(shè)計及其相關(guān)試驗。

1? 整平機工作原理

激光整平機機械結(jié)構(gòu)如圖1所示，傳統(tǒng)的激光整平機有兩個激光接收器[5]，由于激光接收器的價格比慣性傳感器昂貴，所以本設(shè)計創(chuàng)新性地采用慣性傳感器替代一側(cè)激光接收器的方案。整平機機械結(jié)構(gòu)主要由刮平板、平地鏟、激光接收器、慣性傳感器、水平調(diào)節(jié)推桿、高程調(diào)節(jié)推桿及拖拉機組成。水平調(diào)節(jié)推桿通過四聯(lián)機構(gòu)連接于刮平板，高程調(diào)節(jié)桿直接連接于機身，慣性傳感器固定于刮平板一側(cè)，刮平板與平地鏟通過連接機構(gòu)相連。高程調(diào)節(jié)推桿一般采用手動方式根據(jù)水泥厚度實現(xiàn)粗調(diào)。平地鏟整平作業(yè)時會前后、左右傾斜，因此激光整平機自動調(diào)節(jié)分為兩種方式：俯仰角控制和左右水平控制。整平機水平調(diào)節(jié)時，水平調(diào)節(jié)推桿通過激光偏差信號實現(xiàn)高程精調(diào)，并利用慣性傳感器使得平地鏟維持在水平位置[6]。

2? 總體設(shè)計

通過比較各種整平機控制系統(tǒng)的性能優(yōu)劣，本文中整平機水平控制系統(tǒng)結(jié)構(gòu)如圖2所示，主要由慣性傳感器、姿態(tài)解算控制器、整平機控制器、平地鏟組成。其中，姿態(tài)解算控制器通過I2C總線讀取陀螺儀、加速度計和磁力計的數(shù)據(jù)，通過擴展卡爾曼濾波算法進行數(shù)據(jù)融合，為整平機水平控制系統(tǒng)提供實時傾角測量。執(zhí)行機構(gòu)為電推桿，采用H橋電路作為電機驅(qū)動模塊，通過PWM信號控制。

2.1? 姿態(tài)解算模塊

角度測量模塊由MEMS陀螺儀、加速度計和磁力計組成[7]，本系統(tǒng)選擇MPU6050作為測量水平傾角的慣性傳感器。MPU6050是用于慣導(dǎo)、航姿領(lǐng)域的整合性6軸處理模塊[8]。該芯片具有I2C通信接口，用于數(shù)據(jù)交互和外接磁力計。磁力計的型號為HMC5983，其內(nèi)置三軸磁阻傳感器，能在±8高斯的磁場中實現(xiàn)2毫高斯的分辨率。提供I2C通信接口和12位ADC轉(zhuǎn)換器。此外，它具有偏置和溫度補償功能，抗干擾能力強。選擇意法半導(dǎo)體公司（ST）的以ARM Cortex?M3為內(nèi)核的STM32F103TB作為姿態(tài)解算處理器，它是市場上32位處理器中功耗較低的產(chǎn)品，含有兩個I2C接口及USART接口[9]，符合本系統(tǒng)的要求。

2.2? 整平機控制器

整平機控制器以STM32F407為核心，該芯片是基于ARM的32位Cortex?M4內(nèi)核的通用MCU，具有低成本、低功耗、高性能的特點。STM32F407接口資源豐富，可擴展性強，工作頻率最高可達168 MHz，具有哈佛總線結(jié)構(gòu)[10]。它不僅能提供高速的2通道12位ADC轉(zhuǎn)換器，還能提供CAN，I2C，SPI，USART等總線，符合本系統(tǒng)的要求。

2.3? 驅(qū)動模塊

H橋是常用的直流電機驅(qū)動電路，主要由4個開關(guān)管和1個直流電機組成[11]。根據(jù)不同對角線上2個開關(guān)管的導(dǎo)通情況，電流能夠雙向通過直流電機，從而實現(xiàn)電機轉(zhuǎn)向的切換。開關(guān)管的導(dǎo)通和關(guān)斷時長由PWM波的占空比決定，通過調(diào)節(jié)占空比可以控制電機的轉(zhuǎn)速。為了防止電機轉(zhuǎn)向切換時過大反向電動勢損壞開關(guān)管的情況，采用4個反并聯(lián)二極管進行保護。

3? 算法設(shè)計

3.1? 姿態(tài)解算

四元數(shù)[q（t）]滿足以下微分方程 ：

通過陀螺儀的測量數(shù)據(jù)可以獲得系數(shù)矩陣[Ωb]：

式中[gx]，[gy]，[gz]為陀螺儀輸出的角速度。為了補償陀螺儀的漂移誤差，一般式（1）為狀態(tài)方程，以加速度計和磁力計的輸出為觀測量，利用卡爾曼濾波融合三種傳感器的數(shù)據(jù)[12]。

令導(dǎo)航坐標(biāo)系下的重力向量和磁場向量分別為[00gT]和[bx0bzT]。其中，[g]為重力加速度，[bx]，[bz]為當(dāng)?shù)卮艌鰪姸?。則：

式中：[Cnb]為載體坐標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)系的姿態(tài)矩陣，其元素可以表示為四元數(shù)的函數(shù);[Cnb*]為姿態(tài)矩陣[Cnb]的轉(zhuǎn)置;[ax]，[ay]，[az]是計算得到的加速度在三軸上的分量;[mx]，[my]，[mz]是計算得到的磁場強度在三軸上的分量。設(shè)加速度計的實際輸出為[a1]，[a2]，[a3]，磁力計的實際輸出為[m1]，[m2]，[m3]，則觀測誤差為[T]：

利用觀測誤差補償四元數(shù)：

式中：[q-]為補償前的狀態(tài)變量;[q]為補償后的狀態(tài)變量;[K]為卡爾曼增益矩陣。最終輸出的姿態(tài)角可由更新后的四元數(shù)得到[12]。

3.2? 增量式PID控制算法

數(shù)字PID控制算法有兩種表達式，其中，位置式PID的輸出量由此刻設(shè)定值與當(dāng)前值的偏差、歷史累計偏差之和及最近兩次偏差之差獲得[13?14]。設(shè)位置式PID的第[k]次輸出為[Uk]。

式中：[KP]為比例系數(shù);[KI]為積分系數(shù);[KD]為微分系數(shù);[Ek]為第[k]時刻設(shè)定值與當(dāng)前值的偏差。

由于位置式PID計算了歷史累計偏差之和，容易產(chǎn)生較大的累計誤差。而增量式PID輸出量的大小只與最近三次的偏差有關(guān)，是一種遞推式的算法，計算誤差對控制量的影響很小，因此增量式PID的應(yīng)用更加廣泛。增量式PID的第[k]次輸出記為[ΔUk]：

參數(shù)整定時，先整定參數(shù)[KI]，其大小影響了系統(tǒng)的響應(yīng)速度，再整定參數(shù)[KP]，其大小反映了系統(tǒng)對超調(diào)的限制。

4? 實? 驗

開發(fā)環(huán)境選擇MDK5，在自主研發(fā)的整平機實驗平臺上進行姿態(tài)解算實驗和PID控制實驗。

4.1? 姿態(tài)解算實驗

將姿態(tài)解算模塊的輸出與工業(yè)級航姿參考系統(tǒng)（北微傳感BW?AH200）的輸出進行對比，BW?AH200采用高質(zhì)量的MEMS加速度計、陀螺儀和磁力計，嚴格的密封設(shè)計保證在惡劣環(huán)境下仍可以準(zhǔn)確測量載體的角速度、加速度和姿態(tài)等信息。姿態(tài)角分辨率達0.01°，誤差精度在1°以內(nèi)。實驗過程中，每隔5 s將兩者同時旋轉(zhuǎn)一定角度。多次記錄測量數(shù)據(jù)，實驗結(jié)果如圖3所示。

圖3顯示，BW?AH200的測量誤差一般不超過0.2°，而MPU6050測量的姿態(tài)角度與實際角度的偏差最大為2.8°，其余都在2.2°以內(nèi)。整平機實際工作時，平地鏟水平傾角變化范圍一般在-5°～5°，由圖3可知，偏差最大不超過0.25°，對于2 m的平地鏟來說，高度調(diào)節(jié)誤差可控制在0.4 cm以內(nèi)，滿足實際要求。

4.2? PID控制實驗

本課題組設(shè)計了一套整平機控制系統(tǒng)實驗平臺。采用模擬路況推桿來模擬整平機在實際工作狀態(tài)下平地鏟的水平傾角擾動。實驗平臺如圖4所示。

圖4中，1為俯仰角調(diào)節(jié)電動推桿，2為模擬路況電動推桿，3為左右水平調(diào)節(jié)電動推桿（2個），4為24 V直流穩(wěn)壓電源。對刮平板進行水平控制實驗，為了測試不同參數(shù)下的水平控制效果，在給定階躍擾動下觀察刮平板的姿態(tài)角變化。首先調(diào)節(jié)參數(shù)[KI]，系統(tǒng)的響應(yīng)如圖5，圖6所示。

當(dāng)[KI]=300時，姿態(tài)角進入穩(wěn)定狀態(tài)所需時間為7 s左右;當(dāng)[KI]=600時，姿態(tài)角進入穩(wěn)定狀態(tài)所需時間為3.2 s左右。由圖5，圖6可知，[KI]越大，系統(tǒng)響應(yīng)速度越快。

調(diào)節(jié)[KP]以減小控制過程中存在的超調(diào)，系統(tǒng)響應(yīng)曲線如圖7所示，參數(shù)[KP]=500，[KI]=600。由圖6，圖7可知，超調(diào)量降低了約0.4°。

5? 結(jié)? 論

本文基于STM32設(shè)計了一套激光整平機水平控制系統(tǒng)，利用擴展卡爾曼濾波實現(xiàn)了慣性傳感器的數(shù)據(jù)融合，完成了刮平板調(diào)平實驗，并比對了不同參數(shù)下PID的控制效果，控制系統(tǒng)可以穩(wěn)定運行。為了實現(xiàn)精度更高的動態(tài)響應(yīng)，在以后的研究中將采用其他精度更高、抗干擾能力更強的角度傳感器，利用非線性、自適應(yīng)等控制算法實現(xiàn)高精度控制。

注：本文通訊作者為黃歡。

參考文獻

[1] 李慶，羅錫文，汪懋華，等.采用傾角傳感器的水田激光平地機設(shè)計[J].農(nóng)業(yè)工程學(xué)報，2007，23（4）：88?93.

[2] 李榮冰，劉建業(yè)，曾慶化，等.基于MEMS技術(shù)的微型慣性導(dǎo)航系統(tǒng)的發(fā)展現(xiàn)狀[J].中國慣性技術(shù)學(xué)報，2004，12（6）：88?94.

[3] 趙祚喜，羅錫文，李慶，等.基于MEMS慣性傳感器融合的水田激光平地機水平控制系統(tǒng)[J].農(nóng)業(yè)工程學(xué)報，2008，24（6）：119?124.

[4] 黎永鍵，趙祚喜.水田激光平地機平地鏟姿態(tài)測量系統(tǒng)的設(shè)計[J].農(nóng)機化研究，2012，34（2）：69?75.

[5] 侯明亮，毛恩榮，劉剛.激光控制平地系統(tǒng)控制技術(shù)的研究與試驗[J].農(nóng)業(yè)工程學(xué)報，2006，22（1）：110?113.

[6] 胡煉，林潮興，羅錫文，等.農(nóng)機具自動調(diào)平控制系統(tǒng)設(shè)計與試驗[J].農(nóng)業(yè)工程學(xué)報，2015，31（8）：15?20.

[7] 林建涵.激光控制平地系統(tǒng)接收和控制裝置的研究與開發(fā)[D].北京：中國農(nóng)業(yè)大學(xué)，2004.

[8] 李臣龍，強俊.基于STM32和MPU6050姿態(tài)解算的研究與實現(xiàn)[J].佳木斯大學(xué)學(xué)報，2017，35（2）：295?298.

[9] 潘春榮，許化.基于STM32的X型四旋翼無人機設(shè)計[J].工程設(shè)計學(xué)報，2017，24（2）：196?202.

[10] 黃琦，韓廣源，吳瑞東，等.基于STM32的高精度恒溫控制系統(tǒng)設(shè)計[J].儀表技術(shù)與傳感器，2017（5）：71?74.

[11] 胡發(fā)煥，邱小童，蔡咸健.基于場效應(yīng)管的大功率直流電機驅(qū)動電路設(shè)計[J].電機控制與應(yīng)用技術(shù)，2011，38（4）：21?24.

[12] 苑艷華，李四海，南江.基于卡爾曼濾波器的航姿系統(tǒng)測姿算法研究[J].傳感技術(shù)學(xué)報，2011，24（12）：1718?1722.

[13] 羅天資，陳衛(wèi)兵，鄒豪杰，等.直線電機模糊增量PID控制算法的研究[J].測控技術(shù)，2011，30（2）：56?59.

[14] 李迎，孫亞飛.基于增量PID的壓電微位移器驅(qū)動控制系統(tǒng)開發(fā)[J].測控技術(shù)，2011，30（3）：40?44.

[15] 陳孟元，謝義建，陳躍東.基于四元數(shù)改進型互補濾波的MEMS姿態(tài)解算[J].電子測量與儀器學(xué)報，2015，29（9）：1391?1397.