專利名稱：一種農(nóng)業(yè)機械的導航控制方法
技術(shù)領(lǐng)域：
本發(fā)明涉及農(nóng)業(yè)機械領(lǐng)域，特別涉及一種農(nóng)業(yè)機械的導航控制方法。
背景技術(shù)：
農(nóng)業(yè)機械導航控制的主要任務(wù)是根據(jù)導航定位結(jié)果，確定農(nóng)業(yè)機械與預定路線的位置關(guān)系，進而結(jié)合農(nóng)業(yè)機械的運動狀態(tài)決策出合適的轉(zhuǎn)向輪操縱角，以修正路徑跟蹤誤差。國外常用的導航控制方法有三種，即線性模型控制方法、最優(yōu)控制方法和模糊控制方法。東京大學利用機器視覺技術(shù)進行KUBOTA SPJ45A型久保田插秧機的導航控制技術(shù)研究，他將目標方向角度與車輛縱向方向角度進行對比，用Abe的線性轉(zhuǎn)向控制模型計算得出轉(zhuǎn)向輪轉(zhuǎn)向角度。Qiu提出了一種基于預瞄的導航控制算法，該導航控制器包括一個前饋比例控制器和反饋PD控制器；前進速度決定前視距離，位置誤差決定前饋增益；前饋控制器的輸入是路徑曲率，反饋控制器的輸入是航向偏差，兩控制器的輸出合成為期望轉(zhuǎn)向輪操縱角。東京大學采用模糊控制技術(shù)來進行導航控制研究，模糊控制器根據(jù)方向偏差和位置偏差確定轉(zhuǎn)向角度和左轉(zhuǎn)執(zhí)行時間及右轉(zhuǎn)執(zhí)行時間。韓國的Lee等人利用機器視覺、DGPS、超聲波傳感器和模糊控制技術(shù)進行果樹噴霧機的導航控制研究。O’Connor等人采用方向偏差、方向偏差變化率、轉(zhuǎn)向角度、轉(zhuǎn)向角度變化率、跟蹤誤差等五個狀態(tài)變量建立車輛運動學方程，以轉(zhuǎn)向角度變化率最小為優(yōu)化指標，利用車輛線性運動學方程組建立了基于LQR的最優(yōu)導航控制器。Kise等人開發(fā)了最優(yōu)操縱控制器用于農(nóng)用車輛導航，在曲線路徑跟蹤方面取得了一定的效果。周俊(2003)提出了視覺導航的圖象處理技術(shù)，同時還利用線性狀態(tài)反饋控制方法、二值控制方法建立了導航控制的內(nèi)外環(huán)結(jié)構(gòu)；在車輛縱向速度較高的狀態(tài)下，提出了把橫向偏差、航向偏差以及橫向加速度作為輸入量的三維橫向模糊控制算法，以保證車輛橫向運動的平穩(wěn)性。毛恩榮等人(2005)研究了在鐵牛654拖拉機上搭建DGPS自動駕駛系統(tǒng)的硬件組成及關(guān)鍵技術(shù)，根據(jù)最優(yōu)控制理論，研究了橫向偏差和航向角輸出反饋控制系數(shù)的離線優(yōu)化調(diào)節(jié)方法，為試驗湊試最優(yōu)控制參數(shù)提供依據(jù)。本發(fā)明人在2005年將GPS技術(shù)、計算機技術(shù)、傳感器技術(shù)進行集成，研制了一種以蓄電池為電源、電動機為動力的農(nóng)用智能移動作業(yè)平臺，該樣機采用前輪驅(qū)動、差速轉(zhuǎn)向方式；并以DGPS、電子羅盤為主要導航傳感器，在樣機上建立了DGPS導航控制系統(tǒng)；還設(shè)計了基于預瞄跟隨的導航控制算法，其核心是由航向偏差線性決定驅(qū)動輪速度差。由于該智能移動作業(yè)平臺采用差速轉(zhuǎn)向方式，路徑跟蹤的控制難度較大，導航精度難以提高。
常規(guī)線性模型控制方法，包括PID控制方法，可以獲得高精度的路徑跟蹤效果，但是抗干擾能力弱。模糊控制方法具有良好的穩(wěn)定性效果，但路徑跟蹤的穩(wěn)態(tài)精度難以保證。最優(yōu)控制器的建立主要依據(jù)農(nóng)業(yè)機械運動學和動力學模型，其模型精確性對控制器性能有重要影響。但是，田間作業(yè)條件下農(nóng)業(yè)機械運動學和動力學模型的建立是比較困難的。田間作業(yè)時，隨著農(nóng)業(yè)機械、作業(yè)機具與地面相互作用的變化，田間土壤特性的變化，農(nóng)業(yè)機械運動特性必然是時變的，還有作業(yè)工況差引起的傳感器測試干擾等，這些因素對常規(guī)線性模型控制方法和最優(yōu)控制方法的影響都較大。

