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

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

1 引言

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

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

2 系統(tǒng)結(jié)構(gòu)

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

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

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

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

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

圖2 系統(tǒng)結(jié)構(gòu)框圖

3 目標(biāo)識(shí)別與位置解算

3.1 目標(biāo)外觀特征

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

圖3 目標(biāo)外觀特征圖

3.2 目標(biāo)識(shí)別過程

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

圖4 識(shí)別處理過程圖

3.3 燈條匹配與PnP位置解算

得到燈條后，通過裝甲板幾何特征對(duì)燈條兩兩匹配，以得到裝甲板角點(diǎn)。設(shè)每次取出的2個(gè)燈條矩形的4個(gè)角點(diǎn)分別為{p00，p01，p02，p03}，p10，p11，p12，p13，順序?yàn)樽笙隆⒆笊稀⒂疑?、右下，矩形中心分別為p0c、p1c。令：

組成裝甲板的一對(duì)燈條，應(yīng)滿足條件：

(1)

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

燈條匹配完成后延長(zhǎng)燈條即可得到裝甲板4個(gè)角點(diǎn)，如圖5所示。

圖5 裝甲板角點(diǎn)圖

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

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

云臺(tái)姿態(tài)估計(jì)用于求解云臺(tái)系到慣性系的坐標(biāo)變換矩陣與姿態(tài)歐拉角，準(zhǔn)確的姿態(tài)信息對(duì)目標(biāo)運(yùn)動(dòng)估計(jì)和云臺(tái)運(yùn)動(dòng)控制至關(guān)重要。

4.1 坐標(biāo)系定義

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

2) 相機(jī)坐標(biāo)系(c系)。相機(jī)坐標(biāo)系與云臺(tái)坐標(biāo)系相對(duì)靜止，其變換關(guān)系由相機(jī)安裝位置與姿態(tài)決定，記為OXcYcZc。

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

4.2 姿態(tài)描述

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

4.2.1姿態(tài)角

姿態(tài)角是一種常用且直觀的姿態(tài)描述方法，幾何意義明確，因此在姿態(tài)控制中被廣泛應(yīng)用。機(jī)體姿態(tài)即b系是由n系經(jīng)過轉(zhuǎn)動(dòng)順序?yàn)閦-x-y，轉(zhuǎn)角分別為ψ、θ、γ的3次旋轉(zhuǎn)得到，其中ψ、θ、γ分別為航向角(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ù)關(guān)于時(shí)間的微分方程為：

(3)

式(3)中：

(4)

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

4.3 過程模型

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

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

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

對(duì)時(shí)間t求導(dǎo)，得：

(5)

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

(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為機(jī)體坐標(biāo)系 (b系) 相對(duì)慣性坐標(biāo)系(n系)的姿態(tài)四元數(shù)；g=[gx,gy,gz]T為重力加速度在機(jī)體坐標(biāo)系下的單位向量，即滿足 |g|=1。

過程模型為：

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

(9)

式(9)中：

4.4 量測(cè)模型

量測(cè)模型為：

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

定義誤差ek：

1) 當(dāng)|ek|

2) 當(dāng)a1<|ek|

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

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

1) 過程更新：

2) 先驗(yàn)估計(jì)誤差協(xié)方差更新：

3) 計(jì)算卡爾曼增益：

4) 量測(cè)更新：

5) 后驗(yàn)估計(jì)誤差協(xié)方差更新：

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

Dx=b

(10)

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

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

(11)

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

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

g(x)=b

有：

4.6 約束模型

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

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

(12)

有：

(13)

式(13)中，矩陣M為：

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

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

(14)

有：

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

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

5 目標(biāo)運(yùn)動(dòng)預(yù)測(cè)與彈道補(bǔ)償

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

5.1 目標(biāo)運(yùn)動(dòng)狀態(tài)估計(jì)

5.1.1坐標(biāo)變換

設(shè)由miniPC解算出目標(biāo)在相機(jī)坐標(biāo)系(c系) 的坐標(biāo)為：

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

(15)

5.1.2運(yùn)動(dòng)模型

采用勻速模型描述目標(biāo)在慣性系的運(yùn)動(dòng),即：

(16)

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

5.1.3過程模型

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

過程模型為：

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

其中：

過程噪聲方差陣Qk為：

(17)

5.1.4量測(cè)模型

量測(cè)模型如下：

zk=Hkxk+vk

采用目標(biāo)慣性系坐標(biāo)rn=[xn,yn,zn]T作為量測(cè)向量，則：

相機(jī)模型如圖7所示。

圖7 相機(jī)模型示意圖Fig.7 Camera model

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

