基于運動特性的農機導航控制方法

白曉平 孟 鵬 王 卓 時 佳

(1.中國科學院沈陽自動化研究所， 沈陽 110016；2.遼寧省農業裝備智能化技術重點實驗室， 沈陽 110016； 3.中國科學院大學， 北京 100049)

0 引言

隨著“智慧農業”的提出，農業機械(農機)自動導航技術已經成為了智能化精準農業中的重要組成部分[1-8]。為了提高農機導航控制的精度和穩定性，許多學者對農機運動學模型和導航控制方法進行了大量的研究[9-21]。運動學模型的研究仍舊主要集中在KELLY[22]提出的傳統“二輪車模型”方面；控制方法的研究則不再局限于傳統的輸出控制上，路徑跟蹤模型控制也成了一個重要的研究方向。由于農機作業環境較差，輪胎與地面作用過程復雜，建立精確的動力學模型比較困難，現有模型多采用傳統“二輪車模型”和傳統輸出控制方法進行導航控制器設計。傳統“二輪車模型”的優點是運動學模型簡單，考慮因素較少，易于建模；缺點是模型精度較低，會給基于模型的控制方法的控制效果帶來不良影響，這會成為進一步提高農機自動導航精度的最大阻礙。

國內外眾多學者對路徑跟蹤控制方法的研究，包括基于農機運動學模型的控制算法和基于農機動力學模型的控制算法[23-25]。農機運動學模型采用小角度近似化方法簡化兩輪車運動學模型，主要考慮了農機的運動學約束對農機控制的影響，建模簡單，便于設計控制器實現對農機轉向的有效控制。但是多處采用小角度近似替代，在曲線跟蹤過程中控制系統會存在穩定性變差問題，這種建模方法適用于低速行駛農機或對橫向誤差要求不是很高的農機。農機動力學模型主要通過車輪與地面之間的復雜作用來對車輛進行建模，在一定環境中可以滿足農機路徑跟蹤時的穩定性控制要求。但是忽略了農機作業環境的復雜性和不確定性，建立動力學模型較為困難。同時，農機作業環境發生改變，基于農機動力學模型設計的控制器也會發生較大偏差，不適用于多種作業環境下的農機建模。

精確的農機運動學模型可以提高路徑跟蹤控制器的控制品質，減小農機在路徑跟蹤過程中的誤差。傳統二輪車運動學模型通常利用后輪為目標點進行運動學分析，采用小角度近似將三角函數轉換為線性函數和小角度近似替代而簡化運算，只考慮橫向偏差和航向偏差，忽略了車輛側偏角對車輛航向的影響。本文將以車輛質心為目標點進行運動學建模分析，在建立的車輛運動學模型中引入側偏角，以提高方向角精度和控制器的收斂速度，減小小角度近似替代帶來的誤差，并基于該模型進行狀態反饋控制器設計。

1 農機運動學模型

1.1 側向運動分析

圖1所示二輪車模型中，左右2個前輪由位于點A的車輪代替，后輪由位于點B的中央后輪代替。前輪轉向角用δ表示。此模型成立的前提是假設前輪轉向，后輪驅動且后輪不可轉向。圖中坐標系XOY為大地坐標系，點C為車輛的瞬時旋轉中心，車輛質心為點D，從農機質心到點A和點B的距離分別為lf和lr，農機軸距為l=lf+lr，R為農機旋轉半徑，v為農機質心處速度，ψ為農機當前時刻的航向角，β為農機側偏角即車輛質心處速度方向與前后輪軸向之間的夾角，利用三角形內角與外角之間的關系，由△CO′A與△CO′B可得三角形各內角值。

當時間t=t1時，在△CDA和△CDB上分別使用正弦定理得

(1)

(2)

(3)

由式(3)得

(4)

(5)

將式(2)兩側同時乘以lflr可得

(6)

可由式(5)減去式(6)得到側偏角β為

(7)

1.2 運動學建模

