趙宏強(qiáng), 鄧文斌, 辛 濤, 吳 鈧

(1.中南大學(xué) 機(jī)電工程學(xué)院,湖南 長(zhǎng)沙 410083； 2.山河智能裝備股份有限公司,湖南 長(zhǎng)沙 410100； 3.中國(guó)人民解放軍32181部隊(duì),河北 石家莊 050003)

0 引 言

目標(biāo)跟蹤是無(wú)人車(chē)領(lǐng)域中的一個(gè)研究熱點(diǎn),許多傳感器被應(yīng)用于解決這個(gè)問(wèn)題。相機(jī)具有采集信息豐富、成本低的優(yōu)點(diǎn),目前較為主流的應(yīng)用方法有檢測(cè)—跟蹤框架,思路是采集顏色信息、深度信息作為感知輸入,在每幀圖像中檢測(cè)出行人,然后對(duì)不同圖像幀中的行人進(jìn)行數(shù)據(jù)關(guān)聯(lián),使用算法模型預(yù)測(cè)行人狀態(tài),進(jìn)而實(shí)現(xiàn)對(duì)行人目標(biāo)的跟蹤[1,2]。但相機(jī)工作受到光照亮度限制,且容易因?yàn)槟繕?biāo)走出攝像范圍而跟蹤失敗。此外,激光雷達(dá)也被用于對(duì)目標(biāo)進(jìn)行檢測(cè)與跟蹤。李帥印等人對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行目標(biāo)聚類(lèi),利用改進(jìn)的匈牙利算法建立跟蹤器,通過(guò)卡爾曼濾波(Kalman filtering,KF)算法對(duì)目標(biāo)進(jìn)行估計(jì)[3,4]。但激光雷達(dá)是通過(guò)點(diǎn)云信息進(jìn)行檢測(cè),計(jì)算復(fù)雜耗時(shí)長(zhǎng)、檢測(cè)準(zhǔn)確率低,當(dāng)遇到多個(gè)相似目標(biāo)時(shí),難識(shí)別期望跟蹤目標(biāo)。文獻(xiàn)[5]使用擴(kuò)展卡爾曼濾波(extended Kalman filtering,EKF)算法對(duì)行人位置進(jìn)行估計(jì)與預(yù)測(cè),定位效果比最小二乘估計(jì)(least square estimation,LSE)定位更好。文獻(xiàn)[6]將無(wú)跡卡爾曼濾波(unscented Kalman filtering,UKF)應(yīng)用于目標(biāo)跟蹤,利用無(wú)跡變換計(jì)算目標(biāo)跟蹤問(wèn)題的后驗(yàn)密度,實(shí)驗(yàn)結(jié)果表明,在系統(tǒng)非線(xiàn)性化高時(shí)UKF的精度要優(yōu)于EKF。Arasaratnam I等人提出了容積卡爾曼濾波(cubature Kalman filtering,CKF),仿真實(shí)驗(yàn)表明其在高維度下精度比UKF更高[7]。在濾波中,不準(zhǔn)確的噪聲統(tǒng)計(jì)特性也會(huì)嚴(yán)重影響濾波精度和穩(wěn)定性。文獻(xiàn)[8]提出了一種自適應(yīng)UKF,其在精度與穩(wěn)定性上比常規(guī)濾波方法更好。

本文提出的無(wú)人車(chē)目標(biāo)跟蹤方法使用超寬帶(ultra-wideband,UWB)傳感器采集距離信息作為觀(guān)測(cè)量,通過(guò)KF定位算法對(duì)噪聲統(tǒng)計(jì)特性進(jìn)行在線(xiàn)估計(jì)并計(jì)算動(dòng)態(tài)目標(biāo)后驗(yàn)概率密度,實(shí)現(xiàn)對(duì)期望目標(biāo)的定位跟蹤。并以8×8全地形無(wú)人車(chē)作為實(shí)驗(yàn)平臺(tái)[9],通過(guò)多種場(chǎng)景下的實(shí)車(chē)試驗(yàn),證明了該方法在目標(biāo)跟蹤問(wèn)題中具有良好的定位精度和穩(wěn)定性。

1 UWB技術(shù)

1.1 概 念

