基于干擾觀測器的直播機路徑跟蹤快速終端滑模控制

武 濤 李彥明 林洪振,2 貢 亮 劉成良,2

(1.上海交通大學機械與動力工程學院， 上海 200240； 2.機械系統與振動國家重點實驗室， 上海 200240)

0 引言

在土壤特性復雜多變，高度非結構化的水田環境下，農業機械(農機)本身會受到許多不確定性因素干擾，這給智能農機精確地按照既定軌跡自動駕駛帶來了極大困難。要求使用的控制算法具備一定抗干擾和自適應能力，才能保證農機自動駕駛具有較高的精度和穩定性。

國內外學者在農機自動駕駛路徑跟蹤控制算法上已進行了廣泛研究[1-2]。主要集中在建立農機的運動學[3-9]、動力學[10-14]或純追蹤模型[15-18]，以及常用的模糊控制[19-22]、PID控制[23-25]、最優控制[26]和滑模控制[27-30]等控制方法上。由于運動學模型簡單高效，對參數精確度要求較低，因此將農機簡化為二輪車，用一些狀態變量描述農機位置和姿態能建立較為準確的農機運動學模型。文獻[3，5]較早建立了拖拉機的二輪車運動學模型，并將航向角偏差、橫向偏差和車輪轉角誤差作為參量設計自動駕駛控制算法。建立的二輪車運動學模型適用于任何前輪轉向的車輛，控制算法可以對自動耕作系統所需的各種軌跡進行較為精確的跟蹤。由于農田中地況復雜，農機負載多變，因此農機的運動學模型與實際系統會有一定偏差。

為保證控制的魯棒性，需要更高階的動力學模型來保證精確的橫向控制。文獻[10]基于大量實驗提出一種農機偏航動力學模型系統，在速度8 m/s的情況下也能保證拖拉機的橫向位置精度。文獻[14]建立了農機的二輪車動力學模型，應用擴展卡爾曼濾波理論和農機狀態方程估計出無法測量的狀態參數。總的來說，動力學模型對參數的精確度要求較高，但在實際工程中車輛和田地的參數又很難準確獲得。因此為避免建模不準確導致控制效果較差等問題，有學者提出一種幾何控制方法——純追蹤控制。文獻[16]提出的純追蹤算法，前視距離由BP神經網絡確定，并將其應用于農機地邊界調頭導航。文獻[18]針對插秧機自動導航建立模糊控制器，以速度和橫向位置偏差作為輸入，輸出純追蹤算法的前視距離。雖然純追蹤控制能有效避免由模型不準確帶來的問題，但對前視距離的計算精度要求較高，否則會引起控制過程中的振蕩。PID控制結構簡單，適應性強，因此也經常被應用于農機導航控制。但PID控制的主要問題在于針對不同工況時參數整定，以及控制超調和響應時間較為困難。

滑模變結構控制因其對擾動不敏感、調節參數少、響應速度快等優點被廣泛應用于非線性控制中。文獻[27]針對農用履帶機器人提出了一種基于等效控制和切換控制的自適應滑模控制策略，并根據農機運動學模型和電機驅動特性設計了一種變傾斜參數的自適應積分滑模切換函數。該自適應滑模控制方法具有較高的抗干擾性，但控制過程中會有抖振發生。滑模控制的缺點主要是控制的不連續性會引起抖振以及收斂時間無限等。為解決此問題，文獻[29]使用自適應滑模控制來提高農機自動轉向控制精度，滑模面采用非線性積分的形式。同時通過使用模糊控制方法來自適應調節滑模控制的開關增益以減輕控制輸入抖振現象。文獻[30]同樣在滑模控制中使用模糊控制方法抑制抖振，提高了農機的導航控制精度，但不能保證收斂時間。

針對滑模控制的抖振和收斂時間無限等問題，本文設計一種帶非線性干擾觀測器的快速終端滑模控制算法，以提高水稻直播機在水田環境下的抗干擾能力和路徑跟蹤精度。

