夏聰明，喻擎蒼，賈 靖，趙 暉

(浙江理工大學(xué) 信息學(xué)院，杭州 310018)

0 引言

促進(jìn)農(nóng)業(yè)發(fā)展、農(nóng)民增收一直是近幾年國家關(guān)注的重點(diǎn)。2004-2015年的11年時間，全國農(nóng)作物耕種收綜合機(jī)械化水平增長了26.3%[1]。實(shí)踐表明：提高農(nóng)業(yè)機(jī)械化水平，可以促進(jìn)農(nóng)產(chǎn)品的增產(chǎn)增收，提高農(nóng)民收入，還能減少農(nóng)民勞作時間，提高勞動效率[1-2]。農(nóng)機(jī)自主行走可以提升田間作業(yè)的效率，降低勞動者的工作強(qiáng)度[3]，有利于我國農(nóng)業(yè)的智能化和自動化；還能提高機(jī)械作業(yè)精度，增加機(jī)械作業(yè)的時間，讓勞動者免于機(jī)械重復(fù)的勞動，確保了勞動者的人身安全。傳統(tǒng)的定位方式中，GPS定位的精度低，差分GPS價格比較昂貴，慣性導(dǎo)航定位存在累加誤差問題，激光測距存在對準(zhǔn)問題，視覺定位難以適應(yīng)田間復(fù)雜環(huán)境。為此，研究設(shè)計(jì)了柔索長度測量裝置和基于激光測距的緩沖機(jī)構(gòu)，利用三角法對農(nóng)機(jī)進(jìn)行定位。

1 三角法定位原理分析

在田間地頭固定兩個立桿(定點(diǎn)A與定點(diǎn)B)。在農(nóng)機(jī)上選取一固定點(diǎn)，用柔索將農(nóng)機(jī)上的固定點(diǎn)分別與兩個立桿相連接，且將農(nóng)機(jī)與兩桿抽象成質(zhì)點(diǎn)，這樣農(nóng)機(jī)與地頭的兩個桿之間便形成一個三角形，如圖1所示。

先測出兩個桿之間的距離L,通過柔索測距裝置測出農(nóng)機(jī)固定點(diǎn)到兩個桿之間的距離L1和L2，據(jù)此可計(jì)算出農(nóng)機(jī)在田間的坐標(biāo)。

圖1 三角定位原理圖Fig.1 The principle of triangulation

2 柔索長度測量裝置設(shè)計(jì)研究

2.1 長度測量原理

柔索裝置采用的柔索是直徑為0.5mm的不銹鋼鋼絲繩，強(qiáng)度高、質(zhì)量小。實(shí)驗(yàn)測量出100m的鋼絲繩平衡力為16N，拉斷力為215N。在長度測量裝置中，柔索的一端連接在地頭一定點(diǎn)，另一端經(jīng)過長度測量裝置與農(nóng)機(jī)相連。本文采用的是增量式光電旋轉(zhuǎn)編碼器，在編碼器的輸出軸上套上一個固定直徑的滾輪，將柔索纏繞在滾輪上；農(nóng)機(jī)在田間作業(yè)時，滾輪會隨著機(jī)器的行走帶動編碼器旋轉(zhuǎn)，對編碼器的脈沖進(jìn)行計(jì)數(shù)，即可得出機(jī)器在田間行走的距離。

2.2 長度測量核心部件

本裝置采用的是600線編碼器，旋轉(zhuǎn)1圈可輸出600個脈沖，可通過ArduinoMega 2560對編碼器輸出的脈沖進(jìn)行計(jì)數(shù)。測試前，將Arduino的數(shù)字引腳2與3連接編碼器的A相與B相輸出線，如圖2所示。

圖2 Mega 2560讀取編碼器脈沖Fig.2 Using Mega2560 to read the encoder pulse

