基于概率匹配的柵格地圖拼接算法

郝 奇，陳智君，鄭 亮，陳 雙，曹雛清

(1.哈爾濱工業大學蕪湖機器人產業技術研究院，蕪湖 241000；2.蕪湖哈特機器人產業技術研究院有限公司，蕪湖 241000)

0 引言

SLAM是現如今主流的地圖構建方法，可以得到占據概率柵格地圖，用于AGV導航，而柵格地圖拼接是柵格地圖擴展和多地圖合并成大地圖的一項關鍵技術。現有的地圖拼接方法主要是基于圖像的特征提取和匹配，如SIFT、SURF、ORB等，這些算法在彩色圖像中的匹配效果很好，但是應用在柵格地圖中的可靠性卻很差，主要有兩個方面原因：一方面是地圖有很多局部相似的場景；另一方面是SLAM地圖是基于一定區域范圍的數據更新，使得很多柵格的概率和梯度值相同，最終導致柵格地圖的特征和描述都非常相似[1-3]。

針對以上問題，本文提出了一個自適應分支定界匹配和雙向概率殘差優化的柵格地圖拼接算法，并使用SLAM建立的柵格地圖進行了拼接測試。

1 柵格地圖匹配算法流程概述

算法流程如圖1所示，首先讀取源地圖和目標地圖，將地圖像素的灰度值轉化為占據概率，以概率閾值判別并提取占據點，得到源點云和目標點云；其次將目標點云和源地圖進行自適應分支定界匹配，獲取目標點云在源地圖中的初始位姿；之后將目標點云通過初始位姿投影到源地圖，構建概率殘差的第一個約束，再將源點云通過初始位姿的逆變換投影到目標地圖，構建概率殘差的第二個約束；然后通過殘差約束構建目標代價函數，進行非線性優化得到目標地圖在源地圖中的高精度位姿；……