多地面无人平台协同尾随跟踪

宋志强，周献中，李华雄

（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.