白曉平 孟 鵬 王 卓 時(shí) 佳

(1.中國(guó)科學(xué)院沈陽自動(dòng)化研究所， 沈陽 110016；2.遼寧省農(nóng)業(yè)裝備智能化技術(shù)重點(diǎn)實(shí)驗(yàn)室， 沈陽 110016； 3.中國(guó)科學(xué)院大學(xué)， 北京 100049)

0 引言

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

國(guó)內(nèi)外眾多學(xué)者對(duì)路徑跟蹤控制方法的研究，包括基于農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型的控制算法和基于農(nóng)機(jī)動(dòng)力學(xué)模型的控制算法[23-25]。農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型采用小角度近似化方法簡(jiǎn)化兩輪車運(yùn)動(dòng)學(xué)模型，主要考慮了農(nóng)機(jī)的運(yùn)動(dòng)學(xué)約束對(duì)農(nóng)機(jī)控制的影響，建模簡(jiǎn)單，便于設(shè)計(jì)控制器實(shí)現(xiàn)對(duì)農(nóng)機(jī)轉(zhuǎn)向的有效控制。但是多處采用小角度近似替代，在曲線跟蹤過程中控制系統(tǒng)會(huì)存在穩(wěn)定性變差問題，這種建模方法適用于低速行駛農(nóng)機(jī)或?qū)M向誤差要求不是很高的農(nóng)機(jī)。農(nóng)機(jī)動(dòng)力學(xué)模型主要通過車輪與地面之間的復(fù)雜作用來對(duì)車輛進(jìn)行建模，在一定環(huán)境中可以滿足農(nóng)機(jī)路徑跟蹤時(shí)的穩(wěn)定性控制要求。但是忽略了農(nóng)機(jī)作業(yè)環(huán)境的復(fù)雜性和不確定性，建立動(dòng)力學(xué)模型較為困難。同時(shí)，農(nóng)機(jī)作業(yè)環(huán)境發(fā)生改變，基于農(nóng)機(jī)動(dòng)力學(xué)模型設(shè)計(jì)的控制器也會(huì)發(fā)生較大偏差，不適用于多種作業(yè)環(huán)境下的農(nóng)機(jī)建模。

精確的農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型可以提高路徑跟蹤控制器的控制品質(zhì)，減小農(nóng)機(jī)在路徑跟蹤過程中的誤差。傳統(tǒng)二輪車運(yùn)動(dòng)學(xué)模型通常利用后輪為目標(biāo)點(diǎn)進(jìn)行運(yùn)動(dòng)學(xué)分析，采用小角度近似將三角函數(shù)轉(zhuǎn)換為線性函數(shù)和小角度近似替代而簡(jiǎn)化運(yùn)算，只考慮橫向偏差和航向偏差，忽略了車輛側(cè)偏角對(duì)車輛航向的影響。本文將以車輛質(zhì)心為目標(biāo)點(diǎn)進(jìn)行運(yùn)動(dòng)學(xué)建模分析，在建立的車輛運(yùn)動(dòng)學(xué)模型中引入側(cè)偏角，以提高方向角精度和控制器的收斂速度，減小小角度近似替代帶來的誤差，并基于該模型進(jìn)行狀態(tài)反饋控制器設(shè)計(jì)。

1 農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型

1.1 側(cè)向運(yùn)動(dòng)分析

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

當(dāng)時(shí)間t=t1時(shí)，在△CDA和△CDB上分別使用正弦定理得

(1)

(2)

(3)

由式(3)得

(4)

(5)

將式(2)兩側(cè)同時(shí)乘以lflr可得

(6)

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

(7)

1.2 運(yùn)動(dòng)學(xué)建模

在不考慮車輪與地面的相互作用，無側(cè)傾、俯仰、側(cè)滑等運(yùn)動(dòng)的情況下，可將拖拉機(jī)、插秧機(jī)等多種四輪農(nóng)機(jī)簡(jiǎn)化為二輪車模型進(jìn)行運(yùn)動(dòng)學(xué)分析。在建模過程中將側(cè)偏角引入農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型，提高農(nóng)機(jī)方向角的精度。定義速度方向與車輛縱軸方向?yàn)閭?cè)偏角β。農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型示意圖如圖2所示。

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

