基于運動學(xué)建模的自動瞄準(zhǔn)系統(tǒng)

宋珣，馬智博

（西安交通大學(xué)自動化系，西安710049）

RoboMaster全國機器人大賽是機器人競技類比賽，在比賽中雙方機器人需要用彈丸精準(zhǔn)打擊對方機器人的特定部位。近年來諸多參賽隊伍意識到給機器人裝配自動瞄準(zhǔn)系統(tǒng)的重要性，同時每個隊伍也都做了一定的反自動瞄準(zhǔn)措施。多數(shù)隊伍采用的傳統(tǒng)自瞄系統(tǒng)在面對反自瞄措施時都會失靈，因此針對反自瞄措施的技術(shù)原理，提出基于運動學(xué)建模的自動瞄準(zhǔn)系統(tǒng)，能夠有效地彌補傳統(tǒng)自瞄系統(tǒng)的缺陷。

自動瞄準(zhǔn)；反自瞄；運動學(xué)

0 引言

在RoboMaster機器人比賽中，機器人的前后左右方向各有一塊裝甲板。機器人需要用彈丸精準(zhǔn)打擊敵方機器人的裝甲板。打擊的精確度是決定比賽的勝負走向的關(guān)鍵因素，所以近年來幾乎所有隊伍都開始給機器人配備自動瞄準(zhǔn)系統(tǒng)，用以提升命中率。

大多數(shù)隊伍的自動瞄準(zhǔn)系統(tǒng)其原理和流程是相似的，大體都可以分為目標(biāo)檢測和運動預(yù)測兩步。反自瞄系統(tǒng)基本是針對后者研制的，即利用自瞄系統(tǒng)中運動預(yù)測功能的缺陷，使得敵方機器人的自瞄系統(tǒng)在特定情況下失靈。本文將介紹一種在傳統(tǒng)運動預(yù)測功能上有較大改進的自動瞄準(zhǔn)系統(tǒng)，其綜合效果良好，我們的參賽隊伍在2019年RoboMaster全國機器人競賽中獲得一等獎。

1 基礎(chǔ)模型

1.1 目標(biāo)檢測

目標(biāo)檢測是通過對攝像頭捕獲的圖像進行處理，從而得到視野中敵方機器人暴露出的裝甲板的空間位置。這是自動瞄準(zhǔn)的首要步驟。比賽中的步兵機器人和其裝甲板如圖1所示?？梢钥吹窖b甲板有一些明顯的視覺特征，于是先通過顏色和亮度篩序出光條（圖中數(shù)字5兩側(cè)的亮藍色部分即為光條），再通過他們之間的幾何關(guān)系匹配出可能是裝甲板的組合，最后將可能的組合通過神經(jīng)網(wǎng)絡(luò)對其中間部分的區(qū)域進行判斷，判斷其是裝甲板還是干擾組合，如果是裝甲板則給出其數(shù)字，如圖1中，裝甲板數(shù)字為5。

圖1 步兵機器人

判斷出燈條組合確實為裝甲板后，因為裝甲板的真實尺寸是已知的，攝像頭的內(nèi)參矩陣也是已知的，所以能夠結(jié)合裝甲板在圖像中的位置計算出其相對于相機的空間坐標(biāo)。

1.2 坐標(biāo)系建立

通過目標(biāo)檢測，得到了視野中的裝甲板在相機坐標(biāo)系中的位置，然而機器人的相機是跟隨云臺一起轉(zhuǎn)動的，所以相機坐標(biāo)系本身的方向是不斷變化的。我們希望能夠在一個方向不變的坐標(biāo)系中研究目標(biāo)的運動，所以就利用陀螺儀計算出一個方向不變的“絕對”坐標(biāo)系，該坐標(biāo)系的原點在己方機器人中心，方向始終固定在陀螺儀上電時云臺的方向。

圖2 坐標(biāo)系轉(zhuǎn)換關(guān)系

