激光導(dǎo)航設(shè)施管理機(jī)器人自動(dòng)控制系統(tǒng)的研究

王艷莉， 曹仁勇， 耿長(zhǎng)興

(1.江蘇農(nóng)林職業(yè)技術(shù)學(xué)院，江蘇句容 212400； 2.蘇州大學(xué)，江蘇蘇州 215155)

十二五以來，我國大力發(fā)展設(shè)施農(nóng)業(yè)和精準(zhǔn)農(nóng)業(yè)，促進(jìn)了傳統(tǒng)農(nóng)業(yè)的產(chǎn)業(yè)升級(jí)[1-2]。自動(dòng)導(dǎo)航技術(shù)是實(shí)現(xiàn)設(shè)施農(nóng)業(yè)機(jī)械自動(dòng)化、智能化和信息化的關(guān)鍵技術(shù)，開發(fā)一套操作簡(jiǎn)便、導(dǎo)航精度高、經(jīng)濟(jì)適用的大棚管理機(jī)械自動(dòng)導(dǎo)航系統(tǒng)成為設(shè)施農(nóng)業(yè)生產(chǎn)中亟待解決的問題[3]。近年來激光導(dǎo)航技術(shù)得到廣泛的應(yīng)用，具有定位精度高、可全時(shí)段工作、不受電磁干擾等優(yōu)點(diǎn)[4]。CMU大學(xué)的Hamner、北海道大學(xué)的Barawid等采用激光掃描技術(shù)，實(shí)時(shí)采集果樹間的位置信息，利用霍夫變換設(shè)計(jì)導(dǎo)航路徑，基于PID控制器沿著擬合路徑完成車輛的自動(dòng)行走，實(shí)現(xiàn)設(shè)施農(nóng)業(yè)機(jī)械的自動(dòng)導(dǎo)航[5-6]。

本研究以激光掃描儀作為自動(dòng)導(dǎo)航設(shè)備，實(shí)時(shí)提取溫室大棚植物行間距信息，進(jìn)行不同環(huán)境下設(shè)施管理機(jī)器人自動(dòng)導(dǎo)航控制系統(tǒng)的研究。首先在大棚內(nèi)設(shè)立幾處基點(diǎn)，通過激光掃描設(shè)備建立掃描直角坐標(biāo)系，再應(yīng)用最小二乘法對(duì)規(guī)劃路徑進(jìn)行擬合，然后基于PID控制理論設(shè)計(jì)比例控制器，建立運(yùn)動(dòng)學(xué)模型，最后通過現(xiàn)場(chǎng)試驗(yàn)驗(yàn)證自動(dòng)導(dǎo)航控制系統(tǒng)的可靠性。

1 管理機(jī)器人的導(dǎo)航系統(tǒng)設(shè)計(jì)

1.1 工作原理

在溫室內(nèi)均勻布置4臺(tái)激光發(fā)射器和接收器，激光接收器安裝在管理機(jī)機(jī)架上，管理機(jī)器人在行進(jìn)中實(shí)時(shí)檢測(cè)激光位置信號(hào)，這些信號(hào)數(shù)據(jù)持續(xù)傳遞給轉(zhuǎn)向控制器，反饋信號(hào)給計(jì)算機(jī)。激光接收器的左中右3個(gè)區(qū)域負(fù)責(zé)接收信號(hào)，當(dāng)接收器左側(cè)收到激光信號(hào)時(shí)，則管理機(jī)向右偏移，此時(shí)轉(zhuǎn)向繼電器接收到左轉(zhuǎn)的指令，糾正機(jī)器偏移直到激光信號(hào)處于接收器中間區(qū)域；反之，當(dāng)接收器右側(cè)收到激光信號(hào)，說明管理機(jī)向左偏移，只有當(dāng)激光信號(hào)處于接收器的中間區(qū)域，說明此時(shí)機(jī)具正沿著激光垂直基準(zhǔn)面直線行走，無需修改行走狀態(tài)[7]。綜上所述可知，激光信號(hào)通過發(fā)射器、接收器反饋信號(hào)進(jìn)入工控系統(tǒng)，然后由RS232控制繼電器模組輸出開關(guān)信號(hào)，通過電磁繼電器控制并驅(qū)動(dòng)電動(dòng)推桿，實(shí)現(xiàn)左右轉(zhuǎn)向控制，按照工況要求實(shí)現(xiàn)導(dǎo)航和轉(zhuǎn)向控制。激光導(dǎo)航直線行走工作原理如圖1所示。

