基于卡爾曼濾波的目標識別跟蹤與射擊系統(tǒng)設計

王洪璽,計澤賢,張?zhí)m勇

(1.哈爾濱工程大學 智能科學與工程學院, 哈爾濱 150001； 2.哈爾濱工程大學 機電工程學院, 哈爾濱 150001)

1 引言

在當今軍事領域和科技領域中，機動目標的識別、跟蹤具有廣泛應用。設計目標跟蹤系統(tǒng)主要分為建立目標的運動狀態(tài)模型和設計估計器進行濾波估計[1-2]。一般常用卡爾曼濾波器等最優(yōu)估計算法，文獻[3]中利用EKF對地理坐標系下靜止目標具有較好的定位效果，文獻[4]中利用勻速(CV)模型建立EKF實現(xiàn)對運動目標的跟蹤。勻速模型，假設目標勻速直線運動，將加速度建模為隨機過程噪聲，結構簡單、計算量小。文獻[5]利用自適應高階容積卡爾曼濾波器與勻速轉彎模型對“S”型平面飛行目標具有相比于3階容積卡爾曼濾波器更高的跟蹤精度與更快的收斂速度。本文系統(tǒng)跟蹤的目標通常機動較弱，采用勻速模型構建卡爾曼濾波器，可以極小的運算量取得較好的跟蹤效果。

目標跟蹤過程中坐標變換矩陣需通過慣導姿態(tài)解算獲得。文獻[6]中通過4元數(shù)微分方程更新姿態(tài)，并利用加速度計測量值修正姿態(tài)4元數(shù)。但加速度測量值中的運動加速度會影響修正準確性，針對此問題，本文云臺姿態(tài)估計算法融合陀螺儀與加速度計信息估計重力加速度，并利用估計得到的重力加速度通過非線性約束更新對4元數(shù)進行修正，可較大程度減小運動加速度對姿態(tài)解算的影響，同時對加速度量測噪聲有較好的抑制作用。

2 系統(tǒng)結構

RoboMaster機甲大師賽要求雙方操作手操控機器人通過云臺瞄準，并使用摩擦輪發(fā)射彈丸打擊對方機器人裝甲板，如圖1所示。因此，高性能的目標識別與跟蹤系統(tǒng)在比賽中尤為重要。文獻[7]中自瞄系統(tǒng)具有遠優(yōu)于操作手人為瞄準的性能，并在比賽中有出色表現(xiàn)。本文以RoboMaster機甲大師賽為研究背景，設計出基于最優(yōu)估計理論的目標識別跟蹤與射擊系統(tǒng)。

圖1 機器人發(fā)射 17 mm 彈丸擊打裝甲板場景圖

本文系統(tǒng)通過與云臺固聯(lián)的相機采集畫面，機載 miniPC 根據(jù)目標外觀特征利用 OpenCV 識別目標裝甲板，并通過Perspective-n-Point (PnP)解算出目標在相機坐標系中的位置。相機選用分辨率640*360、幀率330FPS的USB2.0相機，miniPC處理器為酷睿i7-8550U。

miniPC解算出目標裝甲板相機系坐標后，通過串口發(fā)送給下位機，下位機以STM32F407IGH6為核心處理器、BMI088為云臺IMU。根據(jù)相機與云臺IMU的相對位置與姿態(tài)可得到目標在云臺坐標系中的位置后，通過云臺姿態(tài)估計確定的坐標變換矩陣得到目標的慣性系位置。得到目標慣性系位置后，利用勻速模型卡爾曼濾波器估計目標在慣性系的運動狀態(tài)，即位置與速度。云臺姿態(tài)估計與目標運動狀態(tài)估計中卡爾曼濾波器均利用CMSIS DSP矩陣運算庫實現(xiàn)。

最后根據(jù)目標運動狀態(tài)與彈道模型實現(xiàn)運動預測與彈道補償，進而得到云臺的期望姿態(tài)角。在串級控制器基礎上設計的云臺控制系統(tǒng)實現(xiàn)對目標的跟蹤與打擊。云臺電機采用GM6020直流無刷電機，下位機通過CAN總線向電機驅動器發(fā)送控制指令控制其輸出電壓。系統(tǒng)結構如圖2所示。

圖2 系統(tǒng)結構框圖

3 目標識別與位置解算

