Design of Team Formation Simulation System for Unmanned Ground Vehicles Based on USARSim and ROS
-
摘要: 针对越野非结构化环境下的地面无人平台(Unmanned ground vehicle, UGV)编队仿真系统存在功能模块不完善及算法集成测试困难等问题, 为便于有效测试地面无人平台编队协同控制方法性能及其适用的任务场景, 降低编队协同系统的开发成本, 本文提出了一种基于USARSim (Unified System for Automation and Robotics Simulator)和ROS (Robot Operating System)的地面无人平台编队协同仿真系统. 该仿真系统由人机交互界面、基于ROS架构的地面无人平台控制系统和基于USARSim的虚拟仿真场景三个部分组成, 其测试对象为地面无人平台编队协同控制算法. 通过充分利用ROS中集成的开源导航算法和USARSim中丰富的机器人及环境模型, 该系统为研究地面无人平台编队协同控制算法提供了新的思路和快速验证工具. 以领航者−跟随者编队控制方法为例进行该仿真系统的性能测试, 实验结果表明, 该仿真系统能够在外界条件一致的情况下完成对编队协同控制方法的性能测试, 系统稳定可靠.Abstract: Aiming at the problems of imperfect modules and difficult integration of UGV formation control system in off-road unstructured environment, a formation collaborative simulation system is proposed for testing the formation control method of UGV or its applicable mission scenarios, which is based on USARSim (Unified System for Automation and Robotics Simulator) and ROS (Robot Operating System). In this way, the cost for developing formation collaborative system should be reduced. The simulation system is composed of human-machine interaction, UGV control system based on ROS and virtual simulation scenarios based on USARSim. The test object is the formation cooperative control algorithm of UGV. The simulation system makes full use of the open-source navigation algorithm integrated in ROS and the rich robotic models and environment models in USARSim to provide new ideas and rapid verification tools for the research of formation control algorithms. Taking the leader-followers formation control method as an example to test the performance of the proposed simulation system, the experimental results show that the simulation system can test the performance of the formation cooperative control method under the same external conditions, and the system is stable and reliable.
-
Key words:
- UGV /
- team formation /
- USARSim /
- ROS /
- simulation system
-
美俄等军事强国近年来积极推动无人作战平台发展, 带来无人作战平台发展新高潮, 其正在从辅战装备向主战装备发展, 将成为未来战争取得胜利的决定性因素. 长期以来, 美军一直致力于地面无人系统集群作战能力生成, 加快推进地面无人自主系统集群实战化应用[1]. 地面无人平台编队协同是地面无人自主系统的一个重要研究方向, 其致力于实现地面无人平台间协同以及地面无人平台与士兵协同[2], 以提高未来部队的防御、火力攻击、精确打击以及情报、监视与侦察能力.
地面无人平台编队协同是指控制多个地面无人平台的方向角、速度以及无人平台之间的相对距离形成一致, 在适应环境约束的条件下保持设定的队形, 朝着同一个方向运动. 一般来说, 早期的地面无人平台编队协同控制方法可以分为基于领航者−跟随者的编队控制方法[3]、基于虚拟结构的编队控制方法[4]、基于人工势场的编队控制方法[5]、基于行为的编队控制方法和基于路径跟随的编队控制方法[6]. 这类方法都只在编队控制的某个环节有较好的效果, 并没有有效解决编队组建、行进、变换和避障等一系列完整的动作[7].
信息一致性方法描述了每个个体与它相邻个体间的信息交互过程, 确定了个体之间相互作用的规则, 它通过局部的信息交互而使整个群体的状态信息量形成一致. 因此, 在一致性控制协议的基础上通过简单的线性变换就可以将一致性算法应用于编队控制. 近年来越来越多的研究者致力于将信息一致性方法用于地面无人平台编队协同研究, 并取得了一定的研究成果[8-9]. 文献[10]介绍了一种基于一致性的多智能体编队控制方法, 通过选择合适的一致性状态信息量, 将一致性变体算法运用到编队控制中. 在网络条件下, 引入随机通信噪声和信息丢包问题, 文献[11]采用一致性策略研究基于信息交换的多边形编队控制方法. 文献[12]针对复杂地形地面崎岖起伏的特点, 通过建立三维地形环境下编队系统的误差模型, 并根据环境中的特定地形设计相应的编队行驶策略, 实现了多轮式移动机器人系统在复杂地形环境下的编队控制. 文献[13]采用迭代学习控制方法产生一系列的控制信号, 用于多智能体的编队控制. 编队协同控制可使平台保持一定的队形进行运动, 然而这种运动不具有目标性, 要实现其在执行任务过程中运动到指定位置, 则需要进行地面无人平台编队导航控制. 文献[14]研究了领航者具有未知线速度、角速度及其未知上界等多种未知条件下的地面无人平台编队控制问题, 该方法通过在控制器中引入一个时间函数对未知性进行补偿. 文献[15]开展了基于鱼群效应的无人车辆编队控制方法研究, 该方法将无人车辆编队控制与鱼群行为进行类比建模, 以提升无人车辆编队效率. 文献[16]研究了传感器约束条件下基于领航者信息的编队导航, 提出了一种局部控制策略, 即采用动态信息交互拓扑结构, 也能确保形成预定的编队模式.
地面无人平台编队协同控制方法多种多样, 每种方法需要解决的目标任务及最适合的应用场景也不尽相同, 直接在实际环境中进行编队控制算法性能测试既耗时又难于改进, 而地面无人平台编队协同仿真系统则为编队控制算法测试提供了一种快速验证途径. 目前, 大多数的开源或者商业化的编队仿真系统主要支持智能车辆在城市结构化环境下的编队算法测试, 比如道路交通仿真系统Simulation of Urban Mobility[17-18]和自动驾驶仿真软件Carla[19]等. 对于越野非结构化环境, 美国陆军工程研究与发展中心开发的地面无人平台自主导航仿真系统Virtual Autonomous Navigation Environment能够实现对编队控制算法的测试[20], 然而该仿真系统并不开源, 大多数开源的仿真系统仅限于提供仿真环境模块, 比如Real/Player、Microsoft Robotics Developer Studio和Gazebo等, 在这些平台下测试编队协同控制算法需要进行大量平台自主性和环境感知等模块的前期开发工作. 因此, 针对越野非结构化环境下地面无人平台编队仿真系统存在功能模块不完善及算法集成测试困难等问题, 为了便于有效测试不同地面无人平台编队协同控制算法的性能及适用的特定任务场景, 本文设计了一种基于USARSim和ROS的地面无人平台编队协同仿真系统. USARSim中含有丰富的高逼真环境模型和地面无人平台模型[21], 而ROS开源了大量地面无人平台导航相关的算法[22], 将二者结合可用于实现在外界条件一致的情况下测试不同的地面无人平台编队协同控制算法. 该仿真系统主要基于USARSim和ROS两个开源架构设计, 能够有效地用于地面无人平台编队协同控制方法的设计、调试及二次集成开发, 降低地面无人平台编队协同系统的开发成本, 为其研究提供了一种新的思路和方法.
1. 系统结构
基于USARSim和ROS的地面无人平台编队协同仿真系统主要由人机交互界面、基于ROS架构的地面无人平台控制系统和基于USARSim的虚拟仿真场景三个部分组成, 其系统结构如图1所示. 地面无人平台编队协同仿真系统中各部分之间通过组建的局域网进行数据信息交互, 人机交互界面通过UDP网络协议将当前任务下发给地面无人平台控制器. 在仿真系统中可以同时挂载
$1,\; 2,\; \cdots ,\;n$ 个基于ROS架构的地面无人平台控制器进行编队协同测试, 每一个地面无人平台控制器对应于USARSim仿真场景中的一个地面无人平台运动控制模型, 从而按照编队控制方法生成的控制指令驱动地面无人平台运动, 实现在USARSim虚拟场景中进行动态编队演示.
图 1 基于USARSim和ROS的地面无人平台编队协同仿真系统结构Fig. 1 The architecture of formation simulation system for UGV based on USARSim and ROS人机交互界面主要进行遥控操作任务、单平台自主路径跟随任务和多平台编队协同任务下发. 遥控操作任务和单平台自主路径跟随任务主要用于测试单个地面无人平台的自主性能, 它们分别以遥控指令控制量和期望跟随的路径点文件发送给指定的地面无人平台控制器进行解析. 多平台编队协同任务则将地面无人平台设定保持的队形信息、任务目标点信息以及领航者编号等信息发送给仿真系统中的所有地面无人平台控制器. 同时, 人机交互界面也将接收地面无人平台反馈的自身姿态信息, 从而进行数据记录和位姿实时更新显示.
基于ROS架构的地面无人平台控制系统运行Ubuntu Bionic 18.04/ROS Melodic系统, 其上集成了开源的传感器数据处理节点和导航模块[23]. 根据人机交互界面下发的任务类型及从USARSim虚拟场景中获取的地面无人平台自身的位姿信息, 地面无人平台控制器进行解析计算相应的运动控制量, 并通过ROS-USARSim接口模块将该运动控制量发送至USARSim, 从而驱动USARSim虚拟场景中相应的地面无人平台模型运动.
基于USARSim的虚拟仿真场景集成了丰富的地面无人平台模型及特殊场景环境模型, 也可根据实际的地面无人平台特性参数进行模型设计. 它通过ROS-USARSim接口模块将虚拟仿真场景中的地面无人平台模型与地面无人平台控制器一一关联, 并根据接收到的地面无人平台期望控制量驱动模型运动. 同时, 将地面无人平台模型在虚拟场景中的位姿通过ROS-USARSim接口模块反馈给地面无人平台控制器, 以进行更高级的自主运动规划.
2. 人机交互界面设计
人机交互界面主要用于任务下发及地面无人平台位姿实时更新显示, 它由遥控操作发送模块、自主路径跟随任务发送模块、编队协同任务发送模块和地面无人平台位姿显示模块四部分组成, 如图2所示.
2.1 遥控操作发送模块
遥控操作发送模块主要用来测试单个地面无人平台的运动响应能力, 如图3中左侧所示. 它通过选择IP地址, 指定要连接测试的特定地面无人平台, 将期望的线速度
$ v $ 和角速度$ \omega $ 发送给相应的地面无人平台控制器, 从而控制USARSim虚拟场景中的地面无人平台模型运动.2.2 自主路径跟随任务发送模块
自主路径跟随任务发送模块主要用来测试单个地面无人平台对特定路径的自主运动跟随能力, 如图3中中部所示. 它通过选择IP地址, 指定要连接测试的特定地面无人平台, 将预先设定的全局路径点文件发送给相应的地面无人平台控制器, 实现对全局路径的自主跟踪运动控制.
2.3 编队协同任务发送模块
编队协同任务发送模块主要用来测试仿真系统中多个地面无人平台编队协同能力, 如图3中右侧所示. 在该任务界面, 它通过选择IP地址选定需要进行编队测试的所有地面无人平台. 同时, 也需要通过IP地址指定某一个地面无人平台作为编队任务的领航者, 其他地面无人平台则成为跟随者.
编队协同任务的目标点及领航者的路径通过地面无人平台位姿显示模块进行人为设定, 队形设定包括各个跟随者与领航者之间的距离和方位角. 编队任务测试中的各个地面无人平台根据收到的编队任务, 确定自身是领航者还是跟随者. 如果是领航者, 则自主跟随任务路径行驶, 相反, 如果是跟随者, 则根据队形保持算法计算目标位置, 并解算单平台自主运动控制量. 同时, 各个地面无人平台控制器将地面无人平台的位姿反馈给人机交互界面, 在地面无人平台位姿显示模块实时更新编队队形保持状态.
2.4 地面无人平台位姿显示模块
地面无人平台位姿显示模块主要用来设定任务目标点及任务路径, 同时, 接收仿真系统中所有地面无人平台控制器反馈的地面无人平台在USARSim虚拟场景中的位姿数据, 主要以数字和图形化的形式显示地面无人平台在编队任务执行过程中的位姿与行驶轨迹.
3. 基于ROS架构的地面无人平台控制系统
基于ROS架构的地面无人平台控制系统包括通信中间件、地面无人平台控制器和ROS-USARSim接口三个部分, 如图4所示. 通信中间件嵌入在地面无人平台导航模块中实现, 而地面无人平台控制器和ROS-USARSim两者之间通过ROS主题订阅/发布消息机制进行通信, 实现信息传递.
图 4 基于ROS架构的地面无人平台控制系统结构Fig. 4 The architecture of control system for UGV based on ROS3.1 通信中间件
本文采用的通信中间件基于开放式机器人技术中间件(Open robot technology middleware, OpenRTM)实现[24], 接收来自人机交互界面发送的任务类型及与任务相关联的数据, 并将解析后的数据存入到指定的存储区或赋值给指定的变量, 同时将地面无人平台自身的位姿数据发送给人机交互界面. 任务类型由数据接收函数的返回值确定, 其中1代表遥控操作任务, 2代表自主路径跟随任务, 3代表编队协同任务. 同时, 根据不同的任务类型, 通过相应的通信中间件封装函数获取相应的数据, 比如获取的姿态数据主要包括仿真系统中其他地面无人平台的位置、平移速度和横摆角速度等信息.
3.2 地面无人平台控制器
地面无人平台控制器从人机交互界面获取任务类型和其他地面无人平台的位姿数据, 以符号
$ v $ 表示地面无人平台的平移速度, 符号$ \omega $ 表示其横摆角速度, 如图5 所示. 它根据任务类型进入不同的线程, 在独立线程内根据待执行的任务进行相应的期望运动控制量计算, 具体如下:1) 遥控操作任务: 将通过人机交互界面设定的地面无人平台控制量(
$ v,\omega $ )转发到/cmd_vel主题上.2) 自主路径跟随任务: 将通过人机交互界面设定的全局路径发送给地面无人平台导航模块.
3) 编队协同任务: 根据编队任务数据确定自身的角色, 若为领航者, 则将任务目标点发送给地面无人平台导航模块, 反之, 若为跟随者, 则将领航者位姿和队形信息发送给领航者−跟随者编队协同控制方法, 以计算跟随者当前的跟踪目标点, 并将其发送给地面无人平台导航模块进行运动控制量计算.
地面无人平台导航模块计算单个地面无人平台到达目标点或者跟踪期望全局路径的实时控制量(
$ v,\omega $ ), 并发布在/cmd_vel主题上. 该模块采用ROS中已经集成的大量开源导航算法, 它并不是本文的研究重点, 具体可参见http://wiki.ros.org/navigation , 在此不作更深入的介绍.3.3 ROS-USARSim接口
ROS-USARSim接口的目的是在地面无人平台控制器和USARSim之间提供无缝数据交互桥梁, 如图6所示. ROS-USARSim与地面无人平台控制器之间通过ROS主题订阅/发布消息机制进行通信, 而与USARSim之间通过Socket进行通信.
ROS-USARSim接口主要用来传输运动控制量和位姿数据两部分, 它可以自动发现地面无人平台和传感器, 订阅地面无人平台控制器发布在/cmd_vel主题上的控制量, 并传递给USARSim, 从而驱动地面无人平台运动. 同时, 从USARSim接收地面无人平台位姿数据和传感器数据(
$x,\;y,\;\theta,\cdots$ ), 将其发布在/ugv_pose等主题上, 供地面无人平台控制器节点订阅.4. 基于USARSim的虚拟仿真场景
USARSim是一个基于虚拟竞技场引擎设计的高保真多机器人环境仿真平台, 旨在研究城市搜救环境下的机器人、多智能体和人员. 通过使用UDK (Unreal development kit), USARSim利用physX物理引擎和高质量的3D渲染工具创建了逼真的仿真环境, 为机器人系统提供了经过测试的平台模型、传感器模型和环境模型.
4.1 USARSim仿真框架
USARSim通过在游戏引擎之上构建仿真, 可以以低成本实现高保真度. 仿真场景中包括各种环境模型、机器人模型以及传感器模型, 可以将使用者的全部精力投入到与机器人特定任务相关的平台建模、控制系统、传感器、交互工具和环境模型研究.
USARSim最初建立了现实的灾难场景以及机器人测试方法, 随后, USARSim获得了广泛的使用, 并针对不同的使用场景开发了更多的环境, 比如图7 (a)的工厂环境及图7 (b)用于DARPA城市挑战赛的道路环境.
USARSim最初的开发重点是差速驱动轮式移动机器人, 包括三种不同转向模式的轮式平台建模, 即差速驱动、全向驱动和阿克曼转向驱动, 如图8所示. USARSim的开源框架吸引了研究者的广泛关注和支持, 这使得USARSim现在可以提供多种机器人, 包括水下机器人、足式机器人、类人机器人和机械臂等. 像传感器和执行器一样, 机器人是通过特定的类别实现的, 可以通过在运行时读取的配置文件进行配置, 比如指定传感器和末端执行器的类型, 还可以配置机器人的属性, 例如电池寿命和数据传输频率. 因此, 可以更轻松地开发对定制设计进行建模的新平台.
4.2 地面无人平台运动控制模型
在USARSim虚拟场景中, 地面无人平台既可平移, 又可转动, 共有6个自由度, 分别以位置量(
$ x,y,z $ )和转动量$ (\phi,\theta,\psi) $ 表示, 其中,$ \phi $ 、$ \theta $ 和$ \psi $ 分别为侧倾角、俯仰角和横摆角. 在本仿真系统中, 假定地面无人平台经常工作于平坦地面, 因此, 可以对地面无人平台的运动进行简化, 仅考虑地面无人平台沿$ x $ ,$ y $ 方向的平移及绕$ z $ 轴的横摆运动, 忽略沿垂直方向的位移、俯仰角和侧倾角, 即$z = \phi = $ $ \psi = 0$ . 基于这一假设, 以符号$ v $ 表示地面无人平台的平移速度, 符号$ \omega $ 表示其横摆角速度, 它应满足如下的运动学约束条件:$$ \left\{\begin{array}{l} \dot{y}\cos \theta-\dot{x}\sin \theta = 0\\ \dot{x} = v\cos \theta\\ \dot{y} = v\sin \theta \end{array}\right. $$ (1) 如果以
$ {{z}} = [x,y,\theta]^{\rm{T}} $ 表示状态矢量,$ {{u}} = [v,\omega]^{\rm{T}} $ 表示控制矢量, 由式(1)可得地面无人平台控制系统的状态转移方程表达式为:$$ \dot{{{z}}} = \left( \begin{array}{ll} \cos \theta & 0\\ \sin \theta & 0\\ \;\;\;0 & 1\\ \end{array} \right) {{u}} $$ (2) 因此, 根据式(2)的状态转移方程表达式可以推算估计地面无人平台在控制量
$ {{u}} $ 的作用下的实时位姿状态$ {{z}} $ .5. 结果与分析
为了验证所提出的基于USARSim和ROS的地面无人平台编队协同仿真系统的有效性, 本文采用在实际工程中被广泛应用的领航者−跟随者编队控制方法进行测试[25], 并对比该控制方法分别在Matlab/Simulink和LabVIEW两种仿真系统下的测试结果. 在本次测试实验中, 采用三辆地面无人平台参与编队任务, 其运动控制模型见第4.2节. 在仿真测试实验阶段, 共采用了五台计算机进行模拟, 计算机配置为酷睿 TMi7-9700处理器, 主频可达3.0 GHz, 其中三台计算机作为地面无人平台控制器, 一台计算机运行USARSim虚拟仿真场景, 另一台计算机运行人机交互界面, 具体的测试硬件配置见表1所示.
表 1 仿真测试硬件配置Table 1 Hardware configuration in simulation计算机编号 操作系统 IP地址 角色 1# Windows10 192.168.0.200 人机交互 2# Windows10 192.168.0.201 USARSim 3# ROS Melodic 192.168.0.100 领航者(UGV_0) 4# ROS Melodic 192.168.0.101 跟随者(UGV_1) 5# ROS Melodic 192.168.0.102 跟随者(UGV_2) 5.1 仿真测试的编队协同控制方法
地面无人平台编队协同仿真系统主要用来验证测试不同的编队协同控制方法, 本文以领航者−跟随者编队控制方法为例进行该编队协同仿真系统的性能测试. 人机交互界面主要通过UDP协议进行编队队形任务发布、接收和发布各地面无人平台的位姿, 而三个地面无人平台则根据编队任务进行自主决策. 通过人机交互界面设定领航者的任务路径, 编队队形为三角形, 如图9所示, 以地面无人平台UGV_0为领航者, UGV_1和UGV_2为跟随者. UGV_0的运动根据任务路径计算获得, 而UGV_1和UGV_2的运动控制量则根据自身的当前位姿和编队队形控制方法实时计算获得.
以(
$ x_{i},y_{i} $ )表示地面无人平台在世界坐标系中的位置坐标,$ \theta_{i} $ 为地面无人平台在世界坐标系中的航向角,$ d_{i} $ 为跟随地面无人平台质心到领航地面无人平台质心的距离,$ \varphi_{i} $ 是跟随地面无人平台的$ y $ 轴到领航地面无人平台质心的角度,$ i $ 为地面无人平台编号. 因此, 领航者UGV_0的位姿向量是$[ x_{0} \quad y_{0} \quad \theta_{0}]^{\rm{T}}$ , 跟随者UGV_1的位姿向量是$ [x_{1} \quad y_{1} \quad \theta_{1} ]^{\rm{T}}$ , 跟随者UGV_2的位姿向量是$[ x_{2} \quad y_{2} \quad \theta_{2} ]^{\rm{T}}$ , 而领航者和跟随者的控制输入分别是平移速度和角速度, 即$[ v_{0} \quad \omega_{0} ]^{\rm{T}}$ 、$[v_{1} \quad \omega_{1}]^{\rm{T}}$ 和$[v_{2} \quad $ $ \omega_{2}]^{\rm{T}}$ . 领航者和跟随者的航向角差值分别是$\alpha_{1} = \theta_{0}- $ $ \theta_{1}$ 和$ \alpha_{2} = \theta_{0}-\theta_{2} $ . 因此, UGV_0和UGV_1编队简化运动控制模型如式(3)所示.$$ \begin{split} {{\dot{ s}}} =\;&{ \left[ {\begin{array}{*{20}{c}} {{{\dot d}_1}}\\ {{{\dot \varphi }_1}}\\ {{{\dot \alpha }_1}} \end{array}} \right] = f({{s}}){{u}} = }\\ &{\left[\!\! {\begin{array}{*{20}{c}} { - \cos {\varphi _1}}&0&{\cos {\delta _1}}&0\\ {\sin {\varphi _1}/{d_1}}&{ - 1}&{ - \sin {\delta _1}/{d_1}}&0\\ 0&{ - 1}&0&1 \end{array}} \!\!\right]\left[\!\! {\begin{array}{*{20}{c}} {{v_1}}\\ {{\omega _1}}\\ {{v_0}}\\ {{\omega _0}} \end{array}}\!\! \right]} \end{split}$$ (3) 式中,
$ \delta_{1} = \varphi_{1}+\alpha_{1} $ .为了达到和维持理想的地面无人平台编队, 地面无人平台系统的状态估计采用无迹卡尔曼滤波(Unscented Kalman filter, UKF)算法. 从式(3)可以看出, 对于距离−角度编队控制方案, UKF滤波算法只需要观测角度信息
$ \varphi_{1} $ 和$ \alpha_{1} $ , 即输出观测向量$ y = [\varphi_{1} \quad \alpha_{1}]^{\rm{T}} $ . 利用文献[26]中UKF算法估计状态$ {{s}} $ 的角度参数, 带有噪声的地面无人平台编队系统的状态方程和观测方程分别以式(4)、(5)表示$$ \dot{{{s}}} = f({{s}}){{u}}+O $$ (4) $$ {{y}} = G{{s}}+N $$ (5) 式中,
$ G $ 是输出转换矩阵,$ O $ 和$ N $ 是零均值、协方差分别是$ P_{L} $ 和$ P_{N} $ 白高斯噪声, 且$ {{s}} $ (0)、$ O $ 和$ N $ 假定为不相关.对式(4)表示的编队状态方程进行变形, 得到与其等价的式(6), 并对
$ \alpha_ {1} = \theta_{0}-\theta_{1} $ 两边求导, 可得式(7).$$ \dot{{{s_{r}}}} = M(s){{u}}_{0}+N(s){{u}}_{1} $$ (6) $$ \dot{\alpha_ {1}} = \omega_{0}-\omega_{1} $$ (7) 式中,
$ {{s}}_r = [d_{1} \quad \varphi_{1}]^{\rm{T}} $ ,$M(s),N(s)\in{\bf{R}}^{2\times 2}$ 分别是$ f({{s}}) $ 的右上角和左上角的子矩阵.基于编队运动控制模型(3)和编队状态方程(6), 采用的地面无人平台编队控制的输入− 输出状态反馈控制规律为:
$$ {{u}}_{1} = [v_{1} \quad \omega_{1}]^{\rm{T}} = N^{-1}(s)({{C}}-M(s){{u}}_{0}) $$ (8) $$ {{C}} = -{{K}}(s_{r}-s_{r}^{\rm{des}}) = \left[ \begin{array}{c} k_{1}(d_{1}^{\rm{des}}-d_{1}) \\ k_{2}(\varphi_{1}^{\rm{des}}-\varphi_{1}) \\ \end{array} \right] $$ (9) 式中,
$ {{C}} $ 是辅助控制变量, 控制增益$ k_{1} $ ,$ k_{2}>0 $ , 上标“des”指相应变量的期望值.UGV_0和UGV_2之间的编队运动控制规律也可按照该方法计算, 从而获得UGV_2的编队控制矢量
$[ v_{2} \quad \omega_{2}]^{\rm{T}}$ .在仿真测试过程中, 通过人机交互界面设定发送给地面无人平台的队形信息
$ d_{1} $ = 15 m,$ d_{2} $ = 15 m,$ \varphi_{1} $ = 5$ \pi $ /3,$ \varphi_{2} $ =$ \pi $ /3, 而领航者的控制输入$ {{u}}_0 $ =$[v_{0} \quad \omega_{0}]^{\rm{T}}$ 根据任务路径实时计算获得, 可视为已知量. 依据式(8)和(9)表示的编队运动控制规律, 可计算获得UGV_1和UGV_2保持队形跟随领航者UGV_0的实时控制量$ {{u}}_1 $ =$[v_{1} \quad \omega_{1} ]^{\rm{T}}$ 和$ {{u}}_2 $ =$[v_{2} \quad \omega_{2}]^{\rm{T}}$ . 由于受环境感知传感器测距误差的限制, 在本次测试中设定地面无人平台在$ x $ 和$ y $ 方向到达期望目标点的阈值为$ \pm $ 5 cm.5.2 测试实验结果
在实验过程中, 在USARSim虚拟仿真场景中加载了三个地面无人平台模型, 如图10所示, 该模型驱动形式为四轮独立差速, 以平移速度和角速度作为输入量进行控制. 通过人机交互界面设定领航者需要到达的任务目标点, 如图11中圆点所示, 三条运动轨迹末端的圆点分别表示领航者UGV_0和跟随者UGV_1、UGV_2在人机交互界面实时更新显示的位姿.
依据人机交互界面设定的编队队形信息, 在编队任务执行过程中, UGV_1和UGV_2逐渐调整为和UGV_0保持等腰三角形队形, 三个地面无人平台运动平顺, 直至UGV_0运动到任务目标点而中止运动. UGV_0、UGV_1和UGV_2在任务执行过程中的运动轨迹分别如图11中的中部、上部和下部的实线所示, 从图中可以看出, 三个地面无人平台的运动轨迹平滑, 且队形保持较为稳定, 更多关于这次实验的结果及视频可见
https://github.com/robotman801/Formation-control. 在编队任务执行过程中, 在某一时间段内UGV_1和UGV_2的平移速度和横摆角速度的变化曲线如图12所示, 航向角误差曲线如图13所示, 运动轨迹曲线如图14所示. 在图12中, 实线分别为UGV_1的平移速度和横摆角速度, 而虚线分别为UGV_2的平移速度和横摆角速度. 从图12可以看出, UGV_1和UGV_2的平移速度和横摆角速度并没有发生剧烈的突变, 根据编队控制规律输出的控制量
$ {{u}}_1 $ 和$ {{u}}_2 $ 合理, 地面无人平台在整个控制周期内运动比较平顺. 在图13中, 实线为在编队控制过程中UGV_1和UGV_2的期望航向角, 而虚线为UGV_1和UGV_2的实际航向角. 从图13可以看出, UGV_1和UGV_2的航向角逐渐调整并逼近期望航向角. 在编队航向角调整过程中, UGV_1的航向角相对误差最大为60.12 %, 最小为0.26 %, 如图13(a)所示, 而UGV_2的航向角相对误差最大为56.78 %, 最小为0.43 %, 如图13 (b)所示. 在图14中, 实线为在编队控制过程中UGV_1和UGV_2的期望轨迹曲线, 而虚线为UGV_1和UGV_2的实际轨迹曲线. 从图14可以看出, UGV_1和UGV_2的运动轨迹逐渐调整并逼近期望轨迹. 在编队运动控制过程中, UGV_1的运动轨迹绝对位置误差最大为1.98 m, 最小为0.04 m, 如图14 (a)所示, 而UGV_2的运动轨迹绝对位置误差最大为2.31 m, 最小为0.06 m, 如图14 (b)所示.
图 13 三角形编队任务中的跟随者航向角误差Fig. 13 Heading angle error for followers during triangle formation control
图 14 三角形编队任务中的跟随者轨迹曲线Fig. 14 Trajectories for followers during triangle formation control为了验证所提出的基于USARSim和ROS的无人平台编队仿真系统的性能优劣, 将领航者−跟随者编队控制方法在本仿真系统下的性能测试结果分别与在Matlab/Simulink[27]和LabVIEW[28]仿真系统下的测试结果进行对比, 对比结果见表2所示. 从表2中可以看出, 本文所提出的编队仿真系统与Matlab/Simulink和LabVIEW搭建的编队仿真系统相比, 在编队任务执行过程中每个地面无人平台均以较小误差到达期望目标点, 对领航者−跟随者编队控制算法的测试结果一致, 表明本文所提出的仿真系统的有效性. 然而, 本文所提出的编队仿真系统在场景逼真度、人机交互界面、开发简易性和可扩展性(比如增加环境感知和导航规划)等方面具有较大优势.
表 2 领航者−跟随者编队方法在不同仿真平台下的测试对比Table 2 The comparison of leader-follower formation in different simulation systems对比项 本文仿真系统 MATLAB LabVIEW UGV_1 位置$x$绝对误差 2.18 cm $\Delta x\to 0$ $\Delta x\to 0$ UGV_1 位置$y$绝对误差 3.14 cm $\Delta y\to 0$ $\Delta y\to 0$ UGV_1 航向角相对误差 0.26 % $\Delta \theta\to 0$ $\Delta \theta\to 0$ UGV_2 位置$x$绝对误差 4.89 cm $\Delta x\to 0$ $\Delta x\to 0$ UGV_2 位置$y$绝对误差 3.38 cm $\Delta y\to 0$ $\Delta y\to 0$ UGV_2 航向角相对误差 0.43 % $\Delta \theta\to 0$ $\Delta \theta\to 0$ 场景逼真度 $\bullet\bullet\bullet$ $\bullet$ $\bullet\bullet$ 人机交互 $\bullet\bullet\bullet$ $\bullet$ $\bullet\bullet$ 可扩展性 (感知及导航) $\bullet\bullet\bullet$ $\bullet\bullet$ $\bullet$ 开发简易性 $\bullet\bullet\bullet$ $\bullet\bullet$ $\bullet$ 综上所述, 基于USARSim和ROS的地面无人平台编队协同仿真系统能够在外界条件一致的情况下仅对编队协同控制方法性能进行测试, 多个地面无人平台能快速形成编队, 并逐渐调整后以很小的航向角误差和位置误差沿期望的编队轨迹运动, 保证了编队的稳定性. 因此, 该编队协同仿真系统稳定可靠, 非常适合用于各种编队协同控制方法的设计及验证测试.
6. 结论
单一地面无人平台在信息获取及处理能力等方面是有限的, 对于复杂的任务及多变的工作环境, 单一地面无人平台的能力更显不足. 多地面无人平台比单一地面无人平台具有更强的优越性, 在军事应用方面, 通过多地面无人平台编队协作可以更有效地完成安全巡逻、区域侦察、探测及协同打击等战术任务.
为了测试地面无人平台编队协同控制方法及其适用的任务场景, 降低编队协同系统的开发成本, 本文提出了一种基于USARSim和ROS的地面无人平台编队协同仿真系统, 它由人机交互界面、基于ROS架构的地面无人平台控制系统和基于USARSim的虚拟仿真场景三个部分组成, 具备编队协同任务下发、任务场景设计、编队控制算法测试和单平台自主导航等功能. 在该仿真系统中, 各个地面无人平台之间依赖局域网(UDP)共享位姿数据, 并以领航者−跟随者编队控制方法为例进行该仿真系统的性能测试. 实验结果表明, 该仿真系统能够在外界条件一致的情况下完成对编队协同控制方法的性能测试, 系统稳定可靠, 为研究地面无人平台编队协同控制算法提供了新的思路和快速验证工具. 通过模块化的接口设计, 该仿真系统具有较好的开放性与通用性, 然而, 各个模块之间需通信网络进行信息传递, 协作性能高度依赖通信网络质量, 当通讯受到强干扰时存在协作失败风险. 因此, 该仿真系统需进一步开发不依赖通信网络的编队协同控制算法测试模块, 以满足不依赖通信网络的编队协同控制算法的测试需求.
-
图 1 基于USARSim和ROS的地面无人平台编队协同仿真系统结构
Fig. 1 The architecture of formation simulation system for UGV based on USARSim and ROS
图 4 基于ROS架构的地面无人平台控制系统结构
Fig. 4 The architecture of control system for UGV based on ROS
图 13 三角形编队任务中的跟随者航向角误差
Fig. 13 Heading angle error for followers during triangle formation control
图 14 三角形编队任务中的跟随者轨迹曲线
Fig. 14 Trajectories for followers during triangle formation control
表 1 仿真测试硬件配置
Table 1 Hardware configuration in simulation
计算机编号 操作系统 IP地址 角色 1# Windows10 192.168.0.200 人机交互 2# Windows10 192.168.0.201 USARSim 3# ROS Melodic 192.168.0.100 领航者(UGV_0) 4# ROS Melodic 192.168.0.101 跟随者(UGV_1) 5# ROS Melodic 192.168.0.102 跟随者(UGV_2) 
下载: 导出CSV
表 2 领航者−跟随者编队方法在不同仿真平台下的测试对比
Table 2 The comparison of leader-follower formation in different simulation systems
对比项 本文仿真系统 MATLAB LabVIEW UGV_1 位置$x$绝对误差 2.18 cm $\Delta x\to 0$ $\Delta x\to 0$ UGV_1 位置$y$绝对误差 3.14 cm $\Delta y\to 0$ $\Delta y\to 0$ UGV_1 航向角相对误差 0.26 % $\Delta \theta\to 0$ $\Delta \theta\to 0$ UGV_2 位置$x$绝对误差 4.89 cm $\Delta x\to 0$ $\Delta x\to 0$ UGV_2 位置$y$绝对误差 3.38 cm $\Delta y\to 0$ $\Delta y\to 0$ UGV_2 航向角相对误差 0.43 % $\Delta \theta\to 0$ $\Delta \theta\to 0$ 场景逼真度 $\bullet\bullet\bullet$ $\bullet$ $\bullet\bullet$ 人机交互 $\bullet\bullet\bullet$ $\bullet$ $\bullet\bullet$ 可扩展性 (感知及导航) $\bullet\bullet\bullet$ $\bullet\bullet$ $\bullet$ 开发简易性 $\bullet\bullet\bullet$ $\bullet\bullet$ $\bullet$
下载: 导出CSV
-
[1] Daniel A K. Robotic and Autonomous Systems Strategy. US Army, 2018. 1−31 [2] Muangmin K, Wanichanon T. Formation keeping of unmanned ground vehicles. MATEC Web of Conferences, 2017, 95: 09006 doi: 10.1051/matecconf/20179509006 [3] 黄健飞, 马彦. 基于跟随领航者的车辆自适应编队控制. 吉林大学学报(信息科学版), 2019, 37(3): 253−259Huang Jian-Fei, Ma Yan. Adaptive formation control for vehicles based on leader-follower strategy. Journal of Jinlin University (Information Science Edition), 2019, 37(3): 253−259 [4] Sun Z, Li B, Cai W C, Liao X H, Song Y D. Virtual leader based robust adaptive formation control of multi-unmanned ground vehicles (UGVs). In: Proceedings of the 2007 American Control Conference. NewYork, USA: IEEE, 2007. 1876−1881 [5] 郑延斌, 席鹏雪, 王林林, 樊文鑫, 韩梦云. 基于人工势场法的多智能体编队避障方法. 计算机应用, 2018, 38(12): 3380−3384 doi: 10.11772/j.issn.1001-9081.2018051119Zheng Yan-Bin, Xi Peng-Xue, Wang Lin-Lin, Fan Wen-Xin, Han Meng-Yun. Obstacle avoidance method for multi-agent formation based on artificial potential field method. Journal of Computer Applications, 2018, 38(12): 3380−3384 doi: 10.11772/j.issn.1001-9081.2018051119 [6] 郭宏达, 李永乐, 娄静涛. 车辆无人编队控制系统技术与应用分析. 军事交通学院学报, 2018, 20(6): 23−28Guo Hong-Da, Li Yong-Le, Lou Jing-Tao. Technology for vehicular autonomous platoon control system and its application. Journal of Military Transportation University, 2018, 20(6): 23−28 [7] 卫恒, 吕强, 刘扬, 林辉灿, 梁冰. 基于状态切换的分布式多机器人编队控制. 兵工学报, 2019, 40(5): 1103−1112 doi: 10.3969/j.issn.1000-1093.2019.05.024Wei Heng, Lv Qiang, Liu Yang, Lin Hui-Can, Liang Bing. State switching-based distributed multi-robot formation control. Acta Armamentarii, 2019, 40(5): 1103−1112 doi: 10.3969/j.issn.1000-1093.2019.05.024 [8] Yang T, Meng Z Y, Dimarogonas D V, Johansson K H. Global consensus for discrete-time multi-agent systems with input saturation constraints. Automatica, 2014, 50(2): 499−506 doi: 10.1016/j.automatica.2013.11.008 [9] 李润梅, 张立威, 王剑. 基于时变间距和相对角度的无人车跟随控制方法研究. 自动化学报, 2018, 44(11): 2031−2040Li Run-Mei, Zhang Li-Wei, Wang Jian. A control method of unmanned car following under time-varying relative distance and angle. Acta Automatica Sinica, 2018, 44(11): 2031−2040 [10] Ren W. Consensus based formation control strategies for multi-vehicle systems. In: Proceedings of the 2006 American Control Conference. New York, USA: IEEE, 2006. 4237−4242 [11] Glavaski S, Chaves M, Day R, Nag P, Williams A, Zhang W. Vehicle networks: achieving regular formation. In: Proceedings of the 2003 American Control Conference. New York, USA: IEEE, 2003. 4095−4100 [12] 张瑞雷, 李胜, 陈庆伟, 杨春. 复杂地形环境下多机器人编队控制方法. 控制理论与应用, 2014, 31(4): 531−537 doi: 10.7641/CTA.2014.30924Zhang Rui-Lei, Li Sheng, Chen Qing-Wei, Yang Chun. Formation control for multi-robot system in complex terrain. Control Theory and Applications, 2014, 31(4): 531−537 doi: 10.7641/CTA.2014.30924 [13] Meng D Y, Jia Y M, Du J P, Zhang J. On iterative learning algorithms for the formation control of nonlinear multi-agent systems. Automatica, 2014, 50: 291−295 doi: 10.1016/j.automatica.2013.11.009 [14] 王晓森, 曹国华. 带有多未知性的无人车编队控制. 车辆与动力技术, 2019, 2: 43−47 doi: 10.3969/j.issn.1009-4687.2019.02.009Wang Xiao-Sen, Cao Guo-Hua. Formation control for unmanned vehicles with multiple unknowns. Vehicle and Power Technology, 2019, 2: 43−47 doi: 10.3969/j.issn.1009-4687.2019.02.009 [15] 田大新, 康璐. 基于鱼群效应的无人驾驶车辆编队算法研究. 无人系统技术, 2018, 1(4): 62−67Tian Da-Xin, Kang Lu. Research on algorithm of unmanned vehicle formation based on fish school. Unmanned Systems Technology, 2018, 1(4): 62−67 [16] Soni A, Hu H. Formation control for a fleet of autonomous ground vehicles: A survey. Robotics, 2018, 7(67): 1−25 [17] Krajzewicz D, Hertkorn G, Wagner P, Rossel C. SUMO(Simulation of Urban Mobility): An open-source traffic simulation. In: Proceedings of the 4th Middle East Symposium on Simulation and Modelling. NewYork, USA: IEEE, 2002. 183−187 [18] 高力, 陆丽萍, 褚端峰, 张勇, 吴超仲. 基于图与势场法的多车道编队控制. 自动化学报, 2020, 46(1): 117−126Gao Li, LU Li-Ping, Chu Duan-Feng, Zhang Yong, Wu Chao-Zhong. Multi-lane convoy control based on graph and potential field. Acta Automatica Sinica, 2020, 46(1): 117−126 [19] Dosovitskiy A, Ros G, Codevilla F, Lopez A, Koltun V. CARLA: An open urban driving simulator. In: Proceedings of the 1st Conference on Robot Learning. New York, USA: IEEE, 2017. 1−10 [20] Goodin C, George T R, Cummins C L, Durst P J, Gates B Q, Mckinley G B. The virtual autonomous navigation environment: High fidelity simulations of sensor, environment, and terramechanics for robotics. In: Proceedings of the 13th ASCE Aerospace Division Conference on Engineering, Science, Construction, and Operations in Challenging Environments. New York, USA: IEEE, 2012. 1441−1447 [21] Carpin S, Lewis M, Wang J J, Balakirsky S. USARSim: a robot simulator for research and education. In: Proceedings of the IEEE International Conference on Robotics and Automation. New York, USA: IEEE, 2007. 1400−1405 [22] Quigley M, Conley K, Gerkey B P, Faust J. ROS: an open-source robot operating system. In: Proceedings of the 2009 IEEE International Conference on Robotics and Automation Workshop on Open Source Software. New York, USA: IEEE, 2009. 1−6 [23] Akash U, Ananya N, Sandeep S, Raj D. Autonomous navigation with collision avoidance using ROS. Journal of Remote Sensing GIS & Technology, 2019, 5(2): 14−18 [24] Ceriani S, Migliavacca M. Middleware in Robotics, Internal Report for Advanced Methods of Information Technology for Autonomous Robotics, 2016 [25] 韩青, 孙树栋, 智睿瑞. 基于纯角度观测信息的leader-followers机器人编队控制方法. 西北工业大学学报, 2015, 33(2): 244−250 doi: 10.3969/j.issn.1000-2758.2015.02.013Han Qing, Sun Shu-Dong, Zhi Rui-Rui. Bearing-only leader-followers multi-robot formation control. Journal of Northwestern Polytechnical University, 2015, 33(2): 244−250 doi: 10.3969/j.issn.1000-2758.2015.02.013 [26] Das A K, Fierro R, Kumar V, Ostrowski J P, Spletzer J, Taylor C J. A vision-based formation control framework. IEEE Transaction on Robotics and Automation, 2002, 18(5): 813−825 doi: 10.1109/TRA.2002.803463 [27] Madhevan B, Sreekumar M. Tracking algorithm using leader follower approach for multi robots. Procedia Engineering, 2013, 64: 1426−1435 doi: 10.1016/j.proeng.2013.09.224 [28] Yu X, Liu L. Leader-follower formation of vehicles with velocity constraints and local coordinate frames. Science China Information Sciences, 2017, 60(7): 070206 doi: 10.1007/s11432-016-9094-1 期刊类型引用(8)
1. 张继文，徐博，王储岩，王朝阳. 基于神经动力学模型预测的多AUV编队自适应控制方案. 中国舰船研究. 2025(01): 326-339 . 
百度学术2. 施雨逢，骆吉安. 三维纯角度极坐标目标运动分析算法. 杭州电子科技大学学报(自然科学版). 2024(03): 5-11 . 百度学术3. 王鹏林，史东辉，江金松，宫小兰，刘梦茹. 虚拟现实技术辅助机器车视觉循迹建模. 安庆师范大学学报(自然科学版). 2024(03): 71-77 . 百度学术4. 吴俊强，邹旻. 基于MVP架构的工业机器人虚拟仿真系统研究. 电气传动自动化. 2023(05): 16-19 . 百度学术5. 赵祥模，赵玉钰，景首才，惠飞，刘建蓓. 面向自动驾驶测试的危险变道场景泛化生成. 自动化学报. 2023(10): 2211-2223 . 本站查看6. 张浩杰，梁荣敏，张玉东. 基于人在回路的地面无人平台仿真系统设计. 系统工程与电子技术. 2022(02): 538-545 . 百度学术7. 付国萍，田小壮，刘家辛，王伟光，王正武，李松，高小伟，王祝. 变电站无人机自主巡检仿真系统设计与搭建. 电力学报. 2022(03): 257-262+278 . 百度学术8. 康照奇，刘洋，刘媛媛，陈展，徐京邦. ROS框架下模块化AGV控制系统设计. 制造业自动化. 2022(12): 80-84 . 百度学术其他类型引用(8)
-
下载:
计量
- 文章访问数: 2076
- HTML全文浏览量: 693
- PDF下载量: 360
- 被引次数: 16
