[發(fā)明專利]一種井下無人駕駛無軌膠輪車的路徑規(guī)劃方法

有效

• 申請?zhí)枺?/td>: 202110306470.0
• 申請日: 2021-03-23
• 公開（公告）號: CN112977443B
• 公開（公告）日: 2022-03-11
• 發(fā)明（設(shè)計）人: 葉賓;馮振楠;楊鑫;李會軍
• 申請（專利權(quán)）人: 中國礦業(yè)大學(xué)
• 主分類號: B60W30/10
• 分類號: B60W30/10;B60W50/00
• 代理公司: 徐州蘇越知識產(chǎn)權(quán)代理事務(wù)所(普通合伙) 32543
• 代理人: 張旭

1.一種井下無人駕駛無軌膠輪車的路徑規(guī)劃方法，其特征在于，具體步驟為：

(1)使用固態(tài)激光雷達(dá)、NUC電腦和移動電源搭建數(shù)據(jù)采集裝置，NUC電腦分別與移動電源和固態(tài)激光雷達(dá)連接；

(2)將數(shù)據(jù)采集裝置安裝在有人駕駛的無軌膠輪車上，使無軌膠輪車在井下巷道內(nèi)遍歷所有行駛區(qū)域，數(shù)據(jù)采集裝置的固態(tài)激光雷達(dá)實(shí)時采集數(shù)據(jù)并反饋給NUC電腦，最終采集獲得原始點(diǎn)云數(shù)據(jù)包；

(3)根據(jù)采集到的數(shù)據(jù)，使用Cartographer算法離線建立井下巷道三維點(diǎn)云地圖，其中設(shè)定地圖原點(diǎn)為井口位置；

(4)對得到的三維巷道地圖進(jìn)行地面分割處理，使用Ground Plane Filter算法分割出地面點(diǎn)云數(shù)據(jù)，得到條帶狀地面點(diǎn)云；

(5)使用Meanshift算法對條帶狀點(diǎn)云進(jìn)行迭代漂移，使邊緣點(diǎn)向著道路中心聚集，從而使條帶狀分布的道路點(diǎn)細(xì)化成線狀，得到巷道中心點(diǎn)集；

(6)由于井下巷道具有多個分叉點(diǎn)與端點(diǎn)，在得到的巷道中心點(diǎn)集中將所有巷道分叉點(diǎn)與端點(diǎn)手動標(biāo)識出來，從而使巷道線狀中心點(diǎn)集拆分成若干段無分叉的點(diǎn)集；

(7)根據(jù)各標(biāo)識點(diǎn)之間線狀點(diǎn)云數(shù)據(jù)，分段擬合出各路段中心線，最終拼接成具有拓?fù)潢P(guān)系的井下巷道路網(wǎng)；

(8)在進(jìn)行路徑規(guī)劃時，首先確定起點(diǎn)與終點(diǎn)在巷道拓?fù)渚W(wǎng)絡(luò)中的位置，然后使用Dijsktra算法搜索從起點(diǎn)至終點(diǎn)距離最短的路徑，最終完成路徑規(guī)劃。

2.根據(jù)權(quán)利要求1所述的一種井下無人駕駛無軌膠輪車的路徑規(guī)劃方法，其特征在于，所述步驟(2)采集獲得原始點(diǎn)云數(shù)據(jù)包的具體過程為：將數(shù)據(jù)采集裝置安裝在有人駕駛的無軌膠輪車前部車頭處，接著開啟固態(tài)激光雷達(dá)，以井口位置為起點(diǎn)，使無軌膠輪車以0.3m/s以下的速度在巷道內(nèi)向前勻速移動，同時使用NUC電腦錄制固態(tài)激光雷達(dá)測量數(shù)據(jù)的bag包；無軌膠輪車在井下巷道內(nèi)遍歷所有行駛區(qū)域，最終獲得原始點(diǎn)云數(shù)據(jù)包。

3.根據(jù)權(quán)利要求1所述的一種井下無人駕駛無軌膠輪車的路徑規(guī)劃方法，其特征在于，所述步驟(4)的具體過程為：

首先沿車輛行進(jìn)方向?qū)Ⅻc(diǎn)云分成若干個子點(diǎn)云段，使每個子點(diǎn)云段中地面近似為平面，減小坡度變化對分割結(jié)果的影響，然后對每個子點(diǎn)云段中點(diǎn)云數(shù)據(jù)使用Ground PlaneFilter分割出地面點(diǎn)云；

Ground Plane Filter算法分割地面具體流程如下：

首先選定點(diǎn)云中n個最低點(diǎn)，計算其平均值，得到LPR值，根據(jù)設(shè)定的高度閾值Th，將點(diǎn)云數(shù)據(jù)中高度在閾值范圍內(nèi)的點(diǎn)提取出來作為種子點(diǎn)集；之后根據(jù)種子點(diǎn)集估計平面模型，采用如下線性模型進(jìn)行估計：

ax+by+cz+d＝0

n^Tx＝-d

其中n＝[a,b,c]^T，x＝[x,y,z]^T；a、b、c、d為平面模型方程的四個參數(shù)，使用協(xié)方差矩陣求解此線性模型，種子點(diǎn)集S∈R³的協(xié)方差矩陣為：

式中，C為協(xié)方差矩陣，S為種子點(diǎn)集，S_i代表此種子點(diǎn)集中第i個點(diǎn)，為種子點(diǎn)集中所有點(diǎn)均值；此協(xié)方差矩陣的三個奇異向量能通過奇異值分解求得，這三個奇異向量描述點(diǎn)集在三個主要方向的散布情況；通過計算具有最小奇異值的奇異向量來求得垂直于平面的法向量n，之后代入求得d，即能求出平面模型；

求出平面模型以后，計算點(diǎn)云數(shù)據(jù)中每一個點(diǎn)到該平面的正交投影的距離，與設(shè)定閾值比較，若小于閾值，該點(diǎn)為地面點(diǎn)，若大于閾值，則為非地面點(diǎn)。