室內(nèi)移動機器人自主導航系統(tǒng)設(shè)計與方法

翟敬梅，劉 坤，徐 曉

(華南理工大學 機械與汽車工程學院，廣東 廣州 510640)

0 引言

移動機器人自主導航在智能制造工廠、智能倉儲、車間裝配和物流運輸?shù)阮I(lǐng)域有著廣泛的應用[1]。移動機器人自主導航主要涉及到環(huán)境檢測、動態(tài)目標識別和跟蹤，以及路徑規(guī)劃與優(yōu)化問題。

目前室內(nèi)移動機器人進行環(huán)境感知及障礙物檢測時搭載的傳感器主要有超聲波、紅外、激光雷達和視覺等，或上述的組合。環(huán)境檢測有局部的，也有全局的。視覺傳感器最大限度獲取環(huán)境圖像信息，提供更為豐富的輸入數(shù)據(jù)，且其價格、功耗較低，在移動機器人平臺上得到了廣泛運用[2]。障礙物檢測方面，基于顏色、紋理、特征點等圖像特征將圖像分割為障礙物、地面和背景區(qū)域，相比單目檢測只能進行平面測量，立體視覺檢測可基于三維點云或視差計算獲取障礙物位置信息[3]。

目標跟蹤包括初始化、運動模型、特征提取、外觀模型和更新機制5個部分。目標初始化有人為和自動兩種方式。運動模型描述連續(xù)幀間的運動關(guān)系，典型模型有均值漂移、滑動窗口和粒子濾波。目標跟蹤時特征描述及其提取直接決定了跟蹤的效果，目前采用的特征有人工特征和學習特征。人工特征包括灰度、顏色、梯度和紋理等外觀特征和運動特征，運動特征運用較為廣泛的是光流法，光流法與其他外觀特征結(jié)合使用更具魯棒性。隨著機器學習的發(fā)展，特征可以在模型訓練中自主學習，稱為學習特征。深度學習通過搭建深層次神經(jīng)網(wǎng)絡(luò)模型，運用訓練好的網(wǎng)絡(luò)模型在各層中依次獲取顏色、紋理、梯度等特征[4-5]。然而深度學習需要大量相似的樣本來訓練，為保證跟蹤模型的準確性，往往還提供一定數(shù)量的跟蹤目標樣本信息，在實際應用中有一定的局限性。深度學習模型比較復雜，通常是基于經(jīng)驗和試驗，而且計算量遠遠大于傳統(tǒng)跟蹤算法，需要圖形處理器(Graphics Processing Unit, GPU)或神經(jīng)網(wǎng)絡(luò)處理器(Neural-network Process Unit, NPU)等支持，獲取高效準確的學習模型是一項很大的挑戰(zhàn)[5-6]。外觀模型在跟蹤算法中處于核心位置，目前產(chǎn)生式模型和判別式模型是兩種主流分類形式。產(chǎn)生式模型只針對目標外觀特性，常用算法有基于模板模型、基于子空間模型、基于稀疏表示模型等算法。判別式模型充分利用背景和前景信息，其算法可以看成一個二分類問題，因此依賴于訓練樣本的數(shù)量和質(zhì)量[7]，主要算法有支持向量模型、Boosting模型和樸素貝葉斯模型等。兩類算法各有優(yōu)劣，當訓練的樣本數(shù)量較多時，判別式模型具有較好的跟蹤結(jié)果，但訓練過程會降低跟蹤效率；當有限數(shù)據(jù)可用時，產(chǎn)生式模型具有更好的泛化能力。

不同于上述兩類方法，時空上下文方法[8]對目標位置的確定使用了時間和空間的局部相關(guān)性輔助信息，通過計算目標及其周圍區(qū)域的低階特征進行統(tǒng)計建模，利用置信圖進行最大似然估計，預測出目標位置。算法本身計算量較小，且在應對短時部分遮擋時具有較高的穩(wěn)定性。目標跟蹤過程中，目標姿態(tài)和環(huán)境信息在時刻發(fā)生變化，如目標旋轉(zhuǎn)、背景干擾，目標遮擋、光線變化等情況，為了適應目標外觀和環(huán)境變化，需要對外觀模型進行實時更新。