綜上所述，在自動瞄準(zhǔn)系統(tǒng)中，一共維護三個坐標(biāo)系：目標(biāo)機器人坐標(biāo)系{XR,YR,ZR}，中心固連于己方機器人中心而方向不變的絕對坐標(biāo)系{XI,YI,ZI}和相機坐標(biāo)系{XC,YC,ZC}。其中{ }XI,YI,ZI和{XC,YC,ZC}的Z軸是同軸的，它們之間的關(guān)系如圖2所示。因此，它們之間可以通過旋轉(zhuǎn)一個角度θ來轉(zhuǎn)換，θ可由陀螺儀測得的角速度做積分直接得到。設(shè)目標(biāo)裝甲板在相機坐標(biāo)系中的位置為PC=(xC,yC,zC)，則它在絕對坐標(biāo)系中的位置PI=(xI,yI,zI)可以由式（1）得到：

其中：

運動預(yù)測需要的是裝甲板的絕對坐標(biāo)PI，所以通過1式進行轉(zhuǎn)換是目標(biāo)檢測的最后一步。需要的注意的是，這里的“絕對”不等同于真正意義上的慣性系。“絕對”只是代表其XI軸和YI軸的指向是不變的，但其坐標(biāo)軸原點卻是隨己方機器人中心一起運動的。

1.3 運動預(yù)測

當(dāng)?shù)玫搅四繕?biāo)在絕對坐標(biāo)系的位置后，最符合直覺的做法是：將云臺轉(zhuǎn)動至目標(biāo)方向，然后射擊彈丸。但攝像頭的讀圖延遲tr、工控機的圖像處理延遲tp和上位機到下位機的信息傳輸延遲tt會導(dǎo)致我們得到的目標(biāo)位置總是滯后于目標(biāo)每個時刻的真實位置。再加上子彈的飛行延遲tf，其結(jié)果是：當(dāng)目標(biāo)在運動時，子彈總會落在目標(biāo)在前幾個時刻的位置，命中率非常低。所以我們最好能夠得到目標(biāo)的運動規(guī)律，將各種延遲考慮之后計算出在保持原有運動規(guī)律的情況下幾個時刻之后目標(biāo)所處的位置，然后以該位置作為擊打?qū)ο蟆?/p>

要得到目標(biāo)的運動規(guī)律，就要用多個時刻的目標(biāo)位置Pi(i=1,2…n)進行計算。例如，對于同一塊裝甲板，我們得到它在k個連續(xù)時刻的絕對位置P1,P2…Pk。假設(shè)目標(biāo)進行的是勻速直線運動且k取8，則可以利用式3計算出目標(biāo)速度，其中tmn代表m時刻到n時刻經(jīng)歷的時間之和。

以上幾種延遲之和?t可由式4得到：則預(yù)測出的實際應(yīng)該擊打的位置Ppre是：

這種傳統(tǒng)的運動預(yù)測方式的核心是假設(shè)目標(biāo)做勻速直線運動，由于工控機有著較高的圖像處理速度，處理效率可達70-80Hz，所以在幾個時刻的時間段內(nèi)實際只經(jīng)過了不到100毫秒，在如此短的時間內(nèi)假設(shè)目標(biāo)做勻速直線運動確實是可行的，但面對后面提到的反自瞄措施時，此種建模方式會呈現(xiàn)出很大的弊端。

2 模型改進

2.1 反自瞄措施

大多數(shù)隊伍采用的自動瞄準(zhǔn)方式都如同上一章所述，且其中有相當(dāng)部分的隊伍連基礎(chǔ)的運動預(yù)測都沒有做。所以針對自動瞄準(zhǔn)系統(tǒng)的固有弊端，即不可消除的延遲，幾乎所有戰(zhàn)隊都給機器人設(shè)置了自旋模式，這就是反自瞄措施。

在開啟自旋模式之后，機器人以每秒2到4圈的速度自轉(zhuǎn)，且由于采用了麥克納姆輪，在自旋的同時還可以進行水平移動。這樣做可以使得機器人四周的四塊裝甲板輪流出現(xiàn)在敵人的視野中，且每塊裝甲板出現(xiàn)的時間很短，通常只有不到150毫秒。在自旋時裝甲板的水平坐標(biāo)XI實際上是一個正弦函數(shù)，在這種情況下，如果使用上一章的運動預(yù)測模型，則預(yù)測的打擊位置大概率會落在敵方機器人外部，預(yù)測位置與真實位置的誤差關(guān)系如圖3所示。

圖3 預(yù)測位置誤差