UWB技術(shù)[10]因具有高時(shí)間分辨力、穿透力強(qiáng)、信號(hào)功率譜密度低、功耗小等特點(diǎn),近年來(lái)被廣泛應(yīng)用于雷達(dá)探測(cè)、無(wú)線(xiàn)通信和高精度室內(nèi)定位等領(lǐng)域[11]。

本文使用的Humatics公司旗下P440 UWB傳感器,通過(guò)脈沖信號(hào)雙向飛行時(shí)間(two-way time-of-flight,TW-ToF)進(jìn)行點(diǎn)對(duì)點(diǎn)測(cè)距,如圖1所示。

圖1 TW-ToF測(cè)距原理示意

S=c·((Ta2-Ta1)-(Tb2-Tb1))/2

(1)

式中S為2個(gè)P440之間的待測(cè)距離；c為光速。在大多數(shù)環(huán)境下P440具有較高的測(cè)距精度,實(shí)測(cè)能達(dá)到厘米(cm)級(jí)精度；在完全空曠的視距環(huán)境(line of sight,LoS)中,其最大測(cè)量距離可達(dá)410 m,誤差在2 cm以?xún)?nèi)。

1.2 應(yīng)用與觀(guān)測(cè)模型

本文將所用UWB傳感器區(qū)分為錨點(diǎn)(anchor)和標(biāo)簽(tag)兩種。標(biāo)簽由期望跟蹤目標(biāo)攜帶,錨點(diǎn)固定在無(wú)人車(chē)上提供距離信息作為感知輸入。

如圖2所示，觀(guān)測(cè)量zk=[r1,r2,…,rm]T,ri(i=1,2,…,m)代表Tag與Anchor間的距離,m代表Anchor數(shù)量,在本文中m=3。設(shè)Anchor的坐標(biāo)位置(xj,yj),j=1,2,3；基于三邊定位法建立觀(guān)測(cè)模型

zk=h(xk)=[r1,r2,r3]T

(2)

(3)

圖2 觀(guān)測(cè)模型

2 KF定位算法

2.1 平方根CKF

xk=f(xk-1,uk)+wk-1，zk=h(xk)+vk

(4)

式中xk和uk為k時(shí)刻的系統(tǒng)狀態(tài)向量(n維)和控制輸入量；f(xk-1)和wk-1～N(0,Qk-1)代表系統(tǒng)狀態(tài)模型函數(shù)和系統(tǒng)噪聲；zk為k時(shí)刻的觀(guān)測(cè)量；h(xk)和vk～N(0,Rk)為系統(tǒng)觀(guān)測(cè)模型函數(shù)與觀(guān)測(cè)噪聲。SRCKF通過(guò)Cubature規(guī)則選取并計(jì)算2n個(gè)權(quán)值相同的容積點(diǎn)集合,利用時(shí)間更新和量測(cè)更新對(duì)系統(tǒng)狀態(tài)值進(jìn)行估計(jì)。

2.2 平方根噪聲估值器

Sage-Husa噪聲估值器被廣泛應(yīng)用于各種濾波算法,但其是在常規(guī)卡爾曼框架所推導(dǎo)[12]。本文引入一種新型噪聲估值方法[13],推導(dǎo)出平方根噪聲估值器,用其估計(jì)系統(tǒng)狀態(tài)噪聲的統(tǒng)計(jì)特性

(5)

(6)

dk=(1-b)/(1-bk)

(7)

2.3 濾波發(fā)散的抑制方法

針對(duì)濾波中可能會(huì)出現(xiàn)的發(fā)散,使用協(xié)方差匹配判據(jù)判斷其是否出現(xiàn)

(8)

式中 trace(A)為求取矩陣A的跡；S為可調(diào)系數(shù)(S≥1)。

在濾波過(guò)程中,若滿(mǎn)足上述判據(jù),則認(rèn)為濾波收斂；否則認(rèn)為濾波出現(xiàn)發(fā)散趨勢(shì),計(jì)算衰減因子λk,對(duì)Sk|k-1進(jìn)行修正

(9)

(10)

式中ρ為衰減系數(shù),取值范圍0<ρ≤1,常取ρ=0.95,ρ越大越突出殘差εk的影響。

2.4 自適應(yīng)平方根CKF

SRCKF算法:

時(shí)間更新

Tria( )為QR分解表達(dá)式

