CN111103887B - 一种基于多传感器的多移动机器人调度系统设计方法 - Google Patents

Abstract

本发明属于计算机视觉技术及机器人控制技术领域，一种基于多传感器的多移动机器人调度系统设计方法，包括以下步骤：(1)搭建多个移动机器人平台，(2)构建二维实验场景地图及实现多移动机器人无线通讯，(3)全局路径规划的设计，(4)设计任务的分配，(5)多移动机器人预防碰撞策略的设计，(6)开发多移动机器人调度平台软件。本发明具有以下优点：一、采用无轨自主导航的方式，扩大了机器人可自由移动范围。二、对多机器人采用分布式控制，提高了整个调度系统的动态协调性和稳定性。三、利用激光雷达及深度实感相机传感器同时对机器人周围环境进行识别检测，提高了机器人在整个任务执行过程中的安全性。

Description

一种基于多传感器的多移动机器人调度系统设计方法

技术领域

本发明涉及一种基于多传感器的多移动机器人调度系统设计方法，属于计算机视觉技术及机器人控制技术领域。

背景技术

近年来，随着无线网络、计算机视觉、自主导航、SLAM等技术的快速发展，以及CPU和传感器等硬件性能的提高，机器人领域有了更广阔的发展前景。与以前相比，现在的机器人对周围环境的识别精度越来越高，处理各类数据的能力也越来越强，正在不断地朝着更智能化的方向发展，并已经开始应用于各种领域。从服务行业的服务机器人到物流行业的自主分拣搬运机器人，智能机器人的身影无处不在。然而，随着现代各行各业产业结构的快速升级，工作任务量也在不断地上涨，生产环境的日渐复杂，不仅对机器人个体的智能化提出新需求，同时也对多机器人控制提出了新挑战。多机器人调度系统是多机器人控制的研究方向之一，其中基于移动机器人的多机器人调度系统，研究内容包含多机器人路径规划、自主导航、任务分配、预防碰撞等。

目前，在许多多移动机器人调度系统中仍使用着传统的磁轨、二维码等有轨导航方式。这种导航方式存在诸多缺点，首先，将机器人限制在固定轨道上运行，极大程度地限制了机器人的移动范围；其次，工作场景空间固定，如果要改变工作场景或者增加额外的运输点，则必须重新部署局部轨道甚至全局轨道；最后，导轨长期使用还可能出现磨损状况，会需要增加额外维修成本。此外，现在的多移动机器人调度系统多采用集中式的控制方式，虽然这种方式协调性好，任务分配时容易获取最优解，但当控制中心出现问题时，整个系统容易出现运行故障。同时，对于机器人个体来说，在现在许多复杂的动态环境下，仅仅依靠单个传感器采集的环境信息，可能难以保证移动机器人在整个运输过程中的安全性。基于以上三个方面考虑，有必要对现阶段的多移动机器人调度系统设计提出更新的策略，使移动机器人在实际调度过程中拥有更自由的移动空间，能够更安全、更流畅地执行工作任务，同时使调度系统能长时间保持在更稳定的运行状态，确保任务准确无误完成。

发明内容

为了克服现有技术中存在的不足，本发明目的是提供一种基于多传感器的多移动机器人调度系统设计方法。该方法旨在对多移动机器人采用分布式控制的方式，使多机器人能以自主协商的方式进行任务分配。同时，对机器人采用激光导航的方式，引入一种在多机器人环境下的全局路径规划方法以及预防碰撞策略，使多机器人能够达到安全稳定地完成运送任务的目的。

为了实现上述发明目的，解决已有技术中所存在的问题，本发明采取的技术方案是：一种基于多传感器的多移动机器人调度系统设计方法，包括以下步骤：

步骤1、搭建多个移动机器人平台，以TurtleBot2移动机器人作为移动底盘，在移动底盘周围外侧设置有红位姿识别标靶，红位姿识别标靶前侧即机器人前侧设置有两道红色条纹，红位姿识别标靶后侧设置有单道红色条纹，红位姿识别标靶前、后侧连接处留出长度为1.5-1.6cm的空白区；在移动底盘上设置有两层亚克力板并与移动底盘形成三层空间，将NVIDIA Jeston TX2开发板作为上位机，安装在第二层中间位置，同时将IntelRealsense ZR300深度实感相机安装第二层前侧正中间；在第三层搭载RPLIDAR-A3激光雷达；

步骤2、构建二维实验场景地图及实现多移动机器人无线通讯，包括以下子步骤：

(a)控制移动机器人在实验场景范围内移动，利用RPLIDAR-A3激光雷达扫描环境信息，通过cartographer建图算法构建二维的实验场景地图；

