一种基于室内全局定位的多移动机器人编队控制方法与流程

本发明涉及机器人导航领域，尤其是一种移动机器人的定位和编队控制方法。

背景技术：

移动机器人作为一种自主、智能的运输平台和载体，适用于侦查、盯梢、搜索和跟踪等危险、污染、极端的环境。自主导航是移动机器人在未知、非结构环境下进行自主作业的核心技术，而定位技术又是机器人完成自主导航任务的最基本环节。与应用比较成熟的gps、北斗等室外定位与导航技术不同，精准室内定位技术目前相对较弱，而消防救援、抢险救灾、冲突反恐、自主对接、合作搬运等应用领域又对室内定位技术提出了现实需求。

根据定位技术和传感器的不同，移动机器人室内定位技术主要包含航迹推算、图形匹配和信标定位。航迹推算定位是一种基于位姿递推方式的相对定位方法，但该方法存在定位误差随时间累积的缺陷；图形匹配定位方法需依据外部环境建立全局或局部地图，该方法受外部环境影响较大，且大多适用于确定性静态环境；基于信标定位方法可以提供稳定、精确的全局绝对位置信息，但该方法的前期部署、后期维护工作量较大。

目前，移动机器人室内信标定位技术主要包括基于无线传感网络和超声波的两种定位方法。采用wlan、zigbee、bluetooth等的无线传感网络定位技术通过信号特征信息来分析推算机器人的当前位置信息，该定位技术不受视距约束，但定位精度相对较低；而基于超声波的信标定位技术具有抗电磁干扰能力强、信标铺设位置灵活、定位精度相对较高等优点。

多机编队控制问题是多移动机器人技术领域的一个重要分支，其在联合侦查、群体盯梢、协作救援、合作搬运、传感网络等军事和民用领域具有广阔的应用背景。多机编队控制问题是多个移动机器人自主协作形成一个预期的几何图形以完成特定的目标任务，并在未知、非结构环境条件下执行作业任务时尽可能保持既定编队队形不变。在多机编队问题的自主成形、队形保持、队形变换等过程中，一个最基本的问题就是各个移动机器人需要实时获取自身的位置指标，因此，精确定位方法是实现多移动机器人编队控制的先决条件和支撑技术。

在多移动机器人编队控制领域，国内目前的现状是学术研究多于实际应用，而学术研究绝大部分都是在假设已知各个机器人位置坐标基础上进行理论分析与仿真计算，真正结合实物机器人的应用验证甚少，其中一个重要原因就是缺乏室内全局定位方法与硬件的有效支持。在专利“基于无线传感网络的多机器人编队系统和编队方法(cn102662377a)”中，利用zigbee协议建立无线局域网，依据移动机器人自身接收信号的强度实时确定自身位置，该方法因定位精度相对较低而导致多机编队效果欠佳。在专利“一种基于蓝牙通讯的多机器人编队控制路径跟踪方法(cn103901889)”中，采用ispace摄像头位置捕捉系统来获取移动机器人的位置信息，并通过蓝牙将位置信息发送给移动机器人，该定位方法存在通讯距离短、成本相对较高等问题。

技术实现要素：

为了克服现有技术的不足，针对室内移动机器人定位精度低、适用范围有限，以及多移动机器人编队控制领域缺乏有效的室内定位方法等实际问题，本发明提出一种基于室内全局定位的多移动机器人编队控制方法，将一种新型的室内全局定位方法引入多移动机器人编队控制问题中，旨在解决室内移动机器人的自主定位问题，从而为多移动机器人的自主成形、队形保持、主动避障、队形变换和目标跟踪等提供一种有效、实时、准确的室内全局定位方法支持。

本发明解决其技术问题所采用的技术方案包括如下步骤：

步骤1、本发明包括一套室内全局定位系统和n个移动机器人，其中室内全局定位系统包括四个固定信标、一台定位基站控制器和n个搭载在移动机器人上的移动信标，将四个固定信标安装在高于移动机器人运动平面的墙面上，在避免遮挡的情况下使固定信标两两相对且不在同一平面内，并保证移动机器人运动区域无遮挡；

固定信标是整个室内定位系统用来构建全局坐标系的基准，接收每个移动信标发出的超声波信号并记录其id码，在接收过程中四个固定信标将会接收所有移动信标的超声波信号；

移动信标携带唯一的id码以标识每个移动机器人，并以固定频率发射超声波信号，该固定频率设定在10-20hz之间；

