張 軍，李丙春

（喀什大學(xué)計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，新疆 喀什 844006）

0 引言

上個(gè)世紀(jì)，農(nóng)業(yè)活動(dòng)實(shí)現(xiàn)了機(jī)械化，農(nóng)業(yè)機(jī)械代替了大部分繁重枯燥的人力工作。進(jìn)入本世紀(jì)，農(nóng)業(yè)活動(dòng)向智能化方向快速發(fā)展，農(nóng)機(jī)駕駛員的工作需要智能化的解決方案，農(nóng)機(jī)自動(dòng)駕駛項(xiàng)目的主要目的就是解放廣大農(nóng)機(jī)駕駛員，實(shí)現(xiàn)農(nóng)機(jī)的智能化工作。

1 農(nóng)機(jī)自動(dòng)駕駛系統(tǒng)概述

目前，業(yè)內(nèi)農(nóng)機(jī)自動(dòng)駕駛有兩種解決方案：一是電動(dòng)方向盤控制自動(dòng)轉(zhuǎn)向；二是改裝油路，利用電磁換向閥和比例閥控制自動(dòng)轉(zhuǎn)向。本文中農(nóng)機(jī)自動(dòng)駕駛系統(tǒng)采用第二種方案，系統(tǒng)主要包括兩套軟件和若干硬件。

1.1 控制軟件

控制系統(tǒng)包括數(shù)據(jù)采集、計(jì)算和車輛控制。農(nóng)機(jī)在田間行駛時(shí)的實(shí)時(shí)數(shù)據(jù)包括實(shí)時(shí)位置、實(shí)時(shí)速度和車輛實(shí)時(shí)姿態(tài)，車輛姿態(tài)包括俯仰角、橫滾角和偏航角。各個(gè)傳感器將采集到的實(shí)時(shí)數(shù)據(jù)通過(guò)CAN總線傳遞給控制軟件，控制軟件比較得到的實(shí)時(shí)數(shù)據(jù)和預(yù)先規(guī)劃行駛線路計(jì)算得出農(nóng)機(jī)發(fā)生實(shí)際偏移的距離，而后通過(guò)控制液壓電磁閥調(diào)整農(nóng)機(jī)行駛方向，使其回到正確的工作路線上。

1.2 顯示器軟件

駕駛?cè)藛T通過(guò)該軟件預(yù)先設(shè)定當(dāng)前農(nóng)機(jī)的尺寸數(shù)據(jù)和天線位置。在工作前，設(shè)定使用農(nóng)具的尺寸以及農(nóng)具和農(nóng)機(jī)之間的距離，根據(jù)實(shí)地作業(yè)需要設(shè)定農(nóng)機(jī)工作線路及工作模式。在工作時(shí)將實(shí)時(shí)工作數(shù)據(jù)傳遞給控制器，控制器根據(jù)目標(biāo)數(shù)據(jù)和農(nóng)機(jī)實(shí)時(shí)數(shù)據(jù)調(diào)整運(yùn)行路線。圖1為設(shè)計(jì)好的農(nóng)機(jī)自動(dòng)駕駛系統(tǒng)。

圖1 農(nóng)機(jī)自動(dòng)駕駛系統(tǒng)整體方案

1.3 工作系統(tǒng)

