奚小波，史揚杰，單 翔，張 琦，金亦富，龔俊杰，張劍峰，張瑞宏,2※

（1.揚州大學(xué)機械工程學(xué)院/揚州大學(xué)江蘇省現(xiàn)代農(nóng)機農(nóng)藝融合技術(shù)工程中心，揚州225127；2.南京沃楊機械科技有限公司，南京211200）

0 引 言

農(nóng)機自動駕駛技術(shù)是精準(zhǔn)農(nóng)業(yè)實施的關(guān)鍵與基礎(chǔ)[1-3]，目前農(nóng)機自動駕駛儀已得到大規(guī)模推廣使用，尤其在新疆、黑龍江、內(nèi)蒙等地，農(nóng)機自動駕駛已廣泛應(yīng)用于耕作、播種、施肥、噴藥、收獲等農(nóng)業(yè)生產(chǎn)過程[4-7]?，F(xiàn)階段的農(nóng)機自動駕駛主要以直線駕駛為主，這主要受制于拖拉機動力換擋技術(shù)，目前中國一拖集團已成功開發(fā)了動力換擋拖拉機，使得農(nóng)機無人自動駕駛成為新的研究熱點[8-9]。

農(nóng)機在田間作業(yè)時可能遇到電線桿、電塔樁、樹木等障礙物，需要農(nóng)機能自動繞過障礙物并迅速回到原有作業(yè)路線，因此自動避障是農(nóng)機無人駕駛的必要技術(shù)條件。對于農(nóng)機自動駕駛過程中的主動避障，一般采取局部路徑規(guī)劃方法，農(nóng)機在行駛過程中利用傳感器獲取自身狀態(tài)信息及周圍環(huán)境信息，系統(tǒng)監(jiān)測行駛路徑上存在障礙物時，將在短時內(nèi)提前規(guī)劃避障路徑，并控制農(nóng)機沿規(guī)劃路徑避障[10-11]。因此，避障路徑的規(guī)劃與控制將直接關(guān)系農(nóng)機能否安全避障、避障精度及避障后直線行駛精度。

現(xiàn)有避障路徑規(guī)劃多采用最短切線法，其具有路徑生成快速簡單、軌跡短等優(yōu)點[12-13]，但其路徑曲線的曲率不連續(xù)且存在折角拐點，易使拖拉機前輪轉(zhuǎn)角驟變，影響農(nóng)機避障行駛精度。Bezier 曲線是一種連續(xù)的平滑曲線，廣泛應(yīng)用于移動機器人軌跡規(guī)劃、機構(gòu)運動規(guī)劃、結(jié)構(gòu)造型等領(lǐng)域，具有曲率連續(xù)、控制簡單等特點[14-18]。本文以最短切線法為基礎(chǔ)，利用Bezier曲線優(yōu)化避障路徑，通過Matlab 仿真研究路徑跟蹤控制的可行性，并以犁耕作業(yè)為例，采用配有動力換擋系統(tǒng)及農(nóng)機自動駕駛儀的東方紅LF954-C型拖拉機研究系統(tǒng)的避障行駛精度。

1 農(nóng)機運動避障路徑規(guī)劃

1.1 最短切線農(nóng)機避障路徑

在設(shè)計農(nóng)機避障路徑前，首先要確認(rèn)農(nóng)機前方障礙物特征圓大小及農(nóng)機與障礙物的距離。為使避障路徑容易控制，采用最短切線法規(guī)劃農(nóng)機避障路徑，如圖1 所示，理論避障路徑由圓弧段AB、直線段BC、圓弧段CE、直線段EF 和圓弧段FG 組成，其中相鄰的圓弧段與直線段相切。根據(jù)實際農(nóng)機作業(yè)情況，一般障礙物（電線桿、樹木等）輪廓半徑小于農(nóng)機最小轉(zhuǎn)彎半徑，因此障礙物特征圓半徑R=rmin+0.5l。該避障路徑是基于最短切線建立的，通過農(nóng)機運動幾何關(guān)系確定各拐點坐標(biāo)，從而完成避障路徑規(guī)劃，具有快速簡單等優(yōu)點。雖然這種路徑在位置和方向上是連續(xù)的，但路徑曲線的曲率不連續(xù)，致使農(nóng)機運動角速度不連續(xù)，農(nóng)機在由圓弧段轉(zhuǎn)直線段（或直線段轉(zhuǎn)圓弧段）時其前輪轉(zhuǎn)角驟變且變幅較大，難以實現(xiàn)快速調(diào)節(jié)，從而無法保證農(nóng)機沿既定路徑行駛，影響農(nóng)機避障的行駛精度。

