邊緣計算卸載策略的ORB-SLAM3建圖算法

摘 要：

針對同步定位與地圖構(gòu)建（SLAM）中需要的大量計算資源和高昂的計算成本，以及ORB-SLAM系統(tǒng)建圖過程中計算資源大量消耗的問題，提出一種基于邊緣計算卸載策略的ORB-SLAM3建圖算法。首先，引入動態(tài)規(guī)劃算法有效篩選關(guān)鍵幀子集，構(gòu)建不確定性量化模型用以評估地圖中的不確定性；然后，結(jié)合最小化馬氏距離優(yōu)化地圖；最后，在移動設(shè)備和邊緣服務(wù)器中分別構(gòu)建最佳局部地圖和全局地圖。采用TUM-RGB-D數(shù)據(jù)集進行實驗。結(jié)果表明，相較于傳統(tǒng)ORB-SLAM3算法，改進后算法在關(guān)鍵幀數(shù)量較少的環(huán)境下精度較高，定位精度平均提高14.2%；改進后算法的CPU占用率較低，平均減少了20.7%。驗證了在計算資源受限時，改進型算法在構(gòu)建最佳局部和全局地圖的可行性及有效性。

關(guān)鍵詞：邊緣計算；不確定性最小化；動態(tài)規(guī)劃算法；關(guān)鍵幀子集；局部建圖