系統(tǒng)包括顯示器軟件、控制器軟件、北斗天線（包括電臺(tái)）、液壓閥和角度傳感器。顯示器軟件的作用是設(shè)定農(nóng)機(jī)、農(nóng)具參數(shù)，設(shè)定耕作工作路線以及顯示實(shí)際工作狀態(tài)。北斗天線接收北斗衛(wèi)星的衛(wèi)星信號(hào)并將衛(wèi)星數(shù)據(jù)傳入控制箱，控制箱中北斗芯片解析衛(wèi)星信號(hào)得到衛(wèi)星數(shù)據(jù)，同時(shí)結(jié)合差分基準(zhǔn)站的實(shí)時(shí)誤差數(shù)據(jù)比較實(shí)際位置與預(yù)先規(guī)劃路線，得出偏離值，差分定位計(jì)算模塊結(jié)合這兩個(gè)數(shù)據(jù)進(jìn)行差分定位計(jì)算，得到當(dāng)前實(shí)際位置，定位誤差可達(dá)厘米級(jí)。控制箱包含電子陀螺儀，持續(xù)輸出農(nóng)機(jī)的橫滾角和俯仰角，計(jì)算模塊結(jié)合偏離值和農(nóng)機(jī)實(shí)時(shí)姿態(tài)和速度計(jì)算得出液壓電磁閥進(jìn)行轉(zhuǎn)向數(shù)據(jù)控制并向液壓電磁閥發(fā)出指令。液壓電磁閥收到指令后通過(guò)控制液壓油入口大小來(lái)實(shí)施車輪轉(zhuǎn)向。車輪轉(zhuǎn)向的實(shí)時(shí)角度會(huì)通過(guò)角度傳感器反饋給計(jì)算模塊。計(jì)算模塊根據(jù)車輪實(shí)時(shí)轉(zhuǎn)向角度來(lái)調(diào)節(jié)轉(zhuǎn)向控制數(shù)據(jù)，直至拖拉機(jī)回到預(yù)先規(guī)劃好的路線上。

2 農(nóng)機(jī)自動(dòng)駕駛系統(tǒng)要解決的問(wèn)題

要實(shí)現(xiàn)以上功能，系統(tǒng)必須解決下面問(wèn)題：（1）農(nóng)具實(shí)時(shí)位置和實(shí)時(shí)速度的獲??；（2）農(nóng)機(jī)姿態(tài)數(shù)據(jù)的獲??；（3）農(nóng)機(jī)轉(zhuǎn)向的精確控制；（4）農(nóng)機(jī)方向的自動(dòng)調(diào)整。

3 問(wèn)題解決

3.1 農(nóng)具實(shí)時(shí)位置和實(shí)時(shí)速度的獲取

實(shí)時(shí)位置的獲取依賴北斗定位系統(tǒng)（BDS），北斗定位系統(tǒng)結(jié)構(gòu)如圖2。

圖2 北斗衛(wèi)星定位系統(tǒng)結(jié)構(gòu)

北斗系統(tǒng)包括3部分：空間部分、控制部分和用戶部分?？臻g部分截至2020年7月，共有55顆導(dǎo)航衛(wèi)星，其主要作用是發(fā)送用于導(dǎo)航定位的衛(wèi)星信號(hào)。衛(wèi)星的分布保證在任一時(shí)刻、任一地點(diǎn)高度角15°以上都能夠觀測(cè)到4顆以上的衛(wèi)星。控制部分由主控站、監(jiān)控站和注入站組成。

主控站：從各個(gè)監(jiān)控站收集衛(wèi)星數(shù)據(jù)，計(jì)算出衛(wèi)星的星歷和時(shí)鐘修正參數(shù)等，并通過(guò)注入站注入衛(wèi)星，向衛(wèi)星發(fā)布指令控制衛(wèi)星，在衛(wèi)星出現(xiàn)故障時(shí)調(diào)度備用衛(wèi)星。

監(jiān)控站：接收衛(wèi)星信號(hào)，檢測(cè)衛(wèi)星運(yùn)行狀態(tài)，收集天氣數(shù)據(jù)并將這些信息傳送給主控站。

注入站：將主控站計(jì)算的衛(wèi)星星歷及時(shí)鐘修正參數(shù)等注入衛(wèi)星。

用戶部分包含北斗接收機(jī)及相關(guān)設(shè)備，能夠捕獲到按一定衛(wèi)星高度截止角所選擇的待測(cè)衛(wèi)星的信號(hào)，并跟蹤這些衛(wèi)星的運(yùn)行，對(duì)所接收到的衛(wèi)星信號(hào)進(jìn)行變換、放大和處理以便測(cè)量出衛(wèi)星信號(hào)從衛(wèi)星到接收機(jī)天線的傳播時(shí)間，解譯出衛(wèi)星所發(fā)送的導(dǎo)航電文，實(shí)時(shí)計(jì)算出測(cè)站的三維位置甚至三維速度和時(shí)間。

