楊 彪，王 狄，沈紹博，楊長春+，劉小峰

(1.常州大學 微電子與控制工程學院，江蘇 常州 213164；2.河海大學 物聯(lián)網(wǎng)工程學院，江蘇 常州 213022)

0 引 言

室內(nèi)外環(huán)境的復雜性為移動機器人的精準跟隨提出了挑戰(zhàn)。為了更好地完成行人跟隨任務，需要精確測量人-機距離[1，2]，根據(jù)距離和偏航角調(diào)整機器人運動狀態(tài)，從而魯棒地跟隨行人。

為了準確測量前人距離，研究人員使用多傳感器融合的方法來檢測和跟蹤行人[3，4]。Tripathi等[5]使用超聲波和紅外傳感器跟隨指定行人。隨著激光雷達技術(shù)的發(fā)展，可以實現(xiàn)遠距離人體目標的測距[6，7]。Chebotareva等[8]基于激光測距儀和單目相機的數(shù)據(jù)融合方法，提出了人體跟蹤算法。Zhu等[9]使用毫米波雷達和單目相機的信息感知環(huán)境，使機器人以預期的距離跟蹤人體目標。毫米波雷達和激光雷達可以直接獲取目標位置，但是兩者成像分辨率低，難以區(qū)分物體邊界，而多傳感器融合方法因其高昂的售價難以在移動機器人中普及[10]。

本文基于低成本的RGB-D相機，搭建了移動機器人平臺，提出了一種基于MRSD測距的移動機器人跟隨系統(tǒng)，主要工作在于：①本文引入Mask R-CNN算法[11]對行人進行實例分割，根據(jù)行人掩膜從深度圖像中提取深度像素點，測量行人距離。②針對深度偽影問題，當前工作多使用傳統(tǒng)濾波算法[12]，導致測距精度低。本文引入S2R-DepthNet網(wǎng)絡[13]從彩色圖像中推理深度，替換相機深度圖像中的無效點，提高測距精度。③針對深度融合的測距信息，建立基于Sage-Husa自適應濾波器的測距模型，抑制量測異常值的影響，從而使得移動機器人能夠更加魯棒地跟隨行人。

1 算法理論基礎

1.1 實例分割算法

實例分割是計算機視覺領域經(jīng)典任務之一。在移動機器人領域，實例分割可以分析場景，檢測場景中的不同實例并完成像素層面的分類。隨著深度學習技術(shù)的發(fā)展，科研工作者相繼提出了Faster R-CNN[14]、Cascade R-CNN[15]和YOLO系列[16，17]網(wǎng)絡模型。相比于傳統(tǒng)目標檢測算法，上述網(wǎng)絡模型在準確性有著顯著優(yōu)勢。但是，上述算法的目標檢測框無法準確分離物體的前景和背景，從而降低深度測距精度。因此，本文引入Mask R-CNN模型，檢測場景中的行人目標，獲得行人掩膜。

通過預訓練的RPN獲得行人目標檢測框、使用ROIPool從檢測框中提取特征，在進行分類和邊界框回歸的同時輸出二進制掩膜。在具體訓練過程中，分類損失函數(shù)Lc和邊界框回歸損失函數(shù)Lb的運算如下

(1)

(2)

其中，s是分類器預測的softmax概率分布，u為目標真實類別標簽，tu為預測回歸參數(shù)，v為真實目標邊界框參數(shù)。

Mask的損失函數(shù)Lm為

Lm=-ylog(p(x))-(1-y)log(1-p(x))

(3)

式中：p(x) 是模型輸出，y是真實標簽。

與單任務網(wǎng)絡相比，多任務網(wǎng)絡共享特征提取層參數(shù)，從而減小模型參數(shù)和計算量，完成檢測和分割任務。

1.2 單目深度估計算法

單目深度估計網(wǎng)絡的任務是預測彩色圖像中像素的深度值。S2R-DepthNet模型借鑒了人對場景的三維感知的特點，重點關注彩色圖像物體的空間結(jié)構(gòu)，結(jié)合深度注意力模塊預測彩色像素的深度信息。S2R-DepthNet模型可以分離彩色圖像結(jié)構(gòu)信息并提取互補的高層語義，抑制與深度無關的信息，從而增加深度推理的準確性。

