本技術(shù)涉及位姿估計(jì)，尤其涉及一種位姿估計(jì)方法、裝置和電子設(shè)備。

背景技術(shù)：

1、目前，在對(duì)對(duì)稱平面物體進(jìn)行作業(yè)過(guò)程中，通常采用傳統(tǒng)的定向包圍盒（orientedbounding?box，obb）算法求解對(duì)稱平面物體的位姿，該方式求解出的位姿中，對(duì)稱平面物體的主方向可能與對(duì)稱平面物體的邊長(zhǎng)方向存在誤差，例如，當(dāng)對(duì)稱平面物體的長(zhǎng)寬比較小時(shí)，其主方向會(huì)沿著對(duì)稱平面物體的對(duì)角線方向，從而導(dǎo)致對(duì)稱平面物體的位姿估計(jì)出現(xiàn)偏差，進(jìn)而影響對(duì)稱平面物體的作業(yè)精準(zhǔn)度。

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

1、本技術(shù)提供了一種位姿估計(jì)方法、裝置和電子設(shè)備，用以解決現(xiàn)有技術(shù)針對(duì)對(duì)稱平面物體的位姿估計(jì)準(zhǔn)確度較差的問(wèn)題。

2、本技術(shù)提供的技術(shù)方案如下：

3、一方面，本技術(shù)提供了一種位姿估計(jì)方法，包括：

4、獲取對(duì)稱平面物體的平面點(diǎn)云數(shù)據(jù)；其中，平面點(diǎn)云數(shù)據(jù)包括對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的點(diǎn)云數(shù)據(jù)；

5、基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，確定對(duì)稱平面物體對(duì)應(yīng)的最小有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣；

6、基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，以平面法向量與旋轉(zhuǎn)矩陣中的豎軸方向向量的方向相同為約束條件，確定對(duì)稱平面物體對(duì)應(yīng)的最大擬合平面；

7、基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)各自投影到最大擬合平面上的投影采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，確定最大擬合平面的邊長(zhǎng)方向向量；

8、基于最大擬合平面的邊長(zhǎng)方向向量，對(duì)旋轉(zhuǎn)矩陣進(jìn)行校正得到目標(biāo)旋轉(zhuǎn)矩陣；

9、基于質(zhì)心位置和目標(biāo)旋轉(zhuǎn)矩陣，確定對(duì)稱平面物體的目標(biāo)位姿。

10、可選的，基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，確定對(duì)稱平面物體對(duì)應(yīng)的最小有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣，包括：

11、基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，采用主成分分析算法分析出對(duì)稱平面物體的特征值和特征向量；

12、基于對(duì)稱平面物體的特征值和特征向量，確定對(duì)稱平面物體對(duì)應(yīng)的最小有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣。

13、可選的，基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，確定對(duì)稱平面物體對(duì)應(yīng)的最小有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣，包括：

14、基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，迭代執(zhí)行有向包容盒求解操作，直至確定滿足迭代終止條件時(shí)，將當(dāng)前執(zhí)行有向包容盒求解操作時(shí)求解出的當(dāng)前有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣確定為最小有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣；其中，有向包容盒求解操作包括：

15、基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，計(jì)算各個(gè)原始采樣點(diǎn)各自投影到指定平面上的投影采樣點(diǎn)的點(diǎn)云數(shù)據(jù)；

16、基于各個(gè)原始采樣點(diǎn)各自投影到指定平面上的投影采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，采用凹包算法提取對(duì)稱平面物體的凹邊界上的各個(gè)輪廓采樣點(diǎn)的點(diǎn)云數(shù)據(jù)；其中，在首次執(zhí)行有向包容盒求解操作時(shí)，凹包算法的學(xué)習(xí)參數(shù)為初始化值；在非首次執(zhí)行有向包容盒求解操作時(shí)，凹包算法的學(xué)習(xí)參數(shù)為上一次執(zhí)行有向包容盒求解操作時(shí)調(diào)整的目標(biāo)值；

17、基于對(duì)稱平面物體的凹邊界上的各個(gè)輪廓采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，采用主成分分析算法分析出對(duì)稱平面物體的特征值和特征向量；

