農機自動駕駛系統方案剖析

張 軍，李丙春

（喀什大學計算機科學與技術學院，新疆 喀什 844006）

0 引言

上個世紀，農業活動實現了機械化，農業機械代替了大部分繁重枯燥的人力工作。進入本世紀，農業活動向智能化方向快速發展，農機駕駛員的工作需要智能化的解決方案，農機自動駕駛項目的主要目的就是解放廣大農機駕駛員，實現農機的智能化工作。

1 農機自動駕駛系統概述

目前，業內農機自動駕駛有兩種解決方案：一是電動方向盤控制自動轉向；二是改裝油路，利用電磁換向閥和比例閥控制自動轉向。本文中農機自動駕駛系統采用第二種方案，系統主要包括兩套軟件和若干硬件。

1.1 控制軟件

控制系統包括數據采集、計算和車輛控制。農機在田間行駛時的實時數據包括實時位置、實時速度和車輛實時姿態，車輛姿態包括俯仰角、橫滾角和偏航角。各個傳感器將采集到的實時數據通過CAN總線傳遞給控制軟件，控制軟件比較得到的實時數據和預先規劃行駛線路計算得出農機發生實際偏移的距離，而后通過控制液壓電磁閥調整農機行駛方向，使其回到正確的工作路線上。

1.2 顯示器軟件

駕駛人員通過該軟件預先設定當前農機的尺寸數據和天線位置。在工作前，設定使用農具的尺寸以及農具和農機之間的距離，根據實地作業需要設定農機工作線路及工作模式。在工作時將實時工作數據傳遞給控制器，控制器根據目標數據和農機實時數據調整運行路線。圖1為設計好的農機自動駕駛系統。

圖1 農機自動駕駛系統整體方案

1.3 工作系統

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

2 農機自動駕駛系統要解決的問題

要實現以上功能，系統必須解決下面問題：（1）農具實時位置和實時速度的獲取；（2）農機姿態數據的獲取；（3）農機轉向的精確控制；（4）農機方向的自動調整。

3 問題解決

3.1 農具實時位置和實時速度的獲取

實時位置的獲取依賴北斗定位系統（BDS），北斗定位系統結構如圖2。

圖2 北斗衛星定位系統結構

北斗系統包括3部分：空間部分、控制部分和用戶部分。空間部分截至2020年7月，共有55顆導航衛星，其主要作用是發送用于導航定位的衛星信號。衛星的分布保證在任一時刻、任一地點高度角15°以上都能夠觀測到4顆以上的衛星。控制部分由主控站、監控站和注入站組成。

主控站：從各個監控站收集衛星數據，計算出衛星的星歷和時鐘修正參數等，并通過注入站注入衛星，向衛星發布指令控制衛星，在衛星出現故障時調度備用衛星。

監控站：接收衛星信號，檢測衛星運行狀態，收集天氣數據并將這些信息傳送給主控站。

注入站：將主控站計算的衛星星歷及時鐘修正參數等注入衛星。

用戶部分包含北斗接收機及相關設備，能夠捕獲到按一定衛星高度截止角所選擇的待測衛星的信號，并跟蹤這些衛星的運行，對所接收到的衛星信號進行變換、放大和處理以便測量出衛星信號從衛星到接收機天線的傳播時間，解譯出衛星所發送的導航電文，實時計算出測站的三維位置甚至三維速度和時間。

實時定位原理：通過4顆已知位置的衛星來確定接收器的位置。

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

圖3 衛星系統定位原理

由于電離層和對流層的折射及反射等因素影響，會導致測距不準，即圖4中的四個衛星信號圓球很難交匯于一點，因此需要消除這些影響帶來的誤差，業內采用RTK技術來減小誤差。

圖4 理想情況下衛星信號交于一點

實時動態差分技術（RTK）原理：基準站用一個固定坐標來做參考，以后基站每通過接收衛星算得一個坐標就跟固定坐標作比對得到差值，然后將這個差值發送給移動站，移動站用衛星接收到的坐標再減去基站發過來的差值，就得到了改正后的坐標，如圖5。

圖5 實時動態差分

系統裝備了電臺和接收機，采用RTK技術進行定位，實現了系統要求的高精度定位，可測量出連續位置的精確坐標以及位置間的時間間隔并通過計算得出農機的實時速度。

3.2 農機姿態數據的獲取

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

圖6 陀螺儀測量姿態

3.3 農機轉向的精確控制

為了實現農機轉向的精確控制，對農機進行改造，采用電液方式進行轉向控制，電控液壓轉向系統工作原理如下。

圖7 電控液壓轉向系統原理

電控液壓轉向系統在原有油路的基礎上并聯安裝集成液壓閥（由比例閥、三位四通電磁換向閥和安全閥組成，如途中虛線部分所示），實現農機的自動換向。系統工作時，根據控制器指令，三位四通電磁換向閥實現油路方向的切換控制，達到控制前輪左右轉向的效果，比例閥實現油路流量的控制，達到控制轉向速度快慢的效果。比例閥前端安裝安全閥，保證系統工作可靠度。