所述固定信标和移动信标均采用相同结构，均包含一个正方形底座和5个超声波发生接收器，5个超声波发生接收器分别安装在正方形底座的四边和中心，其中一个超声波发生接收器安装在底座中心位置且发生接收探头竖直向上，其余四个安装在正方形底座各边的中点处，且四个超声波发生接收器的发生接收探头与正方形底座保持15度仰角向上倾斜，超声波发生接收器以实现信标的全向发送和接收，同时每个移动信标配备角度姿态传感器，可动态测量移动机器人的偏角信息，移动信标和固定信标都有各自唯一的id标识码，整套定位系统同时测定所有的移动信标的位置坐标，在定位系统的覆盖区域内，定位系统利用固定信标对所有的移动信标进行实时测量，从而满足多机编队运动中对各个移动机器人的位置坐标计算需求；

定位基站控制器是整个室内全局定位系统的中心控制器，其信号覆盖定位区域内的所有固定信标和移动信标；

步骤2、在室内全局定位系统初始化过程中，以地面为基准对各固定信标的高度进行标定，并选择其中任一固定信标作为z轴，以相邻的任一固定信标的正下方为x轴建立三维坐标系，其他各固定信标以该坐标系为基准，确定其余固定信标在坐标系中的位置坐标，整个坐标系建立完成后，设置固定信标为接收模式；

步骤3、采用三边测量法对移动信标的位置坐标进行计算：固定信标接收移动信标的超声波信号并记录接收到该信号的时间点；定位基站控制器通过计算移动信标到达各固定信标的时间间隔，依此计算出该移动信标和四个固定信标之间的直线距离分别为d1、d2、d3、d4，当某一移动信标发射的超声波信号被3个或3个以上的固定信标接收到后，基站控制器通过三边测量法计算得到该移动信标在全局坐标系内的位置信息；

详细计算步骤为：假设定位系统在初始标定时，从4个固定信标中任选3个，即可得1号固定信标、2号固定信标和3号固定信标的坐标分别为(0,0,z1)、(a,0,z2)、(0,b,z3)，当这3个固定信标分别接收到移动信标发送的超声波信号时，依据三边测量计算原理，该移动信标的当前位置坐标(x,y,z)可计算如下：

即通过式(1)计算，即可得到某一移动信标在全局坐标内的当前位置坐标(x,y,z)；

步骤4、构建基于超声波信标的室内全局定位系统，用以获取各移动机器人的实时位姿；

1)部署室内全局定位系统：将固定信标安装在四个顶角位置，选择其中两个固定信标作为坐标系基准建立三维坐标系，移动信标分别安装在每个移动机器人上，移动信标与移动机器人控制器均采用串口通讯连接方式；定位基站控制器实时计算各移动机器人的位置坐标信息，并结合偏角信息以位姿数据包的形式下传至对应的移动信标；

2)移动机器人解析位姿数据包：移动机器人通过串口通讯方式读取移动信标接收到的位姿数据包，采用奇偶校验位进行判断，并判断校验位是否正确，当校验位为1则为正确，校验位为0则为错误，当校验位信息正确，则移动机器人将正确的位姿坐标信息存入处理器的缓存中，并进行字符串格式的数据类型转换为浮点型的位姿数据，并存入位姿数组中，如果校验位信息错误，将舍弃此次位姿坐标信息；

3)位姿数据依次存储：随着移动机器人的自主运动，其位置信息进行实时更新，通过定位基站控制器向各移动信标发送并解析位姿数据包后，位姿坐标信息遵循“先进先出”的顺序原则依次更新到移动机器人控制器的位姿坐标数组中；

步骤5：移动机器人的初始化

移动机器人的初始化主要包括：分配领航、跟随两种移动机器人角色，即任意选择一个机器人为领航机，其它均为跟随机器人，根据需求设定移动机器人编队需要保持的队形图案，配置领航机器人为发送数据状态，跟随机器人为接收数据状态；

设计移动机器人运动控制器，实现多机器人编队作业任务，详细步骤如下：

1)根据移动机器人车体结构及驱动方式，建立移动机器人运动学模型，并依此构建多移动机器人编队的运动学模型；

