基于Kalman濾波器的車式移動(dòng)機(jī)器人跟蹤方法

張香竹，余輝榮，汪俊彬

（華南理工大學(xué) 自動(dòng)化科學(xué)與工程學(xué)院控制與優(yōu)化中心，廣州 510641）

目前，在智能監(jiān)控領(lǐng)域，對視頻序列中的多移動(dòng)機(jī)器人進(jìn)行實(shí)時(shí)跟蹤已成為研究熱點(diǎn)[1-2]。跟蹤過程分為兩種基本策略：一種是對機(jī)器人建立模型，并通過模型進(jìn)行算法的應(yīng)用；另一種則是不建立模型，如文獻(xiàn)[3]中提出的直接利用反饋信息對機(jī)器人進(jìn)行控制，但這種控制方式對于需要知道車頭方向的車式移動(dòng)機(jī)器人并不適用。文獻(xiàn)[4]建立了非線性的機(jī)器人模型，雖然模型的精度較高，但是不適用于計(jì)算機(jī)控制。文獻(xiàn)[5]中給出的機(jī)器人模型適用于輪式機(jī)器人，卻不適用于車式機(jī)器人。Kalman濾波算法[6]、Condensation算法[7]等數(shù)學(xué)算法經(jīng)常用于目標(biāo)跟蹤過程。由于Condensation算法實(shí)時(shí)性較差且動(dòng)態(tài)模型精度較低，故很少使用。Kalman濾波是在已知系統(tǒng)和量測的數(shù)學(xué)模型、量測噪聲統(tǒng)計(jì)特性及系統(tǒng)狀態(tài)初值的情況下，利用輸出信號的量測數(shù)據(jù)和系統(tǒng)模型方程，實(shí)時(shí)獲得系統(tǒng)狀態(tài)變量和輸入信號的最優(yōu)估計(jì)。Kalman濾波器不僅能對目標(biāo)軌跡進(jìn)行濾波和優(yōu)化，還可以對目標(biāo)狀態(tài)進(jìn)行預(yù)估[8]，且實(shí)時(shí)性較好，適用于多目標(biāo)跟蹤。

Kalman濾波器需使用線性化模型[9]，為了便于引入Kalman濾波器，本文首先對車式移動(dòng)機(jī)器人進(jìn)行建模，再利用線性化方法進(jìn)行線性化和離散化，最后將多Kalman濾波器應(yīng)用于多機(jī)器人的軌跡跟蹤優(yōu)化。

1 實(shí)驗(yàn)平臺

實(shí)驗(yàn)平臺硬件結(jié)構(gòu)如圖1所示。系統(tǒng)在邏輯層面上可劃分為監(jiān)控層、通信層和對象層。從實(shí)體結(jié)構(gòu)分析，監(jiān)控層包括上位機(jī)和攝像機(jī)，兩者由IEEE1394接口連接。其中攝像機(jī)負(fù)責(zé)感知并收集圖像幀、信息預(yù)處理、目標(biāo)分割和目標(biāo)定位等序列任務(wù)，最終完成監(jiān)控區(qū)域內(nèi)移動(dòng)目標(biāo)的坐標(biāo)狀態(tài)提取和實(shí)時(shí)監(jiān)控工作；而上位機(jī)負(fù)責(zé)對攝像機(jī)的采樣數(shù)據(jù)進(jìn)行信息融合，通過對目標(biāo)模型的建立及Kalman濾波器的運(yùn)用，獲得目標(biāo)對象的多維狀態(tài)，并實(shí)時(shí)顯示于屏幕，為對象進(jìn)一步的精確控制提供參考信息。通信層包含基站和無線射頻RF（radio frequency）模塊，其中基站負(fù)責(zé)整個(gè)系統(tǒng)運(yùn)行的時(shí)間資源分配，規(guī)定系統(tǒng)內(nèi)各通信單元的工作時(shí)隙，保證通信過程有條不紊地進(jìn)行。無線射頻模塊的任務(wù)是通過空中接口實(shí)現(xiàn)基站與移動(dòng)機(jī)器人之間的無線通信。對象層由多個(gè)被控對象，即多臺車式移動(dòng)機(jī)器人組成。被控對象和基站均采用ST公司的STM32系列32位芯片作為主控制器，提高系統(tǒng)總體通用性的同時(shí)也便于后續(xù)擴(kuò)展。

