高研 李昕宇 史穎剛 劉利

摘 ? 要：文章設(shè)計了一種面向中國機器人大賽的果園噴藥機器人，根據(jù)競賽要求，結(jié)合平面四桿機構(gòu)的工作原理，設(shè)計了機器人的機械系統(tǒng);以STM32F103ZET6單片機為控制核心，通過搭載電機驅(qū)動器、循跡傳感器、平面定位系統(tǒng)以及噴藥系統(tǒng)，設(shè)計了競賽機器人的控制系統(tǒng)，其中，以步進(jìn)電機控制器、絲杠滑臺、電磁繼電器、電磁閥、增壓泵等構(gòu)建了機器人噴藥系統(tǒng)。最后，設(shè)計了控制算法，實現(xiàn)了果園噴藥機器人的競賽功能。

關(guān)鍵詞：機器人競賽;果園噴藥;自主尋跡;平面定位系統(tǒng);單片機控制

1 ? ?果園噴藥機器人競賽相關(guān)簡介

果園噴藥機器人競賽是中國機器人大賽的子項目，比賽場地如圖1所示。比賽時，機器人從起始區(qū)出發(fā)，經(jīng)A，B，C，D四區(qū)，對不同類型的靶標(biāo)完成自主對靶噴藥，并在完成所有作業(yè)任務(wù)后返回起始區(qū)。

A區(qū)為對靶噴藥，靶標(biāo)豎直方向上均勻分布3個同心圓，有白色循跡線和作業(yè)點提示線。B區(qū)為固定樹形噴藥，樹形為三角形、豎橢圓形和平橢圓形，有白色循跡線，無作業(yè)點提示線。C區(qū)為不同樹齡的樹形噴藥，樹形和B區(qū)一致，大小分別為B區(qū)樹形的90%和80%，有作業(yè)點提示線，無循跡線。D區(qū)有3個模擬樹形，大小為B區(qū)樹形的70%，無循跡線，靶標(biāo)位置隨機。

根據(jù)競賽要求，為實現(xiàn)精準(zhǔn)噴藥，機器人需自主移動到作業(yè)點，并對靶標(biāo)指定區(qū)域噴藥。因此，機器人應(yīng)包括基本運動模塊、循跡模塊、平面定位系統(tǒng)、噴藥執(zhí)行機構(gòu)等部分，需從機械結(jié)構(gòu)、控制系統(tǒng)和軟件設(shè)計3方面進(jìn)行研究。

2 ? ?機械結(jié)構(gòu)設(shè)計

機器人車體為鋁合金板材，4個橡膠車輪對稱分布在兩側(cè);底層前端安裝4路循跡傳感器，引導(dǎo)機器人運動，左右各裝一個光電開關(guān)，檢測靶標(biāo)位置[1];上層考慮防水要求，將單片機、直流電機驅(qū)動器、穩(wěn)壓模塊、電子羅盤等電子元件裝在前側(cè)，增壓泵、水箱、電磁閥等涉水元件裝在后側(cè)，噴藥執(zhí)行機構(gòu)固定在中央。整體結(jié)構(gòu)如圖2所示。

噴藥執(zhí)行機構(gòu)主要由滾珠絲杠副、平面四桿機構(gòu)及噴板噴頭組成。滾珠絲杠副固定在車體中央，平面四桿機構(gòu)安裝在絲杠螺母上，噴板噴頭通過L形連接件與平面四桿機構(gòu)相連[2]。通過控制步進(jìn)電機及舵機的轉(zhuǎn)動可使平面四桿機構(gòu)上下移動、左右伸縮，滿足不同噴藥要求，在調(diào)節(jié)噴頭與噴靶距離的同時，保證噴頭始終與噴靶垂直。

針對精準(zhǔn)噴藥的要求，將不同樹形靶標(biāo)圖紙重疊，再重合部分合理設(shè)置噴頭數(shù)量及位置，以適應(yīng)普遍噴藥要求。在保證有效噴藥的前提下，減少噴頭數(shù)量，縮小絲杠移動范圍。以B區(qū)靶標(biāo)為例，其噴頭作業(yè)效果如圖3所示。圖4為噴藥機器人實物。

3 ? ?控制系統(tǒng)設(shè)計

果園噴藥競賽機器人以STM32F103ZET6單片機為主控制核心，控制系統(tǒng)架構(gòu)如圖5所示。

