基于超聲波測距的農機行間自行走控制系統

蒿晟昆，霍靜琦，張宇，李志偉

（山西農業大學農業工程學院，山西晉中 030801）

農機田間自主行走作為精準農業的關鍵性技術，其自動化技術可極大減輕作業人員的勞動強度，適應現代化精細農業生產的發展趨勢[1]。

國內外關于農機平臺路徑規劃均有研究，并在農機旋耕播種、農機田間施肥、植保噴藥、作物收獲等方面應用廣泛[2]。日本的yoshisda 等人采取GPS 與FOG 相融合技術，以PH-69 插秧機為樣機研究出PID算法控制農機自動駕駛[3]。周俊等對農業機器人通過機器視覺進行路徑跟蹤研究，設計出基于輪式農業機器人的機器視覺導航控制系統[4]。

農機導航定位的研究，基于多種方法，諸如GPS導航、雙激光雷達定位、機器視覺尋跡、多傳感器融合位置檢測等。但是目前可應用的農機尺寸往往偏大，此類大型農機通常搭載GPS 全球定位系統，僅適合大型農場中的作業。但是田間作業往往還只能依靠人力徒步。現階段對于狹小空間自主行走的研究主要集中在智能巡檢機器人及電動智能車，對于在復雜多變的行間行駛的農機研究較少。現將農機與實際農藝相結合，針對農機目前在農業生產中所存在的缺陷，設計小型農機行間自行走控制系統具有很強的實際應用價值。

1 履帶式農機運動學模型

行間自行走的控制策略和農機位置調整需要基于履帶式農機的運動學模型[5]。如圖1 所示為履帶式農機運動學模型，分別建立全局地面坐標系XOY和農機車體坐標系xoy，農機車體坐標系位于農機車體中心處。y軸方向為農機前進方向。農機行進中轉向時，農機以平面內一點作為其轉動瞬心，記為點Q。農機中心點o處到點Q的連線距離即為轉向半徑R，農機繞點Q轉動的速度即為轉向角速度ω。

圖1 履帶式農機運動學模型

對履帶式農機的運動學模型作如下假設：①農機時刻處在平面上運動且阻力系數固定；②兩側履帶在均勻低速轉向時無滑移現象出現；③兩側履帶完全相同且均不影響行駛軌跡；④農機的中心點o視為實際農機質心；⑤履帶式農機轉向時忽略離心力和轉向角速度ω的變化影響[6]。當農機以Q為轉動瞬心，R為轉向半徑時，其左右兩側驅動輪速度、兩側履帶速度和農機運行速度、轉向角速度、轉向半徑的關系表示為式（1）-（5）。

其中，ωR、ωL分別為左右兩側驅動輪的角速度，r為驅動輪半徑，vR、vL分別為左右兩側履帶的行駛速度，ω為農機繞點Q的轉向角速度，v為履帶式農機車體中心o點的線速度，L為農機車體寬度，R為農機轉向半徑。分析農機的不同行駛狀態，當vR與vL相等，即農機直線行駛時，轉向半徑視為無窮大；當vR與vL大小相等方向相反時，即農機處于原地差速轉向狀態，轉向半徑為零，農機轉動瞬心點Q與農機中心點o重合；當vR與vL不相等時，農機繞轉動瞬心點Q以任意R作為轉向半徑進行轉向[7]。

農機轉向角速度為ω，則在Δt時間內農機運動的弧長記為E，通過弧長角度公式將農機運動的弧長轉化為農機在Δt時間內轉過的角度n，其與左右兩側履帶行駛速度的關系可表示為式（6）-（7）：

目前履帶類車輛常有三種差速轉向方式[8]，該文選擇獨立式轉向方式，該轉向方式對功率的需求小于中心差速式轉向和外側提速式轉向。其原理是農機轉向過程中，靠近轉動瞬心點Q側履帶速度降低，遠離轉動瞬心點Q側履帶速度保持不變。

2 車身位置檢測策略

目前廣泛采用非接觸式距離傳感器有激光測距、紅外測距、超聲波測距；其中紅外測距傳感器容易受到光強影響，且范圍較小，在復雜的田間環境易受干擾；激光測距精度高但是結構復雜，成本較高，適合于高精準度運行的智能巡檢機器人；超聲波測距方向性好，穿透力強，超聲波傳感器防水防污性能強，在復雜的作業環境下仍能保持較優水準[9-11]。

農機在田間作業時，傳感器傳輸的信號常受安裝位置、農機自身噪音或者環境因素變化的影響，超聲波傳感器輸出距離信號存在較大誤差，需要對超聲波測得的距離信號進行預補償處理，以保證超聲波傳感器輸出的距離信號更加貼近實際距離。基于相對誤差最小二乘法對超聲波距離信號進行處理。