圖1 實(shí)驗(yàn)平臺結(jié)構(gòu)圖Fig.1 Configuration of experimental platform

為了更直觀地體現(xiàn)系統(tǒng)各部分之間的聯(lián)系，給出系統(tǒng)框圖如圖2所示。本文選用積分分離PID控制器[10]，既能保證控制的實(shí)時(shí)性，又能減小穩(wěn)態(tài)誤差，保證控制精度。反饋環(huán)節(jié)包括上述的監(jiān)控系統(tǒng)以及Kalman濾波器。其中監(jiān)控系統(tǒng)的采樣數(shù)據(jù)作為原始觀測信息，而Kalman濾波器則根據(jù)機(jī)器人模型，對該觀測信息和PID控制器輸出的控制信號作進(jìn)一步濾波和優(yōu)化，并反饋給控制器，以輸出作用于被控對象。

圖2 系統(tǒng)框圖Fig.2 System block diagram

該算法可以很好地實(shí)現(xiàn)對機(jī)器人的速度控制，使機(jī)器人在給定速度下平穩(wěn)的運(yùn)動(dòng)。

2 具體實(shí)現(xiàn)

2.1 車式移動(dòng)機(jī)器人建模

運(yùn)用Kalman濾波器的前提是已知被控對象的線性化模型。本文所采用的被控對象是四輪車式移動(dòng)機(jī)器人，其中兩個(gè)后輪依靠直流電機(jī)提供前進(jìn)動(dòng)力，即為驅(qū)動(dòng)輪，驅(qū)動(dòng)方向與機(jī)器人前進(jìn)方向一致；兩個(gè)前輪通過舵機(jī)控制機(jī)器人的轉(zhuǎn)向，即為轉(zhuǎn)向輪。車式機(jī)器人的模型如圖3所示。

圖3 車式移動(dòng)機(jī)器人模型圖Fig.3 Model of car-type mobile robot

根據(jù)四輪移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型

式中：（xw，yw）為機(jī)器人中心的世界坐標(biāo)；θ為航向角即機(jī)器人車身方向與橫軸夾角；v為機(jī)器人后軸中心點(diǎn)速度；φ為機(jī)器人前輪轉(zhuǎn)向角；d為機(jī)器人前后軸中心點(diǎn)之間的距離。軸間距d為常值，可人工測得；轉(zhuǎn)向角φ可根據(jù)舵機(jī)當(dāng)前的偏轉(zhuǎn)量求得；機(jī)器人的實(shí)際速度v可通過光電編碼器測速模塊測得，并輸入單片機(jī)控制器，組成被控對象的速度閉環(huán)負(fù)反饋控制。

取狀態(tài)變量 x1=xw，x2=yw，x3=θ~，其中θ~=θ-θ0，代表當(dāng)前時(shí)刻車身方向夾角相對上一時(shí)刻夾角θ0（即當(dāng)前時(shí)刻的基準(zhǔn)參考量）的微小增量；取控制變量u1=v，u2=tanφ。 狀態(tài)向量為 X（k）=（x1（k） x2（k） x3（k））T，觀測向量為 Y（k）=（xw（k） yw（k））T，控制向量為U（k）=（u1（k） u2（k））T。

為了使模型更具普遍性，引入高斯白噪聲。離散化后系統(tǒng)模型的線性方程組為

式中：T表示采樣周期；ωz，υz分別為 3維的系統(tǒng)白噪聲和2維的測量噪聲，且0，其協(xié)方差分別為 Qz，Rz，且 Cov（wz，υz）＝0。

