無人駕駛拖拉機田間路徑規(guī)劃方法研究

張炳力, 程嘯宇, 程 進, 李傲伽, 鄭平平

(1.合肥工業(yè)大學 汽車與交通工程學院,安徽 合肥 230009;2.安徽省智能汽車工程實驗室,安徽 合肥 230009)

隨著精準農業(yè)實施,無人駕駛拖拉機已成為研究熱點,它的應用可解決農村勞動力不足等問題[1-2],而其規(guī)劃路徑的合理高效性將影響田間作業(yè)的精度和質量。根據環(huán)境信息掌握的情況,無人駕駛拖拉機路徑規(guī)劃可分為全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃是在農田環(huán)境信息已知的情況下提前離線規(guī)劃出全局路徑,以實現(xiàn)對地塊全覆蓋,減少對地塊不必要碾壓,尋求最優(yōu)解;局部路徑規(guī)劃是在環(huán)境信息未知的情況下,由感知傳感器實時獲取行駛路徑周圍障礙物信息進行在線路徑規(guī)劃,注重路徑安全性與實時性[3-4]。

現(xiàn)階段國內外關于拖拉機路徑規(guī)劃的研究較少。對于全局路徑規(guī)劃:文獻[5]對地塊最佳作業(yè)角展開研究,提出了“哈密頓路徑搜索”和“幾何推理”2種路徑軌跡構建法;文獻[6]將地塊分割為若干單元,再利用遺傳算法尋找最優(yōu)覆蓋路徑;文獻[7]以鐵牛TN654為實驗平臺,聯(lián)合田間作業(yè)方式和拖拉機自動導航系統(tǒng)的特點,對耕地和播種這2種作業(yè)軌跡進行設計。對于局部路徑規(guī)劃:文獻[8-9]根據障礙物大小與農機作業(yè)寬度的關系劃分障礙物,針對含大障礙物的地塊,多通過“自上而下法”、“細胞分割法”等對地塊進行分割和合并,提出一種折線避障措施避開小型靜態(tài)障礙物;文獻[10]提出最短切線避障路徑構建方法,針對有靜態(tài)障礙物的情況可快速規(guī)劃出避障路徑。目前,國內外對于全局路徑規(guī)劃的研究主要是針對規(guī)則地塊在不同評價指標和不同作業(yè)方式下的路徑規(guī)劃,對不規(guī)則地塊的全局路徑規(guī)劃研究較少,且缺乏一定的通用性。而現(xiàn)有局部路徑規(guī)劃多采用最短切線法和折線法[11-12],沒有考慮農機作業(yè)寬度和最小轉彎半徑的存在,且避障策略過于簡單，只考慮單個靜態(tài)障礙物避障情況,沒有對障礙物存在的不同情況進行系統(tǒng)性分析研究。

本文在現(xiàn)有研究的基礎上,提出一種面向不規(guī)則地塊全區(qū)域覆蓋通用路徑規(guī)劃方法,分析不同障礙物對作業(yè)路徑的影響并進行系統(tǒng)性的劃分整理,針對障礙物不同情況且考慮到農機固有參數限制而規(guī)劃出相應的局部路徑。最后,本文合理選擇切換條件,將離線全局路徑與實時局部路徑相結合形成完整的田間作業(yè)路徑,并設計開發(fā)無人駕駛拖拉機路徑規(guī)劃系統(tǒng)。

1 全局路徑規(guī)劃算法

1.1 地塊幾何建模

農田地塊是拖拉機主要的工作對象,考慮到實際地塊環(huán)境復雜性,為便于驗證算法的正確性,使用 GPS/BDS等導航系統(tǒng)實地測量獲取農田地塊頂點位置信息，并以E120°為中央子午線進行高斯投影正算，以獲得農田頂點的平面坐標[13]。將地塊抽象為2D幾何多邊形后放入平面直角坐標系第一象限中分析,把路徑規(guī)劃問題轉換為幾何數學問題。若建模時有地塊邊界垂直于坐標系x軸,應將幾何模型旋轉一定角度直至無邊界垂直于該軸。設n邊形地塊頂點坐標為(xi,yi)(i=1,2,…,n),其邊的大小和方向以向量形式表示。

