本發(fā)明涉及船艙內(nèi)挖掘機定位，具體涉及一種基于三維點云的船艙內(nèi)挖掘機防撞定位方法及系統(tǒng)。

背景技術：

1、裝有干散貨的船艙多采用門機、裝載機、挖掘機作業(yè)的方式進行物料清運，船艙內(nèi)裝載有物料，船艙為頂部開口的多邊形封閉式環(huán)境，四周為艙壁，艙頂設置有卸船機，挖掘機負責將遠離卸船機的物料聚攏到卸船機的工作區(qū)域，卸船機將物料抓取并轉(zhuǎn)移到船艙外。

2、為對挖掘機的工作進行精確控制，挖掘機在船艙封閉環(huán)境內(nèi)作業(yè)時需要對環(huán)境信息進行感知，現(xiàn)有技術中通常采用激光雷達向船艙四周發(fā)送激光，獲取由艙壁反射的激光信息，感知挖掘機在船艙內(nèi)的位置，將挖掘機的位置信息發(fā)送至控制端并共享給卸船機以及裝載機，能夠與卸船機、裝載機以及其他挖掘機同時進入同一區(qū)域發(fā)生碰撞。

3、現(xiàn)有的挖掘機自身具備回轉(zhuǎn)結(jié)構(gòu)和機械臂，回轉(zhuǎn)結(jié)構(gòu)能夠通過機械臂回轉(zhuǎn)帶動鏟斗來回運料，機械臂自身的旋轉(zhuǎn)能夠調(diào)整鏟斗位置以實現(xiàn)上料和卸料，回轉(zhuǎn)結(jié)構(gòu)和機械臂的位置不定且無法精準把控回轉(zhuǎn)結(jié)構(gòu)和機械臂的位置，極易在挖掘機靠近艙壁時造成機械臂、鏟斗碰撞艙壁，因此，僅僅通過獲取并控制挖掘機的位置信息并不能保證避免挖掘機上結(jié)構(gòu)的碰撞，機械運轉(zhuǎn)安全性無法保證。

技術實現(xiàn)思路

1、為此，本發(fā)明提供一種基于三維點云的船艙內(nèi)挖掘機防撞定位方法及系統(tǒng)，有效的解決了現(xiàn)有技術中僅僅通過獲取并控制挖掘機的位置信息不能保證避免挖掘機上結(jié)構(gòu)的碰撞，機械運轉(zhuǎn)安全性無法保證的技術問題。

2、為解決上述技術問題，本發(fā)明具體提供下述技術方案：一種基于三維點云的船艙內(nèi)挖掘機防撞定位方法，包括以下步驟：

3、采用激光雷達掃描船艙內(nèi)環(huán)境得到三維點云數(shù)據(jù)并映射到二維圖像平面上，以形成深度圖；

4、對三維點云數(shù)據(jù)依次進行點云分割、特征抽取并結(jié)合不同幀的三維點云數(shù)據(jù)進行點云配準，輸出定位信息；

5、以深度圖中每一個像素點及其相鄰像素點組成一個點單元并進行特征提取，篩除平滑度不達標的非船艙壁面點，以得到船艙內(nèi)的艙壁深度圖；

6、獲取挖掘機上回轉(zhuǎn)結(jié)構(gòu)、機械臂的空間參數(shù)，以構(gòu)建挖掘機模型，并基于挖掘機模型和艙壁深度圖推算鏟斗與艙壁的距離；

7、進行鏟斗碰撞實驗，獲取鏟斗與艙壁的最大間距、最小間距以及不同間距下回轉(zhuǎn)結(jié)構(gòu)的回轉(zhuǎn)角度極限值、機械臂的關節(jié)角度極限值并計算間距差，以間距差作為位移向量構(gòu)建虛擬點云集，以虛擬點云集為基準根據(jù)不同間距下回轉(zhuǎn)結(jié)構(gòu)的回轉(zhuǎn)角度極限值、機械臂的關節(jié)角度極限值建立防撞約束。

8、進一步地，

9、對三維點云數(shù)據(jù)依次進行點云分割、特征抽取，包括以下步驟：

10、將深度圖水平方向等分為若干個子圖像；

11、對t時刻點云集pt＝{p1，p2，……，pn}中每個點水平相鄰位置各取3個點形成點云集s，計算平滑度：

