基于路徑彎曲度動態預瞄搜索算法的車輛路徑跟蹤控制軟件系統設計

摘要：為實現農業機械田間作業時的路徑跟蹤控制，在無人駕駛高速插秧機硬件系統基礎上，采用基于規劃路徑彎曲度的動態搜索預瞄算法設計了路徑跟蹤控制軟件。該算法以作業機械橫向偏差和航向偏差作為航向模糊控制器的輸入變量，以前輪轉角期望值為輸出變量，設計航向模糊控制器，實現航向控制；同時結合轉向模糊免疫PID控制器對步進電機的PWM頻率進行控制，實現水田作業機械的方向改變，從而完成作業機械沿規劃路徑自動行駛跟蹤，并用Matlab/Simulink仿真平臺對所采用的路徑跟蹤控制原理和所設計的模糊控制器進行了有效性驗證，結果表明所采用的控制方法是可行的。經試驗，插秧機以1 m/s的速度進行直線跟蹤時最大的跟蹤偏差只有4 cm，且始終圍繞零值附近上下波動，計算平均跟蹤偏差為0.84 cm，能夠滿足水田作業機械路徑跟蹤控制的要求。

關鍵詞：自動導航；農業機械；軟件設計；路徑規劃；路徑跟蹤算法

中圖分類號： U463.6文獻標志碼： A文章編號：1002-1302（2015）02-0370-04

收稿日期：2014-04-22

基金項目：貴州省科學技術基金（編號：黔科合J字LKZS[2014]24）；貴州省科學技術重點項目（編號：黔科合J字LKZS[2014]05）；貴州省級重點學科資助課題（編號：黔學位辦[2013]18）；貴州省遵義師范學院教研基金（編號：13-56）。

作者簡介：熊中剛（1986—），男，湖南常德人，碩士，講師，主要從事電氣自動化和農業機械智能控制方面的研究。E-mail：xzglsl2013@163.com。隨著計算機技術和傳感器技術的不斷發展，農業機械自動導航技術在很多發達國家和地區已有了很深入的研究，但亞洲除了日本、韓國和中國臺灣外，其他國家和地區在這一領域研究較少[1]。農業機械自動導航技術是精細農業的基礎平臺，也是當前農業工程領域的研究熱點之一[2]。然而在自動導航過程中，輪式機械在田間作業時需要實現地頭轉向和直線行走2個動作。地頭轉向是農業機械完成當前行作業，通過姿態調整以精確進入下一行的重要步驟，大多情況是一種180°轉向動作，與直線行走有著明顯不同[3-4]。目前，完成上述2個動作采用的車輛跟蹤控制的方法主要有基于PID控制算法和基于模糊控制算法等[5]。本設計采用的是基于路徑彎曲度動態預瞄搜索算法的車輛路徑跟蹤方法，該方法不僅可以使作業機械滿足直線行走，而且可以提高農業機械換行作業的精度，縮短農業機械在地頭轉向中的時間，最終提高整個農業機械作業的效率。

1系統總體原理設計

路徑跟蹤就是車輛在獲得已經規劃好的目標路徑的前提下，根據車輛當時的行駛狀態和它與目標路線間的相對位置關系，按照一定的控制法則綜合車輛的行駛速度和行駛方向，使車輛的運動軌跡與目標路徑的偏離誤差滿足設計的要求。車輛的路徑跟蹤控制是根據車輛即時偏離行駛路線的橫向偏差決定車輛的行駛方向。車輛的航向跟蹤控制是在轉向控制的基礎上，使車輛沿目標航向角行駛，即實現車輛行駛方向[6]。本設計采用的車輛路徑跟蹤控制方法主要是基于規劃路徑彎曲度來模糊確定前視距離即預估距離的大小，然后由前視距離找到預瞄點，由預瞄點和車輛當前位置確定目標方向，由目標方向跟當前的航向角之間的差值得出航向偏差，橫向偏差就是車輛當前的坐標與預定義路線的投影。

2軟件理論設計

2.1基于規劃路徑彎曲度預瞄前視距離方法

當駕駛員在實際駕駛過程中，不僅要觀察車輛當前的道路狀況，而且要同時考慮車輛前方一定距離范圍內的道路信息，而前方路徑的彎曲程度是決定車輛的轉彎方向、轉角大小及車速大小的重要因素[7].在路徑跟蹤控制中，預瞄距離的選擇原則跟車輛的行駛速度有關，行駛速度快時，需要預瞄的距離就要大些，而車速慢時，距離小些就會達到控制效果。根據這一思想，路徑的彎曲率的確定是進行路徑預瞄的第一步，彎曲率的確定方法[8]如圖1所示。

如圖1所示，圖中β1是路徑第1條折線和車輛前進方向的夾角改變量，βi是第i條折線和第i-1條折線的方向角改變量。定義C為所有區域路徑的彎曲度和，則有：

C=∑ni=1|βi|。（1）

