基于狀態空間建模的智能農機模型辨識與柔化控制

袁洪良 郭 銳 薛夢琦 盧瀟瀟 楊浚宇 徐立鴻

(1.同濟大學電子與信息工程學院， 上海 201804； 2.中移(成都)產業研究院， 成都 610041)

0 引言

我國各類農業機械保有量大，研制智能農機裝備是我國智慧農業發展的重點任務。傳統農業機械(農機)作業勞動強度大，作業效率不高，作業質量參差不齊。隨著導航技術和車輛控制技術的發展，傳統農機裝備正在逐步向智能化精準化發展。自動導航控制算法具有比人類駕駛員的作業精度更高，降低農機駕駛員的工作負擔，提高生產效率等優勢[1-4]。

農機自動導航算法是農機自動導航系統的核心，主要有預瞄、追蹤、模糊、PID和基于模型的控制等類別[5-9]。

許多研究中使用了基于運動學模型的控制算法[10-15]。運動學模型的特點是建模簡單、易于實現，但模型精度不高。在變速行駛、高速行駛和不平整地面工況中，往往無法滿足橫向控制的精確性和穩定性要求，因此有研究提出了基于農機動力學模型的控制方法。文獻[15-17]建立了一種新型的農機航向角速度動力學模型，在此基礎上設計了LQR控制器，在農機的高速行駛時具有較好的控制效果。EATON等[18-19]將反步(Backstepping)控制方法應用到農機的路徑跟蹤控制中，取得了較好的效果。RAJAMANI[20]指出隨著車輛速度的提高，車輛運動學模型不能準確描述系統，動力學模型具有更高的精度。

本文以東風DF1004-2型拖拉機為實驗平臺，使用實驗法對其橫擺運動進行狀態空間動力學模型辨識，并通過仿真平臺和實驗測試進行模型驗證。在此基礎上設計柔化LQR控制策略以期較好地解決農機速度切變時的弱穩定問題。

1 農機自動導航系統設計與實現

農機自動導航系統包括農機自動導航控制終端、伺服電機、角度傳感器、北斗衛星定位模塊、RTK基準站、車載觸控平板計算機等。控制終端負責信息采集和控制計算；伺服電機是方向盤執行機構，用于轉動方向盤；角度傳感器用于反饋導向輪角度；RTK基準站提供北斗差分校正信息，北斗模塊提供農機位置航向信息，RTK-北斗能夠實現厘米級定位[21]；觸控平板計算機用于人機交互。

1.1 自動導航硬件系統搭建

硬件部分設計主要包括自動導航終端和RTK基站的印刷電路板，以及前輪轉角傳感器、方向盤電機和轉向管柱的適配機構，系統架構如圖1所示。

圖1 智能農機系統架構Fig.1 Intelligent agricultural machinery system architecture

上述硬件設計合適的結構后安裝固定在農機上，使用農機自身12 V蓄電池供電，圖2為完成硬件搭建的東風DF1004-2型拖拉機自動導航實驗平臺。

圖2 智能農機自動導航系統實驗平臺Fig.2 Intelligent automatic driving system for agricultural tractor

1.2 農機模型辨識

為提升控制性能，對農機自動導航系統進行包括橫擺動力學的狀態空間建模。在運動學模型中，橫向運動模型只有航向角一個狀態，這使得控制策略中也只能使用當前的航向角作為反饋。如果橫向運動模型階數提高，即把航向角速度也作為一個狀態進行建模，則控制器將具備根據航向角運動的趨勢進行調節的能力，從而提升控制系統的快速性、穩定性和精度。

基于上述思路，建立農機運動四維狀態空間模型，4個狀態分別為農機橫向誤差、航向角、航向角速度和前輪轉角，農機路徑跟蹤示意圖如圖3所示。

圖3 農機路徑跟蹤示意圖Fig.3 Path following diagram of agricultural tractor

圖3中，e為農機橫向誤差，θyaw為農機航向角，δact為農機實際前輪轉角，v為農機縱向速度。由運動學幾何關系得到農機橫向誤差e的變化率

(1)

在小角度條件下，式(1)可近似為

(2)

本文所選方向盤執行機構是直流伺服電機，通過雙環反饋控制設計為轉角伺服系統。控制量輸入到實際的前輪轉角的動力學模型為一階模型，即

(3)

式中δcmd——農機指令前輪轉角

K1——一階系統增益

τ——一階系統時間常數

s——拉普拉斯算子

根據車輛動力學理論，在硬路面上輪胎抓地良好的條件下，航向角速度、車速和前輪轉角之間的關系式為

(4)

式中L——農機軸距

在小角度條件下可以對tanδact進行近似，得

(5)

考慮到農田土壤松軟、農機輪胎側滑等因素，輪胎側向力的建立不像硬路面那么快速，因此本文假設，航向角速度在式(5)的基礎上，還存在一個一階滯后環節。因此在s域，航向角速度和前輪轉角的關系可表示為

(6)

式中r——航向角速度