移動機器人路徑規(guī)劃方法主要分為全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃方法主要有自由空間法、柵格法、可視圖法等，這些方法只適用于環(huán)境先驗信息已知的場景[9]；局部路徑規(guī)劃算法主要有人工勢場法和快速擴展隨機樹算法等，這些方法適用于環(huán)境未知或部分未知的場景，障礙物位置、尺寸等信息由各傳感器實時獲取，能夠?qū)崟r進行動態(tài)路徑規(guī)劃?？焖贁U展隨機樹算法基于隨機采樣點生成路徑，使得路徑不平滑、非最優(yōu)和缺乏確定性[10]。人工勢場法操作簡單、易于實現(xiàn)，但存在局部極小值等問題。近年來，一些智能算法如遺傳算法、模糊邏輯算法、粒子群優(yōu)化算法和蟻群優(yōu)化算法等與傳統(tǒng)路徑規(guī)劃算法結(jié)合，在優(yōu)化最優(yōu)路徑求解的同時提高了路徑規(guī)劃的效率[11]。

本文設(shè)計了一個室內(nèi)環(huán)境下移動機器人自主導航系統(tǒng)平臺?；趩文亢碗p目視覺信息融合，構(gòu)建全局實時地圖；提出了特征點輔助的改進時空上下文目標定位方法；改進了速度向量場路徑規(guī)劃及優(yōu)化方法。

1 機器人自主導航系統(tǒng)框架

移動機器人自主導航中需要：①障礙物尺寸及實時位置信息;②移動機器人實時位置和姿態(tài)信息;③目標實時位置信息。

在大的室內(nèi)場景下，移動機器人要感知周圍環(huán)境信息，檢測障礙物并進行避障。避障的同時需要獲取目標位置信息，由于視覺傳感器視野范圍有限，移動機器人在向目標點行進和避碰過程中可能使目標離開視野范圍。因此，本文設(shè)計了“雙目視覺+單目視覺”組合式的環(huán)境信息獲取方案，系統(tǒng)框架如圖1所示。

(1)雙目視覺系統(tǒng) 雙目相機以Eye-in-hand方式水平放置于移動機器人平臺上，獲取更精確的移動機器人周邊環(huán)境信息，以此獲取障礙物尺寸、位置，以及運動可行路面信息。

(2)單目視覺系統(tǒng) 廣角攝像頭以Eye-to-hand方式置于移動機器人工作區(qū)域上方，獲取目標和移動機器人信息?；谠撔畔ⅲ蛇M行目標、移動機器人的識別和定位，以及動態(tài)目標跟蹤。

2 地圖構(gòu)建

雙目視覺系統(tǒng)檢測移動機器人周邊環(huán)境，獲取障礙物相對于機器人的位置和尺寸信息，從而構(gòu)建局部地圖。單目視覺系統(tǒng)實現(xiàn)室內(nèi)環(huán)境中機器人和目標體的識別和實時定位。通過坐標轉(zhuǎn)化，將兩者信息有機融合，可以獲取全局環(huán)境下障礙物、目標和機器人空間位置、尺寸信息，構(gòu)建出全局地圖，為機器人路徑規(guī)劃提供必要的信息。

2.1 雙目視覺下障礙物檢測與測量

2.1.1 融合色彩飽和度信息的Retinex陰影消除

障礙物檢測時，往往存在地面陰影等干擾。單尺度Retinex算法[12]基于圖像增強原理消除陰影，運用高斯濾波后圖像近似預估光照分量L(x,y)，得到物體本質(zhì)色彩信息，以此得到陰影消除后圖像R(x,y)。然而，人為選擇模糊尺度時，陰影消除中易出現(xiàn)過度增強，導致地面與障礙物灰度差較小而影響后續(xù)障礙物的提取。