3.1 目標外觀特征

裝甲板具有明顯外觀特征，矩形裝甲板邊緣具有2條平行發(fā)光燈條，其會根據(jù)隊伍紅藍方發(fā)出紅色或藍色亮光,如圖3所示。

圖3 目標外觀特征圖

3.2 目標識別過程

首先利用OpenCV中split函數(shù)提取對應顏色的色彩通道并根據(jù)灰度值尋找燈條。為避免白光干擾，使目標燈條顏色通道的灰度圖減去綠色通道灰度圖，得到剔除白光的灰度圖。后對灰度圖進行二值化，利用OpenCV中findContours函數(shù)在二值圖中尋找輪廓，再通過最小外接矩形擬合輪廓點集得到燈條輪廓旋轉矩形框。為避免在復雜背景環(huán)境下可能存在的誤識別，通過擬合得到的旋轉矩形長寬比、傾斜角與輪廓面積比等幾何約束篩選掉畫面當中非燈條的發(fā)光物體，識別處理過程如圖4所示。

圖4 識別處理過程圖

3.3 燈條匹配與PnP位置解算

得到燈條后，通過裝甲板幾何特征對燈條兩兩匹配，以得到裝甲板角點。設每次取出的2個燈條矩形的4個角點分別為{p00，p01，p02，p03}，p10，p11，p12，p13，順序為左下、左上、右上、右下，矩形中心分別為p0c、p1c。令：

組成裝甲板的一對燈條，應滿足條件：

(1)

式(1)中：C0=0.994 5，描述2燈條角度差應小于6°；C1=0.35，描述2燈條連接端點后應接近矩形而非平行四邊形；C2=1.25，C3=5描述2燈條間距應在一定范圍內；C4=1.5，描述2燈條長度比應在一定范圍內。

燈條匹配完成后延長燈條即可得到裝甲板4個角點，如圖5所示。

圖5 裝甲板角點圖

根據(jù)裝甲板4個角點在像平面坐標與裝甲板實際尺寸，與預先標定得到的相機內參矩陣與畸變參數(shù)，通過PnP算法即可解算出裝甲板中心在相機系的坐標rc=[xc,yc,zc]T。

4 云臺姿態(tài)估計

云臺姿態(tài)估計用于求解云臺系到慣性系的坐標變換矩陣與姿態(tài)歐拉角，準確的姿態(tài)信息對目標運動估計和云臺運動控制至關重要。

4.1 坐標系定義

1) 云臺坐標系(b系)。云臺坐標系三軸與載體固聯(lián)，三軸分別與云臺IMU三軸平行，記為OXbYbZb。

2) 相機坐標系(c系)。相機坐標系與云臺坐標系相對靜止，其變換關系由相機安裝位置與姿態(tài)決定，記為OXcYcZc。

3) 慣性坐標系(n系)。慣性坐標系各軸相對慣性空間的指向保持不變，記為OXnYnZn。

4.2 姿態(tài)描述

姿態(tài)描述的是云臺坐標系(b系)相對慣性坐標系(n系)的旋轉關系。常見的描述方法有3種，每種各有其優(yōu)缺點，本節(jié)將給出歐拉角、4元數(shù)2種姿態(tài)描述方法。

4.2.1姿態(tài)角

姿態(tài)角是一種常用且直觀的姿態(tài)描述方法，幾何意義明確，因此在姿態(tài)控制中被廣泛應用。機體姿態(tài)即b系是由n系經(jīng)過轉動順序為z-x-y，轉角分別為ψ、θ、γ的3次旋轉得到，其中ψ、θ、γ分別為航向角(Yaw)、俯仰角(Pitch)、橫滾角(Roll)。

4.2.24元數(shù)

4元數(shù)可定義為：

q=q0+q1i+q2j+q3k(q0,q1,q2,q3∈R)

(2)

式(2)中，i2=j2=k2=ijk=-1。4元數(shù)可以看作基{1,i,j,k} 的線性組合，因此，4元數(shù)也可以寫成向量形式，即：

4元數(shù)關于時間的微分方程為：

(3)

式(3)中：

(4)

式(4)中，ωx、ωy、ωz為b系相對n系的角速度。

4.3 過程模型

4.3.14元數(shù)狀態(tài)空間方程

4.3.2重力加速度狀態(tài)空間方程