2)基于上述超声波信标的室内全局定位方法，实时获取领航机器人和跟随机器人的当前位姿；再根据预设的多机编队队形几何信息，计算出各跟随机器人当前时刻的理想位姿，将跟随机器人任意放置，然后，跟随机器人依据各自的理想位姿和实际位姿，计算得到各跟随机器人的跟踪位姿误差；最后，运用控制方法，设计运动控制器使跟随机器人的跟踪位姿误差满足精度要求，依此控制跟随机器人的跟踪位姿误差满足精度要求，从而实现基于领航跟随型的多机编队作业任务。

本发明的有益效果在于由于采用基于超声波信标的室内全局定位方法，对移动机器人进行室内定位服务，解决了移动机器人自主导航中的基础性定位问题；该定位方法具有容易部署、简单实用、定位精度高、可扩展性好等特点，它适用于单个移动机器人的自主导航，也可适用于多移动机器人的编队协作，同时也支持无人系统的三维坐标定位，为无人机、空地协同等的室内定位提供使能技术支撑。

附图说明

图1是基于室内全局定位的多移动机器人编队系统总体部署图，其中：1为固定信标，2为定位基站控制器，3是移动机器人，4为安装在各移动机器人上的移动信标。

图2是超声波信标结构图，其中，①、②、③、④和⑤均为固定信标超声波接收探头或移动信标的超声波发送探头。

图3是基于超声波信标和三边测量法的室内全局定位原理图，其中，d1、d2、d3和d4分别为固定信标到移动信标的直线距离。

图4是基于领航跟随型的多移动机器人编队场景图，其中，3-1为领航机器人，3-4、3-5为两个跟随机器人，3-2、3-3分别是两个跟随机器人对应的理想位置的虚拟机器人。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

基于超声波信标的室内全局定位技术，解决了移动机器人自主导航领域中的基本定位问题；并依此定位方法为支撑，提出一种适用于领航跟随型(leader-follower)多机编队问题的机器人运动控制方法。该发明为移动机器人在自主导航、主动避障、目标跟踪，以及多机器人的协作救援、合作搬运等应用领域提供定位方案支持。

本发明是从室内全局定位的角度入手，采用超声波信标定位方式，提供一种新的室内定位方法；并在该定位技术基础上，提出一种较为通用的多机编队控制方法，旨在为室内移动机器人自主导航、多移动机器人编队协作等提供定位方法支持。

步骤1、本发明包括一套室内全局定位系统和n个移动机器人，其中室内全局定位系统包括四个固定信标、一台定位基站控制器和n个搭载在移动机器人上的移动信标，将四个固定信标安装在高于移动机器人运动平面的墙面上，在避免遮挡的情况下使固定信标两两相对且不在同一平面内，并保证移动机器人运动区域无遮挡；

固定信标是整个室内定位系统用来构建全局坐标系的基准，接收每个移动信标发出的超声波信号并记录其id码，在接收过程中四个固定信标将会接收所有移动信标的超声波信号；

移动信标携带唯一的id码以标识每个移动机器人，并以固定频率发射超声波信号，该固定频率设定在10-20hz之间；

所述固定信标和移动信标均采用相同结构，如图2所示，其中，①、②、③、④和⑤均为固定信标超声波接收探头或移动信标的超声波发送探头，各探头均匀布置以实现定位覆盖范围的最大化。固定信标和移动信标均包含一个正方形底座和5个超声波发生接收器，5个超声波发生接收器分别安装在正方形底座的四边和中心，其中一个超声波发生接收器安装在底座中心位置且发生接收探头竖直向上，其余四个安装在正方形底座各边的中点处，且四个超声波发生接收器的发生接收探头与正方形底座保持15度仰角向上倾斜，超声波发生接收器以实现信标的全向发送和接收，同时每个移动信标配备角度姿态传感器，可动态测量移动机器人的偏角信息，移动信标和固定信标都有各自唯一的id标识码，整套定位系统同时测定所有的移动信标的位置坐标，在定位系统的覆盖区域内，定位系统利用固定信标对所有的移动信标进行实时测量，从而满足多机编队运动中对各个移动机器人的位置坐标计算需求；

定位基站控制器是整个室内全局定位系统的中心控制器，其信号覆盖定位区域内的所有固定信标和移动信标，以四个固定信标建立的全局坐标系作为基准，根据固定信标所采集的距离信息计算各个移动信标在全局坐标系下的位置坐标，定位基站控制器并将位置信息以数据包形式发送给移动信标，以供各个移动机器人解析各自所在全局坐标系中的位置信息；

