基于改進(jìn)A*算法的無人機(jī)目標(biāo)跟蹤研究*

張 鑫

（商洛學(xué)院電子信息與電氣工程學(xué)院 商洛 726000）

1 引言

無人機(jī)（Unmanned Aerial Vehicle，UAV）是一種不搭載操作人員能夠自主飛行或進(jìn)行遠(yuǎn)程引導(dǎo)的航空器。針對無人機(jī)目標(biāo)跟蹤問題，國內(nèi)外有較多研究。文獻(xiàn)［1］針對部隊(duì)轉(zhuǎn)移掩護(hù)任務(wù)研究無人機(jī)跟蹤轉(zhuǎn)移部隊(duì)的方法，該研究假設(shè)部隊(duì)位置實(shí)時發(fā)送給無人機(jī)，即目標(biāo)為合作目標(biāo)，因此，無人機(jī)所要考慮的問題是如何保持指定距離跟蹤轉(zhuǎn)移部隊(duì)。文獻(xiàn)［2］把目標(biāo)的狀態(tài)估計引入到平臺的運(yùn)動控制策略中進(jìn)行考慮，在對移動目標(biāo)跟蹤的同時，估計運(yùn)動目標(biāo)的狀態(tài)，然后采用Lyapunov控制方法保持傳感器對目標(biāo)的持續(xù)覆蓋。但這兩種方法并沒有充分考慮無人機(jī)的動力學(xué)約束。

本文考慮的是對地面/海面機(jī)動目標(biāo)的視覺跟蹤，這需要對機(jī)載攝像機(jī)拍攝的圖像進(jìn)行處理，識別目標(biāo)并計算目標(biāo)的參數(shù)信息。利用改進(jìn)A*算法和文獻(xiàn)［3］的跟蹤算法進(jìn)行了目標(biāo)跟蹤，通過仿真，驗(yàn)證了改進(jìn)A*算法的可靠性。

2 無人機(jī)機(jī)動模型

機(jī)動目標(biāo)的模型有很多，其中協(xié)同轉(zhuǎn)彎模型［4］和“當(dāng)前”統(tǒng)計模型［5］是兩種目前最常用的機(jī)動目標(biāo)運(yùn)動模型，下面分別對這兩種模型進(jìn)行介紹。

1）協(xié)同轉(zhuǎn)彎模型

設(shè)運(yùn)動目標(biāo)的狀態(tài)方程為

式中：A為狀態(tài)轉(zhuǎn)移矩陣；X(k)為目標(biāo)狀態(tài)矢量，X(k)=[x(k)(k)y(k)(k)]T，x、y表示目標(biāo)位置；B為噪聲驅(qū)動矩陣；W為服從N(0，Q)的高斯白噪聲，Q為協(xié)方差矩陣。

對于轉(zhuǎn)移矩陣A及噪聲驅(qū)動矩陣B分別有：

式中：T為采樣時間/s；ω為運(yùn)動角速度/（rad/s），代表機(jī)動情況的參數(shù)，ω＞0表示左轉(zhuǎn)彎，ω＜0表示右轉(zhuǎn)彎，ω=0則表示勻速直線運(yùn)動。

2）“當(dāng)前”統(tǒng)計模型

設(shè)目標(biāo)運(yùn)動的狀態(tài)方程為

式中：X(k)為目標(biāo)狀態(tài)矢量，X(k)=[x(k)(k)(k)y(k)(k)(k)]T；Φ為狀態(tài)轉(zhuǎn)移矩陣；W(k)為白噪聲序列，均值為0，方差為Q(k)。

對于狀態(tài)轉(zhuǎn)移矩陣Φ及白噪聲序列方差Q(k)有：

式中：α為目標(biāo)的機(jī)動頻率。

3 航跡點(diǎn)規(guī)劃方法