中圖分類號：TP"" 文獻標(biāo)志碼：A""" 文章編號：1001-3695（2024）12-030-3749-06

doi： 10.19734/j.issn.1001-3695.2024.05.0147

Research on ORB-SLAM3 mapping algorithm for edge computing offloading strategy

Zhang Jie， Dang Shuwen， Chen Li

（College of Air Transport， Shanghai University of Engineering Science， Shanghai 201600， China）

Abstract：

To address the large amount of computational resources and high computational cost required in simultaneous localization and map construction （SLAM）， as well as the significant consumption of computational resources during the map construction process of the ORB-SLAM system， this paper proposed a map construction algorithm based on an edge computation offloading strategy for ORB-SLAM3. Firstly， it introduced a dynamic programming algorithm to effectively filter a subset of key frames and construct a quantitative model to assess uncertainties in the map. It evaluated the uncertainty in the map using the constructed uncertainty quantization model. Then， it optimized the map by minimizing the Mahalanobis distance. Finally， it constructed the optimal local and global maps on the mobile device and the edge server， respectively. It conducted experiments using the TUM-RGB-D dataset. The results show that， compared with the traditional ORB-SLAM3 algorithm， the improved algorithm achieves higher accuracy in environments with fewer key frames， with localization accuracy improved by an average of 14.2%. Additionally， the improved algorithm has lower CPU usage， reducing it by an average of 20.7%. Therefore， it verifies the feasibility and effectiveness of the improved algorithm in constructing the optimal local and global maps under limited computational resources.

Key words：edge computing; minimization of uncertainty; dynamic programming algorithm; key frame subset; local mapping

0 引言

同步定位與地圖構(gòu)建（simultaneous localization and mapping，SLAM）［1～3］是指在未知環(huán)境中通過各種傳感器實時建立地圖并確定自身位置的技術(shù)。視覺SLAM（visual SLAM，VSLAM）用視覺傳感器來實現(xiàn)機器人在未知環(huán)境中的定位和地圖構(gòu)建，成本相對激光雷達等其他SLAM技術(shù)更加低廉，是當(dāng)前SLAM技術(shù)研究領(lǐng)域的熱點之一［4］。當(dāng)前SLAM技術(shù)中，ORB-SLAM較受歡迎。Campos等人［5］在2021年提出的ORB-SLAM3是目前較先進的ORB-SLAM算法，它在ORB-SLAM2的基礎(chǔ)上增加了視覺慣性（VI）SLAM支持、改進的場景識別技術(shù)、多地圖機制以及相機模型抽象化等新特性。ORB-SLAM3能夠利用單目、雙目和RGB-D相機實現(xiàn)視覺、視覺慣性和多地圖SLAM算法，它通過多地圖系統(tǒng)和新位置識別方法提高了在視覺信息長期缺乏情況下的SLAM系統(tǒng)魯棒性。在ORB-SLAM3中，局部建圖線程是核心線程之一，一般在初始化時被創(chuàng)建和啟動，主要作用是為跟蹤線程（跟蹤局部地圖）以及回環(huán)檢測線程服務(wù)，并進行局部地圖優(yōu)化以及消除軌跡的累計誤差。局部地圖線程［6］是由共視圖、本質(zhì)圖和生成樹組成的局部地圖，它包含了當(dāng)前幀的共視關(guān)鍵幀以及關(guān)鍵幀之間的連接關(guān)系和關(guān)鍵幀對應(yīng)的3D空間點。但是ORB-SLAM系列在構(gòu)建局部地圖和地圖優(yōu)化時會對資源產(chǎn)生消耗，且計算也需要強大資源，這為VSLAM移動設(shè)備提出了高成本計算要求，為此，基于邊緣計算［6］的邊緣輔助SLAM［7］被提出以幫助減少運行SLAM時移動設(shè)備資源的消耗。將部分工作負(fù)載卸載到邊緣服務(wù)器［8］已成為減少移動設(shè)備負(fù)載和提高整體性能的一種很有優(yōu)勢的解決方案，但是該方案在資源限制和波動的情況下會出現(xiàn)性能下降、精度不足等問題。現(xiàn)有的邊緣輔助SLAM解決方案一般先假設(shè)無線網(wǎng)絡(luò)資源足以進行不受限制的卸載，或者依賴于啟發(fā)式方法作出卸載決策［9］。另外，在實際應(yīng)用場景中，邊緣輔助SLAM與目前熱門的自動駕駛技術(shù)息息相關(guān)。在自動駕駛汽車中，使用SLAM技術(shù)進行實時環(huán)境感知和導(dǎo)航定位，邊緣計算可以將SLAM算法部署在路邊設(shè)備或車輛邊緣節(jié)點上，減少數(shù)據(jù)傳輸延遲，提高實時性和安全性。本文基于理論的方法可以使邊緣輔助SLAM變得更加穩(wěn)定和精確，為邊緣輔助SLAM解決方案落地作出了基礎(chǔ)理論的貢獻。

為解決ORB算法在移動設(shè)備通信和計算資源受限、計算成本高等問題，本文提出一種基于邊緣輔助視覺（V）和視覺慣性（VI）系統(tǒng)的新型邊緣輔助SLAM算法。該算法首先引入動態(tài)規(guī)劃算法在局部地圖構(gòu)建中篩選出已經(jīng)優(yōu)化過的關(guān)鍵幀集和需要優(yōu)化的關(guān)鍵幀集，在全局地圖構(gòu)建中篩選出由移動設(shè)備上傳到邊緣服務(wù)器的所有關(guān)鍵幀；然后通過最小化馬氏距離之和改善位姿估計，完成局部和全局地圖的優(yōu)化；最后在計算資源約束的情況下，在移動設(shè)備和邊緣服務(wù)器中構(gòu)建最佳局部地圖和全局地圖［10］。本文選用ORB-SLAM3作為基礎(chǔ)框架，用邊緣輔助SLAM算法實現(xiàn)資源受限情況下系統(tǒng)性能的整體提高。本文利用TUM標(biāo)準(zhǔn)RGB-D數(shù)據(jù)集進行仿真實驗，通過與傳統(tǒng)算法對比，驗證本文算法的有效性和優(yōu)越性。

1 算法介紹

改進后的SLAM系統(tǒng)框架如圖1所示。配備攝像頭和IMU的移動設(shè)備可以與邊緣服務(wù)器進行雙向通信。移動設(shè)備和邊緣服務(wù)器通過協(xié)同運行SLAM算法來估計移動設(shè)備的姿態(tài)和構(gòu)建環(huán)境地圖。

首先改進型SLAM算法在構(gòu)建局部地圖時，通過將關(guān)鍵幀子集的選擇問題轉(zhuǎn)換為一個具有子集性質(zhì)的函數(shù)，引用動態(tài)規(guī)劃算法尋找最優(yōu)解。然后挑選出具有最小化不確定性的關(guān)鍵幀去建立關(guān)鍵幀子集Klocal和Kfixed，接下來通過不確定量化模型將局部地圖的不確定性最小化；但在全局地圖構(gòu)建中，候選關(guān)鍵幀會先卸載到邊緣服務(wù)器中去建立關(guān)鍵幀子集Kedge，再通過不確定性量化模型將全局地圖的不確定性最小化。最后通過計算關(guān)鍵幀的估計位姿將局部地圖和全局地圖進行優(yōu)化。改進的SLAM算法通過邊緣服務(wù)器進行建圖和優(yōu)化，使邊緣計算與SLAM完美結(jié)合形成邊緣輔助SLAM系統(tǒng)，這樣就可以在移動設(shè)備的計算資源限制下優(yōu)化SLAM的性能。本文在移動設(shè)備和邊緣服務(wù)器之間劃分模塊［11～13］，移動設(shè)備將閉環(huán)回路和全局地圖優(yōu)化模塊卸載到邊緣服務(wù)器，同時運行實時跟蹤和局部地圖。與現(xiàn)有的邊緣輔助SLAM系統(tǒng)不同，改進算法的目標(biāo)是在計算資源約束下通過選擇較少的關(guān)鍵幀去構(gòu)建優(yōu)化局部和全局地圖。

改進型ORB-SLAM3建圖算法的關(guān)鍵是在局部地圖和全局地圖的構(gòu)建過程中。在局部地圖構(gòu)建中，由于計算資源的限制，移動設(shè)備從候選關(guān)鍵幀中引用動態(tài)規(guī)劃算法去選擇關(guān)鍵幀的子集從而構(gòu)建出局部地圖；而在全局地圖構(gòu)建中，需要將候選關(guān)鍵幀的子集傳輸?shù)竭吘壏?wù)器來構(gòu)建全局地圖。最后將選定的關(guān)鍵幀從移動設(shè)備傳輸?shù)竭吘壏?wù)器中進行優(yōu)化，將全局地圖優(yōu)化后的地圖又從邊緣服務(wù)器傳輸?shù)揭苿釉O(shè)備中。改進型SLAM以最佳方式選擇關(guān)鍵幀來構(gòu)建局部和全局地圖，這種選擇最佳關(guān)鍵幀的方法可以使SLAM系統(tǒng)的計算復(fù)雜度大大減少，從而最大限度地減少資源約束下的位姿估計不確定性。

2 關(guān)鍵幀子集的篩選

本文將挑選關(guān)鍵幀子集問題轉(zhuǎn)換為子集函數(shù)的求解問題。改進型SLAM采用動態(tài)規(guī)劃算法去求解最優(yōu)化問題，改進型算法無須所有關(guān)鍵幀參與構(gòu)建局部地圖和全局地圖，只需通過算法篩選出來的關(guān)鍵幀子集去構(gòu)建局部地圖和全局地圖。

本文將多重圖用于表示關(guān)鍵幀之間的相對位姿關(guān)系。多vi，vj∈V重圖被定義為集合G=（V，E，C），其中V={v1，…，v|v|}是節(jié)點的集合，E是關(guān)鍵幀的邊集合，C是邊類別的集合，設(shè)e={（vi，vj），c}∈E表示關(guān)鍵幀的邊，其中節(jié)點分別表示為e的頭和尾，c∈C是e的類別。設(shè)We是邊e的權(quán)值，用Ei， j表示從vi到vj的邊集合。從vi到vj的所有邊的權(quán)值之和為Wi， j=∑e∈Eiwe。

集合G的加權(quán)拉普拉斯矩陣L是一個|V|×|V|的矩陣，其中第i， j個元素Li， j為

Li， j=－wi， j""" i≠j

