基于UWB局部测量的集群协同平台实现

孙大洋，白恩浩，李红阳，岳书宁

（吉林大学通信工程学院，长春 130012）

1 引 言

随着通信、微电子和人工智能等技术的不断突破，单体智能发展迅速，集群协作将变得十分重要，它可以有效提高整体工作效能，实现单体性能到群体性能的提升，进而实现群体智能。

雷诺兹（Reynolds）在1986年首次提出一个仿真生物群体行为模型BOID[1]，之后科研人员在集群技术领域展开了多年的研究，已经取得了可喜的进展。国内外对群体智能技术的研究侧重于整体性研究，主要针对集群结构框架、控制与优化技术、任务管理与协同等开展研究工作。如美国国防部高级研究计划局主导的自主编队混合控制项目（MICA），对协同任务分配、协同路径规划、混合主动与自主编队控制、协同跟踪、信息共享等有关无人机集群的技术进行了全面的研究。国内对无人机系统的协同感知与信息共享、路径实时规划、自主编队与编队重构、智能协同决策等研究较为深入。集群中融合了视觉技术[2]、UWB 技术[3-4]、激光雷达技术[5]及GPS RTK 技术等，在理论仿真与实机验证方面均有所突破[6-7]。中国电子科技集团、清华大学泊松科技已经开展了集群飞行的实践研究，且不完全依赖地面站[8-9]。另外，近年来，采用GPS RTK 技术进行的无人机灯光秀屡见报端，可实现上千架无人机的飞行表演。

然而，面对群体智能的实际应用，群体智能技术的未来发展还要解决研发过程中的技术挑战问题，主要体现在集群的自主性、协同性和算法智能性等方面[10-11]。对于集群技术而言，智能单体间的信息共享是集群技术至关重要的一环。充分利用网络节点资源实现信息共享的集群协同技术[12-13]能够优化系统性能，尤其适用于无中心的无线通信网络。自主性与协同性是群体智能的基础，在单体自主已经得到飞速发展的情况下，群体自主与协同成为群体智能发展的关键技术之一。而无论是自主性还是协同性，协同定位技术[14]作为一项集群的基础服务技术都是不可忽视的关键技术之一，面对未来群体智能的趋势需求，目前的群体智能在协同定位技术上主要还存在如下两个方面的问题。

（1）面向实战的群体智能协同关键技术的研究并不多见，尤其是GPS/北斗拒止条件下，如何不依赖基础设施进行位置协同尚待研究。

（2）集群尚未真正群体智能化，以地面站作为主控的方式进行多机协同，本质上还没有脱离对传统集群技术的依赖，去中心化群体智能协作技术还有待进一步研究。

上述问题是群体智能协同技术的关键问题，从技术的角度看，群体自主与协同需要单体间具有良好的自主感知能力，减小对集中调度的依赖。而实现高精度的位置协同是群体智能协同的基础[15]，群体智能中的智能单体需要通过视频、通信、激光雷达等技术手段实现自定位功能，具有障碍感知能力和决策能力来规避碰撞，最大限度地实现单体运动位置的自主决策。

针对以上问题，本文从集群协同平台实现的角度，以UWB 自定位为位置协同基础，进行集群协同平台设计，提出分布式方案以解决集群中的位置协同问题。目的在于使用UWB 技术搭建集群协作平台，探究实现群体智能的新方法，完善群体智能理论，扩展群体智能的应用范围。

2 集群协同平台系统架构

本文针对集群协同技术中的自定位与去中心化问题，提出并设计了基于UWB 局部测量的集群协同平台，并进行了实机验证。平台由12 辆无人单体车组成，单体车采用ROS 对自身进行控制，单体车之间通过UWB 节点进行通信与位置协同，当集群系统收到目标识别指令后，无人单体车会自主识别目标，根据单体的当前位置和目标点位置来解算出单体的跟随路径。

2.1 集群协同平台软硬件体系

