基于多目标路径规划与智能拾取的机器人控制方法

1.本发明涉及机器人控制技术领域，尤其涉及基于多目标路径规划与智能拾取的机器人控制方法。


背景技术：

2.随着人工智能的发展，机器人在工业和日常生活中扮演着越来越重要的角色，在机器人多目标拾取任务中，更好的路径规划效果和高效的拾取方法已成为当前研究热点问题。一般情况下，多目标路径规划可考虑为旅行商(tsp)问题，解决tsp问题的方法可分为经典方法和启发式方法，经典方法包括：回溯法、分支定界法、动态规划法、贪心算法、栅格法等；经典方法的时间复杂度和空间复杂度随目标数目的增加而急剧增大，计算成本呈指数级上升，同时遇到不确定因素时，算法容易失效。启发式方法包括：模拟退火法、神经网络算法、遗传算法、蚁群算法等，模拟退火法具有较强的局部搜索能力，但全局搜索能力较差；神经网络算法在目标规模较大时易于收敛到局部最小解；遗传算法局部搜索能力较差，易于早熟；蚁群算法具有并行性、正反馈性和鲁棒性的优点，求解多目标路径规划问题具有较大优势，算法求解问题时，在解空间中同时构建多个解，并行求解能力强，算法通过信息素引导蚂蚁选择路径，信息素越强，蚂蚁通过的可能性越大，通过的蚂蚁越多，路径上的信息素越强，以此形成正反馈，算法运行结束后，从解空间中选择最优解作为输出，因此，不会因为某个解较差而使算法失效，具有较强的鲁棒性。
3.基于蚁群算法以上优点，越来越多的学者不断改进蚁群算法，使其能够更好的运用到机器人多目标路径规划中；zhen等人使用交叉操作，改善算法停滞问题，提高了蚁群算法的优化能力；li等人通过优化初始信息素的分布，加快了蚁群算法的搜索速度；liao等人使用密度峰值聚类算法将旅行商问题分解成规模较小的子问题，每个子问题采用蚁群算法进行路径规划，从而能够解决目标数量较多的问题；yu等人提出了一种粒子群优化算法和蚁群算法组成的融合算法，通过赋予蚁群一定的“粒子性质”来获得最佳效果；paniri等人提出一种新的蚁群优化方法用于解决多标签相关性-冗余特征选择问题，该方法通过引入有监督的启发式函数，加快了算法的收敛速度；yang等人提出了一种双层蚁群路径优化算法，首先通过并行精英蚁群算法生成初始路径，然后使用转折点算法和路段平滑算法对分路径进行平滑处理，得到机器人运动最优路径。以上例子中，由于作者只考虑目标距离的路径规划，得到的只是最短路径，然而，在机器人拾取多目标中，机器人除了直行拾取目标外，还需要旋转一定角度对准目标，因此，只考虑目标距离的路径规划并不能得到机器人拾取目标的最优路径。


技术实现要素：