∑e∈Eiwei=j（1）

其中：Ei∈E是節(jié)點vi的所有邊的集合。通過從L中移除任意節(jié)點（即移除與該節(jié)點相關(guān)的行和列），得到一個簡化的拉普拉斯矩陣L。

一般地，有限集合V的集合函數(shù)f為一個映射。該映射將一個值f（S）賦給每個子集S∈V。如果集合f（L）+f（S）≥f（L∪S）f（L∩S），對于所有L，S≤V，則集合函數(shù)為子集函數(shù)。集合函數(shù)f對參數(shù)s的子集比：

γ=minLV，SV，|S|≤s，x∈V＼（S∪L）f（L∪{x}）－f（L）f（L∪S∪{x}）－f（L∪S）（2）

所以子集函數(shù)最大化問題是

maxSV，|S|=sf（S）（3）

改進算法的關(guān)鍵幀選擇優(yōu)化與上面介紹的子集函數(shù)最大化問題密切相關(guān)，這是一個NP難問題［34］，但該問題滿足最優(yōu)子結(jié)構(gòu)性質(zhì)，所以該問題的最優(yōu)解由相關(guān)子問題的最優(yōu)解組合而成，且這些子問題可以單獨求解。假設(shè)S#為算法1的解，S*是式（3）的解，則f（S#）≥（1－exp（－γ））f（S*）。

算法1 動態(tài)規(guī)劃算法求解式（2）

1 S#←φ，|S#|lt;s；

2 x←argmaxxf（S#∪{x}）－f（S#）.S#←S#∪{x}。

3 局部地圖及全局地圖的構(gòu)建優(yōu)化

接下來，本文將介紹如何通過最小化馬氏距離之和來提高位姿估計，以進一步實現(xiàn)地圖優(yōu)化。這里將時間劃分為大小相等的Δt間隙，接下來介紹在Δt時間內(nèi)的位姿和地圖的更新。

一般地，位姿圖被定義為無向多重圖G=（K，E，C），其中K是位姿圖中關(guān)鍵幀的集合，E是位姿圖邊的集合，C={IMU，vis}是位姿圖中所屬類別集合。IMU代表IMU邊，vis代表共視性邊。

對于所有n∈K，假定相機姿態(tài)Pn=（x，y，z，wx，wy，wz），其中（x，y，z）是相機位置，（wx，wy，wz）表示相機姿態(tài)（偏航、俯仰和滾轉(zhuǎn)）。E中的邊為e={（n，m），c}，表示為邊連接的兩個關(guān)鍵幀和它們的類別，其中n，m∈K、c∈C。如果在K的兩個關(guān)鍵幀中都觀察到同一地圖點，則這兩個關(guān)鍵幀通過共視邊連接。對于每個e={（n，m），c}∈E，觀察到n和m之間的相對噪聲姿態(tài)測量Δe，其中Δe=Pm－Pn+Xe，Xe是測量e邊上的噪聲。地圖優(yōu)化問題是找到實際相機姿態(tài){Pn}n∈K的最大似然估計{Pn}n∈K。所以地圖優(yōu)化問題為

min{Pn}n∈K∑e∈E（Xe）TLeXe（4）

其中：Xe=Δe－Pm+Pn，Le為e上測量誤差的信息矩陣（即協(xié)方差矩陣）。（Xe）TLeXe是e相對于Le估計的測量噪聲的馬氏距離，本文利用Ceres解算器來解決地圖優(yōu)化問題。

如果一個節(jié)點的姿態(tài)已知，就定義這個節(jié)點是位姿圖的錨點。錨點是指在SLAM系統(tǒng)中用于固定關(guān)鍵幀的節(jié)點，通常被優(yōu)化在邊緣服務(wù)器上，并傳輸?shù)揭苿釉O(shè)備上。錨點可以減少關(guān)鍵幀之間的不確定性，可以幫助系統(tǒng)更好地估計關(guān)鍵幀的位姿，并提高系統(tǒng)的魯棒性。在本文中，錨點被用于解算最小化不確定性，從而提高系統(tǒng)的性能。

本文假設(shè)全局（或局部）地圖錨定在第一個節(jié)點上，這是因為SLAM只能根據(jù)共視性和慣性測量來估計相對位姿變化，而無法提供全局坐標(biāo)系下的絕對位姿估計。

根據(jù)ORB-SLAM3算法中的選擇策略從相機幀中選擇候選關(guān)鍵幀，并且這些候選關(guān)鍵幀形成集合K。由于計算資源受限，移動設(shè)備從候選關(guān)鍵幀中選出固定關(guān)鍵幀集合Kfixed和局部關(guān)鍵幀集Klocal去構(gòu)建局部地圖，其中|Kfixed|≤lfixed，且|Klocal|≤llocal。固定關(guān)鍵幀集Kfixed是從全局地圖的候選關(guān)鍵幀Kcan中選擇的，最后從邊緣服務(wù)器發(fā)送。Kfixed中關(guān)鍵幀的姿態(tài)不需要在局部地圖中優(yōu)化，這是因為Kcan中的關(guān)鍵幀姿態(tài)已經(jīng)在全局地圖優(yōu)化中進行了優(yōu)化，所以具有較低的不確定性。局部關(guān)鍵幀集Klocal中關(guān)鍵幀的姿態(tài)將根據(jù)上文中地圖優(yōu)化方法進行優(yōu)化。Klocal中兩個關(guān)鍵幀共有的邊組成集合Elocal，對于一個節(jié)點屬于Klocal，另一個節(jié)點屬于Kfixed的邊，稱為集合El，f。