圖1 最短切線避障軌跡圖Fig.1 Trace of shortest tangent obstacle avoidance path

1.2 Bezier曲線避障路徑優(yōu)化

給定空間n+1 個控制點Pi（i=0,1,···,n），則Bezier 曲線方程可表示為[19-20]：

式中t 為位置參數(shù)，Bi,n(t)是n 次Bernstein 基函數(shù)。對Bezier曲線方程求導(dǎo)得：

將t=0與t=1帶入公式（2），則有：

式（3）說明，Bezier 曲線在起點和終點處的切線方向與控制多邊形第一條邊和最后一條邊的走向一致。因此，通過規(guī)劃Bezier曲線起點和終點的切線方向，即可實現(xiàn)對農(nóng)機初始位姿和目標(biāo)位姿的確定，有利于農(nóng)機避障完成后繼續(xù)沿初始姿態(tài)行駛。

本文采用Bezier曲線對最短切線規(guī)劃路徑作進一步優(yōu)化，以期形成曲率連續(xù)的光滑路徑。依據(jù)最短切線避障路徑可知，農(nóng)機在避障過程中有A、B、C、D、E、F、G 共計7 個控制點，理論上可采用六階Bezier 曲線，為降低Bezier 曲線復(fù)雜性，且基于原避障路徑的對稱性，采用2段三階Bezier 曲線，即A、B、C、D 為前半段，D、E、F、G 為后半段。同時采用2 段三階Bezier 曲線替代六階Bezier曲線，可有效避免因Bezier 曲線凸包性[21]造成的避障曲線與實際控制點的相對偏差過大造成避障任務(wù)失敗。三階Bezier曲線為三次多項式，其矩陣為[22]：

則有

式中x0、x1、x2、x3分別為P0、P1、P2、P3的橫坐標(biāo)，cm；y0、y1、y2、y3分別為P0、P1、P2、P3的縱坐標(biāo)，cm。

以東方紅LF954-C 拖拉機為例，農(nóng)具以250 cm 幅寬犁為例（與下文試驗一致），根據(jù)該型拖拉機技術(shù)參數(shù)得出控制點的相對坐標(biāo)（單位：cm）：A（333.28, 0）、B（501.47,56.22）、C（694.94,201.43）、D（938.50,282.85）、E（1182.06,201.43）、F（1375.52,56.22）、G（1543.71,0）。在Matlab中對上述坐標(biāo)進行2段Bezier曲線創(chuàng)建，如圖2所示，可以看出，該路徑存在顯著拐點，如點A、D、G，且起始點A與目標(biāo)點G的走向并不一致。造成上述現(xiàn)象的原因是，控制點分布的離散性較小，致使擬合成的Bezier曲線過于線性，為此需要作進一步優(yōu)化調(diào)整。

圖2 最短切線避障路徑控制點生成的Bezier曲線Fig.2 Bezier curve generated by control points on shortest tangent obstacle avoidance path

為增大控制點的離散性且不改變Bezier 曲線的走勢，將控制點B、C、E、F 分別向曲線兩側(cè)拉伸。為保證整段曲線起點與終點的走向一致，需使前段曲線終點與后段曲線起點的走向一致，優(yōu)化調(diào)整后控制點的相對坐標(biāo)（單位：cm）：A（333.28, 0）、B（1635.89, 0）、C（1635.89,282.85）、D（938.50, 282.85）、E（11241.105, 282.85）、F1（1241.105,0）、G（1543.71,0），新生成的Bezier 曲線如圖3 所示。通過上述方法可提高整段曲線對稱性，其每段曲線呈中心對稱，整段曲線呈軸對稱。

一般參數(shù)方程曲線的曲率c為[23]

對式（5）進一步求導(dǎo)并將各點坐標(biāo)值代入式（6），求得前段曲線AD 的終點曲率與后端曲線DG 的起點曲率一致，說明上述整段避障路徑AG 的曲率連續(xù)，農(nóng)機沿著該曲線行走容易控制。

圖3 優(yōu)化控制點生成的Bezier曲線Fig.3 Bezier curve generated by optimized control points

2 農(nóng)機運動控制方法

