宋志強(qiáng)+周獻(xiàn)中+李華雄

摘 要： 針對帶防守區(qū)域約束的多地面無人平臺（UGV）協(xié)同跟蹤單目標(biāo)問題，提出了基于多UGV最佳觀測位置的目標(biāo)跟蹤算法。首先在無約束情況下，研究基于梯度法的UGV觀測位置調(diào)整，然后添加約束條件，利用Voronoi圖的相關(guān)特性，研究在UGV各自防守區(qū)域及速度等約束條件下的UGV觀測位置優(yōu)化問題，通過最小化數(shù)據(jù)融合后的協(xié)方差矩陣行列式，從而調(diào)整UGV的觀測位置，來獲得更好的跟蹤質(zhì)量。仿真結(jié)果表明了算法的有效性和合理性。

關(guān)鍵詞： Voronoi圖； 地面無人平臺； 協(xié)同控制； 主動(dòng)目標(biāo)跟蹤

中圖分類號：TP11 文獻(xiàn)標(biāo)志碼：A 文章編號：1006-8228（2016）04-17-04

Research on target tracking based on voronoi diagram and optimal

observation of unmanned ground vehicles

Song Zhiqiang1， Zhou Xianzhong2， Li Huaxiong2

（1. Institute of Electrical and Information Technology， Suzhou Institute of Trade & Commerce， Suzhou， Jiangsu 215009， China；

2. School of Management and Engineering， Nanjing University）

Abstract： For the problem of multiple unmanned ground vehicles （UGVs） with defense area constraints collaboratively tracking a single target， a target tracking algorithm is proposed based on the optimal observation position of UGVs. Firstly， in the unconstrained case the observation position adjustment of UGVs based on the gradient method is researched. Then the observation position optimization problem of UGVs with the constraints of UGV's defensive area and velocity is addressed using the characteristics of Voronoi diagram. The UGVs can adjust the observation position by minimizing the determinant of the fused covariance matrix and get better tracking quality. The simulation results showed the validity and rationality of the algorithm.

Key words： Voronoi diagram； unmanned ground vehicle； collaborative control； active target tracking

0 引言

地面無人平臺（Unmanned Ground Vehicle，UGV）又名無人車，其在軍事和民用領(lǐng)域都具有廣泛的應(yīng)用[1-3]。單個(gè)UGV在探測能力和信息獲取方面有一定局限性，多UGV之間協(xié)同可以完成單平臺不能完成的任務(wù)。本文主要研究多地面無人平臺的部署問題，以最大化移動(dòng)目標(biāo)的位置估計(jì)準(zhǔn)確性。多無人平臺跟蹤目標(biāo)問題在監(jiān)視、巡邏、環(huán)境監(jiān)測等方面具有應(yīng)用價(jià)值。對于靜態(tài)傳感器的目標(biāo)跟蹤已有較廣泛的研究[4-6]，相對于靜態(tài)傳感器，移動(dòng)傳感器在傳感質(zhì)量、區(qū)域覆蓋、環(huán)境適應(yīng)性等方面均具有優(yōu)勢，多移動(dòng)傳感器的協(xié)作傳感可利用傳感器的移動(dòng)能力以提高目標(biāo)跟蹤性能[7-11]。文獻(xiàn)[7-8]研究了跟蹤目標(biāo)時(shí)僅有距離測量的多移動(dòng)傳感器最優(yōu)運(yùn)動(dòng)策略問題。文獻(xiàn)[9-10]使用距離和方位測量，提出一種移動(dòng)傳感器近似跟蹤策略以最小化目標(biāo)的位置不確定性。文獻(xiàn)[11]提出基于多無人機(jī)的主動(dòng)目標(biāo)跟蹤與聯(lián)合定位方法，采用卡爾曼濾波器估計(jì)目標(biāo)位置?，F(xiàn)有文獻(xiàn)較少考慮各平臺的運(yùn)動(dòng)約束，均沒有考慮各平臺的防守區(qū)域約束。本文考慮各平臺的運(yùn)動(dòng)約束和平臺防守區(qū)域約束，研究多UGV主動(dòng)目標(biāo)跟蹤問題。

