CN102662377B - 基于无线传感器网络的多移动机器人编队系统和编队方法 - Google Patents

Abstract

基于无线传感器网络的多移动机器人编队系统和编队方法，属于无线传感器网络和多机器人控制领域。本发明解决了现有多机器人编队中依赖多种传感器，而导致的测量范围和精度易受环境因素影响的问题。本发明所述的多移动机器人编队系统中的网关节点、多个信标节点、多个主机器人节点和从机器人节点组成无线传感器网络，上位机监控系统通过串行通信端口与网关节点实现通信，并实现对无线传感器网络中移动节点的状态监控；信标节点作为固定节点为移动节点提供位置信息支持。本发明的编队方法采用上位机通过网关节点将目标点位置发送给主机器人节点，主、从机器人节点通过信标节点确定自身位置，主机器人节点带领所有从机器人节点向目标点自主导航。

Description

基于无线传感器网络的多移动机器人编队系统和编队方法

技术领域

本发明设计了基于无线传感器网络的多移动机器人编队系统和编队方法，属于无线传感器网络和多移动机器人控制领域。

背景技术

在现代社会的发展中，自动化的机器人在发挥着越来越重要的作用。目前，机器人技术在向着智能化和网络化的方向发展。在很多环境恶劣、任务繁重的场合，单个移动机器人已经无法代替人类完成复杂任务，更多地需要多个移动机器人编队组成一定的队形，协作完成任务。此时，编队的快速性、稳定性和可靠性将直接影响多移动机器人合作完成任务的质量。所以，发明一种智能化、网络化的多移动机器人编队系统和编队方法就显得十分重要。

目前，已有的多移动机器人编队系统和编队方法都是通过在机器人上安装多种复杂传感器来感知外围环境的基础上完成的。对于编队系统，在实现编队行为时，需要配备昂贵、复杂的辅助传感设备；对于编队方法，在编队控制的同时，还要采样和处理多个传感器的输出信号，降低了控制系统的响应速度。目前，在多移动机器人编队时，常见的传感器有，视觉传感器、超声波测距传感器和红外测距传感器。但是，每种传感器都有其局限性，视觉传感器易受光照的影响，在光照变弱时，会影响对图像色彩和纹理识别的准确性；超声波测距传感器的有效工作范围较小，而且声波的传播受到介质密度的影响，不适合于浓烟和火灾场合；红外测距传感器的方向性很强，测量距离很短。而且，传感器的测量还受到物体的外形尺寸和运动速度的影响，测量精度不高。

综上所述，目前的多移动机器人编队系统成本较高，精度较低，实现复杂，应用场合受传感器局限；相应地，编队方法也较复杂，响应速度慢，控制上对传感器检测的反馈量敏感，稳定性和可靠性较低，限制了其应用。所以，需要一种简单可行、成本低、精度高、可靠性高、响应速度快，并能够广泛推广的多移动机器人编队系统和编队方法。

发明内容

本发明的目的是为了解决现有的多移动机器人编队系统和编队方法的成本高、精度低、实现复杂、应用范围小、响应速度慢、可靠性不高的问题，提出基于无线传感器网络的多移动机器人编队系统和编队方法。

本发明所述的基于无线传感器网络的多移动机器人编队系统，它包括上位机监控系统(1)、1个网关节点(2)、m个信标节点(3)、1个主机器人节点(4)和k个从机器人节点(5)；其中主机器人节点(4)和从机器人节点(5)为移动节点；

所述网关节点(2)、m个信标节点(3)、主机器人节点(4)和k个从机器人节点(5)通过ZigBee协议共同构成无线传感器网络；

网关节点(2)是网络中通信的控制中心，用于实现上位机监控系统(1)和主机器人节点(4)之间的数据传输；

信标节点(3)是位置已知的固定节点，用于广播其自身坐标位置信息；

每个移动节点在运动过程中，能够和n个信标节点(3)建立无线通信连接；

主机器人节点(4)接收网管节点(2)发送的目标点的位置信息，并向目标点的位置自主导航，在自主导航的过程中，向从机器人节点(5)发送编队指令信息，

从机器人节点(5)根据接收到的编队指令信息实现自主调整；

m为大于或等于3的自然数，k为大于等于1的自然数。n为大于或等于3的自然数，而且n小于或等于m。

所述无线传感器网络是基于Zigbee协议构建的。