1 基于干擾補償的農機運動學模型

由于水田環境下土壤特性復雜，難以建立準確的直播機動力學模型，因此選擇建立帶滑移和轉向干擾補償的農機運動學模型。如圖1所示，不考慮農機動力學特性，將直播機的運動學模型簡化為二輪車模型[31]。圖中，M(xm,ym)為后輪中心，車體控制點；T(xt,yt)為M點在期望路徑T上的垂足;θp為y軸與期望路徑的夾角，即航向角的期望值。根據以下5組參數來描述直播機的位姿：直播機后輪中心的大地坐標與期望路徑的距離、前輪轉角、行駛速度、農機行駛方向與期望路徑之間的航向角偏差、前后輪軸距。

由于期望路徑AB固定，所以航向角偏差的導數與航向角速度相等，即

(1)

式中V——行進速度，m/s

R——直播機轉彎半徑，m

θ——航向角，(°)

θe——航向角偏差，(°)

由圖中幾何關系可得

(2)

式中L——前后輪軸距，m

δ——前輪轉角，控制輸入，(°)

所以

(3)

橫向偏差的導數即為速度V在該方向上的分量，即

(4)

式中de——橫向偏差，m

以橫向偏差de和航向角偏差θe為狀態變量，同時考慮水田中地況復雜多變，為避免未知干擾導致建模不準確和滑模控制器抖振現象發生，特引入農機的滑移和轉向干擾。由式(3)、(4)可得帶干擾的直播機運動學模型的狀態方程為

(5)

式中VR——滑移速度干擾，m/s

δR——前輪轉向干擾，(°)

引入復合干擾項：橫向位置干擾|ξ|≤P和航向角干擾|ζ|≤K，P和K為干擾的最大值，之后用非線性干擾觀測器對其進行精確觀測，并補償到控制器中。復合干擾綜合滑移和轉向干擾后的直播機運動學模型為

(6)

式中x1——橫向偏差，m

x2——航向角偏差，(°)

2 非線性干擾觀測器設計

由于在實際工作過程中直播機受到的復合干擾很難直接獲得，因此建立非線性干擾觀測器對其進行觀測。令

(7)

(8)

式中x(t)——狀態變量

d(t)——復合干擾

u(t)——控制輸入，即前輪轉角δ

由式(6)可知，直播機狀態方程為

(9)

令復合干擾d的估計為，求得實際值d與估計值的差值，然后對估計值進行補償得

(10)

式中L(de,θe)——正定的增益矩陣

z=-p(de,θe)

(11)

其中

p(de,θe)=L(de,θe)x+D

(12)

式中z——輔助向量

p(de,θe)——設計的函數向量

D——輔助常量矩陣

由式(10)～(12)求得z的導數

(13)

因此，非線性干擾觀測器改進后的方程為

(14)

令復合干擾的觀測誤差ed=d-，由于觀測器和復合干擾的動態特性相比，前者變化較為迅速，于是可作出假設所以

(15)

將式(9)、(12)、(14)代入式(15)整理可得

(16)

(17)

因此估計誤差ed漸進收斂，選取合適的正定增益矩陣L(de,θe)能讓估計誤差迅速趨于0，可準確估計出直播機運動學模型中的復合干擾。

3 非奇異快速終端滑模控制器設計

終端滑模[32]的設計思想是在滑模面中引入非線性項，使得系統進入滑模面后能在有限時間內收斂，可解決線性滑模面不適用復雜非線性系統且收斂時間無窮大的問題。傳統終端滑模面為

(18)

式中β——非線性項系數，β>0

p、q——正奇數，p>q

系統收斂時間為

(19)

但當系統狀態遠離滑模面時，以上終端滑模控制收斂速度比較慢。而且由于控制律可能會包含帶有負指數的項，當該狀態趨于零時會導致控制趨于無窮，即產生奇異現象。為解決以上問題，本文設計能使橫向偏差de全局快速收斂的快速終端滑模面

(20)

