田間道路無(wú)人駕駛搬運(yùn)車(chē)自動(dòng)循跡行駛控制策略

孫紅偉， 李云伍， 王小娟， 黃雪燕， 劉得雄

(西南大學(xué)工程技術(shù)學(xué)院，重慶 400715)

農(nóng)用物資在田間道路(特別是在丘陵山區(qū))上的搬運(yùn)耗費(fèi)時(shí)間長(zhǎng)、勞動(dòng)強(qiáng)度大、工作效率低下，亟須自動(dòng)行駛的搬運(yùn)車(chē)來(lái)解放勞動(dòng)力、提高工作效率[1-2]。自動(dòng)循跡行駛是農(nóng)業(yè)機(jī)械自動(dòng)行駛的關(guān)鍵技術(shù)之一，近年來(lái)受到國(guó)內(nèi)外學(xué)者的廣泛研究，如張聞?dòng)畹忍岢隽艘环N基于支持向量回歸(support vector regression，簡(jiǎn)稱(chēng)SVR)逆向模型的拖拉機(jī)導(dǎo)航純追蹤控制方法[3]；熊中剛等提出了一種基于免疫模糊PID控制[比例(proportion)-積分(integral)-微分(derivative)控制器，簡(jiǎn)稱(chēng)PID控制]的小型農(nóng)業(yè)機(jī)械路徑智能跟蹤控制方法[4]；韓科立等提出了基于最優(yōu)控制的拖拉機(jī)路徑跟蹤方法[5]。

在已有的自動(dòng)循跡控制方法中，PID控制及變形PID控制擁有結(jié)構(gòu)簡(jiǎn)單、控制實(shí)時(shí)性高等特點(diǎn)[4，6-7]而受到廣泛應(yīng)用，但常規(guī)的PID控制是一種線性控制器，田間道路曲率變化大、起伏不平，在各種田間道路上常規(guī)PID控制系統(tǒng)難以均達(dá)到精確的自動(dòng)循跡行駛控制。因此，提出一種基于分段式PID控制并預(yù)瞄前方路徑曲率的自動(dòng)循跡行駛控制方法。分段式PID能根據(jù)搬運(yùn)車(chē)行駛的狀態(tài)自適應(yīng)調(diào)整PID控制系統(tǒng)的參數(shù)，有效改善常規(guī)PID控制的線性定常組合方式，提高搬運(yùn)車(chē)自動(dòng)循跡行駛的精度。預(yù)瞄控制能使搬運(yùn)車(chē)根據(jù)前方路徑的彎曲程度提前做出相應(yīng)的轉(zhuǎn)彎動(dòng)作，以保證自動(dòng)循跡行駛搬運(yùn)車(chē)轉(zhuǎn)向的平穩(wěn)性。

1 數(shù)學(xué)模型的建立

試驗(yàn)用田間道路自動(dòng)行駛搬運(yùn)車(chē)是針對(duì)丘陵山區(qū)田間道路狹小、起伏、曲折等特征專(zhuān)門(mén)設(shè)計(jì)的(圖1)。其主要參數(shù)如下：(1)質(zhì)量：滿載150 kg；(2)驅(qū)動(dòng)：電動(dòng)后輪驅(qū)動(dòng)，功率 300 W；(3)轉(zhuǎn)向：舵機(jī)驅(qū)動(dòng)前輪轉(zhuǎn)向；(4)前/后輪距：480 mm/510 mm；(5)軸距：1 100 mm；(6)前輪最大轉(zhuǎn)角：30°。

1.1 搬運(yùn)車(chē)轉(zhuǎn)向幾何模型

根據(jù)阿克曼轉(zhuǎn)向原理，假設(shè)搬運(yùn)車(chē)的每個(gè)車(chē)輪在任意時(shí)刻都是在做以側(cè)向加速度為0的圓周運(yùn)動(dòng)或直線運(yùn)動(dòng)，且搬運(yùn)車(chē)只做平行于地面的平面運(yùn)動(dòng)，得到搬運(yùn)車(chē)的完整轉(zhuǎn)向模型(圖2-a)[8-9]。為計(jì)算簡(jiǎn)便，將搬運(yùn)車(chē)4輪完整模型簡(jiǎn)化為中心線上布置前后輪的2輪模型，得到簡(jiǎn)化的轉(zhuǎn)向模型(圖2-b)。