4.针对上述存在的问题，本发明旨在提供一种基于多目标路径规划与智能拾取的机器人控制方法，该方法能够提升机器人路径规划能力，使路径规划结果满足机器人拾取路径最优。
5.为了实现上述目的，本发明所采用的技术方案如下：
6.基于多目标路径规划与智能拾取的机器人控制方法，其特征在于，包括以下步骤，
7.s1：建立机器人拾取所有目标的最优路径数学模型；
8.s2：利用同时考虑旋转角度和目标实际距离的蚁群优化算法，对步骤s1中建立的最优路径数学模型进行求解，得到最优路径；
9.s3：利用双摄像头智能拾取方法，引导机器人进行目标拾取。
10.进一步的，步骤s1的具体操作包括以下步骤，
11.s101：定义机器人拾取所有目标的数学模型为g(v，e，f)，式中，v为待拾取目标集v＝{v1，v2，...，vn}，vi为第i个目标，目标总数为n，e为两目标之间实际距离路径集合，e＝{l1(vi，vj)，l1(vi，vj)＞0，l1(vi，vi)＝0
，
i，j∈n}，f为拾取完当前目标后，拾取下一个目标时需要旋转的弧长集合，f＝{l2(vi，vj)，l2(vi，vj)≥0，l2(vi，vi)＝0，i，j∈n}；
12.式中，l1(vi，vj)表示目标vi与目标vj之间的实际距离，式中，(xi，yi)和(xj，yj)分别为目标vi和vj的实际坐标；
13.l2(vi，vj)表示拾取完目标vi后拾取下一个目标vj时机器人需要旋转的弧长，式中，r为机器人动力轴半径，θ为拾取完目标vi后，在拾取目标vj前需要旋转的角度；
14.s102：令机器人拾取每个目标的有效距离为l(vi，vj)，则l(vi，vj)＝l1(vi，vj) l2(vi，vj)；
15.s103：定义f(v)为拾取所有目标v＝{v1，v2，...，vn}的路径目标函数，则
16.s104：机器人拾取所有目标的最优路径问题，也即目标函数最小化问题，则机器人拾取所有目标的最优路径数学模型为
17.进一步的，步骤s2的具体操作包括以下步骤，
18.s201：基于两点像素坐标计算两点实际距离的方法，计算目标vi和vj的实际距离值；
19.s202：使用蚁群优化算法求解机器人拾取所有目标的最优路径数学模型
20.进一步的，步骤s201的具体操作包括以下步骤，
21.s2011：定义目标vi与目标vj所在平面内任一点q在空间坐标系中的坐标为(xm，ym，zm)；以主摄像头为坐标原点建立相机坐标系oxyz，q点在相机坐标系oxyz中的坐标为(xc，yc，zc)；在相机成像的图像平面建立图像坐标系o1x1y1，q点映射到图像坐标系o1x1y1中的坐
标定义为(x
p
，y
p
)；
22.s2012：定义r为3*3的旋转矩阵，ts为3*1的平移矩阵，则q点在空间坐标系和相机坐标系oxyz之间的转换关系[xc，yc，zc]
t
＝r*[xm，ym，zm]
t
ts，矩阵形式表示为
[0023]
s2013：q点从相机坐标系oxyz映射到图像坐标系o1x1y1的变换关系为其中，f为摄像头到图像平面的距离，因此矩阵形式表示为
[0024]
s2014：定义相机成像的图像中每个像素点面积为dx
×
dy，以图像中点为坐标原点，建立像素坐标系ouv，定义q点映射到像素坐标系ouv中的坐标为(u，v)，则q点从图像坐标系o1x1y1到像素坐标系ouv的变换关系为矩阵形式表示为式中，(u0，v0)为图像中点在像素坐标系中的映射坐标；
[0025]
s2015：结合步骤s2012、s2013和s2014中的矩阵表达式，可得整理后可得其中，f
x
＝f/dx，fy＝f/dy；
[0026]
s2016：令为相机内参矩阵，为相机外参矩阵，u＝[u，v，1]
t
为像素坐标向量，m＝[xm，ym，zm，1]
t
为空间实际坐标向量，则zc·
u＝h*a*m，整理得m＝zc·
a-1
*h-1
*u；
[0027]
s2017：根据图像平面中任意两个目标vi和vj的实际坐标(xi，yi)，(xj，yj)，计算vi和vj的实际距离。
[0028]
进一步的，步骤s202的具体操作包括以下步骤，
[0029]
s2021：设蚂蚁数量为ms，目标数量为ns，ns＝n，目标vi和vj之间的实际距离为d
ij
(i，j＝1，2，3...，ns)，t时刻任意目标之间的信息素浓度为τ
ij
(t)(i，j＝1，2，3...，ns)，初始时刻各目标之间的信息素浓度为τ
ij
(0)＝τ0，表示t时刻蚂蚁k(k＝1，2，3...，ms)从目标vi转移到目标vj之间的概率，则其中，allowedk为即将访问的网球集合，β为启发函数因子，α为信息素因子；
[0030]
s2022：设ρ(0＜ρ＜1)为信息素的挥发率，蚁群完成一次路径搜索后，将对所有目标之间的信息素进行更新，标之间的信息素进行更新，为第k只蚂蚁在目标vi与目标vj之间留下来的信息素，其中，qw为信息素强度，lk为本轮循环蚂蚁所走过的路径长度；
[0031]
s2023：重复步骤s2022和s2022进行迭代，从每次迭代产生的路径中选择最短路径，得到最优路径。
[0032]
进一步的，步骤s3的具体操作包括以下步骤，
[0033]
s301：使用机器人定位方法实时计算机器人方位；
[0034]
s302：根据待拾取目标坐标，使用双摄像头导航方法，计算机器人旋转方向和角度，控制机器人旋转对准目标；
[0035]
s303：利用主摄像头实时数据引导机器人接近待拾取目标，当机器人靠近目标后，启动辅助摄像头修正误差，使机器人再次对准目标；
[0036]
s304：控制机器人前进拾取目标；
[0037]
s305：按路径规划顺序，重复步骤s301～304，拾取全部目标。
[0038]
进一步的，所述机器人包括第一定位色板、第二定位色板、拾取机构、辅助摄像头、电路控制箱、轮式底盘和机器人控制电路，所述轮式底盘上可拆卸连接有拾取机构和电路
控制箱，所述机器人控制电路安装在所述电路控制箱内，所述第一定位色板安装在所述电路控制箱顶部，所述拾取机构的上方设有第二定位色板和辅助摄像头，且所述第二定位色板和辅助摄像头均安装在所述轮式底盘上；所述第一定位色板的面积大于所述第二定位色板的面积。
[0039]
进一步的，步骤s301中所述的机器人定位方法具体包括以下步骤，
[0040]
定义第二定位色板中心点坐标为a(x，y)，第一定位色板中心点坐标为b(x1，y1)，以b点为坐标原点，建立bxy坐标系，b点到a点的方向为机器人前进方向，连接a、b两点记为l，定义l线段的中心点w(x0，y0)为机器人坐标点，则其中，|*|为绝对值；
[0041]
机器人在bxy中方向角度θ为
[0042][0043]
进一步的，步骤s302中机器人旋转方向和角度的计算方法包括以下步骤，
[0044]
设待拾取目标c在bxy坐标系中的坐标点为c(x2，y2)，连接ba、bc，在bxy坐标系中ba与by存在夹角n，bc与by存在夹角m，机器人旋转角度为θ，f为象限值，则f＝{1，2，3，4}；当bc∈f＝1且ba∈f＝1时：
[0045]
若m＝n，则θ＝0，表示机器人与目标共线；
[0046]
若m＝n，则θ＝n-m，表示网球在机器人左边；
[0047]
若m＞n，则θ＝m-n，表示网球在机器人右边；
[0048]
当bc∈f＝1且ba∈f＝2时，θ＝m n，表示网球在机器人右边；
[0049]
当bc∈f＝1且ba∈f＝3时，θ＝m n，表示网球在机器人右边；
[0050]
当bc∈f＝1且ba∈f＝4时，θ＝2π-(m n)，表示网球在机器人左边；
[0051]
同理，bc∈f＝{2，3，4}时，可计算出目标在第二、第三、第四象限时机器人需要旋转的角度和方向。
[0052]
本发明的有益效果是：
[0053]
本发明基于多目标路径规划与智能拾取的机器人控制方法中，针对目标之间实际距离计算问题提出了基于两点像素坐标计算实际距离的方法(pdc)，该方法首先将像素坐标转换为实际坐标，然后利用两点间距离公式计算实际距离；针对多目标路径规划问题，提
出了一种同时考虑旋转角度和目标距离的蚁群优化算法(ad-aco)，ad-aco算法使用有效距离作为选择概率参数进行路径规划，以此获得机器人拾取目标的最优路径；针对目标拾取问题，提出了双摄像头智能拾取方法(dip)，dip由拾取机器人、机器人定位方法和双摄像头导航方法组成，拾取机器人用于拾取目标，定位方法用于机器人的实时定位，双摄像头导航方法用于引导机器人到达待拾取目标处。本发明中的机器人控制方法能够提升机器人路径规划能力，使路径规划结果满足机器人拾取路径最优，将其应用于多目标智能网球拾取中，拾取率在92％以上，与现有方法相比，路径优化能力更强，拾取网球的准确性和拾球效率更高，且有较强可移植性。
附图说明
[0054]
图1为本发明基于多目标路径规划与智能拾取的机器人控制的问题场景图。
[0055]
图2为本发明中mtpp方法流程图。
[0056]
图3为本发明中机器人拾取目标过程图。
[0057]
图4为本发明中坐标转换过程示意图。
[0058]
图5为本发明中图像坐标与像素坐标转换示意图。
[0059]
图6为本发明中蚁群优化算法流程图。
[0060]
图7为本发明中机器人结构示意图。
[0061]
图8为本发明中机器人控制电路图。
[0062]
图9为本发明中机器人定位示意图。
[0063]
图10为本发明中机器人旋转方向判定及角度计算示意图。
[0064]
图11为本发明中双摄像头拾取方法原理图。
[0065]
图12为本发明实施例中实验场景图。
[0066]
图13为本发明实施例中网球坐标提取示意图。
[0067]
图14为本发明实施例中相机标定板示意图。
[0068]
图15为本发明实施例中机器人拾球俯视图。
[0069]
图16为本发明实施例中机器人拾取机构误差测量示意图。
[0070]
图17为本发明实施例中实际距离测试示意图。
[0071]
图18为本发明实施例中辅助摄像头直行允许范围示意图。
[0072]
图19为本发明实施例中各算法距离对比图。
[0073]
图20为本发明实施例中基于不同算法的路径规划结果。
[0074]
图21为本发明实施例中拾球数量对比图。
[0075]
图22为本发明实施例中机器人平均拾取时间对比图。
[0076]
图23为本发明实施例中单双摄像头拾球率对比图。
[0077]
其中：1-第一定位色板，2-拾取机构，3-第二定位色板，4-辅助摄像头，5-电路控制箱，6-轮式底盘。
具体实施方式
[0078]
为了使本领域的普通技术人员能更好的理解本发明的技术方案，下面结合附图和实施例对本发明的技术方案做进一步的描述。
[0079]
基于多目标路径规划与智能拾取的机器人控制方法，包括以下步骤，
[0080]
s1：建立机器人拾取所有目标的最优路径数学模型；
[0081]
如附图1所示，绿色区域为已知场景，黄色物体为某一种物品，该物品可以是球类、物流包裹或其他，场景中随机放置一定数量的同类物品，场景顶部安装摄像头，作为主摄像头，主摄像头与pc机相连，pc机通过wifi与机器人进行数据传输；要求用最短时间将场景中的物品全部拾取。为了最快的拾取全部物品，首先需要找出机器人拾取每个物品的运动情况，然后根据运动情况进行路径规划，拾取每个物品，需要完成两个步骤：第一，控制机器人旋转一定角度，使机器人拾取机构对准物品；第二，控制机器人前进拾取物品；由于旋转角度和直行距离不能直接做代数运算，为了得到机器人拾取每个目标的完整路径，需要将旋转角度和直行距离统一为相同单位下的数值；因此，需要解决数值统一问题。具体的，
[0082]
s101：定义机器人拾取所有目标的数学模型为g(v，e，f)，式中，v为待拾取目标集v＝{v1，v2，...，vn}，vi为第i个目标，目标总数为n，e为两目标之间实际距离路径集合，e＝{l1(vi，vj)，l1(vi，vj)＞0：li(vi，vi)＝0，j，j∈n}，f为拾取完当前目标后，拾取下一个目标时需要旋转的弧长集合，f＝{l2(vi，vj)，l2(vi，vj)≥0，l2(vi，vi)＝0，i，j∈n}；
[0083]
式中，l1(vi，vj)表示目标vi与目标vj之间的实际距离，式中，(xi，yi)和(xj，yj)分别为目标vi和vj的实际坐标；
[0084]
l2(vi，vj)表示拾取完目标vi后拾取下一个目标vj时机器人需要旋转的弧长，式中，r为机器人动力轴半径，θ为拾取完目标vi后，在拾取目标vj前需要旋转的角度；
[0085]
s102：令机器人拾取每个目标的有效距离为l(vi，vj)，则l(vi：vj)＝l1(vi，vj) l2(vi，vj)；
[0086]
s103：定义f(v)为拾取所有目标v＝{v1，v2，...，vn}的路径目标函数，则
[0087]
s104：机器人拾取所有目标的最优路径问题是需要寻找一条最优路径在最短时间内拾取全部目标，也即目标函数最小化问题，则机器人拾取所有目标的最优路径数学模型为
[0088]
进一步的，为了求解上述机器人拾取所有目标的最优路径数学模型问题，本发明中提出一种融合多目标路径规划和智能拾取的机器人控制方法(mtpp)，mtpp方法由同时考虑旋转角度和目标距离的蚁群优化算法(ad-aco)与双摄像头智能拾取方法(dip)组成，流程图如附图2所示，具体包括
[0089]
步骤s2：利用同时考虑旋转角度和目标距离的蚁群优化算法(ad-aco)，对步骤s1中建立的最优路径数学模型进行求解，得到最优路径；以及步骤s3：利用双摄像头智能拾取
方法，引导机器人进行目标拾取。
[0090]
具体的，如附图3所示，机器人拾取每一个目标需要旋转一个角度θ和直行一段距离l；为了得到机器人拾取每个目标的有效距离，首先，使用弧长计算方法将旋转角度θ转换为旋转弧长，然后，使用pdc方法计算目标之间的实际距离，最后，将旋转弧长和直行距离求和得到效距离；在步骤s1中已经公开了将旋转角度θ转换为旋转弧长的具体计算方法，也即步骤s2中对目标vi和vj的实际坐标进行求解，基于求解得到的目标vi和vj的实际坐标求解目标vi和vj之间的实际距离，使用同时考虑旋转角度和目标实际距离的蚁群优化算法(ad-aco)求解机器人拾取所有目标的最优路径数学模型具体的，
[0091]
s201：基于两点像素坐标计算两点实际距离的方法，计算目标vi和vj的实际距离值；
[0092]
更具体的，s2011：定义目标vi与目标vj所在平面内任一点q在空间坐标系中的坐标为(xm，ym，zm)；以主摄像头为坐标原点建立相机坐标系oxyz，q点在相机坐标系oxyz中的坐标为(xc，yc，zc)；在相机成像的图像平面建立图像坐标系o1x1y1，q点映射到图像坐标系o1x1y1中的坐标定义为(x
p
，y
p
)。
[0093]
s2012：定义r为3*3的旋转矩阵，ts为3*1的平移矩阵，则q点在空间坐标系和相机坐标系oxyz之间的转换关系[xc，yc，zc]
t
＝r*[xm，ym，zm]
t
ts，矩阵形式表示为
[0094]
s2013：q点从相机坐标系oxyz映射到图像坐标系o1x1y1的变换关系为其中，f为摄像头到图像平面的距离，因此矩阵形式表示为
[0095]
s2014：定义相机成像的图像中每个像素点面积为dx
×
dy，以图像中点为坐标原点，建立像素坐标系ouv，定义q点映射到像素坐标系ouv中的坐标为(u，v)，则q点从图像坐
aco)求解机器人拾取所有目标的最优路径数学模型流程图如附图6所示。
[0102]
具体的，s2021：设蚂蚁数量为ms，目标数量为ns，ns＝n，目标vi和vj之间的实际距离为d
ij
(i，j＝1，2，3...，ns)，t时刻任意目标之间的信息素浓度为τ
ij
(t)(i，j＝1，2，3...，ns)，初始时刻各目标之间的信息素浓度为τ
ij
(0)＝τ0，表示t时刻蚂蚁k(k＝1，2，3...，ms)从目标vi转移到目标vj之间的概率，蚁群算法路径规划中，使用目标之间的距离和信息素浓度作为转移概率参数，最终只能得到最短路径而非机器人最优路径，本文根据机器人拾取目标特点，对转移概率中距离参数进行改进，将目标之间的距离改为由旋转弧长和直行距离求和得到的有效距离，路径规划中，算法会同时权衡旋转弧长、直行距离和信息素浓度的影响，使规划完成的路径达到机器人拾取路径最优，改进后的转移概率为其中，allowedk为即将访问的网球集合，β为启发函数因子，α为信息素因子；
[0103]
s2022：蚂蚁在释放信息素的同时，各目标之间的信息素也在挥发，设ρ(0＜ρ＜1)为信息素的挥发率，蚁群完成一次路径搜索后，将对所有目标之间的信息素进行更新，更新，为第k只蚂蚁在目标vi与目标vj之间留下来的信息素，其中，qw为信息素强度，lk为本轮循环蚂蚁所走过的路径长度；
[0104]
s2023：重复步骤s2022和s2022进行迭代，从每次迭代产生的路径中选择最短路径，得到最优路径。
[0105]
进一步的，步骤s3的具体操作包括以下步骤，
[0106]
s301：使用机器人定位方法实时计算机器人方位；
[0107]
所述机器人包括第一定位色板1、第二定位色板3、拾取机构2、辅助摄像头4、电路控制箱5、轮式底盘6和机器人控制电路，如附图7所示，在附图7中没有示出主摄像头，主摄像头安装在天花板是行，所述轮式底盘6上可拆卸连接有拾取机构2和电路控制箱5，更换拾取机构2可拾取不同物品，所述机器人控制电路安装在所述电路控制箱5中；所述第一定位色板1安装在所述电路控制箱5顶部，所述拾取机构2的上方设有第二定位色板3和辅助摄像头4，所述辅助摄像头4用于后期精确定位，保证拾取准确性；所述第二定位色板3和辅助摄像头4均安装在所述轮式底盘6上；所述第一定位色板1的面积大于所述第二定位色板3的面积。
[0108]
所述机器人控制电路如附图8所示，双路电机驱动模块负责驱动机器人运动，陀螺仪模块用于检测机器人旋转角度，主控制器主要功能是：第一，通过wifi接收pc机控制命令，并返回应答，第二，通过开关信号控制双路电机驱动模块，第三，读取陀螺仪数据计算旋
转角度；辅助摄像头通过wifi直接与pc机相连，当机器人靠近待拾取目标时，pc机通过辅助摄像头读取图像数据。机器人运动方式为：机器人开机后实时处于接收命令状体，等待接收pc机命令，当控制命令为直行、后退、停止时，机器人直接发送开关量信号控制双路电机驱动模块；当控制命令为左转、右转命令时，主控板控制机器人旋转，同时，实时读取陀螺仪数据并计算当前已旋转角度值，到达命令角度值后停止。
[0109]
机器人定位使用色块定位系统实现，色块定位系统由大小不同的两块红色板组(第一定位色板和第二定位色板)成，小色块(第二定位色板)放置在机器人前端，大色块(第一定位色板)放置在机器人后端，机器人定位时，使用rgb颜色特征提取大小色块坐标，根据大小色块的坐标关系实现对机器人的定位；定义第二定位色板中心点坐标为a(x，y)，第一定位色板中心点坐标为b(x1，y1)，以b点为坐标原点，建立bxy坐标系，b点到a点的方向为机器人前进方向，连接a、b两点记为l，定义l线段的中心点w(x0，y0)为机器人坐标点，如附图9所示，在附图9中，a为机器人示意图，b为色块提取，c为机器人定位；则其中，|*|为绝对值；
[0110]
机器人在bxy中方向角度θ为
[0111][0112]
s302：根据待拾取目标坐标，使用双摄像头导航方法，计算机器人旋转方向和角度，控制机器人旋转对准目标，如附图10所示。
[0113]
设待拾取目标c在bxy坐标系中的坐标点为c(x2，y2)，连接ba、bc，在bxy坐标系中ba与by存在夹角n，bc与by存在夹角m，机器人旋转角度为θ，f为象限值，则f＝{1，2，3，4}；当bc∈f＝1且ba∈f＝1时：
[0114]
若m＝n，则θ＝0，表示机器人与目标共线，如附图10中a所示；
[0115]
若m＝n，则θ＝n-m，表示网球在机器人左边，如附图10中b所示；
[0116]
若m＞n，则θ＝m-n，表示网球在机器人右边，如附图10中c所示；
[0117]
当bc∈f＝1且ba∈f＝2时，θ＝m n，表示网球在机器人右边，如附图10中d所示；
[0118]
当bc∈f＝1且ba∈f＝3时，θ＝m n，表示网球在机器人右边，如附图10中e所示；
[0119]
当bc∈f＝1且ba∈f＝4时，θ＝2π-(m n)，表示网球在机器人左边，如附图10中f所
示；
[0120]
同理，bc∈f＝{2，3，4}时，根据以上方法可计算出目标在第二、第三、第四象限时机器人需要旋转的角度和方向。
[0121]
s303：利用主摄像头实时数据引导机器人接近待拾取目标，当机器人靠近目标后，启动辅助摄像头修正误差，使机器人再次对准目标；
[0122]
如附图11所示，机器人从a位置出发拾取d目标时，首先，计算机器人旋转方向和角度θ，控制机器人从a位置按θ角旋转至b位置，此时可能产生旋转误差
±
θ1，其次，控制机器人从b位置运动到c位置，此时可能产生运动误差，第三，旋转误差和运动误差产生叠加形成总误差角
±
θ2，最终可能导致机器人运动到c位置时偏离d目标，此时，启动辅助摄像头从新计算误差角，控制机器人旋转消除总误差角
±
θ2，使机器人再次对准目标，最后，控制机器人前进拾取目标；
[0123]
s304：控制机器人前进拾取目标；
[0124]
s305：按路径规划顺序，重复步骤s301～304，拾取全部目标。
[0125]
实施例：
[0126]
将本发明中的机器人控制方法应用于网球拾取，对本发明中的控制方法进行性能评估，具体包括以下内容：
[0127]
(1)实验配置
[0128]
主摄像头采用1920*1080非畸变工业摄像头，辅助摄像头采用1920*1080萤石c2c无线wifi摄像头，pc上位机采用intel(r)core(tm)i7-5557u cpu，内存4gb，频率3.1ghz，64位操作系统；图像处理库采用opencv3.3.0。为了验证蚁群优化算法的路径规划性能，在实际场景中随机放置20、25、30、35、40、45个网球进行路径规划实验；为了验证双摄像头拾取方法的有效性，在场景中放置5、10、15、20、25个网球进行目标拾取实验。如附图12为实验场景图，绿色范围为实验场景区域，黄色圆点为网球随机位置点。
[0129]
(2)评估指标
[0130]
a.误差百分比
[0131]
定义误差百分比对旋转弧长和距离计算进行精确度评估，误差百分比越小精确度越高，误差百分比定义为其中，e为误差百分比，lm为测量值，l
t
为计算值。
[0132]
b.最短路径
[0133]
定义最短路径对最优路径进行评估，路径越短越优，最短路径定义为l
min
＝l1 l2，其中，l1为旋转总弧长，l2为直行总距离，l
min
为最短路径。
[0134]
c.拾球率
[0135]
定义拾球率对机器人拾球准确性进行评估，拾球率越大拾球准确性越高，拾球率定义为其中，s为拾球率，k为拾取数量，n为目标数量；s越大表示拾取率越高，拾取效果越好。
[0136]
d.效率和可移植性评估
[0137]
使用拾球时间进行效率评估，拾球时间越短效率越高；将dip方法移植到路径规划算法中，对比移植前后各算法的准确性，以此评估dip方法的可移植性。
[0138]
(3)数据预处理
[0139]
附图13为网球坐标提取过程流程，图13中，(a)为网球图像数据，首先，通过设置rgb颜色通道阈值提取网球，如(b)所述，其次，使用膨胀和腐蚀方法去噪，如图(c)所示，第三，对附图13的(c)中网球区域提取外轮廓，如(d)所述，第四，计算(d)中各轮廓的最小外切矩形，如图(e)，最后，计算最小外切矩形的中心点，以此作为网球坐标点，如图(f)。
[0140]
为了得到主摄像头内外参矩阵数据，首先，通过标定板采集角点坐标数据，标定板尺寸为1700*1200mm，标定板距摄像头3500mm，角点之间实际距离为600mm，如附图14所示；然后，根据角点数据利用现有技术中的计算方法，计算出摄象头的内外参矩阵数据分别为：[636，0，957；0，633，577；0，0，1]，[-0.5035，-0.5191，-986；0.8094，-0.5629，-805；-0.3021，-0.6431，2751]。
[0141]
(4)实验评估
[0142]
对本发明中提出的mtpp方法进行评估，首先，在ad-aco方法中对路径计算方法和路径规划方法进行评估，然后，在规划好的路径上加入dip方法融合为mtpp方法，并进行评估，同时，对dip方法进行可移植性评估。
[0143]
1)路径计算评估
[0144]
旋转弧长和实际距离计算中，存在最大允许误差，如附图15所示，m区域为辅助摄像头图像采集有效区域，e点为中心点，同时为理想拾取位置点，
±
α为网球横向最大允许误差，
±
α1为纵向最大允许误差，因此，定义最大允许误差百分比作为误差百分比的评价标准，当误差百分比小于最大允许误差百分比时，表示路径计算精度满足要求，且误差百分比和最大允许误差百分比数值相差越大，表示精度越高，定义最大允许误差百分比为式中，l
t
为距离计算值，α1为最大允许误差值。
[0145]
a.弧长评估
[0146]
机器人从起点开始，以60度为旋转单位测量旋转弧长，机器人轴长半径为200mm，计算每次旋转弧长值，根据测量值和计算值计算旋转误差百分比；如附图16为拾取机构，该机构为滚轮式笼状结构，长度为290mm，a至b范围为有效拾取区域，以中心点c为拾球理想位置点，左右两边最大允许误差值为
±
α＝l
s-rs＝145-3.35＝141.65mm，式中，ls＝290/2＝145mm，rs＝3.35mm为网球半径，实验结果如下表1所示。
[0147]
表1机器人旋转角度测试数据表
[0148][0149]
如表1所示，机器人从起始位置开始，角度值从60度至360度测试中，实验误差百分比为2％-4.60％，而最大允许误差百分比在11.46％-68.76％之间，实验误差百分比远远小于最大允许误差百分比，由此可以看出，本发明提出的弧长计算方法具有较高的精确度。
[0150]
b.pdc方法评估
[0151]
为了评估pdc方法在实际场景中精度的有效性，在场景中放置标准尺s,将a、b两球放置在刻度旁，从两球相距1000mm开始，以500mm为增加单位，如附图17所示，使用pdc方法进行距离计算，根据距离计算数据和实际距离数据使用公式25计算直行误差百分百，结果如表2。
[0152]
根据附图15，存在直行最大允许误差，在附图17中，当拾取完a网球后，机器人从a点出发前往b点中，只要辅助摄像头中心点到达b点时产生的误差在
±
α1之间，b网球就能在辅助摄像头有效区域内，如附图18所示；此时，通过辅助摄像头数据，可从新控制机器人对准目标。经实际测量，最大允许误差α1＝490mm；计算直行最大允许误差百分比，结果如表2。
[0153]
表2测量结果与实际结果对比表
[0154][0155]
如表2所示，实际距离从1000mm开始，每次以500mm递增至7000mm时，误差百分比在0.50％-2.47％之间，而最大允许误差百分比在7.00％-49％之间，我们的实验误差远小于
最大允许误差百分比，由此可以看出，我们的距离计算方法有较高的精确度。
[0156]
2)ad-aco方法评估
[0157]
实验场景中随机放置20、25、30、35、40、45个网球样本作为实验对象，分别执行蚁群优化算法、蚁群算法、遗传算法、贪心算法、栅格法，对我们提出的蚁群优化算法进行最优路径评估，实验结果见表3和表4以及附图19所示。在附图19中，a为旋转弧长对比，b为各算法直行距离对比，c为总距离对比。
[0158]
表3各算法实验结果表(单位：mm)
[0159][0160]
表4蚁群优化算法相对各算法路径优化能力提升百分比
[0161][0162]
路径总长度由旋转弧长和直行距离构成，虽然在旋转弧长或直行距离中，有其它算法优于蚁群优化算法的情况，如旋转弧长中，如表3和附图19中a所示，目标数为25时，蚁群算法和遗传算法优于蚁群优化算法，从表4中看，分别高于2.31％和1.08％；目标数为40时，蚁群算法优于蚁群优化算法，从表4中看，高于4.8％；直行距离中，如表3和附图19中b所示，目标数为40时，遗传算法优于蚁群优化算法，从表4中看，高于0.15％；但是，从总长度中看，如表3和附图19的c所示，目标数在20、25、30、35、40、45时，蚁群优化算法都优于蚁群算法、遗传算法、贪心算法和栅格法，从表4中看，分别高于1.17％-10.32％、0.31％-11.20％、7.37％-13.48％、14.17％-25.79％。
[0163]
从路径平滑度看，如图20所示，纵向分别表示ad-aco、ac、ga、a*和gm算法在不同目标下的路径规划效果，横向分别表示不同目标数下哥算法的路径规划效果，ad-aco明显平滑于其他算法。
[0164]
以上结果原因在于，ad-aco方法中转移概率参数由有效距离和信息素浓度决定，路径规划时，算法会同时权衡旋转弧长、直行距离和信息素浓度的影响，算法追求整体路径
最优而非单项指标最优，因此，出现其它算法的某些指标高于蚁群优化算法现象；从总路径长度上看，在所有目标数下，蚁群优化算法都优于其它四种算法。从路径规划效果上看，如图22所示，从路径平滑度看，蚁群优化算法同样好于其它四种算法，因此，在路径规划中，蚁群优化算法明显优于蚁群算法、遗传算法、贪心算法和栅格法。
[0165]
3)mtpp方法评估
[0166]
在实验场景中分别放置5、10、15、20、25个随机网球样本作为实验对象，以此评估mtpp方法的准确性和拾球效率,同时，对dip方法进行可移植性评估。
[0167]
a.准确性评估
[0168]
将mtpp方法与单摄像头下蚁群优化算法、蚁群算法、遗传算法、贪心算法、栅格法进行拾球率对比，以此评估mtpp方法的准确性，各算法在每种目标数下分别进行10次实验取平均值，实验结果如表5。
[0169]
表5各算法平均拾球数和拾球率
[0170][0171]
表6拾球率提升百分百
[0172][0173]
如表5所示，mtpp方法的拾取率为92％-100％，单摄像头下蚁群优化算法、蚁群算法、遗传算法、贪心算法、栅格法的拾取率分别为64.8％-80％、63.2％-80％、63.2％-84％、60％-84％、58.4％-80％，从附图21中可以看出，mtpp方法在5、10、15、20、25目标数下，拾球数量都高于其它四种算法；从表6(拾球率提升百分百实验结果)中可以看出，mtpp方法相对其它几种算法拾球率分别提升20.00％-26.36％、20.00％-28.18％、16.00％-28.18％、16.00％-31.82％、20.00％-33.64％；实验结果表明，mtpp方法与单摄像头下蚁群优化算法、蚁群算法、遗传算法、贪心算法、栅格法进行相比具有较高的准确性。
[0174]
b.效率评估
[0175]
将dip方法分别运用到蚁群算法、遗传算法、贪心算法、栅格法中与mtpp方法进行比较，以此评估mtpp方法的拾球效率。分别在5、10、15、20、25目标数下进行实验，记录各算法的拾取时间，实验结果如表7。
[0176]
表7 dip方法下各算法的平均拾取时间
[0177][0178]
表8蚁群优化算法相对各算法提升百分比
[0179][0180]
如附图22所示，目标数为5时，五种算法的拾球时间非常接近，如表10所示，虽然蚁群优化算法比遗传算法、贪心算法、栅格法分别降低-0.03％、-0.38％、-1.19％，但是，如表7所示，降低的时间都在1秒以内，此原因在于，目标数为5时，5种路径规划的路径一致，相差的时间主要是随机误差导致；目标数为10、15、20、25时，如表7所示，蚁群优化算法的拾球时间短于蚁群算法、遗传算法、贪心算法和栅格法，从表8中看，蚁群优化算法相对于其它几种算法拾球时间分别提升2.91％-4.46％、1.75％-5.48％、1.20％-9.31％、9.24％-20.41％，原因在于，随着目标数的增加，蚁群优化算法对路径的优化能力越来越强于其他四种算法，从而使路径总长度更短，拾球时间下降；由此可得，mtpp方法具有更高的拾球效率。
[0181]
c.可移植性评估
[0182]
将dip方法分别与蚁群优化算法、蚁群算法、遗传算法、贪心算法、栅格法进行组合实验，实验结果如表9。
[0183]
表9双摄像头智能拾取方法下各算法的平均拾球率
[0184][0185]
将表9中双摄像头下各算法的拾球率与表5中单摄像头下各算法的拾球率进行对比，对比结果如表10和附图23所示。
[0186]
表10双摄像头比单摄像头在各算法下拾球率提升百分百
[0187][0188]
如表5所示，蚁群优化算法、蚁群算法、遗传算法、贪心算法、栅格法在单摄像头智能拾取方法下拾取率都在85％以下，将dip方法运用到各算法中时，如表9和附图23所示，各算法的拾球率得到明显提高；从表10中可以看出，各算法的拾球率分别提升了20.00％-29.57％、19.57％-27.52％、16.00％-28.18％、16.00％-28.57％，18.60％-27.00％；实验结果表明，dip方法运用在蚁群优化算法中时，使蚁群优化算法的拾球率大幅提升，将其分别运用到蚁群算法、遗传算法、贪心算法和栅格法中时，同样使得四种算法的拾球率大幅提升，由此可得，本发明提出的dip方法具有较强的可移植性。
[0189]
以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。