CN102662399B - 一种面向多目标的水果采摘机器人的路径优化方法 - Google Patents

Abstract

本发明公开了一种面向多目标的水果采摘机器人的路径优化方法，包括以下步骤：（1）采用三点三步法对六自由度水果采摘机器人末端执行器的坐标系进行标定；（2）通过六自由度水果采摘机器人的机械手末端摄像头动态采集整个果树树冠的图像，并得到多个水果中心位置的三维坐标；（3）将所述水果中心位置的三维坐标视为树冠图像的顶点，并构造出一个无方向的连接图，采用哈密尔顿图形法实现六自由度水果采摘机器人路径的优化；（4）快速搜索出哈密尔顿图中的一个关于所有所述顶点的排列。本发明使六自由度水果采摘机器人在最短的时间内，采摘到最多的水果，大大提高了六自由度水果采摘机器人的工作效率。

Description

一种面向多目标的水果采摘机器人的路径优化方法

技术领域

本发明涉及一种面向多目标（采摘对象）的水果采摘机器人的路径优化方法，尤其是一种通过采摘机器人的末端执行器坐标系统对水果进行标定，然后采用哈密尔顿图形法对水果采摘路径进行优化的方法。

背景技术

我国是一个农业大国，经过半个多世纪的迅速发展，水果产业，尤其是苹果产业取得了举世瞩目的成就。收获作业是苹果种植生产的重要环节，但目前基本是手工作业，其费用约占成本的50%-70%；而随着我国社会逐渐进入老龄化阶段，劳动力资源更是日趋紧张。随着工业化、城镇化的深入发展，农民对农机作业的需求越来越迫切，农业生产对农机应用的依赖也越来越明显。因此，开展苹果收获机器人的研究开发，对于实现苹果采摘作业的机械化、自动化和智能化，保证苹果的适时高效采收、降低收获作业费用，同时解决当前国家面临的劳动力短缺问题具有重大现实意义。

目前，国内外的多个研究机构已研发出数种类型苹果采摘机器人样机，但由于工作效率低，远不如人工采摘速度快，而停留在实验室研究阶段。要使得苹果收获机器人达到实用化水平，就必须提高机器人的采摘速度，而对采摘效率起决定性因素的就是采摘路径的优化问题。近年来，为了实现机器人顺利采摘并进一步提高工作效率，国内外一些学者从路径规划角度对苹果采摘机器人控制系统进行了研究，但这些规划算法都比较复杂，计算时间较长，导致机器人末端执行器接近采摘目标的步骤中每一次优化过程都需要占用较多时间，从而严重影响了采摘效率。因此，为了减少机器人整体采摘时间，提高整体工作效率，需要研究一种简单实用的机器人采摘路径优化方法。

发明内容

发明目的：针对现有技术中存在的问题与不足，本发明提供一种提高水果采摘效率、算法简单且计算时间段的面向多目标的水果采摘机器人的路径优化方法。

技术方案：一种面向多目标的水果采摘机器人的路径优化方法，包括以下步骤：

（1）采用三点三步法对六自由度水果采摘机器人末端执行器的坐标系进行标定；

（2）通过六自由度水果采摘机器人的机械手末端摄像头动态采集整个果树树冠的图像，并从处理后的树冠图像上得到某一时刻果树上多个水果中心位置的三维坐标；

（3）将所述水果中心位置的三维坐标视为树冠图像的顶点，并通过连接顶点在静态的树冠图像中构造出一个无方向的连接图，采用哈密尔顿图形法实现六自由度水果采摘机器人路径的优化；

（4）快速搜索出哈密尔顿图中的一个关于所有所述顶点的排列，所述排列为使六自由度水果采摘机器人末端经过的路径最短的排列。

采用三点三步法对六自由度水果采摘机器人末端执行器的坐标系进行标定的具体步骤如下：

a，设所述六自由度水果采摘机器人基座坐标系为{0}，六自由度水果采摘机器人末端坐标系为{6}，操纵六自由度水果采摘机器人六个关节使得其末端执行器分别从三个不同的点指向锥体，使六自由度水果采摘机器人末端执行器接近锥体顶部；其中，三个不同的点分别指第一个点、第二个点和第三个点；分别记录六自由度水果采摘机器人末端执行器从第一个点、第二个点和第三个点指向锥体时的机器人末端坐标系相对于机器人基座坐标系的坐标变换阵，并分别记为 $T_1^6{}_0,T_2^6{}_0,T_3^6{}_0;$

