基于双目视觉和蚁群算法的捡乒乓球机器人运行方法及设备与流程

本发明涉及机器人技术领域，具体涉及一种基于双目视觉和蚁群算法的捡乒乓球机器人运行方法及设备。

背景技术：

随着人工智能技术以及我国社会经济的发展，服务型机器人逐渐走进大众的家庭。乒乓球作为国球，深受人民群众的喜爱。但是随之而来的大量的捡球工作却让人很烦恼，目前乒乓球俱乐部中有专人负责捡球，人力成本较高。而市面上没有一款可以普及的捡乒乓球机器人，仅有的研究也偏重于学术研究，实际的工程应用很少，且大都只有零散的方法。

现有的检测和定位技术复杂繁琐，利用普通的rgb摄像头很难完成对乒乓球的定位任务，因此大多数捡球机器人无法对球进行定位，因而无法实现对所有球的全局遍历路径规划，导航方法很低效。

蚁群算法是一种仿生学算法，主要用于tsp问题，它可在很短时间内求得一个接近最优的解。但是它实际应用在捡球机器人上时，受到机器人宽度的限制，需要进行进一步优化。

技术实现要素：

本发明的目的在于克服现有技术中的不足，提出了一种基于双目视觉和蚁群算法的捡乒乓球机器人运行方法及设备，实现了乒乓球的检测和定位，并对规划出的路径进行冗余路线优化，进一步减少了机器人的行走路径，节约了时间。

为解决上述技术问题，本发明提供了一种基于双目视觉和蚁群算法的捡乒乓球机器人运行方法，其特征是，包括以下过程：

建立乒乓球训练场地的全局坐标系；

获取双目摄像机拍摄的场地中地面乒乓球散落情况的rgb图像，从rgb图像中检测出乒乓球并获得各乒乓球的坐标位置；

获取双目摄像机的深度数据，得到各乒乓球距离摄像机的距离；

根据各乒乓球的坐标位置和距离摄像机的距离，计算得到在全局坐标系下乒乓球的坐标点集；

对坐标点集使用蚁群算法进行最短遍历路径规划，得到初步的捡球路径；

从初步的捡球路径去除多次经过的乒乓球，得到没有冗余路线的路径。

进一步的，从rgb图像中检测出乒乓球并获得各乒乓球的坐标位置的具体过程包括：

将rgb图像转换为hsv图像；

将hsv图像转成灰度图，进行高斯模糊；

使用canny算子进行轮廓检测，获得图中所有物体的轮廓；

使用霍夫圆变换检测图中的圆，即可检测出场地中的乒乓球，并计算得到所有乒乓球在图中的坐标位置。

进一步的，根据各乒乓球的坐标位置和距离摄像机的距离计算得到在全局坐标系下乒乓球的坐标点集包括以下过程：

将各乒乓球的坐标位置和距离摄像机的距离，转换到相机三维坐标系下，得到在相机三围坐标系下乒乓球的坐标点集；

再将乒乓球在相机三维坐标系下的坐标转换到机器人坐标系下；

最后将乒乓球在机器人坐标系下的坐标转换到全局坐标系下，得到在全局坐标系下乒乓球的坐标点集。

进一步的，将各乒乓球的坐标位置和距离摄像机的距离转换到相机三维坐标系下的过程包括：

建立相机三维坐标系xyz、图像平面坐标系xy；

相机三围坐标系中点p(x，y，z)和图像平面坐标系中点p(x，y)两者的映射关系为：

其中，f为相机焦距，z为从深度数据中获取的乒乓球的深度，x表示图像平面坐标系的x轴坐标，x表示相机三维坐标系中的x轴坐标；y表示图像平面坐标系中的y轴坐标，y表示相机三维坐标系中的y轴坐标；cx为主点在x轴上的坐标，cy为主点在y轴上的坐标，fx为f在x轴上的像素量纲，fy为f在y轴上的像素量纲；

各乒乓球的坐标位置为图像平面坐标系中坐标，距离摄像机的距离为相机坐标系中z轴数值，根据上述公式计算得到乒乓球在相机坐标系中的三维坐标p(x，y，z)。

进一步的，将乒乓球在机器人坐标系下的坐标转换到全局坐标系下的过程为：

起始时机器人坐标系在全局坐标系的原点，坐标为(x0，y0，th0)＝(0，0，0)，设机器人移动速度v＝(vx，vy，vth)，vx指沿x轴方向的速度，vy指沿着y轴方向的速度，vth指机器人旋转速度；则经过dt时间后，机器人坐标系相对于全局坐标原点的坐标为：