在局部地圖構(gòu)建中選擇Klocal后，局部地圖優(yōu)化的目標(biāo)是通過最小化馬氏距離的總和來優(yōu)化估計姿勢{Pn}n∈Klocal。

∑e∈Eloc∪El，f（Xe）TLeXe（5）

在局部地圖優(yōu)化中，Kfixed中關(guān)鍵幀的姿態(tài)得已確定，因此求解式（4）即可實現(xiàn)局部地圖優(yōu)化。

min{Pn}n∈Kloc∑e∈Eloc∪El， f（Xe）TLeXe（6）

由于移動設(shè)備和邊緣服務(wù)器之間的帶寬有限，只有候選關(guān)鍵幀的子集被卸載到邊緣服務(wù)器以構(gòu)建全局地圖，在考慮潛在無線網(wǎng)絡(luò)約束的情況下，對需要卸載的關(guān)鍵幀的選擇進行優(yōu)化，使全局地圖的姿態(tài)估計不確定性最小化。在邊緣服務(wù)器中構(gòu)建全局地圖，邊緣服務(wù)器中的關(guān)鍵幀集合為Kedge，Kedge中兩個關(guān)鍵幀之間共有的邊構(gòu)成集合Eedge。

在全局地圖構(gòu)建中選擇Kedge后，邊緣服務(wù)器執(zhí)行全局地圖優(yōu)化來估計Kedge中的關(guān)鍵幀姿態(tài)，當(dāng)E=Eedge且K=Kedge時，就可以實現(xiàn)全局地圖的優(yōu)化求解：

min{Pn}n∈Kedge∑e∈Eedge（Xe）TLeXe（7）

改進算法聚焦在有效地選擇關(guān)鍵幀來構(gòu)建最優(yōu)的局部和全局地圖，為局部地圖篩選出Klocal和Kfixed中的關(guān)鍵幀，為全局地圖篩選出Kedge中的關(guān)鍵幀。接下來，本文通過最小化關(guān)鍵幀估計姿態(tài)的不確定性來構(gòu)建最優(yōu)局部地圖和全局地圖。下文主要將量化有向多重圖的不確定性，并求解上述集合函數(shù)（式（4）（6）（7））所表述的不確定性最小化問題。

4 不確定性量化模型的確立

本章將構(gòu)建不確定量化模型去量化地圖的不確定性，該模型通過計算兩個關(guān)鍵幀之間的相對位姿變化來量化不確定性。

令pn=Pn－Pn，用于表示關(guān)鍵幀n的姿態(tài)估計誤差。估計的測量噪聲可以表達為

Xe=pn－pm+Xe=pn，m+Xe（8）

其中：pn，m=pn－pm。把所有的Pn（n∈K）疊加起來，就可以得到姿態(tài)估計誤差向量：

w=（pT1，pT2，L，pT|K|）

將地圖優(yōu)化的目標(biāo)函數(shù)式（4）重寫為

∑e∈E（Xe）TLeXe=∑e=（（n，m），c）∈EpTn，mLepn，m+2∑e=（（n，m），c）∈EpTn，mLexe+∑e∈ExTeLexe（9）

把二次項∑e=（（n，m），c）∈EpTn，mLepn，m寫成wLwwT的形式，其中Lw被稱為姿態(tài)圖的信息矩陣，則姿態(tài)圖的不確定性根據(jù)貝葉斯的D優(yōu)化方法用－log det（Lw）來量化［14］。

將全局和局部地圖的姿態(tài)估計誤差向量表示為wg=（pTu1L，pTu|Kg，edge|）和wl=（pTr1L，pTr|Kloc|）。

其中：u1，…，u|Kg，edge|是Kedge中的關(guān)鍵幀，r1，…，r|Kloc|是Klocal中的關(guān)鍵幀。將式（4）（6）中全局和局部地圖優(yōu)化的目標(biāo)函數(shù)的二次項重寫為

∑e=（（n，m），c）∈EglobpTn，mLepn，m=wgLglob（Kg，edge）wTg

或者

∑e=（（n，m），c）∈Elocal∪El，fpTn，mLepn，m=wlLlocal（Klocal，Kfixed）wTl（10）

其中：Lglob（Kg，edge）和Llocal（Klocal，Kfixed）是全局和局部地圖的信息矩陣，所以不確定性量化是基于全局和局部地圖優(yōu)化的。一般地，全局姿態(tài)圖的不確定性可以根據(jù)簡化的拉普拉斯矩陣與樹結(jié)構(gòu)關(guān)系［15］來確定，不確定性與全局姿態(tài)圖中生成樹的加權(quán)數(shù)的對數(shù)成反比。另外，局部地圖的不確定性與位姿圖G錨定在Klocal中的第一個節(jié)點和Kfixed中的所有節(jié)點的不確定性成正比，其中G的節(jié)點集是Kfixed∪Klocal。注意，Kfixed中的關(guān)鍵幀姿態(tài)是在邊緣服務(wù)器上優(yōu)化并傳輸?shù)揭苿釉O(shè)備的，它們在局部姿態(tài)圖優(yōu)化中被視為常量。從不確定性角度來說，在Kfixed中添加固定關(guān)鍵幀相當(dāng)于錨定這些關(guān)鍵幀的姿態(tài)。雖然姿態(tài)是固定的，但錨定節(jié)點仍然降低了姿態(tài)圖的不確定性。因此，除了Klocal，還將選擇固定關(guān)鍵幀集Kfixed去最小化不確定性。