根據(jù)搬運(yùn)車(chē)的轉(zhuǎn)向幾何模型，得到式(1)、(2)、(3)的幾何關(guān)系。

(1)

(2)

(3)

根據(jù)式(1)、(2)、(3)得到搬運(yùn)車(chē)簡(jiǎn)化轉(zhuǎn)向模型的轉(zhuǎn)角與轉(zhuǎn)彎半徑的關(guān)系，如式(4)所示。

(4)

1.2 二維車(chē)路關(guān)系模型

根據(jù)簡(jiǎn)化的搬運(yùn)車(chē)轉(zhuǎn)向幾何模型，再把道路以中心線為基準(zhǔn)簡(jiǎn)化為一條曲線，以搬運(yùn)車(chē)的前輪為研究對(duì)象，取離前輪中心最近的路徑中心點(diǎn)為坐標(biāo)原點(diǎn)O，以道路中心線的切線為x軸建立坐標(biāo)系，構(gòu)建二維車(chē)路關(guān)系模型(圖3)。

對(duì)搬運(yùn)車(chē)橫向偏差求導(dǎo)得到橫向偏差變化率，此變化率與搬運(yùn)車(chē)相對(duì)道路中心線的航向角θ成正相關(guān)關(guān)系，此關(guān)系可以用式(5)表示。

(5)

式中：e(k)、e(k-1)分別表示k時(shí)刻與k-1時(shí)刻的橫向偏差；T為2次偏差的時(shí)間間隔。

為使搬運(yùn)車(chē)沿著曲率半徑為ρ的道路行駛，根據(jù)式(4)可得搬運(yùn)車(chē)的轉(zhuǎn)角δ1，如式(6)所示。

(6)

2 控制策略

2.1 分段式PID控制的設(shè)計(jì)

在數(shù)字化控制中，常規(guī)的離散PID控制表達(dá)式如式(7)所示。

(7)

式中：kP、kI、kD分別為比例控制、積分控制、微分控制的常數(shù)；k表示積分總時(shí)間；T為1個(gè)控制周期的時(shí)間；e(j)、e(k)與e(k-1) 分別為j時(shí)刻、k時(shí)刻與k-1時(shí)刻的偏差值。

在PID控制策略中以搬運(yùn)車(chē)的橫向偏差e(k)作為比例環(huán)節(jié)的輸入變量，一旦產(chǎn)生偏差立即產(chǎn)生控制作用，提高控制系統(tǒng)的響應(yīng)速度；橫向偏差e(k)的積分值作為積分環(huán)節(jié)的輸入變量，以消除系統(tǒng)中存在的靜差；由式(5)計(jì)算得到的航向角θ作為微分環(huán)節(jié)的輸入變量，θ能反映e(k)的變化趨勢(shì)，為控制系統(tǒng)引入1個(gè)有效的早期修正信號(hào)。

在常規(guī)離散PID控制中積分常數(shù)為固定值，當(dāng)偏差較大時(shí)積分作用會(huì)造成大偏差的積累，以及造成較大的系統(tǒng)超調(diào)，這對(duì)搬運(yùn)車(chē)循跡的精度影響很大。在分段式PID控制系統(tǒng)中，當(dāng)目標(biāo)值與實(shí)際反饋值的偏差達(dá)到切換閾值時(shí)，系統(tǒng)從現(xiàn)行的PID控制算法跳轉(zhuǎn)到另外一個(gè)PID參數(shù)更合適的控制算法，使控制系統(tǒng)更加精確可靠[10]。田間道路曲折多變?nèi)菀自斐煞e分環(huán)節(jié)大偏差的積累，因此田間道路搬運(yùn)車(chē)自動(dòng)循跡行駛控制采用分段PID控制算法，以提高自動(dòng)循跡行駛控制的精度。

以搬運(yùn)車(chē)的橫向偏差e(k)作為分段PID控制的輸入變量，并以其值的域值范圍把PID控制分為以下幾段：

(1)當(dāng)-e1

(2)當(dāng)e1

