[發(fā)明專利]考慮輪式機(jī)器人位姿估計(jì)的激光雷達(dá)動(dòng)態(tài)障礙物檢測(cè)方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202110601426.2
• 申請(qǐng)日: 2021-05-31
• 公開(kāi)（公告）號(hào): CN113345008B
• 公開(kāi)（公告）日: 2022-05-24
• 發(fā)明（設(shè)計(jì)）人: 秦曉輝;蘆濤;邊有鋼;徐彪;謝國(guó)濤;秦兆博;胡滿江;王曉偉;秦洪懋;丁榮軍
• 申請(qǐng)（專利權(quán)）人: 湖南大學(xué)
• 主分類號(hào): G06T7/70
• 分類號(hào): G06T7/70;G06T7/66;B25J9/16
• 代理公司: 北京匯智勝知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 11346
• 代理人: 石輝;趙立軍

1.一種考慮輪式機(jī)器人位姿估計(jì)的激光雷達(dá)動(dòng)態(tài)障礙物檢測(cè)方法，其特征在于，包括：

步驟1，通過(guò)激光點(diǎn)云數(shù)據(jù)的幾何特性，提取符合預(yù)設(shè)曲率特征的點(diǎn)云作為特征點(diǎn)，建立同一特征點(diǎn)在兩幀相鄰時(shí)刻點(diǎn)云數(shù)據(jù)中的匹配關(guān)系，并構(gòu)建代價(jià)函數(shù)，以輪式機(jī)器人位姿為變量構(gòu)造ICP問(wèn)題，獲得輪式機(jī)器人位姿信息；

步驟2，根據(jù)步驟1獲得的輪式機(jī)器人的位姿信息，檢測(cè)候選動(dòng)態(tài)障礙物；

步驟3，根據(jù)步驟2檢測(cè)得到的候選動(dòng)態(tài)障礙物，估計(jì)動(dòng)態(tài)障礙物狀態(tài)；

步驟1中，構(gòu)建代價(jià)函數(shù)，以輪式機(jī)器人位姿為變量構(gòu)造ICP問(wèn)題，獲得輪式機(jī)器人位姿信息的方法具體包括：

步驟b1，考慮其水平方向平移及偏航方向旋轉(zhuǎn)對(duì)應(yīng)的二維變換矩陣根據(jù)對(duì)應(yīng)點(diǎn)云的距離關(guān)系，建立代價(jià)函數(shù)，將位姿解算問(wèn)題轉(zhuǎn)化成一個(gè)非線性最小二乘的求解問(wèn)題，如下式(5)：

其中，表示最優(yōu)變換矩陣估計(jì)值，表示P_k中第p個(gè)特征點(diǎn)在雷達(dá)坐標(biāo)系中去除z軸維度后的坐標(biāo)，q表示特征點(diǎn)的數(shù)量，為在P_k-1中的去除z軸維度后的齊次坐標(biāo)，表示在P_k中的去除z軸維度后的齊次坐標(biāo)；

步驟b2，定義第k幀特征點(diǎn)的質(zhì)心在雷達(dá)坐標(biāo)系中去除z軸維度后的坐標(biāo)以及第k-1幀特征點(diǎn)的質(zhì)心在雷達(dá)坐標(biāo)系中去除z軸維度后的坐標(biāo)

步驟b3，將去質(zhì)心坐標(biāo)定義為將上述問(wèn)題(5)解耦，轉(zhuǎn)化為式(6)表示的問(wèn)題：

式中，表示最優(yōu)的輪式機(jī)器人偏航方向旋轉(zhuǎn)矩陣估計(jì)值，表示單次迭代的旋轉(zhuǎn)矩陣，表示最優(yōu)的水平方向平移估計(jì)值；

步驟b4，通過(guò)SVD分解求出上述問(wèn)題(6)中最優(yōu)定義矩陣W為式(7)：

W為一個(gè)2×2矩陣，對(duì)其進(jìn)行SVD分解，可得式(8)：

W＝UΣV^T (8)

其中，Σ為奇異值對(duì)角矩陣，U和V為正交矩陣；

當(dāng)W滿秩時(shí)，表示為式(9)：

若計(jì)算得到的行列式為負(fù)，則取為當(dāng)前迭代最優(yōu)值；

步驟b5，按照得到輪式機(jī)器人在x方向和y方向上的平移量。

2.如權(quán)利要求1所述的考慮輪式機(jī)器人位姿估計(jì)的激光雷達(dá)動(dòng)態(tài)障礙物檢測(cè)方法，其特征在于，步驟1中，所述提取符合預(yù)設(shè)曲率特征的點(diǎn)云作為特征點(diǎn)的方法具體包括：

步驟a1，對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行體素濾波；

步驟b1，利用下式(1)計(jì)算點(diǎn)云數(shù)據(jù)中各點(diǎn)云的曲率值r，并按照曲率值r的大小排序，曲率值最大的點(diǎn)云定義為特征角點(diǎn)，曲率值最小的點(diǎn)云定義為特征平面點(diǎn)：

式中，X_(k,i)表示第k幀點(diǎn)云數(shù)據(jù)P_k中的第i個(gè)點(diǎn)云在雷達(dá)坐標(biāo)系中的坐標(biāo)，X_(k,j)表示P_k中的第j個(gè)點(diǎn)云在雷達(dá)坐標(biāo)系中的坐標(biāo)，S表示X_(k,i)的鄰域。