b，操纵六自由度水果采摘机器人末端沿着所述六自由度水果采摘机器人基座坐标系{0}的X轴正向平移一段距离，记为Δx，并记录当前六自由度水果采摘机器人末端坐标系{6}相对于六自由度水果采摘机器人基座坐标系{0}的坐标变换阵

c，操纵六自由度水果采摘机器人使其末端沿着所述六自由度水果采摘机器人基座坐标系{0}的Z轴正向平移一段距离，记为Δz，并记录当前六自由度水果采摘机器人末端坐标系{6}相对于所述六自由度水果采摘机器人基座坐标系{0}的坐标变换阵

采用哈密尔顿图形法实现六自由度水果采摘机器人路径的优化是在哈密尔顿图中找到一个所有顶点的排列，且所述六自由度水果采摘机器人的末端执行器按照所述排列经过的路径是一条经过所有顶点且只经过一次、具有最短距离的回路min L。

所述锥体设有尖的顶部，倒置固定在表面平整且固定不动的支撑体上。

采用哈密尔顿图形法实现六自由度水果采摘机器人路径的优化的具体步骤如下：

首先，在所述的静态树冠图像中，假设一个无方向的连接图G（V，E,R,ω:E→Z⁺），其中V表示顶点（水果中心位置的三维坐标）集，E表示边集(任意两个顶点的距离连线)，R表示E的任意一个子集，ω表示边的权（长度）；然后，采用哈密尔顿图形法实现六自由度水果采摘机器人路径的优化，即：通过所述连接图G中的顶点，形成一个完整的哈密尔顿图：g（V^H,E^H,ω^H），其中V^H表示顶点集，E^H表示边集，ω^H表示边的权且ω^H∈D；所述哈密尔顿图g中，每条边的权值由连接图G中同一条边上的顶点的排列顺序决定；设对于哈密尔顿图g中顶点集V^H={v₁,v₂......v_n}的一个排列顺序为T＝（t₁,t₂......t_n），t_i∈V^H(i＝1,2,...,n)，则哈密尔顿图形中六自由度水果采摘机器人路径优化后的最短路径为：

$\min L=\underset{j=1}{\overset{n-1}{\mathrm{\Σ}}}{\mathrm{\ω}}_j^H$

其中表示哈密尔顿图g中边的权值。

六自由度水果采摘机器人路径优化的计算步骤如下：

1）建立个体信息：所述个体信息包括未访问顶点列表（not visited vertices，NVV）、已访问顶点列表（visited vertices，VV）、未访问边列表（not visited edges，NVE）、已访问边列表（visited edges，VE）和个体走过的路径长度（tour length，TL）；其中，个体指的是蚁群算法中的一个蚂蚁个体。

2）初始化：令时间t=0，循环次数N＝0，设置最大循环次数N_max；初始化τ_ij(t)、Δτ_ij；设定参数α、β、ρ、Q；顶点数为m；其中ρ表示信息素挥发率，τ_ij(t)表示t时刻路径（i，j）上信息素存积量，Δτ_ij表示经过时间△t路径（i，j）上信息素的增量，参数α和β分别表示τ_ij(t)和路径（i，j）上的能见度η_ij(t)对整个转移概率的影响权值，Q为体现个体所留轨迹数量的一个常数；

3）循环次数N←N+1；

4）个体数目k←k+1。