當(dāng)dt非常小時(shí)，由△BB′H得

dy=vcosβsinθdt

(8)

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

(9)

式中l(wèi)CE——理論路徑瞬時(shí)旋轉(zhuǎn)半徑

lCH——理論路徑瞬時(shí)旋轉(zhuǎn)中心到后輪的距離與車輛橫向位移之和

lB′H——實(shí)際路徑中車輛在相鄰時(shí)刻dt內(nèi)后輪移動(dòng)的距離

lEF——理論路徑中車輛在相鄰時(shí)刻dt內(nèi)后輪移動(dòng)的距離

由式(9)可得

(10)

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

(11)

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

(12)

將式(12)代入dθ可得

(13)

綜合式(8)、(11)、(13)，可得農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型為

(14)

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

2 控制器設(shè)計(jì)

其中

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

u=-KX

(15)

其中

K=[k1k2]

(16)

(17)

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

(18)

由式(18)可得

(19)

參數(shù)k1和k2的影響因素主要有3方面：極點(diǎn)配置得到的比值、車輛轉(zhuǎn)向機(jī)構(gòu)的性能和農(nóng)機(jī)作業(yè)環(huán)境。在確定參數(shù)k1和k2的比值后，根據(jù)農(nóng)機(jī)作業(yè)時(shí)的實(shí)際工作環(huán)境對(duì)具體取值進(jìn)行適當(dāng)調(diào)整。

3 實(shí)車實(shí)驗(yàn)

3.1 實(shí)驗(yàn)平臺(tái)

實(shí)驗(yàn)平臺(tái)為配有自動(dòng)導(dǎo)航控制系統(tǒng)的拖拉機(jī)，前輪轉(zhuǎn)向，后輪驅(qū)動(dòng)，如圖3所示。

實(shí)驗(yàn)平臺(tái)上的自動(dòng)導(dǎo)航控制系統(tǒng)采用分布式控制策略，由位置檢測(cè)單元、航向檢測(cè)單元、導(dǎo)航控制器和轉(zhuǎn)向控制單元4部分組成，各部分之間通過CAN總線進(jìn)行通信。其中，位置檢測(cè)單元由GPS和智能節(jié)點(diǎn)組成，GPS實(shí)時(shí)采集車輛的位置信息，采樣頻率為20 Hz，定位精度為(0.01±10-12)m。航向檢測(cè)單元由慣導(dǎo)MTI和智能節(jié)點(diǎn)組成，MTI實(shí)時(shí)采集車輛的航向信息，采樣頻率為20 Hz，測(cè)量精度為±1°。

3.2 實(shí)驗(yàn)設(shè)計(jì)

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

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

設(shè)計(jì)2組對(duì)照實(shí)驗(yàn)：第1組為基于傳統(tǒng)二輪車運(yùn)動(dòng)學(xué)模型設(shè)計(jì)的控制方法進(jìn)行路徑跟蹤；第2組為基于優(yōu)化后的運(yùn)動(dòng)學(xué)模型設(shè)計(jì)的控制方法進(jìn)行路徑跟蹤。車輛從起始點(diǎn)出發(fā)時(shí)開始從CAN總線上采集數(shù)據(jù)，接近終點(diǎn)時(shí)停止數(shù)據(jù)采集，對(duì)設(shè)定路徑段內(nèi)的2組對(duì)照實(shí)驗(yàn)數(shù)據(jù)分別進(jìn)行路徑跟蹤分析和誤差分析，并將2組實(shí)驗(yàn)進(jìn)行對(duì)比分析。

3.3 實(shí)驗(yàn)結(jié)果及分析

采用不同的運(yùn)動(dòng)學(xué)模型，相同的控制方法(以狀態(tài)反饋控制方法為例)設(shè)計(jì)對(duì)照實(shí)驗(yàn)證明本文提出優(yōu)化模型的有效性。