機器人工作時，主控制器通過向直流電機驅(qū)動器發(fā)送可調(diào)脈沖寬度調(diào)制（Pulse Width Modulation，PWM）波，驅(qū)動電機轉(zhuǎn)動，控制電機轉(zhuǎn)速;通過控制各直流電機驅(qū)動器轉(zhuǎn)向邏輯電平，調(diào)節(jié)電機轉(zhuǎn)向，實現(xiàn)機器人進(jìn)退、轉(zhuǎn)向等基本運動。比賽過程中，有循跡線時，通過4路循跡傳感器輸出邏輯電平的變化，引導(dǎo)機器人循線運動。無循跡線時，由電子羅盤MPU9250和直流電機上搭載的霍爾編碼器實時傳輸機器人的姿態(tài)、速度和位移信息，引導(dǎo)機器人到達(dá)指定位置。根據(jù)光電開關(guān)電平變化，判斷機器人是否到達(dá)噴藥作業(yè)點[3]。

到達(dá)靶標(biāo)位置后，主控制器根據(jù)預(yù)定的作業(yè)位置，驅(qū)動噴藥平臺移動，并進(jìn)行藥液噴射。主控制器通過步進(jìn)電器驅(qū)動器控制步進(jìn)電機轉(zhuǎn)動，帶動絲杠螺母升降，以調(diào)整噴藥機構(gòu)高度;通過控制舵機轉(zhuǎn)動，實現(xiàn)噴頭伸縮，以貼近噴藥位置;到達(dá)目標(biāo)位置后，主控制器通過直流電機驅(qū)動器V調(diào)節(jié)增壓泵壓力和流量，并通過電磁繼電器控制相應(yīng)電磁閥閥口的開閉，實現(xiàn)多噴頭獨立控制，完成對靶標(biāo)指定區(qū)域的精準(zhǔn)噴藥。

在整個過程中，語音模塊實時播報作業(yè)任務(wù)完成情況。

4 ? ?軟件系統(tǒng)設(shè)計

4.1 ?機器人基本運動程序設(shè)計

機器人采用4輪獨立驅(qū)動、差速控制，通過控制各直流電機的運轉(zhuǎn)狀態(tài)，實現(xiàn)車體的基本運動。在運動過程中，為減少車體重心偏移及摩擦力不同引起的速度誤差，保證機器人沿直線快速穩(wěn)定運動，采用比例—積分—微分（Proportion-Integral-Differential，PID）控制算法調(diào)節(jié)車輪轉(zhuǎn)速。其基本原理為：由主控制器設(shè)定各車輪的給定轉(zhuǎn)速，并對霍爾編碼器返回脈沖進(jìn)行定時采樣，利用采樣接收結(jié)果計算各車輪的實際轉(zhuǎn)速，并以此作為反饋信號進(jìn)行偏差調(diào)節(jié)。

PID控制算法的數(shù)學(xué)模型如下：

OUT=KP×EK+KI+SK+KD×KD×DK+OUT0

其中，KP，KI，KD分別為比例常數(shù)、積分常數(shù)、微分常數(shù)，EK表示第K次編碼器測量的實際速度與給定速度偏差，SK表示前K次速度偏差代數(shù)和，DK表示最近兩次速度偏差之差。

EK，SK，DK的數(shù)學(xué)模型如下：

公式中，SV，PV分別為通過控制程序設(shè)定的給定轉(zhuǎn)速和編碼器測得的實際轉(zhuǎn)速。

在果園噴藥競賽機器人的實際應(yīng)用中，設(shè)定KP值為60，KI值為0.5，KD值為0，可實現(xiàn)機器人車輪速度的PID控制，使機器人沿直線穩(wěn)定運動。

4.2 ?機器人循跡控制程序設(shè)計

果園噴藥競賽機器人車體前端裝有4路循跡傳感器SEN-1595，從左至右依次為L2，L1，R1，R2。

循跡傳感器初始電平信號為1，檢測到循跡線時為0。若車體向右偏轉(zhuǎn)，L1路檢測到循跡線，電平置0，主控制器分別調(diào)節(jié)兩側(cè)PWM波占空比，使左側(cè)兩輪速度小于右側(cè)兩輪速度，車體向左校正。若到達(dá)左轉(zhuǎn)位置，L2路檢測到循跡線，電平置0，主控制器控制車體左側(cè)兩電機同速反轉(zhuǎn)，右側(cè)兩電機同速正轉(zhuǎn)，車體左轉(zhuǎn);反之亦然。

