田聯(lián)房，陳浩權(quán)，杜啟亮+，鄭燦濤

(1.華南理工大學 自動化科學與工程學院，廣東 廣州 510640；2.華南理工大學自主系統(tǒng)與網(wǎng)絡控制教育部重點實驗室，廣東 廣州 510640)

0 引 言

目前行人檢測與跟蹤的方案主要有普通攝像頭[1]、紅外攝像頭[2,3]、激光雷達[4-6]等。由于攝像頭容易受到光線、視場大小、拍攝角度等影響，只適用于照明穩(wěn)定，拍攝位置相對固定的應用場景。紅外方案工作范圍遠且不受可見光線影響，即使在較暗，甚至全黑的環(huán)境中也可以正常工作，但受技術(shù)限制，紅外圖像存在信噪比低、對比度低、分辨率低以及噪聲多等問題，且成本較高，一般只用于光線較暗的場景。因激光雷達具有不受光線影響、精度高、成本低等優(yōu)點，目前智能服務機器人一般會配備二維的激光雷達。由于只有二維平面信息，且當距離較遠時分辨率較低，使得基于二維激光雷達的行人檢測難度很高。

為此Leigh A等[5]選擇了15種跟人腿相關(guān)的特征，使用隨機決策森林訓練了一個分類器進行分類檢測。Lucas Beyer等[6]提出利用卷積神經(jīng)網(wǎng)絡訓練分類器來對輪椅以及人腿進行分類。該算法由于不需要人為選擇特征，一定程度上能更好地檢測行人，但卷積神經(jīng)網(wǎng)絡對運算資源需求極高，不適合需要控制成本的應用場合。

由于二維信息不足，復雜的背景容易對行人檢測和跟蹤產(chǎn)生干擾，比如靠近墻角的椅子腿，即使以上兩種復雜的分類器仍然會誤判為人的腿部。為減小復雜背景的干擾，魏麗等[7]通過對激光雷達數(shù)據(jù)進行魯棒主成份分析，可以將短時間內(nèi)連續(xù)多個掃描周期的激光雷達數(shù)據(jù)分解成背景與運動目標。該方法可以很好地應用在走廊等相對空曠的環(huán)境，但無法應對行人靜止以及環(huán)境擁擠的情況。

針對上述基于激光雷達的行人檢測、跟蹤存在的背景干擾問題，本文提出先構(gòu)建環(huán)境地圖，再利用似然域模型的思想來分割當前激光雷達數(shù)據(jù)幀中前景與背景，然后只對分割出來的前景進行行人檢測，并利用改進的卡爾曼濾波器實現(xiàn)移動機器人在復雜背景下對行人的跟蹤與跟隨。

1 系統(tǒng)介紹

1.1 平臺介紹

本文使用的實驗平臺為一個輪式機器人，如圖1所示，左右輪子都帶有測距編碼器。在距離地面37 cm處安裝了一個單線的Flash Lidar F4激光雷達，掃描角度360°，角分辨率0.5°，幀率10FPS。

圖1 實驗硬件平臺

1.2 流程框架

如圖2所示，在完成環(huán)境地圖構(gòu)建后，先利用蒙特卡洛定位確定機器人位姿。如果定位失敗則直接進行數(shù)據(jù)聚類，否則進行似然域背景差分，只對提取到的前景對應的激光雷達數(shù)據(jù)進行數(shù)據(jù)聚類，避免復雜背景對后續(xù)行人檢測與跟蹤的干擾。最后根據(jù)一定的規(guī)則對行人跟蹤器輸出的跟蹤目標進行判斷，如存在目標行人，則讓機器人對目標行人進行自動跟隨，否則結(jié)束進入下一個掃描幀的循環(huán)。

圖2 基于似然域背景差分的行人檢測和跟蹤流程

2 建圖與定位

2.1 Gmapping環(huán)境地圖構(gòu)建

機器人同時定位與建圖(simultaneous localization and mapping，SLAM)問題[8,9]旨在實現(xiàn)機器人從未知環(huán)境的未知地點出發(fā)，在運動過程中定位自身位姿，再根據(jù)機器人位姿構(gòu)建地圖，從而達到同時自定位與建圖的目的。

