基于改進NeRF-Navigation的移動機器人三維路徑規劃方法

楊鋮,侯耀斐

（1.烏江能源投資有限公司，貴州 貴陽 550081；2.貴州大學現代制造技術教育部重點實驗室，貴州 貴陽 550025；3.貴州省裝備制造數字化車間建模與仿真工程研究中心，貴州 貴陽 550025）

0 引言

在移動機器人路徑規劃研究領域，構建精確穩定的三維環境地圖對于機器人路徑規劃的成功至關重要，已引起國內外學者的廣泛關注。但在動態變化場景中，機器人傳感器位姿與場景的對應關系難以穩定，這給網格地圖障礙物信息的感知分析帶來了困難[1]。同時，由于機器人需要在不斷變化的環境中獲取實時的傳感器數據并進行處理，以便于準確地更新自身位姿，在場景復雜、安全性要求較高時難以保證路徑規劃的實時性[2]。因此，路徑規劃方法需要在地圖網格精度與路徑更新實時性之間做出權衡。

早期的人工勢場路徑規劃研究是針對靜態環境而設計的，障礙物和目標物均被視為靜態元素。然而，這種做法忽略了現實環境的動態性和變化性，因此不能適應復雜多變的實際場景。Fujimura 等人[3]提出了相對動態的人工勢場方法來解決移動機器人的動態路徑規劃問題，利用靜態路徑規劃的思想，通過將時間視為規劃模型的一維參數，實現了對動態環境下移動障礙物的處理。Ge 等人[4]在吸引勢函數中關聯移動機器人與目標物的相對位置與相對速度，在排斥勢函數中關聯移動機器人與障礙物的相對位置與相對速度，提出動態環境下的機器人路徑規劃算法。馬小陸[5]提出了基于勢場跳點的蟻群算法,該算法能夠有效地減少收斂迭代的次數，縮短搜索收斂的時間，且搜索路徑更優。

在實際工程應用中，運動目標的形貌、體積和姿態是路徑選擇的直接影響因素，面對三維復雜場景，越來越多的深度學習路徑規劃算法涌現出來。鄭紅波等人[6]在傳統A*算法進行路徑規劃的基礎上，提出了一種基于層次包圍盒碰撞檢測的實時路徑規劃優化算法，在時間效率和規劃效果上都有一定的優勢。Pinto 等人[7]提出了一種新的視覺引導路徑規劃系統（V-GPP），它確定了一個無碰撞的三維軌跡，考慮了障礙與場景的相互作用。Smith 等人[8]開發了一種新的基于啟發式的多視點立體重建質量連續優化方法，并將其應用于路徑規劃問題，其規劃速率達到毫秒級。Koch 等人[9]提出了一個三維路徑規劃框架，設計用于詳細和完整的小規模三維重建。Kompis 等人[10]提出了一種在線路徑規劃算法，用于快速探索和規劃先前未知興趣區域的三維場景路徑。

綜上，三維場景下的路徑規劃需要考慮移動機器人本身位姿與環境之間的碰撞關系，以及移動軌跡與場景之間的相互作用。結合目前醫療環境所面臨的配送遲緩性問題，本文將利用深度學習方法解決重建場景下的高效路徑更新問題作為研究方向，在三維重建場景的基礎上實現環境知情、安全和可解釋的探索映射及路徑規劃。

1 NeRF-Navigation

神經輻射場（NeRF）是從觀測圖像中學習三維場景表征來進行新視圖重建的神經網絡模型，結構分為正余弦位置編碼網絡、特征提取網絡和體積渲染網絡，整體網絡結構如圖1所示。其中，特征提取網絡包含兩個MLP 模型，一個為8 層全連接的采樣MLP 模型，另一個為3層全連接的推理MLP模型。

圖1 NeRF網絡結構圖

NeRF-Navigation 是一種基于神經輻射三維重建的路徑規劃方法，該方法將NeRF 生成的隱式采樣模型等價為空間中網格的存在概率，從而使機器人可以在只使用單目相機視覺傳感器的情況下進行導航。整體網絡結構如圖2 所示，主要包括NeRF 場景表示模塊和機器人狀態信息更新模塊，通過預測機器人的軌跡生成控制指令。其中，軌跡預測和控制指令生成分別通過基于梯度的優化器和深度逆強化學習（Deep Inverse Reinforcement Learning）實現。

圖2 NeRF-Navigation網絡結構圖

NeRF-Navigation采用iNeRF的思想，假設由Fθ參數化的場景NeRF已經存在，然而觀測圖像I的相機姿態Vd尚未確定，根據極大似然估計原理，通過反向推理一個訓練過的NeRF進行六自由度姿態估計，如式(1)所示：

然而，NeRF采樣模型僅表示空間中幾何點截停光線的微分概率，無法作為場景查詢的顯式約束，故NeRF-Navigation 將光線的終止概率等價為質量粒子的終止概率，借助三維網格地圖令機器人在未被占用的NeRF 表達空間中導航，以到達給定目標點。其中，機器人的外形由其邊界點集B 組成的三維網格近似表示，則t時刻的碰撞概率，如式(2)所示：