將圖像色彩飽和度信息S(x,y)與R(x,y)相融合，消除陰影的同時增強障礙物與地面的灰度差。通常，光照較大時陰影較為嚴重，因此引入圖像環(huán)境亮度信息V(x,y)，計算融合權(quán)重，自適應地調(diào)節(jié)光照不同對陰影消除的影響。算法流程如表1所示，圖2所示為室內(nèi)3個障礙物陰影去除過程。

表1 陰影去除算法

續(xù)表1

2.1.2 區(qū)域分割與障礙物檢測

對融合圖像I(x,y)進行種子填充，獲得圖3a中的填充地面。運用二值化和閉合運算去除較小的噪聲區(qū)域，再提取地面和障礙物的邊緣獲取地面和障礙物區(qū)域，如圖3b所示。如圖3c所示為環(huán)境信息檢測結(jié)果，將場景分割為背景區(qū)域(灰色)、障礙物區(qū)域(白色)和地面區(qū)域(黑色)，障礙物和背景為非可行區(qū)域，地面區(qū)域為可行區(qū)域范圍。

雙目相機進行標定和立體矯正后，以障礙物區(qū)域塊為模板在右相機圖像中進行匹配，計算出障礙物中心點三維坐標和障礙物尺寸[13]。移動機器人導航中障礙物距離和寬度信息最為重要，表2列出了圖3所示障礙物在機器人當前視野下的寬度和相對距離測量結(jié)果，以及檢測時間(core i5處理器，主頻2.50 GHz環(huán)境下)，結(jié)果滿足了移動機器人避障導航精度和實時性的要求。

表2 障礙物測量信息

2.2 基于單目視覺的移動機器人定位

為了識別機器人并對其定位，設(shè)計機器人靶標如圖4a所示。在白色的A4紙內(nèi)設(shè)置一個粉紅色矩形框，將圖4a轉(zhuǎn)化為HSV顏色空間，根據(jù)H分量下設(shè)定的先驗閾值分割獲取粉紅色區(qū)域如圖4b，獲取靶標區(qū)域掩碼如圖4d，同時由S分量分割獲得圖4c區(qū)域。在掩碼區(qū)域?qū)D4c進一步精確分割得到圖4e的A4紙區(qū)域圖像。計算圖4e白色A4紙區(qū)域面積得到重心點，則圖4a中離重心點最近的C點設(shè)為原點，點C與離重心第二近的點B構(gòu)成X軸，點C與離重心第三近的點D構(gòu)成Y軸。由單目測量方法[15]可得B、C、D三點在單目測量坐標系XcOcYc下二維坐標Ol、Px和Py，以O(shè)l為原點，OlPx為X軸，OlPy為Y軸，建立圖4f所示的機器人靶標坐標系XlOlYl，各坐標系示意圖如圖5所示。

圖5中取單目測量坐標系XcOcYc的Y軸單位向量為y=(0,1)，則XlOlYl坐標系與XcOcYc坐標系的夾角θ為：

(1)

至此，獲取到了機器人位置Ol和姿態(tài)傾角θ。

2.3 改進的時空上下文動態(tài)目標定位算法

時空上下文跟蹤算法[8]以固定更新率更新模型，在背景光照干擾和目標長時間遮擋時，易引入錯誤背景信息造成目標漂移，在目標快速運動時易跟蹤失敗?；跁r空上下文跟蹤算法，本文提出一種特征點輔助的動態(tài)目標跟蹤算法。

2.3.1 光流追蹤和特征點匹配的目標位置預估

2.3.2 基于特征點變化率的時空上下文模型更新

對于目標位置x和目標位置中心x*，建立上下文特征集Xc={v(z)=(I(z),z)|z∈Ωc(x*)}，其中I(z)表示在z點的灰度值，Ωc(x*)表示x*為中心的局部區(qū)域。