1 主動(dòng)目標(biāo)跟蹤簡介

1.1 基于Voronoi圖的多UGV部署

根據(jù)Voronoi圖原理，各UGV可根據(jù)自身及周圍其他UGV的位置信息來構(gòu)造各自Voronoi多邊形區(qū)域，再計(jì)算該區(qū)域質(zhì)心位置，最后各UGV以各自區(qū)域質(zhì)心為目的地，運(yùn)動(dòng)至該目的地，最終各UGV可形成更為穩(wěn)定的質(zhì)心Voronoi分布（CVT）。基于Voronoi圖的多UGV部署如圖1所示。從圖1（a）和圖1（b）的比較可以看出，CVT劃分要比Voronoi劃分更加均勻，可使各跟蹤平臺負(fù)擔(dān)較為均衡。

1.2 測量模型

UGV跟蹤和觀測目標(biāo)示意圖如圖2所示。

目標(biāo)運(yùn)動(dòng)模型為：

⑴

觀測模型為：

⑵

其中，F(xiàn)和H分別為線性化后的過程模型矩陣與觀測模型矩陣，w和v均為零均值高斯白噪聲。由于存在局部坐標(biāo)至全局坐標(biāo)的轉(zhuǎn)化，因此目標(biāo)跟蹤傳感器的觀測模型為非線性化的。典型傳感器傳輸為帶噪聲的距離和方位測量值。第i個(gè)UGV的觀測模型可表述為：

⑶

假設(shè)在整個(gè)防守區(qū)域內(nèi)共有M個(gè)UGV，UGV攜帶的傳感器主要為超聲波傳感器和攝像頭，用超聲波傳感器可獲得較精確的UGV到目標(biāo)的距離測量，使用攝像頭可獲得較精確的UGV與目標(biāo)之間的方位。定義協(xié)方差矩陣Ri表示第i個(gè)UGV在全局坐標(biāo)系下測量值的不確定性，在局部坐標(biāo)系（即極坐標(biāo)系）下，由超聲波傳感器和攝像頭帶來的測量噪聲協(xié)方差矩陣可表述成如下形式

⑷

其中，，分別為第i個(gè)UGV距離和方位的測量噪聲方差?？捎蒛GVi到目標(biāo)的距離函數(shù)fr（ri）表示，可由UGVi到目標(biāo)的距離函數(shù)fb（ri）表示。采用文獻(xiàn)[11]的假設(shè)，fr（ri）=a2（ri-a1）2+a0，fb（ri）=αfr（ri），其中a0，a1，a2，α為常系數(shù)，ri為UGVi和目標(biāo)之間的距離。為了把局部坐標(biāo)下的測量噪聲轉(zhuǎn)換到全局坐標(biāo)系中，定義轉(zhuǎn)換矩陣：

⑸

則在全局坐標(biāo)系下測量協(xié)方差矩陣可表述為：

⑹

假設(shè)每個(gè)平臺傳感器獲得的測量值能相互共享，在每一傳感器中，使用多個(gè)傳感器的測量值進(jìn)行數(shù)據(jù)融合。使用通用的融合規(guī)則為：

⑺

文獻(xiàn)[12]以融合后的協(xié)方差矩陣的行列式作為優(yōu)化對象目標(biāo)函數(shù)J。

⑻

2 協(xié)同控制律設(shè)計(jì)

我們采用梯度下降法最小化目標(biāo)函數(shù)J。目標(biāo)函數(shù)在極坐標(biāo)下的梯度可表示為：

⑼

結(jié)合矩陣論相關(guān)知識：

⑽

⑾

⑿

可得：

⒀

其中，zi為ri或ψi。

證明：

對行列式求微分得

因此有：

所以有：

而在極坐標(biāo)下，對Ri求偏導(dǎo)數(shù)可得：

于是可求得及。

UGV運(yùn)動(dòng)模型采用二階積分模型[11]：

⒁

設(shè)計(jì)基于梯度的控制律為

⒂

其中，B?0，為阻尼矩陣。C?0，為增益矩陣。

3 協(xié)同目標(biāo)跟蹤算法描述