集群协同平台的硬件层面由各单体车组成，移除了地面站，实现了集群协同平台的去中心化。单体车的硬件层面由运动控制部分、主控制部分、通信测距部分（通信测距模块、辅助测距模块）三大部分组成。运动控制部分由电机驱动模块、MPU6050、磁力计、测速模块、红外避障模块组成，控制芯片为STM32F103RCT6；主控制部分由外部遥控模块、树莓派4B 组成；通信测距部分由两个UWB Node 组成，不依赖系统之外的基础设施，无须地面站辅助。

在硬件设计的基础上，本文对无人车单体进行了集成，单体车安装有底板驱动及ROS 系统板，预留扩展口，可扩展附加功能（图1 右）。图1 左为扩展有激光雷达与深度摄像头的单体车，本文将此车称为头车。

图1 集群单体无人车Fig.1 Unmanned vehicles for clustering

集群协同平台的软件层面有姿态解算、避障信息、广播、一对一通信、动作控制、目标识别6 个功能。其通过读取MPU6050 和磁力计的数据，并对其进一步融合处理，得到集群单体车当前的姿态数据；通过读取红外避障模块得到集群单体车当前的避障信息；通过目标识别模块来寻找目标；通过两个UWB Node 实现测距、广播、一对一通信等功能；最后将得到的信息进一步整合计算，得出集群单体车下一步的方向和速度。

集群单体车的系统架构如图2 所示。

图2 集群单体车系统架构体系Fig.2 The system architecture of the car

2.2 集群协同平台位置协同技术

集群位置协同部分主要通过UWB 节点实现，节点如图3 所示。每辆单体车配置2 个UWB 节点，分为主副节点，分别安装在单体车的两侧。主节点有节点间测距、通信、邻居发现等功能；副节点只有测距功能，对于其他节点发送的帧，如邻居发现帧、广播帧等，不做任何响应。

本文针对集群协同平台的需求对UWB 节点进行了设计与开发，它包含了DWM1000 模组、九轴惯性传感器、气压计等，实物效果如图3 所示。通过软件设置，每个节点具有一个ID，作为车间测量的标识。该节点尺寸为3.5 cm×3.5 cm，采用双边双向测量，测量误差可控制在5 cm 以内，通信范围为150 m。

图3 UWB 节点实物图Fig.3 Physical view of UWB node

UWB 节点是位置协同的核心部件，主副2个UWB 节点组成Node，主节点的主要功能是与其他集群单体车进行一对一通信、接收广播（如果是头车，可以对其他集群单体车进行广播）、与其他集群单体车进行主动测距、响应其他UWB节点信息帧、邻居发现、提供自身ID 作为集群单体车ID 等；副节点相比于主节点功能较为单一，只有主动测距功能，且不响应其他UWB 节点信息帧。

无人单体车间测距、广播和一对一通信由单体车通信测距部分完成；广播和一对一通信由主节点独立完成；车间测距和车间跟随由主副节点共同完成。在进行车间测距时，主副节点同时对被测车的主节点进行测距，得到两个距离l1和l2，如图4 所示。A和A′分别为单体车的主副节点；B为被邻居单体车主节点；d为主副节点的距离，是一个常量。实际运行中，两车的距离通常远远大于主副节点之间的距离（即l1,l2≫d），所以可以近似认为，被测车的主节点就是被测车的中心点，那么两车之间的距离就可以估算为

图4 单体车间测距与方位判定Fig.4 Azimuth measurement based on distance information

一般情况下，车间判断方位与车间测距是同时进行的，使用的数据也是一样的。进行车间判断方位时，如图4 所示，由于两单体车的距离通常远远大于主副节点之间的距离，所以可以近似地认为线段BC=BA、∠ACA′为直角，则∠CAA′ 就容易求得

车间判断方位一般用于车间跟随，单体车只需转动∠CAA′即能向被跟随车方向前进。

