[發(fā)明專利]基于LEGO-LOAM的可調(diào)式大角度探測(cè)感知及控制方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202110822519.8
• 申請(qǐng)日: 2021-07-21
• 公開（公告）號(hào): CN113487631A
• 公開（公告）日: 2021-10-08
• 發(fā)明（設(shè)計(jì)）人: 索旭東;管西強(qiáng);劉宇飛;彭為強(qiáng);李冀川;趙聯(lián)彬;梁家輝;危義坤;朱佳鵬
• 申請(qǐng)（專利權(quán)）人: 智能移動(dòng)機(jī)器人（中山）研究院
• 主分類號(hào): G06T7/11
• 分類號(hào): G06T7/11;G06T7/12;G06T7/13;G06T7/33;G06T7/73;G06K9/62;G01S17/86;G01S17/89
• 代理公司: 深圳市興科達(dá)知識(shí)產(chǎn)權(quán)代理有限公司 44260
• 代理人: 徐民奎

1.一種基于LEGO-LOAM的可調(diào)式大角度探測(cè)感知及控制方法，其特征在于，該方法包括如下步驟：

步驟一、數(shù)據(jù)讀取，讀取多線激光雷達(dá)掃描的點(diǎn)云數(shù)據(jù)和IMU慣性測(cè)量單元獲取的位姿數(shù)據(jù)；

步驟二、點(diǎn)云分割，對(duì)讀取的點(diǎn)云數(shù)據(jù)和位姿數(shù)據(jù)先進(jìn)行地面提取，然后對(duì)剩下的點(diǎn)云進(jìn)行分割，將分割好的三維的點(diǎn)云數(shù)據(jù)進(jìn)行平面投射，投影得到二維深度圖，在二維深度圖上進(jìn)行地面提取，得到地面點(diǎn)云和非地面點(diǎn)云，然后對(duì)非地面點(diǎn)云進(jìn)行聚類，得到聚類簇，然后在聚類簇中提取曲率較大的特征點(diǎn)，將點(diǎn)云分割為邊緣點(diǎn)類和平面點(diǎn)類；

步驟三、特征提取，在地面點(diǎn)云上提取平面特征，而在聚類簇上提取線特征；

步驟四、雷達(dá)里程計(jì)，通過點(diǎn)對(duì)線和點(diǎn)對(duì)面進(jìn)行相鄰兩幀的匹配來(lái)估計(jì)激光雷達(dá)的姿態(tài)變化，得到高頻率的里程計(jì)信息；

步驟五、雷達(dá)建圖，根據(jù)雷達(dá)里程計(jì)信息，繼續(xù)用線面特征進(jìn)行匹配，將特征點(diǎn)與周圍點(diǎn)云圖進(jìn)行配準(zhǔn)，采用最小二乘化方法得到精準(zhǔn)的低頻率的位姿信息；

步驟六、變換融合，根據(jù)高頻率的雷達(dá)里程計(jì)信息和低頻率的精準(zhǔn)位姿信息進(jìn)行融合，輸出一個(gè)最終的高頻位姿估計(jì)信息。

2.如權(quán)利要求1所述的基于LEGO-LOAM的可調(diào)式大角度探測(cè)感知及控制方法，其特征在于，在步驟一中，所述多線激光雷達(dá)為16線激光雷達(dá)。

3.如權(quán)利要求1所述的基于LEGO-LOAM的可調(diào)式大角度探測(cè)感知及控制方法，其特征在于，在步驟一中，所述位姿信息在ROS下通過話題的方式進(jìn)行發(fā)布。

4.如權(quán)利要求1所述的基于LEGO-LOAM的可調(diào)式大角度探測(cè)感知及控制方法，其特征在于，在步驟三中，特征提取具體包括如下步驟：先在水平方向上將深度圖均分成許多子圖像，然后計(jì)算每個(gè)點(diǎn)的曲率，然后在每個(gè)子圖像的每一行根據(jù)曲率的大小選擇若干個(gè)邊緣點(diǎn)和若干個(gè)平面點(diǎn)，其中，粗糙度較大的是邊緣點(diǎn)，粗糙度較小的是平面點(diǎn)。

5.如權(quán)利要求1所述的基于LEGO-LOAM的可調(diào)式大角度探測(cè)感知及控制方法，其特征在于，在步驟四中，在匹配的過程中采用兩步L-M優(yōu)化，提取相鄰幀的平面特征進(jìn)行匹配，并用來(lái)計(jì)算豎直維度的變動(dòng)，通過線特征匹配獲取水平維度的變動(dòng)。

6.如權(quán)利要求1所述的基于LEGO-LOAM的可調(diào)式大角度探測(cè)感知及控制方法，其特征在于，在步驟五中，所述位姿信息作為節(jié)點(diǎn)加到位姿圖里面，將提取的特征點(diǎn)作為約束邊，用GTSAM進(jìn)行整體的平滑優(yōu)化；通過空間距離、時(shí)間限制、里程距離約束和ICP來(lái)檢測(cè)回環(huán)，檢測(cè)到了回環(huán)，則計(jì)算回環(huán)幀與當(dāng)前幀的相對(duì)位姿，作為約束邊繼續(xù)加入到位姿圖中，來(lái)減小漂移。