具體過程如下：輸入彩色圖像，通過結(jié)構(gòu)提取模塊分離彩色圖像中不變的結(jié)構(gòu)信息，生成結(jié)構(gòu)圖，真實深度為T，損失函數(shù)如下所示

(4)

式中：T′是預測的深度值，p是深度像素索引，Δx和Δy是深度圖的水平和垂直算子，α和β是超參數(shù)。

結(jié)構(gòu)圖中包含了大量與深度無關的低層信息，會降低深度估計的精度。而利用深度注意力模塊產(chǎn)生的深度注意力圖，優(yōu)化結(jié)構(gòu)圖，輸出預測深度圖，從而提高深度估計質(zhì)量。注意力圖和深度預測的損失函數(shù)為

(5)

2 基于MRSD測距的跟隨系統(tǒng)

本文搭建了一個移動機器人作為行人跟隨的平臺，提出的基于MRSD測距的移動機器人跟隨系統(tǒng)的運行流程，如圖1所示，主要包含MRSD測距算法和行人跟隨控制兩個部分。首先對輸入的彩色圖像進行目標檢測；然后深度融合測距模塊獲取人-機距離，根據(jù)目標距離建立Sage-Husa自適應卡爾曼預測和測量方程輸出距離信息；最后，當人-機距離與設定距離不等時，通過PID控制器輸出相應角速度和線速度，驅(qū)動小車跟隨行人移動至設定距離后，若行人繼續(xù)運動，小車持續(xù)跟隨行人，否則，小車停止運動。

圖1 機器人跟隨系統(tǒng)流程

2.1 MRSD測距算法結(jié)構(gòu)

本文提出的MRSD測距算法包含兩個部分：行人檢測和深度測距，如圖2所示。具體流程為：將彩色圖像輸入Mask R-CNN和S2R-DepthNet網(wǎng)絡，前者用于檢測行人，獲取行人前景掩膜；后者用于推理深度圖像。根據(jù)掩膜從深度圖像中提取行人區(qū)域深度點，使用推理深度圖像替代相機深度圖像中的無效點，從而精確計算人-機距離。下面分別介紹MRSD測距算法的兩個模塊。

圖2 MRSD測距算法結(jié)構(gòu)

2.1.1 行人檢測

行人檢測既是深度融合測距的前提，又是移動機器人感知環(huán)境的基礎[18]。YOLOv5雖然可以快速檢測行人，但目標檢測框會選中圖像的背景像素，導致深度測距精度降低。因此，準確表示行人區(qū)域，對于測量人-機距離至關重要。本文引入Mask R-CNN網(wǎng)絡準確分割行人區(qū)域并輸出前景掩膜。定義輸入彩色圖像X，則掩碼Y的公式如下

Y=Mask(X，Ws)

(6)

式中：Mask()表示預訓練的Mask R-CNN網(wǎng)絡，Ws表示權(quán)重矩陣。

Mask R-CNN結(jié)合Faster R-CNN和FCN的功能，前者用于提取特征，隨后在特征圖上，利用分類器鑒別的候選ROI并檢測行人；后者利用卷積和去卷積操作，對每個像素分類后得到行人掩膜，從而準確分割行人前景和背景。同時為了降低誤判率，本文設置算法的行人置信度為0.92。

2.1.2 深度融合測距

行人運動速度的動態(tài)變化，導致人-機距離也隨之變化。若移動機器人能精確測量人-機距離，就能通過控制算法控制其運動狀態(tài)，準確跟隨行人。在跟隨行人的過程中，由于行人運動狀態(tài)的不確定性和RGB-D相機的成像限制，移動機器人不可避免地產(chǎn)生距離誤差。深度測距誤差原因通常來自于兩個方面：不準確的目標檢測框以及深度圖像中無效像素值，前者可以通過Mask R-CNN輸出行人掩膜解決；后者指深度圖像中距離為0的深度像素點。

