本發(fā)明涉及自動(dòng)充電，具體而言，涉及一種基于點(diǎn)云模板匹配的充電樁位姿識(shí)別方法、裝置及設(shè)備。

背景技術(shù)：

1、移動(dòng)機(jī)器人自主回充技術(shù)是智能機(jī)器人領(lǐng)域的一項(xiàng)重要技術(shù)，它使得機(jī)器人能夠在電量低時(shí)自動(dòng)尋找充電樁進(jìn)行充電，從而實(shí)現(xiàn)長(zhǎng)時(shí)間、高效率的自主運(yùn)行。在這個(gè)過(guò)程中，移動(dòng)機(jī)器人需要準(zhǔn)確識(shí)別充電樁的位置和姿態(tài)，以便進(jìn)行后續(xù)的自主對(duì)接充電操作。

2、現(xiàn)有技術(shù)多采用單線激光雷達(dá)，單線激光雷達(dá)僅能提供二維平面上的固定距離的數(shù)據(jù)，無(wú)法獲得充電樁的完整三維結(jié)構(gòu)信息。這導(dǎo)致在進(jìn)行位姿估計(jì)時(shí)精度較低，尤其在充電樁姿態(tài)變化較大時(shí)，難以提供高精度的位姿估計(jì)。此外，由于單線激光雷達(dá)獲取的特征信息較少，充電樁識(shí)別容易受到障礙物和環(huán)境噪聲的影響，導(dǎo)致識(shí)別結(jié)果不準(zhǔn)確。

技術(shù)實(shí)現(xiàn)思路

1、有鑒于此，本發(fā)明的目的在于提供一種基于點(diǎn)云模板匹配的充電樁位姿識(shí)別方法、裝置及設(shè)備，以提高充電樁位姿識(shí)別的準(zhǔn)確度。

2、第一方面，提供一種基于點(diǎn)云模板匹配的充電樁位姿識(shí)別方法，應(yīng)用于待充電機(jī)器人，該方法包括：

3、獲取目標(biāo)充電樁所在區(qū)域內(nèi)的初始三維點(diǎn)云數(shù)據(jù)；初始三維點(diǎn)云數(shù)據(jù)包括充電樁及其周圍其他物體的點(diǎn)云的三維坐標(biāo)數(shù)據(jù)，并由安裝在待充電機(jī)器人前端的多線激光雷達(dá)按照預(yù)設(shè)幀率連續(xù)掃描獲得；

4、針對(duì)連續(xù)掃描的每幀初始三維點(diǎn)云數(shù)據(jù)進(jìn)行濾波預(yù)處理得到預(yù)處理三維點(diǎn)云數(shù)據(jù)；

5、對(duì)預(yù)處理三維點(diǎn)云數(shù)據(jù)進(jìn)行分類檢測(cè)以檢測(cè)出每幀初始三維點(diǎn)云數(shù)據(jù)中的目標(biāo)充電樁三維點(diǎn)云數(shù)據(jù)；

6、將每幀識(shí)別出的目標(biāo)充電樁三維點(diǎn)云數(shù)據(jù)與預(yù)構(gòu)建的充電樁點(diǎn)云模板進(jìn)行點(diǎn)云配準(zhǔn)得到每幀對(duì)應(yīng)的配準(zhǔn)結(jié)果，并將該配準(zhǔn)結(jié)果確定為充電樁位姿數(shù)據(jù)。

7、可選地，針對(duì)連續(xù)掃描的每幀初始三維點(diǎn)云數(shù)據(jù)進(jìn)行濾波預(yù)處理得到預(yù)處理三維點(diǎn)云數(shù)據(jù)包括：

8、針對(duì)連續(xù)掃描的每幀初始三維點(diǎn)云數(shù)據(jù)進(jìn)行直通濾波以濾除預(yù)設(shè)高度以外的三維點(diǎn)云；