實(shí)時(shí)定位原理：通過(guò)4顆已知位置的衛(wèi)星來(lái)確定接收器的位置。

如圖3，每個(gè)衛(wèi)星不間斷向外發(fā)射信息，信息內(nèi)容包括衛(wèi)星的實(shí)時(shí)坐標(biāo)x、y、z和發(fā)射時(shí)間Ts，假設(shè)位于地面上的接收機(jī)在Te接收到衛(wèi)星信號(hào)，接收機(jī)上信號(hào)的接收時(shí)間與衛(wèi)星信號(hào)數(shù)據(jù)的時(shí)間差是信號(hào)傳輸?shù)臅r(shí)間T=Te-Ts，信號(hào)傳播速度V=C=3×108m/s，以衛(wèi)星1為例，得出傳輸距離d1=（Te-Ts）×V，得到方程d12=（x1-x0）2+（y1-y0）2+（z1-z0）2，其中未知數(shù)為x0，y0，z0，即接收機(jī)的位置。同理，可列出衛(wèi)星2和衛(wèi)星3的方程，聯(lián)立得到三元方程組，可以解出接收機(jī)位置x0，y0，z0。然而考慮到地面接收機(jī)時(shí)間存在誤差，會(huì)影響距離結(jié)果，故將地面接收時(shí)間也設(shè)為未知數(shù)t0，重新列方程（t1-t0）2×V2=（x1-x0）2+（y1-y0）2+（z1-z0）2。方程中包含4個(gè)未知數(shù)x0，y0，z0，t0，因此，需要四元方程組方可求解4個(gè)未知數(shù)，故至少需要接收到四個(gè)衛(wèi)星信號(hào)數(shù)據(jù)才可以進(jìn)行地面定位。

圖3 衛(wèi)星系統(tǒng)定位原理

由于電離層和對(duì)流層的折射及反射等因素影響，會(huì)導(dǎo)致測(cè)距不準(zhǔn)，即圖4中的四個(gè)衛(wèi)星信號(hào)圓球很難交匯于一點(diǎn)，因此需要消除這些影響帶來(lái)的誤差，業(yè)內(nèi)采用RTK技術(shù)來(lái)減小誤差。

圖4 理想情況下衛(wèi)星信號(hào)交于一點(diǎn)

實(shí)時(shí)動(dòng)態(tài)差分技術(shù)（RTK）原理：基準(zhǔn)站用一個(gè)固定坐標(biāo)來(lái)做參考，以后基站每通過(guò)接收衛(wèi)星算得一個(gè)坐標(biāo)就跟固定坐標(biāo)作比對(duì)得到差值，然后將這個(gè)差值發(fā)送給移動(dòng)站，移動(dòng)站用衛(wèi)星接收到的坐標(biāo)再減去基站發(fā)過(guò)來(lái)的差值，就得到了改正后的坐標(biāo)，如圖5。

圖5 實(shí)時(shí)動(dòng)態(tài)差分

系統(tǒng)裝備了電臺(tái)和接收機(jī)，采用RTK技術(shù)進(jìn)行定位，實(shí)現(xiàn)了系統(tǒng)要求的高精度定位，可測(cè)量出連續(xù)位置的精確坐標(biāo)以及位置間的時(shí)間間隔并通過(guò)計(jì)算得出農(nóng)機(jī)的實(shí)時(shí)速度。

3.2 農(nóng)機(jī)姿態(tài)數(shù)據(jù)的獲取