發(fā)明內(nèi)容
本發(fā)明的目的在于克服現(xiàn)有技術(shù)中存在的缺點，提供一種導航跟蹤精度高、穩(wěn)定性好的農(nóng)業(yè)機械的導航控制方法。該方法基于預瞄跟隨理論，采用PID和模糊邏輯相結(jié)合的方法來設(shè)計路徑跟蹤的控制算法，充分利用PID路徑跟蹤的高精度和模糊控制的穩(wěn)定性特點，建立穩(wěn)定性好、精度高的路徑跟蹤控制算法。
本發(fā)明一種農(nóng)業(yè)機械的導航控制方法，包括下述步驟(1)通過導航傳感器測量得到農(nóng)業(yè)機械當前時刻的位姿和運動狀態(tài)數(shù)據(jù)；(2)采用基于預瞄控制的動態(tài)目標點搜索算法，將所述位姿和運動狀態(tài)數(shù)據(jù)與預定義路線數(shù)據(jù)進行對比分析，推算出兩個導航狀態(tài)參數(shù)即橫向跟蹤誤差和縱向航向偏差；(3)再通過Fuzzy-PID控制算法推算出操縱控制量即前輪轉(zhuǎn)向偏角；(4)最后根據(jù)所述操縱控制量對農(nóng)業(yè)機械進行實時控制，實現(xiàn)路徑跟蹤。
所述的預定義路線P[N]由點數(shù)組表示，包含元素為{P0，P1，...，Pk-1，Pk，Pk+1，...，PN}
其數(shù)學表達式為{(x0，y0)，(x1，y1)，...，(xk-1，yk-1)，(xk，yk)，(xk+1，yk+1)，...，(xn，yn)}在GIS中，預定義路線由點數(shù)據(jù)集表示。Pk點的坐標(xk，yk)為WGS-84大地坐標系的高斯投影平面坐標。點與點之間的平均間距表示為Ls。本發(fā)明中所述坐標點均為WGS-84大地坐標系的高斯投影平面坐標。
所述導航傳感器包括差分全球定位系統(tǒng)(DGPS)接收模塊，機器視覺定位，超聲波、激光等測距定位，以及陀螺儀、電子羅盤、速度計、里程計等航位推算定位(DR)設(shè)備。
所述的農(nóng)業(yè)機械當前時刻的位姿和運動狀態(tài)數(shù)據(jù)包括當前定位點、當前航向角度和當前速度，如圖1所示，具體定義如下(1)當前定位點Pc由DGPS接收機獲得的當前時刻差分定位數(shù)據(jù)，以(xc，yc)表示；(2)當前航向角度θ由電子羅盤數(shù)據(jù)獲得的當前時刻農(nóng)業(yè)機械縱向方向的角度；以高斯投影平面坐標系xoy中x軸正向為0度，逆時針方向為正；(3)當前速度V由速度傳感器獲得的當前時刻農(nóng)業(yè)機械縱向方向的前進速度。
所述的基于預瞄控制的動態(tài)目標點搜索算法，是通過動態(tài)計算前視距離，確定預定義路線上的預瞄點，具體步驟如下(1)前視距離Lf的確定如圖1所述，前視距離Lf可解釋為在農(nóng)業(yè)機械當前縱向方向上的預瞄距離；考慮農(nóng)業(yè)機械速度的影響，前視距離Lf可通過下述公式計算得到Lf＝L0+KlfvV+KlfrRdf(1)其中，L0為前視距離基值；Klfv，Klfr分別為前進速度、前視偏差變化率的比例系數(shù)，可在仿真或?qū)嶒炦^程中通過整定獲得，Klfv與Klfr均取正值，Klfv與Klfr的取值范圍為0≤Klfv≤1，0≤Klfr≤1；Rdf為前視偏差變化率，定義為Rdf=|Pk+2m+P′k+2m→|-|PkP′k→||Pk+mP′k+m→|-|PkP′k→|---(2)]]>其中m=int(VTsLs),]]>Pk表示預定義路線上的第k坐標點，P′k映射點為在直線PkPk-1上以Pk為垂足引垂線和農(nóng)業(yè)機械當前縱向方向線的相交點，Ts為導航的采樣和控制周期，Ls為預定義路線上點與點之間的平均間距；(2)預瞄點P′p的確定在農(nóng)業(yè)機械當前縱向方向上，由當前定位點Pc點起截取前視距離Lf，得到Pp點；再由Pp點向預定義路線投影，得到預瞄點P′p。
所述的兩個導航狀態(tài)參數(shù)即橫向跟蹤誤差和縱向航向偏差的推算過程如下(1)縱向航向偏差θe的確定縱向航向偏差θe為目標方向θp與當前農(nóng)業(yè)機械航向角度θ之間的差值；其中目標方向θp為矢量PcP′p的方向，以高斯投影平面坐標系xoy中x軸正向為0度，逆時針方向為正；(2)橫向跟蹤誤差XTE的確定如圖1所示，設(shè)Pc到預定義路線的投影是P′c，則XTE＝|PcP′c|，即XTE=(xc-x′c)2+(yc-y′c)2---(3)]]>為了導航控制算法設(shè)計的方便，這里對XTE規(guī)定正負號，在農(nóng)業(yè)機械沿預定義路線的前進方向上，若農(nóng)業(yè)機械當前坐標點位于預定義路線的右側(cè)，XTE為正；若當前坐標點位于預定義路線的左側(cè)，XTE為負；判斷XTE正負號的方法是如圖2所示，首先在預定義路線P[N]中，尋找到距離當前定位點Pc最近的節(jié)點Pk，沿預定義路線前進方向?qū)ふ蚁乱还?jié)點Pk+1；規(guī)定向量 到 的夾角順時針為負，逆時針為正；若向量 到 的夾角為正，表明當前坐標點位于預定義路線的左側(cè)，XTE為負；若向量 到 的夾角為負，表明當前坐標點位于預定義路線的右側(cè)，XTE為正。
所述通過Fuzzy-PID控制算法推算出操縱控制量即前輪轉(zhuǎn)向偏角的具體方法步驟是當位置偏差在區(qū)間[-a，a]且航向偏差在區(qū)間[-b，b]時，為PID控制方法的作用范圍；其他區(qū)間為模糊邏輯方法Fuzzy的作用范圍，實現(xiàn)兩種決策算法的復合控制；其中，a和b的取值范圍為0≤a≤0.5，0≤b≤45，其原理如圖7所示。
其中，PID控制方法的算法為該算法的實質(zhì)是將位置偏差和航向偏差轉(zhuǎn)換為合成誤差ERROR，并將合成誤差ERROR作為控制輸入量輸入到PID控制器，然后決策輸出前輪轉(zhuǎn)向偏角，達到在路徑跟蹤過程中既消除航向偏差又消除位置誤差的目的；ERROR定義為ERROR＝α×XTE+β×θe(4)其中，α和β分別為橫向跟蹤誤差、縱向航向偏差相對于合成誤差的折算因子，α和β的確定主要根據(jù)試驗效果進行現(xiàn)場整定，其中α和β的取值范圍為0≤α≤10，0≤β≤1。
采用增量式PID控制算法來決策前輪轉(zhuǎn)向偏角，具體如下
Ui=K[Ei-Ei-1+TsTiEi+TdTs(Ei-2Ei-1+Ei-2)]+Ui-1---(5)]]>其中，Ui、Ui-1為第i、i-1時刻的轉(zhuǎn)向輪期望偏角，Ei、Ei-1、Ei-2為第i、i-1、i-2時刻的轉(zhuǎn)向輪期望偏角與實際偏角之差。Ts為采樣周期，K為比例系數(shù)，Td為微分時間常數(shù)，Ti為積分時間常數(shù)；K、Td、Ti三參數(shù)根據(jù)試驗效果進行現(xiàn)場整定，其取值范圍為0≤K≤10，0≤Td≤1，0≤Ti≤0.5。
本發(fā)明將基于合成誤差的PID決策算法用于農(nóng)業(yè)機械導航，決策前輪轉(zhuǎn)向偏角，對提高導航系統(tǒng)性能具有一定作用。PID控制器的比例調(diào)節(jié)系數(shù)K起偏角決策作用，而其中的微分環(huán)節(jié)可有效抑制導航控制系統(tǒng)中的振蕩和不穩(wěn)定，積分環(huán)節(jié)對于消除導航控制系統(tǒng)中的系統(tǒng)偏差將起到重要作用。PID決策算法不僅繼承了傳統(tǒng)線性決策模型中零穩(wěn)態(tài)跟蹤誤差的重要特性，而且對于提高系統(tǒng)穩(wěn)定性也具有重要作用。
其中，使用模糊邏輯Fuzzy方法來決策轉(zhuǎn)向輪偏角的思路是將駕駛員的駕駛經(jīng)驗轉(zhuǎn)化為模糊規(guī)則，建立模糊邏輯控制器，實現(xiàn)前輪轉(zhuǎn)向偏角控制量決策。駕駛經(jīng)驗轉(zhuǎn)化為模糊規(guī)則的步驟是將農(nóng)業(yè)機械與預定義路線之間的位置關(guān)系分為九種狀態(tài)，如圖3所示；根據(jù)駕駛員控制作業(yè)機械沿預定路線行駛的經(jīng)驗，建立了相應(yīng)的轉(zhuǎn)向控制策略，如表1所示；控制強度指轉(zhuǎn)向輪偏轉(zhuǎn)幅度，與期望轉(zhuǎn)向輪偏角成正比；控制方向為前輪偏轉(zhuǎn)方向，向左時期望偏角為正，向右時期望偏角為負；據(jù)此建立模糊控制規(guī)則表，如表2所示。
在Matlab環(huán)境下設(shè)計該模糊邏輯控制器的具體步驟如下(1)首先將輸入變量模糊化在Matlab環(huán)境下，建立模糊變量橫向跟蹤誤差XTE，方位偏差θe的隸屬函數(shù)曲線，如圖4(a)、(b)所示；建立模糊變量轉(zhuǎn)向控制量δp的隸屬函數(shù)曲線，如圖5所示；對應(yīng)于轉(zhuǎn)向模糊控制規(guī)則表，利用Matlab軟件繪制出模糊控制表三維外觀圖，如圖6所示。
其中，XTE、θe和δp作為模糊語言變量時，其模糊子集分別選為Fxte＝{NB，NM，NS，NO，PO，PS，PM，PB}Fθe={NB,NM,NS,NO,PO,PS,PM,PB}]]>Fδp={NB,NM,NS,O,PS,PM,PB}]]>語言變量XTE和θe組成模糊輸入空間，δp為模糊輸出空間，輸入和輸出空間的模糊分割分別為Fxte分為8級，即負大、負中、負小、負零、正零、正小、正中和正大；Fθe分為8級，即負大、負中、負小、負零、正零、正小、正中和正大；Fδp分為7級，即負大、負中、負小、零、正小、正中和正大。
選取橫向跟蹤誤差XTE、方位偏差θe和轉(zhuǎn)向控制量δp的離散論域分別為XTE＝{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}；θe＝{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}；δp＝{-7，-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6，7}；模糊推理系統(tǒng)的基本屬性確定為“與”運算采用極小運算，“或”運算采用極大運算，模糊蘊涵采用極小運算，模糊規(guī)則綜合采用極大運算，去模糊化采用重心法?；具B續(xù)論域離散化采用均勻量化的方法，即將連續(xù)論域中的連續(xù)值經(jīng)量化因子比例變換后四舍五入變?yōu)殡x散論域的整數(shù)值，具體算式如下XTE′＝int(KxteXTE)(6)θe′＝int(Kθeθe)(7)上兩式中，輸入量化因子Kxte、Kθe的確定方法是根據(jù)試驗測試結(jié)果，確定橫向跟蹤誤差XTE、方位偏差θe的誤差范圍，由離散論域最大值除于相應(yīng)誤差范圍最大值即可獲得量化因子值。
(2)然后模糊控制表查詢打開規(guī)則觀測器，輸入不同的離散量XTE′，θe′，得到對應(yīng)的清晰控制量δp，進而構(gòu)成模糊控制表，如表3所示。
(3)最后輸出去模糊控制量從模糊控制量到實際輸出控制量的轉(zhuǎn)換算式如下δp′=Kδpδp---(8)]]>上式中，輸出比例因子Kδp的確定方法同輸入量化因子。
由于PID控制方法有利于提高路徑跟蹤的穩(wěn)態(tài)精度，但不能提高路徑跟蹤的穩(wěn)定性，抗干擾能力較差；模糊邏輯控制方法有利于提高路徑跟蹤的穩(wěn)態(tài)性，但不能顯著提高路徑跟蹤的精度。PID控制方法和模糊邏輯控制方法在路徑跟蹤控制過程中存在互補性。因此，本發(fā)明采用PID和FUZZY的轉(zhuǎn)向輪偏角復合決策算法來進行實際控制。實際應(yīng)用中，可根據(jù)試驗效果對兩個偏差區(qū)間進行合理調(diào)節(jié)，以確定PID和FUZZY控制的相應(yīng)作用范圍。
本發(fā)明與現(xiàn)有技術(shù)相比具有如下優(yōu)點和效果(1)本發(fā)明充分利用傳統(tǒng)導航控制方法的優(yōu)點，結(jié)合模糊控制方法，兼顧穩(wěn)態(tài)跟蹤精度和控制穩(wěn)定性，提高了農(nóng)業(yè)機械作業(yè)條件下的導航跟蹤精度、穩(wěn)定性和安全性。
(2)本發(fā)明可以控制農(nóng)業(yè)機械沿著預定義路線或者地物標志線自動行走，路徑跟蹤誤差小于30cm。
(3)本發(fā)明適用于智能農(nóng)業(yè)機械裝備的輔助或者自動導航、變量處方作業(yè)、農(nóng)情信息采集的場合；不僅適用于旱地農(nóng)業(yè)機械的作業(yè)需要，還適用于水田農(nóng)業(yè)機械的作業(yè)需要，具有廣闊的推廣應(yīng)用前景。


