无人机与地面机器人协同编队实现方法及系统
技术领域
本发明涉及无人机与地面机器人协同编队领域、无线数据传输通信领域、嵌入式开发技 术领域,具体讲,涉及无人机与地面机器人协同编队系统。
背景技术
近年来伴随着互联网、大数据、云计算等热门话题的另一个话题就是机器人。随着各种 智能硬件与软件产品的出现,高性能机器人的实现成为了可能。机器人的发展已经历经了近 60年,机器人功能由简单到复杂,随着硬件配置的逐渐提高,机器人可以逐渐为更多的行业 和更多的人服务,从地面移动机器人到水下与空中机器人,从轮式等简单移动方式到类人或 者类生物的行走、爬行机器人,机器人正在各行各业发挥着巨大作用。
ROS(Robot Operating System)是一个用于机器人软件开发的软件框架集合。他可以在异 构计算机集群上提供类似操作系统的功能。ROS能够提供标准的操作系统服务,例如硬件的 抽象描述、底层的驱动控制、通用功能的完成与实现、进程间消息的传递以及程序包的管理。 ROS基于图状架构,通过分布式节点框架来实现不同功能进程间信息的发布、接收。
无人机是一种无驾驶员、可以进行遥控或自主飞行的无人驾驶航空器,在尺寸、机动性、 成本等方面比有人机更具优势。军用无人机不必考虑人员伤亡,而且具有超长时间续航能力 和超高机动飞行,并且其目标特征小,具有很强的突防能力和生存能力,是集探测、识别与 跟踪、决策和作战功能为一体的先进综合武器系统。美国空军研究实验室根据OODA(Observe-Orient-Decide-Act)模型将无人机自主控制分为十个等级。自主级别越高,无 人机就具有更高的智能性和适应性,可完成的任务越复杂。无人机在现代战争中被视为战场 中的先锋,承担着战前情报收集、重点目标监视和打击等关键职责。目前,单一无人机的能 力有限,无论是军用还是民用,为了应对更加复杂的应用环境,集群化是无人机发展的必经 之路。
近年来,无人机与地面机器人协同编队是异构无人器联合编队研究的前沿热点问题。在 新兴领域和现代化国防上,无人机与地面机器人协同编队比单独无人机或地面机器人编队更 具优势。无人机在空中视野宽广,而地面机器人则能够近距离靠近目标进行细致的观测并执 行任务,通过结合两者各自的优点,无人机与地面机器人协同编队可以完成地理勘测、复杂 环境下的目标跟踪、救援和运输、武装搜寻以及协同攻击军事目标等任务。
目前关于无人机与地面机器人协同编队的相关研究成果较少,大多都是基于理想的机器 人仿真平台进行算法研究,而且对无人机与地面机器人个体的模型以及异构机器人之间的通 信大多采用的是理想化方法。综上所述,无人机与地面机器人空地协同编队属于一个新的研 究领域,要应用于实际中需要解决很多问题,到目前为止仍然是一项具有挑战性的技术难题, 也是我国亟待解决的关键技术之一,对我国的科学技术发展以及提升国防实力都具有重要意 义。
发明内容
为克服现有技术的不足,本发明旨在提出一种可扩展性好、稳定性强,并且能够支持无 人机与地面机器人平台协同编队的系统,使得研究者可以通过该系统对空地协同编队算法进 行实物仿真与实验验证。本发明采用的技术方案是,无人机与地面机器人协同编队系统,由 无人机编队、室内定位系统、地面移动机器人以及地面工作站构成;
无人机编队包括多架多种类型的四旋翼无人机,每架无人机均搭载机载控制器、传感器 设备和WiFi模块,机载控制器实时运行飞行控制算法,负责协调和指挥无人机的所有行为, 无人机的姿态信息通过传感器设备获取,位置信息由室内定位系统通过WiFi模块来提供给机 载控制器,室内定位系统在实时解算无人机当前位置后,将位置信息发送至地面工作站;
机器人可以搭载载荷,每个机器人上分别搭载具有WiFi模块的控制器,并具备实时反馈 位置信息的能力;
地面工作站是一台具有较强运算能力的计算机,负责接收无人机和地面机器人发送的姿 态信息与位置信息,并实时运行无人机与地面机器人协同编队算法。
协同编队算法包括三个部分:定义势场函数,设计地面机器人势场力函数和设计无人机 势场力函数;第一部分:首先定义势场函数,定义约束相邻的两个机器人i和j的势场函数, 势场力为势场函数的梯度函数;第二部分:设计地面机器人的势场力函数,地面机器人分为 普通跟随者和第一跟随者两类;第三部分:结合地面机器人的位置信息设计无人机的势场力 函数;最后根据势场函数确定每个地面机器人和无人机所受到的势场力合力,控制机器人和 无人机的速度输出,实现无人机和地面机器人协同编队。
无人机与地面机器人协同编队实现方法,利用无人机编队、室内定位系统、地面移动机 器人以及地面工作站实现,具体步骤如下:
首先,基于WiFi建立无线通信网络,利用基于TCP/IP协议的ROS节点通信技术,以订 阅和发布话题的形式收发数据,实现无人机、地面移动机器人与地面工作站三者之间的实时 信息交互;
然后,通过室内定位系统根据无人机上的标记点来计算其位置,进而发送给地面工作站; 地面机器人利用自身里程计对航迹预测并利用陀螺仪校正,获得位置信息再发送给地面工作 站,地面工作站运行无人机与地面机器人协同编队算法,计算出整个编队内每个单元的期望 位置,并发送给相应单元;
最后,无人机和地面机器人的控制器分别运行相应的机载控制算法,进而驱动无人机和 地面机器人移动到求解出的期望位置,形成无人机与地面机器人编队。
协同编队算法分为三个部分:定义势场函数,设计地面机器人势场力函数和设计无人机 势场力函数:
第一部分:定义势场函数,定义约束相邻的两个机器人i和j的势场函数为Uij(qi),对于 任意位姿,Uij(qi)为可微分的、并且是非负的,满足机器人i相对于机器人j的约束条件的势 场函数为
其中η为正比例增益系数,qi,qj为节点的位置,ρij=||qi-qj||为节点i与节点j之间的实 际欧式距离,而ρ0为期望的距离,该势场函数特点如下:
1)、当ρij→∞时,Uij(qi)→∞;
2)、当ρij=ρ0时,Uij(qi)取最小值;
按照势场力的定义,势场力为势场函数的梯度函数,如式(2)所示:
其中,为qi指向qj的单元向量,
势场力函数在ρij<ρ0时为负,机器人之间关系体现为斥力作用;势场力函数在ρij>ρ0时 为负,机器人之间关系体现为引力作用;势场力函数在ρij=ρ0时为负,机器人距离达到稳定 状态;
第二部分:地面机器人势场力函数设计,地面机器人分为第一根随着和普通跟随者,对 于普通机器人跟随者Vi,Vi需要同时跟随两个机器人Vj和Vk,即与Vj和Vk之间的距离||qi(t)-qj(t)||与||qi(t)-qk(t)||需要保持在相应的期望距离ρ0ij、ρ0ik,在任意时刻t>0,Vi的期 望位置即以Vj、Vk两为圆心,以ρ0ij、ρ0ik为半径的两个圆的交点。为使Vi达到该点,以Vj、 Vk为势能中心,以公式(1)(2)所述势场函数建立势场,则Vi所受到的势场力合力为:
其中η、ξ为相应的正比例增益,ρij、ρik为Vi与Vj、Vk之间的实际距离,ρ0ij、ρ0ik为 相应的期望距离,为Vi指向Vj、Vk单位向量;
然后,假设领航机器人为V1,第一跟随者为V2,V2仅需满足与V1的距离约束即可,即其运动轨迹为V1以为圆心、以期望距离ρ012为半径的圆,V2与V1的距离||q1(t)-q2(t)||始终保 持为期望距离ρ012,据此对第一跟随者V2设定如下势场力函数:
其中η为正比例约束系数,ρ12为V2与V1之间的实际距离,ρ012为期望距离,为由V2指向V1的单位向量;
第三部分:无人机势场力函数设计,结合地面机器人位置信息构造势场力函数如下:
式中,α,β,γ为正比例系数,dcl,dcj,dci分别为无人机Vc与三个机器人的实际距离,lcl,lcj,lci分别为无人机Vc与三个机器人的期望距离,最后根据势场函数确定每个地面机器人和无人机 所受到的势场力合力,控制机器人和无人机的速度输出,实现无人机和地面机器人协同编队。
本发明的特点及有益效果是:
此项发明对于无人机与地面机器人协同编队领域的算法研究具有十分重要的意义。该项 发明稳定可靠,可直接作为无人机与地面机器人协同编队研究的实验平台,进而解决编队控 制,路径规划,编队协同避障问题,推动无人机与地面机器人编队控制理论的发展。还可将 平台内的地面机器人作为无人机的跟踪或打击目标,研究目标跟踪控制等问题。该平台可扩 展性好,可较为容易的增加无人机或地面机器人的数量,根据自身需要扩大编队的规模。该 平台的稳定性强,由于主要的协同控制工作在地面工作站完成,有效的降低了无人机和地面 机器人的计算压力,提高了整个系统的稳定性和可靠性。此外,在编队移动过程中的实时数 据不仅可以为协同编队算法提供数据支持,还可以通过对它的分析进一步改进算法。总而言 之,该平台可以很好的为理论研究服务,避免不必要的经费投入,具有很高的实用价值。
本发明主要具有如下特点和优点:
(1)通信功能:无人机与地面机器人编队中比较重要的问题就是无线通信,与仿真情况下 不同,仿真环境中都是理想的认为个体之间的通信是已经完成的,而在真实的机器人编队平 台中,无人机、地面机器人与地面工作站之间的都要建立通信联系。本发明搭建的无人机与 地面机器人协同编队系统平台选用的通信方式是基于WIFI建立通信网络,利用基于TCP/IP 协议的ROS节点通信机制进而实现无人机与机器人及工作站之间的信息交互。
(2)定位功能:本系统在室内环境进行平台搭建,解决了编队内成员的室内定位问题。考 虑到无人机和地面机器人各自的不同特点,采用室内定位系统捕捉无人机的位置,该系统可 快速准确地解算出无人机的三维坐标数据与三轴姿态数据。对于地面机器人,我们则考虑利 用机器人自身里程计和陀螺仪进行自主定位。
(3)控制器:本发明搭建的系统平台中无人机和地面机器人均需要控制器,控制器是它们 的核心部件,承担着采集数据,实时控制和无线通信等任务。平台中无人机采用Pixhawk机 载飞行控制器,该飞控提供了软件接口,并且开放源代码,使用者可对其进行开发,方便快 捷,大幅缩短研发周期;地面机器人采用嵌入式迷你计算机Pcduino V3B作为机器人的控制 器,相比利用计算机作为机器人的控制器,Pcduino更加轻便,性价比更高,可通过移动电源 供电,开发者可直接进行程序的编写、编译,便于用户开发。
(4)地面工作站:在本发明中采用地面工作站来作为算法运行站,集中处理收到的来自每 个无人机和地面机器人的位置信息,经过相应的编队算法处理之后,由控制器给出每一个机 器人当前的动作和行为。本发明中无人机和地面机器人的任务分配与协同控制主要在地面工 作站完成,集中式处理有效降低了无人机与机器人的计算负荷,仅需要无人机和机器人的控 制器执行较为简单的指令,降低了控制难度,提高了整个系统的稳定性和可靠性。
(5)编队控制算法:本发明提出了一种基于势场函数的无人机与地面机器人协同编队控制 算法,通过设计地面机器人势场力函数和无人机势场力函数,可以有效且持久的维持无人机 与地面机器人协同编队队形。
附图说明:
附图1无人机与地面机器人协同编队系统。
附图2基于WIFI的通信网络构建。
附图3ROS的通信机制。
附图4整体平台运行结构。
附图5地面工作站功能结构。
附图6无人机编队飞行实现效果图。
附图7、8地面机器人队形的切换。
具体实施方式
本发明涉及无人机与地面机器人协同编队领域、无线数据传输通信领域、嵌入式开发技 术领域,设计实现了无人机与地面机器人协同编队系统,解决了异构多机器人之间的通信问 题,为空地协同编队算法的写入提供了硬件平台。
针对现有技术存在的问题,本发明旨在提供一种可扩展性好、稳定性强,并且能够支持 无人机与地面机器人平台协同编队的系统,使得研究者可以通过该系统对空地协同编队算法 进行实物仿真与实验验证。在此基础上,本发明提出了一种基于势场函数的无人机与地面机 器人协同编队控制算法,可实现无人机与地面机器人的空地协同编队。本发明有利于对相关 算法进行反复的参数修正与算法改进,有效提高了无人机与地面机器人协同编队等理论研究 与应用开发的效率。
本发明提出了一种无人机与地面机器人协同编队系统,该系统主要包括三个部分:无人 机平台,地面移动机器人平台以及地面工作站。
无人机编队包括多架多种类型的四旋翼无人机,每架无人机均可搭载传感器设备和WiFi 模块。机载控制器实时运行飞行控制算法,负责协调和指挥无人机的所有行为。其中,无人 机的姿态信息通过机载惯性测量单元获取,位置信息由室内定位系统来提供。室内定位系统 在实时解算无人机当前位置后,将位置信息发送至地面工作站。
地面移动机器人编队由具有自主移动功能的机器人构成,可以搭载载荷,每个机器人上 分别搭载具有WiFi模块的控制器,并具备实时反馈位置信息的能力。
地面工作站是一台具有较强运算能力的计算机,负责接收无人机和地面机器人发送的姿 态信息与位置信息,并实时运行无人机与地面机器人协同编队算法。
本发明提出的无人机与地面机器人空地协同编队的实现方法为:
首先,基于WiFi建立无线通信网络,利用基于TCP/IP协议的ROS节点通信技术,以订 阅和发布话题的形式收发数据,实现无人机、多地面移动机器人与工作站三者之间的实时信 息交互。
然后,用于编队的无人机与地面机器人需要具备的最基本的条件是清楚其在所处环境中 的位置,自身位置的确定是无人机与地面机器人协同编队的关键技术。本发明中通过室内定 位系统根据无人机上的标记点来计算其位置,进而发送给地面工作站;地面机器人利用自身 里程计对航迹预测并利用陀螺仪校正,获得位置信息再发送给地面工作站。地面工作站采用 Ubuntu操作系统,运行无人机与地面机器人协同编队算法,计算出整个编队内每个单元的期 望位置,并发送给相应单元。
最后,无人机和地面机器人的控制器分别运行相应的机载控制算法,进而驱动无人机和 地面机器人移动到求解出的期望位置,形成无人机与地面机器人编队。
此外,本发明提出了一种基于势场函数的无人机与地面机器人协同编队控制算法,可实 现无人机与地面机器人的空地协同编队。该算法主要分为三个部分:定义势场函数,设计地 面机器人势场力函数和设计无人机势场力函数。第一部分:首先定义势场函数,定义约束相 邻的两个机器人i和j的势场函数,势场力为势场函数的梯度函数;第二部分:设计地面机 器人的势场力函数,地面机器人分为普通跟随者和第一跟随者两类;第三部分:结合地面机 器人的位置信息设计无人机的势场力函数。最后根据势场函数确定每个地面机器人和无人机 所受到的势场力合力,控制机器人和无人机的速度输出,实现无人机和地面机器人协同编队。
下面结合附图对本发明做进一步描述。
无人机与地面机器人协同编队系统组成图参见图1。整个系统由无人机、地面机器人, 地面工作站,室内定位系统和无线通信装置组成。
本发明的无人机编队平台由3架四旋翼无人机直升机组成,采用Pixhawk作为机载控制 器,Pixhawk的软件与硬件全部开源,且内置mavlink通信协议。通过美国NaturalPoint公 司的Optitrack室内定位系统,捕捉无人机上安装的反射标记点实时精确地计算出无人机的 位置信息和姿态信息,进而通过TCP/IP协议将信息发送给需要的计算机。
本发明的地面移动机器人编队平台由4个移动机器人组成。本发明选择双轮差速机器人 作为机器人移动底座,采用迷你嵌入式计算机Pcduino作为机器人的控制器来运行机器人的 控制中枢。目前市面上的移动机器人开发平台有些在位置估计上不够完善,需要我们额外的 去添加编码器和陀螺仪等设备去做位置估计,有些体积上比较大价格也比较昂贵。所以在机 器人的移动底座上我们选用的是韩国Yujin robot生产的Kobuki低成本双轮差速底座, Kobuki通过里程计提供位置信息,利用陀螺仪对编码器得到的里程计信息进行修正以保证里 程计的准确性,该移动底座还提供了外部计算机的控制接口使其可以方便的对执行器进行控 制并采集其内置的传感器信息,进而可以通过WiFi以TCP/IP协议格式发送给其他需要这些 信息的计算机。控制器是地面机器人的核心部件,它承担着机器人上的所有计算任务,包括 采集数据,实时控制及无线通信等。考虑到机器人的运行时间和笔记本电池的续航能力有限, 为了减轻机器人的载重负担,本发明选用嵌入式迷你计算机Pcduino V3B作为机器人的控制 器,与利用计算机作为机器人的控制器相比,Pcduino更加的轻便、性价比更高,通过5V/2A 的移动电源供电就可以具备良好的续航能力。与其他需要剪裁内核、交叉编译的嵌入式控制 器不同的是,Pcduino直接搭载Ubuntu 12.04操作系统,可以直接进行程序的编写、编译, 便于用户进行开发。在Pcduino中完成的软件功能有:装载ROS机器人操作系统、机器人信 息交互、机器人运动学控制算法、编队算法的写入等。
基于WIFI的通信网络构建示意图参见图2。本发明设计搭建的平台选用的通信方式是基 于WIFI建立通信网络,之后利用基于TCP/IP协议的ROS节点通信机制来实现无人机、机器 人、以及地面工作站之间的信息交互。
发明中基于ROS的通信机制参见图3。在ROS中节点(Node)是主要的计算执行进程,是 实现一个独立任务的基本单元,一个节点可以为传感器信息的获取、算法生成或控制指令的 发送等独立功能;所有节点都在节点管理器(Master)的管理下运行相应的任务,节点管理器 用于节点的名称注册和查找等;节点之间通过消息(Message)传递数据信息完成彼此的沟通, 消息包含一个节点发送到其他节点的数据信息;节点之间通过话题(Topic)传递消息,话题是 由ROS网络对消息进行路由和消息管理的数据总线。图3中Publisher与Subcriber分别是 运行在ROS系统中的两个节点,Publisher对外advertise一个”cmd/vel”话题,ROS Master 会根据publisher这个advertise请求,来寻找相应的订阅节点,这里找到的是Subcriber 节点,因为他正在订阅‘cmd/vel’这个话题,接下来Master会将这两个节点基于TCP/IP协 议进行衔接,进行Twist消息类型的传递。
整体系统运行结构图参见图4。在WiFi无线通信网络搭建好的条件下,室内定位系统将 无人机编队的位置信息发送给地面工作站,每个无人机上均设有飞行控制器,飞行控制器将 无人机的姿态信息反馈给地面工作站,地面工作站收到无人机和地面机器人的位置信息和姿 态信息后,通过实时地运行协同编队算法生成每个成员相应的航线轨迹、速度等指令,并将 其发送给对应的无人机和地面机器人,相应的无人机和地面机器人控制器生成相应的行动指 令控制无人机和地面机器人按照算法求解出的速度移动到相应的位置。
地面工作站功能结构图参见图5。地面工作站与无人机编队和地面机器人编队通过无线 通信连接,计算机上装有Ubuntu操作系统,在Ubuntu环境中的终端窗口通过输入命令行下 载安装并运行ROS操作系统。地面工作站的主要功能有接收来自无人机和地面机器人以及定 位系统的数据,显示无人机的状态信息,运行无人机与地面机器人协同编队控制算法,生成 相应的控制指令。每个功能分别是一个独立的节点(Node),它们在ROS环境中运行,以“发 布/订阅”的形式完成彼此之间数据的交互。
其中,本发明提出了一种基于势场函数的无人机与地面机器人协同编队控制算法。可实 现一架无人机与3架地面机器人的协同编队。该算法主要分为三个部分:定义势场函数,设 计地面机器人势场力函数和设计无人机势场力函数。
第一部分:定义势场函数。定义约束相邻的两个机器人i和j的势场函数为Uij(qi),对 于任意位姿,Uij(qi)为可微分的、并且是非负的。满足机器人i相对于机器人j的约束条件 的势场函数为
其中η为正比例增益系数,qi,qj为节点的位置,ρij=||qi-qj||为节点i与节点j之间的实 际欧式距离,而ρ0为期望的距离。该势场函数特点如下:
1、当ρij→∞时,Uij(qi)→∞;
2、当ρij=ρ0时,Uij(qi)取最小值。
按照势场力的定义,势场力为势场函数的梯度函数,如式(2)所示:
其中,为qi指向qj的单元向量,
容易看出,势场力函数在ρij<ρ0时为负,机器人之间关系体现为斥力作用;势场力函数 在ρij>ρ0时为负,机器人之间关系体现为引力作用;势场力函数在ρij=ρ0时为负,机器人距 离达到稳定状态。
第二部分:地面机器人势场力函数设计。地面机器人分为第一根随着和普通跟随者。对 于普通机器人跟随者Vi,Vi需要同时跟随两个机器人Vj和Vk,即与Vj和Vk之间的距离||qi(t)-qj(t)||与||qi(t)-qk(t)||需要保持在相应的期望距离ρ0ij、ρ0ik,在任意时刻t>0,Vi的期 望位置即以Vj、Vk两为圆心,以ρ0ij、ρ0ik为半径的两个圆的交点。为使Vi达到该点,我们以 Vj、Vk为势能中心,以公式(1)(2)所述势场函数建立势场,则Vi所受到的势场力合力为:
其中η、ξ为相应的正比例增益,ρij、ρik为Vi与Vj、Vk之间的实际距离,ρ0ij、ρ0ik为 相应的期望距离,为Vi指向Vj、Vk单位向量。
然后,假设领航机器人为V1,第一跟随者为V2,V2仅需满足与V1的距离约束即可,即其 运动轨迹为V1以为圆心、以期望距离ρ012为半径的圆。V2与V1的距离||q1(t)-q2(t)||始终保持为 期望距离ρ012,我们据此对第一跟随者V2设定如下势场力函数:
其中η为正比例约束系数,ρ12为V2与V1之间的实际距离,ρ012为期望距离。为由V2指向V1的单位向量。
第三部分:无人机势场力函数设计。结合地面机器人位置信息构造势场力函数如下:
式中,α,β,γ为正比例系数,dcl,dcj,dci分别为无人机Vc与三个机器人的实际距离,lcl,lcj,lci分别为无人机Vc与三个机器人的期望距离。最后根据势场函数确定每个地面机器人和无人机 所受到的势场力合力,控制机器人和无人机的速度输出,实现无人机和地面机器人协同编队。
无人机编队飞行实现效果图参见图6。图中演示的为2架四旋翼无人机进行编队飞行。 由地面工作站运行控制算法控制两架无人机起飞,然后无人机控制器控制无人机到给定的期 望位置。图中显示的为两架无人机编队先悬停,然后按照顺时针方向以圆形轨迹飞行。
机器人编队行驶与机器人队形的切换实现效果图参见图7、图8。图7为机器人Leader 机器人带领三个机器人行驶的效果示意。机器人在行进过程的同时可以通过上位机手动或者 用程序进行自动切换队形,切换程序自行运行一个节点,对正在行驶的机器人编队发布切换 指令。图8即为机器人在编队运行中可以根据给定的队形进行切换,在跟随Leader机器人的 过程中,可以同时进行编队队形的切换。
无人机与地面机器人协同编队,通过地面工作站实时运行本发明提出的无人机与地面机 器人协同编队控制算法,然后通过WiFi向无人机和地面机器人发送解算出的期望位置信息, 无人机和地面机器人实时接收地面站传过来的位置信息,最终实现一架无人机与三个机器人 协同编队。图中白色绳子为保护绳,仅仅起到保护作用。