利用MEMS陀螺儀能夠獲取農(nóng)機(jī)的姿態(tài)數(shù)據(jù)。陀螺儀的原理是一個(gè)旋轉(zhuǎn)物體的旋轉(zhuǎn)軸所指的方向在不受外力影響時(shí)保持不變。傳統(tǒng)的陀螺儀是一個(gè)不停轉(zhuǎn)動(dòng)的物體，其轉(zhuǎn)軸的指向不隨承載它的支架旋轉(zhuǎn)而變化。如圖6，陀螺儀轉(zhuǎn)動(dòng)軸在空中始終處于豎直方向，當(dāng)飛機(jī)在空中姿態(tài)變化時(shí)，會(huì)帶動(dòng)陀螺儀的框架一起同步轉(zhuǎn)動(dòng)（左右擺動(dòng)，俯仰擺動(dòng)、橫滾擺動(dòng)），測(cè)出框架和陀螺儀轉(zhuǎn)軸的各個(gè)角度，就是當(dāng)前飛機(jī)在空間中的各個(gè)角度。由于測(cè)量過(guò)程中會(huì)存在少量誤差，若長(zhǎng)期使用陀螺儀測(cè)量誤差會(huì)累計(jì)變大。因此在使用過(guò)程中通常與衛(wèi)星定位一起，通過(guò)衛(wèi)星定位定期消除陀螺儀的誤差。目前在工業(yè)上應(yīng)用的主要是MEMS陀螺儀，本系統(tǒng)采用MEMS陀螺儀。

圖6 陀螺儀測(cè)量姿態(tài)

3.3 農(nóng)機(jī)轉(zhuǎn)向的精確控制

為了實(shí)現(xiàn)農(nóng)機(jī)轉(zhuǎn)向的精確控制，對(duì)農(nóng)機(jī)進(jìn)行改造，采用電液方式進(jìn)行轉(zhuǎn)向控制，電控液壓轉(zhuǎn)向系統(tǒng)工作原理如下。

圖7 電控液壓轉(zhuǎn)向系統(tǒng)原理

電控液壓轉(zhuǎn)向系統(tǒng)在原有油路的基礎(chǔ)上并聯(lián)安裝集成液壓閥（由比例閥、三位四通電磁換向閥和安全閥組成，如途中虛線部分所示），實(shí)現(xiàn)農(nóng)機(jī)的自動(dòng)換向。系統(tǒng)工作時(shí)，根據(jù)控制器指令，三位四通電磁換向閥實(shí)現(xiàn)油路方向的切換控制，達(dá)到控制前輪左右轉(zhuǎn)向的效果，比例閥實(shí)現(xiàn)油路流量的控制，達(dá)到控制轉(zhuǎn)向速度快慢的效果。比例閥前端安裝安全閥，保證系統(tǒng)工作可靠度。

自動(dòng)轉(zhuǎn)向控制的原理是：發(fā)動(dòng)機(jī)啟動(dòng)后，轉(zhuǎn)向液壓泵開(kāi)始工作，控制器監(jiān)測(cè)農(nóng)機(jī)前輪轉(zhuǎn)向信息并發(fā)出控制指令，控制集成液壓閥的開(kāi)度和方向；液壓油經(jīng)過(guò)比例閥和電磁閥時(shí)改變方向和流量，進(jìn)入轉(zhuǎn)向油缸，油缸活塞在油壓作用下移動(dòng)，實(shí)現(xiàn)拖拉機(jī)前輪向目標(biāo)角度轉(zhuǎn)向。

3.4 農(nóng)機(jī)方向的自動(dòng)調(diào)整

農(nóng)機(jī)在工作過(guò)程中行駛路線發(fā)生偏移時(shí)應(yīng)遵循如下調(diào)整原則：

（1）農(nóng)機(jī)應(yīng)以小角度長(zhǎng)距離平緩轉(zhuǎn)向返回預(yù)設(shè)路線，否則容易造成過(guò)調(diào)，出現(xiàn)農(nóng)機(jī)以Z字形在規(guī)劃路線左右反復(fù)擺動(dòng)的情況。

（2）農(nóng)機(jī)工作時(shí)的角度誤差控制在1°～2°，距離誤差控制在8cm內(nèi)，即誤差在該范圍內(nèi)均認(rèn)為正常工作，無(wú)需糾偏，避免因系統(tǒng)過(guò)敏導(dǎo)致反復(fù)調(diào)整，造成液壓轉(zhuǎn)向裝置過(guò)熱過(guò)壓，影響系統(tǒng)使用壽命。

