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

（1.南京大學(xué) 控制與系統(tǒng)工程系，江蘇 南京210008；2.蘇州經(jīng)貿(mào)職業(yè)技術(shù)學(xué)院 機(jī)電與信息技術(shù)學(xué)院，江蘇 蘇州215009）

在現(xiàn)代戰(zhàn)爭中，地面無人平臺（unmanned ground vehicle，UGV）可代替人類執(zhí)行有風(fēng)險(xiǎn)的任務(wù)，如：目標(biāo)跟蹤［1］、監(jiān)視［2］、偵察［3］、營救［4］、通信中繼、火力打擊等.基于多UGV 的目標(biāo)協(xié)同跟蹤技術(shù)在更有效的跟蹤敵方入侵目標(biāo)、決策和打擊敵方入侵目標(biāo)等方面具有重要的意義.單一UGV 在探測能力和信息獲取方面具有一定的局限性，多UGV之間的協(xié)同可以提升目標(biāo)的探測和跟蹤能力.多UGV 協(xié)同跟蹤技術(shù)通過分散部署和協(xié)同工作，可以完成單平臺跟蹤不能完成的任務(wù)，能提高目標(biāo)跟蹤的精度、可靠性以及容錯(cuò)能力，具有更廣泛的應(yīng)用和重要的發(fā)展前景.監(jiān)視和跟蹤是多地面無人平臺應(yīng)用的一個(gè)典型例子.在監(jiān)視過程中，多個(gè)UGV 跟蹤入侵者，可以相互協(xié)作并在必要的時(shí)候?qū)崿F(xiàn)對入侵者的圍捕.此類問題的一個(gè)挑戰(zhàn)是在這種情況下由于缺乏全局信息，每個(gè)UGV 只能基于車載傳感器獲得的局部信息實(shí)現(xiàn)跟蹤和編隊(duì)控制.

協(xié)同跟蹤是當(dāng)前目標(biāo)跟蹤領(lǐng)域的研究熱點(diǎn)，國內(nèi)外很多學(xué)者在協(xié)同跟蹤方面進(jìn)行了大量研究.Lim 等［5-6］采用Lyapunov矢量場對多無人機(jī)協(xié)同對峙跟蹤目標(biāo)進(jìn)行了較為深入的研究，使得無人機(jī)與目標(biāo)保持一定的對峙距離，表現(xiàn)為無人機(jī)在目標(biāo)周圍環(huán)繞，形成對峙圓，同時(shí)多無人機(jī)之間保持一定的相位.另有文獻(xiàn)采用非線性預(yù)測控制［7］、擺動運(yùn)動［8］、路徑成 形［9］、自 適 應(yīng) 滑 模 控 制［10］等 技 術(shù) 研 究對峙跟蹤.近年來，Oh等［11-12］采用微分方程對多無人機(jī)協(xié)同對峙跟蹤進(jìn)行了研究.Song等［13］對多地面無人平臺接力型跟蹤進(jìn)行了較為深入的研究.另外與協(xié)同跟蹤較為相關(guān)的研究是編隊(duì)控制，編隊(duì)控制采用的方法主要有領(lǐng)航-跟隨者方法［14-16］、基于行為的 方 法［17-18］、虛 擬 結(jié) 構(gòu) 法［19-21］等.Lee等［22］研 究了航天器編隊(duì)飛行中的跟蹤控制，其控制方案基于離線產(chǎn)生的虛擬領(lǐng)導(dǎo)者軌跡.Lee等［23］提出了虛擬目標(biāo)跟蹤方法解決編隊(duì)控制問題.劉欽等［24］研究了傳感器網(wǎng)絡(luò)下的協(xié)同跟蹤問題.目前研究協(xié)同尾隨跟蹤的文獻(xiàn)還比較少，本文將虛擬目標(biāo)跟蹤方法應(yīng)用于協(xié)同尾隨跟蹤，并在控制算法中集成避障算法，使得各UGV 具有較好的智能性.多UGV 協(xié)同尾隨跟蹤可以使得被跟蹤目標(biāo)始終處于UGV 的監(jiān)控范圍內(nèi)，并且多個(gè)UGV 相互協(xié)同，在跟蹤目標(biāo)的同時(shí)保持一定的編隊(duì)隊(duì)形，使得跟蹤系統(tǒng)具有較好的魯棒性.