為了降低無效深度點對測距精度的影響，本文首先定位相機深度圖像中深度無效點的位置，然后提取單目深度估計網(wǎng)絡推理的深度圖像對應位置的距離，最后完成像素替換，從而提高測距精度，完成行人跟隨任務。定義結(jié)構(gòu)提取模塊的編碼器、解碼器，前者將彩色圖像X映射到潛在空間[19]，后者生成結(jié)構(gòu)圖Ms。由于結(jié)構(gòu)圖中存在大量無用的低維信息，S2R-DepthNet將注意力機制與結(jié)構(gòu)圖結(jié)合，消除低維信息對深度估計的影響，進一步建立彩色像素和深度像素的映射關系。

深度注意力模塊中的編碼器使用擴展殘差網(wǎng)絡[20]，用于增加編碼器網(wǎng)絡的感受野。在解碼器后增加sigmoid函數(shù)，輸出深度注意力MA。S2R-DepthNet的推理深度為

Tt=f(Ms×MA)

(7)

式中：f()表示深度預測模塊。

RGB-D相機的深度圖像存在深度偽影的固有問題，若直接使用相機深度圖像測量人-機距離，會降低測距精度，進而降低移動機器人跟隨的穩(wěn)定性。因此本文引入單目深度估計網(wǎng)絡，彌補相機深度圖像中的無效像素點。在獲得行人掩膜后，融合相機深度和推理深度，計算人-機距離。定義Pli表示相機深度圖像的行人區(qū)域第i個像素點的距離，Pti表示S2R-DepthNet網(wǎng)絡推理深度圖像的行人區(qū)域第i個像素點的距離，測量人-機距離Edis的公式如下所示

(8)

(9)

其中，Pi表示融合后行人區(qū)域第i個像素的距離；n表示行人掩膜區(qū)域的深度像素的總數(shù)量。

2.1.3 偏航角計算

偏航角φ表示為行人目標偏離相機視野中間位置的角度，如圖3所示。在行人跟隨過程中，人-機距離Edis和偏航角φ共同決定移動機器人的運動狀態(tài)。在相機的成像平面中，行人目標偏移距離所占像素PQ誤差小，相機焦距f為固定值，由兩者計算得到的偏航角具有較高的準確性[21，22]。

圖3 偏航角計算模型

定義P是人體中心點，其坐標為(Px，Py)，O是相機光心，AQ是彩色圖像寬度的一半，ζ是AO與QO之間的角度，則偏航角φ為

(10)

2.2 跟隨與運動控制

2.2.1 系統(tǒng)模型建立

在行人跟隨過程中，距離信息受光照變化、行人和移動機器人速度的影響較大，導致測量距離存在較大波動的問題?；诖?，本文使用卡爾曼濾波算法對測距值進行平滑處理，使得測量距離更加趨近真實值，從而驅(qū)動移動機器人更加平穩(wěn)地跟隨前人。卡爾曼濾波算法將加速度建模為隨機噪聲，使用人-機距離、速度和加速度作為狀態(tài)變量，其中人-機距離p由觀測得到，速度為v，加速度為a，那么移動機器人在k時刻的狀態(tài)向量

X=[pva]T

(11)

系統(tǒng)的狀態(tài)方程和觀測方程如下

(12)

式中：Xk是目標k時刻的狀態(tài)向量；Zk是觀測向量；Fk，k-1是狀態(tài)轉(zhuǎn)移矩陣；Hk是觀測矩陣；Wk-1和Vk分別是狀態(tài)預測過程和觀測過程的高斯噪聲且相互獨立。狀態(tài)轉(zhuǎn)移矩陣F為

(13)

式中：T代表采樣時間。

由于本文只有人-機距離一個觀測量，故設置觀測矩陣H為

(14)

2.2.2 Sage-Husa自適應濾波器

測距系統(tǒng)常采用卡爾曼濾波算法進行運動控制，但在濾波過程中，若噪聲特性時變，易發(fā)生濾波發(fā)散和精度降低的現(xiàn)象。因此，本文引入Sage-Husa自適應濾波器(SHAKF)，減小時變噪聲的影響，測距系統(tǒng)的狀態(tài)空間方程見式(12)。