2.1 農(nóng)機運動模型

本文研究的避障控制方法主要針對輪式拖拉機，鑒于農(nóng)機作業(yè)的低速性，將四輪拖拉機簡化成二輪車模型進行運動學(xué)分析[24]，如圖4 所示。規(guī)定當(dāng)農(nóng)機控制點N沿曲線順時針運動時的曲率c 為負(fù)，逆時針運動時c 為正；當(dāng)控制點N 在曲線外側(cè)時的橫向位置誤差d 為正，在曲線內(nèi)側(cè)時d為負(fù)。

為簡化農(nóng)機運動模型建立過程，需作如下假設(shè)[25]：一、農(nóng)機與農(nóng)機具是一個獨立剛體，即農(nóng)機運動時只有一個瞬時旋轉(zhuǎn)中心；二、農(nóng)機運動為純滾動而非滑動，即農(nóng)機的運動速度方向沿著車身中心線方向。一般農(nóng)機具與農(nóng)機的連接為三點懸掛連接，該連接方式基本不會滑移竄動，符合剛性連接條件。同時，農(nóng)機作業(yè)工況相對平坦，符合滾動運動條件?；谏鲜黾僭O(shè)，則有[26-27]：

式中c(s)為M點處的路徑曲率。有2種情況使方程（7）不成立，一是當(dāng)d=1/c(s)時，即農(nóng)機與路徑間的位置誤差等于路徑曲線的半徑，依據(jù)農(nóng)機自動駕駛儀的相關(guān)標(biāo)準(zhǔn)規(guī)定，導(dǎo)航作業(yè)精度為±2.5 cm，說明農(nóng)機與路徑間的位置誤差為厘米級，而拖拉機的轉(zhuǎn)彎半徑一般為米級，因此d=1/c(s)的種情況不會發(fā)生；二是θe為直角的倍數(shù)，即農(nóng)機行駛方向與路徑方向垂直，這顯然不符實際。因此，方程（7）的農(nóng)機運動模型成立。

2.2 農(nóng)機運動控制

通過上述方法建立的農(nóng)機運動模型是一種高度的非線性模型，為了應(yīng)用線性系統(tǒng)控制方法，采用鏈?zhǔn)娇刂评碚搶⒎蔷€性模型轉(zhuǎn)化成近似線性模型，通用鏈?zhǔn)较到y(tǒng)一般為三狀態(tài)兩輸出系統(tǒng)，其表達式如下[28]：

設(shè)Q=(a1,a2,a3)T表示系統(tǒng)狀態(tài)變量，U=(m1,m2)T表示系統(tǒng)控制變量，為使方程（8）線性化，只需用狀態(tài)變量a1的導(dǎo)數(shù)替換時間導(dǎo)數(shù)，令：

則方程（9）可以表達成：

顯然，方程（10）為線性方程。令a1=s，a2=d，則變量m1、m2與a3可表達成：

因為方程（10）是線性系統(tǒng)，可采用狀態(tài)反饋控制方法對系統(tǒng)進行控制[29]，令：

式中控制參數(shù)(K1,K2)∈R+。將方程（12）帶入方程（10）中得控制律：

上述控制律是為了控制a2和a3趨近于0，同樣可用來控制d 和θe趨近于0，從而達到農(nóng)機路徑曲線的跟蹤控制。聯(lián)立方程（13）與（11）可以得到實際的農(nóng)機前輪轉(zhuǎn)角控制律δ(d,θe)為：

由于農(nóng)機運動模型的精確度直接影響路徑跟蹤的控制質(zhì)量，且隨著路徑曲率的增大，運動模型的精確度也會下降，控制性能將會降低。本文采用鏈?zhǔn)娇刂破鞯玫嚼碚撉拜嗈D(zhuǎn)向角δ(d,θe)后，利用PI控制器解算出期望轉(zhuǎn)向補償角δc，將δ(d,θe)與δc相加后得到農(nóng)機實際轉(zhuǎn)向角δt，并輸出給農(nóng)機模型，控制農(nóng)機沿路徑行走。

傳統(tǒng)PI控制器的控制律[30]：

式中，u(k)為PI 控制的輸出，Kp為比例增益，Ki為積分增益，e(i)為i 時間對應(yīng)誤差輸入，k 為總采樣時間點數(shù)。計算時，將農(nóng)機航向誤差角θe作為PI 的誤差輸入，則當(dāng)前期望轉(zhuǎn)向補償角δc即為u(k)。