(3)當(dāng)e(k)>e2或e(k)<-e2時(shí)，搬運(yùn)車(chē)的橫向偏差較大，去除積分控制部分即令式(7)中的ki=0，只使用PD控制。這樣既可以防止積分項(xiàng)積累大偏差而難以消除導(dǎo)致系統(tǒng)超調(diào)，又可以快速調(diào)節(jié)橫向的偏差，使系統(tǒng)快速回到正常狀態(tài)。

根據(jù)以上的分析，自動(dòng)循跡的分段PID控制系統(tǒng)框架如圖4所示。

自動(dòng)循跡行駛的分段PID控制與常規(guī)PID控制的區(qū)別是加入了對(duì)偏差的選擇判斷，圖4中積分作用I在不同的橫向偏差e(t)下設(shè)定為不同的積分系數(shù)。由于進(jìn)行了分段，被控對(duì)象狀態(tài)發(fā)生變化時(shí)積分系數(shù)也會(huì)隨之發(fā)生改變，使控制系統(tǒng)具有了一定的智能適應(yīng)性。

2.2 預(yù)瞄前方路徑曲率修正PID控制

前方路徑彎曲程度是決定車(chē)輛轉(zhuǎn)彎方向、轉(zhuǎn)角大小及車(chē)速大小的重要因素[11-12]，由式(6)可知，若前方一段路徑的平均曲率為ρ，為了使搬運(yùn)車(chē)沿著道路中心線行駛，搬運(yùn)車(chē)的轉(zhuǎn)角δ應(yīng)為arcsin(Lρ)，而分段式PID控制自動(dòng)循跡行駛是一種針對(duì)橫向偏差的實(shí)時(shí)控制，缺少對(duì)搬運(yùn)車(chē)前方路徑的預(yù)瞄性控制，這會(huì)導(dǎo)致搬運(yùn)車(chē)在彎曲路徑上自動(dòng)循跡行駛時(shí)左右擺動(dòng)幅度較大。因此，根據(jù)搬運(yùn)車(chē)的運(yùn)動(dòng)參數(shù)選取搬運(yùn)車(chē)前方一段路徑的平均曲率進(jìn)行預(yù)瞄控制，修正PID控制，從而使搬運(yùn)車(chē)能精確地沿著彎曲路徑自動(dòng)循跡行駛，同時(shí)提高轉(zhuǎn)向的平穩(wěn)性。具體方法是在PID控制的基礎(chǔ)上增加一個(gè)修正變量u(ρ)，表達(dá)式如式(8)所示。

(8)

式中：u(ρ)是根據(jù)搬運(yùn)車(chē)前方一段路徑的平均曲率ρ得到的修正量，它正相關(guān)于搬運(yùn)車(chē)的轉(zhuǎn)角δ，關(guān)系可以用式(9)表示。

u(ρ)∝δarcsin(Lρ)。

(9)

搬運(yùn)車(chē)分段PID控制并使用前方一段路徑曲率預(yù)瞄修正的自動(dòng)循跡行駛控制流程如圖5所示。

3 仿真分析

3.1 建立仿真分析模型

根據(jù)上述的田間道路搬運(yùn)車(chē)的實(shí)際結(jié)構(gòu)參數(shù)和建立的轉(zhuǎn)向模型、車(chē)路關(guān)系模型和控制策略，用Simulink建立仿真分析模型(圖6)。

3.2 仿真結(jié)果分析

分別進(jìn)行直線道路和曲線道路的自動(dòng)循跡行駛仿真，驗(yàn)證提出的分段式PID控制并預(yù)瞄前方路徑曲率的自動(dòng)循跡行駛控制策略的可行性。定義較大的初始偏差為27 cm，仿真結(jié)果如圖7、圖8所示。

由圖7、圖8可知，即使在較大的初始偏差情況下，系統(tǒng)能迅速且平穩(wěn)地作出相應(yīng)的調(diào)整，且穩(wěn)定后偏差很小。在直線道路循跡行駛仿真(圖7)中，搬運(yùn)車(chē)迅速平穩(wěn)地向道路中心線靠近，當(dāng)行駛到2.5 m時(shí)，27 cm的初始偏差已調(diào)節(jié)到只有2 cm，之后就處于穩(wěn)定行駛狀態(tài)，穩(wěn)定后最大偏差為 0.4 cm。在曲線循跡行駛仿真(圖8)中，搬運(yùn)車(chē)也能夠迅速平穩(wěn)地向道路中心線靠近，行駛到2.8 m的時(shí)候，由27 cm的初始偏差調(diào)節(jié)到2 cm，隨后搬運(yùn)車(chē)循跡行駛只在路徑曲率比較大的波峰或波谷處出現(xiàn)稍微的偏差，穩(wěn)定后最大偏差為 1.3 cm。