(x1，y1，th1)＝(x0+(vx*cos(th0)-vy*sin(th0))*dt，y0+(vx*sin(th0)+vy*cos(th0))*dt，th0+vth*dt)(5)

根据公式(5)得到的机器人坐标系的坐标转换关系，可以进行坐标转换，在机器人坐标系下的目标乒乓球坐标(x′，y′)转换到全局坐标系下的坐标(x″，y″)为：

(x″，y″)＝(x1+(x′*cos(th1)-y′*sin(th1))，

y1+(x′*sin(th1)+y′*cos(th1)))

完成所有乒乓球的坐标转换，得到在全局坐标系下乒乓球的坐标点集。

进一步的，从初步的捡球路径中去除多次经过的乒乓球，得到没有冗余路线的路径包括以下过程：

根据初步的捡球路径，假设机器人移动从出发点移动到第1个目标乒乓球，

画出直线b1、b2与d1、d2，b1、b2平行于出发点点和目标乒乓球两点的连线，并且与机器人在出发点的最小外接圆相切；直线d1与两点连线方向垂直并与机器人在出发点的最小外接圆的尾部相切；直线d2与两点连线方向垂直并与机器人在目标点的最小外接圆的头部相切；这四条线构成的矩形表示机器人从出发点到目标点扫过的区域，计算取得在四条线构成的矩形中的所有乒乓球坐标点，将这些点从坐标点集中删去，

然后将此目标点作为起始点，从初步的捡球路径中选取下一个目标点，进行上述同样的操作；以此类推，直到捡球路径上所有的点全部处理完毕，最终得到一条没有冗余路线的路径。

相应的，本发明还提供了一种存储一个或多个程序的计算机可读存储介质，所述一个或多个程序包括指令，所述指令当由计算设备执行时，使得所述计算设备执行上述方法。

相应的，本发明还提供了一种计算设备，包括，

一个或多个处理器、存储器以及一个或多个程序，其中一个或多个程序存储在所述存储器中并被配置为由所述一个或多个处理器执行，所述一个或多个程序包括用于执行上述方法的指令。

与现有技术相比，本发明所达到的有益效果是：本发明使用双目摄像机同时实现了乒乓球的检测和定位，方便高效，同时保证了精度，使得捡球过程更加智能化；对蚁群算法规划出的路径进行冗余路线优化，进一步减少了机器人的行走路径，节约了时间。

附图说明

图1是本发明的方法的流程示意图；

图2是乒乓球检测和定位的流程示意图；

图3是相机小孔成像模型示意图；

图4是base_link和map坐标系转换图；

图5是蚁群算法用于遍历路径规划的流程示意图；

图6是冗余点示意图。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

本发明的一种基于双目视觉和蚁群算法的捡乒乓球机器人运行方法，参见图1所示，包括以下步骤：

步骤1，获取乒乓球训练场地的地图，建立全局坐标系。

首先使用激光雷达以及ros(机器人操作系统)平台下的gmapping建图程序获得乒乓球训练场地的静态地图，然后对地图进行去噪和修饰，得到可用的全局静态栅格地图，机器人根据这个地图来进行导航避障。作为全局坐标系map，此坐标系以矩形训练场地的某个角点作为原点，沿着训练场地的长宽方向分别作为横纵坐标轴。后面检测到的乒乓球将会转换到这个坐标系下显示。

步骤2，获取双目摄像机的rgb图像，从rgb图像中检测出乒乓球并获取其在图中的坐标点。

双目摄像机使用kinect，它有一个rgb彩色摄像头和一个深度摄像头。双目摄像机安装在机器人前部正中位置，视角平视前方，距离地面高度取15cm到40cm为宜。机器人在捡球任务开始之前拍摄地面的乒乓球，在ros平台中同时接收同一时刻的kinect的rgb数据和深度数据，保证获得的两个数据时间戳是一致的。

如图2所示，取出某一时间kinect拍摄的场地内包含若干乒乓球的rgb图像，并将图像转换为hsv图像，按白色乒乓球的hsv数值阈值进行过滤，取出图像中白色部分，即取出包括乒乓球在内的白色物体图像。然后将hsv图像转成灰度图，进行高斯模糊，让图像边缘更平滑。再使用canny算子进行轮廓检测，设置阈值为100到200，取出图中所有物体的轮廓。最后使用霍夫圆变换检测图中的圆，即可检测出场地中的乒乓球(乒乓球在图中的坐标)，并计算得到所有乒乓球在图中的坐标点集，并对这些检测出的乒乓球按照检测的顺序从1到n(乒乓球个数)进行编号。