在不考慮車輪與地面的相互作用，無側傾、俯仰、側滑等運動的情況下，可將拖拉機、插秧機等多種四輪農機簡化為二輪車模型進行運動學分析。在建模過程中將側偏角引入農機運動學模型，提高農機方向角的精度。定義速度方向與車輛縱軸方向為側偏角β。農機運動學模型示意圖如圖2所示。

圖2中，S為理論路徑，點E和點G分別對應t1和t2時刻車輛在理論路徑S上的位置，EK和KG分別對應點E和點G的切線，β-θ為航向偏差角，γ為航向角變化量，y為車輛當前位置的橫向偏差，dy為dt=t2-t1時間內橫向偏差的變化量，dθ1為實際航向角變化量，由圖中幾何關系可知dθ=γ，dθ2為理論航向角變化量。

當dt非常小時，由△BB′H得

dy=vcosβsinθdt

(8)

如圖2所示△CEF～△CHB′，且

(9)

式中lCE——理論路徑瞬時旋轉半徑

lCH——理論路徑瞬時旋轉中心到后輪的距離與車輛橫向位移之和

lB′H——實際路徑中車輛在相鄰時刻dt內后輪移動的距離

lEF——理論路徑中車輛在相鄰時刻dt內后輪移動的距離

由式(9)可得

(10)

假設曲率c(x)和速度v均為一固定范圍內的值，當dt很小時，sinθ趨向于0，dt趨向于0，可將c(x)vcosβsinθdt近似為0，所以可將式(10)近似化為

(11)

航向角變化量由2部分組成：dθ=dθ1+dθ2，由圖2所示幾何關系可知

(12)

將式(12)代入dθ可得

(13)

綜合式(8)、(11)、(13)，可得農機運動學模型為

(14)

實際情況中，曲率c(x)和橫向誤差y一般小于1，所以1-c(x)y≠0，在模型推導過程中采用小尺寸近似替代和極小項近似為0的方法，所以實際情況下，曲率c(x)越小，模型的精度越高，控制效果也更好。

2 控制器設計

其中

根據線性系統理論，對該系統采用極點配置法可以獲得漸近穩定的控制率。由rank(BAB)=2可知，系統完全可控，可對其進行極點配置。在s平面上選擇一組合理的、具有所期望性能品質指標的極點-3±2j為該閉環系統的極點。反饋控制率為

u=-KX

(15)

其中

K=[k1k2]

(16)

(17)

將式(14)、(17)代入式(15)得

(18)

由式(18)可得

(19)

參數k1和k2的影響因素主要有3方面：極點配置得到的比值、車輛轉向機構的性能和農機作業環境。在確定參數k1和k2的比值后，根據農機作業時的實際工作環境對具體取值進行適當調整。

3 實車實驗

3.1 實驗平臺

實驗平臺為配有自動導航控制系統的拖拉機，前輪轉向，后輪驅動，如圖3所示。

實驗平臺上的自動導航控制系統采用分布式控制策略，由位置檢測單元、航向檢測單元、導航控制器和轉向控制單元4部分組成，各部分之間通過CAN總線進行通信。其中，位置檢測單元由GPS和智能節點組成，GPS實時采集車輛的位置信息，采樣頻率為20 Hz，定位精度為(0.01±10-12)m。航向檢測單元由慣導MTI和智能節點組成，MTI實時采集車輛的航向信息，采樣頻率為20 Hz，測量精度為±1°。

3.2 實驗設計

車輛行駛速度v=1.0 m/s。模型控制參數根據極點配置法得到的比例值結合實際情況微調。采用高精度GPS采點并獲取車輛當前位置信息，慣導MTI獲取車輛航向信息，車輛控制器從CAN總線上獲取位置信息和航向信息進行解析。