在圖3中，我們選取t1作為計算目標(biāo)速度的起始時刻，t2作為終止時刻，它們分別對應(yīng)上一章的t1和tk，以此結(jié)合式（3）計算出目標(biāo)的運動速度，如圖中的直線所示，直線斜率即為在XI軸方向的運動速度。在t2位置的基礎(chǔ)上，用預(yù)測的速度延后一個時間?t，得到預(yù)測位置Xpre，然而實際的位置是Xtrue。它們之間的誤差為Error=Xpre-Xtrue。在如此大的誤差下基本是不可能打中的。

通過機器人的簡化自旋示意圖可以直觀理解打不中的原因。如圖4所示，圖中ω代表旋轉(zhuǎn)角速度，由中心處引出的兩個箭頭分別代表機器人在t1和t2兩個時刻的正前方，本例中我們跟蹤的是機器人右側(cè)的裝甲板。選取好t1和t2之后可以利用（3）式計算出裝甲板速度Vpre。

圖4 簡化自旋示意圖

在t2位置基礎(chǔ)上，以速度Vpre延遲?t的時間得到預(yù)測位置Ppre，然而經(jīng)過時間?t之后跟蹤的裝甲板已經(jīng)轉(zhuǎn)到了我們看不到的側(cè)面，所以這時已經(jīng)不可能打中了，如圖5所示。

圖5 預(yù)測位置示意圖

2.2 運動學(xué)建模

由上一節(jié)分析可知，在敵方機器人開啟自旋之后，如果我們想打中正在觀察的裝甲板，是很難的。這主要是由于當(dāng)我們做出預(yù)測反應(yīng)之后，被觀察的裝甲板已經(jīng)消失在視野中了，所以這時無論用何種方式優(yōu)化單一裝甲板運動規(guī)律的求解方式都是徒勞的。

所以我們應(yīng)當(dāng)將注意力放在其他的裝甲板上面，也就是說，雖然前幾個時刻被觀察的裝甲板已經(jīng)轉(zhuǎn)到了我們無法射擊到的視野盲區(qū)，但機器人的四周都有裝甲板，所以會有下一塊裝甲板出現(xiàn)在視野中。如果我們知道了敵方機器人的幾何參數(shù)和整體運動規(guī)律，則可以通過當(dāng)前正在觀察的裝甲板來預(yù)測下一塊裝甲板的運動規(guī)律，轉(zhuǎn)而將攻擊對象變?yōu)橄乱粔K裝甲板，這樣就有充足的時間準(zhǔn)備射擊。從而可以有效提升命中率。

在確定模型參數(shù)之前，為便于分析，需要先確定一些假設(shè)條件。首先是假設(shè)目標(biāo)機器人在自旋時會進行水平移動，且它相對于己方機器人的移動是勻速直線運動。其次假設(shè)目標(biāo)機器人的自轉(zhuǎn)速度是恒定的。最后假設(shè)目標(biāo)機器人的4塊裝甲板的中心經(jīng)過機器人的兩條對稱軸。通過我們進行的實驗和對大多數(shù)隊伍機器人的觀察可以得出，這3條假設(shè)是合理的。

對于目標(biāo)機器人，幾乎所有參數(shù)都是未知的，所以需要列出所有我們感興趣的參數(shù)，然后用觀察到的信息進行建模和求解。需要的參數(shù)如表1所示。

表1 符號含義

運動學(xué)建模后機器人的表示如圖6所示。

圖6 運動學(xué)建模

若在某一起始時刻t1觀察到目標(biāo)裝甲板位置為(XO1,YO1)，則可以寫出式（6）的一組約束條件。

從起始時刻經(jīng)過一個時間?t之后，到達時刻i。則會得到式（7）的一組約束條件。

當(dāng)然，式（6）可以看成式（7）的特殊情況，即?t=0的情況?？梢钥吹椒匠探M的未知參數(shù)有：XC,YC,L1,θR,θV,V,ω。每一次觀察會產(chǎn)生一組（2個）方程，其中XOi,YOi,?t是已知的。所以理論上可以通過至少3次觀察得到的6個方程解出6個未知參數(shù)。

不過在（7）式中，有非線性部分sin(θR+ω·?t)和cos(θR+ω·?t)，如果存在這樣的非線性部分，會給求解帶來很大麻煩。所以，使用一階泰勒展開將其化為線性形式。一階泰勒展開公式如式（8）所示?？梢允褂锰├照归_進行化簡是因為我們是在一個很小的時間間隔內(nèi)求解運動學(xué)參數(shù)，所以?t也是較小的值。

