一种基于机器人操作系统的无人车编队实验平台

摘要

本发明公开了一种基于机器人操作系统的无人车编队实验平台，包括定位模块、通信模块、编队控制模块以及避障模块，定位模块获取各个无人车本体的实时位置信息，通信模块用于无人车本体与主控端之间以及无人车本体之间的实时信息交互，编队控制模块根据实时位置信息，生成无人车本体编队的行进切换指导信息，并通过通信模块下发至相应的无人车本体，避障模块用于根据预先采集到的激光点云信息以及无人车本体编队的行进切换指导信息解算得到避障信息，并通过通信模块发送至无人车本体进行避障。该无人车编队实验平台在保证定位能力和协同通信能力的基础上，同时实现了编队控制和自主避障等功能，更能满足无人车的编队控制需求。

权利要求

1.一种基于机器人操作系统的无人车编队实验平台，其特征在于，包括：定位模块，所述定位模块用于获取各个无人车本体的实时位置信息；

通信模块，所述通信模块用于无人车本体与主控端之间以及无人车本体之间的实时信息交互；

编队控制模块，所述编队控制模块用于根据所述定位模块获取的实时位置信息，生成无人车本体编队的行进切换指导信息，并通过所述通信模块将所述无人车本体编队的行进切换指导信息下发至相应的无人车本体；以及避障模块，所述避障模块用于根据预先采集到的激光点云信息以及所述编队控制模块生成的无人车本体编队的行进切换指导信息解算得到避障信息，并通过所述通信模块将所述避障信息发送至无人车本体进行避障；

所述避障模块包括激光雷达和避障数据处理单元；

所述激光雷达安装于所述无人车本体上，所述激光雷达用于采集无人车本体的激光点云数据，并将所述激光点云数据发送至所述避障数据处理单元，所述避障数据处理单元用于接收所述激光点云数据，并获取所述编队控制模块生成的无人车本体编队的行进切换指导信息，根据所述激光点云数据计算得到避障方向和初始避障速度，并将所述初始避障速度与所述无人车本体编队的行进切换指导信息中编队速度进行整合，获得最终的避障速度；

所述初始避障速度包括避障线速度和避障角速度；

所述避障线速度的计算公式为：

所述避障角速度的计算公式为：

其中，kv、kω均为预设避障系数，vmax为避障线速度的上限值，ωmax为避障角速度的上限值，dmin为激光雷达扫描到的最小距离，r为无人车本体的半径，r1为避障半径，θ为避障速度的矢量方向。

2.根据权利要求1所述的一种基于机器人操作系统的无人车编队实验平台，其特征在于，还包括人机交互模块，所述人机交互模块用于通过所述通信模块将主控端的高层级指令派发至相应的无人车本体。

3.根据权利要求1所述的一种基于机器人操作系统的无人车编队实验平台，其特征在于，所述定位模块采用UWB导航定位系统。

4.根据权利要求3所述的一种基于机器人操作系统的无人车编队实验平台，其特征在于，所述定位模块包括基站、控制台和标签，所述基站设有四个，所述控制台与主控端连接，所述标签与无人车本体连接，所述主控端通过所述控制台获取所述基站的定位坐标，所述标签用于获取所述无人车本体的位置信息。

5.根据权利要求1所述的一种基于机器人操作系统的无人车编队实验平台，其特征在于，所述通信模块采用网络交换机或局域网路由器。

6.根据权利要求1所述的一种基于机器人操作系统的无人车编队实验平台，其特征在于，所述无人车本体包括一个领导者无人车以及多个跟随者无人车。

7.根据权利要求6所述的一种基于机器人操作系统的无人车编队实验平台，其特征在于，所述编队控制模块针对所述领导者无人车的控制协议为：其中， 表示初始时刻给定的领导者无人车的编队机动速度或通过主控端实时发送的速度指令；

针对所述跟随者无人车的控制协议为：

其中， 分别表示第i个跟随者无人车和领导者无人车的二维位置坐标， 表示第i个跟随者无人车在期望编队形状中相对于领导者无人车的相对二维位置坐标。

说明书

一种基于机器人操作系统的无人车编队实验平台

技术领域

[0001] 本发明涉及集群系统协同控制技术领域，更具体的说是涉及一种基于机器人操作系统的无人车编队实验平台。

背景技术

