基于柔索測(cè)距裝置的農(nóng)機(jī)定位方法研究

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

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

0 引言

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

1 三角法定位原理分析

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

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

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

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

2.1 長(zhǎng)度測(cè)量原理

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

2.2 長(zhǎng)度測(cè)量核心部件

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

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

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

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

在實(shí)際的應(yīng)用中，對(duì)于編碼器脈沖的讀取肯定會(huì)出現(xiàn)少讀或誤讀的情況，為了檢測(cè)脈沖的讀取誤差，用步進(jìn)電機(jī)通過(guò)聯(lián)軸器帶動(dòng)編碼器轉(zhuǎn)動(dòng)固定的圈數(shù)，計(jì)算Arduino讀取的脈沖數(shù)并與理論情況的脈沖數(shù)比較。進(jìn)行50次測(cè)量，取平均值，如表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)動(dòng)500圈以下時(shí)，檢丟的脈沖數(shù)逐漸變多；到了500圈以后，檢丟的脈沖數(shù)穩(wěn)定在15個(gè)左右，脈沖丟失率在0.002 5%以內(nèi)。

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

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

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

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

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

3.2 緩沖位移測(cè)量

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

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

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

(a) 50mm

(b) 100mm

(c) 150mm

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

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

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

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

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

(a) 50mm

(b) 100mm

(c) 150mm

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

測(cè)量距離/mm均值/mm方差平均相對(duì)誤差/%5051.36731.20382.73100102.11002.32742.11150151.97001.86641.31200202.44003.02491.22

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

長(zhǎng)度測(cè)量裝置得到了農(nóng)機(jī)與定點(diǎn)之間的距離，緩沖機(jī)構(gòu)測(cè)量出了沖擊帶來(lái)的偏移的位移，將以上數(shù)據(jù)通過(guò)Arduino得到，通過(guò)I2C將數(shù)據(jù)傳給樹(shù)莓派，由樹(shù)莓派根據(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)時(shí)，手動(dòng)拉著裝置沿Y軸行走，得到的距離即為Y軸坐標(biāo)。在1～10m的距離上每隔1m輸出一次坐標(biāo)值，進(jìn)行50次試驗(yàn)，結(jié)果如表4所示。

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

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

5 結(jié)論

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