普通单体车以头部三个红外设备作为主要的避障手段。当三个红外模块中的任一模块检测到障碍物时，小车首先停止运动，随后做出相应的避障动作。左红外检测到障碍物，小车向右转动，直到左红外检测不到障碍物；右红外检测到障碍物，小车向左转动，直到右红外检测不到障碍物；中间的红外检测到障碍物，小车随机转动直到红外检测不到障碍物。头车则是以激光雷达作为避障手段。雷达输出最近障碍物的距离值，小车通过数据做出相应动作，这里不再赘述。

UWB 技术具有系统复杂度低、发射信号功率谱密度低、对信道衰落不敏感、截获能力低、定位精度高等优点[16]。UWB 技术具有较高的距离测量精度，与GPS/北斗提供绝对地理位置不同，使用UWB 技术可以给出相对位置，其定位精度可达厘米级[17]。而且采用UWB 技术，很容易将定位与通信相结合，可在室内和地下进行精确定位，而GPS/北斗只能在定位卫星的可视范围之内定位。

2.3 集群协同平台的实现

本文在人工鱼群算法[18]的基础上，将人工鱼群的4 种行为改编为5 个状态，如表1 所示。

表1 集群单体车状态表Table 1 Cluster status table

Finding：集群单体车没有发现目标，也没有跟随其他车辆。此状态下，每隔一个节拍，目标识别模块判断当前视野内是否存在目标。如果没有，车体转动一个随机角度，再次判断；如果还是没有，那么单体车以随机速度前进，直到下个节拍。如果发现了目标那么状态切换到Found。

Found：集群单体车发现了目标，向目标移动，并通过主节点，每隔一个节拍，广播自身状态一次。此状态下，单体车每隔一个节拍目标识别模块就会计算与目标的距离以及目标的方位，并调整自身方向和速度。

Following：集群单体车没有发现目标，但是正在跟随其他车辆。此状态下，每隔一个节拍，单体车会对正在跟随的车进行车间测距以及车间判断方位，并调整方向和速度。另外，单体车仍会每隔一个节拍寻找目标一次，若找到目标，状态切换为Found。

I_GOT_IT：集群单体车已经发现目标，且与目标的距离小于2 m，此时单体车不再运动。此状态下，单体车仍会每隔一个节拍计算与目标的距离以及目标的方位，当目标与单体车距离超过2 m 时，状态切换为Found。如果目标直接消失，状态切换为Finding。

Following_Pause：集群单体车正在跟随其他车辆，但是由于离目标车辆太近（1.5 m），会令其速度为0，待距离变大后，继续跟随。这个状态是Following 的暂停模式，除了单体车速度为0，其他保持不变。

集群协同算法流程图如图5 所示。

图5 集群单体车行为流程图Fig.5 Behavior flow chart of the car

3 集群协作平台通信协议设计

3.1 主控制模块与运动控制模块的通信协议

集群单体的运动控制模块与主控模块通过串口方式通信，波特率为115200 bps，有1 位起始位、8 位数据位，无奇偶校验位。传输数据皆以十六进制高位字节在前进行收发。协议格式如表2 所示。

表2 主控制模块与运动控制模块协议格式Table 2 Protocol format of the main control module and the motion control module

主控模块发送给运动控制模块控制速度的协议格式如表3 所示，这里帧功能码只用到了0x01，其余可用于二次开发。

表3 单体车速度设置帧（主控制芯片→运动控制芯片）Table 3 Format of the frame sent by the main control module to the motion control module in order to set the speed of the car

举例：设置车体X轴线速度0.2 m/s，车体Y轴线速度0 m/s，车体Z轴角速度 0 rad/s 的指令如下（HEX 格式）：

运动控制模块返回给主控模块的协议格式如表4 和表5 所示。这里帧功能码用到了0x01（向主控制芯片返回单体车信息）、0x02（避障动作开始）、0x03（避障动作结束），其余可用于二次开发。