18、基于對(duì)稱平面物體的特征值和特征向量，確定對(duì)稱平面物體對(duì)應(yīng)的當(dāng)前有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣；

19、提取當(dāng)前有向包圍盒的盒邊界上的各個(gè)輪廓采樣點(diǎn)的點(diǎn)云數(shù)據(jù)；

20、將對(duì)稱平面物體的凹邊界上的各個(gè)輪廓采樣點(diǎn)的點(diǎn)云數(shù)據(jù)與當(dāng)前有向包圍盒的盒邊界上的各個(gè)輪廓采樣點(diǎn)的點(diǎn)云數(shù)據(jù)進(jìn)行點(diǎn)云配準(zhǔn)，得到對(duì)稱平面物體的凹邊界與當(dāng)前有向包圍盒的盒邊界之間的匹配分?jǐn)?shù)；

21、確定匹配分?jǐn)?shù)小于設(shè)定分?jǐn)?shù)閾值時(shí)，基于對(duì)稱平面物體的凹邊界與當(dāng)前有向包圍盒的盒邊界之間的差異，將凹包算法的學(xué)習(xí)參數(shù)調(diào)整為目標(biāo)值。

22、可選的，基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，以平面法向量與旋轉(zhuǎn)矩陣中的豎軸方向向量的方向相同為約束條件，確定對(duì)稱平面物體對(duì)應(yīng)的最大擬合平面，包括：

23、基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，迭代執(zhí)行平面擬合操作，直至確定當(dāng)前執(zhí)行平面擬合操作時(shí)計(jì)算出的當(dāng)前內(nèi)點(diǎn)比例不小于設(shè)定內(nèi)點(diǎn)比例閾值時(shí)，將當(dāng)前執(zhí)行平面擬合操作時(shí)擬合出的當(dāng)前擬合平面確定為對(duì)稱平面物體的最大擬合平面；其中，平面擬合操作包括：

24、從對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)中，隨機(jī)選取設(shè)定數(shù)量的原始采樣點(diǎn)作為候選采樣點(diǎn)；

25、基于各個(gè)候選采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，以平面法向量與旋轉(zhuǎn)矩陣中的豎軸方向向量的方向相同為約束條件，確定當(dāng)前擬合平面；

26、基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，計(jì)算對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)各自到達(dá)當(dāng)前擬合平面的距離；

27、將距離小于設(shè)定距離閾值的各個(gè)原始采樣點(diǎn)分別確定為內(nèi)點(diǎn)，并基于內(nèi)點(diǎn)的數(shù)量以及對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的數(shù)量，計(jì)算當(dāng)前內(nèi)點(diǎn)比例。

28、可選的，基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)各自投影到最大擬合平面上的投影采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，確定最大擬合平面的邊長(zhǎng)方向向量，包括：

29、基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)各自投影到最大擬合平面上的投影采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，提取最大擬合平面的各條輪廓線；

30、基于各條輪廓線中相交輪廓線對(duì)應(yīng)的交點(diǎn)的點(diǎn)云數(shù)據(jù)，計(jì)算最大擬合平面的邊長(zhǎng)方向向量。

31、可選的，基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)各自投影到最大擬合平面上的投影采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，提取最大擬合平面的各條輪廓線，包括：

32、基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，計(jì)算各個(gè)原始采樣點(diǎn)各自投影到最大擬合平面上的投影采樣點(diǎn)的點(diǎn)云數(shù)據(jù)；

33、基于各個(gè)原始采樣點(diǎn)各自投影到最大擬合平面上的投影采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，采用凹包算法提取最大擬合平面的凹邊界上的各個(gè)輪廓采樣點(diǎn)的點(diǎn)云數(shù)據(jù)；

34、對(duì)各個(gè)輪廓采樣點(diǎn)的點(diǎn)云數(shù)據(jù)進(jìn)行直線提取得到最大擬合平面的各條輪廓線。

35、可選的，最大擬合平面為方形；基于各條輪廓線中相交輪廓線對(duì)應(yīng)的交點(diǎn)的點(diǎn)云數(shù)據(jù)，計(jì)算最大擬合平面的邊長(zhǎng)方向向量，包括：

36、從最大擬合平面的四條輪廓線中選擇一條輪廓線作為基準(zhǔn)輪廓線，計(jì)算基準(zhǔn)輪廓線與四條輪廓線中其余的三條輪廓線之間的距離；

