楊鋮,侯耀斐

（1.烏江能源投資有限公司，貴州 貴陽(yáng) 550081；2.貴州大學(xué)現(xiàn)代制造技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，貴州 貴陽(yáng) 550025；3.貴州省裝備制造數(shù)字化車(chē)間建模與仿真工程研究中心，貴州 貴陽(yáng) 550025）

0 引言

在移動(dòng)機(jī)器人路徑規(guī)劃研究領(lǐng)域，構(gòu)建精確穩(wěn)定的三維環(huán)境地圖對(duì)于機(jī)器人路徑規(guī)劃的成功至關(guān)重要，已引起國(guó)內(nèi)外學(xué)者的廣泛關(guān)注。但在動(dòng)態(tài)變化場(chǎng)景中，機(jī)器人傳感器位姿與場(chǎng)景的對(duì)應(yīng)關(guān)系難以穩(wěn)定，這給網(wǎng)格地圖障礙物信息的感知分析帶來(lái)了困難[1]。同時(shí)，由于機(jī)器人需要在不斷變化的環(huán)境中獲取實(shí)時(shí)的傳感器數(shù)據(jù)并進(jìn)行處理，以便于準(zhǔn)確地更新自身位姿，在場(chǎng)景復(fù)雜、安全性要求較高時(shí)難以保證路徑規(guī)劃的實(shí)時(shí)性[2]。因此，路徑規(guī)劃方法需要在地圖網(wǎng)格精度與路徑更新實(shí)時(shí)性之間做出權(quán)衡。

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

在實(shí)際工程應(yīng)用中，運(yùn)動(dòng)目標(biāo)的形貌、體積和姿態(tài)是路徑選擇的直接影響因素，面對(duì)三維復(fù)雜場(chǎng)景，越來(lái)越多的深度學(xué)習(xí)路徑規(guī)劃算法涌現(xiàn)出來(lái)。鄭紅波等人[6]在傳統(tǒng)A*算法進(jìn)行路徑規(guī)劃的基礎(chǔ)上，提出了一種基于層次包圍盒碰撞檢測(cè)的實(shí)時(shí)路徑規(guī)劃優(yōu)化算法，在時(shí)間效率和規(guī)劃效果上都有一定的優(yōu)勢(shì)。Pinto 等人[7]提出了一種新的視覺(jué)引導(dǎo)路徑規(guī)劃系統(tǒng)（V-GPP），它確定了一個(gè)無(wú)碰撞的三維軌跡，考慮了障礙與場(chǎng)景的相互作用。Smith 等人[8]開(kāi)發(fā)了一種新的基于啟發(fā)式的多視點(diǎn)立體重建質(zhì)量連續(xù)優(yōu)化方法，并將其應(yīng)用于路徑規(guī)劃問(wèn)題，其規(guī)劃速率達(dá)到毫秒級(jí)。Koch 等人[9]提出了一個(gè)三維路徑規(guī)劃框架，設(shè)計(jì)用于詳細(xì)和完整的小規(guī)模三維重建。Kompis 等人[10]提出了一種在線路徑規(guī)劃算法，用于快速探索和規(guī)劃先前未知興趣區(qū)域的三維場(chǎng)景路徑。

綜上，三維場(chǎng)景下的路徑規(guī)劃需要考慮移動(dòng)機(jī)器人本身位姿與環(huán)境之間的碰撞關(guān)系，以及移動(dòng)軌跡與場(chǎng)景之間的相互作用。結(jié)合目前醫(yī)療環(huán)境所面臨的配送遲緩性問(wèn)題，本文將利用深度學(xué)習(xí)方法解決重建場(chǎng)景下的高效路徑更新問(wèn)題作為研究方向，在三維重建場(chǎng)景的基礎(chǔ)上實(shí)現(xiàn)環(huán)境知情、安全和可解釋的探索映射及路徑規(guī)劃。

1 NeRF-Navigation

神經(jīng)輻射場(chǎng)（NeRF）是從觀測(cè)圖像中學(xué)習(xí)三維場(chǎng)景表征來(lái)進(jìn)行新視圖重建的神經(jīng)網(wǎng)絡(luò)模型，結(jié)構(gòu)分為正余弦位置編碼網(wǎng)絡(luò)、特征提取網(wǎng)絡(luò)和體積渲染網(wǎng)絡(luò)，整體網(wǎng)絡(luò)結(jié)構(gòu)如圖1所示。其中，特征提取網(wǎng)絡(luò)包含兩個(gè)MLP 模型，一個(gè)為8 層全連接的采樣MLP 模型，另一個(gè)為3層全連接的推理MLP模型。

圖1 NeRF網(wǎng)絡(luò)結(jié)構(gòu)圖