表4 单体车信息帧（运动控制芯片→主控制芯片）Table 4 Format of the frame sent by the motion control module to the main control module about information of the car

表5 避障指令帧（主控制芯片→运动控制芯片）Table 5 Format of the frame sent by the main control module to the motion control module about obstacle avoidance

3.2 主控制芯片与UWB 主副节点的通信协议

本文共用到了UWB 节点的主动测距、重启、邻居发现、响应其他UWB 节点、广播、节点一对一通信6 个功能。其中，主节点用到了以上6个功能；副节点只用到了主动测距功能，且不会响应其他节点的帧。

在主控模块与主副节点的通信中，基本协议由帧头、目标ID、功能码、消息4 部分组成，如表6 所示，每一部分由英文字符‘；’隔开。

表6 主控模块与主副UWB 节点协议格式Table 6 Protocol format of the main control module and UWB nodes

通信时如果不需要某一部分，省略即可。如广播时，不需要目标ID，最后程序生成的帧是“帧头+功能码+消息”。具体协议格式如表7 和表8所示。

表7 主控模块发送给主副节点的协议帧格式Table 7 Format of the frame sent by the main control module to UWB nodes

表8 主节点回复主控模块协议帧格式Table 8 Format of the frame sent by the primary UWB node to the main control module

3.3 集群单体车之间的通信协议

集群单体车间的通信包括广播和车间一对一通信两部分。车间通信的基本协议由帧头、数据长度、功能码、数据、校验和5 部分构成，直接相连，没有间隔，如表9 所示。广播和单体车一对一通信通过帧头来区分，广播帧头为 0x01, 0x10，一对一通信帧头为0x02,0x20。

表9 基本协议格式（广播、一对一通信）Table 9 Protocol format for broadcasting and peer-to-peer communicating

广播的协议格式见表10，广播的功能码只用到了0x01，其余可用于二次开发。

表10 单体车信息广播帧Table 10 Format of the frame broadcast by the car with its own status

一对一通信的协议格式见表11 和表12。一对一通信中，功能码只用到了0x01（询问其他集群单体车信息）和0x02（回应其他集群单体车的询问），其余可用于二次开发。

表11 请求帧（following 状态单体车→被跟随单体车）Table 11 Format of the frame sent by the following car to the followed car for inquiring

表12 单体车信息帧（被跟随单体车→ following 状态单体车）Table 12 Format of the frame sent by the followed car to the following car with its own status

3.4 单体车间通信关系

单体车间一对一通信和广播中的通信协议，其示意关系如图6～图8 所示。

图6 B 车询问A 车的信息Fig.6 Car B asks to car A

图7 A 车回应B 车的询问Fig.7 Car A responds to car B

图8 头车广播自身信息Fig.8 The leading car broadcasts its own information

4 集群协同平台实验验证

在集群协同平台体系设计以及通信协议设计的基础上，本文进行了系统平台的总体集成并进行了实机验证，在室内进行了2 辆与4 辆单体车的跟随实验，在室外进行了12 辆车的跟随实验。其中2 辆单体车的跟随实验以测试单体车的跟随效果；4 辆单体车的跟随实验以说明本系统平台不受GPS/北斗拒止影响。

4.1 单体车跟随实验

图9 为单体车跟随实验展示图。

图9 单个单体车跟随中Fig.9 Single car are following

4.1.1 通信延迟对实验的影响

单体车间单数据帧的通信延迟由传输延迟与传播延迟组成。由于室内定位的通信距离较短，传播延迟可忽略不计；而由于单体车间通信是点对点之间的直接通信，其传输时延由数据的处理时间决定，经实验测得的单数据帧通信延迟约为1 ms。本文实验中单体车间的数据交换次数为4，因此通信延迟为毫秒级延迟。在单体车间通信毫秒级的通信延迟条件下，假设单体车的运动速度为1 m/s，可知通信延迟对位置估计与控制是毫米级的影响，可忽略不计。