4.3 ?機器人平面定位系統(tǒng)程序設(shè)計

平面定位系統(tǒng)由電子羅盤和直流電機霍爾編碼器組成。主控制器通過中斷計算編碼器返回脈沖數(shù)，得到車輪轉(zhuǎn)過的路程，并結(jié)合電子s羅盤返回的航向角信息，實現(xiàn)平面定位[4]。其流程如圖6所示。

以起始區(qū)為坐標(biāo)原點，橫向為X軸，縱向為Y軸。設(shè)機器人當(dāng)前位置坐標(biāo)為（X0，Y0），目標(biāo)點坐標(biāo)為（X1，Y1）。通過比較Y1，Y0，控制車體進(jìn)退，車輪轉(zhuǎn)動同時編碼器外部中斷計數(shù)，達(dá)到預(yù)設(shè)值Z1時，到達(dá)（X0，Y1）點。再根據(jù)橫向距離，即X1-X0，判斷車體轉(zhuǎn)向，同時，編碼器外部中斷計數(shù)，達(dá)到預(yù)設(shè)值Z2時，車體制動，到達(dá)目標(biāo)點（X1，Y1），完成導(dǎo)航。其中，預(yù)設(shè)值Z根據(jù)編碼器的線數(shù)n、電機的減速比i、相對坐標(biāo)的差值△、車輪直徑d確定，公式如下：

4.4 ?機器人噴藥程序設(shè)計

在運動過程中，光電開關(guān)檢測到噴靶，電平置1，到達(dá)噴藥作業(yè)點，車體制動。在主控制器控制下，步進(jìn)電機工作，滾珠絲杠副帶動平面四桿機構(gòu)移動到預(yù)定位置;舵機正轉(zhuǎn)，噴頭伸出。同時，主控制器控制電磁繼電器打開相應(yīng)電磁閥閥口，電機驅(qū)動器V驅(qū)動增壓泵工作，開始噴藥。主控制器通過控制輸入直流電機驅(qū)動器V的PWM波占空比調(diào)節(jié)流經(jīng)噴頭藥液的流量和壓強，實現(xiàn)變量、精準(zhǔn)噴藥。噴藥完成后，增壓泵停止工作，電磁閥閥口關(guān)閉，舵機反轉(zhuǎn)，收回噴頭。最后，步進(jìn)電機反轉(zhuǎn)，滾珠絲杠副回到初始位置，噴藥結(jié)束。

5 ? ?結(jié)語

根據(jù)競賽要求，基于平面四桿機構(gòu)的工作原理，以STM32F103ZET6單片機作為控制核心，設(shè)計了果園噴藥競賽機器人的機械結(jié)構(gòu)、控制系統(tǒng)和軟件系統(tǒng)。機器人在直流電機的驅(qū)動下完成前進(jìn)、后退及轉(zhuǎn)彎等基本運動。在運動的過程中，利用循跡傳感器和電子羅盤實現(xiàn)循跡、導(dǎo)航定位功能。在到達(dá)噴藥作業(yè)點后，通過步進(jìn)電機和舵機控制噴藥執(zhí)行機構(gòu)到達(dá)預(yù)定噴藥位置;控制電磁繼電器和直流電機驅(qū)動器V實現(xiàn)變量、精準(zhǔn)噴藥。經(jīng)過實地調(diào)試，參加競賽，得到了良好的效果。

[參考文獻(xiàn)]

[1]孫雪，姚名暉，宋朋.果園噴藥機器人的創(chuàng)新設(shè)計[J].安徽農(nóng)業(yè)科學(xué)，2013（30）：12204-12206.

[2]李錕，王洪臣，鄭曉培，等.微型噴藥機構(gòu)型設(shè)計與仿真分析[J].農(nóng)機化研究，2017（8）：72-76.

[3]劉濤，呂勇，劉立雙.智能車路徑識別與控制性能提高方法研究及實現(xiàn)[J].電子技術(shù)應(yīng)用，2016（1）：54-57.

[4]曹慧芳，呂洪波，孫啟國.基于MEMS陀螺儀的隨機誤差分析[J].計算機測量與控制，2016（1）：178-181.