在Arduino程序[4]中，用中斷的方式對脈沖計(jì)數(shù)，使用中斷的優(yōu)點(diǎn)是中斷響應(yīng)時間快，不占用資源，主程序同時還可以完成其它任務(wù)。Mega2560的2號引腳和3號引腳可作為中斷引腳。當(dāng)Arduino檢測到A通道有變化, 程序就立刻跳轉(zhuǎn)到中斷函數(shù),中斷的觸發(fā)方式是電平變化，中斷函數(shù)會在上升沿和下降沿都會被調(diào)用，B通道的變化也是同樣的道理。中斷函數(shù)的執(zhí)行流程如圖3(以A通道為例)所示。

圖3 中斷函數(shù)執(zhí)行過程Fig.3 Interrupt function execution process

在實(shí)際的應(yīng)用中，對于編碼器脈沖的讀取肯定會出現(xiàn)少讀或誤讀的情況，為了檢測脈沖的讀取誤差，用步進(jìn)電機(jī)通過聯(lián)軸器帶動編碼器轉(zhuǎn)動固定的圈數(shù)，計(jì)算Arduino讀取的脈沖數(shù)并與理論情況的脈沖數(shù)比較。進(jìn)行50次測量，取平均值，如表1所示。

表1 編碼器讀取脈沖數(shù)與實(shí)際脈沖數(shù)

Table 1 The number of pulses read by the encoder and the number of actual pulses

當(dāng)編碼器轉(zhuǎn)動500圈以下時，檢丟的脈沖數(shù)逐漸變多；到了500圈以后，檢丟的脈沖數(shù)穩(wěn)定在15個左右，脈沖丟失率在0.002 5%以內(nèi)。

2.3 納線機(jī)構(gòu)研究設(shè)計(jì)

完成距離的測量需要有一個納線機(jī)構(gòu)對柔索進(jìn)行收放線的控制，其必須可以自動進(jìn)行繞線，以保證柔索能均勻地纏繞在繞線軸上。

如圖4所示：57步進(jìn)電機(jī)旋轉(zhuǎn)帶動連接在其上面的繞線軸轉(zhuǎn)動，同時42步進(jìn)電機(jī)會協(xié)同轉(zhuǎn)動，通過聯(lián)軸器帶動其上面的絲桿轉(zhuǎn)動，進(jìn)而推動其上面的螺母在絲桿上來回運(yùn)動。為此，設(shè)計(jì)了一個推動裝置，套在螺母上，隨著螺母的來回移動而移動，此裝置負(fù)責(zé)將線來回推動，使得繩索在繞線軸上纏繞1圈，裝置將繩頭向前推動，使繞線均勻纏繞在繞線軸上。為了防止推動裝置傾斜，在旁側(cè)用一固定光桿使推動裝置保持穩(wěn)定。控制電機(jī)的芯片是Arduino Mega2560, 步進(jìn)電機(jī)驅(qū)動模塊采用的是雙路輸出的DRV8825步進(jìn)電機(jī)驅(qū)動器。納線機(jī)構(gòu)工作流程如圖5所示。

圖4 收納線裝置結(jié)構(gòu)示意圖Fig.4 The structure of the receiving line device

圖5 納線機(jī)構(gòu)工作流程圖Fig.5 Working process of nanowire mechanism

3 基于激光測距的緩沖機(jī)構(gòu)設(shè)計(jì)

3.1 緩沖機(jī)構(gòu)總體設(shè)計(jì)

田間作業(yè)時，機(jī)器擺動導(dǎo)致柔索突然張緊或松弛，造成柔索斷裂或?qū)ρb置拉力過度。增加彈簧緩沖，用激光測距傳感器測量繞線點(diǎn)的偏移。柔索測距如圖6所示。柔索的一端固定在立桿上，在編碼器上套上帶有凹槽的滾輪，柔索在滾輪上饒1圈后，通過緩沖的滾輪，納線機(jī)構(gòu)上緩沖滾輪用鋁桿支撐。

3.2 緩沖位移測量

激光測距傳感器VL53L0X采用飛行時間原理實(shí)現(xiàn)測距應(yīng)用，使用I2C接口進(jìn)行設(shè)備控制和數(shù)據(jù)傳輸,激光測距傳感器一次測量可在30ms內(nèi)完成。對VL53L0X進(jìn)行測試，將Arduino的[5]SDA與SCL和VL53L0X的SDA、SCL相連接，Arduino的3.3V引腳連VL53L0X的VIN,兩者的GND連接。