9、對(duì)直通濾波后的三維點(diǎn)云進(jìn)行平面濾波以消除充電樁后方的平面遮擋物的點(diǎn)云；其中，平面遮擋物至少為墻面；

10、對(duì)平面濾波后的三維點(diǎn)云進(jìn)行半徑濾波，以濾除孤立點(diǎn)云及異常點(diǎn)云，得到預(yù)處理三維點(diǎn)云數(shù)據(jù)。

11、可選地，對(duì)直通濾波后的三維點(diǎn)云進(jìn)行平面濾波以消除充電樁后方的平面遮擋物的點(diǎn)云包括：

12、在直通濾波后的三維點(diǎn)云中隨機(jī)選擇預(yù)設(shè)數(shù)量的點(diǎn)云，預(yù)設(shè)數(shù)量大于或等于3；

13、基于預(yù)設(shè)數(shù)量的點(diǎn)云的三維坐標(biāo)數(shù)據(jù)對(duì)充電樁后方的平面遮擋物進(jìn)行擬合得到擬合平面；其中，擬合平面的表達(dá)式為：

14、ax+by+cz+d＝0

15、其中，(a、b、c)是平面的法向量，d是一個(gè)常數(shù)，(x、y、z)是選擇的點(diǎn)云的三維坐標(biāo)數(shù)據(jù)；

16、計(jì)算隨機(jī)選擇后剩余的三維點(diǎn)云與擬合平面之間的距離，并基于距離確定擬合平面的內(nèi)點(diǎn)；內(nèi)點(diǎn)是指屬于該擬合平面的點(diǎn)；

17、重復(fù)上述擬合過(guò)程進(jìn)行多次迭代，以得到多個(gè)擬合平面及其內(nèi)點(diǎn)，并將內(nèi)點(diǎn)數(shù)量最多的擬合平面作為目標(biāo)擬合平面；

18、濾除目標(biāo)擬合平面上的內(nèi)點(diǎn)以消除充電樁后方的平面遮擋物的點(diǎn)云。

19、可選地，對(duì)預(yù)處理三維點(diǎn)云數(shù)據(jù)進(jìn)行分類檢測(cè)以檢測(cè)出每幀初始三維點(diǎn)云數(shù)據(jù)中的目標(biāo)充電樁三維點(diǎn)云數(shù)據(jù)包括：

20、利用歐式聚類算法對(duì)預(yù)處理三維點(diǎn)云數(shù)據(jù)進(jìn)行分類檢測(cè)，以得到候選充電樁三維點(diǎn)云數(shù)據(jù)；

21、利用預(yù)設(shè)篩選規(guī)則對(duì)候選充電樁三維點(diǎn)云數(shù)據(jù)進(jìn)行篩選以得到每幀初始三維點(diǎn)云數(shù)據(jù)中的目標(biāo)充電樁三維點(diǎn)云數(shù)據(jù)，預(yù)設(shè)篩選規(guī)則為：按照預(yù)設(shè)充電樁的點(diǎn)云數(shù)量閾值及預(yù)設(shè)充電樁的尺寸閾值進(jìn)行篩選。

22、可選地，充電樁點(diǎn)云模板的構(gòu)建過(guò)程包括：

23、根據(jù)預(yù)設(shè)充電樁的尺寸閾值對(duì)多線激光雷達(dá)掃描的其中任意一幀的三維點(diǎn)云數(shù)據(jù)進(jìn)行裁剪得到初始充電樁點(diǎn)云模板；

24、計(jì)算初始充電樁點(diǎn)云模板的質(zhì)心；

25、將初始充電樁點(diǎn)云模板的質(zhì)心移動(dòng)到多線激光雷達(dá)坐標(biāo)系的原點(diǎn)處，得到充電樁點(diǎn)云模板。

26、可選地，將每幀識(shí)別出的目標(biāo)充電樁三維點(diǎn)云數(shù)據(jù)與預(yù)構(gòu)建的充電樁點(diǎn)云模板進(jìn)行點(diǎn)云配準(zhǔn)的過(guò)程包括：