(b)以一台台式电脑作为多移动机器人调度平台，设置调度平台与移动机器人之间的信息交互方式，使调度平台与移动机器人之间的交互方式为调度平台发送总任务信息给移动机器人、移动机器人反馈位置信息给调度平台；使移动机器人之间的信息交互方式为移动机器人之间任务信息相互交换；

(c)定义任务消息格式，基于TCP/IP协议在移动机器人及调度平台上创建基于不同网络端口的TCP服务器及客户端，在机器人之间及机器人与调度平台之间建立起TCP连接，通过socket实现任务信息、机器人位置信息、总任务消息的收发；

(d)在移动机器人上创建两个ROS节点，一个用于将其它移动机器人的任务消息通过ROS话题发布，另一个用来将不同移动机器人的任务消息同步订阅下来，用作任务协商；

步骤3、全局路径规划的设计，基于实验场景地图创建一个简化地图，结合A*算法和二阶Bezier曲线实现全局路径规划；全局路径规划具体执行过程包括以下四个方面的内容，一是将起点与终点动态地加入简化地图，二是在简化地图上利用A*算法计算得到一条粗略路径，三是基于实际地图，在粗略路径的相邻节点间，通过A*算法以及二阶Bezier曲线得到各节点间的精确路径，四是将各节点间的路径按节点顺序串联起来作为最终路径；全局路径规划具体包含以下三个设计要点：

(1)基于实验场景地图上创建一个简单有向图G(V,E)，即简化地图，图中节点V对应着实际地图中特定的坐标点，主要分布在道路的两侧及弯道处，节点之间的边E为有向边，用于限制移动机器人路径规划时的路径方向；

(2)设定起点和终点动态添加的规则，起点动态添加规则为：先添加图G中起点周围的节点作为起点下一步可达点，再筛选出这些节点中满足存在出边向外的节点，即有出边不是指向这些周围节点的节点，作为最终与起点相连的节点；终点动态添加规则与前者唯一不同的地方在于它是筛选出周围节点中满足存在外来入边的点作为最终与终点相连的节点，即有入边来自于周围节点之外的节点的节点；

(3)在大弧度转弯处采用二阶Bezier曲线构造路径，首先，判断是否位于大弧度转弯处，即判断是否满足式(1)条件，

式中，start、goal分别表示粗略路径中相邻节点中的起点和终点，x、y分别表示实际地图中的横坐标与纵坐标，单位为m，K_sg表示起点与终点间的斜率，

表示起点与粗略路径中起点的前一节点p_s间的斜率；其次，若满足上述条件，则确定二阶Bezier曲线中的定点point2，即直线

与

的交点为point2，

为粗略路径中过goal和goal下一个节点n_g的直线，若该交点位于障碍区，则将该点沿着与直线l_sg相互垂直的方向移动，直至其位于无障碍区；最后，再利用二阶Bezier曲线构造公式(2)求解构造弯道局部路径的点，

P＝(1-t)²S+2t(1-t)P₂+t²G (2)

式中，S、P₂、G分别对应着上述start、goal、point2，P为待求取的路径上的点，t取值范围为[0,1]的变量；

步骤4、设计任务的分配，多移动机器人调度平台负责向多移动机器人发送总任务，多移动机器人在接收到总任务后开始协商并分配任务，任务分配方式采用多轮分配的方式，每个移动机器人每一轮只分配下一刻要执行的任务，在移动机器人完成当前任务后，再开始下一轮任务分配，直至各移动机器人总任务槽为空才停止任务分配，移动机器人每轮任务协商、分配，具体包括以下子步骤：

(a)在接受到总任务信息后获取自身位置信息，根据总任务信息中各任务的起止点信息，从移动机器人角度考虑，选取代价最小的n个任务t，n为移动机器人数量；有3个移动机器人，故取n为3，选取3个代价最小的任务组成自身任务序列l₁＝(t₁,t₂,t₃)^T，任务选取可根据移动机器人自身约束条件以及任务条件建立代价函数，通过式(3)进行描述，

c(i)＝distance(cur,start_i)+distance(start_i,goal_i) (3)

式中c(i)表示执行任务i所花费的代价值大小，cur表示移动机器人的当前位置，start_i表示任务i起点，goal_i表示任务i终点，distance表示两点间的曼哈顿距离；

(b)将自身任务序列打包成任务消息发送给其它移动机器人，同时接收来自于其它移动机器人的任务序列l₂、l₃，若一段时间T内未接收到移动机器人i发送过来的任务序列，则取上次协商完成后机器人i分配的任务t′，重新构建一个任务序列l_i＝(t′,t′,t′)，并将任务序列l_i作为移动机器人i此刻的任务序列，然后，将步骤(a)中自身挑选的任务序列与接收到的其它移动机器人的任务序列汇总到一个任务序列集S＝(l₁,l₂,l₃)；

