2024年10月14—18日，智能機(jī)器人與系統(tǒng)國際會(huì)議（IROS 2024）在阿聯(lián)酋阿布扎比舉行。作為全球規(guī)模最大、最具影響力的機(jī)器人研究會(huì)議之一，IROS每年吸引來自世界各地的科研人員、學(xué)者以及行業(yè)專家，共同探討機(jī)器人與智能系統(tǒng)領(lǐng)域的最新進(jìn)展。
本次會(huì)議中，光鑒科技的研究成果 C3P-VoxelMap: Compact, Cumulative and Coalescible Probabilistic Voxel Mapping 憑借其創(chuàng)新性成功入選，并作為口頭報(bào)告（Oral Pitch）展示。該研究提出了在嵌入式平臺(tái)有限算力下實(shí)現(xiàn) real-time 3D SLAM 的解決方案，為移動(dòng)機(jī)器人自主導(dǎo)航提供了參考設(shè)計(jì)。
3D SLAM 作為機(jī)器人自主導(dǎo)航的核心技術(shù)之一，如何在有限的計(jì)算和存儲(chǔ)資源下實(shí)現(xiàn)高精度定位與建圖，具有重要的理論和現(xiàn)實(shí)意義。
VoxelMap是3D SLAM中廣泛使用的地圖結(jié)構(gòu)，通過將三維空間劃分成相同結(jié)構(gòu)的體素，并利用哈希表進(jìn)行索引，具有維護(hù)簡(jiǎn)單，訪存高效的優(yōu)勢(shì)。
然而，常規(guī)的VoxelMap在實(shí)際中存在局限，難以捕捉真實(shí)世界中的大尺度幾何特征，例如墻壁、地板和天花板，使得同一個(gè)物理平面需要大量體素重復(fù)表示。另一方面，固定大小的體素面臨 bias-variance trade-off 難題。以下圖為例，小體素能更好地捕捉地圖細(xì)節(jié)（bias小），但更容易受噪聲影響，尤其是在點(diǎn)云稀疏的情況下（variance大）; 相反，大體素可以收集更多觀測(cè)點(diǎn)來降低方差（variance小），但可能會(huì)過度平滑環(huán)境，導(dǎo)致表示偏差（bias大）。
除此之外，Probabilistic VoxelMap 需要存儲(chǔ)每個(gè)體素的歷史點(diǎn)云數(shù)據(jù)，并在新的觀測(cè)點(diǎn)加入時(shí)重新遍歷所有點(diǎn)，以更新體素平面的不確定性。這是由于計(jì)算平面的協(xié)方差矩陣時(shí)，需要計(jì)算法向量對(duì)逐個(gè)點(diǎn)求導(dǎo)的雅可比矩陣。其中，點(diǎn)p以及點(diǎn)p的協(xié)方差矩陣是和觀測(cè)有關(guān)的項(xiàng)（以下簡(jiǎn)稱觀測(cè)項(xiàng)），q, F, 和U是和平面點(diǎn)集總體分布有關(guān)的項(xiàng)（以下簡(jiǎn)稱分布項(xiàng)。q代表點(diǎn)集均值，F和 U是由 SVD 結(jié)果構(gòu)成的矩陣，詳細(xì)過程請(qǐng)參考論文）。當(dāng)新的觀測(cè)加入時(shí)，由于雅可比矩陣所包含的分布項(xiàng)會(huì)發(fā)生變化，需要將每個(gè)點(diǎn)再次代入公式，重新進(jìn)行求導(dǎo)。
這種遍歷消耗了大量?jī)?nèi)存和CPU資源。在一些資源受限的機(jī)器人平臺(tái)上可能導(dǎo)致定位延遲增加，或者沒有足夠的內(nèi)存來維護(hù)地圖，進(jìn)而影響下游的避障和路徑規(guī)劃等任務(wù)。
為了解決以上痛點(diǎn)，光鑒科技創(chuàng)新提出了C3P-VoxelMap，從計(jì)算效率、內(nèi)存效率、算法精度三方面對(duì) VoxelMap 進(jìn)行了提升。
為了避免重復(fù)求導(dǎo)，我們希望將觀測(cè)項(xiàng)和分布項(xiàng)解耦，使得二者能夠分別進(jìn)行增量更新。
利用矩陣計(jì)算的分配律將 U 矩陣分離出來比較容易，難點(diǎn)是如何從B 矩陣分離出q和F。考慮到B矩陣整體不容易分解，我們根據(jù)其表達(dá)式，從單個(gè)元素入手進(jìn)行考察。
由此可見，B 矩陣第m行n列的元素能夠分解成四個(gè)形式相似的求和項(xiàng)，以其中一項(xiàng)為例
我們利用以下兩條引理分離觀測(cè)項(xiàng)
第一條性質(zhì)幫助我們提取矩陣元素，第二條性質(zhì)用來完成矩陣位置交換。
利用這兩條性質(zhì)，我們成功得到了只有觀測(cè)項(xiàng)的累加形式X。其他各項(xiàng)的推導(dǎo)類似，請(qǐng)參考我們的論文和補(bǔ)充材料。當(dāng)新的觀測(cè)點(diǎn)加入時(shí)，我們只需將點(diǎn)和協(xié)方差映射成上面的累加形式即可完成增量更新。這種緊湊的表示方法避免了針對(duì)每個(gè)點(diǎn)重新求導(dǎo)，大幅減少了計(jì)算量和內(nèi)存占用。
為了進(jìn)一步解決bias-variance trade-off難題，我們引入了一種體素合并策略，使其能夠動(dòng)態(tài)適應(yīng)真實(shí)世界中的大平面特征。這一步的難點(diǎn)是如何保證合并效率，如果采用暴力搜索的方式，將會(huì)影響到算法的實(shí)時(shí)性。
為此，我們?cè)O(shè)計(jì)了一種基于局部敏感哈希（Locality Sensitive Hash）的合并方法。具有相似平面參數(shù)，同時(shí)物理距離較近的體素更有機(jī)會(huì)計(jì)算出相同的哈希值，進(jìn)而被推入同一個(gè) bucket 等待合并。當(dāng) bucket 元素達(dá)到一定數(shù)量時(shí)，合并操作被觸發(fā)，點(diǎn)數(shù)最多的體素被選為參考體素，用來作為其它體素合并的比較基準(zhǔn)。這樣做的原因是，點(diǎn)數(shù)多的體素通常平面估計(jì)更準(zhǔn)確。如果其它體素嘗試合入后，點(diǎn)集分布的奇異值仍然滿足平面條件，該體素將被允許合入，反之將被拒絕合入。
得益于平面協(xié)方差矩陣的緊湊形式，我們能夠增量更新合并后的平面。單個(gè)體素和跨體素的不確定度更新具有統(tǒng)一的增量形式。因此，合并后的平面仍然具有完備的不確定度表達(dá)。通過合并，一個(gè)大平面特征只需一個(gè)或少量體素即可表示，進(jìn)一步減少了內(nèi)存占用。此外，在噪聲水平相同的情況下，通過體素合并能夠更加準(zhǔn)確地估計(jì)平面，進(jìn)而提高SLAM 精度(如下方示意圖，理論證明請(qǐng)參考論文V-C小節(jié))。
為了驗(yàn)證C3P-VoxelMap的有效性，我們?cè)诠_數(shù)據(jù)集上進(jìn)行了充分測(cè)試。
在 KITTI 數(shù)據(jù)集上，相比于非增量式更新方法，C3P-VoxelMap在地圖更新階段實(shí)現(xiàn)了 70% 的性能提升。
此外，得益于每個(gè)體素的內(nèi)存消耗與點(diǎn)數(shù)無關(guān)，即具有常量復(fù)雜度，C3P-VoxelMap具備更高的內(nèi)存效率。
在 UTBM 數(shù)據(jù)集測(cè)試中，C3P-VoxelMap 相比SOTA 方法實(shí)現(xiàn)了 20% 的精度提升。
本工作能夠在算力受限的平臺(tái)上（例如樹莓派4B），使用3D ToF深度相機(jī)實(shí)現(xiàn) 500+m2的實(shí)時(shí)高精度3D SLAM建圖與定位，助力掃地機(jī)等移動(dòng)機(jī)器人在嵌入式平臺(tái)上實(shí)現(xiàn)高精度三維導(dǎo)航和避障。
EN
