基于真實環境點云的智能電鏟挖掘軌跡規劃研究

高槐澤， 張天賜， 付 濤， 崔允浩， 孫 偉， 宋學官

（大連理工大學 機械工程學院， 遼寧 大連 116024）

0 引言

大型礦用電鏟中的核心設備，因其環境適應性強、挖掘效率高等原因，被廣泛用于各大礦山開采系統中。但傳統電鏟由于人工作業時振動劇烈、環境惡劣等多因素影響，往往導致整體開采效率低下[1]。

隨著智慧礦山的提出，近年來已得到廣泛關注。目前，電鏟智能化工作主要包括兩個方面： 環境感知與軌跡規劃。 三維點云處理是環境感知的重要環節之一。 目前，國內外學者已提出多種可用來解決點云處理問題的算法。Zucker 最先提出區域增長算法， 經改進后被廣泛用于點云處理中[2]；朱建新等針對挖掘機實際工況，提出一種基于PFH 特征的目標識別方法，通過聚類分離獲得目標點云[3]。由于不同的挖掘軌跡對應了挖掘過程中不同的挖掘阻力、功率等重要指標，因此挖掘軌跡的好壞將直接決定挖掘質量。 孫偉等使用T 型曲線進行電鏟軌跡優化研究[4]；畢秋實使用對數螺旋曲線，結合電鏟結構等參數，利用迭代計算挖掘軌跡[5]。

雖然目前有關環境感知與軌跡規劃的工作已取得一定進展， 然而大多數工作僅關注點云處理或電鏟軌跡規劃，沒有將二者統籌結合。 因此，本文針對智能電鏟在真實環境下的自主挖掘問題，提出了一種基于七段式S 型速度曲線的挖掘軌跡優化方法與一種基于Kd-tree 與K 最鄰近搜索的物料點云數據查找方法來計算挖掘體積。 將單位挖掘體積下的最小能耗作為目標函數建立軌跡優化模型，使用GA 算法對挖掘軌跡及參數進行優化，從而獲得最優挖掘參數。

1 點云配準

1.1 點云預處理

點云處理基于C++的PCL 點云庫進行研究[6]。 加載源點云與目標點云的PCD 文件， 設置點云數據類型為PointXYZ。 去除點云數據中存在的無效點，點云降采樣使用體素柵格法進行數據量縮減，提高計算效率。 點云法向量是基于最小二乘法構建局部擬合平面進行計算。

1.2 點云配準

點云粗配準使用基于FPFH 特征的SAC-IA 算法。FPFH 是一種基于點及其近鄰點間法線夾角、點間連線夾角關系的特征描述子，由PFH 改進而來[7]，計算復雜度降低，在實際應用中更為常見。粗配準計算得到的變換矩陣將作為精配準的初始值。

點云精配準使用ICP 迭代最近點算法，通過尋找源點云與目標點云中的最鄰近點對， 計算出最優匹配參數：旋轉矩陣R 與平移向量t，使得誤差函數最小[8]，誤差函數為：

式中：n—最鄰近點對的個數；pi—源點云P 中的一點，qi—目標點云Q 中與pi對應的最鄰近點。。

2 點云分割

2.1 水平校準及地面點去除

由于礦場地面凹凸不平導致點云數據存在相對于水平面的側傾角與俯仰角誤差， 在地面點去除前須進行水平校準。 在水平面內以垂直于電鏟履帶方向為x 軸，以平行于履帶方向為y 軸，以垂直于水平面方向為z 軸建立坐標系。 坐標轉換矩陣如下所示：

地面點通過設置高度閾值進行去除，若點云數據的z值小于最大高度，則認其為地面點，篩去；若大于或等于最大高度，則認其為非地面點，保留。

2.2 區域生長算法

區域生長算法作為一種點云分割方法， 能夠將點云中具有相同特征的連通區域分割出來，并能夠提供很好的邊界信息和分割結果[6]。 基本原理是在已知點云中按照一定條件選取種子點作為生長起點， 在該點附近尋找相似特征的鄰域點進行比對， 將符合條件的鄰域點放置進種子區域內作為新的種子點繼續向外生長， 直至在該點云搜索不到符合條件的點，即算法停止。

3 軌跡規劃

3.1 設計方法及設計變量確定

由于T 型速度曲線加速度不連續， 在作業中會使機械系統受到柔性沖擊。 因此， 本文選用一個加速度連續的，呈線性的七段式S 型速度曲線。 挖掘軌跡采用兩個不同方向的速度曲線來描述， 分別為沿x 軸方向上的Vx（t）與沿y 軸方向上的Vy（t）：

式中i=1～6；T4=T-（T1+T2+T3+T5+T6+T7），T1～T7分別為速度曲線各段時間。 T 為軌跡規劃總時間。 為了保證挖掘初始與終止時刻的平穩性， 兩個方向上的初始與終止速度與加速度均設置為0，各時間段的末端速度分別為：

