基于Bezier曲線優化的農機自動駕駛避障控制方法

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

（1.揚州大學機械工程學院/揚州大學江蘇省現代農機農藝融合技術工程中心，揚州225127；2.南京沃楊機械科技有限公司，南京211200）

0 引 言

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

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

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

1 農機運動避障路徑規劃

1.1 最短切線農機避障路徑

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

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

1.2 Bezier曲線避障路徑優化

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

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

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

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

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

則有

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

以東方紅LF954-C 拖拉機為例，農具以250 cm 幅寬犁為例（與下文試驗一致），根據該型拖拉機技術參數得出控制點的相對坐標（單位：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中對上述坐標進行2段Bezier曲線創建，如圖2所示，可以看出，該路徑存在顯著拐點，如點A、D、G，且起始點A與目標點G的走向并不一致。造成上述現象的原因是，控制點分布的離散性較小，致使擬合成的Bezier曲線過于線性，為此需要作進一步優化調整。

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

為增大控制點的離散性且不改變Bezier 曲線的走勢，將控制點B、C、E、F 分別向曲線兩側拉伸。為保證整段曲線起點與終點的走向一致，需使前段曲線終點與后段曲線起點的走向一致，優化調整后控制點的相對坐標（單位：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 所示。通過上述方法可提高整段曲線對稱性，其每段曲線呈中心對稱，整段曲線呈軸對稱。

一般參數方程曲線的曲率c為[23]

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

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

2 農機運動控制方法

2.1 農機運動模型

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

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

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

2.2 農機運動控制

通過上述方法建立的農機運動模型是一種高度的非線性模型，為了應用線性系統控制方法，采用鏈式控制理論將非線性模型轉化成近似線性模型，通用鏈式系統一般為三狀態兩輸出系統，其表達式如下[28]：

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

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

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

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

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

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

由于農機運動模型的精確度直接影響路徑跟蹤的控制質量，且隨著路徑曲率的增大，運動模型的精確度也會下降，控制性能將會降低。本文采用鏈式控制器得到理論前輪轉向角δ(d,θe)后，利用PI控制器解算出期望轉向補償角δc，將δ(d,θe)與δc相加后得到農機實際轉向角δt，并輸出給農機模型，控制農機沿路徑行走。

傳統PI控制器的控制律[30]：

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

3 仿真分析

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

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

圖5 農機避障運動仿真結果Fig.5 Simulation results of tractor obstacle avoidance movement

4 避障精度試驗

4.1 試驗過程

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

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

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

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

4.2 試驗結果與分析

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

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

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

本文設計的避障路徑規劃與控制方法也可適用于拖拉機田間調頭作業，可為農機全程無人化自動駕駛提供技術依據。

5 結論

1）本文通過2 段三階Bezier 曲線優化了農機避障路徑，優化后的曲線曲率連續，避障路徑平滑無拐點，利于農機行走控制。針對農機運動模型精度低的問題，通過鏈式控制理論建立農機運動的線性控制模型，利用PI 控制器進行轉角補償，提高農機避障行駛精度。

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

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