全局路徑需滿足連續(xù)行駛作業(yè)、轉彎軌跡應大于拖拉機的最小轉彎半徑r等條件,因此本文選用梭式法作為拖拉機田間行駛方式,如圖1所示。該轉彎方式由農機具作業(yè)寬度w和r之間的關系確定[14]:w<2r,選用梨形轉彎;w=2r,選用半圓形轉彎;w>2r,選用弓形轉彎。因半圓形轉彎可視為弓形轉彎特例,本文僅考慮弓形和梨形轉彎2種轉彎方式。

圖1 梭式法全覆蓋示意圖

1.2 幾何地塊處理

1.2.1 最佳作業(yè)方向求取

全局路徑規(guī)劃重在提高作業(yè)效率,使無效路徑上的消耗達到最小,因此本節(jié)將對最佳作業(yè)角ω進行求取,得到最優(yōu)全局路徑。4種評價指標中[15],選取地頭消耗最少作為ω求取的指標,即

Ci=TiNi,

(1)

其中:αi為作業(yè)直線與第i條邊界夾角;Li為第i條邊界的長度;Ti為第i條邊界上單個轉彎消耗時間;Ni第i條邊界上轉彎次數;Ci為第i條邊界上的轉彎消耗;C為地頭消耗。

設ω∈[0,180°],當αi≠0時,C為連續(xù)函數,以作業(yè)角θ為變量,θ=0.5°為起始角度,以0.5°為間隔分別計算該角度下的C值,通過選取C最小值,確定ω取值。需注意當αi=0時,在該地塊邊界無轉彎，則C=0。同時,地頭消耗求取不考慮整體地塊的覆蓋,只以各邊界為一單位求取對應地頭消耗值進行匯總。

1.2.2 幾何地塊分割

地塊建模后,被分為凸形、凹形、復雜多邊形地塊。由于拖拉機沿ω行駛作業(yè)時對某些凹形地塊不能一次性遍歷,需判斷凹形地塊是否分割。設過凹點Pi(xi,yi)以ω為傾角做其直線方程y-kωx-b=0,且設與凹點相鄰的2個頂點坐標分別為Pi-1(xi-1,yi-1)、Pi+1(xi+1,yi+1)。當(yi-1-kωxi-1-b)(yi+1-kωxi+1-b)>0時,Pi-1、Pi+1兩點分布于該直線同側,需沿最佳作業(yè)方向角對凹點進行分割,形成新子地塊;反之,兩點分布于直線兩側,地塊無需分割,如圖2所示。

圖2 凹點是否需要分割示意圖

地塊內含障礙物的長或寬大于等于3w時,如存在大溝渠、水坑等,對其采點建模,其在模型中顯示為復雜多邊形中的“島嶼”或“空洞”。由于大障礙物區(qū)域對全局路徑規(guī)劃影響過大,必會增加轉彎區(qū)域,因此這些轉彎區(qū)域可被用于子地塊之間的路徑銜接。以ω為最優(yōu)分割方向選用“掃線法”對地塊進行最優(yōu)分割,形成若干子地塊。需注意,對復雜多邊形地塊的ω求取應包含地塊中大障礙物邊界處的地頭消耗,以尋求全局最優(yōu)。當分割出若干子地塊時,對各子地塊逐個進行路徑規(guī)劃,但子地塊的遍歷次序也將干擾整體路徑作業(yè)效率,故本文采用“自上而下、由右到左”的優(yōu)化次序銜接各子地塊,保證整地塊的最優(yōu)全局路徑,以復雜多邊形地塊為例,如圖3所示。

圖3 大障礙物地塊分割圖

1.3 全局作業(yè)路徑構建