GMapping算法具有不依賴激光雷達性能、不需要大量計算資源、建圖精確等優(yōu)點，故本文選用其來構(gòu)建環(huán)境地圖。圖3為圖1實驗平臺在一個辦公室內(nèi)運行GMapping算法后得到的柵格地圖。圖中白色部分表示該區(qū)域不被占據(jù)；黑色部分則表示該區(qū)域被障礙占據(jù)；灰色則是狀態(tài)不明，一般是被障礙隔擋無法探測或未建圖區(qū)域；數(shù)字對應背景差分實驗位置。

圖3 GMapping構(gòu)建的柵格地圖

2.2 蒙特卡洛定位

蒙特卡洛定位[10]是在地圖信息已知的情況下，機器人利用里程計信息和激光雷達數(shù)據(jù)確定自身位姿的一種算法。該算法會先根據(jù)一定的均值和方差，呈正態(tài)分布地初始化一個粒子群，然后利用里程計數(shù)據(jù)和機器人的運動模型更新粒子群中所有粒子的位姿，接著通過計算在對應位姿下激光雷達數(shù)據(jù)與地圖的契合程度來獲得粒子的重要性權(quán)重，最后根據(jù)一定的規(guī)則進行粒子群重采樣，并給出權(quán)重最大的位姿作為后驗位姿。實際使用中一般會在重采樣步驟中增加隨機粒子來使機器人從全局定位失效以及陷入局部最優(yōu)解中恢復。

3 似然域背景差分

似然域模型是一種傳感器測量模型[10]，其考慮了傳感器固有的不確定性，并為傳感器測量噪聲建立模型來獲得測量過程的概率。其一般用于在已知地圖與測量值時，估計機器人不同位姿的概率。本文基于似然域模型的思想，提出一種在已知地圖與機器人位姿時，根據(jù)似然度大小進行分割的背景差分方法。

3.1 似然域模型介紹

似然域模型定義了一個條件概率分布p(zt|xt,m)，其中zt是t時刻的測量，xt=(xyθ)T是機器人的位姿，m是環(huán)境地圖。而對于激光雷達一類傳感器，其測量束不止一個，在滿足每個測量束之間噪聲獨立分布的假設時

(1)

似然域模型簡單地將最大距離讀數(shù)丟棄，將每個非最大距離的測量束的端點坐標通過下面的式子轉(zhuǎn)換到地圖坐標系

(2)

式中： (xk,sensyk,sens)T表示激光雷達傳感器在機器人坐標系中的位置，θk,sens表示測量束相對于機器人航向的角度。

在得到每個測量束端點的全局坐標后，就可以計算該測量束掃到了地圖障礙的似然。在計算似然時，考慮兩種噪聲和不確定性來源：

(1)測量噪聲

找到端點坐標 (xk,sensyk,sens)T到地圖m上的障礙的最短的歐氏距離dist，認為該測量束探測到地圖障礙的似然服從以0為中心的高斯分布

(3)

(2)無法解釋的隨機測量

認為該測量束探測到地圖障礙的似然服從一個平均分布，其中zmax是最大測量距離

(4)

綜合以上兩個考慮，可以加權(quán)得到對應傳感器測量束探測到地圖中物體的似然，其中zhit和zrand是權(quán)重

(5)

3.2 激光雷達數(shù)據(jù)的背景差分

如圖4所示，似然度的大小表征著該測量束探測到背景的概率，其與測量束端點到地圖障礙的最短距離dist有關(guān)。

圖4 dist與,m)的關(guān)系

同時為了避免機器人定位不準確時造成前景提取錯誤，首先計算總體測量的似然度p(zt|xt,m)，僅當該似然度大于一個固定閾值theta_p，才進行前景提取。否則不提取前景，使用全部激光雷達數(shù)據(jù)進行行人的檢測和跟蹤。

4 行人檢測與跟蹤

4.1 行人檢測

雖然人腿相關(guān)的可提取特征非常多，但是對多種環(huán)境的實驗結(jié)果表明，僅有少數(shù)特征對檢測器的貢獻較大，使用太多對檢測器貢獻不大的特征反而會增加過擬合的風險。同時，考慮到二維激光雷達信息的不足，僅識別一只腿容易誤檢。因此，本文提出一種基于行人雙腿模型的行人檢測算法。