實驗過程：在實驗場地選取A、B兩點，將A、B兩點確定的直線作為車輛跟蹤的目標路徑，如圖4所示，在點A附近啟動車輛和控制程序開始預定路徑的跟蹤，并從CAN總線實時獲取車輛當前位置信息和航向信息，當車輛接近點B時，停止數據采集；曲線路徑跟蹤實驗的目標路徑是由2條直線和2條曲線組合而成，需在實驗場地采集6個點，目標路徑如圖5所示，實驗過程和數據采集過程與直線路徑跟蹤實驗類似。

設計2組對照實驗：第1組為基于傳統二輪車運動學模型設計的控制方法進行路徑跟蹤；第2組為基于優化后的運動學模型設計的控制方法進行路徑跟蹤。車輛從起始點出發時開始從CAN總線上采集數據，接近終點時停止數據采集，對設定路徑段內的2組對照實驗數據分別進行路徑跟蹤分析和誤差分析，并將2組實驗進行對比分析。

3.3 實驗結果及分析

采用不同的運動學模型，相同的控制方法(以狀態反饋控制方法為例)設計對照實驗證明本文提出優化模型的有效性。

3.3.1直線路徑跟蹤

第1組實驗的直線路徑跟蹤局部放大情況如圖6a所示；第2組實驗的直線路徑跟蹤局部放大情況如圖6b所示。

2組實驗的直線跟蹤誤差分別如圖7所示。

2組對照實驗的直線路徑跟蹤實驗統計結果如表1所示。

表1 控制直線路徑跟蹤橫向誤差統計Tab.1 Control statistics of lateral error in linear path tracking m

由圖7和表1可知，基于優化后運動學模型設計的控制方法在直線路徑跟蹤時與基于傳統二輪車模型設計的路徑跟蹤控制方法相比，最大橫向誤差、絕對平均誤差以及標準差均有所減小。表明在直線路徑跟蹤過程中，控制方法中加入側偏角可以在一定程度上提高控制方法的精度和穩定性。

直線行駛狀態下，當航向偏差角和橫向偏差較小時，側偏角近似為0，對控制器的影響很小；當航向偏差或橫向偏差較大時，側偏角增大，可提高控制器的收斂速度，對控制器有一定的影響。

3.3.2曲線路徑跟蹤

第1組實驗未考慮農機側偏角，曲線路徑跟蹤局部放大情況如圖8a所示；第2組實驗在運動學模型建模過程中引入側偏角β，曲線路徑跟蹤局部放大情況如圖8b所示。

2組實驗的曲線跟蹤誤差如圖9所示。對曲線路徑的跟蹤實驗統計結果如表2所示。

由圖9和表2可知，優化模型控制的曲線路徑跟蹤橫向誤差與傳統模型控制相比，絕對平均誤差減少0.022 9 m,標準差減少0.033 6 m，兩者均有大幅降低。表明在農機運動學模型建模過程中加入側偏角對路徑跟蹤控制方法的控制精度有很大提高，會提高控制方法的控制性能。

表2 控制曲線路徑跟蹤橫向誤差統計Tab.2 Control curve path tracking lateral error statistics m

曲線行駛狀態下，傳統二輪車運動學模型只考慮了航向偏差和橫向偏差，采用了小角度近似替代；優化后的模型將側偏角引入運動學模型，提高車輛建模精度，曲線行駛過程中側偏角對車輛航向角的影響較大，可有效提高控制器的收斂速度。

4 結束語

針對農機運動學模型中的傳統二輪車模型只考慮橫向誤差和航向誤差而采用小角度近似替代的問題，本文提出了一種考慮車輛側偏角的農機運動學建模方法，在傳統運動學模型的基礎上加入側偏角β弱化小角度近似替代帶來的誤差，有效提高農機運動學模型的建模精度。通過實車道路實驗測試結果表明，2組對照實驗曲線路徑跟蹤的絕對平均橫向誤差分別為0.091 7、0.068 8 m，標準差分別為0.077 0、0.043 4 m，實驗結果表明本文提出的農機運動學建模方法對路徑跟蹤控制方法在曲線路徑跟蹤時的控制性能有明顯提升。