[發明專利]礦區三維地圖構建系統和方法

在審

• 申請號: 202110920940.2
• 申請日: 2021-08-11
• 公開（公告）號: CN113706702A
• 公開（公告）日: 2021-11-26
• 發明（設計）人: 肖天中;張昊;李軍
• 申請（專利權）人: 重慶九洲星熠導航設備有限公司
• 主分類號: G06T17/05
• 分類號: G06T17/05;G06T17/20
• 代理公司: 佛山市海融科創知識產權代理事務所(普通合伙) 44377
• 代理人: 陳志超;許家裕

1.一種礦區三維地圖構建系統，其特征在于，包括無人機、礦車和多個路側感知端，所述無人機、礦車和路側感知端均包括激光雷達、差分GNSS導航設備、IMU慣性測量單元；

所述無人機用于通過對應的激光雷達、差分GNSS導航設備、IMU慣性測量單元獲取礦區全局的三維點云圖作為底圖，并發送至所述礦車；

所述路側感知端設置在礦區道路的路側，并用于通過對應的激光雷達、差分GNSS導航設備、IMU慣性測量單元獲取路側感知端所在區域的第一局部三維點云圖，并發送至所述礦車；

所述礦車用于通過對應的激光雷達、差分GNSS導航設備、IMU慣性測量單元獲取礦車所在區域的第二局部三維點云圖，把所述第一局部三維點云圖和所述第二局部三維點云圖進行融合，得到初步融合的局部三維點云圖，然后把所述初步融合的局部三維點云圖與所述底圖進行融合得到融合的全局三維點云圖。

2.根據權利要求1所述的礦區三維地圖構建系統，其特征在于，所述無人機、礦車和路側感知端在通過對應的激光雷達、差分GNSS導航設備、IMU慣性測量單元獲取對應的三維點云圖的時候：

用激光雷達獲取掃描點在激光雷達坐標系中的坐標；

根據以下公式計算所述掃描點在地心直角坐標系中的坐標：

；

其中，為掃描點在地心直角坐標系中的坐標，為對應的IMU慣性測量單元中心在地心直角坐標系中的坐標，為對應的IMU慣性測量單元坐標系至地心直角坐標系的旋轉矩陣，為所述激光雷達的激光雷達坐標系中心到所述IMU慣性測量單元中心的偏置在所述IMU慣性測量單元坐標系中的分量，為所述激光雷達坐標系到所述IMU慣性測量單元坐標系的旋轉矩陣，為所述掃描點在所述激光雷達坐標系中的坐標。

3.根據權利要求2所述的礦區三維地圖構建系統，其特征在于，所述礦車在把所述第一局部三維點云圖和所述第二局部三維點云圖進行融合的時候：

基于各點在地心直角坐標系中的坐標，對所述第一局部三維點云圖和所述第二局部三維點云圖進行拼接。

4.根據權利要求1所述的礦區三維地圖構建系統，其特征在于，所述礦車在把所述初步融合的局部三維點云圖與所述底圖進行融合得到融合的全局三維點云圖的時候：

采用基于ICP算法的點云匹配方式對所述初步融合的局部三維點云圖與所述底圖進行點云拼接。

5.根據權利要求4所述的礦區三維地圖構建系統，其特征在于，所述礦車在采用基于ICP算法的點云匹配方式對所述初步融合的局部三維點云圖與所述底圖進行點云拼接的時候，執行以下步驟：

C1.選定所述初步融合的局部三維點云圖和所述底圖的同名特征點，得到初步融合的局部三維點云圖對應的第一同名特征點集和底圖對應的第二同名特征點集；

C2.重復執行以下迭代過程，直到誤差小于預設的閾值：

使第一同名特征點集的中心點和所述第二同名特征點集的中心點重合，并計算重合操作中第一同名特征點集的中心點的平移向量；

針對第一同名特征點集中的每一點，在第二同名特征點集中搜索其空間最近鄰點，以建立對應關系，并計算最近點之間的旋轉矩陣；

根據以下公式計算總旋轉矩陣、旋轉向量和第n次迭代處理的旋轉后的第一同名特征點集：

；

其中， 為總旋轉矩陣， 為第i次迭代處理中計算得到的最近點之間的旋轉矩陣， 為旋轉向量， 為第n次迭代處理的旋轉后的第一同名特征點集，為初始的第一同名特征點集；

通過以下公式計算誤差 ：

；

其中， 為中的第i個點的位置，為第二同名特征點集中的第i個點的位置；

判斷所述誤差是否小于預設的閾值；

C3.根據以下公式對所述初步融合的局部三維點云圖的各點進行轉換處理，并用轉換處理后的初步融合的局部三維點云圖與所述底圖進行拼接：

；

其中，為轉換處理后的初步融合的局部三維點云圖中的i個點的位置，為轉換處理前的初步融合的局部三維點云圖中的i個點的位置。