一种混合协同定位的多机器人通信方法研究

艾浩军，韩亮亮，黄 乐，金子龙，王壹丰

(1.武汉大学国家网络安全学院，武汉430073；2.上海宇航系统工程研究所，上海201109；3.武汉工程大学计算机科学与工程学院，武汉430205；4.中国航天科技集团有限公司空间结构与机构技术实验室，上海201108)

1 引言

在月球表面广泛部署定位基础设施是非常困难的。因此需研究以母机器人为中心的多机器人通信与定位系统，计算子机器人到母机器人的相对位置，最终转换为所有机器人的绝对位置。目前，采用无线传感器网络实现机器人通信与定位的研究已经成为热点问题。

通信方面，在美国国防高级研究计划局的大力支持下，卡内基梅隆大学于1978年开展分布式传感器网络课题研究。上世纪90年代之后，很多美国大学也争相进入了这个新兴研究领域，如加州大学、南加州大学、麻省理工学院、康奈尔大学、佐治亚理工学院等。这些知名大学对无线传感器网络的关键技术和系统理论进行了深入研究[1-2]。随后，国内外不同研究机构分别从物理层、数据链路层、网络层、传输层、应用层等方面着手，并取得了一系列可喜的研究进展[3-5]。如张清国等[6]提出一个基于蜂窝结构的覆盖优化算法，算法每次选取一个移动传感器节点，寻找传感器网络中离它最近的覆盖漏洞位置，基于蜂窝结构计算移动节点的候选目标位置，修补漏洞，提高网络的覆盖率。此外，美国加州大学伯克利分校开展的大鸭岛生态环境监测项目、英特尔公司演示的有关家庭医疗护理的无线传感器网络系统等[7-9]都是在开发出的Mote节点及相应无线传感器网络系统基础上，开展的广泛应用研究。

定位方面，月面环境下的定位技术在月球车上应用逐渐成熟。近年来多位学者进行了月面巡视器的定位研究。徐辛超等[10-12]应用单摄像机完成近距离的定位；刘少创等[13]完成了月面巡视器立体相机共线方程的建立；SQUYRES等[14]研究了火星探测器的定位方法。但上述月球车导航研究中都是假设机器人已经知道自己的全局位置，只探讨如何进行机器人的路径规划。随着月球车自主化要求越来越高，其定位问题将受到越来越广泛的重视。

针对月面环境下机器人组无法获取全局位置的问题，借鉴地面类似应用场景下的前沿技术，提出多机器人通信与定位的系统设计方案，研究多机器人的无线数字通信技术，在无法依赖外界设备环境下(如无GPS环境、无场景建模条件)，实现多机器人混合协同定位。研制母子机器人板载通信与定位模块，及与机器人其它模块的集成，形成多机器人通信、定位与系统设计的完整解决方案。并通过基于STM32平台开发相应的通信与定位测试平台，验证系统的可行性及是否达到实际应用要求。

2 系统框架和技术指标

本项目由多机器人通信、定位两大模块以及整体系统设计来解决月球表面的通信和定位的问题。方案包括：多机器人通信方案、多机器人定位方案、多机器人系统设计方案，如图1所示。其技术指标如下：近作业区(200 m范围)，视距条件下定位精度分米级，优于1 m；远作业区(2 km范围)，视距条件下，定位精度米级，优于10 m。近作业区，一般工作条件下，以母机器人为中心，可实现200 m半径的通信，相对定位精度达到分米级；远作业区，极限工作条件，子母机器人通信距离大于2 km，相对定位精度达米级。

图1 系统总体架构图Fig.1 System architecture

3 方案设计

3.1 多机器人通信设计

多机器人的通信系统中，母机器人拥有对外通信的信道，所有子机器人通过母机器人间接对外通信。母机器人对外通信，如与基站的通信，需根据基站采用的通信方式决定，在此不加讨论。多机器人通信需要在月球表面实现以母机器人为中心的多机器人全双工实时无线通信信道，有多机器人组成的本地通信网络，能满足所有机器人间的通信需求。

根据多机器人远距离通信和定位技术需求，组网通信以母机器人为中心，形成星型网络，如图2所示；在实现定位时，母机器人及子机器人均可互通信，形成网状网，如图3所示。