步骤3，获取双目摄像机的深度数据，根据乒乓球的二维坐标得到其对应的深度信息(即乒乓球距离相机的水平距离)，根据所述深度信息计算出乒乓球在机器人坐标系下的坐标。

根据相机针孔模型，即理想化的小孔成像模型，得到两个坐标系，如图3所示一个是相机三维坐标系xyz，其中x轴平行于地面且平行于图像平面，y轴垂直于地面，z轴垂直于图像平面；一个是图像平面坐标系xy，图像平面垂直于地面；根据几何中的截线定理，得到相机三围坐标系中点p(x，y，z)和图像平面坐标系中点p(x，y)两者的映射关系为：

其中，f为相机焦距，z为从深度数据中获取的乒乓球的深度，x表示图像平面坐标系的x轴坐标，x表示相机三维坐标系中的x轴坐标；y表示图像平面坐标系中的y轴坐标，y表示相机三维坐标系中的y轴坐标。

根据相机内参，对以上步骤进行改进：

由于实际的差异，每个相机的主点(图像平面坐标系的原点)，不一定都在图像的正中心；x和y方向的像素也有差异，因此将上述公式(1)(2)变换为：

其中，cx为主点在x轴上的坐标，cy为主点在y轴上的坐标，fx为f在x轴上的像素量纲，fy为f在y轴上的像素量纲。

如流程图2所示。首先调用接口函数获取相机的内参矩阵，然后根据上述公式(3)(4)计算得到目标乒乓球在相机坐标系中的三维坐标p(x，y，z)(x、y由上面计算得到，z即为该点的深度，也就是乒乓球距离相机的水平距离)。由于相机三维坐标系和机器人坐标系(名为base_link，以机器人前进方向为x轴，左边为y轴)方向不一致，存在转换关系，最终乒乓球在机器人坐标系下的坐标为(x′，y′)＝(z，x)(假设乒乓球全部在地面上，高度坐标为0)，并在ros平台下的rviz(ros可视化工具)中显示。

重复以上过程，得到图像里地面上所有的乒乓球组成的在机器人坐标系下的坐标点集。

步骤4：根据机器人坐标系和全局坐标系的转换关系，对机器人坐标系下的乒乓球进行坐标转换，得到乒乓球在全局坐标系下的坐标点集。

全局坐标系名为map，机器人坐标系名为base_link。起始时base_link坐标系在map坐标系的原点(坐标为(x0，y0，th0)＝(0，0，0)，x，y，th分别指x轴坐标，y轴坐标，两个坐标系偏移的角度)，设机器人移动速度v＝(vx，vy，vth)，vx指沿x轴方向的速度，vy指沿着y轴方向的速度，vth指机器人旋转速度。则经过dt时间后，base_link坐标系相对于全局坐标原点的坐标为：

(x1，y1，th1)＝(x0+(vx*cos(th0)-vy*sin(th0))*dt，y0+(vx*sin(th0)+vy*cos(th0))*dt，th0+vth*dt)(5)

根据公式(5)得到的base_link坐标系的坐标转换关系，可以进行坐标转换。由base_link坐标系的原点(x1，y1)以及偏转角th1可以计算全局坐标系下的坐标(由图4所示)。在机器人坐标系base_link下的目标乒乓球坐标(x′，y′)转换到全局坐标系map下的坐标(x″，y″)为：

(x″，y″)＝(x1+(x′*cos(th1)-y′*sin(th1))，

y1+(x′*sin(th1)+y′*cos(th1)))

至此完成了全部的坐标转换工作。

步骤5：对得到的乒乓球坐标点集使用蚁群算法进行最短遍历路径规划，得到初步的捡球顺序。

首先按照检测乒乓球时的编号顺序，计算所有乒乓球两两之间的距离并组合成二维邻接矩阵graph，矩阵行和列维度都为图中检测出的乒乓球个数n，用于进行最短路径的计算。矩阵中的元素即为乒乓球之间的距离，如行i和列j表示第i个球和第j个球之间的距离。有n个乒乓球，则矩阵中元素的个数为n²。

如图5所示，开始时先初始化蚁群算法的参数α(信息素的加权值)、β(能见度的加权值)、ρ(信息素的蒸发率)、q(信息素增益系数)、初始化信息素τ0、迭代次数n。