式中α——線性項系數

將橫向偏差de作為重要的路徑跟蹤精度評價指標。在滑模面的設計中引入de的線性項和非線性冪次項，使得當系統狀態遠離滑模面時由線性項保證收斂速度，進入滑模面后非線性冪次項起主要作用，從而保證系統具備全局快速收斂的特性。同時控制律中不包含負指數，也不會產生奇異。

趨近律設計為

(21)

式中k、λ——系數，均大于0

由式(6)、(14)、(20)、(21)設計控制律為

(22)

控制律收斂性證明，選取李雅普諾夫函數

(23)

由式(20)、(21)得

(24)

當s>0時

(25)

當s<0時

(26)

4 仿真驗證

使用Matlab/Simulink驗證本文所提控制算法的有效性。根據圖2控制系統結構圖建立相應的仿真模型。直播機系統仿真模型參數如表1所示。

表1 控制系統仿真參數Tab.1 Simulation parameters of control system

為了驗證本文設計的非線性干擾觀測器對復合干擾的估計效果，對控制系統模型施加復合干擾。干擾及其估計曲線如圖4、5所示。由圖4、5可以看出，觀測器對橫向位置干擾ξ和航向角干擾ζ的估計值與施加的實際值基本吻合，說明觀測器能實現對復合干擾的精確觀測，可將觀測器的估計值補償到直播機運動學模型中以提高模型的準確度。

最后驗證將觀測器估計值補償到控制器中能否降低干擾造成的抖振。在施加干擾的情況下，仿真對比了3種算法的路徑跟蹤效果：①帶干擾觀測的快速終端滑模控制。②不帶干擾觀測的快速終端滑模控制。③不帶干擾觀測的傳統滑模控制。仿真結果如圖6所示。3種算法路徑跟蹤過程中橫向偏差的均方差分別為0.003 3、0.012 6、0.037 7 m。

從圖6可知，在有干擾存在但無補償的情況下，路徑跟蹤過程中會發生明顯的抖振。本文所提的快速終端滑模控制相比傳統滑模控制具備一定的抗抖振能力。當有干擾觀測補償時，系統抖振明顯降低，說明采用干擾觀測器估計干擾值并補償到控制器中能有效降低滑模控制抖振，提高路徑跟蹤控制精度，證明了本文所提控制算法的有效性。

5 實驗驗證

5.1 實驗平臺

對洋馬集團生產的YR10D型水稻直播機進行無人化改造，如圖7所示。整車自動駕駛控制系統由上位機主控制器通過CAN/RS-232的通信方式接收北斗位置信息，并對車身各子控制系統進行控制，各子系統相互協作使得直播機完成自主駕駛和作業。主控器采用微嵌Linux工業計算機，具有4核1.6 GHz高性能處理器，以及1 GB DDR3運行內存，支持RS-232串口、CAN等通信接口。采用北京天海科公司生產的DWQT-CAN-360-G型轉角傳感器，精度為0.1°。RTK-GPS使用4G網絡差分基站，動態定位精度在0.02 m以內，數據更新頻率為20 Hz。方向盤使用直流電機驅動，采用位置-速度雙閉環控制。所使用硬件的精度和頻率足以應用于農機自動駕駛中。改造后的直播機能實現自動導航、精量播種、容量檢測、堵塞報警、自動倒車加種及自動封邊作業。同時建立了云平臺管理系統，具備路徑規劃、遠程控制、一機多控及多機協同等功能。實現了“智慧在云，智能在端，管控在屏”的設計理念。

5.2 農機作業實驗