5 不確定性最小化的實現(xiàn)

基于上述不確定性量化模型的構(gòu)建，接下來求解局部地圖、全局地圖構(gòu)造及優(yōu)化的不確定性最小化問題。

問題1：局部地圖構(gòu)造。

maxKlocal，Kfixedlog det（Ilocal（Klocal∪{k}，Kfixed））（11）

約束條件為

|Kfixed|≤lf，KfixedKcan（13）

本文對每個關(guān)鍵幀k求解式（11），問題1的目標(biāo)是最小化局部地圖的不確定性。約束式（12）是指約束Klocal的大小以降低局部地圖優(yōu)化的計算復(fù)雜度，并且在局部地圖中要優(yōu)化的關(guān)鍵幀是在Kcan以外的關(guān)鍵幀中選擇的，約束式（13）意味著Kfixed的大小受到約束，并且固定的關(guān)鍵幀是從Kcan中選擇的，并先前就在邊緣服務(wù)器上進行優(yōu)化且從邊緣服務(wù)器傳輸。

問題2：全局地圖構(gòu)造。

maxK′K＼Kg，edgelog det（Γglob（Kg，edge∪K′））（14）

約束條件為

d|K′|≤D（15）

式（14）將關(guān)鍵幀自適應(yīng)地卸載到邊緣服務(wù)器，從而達到最小化全局地圖不確定性的目的。K＼Kg，edge是尚未卸載到邊緣服務(wù)器的關(guān)鍵幀的集合，改進算法從K＼Kg，edge中選擇關(guān)鍵幀的子集K′。約束式（15）保證關(guān)鍵幀不能以高于可用通道容量的比特率從設(shè)備卸載到服務(wù)器，其中D是通道容量約束，表示在給定的傳輸窗口中可以傳輸最大的比特數(shù)。

問題3：最優(yōu)局部關(guān)鍵幀集Klocal的選取。

Klocal=argmaxKloc log det（Γlocal（Klocal∪{k}，Φ））（16）

約束條件：同式（12）。

問題4：最優(yōu)固定關(guān)鍵幀集Kfixed的選取。

Kfixed=arg maxKfixed log det（Γlocal（Klocal∪{k}，Kfixed））（17）

約束條件：同式（13）。

為了有效地求解式（11），對于局部關(guān)鍵幀中包括的Klocal和Kfixed，本文將式（11）分兩步去計算，分別為式（16）（17）。從式（16）可以獲得最優(yōu)的局部關(guān)鍵幀集Klocal，基于Klocal，就可以得到式（17）中的最優(yōu)固定關(guān)鍵幀集Kfixed。

首先求解式（16）。這是一個帶約束的優(yōu)化問題，本文利用動態(tài)規(guī)劃算法將式（16）繼續(xù)分解為與式（16）等價的式（18）和（20）。

OPTadd（Kbase）=argmaxKadd－Unc（Kadd∪Kbase∪{k}）（18）

約束條件：

|Kadd|≤llocal－lb（19）

在給定關(guān)鍵幀子集Kbase的情況下，挑選關(guān)鍵幀子集Kadd以最小化不確定性。在得到式（18）的所有可能大小為lb的Kbase的解即（OPTadd（Kbase））之后，就可以得到式（20）中的最優(yōu)Kbase（記作Kbase）。將目標(biāo)改寫為Unc（Kadd∪Kbase∪{k}，Φ），所以問題是在給定Kbase的情況下求最優(yōu)Kadd。

Kbase=argmaxKbase－Unc（OPTadd（Kbase）∪Kbase∪{k}）（20）

約束條件：

|Kbase|=lb（21）

通過求解式（18）（20）就可以求得式（16）的Klocal。動態(tài)規(guī)劃算法適用于重疊的子問題求解，動態(tài)規(guī)劃算法對每個子問題只求解一次，將其保存在一個表格中，從而無須每次求解一個子問題時都重新計算，避免了這種不必要的計算工作。

算法2 在局部地圖中選擇局部關(guān)鍵幀集Klocal

1 ←φ；

2 當(dāng)|Λ|≤llocal的時候；

3 如果|Λ|≤lthr則h←H，否則h←1；

4 選擇Λ，Λ∈和n，n∈K＼Kcan的前h個最高得分組合，使得Unc（Λ∪{n，k}）最小化，使用算法3中的計算重用方法計算Unc（Λ∪{n，k}）；

5 更新為Λ和n的h個最高得分的集合，的每個元素都是對應(yīng)于一個組合的集合（即（Λ∪{n}））；

6 Klocal←argminΛ∈Unc（Λ∪{k}）。

當(dāng)現(xiàn)有關(guān)鍵幀集的大小（即|Kbase|）遠大于|Kadd|時，式（18）中的目標(biāo)函數(shù)接近子集函數(shù)。因此，可以使用近似算法來近似求解。式（18）得到的近似最優(yōu)解用OPT#add（Kbase）表示。根據(jù)對式（18）（20）的分析，現(xiàn)在使用算法2來解決式（16）從而挑選局部關(guān)鍵幀集Klocal，是局部地圖中的可能關(guān)鍵幀集的集合，并且只維護（需要）h個關(guān)鍵幀集來節(jié)省計算資源。Λ∈，表示中的元素，也表示為一個可能的關(guān)鍵幀集。當(dāng)Λ小于閾值lthr時，選擇Λ和n（n∈K＼Kcan）的前H（Hgt;1）個最高得分組合去使Unc（Λ∪{k，n}）最小化。當(dāng)|Λ|變大時，只選擇得分最高的組合。原因在于Λ可以看做是現(xiàn)有的關(guān)鍵幀集Kbase。當(dāng)現(xiàn)有關(guān)鍵幀集很小時，不能保證Unc（Kadd∪Kbase∪{k}）接近子集函數(shù)（即子集比遠小于1）。因此嘗試Λ和n的不同組合，來搜索每次迭代后使不確定性最小化的組合。隨著|Λ|的增長，子集比接近1，動態(tài)規(guī)劃算法可以實現(xiàn)。另外，應(yīng)用動態(tài)規(guī)劃算法時，只保留每一步實現(xiàn)最小不確定性的組合。

