秦海鵬 張帥 黃歡 曹競(jìng)豪

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

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

中圖分類號(hào)： TN876?34; TP202? ? ? ? ? ? ? ? ? ?文獻(xiàn)標(biāo)識(shí)碼： A? ? ? ? ? ? ? ? ? ? ? 文章編號(hào)： 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? 引? 言

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

1? 整平機(jī)工作原理

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

2? 總體設(shè)計(jì)

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

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

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

2.2? 整平機(jī)控制器

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

2.3? 驅(qū)動(dòng)模塊

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

3? 算法設(shè)計(jì)

3.1? 姿態(tài)解算

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

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

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

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

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

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

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

3.2? 增量式PID控制算法

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

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

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

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

4? 實(shí)? 驗(yàn)

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

4.1? 姿態(tài)解算實(shí)驗(yàn)

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

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

4.2? PID控制實(shí)驗(yàn)

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

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

當(dāng)[KI]=300時(shí)，姿態(tài)角進(jìn)入穩(wěn)定狀態(tài)所需時(shí)間為7 s左右;當(dāng)[KI]=600時(shí)，姿態(tài)角進(jìn)入穩(wěn)定狀態(tài)所需時(shí)間為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è)計(jì)了一套激光整平機(jī)水平控制系統(tǒng)，利用擴(kuò)展卡爾曼濾波實(shí)現(xiàn)了慣性傳感器的數(shù)據(jù)融合，完成了刮平板調(diào)平實(shí)驗(yàn)，并比對(duì)了不同參數(shù)下PID的控制效果，控制系統(tǒng)可以穩(wěn)定運(yùn)行。為了實(shí)現(xiàn)精度更高的動(dòng)態(tài)響應(yīng)，在以后的研究中將采用其他精度更高、抗干擾能力更強(qiáng)的角度傳感器，利用非線性、自適應(yīng)等控制算法實(shí)現(xiàn)高精度控制。

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

參考文獻(xiàn)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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