K2——一階滯后環節的比例系數

T——一階滯后環節的時間常數

式(6)反變換至時間域即可得農機橫擺動力學模型

(7)

又由于農機航向角速度為航向角的導數，因此有

(8)

聯立式(2)～(8)可整理得農機橫向動力學狀態空間方程為

(9)

考慮到農機循AB線穩態行駛時導向輪調整幅度較小，因此在系統辨識實驗設計中分別以±3°、±5°和±8°作為農機前輪轉角的階躍輸入信號，每個指令轉角均在旱田中進行了多組實驗。通過Matlab系統辨識工具箱處理，結果顯示在不同指令前輪轉角作用下，各參數辨識結果較為一致，實驗結果如表1所示。該結果也驗證了式(6)中一階滯后環節的假設具有可行性。

對表1中各組數據作均值處理可得農機的各項參數最終值，K1為1.01、τ為0.32、K2為-1.16、T為0.06。

為驗證農機橫向動力學模型參數的準確性，在Simulink中搭建農機狀態空間模型進行模型驗證，如圖4所示。

表1 農機系統辨識實驗結果Tab.1 Experimental result of agricultural tractor system identification

圖4 拖拉機橫向動力學模型驗證平臺Fig.4 Verification platform for lateral dynamic model of agricultural tractor

仿真實驗中選取指令前輪轉角為5°，行駛速度為1.5 m/s的工況，得到農機指令前輪角-實際前輪轉角模型驗證效果如圖5所示，農機實際前輪轉角-航向角模型驗證效果如圖6所示。

圖5 農機前輪轉角模型驗證Fig.5 Verification of front wheel angle model of agricultural tractor

圖6 農機航向角模型驗證Fig.6 Verification of yaw angle model of agricultural tractor

從圖5、6中可以看出，相同輸入下，農機實際航向角和仿真航向角的曲線吻合程度較高，且穩態值驗算也符合式(7)，表明辨識結果準確，所建模型可以支持狀態空間反饋控制。

1.3 農機自動導航控制算法

(1)將所建立和驗證的動力學模型作為控制對象進行狀態空間反饋設計。把式(9)改寫為狀態空間標準表達式

(10)

其中

式中x——系統狀態A——系統矩陣

B——輸入矩陣u——系統輸入

(2)設計LQR控制器對農機前輪轉角進行控制。LQR控制器是以系統狀態變量x和控制變量u的線性二次型為性能指標，使閉環系統漸近穩定且指標最優的控制器。該問題在線性定常系統上有系統性的解法且控制效果良好，因此在工業上有廣泛應用。假設農機自動導航控制系統的性能指標為J，則

(11)

式中Q——狀態變量加權矩陣

R——系統輸入加權矩陣

Q為半正定常數對稱矩陣，R為正定常數對稱矩陣。實際調參過程中，選擇Q為對角陣，又由于Q和R不需要同時變化，且系統是單輸入系統，因此可選取R=1。本文經過實驗調試，Q取值為

Q=diag[0.3 0.5 0.001 0.000 1]

最優控制器是使性能指標式(11)達到最小的狀態反饋控制器，其形式是

u=-Kx

(12)

求解最優控制問題就是要尋找增益矩陣K，使得上述性能指標J最小，根據龐特里亞金極小值原理和李雅普諾夫穩定性理論可以推導出，當

K=R-1BTP

(13)

Q+ATP+PA-PBR-1BTP=0

(14)

成立時性能指標J達到最小值[22]。

式(13)、(14)中P為對稱正定矩陣，式(14)也稱作代數黎卡提方程。代數黎卡提方程可以轉換為一個多元二次方程組進行解析求解，但由于算法需要在嵌入式控制器上使用C語言進行實現，且系統矩陣A中存在變量v，解析法求解代碼較為復雜，故本文中采用迭代法來求解P，算法為

Φ0=0

E=(I-A)-1(I-A)

G=2(I-A)-1B

H=R+BT(I-A)-1Q(I-A)-1B

W=Q(I-A)-1B

while |Φi+1-Φi|≥εdo

T=(ETΦiG+W)(GTΦiG+H)-1(ETΦiG+W)

Φi+1=ETΦiE-T+Q

算法中ε為可調節正常數，I為單位陣，矩陣E、G、H、W、T、Φ為中間變量。算法迭代到滿足條件的Φi+1后(一般迭代30次即可收斂，計算時間不到10 ms)，可以求得黎卡提方程的解為

P=2(I-AT)-1Φi+1(I-A)-1

(15)

把迭代法求解結果式(15)與Matlab中解析法求解的結果進行比對，驗證了2種算法結果是一致的。

(3)設計柔化控制器。LQR控制器在無速度突變時的直線路徑跟蹤中性能較好，但是在農機行駛速度發生突變時，算法輸出的控制指令有時會產生較大的振蕩，控制性能下降。因此在LQR控制器的基礎上，進一步設計了柔化控制器，其表達式為

(16)

式中u——控制輸入m——控制輸出

