本公開涉及信息,尤其涉及一種基于點云數據的路徑規劃方法、基于點云數據的路徑規劃裝置、存儲介質及服務器。
背景技術:
1、路徑規劃技術是移動機器人實現自主導航與避障的一項重要技術。近年來取得很多與之相關的研究成果。原理是指機器人在工作環境中,根據自身傳感器對環境的感知,自行規劃出一條從起點到目標點的最優或接近最優的路徑,使得機器人能夠避開工作環境中的障礙物,安全、高效地到達終點。但有時候在路徑規劃時存在定位不準確,從而造成路徑規劃出現問題。
技術實現思路
1、有鑒于此,本公開實施例期望提供一種基于點云數據的路徑規劃方法、基于點云數據的路徑規劃裝置、存儲介質及服務器。
2、本公開的技術方案是這樣實現的:
3、第一方面,本公開提供一種基于點云數據的路徑規劃方法。
4、本公開實施例提供的基于點云數據的路徑規劃方法,包括:
5、基于初始點和目標點構建全局點云圖,及基于當前節點構建局部點云圖;
6、對所述全局點云圖與所述局部點云圖進行網格化處理;
7、獲取所述局部點云圖中待配準點云轉換到所述全局點云圖的網格時,得到的匹配準確率;其中所述匹配準確率用于作為進行路徑規劃選擇下一個節點時優化代價函數的權重參數;
8、基于所述代價函數,在所述當前節點的相鄰節點中確定出所述路徑規劃的所述下一個節點;其中,所述代價函數的權重中包含有作為所述權重參數的所述匹配準確率。
9、在一些實施例中,所述代價函數f(i)為:
10、
11、所述代價函數的權重為:
12、
13、所述匹配準確率為:p(i);
14、其中,i為待確定的下一個節點,s0為開始點,g0為目標點;d(j,j+1)為從j點到j+1點的歐式距離。
15、在一些實施例中,所述獲取所述局部點云圖中待配準點云轉換到所述全局點云圖的網格時,得到的匹配準確率,包括:
16、確定所述全局點云圖的每個網格的多維正態分布參數均值和協方差矩陣σ;
17、初始化變換參數p=(tx.ty,φ);其中,tx為點云在x軸方向的移動距離,ty為點云在y軸方向的移動距離,φ為點云在空間中旋轉角度;
18、通過變換t將局部點云圖中待配準的點云轉換到全局點云圖的網格中;其中,y′i=t(yi,p);變換之前的待配準的點;
19、其中,y′i=1,…,m表示局部點云圖中待配準的點云網格內所有掃描點,變換t為:
20、
21、根據正態分布參數計算每個掃描點的概率密度p(y'i);
22、其中,
23、根據待配準網格中每個掃描點的概率密度p(y'i),得到所述待配準網格的配準得分score(p);其中,
24、
25、采用hessian矩陣和牛頓迭代法對score(p)求最小值,達到收斂條件,得到所述待配準網格的匹配準確率p(i);其中,收斂條件為hδp=-g,其中g是score(p)的轉置梯度,h是score(p)的hessian矩陣。
26、在一些實施例中,所述基于所述代價函數,在所述當前節點的相鄰節點中確定出所述路徑規劃的所述下一個節點,包括:基于所述代價函數,確定出所述當前節點的每個相鄰節點對應的節點i距離初始點代價g(i)值;
27、確定所述相鄰節點中g(i)值最小的節點作為所述路徑規劃的所述下一個節點。
28、第二方面,本公開提供一種基于點云數據的路徑規劃裝置,包括:
29、點云圖構建模塊,用于基于初始點和目標點構建全局點云圖,及基于當前節點構建局部點云圖;
30、網格化處理模塊,用于對所述全局點云圖與所述局部點云圖進行網格化處理;
31、匹配準確率確定模塊,用于獲取所述局部點云圖中待配準點云轉換到所述全局點云圖的網格時,得到的匹配準確率;其中所述匹配準確率用于作為進行路徑規劃選擇下一個節點時優化代價函數的權重參數;
32、路徑規劃模塊,用于基于所述代價函數,在所述當前節點的相鄰節點中確定出所述路徑規劃的所述下一個節點;其中,所述代價函數的權重中包含有作為所述權重參數的所述匹配準確率。
33、在一些實施例中,所述代價函數f(i)為:
34、
35、所述代價函數的權重為:
36、
37、所述匹配準確率為:p(i);
38、其中,i為待確定的下一個節點,s0為開始點,g0為目標點;d(j,j+1)為從j點到j+1點的歐式距離。
39、在一些實施例中,所述匹配準確率確定模塊,用于
40、確定所述全局點云圖的每個網格的多維正態分布參數均值和協方差矩陣σ;
41、初始化變換參數p=(tx.ty,φ)t;其中,tx為點云在x軸方向的移動距離,ty為點云在y軸方向的移動距離,φ為點云在空間中旋轉角度;
42、通過變換t將局部點云圖中待配準的點云轉換到全局點云圖的網格中;其中,y′i=t(yi,p);變換之前的待配準的點;
43、其中,y′i=1,...,m表示局部點云圖中待配準的點云網格內所有掃描點,變換t為:
44、
45、根據正態分布參數計算每個掃描點的概率密度p(y'i);
46、其中,根據待配準網格中每個掃描點的概率密度p(y'i),得到所述待配準網格的配準得分score(p);其中,
47、
48、采用hessian矩陣和牛頓迭代法對score(p)求最小值,達到收斂條件,得到所述待配準網格的匹配準確率p(i);其中,收斂條件為hδp=-g,其中g是score(p)的轉置梯度,h是score(p)的hessian矩陣。
49、在一些實施例中,所述路徑規劃模塊,用于
50、基于所述代價函數,確定出所述當前節點的每個相鄰節點對應的節點i距離初始點代價g(i)值;
51、確定所述相鄰節點中g(i)值最小的節點作為所述路徑規劃的所述下一個節點。
52、第三方面,本公開提供一種計算機可讀存儲介質,其上存儲有基于點云數據的路徑規劃程序,該基于點云數據的路徑規劃程序被處理器執行時,實現上述第一方面所述的基于點云數據的路徑規劃方法。
53、第四方面,本公開提供一種服務器,包括存儲器、處理器及存儲在存儲器上并可在處理器上運行的基于點云數據的路徑規劃程序,所述處理器執行所述基于點云數據的路徑規劃程序時,實現上述第一方面所述的基于點云數據的路徑規劃方法。
54、根據本公開實施例的基于點云數據的路徑規劃方法包括基于初始點和目標點構建全局點云圖,及基于當前節點構建局部點云圖;對全局點云圖與局部點云圖進行網格化處理;獲取局部點云圖中待配準點云轉換到全局點云圖的網格時,得到的匹配準確率;其中匹配準確率用于作為進行路徑規劃選擇下一個節點時優化代價函數的權重參數;基于代價函數,在當前節點的相鄰節點中確定出路徑規劃的下一個節點;其中,代價函數的權重中包含有作為權重參數的匹配準確率。本技術中,將匹配準確率與局部路徑規劃算法的代價函數相結合,實現對局部路徑規劃的改進,通過代價函數進行路徑規劃確定下一個節點時,在代價函數的權重中引入匹配準確率作為權重參數,提高基于高精度地圖的路徑規劃的置信度。
55、本公開附加的方面和優點將在下面的描述中部分給出,部分將從下面的描述中變得明顯,或通過本公開的實踐了解到。