[0002] 目前，现有的协同集群系统或集群平台主要分为两种类型，以无人车协同控制为例，一种是对主控、传感器、定位、通信等模块进行设计，实现无人车集群的协同通信，但并未考虑无人车的协同编队、导航避障等方案；另一种是针对单个模块设计新型的方法，对基于无人车编队的定位、跟踪、通信等方法进行优化，但并未考虑整体的协同控制或任务。不难发现，现有的方案无法兼顾协同控制、协同编队、导航避障等方案，导致集群平台的控制效果并不理想。

[0003] 因此，如何提供一种兼顾协同控制、协同编队、导航避障等方案的无人车编队实验平台是本领域技术人员亟需解决的问题。

发明内容

[0004] 有鉴于此，本发明提供了一种基于机器人操作系统的无人车编队实验平台，该无人车编队实验平台有效解决了现有的方案无法兼顾协同控制、协同编队、导航避障等方案，导致集群平台的控制效果并不理想的技术问题。

[0005] 为了实现上述目的，本发明采用如下技术方案：

[0006] 一种基于机器人操作系统的无人车编队实验平台，包括：

[0007] 定位模块，所述定位模块用于获取各个无人车本体的实时位置信息；

[0008] 通信模块，所述通信模块用于无人车本体与主控端之间以及无人车本体之间的实时信息交互；

[0009] 编队控制模块，所述编队控制模块用于根据所述定位模块获取的实时位置信息，生成无人车本体编队的行进切换指导信息，并通过所述通信模块将所述无人车本体编队的行进切换指导信息下发至相应的无人车本体；以及

[0010] 避障模块，所述避障模块用于根据预先采集到的激光点云信息以及所述编队控制模块生成的无人车本体编队的行进切换指导信息解算得到避障信息，并通过所述通信模块将所述避障信息发送至所述无人车本体进行避障。

[0011] 本发明的无人车编队实验平台，主要基于ROS(Robot Operating System，机器人操作系统)实现，可以对无人车本体进行定位、通信、编队、避障、控制和交互等操作。

[0012] 进一步地，上述的一种基于机器人操作系统的无人车编队实验平台还包括人机交互模块，所述人机交互模块用于通过所述通信模块将主控端的高层级指令派发至相应的无人车本体。

[0013] 进一步地，所述定位模块采用UWB导航定位系统。UWB(Ultra Wideband)导航定位系统主要利用无载波通信技术实现无人车本体的导航定位，可以满足无人车编队实验平台的导航定位需求。

[0014] 更进一步地，所述定位模块包括基站、控制台和标签，所述基站设有四个，所述控制台与主控端连接，所述标签与无人车本体连接，所述主控端通过所述控制台获取所述基站的定位坐标，所述标签用于获取所述无人车本体的位置信息。

[0015] 进一步地，所述通信模块采用网络交换机或局域网路由器。通信模块作为通信链路支持无人车本体与主控端之间以及无人车本体相互之间的各种通信需求，如高层级的编队指令和低层级的实时位置信息等。

[0016] 进一步地，所述无人车本体包括一个领导者无人车以及多个跟随者无人车。由于本发明中编队过程会涉及两个角色，即领导者和跟随者，两个角色各自的任务不同，领导者能够接收主控端的指令再通过局域网通信下发给其跟随者，而跟随者在接收处理这些指令的同时要能够跟踪目标队形，实现协同。本发明中设置一个领导者无人车本体和多个跟随者无人车本体，可以满足无人车编队实验平台的编队需求。

[0017] 进一步地，本发明中编队控制模块主要通过编队控制算法实现对无人车编队控制，该过程中编队形成和保持的核心是一阶的一致性协议。

[0018] 针对所述领导者无人车的控制协议为：

[0019]

[0020] 其中， 表示初始时刻给定的领导者无人车的编队机动速度或通过控制端实时发送的速度指令；

[0021] 针对所述跟随者无人车的控制协议为：

[0022]

[0023] 其中， 分别表示第i个跟随者无人车和领导者无人车的二维位置坐标，表示第i个跟随者无人车在期望编队形状中相对于领导者无人车的相对二维位置坐标。

[0024] 进一步地，所述避障模块包括激光雷达和避障数据处理单元；

[0025] 所述激光雷达安装于所述无人车本体上，所述激光雷达用于采集无人车本体的激光点云数据，并将所述激光点云数据发送至所述避障数据处理单元，所述避障数据处理单元用于接收所述激光点云数据，并获取所述编队控制模块生成的无人车本体编队的行进切换指导信息，根据所述激光点云数据计算得到避障方向和初始避障速度，并将所述初始避障速度与所述无人车本体编队的行进切换指导信息中编队速度进行整合，获得最终的避障速度。

[0026] 为了满足最好的安全性，本发明中避障模块采用势函数避障的方式。根据点云信息计算初始避障速度，然后和编队控制模块的编队速度进行结合，计算得到最终的速度输出。

