[發(fā)明專利]一種激光點云與地圖的匹配評價方法、介質(zhì)、終端和裝置

有效

• 申請?zhí)枺?/td>: 202010425583.8
• 申請日: 2020-05-19
• 公開（公告）號: CN111707279B
• 公開（公告）日: 2023-09-08
• 發(fā)明（設(shè)計）人: 李國飛
• 申請（專利權(quán)）人: 上海有個機器人有限公司
• 主分類號: G01C21/30
• 分類號: G01C21/30;G01C21/32
• 代理公司: 北京天盾知識產(chǎn)權(quán)代理有限公司 11421
• 代理人: 梁秀秀

本發(fā)明公開一種激光點云與地圖的匹配評價方法、介質(zhì)、終端和裝置，方法包括以下步驟：根據(jù)激光點云在當(dāng)前時刻的先驗位姿將激光點云在環(huán)境地圖上柵格化；獲取激光點云中每個激光點在所述環(huán)境地圖上分別對應(yīng)的目標(biāo)障礙點，并采用預(yù)設(shè)公式計算激光點與對應(yīng)目標(biāo)障礙點的距離；采用預(yù)設(shè)公式計算每個激光點的匹配得分；對激光點云中所有激光點的匹配得分進行求和，生成激光點云與環(huán)境地圖的匹配總分。本發(fā)明不僅保證了運算速度，同時提高了激光點云與柵格地圖匹配精度，增強了定位的容錯性與偏差修正能力，在機器人定位導(dǎo)航中具有重要意義。

【技術(shù)領(lǐng)域】

本發(fā)明涉及導(dǎo)航定位領(lǐng)域，尤其涉及一種激光點云與地圖的匹配評價方法、介質(zhì)、終端和裝置。

【背景技術(shù)】

激光slam(simultaneous?localization?and?mapping)是移動機器人定位導(dǎo)航的核心技術(shù)，全稱為同時定位與建圖。當(dāng)機器人處于未知環(huán)境中的未知位置時，基于自身攜帶的傳感器通過激光slam技術(shù)可以實現(xiàn)機器人邊移動邊估計出機器人實時位姿，然后不斷擴展與更新地圖，逐漸構(gòu)建環(huán)境的完整地圖。目前激光slam的兩大主流的方向是粒子濾波與圖優(yōu)化的方法，兩種方法都涉及點云與柵格地圖的匹配，是激光slam的核心組成部分。匹配的精度對定位建圖精度有較大的影響，同時匹配的速度影響了定位建圖的實時性，進一步影響機器人導(dǎo)航控制的響應(yīng)速度與平滑性。因此提高點云與地圖的匹配精度與速度是機器人高精度高效運行的關(guān)鍵技術(shù)。

點云與地圖的匹配評價標(biāo)準(zhǔn)通常采用殘差或者轉(zhuǎn)化為得分的方式。首先根據(jù)一些搜索方法確定點云在柵格地圖中最近障礙點，計算點云與最近障礙點的距離稱為殘差，再通過指數(shù)函數(shù)得到每個點與地圖匹配的得分，將所有點與地圖匹配得分求和得到當(dāng)前激光與地圖匹配的總得分。該評分函數(shù)對最近點距離的梯度比較小，在距離變化較大的情況下，得分變化不明顯，不能精確評價點云與地圖匹配的效果，因此在匹配過程中導(dǎo)致匹配精度不高，容易陷入局部最優(yōu)值，尤其在初始位姿誤差較大情況下，難以進行位姿的準(zhǔn)確實時修正。

【發(fā)明內(nèi)容】

本發(fā)明提供了一種激光點云與地圖的匹配評價方法、介質(zhì)、終端和裝置，解決了以上所述的技術(shù)問題。

本發(fā)明解決上述技術(shù)問題的技術(shù)方案如下：一種激光點云與地圖的匹配評價方法，包括以下步驟：

步驟1，根據(jù)激光點云在當(dāng)前時刻的先驗位姿將所述激光點云在環(huán)境地圖上柵格化；

步驟2，獲取所述激光點云中每個激光點在所述環(huán)境地圖上分別對應(yīng)的目標(biāo)障礙點，并計算所述激光點與對應(yīng)目標(biāo)障礙點的距離d_min；

步驟3，計算每個激光點的匹配得分；

步驟4，對所述激光點云中所有激光點的匹配得分進行求和，生成所述激光點云與所述環(huán)境地圖的匹配總分。

在一個優(yōu)選實施方式中，所述獲取所述激光點云中每個激光點在所述環(huán)境地圖上分別對應(yīng)的目標(biāo)障礙點具體包括以下步驟：

S201，獲取所述激光點云中每個激光點在所述環(huán)境地圖上的最近點圓，所述最近點圓的圓心為所述激光點所在柵格的中心，半徑為預(yù)設(shè)值；

S202，獲取每個激光點在預(yù)設(shè)搜索范圍內(nèi)的所有障礙點，將處于對應(yīng)的最近點圓范圍內(nèi)的障礙點作為候選障礙點，并保存所述候選障礙點的世界坐標(biāo)形成所述激光點對應(yīng)的最近點集；

S203，將所述最近點集中所有候選障礙點的均值點作為所述目標(biāo)障礙點。

在一個優(yōu)選實施方式中，當(dāng)所述最近點集中包括一個候選障礙點時，所述候選障礙點即為均值點；當(dāng)最近點集中包括兩個以上候選障礙點時，根據(jù)最近點集中每個候選障礙點的權(quán)重計算所有候選障礙點的均值點，具體包括以下步驟：

S2031，計算所述激光點與每個候選障礙點的歐拉距離，并將所述歐拉距離的倒數(shù)作為每個候選障礙點的權(quán)重；