[發明專利]考慮輪式機器人位姿估計的激光雷達動態障礙物檢測方法有效
| 申請號： | 202110601426.2 | 申請日： | 2021-05-31 |
| 公開（公告）號： | CN113345008B | 公開（公告）日： | 2022-05-24 |
| 發明（設計）人： | 秦曉輝;蘆濤;邊有鋼;徐彪;謝國濤;秦兆博;胡滿江;王曉偉;秦洪懋;丁榮軍 | 申請（專利權）人： | 湖南大學 |
| 主分類號： | G06T7/70 | 分類號： | G06T7/70;G06T7/66;B25J9/16 |
| 代理公司： | 北京匯智勝知識產權代理事務所(普通合伙) 11346 | 代理人： | 石輝;趙立軍 |
| 地址： | 410082 湖*** | 國省代碼： | 湖南;43 |
| 權利要求書： | 查看更多 | 說明書： | 查看更多 |
| 摘要： | |||
| 搜索關鍵詞： | 考慮 輪式 機器人 估計 激光雷達 動態 障礙物 檢測 方法 | ||
1.一種考慮輪式機器人位姿估計的激光雷達動態障礙物檢測方法，其特征在于，包括：
步驟1，通過激光點云數據的幾何特性，提取符合預設曲率特征的點云作為特征點，建立同一特征點在兩幀相鄰時刻點云數據中的匹配關系，并構建代價函數，以輪式機器人位姿為變量構造ICP問題，獲得輪式機器人位姿信息；
步驟2，根據步驟1獲得的輪式機器人的位姿信息，檢測候選動態障礙物；
步驟3，根據步驟2檢測得到的候選動態障礙物，估計動態障礙物狀態；
步驟1中，構建代價函數，以輪式機器人位姿為變量構造ICP問題，獲得輪式機器人位姿信息的方法具體包括：
步驟b1，考慮其水平方向平移及偏航方向旋轉對應的二維變換矩陣根據對應點云的距離關系，建立代價函數，將位姿解算問題轉化成一個非線性最小二乘的求解問題，如下式(5)：
其中，表示最優變換矩陣估計值，表示Pk中第p個特征點在雷達坐標系中去除z軸維度后的坐標，q表示特征點的數量，為在Pk-1中的去除z軸維度后的齊次坐標，表示在Pk中的去除z軸維度后的齊次坐標；
步驟b2，定義第k幀特征點的質心在雷達坐標系中去除z軸維度后的坐標以及第k-1幀特征點的質心在雷達坐標系中去除z軸維度后的坐標
步驟b3，將去質心坐標定義為將上述問題(5)解耦，轉化為式(6)表示的問題：
式中，表示最優的輪式機器人偏航方向旋轉矩陣估計值，表示單次迭代的旋轉矩陣，表示最優的水平方向平移估計值；
步驟b4，通過SVD分解求出上述問題(6)中最優定義矩陣W為式(7)：
W為一個2×2矩陣，對其進行SVD分解，可得式(8)：
W＝UΣVT (8)
其中，Σ為奇異值對角矩陣，U和V為正交矩陣；
當W滿秩時，表示為式(9)：
若計算得到的行列式為負，則取為當前迭代最優值；
步驟b5，按照得到輪式機器人在x方向和y方向上的平移量。
2.如權利要求1所述的考慮輪式機器人位姿估計的激光雷達動態障礙物檢測方法，其特征在于，步驟1中，所述提取符合預設曲率特征的點云作為特征點的方法具體包括：
步驟a1，對點云數據進行體素濾波；
步驟b1，利用下式(1)計算點云數據中各點云的曲率值r，并按照曲率值r的大小排序，曲率值最大的點云定義為特征角點，曲率值最小的點云定義為特征平面點：
式中，X(k,i)表示第k幀點云數據Pk中的第i個點云在雷達坐標系中的坐標，X(k,j)表示Pk中的第j個點云在雷達坐標系中的坐標，S表示X(k,i)的鄰域。
該專利技術資料僅供研究查看技術是否侵權等信息，商用須獲得專利權人授權。該專利全部權利屬于湖南大學，未經湖南大學許可，擅自商用是侵權行為。如果您想購買此專利、獲得商業授權和技術合作，請聯系【客服】
本文鏈接：http://www.szxzyx.cn/pat/books/202110601426.2/1.html，轉載請聲明來源鉆瓜專利網。