基于Voronoi圖和UGV最優(yōu)觀測的目標(biāo)跟蹤算法如表1所示。

4 仿真研究

同時(shí)考慮UGV速度、角速度、防守區(qū)域約束，三個(gè)UGV協(xié)同跟蹤一個(gè)目標(biāo)時(shí)的跟蹤效果如圖3所示，仿真參數(shù)如表2所示。

表1 協(xié)調(diào)目標(biāo)跟蹤算法

[基于Voronoi圖和UGV最優(yōu)觀測的目標(biāo)跟蹤算法＼&Step 1：指揮中心根據(jù)各跟蹤平臺位置生成用于協(xié)同跟蹤的Voronoi圖；

Step 2：指揮中心根據(jù)當(dāng)前戰(zhàn)場態(tài)勢，決定是否要生成質(zhì)心Voronoi圖；

Step 2.1：若當(dāng)前態(tài)勢不緊急，則指揮中心命令各跟蹤平臺運(yùn)動(dòng)至自身所在Voronoi區(qū)域的質(zhì)心。

Step 2.2：若當(dāng)前態(tài)勢緊急，則各跟蹤平臺直接處于待命狀態(tài)。

Step 3：在每個(gè)時(shí)間步k

//局部觀測

Step 3.1：進(jìn)行局部測量；

更新局部估計(jì)及誤差協(xié)方差矩陣Pi；

//傳感器融合

Step 3.2：向其他平臺發(fā)送局部信息；

接收來自其他平臺的局部信息（其他平臺也可能返回空信息，即其他平臺還沒有探測到目標(biāo)）；融合所有局部估計(jì)（也有可能只有當(dāng)前跟蹤平臺的估計(jì)）得到全局估計(jì)和全局誤差協(xié)方差矩陣P；

//優(yōu)化平臺/傳感器位置

Step 3.3：使用目標(biāo)函數(shù)的梯度產(chǎn)生平臺下一步的最優(yōu)位置；

//更新位置

Step 3.4：跟蹤平臺移動(dòng)至期望的位置（需考慮UGV的運(yùn)動(dòng)約束）。對于僅有當(dāng)前主跟蹤平臺探測到目標(biāo)而其他鄰近平臺尚未探測到目標(biāo)的情況，當(dāng)前主跟蹤平臺仍運(yùn)動(dòng)到下一步的最優(yōu)位置，且當(dāng)前跟蹤平臺命令鄰居平臺向目標(biāo)移動(dòng)。

Step 4：當(dāng)目標(biāo)接近Voronoi多邊形邊界時(shí)，當(dāng)前主跟蹤平臺若收到來自鄰近平臺的測量估計(jì)，則進(jìn)入準(zhǔn)備階段，當(dāng)目標(biāo)越過Voronoi邊進(jìn)入下一個(gè)Voronoi區(qū)域時(shí)，該區(qū)域的跟蹤平臺切換為當(dāng)前主跟蹤平臺，前一時(shí)刻的主跟蹤平臺切換為從跟蹤平臺。

Step 5：各跟蹤平臺的運(yùn)動(dòng)不超過其自身防衛(wèi)區(qū)域。對當(dāng)前沒有探測到入侵目標(biāo)的平臺，在其所有鄰域平臺失去跟蹤目標(biāo)后，返回Voronoi區(qū)域的質(zhì)心位置，即原位置。＼&]

圖3 有防守區(qū)域約束時(shí)的UGV協(xié)同目標(biāo)跟蹤示意圖

表2 仿真參數(shù)

[參數(shù)＼&參數(shù)值＼&說明＼&T＼&0.1 s＼&采樣周期＼&vmax＼&5.0 m/s＼&UGV最大速度＼&ωmax＼&0.30 rad/s＼&UGV最大角速度＼&a2＼&0.0008＼&傳感器相關(guān)常系數(shù)＼&a1＼&15.625＼&傳感器相關(guān)常系數(shù)＼&a0＼&0.1528＼&傳感器相關(guān)常系數(shù)＼&a＼&5＼&傳感器相關(guān)常系數(shù)＼&]