(c)根据任务争取规则，若存在多个移动机器人争取同一个任务t情况，则选取离任务t起点近的机器人作为任务t的接收者，从任务序列集S第一行开始，根据各移动机器人与任务起点之间的距离信息，循环比较任务序列集S中的每一行元素值即任务标号，直到某行的所有元素均互异为止，此时每个移动机器人都能分配到不同的任务，倘若当前移动机器人对应的任务序列为l_j，第i行各元素的比较过程存在以下两种情况：若任务l_ij异于所在行的第i行的其它元素，则将任务l_ij分配给当前移动机器人j，并将l_j中当第i个元素以后的元素均置为当前元素值l_ij；若第i行存在与任务l_ij相同的元素，则按照任务争取规则，将任务l_ij分配给距离任务l_ij起点最近的移动机器人k，即令l_ik等于l_ij，将第i行中其它与l_ik元素相同的元素置0值，同时将任务序列l_k中第i行以后的元素置为l_ik值；

步骤5、多移动机器人预防碰撞策略的设计，一方面，通过RPLIDAR-A3激光雷达实时扫描移动机器人周围环境信息，当检测到行进路线上出现障碍物时，在障碍物的一定范围内重新规划局部路径，调整当前全局路径；另一方面，通过ZR300深度实感相机实时检测移动机器人周围的其它移动机器人，并对其位姿进行识别，再根据减速策略对移动机器人速度进行调节，以此保证移动机器人之间的相对安全性，包括红色位姿识别标靶检测及减速策略的设计；

(1)红色位姿识别标靶检测，通过ZR300深度实感相机采集移动机器人正前方的环境信息，在ZR300深度实感相机输出的RGB图像基础上根据红色位姿识别标靶的颜色和形状特征检测红色位姿识别标靶，红色位姿识别标靶共有三种姿态，每种姿态都具有不同的矩形形态，且矩形之间存在一定的空间位置关系，姿态一为单个矩形形态，姿态二为三个矩形形态，姿态三为两个矩形形态；若检测到红色识别标靶，则先求出红色识别标靶整体的形心在图像坐标系下的位置，记作(u_c,v_c)；然后，将此刻ZR300深度实感相机采集到的RGB图像与深度图对齐，读取深度图中对应的形心位置处的深度值，记作Z_c；最后，根据相机的内参数矩阵K求出图像坐标系下的红色位姿识别标靶形心在相机坐标系下的空间位置P＝(X_c,Y_c,Z_c)^T，通过式(4)进行描述，

估计移动机器人在相机坐标系下的姿态，由于所有移动机器人均在同一地图平面上，所以也就是估计欧拉角rpy中的偏航角yaw；当检测到的移动机器人姿态为姿态一时，说明此时移动机器人正处于背对着相机的状态，移动机器人坐标系与相机坐标系x轴方向一致，yaw设为0°；当检测到的移动机器人姿态为位姿三时，说明此时移动机器人与相机正处于面对面的状态，此时yaw设为180°；当检测到的移动机器人姿态为姿态二时，说明此时移动机器人相对于相机正处于横行的状态，可根据移动机器人底盘半径、行走方向、红色位姿识别标靶的转向标识点与形心之间的距离计算yaw，转向标志点即红色位姿识别标靶前侧与后侧衔接区域的中心位置，通过下式(5)进行描述，

式(5)中dir代表识别到的目标机器人的方向，当dir等于0时，表示目标机器人正处于姿态一或姿态三状态；当dir大于0时，表示目标机器人正处于向右状态；当dir小于0时，表示机器人正处于向左的状态；

分别表示标靶转向标识点和标靶中心点在相机坐标系下的y坐标；

(2)减速策略的设计，当与识别到的移动机器人之间的距离大于2m时，保持原有最大移动速度；当与识别到的移动机器人之间的距离小于2m，但自身接下来仍然会处于直行状态时或者接下来行走方向跟识别到的移动机器人当前行走方向相反，则依旧保持原有最大移动速度；排除以上两种情况后，当与识别到的移动机器人之间的距离d小于2m且大于0.7m时，移动机器人的最大速度随着距离d的减小而减小，保证距离d越小，最大移动速度减小的幅度就越大；当与识别到的移动机器人之间的距离d小于0.7m时，最大移动速度强制设置为0，停止当前移动机器人运行；

步骤6、开发多移动机器人调度平台软件，通过Qt以及ROS开发多移动机器人调度平台软件，设计总任务消息格式，创建总任务发布功能以及多移动机器人监控界面，移动机器人监控界面中加载实验环境地图，以不同颜色的圆形图标表示不同的移动机器人，测试和监控多移动机器人调度及运行效果；设计总任务输入框，可输入模拟收集到的总任务，通过点击“Send”按键向多移动机器人发送总任务；设置系统停止运行按钮“Stop”,以便出现紧急状况时，能够即时停止系统的运行。