全局路徑構建應對3種地塊模型分類構建,但后2種地塊都可被分割為凸形地塊,且不分割凹形地塊的路徑構建方法與凸形地塊一致,因此本文以凸形地塊為例,采用幾何推理法構建全局路徑。

1.3.1 作業(yè)區(qū)域構建

圖4 內縮多邊形求解

1.3.2 直線路徑構建

為使拖拉機在不規(guī)則地塊作業(yè)時可一次性遍歷全地塊,且行走方式簡單,本文采用“自上而下”的方法進行直線路徑構建。

以多邊形地塊內縮區(qū)域A′B′C′D′為例,如圖5所示。

圖5 直線路徑構建示意圖

以ω為傾角,過A′點所對應的直線L1的截距為最大截距bmax,過C′點所對應的直線L2的截距為最小截距bmin,則A′為上方端點,C′為下方端點,連接線段A′C′,A′C′將A′B′C′D′ 分為左、右兩側。A′C′的下方為地塊的左側,上方為地塊的右側。L1以不同w在y軸上投影值為平移量依次向下平移且不超過L2,依次連接左右兩點并均分形成完整的直線路徑。

自上而下第n行作業(yè)行直線方程為:

(2)

其中,bw為w在y軸上投影。

1.3.3 轉彎路徑構建

拖拉機在地頭區(qū)域內無論采取何種轉彎方式均可被分為4種情形:βi>90°,從地塊右側邊界左轉或右轉;βi<90°,從地塊左側邊界左轉或右轉。以梨形轉彎βi>90°右側轉彎為例進行軌跡構建,如圖6所示。

圖6 βi>90°右側梨形轉彎

(3)

(4)

(5)

(6)

其中:θv為直線P1P4所對應的傾角。

由各圓弧起始角度可知,聯(lián)合各圓心坐標可以求得各圓弧軌跡點,然后按照遍歷次序連接各點即可得到完整的轉彎路徑,同理可求得其他3種情況的轉彎路徑。

在直線路徑的基礎上,按照左右交錯、自上而下依次排序的原則將求得的多個轉彎路徑添加到地頭區(qū)域,形成完整的全局路徑。所規(guī)劃路徑的起點為最上方的直線路徑線段與作業(yè)區(qū)域右側邊界交點,終點則由作業(yè)行數的奇偶性決定:若為奇數,最后一條直線路徑線段與作業(yè)區(qū)域右側邊界交點為終點;反之則左側交點。注意,若起點、終點處的轉彎軌跡超出內縮邊界,應給予刪除。

2 局部路徑規(guī)劃算法

2.1 障礙風險評估

拖拉機沿著全局作業(yè)路徑在地塊內行駛時,其作業(yè)區(qū)域由遠及近可被劃分為:障礙物觀測區(qū)、沖突避讓區(qū)、緊急避障區(qū)。周圍作業(yè)環(huán)境未知,在障礙物觀測區(qū)內利用感知傳感器(激光雷達)不斷采集期望路徑周邊靜態(tài)障礙物信息。由于激光雷達原始數據是以球坐標系的形式儲存,為方便解析處理數據,將激光點球坐標轉化為笛卡爾坐標。激光雷達是安裝在拖拉機機體上,需將激光坐標系通過平移矩陣和旋轉矩陣轉換為拖拉機隨車坐標系,且隨車坐標系是固結于車身上,故需將隨車坐標系轉化到大地坐標系完成對障礙物絕對坐標系的獲取且將其輪廓擬合為最小外接圓形,獲得障礙物最小外接圓圓心坐標(x0,y0)和半徑R。當靜態(tài)障礙物半徑小于3w,即為小障礙物。

當沖突避讓區(qū)內有靜態(tài)障礙物時,需對障礙物進行風險評估。設當前所在直線路徑方程為y=kω+b,可求得障礙物圓心與作業(yè)路徑所在直線的距離d。當R+w/2≤d時為無威脅障礙物,不需要采取任何避障策略,仍沿原作業(yè)路徑行駛;反之為有威脅障礙物,若拖拉機仍沿原路徑行駛,所攜帶的農機具將會與障礙物發(fā)生碰撞,故需進行局部路徑規(guī)劃。