在无线传感器网络组建时，网关节点(2)负责建立稳定的网络通信结构，无线传感器网络中各个节点间的通信链路。在多移动机器人编队的过程中，网关节点(2)用于向主机器人节点(4)发送目标点位置等控制信息，同时接收主机器人节点(4)上传的编队信息。

所述编队指令信息中包括主机器人坐标位置、队形结构参数l和主机器人的速度和航向角。主机器人节点(4)在编队中的作用是，领导编队向目标点运动。

从机器人节点(5)在网络中跟随主机器人节点(4)运动，并保持稳定的编队队形结构。从机器人节点(5)具有自主调整能力，在接收到编队指令信息后，能够及时调整自身的运行速度和航向，以达到期望队形。从机器人节点(5)在编队中的作用是，保持稳定的队形结构。

网络中的所有主机器人节点和从机器人节点，能够同时通过无线传感器网络中的接收信号强度实时确定自身的行进位置，而没有借助于其它的外部传感器，节约了系统硬件资源。

在无线传感器网络中实现多移动机器人的编队，统一协调编队队形。每一个移动机器人都是无线传感器网络中的一个移动节点，受到网络的约束和统一管理。多移动机器人的编队和保持队形的移动是通过无线传感器网络中的通信完成的，没有使用用于检测外部环境中物体或者测距的传感器，降低了系统成本。

信标节点3是位置已知的固定节点，在无线传感器网络中它与其有效通信范围内的所有移动节点进行通信，根据无线传感器网络通信模型，可以由接收信号强度(RSSI)计算出信号传输距离，即信标节点3与移动节点间的距离，由此，当移动节点同时与3个以上的信标节点进行通信时，可以根据接收信号强度，确定自身位置。为减小计算的复杂度，采用信号强度最大的前n个信标节点参与计算，所述n是大于2的整数。这种方法的定位精度高，而且不受物体的外形尺寸和运动速度的影响。

应用于上述多移动机器人编队系统的基于无线传感器网络的多移动机器人编队方法中，主机器人节点的任务有：

一、根据上位机监控系统(1)发送的目标点的信息实现自主导航向目标匀速移动；

二、接收网关节点发送的指令信息；

三、接收所有从机器人反馈的队形信息；

四、向网关节点上传队形信息；

每个从机器人节点的任务有：

一、接收主机器人的队形指令；

二、保持队形中的l-

运动；

三、向主机器人发送队形信息，所述队形信息包括其自身的位置信息，以及行进速度和航向角。

本发明所述的多移动机器人编队方法为改进的leader-follower方法，1个主机器人带领k个从机器人运动。充分发挥了无线传感器网络的优势，采用分布式的控制方式，实时反馈从机器人的队形信息，在行进过程对编队进行实时调整。

相比于传统的leader-follower方法，改进的编队方法的优点主要体现在三个方面。

第一，在无线传感器网络的通信支持下，编队的精度和稳定性更高。在leader-follower方法中，控制距离l和角度

是一种简单、有效的方式。其中，l和

分别是主、从机器人之间的相对距离，及主、从机器人之间连线和主机器人航向间的夹角。但是传统的leader-follower方法中直接测量l和

此时，移动机器人的外形和运动速度都会影响测量的准确性，而且无法对多个从机器人同时控制，这些都会导致编队的队形精度变差。在改进后的leader-follower方法中，建立了基于地磁方向的大地坐标系，可以在网络中方便、准确地得到移动机器人的位置信息。在控制编队队形时，输入量和反馈量均为移动机器人的坐标值。再由坐标值计算得到的l和

会更加准确，而且，可以同时计算多个移动机器人的相对距离l和角度

所以编队的精度和稳定性也更高。

第二，采用基于任务的控制方式，对主、从机器人分别分配不同的任务，在编队过程中，机器人只要完成分配给自身的任务，这样减小了编队控制的复杂程度，提高了编队的效率。在传统的leader-follower控制方法中，主、从机器人的任务不明确，当队形发生变化时，对主、从机器人的运动情况都会产生影响，增加了控制的复杂程度。尤其是主机器人是整个编队控制的核心，需要检测各从机器人的运动状态，并完成大部分的编队控制算法，这种集中式的控制结构严重影响了编队效率。在改进算法中，结合无线传感器网络的特点，将编队任务分配到多个节点中，分布式完成。主机器人只需完成自主导航，并向从机器人提供队形指令信号；从机器人根据指令和自身的运动状态，对自身进行调整，自主地完成编队，提高了整体的编队效率。