图2 母机器人为中心的星型网络Fig.2 Mother robot-centered star network

图3 定位时形成网状网Fig.3 Mesh network during positioning

网络协议采用MODBUS协议。MODBUS是OSI模型第7层上的应用层报文传输协议，其作用是在为连接至不同类型总线或网络的设备提供客户机/服务器通信。MODBUS是一种请求/应答协议，并且可以根据需要设置功能码。MODBUS协议定义了一个与基础通信层无关的简单协议数据单元 PDU(Protocol Data Unit)，MODBUS功能码是MODBUS请求/应答PDU的元素。特定总线或网络上的 MODBUS协议映射能够在应用数据单元ADU(Asynchronous Data Unit)上引入附加域。

3.2 多机器人定位设计

多机器人定位设计主要是为确保位置的精准性，从而达到实时准确的信息配对传递。本文采用自定位和相对定位结合，近定位和远定位相结合的方案。

3.2.1 移动机器人自定位

移动机器人的自定位需要测量出机器人所移动的距离以及在此时间内机器人方位的变化。移动机器人的自定位采用M6050惯性导航模块实现，通过M6050自带陀螺仪和加速度计分别测量出旋转率和加速度，然后对测量结果进行积分运算，以此计算出机器人移动的距离以及机器人方位的变化，再根据轨迹推算算法，求得移动机器人的位置以及姿态，这种方法的优点之一是无需外部参考。但会随时间有漂移，任何小的常数误差在积分之后都会无限增长。因此，系统将自定位和相对定位结合，进行精确定位，并将新的位置记录作为新的起点，以减小累积误差。移动机器人自主定位原理如图4所示，图中显示了移动机器人的加速度积分过程，其中，(X(i)、Y(i))(i=0，1，2)表示机器人的位置信息，φ(i)表示位置偏移量。

图4 移动机器人自主定位原理图Fig.4 Schematic diagram of mobile robot autonomous position

3.2.2 多机器人相对定位

多机器人相对定位是将被定位的子机器人当作是标签节点，母机器人及其它子机器人当作是基站，采用了双向测距的方法来获取标签与基站之间的距离。由于标签与基站之间时间不同步，因此利用标签发出信号与收到反馈信号之间的时间差，加之发送的速度为电磁波的传播速度，可获得基站与标签之间的距离。采用TDOA双曲线定位算法，其原理如图5所示。图中，N(x0，y0)表示被定位的机器人坐标，Ci(xi，yi)(i=1，2，3)表示其他子机器人或母机器人坐标，R表示其之间的距离。

图5 双曲线定位原理图Fig.5 Schematic diagram of hyperbolic position

利用TDOA双曲定位算法进行标签坐标解算，求得3个以上基站到标签的距离，利用双曲线方程就可解算出标签坐标。即只要有1个母机器人，例如C1坐标为(x1，y1)，2个或2个以上的子机器人在视距范围内，例如 C2(x2，y2)和 C3(x3，y3)，就可以计算出被测子机器人的位置N(x0，y0)。TOF测距技术可以认为是飞行时差测距(Time of Flight Measurement)方法，属于双向测距技术，其主要原理是测量信号在2个异步收发机之间往返的飞行时间，结合电波飞行的速度测量节点间的距离。在视距范围内，基于TOF测距方法测得的结果比较理想，随距离呈线性变化。TOF测距方法的关键约束为发送设备和接收设备必须始终同步，但是由于发送端和接收端的时钟不一致，从而导致很难达到绝对时间同步。在此采取改进的TOF测距方法，通过时钟偏移量来解决时钟同步问题，且考虑两端节点中时钟偏移量的影响。即A发送数据包，B收并回应确认，B回应确认不仅带B的时间，同时也是完整的数据包，待A收并确认，也计算回应确认的时间，与B的相比较，均衡处理。实际测量过程中，采用多次测量取平均值的方法计算距离，可达到更好的精度。对于不同的模块，在投入使用前，标定时间作为实际使用过程中对实测值的校正。

3.2.3 多机器人通信与定位硬件设计