算法3 計算結(jié)果復(fù)用算法

1 輸入det（A），A－1；

2 B←A－1，求出BiBTi，i=1，…，|Λ|的值；

3 計算det（A）、A－1和det（Γ（Λ∪{n，k}））的值。

使用計算結(jié)果復(fù)用算法來加速算法2，在矩陣Γ（Λ{n，k}）中只有（3|Λ|+1）個元素是不同的。計算（|Λ|+1）×（|Λ|+1）矩陣Γ（Λ{n，k}）的對數(shù)行列式函數(shù)具有很高的計算復(fù)雜度。因此本文算法不是從頭開始計算每個n的目標(biāo)函數(shù)，而是復(fù)用不同n的部分計算結(jié)果，這樣就可以大大減少計算成本。

設(shè)A=Γ（Λ{k}）表示算法2在第|Λ|次迭代中的局部地圖的信息矩陣，第（|Λ|+1）次迭代中的信息矩陣為

Γ（ΛU{n，k}）=A+dig（a）aTad（22）

其中：a=（a1，a2，L，a|Λ|），ai=wλi，n，λi是Λ的第i個元素；d=wk，n+∑|Λ|i=1ai。

本文的目標(biāo)是使用先前迭代中的det（A）、A-1的計算來求det（Γ（Λ∪{n，k}））。

設(shè)A′=A+diag（a），det（Γ（Λ∪{n，k}））通過式（23）求得。

det（Γ（Λ∪{n，k}））=（d－a（A′）－1aT）det（A′）（23）

接下來，有效地計算（A′）-1和det（A′）去求得det（Γ（Λ∪{n，k}））。可以把A′重寫為

A′=A+∑|Λ|i=1βTiβi，其中βi=（0，…，ai，…，0），根據(jù)謝爾曼莫里森公式，（A′）-1可以由式（25）得出：

（A′）-1≈BReuse-∑|Λ|i=1a1+aiBi，iBiBTiReuse（24）

其中：B=A-1，Bi，i是B的第i個元素，Bi是B的第一個列向量。根據(jù)式（25），B和BiBTi只能計算一次以用于不同的n∈K＼Kcan，這大大降低了計算成本。

如果使用基于關(guān)鍵幀組合的窮舉枚舉的暴力破解算法來選擇Klocal中的關(guān)鍵幀，這個復(fù)雜性為Ορllocall3local，其中ρ=|K＼Kcan|是尚未卸載到邊緣服務(wù)器的關(guān)鍵幀數(shù)。在沒有計算結(jié)果復(fù)用的情況下，該算法的計算復(fù)雜度為Ο（Hρl4local）。在計算結(jié)果復(fù)用的情況下，它被簡化為Ο（Hl4local）+Ο（Hρl3local）。由于只在局部地圖的Klocal中保留llocal關(guān)鍵幀，并在算法2中保留一個小H以節(jié)省計算資源，即ρgt;gt;llocalgt;H，所以所提出的具有計算結(jié)果復(fù)用的基于動態(tài)規(guī)劃的算法顯著降低了計算復(fù)雜度。

通過求解問題3選擇局部關(guān)鍵幀集Klocal后，然后求解問題4來選擇固定關(guān)鍵幀集Kfixed。該問題可以用算法1中的動態(tài)規(guī)劃算法近似求解。對于每次迭代，算法從Kcan中選擇一個關(guān)鍵幀去添加到固定的關(guān)鍵幀集Kfixed中。

本文使用低復(fù)雜的算法來解決問題2去構(gòu)建全局地圖。問題2的目標(biāo)函數(shù)可以重寫為－Unc（Kg，edge∪K′），其結(jié)構(gòu)與問題3的目標(biāo)函數(shù)相同。問題2和3都將關(guān)鍵幀添加到現(xiàn)有的關(guān)鍵幀集中去構(gòu)建位姿圖并優(yōu)化位姿圖中的關(guān)鍵幀姿態(tài)。因此算法2和3可以用于解決問題2。在算法2中，llocal用D/d代替，K＼Kcan用K＼Kg，edge代替。計算大型全局地圖的不確定性是需要消耗大量計算資源的，所提算法引入了動態(tài)規(guī)劃算法以降低計算復(fù)雜度，從而節(jié)省計算成本。

6 實驗及分析

6.1 算法的準(zhǔn)確性

為驗證改進算法的準(zhǔn)確性，通過將所提算法與傳統(tǒng)ORB-SLAM3進行仿真實驗對比分析，本文的實驗平臺是搭載Intel i7-13700處理器，16 GB內(nèi)存的臺式電腦，安裝有Ubuntu 18.04版本Linux操作系統(tǒng)，使用TUM標(biāo)準(zhǔn)RGB-D數(shù)據(jù)集進行改進算法的應(yīng)用驗證和SLAM建圖仿真實驗，該數(shù)據(jù)集主要是在室內(nèi)環(huán)境下進行統(tǒng)計，紋理較為豐富并且場景環(huán)境的深度比較小，比較適合本文算法的測試。