圖1為農(nóng)業(yè)機械與預定義路線的位置關(guān)系解析原理2為XTE正負號的確定方法圖3為農(nóng)業(yè)機械與預定義路線之間的九種位置關(guān)系狀態(tài)4為模糊輸入變量的隸屬函數(shù)曲線圖5為模糊輸出變量的隸屬函數(shù)曲線圖6為模糊控制輸出的三維外觀7為轉(zhuǎn)向輪偏角的FUZZY-PID決策算法原理8為導航控制系統(tǒng)的原理結(jié)構(gòu)框9為轉(zhuǎn)向控制系統(tǒng)框10為轉(zhuǎn)向傳動機構(gòu)的安裝示意11為基于DGPS的路徑跟蹤測試結(jié)果圖12為在有初始偏差條件下基于DGPS的路徑跟蹤測試結(jié)果具體實施方式
下面結(jié)合實施例對本發(fā)明做進一步詳細的描述，但本發(fā)明的實施方式不限于此。
本實施例采用的農(nóng)業(yè)機械為久保田插秧機，在其上裝載了導航傳感器組合、轉(zhuǎn)向控制系統(tǒng)和便攜式計算機，便攜計算機通過USB-RS232數(shù)據(jù)傳輸線與導航傳感器組合、轉(zhuǎn)向控制系統(tǒng)相連接，構(gòu)成導航控制系統(tǒng)，原理結(jié)構(gòu)如圖8。
導航傳感器組合包括偽距差分GPS、電子羅盤和微機械陀螺。偽距差分GPS可以采集插秧機在WGS-84坐標系下的定位坐標，作為初步定位信息；電子羅盤測量其航向角度信息(以正北方向為0，逆時針方向為正)；微機械陀螺測量其橫向角速率(逆時針方向為正)。
轉(zhuǎn)向控制系統(tǒng)包括轉(zhuǎn)向操縱控制器、轉(zhuǎn)向驅(qū)動機構(gòu)和測試傳感器三部分，圖9是轉(zhuǎn)向控制系統(tǒng)組成圖。轉(zhuǎn)向操縱控制器是車載計算機的下位機，主要完成轉(zhuǎn)向控制過程中的轉(zhuǎn)向輪偏角檢測，根據(jù)便攜計算機的轉(zhuǎn)向信息指令，控制驅(qū)動機構(gòu)正確的執(zhí)行動作；轉(zhuǎn)向驅(qū)動機構(gòu)則完成驅(qū)動力傳遞，帶動轉(zhuǎn)向輪偏轉(zhuǎn)，及時準確地實現(xiàn)轉(zhuǎn)向動作。圖10是轉(zhuǎn)向驅(qū)動機構(gòu)的安裝示意圖。
便攜計算機實現(xiàn)傳感器數(shù)據(jù)采集、數(shù)據(jù)預處理、導航控制算法以及控制指令輸出等功能。動態(tài)數(shù)據(jù)采集頻率是1Hz。數(shù)據(jù)采集、數(shù)據(jù)預處理和導航控制算法是采用Visual C++編程工具在Windows XP操作系統(tǒng)下開發(fā)完成的。
具體實施步驟如下1、預定路線設(shè)定由熟練駕駛員駕駛插秧機沿預定作業(yè)路線勻速行走，由導航控制系統(tǒng)采集DGPS導航傳感器數(shù)據(jù)并保存，構(gòu)成預定義路線P[N]。其數(shù)學表達式為{(x0，y0)，(x1，y1)，...，(xk-1，yk-1)，(xk，yk)，(xk+1，yk+1)，...，(xn，yn)}對于直線路徑跟蹤，可以在預定直線上選定兩個坐標點P[1]、P[2](通過DGPS靜態(tài)定位獲得)，建立預定義路線的解析幾何描述。
2、啟動導航控制算法，具體運行流程是(1)插秧機位姿和運動狀態(tài)參數(shù)采集涉及數(shù)據(jù)有當前定位點Pc坐標、當前航向角度θ(以高斯投影平面坐標系xoy中x軸正向為0度，逆時針方向為正)、當前速度v。
(2)動態(tài)目標點搜索基于農(nóng)業(yè)機械與預定跟蹤路線的位置解析圖1，本發(fā)明采用前述前視偏差變化率Rdf確定當前前視距離Lf。Rdf的解算采用公式(2)，前視距離Lf的解算采用公式(1)；并確立預瞄點P′p。
(3)導航狀態(tài)參數(shù)計算所述的兩個導航狀態(tài)橫向跟蹤誤差和縱向航向偏差的定義及計算過程如下1)目標方向θp計算在圖1中，即為矢量PcP′p的方向，以高斯投影平面坐標系xoy中x軸正向為0度，逆時針方向為正。
2)縱向航向偏差θe即為目標方向與當前插秧機航向角度之間的差值。
3)橫向跟蹤誤差XTE
在圖1中，設(shè)Pc到預定義路線的投影是Pc′，則XTE＝|PcP′c|，也即XTE=(xc-x′c)2+(yc-y′c)2]]>判斷XTE正負號的方法同前所述。
(4)轉(zhuǎn)向輪偏角的PID決策算法1)橫向跟蹤誤差和縱向航向偏差合成合成誤差可用式(4)計算，α、β的經(jīng)驗值分別取10，1。
2)PID決策轉(zhuǎn)向輪偏角本發(fā)明使用增量式PID控制算法的算式?jīng)Q策轉(zhuǎn)向輪偏角，如式(5)所示。根據(jù)試驗測試結(jié)果，現(xiàn)場整定K、Td、Ti三參數(shù)分別為0.8，0.5，0.01。
(5)轉(zhuǎn)向輪偏角的模糊邏輯決策算法選取橫向跟蹤誤差XTE與方位偏差θe和轉(zhuǎn)向輪偏角控制量δp的離散論域分別為XTE＝{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}；θe＝{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}；δp＝{-7，-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6，7}；1)輸入變量模糊化基本輸入連續(xù)論域離散化采用均勻量化的方法，即將連續(xù)論域中的連續(xù)值經(jīng)量化因子比例變換后四舍五入變?yōu)殡x散論域的整數(shù)值。具體算式如(6)、(7)。設(shè)定Kxte橫向跟蹤誤差XTE與方位偏差θe的經(jīng)驗最大值分別為1m，90。，則Kxte＝0.6，Kθe＝0.06。
2)模糊控制表查詢輸入不同的離散輸入量XTE′，θe′，依據(jù)模糊控制表，得到對應(yīng)的清晰控制量δp。
3)輸出控制量去模糊模糊量到實際輸出控制量的轉(zhuǎn)換算式如下δp′=Kδpδp]]>設(shè)定轉(zhuǎn)向輪偏角控制量δp的經(jīng)驗最大值為45°，則Kδp=6.43.]]>(6)基于PID和FUZZY的轉(zhuǎn)向輪偏角復合決策設(shè)定位置偏差區(qū)間[-0.3，0.3]、航向偏差區(qū)間[-30，30]為PID控制方法的作用范圍(即a＝0.3m，b＝30°)，其他區(qū)間為模糊邏輯方法的作用范圍，實現(xiàn)兩種決策算法的復合控制，可用下式描述
(7)輸出控制參數(shù)將決策的轉(zhuǎn)向輪偏角按一定的串行通信格式打包后，發(fā)送給轉(zhuǎn)向控制系統(tǒng)，實現(xiàn)轉(zhuǎn)向輪偏轉(zhuǎn)控制。串行通信格式如下$CON，a，b，c*DD<CR><LF>
上述格式中，$CON表示轉(zhuǎn)向輪偏角控制量，是標識字符串，也是字符串起始標志；a，b，c表示一個ACSII字符，實際代表一個十進制數(shù)據(jù)，分別表示轉(zhuǎn)向輪偏角的百位，十位和個位。$與*之間所有字符代碼的校驗和由*后面的兩位字符DD表示，校驗和D為半Byte校驗，*后第1個D表示高4位校驗和，第2個D表示低4位校驗和，得到校驗值后，再轉(zhuǎn)換成ASCII字符得到；<CR><LF>是字符串結(jié)束標志。
導航控制算法的流程按照1Hz的頻率運行，實現(xiàn)插秧機的路徑跟蹤控制。
直線路徑跟蹤的試驗結(jié)果如圖11、12采用DGPS導航插秧機進行路徑跟蹤，以0.75m/s速度直線行走，最大橫向跟蹤誤差0.136m，平均偏差0.044m。有初始航向偏差情況下，路徑跟蹤誤差和航向偏差可以快速修正，路徑跟蹤過程平穩(wěn)，穩(wěn)態(tài)跟蹤精度高，最大橫向跟蹤誤差0.2m，平均偏差0.06m。
表1、駕駛員控制車輛的策略表