27、對(duì)于充電樁點(diǎn)云模板中的每個(gè)點(diǎn)，在目標(biāo)充電樁三維點(diǎn)云中找到與其距離最近的點(diǎn)，組成點(diǎn)對(duì)集；

28、基于點(diǎn)對(duì)集中的各點(diǎn)對(duì)確定充電樁點(diǎn)云模板相對(duì)于目標(biāo)充電樁三維點(diǎn)云的旋轉(zhuǎn)矩陣和平移向量，將旋轉(zhuǎn)矩陣和平移向量作為配準(zhǔn)結(jié)果；

29、基于旋轉(zhuǎn)矩陣和平移向量更新充電樁點(diǎn)云模板中各點(diǎn)云的坐標(biāo)數(shù)據(jù)，并計(jì)算誤差函數(shù)；

30、基于更新后的充電樁點(diǎn)云模板中的各點(diǎn)云的坐標(biāo)數(shù)據(jù)重復(fù)上述配準(zhǔn)過(guò)程，直至達(dá)到預(yù)設(shè)收斂條件；預(yù)設(shè)收斂條件至少為以下其中一種：迭代次數(shù)達(dá)到最大值、誤差函數(shù)的變化小于預(yù)設(shè)誤差閾值、旋轉(zhuǎn)矩陣和平移向量的變化小于預(yù)設(shè)閾值；

31、基于預(yù)設(shè)配準(zhǔn)得分計(jì)算公式計(jì)算最終收斂的配準(zhǔn)結(jié)果的配準(zhǔn)得分；預(yù)設(shè)配準(zhǔn)得分計(jì)算公式為：

32、

33、其中，r為旋轉(zhuǎn)矩陣；t為平移向量；(si，ti)為第i個(gè)點(diǎn)對(duì)；n為點(diǎn)對(duì)的數(shù)量；si是充電樁點(diǎn)云模板中的點(diǎn)云；ti是目標(biāo)充電樁三維點(diǎn)云數(shù)據(jù)中的點(diǎn)云；

34、若配準(zhǔn)得分大于預(yù)設(shè)得分閾值，則將最終收斂的配準(zhǔn)結(jié)果確定為目標(biāo)充電樁的位姿數(shù)據(jù)。

35、可選地，方法還包括：

36、采用預(yù)設(shè)長(zhǎng)度的滑動(dòng)窗口對(duì)每幀的配準(zhǔn)結(jié)果進(jìn)行平滑處理，平滑處理包括如下步驟：

37、計(jì)算預(yù)設(shè)長(zhǎng)度的滑動(dòng)窗口內(nèi)的所有幀的配準(zhǔn)結(jié)果的平均配準(zhǔn)結(jié)果；

38、計(jì)算滑動(dòng)窗口內(nèi)每幀配準(zhǔn)結(jié)果與平均配準(zhǔn)結(jié)果的差值；

39、若差值小于預(yù)設(shè)配準(zhǔn)閾值，則配準(zhǔn)有效，否則無(wú)效；

40、計(jì)算所有有效的配準(zhǔn)結(jié)果的平均值，并將該平均值確定為最終的配準(zhǔn)結(jié)果。

41、第二方面，提供一種基于點(diǎn)云模板匹配的充電樁位姿識(shí)別裝置，應(yīng)用于待充電機(jī)器人，該裝置包括：

42、獲取單元，用于獲取目標(biāo)充電樁所在區(qū)域內(nèi)的初始三維點(diǎn)云數(shù)據(jù)；初始三維點(diǎn)云數(shù)據(jù)包括充電樁及其周圍其他物體的點(diǎn)云的三維坐標(biāo)數(shù)據(jù)，并由安裝在待充電機(jī)器人前端的多線激光雷達(dá)按照預(yù)設(shè)幀率連續(xù)掃描獲得；