1.2 導(dǎo)航系統(tǒng)總體設(shè)計(jì)

設(shè)施管理機(jī)器人由1個(gè)擺線針輪減速電機(jī)來驅(qū)動(dòng)旋耕刀，2個(gè)減速電機(jī)分別驅(qū)動(dòng)左右兩端的履帶行走，由田園管理機(jī)作為執(zhí)行機(jī)構(gòu)，采用自主導(dǎo)航系統(tǒng)引導(dǎo)執(zhí)行機(jī)構(gòu)實(shí)現(xiàn)準(zhǔn)直行走。導(dǎo)航系統(tǒng)由田園管理機(jī)(BAMA 1WG4D)、計(jì)算機(jī)(戴爾Ins14ZD-3526)、激光掃描儀(Micro-Epsilon MSC710-U)和減速電機(jī)(信捷DS2-AS)組成。

激光掃描設(shè)備放置在管理機(jī)前方，實(shí)時(shí)接收前方樹行的位置信息，計(jì)算機(jī)將這些信息數(shù)據(jù)分析規(guī)劃出導(dǎo)航路徑，PID控制器接收計(jì)算機(jī)的信息指令并控制2個(gè)電機(jī)轉(zhuǎn)速，從而實(shí)現(xiàn)管理機(jī)器人的自主導(dǎo)航。自主導(dǎo)航系統(tǒng)如圖2所示。激光掃描儀安裝在機(jī)架正前方，檢測(cè)溫室大棚內(nèi)植株行的位置數(shù)據(jù)，這些數(shù)據(jù)信息反饋給PC機(jī)，用來生成導(dǎo)航路徑，轉(zhuǎn)向控制器用來控制管理機(jī)的轉(zhuǎn)向，確保沿著激光信號(hào)實(shí)現(xiàn)準(zhǔn)直線自主行走。

2 導(dǎo)航路徑規(guī)劃

2.1 樹行位置信息確定

為了實(shí)現(xiàn)設(shè)施管理機(jī)器人的自主導(dǎo)航，首先要對(duì)溫室內(nèi)樹行進(jìn)行精確定位，設(shè)立幾處基準(zhǔn)點(diǎn)，確定樹行與管理機(jī)的相對(duì)位置。經(jīng)過激光掃描的圖像分析，凹點(diǎn)處為樹行的位置，根據(jù)此特征將樹行的位置信息在直角坐標(biāo)中體現(xiàn)，從而確定植株的位置信息，根據(jù)凹陷點(diǎn)特征進(jìn)行樹行位置的定位[8]。

將掃描儀的位置設(shè)為原點(diǎn)，建立直角坐標(biāo)系如圖3所示，激光接收器可以在一個(gè)掃描周期掃過180°，每個(gè)數(shù)據(jù)的掃描區(qū)間是0.5°，一個(gè)周期內(nèi)獲得361個(gè)位置信息，根據(jù)這些數(shù)據(jù)可以精確得到管理機(jī)的位置信息。R代表掃描區(qū)間任一點(diǎn)到掃描設(shè)備的長(zhǎng)度，λ表示任一點(diǎn)的夾角，依據(jù)式(1)[9-10]計(jì)算出λi的數(shù)值，保存后即可知道大棚內(nèi)樹行的位置數(shù)據(jù)。

λi=0.5(i-1)；i=2，…，361。

(1)

2.2 樹行路徑擬合算法

