基于路徑彎曲度動態(tài)預(yù)瞄搜索算法的車輛路徑跟蹤控制軟件系統(tǒng)設(shè)計

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

關(guān)鍵詞：自動導(dǎo)航；農(nóng)業(yè)機(jī)械；軟件設(shè)計；路徑規(guī)劃；路徑跟蹤算法

中圖分類號： U463.6文獻(xiàn)標(biāo)志碼： A文章編號：1002-1302（2015）02-0370-04

收稿日期：2014-04-22

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

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

1系統(tǒng)總體原理設(shè)計

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

2軟件理論設(shè)計

2.1基于規(guī)劃路徑彎曲度預(yù)瞄前視距離方法

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

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

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

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

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

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

當(dāng)確定好前視距離后，就可確定跟蹤路徑上的預(yù)瞄點，如圖2所示。

插秧機(jī)當(dāng)前航向方位θd上，插秧機(jī)當(dāng)前的位置點Dq加上前視距離Lf得到點Ym，點Ym在規(guī)劃路徑上的投影點為Yg，即為預(yù)瞄點，則當(dāng)前目標(biāo)方向θm為：

3軟件功能的實現(xiàn)

3.1軟件功能設(shè)計及工作流程

由于VB是一種可視化的、面向?qū)ο蠛筒捎檬录?qū)動方式的結(jié)構(gòu)化的高級程序語言，本研究以VB作為編程工具，進(jìn)行控制軟件的編寫，根據(jù)系統(tǒng)設(shè)計功能的要求，本研究設(shè)計的軟件主要由通訊模塊、控制決策模塊、顯示模塊、數(shù)據(jù)存貯模塊等組成。通訊模塊主要實現(xiàn)對水田作業(yè)機(jī)械的位置、航向角、前輪轉(zhuǎn)角、速度等模塊的信息采集；控制決策模塊將所設(shè)計的各類模糊控制器、各類控制算法用計算機(jī)語言表述，并進(jìn)行控制決策，實現(xiàn)作業(yè)機(jī)械的路徑跟蹤控制；顯示模塊實時顯示作業(yè)機(jī)械的航向角、前輪轉(zhuǎn)角、橫向偏差、車速等信息，并能實時做出作業(yè)機(jī)械的路徑跟蹤圖與規(guī)劃路徑圖，數(shù)據(jù)存貯模塊主要是完成對采集數(shù)據(jù)的存貯，為以后的數(shù)據(jù)分析和控制調(diào)整提供數(shù)據(jù)參考，其系統(tǒng)軟件功能實現(xiàn)流程圖如圖3所示。endprint

3.2串行通信設(shè)計

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

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

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

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

4路徑跟蹤控制仿真設(shè)計

利用插秧機(jī)的運動學(xué)模型和所設(shè)計的路徑跟蹤算法，在基于航向跟蹤控制系統(tǒng)的基礎(chǔ)上，應(yīng)用Matlab的Simulink環(huán)境進(jìn)行仿真研究[11]，建立插秧機(jī)路徑跟蹤控制系統(tǒng)的仿真框圖，如圖4所示。

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

5路徑跟蹤控制試驗分析

實現(xiàn)路徑跟蹤是評價控制系統(tǒng)設(shè)計好壞的最終標(biāo)準(zhǔn)，因此本研究進(jìn)行了相應(yīng)的路徑跟蹤控制試驗。

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

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

6結(jié)論

本研究在分析了作業(yè)機(jī)械路徑跟蹤預(yù)瞄的原理基礎(chǔ)上，設(shè)計了基于規(guī)劃路徑彎曲度動態(tài)預(yù)瞄搜索算法的路徑跟蹤控制器，為水田作業(yè)機(jī)械的自動導(dǎo)航控制注入了靈魂。通過對插秧機(jī)的路徑跟蹤控制進(jìn)行試驗研究，結(jié)果表明基于自動導(dǎo)航的水田作業(yè)機(jī)械路徑跟蹤控制精度較高，所設(shè)計的控制系統(tǒng)性能良好，能滿足水田作業(yè)機(jī)械的路徑跟蹤控制系統(tǒng)要求。endprint

參考文獻(xiàn)：

[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]張智剛，羅錫文，李俊嶺. 輪式農(nóng)業(yè)機(jī)械自動轉(zhuǎn)向控制系統(tǒng)研究[J]. 農(nóng)業(yè)工程學(xué)報，2005，21（11）：77-80.

[4]于海業(yè)，馬成林，并河清，等. 農(nóng)用自動引導(dǎo)行走車的實驗研究[J]. 農(nóng)業(yè)工程學(xué)報，1998，14（2）：36-39.

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

[6]張智剛，羅錫文. 農(nóng)業(yè)機(jī)械導(dǎo)航中的航向角度估計算法[J]. 農(nóng)業(yè)工程學(xué)報，2008，24（5）：110-114.

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

[8]陳無畏，李進(jìn)，王檀彬，等. 視覺導(dǎo)航智能車輛的路徑跟蹤預(yù)瞄控制[J]. 機(jī)械工程學(xué)報，2008，44（10）：277-282.

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

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

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