第三，在队形控制中，增加了速度和航向的实时反馈，减小了队形控制的响应时间。在传统的leader-follower控制方法中，只是对从机器人跟随主机器人的位置信息，即l和

进行控制。而改进后的方法中，不仅控制位置信息，还对从机器人行进中的位姿信息，即速度和航向进行实时控制，以保证其运动状态能够始终与主机器人保持一致。这样使得在从机器人的航向角发生偏差时，能及时调整和纠正，避免偏差增大，影响编队队形。所以，增加速度和航向的反馈控制，能够提高行进过程中队形的稳定性和调整的快速性。

附图说明

图1是本发明所述的基于无线传感器网络的多移动机器人编队系统的结构示意图；图2是本发明所述的多移动机器人编队系统在大地坐标系中行进的示意图；图3是本发明所述的基于无线传感器网络的多移动机器人编队方法中的机器人任务分配图。

具体实施方式

具体实施方式一：参见图1说明本实施方式。本实施方式所述的基于无线传感器网络的多移动机器人编队系统包括上位机监控系统(1)、1个网关节点(2)、m个信标节点(3)、1个主机器人节点(4)和k个从机器人节点(5)；其中主机器人节点(4)和从机器人节点(5)为移动节点；

所述网关节点(2)、m个信标节点(3)、主机器人节点(4)和k个从机器人节点(5)通过ZigBee协议共同构成无线传感器网络；

网关节点(2)是网络中通信的控制中心，用于实现上位机监控系统(1)和主机器人节点(4)之间的数据传输；

信标节点(3)是位置已知的固定节点，用于广播其自身坐标位置信息；

每个移动节点在运动过程中，能够和n个信标节点(3)建立无线通信连接；

主机器人节点(4)接收网管节点(2)发送的目标点的位置信息，并向目标点的位置自主导航，在自主导航的过程中，向从机器人节点(5)发送编队指令信息，

从机器人节点(5)根据接收到的编队指令信息实现自主调整；

m为大于或等于3的自然数，k为大于等于1的自然数。n为大于或等于3的自然数，而且n小于或等于m。

所述无线传感器网络是基于Zigbee协议构建的。

在无线传感器网络组建时，网关节点(2)负责建立稳定的网络通信结构，无线传感器网络中各个节点间的通信链路。在多移动机器人编队的过程中，网关节点(2)用于向主机器人节点(4)发送目标点位置等控制信息，同时接收主机器人节点(4)上传的编队信息。

所述编队指令信息中包括主机器人坐标位置、队形结构参数l和主机器人的速度和航向角。主机器人节点(4)在编队中的作用是，领导编队向目标点运动。

从机器人节点(5)在网络中跟随主机器人节点(4)运动，并保持稳定的编队队形结构。从机器人节点(5)具有自主调整能力，在接收到编队指令信息后，能够及时调整自身的运行速度和航向，以达到期望队形。从机器人节点(5)在编队中的作用是，保持稳定的队形结构。

本实施方式所述的多移动机器人编队采用了无线传感器网络技术，该技术的优势主要体现在三个方面。一是，利用网络通信快速传递上位机监控系统1对整个编队队形的控制指令，并做到实时监控编队系统的运动状态；二是，在移动机器人之间，尤其是主机器人节点4和从机器人节点5之间快速传递队形指令信息；三是，利用无线传感器网络的信号传输模型，可以快速地确定移动节点的位置，并计算队形结构，而不用复杂而昂贵的外部传感器。

以上所述的多移动机器人编队系统在大地坐标系中的运动情况如图2所示。在某一时刻，主机器节点按照航向角θ₀向目标点自主导航运动；1#从机器人跟随主机器人运动，它在当前位置与主机器人之间的相对距离为l₁，相对角度为

速度为v₁，航向角为θ₁。但是，根据编队要求，它的期望位置与主机器人之间的相对距离为l₀，相对角度为

期望速度和航向与主机器人相同，即v₀和θ₀。在下一时刻，1#从机器人会调整速度和航向，向期望位置运动。最终，在调整之后，1#从机器人处于期望位置，它与主机器人之间的相对距离和角度均为期望值，同时其速度和航向角也和主机器人保持一致。