NeRF-Navigation 是一種基于神經(jīng)輻射三維重建的路徑規(guī)劃方法，該方法將NeRF 生成的隱式采樣模型等價(jià)為空間中網(wǎng)格的存在概率，從而使機(jī)器人可以在只使用單目相機(jī)視覺(jué)傳感器的情況下進(jìn)行導(dǎo)航。整體網(wǎng)絡(luò)結(jié)構(gòu)如圖2 所示，主要包括NeRF 場(chǎng)景表示模塊和機(jī)器人狀態(tài)信息更新模塊，通過(guò)預(yù)測(cè)機(jī)器人的軌跡生成控制指令。其中，軌跡預(yù)測(cè)和控制指令生成分別通過(guò)基于梯度的優(yōu)化器和深度逆強(qiáng)化學(xué)習(xí)（Deep Inverse Reinforcement Learning）實(shí)現(xiàn)。

圖2 NeRF-Navigation網(wǎng)絡(luò)結(jié)構(gòu)圖

NeRF-Navigation采用iNeRF的思想，假設(shè)由Fθ參數(shù)化的場(chǎng)景NeRF已經(jīng)存在，然而觀測(cè)圖像I的相機(jī)姿態(tài)Vd尚未確定，根據(jù)極大似然估計(jì)原理，通過(guò)反向推理一個(gè)訓(xùn)練過(guò)的NeRF進(jìn)行六自由度姿態(tài)估計(jì)，如式(1)所示：

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

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

式中，w?τ為微分平坦?fàn)顟B(tài)wτ的位置分量，Rτ為機(jī)器人坐標(biāo)到世界坐標(biāo)的旋轉(zhuǎn)矩陣，Γ為控制懲罰權(quán)重的對(duì)角矩陣，uτ表示機(jī)器人行為的微分平坦?fàn)顟B(tài)量。

為了更新規(guī)劃路徑，NeRF-Navigation制定了一個(gè)狀態(tài)估計(jì)濾波器，當(dāng)機(jī)器人根據(jù)控制指令運(yùn)行一個(gè)時(shí)間步長(zhǎng)后，估計(jì)器從機(jī)載單目相機(jī)獲取一張當(dāng)前視角的新圖像，通過(guò)遞歸貝葉斯濾波方法傳播估計(jì)結(jié)果與不確定性建立動(dòng)力學(xué)模型聯(lián)系。同時(shí)，使用圖像特征檢測(cè)器（如ORB）識(shí)別特征點(diǎn)，使機(jī)器人轉(zhuǎn)向更高梯度信息的區(qū)域周?chē)M(jìn)行采樣。

2 基于改進(jìn)NeRF-Navigation的三維路徑規(guī)劃

本文關(guān)注室內(nèi)環(huán)境移動(dòng)機(jī)器人的路徑規(guī)劃，對(duì)于給定起始位置與目標(biāo)位置，在三維網(wǎng)格地圖中使用A*搜索初始化路徑，并采用一個(gè)適用于移動(dòng)機(jī)器人的三自由度姿態(tài)過(guò)濾器來(lái)跟蹤初始化軌跡。NeRFNavigation 根據(jù)每個(gè)時(shí)間步長(zhǎng)中可用的有限信息控制移動(dòng)機(jī)器人進(jìn)一步的行為決策，本文在每一步的規(guī)劃中，引入貪婪信息增益策略，對(duì)于一組采樣點(diǎn)N，通過(guò)比較其與當(dāng)前姿態(tài)連接的行為效益，選擇生成路徑的最佳視角；同時(shí)，使用動(dòng)態(tài)權(quán)重的啟發(fā)式函數(shù)提高路徑搜索效率。通過(guò)兩種策略的結(jié)合使用，提出一種具有隱式環(huán)境適應(yīng)的動(dòng)態(tài)加權(quán)A*路徑規(guī)劃方法，網(wǎng)絡(luò)結(jié)構(gòu)如圖3所示：

圖3 改進(jìn)NeRF-Navigation算法網(wǎng)絡(luò)結(jié)構(gòu)圖

2.1 貪婪信息增益策略

在基于采樣的規(guī)劃中，移動(dòng)機(jī)器人以當(dāng)前姿態(tài)的地圖空間視角為觀測(cè)圖像采樣視點(diǎn)，通過(guò)計(jì)算每個(gè)視點(diǎn)的信息增益、消耗成本確定最終組合效益，約束當(dāng)前節(jié)點(diǎn)的視角指向最佳候選點(diǎn)并執(zhí)行動(dòng)作，從而提高搜索的準(zhǔn)確性和效率。