根據(jù)前面對(duì)樹行信息的精確定位，采用最小二乘法對(duì)管理機(jī)的導(dǎo)航路徑進(jìn)行擬合。首先將大棚內(nèi)植株選取中線為規(guī)劃路線，從兩植株行選定接近管理機(jī)的3個(gè)點(diǎn)，依據(jù)式(2)計(jì)算出擬合路徑的信息數(shù)據(jù)。式中，(xLm，yLm)，(xRn，yRn)分別為左右兩行植株的組合點(diǎn)坐標(biāo)；m，n=1，2，3；k=1，2，…，9。(xk，yk)即為式(2)擬合出的坐標(biāo)點(diǎn)，從而獲得管理機(jī)的擬合路徑，擬合結(jié)果如圖4所示。

(2)

根據(jù)計(jì)算的坐標(biāo)點(diǎn)(xk，yk)取橫向偏差最小平方和δ=∑(yk-y)2，從直線方程y=ax+b可推導(dǎo)出：

δ=∑(yk-ax-b)2。

(3)

式(3)中對(duì)a、b進(jìn)行求導(dǎo)數(shù)計(jì)算，得：

(4)

對(duì)式(4)求導(dǎo)，得：

(5)

擬合結(jié)果如圖4所示。

3 導(dǎo)航控制

在控制系統(tǒng)的應(yīng)用中，PID控制算法是應(yīng)用最廣泛的一種算法，PID控制器是根據(jù)PID控制原理對(duì)整個(gè)控制系統(tǒng)進(jìn)行偏差調(diào)節(jié)，從而使被控變量的實(shí)際值與工藝要求的預(yù)定值一致。同時(shí)，控制器具有參數(shù)易調(diào)整、算法簡(jiǎn)單、魯棒性強(qiáng)、系統(tǒng)無靜差等優(yōu)點(diǎn)[9-10]?；谝陨咸攸c(diǎn)，本研究的導(dǎo)航控制器是采用PID算法理論實(shí)現(xiàn)的，管理機(jī)的行走導(dǎo)航控制是應(yīng)用PLC控制系統(tǒng)進(jìn)行控制設(shè)計(jì)，設(shè)計(jì)出的導(dǎo)航系統(tǒng)流程如圖5所示。

本研究在PID控制理論的基礎(chǔ)上，依據(jù)PLC控制系統(tǒng)進(jìn)行導(dǎo)航設(shè)計(jì)，假定大棚管理機(jī)在行走時(shí)發(fā)生的偏移量可以忽略，排除土地狀況對(duì)導(dǎo)航的影響因素，建立了大棚管理機(jī)械的運(yùn)動(dòng)學(xué)模型(圖6)[11]。S為管理機(jī)的目標(biāo)路徑，θ為管理機(jī)航向偏差，L為管理機(jī)前后軸的距離，l為管理機(jī)前軸中心距離航向點(diǎn)B的距離。依據(jù)此管理機(jī)的運(yùn)動(dòng)模型，推導(dǎo)出管理機(jī)的運(yùn)動(dòng)學(xué)方程式[12]：

(6)

按照管理機(jī)器人在大棚內(nèi)的行走特點(diǎn)和影響因素，當(dāng)其行走路徑與擬合路徑存在偏差時(shí)，可依據(jù)式(6)計(jì)算出偏差值，通過控制電機(jī)轉(zhuǎn)速來調(diào)整偏差值，從而實(shí)現(xiàn)對(duì)導(dǎo)航車輛的方向控制。本研究取α=kpζp，kp為運(yùn)動(dòng)模型比例系數(shù)，從而從圖6中可以推出該項(xiàng)的比例控制方程為：

α=k1d+k2sinθ。

(7)