機體坐標系(b系) 下的重力加速度單位向量為：

對時間t求導，得：

(5)

式(5)中，矩陣ω×為b系相對n系的角速度ω=[ωx,ωy,ωz]T構成的反對稱陣：

(6)

式(6)中：

(7)

式(7)中， Δθk為k周期的陀螺儀輸出角增量，即Δθk=ωkΔt。

4.3.3系統(tǒng)狀態(tài)與過程模型

綜上，定義狀態(tài)向量為：

(8)

式(8)中：q=[q0,q1,q2,q3]T為機體坐標系 (b系) 相對慣性坐標系(n系)的姿態(tài)四元數(shù)；g=[gx,gy,gz]T為重力加速度在機體坐標系下的單位向量，即滿足 |g|=1。

過程模型為：

xk+1=Fkxk+wk,wk～N(07×1,Q)

(9)

式(9)中：

4.4 量測模型

量測模型為：

zk=Hkxk+vk,vk～N(03×1,Rk)

定義誤差ek：

1) 當|ek|

2) 當a1<|ek|

4.5 狀態(tài)約束卡爾曼濾波器

傳統(tǒng)卡爾曼濾波包含以下5個基本公式。

1) 過程更新：

2) 先驗估計誤差協(xié)方差更新：

3) 計算卡爾曼增益：

4) 量測更新：

5) 后驗估計誤差協(xié)方差更新：

有些系統(tǒng)除了過程模型描述的系統(tǒng)動態(tài)以外，還包含各種各樣的約束。這些約束運用得當可以有助于提高濾波精度。其中等式約束可以類似量測更新的形式進行約束更新[8]。設系統(tǒng)存在等值約束為：

Dx=b

(10)

式(10)中，約束更新增益：

若等值約束存在不確定性，則：

(11)

式(11)中，Rc為等值約束中不確定性的方差陣。經(jīng)過約束更新后，狀態(tài)誤差協(xié)方差矩陣為：

對于非線性狀態(tài)約束：

g(x)=b

有：

4.6 約束模型

姿態(tài)更新過程中需通過估計得到的重力加速度向量對姿態(tài)4元數(shù)進行修正。另外，在更新過程中需保持4元數(shù)的單位性質。它們均可看作系統(tǒng)的約束，即可以通過約束更新實現(xiàn)4元數(shù)姿態(tài)修正與歸一化。

4.6.1重力加速度向量約束模型

(12)

有：

(13)

式(13)中，矩陣M為：

4.6.2單位4元數(shù)約束模型

根據(jù)單位4元數(shù)性質，存在約束：

(14)

有：

綜上，云臺姿態(tài)估計的狀態(tài)約束卡爾曼濾波器實現(xiàn)更新流程如圖6所示。

圖6 云臺姿態(tài)估計算法更新流程框圖Fig.6 Flowchart for updating the gimbal attitude estimation algorithm

5 目標運動預測與彈道補償

云臺發(fā)射的17 mm彈丸初速度約為15 m/s，飛行時間無法忽略不計。因此在云臺跟蹤目標的過程中需要對目標運動進行預測，從而確定云臺期望姿態(tài)角。為保證運動預測合理性，選取慣性坐標系(n系)對目標運動狀態(tài)進行估計與預測。利用勻速直線模型設計卡爾曼濾波器，以估計目標在慣性系的位置與速度，并通過卡方檢驗判斷目標是否發(fā)生切換。

5.1 目標運動狀態(tài)估計

5.1.1坐標變換

設由miniPC解算出目標在相機坐標系(c系) 的坐標為：

根據(jù)相機的安裝位置，目標在云臺坐標系(b系) 的坐標可由rc經(jīng)過旋轉平移后得到：

(15)

5.1.2運動模型

采用勻速模型描述目標在慣性系的運動,即：

(16)

式(16)中，w(t)～N(0,σ2)。其離散時間形式為：

5.1.3過程模型

利用勻速模型設計卡爾曼濾波器，以估計目標在慣性系的位置與速度，設狀態(tài)為：

過程模型為：

xk+1=Fkxk+Γkwk,wk～N(03×1,Qk)

其中：

過程噪聲方差陣Qk為：

(17)

5.1.4量測模型

量測模型如下：

zk=Hkxk+vk

采用目標慣性系坐標rn=[xn,yn,zn]T作為量測向量，則：