速度曲線在兩個運動方向上共涉及15 個未知參數，確定未知參數，即確定鏟斗齒尖運動軌跡。 最終待優化的設計變量為x=[Tix，J1x，Tiy，J1y，T]（i=1，2，3，5，6，7）， 其中Tix與Tiy分別為x 軸與y 軸方向上除勻速段其余六段時間；J1x，J1y分別為x 軸與y 軸方向上加加速段的加加速度。

3.2 目標函數確定

電鏟作業時主要是由提升與推壓機構兩者協同工作實現挖掘， 前端工作裝置使用拉格朗日方程建立動力學模型，用于計算提升與推壓力。 挖掘阻力模型建立的具體計算步驟詳見文獻[1]。 為了保證電鏟實際作業時的高滿斗率及低耗能， 將單位挖掘體積下的最小能耗作為目標函數，具體表達式為：

3.3 約束函數確定

（1）為了保證每次挖掘結束，挖掘滿斗率應滿足實際需求。 設置滿斗率約束為80%～110%斗容量：

3.4 堆料數據查找方法

Kd-tree 是一種對K 維空間的數據點進行儲存以便能快速檢索的數據結構，主要應用于多維空間中數據的范圍檢索或最鄰近搜索。

本文基于堆料點云數據的x、y 坐標建立二維Kdtree 數據結構，使用K 最鄰近搜索算法在Kd-tree 中搜索挖掘每一時刻下鏟斗齒尖的x、y 坐標所對應的點云數據中最鄰近的物料點的x、y 坐標值， 將此物料點的z 值近似替代為這一時刻下齒尖點正上方所對應的真實物料點的z 值。 由于本文使用80 線激光雷達進行數據采集，獲取點云數據量較大，精度較高且點云分布密集。 因此，利用該方法計算三維空間下的動態挖掘體積也應具有較高的精度。

3.5 優化模型建立與求解

通過上述分析，無人電鏟軌跡優化模型可以歸納如下式所示：

4 實驗驗證

4.1 點云配準結果分析

表1 為設置的ICP 配準收斂參數。粗配準與精配準的程序運行時間分別為3.407s 與0.061s。 點云配準結果見圖1。

圖1 點云配準結果對比

表1 ICP 配準收斂參數設置

4.2 點云分割實驗結果分析

由于電鏟機身安裝的傾角傳感器沒有投入使用，故本文僅給出理論解釋與計算公式，C++程序中側傾角與俯仰角均設置為0.0001°。高度閾值設置為0.55m。表2 為區域生長算法設置的參數。

表2 區域生長算法參數設置

點云聚類結果見圖2，由于聚類點集中僅有料堆與墻壁點集數據量較大， 因此通過數據量排序可篩除其余無用點。 由于激光雷達安裝位置固定， 基站坐標系固定， 每次掃描后點云數據中墻壁點沿x 軸的法向量值分別接近于1（左墻壁）與-1（右墻壁），在排序前三各聚類點集中隨機選擇三個點，設置法向量最小閾值 為-0.8，最大閾值為0.8，若法向量值處在最小與最大閾值之間，則判斷為物料點集，若法向量值處在閾值范圍外，則判斷為左右墻壁點集。 點云分割得到的堆料點云，見圖3。

圖2 點云聚類結果

圖3 點云分割得到的物料點云

4.3 軌跡規劃實驗結果分析

仿真環境采用Matlab 2018b， 設置GA 算法中種群規模、 最大進化代數與最大連續不進化代數為50、30 與20，設計變量、 上下邊界、優化結果與約束函數見表3 與表4。 提升與推壓速度等挖掘參數均滿足約束函數要求。 目標函數f為0.0228kW·h/m3， 挖掘體積V 為0.1399m3，滿斗率為87.4%，滿足實際挖掘滿斗率要求。

圖4 WK-55 電鏟1∶7 縮比實驗樣機

表3 設計變量在上下邊界約束下的優化結果

表4 約束函數取值

圖5 挖掘軌跡優化運行結果

圖6 最優挖掘軌跡參數

5 結論

本文針對傳統電鏟在多因素影響下無法實現高效挖掘， 提出了一種基于七段式S 型速度曲線的挖掘軌跡優化方法和一種基于Kd-tree 與K 最鄰近搜索的物料點云數據查找方法計算挖掘體積。 通過仿真實驗可得到如下結論：

本文所用的點云處理方法可以較好地實現環境點云的配準與物料點云的分割。 挖掘軌跡規劃仿真實驗得到的各挖掘參數均滿足約束函數限制， 挖掘軌跡與目標函數符合實驗前預設需求，可以應用于實際樣機挖掘實驗。在后續研究中， 將繼續完善并優化環境點云處理與挖掘軌跡規劃算法程序，減少程序運行時間及提高計算效率，以滿足實際需求。