5）根据转移概率式 $p_{\mathrm{ij}}^k=\left\{\begin{array}{cc}\frac{{\left({\mathrm{\τ}}_{\mathrm{ij}}\left(t\right)\right)}^{\mathrm{\α}}{\left({\mathrm{\η}}_{\mathrm{ij}}\left(t\right)\right)}^{\mathrm{\β}}}{\underset{s\∈{\mathrm{alowed}}_k}{\mathrm{\Σ}}{\left({\mathrm{\τ}}_{\mathrm{is}}\left(t\right)\right)}^{\mathrm{\α}}{\left({\mathrm{\η}}_{\mathrm{is}}\left(t\right)\right)}^{\mathrm{\β}}},& j\∈{\mathrm{alowed}}_k\\ 0,& \mathrm{otherwise}\end{array}\right.$ 计算的概率，选择顶点j并前进，j∈alowed_k；alowed_k表示个体k在顶点i处的可行邻域，即未访问边列表中边的端点；

6）若k＜m，则跳转至步骤4），否则转至步骤7）；

7）根据式①和式②更新每条路径上的信息量；

τ_ij(t+Δt)＝ρτ_ij(t)+Δτ_ij  ①

${\mathrm{\Δ\τ}}_{\mathrm{ij}}=\underset{k=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\Δ\τ}}_{\mathrm{ij}}^k$ ②

8）若N≥Nmax，循环停止，输出计算结果T_OBJECT，否则更新个体信息并跳转至步骤3）；

9）计算六自由度水果采摘机器人末端实际运动路径，六自由度水果采摘机器人空间路径计算公式为： $P=T_1^6{}_{0}T_2^6{}_{0}T_3^6{}_{0}T_4^6{}_{0}T_5^6{}_0{T}_{\mathrm{OBJECT}}.$

本发明的方法适用于的水果为苹果、橘子、橙子和梨。

有益效果：与现有技术相比，本发明所提供的面向多目标的水果采摘机器人的路径优化方法，首先通过采用三点三步法对六自由度水果采摘机器人末端执行器坐标系进行标定，然后采用哈密尔顿图找出采摘水果的最短路径，从而使六自由度水果采摘机器人在最短的时间内，采摘到最多的水果，大大提高了六自由度水果采摘机器人的工作效率。

附图说明

图1为本发明实施例中涉及到的六自由度水果采摘机器人的示意图；

图2为本发明实施例中六自由度水果采摘机器人的六个关节坐标与参数示意图；

图3为本发明实施例中静态的树冠图像中的苹果三维坐标示意图；

图4为本发明实施例中具有5个顶点的哈密尔顿图。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明而不用于限制本发明的范围，在阅读了本发明之后，本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。

本实施例以苹果为例对本发明公开的面向多目标的水果采摘机器人的路径优化方法进行详细的阐述，

步骤一，采用三点三步法对六自由度水果采摘机器人末端执行器的坐标系进行标定。

要实现六自由度水果采摘机器人正常采摘水果，就要保持机器人末端能够始终处于一个合适的姿态，即机器人末端的坐标与己经规划好的采摘路径坐标系保持一致，因此，需要对水果采摘机器人末端执行器坐标系进行标定。六自由度水果采摘机器人如图1所示，设其基座坐标系以及从下至上的6个关节坐标系分别为（x₀，y₀,z₀）、（x₁，y₁,z₁）、（x₂，y₂,z₂）、（x₃，y₃,z₃）、（x₄，y₄,z₄）、（x₅，y₅,z₅）、（x₆，y₆,z₆），如图2所示。对六自由度水果采摘机器人末端执行器坐标系进行标定之前，在水果采摘机器人工作空间内选择一个水果采摘机器人末端比较容易接近且表面平整的位置，在该位置上倒放一个锥体，为了提高标定精度，该锥体需要有比较尖的顶部且固定在某个不动体上。该标定方法中的三步操作都是在该锥体的顶部为参照对象，三点三步标定法具体步骤为：

（1）设六自由度水果采摘机器人基座坐标系为{0}，六自由度水果采摘机器人末端坐标系为{6}，操纵六自由度水果采摘机器人六个关节使得六自由度水果采摘机器人末端执行器分别从三个不同的点指向锥体，并使六自由度水果采摘机器人末端执行器尽量接近锥体顶部。如图1和图2所示，调整六自由度水果采摘机器人4～6关节，使得第三个姿态下的工具系Z轴与基座坐标系Z轴基本一致，工具系X轴与基座坐标系X轴方向基本一致。分别记下在这三个位置（即，六自由度水果采摘机器人末端执行器从三个不同的点指向锥体时所处的位置）下六自由度水果采摘机器人末端坐标系相对于其基座坐标系的坐标变换阵，分别记为 $T_1^6{}_0,T_2^6{}_0,T_3^6{}_0.$

