基于礦卡和挖機之間精準定向需求的雙層軌跡規劃技術研究

熊焱飛,梅貴周,奚小虎,王亞飛,汪貴冬

(1.安徽海博智能科技有限責任公司, 安徽 蕪湖市 241200;2.上海交通大學 機械與動力學院, 上海 200240)

0 引言

根據挖機所在的位置和挖機附近所在山體形狀,露天礦山作業平臺協作場景一般可分為一般場景和復雜場景。一般場景是指在礦車視線范圍內,且挖機周圍無遮擋,挖機周圍無特殊山體形狀環繞,在一般場景中,礦車從平臺主路到挖機裝料點都能很順利地規劃出泊車軌跡,并能很順利地到達挖機鏟下進行裝料作業。而在復雜場景中,挖機在不同位置時,高精地圖雖然能解決此類路徑規劃問題,但是考慮到礦山場景以小時的頻率在變動,而高精地圖的制作流程相當復雜,周期和成本都非常高,滿足不了礦山實時生產需求,因而通過高精地圖的路徑規劃算法不適用于礦山自動駕駛場景。在無高精地圖的情況下,不同的復雜場景都會導致路徑規劃與山體有碰撞或規劃路徑不合理,從而導致裝載功能失效[1]。

針對礦山復雜場景應用的需要以及軌跡規劃功能需求,礦山自動駕駛技術的難點主要概括如下。

(1)作業平臺協作場景的時變性和復雜性。作業平臺協作場景的時變特性使得無人駕駛礦卡需要實時更新全局地圖信息,提升礦卡軌跡規劃的安全性和準確性。

(2)礦卡挖機精準定向需求。裝載作業需要挖機和礦卡的協作,為了滿足挖機工作需求,礦卡不僅需要行駛到裝載點,還需滿足礦卡和挖機的精準定向。

針對上述礦山自動駕駛技術的難點,為了實現復雜場景下的礦卡精準定位,本文提出了一種基于自感知全局地圖、考慮礦卡挖機精準定向需求的雙層軌跡規劃技術,以此滿足復雜礦山作業平臺協作場景的路徑規劃需要,該技術可為其他礦山的自動駕駛提供參考。

1 作業平臺協作場景的路徑規劃方法

路徑規劃框架主要由三部分組成,基于礦車多源感知數據的地圖構建、基于混合A*算法的軌跡規劃關鍵點選取以及面向礦卡挖機精準定向需求的DWA局部規劃,如圖1所示。

圖1 路徑規劃框架

1.1 基于礦車多源感知數據的地圖構建

為了提高鏟裝區泊車規劃算法的成功率,需要在礦車作業前提供鏟裝點附近的柵格地圖。鏟裝區場景具有場景開闊、紋理較少、路面顛簸等特點,在這樣的場景中,僅使用激光雷達傳感器建立點云地圖的方案(即激光SLAM)的數據匹配環節魯棒性差,整體建圖效果不佳。融合車輛組合導航、毫米波雷達等其他傳感器的信息能夠有效提升建圖精度和建圖效果,為軌跡規劃提供車輛的全局感知地圖。

考慮到在特殊路況下,礦車在行駛過程中會發生較大顛簸,導致車輛的控制結果與預想的結果產生不可預料的偏差;另一方面受限于車輛底層的控制精度,礦車在實際控制中無法理想地跟蹤到混合A*軌跡中的每個點。與此同時,由于在大地圖情況下,混合A*軌跡規劃效率低、時間長,并不能滿足礦卡實時軌跡規劃的應用需求。因此,本研究采用了基于混合A*算法的軌跡規劃關鍵點選取和面向礦卡挖機精準定向需求的DWA局部規劃組成的雙層軌跡規劃框架。基于礦車多源感知數據的全局柵格地圖[2]如圖2所示。

圖2 基于礦車軌多源感知數據的全局地圖

1.2 基于混合A*算法的軌跡規劃關鍵點選取

混合A*考慮了車輛動力學模型,使得規劃出的路徑符合車輛動力學約束,且輸出的路徑為一系列帶有坐標和朝向信息的離散點。基于一些準則,從完整的混合A*路徑中選取某些點作為關鍵點,如圖3所示,紅色點為當前狀態選取的關鍵點。將這些點作為局部規劃的目標點,這樣可以有效降低因為控制問題無法到達目標點從而出現原地轉圈的現象,同時提高系統規劃效率,滿足在復雜礦山作業平臺協作場景的應用需要。

圖3 混合A*關鍵點

1.3 面向礦卡挖機精準定向需求的DWA局部規劃