（3）農(nóng)機(jī)的轉(zhuǎn)向角有上限，糾偏時(shí)的轉(zhuǎn)向角不可超過(guò)此角度，否則無(wú)法執(zhí)行。

3.4.1 理想的糾偏路線

理想的糾偏路線如圖8（a）（平行規(guī)劃路線），車輛以S型曲線切線進(jìn)入規(guī)劃路線。糾偏路線設(shè)計(jì)為兩個(gè)大小相同的相切圓，農(nóng)機(jī)位置位于上圓的最下位置，入線位置位于下圓的最上位置。調(diào)整過(guò)程中會(huì)出現(xiàn)8（b）（朝向規(guī)劃路線），可看作圖8（a）的后半部分。控制思路如上。當(dāng)農(nóng)機(jī)背離規(guī)劃線路時(shí)，如圖8（c），糾偏路線依然為S型曲線，具體位置根據(jù)車輛當(dāng)前角度與切線位置相同確定。

圖8 糾偏路線

3.4.2 糾偏控制算法

農(nóng)機(jī)的路線糾偏采用模糊自適應(yīng)PID控制算法，因農(nóng)機(jī)在田地工作會(huì)受到泥土阻力和坡度影響，因此在傳統(tǒng)PID算法的基礎(chǔ)上添加人類經(jīng)驗(yàn)調(diào)整PID參數(shù)，使參數(shù)自適應(yīng)于不同的農(nóng)田環(huán)境。

3.4.2.1 PID控制器算法

PID控制算法原理：整體上采用基于誤差生成消除誤差的策略。首先，控制系統(tǒng)通過(guò)比較對(duì)象反饋值與目標(biāo)值的大小得到系統(tǒng)偏差值，偏差值經(jīng)由控制器中比例項(xiàng)（P）、積分項(xiàng)（I）和微分項(xiàng)（D）的計(jì)算后，各項(xiàng)相加得到系統(tǒng)控制量并作為對(duì)象輸入量用于對(duì)被控對(duì)象進(jìn)行調(diào)整，如圖9。

圖9 PID控制器算法

PID的公式為增量式和位置式，本系統(tǒng)選擇增量型PID控制算法。增量式PID控制算法如下：

增量型PID控制輸出僅與當(dāng)前偏差值及上兩次的偏差值相關(guān)，無(wú)需累計(jì)偏差值，輸出的是控制量的增量，在執(zhí)行上簡(jiǎn)單有效。公式中各項(xiàng)的作用如下：

（1）P參數(shù)：Kpe（k），比例項(xiàng)，為比例系數(shù)和當(dāng)前時(shí)刻偏差之積。在偏差較大時(shí)能及時(shí)減小偏差。

（2）i參數(shù)：積分項(xiàng)，為積分系數(shù)與歷史偏差累計(jì)之積，消除了靜態(tài)誤差。

靜態(tài)誤差是指單獨(dú)采用P參數(shù)時(shí)當(dāng)偏差非常小的時(shí)候，一些客觀因素（如阻力）會(huì)導(dǎo)致系統(tǒng)無(wú)法進(jìn)行調(diào)節(jié)到目標(biāo)值，最終導(dǎo)致輸出結(jié)果與目標(biāo)值有一定距離的情況，這種情況可通過(guò)累計(jì)偏差來(lái)處理，因?yàn)槔塾?jì)偏差會(huì)趨向一個(gè)固定的數(shù)值。

（3）D參數(shù)：Kd[e（k）-e（k-1）]，微分項(xiàng)，為微分系數(shù)與當(dāng)前偏差變化率之積。其主要作用為抑制超調(diào)、減少震蕩、讓系統(tǒng)快速收斂。體現(xiàn)在與P項(xiàng)進(jìn)行對(duì)抗，根據(jù)偏差變化情況進(jìn)行控制。

3.4.2.2 模糊自適應(yīng)PID控制器算法