[0027] 进一步地，所述初始避障速度包括避障线速度和避障角速度；

[0028] 所述避障线速度的计算公式为：

[0029]

[0030] 所述避障角速度的计算公式为：

[0031]

[0032] 其中，kv、kω均为预设避障系数，vmax为避障线速度的上限值，ωmax为避障角速度的上限值，dmin为激光雷达扫描到的最小距离，r为无人车本体的半径，r1为避障半径，θ为避障速度的矢量方向。

[0033] 经由上述的技术方案可知，与现有技术相比，本发明公开提供了一种基于机器人操作系统的无人车编队实验平台，该无人车编队实验平台通过定位模块、通信模块、编队控制模块以及避障模块间的配合工作，在保证定位能力和协同通信能力的基础上，同时实现了编队控制和自主避障等功能，更能满足无人车的编队控制需求。

附图说明

[0034] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

[0035] 图1为本发明提供的一种基于机器人操作系统的无人车编队实验平台的结构架构示意图；

[0036] 图2为本发明实施例中定位模块的结构架构示意图；

[0037] 图3为本发明实施例中编队控制过程中两种角色间交互过程示意图；

[0038] 图4为本发明实施例中无人车之间及与主控端间的通信机制示意图；

[0039] 图5为本发明实施例中编队控制过程的实现流程示意图；

[0040] 图6为人机交互模块的实现原理示意图。

具体实施方式

[0041] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

[0042] 参见附图1，本发明实施例公开了一种基于机器人操作系统的无人车编队实验平台，包括：

[0043] 定位模块1，定位模块1用于获取各个无人车本体的实时位置信息；

[0044] 通信模块2，通信模块2用于无人车本体与主控端之间以及无人车本体之间的实时信息交互；

[0045] 编队控制模块3，编队控制模块3用于根据定位模块1获取的实时位置信息，生成无人车本体编队的行进切换指导信息，并通过通信模块2将无人车本体编队的行进切换指导信息下发至相应的无人车本体；以及

[0046] 避障模块4，避障模块4用于根据预先采集到的激光点云信息以及编队控制模块生成的无人车本体编队的行进切换指导信息解算得到避障信息，并通过通信模块2将避障信息发送至无人车本体进行避障。

[0047] 更优地，上述的一种基于机器人操作系统的无人车编队实验平台还包括人机交互模块5，人机交互模块5用于通过通信模块2将主控端的高层级指令派发至相应的无人车本体。

[0048] 具体地，本实施例中定位模块1采用一种基于UWB技术的PNTC(具有定位、导航、授时与通信功能)局部定位系统Link Track。利用其LP(即局部定位)模式，在该模式下分为TAG(即标签)、ANCHOR(即基站)、CONSOLE(即控制台)三种角色。参见附图2，首先在实验环境中布置4个基站(即瞄点A0‑A3)，然后将控制台通过USB串口连接到电脑C，利用NAssistant软件中的一键标定功能刷新标定4个基站A0、A1、A2、A3的定位坐标，确定一个三维坐标系，本实施例中四个基站与电脑通过局域网路由器进行数据交互，再将多个标签T利用USB串口分别连接到多个无人车本体搭载的树莓派系统中，可以在软件NAssistant中实时读取所有标签的位置信息，并且每个标签都会实时发布自己的定位坐标、三轴角速度和加速度等信息，可以在每个无人车本体搭载的树莓派系统上运行Link Track ROS开源驱动包，即可实时订阅其位置信息。

[0049] 该定位模块1主要功能是借助基站和控制台来实现标签运动信息的测量、传输等，从而实现每个无人车对于全局运动量的获取。对于定位系统来说，定位精度约为5‑10cm，能够满足无人机编队实验平台的定位需求。

[0050] 对于通信模块2，无人车本体之间的通信本质上依赖局域网建立，通过一台交换机或局域网路由器进行通信，进行主从设置后无人车本体间可以完成互相的识别和信息传输。

[0051] 参见附图3和图4，本实施例采用ROS主从配置实现无人车本体之间的实时通信。首先将所有的无人车本体都连入同一个局域网下，在该项目中，设置领导者无人车(附带标签为T0)为ROS主机，其余跟随者无人车(附带标签为T1,T2,…)为ROS从机，分别在每一个无人车本体上的树莓派系统中订阅任意无人车本体上的标签位置信息，即可实现任意无人车本体之间的实时通信。