由于挖機位置和朝向隨挖機工作狀態動態變化,礦卡在工作區進入裝料點時需進行動態局部軌跡規劃,為滿足挖機對礦卡的裝載作業要求,礦卡和挖機不僅需要到達礦機作業的裝載點,同時還需要保證礦卡和挖機的精準定向。所謂精準定向,指的是礦卡停在挖機鏟斗的正前方,礦卡的航向和挖機臂展方向的朝向角誤差控制在一定范圍內,這樣才能滿足礦卡裝料需求。面向礦卡挖機精準定向需求的DWA局部規劃在軌跡規劃關鍵點的指導下,實現無人駕駛礦卡的局部軌跡規劃和精準定向。

2 關鍵技術

2.1 基于混合A*算法的軌跡規劃關鍵點選取技術

混合A*考慮了車輛動力學模型,使得規劃出的路徑符合車輛的動力學約束,且輸出的路徑為一系列帶有坐標和朝向信息的離散點,可用于DWA等局部路徑規劃。因此,本方案采用混合A*算法進行全局規劃。考慮到混合A*算法應用的廣泛性,在此不對其進行詳細介紹,重點闡述軌跡規劃關鍵點選取技術。

在關鍵點的選取過程中,需將起點和終點作為關鍵點,剩下的關鍵點則應出現在車輛狀態發生較大變化的位置,例如由直線行駛突然切換為轉彎行駛,以防止車輛因為突然的轉向而無法及時調整,所以在遍歷所有A*的路徑點時會比較其與上一個關鍵點車輛朝向的變化幅度,如果大于設置的閾值就將其作為關鍵點;同時,由于混合A*的路徑中經常出現“人”字形拐彎,這意味著車輛必須由切換檔位改變行駛方向,而在這樣的位置往往是車輛調整位姿的重要時刻,所以本研究會在遍歷A*路徑點的同時檢測每個路徑點速度方向與上一時刻是否發生變化,如若變化,則加入關鍵點序列,如圖4所示,紅色箭頭處表示“人”字形倒車轉折點,因此,可選為本方案的關鍵點[3]。

圖4 普通關鍵點與“人”字形關鍵點

此外,為了保證礦車在最后的倒車階段能更好地與目標朝向保持一致,混合A*規劃出的軌跡終點是實際目標終點前移15 m后的結果,即車輛在到達A*終點后還需倒退一段直線距離才會到達真正的終點,所以在關鍵點選取時需計算出真正的終點位置并將其補入關鍵點序列[4]。

2.2 面向礦卡和挖機精準定向需求的DWA局部規劃技術

基于上述關鍵點選取技術,局部規劃模塊可以得到軌跡規劃的關鍵點,DWA算法(Dynamic window approach)的基本流程包括采樣、預測路徑生成、代價評估與最優路徑選取4個部分。圖5為DWA算法流程。

圖5 DWA算法流程

基于礦車基本參數,系統可以對礦車前輪轉角進行采樣,然后基于勻速運動假設與車輛運動學模型對所有前輪轉角進行預測軌跡生成。通過軌跡的代價評估,能夠得出局部軌跡規劃的最后路徑作為局部軌跡規劃的結果。在無人駕駛礦卡應用中,考慮礦卡挖機精準定向需求,設計合理的代價評估目標函數是本研究局部軌跡規劃技術的難點。

在圖6中,藍線礦卡局部軌跡規劃端點e的期望方向;o是后輪軸心。而oa、ob和oc是不同集合（δf,V f）的預測軌跡。因為朝向的代價較小,基于傳統DWA的目標函數,DWA算法傾向于軌跡oa。如果沒有障礙物,礦車將沿著ae停放。即使礦車可以到達終點,方向也會有很大的偏差。

圖6 傳統DWA的規劃策略

2.2.1 代價函數的設計

考慮無人礦卡的精準定向需求,卡車選擇ob是合理的,可以快速縮短到藍線的距離。然后卡車可以反向到達期望方向。為滿足精準定向需求,本研究在現有代價函數設計基礎上定義了垂直距離代價和期望朝向代價。

2.2.1 .1 垂直距離代價

從預測軌跡的末端到預期線的垂直距離為d l(δf,v r),基于該代價,如果權重比其他代價大,卡車將有移動到預期線路的趨勢。

歸一化d l(δf,v r),垂直距離代價為:

2.2.1 .2 期望的方向代價

為了快速將車輛的方向調節到所需的方向,方向代價定義為:

式中,yaw0是目的地的期望方向;yaw(δf,v r)是預測軌跡中端點的偏航角。

將本方案的代價函數與傳統DWA的代價函數相結合,最終的目標函數可以表示為:

式中,heading(δf,v r)表示朝向代價,即挖機臂展方向的期望朝向和礦車當前位置朝向的差值;dist obs(δf,v r)表示到障礙物處的誤差權值;velocity(δf,v r)表示速度代價;w1、w2、w3、w4是各個代價函數的權重值,可用模糊控制邏輯來計算各個代價函數的權重值。

2.2.2 基于模糊邏輯的代價權值

