[发明专利]一种多机器人协作分拣运输方法及系统

权利要求书

1.一种多机器人协作分拣运输方法，其特征在于，包括以下步骤：

S1，构建图像采集单元

所述图像采集单元包括放置在工作场地四周的多个网络监控摄像头，用于对工作场地进行监控，多个所述网络监控摄像头分别与交换器连接，并经由路由器受控于中央处理服务器；

S2，构建任务控制执行单元

所述任务控制执行单元包括运输机器人、分拣机器人、中央处理服务器、物料以及物料台，所述运输机器人和分拣机器人上均贴设有ARtag码，所述运输机器人用于搬运物料，所述分拣机器人用于分拣物料，所述运输机器人和分拣机器人均在工作场地的监控区域内移动，并受控于中央处理服务器；

S3，通过图像采集单元采集工作场地图像；

S4，进行相机标定及相机与物料台位置标定，得到各相机的内参数、像素平面与相机坐标系以及相机坐标系与世界坐标系的映射关系，并建立世界坐标系，同时判断标定是否成功，若是，则进入步骤S5，反之，则返回步骤S3；

S5，通过图像采集单元采集下一帧工作场地图像；

S6，利用图像采集单元采集的所有帧工作场地图像，分别对工作场地内的各机器人、物料识别与定位，具体步骤如下：

S61，分别对工作场地内各机器人贴设的ARtag码进行检测与识别，并判断是否检测识别成功，若是，则进入步骤S62，反之，则返回步骤S5；其中，ARtag码进行检测与识别的具体过程如下：

S611，将采集到的所有帧工作场地图像进行阈值化，得到二值图像，接着进行轮廓检测；

S612，经过滤波筛选，定位得到有效的ARtag码所在图像中的区域；

S613，对步骤S612定位得到的区域进行仿射变换，得到ARtag码中的图案轮廓，接着读取比特位进行解码，得到图像中ARtag码对应的ID号和四个角的像素坐标，计算可得中心坐标；

S614，对每个码进行PnP解码，得到每个ARtag码在图像中的姿态；

S615，判断PnP解码是否成功，若是，则进入步骤S616；反之，则返回步骤S5；

S616，输出ARtag码ID号、中心像素坐标和ARtag码姿态；

S62，对识别到的ARtag码进行筛选、解码并匹配到目标机器人，得到在不同相机上机器人的ARtag码的像素中心坐标和世界坐标系下的姿态信息；

S63，根据得到的机器人ARtag码像素位置信息以及各相机的内参数，去除网络监控摄像头的延时影响，再通过多相机融合定位算法，实时识别并定位世界坐标系中的机器人位置；其中，多相机融合定位算法根据多个相机的内外参数和ARtag码世界坐标中心点投影在多个相机上的像素坐标点，计算得到该ARtag码世界坐标中心点的世界坐标，具体表现为：

S631，推导相机、世界坐标系映射公式，相机标定的映射公式如下：

（1）

式中，P为ARtag码世界坐标中心点，n为摄像头的相机数量分别为P对于相机1、2、...n在投影点的深度信息，分别为相机1、2、...n的相机参数；

S632，移项、代换得到向量H，H表示为：

（8）

式中，I为3*3的单位矩阵，均为中间变量，其中，

（3）

式中，分别为相机外参旋转矩阵的逆矩阵，分别为相机内参矩阵的逆矩阵；

S633，对向量H进行SVD分解，H表示为：

（11）

式中，U、V表示酉矩阵，为半正定对角矩阵；

S634，通过如下公式计算得到ARtag码世界坐标中心点的世界坐标

（13）

式中，为H的伪逆矩阵，Q为中间变量，其表达式如下：

（9）

其中，均为中间变量：

（6）

式中，0为三维零向量；

通过运动补偿算法去除网络监控摄像头的延时影响，具体表现为：

（a）在完成ARtag码识别与多相机融合定位得到机器人在秒之前的位置ARtag码世界坐标中心点P的前提下，通过机器人上的陀螺仪传感器实时采集机器人秒内的线速度与角速度，其中，；

（b）机器人当前实际位置为：

（14）

其中，为秒时间段内机器人的位移，即：

（15）

其中，表示机器人在秒内某时刻的线速度矢量，复数形式为：

（16）

式中，为机器人在秒内某时刻的角度，且其表达式如下：

（17）；

（c）上述为理想情况下时间连续的形式，实际上通过如下时间公式对进行抽样：

（18）

式中，N为正整数，则：

（19）

（d）完成运动补偿，得到当前时刻机器人的真实位置；

S64，识别和定位物料台上的物料，并判断物料是否识别和定位成功，若是，则进入步骤S7，反之，则返回步骤S5；

S7，输出机器人位姿信息和物料位置信息，并根据机器人位姿信息和物料位置信息完成分拣运输任务的分配；

S8，根据工作场地内已知障碍物位置，完成各机器人的路径规划；

S9，中央处理服务器根据得到的机器人、物料位置信息作为参考信号，使用PID闭环控制算法控制各机器人按照步骤S8中规划好的路径完成分拣运输工作。