表2、模糊控制規(guī)則表

表3、路徑跟蹤的模糊控制表

權(quán)利要求
1.一種農(nóng)業(yè)機械的導航控制方法，其特征在于包括下述步驟(1)通過導航傳感器測量得到農(nóng)業(yè)機械當前時刻的位姿和運動狀態(tài)數(shù)據(jù)；(2)采用基于預瞄控制的動態(tài)目標點搜索算法，將所述位姿和運動狀態(tài)數(shù)據(jù)與預定義路線數(shù)據(jù)進行對比分析，推算出兩個導航狀態(tài)參數(shù)即橫向跟蹤誤差和縱向航向偏差；(3)再通過Fuzzy-PID控制算法推算出操縱控制量即前輪轉(zhuǎn)向偏角；(4)最后根據(jù)所述操縱控制量對農(nóng)業(yè)機械進行實時控制，實現(xiàn)路徑跟蹤。
2.根據(jù)權(quán)利要求1所述的農(nóng)業(yè)機械的導航控制方法，其特征在于所述農(nóng)業(yè)機械當前時刻的位姿和運動狀態(tài)數(shù)據(jù)包括當前定位點Pc、當前航向角度θ和當前速度V。
3.根據(jù)權(quán)利要求1所述的農(nóng)業(yè)機械的導航控制方法，其特征在于所述基于預瞄控制的動態(tài)目標點搜索算法，是通過動態(tài)計算前視距離，確定預定義路線上的預瞄點(1)前視距離Lf通過下述公式計算得到Lf＝L0+KlfvV+KlfrRdf其中，L0為前視距離基值；Klfv，Klfr分別為前進速度、前視偏差變化率的比例系數(shù)，可在仿真或?qū)嶒炦^程中通過整定獲得，Klfv與Klfr均取正值；Rdf為前視偏差變化率，定義為Rdf=|Pk+2mP′k+2m→|-|PkP′k|→|Pk+mP′k+m|→-|PkP′k→|---(2)]]>其中m=int(VTsLs),]]>Pk表示預定義路線上的第k坐標點，P′k映射點為在直線PkPk-1上以Pk為垂足引垂線和農(nóng)業(yè)機械當前縱向方向線的相交點，Ts為導航的采樣和控制周期，Ls為預定義路線上點與點之間的平均間距；(2)預瞄點P′p的確定在農(nóng)業(yè)機械當前縱向方向上，由當前定位點Pc點起截取前視距離Lf得到Pp點；再由Pp點向預定義路線投影，得到預瞄點P′p。
4.根據(jù)權(quán)利要求1所述的農(nóng)業(yè)機械的導航控制方法，其特征在于所述推算兩個導航狀態(tài)參數(shù)即橫向跟蹤誤差和縱向航向偏差的過程如下(1)縱向航向偏差θe的確定縱向航向偏差θe為目標方向θp與當前航向角度θ之間的差值；(2)橫向跟蹤誤差XTE的確定XTE=(xc-x′c)2+(yc-y′c)2]]>其中，Pc(xc，yc)為當前定位點；P′c(x′c，y′c)為Pc到預定義路線的投影；并且若農(nóng)業(yè)機械當前坐標點位于預定義路線的右側(cè)，XTE為正；若當前坐標點位于預定義路線的左側(cè)，XTE為負。
5.根據(jù)權(quán)利要求1所述的農(nóng)業(yè)機械的導航控制方法，其特征在于所述通過Fuzzy-PID控制算法推算出操縱控制量即前輪轉(zhuǎn)向偏角是指，當位置偏差在區(qū)間[-a，a]且航向偏差在區(qū)間[-b，b]時，為PID控制方法的作用范圍；其他區(qū)間為模糊邏輯方法Fuzzy的作用范圍，實現(xiàn)兩種決策算法的復合控制；其中，a和b的取值范圍為0≤a≤0.5，0≤b≤45。
6.根據(jù)權(quán)利要求5所述的農(nóng)業(yè)機械的導航控制方法，其特征在于所述PID控制方法是指，將位置偏差和航向偏差轉(zhuǎn)換為合成誤差ERROR，并將合成誤差ERROR作為控制輸入量輸入到PID控制器，然后決策輸出前輪轉(zhuǎn)向偏角；(1)ERROR定義為ERROR＝α×XTE+β×θe其中，α和β分別為橫向跟蹤誤差、縱向航向偏差相對于合成誤差的折算因子，α和β的確定主要根據(jù)試驗效果進行現(xiàn)場整定；(2)采用增量式PID控制算法來決策轉(zhuǎn)向輪偏角Ui=K[Ei-Ei-1+TsTiEi+TdTs(Ei-2Ei-1+Ei-2)]+Ui-1]]>其中，Ui、Ui-1為第i、i-1時刻的轉(zhuǎn)向輪期望偏角，Ei、Ei-1、Ei-2為第i、i-1、i-2時刻的轉(zhuǎn)向輪期望偏角與實際偏角之差。Ts為采樣周期，K為比例系數(shù)，Td為微分時間常數(shù)，Ti為積分時間常數(shù)，K、Td、Ti三參數(shù)根據(jù)試驗效果進行現(xiàn)場整定。
7.根據(jù)權(quán)利要求5所述的農(nóng)業(yè)機械的導航控制方法，其特征在于所述模糊邏輯方法Fuzzy是指(1)首先將輸入變量橫向跟蹤誤差XTE與方位偏差θe模糊化，即采用基本連續(xù)論域離散化采用均勻量化的方法，將連續(xù)論域中的連續(xù)值經(jīng)量化因子比例變換后四舍五入變?yōu)殡x散論域的整數(shù)值，算法為XTE′＝int(KxteXTE)θe′＝int(Kθeθe)其中，輸入量化因子Kxte、Kθe的確定方法是根據(jù)試驗測試結(jié)果，確定橫向跟蹤誤差XTE、方位偏差θe的誤差范圍，由離散論域最大值除于相應(yīng)誤差范圍最大值即可獲得輸入量化因子值；(2)然后進行模糊控制表查詢，即輸入不同的離散輸入量XTE′，θe′，依據(jù)模糊控制表，得到對應(yīng)的清晰控制量即轉(zhuǎn)向輪偏角δp；(3)最后將輸出控制量去模糊，轉(zhuǎn)換公式為δp′=Kδpδp,]]>其中輸出比例因子Kδp的確定方法與上述輸入量化因子的確定方法相同。
全文摘要
本發(fā)明公開了一種農(nóng)業(yè)機械的導航控制方法，包括通過導航傳感器測量得到農(nóng)業(yè)機械當前時刻的位姿和運動狀態(tài)數(shù)據(jù)；采用基于預瞄控制的動態(tài)目標點搜索算法，推算出兩個導航狀態(tài)參數(shù)即橫向跟蹤誤差和縱向航向偏差；再通過Fuzzy－PID控制算法推算出操縱控制量即前輪轉(zhuǎn)向偏角；最后根據(jù)所述操縱控制量對農(nóng)業(yè)機械進行實時控制，實現(xiàn)路徑跟蹤。本發(fā)明充分利用傳統(tǒng)導航控制方法的優(yōu)點，結(jié)合模糊控制方法，兼顧穩(wěn)態(tài)跟蹤精度和控制穩(wěn)定性，提高了農(nóng)業(yè)機械作業(yè)條件的導航跟蹤精度、穩(wěn)定性和安全性；不僅適用于旱地農(nóng)業(yè)機械的作業(yè)需要，還適用于水田農(nóng)業(yè)機械的作業(yè)需要，具有廣闊的推廣應(yīng)用前景。
文檔編號G01C21/20GK101093396SQ200710029018
公開日2007年12月26日 申請日期2007年7月4日 優(yōu)先權(quán)日2007年7月4日
發(fā)明者羅錫文, 張智剛, 趙祚喜, 林衛(wèi)平 申請人:華南農(nóng)業(yè)大學