測(cè)量更新

傳播容積點(diǎn):Zi,k|k-1=h(Xi,k|k-1)

計(jì)算自相關(guān)協(xié)方差、互相關(guān)協(xié)方差:

Szz,k|k-1=Tria([Zk|k-1SR,k-1])

更新協(xié)方差平方根:

Sk|k=Tria([χk|k-1-KkZk|k-1KkSR,k-1])

3 無(wú)人車(chē)目標(biāo)跟蹤方法設(shè)計(jì)與實(shí)現(xiàn)

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

通過(guò)對(duì)無(wú)人車(chē)與目標(biāo)進(jìn)行運(yùn)動(dòng)學(xué)分析建立系統(tǒng)狀態(tài)模型,如圖3。

圖3 運(yùn)動(dòng)模型

取系統(tǒng)初始化時(shí)的車(chē)體坐標(biāo)系作為參考坐標(biāo)系,狀態(tài)變量xk=[xk,yk,vx,k,vy,k]T,(xk,yk)表示k時(shí)刻目標(biāo)坐標(biāo)位置；vx,k與vy,k代表k時(shí)刻目標(biāo)在運(yùn)動(dòng)速度。

假設(shè)目標(biāo)在k-1到k時(shí)刻內(nèi)做勻速運(yùn)動(dòng),時(shí)間間隔為Δt。無(wú)人車(chē)對(duì)目標(biāo)進(jìn)行定位跟蹤時(shí),往往兩者都在運(yùn)動(dòng)。因此,在建立系統(tǒng)狀態(tài)模型時(shí),將無(wú)人車(chē)狀態(tài)[v,ΔX,ΔY,Δθ]T作為系統(tǒng)控制向量uk

(11)

式中wk-1為系統(tǒng)噪聲,其協(xié)方差矩陣為Q2×2。v為無(wú)人車(chē)的速度,ΔX為無(wú)人車(chē)在Δt時(shí)間內(nèi)沿X軸的移動(dòng)距離,ΔY為無(wú)人車(chē)在Δt時(shí)間內(nèi)沿Y軸的移動(dòng)距離,Δθ為無(wú)人車(chē)在Δt時(shí)間內(nèi)的偏轉(zhuǎn)角度。

3.2 無(wú)人車(chē)動(dòng)態(tài)目標(biāo)跟蹤方法實(shí)現(xiàn)步驟

4 實(shí)驗(yàn)與分析

為驗(yàn)證本文所設(shè)計(jì)目標(biāo)跟蹤方法的性能,在多種場(chǎng)景下進(jìn)行了實(shí)車(chē)試驗(yàn)。

4.1 前期工作

本文實(shí)驗(yàn)平臺(tái)為某型8×8差分輪越野無(wú)人平臺(tái),圖4為P440模塊布局示意,以車(chē)體中心建立車(chē)體坐標(biāo)系,3只傳感器的坐標(biāo)位置分別是(1.4,0.43),(0,-0.43),(-1.4,0.43)m。

為了保證數(shù)據(jù)的有效性并得到足夠數(shù)據(jù),對(duì)P440的性能參數(shù)進(jìn)行設(shè)定,設(shè)置其信號(hào)積分增益系數(shù)為7,最大測(cè)距距離小于207 m,最大測(cè)距頻率為46.9 Hz。

圖4 傳感器布局

4.2 實(shí)驗(yàn)場(chǎng)景

本文設(shè)計(jì)了3種實(shí)驗(yàn)場(chǎng)景,使用LSE定位算法、EKF定位算法、UKF定位算法、CKF定位算法作為對(duì)比方法與本文所提方法跟蹤軌跡進(jìn)行對(duì)比分析。

實(shí)驗(yàn)一：在室外有明顯金屬物遮擋的情況下,車(chē)輛靜止時(shí)系統(tǒng)對(duì)動(dòng)態(tài)目標(biāo)位置進(jìn)行跟蹤,如圖5(a)；

實(shí)驗(yàn)二：在室外無(wú)明顯金屬遮擋的情況下,車(chē)輛靜止時(shí)系統(tǒng)對(duì)動(dòng)態(tài)目標(biāo)位置進(jìn)行跟蹤,如圖5(b)；