具体实施方式二：本实施方式是对具体实施方式一所述的基于无线传感器网络的多移动机器人编队系统的进一步限定，本实施方式中，上位机监控系统(1)内嵌入有实时监控软件(1-1)，所述实时监控软件(1-1)用于对无线传感器网络的状态进行监控，所述上位机监控系统(1)通过串行通信接口与网关节点(2)实现串行通信。

具体实施方式三：本实施方式是对具体实施方式一所述的基于无线传感器网络的多移动机器人编队系统的进一步限定，本实施方式中，网关节点(2)包括网关节点无线单片机系统(2-1)和网关节点状态指示模块(2-3)，网关节点无线单片机系统(2-1)具有无线射频通信功能，该网关节点无线单片机系统(2-1)发送状态显示数据给网关节点状态指示模块(2-3)，所述网关节点状态指示模块(2-3)用于显示接收到的状态显示数据。

本实施方式中所述的状态显示数据包括网络中节点的工作状态以及对应节点的位置信息，所述工作状态中包括正常和故障状态，进而实现及时地发出状态指示。

具体实施方式四：本实施方式是对具体实施方式一所述的基于无线传感器网络的多移动机器人编队系统的进一步限定，本实施方式中，信标节点(3)包括信标节点无线单片机系统(3-1)和信标节点状态指示模块(3-2)；信标节点无线单片机系统(3-1)具有无线射频通信功能，用于广播其自身坐标位置信息，信标节点无线单片机系统(3-1)发送状态显示数据给信标节点状态指示模块(3-2)，所述信标节点状态指示模块(3-2)将接收到的显示数据实时显示输出。

本实施方式中所述的状态显示数据包括该信标节点的工作状态和与其通信的移动节点数目。

具体实施方式五：本实施方式是对具体实施方式一所述的基于无线传感器网络的多移动机器人编队系统的进一步限定，本实施方式中，主机器人节点(4)包括主机器人无线单片机系统(4-1)、主机器人航向角检测模块(4-2)和主机器人电机驱动模块(4-3)；主机器人无线单片机系统(4-1)具有无线通信功能，用于接收信标节点(3)广播的坐标位置信息，该主机器人无线单片机系统(4-1)还用于根据接收到的多个信标节点(3)广播的坐标位置信息的信号强度和坐标值获得自身的位置坐标，还用于根据目标点的位置信息、自身的位置坐标和主机器人航向角检测模块(4-2)发送的当前航向信息产生电机驱动PWM控制信号，并将该电机驱动PWM控制信号发送给主机器人电机驱动模块(4-3)；主机器人航向角检测模块(4-2)用于检测所在主机器人节点(4)在大地坐标系的航向，并将该航向信息发送给主机器人无线单片机系统(4-1)；电机驱动模块(4-3)根据接收的电机驱动PWM控制信号产生电机驱动信号，并驱动相应的电机工作，进而实现对主机器人运动方向的调整。

本实施方式中，所述主机器人航向角检测模块(4-2)采用地磁传感器实现。

主机器人节点4在自主导航的过程中，不断地调整自身航向，使其指向目标点方向。同时，主机器人节点4和从机器人节点5建立通信连接，将自身位置、速度和航向以及队形结构参数l和

传送给从机器人节点5。主机器人节点4只是向从机器人节点5传送编队的给定量信息，并不直接控制从机器人的速度和航向等具体的运动状态。此外，主机器人节点4也接收从机器人节点5上传的编队信息，以对队形进行整体监控。在编队行进的过程中，主、从机器人之间的通信充分利用了无线传感器网络的特点，可以及时传送编队信息，实现队形的实时控制。

具体实施方式六：本实施方式是对具体实施方式一所述的基于无线传感器网络的多移动机器人编队系统的进一步限定，本实施方式中，从机器人节点(5)包括从机器人无线单片机系统(5-1)、从机器人航向角检测模块(5-2)和从机器人电机驱动模块(5-3)；从机器人无线单片机系统(5-1)具有无线通讯功能，从机器人无线单片机系统(5-1)用于接收信标节点(3)广播的坐标位置信息，并根据接收到的多个信标节点(3)广播的坐标位置信息的信号强度和坐标值获得自身的位置坐标；从机器人无线单片机系统(5-1)还用于根据编队指令信息、自身的位置坐标和从机器人航向角检测模块(5-2)发送的运动方向信息产生电机驱动PWM控制信号，并将该电机驱动PWM控制信号发送给主机器人电机驱动模块(4-3)；从机器人航向角检测模块(5-2)用于检测从机器人节点5在大地坐标系中的运动方向，并将该运动方向信息发送给从机器人无线单片机系统(5-1)；电机驱动模块(5-3)用于根据接收到的PWM控制信号产生电机驱动信号，并驱动相应的电机工作，进而实现对从机器人运动方向的调整。