1 協(xié)同尾隨跟蹤問題描述

1.1 尾隨跟蹤定義

定義1 尾隨跟蹤：在一定區(qū)域內(nèi)，UGV 尾隨單個(gè)被跟蹤目標(biāo)，或在目標(biāo)后方，或在其側(cè)面，進(jìn)行跟蹤盯梢，在此過程中，UGV 與被跟蹤目標(biāo)保持一定的距離.

定義2 協(xié)同尾隨跟蹤：在一定區(qū)域內(nèi)，多個(gè)UGV（n≥2）協(xié)同尾隨單個(gè)被跟蹤目標(biāo)，UGV 或在目標(biāo)后方，或在其側(cè)面，在跟蹤過程中，每個(gè)UGV與目標(biāo)保持一定的距離，同時(shí)各UGV 之間保持一定的相位.協(xié)同尾隨跟蹤可以使得目標(biāo)一直處于被監(jiān)控狀態(tài)，同時(shí)多UGV 之間可以相互協(xié)作，相互配合，其作業(yè)優(yōu)勢是單個(gè)UGV 尾隨跟蹤所不具備的.

1.2 UGV 運(yùn)動學(xué)模型

在二維平面內(nèi)有n個(gè)UGV，UGVi的運(yùn)動學(xué)模型如下：

式中：pi＝［xi，yi，ψi］T為UGVi在全局 坐 標(biāo) 系x0-y0下的位置及方向角，vi、ωi分別為UGVi的速度和角速度.假設(shè)目標(biāo)的運(yùn)動學(xué)模型如下：

如圖1所示，對于UGVi可建立以其運(yùn)動方向?yàn)閤 軸的局部坐標(biāo)系.對于協(xié)同尾隨跟蹤問題，可以通過控制UGVi與目標(biāo)的距離ri以及UGVi與目標(biāo)的位置角θi實(shí)現(xiàn).

圖1 協(xié)同尾隨跟蹤原理圖Fig.1 Schematic of coordinated stalking tracking

2 協(xié)同尾隨跟蹤算法設(shè)計(jì)

2.1 基本協(xié)同尾隨算法設(shè)計(jì)

對于UGVi要跟蹤的目標(biāo)重新定義其模型：

式中：pdi＝［xt，yt，θi］T，vdi、ωdi分別為新目標(biāo)模型的速度和角速度.

新目標(biāo)模型與UGVi的姿態(tài)誤差為＝pdipi，定義：

式中：

為旋轉(zhuǎn)矩陣.則

由圖1可得

將式（5）代入式（4），利用兩角和差公式進(jìn)行化簡可得

若UGVi到目標(biāo)的距離為ri，其期望距離為rdi，定義距離誤差：

對式（7）求導(dǎo)得

定義位置角誤差：

式中：為期望位置角.對式（9）求導(dǎo)得

定義新的誤差系統(tǒng)方程如下：

式中：

對于新誤差系統(tǒng)，尾隨跟蹤系統(tǒng)的控制目標(biāo)使誤差變量｛xi1，xi2，xi3｝漸進(jìn)收斂.為使｛xi1，xi2，xi3｝漸進(jìn)收斂，基于Lyapunov穩(wěn)定性理論設(shè)計(jì)控制律.假定｛vdi，ωdi｝已知，考慮Lyapunov候選函數(shù)：

對式（12）等號兩邊求導(dǎo)得

設(shè)計(jì)UGVi的控制輸入如下：

式中：｛k1，k2，k3｝為正的控制增益，a0＝sin xi3／xi3為非零函數(shù)，且有

定義函數(shù)f（xi3）如下：

式 中：ε≥0，為 較 小 的 常 量；k2＝／f2（xi3）；k3＝.

由式（6）、（11）得

將式（16）代入式（13）得

將式（14）代入式（17）并化簡得