2.2 單障礙物局部路徑構建

單障礙物局部作業(yè)路徑搭建原則是考慮參數限制，使得拖拉機順利轉彎且實現(xiàn)避障后回到原路徑上。

(1) 因為拖拉機存在r,且存在w,所以需設定障礙物安全圓區(qū)域,保證拖拉機能安全轉彎避讓障礙物。障礙物最小外接圓半徑為R,兩者圓心重合。當R≥r-w/2時,其半徑RO=R;反之,RO=r-w/2。

(2) 判斷安全圓圓心與路徑所在直線方程的關系。若kωx+b≥y0,則從上方避過;反之,則從下方避過。

(3) 設拖拉機的前3 s行駛軌跡點為Pi(xi,yi),當前行駛軌跡點Pi+1(xi+1,yi+1)。局部路徑規(guī)劃時可被分為4種情況:當xixi+1,下行上避障或下避障。

2.3 多障礙物局部路徑構建

當拖拉機行駛路徑上有多威脅性小障礙物時,沿行駛方向定義為Z1,Z2,…,Zn。首先,不考慮其他障礙物存在,構建各小障礙物避障路徑,設各避障路徑為Si(i=1,2,…,n),如圖7所示。

圖7 單/多障礙物避障軌跡示意圖

(1)DiAi+1與直線路徑線段夾角為0°或長度為0時,拖拉機可從2個障礙物之間安全通過,因此,沿路徑Si行駛避過Zi,令i=i+1,對Zi+1采取局部路徑規(guī)劃,直至到檢測范圍內的Zn。

3 路徑規(guī)劃系統(tǒng)軟件設計及測試

3.1 路徑規(guī)劃系統(tǒng)軟件開發(fā)

無人駕駛拖拉機路徑規(guī)劃方法需將全局路徑規(guī)劃方法和局部路徑規(guī)劃方法相結合,使得離線規(guī)劃的全局路徑與實時規(guī)劃的局部路徑相結合形成完整田間作業(yè)路徑,從而保證路徑規(guī)劃的全局最優(yōu)性和局部實時性,具體結合流程如圖8所示。

3.2 路徑規(guī)劃系統(tǒng)軟件測試

基于上述算法,本文使用LabVIEW2016作為開發(fā)工具,該系統(tǒng)軟件通過BDS采集方式獲得試驗地塊電子地圖,可在此軟件中規(guī)劃出最優(yōu)作業(yè)方式的路徑規(guī)劃圖,用戶可以通過一個友好的界面來完成地塊搭建、最佳作業(yè)角選取、自主路徑規(guī)劃等功能。支持各種標準化數據格式的作業(yè)路徑輸出,界面如圖9所示。

圖9 路徑規(guī)劃系統(tǒng)軟件前面板

為了檢驗路徑規(guī)劃算法和系統(tǒng)軟件的有效性,設r=5 m,令w為6、12 m,選取地塊頂點坐標為(108,257)、(288,205)、(327,85)、(147,17)、(76,18)、(10,85)、(1,180),然后進行全局路徑規(guī)劃。

手動輸入期望路徑前、后軌跡點和各靜態(tài)小障礙物坐標Z1(104,227)、Z2(92,199)、Z3(132,185)、Z4(159,196)、Z5(195,171)、Z6(99,99)、Z7(122,109)、Z8(240,140)、Z9(246,145)、Z10(188,80)、Z11(201,84)、Z12(186,52),對應R為3、3、4、3、6、5、5、4、3、3、3、1 m,模擬實時局部路徑規(guī)劃,其結果如圖10所示。

圖10 系統(tǒng)軟件測試結果1