本发明有益效果是：一种基于多传感器的多移动机器人调度系统设计方法，包括以下步骤：(1)搭建多个移动机器人平台，(2)构建二维实验场景地图及实现多移动机器人无线通讯，(3)全局路径规划的设计，(4)设计任务的分配，(5)多移动机器人预防碰撞策略的设计，(6)开发多移动机器人调度平台软件。与已有技术相比，本发明具有以下几个优点：一、采用无轨自主导航的方式，极大程度上扩大了机器人可自由移动范围，同时在多机器人运行环境中使用靠右全局路径规划的方式，有效地减小了机器人之间发生正面冲突的可能，在机器人多的情况下可大幅度提高机器人运输效率。二、对多机器人采用分布式控制以及多轮协商的任务分配方式，有效地提高了整个调度系统的动态协调性和稳定性。三、利用激光雷达及深度实感相机传感器同时对机器人周围环境进行识别检测，实时调整移动机器人自身运行状态，进一步提高了机器人在整个任务执行过程中的安全性。

附图说明

图1是本发明方法步骤流程图。

图2是实验移动机器人组装模型图。

图3是实验移动机器人实物图。

图4是构建的二维实验场景地图。

图5是多机器人无线通讯拓扑图。

图6是两个机器人之间TCP连接示意图。

图7是靠右全局路径规划方法的路径规划最终效果图。

图8是基于实验场景二维地图构造的简单有向图。

图9是未采用二阶Bezier曲线构造前的弯道局部路径规划图。

图10是基于二阶Bezier曲线构造的弯道局部路径图。

图11是红色位姿识别标靶识别效果图。

图12是机器人位姿识别效果图。

图13是多机器人调度平台软件界面图。

具体实施方式

下面结合附图对本发明作进一步说明。

如图1所示，一种基于多传感器的多移动机器人调度系统设计方法，包括以下步骤：

步骤1、搭建多个移动机器人平台，实验移动机器人组装模型如图2所示，以TurtleBot2移动机器人作为移动底盘，在移动底盘周围外侧设置有红位姿识别标靶，红位姿识别标靶前侧即机器人前侧设置有两道红色条纹，红位姿识别标靶后侧设置有单道红色条纹，红位姿识别标靶前、后侧连接处留出长度为1.5-1.6cm的空白区；在移动底盘上设置有两层亚克力板并与移动底盘形成三层空间，将NVIDIA Jeston TX2开发板作为上位机，安装在第二层中间位置，同时将Intel Realsense ZR300深度实感相机安装第二层前侧正中间；在第三层搭载RPLIDAR-A3激光雷达，最终实验移动机器人搭建效果如图3所示；

步骤2、构建二维实验场景地图及实现多移动机器人无线通讯，包括以下子步骤：

(a)控制移动机器人在实验场景范围内移动，利用RPLIDAR-A3激光雷达扫描环境信息，通过cartographer建图算法构建二维的实验场景地图，最终构建的二维实验场景地图如图4所示；

(b)以一台台式电脑作为多移动机器人调度平台，设置调度平台与移动机器人之间的信息交互方式，使调度平台与移动机器人之间的交互方式为调度平台发送总任务信息给移动机器人、移动机器人反馈位置信息给调度平台；使移动机器人之间的信息交互方式为移动机器人之间任务信息相互交换，整体通讯拓扑图如图5所示；

(c)定义任务消息格式，基于TCP/IP协议在移动机器人及调度平台上创建基于不同网络端口的TCP服务器及客户端，在机器人之间及机器人与调度平台之间建立起TCP连接，机器人之间TCP连接过程如图6所示，通过socket实现任务信息、机器人位置信息、总任务消息的收发；

(d)在移动机器人上创建两个ROS节点，一个用于将其它移动机器人的任务消息通过ROS话题发布，另一个用来将不同移动机器人的任务消息同步订阅下来，用作任务协商；

步骤3、全局路径规划的设计，基于实验场景地图创建一个简化地图，结合A*算法和二阶Bezier曲线实现全局路径规划；全局路径规划具体执行过程包括以下四个方面的内容，一是将起点与终点动态地加入简化地图，二是在简化地图上利用A*算法计算得到一条粗略路径，三是基于实际地图，在粗略路径的相邻节点间，通过A*算法以及二阶Bezier曲线得到各节点间的精确路径，四是将各节点间的路径按节点顺序串联起来作为最终路径，最终全局路径规划效果如图7所示；全局路径规划具体包含以下三个设计要点：

(1)基于实验场景地图上创建一个简单有向图G(V,E)，即简化地图，如图8所示，图中节点V对应着实际地图中特定的坐标点，主要分布在道路的两侧及弯道处，节点之间的边E为有向边，用于限制移动机器人路径规划时的路径方向；