2.2 引入Kalman濾波器

為了得到更準(zhǔn)確的系統(tǒng)狀態(tài)信息，并獲知機(jī)器人系統(tǒng)模型狀態(tài)變量中不可直接測量的重要變量θ~，需要通過Kalman濾波器進(jìn)行預(yù)估運(yùn)算。經(jīng)驗(yàn)證，該系統(tǒng)模型是能控能觀的，因此可以引入全維Kalman觀測器進(jìn)行預(yù)估和校正，同時(shí)保證估計(jì)誤差的方差和 E［X（k）-X?（k|k-1））T（X（k）-X?（k|k-1））］最小。其原理框圖如圖4所示。標(biāo)準(zhǔn)Kalman濾波器的基本工作步驟如下。

圖4 Kalman濾波器原理框圖Fig.4 Kalman filter diagram

第1步：預(yù)估：

（1）根據(jù)上一步的狀態(tài)優(yōu)化結(jié)果X?（k-1|k-1）預(yù)估當(dāng)前狀態(tài)X?（k|k-1）：

（2）計(jì)算當(dāng)前預(yù)估值對應(yīng)的系統(tǒng)協(xié)方差：

第2步：校正：

（1）優(yōu)化 Kalman 增益：

（2）根據(jù)測量值及預(yù)估值，最優(yōu)化當(dāng)前狀態(tài)：

（3）最優(yōu)化相應(yīng)的系統(tǒng)協(xié)方差：

引入Kalman濾波器后，利用其預(yù)估作用，即使出現(xiàn)短期內(nèi)目標(biāo)觀測值丟失的情況，仍然能夠根據(jù)歷史的相關(guān)值及狀態(tài)信息推測出系統(tǒng)未來幾步的預(yù)估信息，從而進(jìn)行合理補(bǔ)償，實(shí)現(xiàn)對移動(dòng)機(jī)器人的全程準(zhǔn)確控制。關(guān)于各矩陣的取法，Qz取一個(gè)非負(fù)定的定常矩陣；Rz取一個(gè)對稱的正定定常矩陣。初始值X（0）和P（0）可根據(jù)系統(tǒng)實(shí)際運(yùn)行情況進(jìn)行賦值。

3 實(shí)驗(yàn)結(jié)果與分析

通過實(shí)驗(yàn)平臺的實(shí)際運(yùn)行效果分析本文所提模型及濾波算法的有效性。為了更直觀地顯示對多個(gè)目標(biāo)的實(shí)際跟蹤效果，實(shí)驗(yàn)選用2個(gè)四輪車式機(jī)器人作為目標(biāo)在坐標(biāo)從（0，0）到（400，600）的矩形平面有效監(jiān)控范圍內(nèi)作直線運(yùn)動(dòng)，并檢驗(yàn)最終的跟蹤效果。

圖5 機(jī)器人軌跡跟蹤效果圖Fig.5 Robot trajectory tracking renderings

圖5是同時(shí)跟蹤兩臺機(jī)器人移動(dòng)軌跡的效果圖。其中虛線代表理想軌跡，實(shí)線表示機(jī)器人的實(shí)際運(yùn)動(dòng)軌跡。由圖可知，即使機(jī)器人的初始位置相比理想位置有所偏離，但是經(jīng)過控制器的調(diào)整和Kalman濾波器的優(yōu)化之后，跟蹤軌跡將逐漸與理想軌跡趨于一致。在位置偏差的容許范圍內(nèi)，Kalman濾波算法的跟蹤效果已達(dá)到要求。