本文主要通過SLAM算法的精度分析來進行評估。評估指標(biāo)包括絕對軌跡誤差（ATE）和相對軌跡誤差（RPE）。絕對軌跡誤差評估的是整個軌跡的全局誤差，而相對軌跡誤差評估的是相鄰幀之間的相對誤差。通過選擇絕對軌跡誤差來評估不同算法的精度，并選用平均誤差（MEAN）、均方根誤差（RMSE）、誤差平方和（SSE）、標(biāo)準(zhǔn)差（STD）這4個指標(biāo)來反映絕對軌跡誤差的變化。實驗采用6組RGB-D標(biāo)準(zhǔn)數(shù)據(jù)集，包括fr3-sitting-xyz、fr1-desk、fr1-desk2、fr1-xyz、fr2-xyz和fr1-floor作為實驗環(huán)境，對改進算法及ORB-SLAM3進行評價。表1為兩種算法在不同TUM數(shù)據(jù)集下的ATE對比。圖2為改進算法和ORB-SLAM3的fr1-xyz、fr1-desk2的APE和實際軌跡與估計軌跡的誤差圖對比。

根據(jù)表1可以得出改進算法在fr3-sitting-xyz、fr1-desk、fr1-desk2、fr1-xyz、fr2-xyz和fr1-floor這幾組數(shù)據(jù)集中表現(xiàn)得都比ORB-SLAM3好，精度更高。根據(jù)圖2可以看出改進算法比ORB-SLAM3的誤差更小。從表1可以得知，在基于低復(fù)雜度算法和不確定性量化模型的算法下，改進型算法比ORB-SLAM3的精度平均提高了14.2%。這就表明改進型算法在計算資源受限的情況下仍然能保持較高精度。

6.2 算法資源利用率分析

為了進一步驗證改進算法在SLAM中的資源利用，在實驗中將ORB-SLAM3和改進算法在不同數(shù)據(jù)集下的CPU使用情況進行對比。表2展示了這兩種算法在TUM數(shù)據(jù)集中的CPU使用百分比和空閑CPU百分比的對比結(jié)果。具體數(shù)據(jù)集包括fr3-sitting-xyz、fr1-desk、fr1-desk2、fr1-xyz、fr2-xyz和fr1-floor。

表2顯示出兩種算法在各個數(shù)據(jù)集下的CPU使用百分比和空閑CPU百分比。總體來看，改進算法在所有數(shù)據(jù)集下均表現(xiàn)出較低的CPU使用率和較高的空閑CPU百分比。實驗數(shù)據(jù)表明，ORB-SLAM3的CPU使用百分比在各個數(shù)據(jù)集下平均為70.7%，而改進算法的平均CPU使用百分比為56.05%。這一差異表明改進算法顯著減少了CPU的使用率。此外，ORB-SLAM3的平均空閑CPU百分比為10.75%，而改進算法的平均空閑CPU百分比為22.27%，這表明改進算法所需要的計算資源更少，進一步證明了改進算法在計算資源利用方面的優(yōu)勢。

7 結(jié)束語

本文針對在ORB-SLAM3的關(guān)鍵幀選取和地圖優(yōu)化過程中消耗大量計算資源和計算成本高昂的問題，提出一種引入邊緣計算卸載策略的ORB-SLAM算法。通過動態(tài)規(guī)劃算法有效篩選關(guān)鍵幀子集，進而構(gòu)建局部地圖和全局地圖，并構(gòu)建不確定量化模型以量化分析環(huán)境不確定性，再結(jié)合低復(fù)雜度計算、結(jié)果復(fù)用等方法實現(xiàn)不確定性最小化。實驗結(jié)果表明，本文算法在計算資源受限的情況下，能有效節(jié)省計算成本，同時還表現(xiàn)出較高的定位精度，為后續(xù)SLAM地圖構(gòu)建、路徑規(guī)劃及室內(nèi)導(dǎo)航等多任務(wù)的開展提供保障。

參考文獻：

［1］Jiang Guolai， Yin Lei， Jin Shaokun， et al. A simultaneous localization and mapping （SLAM） framework for 2.5D map building based on low-cost LiDAR and vision fusion ［J］. Applied Sciences， 2019， 9（10）： 2105.

［2］Hautot F， Dubart P， Bacri O C， et al. Visual simultaneous localization and mapping （VSLAM） methods applied to indoor 3D topographi-cal and radiological mapping in real-time ［J］. EPJ Web of Confe-rences， 2017， 153： 01005.

［3］Rosen D M， Doherty K J， Terán Espinoza A， et al. Advances in inference and representation for simultaneous localization and mapping ［J］. Annual Review of Control， Robotics， and Autonomous Systems， 2021， 4： 215-242.

［4］權(quán)美香， 樸松昊， 李國. 視覺SLAM綜述 ［J］. 智能系統(tǒng)學(xué)報， 2016， 11（6）： 768-776. （Quan Meixiang， Piao Songhao， Li Guo. A survey of visual SLAM ［J］. CAAI Trans on Intelligent Systems， 2016， 11（6）： 768-776.）

［5］Campos C， Elvira R， Rodríguez J J G， et al. ORB-SLAM3： an accurate open-source library for visual， visual-inertial， and multimap SLAM ［J］. IEEE Trans on Robotics， 2021， 37（6）： 1874-1890.

［6］Sun Qinxuan，Yuan Jing，Zhang Xuebo， et al. Plane-Edge-SLAM： seamless fusion of planes and edges for SLAM in indoor environments ［J］. IEEE Trans on Automation Science and Engineering， 2021， 18（4）： 2061-2075.

［7］Xu Jingao，Yang Zheng，Liu Yunhao， et al. Edge assisted mobile semantic visual SLAM ［C］//Proc of IEEE Conference on Computer Communications. Piscataway， NJ： IEEE Press， 2020： 1828-1837.