4 實(shí)車(chē)試驗(yàn)驗(yàn)證

采用圖1所示的田間道路搬運(yùn)車(chē)分別進(jìn)行直線和曲線道路的自動(dòng)循跡行駛試驗(yàn)，驗(yàn)證提出的分段式PID控制加預(yù)瞄修正的自動(dòng)循跡行駛控制策略的有效性。試驗(yàn)中使用高精度的全球?qū)Ш叫l(wèi)星系統(tǒng)(global navigation satellite system，簡(jiǎn)稱(chēng)GNSS)采集直線和曲線道路的路徑位置信息并作平滑性處理后儲(chǔ)存起來(lái)作為自動(dòng)循跡行駛的跟蹤線，自動(dòng)循跡行駛過(guò)程中實(shí)時(shí)位置由高精度差分GNSS每0.1 s定位1次得到。

4.1 直線路徑試驗(yàn)

首先，用高精度GNSS定位系統(tǒng)采集處理得到1條直線路徑的位置信息，起點(diǎn)和終點(diǎn)分別為A、B，并以A、B等2點(diǎn)構(gòu)成的直線為x軸，將搬運(yùn)車(chē)的前后軸中心線與x軸對(duì)正，航向角與x軸方向一致；然后啟動(dòng)搬運(yùn)車(chē)，使搬運(yùn)車(chē)以2 m/s的速度自動(dòng)行駛，試驗(yàn)重復(fù)進(jìn)行5次，結(jié)果如表1所示，其中試驗(yàn)中最大1次的路徑跟蹤偏差如圖9所示。

表1直線循跡行駛試驗(yàn)結(jié)果

田間道路搬運(yùn)車(chē)沿直線路徑自動(dòng)循跡行駛時(shí)，剛開(kāi)始由于搬運(yùn)車(chē)沒(méi)有完全擺正、初始偏差值較大等原因使自動(dòng)循跡行駛誤差稍大，隨著行駛距離增大誤差越來(lái)越?。欢矣捎贕NSS定位誤差、轉(zhuǎn)向系統(tǒng)機(jī)械誤差等原因使偏差值始終圍繞0 cm附近小幅度的波動(dòng)。

結(jié)果表明，搬運(yùn)車(chē)以2 m/s的速度自動(dòng)循跡行駛時(shí)路徑跟蹤最大偏差為5.7 cm，平均偏差為2.3 cm，說(shuō)明所設(shè)計(jì)的自動(dòng)循跡行駛控制策略具有良好的控制性能。

4.2 曲線路徑試驗(yàn)

同樣先用高精度GNSS定位系統(tǒng)采集處理得到1條S形弧線路徑的位置信息。采用5種不同車(chē)速共進(jìn)行了5次曲線跟蹤試驗(yàn)，試驗(yàn)結(jié)果如表2所示。其中，最大1次跟蹤偏差的行駛路徑如圖10所示。

表2S型曲線循行駛跡試驗(yàn)結(jié)果

試驗(yàn)結(jié)果表明，搬運(yùn)車(chē)以2 m/s的速度自動(dòng)循跡行駛時(shí)路徑跟蹤的橫向偏差最大，最大的偏差值僅為25.3 cm，平均偏差為15.1 cm，逐漸降低搬運(yùn)車(chē)行駛速度，控制精度也得到相應(yīng)提高，路徑跟蹤的橫向偏差逐漸變小，滿足田間道路搬運(yùn)車(chē)自動(dòng)循跡行駛的要求。

5 結(jié)論