常規(guī)卡爾曼濾波假設系統(tǒng)的量測噪聲具有穩(wěn)定性，但在實際應用中，時變的量測噪聲對濾波效果影響較大。若能夠?qū)崟r推理系統(tǒng)的量測噪聲協(xié)方差矩陣Rk，就可以防止濾波發(fā)散，提高濾波精度。系統(tǒng)的新息公式為

(15)

由上式可得新息協(xié)方差矩陣為

(16)

(17)

式中：b為漸消因子，一般取0.9，故量測噪聲協(xié)方差矩陣Rk為

(18)

聯(lián)合上式，Sage-Husa自適應濾波公式為

(19)

式中：Pk-1和Pk，k-1分別是k-1時刻和k時刻的狀態(tài)估計協(xié)方差；Qk是k時刻的過程噪聲協(xié)方差；Kk是k時刻的卡爾曼增益。針對距離信息受光照條件和行人速度變化大的問題，Sage-Husa自適應濾波能夠有效降低測距誤差，提高距離測量的精度，從而增加移動機器人跟隨的平順性。

2.2.3 行人跟隨控制

當移動機器人獲得行人的距離和偏航角后，需要控制器驅(qū)動移動機器人，完成行人跟隨任務。

PID控制器能夠減小測量位置和設定位置之間的誤差，使得移動機器人能夠快速穩(wěn)定跟隨行人運動。同時，PID控制器能夠避免機器人移動速度過快導致人-機碰撞危險。因此，本文選擇PID控制器作為移動機器人的運動控制模塊。

PID控制器以測量距離和偏航角作為輸入，以線速度和角速度作為輸出，驅(qū)動車輪電機跟隨行人。本文設置跟隨距離為0.6 m，設置偏航角為0°。本文所使用的離散式PID控制器的計算公式為

(20)

(21)

其中，v和w分別表示PID輸出的線速度和角速度；ed(k) 和ew(k) 分別為k時刻的距離偏差和角度偏差；ed(k-1) 和ew(k-1) 分別為k-1時刻的距離偏差和角度偏差；KP、KI和KD分別為速度PID的比例系數(shù)、積分系數(shù)和微分系數(shù)；Kp、Ki和Kd分別為角度PID的比例系數(shù)、積分系數(shù)和微分系數(shù)。

當距離和偏航角與設定值產(chǎn)生誤差時，PID控制器中的比例控制快速響應，積分控制用于消除過去累計的誤差，微分控制用于提高系統(tǒng)動態(tài)性能指標，輸出線速度和角速度。PID控制器能夠使得移動機器人與目標人體保持合適的相對位置，達到理想的跟隨效果。

3 實驗結(jié)果

3.1 實驗平臺搭建

本實驗的硬件平臺由樂視LeTMC-520RGB-D相機、NVIDIA Jetson Xavier、STM32控制器和驅(qū)動電機組成，如圖4所示。本文選擇樂視RGB-D相機作為視覺傳感器，通過深度測距使得機器人能以設定的距離跟隨目標移動。將MRSD算法部署在NVIDIA Jetson Xavier上，既可以配合RGB-D相機測量距離和偏航角，又可以連接STM32控制直流電機，完成行人跟隨任務。

圖4 移動機器人硬件

測試數(shù)據(jù)：本文使用RGB-D相機在室內(nèi)環(huán)境中采集圖像數(shù)據(jù)和深度數(shù)據(jù)，采集的圖像分辨率為640×480，行人和相機間的真實距離由卷尺手工測量。由于無法實時獲得人體目標和移動機器人的真實距離，本文設置移動機器人和人體目標保持靜止狀態(tài)。每隔0.6 m測量人體目標的實際距離，并保存此位置的20張彩色圖像和深度圖像。由于深度相機存在距離越遠誤差越大的限制，本文將距離測量范圍設置為0.6 m至4.8 m，并將測量范圍劃分為7個區(qū)間，方便后續(xù)距離誤差測試。