相機模型如圖7所示。

圖7 相機模型示意圖Fig.7 Camera model

定義目標相機系坐標rc與yOz，xOz平面夾角分別為：α=actan(xc/zc),β=actan(yc/zc)，有：

其中：

則k時刻濾波器量測噪聲方差陣為：

miniPC中目標識別與解算的頻率約為300 Hz，而單片機中卡爾曼濾波器更新頻率為1 kHz。因此并非每個卡爾曼濾波器更新周期中miniPC都能解算出的坐標，故僅在miniPC解算出坐標的周期進行量測更新，否則只進行過程更新，即異步量測[9]。

5.1.5卡方檢驗

若跟蹤過程中miniPC識別的目標發(fā)生了切換，卡爾曼濾波器得到的位置量測會與當前位置估計有顯著差異，直接進行量測更新會得到一個極大的速度估計值。為解決該問題，本文系統(tǒng)通過卡方檢驗判斷跟蹤目標是否發(fā)生變化。定義殘差：

正常情況下，殘差ek符合期望為0的正態(tài)分布，其方差為：

當發(fā)生目標切換時，殘差的值會顯著大于正常情況，定義檢測函數(shù)為：

若rk大于閾值，說明位置量測與位置先驗估計值有較大差別，即發(fā)生了目標切換。發(fā)生目標切換后,應對狀態(tài)及其協(xié)方差矩陣進行復位以迅速重新收斂,即：

Pk=P0

其中：

綜上，運動狀態(tài)估計中卡爾曼濾波器更新流程如圖8所示。

圖8 目標運動估計流程框圖

5.2 彈道模型與彈道補償

得到目標慣性系下的位置與速度估計后，通過彈道模型計算彈丸飛行時間，從而確定運動預測步長與彈道下墜補償。

5.2.1彈道模型

根據(jù)初始條件vx(0)=vx|0解得：

(18)

式(18)中，k1=k/m。水平速度vx(t) 對時間t積分得水平位移x(t)，有：

利用牛頓迭代求解飛行時間T，定義函數(shù)為：

有：

迭代公式為：

綜上，云臺應瞄準經(jīng)過T+τs后目標位置，其中τ為相機圖像采集與傳輸延時、圖像處理與通信延時、控制器跟隨延時之和，即期望航向角ψ為：

5.2.2彈道補償

由于彈丸飛行過程中會受重力作用發(fā)生下墜，因此,為使彈丸擊中目標云臺,應上抬相應角度。簡單起見，本文系統(tǒng)采用比例補償器進行迭代求解。根據(jù)彈丸飛行時間T、彈丸初速度v0、迭代初始俯仰角θ0可求得彈丸落點高度h0為：

ek=hr-hk

(19)

式(19)中，hk為第k次迭代落點高度。

彈道補償過程如圖9所示。

圖9 彈道補償?shù)鞒炭驁D

6 云臺控制系統(tǒng)設計

用于打擊目標的17 mm彈丸由固聯(lián)于云臺的摩擦輪進行發(fā)射，因此設計合理的控制系統(tǒng)，使云臺具有足夠好的響應速度與精度是高命中率的先決條件。

6.1 云臺模型

本系統(tǒng)通過電壓驅動云臺電機，根據(jù)電機模型可建立關于電壓u、電流i、轉速ωa、ωe的微分方程，即：

(20)

式(20)中：Kt為轉矩常數(shù)；Ke為反電動勢常數(shù)；J為負載轉動慣量；b為阻尼系數(shù)；R為電阻；L為電感；ωe為底盤相對慣性系角速度，為電機角速度；ωa為云臺相對慣性系角速度。拉普拉斯變換可得：

KtI(s)=JsΩa(s)+bΩr(s)

U(s)=KeΩr(s)+RI(s)+LsI(s)

Ωa(s)=Ωr(s)+Ωe(s)

進一步計算可得：

(21)

根據(jù)式(21)得到其系統(tǒng)框圖，如圖10所示。

圖10 電機模型框圖

6.2 控制器設計

根據(jù)該模型，設計串級反饋控制器，如圖11所示。

圖11 串級控制器框圖

圖11中，CΘ(s)、CΩ(s)、CI(s)分別為角度環(huán)PD控制器、速度環(huán)PI控制器、電流環(huán)PI控制器。圖11中云臺絕對角速度Ωa(s)為：