3.3.1直線路徑跟蹤

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

2組實(shí)驗(yàn)的直線跟蹤誤差分別如圖7所示。

2組對(duì)照實(shí)驗(yàn)的直線路徑跟蹤實(shí)驗(yàn)統(tǒng)計(jì)結(jié)果如表1所示。

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

由圖7和表1可知，基于優(yōu)化后運(yùn)動(dòng)學(xué)模型設(shè)計(jì)的控制方法在直線路徑跟蹤時(shí)與基于傳統(tǒng)二輪車模型設(shè)計(jì)的路徑跟蹤控制方法相比，最大橫向誤差、絕對(duì)平均誤差以及標(biāo)準(zhǔn)差均有所減小。表明在直線路徑跟蹤過程中，控制方法中加入側(cè)偏角可以在一定程度上提高控制方法的精度和穩(wěn)定性。

直線行駛狀態(tài)下，當(dāng)航向偏差角和橫向偏差較小時(shí)，側(cè)偏角近似為0，對(duì)控制器的影響很??；當(dāng)航向偏差或橫向偏差較大時(shí)，側(cè)偏角增大，可提高控制器的收斂速度，對(duì)控制器有一定的影響。

3.3.2曲線路徑跟蹤

第1組實(shí)驗(yàn)未考慮農(nóng)機(jī)側(cè)偏角，曲線路徑跟蹤局部放大情況如圖8a所示；第2組實(shí)驗(yàn)在運(yùn)動(dòng)學(xué)模型建模過程中引入側(cè)偏角β，曲線路徑跟蹤局部放大情況如圖8b所示。

2組實(shí)驗(yàn)的曲線跟蹤誤差如圖9所示。對(duì)曲線路徑的跟蹤實(shí)驗(yàn)統(tǒng)計(jì)結(jié)果如表2所示。

由圖9和表2可知，優(yōu)化模型控制的曲線路徑跟蹤橫向誤差與傳統(tǒng)模型控制相比，絕對(duì)平均誤差減少0.022 9 m,標(biāo)準(zhǔn)差減少0.033 6 m，兩者均有大幅降低。表明在農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型建模過程中加入側(cè)偏角對(duì)路徑跟蹤控制方法的控制精度有很大提高，會(huì)提高控制方法的控制性能。

表2 控制曲線路徑跟蹤橫向誤差統(tǒng)計(jì)Tab.2 Control curve path tracking lateral error statistics m

曲線行駛狀態(tài)下，傳統(tǒng)二輪車運(yùn)動(dòng)學(xué)模型只考慮了航向偏差和橫向偏差，采用了小角度近似替代；優(yōu)化后的模型將側(cè)偏角引入運(yùn)動(dòng)學(xué)模型，提高車輛建模精度，曲線行駛過程中側(cè)偏角對(duì)車輛航向角的影響較大，可有效提高控制器的收斂速度。

4 結(jié)束語

針對(duì)農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型中的傳統(tǒng)二輪車模型只考慮橫向誤差和航向誤差而采用小角度近似替代的問題，本文提出了一種考慮車輛側(cè)偏角的農(nóng)機(jī)運(yùn)動(dòng)學(xué)建模方法，在傳統(tǒng)運(yùn)動(dòng)學(xué)模型的基礎(chǔ)上加入側(cè)偏角β弱化小角度近似替代帶來的誤差，有效提高農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型的建模精度。通過實(shí)車道路實(shí)驗(yàn)測(cè)試結(jié)果表明，2組對(duì)照實(shí)驗(yàn)曲線路徑跟蹤的絕對(duì)平均橫向誤差分別為0.091 7、0.068 8 m，標(biāo)準(zhǔn)差分別為0.077 0、0.043 4 m，實(shí)驗(yàn)結(jié)果表明本文提出的農(nóng)機(jī)運(yùn)動(dòng)學(xué)建模方法對(duì)路徑跟蹤控制方法在曲線路徑跟蹤時(shí)的控制性能有明顯提升。