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

有效

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

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

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

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

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

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

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

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

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

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

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

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

W為一個2×2矩陣，對其進行SVD分解，可得式(8)：

W＝UΣV^T (8)

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

當W滿秩時，表示為式(9)：

若計算得到的行列式為負，則取為當前迭代最優(yōu)值；

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

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

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

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

式中，X_(k,i)表示第k幀點云數(shù)據(jù)P_k中的第i個點云在雷達坐標系中的坐標，X_(k,j)表示P_k中的第j個點云在雷達坐標系中的坐標，S表示X_(k,i)的鄰域。