為了加快規劃速度，NeRF-Navigation采用了基于微分平坦性的A*路徑規劃方法，引入碰撞懲罰和控制懲罰約束機器人的姿態，通過梯度優化探索一組平坦連續的輸出路徑點W={w0,…,wn}以最小化懲罰項，如式(3)所示：

式中，w?τ為微分平坦狀態wτ的位置分量，Rτ為機器人坐標到世界坐標的旋轉矩陣，Γ為控制懲罰權重的對角矩陣，uτ表示機器人行為的微分平坦狀態量。

為了更新規劃路徑，NeRF-Navigation制定了一個狀態估計濾波器，當機器人根據控制指令運行一個時間步長后，估計器從機載單目相機獲取一張當前視角的新圖像，通過遞歸貝葉斯濾波方法傳播估計結果與不確定性建立動力學模型聯系。同時，使用圖像特征檢測器（如ORB）識別特征點，使機器人轉向更高梯度信息的區域周圍進行采樣。

2 基于改進NeRF-Navigation的三維路徑規劃

本文關注室內環境移動機器人的路徑規劃，對于給定起始位置與目標位置，在三維網格地圖中使用A*搜索初始化路徑，并采用一個適用于移動機器人的三自由度姿態過濾器來跟蹤初始化軌跡。NeRFNavigation 根據每個時間步長中可用的有限信息控制移動機器人進一步的行為決策，本文在每一步的規劃中，引入貪婪信息增益策略，對于一組采樣點N，通過比較其與當前姿態連接的行為效益，選擇生成路徑的最佳視角；同時，使用動態權重的啟發式函數提高路徑搜索效率。通過兩種策略的結合使用，提出一種具有隱式環境適應的動態加權A*路徑規劃方法，網絡結構如圖3所示：

圖3 改進NeRF-Navigation算法網絡結構圖

2.1 貪婪信息增益策略

在基于采樣的規劃中，移動機器人以當前姿態的地圖空間視角為觀測圖像采樣視點，通過計算每個視點的信息增益、消耗成本確定最終組合效益，約束當前節點的視角指向最佳候選點并執行動作，從而提高搜索的準確性和效率。

本文采用了一種基于采樣的Next-best-view（NBV）規劃策略。當機器人向當前的NBV移動時，計算觀測圖像平面內視點集{Ni}的綜合效益。每個視點Ni={ga(Ni),ct(Ni),u(Ni)}都包含信息增益ga、消耗成本ct及綜合行為效益u，信息增益ga估計了訪問一個視點Ni的積極影響，消耗成本ct估計了訪問一個視點Ni的負面影響，最后，綜合行為效益u將信息增益和消耗成本合并成統一的優化目標。本文使用每次移動的執行時間作為其消耗成本，以作計算質量與計算效率的權衡，通過估計執行時間內動力學模型的位置和方向，得到采樣點集的綜合行為效益，如式(4)所示：

式中，sampling(Ni)表示觀測圖像平面內的采樣點集，path(Nj)表示連接到當前姿態的節點集，節點被不斷地重新連接計算，促使觀測圖像平面內的u最大化。即在每個時間步長識別出一個最佳觀測視角，其中的節點序列使總累積信息增益與總累積消耗成本之比最大化。

基于上述策略，本文使用移動機器人傳感器正向模擬來計算信息增益ga(Ni)，如式(5)所示：

式中，visible(Vd)表示從姿態Vd中可見的所有采樣圖像平面的集合，Vd(Ni)是視點Ni的NBV 姿態，I(u)是采樣圖像平面內連接像素點的梯度信息。采用文獻[9]中的方向優化方法來選擇每個位置移動機器人的偏航角，如式(6)所示：

式中，xNi，yNi表示輪式移動機器人在視點Ni處的位置分量，ψ表示機器人的轉角，這些參數可以在每個視點上貪婪地優化，而每個采樣圖像平面是獨立的，因此姿態的最佳偏航會自動使等式(6)最大化。

visible(Vd)是使用迭代光線投射方法得到的，而光線投射期間是否存在精確的網格先驗會導致不同的選擇結果。如果光線終止在高質量的占用網格預測上，說明visible(Vd)更優地反映了真實場景，從而導致更準確的增益估計。然而，如果有錯誤的預測，移動機器人則放棄該傳感器視圖，并重新規劃預測。

2.2 動態權重策略

為了加快搜索速度，基于A*算法引入了動態加權策略。該策略旨在通過自適應調整啟發式函數的權重以平衡探索和行動，以應對不確定性和變化的環境帶來的挑戰。A*算法是一種基于Dijkstra發展而來的啟發式搜索算法，其關鍵是確定啟發式函數，形式如式(7)所示：