首先選用層次聚類算法[11]對激光雷達數(shù)據(jù)進行聚類，然后根據(jù)行人腿部特點，將激光點數(shù)在5個～30個之間的簇作為行人腿部候選點集。

如圖5所示，左右兩個點集代表數(shù)據(jù)聚類之后的兩個不同的簇。Dk代表對應簇的首尾元素的距離，Lk代表對應簇的總長度。檢測器最終給出行人雙腿模型的中間位置作為行人坐標。檢測器具體流程如圖6所示。

圖5 行人雙腿模型

圖6 行人雙腿模型檢測流程

4.2 多行人跟蹤

由于行人的運動和機器人本身的運動都存在噪聲，只依賴行人檢測得到的結(jié)果難以得到較好的跟隨效果。文獻[12]提出了一種采用最小歐氏距離的目標匹配方法來對多行人進行卡爾曼跟蹤，在對12段手扶電梯出入口視頻序列進行跟蹤實驗后，結(jié)果表明，多行人跟蹤的總調(diào)和均值達到96.1%，能夠穩(wěn)定地跟蹤多個行人。二維柵格地圖一定程度上跟圖片一樣，且本文應用場景與手扶電梯相似，背景都相對固定，都存在不確定數(shù)量行人的靠近、相互遮擋、遠離等，所以本文在文獻[12]提出的跟蹤算法基礎上稍作改進后應用到行人跟蹤中。

文獻[12]中的跟蹤強度公式僅考慮了檢測成功與檢測失敗兩種情況，沒有考慮目標長時間被遮擋的情況。針對非目標行人在經(jīng)過機器人旁邊只能探測到一條腿以及經(jīng)過目標行人旁邊完全被遮擋，無法檢測到的情況，如果直接采用文獻[12]中的跟蹤強度公式，會使得非目標行人的跟蹤強度迅速降低，導致目標跟蹤失敗。本文在文獻[12]提出的多行人卡爾曼跟蹤算法的基礎上，修改了跟蹤強度的計算公式如下

(6)

在非目標行人丟失時，利用最近50次的跟蹤信息判斷是否出現(xiàn)了上述兩種被遮擋情況，如沒有出現(xiàn)則將hide置0，認為行人走出檢測范圍，對目標跟蹤強度進行較大幅度的削減；否則置1，認為行人只是被遮擋，緩慢降低跟蹤強度等待目標走出遮擋區(qū)域。

本文在實驗中發(fā)現(xiàn)，改進后的算法能夠利用跟蹤有效減少目標被遮擋情況下的漏檢，對于突然闖進、離開都有較好的適應性，能夠滿足本文的需要。

4.3 機器人自動跟隨

在本文中，選擇出現(xiàn)在機器人正前方30 cm以內(nèi)的行人作為跟隨目標，一直到目標行人對應跟蹤器的跟蹤強度減小到一定程度或者人為設定放棄跟隨。

如圖7所示，獲得目標行人在全局坐標系下的坐標 (xp,yp) 以及機器人自身的坐標 (xr,yr) 后，在機器人與目標行人的連線上，以距離行人位置0.2 m處為終點，指向行人為目標方向設定機器人的目標位姿 (xg,yg,yawg)

drp=sqrt((xp-xr)2+(yp-yr)2)

(7)

drt=drp-0.2

(8)

xg=xr+drt*((xp-xr)/drp)

(9)

yg=yr+drt*((yp-yr)/drp)

(10)

yawg=arctan((yp-yr),(xp-xr))

(11)

將目標位姿發(fā)布到機器人操作系統(tǒng)(robot operating system，ROS)，借助ROS的navigation功能完成機器人自動跟隨。

圖7 跟隨目標

5 實驗結(jié)果