（2）操纵六自由度水果采摘机器人使其末端沿着基座坐标系{0}的X轴正向平移一段距离(记为Δx)，并记录当前六自由度水果采摘机器人末端坐标系{6}相对于基座坐标系{0}的坐标变换阵

（3）操纵六自由度水果采摘机器人使机器人其末端沿着{0}的Z轴正向平移一段距离(记为Δz)，并记录当前六自由度水果采摘机器人末端坐标系{6}相对于其基座坐标系{0}的坐标变换阵

步骤二，通过六自由度水果采摘机器人的机械手末端双目摄像头动态采集整个果树树冠的图像，经图像处理得到苹果树上所有苹果中心位置的三维坐标，如图3所示。

步骤三，将苹果中心位置的三维坐标视为树冠图像的顶点，则任意两个苹果中心位置间的连线是图的边。六自由度水果采摘机器人路径优化问题的实质就是机械手依照怎样的顺序采摘树冠图像中的每一个苹果，使得机械手经过的路径最短。按照图论原理，在果树的静态树冠图像中假设一个无方向的连接图G（V，E,R,ω:E→Z⁺），其中V表示顶点集(苹果)，E表示边集(任意两个苹果的距离连线)，R表示E的任意一个子集，ω表示边的权（长度）。六自由度水果采摘机器人路径优化问题就是在连接图G中求出一条经过所有顶点且只经过一次的具有最短距离的回路。为了使问题便于求解，采用哈密尔顿图形法表示苹果采摘机器人路径优化问题。如图4所示，用一个顶点代表原始连接图G中的顶点，从而形成一个完整的哈密尔顿图：g（V^H,E^H,ω^H），其中V^H表示顶点集，E^H表示边集，ω^H表示边的权且ω^H∈D。哈密尔顿图g中，每条边的权值是不固定的，其值由原始的连接图G中同一条边上的顶点的排列顺序决定。设对于哈密尔顿图g中顶点集V^H={v₁,v₂......v_n}的一个排列顺序为T＝（t₁,t₂......t_n），t_i∈V^H(i＝1,2,...,n)，则哈密尔顿图形中六自由度水果采摘机器人路径优化组合问题可表示为：

$\min L=\underset{j=1}{\overset{n-1}{\mathrm{\Σ}}}{\mathrm{\ω}}_j^H$

其中表示图g中边的权值。由此，六自由度水果采摘机器人路径优化问题就变为在哈密尔顿图中找到一个所有顶点的排列，按照这个排列，使得六自由度水果机器人末端执行器经过的路径L最短。

步骤四，六自由度水果采摘机器人路径优化问题计算步骤为：

（1）建立以下个体信息：未访问顶点列表NVV，已访问顶点列表VV，未访问边列表NVE，已访问边列表VE，个体走过的路径长度TL。以图4所示的连接具有5个顶点的哈密尔顿图g为例，若个体1在顶点1处时算法开始，则个体1初始化信息为：

NVV[1]={1,2,3,4,5,6,7,8,9,10}

VV[1]={}

NVE[1]={(1,2),(3,4),(5,6),(7,8),(9,10)}

VE[1]={}

TL[1]=0.0

（2）初始化。令时间t=0，循环次数N＝0，设置最大循环次数N_max；τ_ij(t)、Δτ_ij初始化；设定参数α、β、ρ、Q；顶点数为m。其中ρ表示信息素挥发率，τ_ij(t)表示t时刻路径（i，j）上信息素存积量，Δτ_ij表示经过时间△t路径（i，j）上信息素的增量，参数α和β分别表示τ_ij(t)和路径（i，j）上的能见度η_ij(t)对整个转移概率的影响权值，Q为体现蚂蚁所留轨迹数量的一个常数。

（3）循环次数N←N+1。

（4）个体数目k←k+1。

