[發(fā)明專(zhuān)利]一種基于雙激光雷達(dá)數(shù)據(jù)融合的地圖構(gòu)建方法、系統(tǒng)及地圖

在審

• 申請(qǐng)?zhí)枺?/td>: 201910708030.0
• 申請(qǐng)日: 2019-08-01
• 公開(kāi)（公告）號(hào): CN110658530A
• 公開(kāi)（公告）日: 2020-01-07
• 發(fā)明（設(shè)計(jì)）人: 張軍;劉元盛;王慶閃;程志明;朱海東;鐘昊
• 申請(qǐng)（專(zhuān)利權(quán)）人: 北京聯(lián)合大學(xué)
• 主分類(lèi)號(hào): G01S17/89
• 分類(lèi)號(hào): G01S17/89
• 代理公司: 11367 北京馳納智財(cái)知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙)
• 代理人: 謝亮

1.一種基于雙激光雷達(dá)數(shù)據(jù)融合的地圖構(gòu)建方法，包括構(gòu)建點(diǎn)云地圖，其特征在于，還包括以下步驟：

步驟1：對(duì)雙激光雷達(dá)進(jìn)行標(biāo)定和數(shù)據(jù)融合；

步驟2：采用快速空間模型點(diǎn)云配準(zhǔn)算法進(jìn)行幀間點(diǎn)云粗配準(zhǔn)；

步驟3：采用最近鄰迭代進(jìn)行位姿的矯正，得到最優(yōu)的幀間變換，實(shí)現(xiàn)幀間點(diǎn)云精準(zhǔn)的拼接。

2.如權(quán)利要求1所述的基于雙激光雷達(dá)數(shù)據(jù)融合的地圖構(gòu)建方法，其特征在于，所述雙激光雷達(dá)標(biāo)定是指對(duì)不同位置的兩個(gè)激光雷達(dá)進(jìn)行標(biāo)定，包括空間同步和/或時(shí)間同步。

3.如權(quán)利要求2所述的基于雙激光雷達(dá)數(shù)據(jù)融合的地圖構(gòu)建方法，其特征在于，所述空間同步是指進(jìn)行物理標(biāo)定，以其中的一個(gè)激光雷達(dá)為中心，另一個(gè)激光雷達(dá)調(diào)整角度值和方向值進(jìn)行標(biāo)定，以掃描同一個(gè)障礙物進(jìn)行空間標(biāo)定。

4.如權(quán)利要求3所述的基于雙激光雷達(dá)數(shù)據(jù)融合的地圖構(gòu)建方法，其特征在于，所述空間標(biāo)定就是利用三維坐標(biāo)系的旋轉(zhuǎn)矩陣及平移矩陣，三維坐標(biāo)系的旋轉(zhuǎn)表述為向量與合適尺寸的方陣的乘積。

5.如權(quán)利要求4所述的基于雙激光雷達(dá)數(shù)據(jù)融合的地圖構(gòu)建方法，其特征在于，設(shè)定所述三維坐標(biāo)系中的一個(gè)向量其中點(diǎn)P在XY平面、XZ平面、YZ平面的投影分別為點(diǎn)M、點(diǎn)Q、點(diǎn)N，繞Z軸旋轉(zhuǎn)θ角，旋轉(zhuǎn)矩陣為繞X軸旋轉(zhuǎn)θ角，旋轉(zhuǎn)矩陣為：繞Y軸旋轉(zhuǎn)θ角，旋轉(zhuǎn)矩陣為：在Z、X、Y軸的平移矩陣為：

6.如權(quán)利要求5所述的基于雙激光雷達(dá)數(shù)據(jù)融合的地圖構(gòu)建方法，其特征在于，所述步驟2包括以下子步驟：

步驟21：設(shè)相鄰幀間點(diǎn)云集為S和W，計(jì)算S點(diǎn)云集的正態(tài)分布；

步驟22：初始化坐標(biāo)變換參數(shù)P；

步驟23：將W點(diǎn)云集根據(jù)坐標(biāo)轉(zhuǎn)換參數(shù)映射到與S同一坐標(biāo)系中，并得到映射后的點(diǎn)云集合W’；

步驟24：求每個(gè)點(diǎn)映射變換后的正態(tài)分布；

步驟25：將每個(gè)點(diǎn)的概率密度相加，評(píng)估坐標(biāo)的變換參數(shù)；

步驟26：使用Hessian矩陣優(yōu)化每個(gè)點(diǎn)云的概率密度之和s(p)；

步驟27：判斷是否滿足收斂條件，如果不滿足則重新執(zhí)行步驟23至26，如果滿足收斂條件，則完成點(diǎn)云粗配準(zhǔn)。

7.如權(quán)利要求6所述的基于雙激光雷達(dá)數(shù)據(jù)融合的地圖構(gòu)建方法，其特征在于，所述坐標(biāo)變換參數(shù)P的計(jì)算公式為：其中，P為包括旋轉(zhuǎn)矩陣和平移矩陣的4*4矩陣，R是3*3的旋轉(zhuǎn)矩陣，t是3*1的平移矩陣。

8.如權(quán)利要求7所述的基于雙激光雷達(dá)數(shù)據(jù)融合的地圖構(gòu)建方法，其特征在于，所述正態(tài)分布表示為：其中，q是每個(gè)立方體內(nèi)點(diǎn)的均值，∑是每個(gè)立方體內(nèi)點(diǎn)的協(xié)方差矩陣。

9.一種基于雙激光雷達(dá)數(shù)據(jù)融合的地圖構(gòu)建系統(tǒng)，包括用于構(gòu)建的點(diǎn)云地圖的地圖構(gòu)建模塊，其特征在于，還包括以下模塊：

雙雷達(dá)標(biāo)定模塊：用于對(duì)雙激光雷達(dá)進(jìn)行標(biāo)定和數(shù)據(jù)融合；

粗配準(zhǔn)模塊：用于采用快速空間模型點(diǎn)云配準(zhǔn)算法進(jìn)行幀間點(diǎn)云粗配準(zhǔn)；

點(diǎn)云拼接模塊：用于采用最近鄰迭代進(jìn)行位姿的矯正，得到最優(yōu)的幀間變換，實(shí)現(xiàn)幀間點(diǎn)云精準(zhǔn)的拼接。

10.一種基于雙激光雷達(dá)數(shù)據(jù)融合的地圖，所述地圖采用如權(quán)利要求1所述的方法構(gòu)建。