步骤2、在室内全局定位系统初始化过程中，以地面为基准对各固定信标的高度进行标定，并选择其中任一固定信标作为z轴，以相邻的任一固定信标的正下方为x轴建立三维坐标系，其他各固定信标以该坐标系为基准，确定其余固定信标在坐标系中的位置坐标，整个坐标系建立完成后，设置固定信标为接收模式；

步骤3、采用三边测量法对移动信标的位置坐标进行计算：固定信标接收移动信标的超声波信号并记录接收到该信号的时间点；定位基站控制器通过计算移动信标到达各固定信标的时间间隔，依此计算出该移动信标和四个固定信标之间的直线距离分别为d1、d2、d3、d4，当某一移动信标发射的超声波信号被3个或3个以上的固定信标接收到后，基站控制器通过三边测量法计算得到该移动信标在全局坐标系内的位置信息；

详细计算步骤为：假设定位系统在初始标定时，从4个固定信标中任选3个，即可得1号固定信标、2号固定信标和3号固定信标的坐标分别为(0,0,z1)、(a,0,z2)、(0,b,z3)，当这3个固定信标分别接收到移动信标发送的超声波信号时，依据三边测量计算原理，该移动信标的当前位置坐标(x,y,z)可计算如下：

即通过式(1)计算，即可得到某一移动信标在全局坐标内的当前位置坐标(x,y,z)；

图3是基于超声波信标和三边测量法的室内全局定位原理图，其中，d1、d2、d3和d4分别为固定信标(1-1、1-2、1-3、1-4)到移动信标(4)的直线距离。

基于上述超声波信标室内定位方法，实时获取各个移动机器人的当前位置坐标，并依此定位技术为支撑，最终形成一种简单、易用的多移动机器人编队控制解决方案，具体实现步骤见步骤4；

步骤4、构建基于超声波信标的室内全局定位系统，用以获取各移动机器人的实时位姿；

1)部署室内全局定位系统：将固定信标安装在四个顶角位置，本发明选择50m²室内，如图1所示，其中：1为固定信标，2为定位基站控制器，3是移动机器人，4为安装在各移动机器人上的移动信标。选择其中两个固定信标作为坐标系基准建立三维坐标系，使其完全覆盖移动机器人的运动范围；移动信标分别安装在每个移动机器人上，移动信标与移动机器人控制器均采用串口通讯连接方式；定位基站控制器实时计算各移动机器人的位置坐标信息，并结合偏角信息以位姿数据包的形式下传至对应的移动信标；

2)移动机器人解析位姿数据包：移动机器人通过串口通讯方式读取移动信标接收到的位姿数据包，采用奇偶校验位进行判断，并判断校验位是否正确，当校验位为1则为正确，校验位为0则为错误，当校验位信息正确，则移动机器人将正确的位姿坐标信息存入处理器的缓存中，并进行字符串格式的数据类型转换为浮点型的位姿数据，并存入位姿数组中，如果校验位信息错误，将舍弃此次位姿坐标信息；

3)位姿数据依次存储：随着移动机器人的自主运动，其位置信息进行实时更新，通过定位基站控制器向各移动信标发送并解析位姿数据包后，位姿坐标信息遵循“先进先出”的顺序原则依次更新到移动机器人控制器的位姿坐标数组中；

步骤5：移动机器人的初始化

移动机器人的初始化主要包括：分配领航、跟随两种移动机器人角色，即任意选择一个机器人为领航机，其它均为跟随机器人，根据需求设定移动机器人编队需要保持的队形图案，配置领航机器人为发送数据状态，跟随机器人为接收数据状态；

设计移动机器人运动控制器，实现多机器人编队作业任务，详细步骤如下：

1)根据移动机器人车体结构及驱动方式，建立移动机器人运动学模型，并依此构建多移动机器人编队的运动学模型；

2)基于上述超声波信标的室内全局定位方法，实时获取领航机器人和跟随机器人的当前位姿；再根据预设的多机编队队形几何信息，计算出各跟随机器人当前时刻的理想位姿，将跟随机器人任意放置，然后，跟随机器人依据各自的理想位姿和实际位姿，计算得到各跟随机器人的跟踪位姿误差；最后，运用控制方法，如：pid控制、滑膜控制法等，设计运动控制器使跟随机器人的跟踪位姿误差满足精度要求，依此控制跟随机器人的跟踪位姿误差满足精度要求，从而实现基于领航跟随型的多机编队作业任务。

以下结合我们已经成功实现的原理样机与原型系统，以领航跟随型的三角形多机编队为例，对该发明的具体实施方式说明如下：