目標與其相鄰區(qū)域的空間上下文關(guān)系模型hsc(x)[8]：

(2)

式中：b為歸一化系數(shù)；α是尺度參數(shù)；β是形狀參數(shù)；ωδ(·)表示權(quán)重函數(shù)；F(·)表示快速傅里葉變換；F-1(·)表示傅里葉逆變換。

(3)

2.3.3 基于時空上下文信息的目標精確定位

以t+1幀時的目標位置置信圖中極大值的位置作為該幀目標位置，目標精確位置為：

(4)

在文獻[15]分別展現(xiàn)了背景干擾、全局和部分遮擋、快速運動與運動模糊、目標旋轉(zhuǎn)等情況下展現(xiàn)本文算法的跟蹤效果，并與傳統(tǒng)時空上下文 (Spatio-temporal Context learning, STC)以及當前比較熱門的快速壓縮跟蹤(fast Compressive Tracking, CT)算法 、跟蹤學習檢測(Tracking Learning Detection, TLD)算法 、在線增強視覺(On-line Boosting and vision, OAB )跟蹤算法作比較，結(jié)果表明本文跟蹤算法綜合測試指標中跟蹤效果最佳，具有較好的魯棒性和跟蹤精度。

2.4 視覺信息融合的二維地圖實時構(gòu)建

(5)

至此，獲取了目標體、機器人和障礙物在單目測量坐標系XcOcYc下的位置等信息，構(gòu)建如圖6b所示的全局地圖。

3 基于速度向量場的路徑規(guī)劃算法

速度向量場方法由吸引速度va、排斥速度vr和切向速度vt控制機器人避障運動。機器人在這3個速度綜合作用下實現(xiàn)避碰運動，原理如圖7所示。圖中O、G和R分別為當前障礙物、目標和機器人位置；dOG、dOR和dRG分別代表各位置之間的距離；robs為障礙物半徑；s為避碰環(huán)半徑；rd為障礙物排斥場作用半徑。

傳統(tǒng)速度向量場方法路徑規(guī)劃時存在以下問題[16]：①機器人吸引速度過大，到達目標點時會引起來回震蕩；②機器人進入排斥場時由于排斥速度突然變化，產(chǎn)生較大的軌跡波動；③當障礙物處于目標點附近，此時排斥速度場大于吸引速度場而使得目標不可到達；④機器人運行速度較大時在排斥場邊緣處易造成軌跡抖動；⑤未考慮動態(tài)障礙物下切向速度方向優(yōu)化。

3.1 衰減吸引速度函數(shù)設(shè)計

目標點區(qū)域設(shè)置一個半徑為ra的緩沖帶，吸引

速度va定義如下：

(6)

(7)

其中：RG為R指向G的向量；M為吸引速度系數(shù)，為了使機器人較快到達目標點，M通常取較大值。

當機器人運行在目標緩沖區(qū)域外時，機器人以較快的速度運行。當進入目標緩沖區(qū)域，機器人以遞減速度接近目標點，避免了目標點附近震蕩現(xiàn)象。

3.2 對數(shù)形式排斥速度函數(shù)設(shè)計

為解決傳統(tǒng)速度向量場方法中軌跡波動、目標不可到達等問題，設(shè)計了如下對數(shù)形式排斥速度vr：

(8)

(9)

其中：OR表示O指向R的向量，β表示排斥速度系數(shù)。

機器人剛進入排斥場時排斥速度較小，越接近障礙物排斥速度越大，避免了速度突變。另一方面，機器人速度較大時排斥場邊緣處易產(chǎn)生軌跡抖動，在式(8)的排斥速度中引入機器人與障礙物距離的導數(shù)(dOR-robs)′，其代表機器人與障礙物相對距離變化速度，變化越快則接近速度越大，此時排斥速度相應變大，避免了軌跡波動。