3 仿真分析

采用Matlab 對農(nóng)機運動的航向誤差角、橫向位置誤差及前輪轉(zhuǎn)向角情況進行了仿真分析。農(nóng)機選用東方紅LF954-C，軸距231.4 cm，前輪轉(zhuǎn)角范圍（-π/6,π/6），最小轉(zhuǎn)彎半徑5.6 m，配套農(nóng)機具幅寬250 cm。依據(jù)Bezier曲線優(yōu)后的路徑建立軌跡約束模型（圖3），按照仿真設(shè)計描述在Matlab 軟件編程實現(xiàn)預(yù)設(shè)軌跡并建立路徑跟蹤仿真模型，設(shè)定農(nóng)機起始位置坐標(biāo)為（333.28, 0）（單位：cm），初始前輪轉(zhuǎn)角為0，農(nóng)機運動速度1 m/s，選取理論前輪轉(zhuǎn)向角控制參數(shù)K1=0.09，K2=0.6，選取期望轉(zhuǎn)向補償角控制參數(shù)Kp=2，Ki=0.01。

圖5 為農(nóng)機避障運動仿真結(jié)果。由圖5a 可知，農(nóng)機的跟蹤軌跡與預(yù)設(shè)軌跡基本重合，說明本控制方法具有較高的路徑跟蹤精度。由圖5c 可以看出，農(nóng)機行駛的航向誤差角在-0.06～0.06 rad 范圍內(nèi)，其數(shù)值較大點主要分布于路徑軌跡曲線斜率較大位置處，這是由于此處采樣點比較稀疏，導(dǎo)致M 點相對較少，但在實際運行時采樣點比較密集。從圖5b 可以看出，農(nóng)機行駛的橫向位置誤差在13 cm 內(nèi)，能符合農(nóng)機曲線避障行駛要求。從圖5d 可以看出，前輪轉(zhuǎn)向角δ 變化平緩，總體上由0 逐漸增大，后又減小至0，然后變?yōu)樨?fù)值繼續(xù)增大（δ 負(fù)值表示農(nóng)機沿順時針轉(zhuǎn)彎行駛、δ 正值表示農(nóng)機沿逆時針轉(zhuǎn)彎行駛），呈周期變化，與農(nóng)機實際轉(zhuǎn)彎過程相符，整個運動曲線中過程δ 沒有顯著突變，這有利于控制器對期望角度的跟蹤。上述結(jié)果顯示，通過本控制方法進行的農(nóng)機避障路徑跟蹤控制，其控制精度高，農(nóng)機基本按照預(yù)設(shè)軌跡行駛。

圖5 農(nóng)機避障運動仿真結(jié)果Fig.5 Simulation results of tractor obstacle avoidance movement

4 避障精度試驗

4.1 試驗過程

拖拉機選用東方紅LF954-C 型，農(nóng)具選用250 cm 幅寬犁。拖拉機安裝北斗農(nóng)機自動駕駛儀（南京沃楊KM-502，直線行駛精度±2.5 cm），并通過超聲波雷達、圖像識別技術(shù)實現(xiàn)作業(yè)環(huán)境的障礙物感知。該型拖拉機配有動力換擋裝置，可進行自動啟停、換擋、農(nóng)具升降操作。試驗在無錫卡爾曼導(dǎo)航技術(shù)公司的導(dǎo)航試驗基地進行，場地平坦，設(shè)置草堆障礙物，其輪廓半徑約3 m（小于拖拉機最小轉(zhuǎn)彎半徑），障礙物特征圓半徑R=rmin+0.5l=6.85 m。拖拉機一檔行進（約3.6 km/h），犁耕深度25 cm，整機工作時，通過超聲波雷達與視覺機器識別障礙物信息，通過拖拉機位置傳感器計算避障安全距離，并規(guī)劃避障路徑軌跡，拖拉機在北斗農(nóng)機自動駕駛儀的控制下按照預(yù)設(shè)軌跡路徑完成自動避障過程。

對機具避障精度進行了試驗，在拖拉機車身中心線上安裝高精度衛(wèi)星接收機，即RTK 測量儀，測量精度0.01 cm。由于RTK 測量儀系統(tǒng)內(nèi)難以設(shè)置Bezier曲線，設(shè)置測量基準(zhǔn)線為直線A'B'，通過衛(wèi)星接收機記錄的RTK 位置數(shù)據(jù)xi（監(jiān)測點至A'B'的距離）減去理論避障路徑至A'B'的距離，得到監(jiān)測點與理論避障曲線的誤差值hi（取正），如圖6所示。