4.1.2 主副节点间距对实验的影响

设主副节点测距误差产生的噪声分别为e1和e2，则式(1)可以改为

其中，AA′为主副节点间距。由式(2)可以看出，分母AA′越小，噪声对方位角计算的影响越大。

在单体车静止条件下，多次由节点测距数据计算节点间距为15 cm 与25 cm 时小车的方位角误差，实验结果如图10 所示，显然主副节点间距越大，得到的方位角误差最小。

图10 主副节点间距离分别为15 cm 和25 cm 时的方位角误差Fig.10 The error of the azimuth when the distance between primary and secondary nodes is 15 cm and 25 cm respectively

4.2 集群协同实验

由于4 辆单体车构成的集群系统车群比较小，所以单体车不会跟随处于 Following 或者Following_Pause 状态下的车辆。为了实验效果更明显，只让0 号车具有识别目标的能力，由于1、2、3 号车找不到目标，所以它们就会进入Following 状态，跟随已经发现目标的0 号车（此时0 号车处于Found 状态），如图11～图13 所示。单体车运动的过程中遇到障碍物，或者两个单体车距离过近，就会触发中断，执行避障操作；当0 号车到达目标附近或收到停止指令时，就会进入I_GOT_IT 状态；当单体车与0 号车距离过近时，就会进入Following_Pause 状态，如图13 所示，1、2、3 号车都停止了跟随。

图11 4 辆单体车集群启动跟随Fig.11 Four cars are starting to follow

图12 4 辆单体车集群跟随中Fig.12 Four cars are following

图13 4 辆单体车集群停止跟随Fig.13 Four cars stopped following

12 辆单体车集群协同实验在室外进行，选用了吉林大学南湖校区网球场作为实验场地。由于单体车流程可分为发现目标和跟随两部分，而目标识别功能相对独立，本文即采用深度相机作为目标识别手段，因此实验中仅对头车配置了目标识别功能，主要验证其他单体车的跟随功能，实际使用中可以将所有车辆配置激光雷达和深度相机，以便每一个单体都可进行目标发现。实验中，由于车群比较大，所以单体车会跟随处于Following 或者Following_Pause 状态下的车辆。实验效果如图14～图16 所示。经过观察发现，1、2、3、11 号车直接跟随0 号车；4、5 号车跟随1号车；6、7 号车跟随2 号车；8、10 跟随3 号车；8 号车跟随11 号车。

图14 集群初始状态Fig.14 Initial state of clustering

图15 集群初始跟随Fig.15 Start to follow

图16 集群跟随中Fig.16 Following phase of clustering

5 结 论

本文基于UWB 局部测量的集群协同平台，验证了GPS/北斗拒止条件下，使用局部测量信息进行多机位置协同的方案。该研究实现了12 辆无人车单体的集群协作，可在无GPS/北斗以及地面基础设施的条件下进行位置协同，与视觉技术相比，在有遮挡的情况下，也可实现定位，且该平台不受地面站约束，可实现自主协作与灵活的任务控制；实现了集群协同系统的自定位与去中心化。该平台以单体智能为基础，使用UWB 技术实现了单体到群体的整合，且模块化的设计使得集群协作平台可以采用异构单体车，甚至可以整合无人机协同，从设计层面实现了单体设计与群体设计的分离，可将群体智能任务划分为单体智能与集群协同智能两部分独立研究。

单体智能与集群协同智能的分离使得未来可以一方面从提高单体智能化水平的角度进行集群协同平台研究，单体智能化水平的提高将有助于简化协同流程，实现明确的任务划分，提高系统协同效率；另一方面在已搭建的集群验证平台基础上，探讨与其他集群技术的融合方法，形成技术优势互补，研究通用化非中心式集群协作架构，独立于无人车与无人机，搭建空地协同的无人集群协作平台。