由Lyapunov 穩(wěn)定性定理可知跟蹤系統(tǒng)漸進(jìn)收斂.由式（14）可知，各個(gè)UGV 的控制算法獨(dú)立于其他UGV，各UGV 之間無須交換控制變量.在協(xié)同尾隨跟蹤過程中，各UGV 僅須交換各自的當(dāng)前位置信息，以供UGV 之間避障使用.若某個(gè)UGV 出現(xiàn)故障，其他UGV 仍然可按照原定跟蹤任務(wù)跟蹤目標(biāo)，此時(shí)指揮控制中心只須指定另外一個(gè)UGV 替代故障UGV 或改變各UGV 與目標(biāo)的期望位置角即可完成協(xié)同尾隨跟蹤任務(wù).

2.2 避障算法設(shè)計(jì)

上述協(xié)同尾隨算法只適用于沒有障礙物的環(huán)境，因此，對于存在障礙物的環(huán)境還需要在基本算法的基礎(chǔ)上使得各UGV 具備避障能力，地面無人平臺跟蹤算法流程圖如圖2所示.

文獻(xiàn)［25］將模糊邏輯應(yīng)用于避障，但所設(shè)計(jì)的模糊控制器是雙輸入單輸出，僅對速度進(jìn)行了控制.本文設(shè)計(jì)的模糊控制器為雙輸入雙輸出［26］，輸入為障礙物位置角和障礙物距離，輸出為速度和角速度，具有更好的避障性能.為實(shí)現(xiàn)UGV 的自動避障，在UGV 前方位置均勻安裝有11個(gè)紅外傳感器，用以檢測障礙物，如圖3所示.紅外傳感器之間間隔18°，假設(shè)各紅外傳感器的有效探測距離為3 m，障礙物的長、寬均大于0.94m，高度大于紅外傳感器的安裝高度，即保證傳感器能夠探測到其有效探測距離范圍的障礙物.將UGV方向和障礙物與紅外傳感器連線形成的夾角定義為障礙物的位置角θo.規(guī)定當(dāng)障礙物位于UGV 運(yùn)動方向左側(cè)時(shí)，θo為正；當(dāng)障礙物位于UGV運(yùn)動方向右側(cè)時(shí)，θo為負(fù).θo可通過安裝在UGV 上的紅外傳感器位置估計(jì)，如圖3所示，當(dāng)障礙物位于UGV 運(yùn)動方向右側(cè)時(shí)，θo≈-36°.

障礙物位置角θo的實(shí)際取值范圍為［-90°，90°］，為方便設(shè)計(jì)模糊控制器，將模糊控制器的輸入θo定為［0，90°］，根據(jù)θo的實(shí)際正負(fù)確定角速度的正負(fù).將輸入變量θo描述為很危險(xiǎn)（VD）、危險(xiǎn)（D）、不確定（U）、安全（S）、很安全（VS）.在仿真程序中，設(shè)定紅外傳感器的有效探測距離為3 m，則紅外傳感器返回的障礙物距離P 可能是0～3的數(shù)值.將輸入變量P 描述為很近（VN）、近（N）、中等（M）、遠(yuǎn)（F）、很遠(yuǎn)（VF），障礙物距離P 的隸屬度函數(shù)μ 如圖4所示.

圖2 地面無人平臺跟蹤算法流程圖Fig.2 Flow chart of tracking algorithm for unmanned ground vehicles

輸出變量確定為速度v及角速度ω.v描述為慢速（SS）、中速（MS）、快速（FS），ω 描述為很?。╒S）、?。⊿M）、中（MM）、大（LM）、很大（VL）.根據(jù)UGV與障礙物所構(gòu)成的不同方位角及距離，可以制定速度及角速度的模糊規(guī)則，分別如表1、2所示.

圖3 地面無人平臺（UGV）攜帶傳感器示意圖Fig.3 Schematic of unmanned ground vehicle（UGV）carrying sensors

圖4 輸入變量障礙物距離的隸屬度函數(shù)Fig.4 Membership functions of input variable obstacles distance

表1 速度v的模糊規(guī)則表（行：θo，列：P）Tab.1 Fuzzy rules of velocity v（row：θo，column：P）

表2 角速度ω 的模糊規(guī)則表（行：θo，列：P）Tab.2 Fuzzy rules of angular velocityω（row：θo，column：P）

3 仿真研究