圖6 避障精度測試示意圖Fig.6 Schematic diagram of obstacle avoidance accuracy test

通過公式（16）計算得出跟蹤軌跡與理論避障曲線的標(biāo)準(zhǔn)差，該標(biāo)準(zhǔn)差即為避障精度，標(biāo)準(zhǔn)差越大，表示該避障跟蹤精度越差[31]。

4.2 試驗結(jié)果與分析

避障精度測試分2 個部分，第一部分為避障曲線部分（Bezier 曲線起點至終點），第二部分為避障后繼續(xù)沿直線行駛部分。每監(jiān)測點相距100 cm，由于障礙物特征圓半徑為6.85 m，因此前13 個監(jiān)測點為避障曲線位置點，直線行駛部分亦取13點，共計26個監(jiān)測點，結(jié)果見表1。

表1 監(jiān)測點到理論避障曲線的距離Table1 Distance from monitoring point to theoretical obstacle avoidance curve

根據(jù)表1 數(shù)據(jù)，通過公式（16）求得曲線部分的監(jiān)測點到理論避障曲線的誤差平均值為13.63cm，標(biāo)準(zhǔn)差為5.21cm，曲線起始部分的誤差較大，最大誤差達21.53 cm，主要是因為犁耕作業(yè)時拖拉機前進方向的左側(cè)輪胎置于耕地中行駛，拖拉機處于一邊傾斜狀態(tài)，避障時拖拉機則又駛?cè)肫降?，行駛工況的變化導(dǎo)致避障精度的穩(wěn)定性有波動，同樣從平地進入耕地時，直線行駛精度的穩(wěn)定性也會有所波動。上述結(jié)果顯示，曲線部分的避障精度為5.21 cm，說明本文設(shè)計的避障曲線及控制方法具有較高的避障精度，曲線路徑的跟蹤控制效果較好，在不平整的犁耕地中表現(xiàn)出較好的魯棒性和適應(yīng)性，可滿足拖拉機作業(yè)的避障要求。同時，避障后農(nóng)機繼續(xù)沿直線行駛的監(jiān)測點到理論軌跡的誤差平均值為4.83 cm，標(biāo)準(zhǔn)差為1.98 cm，即避障后農(nóng)機繼續(xù)沿直線行駛的精度為1.98 cm，說明本文設(shè)計的路徑跟蹤控制方法可保證農(nóng)機在避障后恢復(fù)直線自動駕駛。

本文設(shè)計的避障路徑規(guī)劃與控制方法也可適用于拖拉機田間調(diào)頭作業(yè)，可為農(nóng)機全程無人化自動駕駛提供技術(shù)依據(jù)。

5 結(jié)論

1）本文通過2 段三階Bezier 曲線優(yōu)化了農(nóng)機避障路徑，優(yōu)化后的曲線曲率連續(xù)，避障路徑平滑無拐點，利于農(nóng)機行走控制。針對農(nóng)機運動模型精度低的問題，通過鏈?zhǔn)娇刂评碚摻⑥r(nóng)機運動的線性控制模型，利用PI 控制器進行轉(zhuǎn)角補償，提高農(nóng)機避障行駛精度。

2）仿真結(jié)果顯示，農(nóng)機行駛的航向誤差角在-0.06～0.06 rad范圍內(nèi)，橫向位置誤差小于13 cm，前輪轉(zhuǎn)向角變化平緩，沒有顯著突變，說明該避障路徑控制方法的控制精度高，農(nóng)機能按預(yù)設(shè)軌跡行駛。

3）犁耕作業(yè)試驗結(jié)果顯示：Bezier 曲線部分的避障精度為5.21 cm，曲線路徑的跟蹤控制效果較好；避障后農(nóng)機繼續(xù)沿直線行駛的精度為1.98 cm，說明該路徑跟蹤控制方法可保證農(nóng)機在避障后恢復(fù)直線自動駕駛。上述結(jié)果表明，該避障路徑及控制方法在不平整的犁耕地中表現(xiàn)出較好的魯棒性和適應(yīng)性，可滿足拖拉機作業(yè)的避障要求。