圖6是車式機(jī)器人的實(shí)時(shí)航向角及航向角增量的效果圖。其中實(shí)線表示Kalman算法的優(yōu)化值；虛線表示理想航向角，大約為-90°。從圖可得，雖然濾波器給定的航向角狀態(tài)的初值與小車實(shí)際的航向角有較大的偏差，但經(jīng)過Kalman濾波器的不斷預(yù)估、矯正和優(yōu)化，估計(jì)值與實(shí)際值之間的穩(wěn)態(tài)誤差最終趨于較小的可容許范圍，即對于該狀態(tài)的預(yù)估能達(dá)到準(zhǔn)確跟蹤的要求。當(dāng)濾波器的給定狀態(tài)初始值越接近實(shí)際狀態(tài)初始值時(shí)，Kalman對于狀態(tài)的精確跟蹤也將更加迅速。

圖6 機(jī)器人航向角跟蹤效果Fig.6 Robot heading angle tracking renderings

4 結(jié)語

本文通過建立四輪車式移動(dòng)機(jī)器人的模型，并對模型進(jìn)行線性化和離散化等系列操作，使其適用于Kalman濾波算法的引入。通過成功將多Kalman濾波器應(yīng)用到多個(gè)機(jī)器人控制系統(tǒng)，并利用實(shí)驗(yàn)平臺驗(yàn)證Kalman濾波器對多機(jī)器人軌跡跟蹤優(yōu)化的效果，得出本文提出的算法及應(yīng)用方法的合理性和有效性。實(shí)驗(yàn)結(jié)果表明Kalman濾波在車式移動(dòng)機(jī)器人的軌跡跟蹤中的應(yīng)用能保證跟蹤的精確度和實(shí)時(shí)性，并為進(jìn)一步的機(jī)器人控制提供更多的狀態(tài)信息，其中也包含一些無法被直接測量的狀態(tài)。本文中對多目標(biāo)的實(shí)時(shí)跟蹤，為后續(xù)的模擬飽和攻擊等方面的實(shí)現(xiàn)奠定了基礎(chǔ)，對智能交通、高科技軍事等領(lǐng)域的研究也提供了參考。

[1] Ristic B，Clark D E，Gordon N.Calibration of multi-target tracking algorithms using non-cooperative targets[J].IEEE Journal on Selected Topics in Signal Processing，2013，7 （3）：390-398.

[2] Anderson R J，Spong M W.Bilateral control of teleoperators with time delay[J].IEEE Transactions on Automation Control，1989，34（5）：494-501.

[3] Ai-Guo Wu，Guang-Ren Duan.Robust global stabilization for nonlinear systems with unmodeled dynamics by output feedback[C]//IEEE International Conference on Control and Automation，ICCA，2007：1129-1132.

[4] Shigeru H.Robust nonlinear model predictive control with variable blocklength[J].IEEE TRANSACTIONS ON AUTOMATIC CONTROL，2009，54（7）：1618-1622.

[5] Ling Zhao，Jing ZhiYe，Wen Feng Luo.A prototype target tracking network system for robot feedback control[J].Applied Mechanics and Materials，2012，157-158：1431-1435.

[6] Long Zhao，Hongyu Yan.An adaptive dynamic kalman filtering algorithm based on cumulative sums of residuals[C]//CSNC2013 Electronic Proceedings，2013，245：727-735.

[7] Hui-Ming Hung.Measurement of internally mixed aerosols acting as cloud condensation nuclei[C]//The 7th Cross-straits Air Adhesive Technology Workshop and the 2nd Air Pollution Technology Seminar，2010.

[8] Razak N A.A study of Kalman’s filter in embedded controller for real-time quadrocopter roll and pitch measurement[C]//Proceedings-2012 IEEE International Conference on Control System，Computing and Engineering，ICCSAE 2012：590-595.

[9] PopovA S.Synthesis of Kalman’s algorithm of functioning of an adaptive antenna array[C]//Proceedings of SPIE-The International Society for Optical Engineering，2008.

[10]QIU Xinyun，GAO Yuan.Adaptive PID controllerbased on single neuron[J].Applied Mechanics and Materials，2012，466-467：981-985. ■