［8］Zhang Guoxuan. A*SLAM： a dual fisheye stereo edge SLAM ［EB/OL］. （2019）. https：//arxiv.org/abs/1911. 04063.

［9］Placed J A， Castellanos J A. Fast autonomous robotic exploration using the underlying graph structure ［C］// Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ： IEEE Press， 2021： 6672-6679.

［10］Khosoussi K， Giamou M， Sukhatme G S， et al. Reliable graphs for SLAM ［J］. The International Journal of Robotics Research， 2019， 38（2-3）： 260-298.

［11］Xu Jingao， Cao Hao， Yang Zheng， et al. SwarmMap： scaling up real-time collaborative visual SLAM at the edge ［C］//Proc of the 19th USENIX Symposium on Networked Systems Design and Implementation. Berkeley， CA： USENIX Association， 2022： 977-993.

［12］Ben Ali A J， Kouroshli M， Semenova S， et al. Edge-SLAM： edge-assisted visual simultaneous localization and mapping ［J］. ACM Trans on Embedded Computing Systems， 2022， 22（1）： 1-31.

［13］Deutsch I， Liu Ming， Siegwart R. A framework for multi-robot pose graph SLAM ［C］// Proc of IEEE International Conference on Real-time Computing and Robotics. Piscataway， NJ： IEEE Press， 2016： 567-572.

［14］Nemhauser G L， Wolsey L A， Fisher M L. An analysis of approximations for maximizing submodular set functions—I ［J］. Mathematical Programming， 1978， 14： 265-294.

［15］Khosoussi K， Huang S， Dissanayake G. Novel insights into the impact of graph structure on SLAM ［C］// Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway， NJ： IEEE Press， 2014： 2707-2714.

［16］曹一波， 張智輝， 趙鵬飛， 等. 基于多幀局部地圖的室內(nèi)TSDF建圖算法 ［J］. 計算機與數(shù)字工程， 2023， 51（9）： 2043-2047. （Cao Yibo， Zhang Zhihui， Zhao Pengfei， et al. Indoor TSDF mapping algorithm based on multi-frame local map ［J］. Computer and Digital Engineering， 2023， 51（9）： 2043-2047.）

［17］蔣鵬， 秦小麟. 基于視覺注意模型的自適應(yīng)視頻關(guān)鍵幀提取 ［J］. 中國圖象圖形學(xué)報， 2009， 14（8）： 1650-1655. （Jiang Peng， Qin Xiaolin. Adaptive video keyframe extraction based on visual attention model ［J］. Journal of Image and Graphics， 2009， 14（8）： 1650-1655.）

［18］艾青林， 余杰， 胡克用， 等. 基于ORB關(guān)鍵幀匹配算法的機器人SLAM實現(xiàn) ［J］. 機電工程， 2016， 33（5）： 513-520. （Ai Qinglin， Yu Jie， Hu Keyong， et al. Implementation of robot SLAM based on ORB keyframe matching algorithm ［J］. Electromechanical Engineering， 2016， 33（5）： 513-520.）

［19］陳世浪， 吳俊君. 基于RGB-D相機的SLAM技術(shù)研究綜述 ［J］. 計算機工程與應(yīng)用， 2019， 55（7）： 30-39，126. （Chen Shilang， Wu Junjun. A survey of SLAM technology based on RGB-D camera ［J］. Computer Engineering and Applications， 2019， 55（7）： 30-39，126.）

［20］Wang Yuping， Zou Zixin， Wang Cong， et al. ORBBuf： a robust buffering method for remote visual SLAM ［C］//Proc of IEEE/RSJ International Conference on Intelligent Robots and Systems. Pisca-taway， NJ： IEEE Press， 2021： 8706-8713.

［21］Schmuck P， Chli M. CCM-SLAM： robust and efficient centralized collaborative monocular simultaneous localization and mapping for robotic teams ［J］. Journal of Field Robotics， 2019， 36（4）： 763-781.

［22］Chen Yongbo， Zhao Liang， Lee K M B， et al. Broadcast your weaknesses： cooperative active pose-graph SLAM for multiple robots ［J］. IEEE Robotics and Automation Letters， 2020， 5（2）： 2200-2207.

［23］Khosoussi K， Huang S， Dissanayake G. Tree-connectivity： evaluating the graphical structure of SLAM ［C］// Proc of IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE Press， 2016： 1316-1322.

［24］Scargill T， Premsankar G， Chen J， et al. Here to stay： a quantitative comparison of virtual object stability in markerless mobile AR ［C］// Proc of the 2nd International Workshop on Cyber-Physical-Human System Design and Implementation. Piscataway， NJ： IEEE Press， 2022： 24-29.

［25］Tang Jiexiong， Ericson L， Folkesson J， et al. GCNv2： efficient correspondence prediction for real-time SLAM ［J］. IEEE Robotics and Automation Letters， 2019， 4（4）： 3505-3512.

［26］Fu Qiang，Yu Hongshan，Wang Xiaolong， et al. Fast ORB-SLAM without keypoint descriptors ［J］. IEEE Trans on Image Proces-sing， 2021， 31： 1433-1446.

［27］Gomez-Ojeda R， Moreno F A， Zuniga-Nol D， et al. PL-SLAM： a stereo SLAM system through the combination of points and line segments ［J］. IEEE Trans on Robotics， 2019， 35（3）： 734-746.

［28］伍曉東， 張松柏， 湯適榮， 等. 基于改進關(guān)鍵幀選擇的ORB-SLAM3算法 ［J］. 計算機應(yīng)用研究， 2023， 40（5）： 1428-1433. （Wu Xiaodong， Zhang Songbai， Tang Shirong， et al. ORB-SLAM3 algorithm based on improved keyframe selection ［J］. Application Research of Computers， 2023， 40（5）： 1428-1433.）