[发明专利]一种基于多传感器的多移动机器人调度系统设计方法

权利要求书

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”,以便出现紧急状况时，能够即时停止系统的运行。