（5）个体根据转移概率式 $p_{\mathrm{ij}}^k=\left\{\begin{array}{cc}\frac{{\left({\mathrm{\τ}}_{\mathrm{ij}}\left(t\right)\right)}^{\mathrm{\α}}{\left({\mathrm{\η}}_{\mathrm{ij}}\left(t\right)\right)}^{\mathrm{\β}}}{\underset{s\∈{\mathrm{alowed}}_k}{\mathrm{\Σ}}{\left({\mathrm{\τ}}_{\mathrm{is}}\left(t\right)\right)}^{\mathrm{\α}}{\left({\mathrm{\η}}_{\mathrm{is}}\left(t\right)\right)}^{\mathrm{\β}}},& j\∈{\mathrm{alowed}}_k\\ 0,& \mathrm{otherwise}\end{array}\right.$ 计算的概率选择顶点j并前进，j∈alowed_k；alowed_k表示个体k在顶点i处的可行邻域（即列表NVE中边的端点）。

（6）若k＜m，则跳转至（4），否则转至（7）。

（7）根据式①和式②更新每条路径上的信息量。

τ_ij(t+Δt)＝ρτ_ij(t)+Δτ_ij  ①

${\mathrm{\Δ\τ}}_{\mathrm{ij}}=\underset{k=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\Δ\τ}}_{\mathrm{ij}}^k$ ②

（8）若N≥N_max，循环停止，输出计算结果T_OBJECT，否则更新个体信息并跳转至步骤（3）。

（9）计算六自由度采摘机器人实际空间优化运动路径。采摘机器人末端实际空间路径计算公式为： $P=T_1^6{}_{0}T_2^6{}_{0}T_3^6{}_{0}T_4^6{}_{0}T_5^6{}_0{T}_{\mathrm{OBJECT}}.$

本发明公开的面向多目标的水果采摘机器人的路径优化方法，用于采摘其他水果（如，橘子、橙子和梨）的方法和采摘上述采摘苹果的方法是一样的，所不同的就是采摘目标的变化，但并不影响方法的使用及其采摘效果。

Claims (3)

1.一种面向多目标的水果采摘机器人的路径优化方法，其特征在于，包括以下步骤：

（1）采用三点三步法对六自由度水果采摘机器人末端执行器的坐标系进行标定；

（2）通过六自由度水果采摘机器人的机械手末端摄像头动态采集整个果树树冠的图像，并从处理后的树冠图像上得到某一时刻果树上多个水果中心位置的三维坐标；

（3）将所述水果中心位置的三维坐标视为树冠图像的顶点，并通过连接顶点在静态的树冠图像中构造出一个无方向的连接图，采用哈密尔顿图形法实现六自由度水果采摘机器人路径的优化；

（4）快速搜索出哈密尔顿图中的一个关于所有所述顶点的排列，所述排列为使六自由度水果采摘机器人末端经过的路径最短的排列；

采用三点三步法对六自由度水果采摘机器人末端执行器的坐标系进行标定的具体步骤如下：

a，设所述六自由度水果采摘机器人基座坐标系为{0}，六自由度水果采摘机器人末端坐标系为{6}，操纵六自由度水果采摘机器人六个关节使得其末端执行器分别从三个不同的点指向锥体，使六自由度水果采摘机器人末端执行器接近锥体顶部；其中，三个不同的点分别指第一个点、第二个点和第三个点；分别记录六自由度水果采摘机器人末端执行器从第一个点、第二个点和第三个点指向锥体时的机器人末端坐标系相对于机器人基座坐标系的坐标变换阵，并分别记为

b，操纵六自由度水果采摘机器人末端沿着所述六自由度水果采摘机器人基座坐标系{0}的X轴正向平移一段距离，记为Δx，并记录当前六自由度水果采摘机器人末端坐标系{6}相对于六自由度水果采摘机器人基座坐标系{0}的坐标变换阵

c，操纵六自由度水果采摘机器人使其末端沿着所述六自由度水果采摘机器人基座坐标系{0}的Z轴正向平移一段距离，记为Δz，并记录当前六自由度水果采摘机器人末端坐标系{6}相对于所述六自由度水果采摘机器人基座坐标系{0}的坐标变换阵

采用哈密尔顿图形法实现六自由度水果采摘机器人路径的优化是在哈密尔顿图中找到一个所有顶点的排列，且所述六自由度水果采摘机器人的末端执行器按照所述排列经过的路径是一条经过所有顶点且只经过一次、具有最短距离的回路min L；

采用哈密尔顿图形法实现六自由度水果采摘机器人路径的优化的具体步骤如下：