12、

13、式中，c為平滑度，r1為點云集中點p1到激光雷達的歐氏距離，r2為點云集中點p2到激光雷達的歐氏距離；

14、設定平滑度閾值c1，將平滑度c大于c1的點分割成邊緣特征點并組合形成點云集fb，將平滑度c小于c1的點分割成地面特征點并組合形成點云集fd，對點云集fd進行濾除，得到點云集p1。

15、進一步地，

16、獲取不同幀的三維點云數(shù)據(jù)，依次進行點云分割、特征抽取，得到點云集p2；

17、以點云集p1為待融合三維點云集，以點云集p2為目標三維點云集；

18、采用icp方法求解得出從待融合三維點云集到目標三維點云集的旋轉(zhuǎn)矩陣和平移向量，通過旋轉(zhuǎn)矩陣和平移向量對待融合三維點云集進行坐標變化，得到變換后的點云集pb，輸出定位信息。

19、進一步地，

20、設定平滑度閾值c2；

21、對點云集pb中每個點水平、豎直相鄰位置各取2個點形成點云集s1，計算平滑度c：

22、將平滑度c小于c2的點分割成非艙壁特征點并組合形成點云集fc；

23、對點云集fc進行濾除，得到最終的點云集pz。

24、進一步地，

25、建立挖掘機上各液壓缸的長度與機械臂的角度值的對應關系；

26、通過位移傳感器測量挖掘機上各液壓缸的長度，基于挖掘機上各液壓缸的長度與機械臂的角度值的對應關系換算得出機械臂的角度值；

27、以回轉(zhuǎn)結(jié)構(gòu)中心位置為原點，建立坐標系，基于機械臂的角度值計算鏟斗在坐標系中的狀態(tài)向量m＝(x，y，z，θc)：

28、

29、式中，x為鏟斗在x軸方向的坐標值，y為鏟斗在y軸方向的坐標值，z為鏟斗在z軸方向的坐標值，θc為鏟斗的旋轉(zhuǎn)角，d0為第一動臂首端端點到原點的距離，a0為第二動臂首端端點到第一動臂首端端點的距離，a1為第三動臂首端端點到第二動臂首端端點的距離，a2為鏟斗首端端點到第三動臂首端端點的距離，a3為鏟斗的長度，θ0為回轉(zhuǎn)結(jié)構(gòu)的回轉(zhuǎn)角度，θ1為第一動臂的關節(jié)角度，θ2為第二動臂的關節(jié)角度，θ3為第三動臂的關節(jié)角度；

30、根據(jù)鏟斗的狀態(tài)向量計算鏟斗與回轉(zhuǎn)結(jié)構(gòu)的水平距離：

31、

32、式中，l為鏟斗與回轉(zhuǎn)結(jié)構(gòu)的水平距離。

33、進一步地，

34、根據(jù)艙壁深度圖通過點到平面的距離計算方法計算激光雷達到艙壁的距離，并結(jié)合鏟斗與回轉(zhuǎn)結(jié)構(gòu)的水平距離推算鏟斗與艙壁的距離：

35、

36、式中，s為鏟斗與艙壁的距離，l為激光雷達到艙壁的距離，r為激光雷達到回轉(zhuǎn)中心的水平距離。

37、進一步地，

38、基于回轉(zhuǎn)結(jié)構(gòu)的不同回轉(zhuǎn)角度、機械臂的不同關節(jié)角度進行鏟斗碰撞實驗，得出同一挖掘機位置下鏟斗與艙壁的最大間距、最小間距并計算間距差：

39、δs＝smax-smin；

40、式中，δs為間距差，smax為鏟斗與艙壁的最大間距，smin為鏟斗與艙壁的最小間距。

41、進一步地，

42、設定至少四組邊緣角度范圍(d°，a°)、(a°，b°)、(b°，c°)、(c°，d°)；

43、在點云集pz中，將處于邊緣角度范圍區(qū)域(d°，a°)的點分割成第一邊緣特征點并組合形成子點云集f1，將處于邊緣角度范圍區(qū)域(a°，b°)的點分割成第二邊緣特征點并組合形成子點云集f2，將處于邊緣角度范圍區(qū)域(b°，c°)的點分割成第三邊緣特征點并組合形成子點云集f3，將處于邊緣角度范圍區(qū)域(c°，d°)的點分割成第四邊緣特征點并組合形成子點云集f4；