路徑順時針方向變化時，βi為負值，反之為正值。 βi取絕對值的意義是保證路徑的彎曲不管是什么變化，其彎曲度是增加的，即彎曲的效果一致。采取文獻[8]所述的車速v、預瞄距離d確定方法隨C的變化而變化，其變化規律遵循C越大，v、d越小的原則，方法如下：

預瞄距離確定后，預瞄點及車輛的航向偏差、橫向偏差也隨之確定，這樣就為車輛的轉向控制提供參數，實現路徑跟蹤。在實際預估的前視距離中還應包括車輛的本身軸距長度和車輛行駛的速度。

2.2路徑跟蹤航向偏差和橫向偏差確定

當確定好前視距離后，就可確定跟蹤路徑上的預瞄點，如圖2所示。

插秧機當前航向方位θd上，插秧機當前的位置點Dq加上前視距離Lf得到點Ym，點Ym在規劃路徑上的投影點為Yg，即為預瞄點，則當前目標方向θm為：

3軟件功能的實現

3.1軟件功能設計及工作流程

由于VB是一種可視化的、面向對象和采用事件驅動方式的結構化的高級程序語言，本研究以VB作為編程工具，進行控制軟件的編寫，根據系統設計功能的要求，本研究設計的軟件主要由通訊模塊、控制決策模塊、顯示模塊、數據存貯模塊等組成。通訊模塊主要實現對水田作業機械的位置、航向角、前輪轉角、速度等模塊的信息采集；控制決策模塊將所設計的各類模糊控制器、各類控制算法用計算機語言表述，并進行控制決策，實現作業機械的路徑跟蹤控制；顯示模塊實時顯示作業機械的航向角、前輪轉角、橫向偏差、車速等信息，并能實時做出作業機械的路徑跟蹤圖與規劃路徑圖，數據存貯模塊主要是完成對采集數據的存貯，為以后的數據分析和控制調整提供數據參考，其系統軟件功能實現流程圖如圖3所示。endprint

3.2串行通信設計

系統軟件采用SerialPort控件來進行串行通訊，該控件主要包括BaudRate、PortName、Parity、DataBitss、StopBit等屬性，這幾個屬性主要用來設置波特率、端口號、奇偶校驗、數據位以及停止位；主要方法有Read、Write、Open、Close、ReadtoBytes等用來讀數據、寫數據、打開端口、關閉端口和讀取字節數；主要事件為DataReceived。串口的初始化主要是對SerialPort控件[9-10]的屬性進行設置。系統軟件的通訊參數設置部分主要用來進行串行端口的設置，波特率的設置、奇偶校驗設置以及數據位的設置等。串行端口的選擇可以根據當前PC機上可用串行端口的端口號；波特率是數據傳輸的速率，在該系統中默認為9 600 bps，可以根據需要選擇其他的波特率；為提高數據的準確率，可使用奇偶校驗來進行控制，奇偶校驗設置根據需要可從Odd、Even、Mark、Space、None等幾種方式中選擇，本軟件系統默認為None的方式；數據位的設置用于設置每個字節中數據位的長度通常為5～8位，默認為8位。所有參數設置完成后利用SerialPort控件的Open方法就能打開通訊串口。

VisualBasic.net2008中進行串行通訊時，對于通訊數據的發送采用SerialPort的Write方法完成。然而所采用的Write方法主要分為3種形式，它們分別是Write（String）、Write（Byte（），Offset（），Count（））和Write（Char（），Offset（），Count（））。發送方式為SerialPort1.啟動定時器。在Timer的Tick事件中就可以用 Write（Byte（），Offset（），Count（）），該命令語句中的Byte（）表示的含義是一個字節型的數組，Offset（）表示的含義是字節數組中從0開始的字節偏移量，Count（）表示的含義是所要寫進命令語句中的字節數。

該系統串口通信中數據的接收主要可以分為DataReceived驅動和利用定時器完成2種方式。第1種數據接收的方式就是通過DataReceived事件驅動的方式，該方式主要是在串行端口初始化的時候，對ReceivedBytesThreshold的屬性值進行設置，所設置的該屬性值即是表示對觸發DataReceived事件進行設置的時候，其緩沖區所接收到的字節數。系統中默認ReceivedBytesThreshold的屬性為1，通過測試實驗證明了如果ReceivedBytesThreshold的屬性設為1時，系統不能完全確保DataReceived 事件被接收到的每個字節都能引發，只有屬性值大于1的ReceivedBytesThreshold能夠完全按照要求把DataReceived事件進行觸發。因此為了獲取接收緩沖區中的數據字節數，當DataReceived事件產生后須要立即讀取 BytesToRead 屬性值，然后再根據通信需求將緩沖區中的相應字節讀取出來。在VisualBasic.net2008中，如果要更改相應控件的屬性和處理數據，則須要采用委托的方式進行。因為當數據從SerialPort對象上獲取時，引發的相應DataReceived事件是發生在輔助線程上，如果要將主線程中的控件屬性或其他UI元素等直接在DataReceived過程中進行修改，將會引發線程異常錯誤。