圖6 柔索測距詳細(xì)圖Fig.6 Flexible cable distance measuring structure

為了測試激光測距傳感器的準(zhǔn)確性及穩(wěn)定性，在50、100、150mm的距離上每隔1s進(jìn)行1次測量，得到500組數(shù)據(jù)，從得到的數(shù)據(jù)中選取100組進(jìn)行分析，如圖7所示。

(a) 50mm

(b) 100mm

(c) 150mm

(d) 200mm圖7 不同距離下VL53L0X測試數(shù)據(jù)Fig.7 VL53L0X test data at different distances

對上述數(shù)據(jù)進(jìn)行分析，統(tǒng)計(jì)結(jié)果如表2所示。由表2發(fā)現(xiàn)：測量距離為200mm時最大誤差達(dá)到了10mm，50mm時平均相對誤差為7.9%，難以滿足田間定位高精度的要求，因此要用卡爾曼濾波的方法進(jìn)一步減少誤差，提高測量精度。

表2 VL53L0X測試數(shù)據(jù)統(tǒng)計(jì)結(jié)果Table 2 VL53L0X test data statistical results

對上面測量的數(shù)據(jù)進(jìn)行卡爾曼濾波處理后，如圖8所示。

對卡爾曼濾波后的數(shù)據(jù)進(jìn)行分析，結(jié)果如表3所示。由表3可見：無論是均值還是平均相對誤差較卡爾曼濾波之前的數(shù)據(jù)都更加精確；測量誤差控制在3%以內(nèi)，滿足對緩沖裝置伸縮位移進(jìn)行測量的需求。

(a) 50mm

(b) 100mm

(c) 150mm

(d) 200mm圖8 卡爾曼濾波后數(shù)據(jù)Fig.8 Data after Calman filtering表3 卡爾曼濾波后VL53L0X測試數(shù)統(tǒng)計(jì)結(jié)果Table 3 Statistical results of VL53L0X test number after Calman filtering

測量距離/mm均值/mm方差平均相對誤差/%5051.36731.20382.73100102.11002.32742.11150151.97001.86641.31200202.44003.02491.22

4 定位試驗(yàn)結(jié)果與分析

長度測量裝置得到了農(nóng)機(jī)與定點(diǎn)之間的距離，緩沖機(jī)構(gòu)測量出了沖擊帶來的偏移的位移，將以上數(shù)據(jù)通過Arduino得到，通過I2C將數(shù)據(jù)傳給樹莓派，由樹莓派根據(jù)建立的坐標(biāo)系計(jì)算出農(nóng)機(jī)的坐標(biāo)。坐標(biāo)系以定點(diǎn)A與定點(diǎn)B的中心點(diǎn)作為坐標(biāo)原點(diǎn)，如圖9所示。

圖9 定位坐標(biāo)系Fig.9 Positioning coordinate system

試驗(yàn)時，手動拉著裝置沿Y軸行走，得到的距離即為Y軸坐標(biāo)。在1～10m的距離上每隔1m輸出一次坐標(biāo)值，進(jìn)行50次試驗(yàn)，結(jié)果如表4所示。

表4 不同測量點(diǎn)距離值Table 4 3Distance values of different measuring points mm

通過表4可以看出，該方法的定位誤差在20mm以內(nèi)。這表明該方法可靠、精確。

5 結(jié)論

研究了基于柔索測距裝置的農(nóng)機(jī)定位方法。通過讀取編碼器脈沖計(jì)算柔索長度，設(shè)計(jì)納線機(jī)構(gòu)進(jìn)行收放線的控制。利用激光測距芯片對緩沖機(jī)構(gòu)伸縮位移進(jìn)行測量。編寫了Arduino程序，讀取了激光測距傳感器的測量數(shù)據(jù)，用卡爾曼濾波的方法對數(shù)據(jù)進(jìn)行了處理，濾波后的數(shù)據(jù)最大平均相對誤差為2.73%。對該方法進(jìn)行了定位試驗(yàn)，結(jié)果表明：定位誤差小于20mm，適用于農(nóng)機(jī)田間定位。