劉宇峰，姬長英，田光兆，顧寶興，魏建勝，陳 凱

(1 南京農(nóng)業(yè)大學 工學院，江蘇 南京 210031； 2 東南大學 儀器科學與工程學院，江蘇 南京 210096)

科學技術(shù)的進步正推動現(xiàn)代農(nóng)業(yè)生產(chǎn)向自動化、規(guī)?；?、精確化方向快速發(fā)展，具備自主導航功能的農(nóng)業(yè)機械越來越多地應用到農(nóng)業(yè)生產(chǎn)中[1]。自主導航拖拉機作為牽引機械能夠高效完成田間播種、施肥和耕地[2-7]，自主導航聯(lián)合收割機能夠在無人干預的情況下收獲小麥、水稻和玉米[8-13]。當農(nóng)機工作環(huán)境中出現(xiàn)少量障礙物時，障礙物可能會阻礙農(nóng)機正常工作，因此規(guī)劃一條建立在農(nóng)機工作平臺上的避障路徑、幫助農(nóng)機在存在障礙物的環(huán)境中正常完成作業(yè)任務必不可少。

現(xiàn)階段國內(nèi)外關(guān)于農(nóng)機平臺避障路徑規(guī)劃的研究較少，國內(nèi)研究更多建立在無障礙物環(huán)境中，劉剛等[14]提出一種基于GNSS的農(nóng)田平整自動導航路徑規(guī)劃方法，實現(xiàn)拖拉機的高效平地作業(yè)；孟志軍等[15]提出一種面向農(nóng)田作業(yè)機械的地塊全區(qū)域覆蓋路徑優(yōu)化方法，以生成轉(zhuǎn)彎數(shù)最少、作業(yè)消耗最小、總作業(yè)路徑最短或有效作業(yè)路徑比最大的最優(yōu)作業(yè)路徑。國外學者針對有障礙物環(huán)境展開了相應的研究，Ta?x等[16]針對農(nóng)田中小型靜態(tài)障礙物提出一種折線避障方法，雖能實現(xiàn)避障功能，但避障代價過大，且未充分考慮農(nóng)機的運動特性；圓弧避障方法是現(xiàn)階段應用較為廣泛的一種避障路徑規(guī)劃方法，但避障策略較為簡單，當障礙物間距離較近時會出現(xiàn)算法失效的情況。現(xiàn)階段成熟的路徑規(guī)劃算法如A-star算法[17-18]、SA算法[19]和人工勢場法[20-21]等大多運用在農(nóng)業(yè)機器人和AGV小車[22-26]等小型精準控制平臺上，這些算法傾向于將控制物體等效于一個運動點，但運用在農(nóng)機平臺(如東方紅SG250拖拉機)上時，將農(nóng)機等效為單個運動點會丟失大量的農(nóng)機自身運動模型信息。算法提供的控制策略也未充分考慮應用在農(nóng)機平臺的可行性，使得現(xiàn)階段傳統(tǒng)路徑規(guī)劃算法或其改進版無法適用于農(nóng)機工作平臺。

為了自主導航農(nóng)業(yè)機械能夠順利避障，建立在農(nóng)機自身運動模型以及農(nóng)機工作環(huán)境基礎(chǔ)上的避障路徑規(guī)劃必不可少。規(guī)劃的路徑指導自主導航農(nóng)業(yè)機械在工作過程中以盡量小的避障代價實現(xiàn)障礙物的避障，并在避障完成后合理地回歸原始工作路徑，因此，自主導航農(nóng)業(yè)機械避障路徑規(guī)劃的研究具有很強的實際意義。

1 避障路徑規(guī)劃算法

1.1 單障礙物避障路徑規(guī)劃算法

1.1.1 判斷障礙物的威脅性 確定障礙物位置后，需判斷障礙物是否會對拖拉機正常工作產(chǎn)生影響，具體工作流程如下：

1) 以一矩形示意表達拖拉機模型，以矩形中心O為原點建立平面坐標系，x軸正方向為水平向右，y軸正方向與拖拉機航向重合。