基于第1節(jié)所介紹的實驗平臺，本文使用C++語言在ROS操作系統(tǒng)下實現(xiàn)了以上所述基于激光雷達的似然域背景差分行人檢測、跟蹤與跟隨。算法運行在CPU為Intel i7-3610QE的工控機上，在不使用GPU加速的情況下，共統(tǒng)計了380幀正常工作中的有效激光雷達數(shù)據(jù)，實現(xiàn)了實時跟蹤多行人以及目標行人跟隨。

5.1 似然域背景差分實驗

如圖8所示，機器人分別位于圖3中的1-3號位置，圖中黑色輪廓為地圖原有障礙，障礙上的白色細點為似然域分割出來的激光雷達背景數(shù)據(jù)，白色較粗且有黑邊包裹的點為似然域方法提取出來的激光雷達前景數(shù)據(jù)，中間黑色的圓為機器人所在位置，并且圖中有標出前景中實驗者的位置。

圖8 似然域背景差分結(jié)果

從地圖3個位置的實驗結(jié)果可以看到，如果激光雷達數(shù)據(jù)比較靠近地圖原有障礙，均被表示成背景。只有當激光雷達數(shù)據(jù)比較遠離地圖原有障礙，似然度較低時，才會被當成前景。表明本文提出的基于似然域模型的背景差分可以有效地將激光雷達數(shù)據(jù)中的前景和背景區(qū)分開來。

5.2 似然域背景差分對復雜背景干擾的抑制實驗

本文利用ROS上公開的人腿檢測實現(xiàn)分別對原始激光雷達數(shù)據(jù)以及利用似然域背景差分提取到的前景進行了行人檢測實驗。如圖9(a)所示，機器人位于位置1處，行人位于一個表面較窄的柜子旁邊。圖9(b)是在圖9(a)情況下直接采用全部激光雷達數(shù)據(jù)進行檢測得到的結(jié)果，行人左腿與呈直角型的柜子被誤檢成一個行人。圖9(c)是在圖9(a)情況下采用似然域背景差分提取到的前景數(shù)據(jù)進行檢測得到的結(jié)果，作為背景的柜子數(shù)據(jù)不參與檢測，行人被正確檢測。如圖9(d)所示，機器人位于位置2處，一張椅子位于一個辦公桌前。圖9(e)是在圖9(d)情況下采用全部數(shù)據(jù)進行檢測得到的結(jié)果，椅子與柜子分別被當成兩條腿誤檢成行人。圖9(f)是在圖9(d)情況下采用前景數(shù)據(jù)檢測得到，排除了背景的干擾，椅子未被誤檢。如圖9(g)所示，機器人位于位置3處，行人靠近一個柜子的邊沿。圖9(h)和圖9(i)分別是在圖9(g)情況下利用全部數(shù)據(jù)以及利用前景數(shù)據(jù)檢測得到的結(jié)果。在利用全部數(shù)據(jù)時，行人左腿與柜子被誤檢成一個行人。而在僅利用前景進行檢測時，行人被正確檢測。

圖9 似然域背景差分對復雜背景干擾的抑制

實驗結(jié)果表明，進行似然域背景差分后，背景部分激光雷達數(shù)據(jù)不參與后續(xù)行人的檢測和跟蹤，可以一定程度上降低復雜背景對行人檢測的干擾。

5.3 行人檢測實驗

為了驗證本文提出的背景差分與行人檢測算法，在圖3中的1-3號位置分別進行了單人、雙人、拄拐杖單人3項檢測，其中雙人實驗中兩個人并排走動，距離不超過50 cm。

對于每項檢測，行人在距離激光雷達50 cm到150 cm之間走動，共有3個穿長褲及4個穿短褲的行人參與檢測。本文利用采集到的有效幀分別在有無背景分割情況下，利用本文提出的檢測算法進行檢測，記為方法1；利用ROS上公開的實現(xiàn)進行檢測，記為方法2。如圖10所示，圖10(a)～圖10(c)分別為位置1-3的3次實驗。3項實驗的統(tǒng)計結(jié)果見表1，P為準確率(Precision)，R為召回率(Recall)。行人在距離激光雷達50 cm到150 cm之間移動，約占激光雷達數(shù)據(jù)的1/4，考慮到激光雷達數(shù)據(jù)聚類的連續(xù)性，假定每幀數(shù)據(jù)有4個樣本，僅對正樣本進行標注，除去正樣本數(shù)即每個有效幀的負樣本數(shù)。

