張香竹，余輝榮，汪俊彬

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

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

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

1 實驗平臺

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

圖1 實驗平臺結構圖Fig.1 Configuration of experimental platform

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

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

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

2 具體實現(xiàn)

2.1 車式移動機器人建模

運用Kalman濾波器的前提是已知被控對象的線性化模型。本文所采用的被控對象是四輪車式移動機器人，其中兩個后輪依靠直流電機提供前進動力，即為驅動輪，驅動方向與機器人前進方向一致；兩個前輪通過舵機控制機器人的轉向，即為轉向輪。車式機器人的模型如圖3所示。

圖3 車式移動機器人模型圖Fig.3 Model of car-type mobile robot

根據(jù)四輪移動機器人的運動學模型

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

取狀態(tài)變量 x1=xw，x2=yw，x3=θ~，其中θ~=θ-θ0，代表當前時刻車身方向夾角相對上一時刻夾角θ0（即當前時刻的基準參考量）的微小增量；取控制變量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濾波器

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

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

第1步：預估：

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

（2）計算當前預估值對應的系統(tǒng)協(xié)方差：

第2步：校正：

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

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

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

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

3 實驗結果與分析

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

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

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

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

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

4 結語

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

[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. ■