由圖10的全局路徑規(guī)劃結果可知:w=6 m時,選用梨形轉彎,其ω=20.5°;w=12 m時,選用弓形轉彎,其ω=20.5°,相較于梨形轉彎,由于w增加導致地頭消耗顯著減少。在2種轉彎方式下,沿該方向對地塊進行路徑規(guī)劃,S為起點,E為終點,其規(guī)劃的路徑末端作業(yè)行與地塊距離均大于w/2,起點、終點和轉彎方式均符合作業(yè)要求,且都能滿足對地塊的全覆蓋要求,符合本文設計要求。

由圖10的局部路徑規(guī)劃結果可知,因w不同,障礙物風險評估的結果、相鄰障礙物的評判標準不同。w=6 m,Z1、Z2、Z3、Z4對多條作業(yè)路徑都具備威脅性,上下都需進行避障;Z10、Z11相鄰但不滿足合并條件,因而各視為單一障礙物進行避障路徑規(guī)劃。w=12 m,Z1、Z2、Z3、Z4僅對單一作業(yè)路徑具備威脅性,只需進行上/下避障;Z10、Z11相鄰但避障圓有交點,拖拉機不能從兩障礙物之間安全經過,需合成一新障礙物進行避障路徑規(guī)劃,合成的新障礙物圓內切于Z10、Z11兩障礙物圓且圓心坐標為(194,82),R=10 m。由于Z8、Z9兩障礙物圓相鄰過近,無論w數值為多少都需合成一新障礙物進行避障路徑規(guī)劃,其圓心坐標為(243,142),R=7 m,該圓內切于Z8、Z9兩障礙物圓。

綜上所述,本文所提出的局部路徑規(guī)劃可針對單障礙物、多相鄰障礙物進行實時局部路徑規(guī)劃避障,且避障后仍回到原期望路徑,符合本文設計要求。

對其余地塊以及相應轉彎工況類型進行系統(tǒng)測試。

設地塊頂點坐標為(43,247)、(184,236)、(236,369)、(391,359)、(347,163)、(18,13),令w=7 m,r=3 m,結果如圖11所示。

仍選取上述凸形地塊,內設一大障礙物,其頂點坐標為(139,161)、(200,130)、(170,80)、(120,120),w=5 m,r=2.5 m,結果如圖12所示。

圖12 系統(tǒng)軟件測試結果3

由圖11可知,初始地塊ω=24.5°,由于在該方向作業(yè)時不能一次性遍歷地塊,需沿凹點進行分割,子地塊① 的頂點坐標為(43,247)、(184,236)、(384,328)、(347,163)、(18,13),子地塊② 的頂點坐標為(236,369)、(391,25)、(384,328)、(184,236),對子地塊① 、子地塊② 重新進行全局路徑規(guī)劃,兩子地塊ω均為24.5°,在該方向下對兩子地塊所規(guī)劃的路徑均符合本文設計要求。

由圖12可知,原始地塊ω=20.5°,地塊建模時為復雜多邊形,需沿此角度對地塊進行掃線分割,子地塊① 的頂點坐標為(108,257)、(270,209)、(7,109)、(1,180),子地塊② 的頂點坐標為(270,211)、(288,205)、(170,80)、(200,130)、(139,161),子地塊③ 的頂點坐標為(139,161)、(120,120)、(170,80)、(57,38)、(10,85)、(7,109),子地塊④ 的頂點坐標為(312,134)、(327,85)、(147,17)、(76,18)、(57,38),對子地塊逐個進行全局路徑規(guī)劃。子地塊① 、子地塊③ 的ω均為21.5°,子地塊② 、子地塊④ 的ω均為20.5°,在各自最佳作業(yè)角下對子地塊規(guī)劃的路徑均符合本文設計要求。

4 路徑規(guī)劃實車試驗