多机器人通信与定位硬件实现框图如图6所示。多机器人通信与定位系统主控制器选择STC12LE5A60S2单片机，组网选择CC1310，433M技术组网，与主控制器之间采用串口通信；近距离定位采用 UWB(Ultra Wideband)技术，选择DWM1000模块，与主控制器采用SPI(Serial Peripheral Interface)接口连接；远距离定位采用433M扩频技术远距离组网技术，自定位采用陀螺仪和加速度计获得方向和位移参数进行积分计算获得，选择MPU6050，采用I2C接口与主控制器相连；如有需要，可设置键盘控制电路、显示输出模块、指示灯或报警输出等。

图6 硬件组成框图Fig.6 Block diagram of hardware composition

4 实验结果

在基于STM32平台上开发通信与定位测试平台，系统测试在空旷无干扰的环境下进行，主机和从机分别用串口线连接电脑，电脑端打开串口调试助手，并调整波特率为115.2 kbps，停止位1位，数据位8位，无奇偶校验。主机连接的电脑可充当上位机功能，从机连接的电脑充当下位机。实验由3部分组成：多机器人通信实验、多机器人定位实验和多机器人通信与定位联调实验。室外通信测试范围为武汉工程大学图书馆楼顶到大化工楼。测试工具为Win10环境的笔记本电脑，采用XCOM串口调试助手。

4.1 多机器人通信实验

组网通信以母机器人为中心形成星型网络，采用MODBUS协议。母机器人作为主节点，子机器人作为子节点。通信总是由主节点发起，母机器人作为主控节点。在实验过程中，采用电源供电，为保证母机器人信息不丢失，在母机器人的硬件上加有辅助存储器。考虑母机器人负责对外通信，采用485串口与上位机通信模拟母机器人对外通信。实验中子机器人作为子节点，从降低功耗的角度考虑，其通信模块可以采用电池供电，亦可采用电源供电。子机器人有唯一的地址，母机器人也有固定地址，保证了多机器人的信号传输的准确性。

母机器人可发送广播命令及单播命令单独给各子机器人。在2 km范围内，每隔100 m进行不同命令测试、极限带宽测试以及误码率测试。表1给出了主机与从机2的通讯测试结果，其测出的各项指标均达到设计要求。

表1 主机与从机2通信测试结果Table 1 Test result of communication between host and slave 2

4.2 多机器人定位实验

多机器人定位实验包括200 m内UWB定位，远距离采用LORA(Long Range Radio)远距离定位，先分别进行定位实验再进行联合测试。在正式定位前，借助UNI-T手持式激光测量仪测量各节点间距离完成标定，然后利用标定结果，进行具体定位测试。经过反复测试，将定标程序及测试算法程序标准化，集成在系统中，以方便不同环境下的标定。表2给出了UWB近距离定位测试和LORA远距离定位测试结果。

表2 多机器人定位测试结果Table 2 Test result of multi-robot position

4.3 多机器人通信与定位联调实验

多机器人通信中母机器人作为主节点，子机器人作为子节点。母机器人发定位指令给子机器人，子机器人便进入定位程序，利用UWB近距离定位、LORA远距离定位，同时计算自定位值估算与母机器人的距离，并将所得定位数据通过扩张的卡尔曼滤波算法得到期望数据。此时母机器人作为主控节点，以广播形式发送定位信息，获取周围其他机器人的响应，以测得相应的距离信息来定位，并将定位结果给母机器人。

联调实验中，实验主机位于武汉工程大学图书馆8楼楼顶，从机位于大化工C613室，利用Google地图测量距离显示距离为399.99 m。每次记录的测试数据为10次的平均值，分别记录3次，如表3所示。平均值为3次测试数据的平均值，误差值为平均值与Google地图测量距离的差值，可看出误差值小于3 m。

表3 平均测量距离统计表Table 3 Average measurement distance statistics/m

5 结论

针对月面环境多机器人通信与定位，进行了系统方案设计、算法研究及硬件实现。设计了基于混合协同定位的多机器人通信方法，定位模块采用自定位和相对定位结合、近定位和远定位相结合的方法。采用基于MODBUS的网络通信方法进行信息互通，能够实现多机器人之间的精准通信。在基于STM32平台上开发了相应的通信与定位测试平台，实验结果表明，在400 m范围内机器人组通信正常，定位准确，验证了系统的可行性，达到了技术指标要求。