為了方便計算機(jī)解析，用航路點(diǎn)序列代替航路曲線［6～7］。無人機(jī)在固定高度定速飛行時，飛行軌跡從三維變?yōu)槎S。圖1表示此時的航跡點(diǎn)。

如圖1所示，假設(shè)在t時刻，無人機(jī)所處位置為O，最大偏航角速率為ψ?max。假設(shè)相鄰航跡點(diǎn)的時間間隔為Δt，每個點(diǎn)有3種可能，則在3Δt內(nèi)航路就有27種可能［8］。

圖1 航跡點(diǎn)擴(kuò)展圖

設(shè)無人機(jī)航跡點(diǎn)為(x，y，ψ)，其中x，y表示二維平面位置，ψ表示偏航角。假設(shè)t時刻航跡點(diǎn)O為(x0，y0，ψ0)，取Δt=1，那么航跡點(diǎn)A的狀態(tài)信息為(x0-Rsinψ1+Rsinψ0，y0+Rcosψ1-Rcosψ0，ψ1)，其中ψ1=ψ0-ψ?Δt，R=V/ψ?為無人機(jī)轉(zhuǎn)彎半徑，V為無人機(jī)的速度。同理，其他的擴(kuò)展節(jié)點(diǎn)信息可以依次求得。

根據(jù)模型預(yù)測控制思想，航跡點(diǎn)的規(guī)劃步驟如下：

1）當(dāng)前t0時刻，以Δt為步長，規(guī)劃以后N步最優(yōu)航跡點(diǎn)序列；

2）實(shí)際航跡點(diǎn)用前K步規(guī)劃的點(diǎn)代替，其中0＜K＜N；

3）t0+KΔt時繼續(xù)進(jìn)行1）、2），直到任務(wù)終止。

目標(biāo)跟蹤流程圖如圖2所示。

圖2 目標(biāo)跟蹤流程圖

步驟1）中需要獲得最優(yōu)的航跡點(diǎn)序列。若無人機(jī)在每個位置上都有M個可以選擇的航向，規(guī)劃N步，則會有MN條不同航路可供選擇。步驟1）的任務(wù)就是在MN條路中找到最優(yōu)的航路。和TSP問題的求解類似，基于對實(shí)時性的考慮，選擇A*算法，該算法效率高、運(yùn)行速度快。

4 改進(jìn)A*算法求解航跡點(diǎn)序列

A*算法是一種啟發(fā)式的最優(yōu)搜索算法。評價函數(shù)定義為

式中：f(k)為從起始節(jié)點(diǎn)經(jīng)過點(diǎn)k到目標(biāo)節(jié)點(diǎn)的代價估計值；wg為真實(shí)代價的加權(quán)；g(k)為從起始節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)k的真實(shí)代價；wh為估計代價的加權(quán)；h(k)為從當(dāng)前節(jié)點(diǎn)k到目標(biāo)位置的代價估計值。

圖3 A*算法流程圖

若UAV在每個位置上都有M個可以選擇的航向，規(guī)劃N步，則擴(kuò)展的航跡點(diǎn)共有∑Mi(1≤i≤N)個。設(shè)每個航跡點(diǎn)編碼為[mn]，其中m(1≤m≤N)表示擴(kuò)展的第m步，n(1≤n≤Mm)表示第m步的第n個擴(kuò)展節(jié)點(diǎn)。求最優(yōu)航跡時將相應(yīng)的航跡點(diǎn)編碼加入Openlist、Closelist，只求出需要的航跡點(diǎn)的狀態(tài)信息(x，y，ψ)。

A*算法［9］中的評價函數(shù)f(k)中包括消耗代價g(k)和估計代價h(k)。無危脅情況下，無人機(jī)在進(jìn)行目標(biāo)跟蹤時，可以使g(k)=0。h(k)用來選擇恰當(dāng)?shù)膶?yōu)方向，又被叫做啟發(fā)函數(shù)［10］。因此，算法的可靠性和快速收斂性主要取決于h(k)。文獻(xiàn)［3］直接將目標(biāo)點(diǎn)與當(dāng)前點(diǎn)間的歐氏距離看作h(k)，對于目標(biāo)跟蹤的問題，h(k)設(shè)為