3個(gè)UGVs協(xié)同尾隨跟蹤目標(biāo)的仿真參數(shù)如表3所示.如圖5 所示為在無障礙物環(huán)境下3 個(gè)UGV 協(xié)同尾隨跟蹤單個(gè)目標(biāo)的效果圖.將本文所設(shè)計(jì)的避障算法集成到基本協(xié)同尾隨跟蹤算法中，仿真中的各UGV 跟蹤軌跡圖如圖6所示.

表3 UGVs協(xié)同尾隨跟蹤目標(biāo)的仿真參數(shù)Tab.3 Simulation parameters of corrdinated stalking tracking of UGVs

當(dāng)UGV2傳感器探測到障礙物時(shí)，先進(jìn)行避障處理；當(dāng)碰撞危險(xiǎn)消除后，UGV2繼續(xù)執(zhí)行原有跟蹤算法跟蹤目標(biāo).避障算法的優(yōu)先級高于跟蹤算法的優(yōu)先級，即當(dāng)UGV 前方出現(xiàn)障礙物時(shí)，UGV 首先要保證自身的安全，先要避開障礙物，待判斷自身已經(jīng)安全后再繼續(xù)執(zhí)行目標(biāo)跟蹤算法.

圖5 無障礙物情況下3個(gè)UGVs協(xié)同尾隨跟蹤Fig.5 Coordinated stalking tracking of three UGVs without obstacles

圖6 有障礙物情況下3個(gè)UGVs協(xié)同尾隨跟蹤Fig.6 Coordinated stalking tracking of three UGVs with obstacles

4 結(jié) 語

本文所提出的算法重新定義了目標(biāo)運(yùn)動模型，設(shè)計(jì)的控制算法通過調(diào)節(jié)各UGVs的速度和角速度使得各UGVs與目標(biāo)保持一定的距離和相位.各UGVs在協(xié)同尾隨跟蹤過程中獨(dú)立于其他UGVs，各UGVs之間無須交換控制變量.所設(shè)計(jì)控制器的優(yōu)點(diǎn)為控制器僅需要相對距離和方位角測量，無人平臺之間僅需要較小的通信量即能實(shí)現(xiàn)協(xié)同跟蹤.另外，在算法中集成避障功能，以適應(yīng)有障礙物的環(huán)境，使得算法具有較好的實(shí)用性.仿真實(shí)驗(yàn)表明了算法的有效性和可行性.

（

）：

［1］FREW E W.Cooperative standoff tracking of uncertain moving targets using active robot networks［C］∥2007 IEEE International Conference on Robotics and Automa-tion.Roma：IEEE，2007：3277-3282.

［2］PEI W.LAN Y B，LUO X W，et al.Integrated Sensor System for Rice Conditions Monitoring Based UGV［J］.International Journal of Agricultural and Biological Engineering，2004，7（2）：75-81.

［3］KHALEGHI A M，XU D，WANG Z R，et al.ADDDAMS-based planning and control framework for surveillance and crowd control via UAVs and UGVs［J］.Expert Systems with Applications，2013，40（18）：7168-7183.

［4］KIM J H，KWON J W，SEO J.Multi-UAV-based stereo vision system without GPS for ground obstacle mapping to assist path planning of UGV ［J］.Electronics Letters，2014，50（20）：1431-1432.

［5］LIM S，KIM Y，LEE D，et al.Standoff target tracking using a vector field for multiple unmanned aircrafts［J］.Journal of Intelligent and Robotic Systems，2013，69（1-4）：347-360.

［6］CHEN H，CHANG K，AGATE C S.UAV path planning with tangent-plus-Lyapunov vector field guidance and obstacle avoidance［J］.IEEE Transactions on Aerospace and Electronic Systems，2013，49（2）：840-856.

［7］KIM S，OH H，TSOURDOS A.Nonlinear model predictive coordinated standoff tracking of a moving ground vehicle［J］.Journal of Guidance，Control，and Dynamics，2013，36（2）：557-566.

［8］YOON S，PARK S，KIM Y.Circular motion guidance law for coordinated standoff tracking of a moving target［J］.IEEE Transactions on Aerospace and Electronic Systems，2013，49（4）：2440-2462.