(2)设定起点和终点动态添加的规则，起点动态添加规则为：先添加图G中起点周围的节点作为起点下一步可达点，再筛选出这些节点中满足存在出边向外的节点，即有出边不是指向这些周围节点的节点，作为最终与起点相连的节点；终点动态添加规则与前者唯一不同的地方在于它是筛选出周围节点中满足存在外来入边的点作为最终与终点相连的节点，即有入边来自于周围节点之外的节点的节点；

(3)在大弧度转弯处采用二阶Bezier曲线构造路径，若转弯处直接采用A*算法求取路径会存在图9所示问题，因此需要利用二阶Bezier曲线构造转弯处的局部路径，如图10所示；首先，判断是否位于大弧度转弯处，即判断是否满足式(1)条件，

式中，start、goal分别表示粗略路径中相邻节点中的起点和终点，x、y分别表示实际地图中的横坐标与纵坐标，单位为m，K_sg表示起点与终点间的斜率，

表示起点与粗略路径中起点的前一节点p_s间的斜率；其次，若满足上述条件，则确定二阶Bezier曲线中的定点point2，即直线

与

的交点为point2，

为粗略路径中过goal和goal下一个节点n_g的直线，若该交点位于障碍区，则将该点沿着与直线l_sg相互垂直的方向移动，直至其位于无障碍区；最后，再利用二阶Bezier曲线构造公式(2)求解构造弯道局部路径的点，

P＝(1-t)²S+2t(1-t)P₂+t²G (2)

式中，S、P₂、G分别对应着上述start、goal、point2，P为待求取的路径上的点，t取值范围为[0,1]的变量；

步骤4、设计任务的分配，多移动机器人调度平台负责向多移动机器人发送总任务，多移动机器人在接收到总任务后开始协商并分配任务，任务分配方式采用多轮分配的方式，每个移动机器人每一轮只分配下一刻要执行的任务，在移动机器人完成当前任务后，再开始下一轮任务分配，直至各移动机器人总任务槽为空才停止任务分配，移动机器人每轮任务协商、分配，具体包括以下子步骤：

(a)在接受到总任务信息后获取自身位置信息，根据总任务信息中各任务的起止点信息，从移动机器人角度考虑，选取代价最小的n个任务t，n为移动机器人数量；有3个移动机器人，故取n为3，选取3个代价最小的任务组成自身任务序列l₁＝(t₁,t₂,t₃)^T，任务选取可根据移动机器人自身约束条件以及任务条件建立代价函数，通过式(3)进行描述，

c(i)＝distance(cur,start_i)+distance(start_i,goal_i) (3)

式中c(i)表示执行任务i所花费的代价值大小，cur表示移动机器人的当前位置，start_i表示任务i起点，goal_i表示任务i终点，distance表示两点间的曼哈顿距离；

(b)将自身任务序列打包成任务消息发送给其它移动机器人，同时接收来自于其它移动机器人的任务序列l₂、l₃，若一段时间T内未接收到移动机器人i发送过来的任务序列，则取上次协商完成后机器人i分配的任务t′，重新构建一个任务序列l_i＝(t′,t′,t′)，并将任务序列l_i作为移动机器人i此刻的任务序列，然后，将步骤(a)中自身挑选的任务序列与接收到的其它移动机器人的任务序列汇总到一个任务序列集S＝(l₁,l₂,l₃)；

(c)根据任务争取规则，若存在多个移动机器人争取同一个任务t情况，则选取离任务t起点近的机器人作为任务t的接收者，从任务序列集S第一行开始，根据各移动机器人与任务起点之间的距离信息，循环比较任务序列集S中的每一行元素值即任务标号，直到某行的所有元素均互异为止，此时每个移动机器人都能分配到不同的任务，倘若当前移动机器人对应的任务序列为l_j，第i行各元素的比较过程存在以下两种情况：若任务l_ij异于所在行的第i行的其它元素，则将任务l_ij分配给当前移动机器人j，并将l_j中当第i个元素以后的元素均置为当前元素值l_ij；若第i行存在与任务l_ij相同的元素，则按照任务争取规则，将任务l_ij分配给距离任务l_ij起点最近的移动机器人k，即令l_ik等于l_ij，将第i行中其它与l_ik元素相同的元素置0值，同时将任务序列l_k中第i行以后的元素置为l_ik值；

步骤5、多移动机器人预防碰撞策略的设计，一方面，通过RPLIDAR-A3激光雷达实时扫描移动机器人周围环境信息，当检测到行进路线上出现障碍物时，在障碍物的一定范围内重新规划局部路径，调整当前全局路径；另一方面，通过ZR300深度实感相机实时检测移动机器人周围的其它移动机器人，并对其位姿进行识别，再根据减速策略对移动机器人速度进行调节，以此保证移动机器人之间的相对安全性，包括红色位姿识别标靶检测及减速策略的设计；