由圖3可知，剛開始時(shí)，三個(gè)UGV協(xié)同跟蹤目標(biāo)，當(dāng)UGV 2即將到達(dá)Voronoi多邊形邊界時(shí)，停止前行，保證UGV 2不離開自身防守區(qū)域，而其他UGV則繼續(xù)對目標(biāo)進(jìn)行跟蹤?；赩oronoi圖的多UGV協(xié)同目標(biāo)跟蹤算法使得各UGV在不離開各自防守區(qū)域的前提下，實(shí)現(xiàn)對目標(biāo)的最優(yōu)觀測。

5 結(jié)束語

本文基于目標(biāo)運(yùn)動(dòng)模型和傳感器觀測模型，提出基于UGV最佳觀測位置的協(xié)同目標(biāo)跟蹤方法。首先對于無約束情況，研究基于梯度法的UGV觀測位置調(diào)整，然后添加約束條件，利用Voronoi圖的相關(guān)特性，研究在UGV各自防守區(qū)域及速度等約束條件的UGV觀測位置優(yōu)化問題，通過最小化數(shù)據(jù)融合后的協(xié)方差矩陣行列式調(diào)整UGV的觀測位置，來獲得更好的跟蹤質(zhì)量。仿真結(jié)果表明了該控制算法的有效性。

后期研究擬針對UGV實(shí)物，將設(shè)計(jì)的控制算法在UGV實(shí)物平臺上測試、運(yùn)行，以檢驗(yàn)算法的實(shí)用性并進(jìn)一步優(yōu)化算法。

參考文獻(xiàn)（References）：

[1] Nigam N， Bieniawski S， Kroo I， et al. Control of Multiple

UAVs for Persistent Surveillance： Algorithm and Flight Test Results[J].IEEE Transactions on Control Systems Technology，2012.20（5）：1236-1251

[2] Forsmo E. J.， Grotli E. I.， Fossen T. I.， et.al. Optimal

search mission with Unmanned Aerial Vehicles using Mixed Integer Linear Programming[C]//2013 International Conference on Unmanned Aircraft Systems （ICUAS）. IEEE，2013：253-259

[3] Acebedo J J， Arrue B C， Maza I， et al. Cooperative Large

Area Surveillance with a Team of Aerial Mobile Robots for Long Endurance Missions[J]. Journal of Intelligent & Robotic Systems Theory & Applications，2013.70（14）：329-346

[4] Bar-Shalom Y， Kirubarajan T， Li X R. Estimation with

Applications to Tracking and Navigation[M].New York： Wiley，2001.

[5] Dariusz U， Optimal Measurement Methods for Distributed

Parameter System Identification[M]. Boca Raton， FL： CRC，2004.

[6] Taylor C， Rahimi A， Bachrach J， et al. Simultaneous

localization， calibration， and tracking in an ad hoc sensor network[C]// In IPSN '06： Proceedings of the fifth international conference on Information processing in sensor networks，2006：27-33

[7] Zhou K， Roumeliotis S I. Optimal motion strategies for

range-only constrained multisensor target tracking[J]. IEEE Transactions on Robotics，2008.24（5）：1168-1185

[8] Stump E， Kumar V， Grocholsky B， et al. Control for

localization of targets using range-only sensors[J]. Int. J. Robot. Res，2009.28（6）：743-757

[9] Stroupe A W， Balch T. Value-based action selection for

observation with robot teams using probabilistic techniques[J].Robotics & Autonomous Systems，2005.50（2-3）：85-97

[10] Zhou K， Roumeliotis S I. Multi-robot active target

tracking with combinations of relative observations[J]. Robotics IEEE Transactions on，2010.27（4）：678-695

[11] Morbidi F， Mariottini G L. Active target tracking and

cooperative localization for teams of aerial vehicles[J]. IEEE Transactions on Control Systems Technology，2013.21（5）：1694-1707

[12] Chung T H， Burdick J W， Murray R M. A decentralized

motion coordination strategy for dynamic target tracking[C]//2006 IEEE International Conference on Robotics and Automation（ICRA 2006）. IEEE，2006：2416-2422