圖10 不同位置的行人檢測結(jié)果

表1 行人檢測結(jié)果

檢測結(jié)果顯示本文提出的檢測算法和方法2在有背景分割的情況下準確率比沒有背景分割的情況分別提高了3.49%、3.64%，召回率分別提高了0.37%、0.43%，表明對激光雷達數(shù)據(jù)進行似然域背景差分能夠有效降低復雜背景對行人檢測的影響。

在同等條件下，本文提出的檢測算法比方法2準確率平均提高0.535%，召回率平均提高0.1%，具有更好的檢測效果。

5.4 行人跟蹤及跟隨實驗

如圖11所示，黑色五角星和黑色圓形分別標識出了機器人與目標行人的起點和終點，起點和終點間的黑色粗線是機器人的軌跡、白色粗線是目標行人檢測與跟蹤后得到的坐標。為了檢驗本文提出的檢測算法與跟蹤算法以及跟隨策略的穩(wěn)定性，有一非目標行人在目標行人出現(xiàn)后出現(xiàn)，然后與目標行人并行，最后超越目標行人并與目標行人交叉運動。非目標行人的運動軌跡由白色箭頭標識出，軌跡上斷續(xù)的灰色粗線為非目標行人檢測得到的坐標。目標行人與非目標行人的步行速度約0.3 m/s。

圖11 行人跟蹤與跟隨圖例

由圖11可以看到，非目標行人與機器人并行時只能檢測到一條腿，以及相交時被目標行人遮擋，會導致非目標行人檢測失敗，軌跡出現(xiàn)斷續(xù)，但本文改進的跟蹤器很好地對非目標行人進行了跟蹤，始終沒有丟失該非目標行人。目標行人的白色軌跡線較為連續(xù)平滑，且與代表非目標行人的灰色軌跡線相交時也沒有出現(xiàn)錯誤，表明本文提出的行人檢測與跟蹤算法能夠有效地對多行人進行檢測與跟蹤。機器人與目標行人軌跡基本重合，表明機器人能夠較好地跟隨目標行人。

跟蹤過程共采集到380幀有效激光雷達數(shù)據(jù)，機器人移動速度如圖12所示，機器人與目標行人間距如圖13所示。在第180幀附近，目標行人與非目標行人交叉出現(xiàn)，且正在轉(zhuǎn)彎，導致目標行人與機器人距離小于設定安全距離0.6 m，機器人停止移動。在第0幀到第150幀以及第230幀到第380幀期間，機器人與行人的間距平穩(wěn)，機器人速度在短暫停頓重新規(guī)劃路徑后也能快速啟動到平穩(wěn)的速度。表明本文提出的跟隨策略能使機器人對目標行人進行平穩(wěn)跟隨。

圖12 機器人移動速度

圖13 機器人與目標行人間距

在有背景分割情況下，對380幀有效數(shù)據(jù)分別使用方法1和方法2進行了行人檢測實驗。檢測時間如圖14所示，其中方法1的平均檢測時間為3.426 ms，方法2的平均檢測時間為5.014 ms。表明本文提出的行人檢測算法能有效降低行人檢測時間。

圖14 兩種方法的行人檢測時間

6 結(jié)束語

針對基于激光雷達的移動機器人，本文提出一種先對環(huán)境建圖，再利用似然域模型提取前景數(shù)據(jù)來降低復雜背景干擾的行人檢測、跟蹤、跟隨方法。實驗結(jié)果表明，本方案能夠有效提取激光雷達的前景數(shù)據(jù)，一定程度上降低了復雜背景對行人檢測、跟蹤的干擾。對于前景數(shù)據(jù)選用層次聚類算法進行聚類后，采用本文提出的行人檢測方法進行多行人的檢測，然后利用本文改進的跟蹤算法對多行人進行有效跟蹤，最后采用本文提出的自動跟隨策略實現(xiàn)了移動機器人在已知地圖環(huán)境下對目標行人的有效跟隨。整個系統(tǒng)實時性高，不受復雜背景干擾，但對于背景變化較快場合的適應性有待進一步研究。