[0052] 该无人车编队实验平台搭建的核心基础在于机器人操作系统，借助于机器人操作系统的分布式通信能力，可实现任意的通信拓扑，并能实现无人车本体之间的信息传递、交换和共享等。同时，ROS节点通信的架构可以优化系统的结构，每个无人车只需启动少量的几个节点即可，主要是基础配置节点，包含自身bring_up节点用于初始化，link track节点、rplidar节点等；以及编队控制节点，包含自身作为Leader或Follower的节点，避障节点和键盘控制节点等。另外还有一些需要的静态坐标变换等。

[0053] 对于编队控制模块3，参见附图3，首先确定编队中的两个角色，即领导者和跟随者，两个角色要执行不同层级的任务，比如领导者接收主控端的指令再通过局域网通信下发给其跟随者，而跟随者在接收处理这些指令的同时要能够跟踪目标队形，实现协同。

[0054] 同时，为了后期开发方便，本实例把编队队形部分单拿出来，直接在其中添加想要的队形即可。

[0055] 在硬件需求上，由于编队方案中需要领导者和跟随者的信息交互，因此实际使用要依托于定位模块1(即用基站和固定于无人车本体上的标签获取位置信息)和通信模块2(即通过交换机交换位置信息)。该部分依赖于link track定位系统和交换机通信方案共同完成，是在此二者基础上的高层级控制方案。

[0056] 本实施例中编队形成和保持的核心是一阶的一致性协议。具体来说，领导者要广播自身的位姿，而跟随者根据领导者和自身的相对位置以及编队中两者期望的相对位置来进行误差反馈，此误差作为目标速度来让底盘进行跟踪，从而实现任意初始位置的编队形成和保持。

[0057] 在编队节点中，都要完成自身位姿信息的获取和发送，以及控制速度的计算并发送给底盘进行跟踪。领导者还要向下和各个跟随者发送命令。而跟随者则要接收命令并进行处理和反馈。

[0058] 代码实现上，主要是领导者Leader、跟随者Follower、编队队形formation_dict这三个模块。

[0059] 本实施例采用的编队原理具体如下：

[0060] 模型： 其中pi∈R2,ui∈R2

[0061] 其中pi∈R2表示无人车本体在全局坐标系下的二维位置坐标，ui∈R2表示本体无人车的速度控制矢量。

[0062] 由于本实施例采用的是基于领导者‑跟随者的编队控制方法，

[0063] 针对领导者的控制协议为：

[0064]

[0065] 其中 表示初始时刻给定的领导者无人车的编队机动速度，也可以是后文人机交互方案中提到的通过控制端实时发送的速度指令。并且本实施例中规定只有一个领导者无人车(即第一个无人车)。

[0066] 针对跟随者的控制协议为：

[0067]

[0068] 其中， 分别表示第i个跟随者无人车和领导者无人车的二维位置坐标，表示第i个跟随者无人车在期望编队形状中相对于领导者无人车的相对二维位置坐标。

[0069] 参见附图5，启动节点后分别对领导者无人车Leader和跟随者无人车Followers进行定位，领导者无人车发出其位姿信息、速度信息以及编队指令，跟随者无人车发出自身位姿、抵达信息以及底层速度信息，并进行避障，领导者无人车接收主控端下发的控制指令，并进行独立运动，跟随者无人车接收编队指令，跟随领导者无人车形成编队，进而实现编队机动。

[0070] 具体地，避障模块4包括激光雷达和避障数据处理单元；

[0071] 激光雷达安装于无人车本体上，激光雷达用于采集无人车本体的激光点云数据，并将激光点云数据发送至避障数据处理单元，避障数据处理单元用于接收激光点云数据，并获取编队控制模块生成的无人车本体编队的行进切换指导信息，根据激光点云数据计算得到避障方向和初始避障速度，并将初始避障速度与无人车本体编队的行进切换指导信息中编队速度进行整合，获得最终的避障速度。

[0072] 为了提高避障环节的安全性，本实施例中避障模块4采用势函数避障的方式。根据点云信息计算避障速度，然后和编队部分的编队速度进行结合，算得最终速度输出。定义了几个相关的参数，如小车自身半径，触发避障距离等等，以此来设计出了一个较为适合该无人车编队平台的避障势函数。

[0073] 硬件依赖方面，避障方案需要每个小车搭载一个2D激光雷达采集激光点信息，将数据传入无人车搭载的树莓派系统，通过避障算法实现对于激光点信息的计算处理，进而实现避障功能。

