EKF在機(jī)器人目標(biāo)跟蹤中的應(yīng)用

溫彬彬 溫鵬智 胡雪花

摘要：擴(kuò)展卡爾曼濾波在機(jī)器人目標(biāo)跟蹤中有非常重要的應(yīng)用，該文重點(diǎn)介紹了擴(kuò)展卡爾曼濾波的工作流程以及如何應(yīng)用在機(jī)器人目標(biāo)跟蹤中。但EKF算法應(yīng)用在非線性系統(tǒng)中會產(chǎn)生二次項(xiàng)以上的截?cái)嗾`差，累計(jì)會影響機(jī)器人的定位精度。該文主要從拓展卡爾曼濾波的收斂性上進(jìn)行分析，利用雙估計(jì)用觀測殘差對過程噪聲進(jìn)行補(bǔ)償，利用新息和觀測值估測觀測噪聲矩陣，通過多次預(yù)測、更新迭代從而達(dá)到對機(jī)器人更精準(zhǔn)的定位。

關(guān)鍵詞：擴(kuò)展卡爾曼濾波;目標(biāo)跟蹤;噪聲補(bǔ)償;觀測噪聲

中圖分類號：TP3? ? ? ? 文獻(xiàn)標(biāo)識碼：A

文章編號：1009-3044（2021）36-0116-03

開放科學(xué)（資源服務(wù)）標(biāo)識碼（OSID）：

Application of EKF in Robot Target Tracking

WEN Bin-bin1， WEN Peng-zhi2，HU Xue-hua1

（1.Hebei Vocational University of Industry and Technology， Shijiazhuang 050000， China; 2.Xingtai Modern Occupation School， Xingtai 054000， China）

Abstract： Extended Kalman filter has a very important application in robot target tracking. This paper focuses on the workflow of extended Kalman filter and how to apply it in robot target tracking. However， when EKF algorithm is applied in nonlinear system， it will produce truncation errors above quadratic term， which will affect the positioning accuracy of the robot. This paper mainly analyzes the convergence of extended Kalman filter， uses double estimation to compensate the process noise with observation residuals， uses innovation and observation values to estimate the observation noise matrix， and achieves more accurate positioning of the robot through multiple prediction and update iterations

Key words： Extended Kalman filter; target tracking; noise compensation; observation noise

在人工智能時(shí)代 ，運(yùn)動目標(biāo)跟蹤與檢測的研究有非常重要的意義，廣泛地應(yīng)用在智能機(jī)器人、智慧交通、無人駕駛汽車等領(lǐng)域。在目標(biāo)跟蹤與檢測中，首先要獲取一個(gè)目標(biāo)的準(zhǔn)確位置，就要建立一個(gè)良好的濾波型。目前常用有卡爾曼濾波KF、擴(kuò)展卡爾曼濾波EKF，兩者都是基于區(qū)域的跟蹤方法，用來解決狀態(tài)估計(jì)問題。卡爾曼濾波器是目前最優(yōu)化的線性濾波，要求觀測方程是線性的，符合高斯分布的。但是實(shí)際生活中，目標(biāo)的移動由于環(huán)境變化出現(xiàn)加速或者減速，影響機(jī)器人判斷目標(biāo)的準(zhǔn)確性，為了解決這個(gè)問題且能繼續(xù)使用卡爾曼濾波來進(jìn)行預(yù)測，人們使用擴(kuò)展卡爾曼濾波，將非線性系統(tǒng)利用數(shù)學(xué)中一階泰勒級數(shù)展開，將狀態(tài)轉(zhuǎn)移方程和觀測方程線性化，再利用卡爾曼濾波進(jìn)行遞歸估計(jì)，這樣能夠?qū)ν蝗患铀佟p速或環(huán)境變化引起的位置和速度變化判斷較為精準(zhǔn)。

1 擴(kuò)展卡爾曼濾波模型

擴(kuò)展卡爾曼濾波重點(diǎn)在解決實(shí)際跟蹤過程中非線性問題，基本思想就是利用線性最小方差估計(jì)理論從與被提取信號有關(guān)的測量中遞推估算出所需的信號，計(jì)算非線性轉(zhuǎn)換后實(shí)際概率分布的高斯近似分布，線性化后繼續(xù)使用卡爾曼濾波來進(jìn)行狀態(tài)估計(jì)。

對于非線性的系統(tǒng)來說，狀態(tài)空間表達(dá)式為：

[xt=fxt?1，μt?1，wt?1]? ? ? ? ? ? ? ? ? ? ? ? （1）

[zt=h（xt，vt）]

其中，非線性函數(shù)[f]描述的是不同時(shí)刻下狀態(tài)量變化之間的關(guān)系，非線性函數(shù)[h]描述的狀態(tài)變量[zt]與觀測變量[xt]的關(guān)系。隨機(jī)變量[wt]代表過程激勵(lì)噪聲，隨機(jī)變量[vt]代表觀測噪聲 [1]。

非線性系統(tǒng)預(yù)測下一時(shí)刻狀態(tài)，需要在某一時(shí)刻點(diǎn)線性化，然后進(jìn)行卡爾曼濾波。但是由于系統(tǒng)存在誤差，無法準(zhǔn)確地獲取真實(shí)點(diǎn)，所以非線性函數(shù)[fxt]在[xt?1]處線性化，即t-1時(shí)刻的后驗(yàn)估計(jì)，系統(tǒng)的噪聲不確定故[wt]設(shè)為0。

擴(kuò)展卡爾曼濾波器的預(yù)測方程為：

[x-^t=f（x^t-1，ut-1，0）p-t=AtPt-1AtT+WtQt-1WtT]? ? ? ? ? ? ? ? ? ? ? ? ? ? （2）

[At]和[Wt]是[t]時(shí)刻的過程協(xié)方差矩陣，[Qt]為[t]時(shí)刻的過程激勵(lì)噪聲協(xié)方差矩陣。

擴(kuò)展卡爾曼濾波器的校正方程如公式（3）所示：

[Kt=Pt-HtT（HtPt-HtT+VtRtVtT）-1x^t=x-^t+Kt（zt-h（x-^t，0））Pt=（I-KtHt）Pt-]? ? ? ? ? ? ? ? ? ? ? ? （3）

狀態(tài)更新方程利用觀測值變量[zt]的值校正狀態(tài)估計(jì)和協(xié)方差估計(jì)。[h]為反映了狀態(tài)變量[zt]與觀測變量[xt]關(guān)系的非線性函數(shù)，[Ht]和[Vt]是[t]時(shí)刻的測量雅克比矩陣，[Rt]為[t]時(shí)刻的觀測噪聲協(xié)方差矩陣[2]。

2擴(kuò)展卡爾曼濾波在機(jī)器人目標(biāo)跟蹤中使用

在實(shí)際運(yùn)動中，只有機(jī)器人做一維運(yùn)動時(shí)，才能保證運(yùn)動方程是線性的，針對速度模型，機(jī)器人的運(yùn)動方程和觀測方程是非線性，當(dāng)轉(zhuǎn)換函數(shù)是非線性時(shí)，盡管自變量是高斯分布，因變量的分布也不再是高斯分布，機(jī)器人在跟蹤人，可以看作一個(gè)非線性系統(tǒng)。使用擴(kuò)展卡爾曼濾波用在機(jī)器人目標(biāo)跟蹤[2]。在機(jī)器人坐標(biāo)系中，人體在[t]時(shí)刻的位置定義為[（xr，yr，zr）]。擴(kuò)展卡爾曼濾波器的狀態(tài)向量為：

[Xr=[xr xr zr xr. yr.]]? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（4）

其中[xr.， yr.]為機(jī)器人在水平面的速度。

在機(jī)器人運(yùn)動狀態(tài)下的狀態(tài)方程可表示為：

[Xrt+1=ft（Xrt，controlt）+Rtwt]v? ? ? ? ? ? ? ? ? ?（5）

其中，[controlt=[vl，vr]]，狀態(tài)轉(zhuǎn)移函數(shù)[f]為：

[ft（xtr，controlt）=（X tr+Δt X tr-ΔXr）cosΔθ+（Y tr+Δt Ytr-ΔYr）sinΔθ-（X tr+Δt X tr-ΔXr）sinΔθ+（Y tr+ΔtYtr -ΔYr）cosΔθZ trXtr cosΔθ+Ytr sinΔθ-v-Xtr sinΔθ+YtrcosΔθ]

（6）

假設(shè)相機(jī)[t]時(shí)刻觀測到的人體3D位置為[ytr]，觀測方程如7所示：

[Ytr=HtXtr+pt]

[Ytr=xtr ytr ztrT]，[Ht=1 0 0 0 00 1 0 0 00 0 1 0 0]? ? ? ? ? ? ? ? （7）

其中觀測噪聲[pt]的協(xié)方差矩陣分別為：

[Rt=Cov（pt）=EptpTt=σ2p100010001.]? ? ? ? ? （8）

擴(kuò)展卡爾曼濾波應(yīng)用在機(jī)器人目標(biāo)跟蹤的過程如圖1所示：

通過擴(kuò)展卡爾曼濾波，可以從上一時(shí)刻[t-1]對當(dāng)前時(shí)刻[t]的人體三維位置與它的不確定性進(jìn)行預(yù)測，得到觀測方程，在觀測過程中獲得新的觀測，對人體狀態(tài)進(jìn)行更新，得到人體的后驗(yàn)估計(jì)，同時(shí)更新該時(shí)刻的協(xié)方差矩陣，為下一次迭代做準(zhǔn)備，通過多次迭代的，得到人體的準(zhǔn)確位置。

3 改進(jìn)的擴(kuò)展卡爾曼濾波

上述機(jī)器人目標(biāo)跟中擴(kuò)展卡爾曼濾波先驗(yàn)人體位置，默認(rèn)噪聲方差為0，但是對于實(shí)際的跟蹤系統(tǒng)，肯定會存在噪聲，噪聲的存在會影響預(yù)估，會導(dǎo)致系統(tǒng)發(fā)散，影響跟蹤性能和準(zhǔn)確率。本文主要從拓展卡爾曼濾波的收斂性上進(jìn)行分析，通過對過程噪聲和觀測噪聲進(jìn)行估計(jì)[3]，讓其收斂，從而預(yù)測下一時(shí)刻的狀態(tài)，得到下一時(shí)刻的協(xié)方差，從而達(dá)到對機(jī)器人更精準(zhǔn)的定位。

對于一個(gè)運(yùn)動狀態(tài)坐標(biāo)系：

用[X（n）=[r_x （n）? （r_x （n）? ） ?r_y （n）? （r_y （n）? ） ?]? ]表示目標(biāo)X，Y方向的位移、速度量。T是采樣周期，n表示第n次采樣，其觀測方程為：

[Z（n+1）=F（n+1，n）X（n）+T?V（n）+v（n）]? ? ? ? ? ? ?（9）

[T?V（n）]是加速度引起的偏量，通常它和[v（n）]作為混合噪聲，因此公式（4）就可以寫為：

[Z（n+1）=F（n+1，n）X（n）+q（n）]? ? ? ? ? ? ?（10）

測量方程為：[Y（n）=?[n，X（n）]+w（n）]，其線性化方程為：

[Yn=?n，Xn|yn?1+HnXn?Xn|yn?1+v（n）]? ? ? ? ?（11）

將[Q1（n）=[q（n） q T（n）]]定義為過程噪聲，將[Q2（n）=[v（n） v T（n）]]定義為觀測噪聲矩陣。

對于一個(gè)運(yùn)動目標(biāo)跟蹤系統(tǒng)模型來說，如果在建模過程中不能考慮所有的噪聲，那么肯定會影響卡爾曼濾波的性能，不能使其達(dá)到最優(yōu)。本文用雙估計(jì)[4]來消除加速度帶來混合噪聲，將測量方程等式兩邊求微分得：

[dYn=??n，Xn?XdX（n）]

設(shè)[?θn=dYn，qn+1=dX（n）]，在采樣周期內(nèi)[?θn≈Yn?Y（n?1）]，實(shí)際上q（n）可以看成加速度帶來的混合過程噪聲?？梢杂梅匠虂砉烙?jì)q（n）。

[qn+1=Fn+1，nqn+vn]? ? ? ? ? ? ? ? （12）

[?θn=Hnqn+v（n）]? ? ? ? ? ? ? ? ? ? ?（13）

通過上式可以得出，加速度的變化可以由觀測值Y（n）的殘差測量來實(shí)時(shí)估計(jì)[4]。

接下來利用新息和觀測值來求修正的觀測噪聲矩陣，假設(shè)均值和方差都為某一常數(shù)[5]，由觀測方程可以得到[v（n）=Y（n）??[n，X（n）]]，將觀測方程的線性方程代入有：

[an=Yn??n，Xn|yn?1=Hnξn，n?1+v（n）]? （14）

其中[ξn，n?1]為n時(shí)刻的預(yù)測誤差，新息的均值和測量噪聲的均值相等，新息協(xié)方差矩陣為：

[P=E[an?an?an?anT]? ? ? ? ? ? ? ? （15）

修正的觀測噪聲矩陣為：

[Q2n=P?E{HnKn，n?1HT（n）}]? ? ?（16）

將公式（12）和公式（16）列入擴(kuò)展卡爾曼濾波流程中，通過多次迭代遞歸，進(jìn)而得到機(jī)器人目標(biāo)跟蹤的更加準(zhǔn)確位置。

4 仿真結(jié)果及結(jié)論

改進(jìn)的卡爾曼濾波算法在仿真中進(jìn)行測試，當(dāng)有一定加速度時(shí)，改進(jìn)后的算法有著明顯的優(yōu)勢，X、Y 方向位置的均方誤差均有明顯減少，證明算法的可行性。利用雙估計(jì)和觀測殘差對過程噪聲進(jìn)行補(bǔ)償，消除加速度帶來的影響，利用新息和觀測值估計(jì)觀測噪聲矩陣，可以較好地消除由觀測噪聲帶來的誤差，通過這些自適應(yīng)算法的加入提高了EKF算法的穩(wěn)定性，提升了跟蹤性能。

參考文獻(xiàn)：

[1] 白聰軒.多線索分塊匹配的移動機(jī)器人目標(biāo)跟蹤[D].北京：北京工業(yè)大學(xué)，2015.

[2] 許亞朝，何秋生，王少江，等.一種改進(jìn)的自適應(yīng)卡爾曼濾波算法[J].太原科技大學(xué)學(xué)報(bào)，2016，37（3）：163-168.

[3] 秦陽.基于深度學(xué)習(xí)的運(yùn)動目標(biāo)識別跟蹤系統(tǒng)設(shè)計(jì)[D].沈陽：沈陽理工大學(xué)，2021.

[4] 唐濤，黃永梅.改進(jìn)的EKF算法在目標(biāo)跟蹤中的運(yùn)用[J].光電工程，2005，32（9）：16-18.

[5] 董俊松.基于擴(kuò)展卡爾曼濾波的單目標(biāo)跟蹤算法研究[D].蘭州：西北師范大學(xué)，2019.

【通聯(lián)編輯：代影】