Δ、k——柔化參數

系統整體控制架構為雙閉環反饋控制，如圖7所示。內環為方向盤電機的角度伺服控制，電機配置為轉速輸入模式，由PID控制器根據指令和實際轉角的誤差進行調節。外環是基于狀態空間模型的LQR柔化控制器，實現農機的路徑跟蹤控制。

圖7 農機雙閉環反饋控制框圖Fig.7 Block diagram of agricultural tractor control system

2 實驗與結果分析

為驗證上述控制方法的性能，在搭建的農機自動導航平臺上實現了LQR和柔化控制算法，并在實驗田中針對未柔化勻速場景、未柔化速度突變場景和柔化速度突變場景分別進行了實驗，測試現場如圖8所示。

圖8 農機測試現場Fig.8 Dongfeng agricultural tractor test scenario

2.1 未柔化、無速度突變場景

本項實驗考察了單獨使用LQR控制器時，在常見的勻速直線路徑跟蹤作業中的性能。圖9、10顯示了在勻速條件下農機橫向誤差、航向角、車輛行駛速度和前輪轉角變化曲線。可以看到控制系統經過5 s左右的調節之后，農機進入穩態行駛階段，穩態誤差波動范圍在0.03 m以內。圖10也顯示在整個行駛過程中農機前輪實際轉角能夠較好地跟蹤控制指令，電機的角度伺服性能良好。可見，單獨LQR控制器對無速度突變的直線路徑跟蹤性能較好。

圖9 未柔化、無速度突變時橫向誤差和航向角Fig.9 Lateral error and yaw angle of agricultural tractor before flexible control without speed change

圖10 未柔化、無速度突變時農機速度和前輪轉角Fig.10 Speed and front wheel angle of agricultural tractor before flexible control without speed change

圖11 未柔化、速度突變時農機橫向誤差和航向角Fig.11 Lateral error and yaw angle of agricultural tractor before flexible control with speed change

2.2 未柔化、有速度突變場景

本實驗考察了單獨LQR控制器在速度突變條件下的響應。圖11、12為農機從勻速突然變速再到勻速工況下的橫向誤差、航向角、速度和前輪轉角的變化曲線。從圖中可以看到，LQR控制器對速度變化表現較為敏感，控制輸出和系統狀態均產生較大幅度的振蕩，穩定性下降。系統經過約10 s調整以后，仍能再次趨于穩定，但動態過程中位置誤差最大值達到了0.4 m以上，農機車身出現明顯的擺動，不滿足實際作業需求。因此，需對速度突變條件下的系統控制進行柔化處理。

圖12 未柔化、速度突變時農機速度和前輪轉角Fig.12 Speed and front wheel angle of agricultural tractor before flexible control with speed change

2.3 柔化、有速度突變場景

實驗中在LQR控制器后串聯了柔化控制器，增加了系統的阻尼，使速度突變造成的狀態變化有一個較平緩的過渡過程，以解決速度突變條件下發生的穩定性弱化的問題。本場景中速度進行了連續階梯狀突變，比圖12中的速度變化更為劇烈，未柔化控制器在本場景中會迅速發散、失控。柔化控制后的效果如圖13、14所示。

圖13 柔化控制后農機橫向誤差和航向角Fig.13 Lateral error and yaw angle of agricultural tractor after flexible control

圖14 柔化控制后農機速度和前輪轉角Fig.14 Speed and front wheel angle of agricultural tractor after flexible control

由圖13、14可知，在連續變速的條件下，進行柔化控制優化之后，農機仍保持穩定，雖然農機速度的變化會導致農機被控狀態產生小幅波動，比無速度突變場景(圖9、10)略微增大，但是由于對算法輸出進行了柔化控制，前輪轉角并未產生大幅振蕩，農機橫向誤差也始終維持在0.05 m以內，農機車身未產生擺動。

總體而言，柔化控制改善了農機在變速條件下的穩定性，無論是勻速情況還是變速情況，農機均保持穩定，并且有較短的調節時間以及較高的控制精度，滿足日常作業需求。

3 結束語

研究了農機自動導航中建模和控制的相關問題。首先，搭建了農機自動導航的硬件系統。其次，對搭建的硬件平臺進行動力學建模，采用實驗法分別建立了實際前輪轉角-航向角模型和期望前輪轉角-實際前輪轉角模型，并建立起包括農機橫擺動力學的狀態空間模型。選取橫向偏差、航向角、航向角速度和前輪轉角4個狀態，與基于運動學模型的控制系統不同，由于使用了航向角速度反饋，使得控制器能夠預見航向角的變化趨勢，并更快做出響應。此外為了優化在農機速度切變時穩定性減弱的問題，對LQR算法的結果進行了柔化處理，避免了導向輪和系統狀態的大幅振蕩，使農機橫向誤差精度在勻速狀態下達到0.03 m，變速狀態下可維持在0.05 m以內，農機路徑跟蹤性能有明顯提升，可以滿足農機日常作業需求。