1、搭建基于超声波信标的室内全局定位系统

以地面为基准对每个固定信标的高度进行标定，选择其中一个固定信标作为坐标系的z轴，并以另一固定信标正下方为x轴建立三维坐标系。如图3所示，选择固定信标1-1作为z轴，并以固定信标1-2正下方为x轴建立三维坐标系。其他各固定信标以该坐标系为基准相互测距确定其所在坐标系中的位置坐标；将移动信标固定在移动机器人上，并使得移动信标的中心点与移动机器人驱动轴中点相重合；启动定位系统的定位基站控制器，设置固定信标超声波接收模式和移动信标超声波发送模式，并测试定位系统是否启动成功。

移动信标与其所属的移动机器人采用串口通讯方式，移动信标的位置数据包包含：x轴的位置坐标(单位:厘米)，y轴的位置坐标(单位:厘米)，z轴的位置坐标(单位:厘米)，偏角信息(单位：rad)、校验位、移动信标id等。

移动机器人接收到该位置数据包后：首先，判断该数据包的校验位是否正确，若正确则保留该位置信息，否则舍弃；然后，将通过校验的数据包存入移动机器人处理器的缓存中；最后，将该数据包中的位姿坐标(包含坐标和偏角信息)数据位进行数据类型转换，转换后的数据单独存入位置信息数组中。至此，移动机器人当前的位姿信息读取成功并释放缓存。

伴随着移动机器人的自主运动，其位置信息按照固定频率更新，并通过基站控制器向各移动信标发送，经解析后将这些位姿坐标依据“先进先出”的顺序方式依次更新到位姿坐标数组中。

2、移动机器人初始化。

移动机器人初始化主要包括：初始化编队图形信息，配置多机之间的无线通讯单元等。以图4所示的三角形多机编队为例进行说明，其中，3-1为领航机器人，3-4、3-5为两个跟随机器人，3-2、3-3分别是两个跟随机器人对应的理想位置的虚拟机器人。

初始化多机编队的几何图形：领航机器人(3-1)为三角形队形的顶点，跟随机器人(3-4、3-5)为三角形编队底边的两个端点，而且分别与领航机器人距离为l、夹角为

配置领航机器人与跟随机器人的无线通讯单元：选用无线通讯单元esp8266作为多机器人之间的通讯模块。esp8266具有三种工作模式即softap模式、station模式和softap+station共存模式。在图4所示的三角形多机编队中，领航机器人(3-1)采用softap模式建立无线局域网，并将其位置信息通过无线局域网向跟随机器人传播；跟随机器人采用softap+station共存模式，这种共存模式可以避免因领航机器人故障而导致整个编队功能失效现象的发生，即：如果领航机器人出现故障，则跟随机器人根据优先级顺序可以自主切换成softap模式，并在该模式下建立局域网，从而替代领航机器人继续完成预期的编队作业任务。

3、设计移动机器人的运动控制器，实现多移动机器人的编队作业任务。

基于超声波信标的室内全局定位系统，可以实时获得领航机器人(3-1)、跟随机器人(3-4、3-5)的当前位姿信息，且该位姿包括位置坐标和方向角信息。

以跟随机器人(3-4)为例进行说明，假设当前时刻领航机器人(3-1)位姿为(x1y1θ1)^t，跟随机器人(3-4)位姿(x2y2θ2)^t。若要保持既定边长为l的三角形编队队形，则需要实时跟踪虚拟机器人(3-2)，而虚拟机器人(3-2)的位姿坐标可计算如下：

依据该虚拟机器人(3-2)的位姿坐标，可计算得到跟随机器人(3-4)的跟踪位姿误差(xeyeθe)^t，其中，(xeyeθe)^t＝(x2-xry2-yrθ2-θr)^t。根据各跟随机器人的跟踪位姿误差来设计对应的运动控制器，以确保各跟随机器人的跟踪位姿误差趋近于0并满足精度要求，从而使得多移动机器人在尽可能保持既定几何形状的条件下执行作业任务。

对于移动机器人运动控制器的设计可以结合李雅普诺夫稳定性定理、反步法、反馈线性化等经典方法；根据运动控制器求解编队所需的运动控制率，并设计控制率中的相关参数；再与当前速度信息结合，可求出采样周期内每个跟随机器人保持编队队形所需的线速度和角速度。跟随机器人按照所得的线速度和角速度进行运动，使得多移动机器人编队队形得以保持。