為驗證本文算法在直播機上的實際運行效果，在上海市崇明區上實集團的水田進行實際帶種作業。實驗前該地塊信息已保存到地塊管理系統中，作業前只需加載地塊信息，將直播機停在起點處，點擊開始，直播機即可自主作業。由于本文所設計的控制算法具備較強的自適應性和魯棒性，并且對參數不敏感，一般情況無需重新調節參數。因此實驗時算法參數與表1中仿真參數保持一致即可取得較好的路徑跟蹤精度。圖8為直播機此次作業的行駛軌跡，可以看出行駛軌跡較為平滑，無偏航。為分析本文所提算法的直線跟蹤效果，截取7段直線路徑進行分析。設定的作業行距為2.5 m，但由于導航定位存在一定的誤差，每段路徑相對于理想軌跡都會有一定的橫向穩態偏差，路徑1～7的穩態誤差分別為-0.001 1、0.036 8、-0.033 7、0.033 6、-0.037 4、0.022 0、-0.031 7 m。

消除穩態偏差后，圖9顯示了每段路徑的橫向偏差，全過程的航向角變化曲線如圖10所示。由圖9可以看出，除路徑1外，所有路徑的橫向偏差基本都保持在0.05 m以內。由于路徑1是路徑跟蹤起始階段，需要一定的適應和調整時間，所以此階段的橫向偏差稍大。由圖10可以看出，直線行駛階段航向角偏差基本為零，每次當直播機行駛到地邊界調頭時航向角會相差180°，之后收斂到0°，由圖中框出部分可以看出，本文所提算法的上線效果優異，收斂速度快，無超調和抖振發生。

各路徑橫向偏差的概率分布如圖11所示，可以看出，消除了穩態誤差影響后各路徑的橫向偏差接近服從以0為均值的正態分布。說明本文所設計的干擾觀測器對復合干擾進行觀測補償后，有效抑制了直播機在水田環境下的抖振，使得直線跟蹤階段橫向偏差在0附近波動幅度較小。各路徑跟蹤過程中橫向偏差和航向角偏差的統計特性如表2所示。橫向偏差總體最大值為0.089 5 m，最小值為-0.128 3 m，平均絕對偏差為0.024 7 m，均方差為0.031 1 m。最值都出現在路徑1中，由于是初始階段，所以路徑1各項數據都較其他稍大，路徑5各項數據最小。航向角偏差總體最大值為4.249 6°，最小值為-5.631 8°，平均絕對偏差為0.923 6°，均方差為1.189 3°，同樣表明本文所提算法具有較優異的路徑跟蹤效果。并且航向角偏差具有和橫向偏差同樣的性質，體現了數據的一致性。

各項實驗數據顯示，本文所設計的控制算法在轉彎和直線跟蹤過程具有快速收斂的特性。通過對復合干擾進行精確觀測并補償到控制器中能有效抑制干擾造成的抖振。本文算法具有優異的路徑跟蹤效果，能較好地滿足實際播種作業的要求。

6 結論

(1)建立基于干擾補償的水稻直播機運動學模型，提出了一種基于非線性干擾觀測器的快速終端滑模控制算法。使用Matlab/Simulink進行了系統仿真，結果表明，本文提出的快速終端滑模控制算法在初始橫向偏差為-0.6 m時，調節時間約為2.5 s，相比于終端滑模和傳統滑模具有全局快速收斂的特點，且無超調。

(2)在水稻直播機的運動學模型中加入橫向位置干擾和航向角干擾，并設計非線性干擾觀測器對其進行估計。結果顯示，系統的不確定性干擾能被建立的非線性干擾觀測器精確地觀測出，補償到滑模控制器后能有效抑制干擾造成的抖振。

表2 各路徑橫向偏差和航向角偏差統計結果Tab.2 Statistical characteristics of lateral deviation and heading angle deviation of all driving paths

(3)以速度1.2 m/s進行了水田播種作業實驗，直線跟蹤過程中橫向平均絕對偏差為0.024 7 m，均方差為0.031 1 m，航向角平均絕對偏差為0.923 6°，均方差為1.189 3°。偏差總體數值較小，分布集中，并且轉彎時收斂速度較快，無超調。本文所提出的快速終端滑模控制算法收斂時間快，所設計的非線性干擾觀測器能有效抑制水田環境下不確定性復合干擾對路徑跟蹤造成的抖振。