為減小底盤運動Ωe(s)對云臺姿態(tài)控制的影響，設計補償器，使得：

KtI(s)+bΩe(s)=KtCcl(s)X(s)

(22)

式(22)中：Ccl(s)為電流環(huán)閉環(huán)傳遞函數(shù)；X(s)為速度環(huán)PI控制器輸出。解得電流I(s)為：

應用該補償器，控制系統(tǒng)如圖12所示。

圖12 云臺控制系統(tǒng)框圖

圖12中，底盤運動Ωe(s)由安裝在機器人底盤的IMU直接測得，補償后云臺絕對角速度Ωa(s)為：

(23)

由于電流環(huán)閉環(huán)系統(tǒng)帶寬遠高于速度環(huán)，低頻段有Ccl(s)=1，故式(23)可近似為：

(24)

式(24)中，bn=b,Kn=Kt，由參數(shù)辨識確定。理想情況下b-Kt/Knbn=0，即該補償器可完全補償?shù)妆P運動對云臺絕對角速度的影響。

7 試驗

7.1 勻速機動

勻速機動試驗使目標機器人以勻速直線運動通過，擊打效果如圖13所示。圖13中藍方機器人勻速通過紅方機器人擊打區(qū)域，電腦屏幕實時顯示紅方機器人估計的目標裝甲板位置及其預測值。

圖13 擊打勻速目標效果圖

目標慣性系坐標與預測結果如圖14所示，圖14中實線為目標慣性系坐標，虛線為慣性系位置預測結果。機器人云臺到目標的航向角及其預測結果如圖15所示，圖15中實線為實際位置航向角，虛線為預測結果，即云臺Yaw期望角度。

圖15 勻速機動云臺到目標航向角與預測結果曲線

預測結果與實際彈丸彈道均穩(wěn)定超前于實際目標裝甲板位置，統(tǒng)計命中率21中20，接近100%。進行多次實驗統(tǒng)計命中率如表1所示，綜合命中率為95.37%。

表1 勻速機動命中率統(tǒng)計

7.2 綜合機動

綜合機動中，手持2塊裝甲板，在機器人擊打區(qū)域內做無規(guī)則機動，同時伴隨2塊裝甲板的跟蹤切換，以評估卡方檢驗有效性與切換目標后濾波器收斂速度，并統(tǒng)計命中率。目標慣性系坐標與預測結果如圖16所示，圖16中實線為目標慣性系坐標，虛線為慣性系位置預測結果。機器人云臺到目標的航向角及其預測結果如圖17所示，圖17中實線為實際位置航向角，虛線為預測結果，即云臺Yaw期望角度。

圖16 綜合機動目標慣性系坐標與預測結果曲線

圖17 綜合機動云臺到目標航向角與預測結果曲線

在時間t=0.67,2.6,4.0 s時發(fā)生了目標切換，濾波器迅速收斂，系統(tǒng)得到有效預測值。統(tǒng)計命中率53中37，為69.81%。進行多次實驗統(tǒng)計命中率如表2所示，綜合命中率為68.27%。

表2 綜合機動命中率

8 結論

1) 利用PnP方法,可準確解算出正對相機目標裝甲板的位置，為后續(xù)運動估計與預測提供了可靠的量測信息。

2) 利用狀態(tài)約束卡爾曼濾波器估計云臺姿態(tài),可避免機器人急起急停時的運動加速度破壞姿態(tài)估計精度，同時對云臺摩擦輪轉動帶來的振動噪聲有較好的抑制作用。

3) 得益于300 Hz的識別頻率，運動估計與預測具有較高的收斂速度，即使采用勻速模型,仍能對做變速機動的目標有較好的跟蹤效果。

4) 在參數(shù)辨識準確情況下云臺控制系統(tǒng)不易受底盤運動的干擾，同時具有良好的響應速度與精度。

5) 本系統(tǒng)打擊效果顯著優(yōu)于操作手人為瞄準，在RoboMaster全國機器人大賽中有出色表現(xiàn)，應用本系統(tǒng)的哈爾濱工程大學創(chuàng)夢之翼戰(zhàn)隊2臺步兵機器人分別在2021賽季北部賽區(qū)機器人實戰(zhàn)獎中獲得前2名。