本实施方式中，从机器人无线单片机系统(5-1)是从机器人的控制核心。

从机器人节点5跟随主机器人节点4保持一定的编队队形运动。从机器人节点5接收主机器人节点4的指令信息，同时它也具有一定的自我调整能力。从机器人节点5接收到的位置指令信息有，主机器人节点4的实时坐标值以及队形参数l和

从机器人节点5再结合自身的坐标值，可以计算出实时的队形结构参数，再通过调整速度和航向，保证结构参数l和

与给定值一致。此外，从机器人节点5接收到的位姿指令信息为主机器人节点4的速度和航向，从机器人节点5在跟随运动的过程中，还要实时地保证位姿与主机器人节点5保持一致。在从机器人节点5运动的过程中，同样要接收多个信标节点3广播的位置信息，以对自身实时定位。从机器人节点5不和网关节点2直接通信，它将自身的运动状态上传至主机器人节点4。

本实施方式中所述的从机器人航向角检测模块(5-2)可以采用地磁传感器实现，地磁传感器型号为HMC1022。

本发明所述的基于无线传感器网络的多移动机器人编队系统中，网关节点无线单片机系统2-1、信标节点无线单片机系统3-1均可采用型号为CC2430的无线单片机实现。

主机器人节点无线单片机系统4-1和从机器人节点无线单片机系统均可采用型号为CC2431的无线单片机实现，该型号的无线单片机内部集成了定位引擎模块，它能够利用多个信标节点进行定位。

网关节点状态指示模块2-3和信标节点状态指示模块3-2可以采用液晶LCD显示器实现。

具体实施方式五或六所述的技术方案中，所述根据接收到的n个信标节点(3)广播的坐标位置信息的信号强度和坐标值获得自身的位置的过程中，当n大于8时，选择信号强度最大的8个信标节点(3)的坐标值和信号强度值计算获得自身的位置；当n小于等于8时，选择接收到的n个信标节点(3)的坐标值和信号强度值计算自身的位置。

具体实施方式七：用于具体实施方式一至六所述的多移动机器人编队系统的基于无线传感器网络的多移动机器人编队方法中，

主机器人节点的任务有：

五、根据上位机监控系统(1)发送的目标点的信息实现自主导航向目标匀速移动；

六、接收网关节点发送的指令信息；

七、接收所有从机器人反馈的队形信息；

八、向网关节点上传队形信息；

每个从机器人节点的任务有：

四、接收主机器人的队形指令；

五、保持队形中的l-

运动。

六、向主机器人发送队形信息，所述队形信息包括其自身的位置信息，以及行进速度和航向角。

本实施方式所述的基于无线传感器网络的编队方法实际上是一种改进的leader-follower方法。结合本发明的编队系统，将移动机器人分为主机器人节点(leader)和从机器人节点(follower)两类，结合无线传感器网络的特点，将主、从机器人的任务明确化，平衡主、从机器人之间的任务分配，具体任务分配参见图3所示。主机器人的任务分为四个具体部分，即自主导航、接收网关节点指令、接收从机器人队形反馈和向网关节点上传队形信息。由此可以看出，主机器人的运动是在网关节点指令作用下的自主导航，并不受到从机器人的影响，这样提高了队形整体向目标点移动的效率。同时，主机器人在导航的同时，向从机器人发送编队指令信息，引导从机器人保持编队队形，跟随前进。所以，主机器人在编队中的主要作用为，引导编队向目标点移动改进的leader-follower方法中。从机器人的任务分为三个具体部分，即接收主机器人的队形指令、保持队形中的l-

运动和向主机器人上传队形信息。由此可以看出，从机器人的运动并不受到主机器人的直接控制，它只是接收主机器人的指令信息，并以此保持稳定的队形。与主机器人的导航不同的是，导航的目标点位置是固定的，而从机器人的目标的指令位置是跟随主机器人在变化的。所以，从机器人在编队中的主要作用为，保持稳定的队形结构。

本实施方式所述的编队方法中，避免主机器人的任务过于繁重，从而提高整体的编队效率。