評價指標：本文使用位移誤差Derr(displacement error)評價機器人和目標之間距離的精度。位移誤差由實際距離Rdis(real distance)和測量距離Edis計算可得

Derr=Rdis-Edis

(22)

3.2 銷蝕實驗

本文提出了一種基于MRSD測距的移動機器人跟隨系統(tǒng)，引入MaskR-CNN實例分割算法和S2R-DepthNet單目深度估計網(wǎng)絡，提高深度測距的精度。為評價不同網(wǎng)絡模塊的作用，在測試數(shù)據(jù)上進行銷蝕實驗。MRSD測距算法銷蝕實驗結(jié)果見表1，與目標檢測算法相比，實例分割算法能大幅提高深度測距精度。在加入單目深度估計算法后，在測試數(shù)據(jù)上的平均誤差相較于之前降低了40.6%，取得更好的測距效果。

表1 MRSD測距算法銷蝕實驗結(jié)果

3.3 人-機測距定量分析

測量人-機距離是移動機器人跟隨系統(tǒng)領域的熱點話題。傳統(tǒng)方法通過YOLOv5算法檢測行人，由于無法準確獲得行人區(qū)域的深度點，導致人-機距離測量不精確。本文以深度圖像為基礎，結(jié)合行人檢測和深度融合算法，提高人-機距離的測量精度。

為了驗證本文方法的測距性能，選取了如下方法對比：YL和YLX表示YOLOv5和YOLOX[23]的目標檢測框的測距方法；IS表示Mask R-CNN的實例分割的測距方法；Fu等[12]表示使用傳統(tǒng)數(shù)字濾波處理深度圖像的實例分割的測距方法；IS_S2R和IS_RMS表示S2R-DepthNet和R-MSFM[24]的深度融合的測距方法；Ours表示本文提出的測距方法。

本文使用測試數(shù)據(jù)上8個位置進行實驗，采用位移誤差指標衡量人-機測距的精度，如表2所示，本文算法由于引入了Mask R-CNN網(wǎng)絡和S2R-DepthNet單目深度估計網(wǎng)絡，抑制深度無效點對深度融合測距的影響，提高了人-機測距的精度。

表2 人-機距離誤差比較

3.4 Mask R-CNN測距方法定性分析

本文引入Mask R-CNN算法，獲得行人掩膜，并根據(jù)掩膜信息提取行人區(qū)域的深度像素點，計算人-機距離。因此，檢測算法的輸出形式?jīng)Q定了行人區(qū)域的表示準確度，從而影響人-機測距的精度。YOLOv5的深度測距方法誤差較大，且隨著距離的增加誤差逐漸增大，如圖5所示。本文引入的Mask R-CNN在人-機距離較近時，深度測距的精度較高。但在人-機距離較遠時，受限于RGB-D相機成像素質(zhì)和深度圖像中的無效深度點，測距誤差逐漸增大。

圖5 不同算法測量距離與實際距離對比結(jié)果

受到光線的影響，YOLOv5算法誤檢和漏檢，無法準確選中行人，影響深度測距的精度，如圖6所示。Mask R-CNN由于應用了ROlAlign策略，可以實現(xiàn)像素級別的分割，使其能夠獲得較高的行人檢測準確率。所以本文引入Mask R-CNN算法分割人體目標的效果較好。

圖6 YOLOv5和Mask R-CNN行人檢測

3.5 深度融合測距方法定性分析

深度可視化結(jié)果如圖7所示，圖7(a)場景中相機深度圖像出現(xiàn)無效點，而S2R-DepthNet算法因加入了結(jié)構(gòu)一致性模塊，其推理圖像具有深度連續(xù)性，避免出現(xiàn)無效深度點。圖7(b)場景中深度相機由于受到室外的強烈光照和人-機距離過遠的影響，其深度圖像像素點的距離為0，而S2R-DepthNet推理深度圖像能夠獲得人-機距離。但相比于相機深度圖像，推理深度的距離并不精確。在室外場景中，通過深度融合算法可獲得大致人-機距離信息，從而提高移動機器人跟隨系統(tǒng)的魯棒性。因此，本文在近距離使用Mask R-CNN的測距方法；在遠距離時使用深度融合測距的方法，將單目深度估計后的深度圖像替換無效深度點進行深度測距，最終提高深度測距的精度。