針對(duì)田間道路無(wú)人駕駛搬運(yùn)車(chē)的自動(dòng)循跡行駛控制問(wèn)題，提出1種分段式PID控制同時(shí)預(yù)瞄前方路徑曲率進(jìn)行PID控制修正的控制策略。分段PID的控制方法能智能適應(yīng)搬運(yùn)車(chē)不同的橫向偏差，提高了搬運(yùn)車(chē)自動(dòng)循跡行駛的控制精度。預(yù)瞄修正控制能使搬運(yùn)車(chē)根據(jù)前方路徑提前作出相應(yīng)的轉(zhuǎn)彎動(dòng)作，提高了自動(dòng)行駛搬運(yùn)車(chē)轉(zhuǎn)向的平穩(wěn)性。

Simulink仿真分析結(jié)果表明，提出的分段PID加預(yù)瞄修正控制策略有很好的控制效果，無(wú)論是直線道路還是曲線道路，控制系統(tǒng)都可以對(duì)偏差作出迅速、平穩(wěn)的響應(yīng)，且系統(tǒng)穩(wěn)定后偏差很小。

試驗(yàn)結(jié)果表明，在分段PID加預(yù)瞄修正控制策略下，搬運(yùn)車(chē)直線自動(dòng)循跡行駛平均偏差在2.3 cm以?xún)?nèi)，曲線自動(dòng)循跡行駛平均偏差在15.1 cm以?xún)?nèi)，滿足無(wú)人駕駛搬運(yùn)車(chē)在田間道路上自動(dòng)循跡行駛的要求。

參考文獻(xiàn)：

[1]馮啟高，毛罕平. 我國(guó)農(nóng)業(yè)機(jī)械化發(fā)展現(xiàn)狀及對(duì)策[J]. 農(nóng)機(jī)化研究，2010，32(2)：245-248.

[2]趙 映，肖宏儒，梅 松，等. 我國(guó)果園機(jī)械化生產(chǎn)現(xiàn)狀與發(fā)展策略[J]. 中國(guó)農(nóng)業(yè)大學(xué)學(xué)報(bào)，2017，22(6)：116-127.

[3]張聞?dòng)?，丁幼春，王雪玲，? 基于SVR逆向模型的拖拉機(jī)導(dǎo)航純追蹤控制方法[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2016，47(1)：29-36.

[4]熊中剛，葉振環(huán)，賀 娟，等. 基于免疫模糊PID的小型農(nóng)業(yè)機(jī)械路徑智能跟蹤控制[J]. 機(jī)器人，2015(2)：212-223.

[5]韓科立，朱忠祥，毛恩榮，等. 基于最優(yōu)控制的導(dǎo)航拖拉機(jī)速度與航向聯(lián)合控制方法[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2013，44(2)：165-170.

[6]He X，Cui T，Zhang D，et al. Development of an electric-driven control system for a precision planter based on a closed-loop PID algorithm[J]. Computers and Electronics in Agriculture，2017，136：184-192.

[7]姜 勇. 基于雙變量PID控制算法的地下智能鏟運(yùn)機(jī)自主導(dǎo)航技術(shù)研究[J]. 機(jī)械制造，2016，54(5)：19-22.

[8]沈法鵬，趙又群，趙洪光，等. 非線性輪胎側(cè)向力對(duì)汽車(chē)轉(zhuǎn)向穩(wěn)定性的影響[J]. 中國(guó)機(jī)械工程，2015，26(1)：135-139.

[9]馬 雷，貢士嬌，李 斌，等. 四輪獨(dú)立轉(zhuǎn)向電動(dòng)汽車(chē)轉(zhuǎn)向控制方法[J]. 汽車(chē)工程，2015，37(9)：1029-1034.

[10]熊鈺杉，徐 群. PID和Fuzzy控制相結(jié)合的分段復(fù)合控制[J]. 計(jì)算機(jī)仿真，2006，23(6)：296-298.

[11]陳煥明，郭孔輝. 基于航向角和位置偏差控制的駕駛員模型[J]. 農(nóng)業(yè)機(jī)械學(xué)報(bào)，2013，44(10)：36-40.

[12]管 欣，王景武，高振海. 基于最優(yōu)預(yù)瞄加速度決策的汽車(chē)自適應(yīng)巡航控制系統(tǒng)[J]. 吉林大學(xué)學(xué)報(bào)(工學(xué)版)，2004，34(2)：189-193.