本文采用了一種基于采樣的Next-best-view（NBV）規(guī)劃策略。當(dāng)機(jī)器人向當(dāng)前的NBV移動(dòng)時(shí)，計(jì)算觀測(cè)圖像平面內(nèi)視點(diǎn)集{Ni}的綜合效益。每個(gè)視點(diǎn)Ni={ga(Ni),ct(Ni),u(Ni)}都包含信息增益ga、消耗成本ct及綜合行為效益u，信息增益ga估計(jì)了訪問(wèn)一個(gè)視點(diǎn)Ni的積極影響，消耗成本ct估計(jì)了訪問(wèn)一個(gè)視點(diǎn)Ni的負(fù)面影響，最后，綜合行為效益u將信息增益和消耗成本合并成統(tǒng)一的優(yōu)化目標(biāo)。本文使用每次移動(dòng)的執(zhí)行時(shí)間作為其消耗成本，以作計(jì)算質(zhì)量與計(jì)算效率的權(quán)衡，通過(guò)估計(jì)執(zhí)行時(shí)間內(nèi)動(dòng)力學(xué)模型的位置和方向，得到采樣點(diǎn)集的綜合行為效益，如式(4)所示：

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

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

式中，visible(Vd)表示從姿態(tài)Vd中可見(jiàn)的所有采樣圖像平面的集合，Vd(Ni)是視點(diǎn)Ni的NBV 姿態(tài)，I(u)是采樣圖像平面內(nèi)連接像素點(diǎn)的梯度信息。采用文獻(xiàn)[9]中的方向優(yōu)化方法來(lái)選擇每個(gè)位置移動(dòng)機(jī)器人的偏航角，如式(6)所示：

式中，xNi，yNi表示輪式移動(dòng)機(jī)器人在視點(diǎn)Ni處的位置分量，ψ表示機(jī)器人的轉(zhuǎn)角，這些參數(shù)可以在每個(gè)視點(diǎn)上貪婪地優(yōu)化，而每個(gè)采樣圖像平面是獨(dú)立的，因此姿態(tài)的最佳偏航會(huì)自動(dòng)使等式(6)最大化。

visible(Vd)是使用迭代光線投射方法得到的，而光線投射期間是否存在精確的網(wǎng)格先驗(yàn)會(huì)導(dǎo)致不同的選擇結(jié)果。如果光線終止在高質(zhì)量的占用網(wǎng)格預(yù)測(cè)上，說(shuō)明visible(Vd)更優(yōu)地反映了真實(shí)場(chǎng)景，從而導(dǎo)致更準(zhǔn)確的增益估計(jì)。然而，如果有錯(cuò)誤的預(yù)測(cè)，移動(dòng)機(jī)器人則放棄該傳感器視圖，并重新規(guī)劃預(yù)測(cè)。

2.2 動(dòng)態(tài)權(quán)重策略

為了加快搜索速度，基于A*算法引入了動(dòng)態(tài)加權(quán)策略。該策略旨在通過(guò)自適應(yīng)調(diào)整啟發(fā)式函數(shù)的權(quán)重以平衡探索和行動(dòng)，以應(yīng)對(duì)不確定性和變化的環(huán)境帶來(lái)的挑戰(zhàn)。A*算法是一種基于Dijkstra發(fā)展而來(lái)的啟發(fā)式搜索算法，其關(guān)鍵是確定啟發(fā)式函數(shù)，形式如式(7)所示：

由于啟發(fā)式函數(shù)h(n)采用歐幾里得距離，其值始終小于或等于從當(dāng)前節(jié)點(diǎn)nc到目標(biāo)點(diǎn)的實(shí)際距離，當(dāng)節(jié)點(diǎn)n到終點(diǎn)的距離較遠(yuǎn)時(shí)，h(n) ?g(n)，將增加算法搜索節(jié)點(diǎn)數(shù)量并降低效率。本文對(duì)A*算法中的啟發(fā)式函數(shù)h(n)進(jìn)行了改進(jìn)。通過(guò)對(duì)h(n)的修改來(lái)控制后續(xù)節(jié)點(diǎn)的選擇過(guò)程，限制了A*算法的節(jié)點(diǎn)擴(kuò)展方向和深度，確保了下一個(gè)搜索節(jié)點(diǎn)nt的方向不會(huì)過(guò)度發(fā)散。

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

圖4 路徑點(diǎn)幾何表示

通過(guò)比較r1、r2，得到跟隨節(jié)點(diǎn)變化的動(dòng)態(tài)參數(shù)ke及啟發(fā)式函數(shù)的加權(quán)因子K(n)，形式如式(10)～(11)所示：

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