2) 以障礙物幾何中心Z(XZ，YZ)為圓點，Z到障礙物邊界的最長距離RZ為半徑作圓，構(gòu)建防碰撞區(qū)域。

3) 計算Z到y(tǒng)軸距離d，設(shè)d和RZ的差值為c，比較c與半車寬L/2的大小。若c＞L/2，為無威脅障礙物；反之為有威脅障礙物。

4) 當障礙物為無威脅障礙物時，拖拉機可沿原有導航路線繼續(xù)自主導航；當障礙物為有威脅障礙物時，拖拉機需根據(jù)障礙物位置以及自身運動模型更新避障路徑。如圖1所示，障礙物Z為有威脅障礙物。

1.1.2 避障安全距離的計算 若障礙物為有威脅障礙物，需計算避障安全距離(D)，如果在自主導航過程中拖拉機與障礙物的距離小于D，即使以最大偏轉(zhuǎn)角避障，拖拉機仍然會與障礙物碰撞。圖1中，當拖拉機采用最大偏轉(zhuǎn)角避障時，拖拉機轉(zhuǎn)彎半徑為最小轉(zhuǎn)彎半徑。具體的計算方法如下：

1) 設(shè)拖拉機最小轉(zhuǎn)彎半徑為rL，則拖拉機轉(zhuǎn)向中心X1位置為(?rL，0)，連接X1與矩形右上角點F(XF，YF)，以X1為圓心，X1到F距離為半徑作圓，則圓X1為拖拉機右側(cè)邊界角點行駛軌跡圖。

2) 拖拉機向前行駛時，圓X1在坐標軸上會隨之沿y軸方向平移，若平移后的圓X1′與防碰撞區(qū)域圓Z相切，設(shè)此時的圓心坐標X1′為 (?rL，y)，則變量y滿足方程：

解得y值，計算YZ與y的差值D，D為避障安全距離。

3) 設(shè)拖拉機當前所在位置yO與YZ的差值為dOZ，若dOZ小于D，則拖拉機無法成功避障，應立即停止工作，防止與障礙物產(chǎn)生碰撞。

4) 由步驟2)確定拖拉機可向前行駛的安全距離區(qū)間為[0，YZ?D]，為使拖拉機盡可能地跟蹤初始路徑，盡量貼近障礙物后再執(zhí)行避障動作，可將該區(qū)間分成若干段 [0，d1]、(d1，d2]、(d2，d3]…(dn?1，dn]，拖拉機在行進一段安全距離dn(n=1, 2, 3, 4…)后再進行避障路徑規(guī)劃并計算避障總代價，篩選出一條代價最小的避障路徑作為最終的避障路徑，n的大小根據(jù)實際避障時具體D值決定。

1.1.3 避障最大轉(zhuǎn)彎半徑的確定

1) 如圖2所示，當拖拉機向前行駛由1.1.2確定的一段避障安全距離后，便可以計算拖拉機的避障代價，但在此之前需根據(jù)障礙物的位置先確定拖拉機避障最大轉(zhuǎn)彎半徑，設(shè)拖拉機能夠避障的最大轉(zhuǎn)彎半徑為rmax，則可確定拖拉機的初始轉(zhuǎn)向中心X1的位置為 (?rmax，0)。

2) 根據(jù)拖拉機避障過程中臨界碰撞條件“拖拉機右邊界運動軌跡與圓Z相切”，列出方程：

3) 解方程，得避障最大轉(zhuǎn)彎半徑rmax。

圖2 確定避障最大轉(zhuǎn)彎半徑Fig. 2 Determination of the maximum turning radius of obstacle avoidance

1.1.4 4 段路徑規(guī)劃

1) 設(shè)車輛實際轉(zhuǎn)向半徑為r(r≤rmax)，則實際轉(zhuǎn)向中心X2為 (?r，0)。以X2為圓心，r為半徑作圓X2，如圖3所示：

圖3 單/雙障礙物避障算法Fig. 3 Single obstacle/double obstacles avoidance algorithm