根據最小二乘擬合的數學定理[12]，將超聲波測量值與實際值作差，選擇相對誤差進行擬合計算，對距離測量值與實際值的線性關系表示如下：

測量的相對誤差通過最小二乘法公式表達如下，其中n為測量距離值的采樣次數，在測量值中任取兩組對應數據建立方程組求取M、N值：

由于超聲波傳感器的回聲測距原理，即超聲波存在測量死區，通過實驗測得超聲波傳感器測量死區為0至26 cm，所以超聲波測距實驗選定在27 cm 至50 cm 之間采樣。實驗測距結果如表1 所示。

表1 超聲波測距采樣值

將表中距離測量值與實際值代入擬合公式（9）、（10）中，解得M=1.077 0、N=-4.070 8，則實際距離值和超聲波測量距離值的關系按式（11）擬合：

通過表1 對比最小二乘擬合前的相對誤差值與最小二乘擬合后的相對誤差值，處理后的超聲波測量值準確度和穩定度有較為明顯的改善。

利用傳感器分別對車右前側、左前側、右后側、左后側的距離進行檢測，且超聲波的安裝位置到車體最外側的距離需大于超聲波自身測量死區，以保證超聲波工作在正常測距范圍內。圖2 所示為通過超聲波測量距離值對農機行駛航向角偏差進行分析計算示意圖。

圖2 中，a為車身左前側測量的距離值，b為左后側測量的距離值，c為農機長度，θ為農機車體坐標系y軸與全局地面坐標系Y軸之間的夾角，即農機相對于行間直線行駛時的航向角偏差值。根據梯形幾何關系得到兩者關系公式，如式（12）所示：

圖2 航向角偏差計算示意圖

如圖3 所示，車體中心位置垂直于兩側農作物種植行的直線距離作為農機距兩側農作物的距離d1，當農機在行間中心線正常行駛時，存在幾何關系：a=b=d1，當農機存在航向角偏差和位置偏差時，根據幾何關系有：

圖3 位置偏差計算示意圖

式（16）利用車身前后四個超聲波傳感器的測量值計算農機作業時，將車身航向角偏差θ和車身相較于行間中心線的位置偏差d作為下文模糊控制算法的輸入量。

3 自行走模糊控制策略

模糊控制法利用模糊數學的基本思想和理論，在分析復雜多變環境下的系統時，變量往往難以用傳統控制理論描述，因此引入模糊控制理論來簡化系統的動態分析與控制，其能夠效仿該領域專家的經驗和策略，智能決策能力遠超傳統控制理論。一個模糊控制器通常包含模糊化、模糊決策、解模糊化[13-14]。

航向角偏差θ、位置偏差d與行間行駛中心線的位置關系大致有圖4 所示的幾種情況。規定當車體位置偏差在行間中心線左側時，位置偏差d為負值；當車體位置偏差在行間中心線右側時，位置偏差d為正值。車頭行駛方向指向左前方時，航向角偏差θ為負值；車頭行駛方向指向右前方時，航向角偏差θ為正值。圖4（a）表示位置偏差為負，航向角偏差為0；圖4（e）表示位置偏差為0，航向角偏差為負；圖4（i）表示位置偏差為正，航向角偏差為正。

圖4 航向角偏差、位置偏差示意圖

模糊控制規則的建立基于人工駕駛農機的操縱經驗，其建立規則應該符合實際操縱方式：當車體位置偏差d、航向角偏差θ與行間中心線偏差較大時，應盡快調整方向，防止農機駛入兩側作物中；當車體位置偏差d、航向角偏差θ與行間中心線偏差較小時，對于消除偏差和農機行駛穩定性要兼顧處理。該模糊控制器輸出參數為農機轉向角n，式（17）為三者的模糊關系：

式中，δ為模糊控制器的修正因子，消除位置偏差和航向角偏差的側重不同，對于行間行駛農機，應更加關注位置偏差的修正，因此δ的取值應適當加大。

該模糊控制系統是一個雙輸入單輸出的模糊控制器；根據控制過程相關專家經驗對輸入輸出進行量化并總結出7×7 條模糊語言規則，其模糊控制規則見表2：

表2 模糊控制規則表

1）航向角偏差θ

基本論域：[-30°,30°]；

量化等級：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}；

量化系數：Kθ=6/30=0.2；

2）位置偏差d

基本論域：[-30 cm,30 cm]；

量化等級：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}；

量化系數：Kd=6/30=0.2；

3）轉向角n

基本論域：[-30°,30°]；

量化等級：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}；

量化系數：Kn=6/30=0.2。

