基于運(yùn)動(dòng)特性的農(nóng)機(jī)導(dǎo)航控制方法

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

(1.中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所， 沈陽(yáng) 110016；2.遼寧省農(nóng)業(yè)裝備智能化技術(shù)重點(diǎn)實(shí)驗(yàn)室， 沈陽(yáng) 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)境較差，輪胎與地面作用過(guò)程復(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ì)給基于模型的控制方法的控制效果帶來(lái)不良影響，這會(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)向的有效控制。但是多處采用小角度近似替代，在曲線跟蹤過(guò)程中控制系統(tǒng)會(huì)存在穩(wěn)定性變差問(wèn)題，這種建模方法適用于低速行駛農(nóng)機(jī)或?qū)M向誤差要求不是很高的農(nóng)機(jī)。農(nóng)機(jī)動(dòng)力學(xué)模型主要通過(guò)車輪與地面之間的復(fù)雜作用來(lái)對(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ī)在路徑跟蹤過(guò)程中的誤差。傳統(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è)偏角，以提高方向角精度和控制器的收斂速度，減小小角度近似替代帶來(lái)的誤差，并基于該模型進(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é)建模

在不考慮車輪與地面的相互作用，無(wú)側(cè)傾、俯仰、側(cè)滑等運(yùn)動(dòng)的情況下，可將拖拉機(jī)、插秧機(jī)等多種四輪農(nóng)機(jī)簡(jiǎn)化為二輪車模型進(jìn)行運(yùn)動(dòng)學(xué)分析。在建模過(guò)程中將側(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)過(guò)程中采用小尺寸近似替代和極小項(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部分組成，各部分之間通過(guò)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)過(guò)程：在實(shí)驗(yàn)場(chǎng)地選取A、B兩點(diǎn)，將A、B兩點(diǎn)確定的直線作為車輛跟蹤的目標(biāo)路徑，如圖4所示，在點(diǎn)A附近啟動(dòng)車輛和控制程序開(kāi)始預(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)過(guò)程和數(shù)據(jù)采集過(guò)程與直線路徑跟蹤實(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í)開(kāi)始從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)差均有所減小。表明在直線路徑跟蹤過(guò)程中，控制方法中加入側(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é)模型建模過(guò)程中引入側(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é)模型建模過(guò)程中加入側(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é)模型，提高車輛建模精度，曲線行駛過(guò)程中側(cè)偏角對(duì)車輛航向角的影響較大，可有效提高控制器的收斂速度。

4 結(jié)束語(yǔ)

針對(duì)農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型中的傳統(tǒng)二輪車模型只考慮橫向誤差和航向誤差而采用小角度近似替代的問(wèn)題，本文提出了一種考慮車輛側(cè)偏角的農(nóng)機(jī)運(yùn)動(dòng)學(xué)建模方法，在傳統(tǒng)運(yùn)動(dòng)學(xué)模型的基礎(chǔ)上加入側(cè)偏角β弱化小角度近似替代帶來(lái)的誤差，有效提高農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型的建模精度。通過(guò)實(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í)的控制性能有明顯提升。