2) 連接圓心X2與Z點，與圓X2相交于O1點，則為第1段避障路徑。

3) 以Z為圓心，以ZO1距離為半徑作圓，與直線y=YZ交于O2點，則為第2段避障路徑。

4) 作O1點關(guān)于直線y=YZ的對稱點O3，則為第3段避障路徑。

5) 作X2關(guān)于直線y=YZ的對稱點X3，以X3為圓心，X3到O3點的距離為半徑作圓X3，與y軸相切于O4點，則為第4段避障路徑。

考慮到自主導航拖拉機在偏轉(zhuǎn)角控制上存在一定的滯后與誤差，因此在避障路徑的規(guī)劃過程中應盡量減少偏轉(zhuǎn)角變化的次數(shù)。4段圓弧避障路徑僅在O1與O3點要求拖拉機偏轉(zhuǎn)角發(fā)生改變，以幫助拖拉機實現(xiàn)最基本的避障與回歸原始路徑功能。若是3段圓弧路徑則只能實現(xiàn)避障功能，拖拉機在完成避障后無法回歸原始航線。若是5段圓弧路徑，則每段圓弧長度被壓縮得相對較短，且會發(fā)生3次偏轉(zhuǎn)角變化，實際操作時會放大拖拉機偏轉(zhuǎn)角控制存在誤差與滯后的問題，使得拖拉機難以跟蹤規(guī)劃路徑。因此，對于拖拉機控制平臺4段圓弧路徑規(guī)劃更穩(wěn)定，實際操作時更易實現(xiàn)。

1.1.5 避障代價的計算

1) 以4段避障路徑總長度S為避障代價，S由構(gòu)成。

2) 設(shè)O1X2與x軸的夾角為θ，則θ的大小為：

由圖2、3可以得出，外加陰極保護電壓能夠適當?shù)匾种其摬牡膽Ωg。材料——介質(zhì)體系在自腐蝕電壓的電位下陰極保護程度最大，試樣的斷裂壽命最長，斷面收縮率最大，體系的應力腐蝕敏感性最小，最不容易發(fā)生應力腐蝕。

則S關(guān)于轉(zhuǎn)向半徑r的函數(shù)為:

4) 在r∈[rL，rmax]區(qū)間內(nèi)，計算可得S最小時的r取值。其中rL為車輛最小轉(zhuǎn)彎半徑，可從車輛說明書中獲得，rmax為1.1.3確定的車輛避障最大轉(zhuǎn)彎半徑。

1.1.6 避障總行駛路徑的計算 自主導航拖拉機執(zhí)行避障動作時，需確定避障起始點，由1.1.2可確定允許拖拉機向前行駛的距離為d1、d2、d3、d4…dn，以1.1.4、1.1.5方法計算相對應的n個避障代價S1、S2、S3…Sn，最終拖拉機完成避障動作所需的總路徑長度為Cn=Sn+dn，當Cn為最小時，設(shè)此時規(guī)劃的避障路徑為最優(yōu)避障路徑，此時確定的拖拉機轉(zhuǎn)彎半徑r為最優(yōu)轉(zhuǎn)彎半徑。

1.2 雙/多障礙物避障路徑規(guī)劃算法

單障礙物避障算法適用于路徑上出現(xiàn)單一障礙物的情況，當拖拉機自主導航行駛路徑上出現(xiàn)2個障礙物時，可使用雙障礙物避障算法予以解決。

1.2.1 障礙物Z避障路徑規(guī)劃 首先根據(jù)障礙物Z的位置，由單障礙物避障算法進行初始避障路徑規(guī)劃。

1.2.2 構(gòu)建安全行駛區(qū)域 如圖3所示，完成路徑規(guī)劃后，構(gòu)建障礙物Z的安全行駛區(qū)域，該部分覆蓋拖拉機避障經(jīng)過的所有區(qū)域，在圖中由A、A1…A6，B、B1…B6以及拖拉機兩側(cè)邊界延長線構(gòu)成。