首先，在所述的静态树冠图像中，假设一个无方向的连接图G（V,E,R,ω），其中V表示顶点集，E表示边集且E为正整数，R表示E的任意一个子集，ω表示边的权；然后，采用哈密尔顿图形法实现六自由度水果采摘机器人路径的优化，即：通过所述连接图G中的顶点，形成一个完整的哈密尔顿图：g（V^H,E^H,ω^H），其中V^H表示顶点集，E^H表示边集，ω^H表示边的权；所述哈密尔顿图g中，每条边的权值由连接图G中同一条边上的顶点的排列顺序决定；设对于哈密尔顿图g中顶点集V^H={v₁,v₂……v_n}的一个排列顺序为T=（t₁,t₂……t_n），t_i∈V^H(i=1,2,…,n)，则哈密尔顿图形中六自由度水果采摘机器人路径优化后的最短路径为：

$\begin{array}{cc}\min & L=\underset{j=1}{\overset{n-1}{\mathrm{\Σ}}}{\mathrm{\ω}}_j^H\end{array}$

其中表示哈密尔顿图g中边的权值；

（4）建立哈密尔顿图g后，采用六自由度水果采摘机器人路径优化方法进行计算，其步骤如下：

1）建立个体信息：所述个体信息包括未访问顶点列表、已访问顶点列表、未访问边列表、已访问边列表和个体走过的路径长度；

2）初始化：令时间t=0，循环次数N=0，设置最大循环次数N_max；初始化τ_ij(t)、Δτ_ij；设定参数α、β、ρ；顶点数为m；其中ρ表示信息素挥发率，τ_ij(t)表示t时刻路径（i,j）上信息素存积量，Δτ_ij表示经过时间△t路径（i,j）上信息素的增量，参数α和β分别表示τ_ij(t)和路径（i,j）上的能见度η_ij(t)对整个转移概率的影响权值；

3）循环次数N←N+1；

4）个体数目k←k+1；

5）根据转移概率式 $p_{\mathrm{ij}}^k=\left\{\begin{array}{cc}\frac{{\left({\mathrm{\τ}}_{\mathrm{ij}}\left(t\right)\right)}^{\mathrm{\α}}{\left({\mathrm{\η}}_{\mathrm{ij}}\left(t\right)\right)}^{\mathrm{\β}}}{\underset{s\∈{\mathrm{alowed}}_k}{\mathrm{\Σ}}{\left({\mathrm{\τ}}_{\mathrm{is}}\left(t\right)\right)}^{\mathrm{\α}}{\left({\mathrm{\η}}_{\mathrm{is}}\left(t\right)\right)}^{\mathrm{\β}}},& j\∈{\mathrm{alowed}}_k\\ 0,& \mathrm{otherwise}\end{array}\right.$ 计算的概率，选择顶点j并前进，j∈alowed_k；alowed_k表示个体k在顶点i处的可行邻域，即未访问边列表中边的端点；

6）若k＜m，则跳转至步骤4），否则转至步骤7）；

7）根据式①和式②更新每条路径上的信息量；

τ_ij(t+Δt)＝ρτ_ij(t)+Δτ_ij      ①

${\mathrm{\Δ\τ}}_{\mathrm{ij}}=\underset{k=1}{\overset{m}{\mathrm{\Σ}}}{\mathrm{\Δ\τ}}_{\mathrm{ij}}^k$       ②

8）若N≥Nmax，循环停止，输出计算结果T_OBJECT，否则更新个体信息并跳转至步骤3）；

9）计算六自由度水果采摘机器人末端实际运动路径，六自由度水果采摘机器人末端实际空间路径计算公式为： $P=T_1^6{}_{0}T_2^6{}_{0}T_3^6{}_{0}T_4^6{}_{0}T_5^6{}_0{T}_{\mathrm{OBJECT}}.$

2.如权利要求1所述的面向多目标的水果采摘机器人的路径优化方法，其特征在于：所述锥体设有尖的顶部，倒置固定在表面平整且固定不动的支撑体上。

3.如权利要求1所述的面向多目标的水果采摘机器人的路径优化方法，其特征在于：所述水果为苹果、橘子、橙子和梨。