當障礙物接近目標點時，一旦出現(xiàn)排斥速度大于吸引速度，機器人無法到達目標點。為此，當障礙物和目標點距離小于rd時，在排斥速度中引入式(9)的排斥速度衰減策略，使得機器人接近目標時，排斥速度以比例形式逐漸衰減，保證機器人最終到達目標點。

3.3 切向速度函數(shù)優(yōu)化設(shè)計

為確保機器人運行路徑最短，如圖8a中對于靜態(tài)障礙物，兩個切向速度vt和v1t選擇切向速度vt與吸引速度va夾角較小的vt。對于動態(tài)障礙物，需進一步考慮障礙物運動方向，機器人的繞行方向應與障礙物移動方向vobs相反，具體如圖8b和圖8c所示。

速度形式與排斥速度類似，引入切向速度向量場作用半徑rt，vt定義如下：

(10)

(11)

其中：OR′表示向量OR的垂直向量，γ表示切向速度系數(shù)。

改進的切向速度可有效避免軌跡波動等問題，同時基于靜態(tài)障礙物下繞行距離最短和動態(tài)障礙物下避障最安全原則，設(shè)計的切向速度可有效避免機器人陷入局部極小值，加速繞開障礙物。

3.4 機器人避碰運動速度計算

在機器人運動場景中，若存在n個障礙物，則機器人避碰運動速度合速度v計算如下：

(12)

3.5 路徑規(guī)劃算法仿真與性能分析

(1)多種吸引速度對比仿真

圖9a為衰減吸引速度與常數(shù)形式吸引速度[17]和比例形式吸引速度[18]對比效果圖。常數(shù)吸引速度下，機器人接近目標點時，沖過目標點而后折返，在圖中表現(xiàn)為距離上下波動，始終無法收斂到目標位置；比例吸引速度避免了機器人在目標點附近震蕩，但其收斂速度較慢，在5 s時才完全停下；本文的衰減吸引速度約在1.5 s時已收斂到達目標點。

(2)多種排斥速度對比仿真

圖9b所示為本文提出的對數(shù)形式排斥速度與目前采用的排斥速度模型(常數(shù)形式[17]、一次反比形式[18]、二次反比形式[19]、指數(shù)形式[20]和虛擬彈簧阻尼形式[16])的仿真對比結(jié)果。結(jié)果表明：①排斥速度為常數(shù)時，機器人進入排斥場后速度發(fā)生突變，軌跡抖動劇烈；②一次反比函數(shù)排斥速度改善了軌跡抖動程度；③二次反比形式排斥場，總體軌跡相對平滑，但離開排斥場區(qū)域段仍存在軌跡抖動；④指數(shù)形式排斥場下，機器人軌跡基本平滑，但進入和離開排斥場時軌跡仍未達到平滑過渡；⑤虛擬彈簧阻尼形式排斥場得到較為理想的軌跡，但需要設(shè)定多個彈簧阻尼系數(shù)，軌跡平滑度很大程度受彈簧阻尼系數(shù)選取的影響。本文設(shè)計的對數(shù)形式速度排斥場軌跡最為平滑，可操作性強，且路徑最短。

(3)目標處于障礙物排斥場內(nèi)的路徑規(guī)劃仿真

目標處于障礙物排斥場內(nèi)，圖9c未加入排斥衰減策略，機器人始終圍繞目標繞行，無法到達目標點。圖9d加入排斥速度衰減策略后，機器人能夠始終朝目標點運動，并在較短時間內(nèi)抵達目標點。

(4)動態(tài)路徑規(guī)劃仿真

4 實驗與分析

實驗場景如圖11所示，一個移動機器人在一個有障礙物Ob1、Ob2和Ob3的室內(nèi)環(huán)境下跟蹤一個掃地機器人。掃地機器人作為動態(tài)目標，其運動軌跡是隨機的，與障礙物碰撞時，其運動隨機改變，同時也改變障礙物位置。實驗系統(tǒng)框架如圖12所示。

