果園噴藥競賽機(jī)器人設(shè)計(jì)

高研 李昕宇 史穎剛 劉利

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4.1 ?機(jī)器人基本運(yùn)動(dòng)程序設(shè)計(jì)

機(jī)器人采用4輪獨(dú)立驅(qū)動(dòng)、差速控制，通過控制各直流電機(jī)的運(yùn)轉(zhuǎn)狀態(tài)，實(shí)現(xiàn)車體的基本運(yùn)動(dòng)。在運(yùn)動(dòng)過程中，為減少車體重心偏移及摩擦力不同引起的速度誤差，保證機(jī)器人沿直線快速穩(wěn)定運(yùn)動(dòng)，采用比例—積分—微分（Proportion-Integral-Differential，PID）控制算法調(diào)節(jié)車輪轉(zhuǎn)速。其基本原理為：由主控制器設(shè)定各車輪的給定轉(zhuǎn)速，并對霍爾編碼器返回脈沖進(jìn)行定時(shí)采樣，利用采樣接收結(jié)果計(jì)算各車輪的實(shí)際轉(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次編碼器測量的實(shí)際速度與給定速度偏差，SK表示前K次速度偏差代數(shù)和，DK表示最近兩次速度偏差之差。

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

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

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

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

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

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

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

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

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

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

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

5 ? ?結(jié)語

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

[參考文獻(xiàn)]

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

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

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

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