果園噴藥競賽機器人設計

高研 李昕宇 史穎剛 劉利

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

關鍵詞：機器人競賽;果園噴藥;自主尋跡;平面定位系統;單片機控制

1 ? ?果園噴藥機器人競賽相關簡介

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

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

根據競賽要求，為實現精準噴藥，機器人需自主移動到作業點，并對靶標指定區域噴藥。因此，機器人應包括基本運動模塊、循跡模塊、平面定位系統、噴藥執行機構等部分，需從機械結構、控制系統和軟件設計3方面進行研究。

2 ? ?機械結構設計

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

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

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

3 ? ?控制系統設計

果園噴藥競賽機器人以STM32F103ZET6單片機為主控制核心，控制系統架構如圖5所示。

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

到達靶標位置后，主控制器根據預定的作業位置，驅動噴藥平臺移動，并進行藥液噴射。主控制器通過步進電器驅動器控制步進電機轉動，帶動絲杠螺母升降，以調整噴藥機構高度;通過控制舵機轉動，實現噴頭伸縮，以貼近噴藥位置;到達目標位置后，主控制器通過直流電機驅動器V調節增壓泵壓力和流量，并通過電磁繼電器控制相應電磁閥閥口的開閉，實現多噴頭獨立控制，完成對靶標指定區域的精準噴藥。

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

4 ? ?軟件系統設計

4.1 ?機器人基本運動程序設計

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

PID控制算法的數學模型如下：

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

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

EK，SK，DK的數學模型如下：

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

在果園噴藥競賽機器人的實際應用中，設定KP值為60，KI值為0.5，KD值為0，可實現機器人車輪速度的PID控制，使機器人沿直線穩定運動。

4.2 ?機器人循跡控制程序設計

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

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

4.3 ?機器人平面定位系統程序設計

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

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

4.4 ?機器人噴藥程序設計

在運動過程中，光電開關檢測到噴靶，電平置1，到達噴藥作業點，車體制動。在主控制器控制下，步進電機工作，滾珠絲杠副帶動平面四桿機構移動到預定位置;舵機正轉，噴頭伸出。同時，主控制器控制電磁繼電器打開相應電磁閥閥口，電機驅動器V驅動增壓泵工作，開始噴藥。主控制器通過控制輸入直流電機驅動器V的PWM波占空比調節流經噴頭藥液的流量和壓強，實現變量、精準噴藥。噴藥完成后，增壓泵停止工作，電磁閥閥口關閉，舵機反轉，收回噴頭。最后，步進電機反轉，滾珠絲杠副回到初始位置，噴藥結束。

5 ? ?結語

根據競賽要求，基于平面四桿機構的工作原理，以STM32F103ZET6單片機作為控制核心，設計了果園噴藥競賽機器人的機械結構、控制系統和軟件系統。機器人在直流電機的驅動下完成前進、后退及轉彎等基本運動。在運動的過程中，利用循跡傳感器和電子羅盤實現循跡、導航定位功能。在到達噴藥作業點后，通過步進電機和舵機控制噴藥執行機構到達預定噴藥位置;控制電磁繼電器和直流電機驅動器V實現變量、精準噴藥。經過實地調試，參加競賽，得到了良好的效果。

[參考文獻]

[1]孫雪，姚名暉，宋朋.果園噴藥機器人的創新設計[J].安徽農業科學，2013（30）：12204-12206.

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

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

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