44、以間距差δs作為位移向量，通過以下公式對進行坐標變換，得到變換后的虛擬點云集px：

45、

46、式中，為變換后的子點云集，β為點與激光雷達連接線的角度。

47、進一步地，

48、基于鏟斗碰撞實驗獲取不同(l+s)下回轉(zhuǎn)結(jié)構(gòu)的回轉(zhuǎn)角度極限值、機械臂的關節(jié)角度極限值、(l+s)極限值；

49、以虛擬點云集為基準，更新虛擬艙壁深度圖并重新推算鏟斗與虛擬艙壁的距離s，計算l+s值，并得到與(l+s)對應的回轉(zhuǎn)結(jié)構(gòu)的回轉(zhuǎn)角度極限值、機械臂的關節(jié)角度極限值，以構(gòu)建防撞約束：

50、γ>γ1andγ<γ2；

51、δ>δ1andδ<δ2；

52、其中，s0為鏟斗與艙壁的距離，γ1、γ2為與(l+s)對應的回轉(zhuǎn)角度極限值，γ為挖掘機的回轉(zhuǎn)角度，δ1、δ2為與(l+s)對應的機械臂的關節(jié)角度極限值，δ為機械臂的關節(jié)角度，l+s為回轉(zhuǎn)結(jié)構(gòu)到虛擬艙壁的距離。

53、為解決上述技術問題，本發(fā)明還進一步提供下述技術方案：一種基于三維點云的船艙內(nèi)挖掘機防撞定位系統(tǒng)，具備：

54、激光雷達，安裝在挖掘機頂部；

55、激光建圖模塊，與所述激光雷達通訊連接，所述激光建圖模塊接收不同幀的三維點云數(shù)據(jù)，對三維點云數(shù)據(jù)依次進行點云分割、特征抽取并結(jié)合不同幀的三維點云數(shù)據(jù)進行點云配準，設定過濾調(diào)節(jié)來濾除非地面、非壁面點云；

56、傳感器，安裝在挖掘機的回轉(zhuǎn)結(jié)構(gòu)、機械臂上，且用以獲取下回轉(zhuǎn)結(jié)構(gòu)的回轉(zhuǎn)角度、液壓缸的長度；

57、統(tǒng)計模塊，與所述傳感器通訊連接，所述統(tǒng)計模塊用于在進行鏟斗碰撞實驗中計算鏟斗與艙壁的最大間距、最小間距并統(tǒng)計不同間距下回轉(zhuǎn)結(jié)構(gòu)的回轉(zhuǎn)角度極限值、機械臂的關節(jié)角度極限值，計算間距差并以間距差作為位移向量構(gòu)建虛擬點云集；

58、更新模塊，與統(tǒng)計模塊通訊連接，所述更新模塊以虛擬點云集為基準更新虛擬艙壁深度圖；

59、控制模塊，與更新模塊、統(tǒng)計模塊通訊連接，所述更新模塊根據(jù)虛擬艙壁深度圖計算鏟斗與虛擬艙壁的最小間距，并根據(jù)不同間距下回轉(zhuǎn)結(jié)構(gòu)的回轉(zhuǎn)角度極限值、機械臂的關節(jié)角度極限值建立防撞約束。

60、本發(fā)明與現(xiàn)有技術相比較具有如下有益效果：

61、本發(fā)明中，利用激光雷達獲取三維點云數(shù)據(jù)，進行數(shù)據(jù)處理得到艙壁深度圖，結(jié)合挖掘機模型能夠推算出鏟斗與艙壁的距離，并進行鏟斗碰撞實驗，獲取鏟斗與艙壁的最大間距和最小間距，計算得出間距差，依據(jù)該間距差構(gòu)建虛擬點云集，并基于虛擬點云集創(chuàng)建回轉(zhuǎn)結(jié)構(gòu)以及機械臂的防撞約束，實現(xiàn)了對挖掘機本身定位信息的獲取，還能夠獲取機械臂、回轉(zhuǎn)結(jié)構(gòu)的活動狀態(tài)，并通過虛擬點云集限制機械臂、回轉(zhuǎn)結(jié)構(gòu)的運動，精確獲取挖掘機整體狀態(tài)，有效保證了挖掘機整機的防撞。