(1)红色位姿识别标靶检测，通过ZR300深度实感相机采集移动机器人正前方的环境信息，在ZR300深度实感相机输出的RGB图像基础上根据红色位姿识别标靶的颜色和形状特征检测红色位姿识别标靶，红色位姿识别标靶共有三种姿态，如图11从左到右所示，每种姿态都具有不同的矩形形态，且矩形之间存在一定的空间位置关系，姿态一为单个矩形形态，姿态二为三个矩形形态，姿态三为两个矩形形态；若检测到红色识别标靶，则先求出红色识别标靶整体的形心在图像坐标系下的位置，记作(u_c,v_c)；然后，将此刻ZR300深度实感相机采集到的RGB图像与深度图对齐，读取深度图中对应的形心位置处的深度值，记作Z_c；最后，根据相机的内参数矩阵K求出图像坐标系下的红色位姿识别标靶形心在相机坐标系下的空间位置P＝(X_c,Y_c,Z_c)^T，通过式(4)进行描述，

估计移动机器人在相机坐标系下的姿态，由于所有移动机器人均在同一地图平面上，所以也就是估计欧拉角rpy中的偏航角yaw；当检测到的移动机器人姿态为姿态一时，说明此时移动机器人正处于背对着相机的状态，移动机器人坐标系与相机坐标系x轴方向一致，yaw设为0°；当检测到的移动机器人姿态为位姿三时，说明此时移动机器人与相机正处于面对面的状态，此时yaw设为180°；当检测到的移动机器人姿态为姿态二时，说明此时移动机器人相对于相机正处于横行的状态，可根据移动机器人底盘半径、行走方向、红色位姿识别标靶的转向标识点与形心之间的距离计算yaw，转向标志点即红色位姿识别标靶前侧与后侧衔接区域的中心位置，通过下式(5)进行描述，

式(5)中dir代表识别到的目标机器人的方向，当dir等于0时，表示目标机器人正处于姿态一或姿态三状态；当dir大于0时，表示目标机器人正处于向右状态；当dir小于0时，表示机器人正处于向左的状态；

分别表示标靶转向标识点和标靶中心点在相机坐标系下的y坐标；最终，经ROS中的tf变换，通过ROS的rviz工具显示出来的机器人位姿识别效果如图12所示；

(2)减速策略的设计，当与识别到的移动机器人之间的距离大于2m时，保持原有最大移动速度；当与识别到的移动机器人之间的距离小于2m，但自身接下来仍然会处于直行状态时或者接下来行走方向跟识别到的移动机器人当前行走方向相反，则依旧保持原有最大移动速度；排除以上两种情况后，当与识别到的移动机器人之间的距离d小于2m且大于0.7m时，移动机器人的最大速度随着距离d的减小而减小，保证距离d越小，最大移动速度减小的幅度就越大；当与识别到的移动机器人之间的距离d小于0.7m时，最大移动速度强制设置为0，停止当前移动机器人运行；

步骤6、开发多移动机器人调度平台软件，通过Qt以及ROS开发多移动机器人调度平台软件，设计总任务消息格式，创建总任务发布功能以及多移动机器人监控界面，移动机器人监控界面中加载实验环境地图，以不同颜色的圆形图标表示不同的移动机器人，测试和监控多移动机器人调度及运行效果；设计总任务输入框，可输入模拟收集到的总任务，通过点击“Send”按键向多移动机器人发送总任务；设置系统停止运行按钮“Stop”,以便出现紧急状况时，能够即时停止系统的运行，软件界面如图13所示。

Claims (1)

1.一种基于多传感器的多移动机器人调度系统设计方法，其特征在于包括以下步骤：

步骤1、搭建多个移动机器人平台，以TurtleBot2轮式机器人作为移动底盘，在移动底盘周围外侧设置有红位姿识别标靶，红位姿识别标靶前侧即机器人前侧设置有两道红色条纹，红位姿识别标靶后侧设置有单道红色条纹，红位姿识别标靶前、后侧连接处留出长度为1.5-1.6cm的空白区；在移动底盘上设置有两层亚克力板并与移动底盘形成三层空间，将NVIDIA Jeston TX2开发板作为上位机，安装在第二层中间位置，同时将Intel RealsenseZR300深度实感相机安装第二层前侧正中间；在第三层搭载RPLIDAR-A3激光雷达；

步骤2、构建二维实验场景地图及实现多移动机器人无线通讯，包含以下子步骤：

(a)控制移动机器人在实验场景范围内移动，利用RPLIDAR-A3激光雷达扫描环境信息，通过cartographer建图算法构建二维的实验场景地图；