根據所建立的模糊控制規則表，此系統內三個變量位置偏差、航向角偏差和轉向角的模糊集合為{負大，負中，負小，零，正小，正中，正大}={NB,NM,NS,ZE,PS,PM,PB}。轉向角n的Gauss 隸屬度函數如圖5 所示。

圖5 轉向角n隸屬度函數

采用面積重心法進行解模糊，面積重心法是取隸屬度函數曲線與橫坐標圍成面積的重心，作為模糊推理的最終輸出值，相較于其他解模糊化方法，面積重心法具有更平滑的輸出推理控制。即使對應于輸入信號的微小變化，輸出也會發生變化。其數學表達式如式（18）所示：

當農機位置偏差d=-6 且航向角偏差θ=-6°時，轉向角n=5.09°。在對基本論域進行量化等級后，在[-6,6]的范圍內調節航向角偏差和位置偏差，即可通過解模糊推理獲取。

4 自行走控制系統的運動仿真

利用Matlab 對農機行間自行走模糊控制算法進行仿真驗證，仿真時設定將y=x的第一象限角平分線作為行間自行走的中心線即預定行駛路徑，仿真的初始位置設為（0,0）處，y＞x部分作為行間中心線左側，y＜x部分作為行間中心線右側。

圖6 的（a）-（c）為農機車身位置偏差d和航向角偏差θ幾種典型取值下的運動仿真模擬。

圖6 農機行間自行走運動仿真

結合式（17）的模糊函數表達式可知，農機行間自行走策略以消除農機車身位置偏差為主，δ的取值適當選擇較大。當位置偏差和航向角偏差均處于正大NB 或負大PB 時，農機需要快速進行行間車身位置調整，此時電動農機需要較大的轉向角n，根據文中建立的模型和公式，此時農機左右兩側的履帶速度差增大以向農機提供更大的轉向角度。當位置偏差處于正小NS 或負小PS 但航向角偏差處于正大NB或負大PB 時，此時農機不需要過多的車身位置調整，只需要及時對航向角偏差進行修正。當農機航向角偏差為零ZE 但存在位置偏差時，也應該及時對農機車身位置進行調整。

5 自行走控制系統的硬件設計

設計的自行走控制系統安裝在小型電動履帶式農機以便更好地將農機與農藝相結合，目前國內的作物種植大多數采用寬窄行種植技術[15]，大多數通過寬窄行種植的農作物其窄行寬度一般處于35～50 cm范圍內，窄行不適宜行走，而寬行的寬度一般根據農作物種植農藝的不同大體在90～130 cm 之間。所采用農機長160 cm，寬66 cm，高45 cm，選用橡膠履帶更適宜在復雜的田間環境行駛。

該農機動力由48 V/800 W 無刷直流電機提供，電機驅動相較于傳統的燃油發動機驅動，無刷直流電機的結構相對封閉，適宜在農田等惡劣環境下使用。其額定電流為20 A，額定扭矩為5 N·m，額定轉速為25 r/s。根據式（19）力矩公式計算單個電機產生的力：

直流電機軸直徑約為0.008 m，則力臂為電機軸的半徑為0.008/2=0.004 m，根據式（19）得到單電機產生的力約為1 250 N，代入重力系數算得：1 250 N/(9.8 N/kg)≈128 kg，其農機整體承受負載的質量約為250 kg。

電動農機采用58.2 Ah 的鉛酸蓄電池作為動力源。電機輸出功率計算如式（20）所示：

式（21）為電池放電，其中FAH 為電池放電時間，ZL 為電池放電電流值，NAH 為電池容量，0.85 為蓄電池放電效率，結合式（21）算得農機田間作業最大行駛距離約為7.5 km：

圖7 所示為超聲波車身檢測系統電路圖。超聲波傳感器選擇DJLK-003 收發一體超聲波模塊，平面小角度測距，工作的溫度范圍為-15～+60 ℃，探頭具備一定的防塵防水等級，適用于潮濕、惡劣的測量環境。通過RS485 協議發送車身位置距離值，RS485總線抗共模干擾能力強，最大數據傳輸速率可達10 Mbps，設置不同從機地址可同時采集四組距離值。

圖7 超聲波車身檢測系統電路圖

農機的行間自行走控制系統通過STM32 芯片對農機車身距離值進行解算后向電機驅動器發送調速電壓。由上文可知，農機差速轉向方式選用獨立式差速轉向，因此通過減法電路與前進電壓信號相減。行間作業時，由調速旋鈕電路持續向兩側電機驅動器輸出恒值為0～5.0 V 的調速電壓。當農機行駛位置偏離行間中心線時，主控芯片接收四組超聲波傳感器測量值后將位置偏差和航向角偏差作為輸入量，經模糊解算后輸出農機轉向角度，再根據轉向模型將-30°～30°農機轉向角轉換為左右履帶的速度差值；通過STM32 的D/A 輸出0～2.4 V 的電壓值，與調速旋鈕電路輸出電壓值相減輸出至近轉動瞬心側的電機驅動器，單側履帶速度降低。其電路原理圖如圖8 所示。