為驗證該系統(tǒng)實用性,本文依托課題組設計的無人駕駛拖拉機實車試驗平臺進行實車試驗,r=5 m,實車平臺如圖13所示。地塊頂點坐標信息可由組合導航測定,其由北斗星通UB842高精定位板卡和驛唐DTU共同組成。采用固定支架將激光雷達安裝在拖拉機前配重上,并通過網線與LabVIEW連接,可實時檢測獲得障礙物雷達坐標,經過雷達坐標與拖拉機隨車坐標轉換及拖拉機隨車坐標與大地坐標轉換獲得障礙物絕對坐標信息。

試驗場地為安徽泗州拖拉機制造有限公司的實車試驗場,該場地由多層瀝青結構面鋪裝而成,試驗車試驗時的擋位為低快3擋,油門開度40%,車輛始終保持10 km/h直線行駛速度,因場地原因未攜帶農機具,虛設w=5 m,當拖拉機轉彎時應自行減速,轉彎成功后自行加速回至原速度。

圖13 無人駕駛拖拉機實車試驗平臺

打開LabVIEW軟件里的“自主路徑規(guī)劃”選項卡進行作業(yè)路徑規(guī)劃:① 采集路徑規(guī)劃的坐標原點,為了避免路徑點出現(xiàn)負值,坐標原點需設置在所規(guī)劃的作業(yè)區(qū)域的東南方;② 從地塊的右上角開始,順時針采集地塊的邊界點的坐標信息;③ 設定 “作業(yè)速度”、“轉彎速度”、“作業(yè)幅寬”和“轉彎半徑”等參數,系統(tǒng)會自主規(guī)劃出期望作業(yè)路徑,并將作業(yè)路徑信息輸入給“循跡系統(tǒng)”,進行路徑跟蹤。

在試驗場地中,采集出一不規(guī)則地塊頂點坐標信息,經處理后的各頂點坐標值為(2 148,2 186)、(2 208,2 258)、(2 249,2 239)、(2 155,2 130)、(2 146,2 131),隨后進行全局路徑規(guī)劃。

由于場地過于平坦,人為設置多個相鄰小障礙物,如圖14a所示。當拖拉機沿該全局路徑行駛至障礙物區(qū)域進行實時局部路徑規(guī)劃,完整路徑規(guī)劃實驗結果如圖14b所示。

由圖14b可知,所規(guī)劃的全局路徑可實現(xiàn)對地塊的全覆蓋。當無人駕駛拖拉機沿期望路徑自主行駛至小障礙物區(qū)域時,進行實時局部路徑規(guī)劃,規(guī)劃的局部路徑可實現(xiàn)對障礙物避障,且避障后仍回到原期望路徑上,滿足本文設計要求。直線路徑跟蹤偏差穩(wěn)定在0.07 m上下波動,轉彎時,由于目標路徑曲率突變,跟蹤偏差會迅速增加,但最大不超過0.23 m,且可迅速被修正。因此本文設計的路徑具有較大的實用價值,可以作為無人駕駛拖拉機的參考作業(yè)路徑,具有一定的現(xiàn)實意義。

圖14 路徑規(guī)劃實車試驗

5 結 論

(1) 本文綜合農田幾何形狀和作業(yè)機具參數等因素設計全局路徑規(guī)劃方法,并在全局路徑的基礎上針對單個及多個相鄰小型靜態(tài)障礙物實時搭建局部路徑;根據切換條件,將離線規(guī)劃全局路徑與實時規(guī)劃局部路徑相結合，形成完整的田間作業(yè)路徑,保證作業(yè)路徑的全局最優(yōu)性和局部實時性。

(2) 基于所述路徑規(guī)劃算法,設計無人駕駛拖拉機路徑規(guī)劃系統(tǒng),設定參數自主規(guī)劃,結果表明對于不同形狀地塊在不同轉彎工況下均可以規(guī)劃出全局路徑,并可根據模擬障礙物信息實時規(guī)劃出局部路徑。為驗證算法的實用性,基于某型拖拉機搭建實車試驗平臺,根據作業(yè)情況設計試驗工況,結果證明本文所設計的算法有較大的實用價值。