第2種就是利用定時器的進行數據接收的方式。假設任意一串數據的接收時間為tms，則該定時器Timer的Interval屬性值即為tms，接著可以在一個Button的Click事件后，立刻把接收緩沖區中的字節通過SerialPort控件的Read方法讀取出來，而且在Timer的Tick事件中可以直接對窗體控件的相關屬性進行修改，不需要采取委托的方式。在對緩沖區中的數據進行讀取時，因為采取的是字節型的數據傳輸方式，所以應采用Read（Byte（），offset（），count（））的方式完成。

4路徑跟蹤控制仿真設計

利用插秧機的運動學模型和所設計的路徑跟蹤算法，在基于航向跟蹤控制系統的基礎上，應用Matlab的Simulink環境進行仿真研究[11]，建立插秧機路徑跟蹤控制系統的仿真框圖，如圖4所示。

對直線y=3x進行跟蹤仿真，所需的基本參數設置如下：由于規劃路徑為直線，所以C=0，則v=1.5 m/s，預瞄距離為4 m，被控對象在XOY坐標系中的初始位置為（0，0），初始航向角指向y軸的正方向。由此得出如圖5的仿真圖。從仿真結果圖中可以看出在初始的一段距離內存在較大的穩態跟蹤誤差，研究中發現這是因為模型實際航向與規劃路徑存在夾角，經過控制器的調整后，完成了之后很平滑的直線跟蹤。

5路徑跟蹤控制試驗分析

實現路徑跟蹤是評價控制系統設計好壞的最終標準，因此本研究進行了相應的路徑跟蹤控制試驗。

首先人工開動插秧機到不同的兩點如A、B處獲得坐標，并連接A、B兩點構成一條直線，以此直線為坐標的x軸，將插秧機的前后軸中心線與基準線重合，航向角與x軸方向一致。同時在插秧機的后軸中心點掛上一裝有白色石灰的漏斗，當插秧機以1 m/s的速度自動行駛時，所得的跟蹤偏差與行駛距離變化曲線如圖6所示。

從圖中可以看出，插秧機以1 m/s的速度進行直線跟蹤時最大的跟蹤偏差只有4 cm，且始終圍繞零值附近上下波動，計算平均跟蹤偏差為0.84 cm。所設計的系統控制器具有良好的控制性能，能滿足水田作業機械的直線跟蹤控制的要求。

6結論

本研究在分析了作業機械路徑跟蹤預瞄的原理基礎上，設計了基于規劃路徑彎曲度動態預瞄搜索算法的路徑跟蹤控制器，為水田作業機械的自動導航控制注入了靈魂。通過對插秧機的路徑跟蹤控制進行試驗研究，結果表明基于自動導航的水田作業機械路徑跟蹤控制精度較高，所設計的控制系統性能良好，能滿足水田作業機械的路徑跟蹤控制系統要求。endprint

參考文獻：

[1]Reid J F，Zhang Q，Noguchi N，et al. Agricultural automatic guidance research in North America[J]. Computers and Electronics in Agriculture，2000，25（1/2）：155-167.

[2]Keicher R，Seufert H. Automatic guidance for agricultural vehicles in Europe[J]. Computers and Electronics in Agriculture，2000，25（1/2）：169-194.

[3]張智剛，羅錫文，李俊嶺. 輪式農業機械自動轉向控制系統研究[J]. 農業工程學報，2005，21（11）：77-80.

[4]于海業，馬成林，并河清，等. 農用自動引導行走車的實驗研究[J]. 農業工程學報，1998，14（2）：36-39.

[5]周建軍，張漫，汪懋華，等. 基于模糊控制的農用車輛路線跟蹤[J]. 農業機械學報，2009，40（4）：151-156.

[6]張智剛，羅錫文. 農業機械導航中的航向角度估計算法[J]. 農業工程學報，2008，24（5）：110-114.

[7]李慶中，顧偉康，葉秀清，等. 移動機器人路徑跟蹤的智能預瞄控制方法研究[J]. 機器人，2002，24（3）：252-255.

[8]陳無畏，李進，王檀彬，等. 視覺導航智能車輛的路徑跟蹤預瞄控制[J]. 機械工程學報，2008，44（10）：277-282.

[9]龔沛曾，陸慰民，楊志強. Visual Basic程序設計簡明教程[M]. 北京：高等教育出版社，2001：218-220.

[10]蔣加伏，張林峰. Visual Basic 程序設計教程[M]. 北京：北京郵電大學出版社，2006：128-130.

[11]石辛民，郝整清. 模糊控制及其MATLAB仿真[M]. 北京：清華大學出版社，2008：127-129.鄭子松，姜永平，李綱，等. 南京地區玻璃溫室中地源熱泵的加溫效果及效益分析[J]. 江蘇農業科學，2015，43（2）：374-377.endprint