(b)以一台台式电脑作为多移动机器人调度平台，设置调度平台与移动机器人之间的信息交互方式，使调度平台与移动机器人之间的交互方式为调度平台发送总任务信息给移动机器人、移动机器人反馈位置信息给调度平台；使移动机器人之间的信息交互方式为移动机器人之间任务信息相互交换；

(c)定义任务消息格式，基于TCP/IP协议在移动机器人及调度平台上创建基于不同网络端口的TCP服务器及客户端，在机器人之间及机器人与调度平台之间建立起TCP连接，通过socket实现任务信息、机器人位置信息、总任务消息的收发；

(d)在移动机器人上创建两个ROS节点，一个用于将其它移动机器人的任务消息通过ROS话题发布，另一个用来将不同移动机器人的任务消息同步订阅下来，用作任务协商；

步骤3、全局路径规划的设计，基于实验场景地图创建一个简化地图，结合A*算法和二阶Bezier曲线实现全局路径规划；全局路径规划具体执行过程包括以下四个方面的内容，一是将起点与终点动态地加入简化地图，二是在简化地图上利用A*算法计算得到一条粗略路径，三是基于实际地图，在粗略路径的相邻节点间，通过A*算法以及二阶Bezier曲线得到各节点间的精确路径，四是将各节点间的路径按节点顺序串联起来作为最终路径；全局路径规划具体包含以下三个设计要点：

(1)基于实验场景地图上创建一个简单有向图G(V,E)，即简化地图，图中节点V对应着实际地图中特定的坐标点，主要分布在道路的两侧及弯道处，节点之间的边E为有向边，用于限制移动机器人路径规划时的路径方向；

(2)设定起点和终点动态添加的规则，起点动态添加规则为：先添加图G中起点周围的节点作为起点下一步可达点，再筛选出这些节点中满足存在出边向外的节点，即有出边不是指向这些周围节点的节点，作为最终与起点相连的节点；终点动态添加规则与前者唯一不同的地方在于它是筛选出周围节点中满足存在外来入边的点作为最终与终点相连的节点，即有入边来自于周围节点之外的节点的节点；

(3)在大弧度转弯处采用二阶Bezier曲线构造路径，首先，判断是否位于大弧度转弯处，即判断是否满足式(1)条件，

式中，start、goal分别表示粗略路径中相邻节点中的起点和终点，x、y分别表示实际地图中的横坐标与纵坐标，单位为m，K_sg表示起点与终点间的斜率，

表示起点与粗略路径中起点的前一节点p_s间的斜率；其次，若满足上述条件，则确定二阶Bezier曲线中的定点point2，即直线

与

的交点为point2，

为粗略路径中过goal和goal下一个节点n_g的直线，若该交点位于障碍区，则将该点沿着与直线l_sg相互垂直的方向移动，直至其位于无障碍区；最后，再利用二阶Bezier曲线构造公式(2)求解构造弯道局部路径的点，

P＝(1-t)²S+2t(1-t)P₂+t²G (2)

式中，S、P₂、G分别对应着上述start、goal、point2，P为待求取的路径上的点，t取值范围为[0,1]的变量；

步骤4、设计任务的分配，多移动机器人调度平台负责向多移动机器人发送总任务，多移动机器人在接收到总任务后开始协商并分配任务，任务分配方式采用多轮分配的方式，每个移动机器人每一轮只分配下一刻要执行的任务，在移动机器人完成当前任务后，再开始下一轮任务分配，直至各移动机器人总任务槽为空才停止任务分配，移动机器人每轮任务协商、分配，具体包括以下子步骤：

(a)在接受到总任务信息后获取自身位置信息，根据总任务信息中各任务的起止点信息，从移动机器人角度考虑，选取代价最小的n个任务t，n为移动机器人数量；有3个移动机器人，故取n为3，选取3个代价最小的任务组成自身任务序列l₁＝(t₁,t₂,t₃)^T，任务选取可根据移动机器人自身约束条件以及任务条件建立代价函数，通过式(3)进行描述，

c(i)＝distance(cur,start_i)+distance(start_i,goal_i) (3)

式中c(i)表示执行任务i所花费的代价值大小，cur表示移动机器人的当前位置，start_i表示任务i起点，goal_i表示任务i终点，distance表示两点间的曼哈顿距离；

(b)将自身任务序列打包成任务消息发送给其它移动机器人，同时接收来自于其它移动机器人的任务序列l₂、l₃，若一段时间T内未接收到移动机器人i发送过来的任务序列，则取上次协商完成后机器人i分配的任务t′，重新构建一个任务序列l_i＝(t′,t′,t′)，并将任务序列l_i作为移动机器人i此刻的任务序列，然后，将步骤(a)中自身挑选的任务序列与接收到的其它移动机器人的任务序列汇总到一个任务序列集S＝(l₁,l₂,l₃)；

