[發(fā)明專利]融合NDT和ICP的柵格地圖定位方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202011548133.4
• 申請(qǐng)日: 2020-12-24
• 公開（公告）號(hào): CN112612029A
• 公開（公告）日: 2021-04-06
• 發(fā)明（設(shè)計(jì)）人: 伍永健;陳智君;郝奇
• 申請(qǐng)（專利權(quán)）人: 哈爾濱工業(yè)大學(xué)蕪湖機(jī)器人產(chǎn)業(yè)技術(shù)研究院
• 主分類號(hào): G01S17/06
• 分類號(hào): G01S17/06;G01S17/88;G01S17/89;G01S7/48;G01C21/00;G06F16/29
• 代理公司: 蕪湖安匯知識(shí)產(chǎn)權(quán)代理有限公司 34107
• 代理人: 鐘雪

1.一種融合NDT和ICP的柵格地圖定位方法，其特征在于，所述方法具體包括如下步驟：

S1、針對(duì)上一幀激光點(diǎn)云和當(dāng)前激光幀點(diǎn)云，采用NDT算法獲取激光雷達(dá)的初始位姿；

S2、基于初始位姿進(jìn)行當(dāng)前激光幀點(diǎn)云與上一幀激光點(diǎn)云的匹配，獲取激光雷達(dá)當(dāng)前的位姿。

2.如權(quán)利要求1所述融合NDT和ICP的柵格地圖定位方法，其特征在于，所述步驟S1具體包括如下步驟：

S11、初始化位姿變換參數(shù)ξ，根據(jù)初始位姿變換參數(shù)ξ將當(dāng)前幀的激光點(diǎn)云轉(zhuǎn)換到上一幀激光點(diǎn)云坐標(biāo)系下，計(jì)算當(dāng)前幀激光點(diǎn)云轉(zhuǎn)換到上一幀激光點(diǎn)云坐標(biāo)系對(duì)應(yīng)的概率值，如公式(2)、(3)所示：

x′_i＝ξ·x_i (2)

p表示當(dāng)前幀激光點(diǎn)云轉(zhuǎn)換到上一幀激光點(diǎn)云坐標(biāo)系對(duì)應(yīng)的概率值，x_i表示當(dāng)前幀激光點(diǎn)云中第i個(gè)點(diǎn)在雷達(dá)坐標(biāo)系中的坐標(biāo)，x_i′表示當(dāng)前幀激光點(diǎn)云中第i個(gè)點(diǎn)轉(zhuǎn)換到上一幀激光點(diǎn)云坐標(biāo)下的坐標(biāo)；

S22、用牛頓法優(yōu)化參數(shù)ξ，獲取p得分最高的位姿變換參數(shù)ξ，將該位姿變換參數(shù)對(duì)應(yīng)的位姿作為初始位姿。

3.如權(quán)利要求1所述融合NDT和ICP的柵格地圖定位方法，其特征在于，所述步驟S2具體包括如下步驟：

S21、基于初始矩陣對(duì)當(dāng)前幀激光點(diǎn)云進(jìn)行變換，得到變換后的激光點(diǎn)云II，然后將激光點(diǎn)云II與上一幀激光點(diǎn)云進(jìn)行比較，在上一幀激光點(diǎn)云中查找距激光點(diǎn)云II中各點(diǎn)最近的鄰近點(diǎn)；

S22、構(gòu)建目標(biāo)函數(shù)E(R,t)，求解最優(yōu)的旋轉(zhuǎn)平移矩陣：

l_i表示當(dāng)前幀激光點(diǎn)云中第i個(gè)點(diǎn)的坐標(biāo)，用n_i表示基于初始位姿將當(dāng)前幀激光點(diǎn)云中第i個(gè)點(diǎn)變換至上一幀激光點(diǎn)云坐標(biāo)系中的坐標(biāo)，m_i表示上一幀激光點(diǎn)云中距點(diǎn)n_i最近點(diǎn)的坐標(biāo)，即點(diǎn)n_i在上一幀激光點(diǎn)云中的鄰近點(diǎn)坐標(biāo)；