[0074] 具体地，避障节点订阅了激光雷达的点云信息并进行计算处理，首先对这些点云进行筛选，去掉太远的点(由于无局部参考价值)，然后每一个点以其距离的倒数做权值来对其角度进行加权求和，从而求得最终合理的避障方向，而避障速度大小则仅由距离最近的障碍点决定。这样在一次对点云的遍历中便可完成避障速度求解。由于雷达点云数据较为庞大，对其的处理占用计算资源，这样可以简洁高效的完成避障任务。

[0075] 本实施例中避障算法主要求解避障速度矢量方向θ，算法程序的执行过程具体如下：

[0076] 初始化，预设避障系数kv、kω，无人车自身半径r，避障半径r1、r2，雷达每圈点数N，避障速度限制vmax、ωmax。

[0077] While True：

[0078] 收到雷达Laser Scan类型消息(角度起点θ0，角度增量δθ，距离di)后：

[0079] 计算并记录雷达扫描到的最小距离dmin。

[0080] if dmin＜r1：

[0081] for每个雷达点pi，if距离di满足r＜di＜r2：

[0082]

[0083]

[0084]

[0085] θ＝θ/d+π

[0086]

[0087]

[0088] else：

[0089] v＝0

[0090] ω＝0

[0091] 避障线速度v、角速度ω即为所求。

[0092] 避障算法的目标是高效给出可行的避障速度指令，算法中应用了人工势场法的思想，但是把大小和方向分开考虑。其主要特点为：

[0093] 1、仅需一次遍历即可求得避障速度，突出了高效求解的特点；

[0094] 2、避障速度v与编队速度做矢量和，能够和编队控制模块协同。

[0095] 对于人机交互模块5，参见附图6，由于需要人和小车之间的交互，主要是主控端通过键盘、语音发送一些速度指令或者编队指令，小车返回一些重要的数据供控制端查看等，实际应用过程中还可以加入手势、语音等指令交互，这里考虑存在需要控制全部无人车和仅控制领导者无人车的情况(后者是实际编队实验的情况)，因此可在不同需求时选择开启不同的控制节点，分别是teleop_twist_keyboard和keyboard_leader等。

[0096] 另外，对于关键信息的读取比如定位信息、里程计信息和雷达点云信息等，对上述信息的读取有助于进一步的分析利用(如建图)和更好的实现控制。

[0097] 对于硬件层面，整个编队实验平台的控制主体是无人车，无人车本体上搭载树莓派3控制器、已配好的PIBOT‑STM32主控板、双电机组成差速轮、6000mah锂电池(续航时间约3‑4小时)、激光雷达(主要用于编队中的避障，还可以开发建图SLAM)等。本实施例中树莓派系统采用Ubuntu MATE，方便在Ubuntu中开发后的移植。

[0098] 其他的辅助设备还有用于完成局域网建立的路由器或交换机，以及一台移动电源，可以为路由器和控制端的电脑设备供电，同时在无人车电量无法完成续航时可以备用充电。

[0099] 本发明实施例公开的上述无人车编队实验平台，在保证定位能力和协同通信的基础上，同时实现了编队形成、编队行进、切换编队、自主避障等功能；并且，基于ROS的模块化设计，便于对各个模块的算法开发和再设计。

[0100] 对于编队形成过程，多个无人车本体可以由任意初始位置，根据编队指令，形成期望的编队形状。

[0101] 对于行进间编队与切换队形过程，多个无人车本体由任意初始位置，根据编队指令，形成期望的初始编队形状，并保持队形前进；收到切换队形指令后，边行进边切换至目标编队形状。

[0102] 对于编队行进并避障过程，多个无人车本体由任意初始位置，根据编队指令，形成期望的编队形状，并保持队形前进；遇到障碍物时，每辆无人车实现自主避障；通过障碍物后恢复原有编队形状。

[0103] 本实施例公开的无人车编队实验平台中每个模块完成指定的功能，从而在整体上达到多机层级的编队和避障效果。该无人车编队实验平台，利用ROS模拟实际通信状况，能实现编队形成、切换、行进等，且具备静态障碍物避障和车间避障的能力，能实现各种情景下的编队越障。

[0104] 一般的无车人编队实验流程为：

[0105] (1)无线网络配置、主从设置(仅首次或更改网络配置后需要)；

[0106] (2)无人车本体上电开机；

[0107] (3)设置实验场地，并调试UWB导航定位系统(主要是四个基站的摆放，首次实验或基站相对位置改变后需要校准)；

[0108] (4)确定信息获取成功(主要是link track定位信息和自身底盘里程计信息)[0109] (5)从键盘控制端发布速度指令或编队指令，进行实验。

[0110] 本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

[0111] 对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。