控制原理：操作人員在長(zhǎng)期操作過(guò)程中積累了一定經(jīng)驗(yàn)知識(shí)，但經(jīng)驗(yàn)不易精確定量表示，因此運(yùn)用模糊數(shù)學(xué)的基本理論和方法，用模糊集表示規(guī)則的條件和操作，將這些模糊控制規(guī)則以及有關(guān)信息（如評(píng)價(jià)指標(biāo)、初始PID參數(shù)等）作為知識(shí)存入表中，隨后根據(jù)控制系統(tǒng)的實(shí)際響應(yīng)情況運(yùn)用模糊推理自動(dòng)實(shí)現(xiàn)對(duì)PID參數(shù)的最佳調(diào)整。模糊自適應(yīng)PID是在PID算法的基礎(chǔ)上通過(guò)計(jì)算當(dāng)前系統(tǒng)誤差e和偏差變化率ec，利用模糊規(guī)則進(jìn)行模糊推理，查詢模糊矩陣表進(jìn)行參數(shù)調(diào)整，原理如圖10。

圖10 PID控制器算法

角度傳感器回傳當(dāng)前角度值A(chǔ)k，控制系統(tǒng)計(jì)算得到error=Ak-Ak-1，ec=de/dt=error/T，作為模糊控制器的輸入變量。模糊控制器的輸出為PID控制器的參數(shù)修正量，PID調(diào)節(jié)器根據(jù)獲取到的調(diào)節(jié)量對(duì)參數(shù)Kp、Ki、Kd進(jìn)行調(diào)整，結(jié)合當(dāng)前的誤差error和ec，PID控制器輸出控制電流I，電流I改變電磁比例閥的開(kāi)口大小，從而控制農(nóng)機(jī)轉(zhuǎn)向角度大小。

模糊控制器設(shè)計(jì)：（1）確定輸入輸出變量的論域。將變量量化設(shè)為7個(gè)等級(jí)：分別是負(fù)大（NB）、負(fù)中（NM），負(fù)?。∟S）、零（ZO）、正?。≒S）、正中（PM）、正大（PB）。取所有變量的量化論域?yàn)閇-6，6]，轉(zhuǎn)向角偏差的基本論域?yàn)閇-30，30]，量化因子為0.2，轉(zhuǎn)向角偏差變化率ec的基本論域?yàn)閇-30，30]，量化因子為 0.2，ΔKp的基本論域?yàn)閇-30，30]，量化因子為 0.2，ΔKi的基本論域?yàn)閇-6，6]，量化因子為 1，ΔKd的基本論域?yàn)閇-3，-3]，量化因子為2。（2）隸屬函數(shù)采用高斯隸屬函數(shù)。（3）確定模糊控制規(guī)則。模糊控制規(guī)則是模糊控制器的的核心，根據(jù)實(shí)踐經(jīng)驗(yàn)編寫(xiě)，形式如表1。根據(jù)角度偏差e和偏差變化率ec的值對(duì)參數(shù)進(jìn)行調(diào)整，每個(gè)參數(shù)都可根據(jù)實(shí)際情況定制控制表格。

表1 PID模糊調(diào)整規(guī)則

4 總結(jié)

作者在2014年～2015年期間曾參與農(nóng)機(jī)自動(dòng)駕駛系統(tǒng)的研發(fā)工作，系統(tǒng)研發(fā)成功后在東北和新疆各地進(jìn)行測(cè)試調(diào)整，測(cè)試結(jié)果同國(guó)外優(yōu)秀產(chǎn)品相比功能上基本相當(dāng)。該系統(tǒng)在直線的穩(wěn)定性上稍有不足，原因?yàn)槲纯紤]平緩小角度，以切線方式入線以及PID算法造成過(guò)調(diào)問(wèn)題，文中已經(jīng)對(duì)入線策略進(jìn)行了詳細(xì)改進(jìn)闡述，并對(duì)控制算法進(jìn)行了優(yōu)化，采用模糊自適應(yīng)PID控制器算法，后期將繼續(xù)研究改進(jìn)方案算法，使產(chǎn)品達(dá)到國(guó)外同類產(chǎn)品效果。