由于啟發式函數h(n)采用歐幾里得距離，其值始終小于或等于從當前節點nc到目標點的實際距離，當節點n到終點的距離較遠時，h(n) ?g(n)，將增加算法搜索節點數量并降低效率。本文對A*算法中的啟發式函數h(n)進行了改進。通過對h(n)的修改來控制后續節點的選擇過程，限制了A*算法的節點擴展方向和深度，確保了下一個搜索節點nt的方向不會過度發散。

以起點ns與終點nd的距離R為直徑做圓，以作策略幾何示意，如圖4 所示，其中點記為no，nc為當前節點，n'為下一搜索節點，r1為起點ns與終點nd間曼哈頓距離的一半，r2為當前節點nc與中點no的距離，形式如式(8)～(9)所示：

圖4 路徑點幾何表示

通過比較r1、r2，得到跟隨節點變化的動態參數ke及啟發式函數的加權因子K(n)，形式如式(10)～(11)所示：

故將新的啟發式函數定義為f(n) =g(n) +K(n)*h(n)，通過對h(n)的修改來控制后續節點的選擇，確保下一搜索節點n'的方向不會過度發散。分析可知，r1在節點探索過程中保持不變，r2總體上具有先減小后增大的連續變化趨勢，則ke在節點探索過程中先表現出擴張性，隨后在no附近開始呈現收縮趨勢。通過實施該策略，使節點探索過程中啟發式函數h(n)的比重增加，減少A*算法的搜索區域，加快搜索速度，且動態權重變化較為緩和，可以得到較優路徑。

本文使用上述兩種策略共同作用以達到平衡路徑代價和平滑程度的目的，鼓勵移動機器人優先考慮高確定性預測區域，使其隱式地適應環境的類型。例如，在直線走廊中，機器人傾向于沿中心直線移動，因為這可以最大程度地觀測到直線走廊的全貌；而在障礙物較多的復雜場景中，機器人傾向于沿障礙物采樣點占比較大的觀測視角方向移動。此時，移動機器人能在未知其所處環境的情況下選擇一條較優路徑。

3 實驗結果與分析

為驗證本文改進方法的有效性，在實驗室大型室內場景、觀察病房走廊縱深場景、一號病室中型室內場景和治療室小型室內場景中，將所提方法與NeRFNavigation 算法進行了比較，不同網絡的重建場景路徑規劃結果如圖5所示，左側圖像為原始算法的路徑規劃結果，右側圖像為改進算法的路徑規劃結果，與結果相關的定量指標見表1，采用加粗字體表示較優指標。

表1 不同方法綜合性能對比

圖5 4種不同環境下路徑規劃對比圖

圖5 展示了實驗室場景、觀察病房走廊場景、一號病室場景和治療室場景下的三維路徑規劃定性結果，圖中第一列為NeRF-Navigation 路徑規劃結果，第二列為本文所提方法路徑規劃結果。由圖中4種類型的重建場景路徑規劃對比結果可以直觀地看出，相較于原始算法，本文改進算法可以實現較為平滑的更新路徑。在相對空曠處，改進算法更傾向于朝前進方向沿直線規劃；在避障轉角處，改進算法能規劃出較為平滑的曲線路徑，且4類場景都能保持較優的路徑覆蓋，證明了所提算法的有效性和魯棒性。

表1 展示了原始NeRF-Navigation 與本文改進方法的綜合性能對比，包括成功路徑實現（SPL）、路徑初始化時間、姿態估計時間和路徑更新時間。由表中數據分析可知，相較于對比實驗組，本文改進方法以平均0.2s 的姿態估計時間增量換取了平均-3.5s 的路徑初始化時間增量和平均-0.8s 的路徑更新時間增量，且成功路徑實現的比例提高了13%，在所有實驗場景下的性能指標均優于對比實驗組，這表明本文所提網絡較對比網絡能有效提升重建場景下的路徑規劃性能。

圖6 展示了實驗室環境下移動機器人在同一位置的姿態估計準確性，左圖代表原始算法，右圖代表改進算法。因實際路徑點不同，故難以選取完全一致的觀測角度，這里選取二者實際路徑上的接近路徑點進行對比，藍色曲線表示實際姿態，橙色曲線表示估計姿態。實驗結果表明，改進算法實現了更貼合實際的姿態估計，可增強路徑更新的安全性與效率。

圖6 實驗室環境下姿態估計對比圖

4 結論

針對醫療環境光線行進較為復雜的特點，本文使用Nerfstudio 框架對重建場景邊界進行限定，生成了高保真的三維網格地圖作為三維路徑規劃的基礎。在NeRF-Navigation 的基礎上，提出一種具有隱式環境適應的動態加權A*路徑規劃方法，并在實驗室場景與自建醫療場景進行改進算法的實驗訓練與測試。實驗結果表明，本文所提方法的三維路徑規劃各項評價指標相較于NeRF-Navigation 均有提高，能有效降低規劃路徑的曲率，提高規劃效率，可以用來指導機器人在不同類型醫療場景下進行自適應三維路徑規劃，即使在復雜的環境中也能顯示出可靠的路徑覆蓋。