忽 略 無 窮 小 項o( x-x0)，得sin( θR+ω·?t)和cos( θR+ω·?t)在θR處的一階泰勒展開為式（9）。

所以式（7）被線性化后的表示如式（10）所示。

將式（10）寫成矩陣形式是：

若k=4，則可以直接通過T1=K-1U1和T2=K-1U2求解。

若k≥4，則式（12）是一個超定方程?？梢杂脗文媲蠼狻＜矗?/p>

當(dāng)解出T1和T2之后，就可以很容易的解出XC,YC,L1,θR,θV,V,ω這6個未知數(shù)。同樣，當(dāng)下一塊裝甲板出現(xiàn)在視野中后，就可以計算出L2的值。如此，我們得到了目標(biāo)機器人必要的幾何參數(shù)和它的運動學(xué)參數(shù)。這樣，通過當(dāng)前正在觀察的裝甲板的位置，可以推知該機器人剩余三塊裝甲板的位置和它們在將來一小段時間內(nèi)的運動軌跡。于是，可以預(yù)測還未出現(xiàn)在視野中的裝甲板在將來幾個時刻的位置并對其進行擊打。而不是像上一章的運動預(yù)測系統(tǒng)——預(yù)測和射擊僅局限于被觀察的一塊裝甲板。在目標(biāo)機器人自旋時，一塊裝甲板被觀察并計算出運動規(guī)律之后，很快就會消失在視野中，所以傳統(tǒng)運動預(yù)測系統(tǒng)面對自旋時效果極差。而引入運動學(xué)建模的運動預(yù)測系統(tǒng)將裝甲板的觀測與射擊分離，就較好地解決了該問題。

2.3 卡爾曼濾波

上述運動預(yù)測過程是在下位機完成的，下位機使用k組觀測進行計算就需要從上位機接收k次數(shù)據(jù)。受攝像頭幀率的限制，上位機產(chǎn)生兩組有效數(shù)據(jù)的時間間隔大約是14毫秒，如果中間目標(biāo)檢測環(huán)節(jié)受到干擾，數(shù)據(jù)間斷的時間還會更久，即數(shù)據(jù)更新頻率在70Hz以下。然而如果下位機對云臺的控制頻率也保持在70Hz上下，會產(chǎn)生較大的延遲和抖動。所以下位機需要在兩組有效數(shù)據(jù)的時間間隔內(nèi)通過已有數(shù)據(jù)自行生成控制指令，這就需要通過目標(biāo)機器人幾何參數(shù)和運動規(guī)律生成狀態(tài)轉(zhuǎn)移方程，假設(shè)通過最后一次有效數(shù)據(jù)計算出的參數(shù)為XC,YC,L1,θR,θV,V,ω，則在基礎(chǔ)上經(jīng)過?t的時間，目標(biāo)右裝甲板位置(Xrnew,Yrnew)的計算如式（14）所示。目標(biāo)后裝甲板位置(Xbnew,Ybnew)的計算如式（15）所示。

當(dāng)下一幀有效數(shù)據(jù)到來時，會產(chǎn)生一個觀測。如果是右側(cè)裝甲板的位置觀測(XrO,YrO)，則通過卡爾曼濾波將觀測狀態(tài)PrO=(XrO,YrO)與預(yù)測狀態(tài)Prnew=(Xrnew,Yrnew)進行融合，如式（16）所示。

其中K=F(F+R)-1，為卡爾曼增益。F為狀態(tài)Prnew的協(xié)方差矩陣，R為觀測噪聲的協(xié)方差矩陣，在實踐中可以通過距離調(diào)整R的值，因為一般來說距離越遠其觀測誤差越大。使用卡爾曼濾波進行狀態(tài)更新可以使得控制信號更加連貫，也使得位置數(shù)據(jù)更新更加平滑。

3 結(jié)語

本文給出了基于運動學(xué)建模的自動瞄準(zhǔn)系統(tǒng)。相較于傳統(tǒng)自瞄算法，該算法對觀測數(shù)據(jù)做到了更好的利用。通過對目標(biāo)車進行運動學(xué)建模，預(yù)測和射擊不用局限于同一塊裝甲板，從而大幅提升了實際效果。最后通過卡爾曼濾波使得控制更加平滑。該自瞄系統(tǒng)在比賽中表現(xiàn)良好，具有一定的實際意義。