S23、回到步驟S21中的尋找最鄰近點(diǎn)方法，進(jìn)行迭代，直到滿足一些迭代終止條件，輸出激光雷達(dá)當(dāng)前的最優(yōu)位姿，實(shí)現(xiàn)連續(xù)定位。

4.如權(quán)利要求1融合NDT和ICP的柵格地圖定位方法，其特征在于，在步驟S1之前還包括激光雷達(dá)的重定位，重定位方法具體如下：

S3、對(duì)當(dāng)前幀激光點(diǎn)云和地圖點(diǎn)云，采用NDT算法獲取激光雷達(dá)的重定位初始位姿；

S4、基于重定位初始位姿進(jìn)行當(dāng)前激光幀點(diǎn)云與地圖點(diǎn)云的匹配，獲取激光雷達(dá)當(dāng)前重定位的最優(yōu)位姿。

5.如權(quán)利要求4所述融合NDT和ICP的柵格地圖定位方法，其特征在于，所述步驟S3具體包括如下步驟：

S31、將點(diǎn)云地圖劃分大小相同的單元格，統(tǒng)計(jì)每個(gè)單元格內(nèi)所有柵格點(diǎn)的坐標(biāo)均值和坐標(biāo)方差；

S32、初始化位姿變換參數(shù)ξ，根據(jù)初始位姿變換參數(shù)ξ將激光點(diǎn)云轉(zhuǎn)換到地圖點(diǎn)云坐標(biāo)系下，計(jì)算當(dāng)前幀激光點(diǎn)云轉(zhuǎn)換到地圖點(diǎn)云坐標(biāo)系對(duì)應(yīng)的概率值，如公式(2)、(3)所示：

x′_i＝ξ·x_i (2)

p表示當(dāng)前幀激光點(diǎn)云轉(zhuǎn)換到地圖點(diǎn)云坐標(biāo)系對(duì)應(yīng)的概率值，x_i表示當(dāng)前幀激光點(diǎn)云中第i個(gè)點(diǎn)在雷達(dá)坐標(biāo)系中的坐標(biāo)，x_i′表示當(dāng)前幀激光點(diǎn)云中第i個(gè)點(diǎn)轉(zhuǎn)換到點(diǎn)云地圖坐標(biāo)下的坐標(biāo)。

S33、用牛頓法優(yōu)化參數(shù)ξ，獲取p得分最高的位姿變換參數(shù)ξ，將該位姿變換參數(shù)對(duì)應(yīng)的位姿作為重定位的初始位姿。

6.如權(quán)利要求4所述融合NDT和ICP的柵格地圖定位方法，其特征在于，所述步驟S4具體包括如下步驟：

S41、基于初始矩陣對(duì)當(dāng)前幀激光點(diǎn)云進(jìn)行變換，得到變換后的激光點(diǎn)云Ⅰ，然后將激光點(diǎn)云Ⅰ與地圖點(diǎn)云進(jìn)行比較，在點(diǎn)云地圖中查找距激光點(diǎn)云Ⅰ中各點(diǎn)最近的鄰近點(diǎn)；

S42、構(gòu)建目標(biāo)函數(shù)E(R,t)，求解最優(yōu)的旋轉(zhuǎn)平移矩陣：

l_i表示當(dāng)前幀激光點(diǎn)云中第i個(gè)點(diǎn)的坐標(biāo)，用n_i表示基于初始位姿將當(dāng)前幀激光點(diǎn)云中第i個(gè)點(diǎn)變換至地圖點(diǎn)云中的坐標(biāo)，則m_i表示地圖點(diǎn)云中距點(diǎn)n_i最近點(diǎn)的坐標(biāo)，即點(diǎn)n_i在地圖點(diǎn)云中的鄰近點(diǎn)坐標(biāo)；

S44、回到步驟S41中的尋找最鄰近點(diǎn)方法，進(jìn)行迭代，直到滿足設(shè)定的迭代終止條件，獲得重定位的最優(yōu)位姿。