1.2.3 判斷障礙物是否存在威脅 首先為障礙物Z1構(gòu)建防碰撞安全區(qū)域，并判斷是否會與1.2.2建立的安全行駛區(qū)域產(chǎn)生交集。若產(chǎn)生交集，說明Z1存在威脅；若無交集，則Z1無威脅，原有路徑無需做出修改，拖拉機可正常行駛通過。

1.2.4 判斷障礙物遠近屬性 若障礙物Z1存在威脅，則需對障礙物遠近屬性進行判斷。如圖3所示，當拖拉機沿著O1…O4完成障礙物Z的避障后回歸原始航線，中心O點隨之運動到O4，以O(shè)4點為原點重新建立避障坐標系，拖拉機可利用單障礙物避障算法沿著O5、O6、O73個點再次實現(xiàn)Z1的避障，此時稱Z1為遠障礙物。當拖拉機完成Z的避障后回歸原始航線，若Z1和拖拉機距離已小于避障安全距離D，拖拉機無法完成Z1的避障，此時稱障礙物Z1為近障礙物，如圖3所示，Z1′為近障礙物，Z1為遠障礙物。

1.2.5 近障礙物避障 若障礙物Z1為近障礙物，則要求拖拉機在障礙物Z的避障過程中，利用單障礙物避障算法對Z1進行實時避障路徑規(guī)劃，幫助拖拉機完成Z1的避障。

在Z避障過程中，根據(jù)Z1位置，單障礙物避障算法會采取左、右避障2種不同的避障策略：

1) 左避障策略。如圖4所示，若拖拉機在障礙物Z的避障過程中行進到O2點時，障礙物Z1處在拖拉機航向右側(cè)，此時應采取左避障策略，沿O2…O5完成避障后，拖拉機距離初始路徑較遠，此時拖拉機應盡快回歸初始路徑，為保持拖拉機運動連貫性，以Z1為圓心，Z1O5為半徑作圓X4。作圓X5與Y軸、圓X4相切于O6、O72個點，圓X5半徑與圓X3半徑相同，圓X3提供Z1左避障時的第1段避障圓弧。當拖拉機沿O2…O5完成Z1避障后，可繼續(xù)跟蹤由O5、O6、O7組成的2段圓弧回歸初始路徑。

圖4 左避障與右避障策略Fig. 4 Strategies of left and right obstacle avoidance

2) 右避障策略。如圖4所示，若拖拉機在障礙物Z的避障過程中行駛經(jīng)過O2點而繼續(xù)行進到O2′點時，Z1處在拖拉機航向左側(cè)，此時應采取右避障策略，完成避障后，與1)存在相同的問題，同樣以圓X2半徑作圓X6，圓X6與圓X4、Y軸相切于O10、O112個點，圓X2提供Z1右避障時的第1段避障圓弧，當拖拉機沿著O2′、O8、O9完成Z1的避障后，可繼續(xù)跟蹤由O9、O10、O11組成的2段圓弧回歸初始路徑。

1.2.6 多障礙物避障算法 多障礙物避障算法建立在雙障礙物避障算法之上，當拖拉機采用單障礙物避障算法實現(xiàn)第1個障礙物的避障動作后，利用雙障礙物避障算法可實現(xiàn)第2個障礙物的避障。若存在第3個障礙物，則可在拖拉機完成第2個障礙物的避障后再次調(diào)用雙障礙物避障算法。以此類推，實現(xiàn)第n?1個障礙物Zn-1的避障后，可調(diào)用雙障礙物避障算法實現(xiàn)障礙物Zn的避障。

2 避障路徑規(guī)劃試驗及結(jié)果分析

2.1 試驗設(shè)計

以具備調(diào)速和自主導航功能的東方紅SG250拖拉機作為試驗平臺，為使GPS系統(tǒng)正常工作，選擇在光線條件良好的晴天的上午開展試驗。GPS采用載波相位實時差分技術(shù)，定位精度達cm級，將拖拉機跟蹤避障路徑時的GPS定位數(shù)據(jù)作為參考標準，以此驗證算法的可行性，試驗在南京農(nóng)業(yè)大學工學院車庫場地進行，如圖5所示。