然后为每个蚂蚁随机选择一个乒乓球作为出发点，并将出发点加入到已经经过的点集合c。分别计算当前点到剩下未经过的所有点(集合o)的概率p＝τ^α*(γ)^β其中τ为两点之间信息素信息素，γ为两点之间距离(距离从邻接矩阵graph中取得)的倒数。采用轮盘赌的方法，为每个蚂蚁选择下一个点。即取一个随机数q，计算到第一个点的概率，如果小于q则继续计算下一个点；如果到某个点的概率累加和大于q，则取该点。例如，q＝0.4，p1＝0.2，p2＝0.3，p3＝0.5，(pn表示选择第n个点的概率)，则由于p1＜0.4，且p1+p2＞0.4，所以取第二个点作为下一个目标。以此类推将所有蚂蚁移动到下一个目标点，并将该点加入到已经经过的集合中(集合c)。

重复以上步骤，直到所有的点都加入到已经经过的集合中。这样就完成一次迭代。

按照选择的路径计算每只蚂蚁走过的路径长度，即根据路径顺序，计算乒乓球之间的距离并累加求和，例如路径顺序为2-1-3-4(序号在前面步骤已经标定)，则路径长度为2号乒乓球到1号乒乓球的距离d1，加上1号球到3号球的距离d2，加上3号球到四号球的距离d3，即l＝d1+d2+d3。所有蚂蚁走完之后更新每一条边的信息素：每条路径上两点之间的信息素增量与他们所在路径的长度成反比：add＝q/lk(q一般取50-100)，其中lk表示第k条路径的长度，add表示信息素的增加量。因此更新后的信息素为τ＝(1-ρ)*τ0+q/lk。

使用新的信息素继续下一轮迭代；如果(1)迭代次数达到设定值n(50次)都已经完成，或者(2)信息素增量add小于一定阈值(如0.01)，则输出遍历所有点的近似最优解(路径长度最短)。

步骤6：根据机器人自身形状和规划出来的路径，去除多次经过的乒乓球，进一步缩短路径长度。

如图6所示(全局坐标系map中)，根据步骤5路径规划得到的顺序，假设机器人移动的第1个目标乒乓球为2号点(数字序号按照步骤2中识别的乒乓球顺序标定，0号点表示机器人在识别乒乓球时的当前坐标点，2号点即为第2个(编号在检测乒乓球时已经标定)识别出的乒乓球的坐标点，它在路径规划中作为第1个目标点)，画出直线b1、b2与d1、d2，b1、b2平行于0号点和2号点两点的连线，并且与机器人在0号点的最小外接圆(由于机器人形状可能不规则，能够完全把机器人包围在内部的最小圆称为最小外接圆)相切；直线d1与两点连线方向垂直并与机器人在出发点的最小外接圆的尾部相切；直线d2与两点连线方向垂直并与机器人在2号点的最小外接圆的头部相切；这四条线构成的矩形表示机器人从0号点到2号点扫过的区域(出于对效率的考虑，对于矩形四个角未覆盖的部分忽略不计)。计算取得在四条线构成的矩形中的所有乒乓球坐标点，即图4中的点4、7、15，将这些点从坐标点集中删去，因为机器人携带吸力装置或者滚轮捡球装置，所以机器人在从0点移动到2号点的过程中，会经过这几个乒乓球，将它们都捡起来，即机器人为可将最小外接圆半径宽度范围内的球都捡起来。删掉这些点就减少冗余遍历点，提高运行效率。

然后将2号点作为起始点，从步骤5的路径规划中选取下一个目标点，对这两个点按照0号点和2号点的操作过程进行同样的操作。以此类推，重复步骤6，直到步骤5中计算得到的路径上所有的点全部处理完毕，最终得到一条没有冗余路线的路径，机器人按这条路径进行捡球工作的效率比仅用步骤5中的路径运行要高效，节省了时间。

相应的，本发明还提供了一种存储一个或多个程序的计算机可读存储介质，所述一个或多个程序包括指令，所述指令当由计算设备执行时，使得所述计算设备执行上述方法。

相应的，本发明还提供了一种计算设备，包括，

一个或多个处理器、存储器以及一个或多个程序，其中一个或多个程序存储在所述存储器中并被配置为由所述一个或多个处理器执行，所述一个或多个程序包括用于执行上述方法的指令。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变型，这些改进和变型也应视为本发明的保护范围。