移動機器人和掃地機器人具有不同高度，需要在對應高度上進行相應測量矩陣的求取[15]，外參數(shù)標定結(jié)果如表3所示。

表3 單目測量外參數(shù)標定結(jié)果

圖13為實驗過程中移動機器人和掃地機器人在地圖中的運動軌跡，圖中同一顏色或灰色對應同一時刻位置，移動機器人軌跡中的ABCDEFGHI點分別對應掃地機器人軌跡的A′B′C′D′E′F′G′H′I′點。

(1)A位置(第31幀)

機器人到達A位置，獲取了障礙物Ob1信息。此時機器人未處于Ob1排斥場內(nèi)，從A到B運動時，機器人向目標做最短的直線運動。

(2)B位置(第64幀)

機器人到達B，進入Ob1排斥場內(nèi)，雙目視覺系統(tǒng)檢測到障礙Ob2、Ob3，將它們信息添加到地圖中。機器人由B向C運動，向目標運動同時還進行Ob1避碰運動。

(3)C位置(第97幀)

機器人到達C，這時掃地機器人碰到Ob2，致其位置發(fā)生變化，Ob2新位置更新至地圖。

(4)D位置(第119幀)

機器人到達D，在Ob2排斥場作用下繼續(xù)由D向E運動，圖13中虛線為Ob2未移動時理論軌跡，對比可知Ob2移動后使移動機器人更貼近Ob2的排斥場，軌跡發(fā)生偏移。

(5)F位置(第183幀)

機器人到達F，此時掃地機器人到達F′點，與邊界擋板發(fā)生碰撞，之后運動方向改變，此時移動機器人不受障礙物排斥場作用，以最短路徑從F向G運動。

(6)H位置(第258幀)

移動機器人從G點向H點運動過程避碰Ob3，到達H點。

(7)I位置(第304幀)

機器人在Ob3排斥場下繼續(xù)向I點運動，最終移動機器人和掃地機器人分別到達I點和I′點，完成了跟蹤任務。

實驗中障礙物位置信息測量結(jié)果如表4所示，最大、最小相對誤差分別為2.1%和0.4%，平均相對誤差為1.55%，滿足移動機器人導航需要。

表4 地圖構(gòu)建中障礙物位置測量結(jié)果

5 結(jié)束語

本文提出了一種基于“Eye-in-hand雙目視覺”和“Eye-to-hand單目視覺”組合的移動機器人自主導航方法。在障礙物檢測時，將圖像色彩飽和度信息和圖像環(huán)境亮度信息引入單尺度Retinex算法，自適應不同光照下陰影的消除和障礙物的有效提取；動態(tài)目標跟蹤定位方面，將目標特征點及其變化信息引入時空上下文目標跟蹤算法，自適應地調(diào)節(jié)時空上下文模型的更新率，較為有效地防止引入錯誤背景信息造成目標漂移和目標快速運動易跟蹤失敗等問題，在保證較好實時性的同時達到了穩(wěn)定的目標跟蹤效果；上述視覺信息處理模型的改進，一方面減少了模型依賴于人為經(jīng)驗或試驗數(shù)據(jù)，另一方面可自適應不同的環(huán)境。在機器人避障路徑實時規(guī)劃方面，對基于速度矢量場路徑規(guī)劃算法進行了一系列改進，避免了機器人在目標附近震蕩，解決了軌跡抖動、動態(tài)障礙物逼近和目標處于障礙物排斥場不可到達問題，以及擺脫局部極小值并以最短安全路徑繞行等問題。通過仿真和實驗驗證了本文算法的有效性和實際應用的可行性。

今后可以結(jié)合遷移學習等方法，進一步改進模型在復雜場景下的自適應性和穩(wěn)定性；另外，可以采用強化學習思想，研究機器人自學習進行室內(nèi)自主導航，這些將是未來的研究方向。