具体实施方式八：本实施方式是对具体实施方式七所述基于无线传感器网络的多移动机器人编队方法的详细说明，本实施方式中，主机器人节点实现其任务的过程包括如下步骤：

确定自身当前的位置信息的步骤；

确定自身当前运行方向的步骤；

根据自身的当前位置信息和运行方向调整其航向角向目标点靠近的步骤；

控制自身保持匀速运行的步骤；

根据自身当前的位置信息判断编队是否到达目标点的步骤，当判定到达目标点时，编队过程结束，执行停止运行的步骤和发送编队结束命令给所有从机器人的步骤；

发送队形控制指令给所有从机器人的步骤，所述控制指令包括主机器人的实时坐标位置、速度和航向角以及队形结构参数l和

具体实施方式九：本实施方式是对具体实施方式七所述基于无线传感器网络的多移动机器人编队方法的详细说明，本实施方式中，从机器人节点实现其任务的过程包括如下步骤：

确定自身当前的位置信息的步骤；

确定自认当前运行方向的步骤；

根据队形控制指令，结合自身当前的位置信息和运行方向信息调整其航向步骤；

判断自身的编队队形是否出现偏差的步骤，并在判定出现偏差时，执行计算偏差值大小的步骤，然后根据所述偏差值调整自身运行状态的步骤；

判断自身的运动状态与主机器人相比是否出现偏差的步骤，并在出现偏差时，执行计算偏差值的大小的步骤，然后根据所述偏差值调整自身运行状态的步骤。

Claims (9)

1.基于无线传感器网络的多移动机器人编队系统，它包括上位机监控系统（1）、1个网关节点（2）、m个信标节点（3）、1个主机器人节点（4）和k个从机器人节点（5）；其中主机器人节点（4）和从机器人节点（5）为移动节点；

所述网关节点（2）、m个信标节点（3）、主机器人节点（4）和k个从机器人节点（5）通过ZigBee协议共同构成无线传感器网络；

上位机监控系统（1）用于通过网关节点（2）向主机器人节点（4）发送目标点信息，并通过网关节点（2）接收主机器人节点（4）和所有从机器人节点（5）组成的编队状态信息；

网关节点（2）是网络中通信的控制中心，用于实现上位机监控系统（1）和主机器人节点（4）之间的数据传输；

信标节点（3）是位置已知的固定节点，用于广播其自身坐标位置信息；

每个移动节点在运动过程中，能够和n个信标节点（3）建立无线通信连接；

主机器人节点（4）接收网关节点（2）发送的目标点的位置信息，并向目标点的位置自主导航，在自主导航的过程中，向从机器人节点（5）发送编队指令信息，

从机器人节点（5）根据接收到的编队指令信息实现自主调整；

m为大于或等于3的自然数，k为大于等于1的自然数，n为大于或等于3的自然数，而且n小于或等于m；

其特征在于，网关节点（2）包括网关节点无线单片机系统（2-1）和网关节点状态指示模块（2-3），网关节点无线单片机系统（2-1）具有无线射频通信功能，该网关节点无线单片机系统（2-1）发送状态显示数据给网关节点状态指示模块（2-3），所述网关节点状态指示模块（2-3）用于显示接收到的状态显示数据。

2.根据权利要求1所述的基于无线传感器网络的多移动机器人编队系统，其特征在于，上位机监控系统（1）内嵌入有实时监控软件（1-1），所述实时监控软件（1-1）用于对无线传感器网络的状态进行监控，所述上位机监控系统（1）通过串行通信接口与网关节点（2）实现串行通信。

3.根据权利要求1所述的基于无线传感器网络的多移动机器人编队系统，其特征在于，信标节点（3）包括信标节点无线单片机系统（3-1）和信标节点状态指示模块（3-2）；信标节点无线单片机系统（3-1）具有无线射频通信功能，用于广播其自身坐标位置信息，信标节点无线单片机系统（3-1）发送状态显示数据给信标节点状态指示模块（3-2），所述信标节点状态指示模块（3-2）将接收到的显示数据实时显示输出。