自動轉向控制的原理是：發動機啟動后，轉向液壓泵開始工作，控制器監測農機前輪轉向信息并發出控制指令，控制集成液壓閥的開度和方向；液壓油經過比例閥和電磁閥時改變方向和流量，進入轉向油缸，油缸活塞在油壓作用下移動，實現拖拉機前輪向目標角度轉向。

3.4 農機方向的自動調整

農機在工作過程中行駛路線發生偏移時應遵循如下調整原則：

（1）農機應以小角度長距離平緩轉向返回預設路線，否則容易造成過調，出現農機以Z字形在規劃路線左右反復擺動的情況。

（2）農機工作時的角度誤差控制在1°～2°，距離誤差控制在8cm內，即誤差在該范圍內均認為正常工作，無需糾偏，避免因系統過敏導致反復調整，造成液壓轉向裝置過熱過壓，影響系統使用壽命。

（3）農機的轉向角有上限，糾偏時的轉向角不可超過此角度，否則無法執行。

3.4.1 理想的糾偏路線

理想的糾偏路線如圖8（a）（平行規劃路線），車輛以S型曲線切線進入規劃路線。糾偏路線設計為兩個大小相同的相切圓，農機位置位于上圓的最下位置，入線位置位于下圓的最上位置。調整過程中會出現8（b）（朝向規劃路線），可看作圖8（a）的后半部分。控制思路如上。當農機背離規劃線路時，如圖8（c），糾偏路線依然為S型曲線，具體位置根據車輛當前角度與切線位置相同確定。

圖8 糾偏路線

3.4.2 糾偏控制算法

農機的路線糾偏采用模糊自適應PID控制算法，因農機在田地工作會受到泥土阻力和坡度影響，因此在傳統PID算法的基礎上添加人類經驗調整PID參數，使參數自適應于不同的農田環境。

3.4.2.1 PID控制器算法

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

圖9 PID控制器算法

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

增量型PID控制輸出僅與當前偏差值及上兩次的偏差值相關，無需累計偏差值，輸出的是控制量的增量，在執行上簡單有效。公式中各項的作用如下：

（1）P參數：Kpe（k），比例項，為比例系數和當前時刻偏差之積。在偏差較大時能及時減小偏差。

（2）i參數：積分項，為積分系數與歷史偏差累計之積，消除了靜態誤差。

靜態誤差是指單獨采用P參數時當偏差非常小的時候，一些客觀因素（如阻力）會導致系統無法進行調節到目標值，最終導致輸出結果與目標值有一定距離的情況，這種情況可通過累計偏差來處理，因為累計偏差會趨向一個固定的數值。

（3）D參數：Kd[e（k）-e（k-1）]，微分項，為微分系數與當前偏差變化率之積。其主要作用為抑制超調、減少震蕩、讓系統快速收斂。體現在與P項進行對抗，根據偏差變化情況進行控制。

3.4.2.2 模糊自適應PID控制器算法

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

圖10 PID控制器算法

角度傳感器回傳當前角度值Ak，控制系統計算得到error=Ak-Ak-1，ec=de/dt=error/T，作為模糊控制器的輸入變量。模糊控制器的輸出為PID控制器的參數修正量，PID調節器根據獲取到的調節量對參數Kp、Ki、Kd進行調整，結合當前的誤差error和ec，PID控制器輸出控制電流I，電流I改變電磁比例閥的開口大小，從而控制農機轉向角度大小。

模糊控制器設計：（1）確定輸入輸出變量的論域。將變量量化設為7個等級：分別是負大（NB）、負中（NM），負小（NS）、零（ZO）、正小（PS）、正中（PM）、正大（PB）。取所有變量的量化論域為[-6，6]，轉向角偏差的基本論域為[-30，30]，量化因子為0.2，轉向角偏差變化率ec的基本論域為[-30，30]，量化因子為 0.2，ΔKp的基本論域為[-30，30]，量化因子為 0.2，ΔKi的基本論域為[-6，6]，量化因子為 1，ΔKd的基本論域為[-3，-3]，量化因子為2。（2）隸屬函數采用高斯隸屬函數。（3）確定模糊控制規則。模糊控制規則是模糊控制器的的核心，根據實踐經驗編寫，形式如表1。根據角度偏差e和偏差變化率ec的值對參數進行調整，每個參數都可根據實際情況定制控制表格。

表1 PID模糊調整規則

4 總結

作者在2014年～2015年期間曾參與農機自動駕駛系統的研發工作，系統研發成功后在東北和新疆各地進行測試調整，測試結果同國外優秀產品相比功能上基本相當。該系統在直線的穩定性上稍有不足，原因為未考慮平緩小角度，以切線方式入線以及PID算法造成過調問題，文中已經對入線策略進行了詳細改進闡述，并對控制算法進行了優化，采用模糊自適應PID控制器算法，后期將繼續研究改進方案算法，使產品達到國外同類產品效果。