圖5 試驗平臺和試驗場地Fig. 5 Experiment platform and field

現(xiàn)階段針對靜態(tài)障礙物避障路徑規(guī)劃的研究較少，文獻[27]提出的避障路徑規(guī)劃算法是現(xiàn)階段使用較為廣泛的方法，與本文所提算法相比，在面對障礙物時均采用4段圓弧避障的形式，具有一定的參考對比價值。下文以L算法指代，利用L算法規(guī)劃避障路徑可通過圖6復現(xiàn)。算法由3段圓弧組成，圓弧半徑均為拖拉機最小轉(zhuǎn)彎半徑，避障路徑由、和3段圓弧組成，L算法未涉及避障安全距離。

圖6 L算法規(guī)劃避障路徑Fig. 6 L algorithm planning obstacle avoidance path

試驗分為單障礙物避障試驗與雙障礙物避障試驗，障礙物為隨機設(shè)置，試驗前由Bumblbee2雙目相機完成障礙物的識別與定位。第1次單障礙物避障試驗障礙物位置為(?0.125，4.820)，第2次單障礙物避障試驗障礙物位置為(?0.125，6.820)，RZ均為0.45。雙障礙物避障試驗障礙物位置分別為 (0.800，2.500)、(?0.200，5.800)，RZ= 0.50。

2.2 結(jié)果與分析

拖拉機分別以 0.3、0.5 m·s?1的恒定速度進行試驗。單障礙物避障試驗結(jié)果如圖7、圖8所示，雙障礙物避障試驗結(jié)果如圖9所示。圖7表示單個障礙物在不同位置，拖拉機以不同速度跟蹤L算法規(guī)劃的路徑時，行駛路徑與規(guī)劃路徑的偏差。圖8表示在與圖7相同條件下，拖拉機以不同速度跟蹤單障礙物避障算法規(guī)劃的路徑時，行駛路徑與規(guī)劃路徑的偏差。試驗中采用0.5 m作為間隔對安全距離區(qū)間進行分段，不同速度下2組單障礙物避障試驗規(guī)劃的第1段路徑均為行駛的安全距離，僅在距離長短上存在差異。當障礙物位置為(?0.125，4.820)時，拖拉機行駛的安全距離為 1.5 m，當障礙物位置為 (?0.125，6.820)時，拖拉機行駛的安全距離為3.5 m。因此最終試驗結(jié)果以行駛dn后的起始避障點作為原點，更新障礙物坐標后，2組試驗障礙物的位置坐標相對拖拉機當前位置均為(?0.125，3.320)。根據(jù)障礙物位置2組試驗采用相同的4段圓弧避障策略，最終不同速度下2組避障試驗中拖拉機行駛路徑與規(guī)劃路徑差異很小(圖8)。圖9表示在雙障礙物條件下拖拉機以不同速度跟蹤雙/多障礙物避障算法規(guī)劃路徑時，行駛路徑與規(guī)劃路徑的偏差。

圖7 單障礙物和不同行駛速度條件下L算法避障試驗Fig. 7 Obstacle avoidance experiments of L algorithm at single obstacle and different driving speeds conditions

圖8 單障礙物和不同行駛速度條件下單障礙物避障算法避障試驗Fig. 8 Obstacle avoidance experiments of single obstacle avoidance algorithm at single obstacle and different driving speeds conditions

圖9 雙障礙物和不同行駛速度條件下雙/多障礙物避障算法避障試驗Fig. 9 Obstacle avoidance experiments of double/multiple obstacles avoidance algorithm at double obstacles and different driving speeds conditions

表1定量分析了在單障礙物和不同行駛速度條件下，單障礙物避障算法與L算法規(guī)劃路徑和行駛路徑的差別。通過行駛路徑累計誤差與方差的對比，反映2個算法在實際應用時穩(wěn)定性與實用性的差別。