管理機(jī)器人在行走過程中的橫向偏差由運(yùn)動(dòng)學(xué)方程得出，將其作為輸入量，依據(jù)式(7)可計(jì)算出管理機(jī)前輪轉(zhuǎn)角，即為輸出量。導(dǎo)航的自動(dòng)控制就是通過電機(jī)的不同轉(zhuǎn)速來實(shí)現(xiàn)管理機(jī)導(dǎo)航的自動(dòng)控制，電機(jī)轉(zhuǎn)速是由直流伺服電機(jī)將轉(zhuǎn)速信號(hào)發(fā)送給電機(jī)驅(qū)動(dòng)器來實(shí)現(xiàn)控制的。

4 結(jié)果與分析

在完成擬合路徑的基礎(chǔ)上，2017年11月設(shè)施大棚管理機(jī)器人在江蘇農(nóng)博園內(nèi)草莓大棚內(nèi)進(jìn)行了實(shí)機(jī)試驗(yàn)，試驗(yàn)?zāi)M現(xiàn)場(chǎng)耕作情境，溫室的長(zhǎng)度約為50 m，樹行間距為2 m。通過預(yù)先試驗(yàn)，設(shè)定式(7)中，k1=2.8，k2=0.7；取L=2.53 m，l=0；采樣周期300 ms；管理機(jī)速度0.14 m/s。通過多次試驗(yàn)最后統(tǒng)計(jì)數(shù)據(jù)見表1。圖7所示為計(jì)算出的平均橫向偏差數(shù)據(jù)變化圖。

表1 橫向偏差統(tǒng)計(jì)結(jié)果

從表1統(tǒng)計(jì)數(shù)據(jù)可以看出，8次試驗(yàn)結(jié)果的偏差在 -0.08～0.15之間，平均偏差0.037，標(biāo)準(zhǔn)差的平均值為0.024 0。將上述表1數(shù)據(jù)輸入到Origin軟件中生成橫向平均偏差圖，從圖7可以看出，負(fù)向橫向偏差出現(xiàn)在3.5 m之后，隨后正向橫向偏差在8.5 m和12 m處出現(xiàn)峰值，之后峰值慢慢變小直至30 m處接近0。結(jié)合表1和圖7進(jìn)行分析，溫室大棚內(nèi)的植株種植存在一定的偏差，偏差范圍在-0.10～0.15 m之間，在8.5 m和12 m處植株行間距增大偏離中線，致使規(guī)劃的路徑相差較大，橫向偏差達(dá)最大，這就說明植株行的整齊度直接影響計(jì)算機(jī)擬合路徑的準(zhǔn)確性。從圖7反映的偏差可以充分說明擬合路徑的偏差與大棚內(nèi)環(huán)境相吻合，本研究所設(shè)計(jì)的自動(dòng)導(dǎo)航控制系統(tǒng)具有一定和可靠性和精確度。根據(jù)溫室大棚內(nèi)的耕作要求，圖7的最大誤差0.15 m完全滿足要求，對(duì)于設(shè)施管理機(jī)器人的作業(yè)具有廣泛的適用性和可靠性。

5 結(jié)論

本研究以設(shè)施管理機(jī)器人為研究對(duì)象，以實(shí)現(xiàn)機(jī)器人自動(dòng)導(dǎo)航為目標(biāo)，采用了激光導(dǎo)航技術(shù)，實(shí)時(shí)采集大棚內(nèi)的樹行信息數(shù)據(jù)，應(yīng)用激光掃描進(jìn)行精確定位，并應(yīng)用最小二乘法對(duì)導(dǎo)航路徑進(jìn)行擬合，設(shè)計(jì)了PID控制器，進(jìn)行管理機(jī)的實(shí)地試驗(yàn)。試驗(yàn)數(shù)據(jù)表明，管理機(jī)器人在大棚內(nèi)自動(dòng)行走30 m，速度0.35 m/s，測(cè)得最大偏差為0.20 m。因此，該激光導(dǎo)航控制系統(tǒng)采集位置信息快速、準(zhǔn)確，對(duì)于設(shè)施管理機(jī)器人的自動(dòng)行走適用性強(qiáng)，可靠性高。