多地面無人平臺協同尾隨跟蹤

宋志強，周獻中，李華雄

（1.南京大學 控制與系統工程系，江蘇 南京210008；2.蘇州經貿職業技術學院 機電與信息技術學院，江蘇 蘇州215009）

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

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

1 協同尾隨跟蹤問題描述

1.1 尾隨跟蹤定義

定義1 尾隨跟蹤：在一定區域內，UGV 尾隨單個被跟蹤目標，或在目標后方，或在其側面，進行跟蹤盯梢，在此過程中，UGV 與被跟蹤目標保持一定的距離.

定義2 協同尾隨跟蹤：在一定區域內，多個UGV（n≥2）協同尾隨單個被跟蹤目標，UGV 或在目標后方，或在其側面，在跟蹤過程中，每個UGV與目標保持一定的距離，同時各UGV 之間保持一定的相位.協同尾隨跟蹤可以使得目標一直處于被監控狀態，同時多UGV 之間可以相互協作，相互配合，其作業優勢是單個UGV 尾隨跟蹤所不具備的.

1.2 UGV 運動學模型

在二維平面內有n個UGV，UGVi的運動學模型如下：

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

如圖1所示，對于UGVi可建立以其運動方向為x 軸的局部坐標系.對于協同尾隨跟蹤問題，可以通過控制UGVi與目標的距離ri以及UGVi與目標的位置角θi實現.

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

2 協同尾隨跟蹤算法設計

2.1 基本協同尾隨算法設計

對于UGVi要跟蹤的目標重新定義其模型：

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

新目標模型與UGVi的姿態誤差為＝pdipi，定義：

式中：

為旋轉矩陣.則

由圖1可得

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

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

對式（7）求導得

定義位置角誤差：

式中：為期望位置角.對式（9）求導得

定義新的誤差系統方程如下：

式中：

對于新誤差系統，尾隨跟蹤系統的控制目標使誤差變量｛xi1，xi2，xi3｝漸進收斂.為使｛xi1，xi2，xi3｝漸進收斂，基于Lyapunov穩定性理論設計控制律.假定｛vdi，ωdi｝已知，考慮Lyapunov候選函數：

對式（12）等號兩邊求導得

設計UGVi的控制輸入如下：

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

定義函數f（xi3）如下：

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

由式（6）、（11）得

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

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

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

2.2 避障算法設計

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

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

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

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

輸出變量確定為速度v及角速度ω.v描述為慢速（SS）、中速（MS）、快速（FS），ω 描述為很小（VS）、小（SM）、中（MM）、大（LM）、很大（VL）.根據UGV與障礙物所構成的不同方位角及距離，可以制定速度及角速度的模糊規則，分別如表1、2所示.

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

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

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

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

3 仿真研究

3個UGVs協同尾隨跟蹤目標的仿真參數如表3所示.如圖5 所示為在無障礙物環境下3 個UGV 協同尾隨跟蹤單個目標的效果圖.將本文所設計的避障算法集成到基本協同尾隨跟蹤算法中，仿真中的各UGV 跟蹤軌跡圖如圖6所示.

表3 UGVs協同尾隨跟蹤目標的仿真參數Tab.3 Simulation parameters of corrdinated stalking tracking of UGVs

當UGV2傳感器探測到障礙物時，先進行避障處理；當碰撞危險消除后，UGV2繼續執行原有跟蹤算法跟蹤目標.避障算法的優先級高于跟蹤算法的優先級，即當UGV 前方出現障礙物時，UGV 首先要保證自身的安全，先要避開障礙物，待判斷自身已經安全后再繼續執行目標跟蹤算法.

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

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

4 結 語

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

（

）：

［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］劉欽，劉崢，劉韻佛，等.多傳感器優化部署下的機動目標協同跟蹤算法［J］.系統工程與電子技術，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.