EKF在機器人目標跟蹤中的應用

溫彬彬 溫鵬智 胡雪花

摘要：擴展卡爾曼濾波在機器人目標跟蹤中有非常重要的應用，該文重點介紹了擴展卡爾曼濾波的工作流程以及如何應用在機器人目標跟蹤中。但EKF算法應用在非線性系統中會產生二次項以上的截斷誤差，累計會影響機器人的定位精度。該文主要從拓展卡爾曼濾波的收斂性上進行分析，利用雙估計用觀測殘差對過程噪聲進行補償，利用新息和觀測值估測觀測噪聲矩陣，通過多次預測、更新迭代從而達到對機器人更精準的定位。

關鍵詞：擴展卡爾曼濾波;目標跟蹤;噪聲補償;觀測噪聲

中圖分類號：TP3? ? ? ? 文獻標識碼：A

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

開放科學（資源服務）標識碼（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

在人工智能時代 ，運動目標跟蹤與檢測的研究有非常重要的意義，廣泛地應用在智能機器人、智慧交通、無人駕駛汽車等領域。在目標跟蹤與檢測中，首先要獲取一個目標的準確位置，就要建立一個良好的濾波型。目前常用有卡爾曼濾波KF、擴展卡爾曼濾波EKF，兩者都是基于區域的跟蹤方法，用來解決狀態估計問題。卡爾曼濾波器是目前最優化的線性濾波，要求觀測方程是線性的，符合高斯分布的。但是實際生活中，目標的移動由于環境變化出現加速或者減速，影響機器人判斷目標的準確性，為了解決這個問題且能繼續使用卡爾曼濾波來進行預測，人們使用擴展卡爾曼濾波，將非線性系統利用數學中一階泰勒級數展開，將狀態轉移方程和觀測方程線性化，再利用卡爾曼濾波進行遞歸估計，這樣能夠對突然加速、減速或環境變化引起的位置和速度變化判斷較為精準。

1 擴展卡爾曼濾波模型

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

對于非線性的系統來說，狀態空間表達式為：

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

[zt=h（xt，vt）]

其中，非線性函數[f]描述的是不同時刻下狀態量變化之間的關系，非線性函數[h]描述的狀態變量[zt]與觀測變量[xt]的關系。隨機變量[wt]代表過程激勵噪聲，隨機變量[vt]代表觀測噪聲 [1]。

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

擴展卡爾曼濾波器的預測方程為：

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

[At]和[Wt]是[t]時刻的過程協方差矩陣，[Qt]為[t]時刻的過程激勵噪聲協方差矩陣。

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

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

狀態更新方程利用觀測值變量[zt]的值校正狀態估計和協方差估計。[h]為反映了狀態變量[zt]與觀測變量[xt]關系的非線性函數，[Ht]和[Vt]是[t]時刻的測量雅克比矩陣，[Rt]為[t]時刻的觀測噪聲協方差矩陣[2]。

2擴展卡爾曼濾波在機器人目標跟蹤中使用

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

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

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

在機器人運動狀態下的狀態方程可表示為：

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

其中，[controlt=[vl，vr]]，狀態轉移函數[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）

假設相機[t]時刻觀測到的人體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]的協方差矩陣分別為：

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

擴展卡爾曼濾波應用在機器人目標跟蹤的過程如圖1所示：

通過擴展卡爾曼濾波，可以從上一時刻[t-1]對當前時刻[t]的人體三維位置與它的不確定性進行預測，得到觀測方程，在觀測過程中獲得新的觀測，對人體狀態進行更新，得到人體的后驗估計，同時更新該時刻的協方差矩陣，為下一次迭代做準備，通過多次迭代的，得到人體的準確位置。

3 改進的擴展卡爾曼濾波

上述機器人目標跟中擴展卡爾曼濾波先驗人體位置，默認噪聲方差為0，但是對于實際的跟蹤系統，肯定會存在噪聲，噪聲的存在會影響預估，會導致系統發散，影響跟蹤性能和準確率。本文主要從拓展卡爾曼濾波的收斂性上進行分析，通過對過程噪聲和觀測噪聲進行估計[3]，讓其收斂，從而預測下一時刻的狀態，得到下一時刻的協方差，從而達到對機器人更精準的定位。

對于一個運動狀態坐標系：

用[X（n）=[r_x （n）? （r_x （n）? ） ?r_y （n）? （r_y （n）? ） ?]? ]表示目標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）]]定義為觀測噪聲矩陣。

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

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

設[?θn=dYn，qn+1=dX（n）]，在采樣周期內[?θn≈Yn?Y（n?1）]，實際上q（n）可以看成加速度帶來的混合過程噪聲。可以用方程來估計q（n）。

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

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

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

接下來利用新息和觀測值來求修正的觀測噪聲矩陣，假設均值和方差都為某一常數[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時刻的預測誤差，新息的均值和測量噪聲的均值相等，新息協方差矩陣為：

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

修正的觀測噪聲矩陣為：

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

將公式（12）和公式（16）列入擴展卡爾曼濾波流程中，通過多次迭代遞歸，進而得到機器人目標跟蹤的更加準確位置。

4 仿真結果及結論

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

參考文獻：

[1] 白聰軒.多線索分塊匹配的移動機器人目標跟蹤[D].北京：北京工業大學，2015.

[2] 許亞朝，何秋生，王少江，等.一種改進的自適應卡爾曼濾波算法[J].太原科技大學學報，2016，37（3）：163-168.

[3] 秦陽.基于深度學習的運動目標識別跟蹤系統設計[D].沈陽：沈陽理工大學，2021.

[4] 唐濤，黃永梅.改進的EKF算法在目標跟蹤中的運用[J].光電工程，2005，32（9）：16-18.

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

【通聯編輯：代影】