表1 單障礙物和不同行駛速度條件下單障礙物避障算法與L算法對比Table 1 Contrast of single obstacle avoidance algorithm and L algorithm at single obstacle and different driving speeds conditoins

當障礙物位于(?0.125，4.820)時，單障礙物避障算法規(guī)劃路徑比L算法減少32%。試驗中當拖拉機行駛速度以 0.3、0.5 m·s?1時跟蹤單障礙物避障算法規(guī)劃路徑時行駛路徑比L算法分別減少35%、38%，行駛路徑累計誤差分別減少53%、66%。

當障礙物位置位于(?0.125，6.820)時，單障礙物避障算法規(guī)劃路徑比L算法減少26%。試驗中當拖拉機行駛速度以 0.3、0.5 m·s?1時跟蹤單障礙物避障算法規(guī)劃路徑時，行駛路徑比L算法分別減少26%、22%，行駛路徑累計誤差分別減少82%、62%。

由表2可知，雙/多障礙物避障算法規(guī)劃路徑為10.29 m，試驗中當拖拉機行駛速度為0.3、0.5 m·s?1時，跟蹤雙/多障礙物避障算法行駛路徑為10.30、10.36 m，與規(guī)劃路徑相比僅分別增加0.09%、0.6%，跟蹤雙/多障礙物避障算法規(guī)劃路徑的行駛路徑累計誤差為9.99、4.13 m。

圖10表示4組試驗中由GPS定位數(shù)據(jù)計算得到的多點誤差值，圖10a表示當障礙物位置為(?0.125，4.820)、(?0.125，6.820)時在單障礙物避障算法下行駛路徑與規(guī)劃路徑的誤差；圖10b表示當障礙物位置為(?0.125，4.820)時在L算法下行駛路徑與規(guī)劃路徑的誤差；圖10c表示當障礙物位置為(?0.125，6.820)時在L算法下行駛路徑與規(guī)劃路徑的誤差；圖10d表示當?shù)缆飞洗嬖?個障礙物，位置為 (?0.200，5.800)、(0.800，2.500)時在雙/多障礙物避障算法下行駛路徑與規(guī)劃路徑的誤差。對應的方差值可參照表1、表2，在相同行駛條件下，跟蹤單障礙物避障算法規(guī)劃路徑獲得的方差值均小于L算法。

表2 雙/多障礙物避障算法試驗數(shù)據(jù)分析Table 2 Data analyses of double/multiple obstacles avoidance algorithm experiments at double obstacles and different driving speeds conditions

圖10 不同障礙物條件下以不同行駛速度跟蹤不同算法避障路徑時行駛路徑與規(guī)劃路徑的誤差Fig. 10 Errors between driving path and theoretical path caused by tracking obstacle avoidance paths of different algrithms at the different obstacle conditions and driving speeds

3 結(jié)論

1)單障礙物避障算法的規(guī)劃路徑與L算法相比大幅減少，障礙物在不同位置下減幅達32%、26%。

2)算法投入實際應用中，由單障礙物避障試驗4組數(shù)據(jù)可得，行駛路徑平均減少30%，行駛路徑累計誤差平均減少65%。

3) L算法在試驗過程中出現(xiàn)了算法起始避障點超過拖拉機當前位置點的情況，導致拖拉機無法完整跟蹤避障路徑。本文算法很好地規(guī)避了這個問題，首先利用避障安全距離盡可能地貼近障礙物，再采用避障策略完成避障動作。若拖拉機當前位置過于貼近障礙物，算法會提示拖拉機停止工作，否則會有與障礙物碰撞的風險。

4)由統(tǒng)計誤差的方差可知，無論拖拉機行駛速度快或慢，跟蹤本文算法避障路徑的穩(wěn)定性更好，基于拖拉機運動模型建立的避障策略更圓滑，很少出現(xiàn)L算法中拖拉機大幅偏離行駛路徑的情況。除此之外，本文算法解決了現(xiàn)階段2個障礙物相隔距離較短時拖拉機無法完成避障的問題。