本文使用上述兩種策略共同作用以達(dá)到平衡路徑代價(jià)和平滑程度的目的，鼓勵(lì)移動(dòng)機(jī)器人優(yōu)先考慮高確定性預(yù)測(cè)區(qū)域，使其隱式地適應(yīng)環(huán)境的類(lèi)型。例如，在直線走廊中，機(jī)器人傾向于沿中心直線移動(dòng)，因?yàn)檫@可以最大程度地觀測(cè)到直線走廊的全貌；而在障礙物較多的復(fù)雜場(chǎng)景中，機(jī)器人傾向于沿障礙物采樣點(diǎn)占比較大的觀測(cè)視角方向移動(dòng)。此時(shí)，移動(dòng)機(jī)器人能在未知其所處環(huán)境的情況下選擇一條較優(yōu)路徑。

3 實(shí)驗(yàn)結(jié)果與分析

為驗(yàn)證本文改進(jìn)方法的有效性，在實(shí)驗(yàn)室大型室內(nèi)場(chǎng)景、觀察病房走廊縱深場(chǎng)景、一號(hào)病室中型室內(nèi)場(chǎng)景和治療室小型室內(nèi)場(chǎng)景中，將所提方法與NeRFNavigation 算法進(jìn)行了比較，不同網(wǎng)絡(luò)的重建場(chǎng)景路徑規(guī)劃結(jié)果如圖5所示，左側(cè)圖像為原始算法的路徑規(guī)劃結(jié)果，右側(cè)圖像為改進(jìn)算法的路徑規(guī)劃結(jié)果，與結(jié)果相關(guān)的定量指標(biāo)見(jiàn)表1，采用加粗字體表示較優(yōu)指標(biāo)。

表1 不同方法綜合性能對(duì)比

圖5 4種不同環(huán)境下路徑規(guī)劃對(duì)比圖

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

表1 展示了原始NeRF-Navigation 與本文改進(jìn)方法的綜合性能對(duì)比，包括成功路徑實(shí)現(xiàn)（SPL）、路徑初始化時(shí)間、姿態(tài)估計(jì)時(shí)間和路徑更新時(shí)間。由表中數(shù)據(jù)分析可知，相較于對(duì)比實(shí)驗(yàn)組，本文改進(jìn)方法以平均0.2s 的姿態(tài)估計(jì)時(shí)間增量換取了平均-3.5s 的路徑初始化時(shí)間增量和平均-0.8s 的路徑更新時(shí)間增量，且成功路徑實(shí)現(xiàn)的比例提高了13%，在所有實(shí)驗(yàn)場(chǎng)景下的性能指標(biāo)均優(yōu)于對(duì)比實(shí)驗(yàn)組，這表明本文所提網(wǎng)絡(luò)較對(duì)比網(wǎng)絡(luò)能有效提升重建場(chǎng)景下的路徑規(guī)劃性能。

圖6 展示了實(shí)驗(yàn)室環(huán)境下移動(dòng)機(jī)器人在同一位置的姿態(tài)估計(jì)準(zhǔn)確性，左圖代表原始算法，右圖代表改進(jìn)算法。因?qū)嶋H路徑點(diǎn)不同，故難以選取完全一致的觀測(cè)角度，這里選取二者實(shí)際路徑上的接近路徑點(diǎn)進(jìn)行對(duì)比，藍(lán)色曲線表示實(shí)際姿態(tài)，橙色曲線表示估計(jì)姿態(tài)。實(shí)驗(yàn)結(jié)果表明，改進(jìn)算法實(shí)現(xiàn)了更貼合實(shí)際的姿態(tài)估計(jì)，可增強(qiáng)路徑更新的安全性與效率。

圖6 實(shí)驗(yàn)室環(huán)境下姿態(tài)估計(jì)對(duì)比圖

4 結(jié)論

針對(duì)醫(yī)療環(huán)境光線行進(jìn)較為復(fù)雜的特點(diǎn)，本文使用Nerfstudio 框架對(duì)重建場(chǎng)景邊界進(jìn)行限定，生成了高保真的三維網(wǎng)格地圖作為三維路徑規(guī)劃的基礎(chǔ)。在NeRF-Navigation 的基礎(chǔ)上，提出一種具有隱式環(huán)境適應(yīng)的動(dòng)態(tài)加權(quán)A*路徑規(guī)劃方法，并在實(shí)驗(yàn)室場(chǎng)景與自建醫(yī)療場(chǎng)景進(jìn)行改進(jìn)算法的實(shí)驗(yàn)訓(xùn)練與測(cè)試。實(shí)驗(yàn)結(jié)果表明，本文所提方法的三維路徑規(guī)劃各項(xiàng)評(píng)價(jià)指標(biāo)相較于NeRF-Navigation 均有提高，能有效降低規(guī)劃路徑的曲率，提高規(guī)劃效率，可以用來(lái)指導(dǎo)機(jī)器人在不同類(lèi)型醫(yī)療場(chǎng)景下進(jìn)行自適應(yīng)三維路徑規(guī)劃，即使在復(fù)雜的環(huán)境中也能顯示出可靠的路徑覆蓋。