43、預(yù)處理單元，用于針對(duì)連續(xù)掃描的每幀初始三維點(diǎn)云數(shù)據(jù)進(jìn)行濾波預(yù)處理得到預(yù)處理三維點(diǎn)云數(shù)據(jù)；

44、分類單元，用于對(duì)預(yù)處理三維點(diǎn)云數(shù)據(jù)進(jìn)行分類檢測(cè)以檢測(cè)出每幀初始三維點(diǎn)云數(shù)據(jù)中的目標(biāo)充電樁三維點(diǎn)云數(shù)據(jù)；

45、配準(zhǔn)單元，用于將每幀識(shí)別出的目標(biāo)充電樁三維點(diǎn)云數(shù)據(jù)與預(yù)構(gòu)建的充電樁點(diǎn)云模板進(jìn)行點(diǎn)云配準(zhǔn)得到每幀對(duì)應(yīng)的配準(zhǔn)結(jié)果，并將該配準(zhǔn)結(jié)果確定為充電樁位姿數(shù)據(jù)。

46、第三方面，提供一種電子設(shè)備，包括處理器、通信接口、存儲(chǔ)器和通信總線，其中，處理器，通信接口，存儲(chǔ)器通過(guò)通信總線完成相互間的通信；

47、存儲(chǔ)器，用于存放計(jì)算機(jī)程序；

48、處理器，用于執(zhí)行存儲(chǔ)器上所存放的程序時(shí)，實(shí)現(xiàn)第一方面任一所述的方法步驟。

49、第四方面，提供一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，所述計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)內(nèi)存儲(chǔ)有計(jì)算機(jī)程序，所述計(jì)算機(jī)程序被處理器執(zhí)行時(shí)實(shí)現(xiàn)第一方面任一所述的方法步驟。

50、本發(fā)明提供的一種基于點(diǎn)云模板匹配的充電樁位姿識(shí)別方法、裝置及設(shè)備，通過(guò)獲取目標(biāo)充電樁所在區(qū)域內(nèi)的初始三維點(diǎn)云數(shù)據(jù)；針對(duì)連續(xù)掃描的每幀初始三維點(diǎn)云數(shù)據(jù)進(jìn)行濾波預(yù)處理得到預(yù)處理三維點(diǎn)云數(shù)據(jù)；對(duì)預(yù)處理三維點(diǎn)云數(shù)據(jù)進(jìn)行分類檢測(cè)以檢測(cè)出每幀初始三維點(diǎn)云數(shù)據(jù)中的目標(biāo)充電樁三維點(diǎn)云數(shù)據(jù)；將每幀識(shí)別出的目標(biāo)充電樁三維點(diǎn)云數(shù)據(jù)與預(yù)構(gòu)建的充電樁點(diǎn)云模板進(jìn)行點(diǎn)云配準(zhǔn)得到每幀對(duì)應(yīng)的配準(zhǔn)結(jié)果，并將該配準(zhǔn)結(jié)果確定為充電樁位姿數(shù)據(jù)。本發(fā)明通過(guò)多線激光雷達(dá)掃描來(lái)獲取充電樁的三維點(diǎn)云數(shù)據(jù)，為位姿識(shí)別提供了豐富的特征數(shù)據(jù)，并且通過(guò)濾波預(yù)處理減少了其他障礙物等的干擾，提高了位姿識(shí)別的準(zhǔn)確度，最后通過(guò)與預(yù)構(gòu)建的充電樁點(diǎn)云模板進(jìn)行點(diǎn)云配準(zhǔn)獲得充電樁的位姿數(shù)據(jù)，大大提高了位姿識(shí)別的精確度。

51、為使本發(fā)明的上述目的、特征和優(yōu)點(diǎn)能更明顯易懂，下文特舉較佳實(shí)施例，并配合所附附圖，作詳細(xì)說(shuō)明如下。