盡管所提出的方法可以確保礦車以準確的方向到達終點,但權重w1通常應該比其他權重大一個數量級。如果卡車在預期的方向線上,偏航角在相反方向,則車輛將逆向行駛。因此,實時調整代價函數權重使車輛能夠適應不同的工作情況非常重要。基于此,本研究提出了基于模糊邏輯的代價權值,提升代價函數在不同工況下的性能。

2.2.2 .1 隸屬函數

權重w1受車輛位置和偏航角的影響,如當車輛遠離方向線時,w1應該較大,而偏航角和預期方向的絕對值(表示為Δθ)較大,w1變小以調整方向。同時,如果車輛靠近障礙物,則應將w1修改得更小,以便車輛更安全地避開障礙物。圖7為本方案設計的隸屬函數。

圖7 隸屬函數

因此,狀態(d l,Δθ,dist obs)是確定w1的輸入變量。每個輸入變量都與2個模糊集M和N相關。

模糊集M和N的度可以分別用μM和μN表示。μM按式(3)計算,μN=1-μM。在μM和μN中應用四次方,結果對輸入敏感。例如,如果Δθ大于90°,則相關輸出將迅速變小以調節車輛的方向[5]。

式中,k i是關鍵參數,取值范圍[k1,k2,k3],由車輛狀態(d l,Δθ,dist obs)決定。每個輸入產生2個結果μM和μN。

模糊隸屬度值的產生式μRi使用AND邏輯運算符計算。

2.2.2 .2 模糊邏輯

鑒于除w1之外的其他權重都處于相同的大小,將表示為模糊邏輯結構的輸出u。模糊規則見表1,由于系統的對稱性,表1中規則4,5,6的值是規則3,2,1的值的倒數,當Δθ很大而dist obs很小時,w1應該是比其他權重最低的階數,而不管d l的值如何,這有助于規則1和6,如果d l或dist obs較大,則相對λi應該較大,以確保車輛盡快到達所需路線。因此,相對排列為:n1＜n2＜n3＜1。

表1 模糊規則

最重要的因素是Δθ,它決定了車輛的方向,而w4只是基于避障方法使車輛遠離障礙物。表1中的每個規則都對最終輸出u有貢獻,使用每個規則的單個輸出的權重平均值。

最終輸出u的計算公式見式(5):

3 算法仿真驗證

在本研究中,基于混合A*算法的軌跡規劃關鍵點選取和面向礦卡挖機精準定向需求的DWA局部規劃組成的雙層軌跡規劃框架為工作平臺協作場景提供了一整套解決方案。DWA局部規劃算法對于無人駕駛礦卡的功能實現和精準定向具有重要意義,故在此進行算法驗證。

如圖8所示,傳統DWA算法在方向調整上性能很差,而本方案提議的方法使無人駕駛礦卡能夠很好地調節其方向,完成車輛的軌跡規劃和精準定向。

圖8(a)中傳統DWA挖機設定目標朝向為湖藍色箭頭方向,礦車起始方向為紅色箭頭方向,礦車起始方向和挖機設定朝向垂直,實際礦車達到目的地后為綠色箭頭方向,綠色箭頭方向和湖藍色箭頭方兩者的朝向誤差為Δθ=29.5667°,礦車在這個朝向下無法完成裝料任務;圖8(b)保持起始方向不變,基于模糊自適應的DWA仿真結果表明,礦車最終朝向和挖機設定目標朝向誤差較小,Δθ=3.0092°,可以滿足挖機裝料要求;為了防止礦車起始朝向對最終停車時的朝向有影響,將礦車起始朝向設置成和挖機目標朝向相對方向(見圖8(c)),即呈180°,在這種極端苛刻的朝向下泊車,礦車依然能精準地停在挖機正下方,朝向誤差控制在合理范圍內,Δθ=3.0982°。此仿真說明了模糊自適應DWA算法的有效性,滿足礦車泊車裝礦需求[6]。

圖8 不同場景下傳統DWA算法與模糊自適應DWA算法的比較

根據仿真結果,保持相同初始條件,將傳統DWA和模糊自適應DWA做實車測試對比,首先設置挖機的目標朝向和記錄礦車的起始朝向,然后記錄礦車泊車到終點時的朝向。

傳統DWA測試數據見表2。

表2 傳統DWA測試結果

模糊自適應DWA測試數據見表3。

表3 模糊自適應DWA測試結果

通過表2和表3的對比表明,模糊自適應DWA能精準定向,滿足自動駕駛裝礦需求。

4 結論

本文結合礦山實際場景,分析了基于混合A*算法的全局路徑關鍵點選取和面向礦卡挖機精準定向需求的模糊自適應DWA局部路徑規劃技術,實現了無人駕駛礦卡的局部軌跡規劃和精準定向。并通過仿真和實車測試驗證了該算法的有效性,提高了無人礦車裝料的成功率,具有一定的推廣和應用價值。