圖7 室內(nèi)室外彩色圖像和深度圖像可視化

3.6 Sage-Husa自適應濾波算法評估

在室外走廊環(huán)境中進行行人跟隨實驗并采集人-機距離，采樣頻率為7 Hz，歷時142 s，共采集994個圖像序列。由于光照強度變化，樂視深度相機采集的距離信息存在野值。以ZED相機輸出的距離信息作為真實值，對比傳統(tǒng)卡爾曼濾波算法、檢測野值的卡爾曼濾波算法和SHAKF算法的測距精度。

本文將不同卡爾曼濾波算法輸出距離數(shù)據(jù)與真實值對比，采用均方根誤差RMSE來量化算法的性能指標，公式為

(23)

式中：n是圖片幀的數(shù)量；Mi是第i個圖像幀的距離估計值；Gi是第i個圖像幀的距離真實值。

3種算法在行人跟隨過程中第500～600幀的距離對比曲線如圖8所示，第39幀距離信息發(fā)生異常，傳統(tǒng)卡爾曼算法出現(xiàn)了濾波拖尾的現(xiàn)象，影響下一幀的距離估計值。而當過濾異常值后，SHAKF算法可以快速計算量測噪聲協(xié)方差，獲得更為精確的距離信息，提高移動機器人跟隨系統(tǒng)的魯棒性。

圖8 500～600幀距離對比

3種算法在行人跟隨過程中第500～600幀的距離均值誤差對比曲線如圖9所示。與傳統(tǒng)卡爾曼濾波和過濾野值的卡爾曼濾波相比，SHAKF算法的均值誤差更小，可以提高算法的測距精度，進而使得移動機器人能夠更加魯棒地跟隨行人，對應的RMSE見表3。

表3 3種算法的均方根誤差

圖9 500～600幀距離均值誤差對比

在本文的實驗場景中，實驗環(huán)境為Ubuntu18.04，軟件環(huán)境為ROS機器人操作系統(tǒng)，硬件平臺由阿克曼小車、樂視RGB-D相機和NVIDIA Jetson Xavier等硬件搭建。移動機器人首先將RGB-D相機采集的彩色圖像和深度圖像送入深度融合測距模塊中，計算粗略的人-機距離；然后通過Sage-Husa自適應濾波算法估計當前時刻的人-機距離，減小測距誤差；最后通過PID控制算法驅(qū)動移動機器人跟隨前人。

移動機器人在室內(nèi)室外的跟蹤結(jié)果如圖10所示。在室內(nèi)場景中，移動機器人使用RGB-D相機采集彩色圖像和深度圖像，并根據(jù)深度圖像獲得行人位置信息，以此調(diào)整人-機相對位置。在室外場景中，移動機器人的視野范圍更廣，但是室外走廊光線條件較為復雜，可以驗證其在復雜光照環(huán)境的行人跟隨能力。移動機器人能夠在室內(nèi)環(huán)境穩(wěn)健地跟蹤行人目標，且在光照強度變化劇烈的室外環(huán)境中依然具有跟隨的魯棒性。

圖10 機器人跟隨

4 結(jié)束語

本文提出了一種基于MRSD測距的移動機器人跟隨系統(tǒng)，主要包含3項工作：①引入Mask R-CNN檢測行人目標獲取掩膜信息，并根據(jù)行人掩膜從深度圖像中提取深度信息。②引入S2R-DepthNet網(wǎng)絡從RGB圖像估計深度信息，融合相機深度圖像解決無效深度點的問題，從而準確測量目標行人的距離和偏航角。③基于該距離，建立了基于SHAKF的測距模型，降低量測異常值的影響，同時設計了PID控制器，能夠以設定的距離魯棒跟隨前方行人。本文在真實環(huán)境中進行了實驗，實驗結(jié)果表明：本文提出的基于MRSD測距的機器人跟隨系統(tǒng)能夠快速準確檢測前人距離并跟隨目標人物。