一種農(nóng)業(yè)機(jī)械自動導(dǎo)航控制方法
【專利摘要】本發(fā)明公開了一種農(nóng)業(yè)機(jī)械自動導(dǎo)航控制方法，該方法包括如下步驟：(1)GPS接收機(jī)實(shí)時測得當(dāng)前時刻農(nóng)業(yè)機(jī)械的運(yùn)動狀態(tài)數(shù)據(jù)；(2)采用預(yù)瞄控制方法，將運(yùn)動狀態(tài)數(shù)據(jù)與預(yù)定義路徑數(shù)據(jù)進(jìn)行比對分析，推算出橫向偏差和航向偏差；(3)通過模糊控制算法進(jìn)行模糊化，然后將模糊量導(dǎo)入模糊控制器中，得到前輪轉(zhuǎn)角；通過改進(jìn)型純追蹤算法建立幾何模型，從而導(dǎo)求出前輪轉(zhuǎn)角；將兩種算法得到的前輪轉(zhuǎn)角合成一個前輪轉(zhuǎn)角；(4)最后根據(jù)前輪轉(zhuǎn)角實(shí)時對農(nóng)業(yè)機(jī)械進(jìn)行控制，實(shí)現(xiàn)路徑跟蹤。本發(fā)明由兩種導(dǎo)航方法結(jié)合在一起實(shí)現(xiàn)自動控制，是穩(wěn)定性良好，抗干擾能力強(qiáng)，能夠提高系統(tǒng)的魯棒性。兩種方法的結(jié)合，優(yōu)勢互補(bǔ)，使其滿足旱地和水田的作業(yè)需要。
【專利說明】
一種農(nóng)業(yè)機(jī)械自動導(dǎo)航控制方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及農(nóng)業(yè)機(jī)械導(dǎo)航控制領(lǐng)域，特別涉及一種農(nóng)業(yè)機(jī)械自動導(dǎo)航控制算法。
【背景技術(shù)】
[0002] 農(nóng)業(yè)機(jī)械導(dǎo)航控制的主要任務(wù)是根據(jù)各個傳感器得到農(nóng)業(yè)機(jī)械的導(dǎo)航位置，確定 該位置與預(yù)定義路徑的位置關(guān)系，綜合農(nóng)業(yè)機(jī)械的運(yùn)動狀態(tài)以及合適的控制算法，決策出 相應(yīng)的前輪轉(zhuǎn)角，以修正路徑跟蹤誤差。
[0003] 導(dǎo)航控制方法常用的有線性模型控制方法、模糊控制方法和最優(yōu)控制方法。日本 東京大學(xué)利用機(jī)器視覺技術(shù)，根據(jù)線性轉(zhuǎn)向控制模型，將目標(biāo)方向角和農(nóng)業(yè)機(jī)械縱向角融 合，計(jì)算出前輪轉(zhuǎn)角，實(shí)現(xiàn)了轉(zhuǎn)彎控制。隨后東京大學(xué)又采用模糊控制技術(shù)來進(jìn)行導(dǎo)航研 究，模糊控制器輸入的是方向偏差和位置偏差，輸出的是前輪的轉(zhuǎn)向角度和左右執(zhí)行的時 間，從而完成導(dǎo)航控制。O'Connor等人采用方向偏差、方向偏差變化率、轉(zhuǎn)向角度、轉(zhuǎn)向角度 變化率和跟蹤位置誤差這五個狀態(tài)變量建立農(nóng)業(yè)機(jī)械運(yùn)動學(xué)方程，以角度變化率最小為優(yōu) 化目標(biāo)，利用農(nóng)業(yè)機(jī)械線性運(yùn)動學(xué)方程組建立了基于LQR的最優(yōu)控制器，從而實(shí)現(xiàn)了導(dǎo)航控 制。Benson等人應(yīng)用PID控制方法設(shè)計(jì)了 PID控制器，該算法不依賴精確的數(shù)學(xué)模型，避免了 繁瑣的建模過程，只需要一些對象的響應(yīng)特征來組合控制，對算法的比例參數(shù)、積分參數(shù)和 微分參數(shù)進(jìn)行合理的調(diào)節(jié)，即可實(shí)現(xiàn)良好的路徑跟蹤效果。專利ZL200710029018.4中應(yīng)用 模糊控制算法和PID算法設(shè)計(jì)控制器，當(dāng)橫向偏差較小時，應(yīng)用PID控制；當(dāng)橫向偏差較大 時，采用模糊控制。該方法在一定范圍內(nèi)保證了穩(wěn)態(tài)跟蹤精度和控制的穩(wěn)定性，但由于在實(shí) 際導(dǎo)航中的每一時刻，只采用了其中一種控制算法，因此很難發(fā)揮兩種控制算法的互補(bǔ)優(yōu) 勢。
[0004] 通過以上分析可知，導(dǎo)航控制的難點(diǎn)和重點(diǎn)是提高路徑跟蹤的精度和轉(zhuǎn)向控制的 穩(wěn)定性。常規(guī)的線性模型控制方法，包括PID控制方法，能夠獲得較高精度的路徑跟蹤效果， 但是抗干擾能力較弱；模糊控制算法具有良好的穩(wěn)定性，但是路徑跟蹤的穩(wěn)態(tài)精度難以保 證;最優(yōu)控制器的建立要依賴精確的農(nóng)業(yè)機(jī)械運(yùn)動學(xué)和動力學(xué)模型，但是在田間作業(yè)條件 下，土壤特性的變化、農(nóng)業(yè)機(jī)械與地面相互作用的變化，導(dǎo)致農(nóng)業(yè)機(jī)械運(yùn)動學(xué)和動力學(xué)模型 的建立比較困難，

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的在于改進(jìn)和完善導(dǎo)航控制技術(shù)中的不足，提供一種路徑跟蹤精度相 對較高、穩(wěn)定性較好的導(dǎo)航控制方法。該方法基于預(yù)瞄跟隨理論，采用改進(jìn)型純追蹤算法和 模糊控制相結(jié)合的方法來設(shè)計(jì)控制器，建立一種穩(wěn)定性好、精度高的路徑跟蹤算法。
[0006] 為了達(dá)到上述目的，本發(fā)明所采用的技術(shù)方案如下：一種農(nóng)業(yè)機(jī)械自動導(dǎo)航控制 方法，該方法包括如下步驟：
[0007] (1)通過GPS接收機(jī)實(shí)時測得當(dāng)前時刻農(nóng)業(yè)機(jī)械的運(yùn)動狀態(tài)數(shù)據(jù)；
[0008] (2)采用預(yù)瞄控制方法，動態(tài)搜索目標(biāo)點(diǎn)，將步驟1得到運(yùn)動狀態(tài)數(shù)據(jù)與預(yù)定義路 徑數(shù)據(jù)進(jìn)行比對分析，推算出橫向偏差和航向偏差；
[0009] (3)通過模糊控制算法將步驟2得到的橫向偏差和航向偏差模糊化，然后將模糊量 導(dǎo)入模糊控制器中，得到前輪轉(zhuǎn)角;通過改進(jìn)型純追蹤算法建立幾何模型，從而由步驟2中 得到的航向偏差推導(dǎo)求出前輪轉(zhuǎn)角;將模糊控制算法和改進(jìn)型純追蹤算法得到的前輪轉(zhuǎn)角 合成一個最終的前輪轉(zhuǎn)角；
[0010] (4)最后根據(jù)前輪轉(zhuǎn)角實(shí)時對農(nóng)業(yè)機(jī)械進(jìn)行控制，實(shí)現(xiàn)路徑跟蹤。
[0011] 進(jìn)一步的，所述當(dāng)前時刻農(nóng)業(yè)機(jī)械的運(yùn)動狀態(tài)數(shù)據(jù)包括當(dāng)前位置pr(xr，yr)、當(dāng)前 速度V和當(dāng)前航向角度q;所述預(yù)定義路徑數(shù)據(jù)為n+1個位置點(diǎn)的集合，即{Po(xo，y())，Ρι(χι, yi)，· · ·，Pk(xk，yk)，Pk+i(xk+i，yk+i)，· · ·，Pn(xn，yn)} 〇
[0012] 進(jìn)一步的，所述預(yù)瞄控制方法，即動態(tài)搜索目標(biāo)點(diǎn)，是通過動態(tài)計(jì)算前視距離，進(jìn) 而確定預(yù)定義路徑上的預(yù)目苗點(diǎn)：
[0013] (1)前視距離公式由下述公式計(jì)算得出：
[0014] Lf = L〇+kvV+(Lb-kwff)
[0015]其中，Lf為前視距離;Lo為前視距離基值;Lb為彎度的最大量化值;kv和1^分別是速 度和彎度的比例系數(shù)，這兩個系數(shù)可在仿真或試驗(yàn)中通過整定獲得;W為彎度，彎度的計(jì)算 方法是：首先通過快速查找算法確定預(yù)定義路徑上與當(dāng)前農(nóng)業(yè)機(jī)械位置最近的點(diǎn)P k(Xk， yk)，由該點(diǎn)向后每隔i個點(diǎn)再取3個點(diǎn)，i為正整數(shù)，由此可得到4個點(diǎn)，分別是Pk、Pk+1、P k+2_ Pk+3i，用直線連接這四個點(diǎn)，取直線PkPk+i和直線Pk+2iPk+3i所夾的較小的角為彎度值，取值為 0 ~180:
[0016] (2)預(yù)瞄點(diǎn)Pm的確定:在農(nóng)業(yè)機(jī)械當(dāng)前縱向方向上，由當(dāng)前位置點(diǎn)Pr向前截取前視 距離Lf，得到P'"點(diǎn)，再通過快速查找算法得到預(yù)定義路徑上與P'm點(diǎn)最近的點(diǎn)，該點(diǎn)即是預(yù) 瞄點(diǎn)Pm。
[0017] 進(jìn)一步的，所述的橫向偏差和航向偏差的求解過程如下：
[0018] (1)橫向偏差(1的確定：
[0019] 首先通過快速查找算法得到預(yù)定義路徑上與當(dāng)前農(nóng)業(yè)機(jī)械位置最近的點(diǎn)Pk(Xk， yk)，然后將農(nóng)業(yè)機(jī)械當(dāng)前位置點(diǎn)Pr向直線Pk(Xk，yk)P k+1(Xk+1，yk+1)投影，得到點(diǎn)Ρ\，則橫向 偏差d= |PrP\| ;
[0020] (2)航向偏差qe的確定：
[0021] 航向偏差規(guī)定為農(nóng)業(yè)機(jī)械的目標(biāo)方向與當(dāng)前農(nóng)業(yè)機(jī)械的航向之間的角度之差:農(nóng) 業(yè)機(jī)械的當(dāng)前位置點(diǎn)Pr與預(yù)目苗點(diǎn)Pm構(gòu)成的向量為，設(shè)該向量沿逆時針方向旋轉(zhuǎn)與y軸正 向的夾角為A，那么航向偏差兔=力-分&
[0022] 進(jìn)一步的，所述的模糊控制方法具體為：
[0023] (1)將步驟2得到的橫向偏差和航向偏差模糊化，即采用基本連續(xù)論域，離散化采 用均勻量化的方法，將連續(xù)論域中的值經(jīng)量化因子變換后取整，具體為：
[0024] q7 e = int(kqqe)
[0025] d'=int(kdd)
[0026] 其中q'hd'均為連續(xù)數(shù)據(jù)取整后的離散數(shù)據(jù);輸入量化因子kq的確定方法是:根據(jù) 仿真和試驗(yàn)測試結(jié)果，確定航向偏差的誤差范圍，由離散論域最大值除以相應(yīng)誤差范圍最 大值即可獲得量化因子，獲得之后再通過試驗(yàn)進(jìn)行修正即可；
[0027] 橫向偏差的連續(xù)論域離散化具體為:將橫向偏差分為四個等級，當(dāng)測得I d|e[0， 0 · 1]時，貝1Jkd = ai，當(dāng)?shù)?I d I e (ο · 1，〇 · 3]時，貝ljkd = a2,當(dāng) I d I e (〇· 3,05]時，貝ljkd = a3,當(dāng)d為 其它值時，則kd = a4，其中ai、a2、a3和a4分別為橫向偏差在四個等級下對應(yīng)的取值；
[0028] (2)使用模糊控制表查詢，打開規(guī)則觀測器，輸入不同的離散量dW e，得到對應(yīng) 的輸出尚散量4 ;
[0029] (3)輸出去模糊控制，即模糊控制算法得出的前輪轉(zhuǎn)角A :從模糊控制量到實(shí)際輸 出控制量的轉(zhuǎn)換公式如下：
[0030] dx =
[0031] 其中，輸出的比例因子Μ勺確定方法同輸入量化因子。
[0032] 進(jìn)一步的，所述的改進(jìn)型純追蹤算法為：
[0033] 利用航向偏差建立幾何模型，求解農(nóng)業(yè)機(jī)械到達(dá)目標(biāo)點(diǎn)所需要的轉(zhuǎn)彎半徑，然后 由轉(zhuǎn)彎半徑再求解出前輪轉(zhuǎn)角，同時把橫向偏差作為觀測量，如果橫向偏差超過設(shè)定值，那 么給前輪轉(zhuǎn)角一個補(bǔ)償量以增大前輪轉(zhuǎn)角，使農(nóng)業(yè)機(jī)械快速轉(zhuǎn)彎到達(dá)目標(biāo)點(diǎn)；
[0034] Pr為當(dāng)前農(nóng)業(yè)機(jī)械的位置，Pk+j為目標(biāo)點(diǎn)，R為轉(zhuǎn)彎半徑，C為圓心點(diǎn)，qe為航向偏 差，L為農(nóng)業(yè)機(jī)械前后輪軸間距，.4為改進(jìn)型純追蹤算法求得的前輪轉(zhuǎn)角，那么在直角三角 形BP rC中可以得出：
[0036]由于qe為圓C的弦切角，因此可得：
[0037] -PrCPk+j = 2qe
[0038] 在等腰三角形PrCPk+j中，由正弦定理可得：
[0040]由上述三式消去R可得到前輪轉(zhuǎn)角：
[0042]式中PrPk+j為農(nóng)業(yè)機(jī)械當(dāng)前位置與目標(biāo)點(diǎn)的之間的距離；另外，將橫向偏差作為控 制器的一個觀測量，再設(shè)定一個預(yù)設(shè)值dr，當(dāng)觀測到橫向偏差d矣dr時，則將改進(jìn)純追型蹤算 法求得的前輪轉(zhuǎn)角4乘以一個比例系數(shù)kdr，由此可以增大前輪轉(zhuǎn)角，使其快速到達(dá)目標(biāo)點(diǎn)； 其中dr和kdr的大小通過仿真或試驗(yàn)整定得出。
[0043] 進(jìn)一步的，所述步驟3具體為:通過改進(jìn)型純追蹤算法和模糊控制算法分別得出前 輪轉(zhuǎn)角，然后將兩個轉(zhuǎn)角合成一個最終的轉(zhuǎn)角，實(shí)現(xiàn)兩種方法復(fù)合控制，其中合成公式如 下：
[0044] d = kjdy +k2d,
[0045] 其中l(wèi)u、k2分別為模糊控制器輸出的前輪轉(zhuǎn)角和改進(jìn)型純追蹤控制器輸出的前輪 轉(zhuǎn)角的折算因子，兩者之和為1，兩者取值由仿真和試驗(yàn)整定得出。
[0046] 進(jìn)一步的，所述快速查找算法是計(jì)算預(yù)定義路徑的每個點(diǎn)與當(dāng)前農(nóng)業(yè)機(jī)械坐標(biāo)點(diǎn) 的距離，距離最小的那個點(diǎn)即為最近點(diǎn)。
[0047] 本發(fā)明與現(xiàn)有技術(shù)相比有如下特點(diǎn)：
[0048] (1)本發(fā)明對傳統(tǒng)純追蹤算法做了改進(jìn)，使其不僅適應(yīng)直線行駛還能滿足曲線轉(zhuǎn) 彎，同時結(jié)合模糊控制算法，在實(shí)際導(dǎo)航中充分發(fā)揮了上述兩種導(dǎo)航算法各自的優(yōu)勢并達(dá) 到互補(bǔ)的效果，因此極大地提高了控制的精度和穩(wěn)定性。
[0049] (2)本發(fā)明在導(dǎo)航農(nóng)業(yè)機(jī)械沿預(yù)定義路徑行駛時，直線段行駛的跟蹤誤差小于 5cm，曲線轉(zhuǎn)彎段行駛的跟蹤誤差小于20cm〇
[0050] (3)本發(fā)明適合于農(nóng)業(yè)機(jī)械自動化裝備，能夠滿足旱地的作業(yè)需要，同時也可滿足 水田作業(yè)要求。
【附圖說明】
[0051] 圖1為彎度求解圖；
[0052]圖2為橫向偏差求解圖；
[0053]圖3為橫向偏差正負(fù)號求解圖；
[0054]圖4為航向偏差求解圖；
[0055]圖5為模糊變量的隸屬函數(shù)圖；
[0056]圖6為模糊控制輸出的三維外觀圖；
[0057]圖7為改進(jìn)型純追蹤算法原理圖；
[0058]圖8為導(dǎo)航控制原理框圖。
【具體實(shí)施方式】
[0059] 為使本發(fā)明更明顯易懂，茲以一實(shí)施例作進(jìn)一步詳細(xì)描述，但不用來限制本發(fā)明 的范圍。
[0060] 一種農(nóng)業(yè)機(jī)械自動導(dǎo)航控制方法，該方法包括如下步驟：
[0061 ] (1)通過GPS接收機(jī)實(shí)時測得當(dāng)前時刻農(nóng)業(yè)機(jī)械的運(yùn)動狀態(tài)數(shù)據(jù)；
[0062] (2)采用預(yù)瞄控制方法，動態(tài)搜索目標(biāo)點(diǎn)，將步驟1得到運(yùn)動狀態(tài)數(shù)據(jù)與預(yù)定義路 徑數(shù)據(jù)進(jìn)行比對分析，推算出橫向偏差和航向偏差；
[0063] (3)通過模糊控制算法將步驟2得到的橫向偏差和航向偏差模糊化，然后將模糊量 導(dǎo)入模糊控制器中，得到前輪轉(zhuǎn)角;通過改進(jìn)型純追蹤算法建立幾何模型，從而由步驟2中 得到的航向偏差推導(dǎo)求出前輪轉(zhuǎn)角;將模糊控制算法和改進(jìn)型純追蹤算法得到的前輪轉(zhuǎn)角 合成一個最終的前輪轉(zhuǎn)角；
[0064] (4)最后根據(jù)前輪轉(zhuǎn)角實(shí)時對農(nóng)業(yè)機(jī)械進(jìn)行控制，實(shí)現(xiàn)路徑跟蹤。
[0065] 進(jìn)一步的，所述當(dāng)前時刻農(nóng)業(yè)機(jī)械的運(yùn)動狀態(tài)數(shù)據(jù)包括當(dāng)前位置Pr(xr，yr)、當(dāng)前 速度V和當(dāng)前航向角度q，如圖1所示;具體定義如下：
[0066] (1)當(dāng)前定位點(diǎn)Pr(xr，yr)，由GPS接收機(jī)獲得當(dāng)前的差分定位數(shù)據(jù)；
[0067] (2)當(dāng)前速度V，由速度傳感器測得當(dāng)前農(nóng)業(yè)機(jī)械縱向方向的前進(jìn)速度；
[0068] (3)當(dāng)前航向角度q，由電子羅盤獲得當(dāng)前農(nóng)業(yè)機(jī)械縱向方向的角度，在高斯投影 平面坐標(biāo)系中，表示為與y軸順時針旋轉(zhuǎn)的夾角，范圍為0~360。
[0069] 所述的預(yù)定義路徑是由人工駕駛車輛，GPS接收機(jī)記錄的路徑數(shù)據(jù)，是一個點(diǎn)數(shù) 據(jù)，其數(shù)學(xué)表達(dá)式為：
[0070] {P〇(x〇，y〇)，Pi(xi，yi)，·· ·，Pk-i(Xk-i，yk-1)，Pk(Xk，yk)}
[0071] 需要說明的是，預(yù)定義路徑由點(diǎn)數(shù)據(jù)集表示，Pk為第k個點(diǎn)，（Xk，yk)為第k個點(diǎn)的坐 標(biāo)，所有點(diǎn)的坐標(biāo)均在WGS-84大地坐標(biāo)系中，表示為高斯投影平面坐標(biāo)。
[0072] 所述預(yù)瞄控制方法，即動態(tài)搜索目標(biāo)點(diǎn)，是通過動態(tài)計(jì)算前視距離，進(jìn)而確定預(yù)定 義路徑上的預(yù)瞄點(diǎn)：
[0073] (1)前視距離公式由下述公式計(jì)算得出：
[0074] Lf = Lo+kvV+ (Lb-kwff)
[0075]其中，Lf為前視距離;Lo為前視距離基值;Lb為彎度的最大量化值;kv和1^分別是速 度和彎度的比例系數(shù)，這兩個系數(shù)可在仿真或試驗(yàn)中通過整定獲得;W為彎度，如圖1所示， 彎度的計(jì)算方法是:首先通過快速查找算法確定預(yù)定義路徑上與當(dāng)前農(nóng)業(yè)機(jī)械位置最近的 點(diǎn)Pk(x k，yk)，由該點(diǎn)向后每隔i個點(diǎn)再取3個點(diǎn)，i為正整數(shù)，由此可得到4個點(diǎn)，分別是Pk、 Pk+i、Pk+2_Pk+3i，用直線連接這四個點(diǎn)，取直線PkPk+i和直線Pk+2iPk +3i所夾的較小的角為彎 度值，取值為〇~180:
[0076] (2)預(yù)瞄點(diǎn)Pm的確定:在農(nóng)業(yè)機(jī)械當(dāng)前縱向方向上，由當(dāng)前位置點(diǎn)Pr向前截取前視 距離L f，得到P'"點(diǎn)，再通過快速查找算法得到預(yù)定義路徑上與P'm點(diǎn)最近的點(diǎn)，該點(diǎn)即是預(yù) 瞄點(diǎn)K。
[0077] 所述的橫向偏差和航向偏差的求解過程如下：
[0078] (1)橫向偏差(1的確定：
[0079] 如圖2所示，首先通過快速查找算法得到預(yù)定義路徑上與當(dāng)前農(nóng)業(yè)機(jī)械位置最近 的點(diǎn)Pk(Xk，yk)，然后將農(nóng)業(yè)機(jī)械當(dāng)前位置點(diǎn)Pr向直線Pk(Xk，yk)Pk+1 (Xk+i，yk+1)投影，得到點(diǎn) P'r，則橫向偏差d= |P^r| ;
[0080] 為了導(dǎo)航控制算法設(shè)計(jì)的方便，這里對橫向偏差正負(fù)號，在車輛沿預(yù)定義路徑前 進(jìn)的方向上，如果車輛位于預(yù)定義路徑右側(cè)，則d為正，如果在左側(cè)，則d為負(fù);判斷正負(fù)的方 法是：如圖3所示，首先在預(yù)定義路徑中找到距離車輛當(dāng)前位置點(diǎn)最近的點(diǎn)P k，然后沿著路 徑前進(jìn)方向?qū)ふ蚁乱粋€點(diǎn)Pk+i，于是構(gòu)成一個向量^^，設(shè)該向量逆時針旋轉(zhuǎn)與y軸正向的 夾角為ai。由車輛當(dāng)前位置點(diǎn)Pr與點(diǎn)Pk+i，又可組成一個向量:設(shè)該向量逆時針旋轉(zhuǎn)與 y軸正向的夾角為a2。如果ai_a2為正，則車輛在右側(cè)，那么橫向偏差為正，否則為負(fù)。
[0081 ] (2)航向偏差qe的確定：
[0082]航向偏差規(guī)定為農(nóng)業(yè)機(jī)械的目標(biāo)方向與當(dāng)前農(nóng)業(yè)機(jī)械的航向之間的角度之差:如 圖4所示，農(nóng)業(yè)機(jī)械的當(dāng)前位置點(diǎn)Pr與預(yù)瞄APm構(gòu)成的向量為7^，設(shè)該向量沿逆時針方向 旋轉(zhuǎn)與y軸正向的夾角為i?，那么航向偏差孓=力-g *角度的取值范圍為-90~90,如果為 正，則表示車輛當(dāng)前的航向在目標(biāo)方向的左邊，否則在右邊。
[0083]所述的模糊控制方法具體為：
[0084]模糊控制是將駕駛員的駕駛經(jīng)驗(yàn)轉(zhuǎn)換為模糊規(guī)則，進(jìn)而決策出前輪轉(zhuǎn)角?；舅?路是當(dāng)橫向偏差和航向偏差越大，那么控制的程度越強(qiáng)，即前輪轉(zhuǎn)角越大；當(dāng)橫向偏差和航 向偏差越小，那么控制的程度越弱，即前輪轉(zhuǎn)角越小；當(dāng)兩者一個較大一個較小，那么控制 的程度為中。規(guī)定前輪右轉(zhuǎn)時為正，左轉(zhuǎn)時為負(fù)。駕駛經(jīng)驗(yàn)轉(zhuǎn)換為模糊規(guī)則的方法是:將農(nóng) 業(yè)機(jī)械相對于預(yù)定義路徑的狀態(tài)分為七種，即嚴(yán)重偏左、偏左、稍微偏左、正對著路徑、稍微 偏右、偏右、嚴(yán)重偏右，根據(jù)這七種狀態(tài)，駕駛員會做出相應(yīng)的轉(zhuǎn)向控制策略，對應(yīng)的也分為 七種，即大幅度右轉(zhuǎn)、右轉(zhuǎn)、稍微右轉(zhuǎn)、不轉(zhuǎn)向、稍微左轉(zhuǎn)、左轉(zhuǎn)、大幅度左轉(zhuǎn)。據(jù)此建立模糊 控制規(guī)則表，如表1所示。具體設(shè)計(jì)步驟是在Matlab環(huán)境中設(shè)計(jì)的，如下所示：
[0085] (1)首先將兩個輸入變量d、qe和一個輸出變量4進(jìn)行模糊化，其模糊子集分別選 為：
[0086] Fd={NL，NM，NS，ZO，PS，PM，PL}
[0087] = J NL NM, ；V5, ZO, PS, PM, PL\
[0088] = ? NL, NM, NS, ZO, PS, PM " PL ^
[0089] 三個變量的模糊分割均為7級，即負(fù)大、負(fù)中、負(fù)小、零、正小、正中和正大。選取橫 向偏差、航向偏差和輸出轉(zhuǎn)角的離散論域分別為：
[0090] d={-6，-5，-4，-3，-2，-l，0，l，2,3,4,5,6}
[0091] qe={-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}
[0092] ： -6,-5,-4, -3? ~2> -1,(),1,2,3,4,5,-7}
[0093]模糊推理系統(tǒng)的基本屬性確定為："與"運(yùn)算采用極小運(yùn)算，"或"運(yùn)算采用極大運(yùn) 算，模糊蘊(yùn)涵米用極小運(yùn)算，模糊規(guī)則綜合米用極大運(yùn)算，去模糊化米用重心法。航向偏差 的基本連續(xù)論域離散化采用均勻量化的方法，即將連續(xù)論域中的連續(xù)值經(jīng)量化因子比例變 換后四舍五入變?yōu)殡x散論域的整數(shù)值，具體求解公式為：
[0094] q7 e = int(kqqe)
[0095] d'=int(kdd)
[0096] 其中q'hd'均為連續(xù)數(shù)據(jù)取整后的離散數(shù)據(jù);輸入量化因子kq的確定方法是:根 據(jù)仿真和試驗(yàn)測試結(jié)果，確定航向偏差的誤差范圍，由離散論域最大值除以相應(yīng)誤差范圍 最大值即可獲得量化因子，獲得之后再通過試驗(yàn)進(jìn)行修正即可；
[0097] 橫向偏差的連續(xù)論域離散化具體為:將橫向偏差分為四個等級，當(dāng)測得|d|e[0， 0· 1]時，貝1Jkd = ai，當(dāng)?shù)?I d I e (ο · 1，〇· 3]時，貝ljkd = a2,當(dāng) | d I e (〇· 3,05]時，貝ljkd = a3，當(dāng)d為 其它值時，則kd = a4，其中ai、a2、a3和a4分別為橫向偏差在四個等級下對應(yīng)的取值，其數(shù)值通 過仿真或試驗(yàn)整定得出。
[0098]在Mat 1 ab環(huán)境中，建立模糊變量橫向偏差、航向偏差和輸出轉(zhuǎn)角的隸屬函數(shù)曲線， 如圖5(a)、(b)和(c)所示。對應(yīng)的模糊控制規(guī)則表，利用Matlab軟件繪出模糊控制表的三維 外觀圖，如圖6所示。
[0099] (2)使用模糊控制表查詢，模糊控制表如表2所示，打開規(guī)則觀測器，輸入不同的離 散量d '，q' e，得到對應(yīng)的輸出離散量
[0100] (3)輸出去模糊控制，即模糊控制算法得出的前輪轉(zhuǎn)角吟?從模糊控制量到實(shí)際輸 出控制量的轉(zhuǎn)換公式如下：
[01 01 ] cfj = k^jdj
[0102 ]其中，輸出的模糊控制算法得出的前輪轉(zhuǎn)角的比例因子的確定方法同輸入量化 因子。
[0103] 進(jìn)一步的，所述的改進(jìn)型純追蹤算法為：
[0104] 利用航向偏差建立幾何模型，求解農(nóng)業(yè)機(jī)械到達(dá)目標(biāo)點(diǎn)所需要的轉(zhuǎn)彎半徑，然后 由轉(zhuǎn)彎半徑再求解出前輪轉(zhuǎn)角，同時把橫向偏差作為觀測量，如果橫向偏差超過設(shè)定值，那 么給前輪轉(zhuǎn)角一個補(bǔ)償量以增大前輪轉(zhuǎn)角，使農(nóng)業(yè)機(jī)械快速轉(zhuǎn)彎到達(dá)目標(biāo)點(diǎn)；
[0105]在圖6中，一組點(diǎn)為預(yù)定義路徑點(diǎn)，Pr為當(dāng)前農(nóng)業(yè)機(jī)械的位置，Pk+j為目標(biāo)點(diǎn)，R為轉(zhuǎn) 彎半徑，C為圓心點(diǎn)，qe為航向偏差，L為農(nóng)業(yè)機(jī)械前后輪軸間距，鋒為改進(jìn)型純追蹤算法求 得的前輪轉(zhuǎn)角，那么在直角三角形BP rC中可以得出：
[0107]由于qe為圓C的弦切角，因此可得：
[0108] -PrCPk+j = 2qe
[0109] 在等腰三角形PrCPk+j中，由正弦定理可得：
由上述三式消去R可得到前輪轉(zhuǎn)角：
[0113] 式中PrPk+j為農(nóng)業(yè)機(jī)械當(dāng)前位置與目標(biāo)點(diǎn)的之間的距離；另外，將橫向偏差作為控 制器的一個觀測量，再設(shè)定一個預(yù)設(shè)值dr，當(dāng)觀測到橫向偏差d矣dr時，則將改進(jìn)純追型蹤算 法求得的前輪轉(zhuǎn)角4乘以一個比例系數(shù)kdr，由此可以增大前輪轉(zhuǎn)角，使其快速到達(dá)目標(biāo)點(diǎn)； 其中dr和kdr的大小通過仿真或試驗(yàn)整定得出。
[0114] 進(jìn)一步的，所述步驟3具體為:通過改進(jìn)型純追蹤算法和模糊控制算法分別得出前 輪轉(zhuǎn)角，然后將兩個轉(zhuǎn)角合成一個最終的轉(zhuǎn)角，實(shí)現(xiàn)兩種方法復(fù)合控制，其中合成公式如 下：
[0115] +k2d,
[0116] 其中ki、k2分別為模糊控制器輸出的前輪轉(zhuǎn)角和改進(jìn)型純追蹤控制器輸出的前輪 轉(zhuǎn)角的折算因子，兩者之和為1，兩者取值由仿真和試驗(yàn)整定得出。
[0117] 進(jìn)一步的，所述快速查找算法是計(jì)算預(yù)定義路徑的每個點(diǎn)與當(dāng)前農(nóng)業(yè)機(jī)械坐標(biāo)點(diǎn) 的距離，距離最小的那個點(diǎn)即為最近點(diǎn)。
[0118] 上述兩種導(dǎo)航控制方法的結(jié)合，既能提高路徑跟蹤的穩(wěn)態(tài)精度，又能提高路徑跟 蹤的穩(wěn)定性。其中改進(jìn)型純追蹤算法是從幾何的角度推算的，由航向偏差經(jīng)過幾何運(yùn)算得 出前輪轉(zhuǎn)角，運(yùn)算過程未進(jìn)行近似處理，因此得出的是精確解。該解對于消除導(dǎo)航控制系統(tǒng) 中的偏差和穩(wěn)態(tài)誤差起到重要作用，有利于提高導(dǎo)航精度。
[0119] 模糊控制方法是一種近似求解方法，該方法的特點(diǎn)是不需要確切的理論模型，僅 根據(jù)人的駕駛經(jīng)驗(yàn)來制定模糊規(guī)則，建立模糊控制器。因此抗干擾能力較強(qiáng)，不會因模型變 化而改變控制效果，能夠消除導(dǎo)航系統(tǒng)的振蕩誤差。
[0120] 本實(shí)施例使用的農(nóng)業(yè)機(jī)械為井關(guān)插秧機(jī)，在其上裝載了導(dǎo)航傳感器組合、轉(zhuǎn)向控 制系統(tǒng)和車載計(jì)算機(jī)，各部分之間通信使用總線，由此構(gòu)成導(dǎo)航控制系統(tǒng)，其原理結(jié)構(gòu)如圖 8所示。導(dǎo)航傳感器組合包括差分GPS、電子羅盤、速度傳感器和微機(jī)械陀螺儀。轉(zhuǎn)向控制系 統(tǒng)包括操縱控制器、轉(zhuǎn)向驅(qū)動機(jī)構(gòu)和測試傳感器。車載計(jì)算機(jī)主要實(shí)現(xiàn)數(shù)據(jù)采集、數(shù)據(jù)預(yù)處 理、導(dǎo)航控制算法和控制指令輸出等功能，動態(tài)數(shù)據(jù)采集頻率為5Hz。
[0121] 具體實(shí)施的步驟如下：
[0122] 1、預(yù)定義路徑數(shù)據(jù)獲取
[0123] 首先由駕駛員駕駛插秧機(jī)在水田中正常作業(yè)行走，由GPS接收機(jī)實(shí)時獲取定位信 息并以總線數(shù)據(jù)的形式輸出，該數(shù)據(jù)構(gòu)成預(yù)定義路徑，路徑包括直線段和曲線轉(zhuǎn)彎段，其數(shù) 學(xué)表達(dá)式為卩|11] = {(叉()，7())，（叉1，71)，...，（叉1<，71〇，...，（叉11，711)}
[0124] 2、導(dǎo)航數(shù)據(jù)獲取
[0125] (1)插秧機(jī)位姿和速度參數(shù)的獲取
[0126] 涉及數(shù)據(jù)包括有:插秧機(jī)當(dāng)前定位點(diǎn)坐標(biāo)、當(dāng)前航向角度和當(dāng)前插秧機(jī)速度。
[0127] (2)前視距離的求解
[0128] 前視距離的定義如圖2所示，求解方法采用前述公式（1)進(jìn)行求解，利用速度和路 徑的彎度來確定合適的前視距離，進(jìn)而可以確定目標(biāo)點(diǎn)。各參數(shù)取值如下：
[0129] L〇 = 0.15，Lb = 0.62，kv = 0.31，kw=0.1
[0130] (3)導(dǎo)航偏差的求解
[0131] 所述的導(dǎo)航偏差分為橫向偏差和航向偏差，定義及計(jì)算過程如下所示：
[0132] 1)目標(biāo)方向i?的計(jì)算
[0133] 在圖2中，即為矢量為的方向，以高斯投影平面坐標(biāo)系xoy中y軸正向?yàn)?度，順 時針旋轉(zhuǎn)到矢量7^的角度即為力，該角度取值范圍為0~360。
[0134] 2)航向偏差qe的計(jì)算
[0135] 在圖2中，即為目標(biāo)方向角與插秧機(jī)當(dāng)前航向角度之間的差值。
[0136] 3)橫向偏差d的計(jì)算
[0137] 在圖2中，設(shè)點(diǎn)為農(nóng)業(yè)機(jī)械當(dāng)前位置點(diǎn)向預(yù)定義路徑上投影的點(diǎn)，則dilPrP'r ，判斷正負(fù)的方法同前所述。
[0138] (4)前輪轉(zhuǎn)角模糊控制算法
[0139] 將橫向誤差和航向誤差以及前輪轉(zhuǎn)角的離散論域分別選取為：
[0140] d={-6，-5，-4，-3，-2，-l，0，l，2,3,4,5,6}
[0141] qe={-6，-5，-4，-3，-2，_l，0，l，2,3,4,5,6}
[0142] 4 = ! -7, - 6,-5,-4,-3, - 2, - 1,0,1,2,3,4,5,6, - 7|·
[0143] 1)輸入變量模糊化
[0144] 將輸入連續(xù)論域中的連續(xù)值經(jīng)量化因子比例轉(zhuǎn)換為離散量，然后四舍五入取值。 其中航向偏差與橫向偏差分別按上述方法進(jìn)行量化，其中各參數(shù)取值如下所示：
[0145] kq = 0 · 25，ai = 12、a2 = 2、a3 = 0 · 2、a4 = 0
[0146] 2)模糊控制表查詢
[0147] 輸入不同的離散輸入量d '和Y e，依據(jù)路徑跟蹤模糊控制表，如表2所示，即可得到 對應(yīng)的輸出尚散量。
[0148] 3)輸出控制量清晰化
[0149] 模糊量到實(shí)際輸出控制量的轉(zhuǎn)換公式如下所示：
[0150] c?! =kd,4
[0151] 其中參數(shù)=-21.429。
[0152] (5)前輪轉(zhuǎn)角改進(jìn)型純追蹤算法
[0153] 首先根據(jù)插秧機(jī)當(dāng)前的航向偏差計(jì)算出插秧機(jī)到達(dá)目標(biāo)點(diǎn)所需的轉(zhuǎn)彎半徑，進(jìn)而 求出前輪轉(zhuǎn)角，計(jì)算公式如公式(9)所示，然后利用橫向偏差作為觀測量，如果偏差過大，那 么將前輪轉(zhuǎn)角乘以一個系數(shù)，將其增大，這樣插秧機(jī)能夠快速到達(dá)目標(biāo)點(diǎn)，各參數(shù)分別為：
[0154] dr = 0.02、kdr = l .08
[0155] (6)模糊控制和改進(jìn)型純追蹤控制復(fù)合決策
[0156] 上述兩種算法得出兩個前輪轉(zhuǎn)角，利用如下公式進(jìn)行綜合：
[0157] d = kj^ + kjd,
[0158] 其中整定出的兩個參數(shù)數(shù)值分別為：
[0159] ki = 0 · 2、k2 = 0 · 8
[0160] (7)輸出控制參數(shù)
[0161]將決策出的前輪轉(zhuǎn)角按一定的總線通信格式打包，發(fā)送給轉(zhuǎn)向控制系統(tǒng)，即可實(shí) 現(xiàn)前輪轉(zhuǎn)角的控制。
[0162]表1、模糊控制規(guī)則表
[0164] 表2、路徑跟蹤模糊控制表
【主權(quán)項(xiàng)】
1. 一種農(nóng)業(yè)機(jī)械自動導(dǎo)航控制方法，其特征在于，該方法包括如下步驟： (1) 通過GPS接收機(jī)實(shí)時測得當(dāng)前時刻農(nóng)業(yè)機(jī)械的運(yùn)動狀態(tài)數(shù)據(jù)； (2) 采用預(yù)瞄控制方法，動態(tài)搜索目標(biāo)點(diǎn)，將步驟1得到運(yùn)動狀態(tài)數(shù)據(jù)與預(yù)定義路徑數(shù) 據(jù)進(jìn)行比對分析，推算出橫向偏差和航向偏差； (3) 通過模糊控制算法將步驟2得到的橫向偏差和航向偏差模糊化，然后將模糊量導(dǎo)入 模糊控制器中，得到前輪轉(zhuǎn)角;通過改進(jìn)型純追蹤算法建立幾何模型，從而由步驟2中得到 的航向偏差推導(dǎo)求出前輪轉(zhuǎn)角;將模糊控制算法和改進(jìn)型純追蹤算法得到的前輪轉(zhuǎn)角合成 一個最終的前輪轉(zhuǎn)角； (4) 最后根據(jù)前輪轉(zhuǎn)角實(shí)時對農(nóng)業(yè)機(jī)械進(jìn)行控制，實(shí)現(xiàn)路徑跟蹤。2. 如權(quán)利要求1所述的農(nóng)業(yè)機(jī)械自動導(dǎo)航控制方法，其特征在于，所述當(dāng)前時刻農(nóng)業(yè)機(jī) 械的運(yùn)動狀態(tài)數(shù)據(jù)包括當(dāng)前位置Pr( Xr，yr)、當(dāng)前速度V和當(dāng)前航向角度q;所述預(yù)定義路徑 數(shù)據(jù)為n+1個位置點(diǎn)的集合，gp{Po(xo，yo)，Pi(xi，yi)，...，Pk(xk，yk)，Pk+i(xk+i，yk+i)，...，P n (Χη,Υη) } 〇3. 如權(quán)利要求1所述的農(nóng)業(yè)機(jī)械自動導(dǎo)航控制方法，其特征在于，所述預(yù)瞄控制方法， 即動態(tài)搜索目標(biāo)點(diǎn)，是通過動態(tài)計(jì)算前視距離，進(jìn)而確定預(yù)定義路徑上的預(yù)瞄點(diǎn)： (1) 前視距離公式由下述公式計(jì)算得出： Lf = L〇+kvV + (Lb_kwff) 其中，Lf為前視距離;L〇為前視距離基值;Lb為彎度的最大量化值;k4Pkw分別是速度和 彎度的比例系數(shù);W為彎度，彎度的計(jì)算方法是:首先通過快速查找算法確定預(yù)定義路徑上 與當(dāng)前農(nóng)業(yè)機(jī)械位置最近的點(diǎn)Pk(x k，yk)，由該點(diǎn)向后每隔i個點(diǎn)再取3個點(diǎn)，i為正整數(shù)，由 此可得到4個點(diǎn)，分別是Pk、Pk+i、Pk +2dPPk+3i，用直線連接這四個點(diǎn)，取直線PkPk+i和直線Pk+ 2iPk+3i所夾的較小的角為彎度值，取值為0~180° ; (2) 預(yù)瞄點(diǎn)Pm的確定：在農(nóng)業(yè)機(jī)械當(dāng)前縱向方向上，由當(dāng)前位置點(diǎn)Pr向前截取前視距離 Lf，得到，再通過快速查找算法得到預(yù)定義路徑上與P、點(diǎn)最近的點(diǎn)，該點(diǎn)即是預(yù)瞄點(diǎn) Pm〇4. 如權(quán)利要求1所述的農(nóng)業(yè)機(jī)械自動導(dǎo)航控制方法，其特征在于，所述的橫向偏差和航 向偏差的求解過程如下： (1) 橫向偏差(1的確定： 首先通過快速查找算法得到預(yù)定義路徑上與當(dāng)前農(nóng)業(yè)機(jī)械位置最近的點(diǎn)Pk(Xk，yk)，然 后將農(nóng)業(yè)機(jī)械當(dāng)前位置點(diǎn)P:向直線Pk(Xk，yk)Pk+1(x k+1，yk+1)投影，得到點(diǎn)P\，則橫向偏差d = PrP7 rI ； (2) 航向偏差的確定： 航向偏差規(guī)定為農(nóng)業(yè)機(jī)械的目標(biāo)方向與當(dāng)前農(nóng)業(yè)機(jī)械的航向之間的角度之差:農(nóng)業(yè)機(jī) 械的當(dāng)前位置點(diǎn)Pr與預(yù)目苗點(diǎn)Pm構(gòu)成的向量為，設(shè)該向量沿逆時針方向旋轉(zhuǎn)與y軸正向 的夾角為b，那么航向偏差qe = b-q。 5 .如權(quán)利要求1所述的農(nóng)業(yè)機(jī)械自動導(dǎo)航控制方法，其特征在于，所述的模糊控制方法 具體為： (1)將步驟2得到的橫向偏差和航向偏差模糊化，即采用基本連續(xù)論域，離散化采用均 勻量化的方法，將連續(xù)論域中的值經(jīng)量化因子變換后取整，具體為： Q e=illt(kqqe) d' = int(kdd) 其中q%、d'均為連續(xù)數(shù)據(jù)取整后的離散量;kq為輸入量化因子； 橫向偏差的連續(xù)論域離散化具體為:將橫向偏差分為四個等級，當(dāng)測得|d|e [〇,〇.1] 時，貝收〇1 = 31，當(dāng)?shù)脇(1|￡(〇.1，〇.3]時，貝1|1^ = 32，當(dāng)|(1|￡(〇.3,05]時，貝1|1^ = 33，當(dāng)(1為其它 值時，則kd = a4，其中ai、a2、a3和a4分別為橫向偏差在四個等級下對應(yīng)的取值； (2) 使用模糊控制表查詢，打開規(guī)則觀測器，輸入不同的離散量d'，q%，得到對應(yīng)的輸出 咼散量d'i; (3) 輸出去模糊控制，即模糊控制算法得出的前輪轉(zhuǎn)角δ::從模糊控制量到實(shí)際輸出控 制量的轉(zhuǎn)換公式如下： di = kdid'i 其中，輸出的比例因子1^的確定方法同輸入量化因子。6. 如權(quán)利要求1所述的農(nóng)業(yè)機(jī)械自動導(dǎo)航控制方法，其特征在于，所述的改進(jìn)型純追蹤 算法為： 利用航向偏差建立幾何模型，Pr為當(dāng)前農(nóng)業(yè)機(jī)械的位置，Pk+j為目標(biāo)點(diǎn)，R為轉(zhuǎn)彎半徑，C 為圓心點(diǎn)，qe為航向偏差，L為農(nóng)業(yè)機(jī)械前后輪軸間距，d2為改進(jìn)型純追蹤算法求得的前輪轉(zhuǎn) 角，那么在直角三角形BP rC中可以得出：由于qe為圓C的弦切角，因此可得： -PrCPk+j = 2qe 在等腰三角形PrCPk+j中，由正弦定理可得：由上述三式消去R可得到前輪轉(zhuǎn)角：式中PrPk+j為農(nóng)業(yè)機(jī)械當(dāng)前位置與目標(biāo)點(diǎn)的之間的距離;另外，將橫向偏差作為控制器 的一個觀測量，再設(shè)定一個預(yù)設(shè)值dr，當(dāng)觀測到橫向偏差d矣dr時，則將改進(jìn)純追型蹤算法求 得的前輪轉(zhuǎn)角d2乘以一個比例系數(shù)kdr，由此可以增大前輪轉(zhuǎn)角，使其快速到達(dá)目標(biāo)點(diǎn)。7. 如權(quán)利要求1所述的農(nóng)業(yè)機(jī)械自動導(dǎo)航控制方法，其特征在于，所述步驟3具體為:通 過改進(jìn)型純追蹤算法和模糊控制算法分別得出前輪轉(zhuǎn)角，然后將兩個轉(zhuǎn)角合成一個最終的 轉(zhuǎn)角，實(shí)現(xiàn)兩種方法復(fù)合控制，其中合成公式如下： d = kidi+k2d2 其中l(wèi)u、k2分別為模糊控制器輸出的前輪轉(zhuǎn)角和改進(jìn)型純追蹤控制器輸出的前輪轉(zhuǎn)角 的折算因子，兩者之和為1。8. 如權(quán)利要求3和4所述的農(nóng)業(yè)機(jī)械自動導(dǎo)航控制方法，其特征在于，所述快速查找算 法是計(jì)算預(yù)定義路徑的每個點(diǎn)與當(dāng)前農(nóng)業(yè)機(jī)械坐標(biāo)點(diǎn)的距離，距離最小的那個點(diǎn)即為最近
【文檔編號】G05D1/02GK105867377SQ201610228933
【公開日】2016年8月17日
【申請日】2016年4月13日
【發(fā)明人】李革, 王宇, 吳苗苗, 章鐵成, 王嘉鵬, 郭劉粉
【申請人】浙江理工大學(xué)