基于柔索測距裝置的農機定位方法研究

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

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

0 引言

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

1 三角法定位原理分析

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

先測出兩個桿之間的距離L,通過柔索測距裝置測出農機固定點到兩個桿之間的距離L1和L2，據此可計算出農機在田間的坐標。

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

2 柔索長度測量裝置設計研究

2.1 長度測量原理

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

2.2 長度測量核心部件

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

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

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

圖3 中斷函數執行過程Fig.3 Interrupt function execution process

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

表1 編碼器讀取脈沖數與實際脈沖數

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

當編碼器轉動500圈以下時，檢丟的脈沖數逐漸變多；到了500圈以后，檢丟的脈沖數穩定在15個左右，脈沖丟失率在0.002 5%以內。

2.3 納線機構研究設計

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

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

圖4 收納線裝置結構示意圖Fig.4 The structure of the receiving line device

圖5 納線機構工作流程圖Fig.5 Working process of nanowire mechanism

3 基于激光測距的緩沖機構設計

3.1 緩沖機構總體設計

田間作業時，機器擺動導致柔索突然張緊或松弛，造成柔索斷裂或對裝置拉力過度。增加彈簧緩沖，用激光測距傳感器測量繞線點的偏移。柔索測距如圖6所示。柔索的一端固定在立桿上，在編碼器上套上帶有凹槽的滾輪，柔索在滾輪上饒1圈后，通過緩沖的滾輪，納線機構上緩沖滾輪用鋁桿支撐。

3.2 緩沖位移測量

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

圖6 柔索測距詳細圖Fig.6 Flexible cable distance measuring structure

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

(a) 50mm

(b) 100mm

(c) 150mm

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

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

表2 VL53L0X測試數據統計結果Table 2 VL53L0X test data statistical results

對上面測量的數據進行卡爾曼濾波處理后，如圖8所示。

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

(a) 50mm

(b) 100mm

(c) 150mm

(d) 200mm圖8 卡爾曼濾波后數據Fig.8 Data after Calman filtering表3 卡爾曼濾波后VL53L0X測試數統計結果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 定位試驗結果與分析

長度測量裝置得到了農機與定點之間的距離，緩沖機構測量出了沖擊帶來的偏移的位移，將以上數據通過Arduino得到，通過I2C將數據傳給樹莓派，由樹莓派根據建立的坐標系計算出農機的坐標。坐標系以定點A與定點B的中心點作為坐標原點，如圖9所示。

圖9 定位坐標系Fig.9 Positioning coordinate system

試驗時，手動拉著裝置沿Y軸行走，得到的距離即為Y軸坐標。在1～10m的距離上每隔1m輸出一次坐標值，進行50次試驗，結果如表4所示。

表4 不同測量點距離值Table 4 3Distance values of different measuring points mm

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

5 結論

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