基于運動學建模的自動瞄準系統

宋珣，馬智博

（西安交通大學自動化系，西安710049）

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

自動瞄準；反自瞄；運動學

0 引言

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

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

1 基礎模型

1.1 目標檢測

目標檢測是通過對攝像頭捕獲的圖像進行處理，從而得到視野中敵方機器人暴露出的裝甲板的空間位置。這是自動瞄準的首要步驟。比賽中的步兵機器人和其裝甲板如圖1所示。可以看到裝甲板有一些明顯的視覺特征，于是先通過顏色和亮度篩序出光條（圖中數字5兩側的亮藍色部分即為光條），再通過他們之間的幾何關系匹配出可能是裝甲板的組合，最后將可能的組合通過神經網絡對其中間部分的區域進行判斷，判斷其是裝甲板還是干擾組合，如果是裝甲板則給出其數字，如圖1中，裝甲板數字為5。

圖1 步兵機器人

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

1.2 坐標系建立

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

圖2 坐標系轉換關系

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

其中：

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

1.3 運動預測

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

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

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

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

2 模型改進

2.1 反自瞄措施

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

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

圖3 預測位置誤差

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

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

圖4 簡化自旋示意圖

在t2位置基礎上，以速度Vpre延遲?t的時間得到預測位置Ppre，然而經過時間?t之后跟蹤的裝甲板已經轉到了我們看不到的側面，所以這時已經不可能打中了，如圖5所示。

圖5 預測位置示意圖

2.2 運動學建模

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

所以我們應當將注意力放在其他的裝甲板上面，也就是說，雖然前幾個時刻被觀察的裝甲板已經轉到了我們無法射擊到的視野盲區，但機器人的四周都有裝甲板，所以會有下一塊裝甲板出現在視野中。如果我們知道了敵方機器人的幾何參數和整體運動規律，則可以通過當前正在觀察的裝甲板來預測下一塊裝甲板的運動規律，轉而將攻擊對象變為下一塊裝甲板，這樣就有充足的時間準備射擊。從而可以有效提升命中率。

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

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

表1 符號含義

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

圖6 運動學建模

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

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

當然，式（6）可以看成式（7）的特殊情況，即?t=0的情況。可以看到方程組的未知參數有：XC,YC,L1,θR,θV,V,ω。每一次觀察會產生一組（2個）方程，其中XOi,YOi,?t是已知的。所以理論上可以通過至少3次觀察得到的6個方程解出6個未知參數。

不過在（7）式中，有非線性部分sin(θR+ω·?t)和cos(θR+ω·?t)，如果存在這樣的非線性部分，會給求解帶來很大麻煩。所以，使用一階泰勒展開將其化為線性形式。一階泰勒展開公式如式（8）所示。可以使用泰勒展開進行化簡是因為我們是在一個很小的時間間隔內求解運動學參數，所以?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）是一個超定方程。可以用偽逆求解。即：

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

2.3 卡爾曼濾波

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

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

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

3 結語

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