(c)根据任务争取规则，若存在多个移动机器人争取同一个任务t情况，则选取离任务t起点近的机器人作为任务t的接收者，从任务序列集S第一行开始，根据各移动机器人与任务起点之间的距离信息，循环比较任务序列集S中的每一行元素值即任务标号，直到某行的所有元素均互异为止，此时每个移动机器人都能分配到不同的任务，倘若当前移动机器人对应的任务序列为l_j，第i行各元素的比较过程存在以下两种情况：若任务l_ij异于所在行的第i行的其它元素，则将任务l_ij分配给当前移动机器人j，并将l_j中当第i个元素以后的元素均置为当前元素值l_ij；若第i行存在与任务l_ij相同的元素，则按照任务争取规则，将任务l_ij分配给距离任务l_ij起点最近的移动机器人k，即令l_ik等于l_ij，将第i行中其它与l_ik元素相同的元素置0值，同时将任务序列l_k中第i行以后的元素置为l_ik值；

步骤5、多移动机器人预防碰撞策略的设计，一方面，通过RPLIDAR-A3激光雷达实时扫描移动机器人周围环境信息，当检测到行进路线上出现障碍物时，在障碍物的一定范围内重新规划局部路径，调整当前全局路径；另一方面，通过ZR300深度实感相机实时检测移动机器人周围的其它移动机器人，并对其位姿进行识别，再根据减速策略对移动机器人速度进行调节，以此保证移动机器人之间的相对安全性，包括红色位姿识别标靶检测及减速策略的设计；

(1)红色位姿识别标靶检测，通过ZR300深度实感相机采集移动机器人正前方的环境信息，在ZR300深度实感相机输出的RGB图像基础上根据红色位姿识别标靶的颜色和形状特征检测红色位姿识别标靶，红色位姿识别标靶共有三种姿态，每种姿态都具有不同的矩形形态，且矩形之间存在一定的空间位置关系，姿态一为单个矩形形态，姿态二为三个矩形形态，姿态三为两个矩形形态；若检测到红色识别标靶，则先求出红色识别标靶整体的形心在图像坐标系下的位置，记作(u_c,v_c)；然后，将此刻ZR300深度实感相机采集到的RGB图像与深度图对齐，读取深度图中对应的形心位置处的深度值，记作Z_c；最后，根据相机的内参数矩阵K求出图像坐标系下的红色位姿识别标靶形心在相机坐标系下的空间位置P＝(X_c,Y_c,Z_c)^T，通过式(4)进行描述，

估计移动机器人在相机坐标系下的姿态，由于所有移动机器人均在同一地图平面上，所以也就是估计欧拉角rpy中的偏航角yaw；当检测到的移动机器人姿态为姿态一时，说明此时移动机器人正处于背对着相机的状态，移动机器人坐标系与相机坐标系x轴方向一致，yaw设为0°；当检测到的移动机器人姿态为位姿三时，说明此时移动机器人与相机正处于面对面的状态，此时yaw设为180°；当检测到的移动机器人姿态为姿态二时，说明此时移动机器人相对于相机正处于横行的状态，可根据移动机器人底盘半径、行走方向、红色位姿识别标靶的转向标识点与形心之间的距离计算yaw，转向标志点即红色位姿识别标靶前侧与后侧衔接区域的中心位置，通过下式(5)进行描述，

式(5)中dir代表识别到的目标机器人的方向，当dir等于0时，表示目标机器人正处于姿态一或姿态三状态；当dir大于0时，表示目标机器人正处于向右状态；当dir小于0时，表示机器人正处于向左的状态；

分别表示标靶转向标识点和标靶中心点在相机坐标系下的y坐标；

(2)减速策略的设计，当与识别到的移动机器人之间的距离大于2m时，保持原有最大移动速度；当与识别到的移动机器人之间的距离小于2m，但自身接下来仍然会处于直行状态时或者接下来行走方向跟识别到的移动机器人当前行走方向相反，则依旧保持原有最大移动速度；排除以上两种情况后，当与识别到的移动机器人之间的距离d小于2m且大于0.7m时，移动机器人的最大速度随着距离d的减小而减小，保证距离d越小，最大移动速度减小的幅度就越大；当与识别到的移动机器人之间的距离d小于0.7m时，最大移动速度强制设置为0，停止当前移动机器人运行；

步骤6、开发多移动机器人调度平台软件，通过Qt以及ROS开发多移动机器人调度平台软件，设计总任务消息格式，创建总任务发布功能以及多移动机器人监控界面，移动机器人监控界面中加载实验环境地图，以不同颜色的圆形图标表示不同的移动机器人，测试和监控多移动机器人调度及运行效果；设计总任务输入框，可输入模拟收集到的总任务，通过点击“Send”按键向多移动机器人发送总任务；设置系统停止运行按钮“Stop”,以便出现紧急状况时，能够即时停止系统的运行。

Images