4.根据权利要求1所述的基于无线传感器网络的多移动机器人编队系统，其特征在于，主机器人节点（4）包括主机器人无线单片机系统（4-1）、主机器人航向角检测模块（4-2）和主机器人电机驱动模块（4-3）；主机器人无线单片机系统（4-1）具有无线通信功能，用于接收信标节点（3）广播的坐标位置信息，该主机器人无线单片机系统（4-1）还用于根据接收到的n个信标节点（3）广播的坐标位置信息的信号强度和坐标值获得自身的位置坐标，还用于根据目标点的位置信息、自身的位置坐标和主机器人航向角检测模块（4-2）发送的当前航向信息产生电机驱动PWM控制信号，并将该电机驱动PWM控制信号发送给主机器人电机驱动模块（4-3）；主机器人航向角检测模块（4-2）用于检测所在主机器人节点（4）在大地坐标系的航向，并将该航向信息发送给主机器人无线单片机系统（4-1）；主机器人电机驱动模块（4-3）根据接收的电机驱动PWM控制信号产生电机驱动信号，并驱动相应的电机工作，进而实现对主机器人运动方向的调整。

5.根据权利要求1所述的基于无线传感器网络的多移动机器人编队系统，其特征在于，从机器人节点（5）包括从机器人无线单片机系统（5-1）、从机器人航向角检测模块（5-2）和从机器人电机驱动模块（5-3）；从机器人无线单片机系统（5-1）具有无线通讯功能，从机器人无线单片机系统（5-1）用于接收信标节点（3）广播的坐标位置信息，并根据接收到的n个信标节点（3）广播的坐标位置信息的信号强度和坐标值获得自身的位置坐标；从机器人无线单片机系统（5-1）还用于根据编队指令信息、自身的位置坐标和从机器人航向角检测模块（5-2）发送的运动方向信息产生电机驱动PWM控制信号，并将该电机驱动PWM控制信号发送给主机器人电机驱动模块（4-3）；从机器人航向角检测模块（5-2）用于检测从机器人节点5在大地坐标系中的运动方向，并将该运动方向信息发送给从机器人无线单片机系统（5-1）；从机器人电机驱动模块（5-3）用于根据接收到的PWM控制信号产生电机驱动信号，并驱动相应的电机工作，进而实现对从机器人运动方向的调整。

6.根据权利要求4或5所述的基于无线传感器网络的多移动机器人编队系统，其特征在于，所述根据接收到的n个信标节点（3）广播的坐标位置信息的信号强度和坐标值获得自身的位置的过程中，当n大于8时，选择信号强度最大的8个信标节点（3）的坐标值和信号强度值计算获得自身的位置；当n小于等于8时，选择接收到的n个信标节点（3）的坐标值和信号强度值计算自身的位置。

7.应用于权利要求1所述的多移动机器人编队系统的基于无线传感器网络的多移动机器人编队方法，其特征在于，该编队方法中，

主机器人节点的任务有：

一、根据上位机监控系统（1）发送的目标点的信息实现自主导航向目标匀速移动；

二、接收网关节点发送的指令信息；

三、接收所有从机器人反馈的队形信息；

四、向网关节点上传队形信息；

每个从机器人节点的任务有：

一、接收主机器人的队形指令；

二、保持队形中的运动；所述l表示“相对距离”，

表示相对角度；

三、向主机器人发送队形信息，所述队形信息包括其自身的位置信息，以及行进速度和航向角。

8.根据权利要求7所述的基于无线传感器网络的多移动机器人编队方法，其特征在于，主机器人节点实现其任务的过程包括如下步骤：

确定自身当前的位置信息的步骤；

确定自身当前运行方向的步骤；

根据自身的当前位置信息和运行方向调整其航向角向目标点靠近的步骤；

控制自身保持匀速运行的步骤；

根据自身当前的位置信息判断编队是否到达目标点的步骤，当判定到达目标点时，编队过程结束，执行停止运行的步骤和发送编队结束命令给所有从机器人的步骤；

发送队形控制指令给所有从机器人的步骤，所述控制指令包括主机器人的实时坐标位置、速度和航向角以及队形结构参数l和。

9.根据权利要求7所述的基于无线传感器网络的多移动机器人编队方法，其特征在于，从机器人节点实现其任务的过程包括如下步骤：

确定自身当前的位置信息的步骤；

确定自认当前运行方向的步骤；

根据队形控制指令，结合自身当前的位置信息和运行方向信息调整其航向步骤；

判断自身的编队队形是否出现偏差的步骤，并在判定出现偏差时，执行计算偏差值大小的步骤，然后根据所述偏差值调整自身运行状态的步骤；

判断自身的运动状态与主机器人相比是否出现偏差的步骤，并在出现偏差时，执行计算偏差值的大小的步骤，然后根据所述偏差值调整自身运行状态的步骤。

Images