式中：ρi為航跡點(diǎn)序列中第i個航跡點(diǎn)與目標(biāo)水平方向的距離；ρd為UAV與目標(biāo)的水平期望距離。

但是隨著迭代次數(shù)的增加，采用上式會使以后增加的節(jié)點(diǎn)代價值增大，算法無法快速收斂。因此，根據(jù)文獻(xiàn)［11～12］設(shè)置h(k)為

5 無威脅環(huán)境下目標(biāo)跟蹤仿真

設(shè)定無人機(jī)的最大偏航角速度ψ?max為0.2rad/s，速度是50m/s，飛行的高度1000m，北東地坐標(biāo)系下的起始坐標(biāo)為（0，0，-1000）。

跟蹤目標(biāo)參數(shù)：靜止，北東地坐標(biāo)系下坐標(biāo)為（2000，2000，0）。

任務(wù)要求：UAV與目標(biāo)水平方向的期望距離ρd=500m。無人機(jī)先飛向目標(biāo)附近，距離為期望距離時再盤旋飛行。

航路規(guī)劃算法相關(guān)參數(shù)設(shè)置如下：步長為1s，每次預(yù)規(guī)劃3步，實(shí)際執(zhí)行1步，每個航跡點(diǎn)有9個航向可以選擇。分別對靜止目標(biāo)和機(jī)動目標(biāo)進(jìn)行跟蹤。

采用改進(jìn)A*算法進(jìn)行仿真，并與文獻(xiàn)［3］算法的仿真結(jié)果進(jìn)行比較，對靜止目標(biāo)的跟蹤結(jié)果如圖4所示，對機(jī)動目標(biāo)的跟蹤結(jié)果如圖5和圖6所示。文獻(xiàn)［3］的跟蹤結(jié)果用虛線，改進(jìn)A*算法的結(jié)果用實(shí)線表示。圖4中，黑點(diǎn)表示靜止實(shí)物，圖5、6中，粗線表示機(jī)動目標(biāo)。

圖4 靜止目標(biāo)的跟蹤結(jié)果

圖5 協(xié)調(diào)轉(zhuǎn)彎模型下跟蹤結(jié)果

圖6 “當(dāng)前”統(tǒng)計模型下跟蹤結(jié)果

從圖4中可以看出，兩種算法都可以滿足任務(wù)要求，但是本文算法比文獻(xiàn)［3］能夠更快地收斂至500m，且震蕩較小，跟蹤效果更優(yōu)。

圖5、6為協(xié)調(diào)轉(zhuǎn)彎模型和“當(dāng)前”統(tǒng)計模型下對機(jī)動目標(biāo)的跟蹤結(jié)果。由跟蹤結(jié)果可知，本文算法與文獻(xiàn)［3］中的算法相比收斂到期望距離的速度更快，并且目標(biāo)產(chǎn)生了較大的機(jī)動時本文算法也能完成跟蹤任務(wù)，偏差明顯小于文獻(xiàn)［3］中算法。

6 結(jié)語

首先分析了無人機(jī)的協(xié)同轉(zhuǎn)彎模型和“當(dāng)前”統(tǒng)計模型，在無危脅條件下，利用文獻(xiàn)［5］的改進(jìn)A*算法和文獻(xiàn)［3］分別對靜止目標(biāo)和機(jī)動目標(biāo)進(jìn)行了跟蹤，并進(jìn)行了仿真對比。對比了兩種算法下的目標(biāo)軌跡與UVA運(yùn)動軌跡，UVA與目標(biāo)的水平距離。仿真結(jié)果表明，改進(jìn)A*算法能有效地實(shí)現(xiàn)目標(biāo)跟蹤。