實(shí)驗(yàn)三：如圖5(c),用粗粉筆在試驗(yàn)場(chǎng)地畫(huà)出長(zhǎng)8 m,寬5.6 m的方框代表目標(biāo)運(yùn)動(dòng)軌跡?？刂栖?chē)輛跟隨目標(biāo)運(yùn)動(dòng),將得到的目標(biāo)軌跡與方框進(jìn)行對(duì)比。

設(shè)R=diag{0.1,0.1,0.1}；設(shè)初始狀態(tài)時(shí)Q=diag{0.05,0.05,0.05},本文方法在迭代中會(huì)不斷對(duì)其進(jìn)行更新。

圖5 實(shí)驗(yàn)場(chǎng)景

4.3 結(jié)果與分析

1)實(shí)驗(yàn)一

由圖6(a)可知,大型金屬遮擋物對(duì)定位跟蹤結(jié)果的影響很大,所以，通過(guò)LSE方法所得軌跡曲線(xiàn)多處出現(xiàn)大的跳變；EKF、CKF和UKF在一定程度減小了定位誤差,平滑了曲線(xiàn),但效果不夠明顯。圖6(b)中曲線(xiàn)明顯比圖6(a)中光滑,且基本消除了大的跳變。本文方法通過(guò)容積變換有效減小了模型誤差,因此,提升了定位精度和穩(wěn)定性。

圖6 實(shí)驗(yàn)一結(jié)果

2)實(shí)驗(yàn)二

場(chǎng)景二中無(wú)明顯遮擋,所以軌跡曲線(xiàn)整體上較圖6平滑。對(duì)比方法中LSE跳變最大；CKF和UKF跟蹤效果較LSE略好；EKF的定位效果最優(yōu)。本文方法使用噪聲估值器自適應(yīng)估計(jì)系統(tǒng)噪聲,能有效地改善定位效果。所以,圖7(b)中軌跡曲線(xiàn)明顯比圖7(a)中曲線(xiàn)更光滑,且基本沒(méi)有位置跳變。

圖7 實(shí)驗(yàn)二結(jié)果

3)實(shí)驗(yàn)三

設(shè)置該場(chǎng)景有兩個(gè)目的：1)分析無(wú)人車(chē)運(yùn)動(dòng)時(shí)本文方法的跟蹤效果；2)分析本文提出目標(biāo)跟蹤方法的定位精度。實(shí)驗(yàn)中,UKF跟蹤結(jié)果出現(xiàn)發(fā)散,所以,圖8(a)中沒(méi)有UKF軌跡曲線(xiàn)。

在第三種場(chǎng)景中,系統(tǒng)噪聲特性更加復(fù)雜,常值噪聲與實(shí)際情況相差較大,所以本文方法的定位效果更好。圖8(b)中曲線(xiàn)比圖8(a)更加光滑且波動(dòng)小。考慮到在實(shí)際運(yùn)動(dòng)中,目標(biāo)運(yùn)動(dòng)軌跡很難與真實(shí)軌跡完全一致,可以認(rèn)為本文方法的跟蹤軌跡與實(shí)際中的目標(biāo)運(yùn)動(dòng)軌跡基本相符,其定位誤差約為15 cm。

圖8 實(shí)驗(yàn)三結(jié)果

5 結(jié) 論

本文設(shè)計(jì)了一種UWB應(yīng)用方案，將3只UWB傳感器固定于無(wú)人車(chē),1只作為目標(biāo),利用傳感器之間的距離信息進(jìn)行目標(biāo)定；設(shè)計(jì)了一種KF定位算法,利用平方根噪聲估值器對(duì)系統(tǒng)噪聲進(jìn)行估計(jì),有效地提高了定位精度和穩(wěn)定性；對(duì)所提無(wú)人車(chē)目標(biāo)跟蹤方法進(jìn)行了實(shí)車(chē)試驗(yàn)。結(jié)果表明：所提方法具有很好的定位精度和穩(wěn)定性,在動(dòng)態(tài)定位中有15 cm的精度,能滿(mǎn)足無(wú)人車(chē)應(yīng)用的要求。本文只使用了單一傳感器,仍存在一定局限性,后續(xù)工作考慮加入視覺(jué)跟蹤等方法進(jìn)行多傳感器融合,提升目標(biāo)跟蹤的可靠性和定位精度。