其中：

則k時(shí)刻濾波器量測(cè)噪聲方差陣為：

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

5.1.5卡方檢驗(yàn)

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

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

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

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

Pk=P0

其中：

綜上，運(yùn)動(dòng)狀態(tài)估計(jì)中卡爾曼濾波器更新流程如圖8所示。

圖8 目標(biāo)運(yùn)動(dòng)估計(jì)流程框圖

5.2 彈道模型與彈道補(bǔ)償

得到目標(biāo)慣性系下的位置與速度估計(jì)后，通過彈道模型計(jì)算彈丸飛行時(shí)間，從而確定運(yùn)動(dòng)預(yù)測(cè)步長(zhǎng)與彈道下墜補(bǔ)償。

5.2.1彈道模型

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

(18)

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

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

有：

迭代公式為：

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

5.2.2彈道補(bǔ)償

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

ek=hr-hk

(19)

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

彈道補(bǔ)償過程如圖9所示。

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

6 云臺(tái)控制系統(tǒng)設(shè)計(jì)

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

6.1 云臺(tái)模型

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

(20)

式(20)中：Kt為轉(zhuǎn)矩常數(shù)；Ke為反電動(dòng)勢(shì)常數(shù)；J為負(fù)載轉(zhuǎn)動(dòng)慣量；b為阻尼系數(shù)；R為電阻；L為電感；ωe為底盤相對(duì)慣性系角速度，為電機(jī)角速度；ωa為云臺(tái)相對(duì)慣性系角速度。拉普拉斯變換可得：

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

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

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

進(jìn)一步計(jì)算可得：

(21)

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

圖10 電機(jī)模型框圖

6.2 控制器設(shè)計(jì)

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

圖11 串級(jí)控制器框圖

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

為減小底盤運(yùn)動(dòng)Ωe(s)對(duì)云臺(tái)姿態(tài)控制的影響，設(shè)計(jì)補(bǔ)償器，使得：

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)為：

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

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

圖12中，底盤運(yùn)動(dòng)Ωe(s)由安裝在機(jī)器人底盤的IMU直接測(cè)得，補(bǔ)償后云臺(tái)絕對(duì)角速度Ωa(s)為：

(23)

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

(24)

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

7 試驗(yàn)

7.1 勻速機(jī)動(dòng)

勻速機(jī)動(dòng)試驗(yàn)使目標(biāo)機(jī)器人以勻速直線運(yùn)動(dòng)通過，擊打效果如圖13所示。圖13中藍(lán)方機(jī)器人勻速通過紅方機(jī)器人擊打區(qū)域，電腦屏幕實(shí)時(shí)顯示紅方機(jī)器人估計(jì)的目標(biāo)裝甲板位置及其預(yù)測(cè)值。

圖13 擊打勻速目標(biāo)效果圖

目標(biāo)慣性系坐標(biāo)與預(yù)測(cè)結(jié)果如圖14所示，圖14中實(shí)線為目標(biāo)慣性系坐標(biāo)，虛線為慣性系位置預(yù)測(cè)結(jié)果。機(jī)器人云臺(tái)到目標(biāo)的航向角及其預(yù)測(cè)結(jié)果如圖15所示，圖15中實(shí)線為實(shí)際位置航向角，虛線為預(yù)測(cè)結(jié)果，即云臺(tái)Yaw期望角度。

圖15 勻速機(jī)動(dòng)云臺(tái)到目標(biāo)航向角與預(yù)測(cè)結(jié)果曲線

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

表1 勻速機(jī)動(dòng)命中率統(tǒng)計(jì)

7.2 綜合機(jī)動(dòng)

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

圖16 綜合機(jī)動(dòng)目標(biāo)慣性系坐標(biāo)與預(yù)測(cè)結(jié)果曲線

圖17 綜合機(jī)動(dòng)云臺(tái)到目標(biāo)航向角與預(yù)測(cè)結(jié)果曲線

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

表2 綜合機(jī)動(dòng)命中率

8 結(jié)論

1) 利用PnP方法,可準(zhǔn)確解算出正對(duì)相機(jī)目標(biāo)裝甲板的位置，為后續(xù)運(yùn)動(dòng)估計(jì)與預(yù)測(cè)提供了可靠的量測(cè)信息。

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

3) 得益于300 Hz的識(shí)別頻率，運(yùn)動(dòng)估計(jì)與預(yù)測(cè)具有較高的收斂速度，即使采用勻速模型,仍能對(duì)做變速機(jī)動(dòng)的目標(biāo)有較好的跟蹤效果。

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

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