自主導(dǎo)航農(nóng)業(yè)機(jī)械避障路徑規(guī)劃

劉宇峰，姬長(zhǎng)英，田光兆，顧寶興，魏建勝，陳 凱

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

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

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

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

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

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

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

1) 以一矩形示意表達(dá)拖拉機(jī)模型，以矩形中心O為原點(diǎn)建立平面坐標(biāo)系，x軸正方向?yàn)樗较蛴?，y軸正方向與拖拉機(jī)航向重合。

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

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

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

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

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

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

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

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

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

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

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

2) 根據(jù)拖拉機(jī)避障過(guò)程中臨界碰撞條件“拖拉機(jī)右邊界運(yùn)動(dòng)軌跡與圓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è)車(chē)輛實(shí)際轉(zhuǎn)向半徑為r(r≤rmax)，則實(shí)際轉(zhuǎn)向中心X2為 (?r，0)。以X2為圓心，r為半徑作圓X2，如圖3所示：

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

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

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

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

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

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

1.1.5 避障代價(jià)的計(jì)算

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.1 試驗(yàn)設(shè)計(jì)

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

圖5 試驗(yàn)平臺(tái)和試驗(yàn)場(chǎng)地Fig. 5 Experiment platform and field

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

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

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

2.2 結(jié)果與分析

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

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

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

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

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

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

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

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

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

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

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

圖10 不同障礙物條件下以不同行駛速度跟蹤不同算法避障路徑時(shí)行駛路徑與規(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算法相比大幅減少，障礙物在不同位置下減幅達(dá)32%、26%。

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

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

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