37、將三條輪廓線中距離最大的輪廓線與基準(zhǔn)輪廓線組成第一組對(duì)邊，以及將三條輪廓線中剩余的兩條輪廓線組成第二組對(duì)邊；

38、基于第一組對(duì)邊中的一條輪廓線與第二組對(duì)邊中的兩條輪廓線之間的交點(diǎn)的點(diǎn)云數(shù)據(jù)，計(jì)算最大擬合平面的邊長(zhǎng)方向向量。

39、可選的，基于最大擬合平面的邊長(zhǎng)方向向量，對(duì)旋轉(zhuǎn)矩陣進(jìn)行校正得到目標(biāo)旋轉(zhuǎn)矩陣，包括：

40、基于邊長(zhǎng)方向向量和旋轉(zhuǎn)矩陣中的豎軸方向向量，采用向量叉乘右手定則計(jì)算橫軸方向向量；

41、基于邊長(zhǎng)方向向量、豎軸方向向量和橫軸方向向量，確定目標(biāo)旋轉(zhuǎn)矩陣。

42、另一方面，本技術(shù)提供了一種位姿估計(jì)裝置，包括：

43、點(diǎn)云獲取單元，用于獲取對(duì)稱平面物體的平面點(diǎn)云數(shù)據(jù)；其中，平面點(diǎn)云數(shù)據(jù)包括對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的點(diǎn)云數(shù)據(jù)；

44、包圍盒確定單元，用于基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，確定對(duì)稱平面物體對(duì)應(yīng)的最小有向包圍盒的質(zhì)心位置和旋轉(zhuǎn)矩陣；

45、平面擬合單元，用于基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，以平面法向量與旋轉(zhuǎn)矩陣中的豎軸方向向量的方向相同為約束條件，確定對(duì)稱平面物體對(duì)應(yīng)的最大擬合平面；

46、校正預(yù)備單元，用于基于對(duì)稱平面物體的表面上的各個(gè)原始采樣點(diǎn)各自投影到最大擬合平面上的投影采樣點(diǎn)的點(diǎn)云數(shù)據(jù)，確定最大擬合平面的邊長(zhǎng)方向向量；

47、姿態(tài)校正單元，用于基于最大擬合平面的邊長(zhǎng)方向向量，對(duì)旋轉(zhuǎn)矩陣進(jìn)行校正得到目標(biāo)旋轉(zhuǎn)矩陣；

48、位姿確定單元，用于基于質(zhì)心位置和目標(biāo)旋轉(zhuǎn)矩陣，確定對(duì)稱平面物體的目標(biāo)位姿。

49、另一方面，本技術(shù)提供了一種電子設(shè)備，包括存儲(chǔ)器、處理器和存儲(chǔ)在存儲(chǔ)器上并可在處理器上運(yùn)行的計(jì)算機(jī)程序，處理器執(zhí)行計(jì)算機(jī)程序時(shí)實(shí)現(xiàn)上述位姿估計(jì)方法。

50、本技術(shù)的有益效果如下：

51、本技術(shù)通過(guò)利用對(duì)稱平面物體對(duì)應(yīng)的最大擬合平面的邊長(zhǎng)方向向量，校正對(duì)稱平面物體對(duì)應(yīng)的最小有向包圍盒的旋轉(zhuǎn)矩陣，能夠使旋轉(zhuǎn)矩陣中的主方向始終沿著對(duì)稱平面物體的邊長(zhǎng)方向，從而降低了對(duì)稱平面物體的位姿估計(jì)偏差，提高了對(duì)稱平面物體的作業(yè)精準(zhǔn)度。

52、本技術(shù)的其它特征和優(yōu)點(diǎn)將在隨后的說(shuō)明書中闡述，并且，部分地可以從說(shuō)明書中變得顯而易見(jiàn)，或者通過(guò)實(shí)施本技術(shù)而了解。本技術(shù)的目的和其他優(yōu)點(diǎn)可通過(guò)在所寫的說(shuō)明書、權(quán)利要求書、以及附圖中特別指出的結(jié)構(gòu)來(lái)實(shí)現(xiàn)和獲得。