［9］OH H，TURCHI D，KIM S，et al.Coordinated standoff tracking using path shaping for multiple UAVs［J］.IEEE Transactions on Aerospace and Electronic Systems，2014，50（1）：348-363.

［10］OH H，KIM S，TSOURDOS A，et al.Decentralised standoff tracking of moving targets using adaptive sliding mode control for UAVs［J］.Journal of Intelligent and Robotic Systems，2014，76（1）：169-183.

［11］OH H，KIM S，SHIN H S，et al.Rendezvous and standoff target tracking guidance using differential geometry［J］.Journal of Intelligent and Robotic Systems，2013，69（1-4）：389-405.

［12］SONG Z Q，LI H X，CHEN C L，et al.Coordinated standoff tracking of moving targets using differential geometry［J］.Journal of Zhejiang University SCIENCE C，2014，15（4）：284-292.

［13］SONG Z Q，ZHOU X Z，LI W，et al.Scheduling strategies of relay tracking for network-based multiple unmanned ground vehicles［C］∥Control Conference（CCC），2014 33rd Chinese.Nanjing：IEEE，2014：7943-7947.

［14］BURLUTSKIY N，TOUAHMI Y，LEE B H.Power efficient formation configuration for centralized leaderfollower AUVs control［J］.Journal of Marine Science and Technology，2012，17（3）：315-329.

［15］DU Z J，REN L M，WANG W D，et al.Kinematics／fuzzy logic combined controller for formation control of mobile robots［J］.Journal of Harbin Institute of Technology，2013，20（4）：121-128.

［16］HU J，ZHENG W X.Adaptive tracking control of leader-follower systems with unknown dynamics and partial measurements［J］.Automatica，2014，50（5）：1416-1423.

［17］LIU B，ZHANG R，SHI C.Formation control of multiple behavior-based robots［C］∥2006International Conference on Computational Intelligence and Security.Guang zhou：IEEE，2006：544-547.

［18］ANTONELLI G，ARRICHIELLO F，CHIAVERINI S.Experiments of formation control with multirobot systems using the null-space-based behavioral control［J］.IEEE Transactions on Control Systems Technology，2009，17（5）：1173-1182.

［19］REN W，BEARD R W.Formation feedback control for multiple spacecraft via virtual structures［J］.Control Theory and Applications，IEE Proceedings.2004，151（3）：357-368.

［20］MEHRJERDI H，GHOMMAM J，SAAD M.Nonlinear coordination control for a group of mobile robots using a virtual structure［J］.Mechatronics，2011，21（7）：1147-1155.

［21］REZAEE H，ABDOLLAHI F.A decentralized cooperative control scheme with obstacle avoidance for a team of mobile robots［J］.IEEE Transactions on Industrial Electronics，2014，61（1）：347-354.

［22］LEE D，SANYAL A K，Butcher E A.Asymptotic tracking control for spacecraft formation flying with decentralized collision avoidance［J］.Journal of Guidance，Control，and Dynamics，2015，38（4）：587-600.

［23］LEE Y H，KIM S G，KUC T Y，et al.Virtual target tracking of mobile robot and its application to formation control［J］.International Journal of Control，Automation and Systems，2014，12（2）：390-398.

［24］劉欽，劉崢，劉韻佛，等.多傳感器優(yōu)化部署下的機(jī)動目標(biāo)協(xié)同跟蹤算法［J］.系統(tǒng)工程與電子技術(shù)，2013，35（2）：304-309.LIU Qin，LIU Zheng，LIU Yun-fo，et al.Maneuvering target collaborative tracking algorithm with multisensor deployment optimization［J］.Systems Engineering and Electronics，2013，35（2）：304-309.

［25］NIA D N，TANG H S，KARASFI B，et al.Virtual force field algorithm for a behaviour-based autonomous robot in unknown environments［J］.Proceedings of the Institution of Mechanical Engineers，Part I：Journal of Systems and Control Engineering，2011，225（1）：51-62.

［26］HONG T S，NAKHAEINIA D，KARASFI B.Application of fuzzy logic in mobile robot navigation［M］.［S.l.］：INTECH Open Access Publisher，2012：21-36.