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

張 鑫

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

1 引言

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

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

2 無(wú)人機(jī)機(jī)動(dòng)模型

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

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

設(shè)運(yùn)動(dòng)目標(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ū)動(dòng)矩陣；W為服從N(0，Q)的高斯白噪聲，Q為協(xié)方差矩陣。

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

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

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

設(shè)目標(biāo)運(yùn)動(dòng)的狀態(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)。

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

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

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

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

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

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

設(shè)無(wú)人機(jī)航跡點(diǎn)為(x，y，ψ)，其中x，y表示二維平面位置，ψ表示偏航角。假設(shè)t時(shí)刻航跡點(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/ψ?為無(wú)人機(jī)轉(zhuǎn)彎半徑，V為無(wú)人機(jī)的速度。同理，其他的擴(kuò)展節(jié)點(diǎn)信息可以依次求得。

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

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

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

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

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

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

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

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

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

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

圖3 A*算法流程圖

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

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

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

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

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

設(shè)定無(wú)人機(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。無(wú)人機(jī)先飛向目標(biāo)附近，距離為期望距離時(shí)再盤(pán)旋飛行。

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

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

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

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

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

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

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

6 結(jié)語(yǔ)

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