一種農業機械的導航控制方法

2023-06-10 11:14:06

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

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


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

表2、模糊控制規則表

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

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