圖8 行間自行走控制系統電路圖

如圖9 所示對農機行間自行走控制系統進行Multisim14.0 電路仿真，調速旋鈕電路采用10 kΩ的分壓變阻器向電機驅動器輸出0～5 V可調電壓信號控制農機行間自行走時的前進速度；STM32的DAC為電壓輸出模式，可直接作為信號源，利用兩個10 kΩ的分壓變阻器模擬雙路DAC 輸出0～2.4 V 差速轉向電壓信號。當控制芯片通過解算判斷農機需要轉向修正前進方向時，PA4或PA5輸出一個轉向角度所對應的電壓信號，經過前級電壓跟隨電路后與調速旋鈕電路輸出的直行電壓信號通過LM358N 電壓減法電路相減，再將該電壓信號經二級電壓跟隨電路輸出至電機驅動器，左右兩側電機驅動器根據雙路D/A 輸出的不同而形成電壓差值；一側電機驅動器驅動電壓降低，另一側電機驅動器驅動電壓不變，即近轉動瞬心側的速度降低，遠轉動瞬心側速度不變，農機修正車身偏差。仿真時PA5控制左轉向，PA4控制右轉向，調速旋鈕持續輸出3.0 V電壓信號模擬農機前進速度，當PA5處DAC產生1.2 V模擬電壓信號時，向兩側電機驅動器分別輸出1.8 V 和2.97 V 電壓信號，左側電機轉速降低，右側電機轉速不變，農機行駛路線向左側修正[16]。

圖9 Multisim14.0電路仿真圖

6 試驗及分析

上文分別對行間自行走模糊控制策略和自行走控制系統電路進行仿真測試后，對農機進行田間實機測試。選擇在山西省大同市云州區黃花菜種植實驗基地實驗田進行試驗，其寬行寬度約為1.3 m。黃花菜作為一種經濟型作物，其采摘過程較為繁瑣，需要勞動者行走在行間進行采摘。

通過對人工黃花菜采摘作業的研究獲得最佳農機行駛速度約為0.83 m/s，在此速度下農機進行單趟距離為500 m 的采摘作業，耗時約為10 min，則20 min即可完成雙人4行黃花菜采摘，其效率約為5 min/行；對比人工步行采摘，雙人可同時采摘4 行黃花菜，但此時人工采摘步行速度約為0.3 m/s，則在人工步行采摘模式下，單趟距離為500 m 的采摘作業耗時為30 min，其效率約為7.5 min/行。通過試驗得：采用農機行間自行走對黃花菜進行采摘時效率提高約30%，小型電動農機的使用不僅不會對作物和土地造成較大破壞，還大幅度減輕農民的勞動壓力，優化農民的工作條件等[17]。

如圖10 和11 所示為單次黃花菜采摘作業時四個傳感器的距離采樣值，其中Echo1、Echo2、Echo3、Echo4 分別為左前側、左后側、右前側、右后側的超聲波傳感器距離采樣值；圖12 為傳輸至電機驅動器的左側調速電壓Vleft和右側調速電壓Vright的曲線。傳感器測量值受復雜外界環境干擾，對于農機行駛姿態和路徑的判斷困難，位置偏差和航向角偏差都較大時，農機會對路徑進行修正，且此時調速電壓變化幅度也較大，以便快速修正，在距離采樣值波動幅度不大的情況下默認為行間復雜的作業環境因素干擾。試驗結果表明：在500 m 的無干預農機自行走作業測試中，農機位置基本位于行中心線上。

圖10 左側前后傳感器距離采樣值

圖11 右側前后傳感器距離采樣值

圖12 左右電機驅動器調速電壓值

7 結論

該文所設計的基于超聲波測距的電動農機行間自行走策略旨在優化諸如黃花菜采摘等人工農業生產采摘方式，農機的智能化極大程度減輕農業生產者的勞動強度。通過距離傳感器和微控制器實時對農機行間行駛姿態進行判斷和修正，即使在復雜多變的田間環境下實機試驗，在不需要人為駕駛干預的情況下保證農機沿著行間道路行駛，解決了在大型農機無法進入田間作業時僅能依靠人力進行農業生產的現狀，未來可對該款農機進行多種改裝后，使其可承擔農作物采摘、行間載人及農產品運輸、行間植保噴藥等任務。