CN111062988B - 一种基于局部点云的葡萄位姿估计方法 - Google Patents
一种基于局部点云的葡萄位姿估计方法 Download PDFInfo
- Publication number
- CN111062988B CN111062988B CN201911198014.8A CN201911198014A CN111062988B CN 111062988 B CN111062988 B CN 111062988B CN 201911198014 A CN201911198014 A CN 201911198014A CN 111062988 B CN111062988 B CN 111062988B
- Authority
- CN
- China
- Prior art keywords
- grape
- point cloud
- point
- particle
- pose
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 235000009754 Vitis X bourquina Nutrition 0.000 title claims abstract description 180
- 235000012333 Vitis X labruscana Nutrition 0.000 title claims abstract description 180
- 235000014787 Vitis vinifera Nutrition 0.000 title claims abstract description 180
- 238000000034 method Methods 0.000 title claims abstract description 58
- 240000006365 Vitis vinifera Species 0.000 title 1
- 241000219095 Vitis Species 0.000 claims abstract description 179
- 239000002245 particle Substances 0.000 claims abstract description 95
- 230000002068 genetic effect Effects 0.000 claims abstract description 12
- 238000012545 processing Methods 0.000 claims abstract description 10
- 238000001914 filtration Methods 0.000 claims abstract description 7
- 239000008187 granular material Substances 0.000 claims abstract description 7
- 241000219094 Vitaceae Species 0.000 claims abstract description 6
- 235000021021 grapes Nutrition 0.000 claims abstract description 6
- 239000012636 effector Substances 0.000 claims abstract description 4
- 235000013399 edible fruits Nutrition 0.000 claims description 16
- 210000000349 chromosome Anatomy 0.000 claims description 12
- 238000013519 translation Methods 0.000 claims description 12
- 239000011159 matrix material Substances 0.000 claims description 11
- 230000009466 transformation Effects 0.000 claims description 10
- 230000011218 segmentation Effects 0.000 claims description 6
- PXFBZOLANLWPMH-UHFFFAOYSA-N 16-Epiaffinine Natural products C1C(C2=CC=CC=C2N2)=C2C(=O)CC2C(=CC)CN(C)C1C2CO PXFBZOLANLWPMH-UHFFFAOYSA-N 0.000 claims description 3
- 230000002146 bilateral effect Effects 0.000 claims description 3
- 238000013135 deep learning Methods 0.000 claims description 3
- 238000001514 detection method Methods 0.000 claims description 3
- 238000011156 evaluation Methods 0.000 claims description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000001788 irregular Effects 0.000 description 2
- UDHXJZHVNHGCEC-UHFFFAOYSA-N Chlorophacinone Chemical compound C1=CC(Cl)=CC=C1C(C=1C=CC=CC=1)C(=O)C1C(=O)C2=CC=CC=C2C1=O UDHXJZHVNHGCEC-UHFFFAOYSA-N 0.000 description 1
- 240000000560 Citrus x paradisi Species 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
- G06T7/75—Determining position or orientation of objects or cameras using feature-based methods involving models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/004—Artificial life, i.e. computing arrangements simulating life
- G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N3/00—Computing arrangements based on biological models
- G06N3/12—Computing arrangements based on biological models using genetic models
- G06N3/126—Evolutionary algorithms, e.g. genetic algorithms or genetic programming
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
- G06T7/11—Region-based segmentation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
- G06T7/194—Segmentation; Edge detection involving foreground-background segmentation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
- G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
- G06T7/344—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods involving models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20081—Training; Learning
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20084—Artificial neural networks [ANN]
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Biophysics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Health & Medical Sciences (AREA)
- Life Sciences & Earth Sciences (AREA)
- Evolutionary Computation (AREA)
- Mathematical Physics (AREA)
- Computational Linguistics (AREA)
- Data Mining & Analysis (AREA)
- Artificial Intelligence (AREA)
- General Health & Medical Sciences (AREA)
- Molecular Biology (AREA)
- Computing Systems (AREA)
- General Engineering & Computer Science (AREA)
- Biomedical Technology (AREA)
- Software Systems (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Evolutionary Biology (AREA)
- Physiology (AREA)
- Genetics & Genomics (AREA)
- Image Analysis (AREA)
Abstract
本发明提供了一种基于局部点云的葡萄位姿估计方法，包括如下步骤：采集葡萄的原始点云数据；把葡萄颗粒进行实例级分割并进行二值化处理，索引葡萄颗粒分割区域的点云数据；滤波处理；采用椭球曲面来计算葡萄颗粒外表面的几何外形尺寸；基于迭代最近点算法点的点云配准；基于遗传算法进行葡萄颗粒几何模型参数调优；将每个葡萄颗粒的最佳模型的中心点分别记为每个葡萄颗粒的三维形心；拟合出用于葡萄串采摘位姿求解的直线L1；采摘机器人末端执行器的位姿求解。该方法可求解出葡萄串相对于采摘机器人的位姿，为采摘机器人合理性采摘葡萄串提供相应的解决方法，避免由于采摘姿态不合理而导致葡萄脱落与损坏。
Description
技术领域
本发明涉及智能采摘机器人技术领域，更具体地说，涉及一种基于局部点云的葡萄位姿估计方法。
背景技术
我国是葡萄产量较多的国家，且近年来葡萄采摘需求量日益增多，全自动采摘机器人的研发重要性不言而喻。但是在现有的机器人采摘作业中，对于葡萄采摘机器人的采摘位姿研究研究相对较少，采取较为普遍方法是通过果实、果梗图像识别，确定果实的位置然后进行直接抓取，较少考虑葡萄采摘机器人的抓取姿态的问题。
由于葡萄皮薄肉嫩，在采摘机器人的采摘作业中，机器人机械手与葡萄果体直接刚性接触，不合理的采摘姿态容易造成葡萄脱落与损坏。而面对果梗被葡萄叶大量枝叶遮挡，视觉系统无法直接获取采摘点的位置；且现有基于串型果实的识别，仅仅是提供了果实的采摘位置，由于葡萄串形状不规则形，因此难以求解其姿态。因此机器手该以一个合理位姿去抓取形状不规则的串型葡萄，是目前采摘机器人领域需要亟待攻克的难题之一。
发明内容
为克服现有技术中的缺点与不足，本发明的目的在于提供一种基于局部点云的葡萄位姿估计方法；该方法可求解出葡萄串相对于采摘机器人的位姿，为采摘机器人合理性采摘葡萄串提供相应的解决方法，避免由于采摘姿态不合理而导致葡萄脱落与损坏。
为了达到上述目的，本发明通过下述技术方案予以实现：一种基于局部点云的葡萄位姿估计方法，其特征在于：包括如下步骤：
S1、利用ZED双目相机在种植园采集葡萄的原始点云数据；
S2、利用深度学习的目标检测Mask R-CNN模型，把葡萄颗粒在复杂的背景进行实例级分割，并对葡萄颗粒区域进行二值化处理，索引葡萄颗粒分割区域的点云数据，从而获取除去背景的葡萄颗粒原始点云数据；
S3、利用统计滤波方法对葡萄颗粒原始点云数据的大尺寸噪声进行处理；利用双边滤波方法对葡萄颗粒原始点云数据的小尺寸噪声进行处理；
S4、将葡萄颗粒原始点云数据的离群点删除；
S5、采用椭球曲面来计算葡萄颗粒外表面的几何外形尺寸；
S6、基于迭代最近点算法点的点云配准：通过迭代最近点算法求取葡萄颗粒的离散点云集与离散的葡萄颗粒点云集的最佳平移向量T和旋转矩阵R，并对点集移动后的位置进行评估；
S7、基于遗传算法的葡萄颗粒几何模型参数调优；
S8、基于遗传算法求得每个葡萄颗粒的最佳模型，将每个葡萄颗粒的最佳模型的中心点分别记为每个葡萄颗粒的三维形心C(xC,yC,zC)；
S9、葡萄串的三维形心点集设为O1{C1,C2,…,Cn},对于点集O1采用点云库中随机样本一致性算法拟合一条用于葡萄串采摘位姿求解的直线L1；
S10、采摘机器人末端执行器的位姿求解：建立葡萄坐标系，求解葡萄坐标系在相机坐标系下的位姿便以求得葡萄串相对于相机坐标系的位姿；再经过仿射变换，求得葡萄串相对于采摘机器人的位姿。
优选地，所述S4包括如下步骤：
S41、K-邻域搜索及点与邻域点的距离计算：设定葡萄颗粒原始点云数据中各个点为Pi(xi,yi,zi)，i＝0，1，…；检索当前点Pi及搜索距离Pi最近的K个邻近点Pj，设定为Pi的K-邻域，记为R(Pi)；计算当前点Pi在其邻域内与各个邻近点Pj的距离，并计算平均值D(Pi)与方差F(Pi)：
S42、重复步骤S41，直至遍历完所有葡萄颗粒原始点云数据；设定平均距离阈值Dg与方差阈值Fg；若D(Pi)>Dg且F(Pi)>Fg,则认为当前点Pi为离散点，并从葡萄颗粒原始点云数据中删除。
优选地，所述步骤S5，是指：设定葡萄颗粒外表面为椭球曲面椭球曲面的坐标值为(xQ,yQ,zQ)：
其中,a为椭球曲面与X轴交点到中心的距离；b为椭球曲面与Y轴交点到中心的距离；c为椭球曲面与Z轴交点到中心的距离；θ和分别为椭球曲面方程参数。
优选地，所述S6步包括如下步骤：
S61、输入滤波后的葡萄颗粒云P与离散的葡萄颗粒点云集O；
S62、设置配准过程中的最大迭代次数max；
S63、利用对应点平均距离最小的刚体变换，求得平移向量T和旋转矩阵R，从而得到新的变换点集P′；
S64、计算点集P′和离散的葡萄颗粒点云集O之间的距离S：
S65、更新最优值：判断当前迭代的SN的大小：若当前迭代的SN小于现有最小距离Smin,则更新两个点集之间的最小距离Smin＝SN；
S66、判断是否达到最大的迭代次数：若是则输出两个点集的距离Smin，并将距离Smin对应的平移向量T和旋转矩阵R作为最佳平移向量T和旋转矩阵R；否则跳至步骤S63进行下一迭代处理。
优选地，所述步骤S7包括如下步骤：
S71、输入滤波后的葡萄颗粒点云P；
S72、随机初始化种群，种群规模记为S；采用二进制编码法对染色体进行编码；其中，染色体代表了葡萄颗粒模型的三个参数a,b,c；
S73、计算种群中每个个体的适应度fitvalue；染色体编码解码可得相应参数a,b,c，由参数a,b,c求得对应离散的葡萄颗粒点云集O，将葡萄颗粒局部点云集P与离散的葡萄颗粒点云集O作为迭代最近点算法的点云输入，将该算法求解出来的适合性评价得分值的Smin作为个体适应度fitvalue；
S74、把最小的个体适应度记为bestfitvalue，求得对应的最佳个体；
S75、对种群进行遗传处理：选择，交叉以及变异；其中的选择操作采用精英选择策略，调节交叉概率参数与变异概率参数；
S76、通过迭代最近点算法计算每次更新的个体适应度，并计算新一代最佳适应度bestvalue和新一代最佳个体bestindividual；
S77、若出现bestvalue比当前的bestvalue值更小时，当前的bestfvalue和bestindividual都会被新一代的bestfvalue和相应的bestindividual替代；
S78、判断是否达到最大迭代次数，若达到最大迭代次数，执行步骤S79；否则返回到步骤S75进行下一迭代处理；
S79、将最佳个体的染色体解码得到葡萄颗粒模型的三个参数a,b,c。
优选地，所述步骤S9包括如下步骤：
S91、输入葡萄串的三维形心点集O1，并随机设置内群点；
S92、计算适合内群的直线模型；
S93、把其他没选到的点带入建立的直线模型中；
S94、计算直线模型中内群点数量；
S95、判断是否达到最大迭代次数，若达到最大迭代次数，执行步骤S96；否则返回到步骤S91进行下一迭代处理；
S96、比较每次迭代计算内群点数量：将内群点数量最多的直线模型设定为所求的用于葡萄串采摘位姿求解的直线L1，并记录当前直线模型的参数。
优选地，所述步骤S10中，葡萄串相对于相机坐标系的位姿的求解方法包括如下步骤：
S101、以直线L1在果实区域的直线段中点Pm(xm,ym,zm)为原点，以直线L1朝上的方向矢量为葡萄坐标系的z轴正方向,记该方向矢量为其单位矢量为/>
S102、过直线L2作采摘机器人基座坐标系OXY平面的垂直平面,求解出该平面的法线L2,以直线L2与相机坐标系中的x轴正向一致的方向矢量为葡萄坐标系的y轴正方向，记为其单位矢量为/>
S103、葡萄坐标系中x轴正方向的方向矢量单位矢量为
S104、葡萄坐标系相对于相机坐标系的姿态为：
与现有技术相比，本发明具有如下优点与有益效果：
1、本发明方法可求解出葡萄串相对于采摘机器人的位姿，为采摘机器人合理性采摘葡萄串提供相应的解决方法，避免由于采摘姿态不合理而导致葡萄脱落与损坏；
2、本发明方法中，局部葡萄颗粒点云与葡萄颗粒模型通过迭代最近点算法进行点云匹配，并通过遗传算法进行葡萄颗粒模型的优化；两种算法的迭代使用，可以模拟出相对优化的颗粒匹配效果；
3、本发明方法中，通过多个葡萄颗粒的形心，利用随机样本一致性算法拟合直线L1，利用直线L1来确定果实的位姿；基于葡萄颗粒形心估计果实的姿态，为形状复杂的串型果实提供一种较为合理的位姿估计方法；
4、本发明方法算法运算量小，有利于提高采摘效率。
附图说明
图1是本发明葡萄位姿估计方法的流程图；
图2(a)～图2(g)是本发明葡萄位姿估计方法的简述图；
图3是本发明葡萄位姿估计方法中步骤S6和S7的流程图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细的描述。
实施例
本实施例一种基于局部点云的葡萄位姿估计方法，其流程如图1以及图2(a)～图2(g)所示；其中，图2(a)原始局部点云，图2(b)是果粒识别后的局部点云，图2(c)是果粒模型建立，图2(d)离散果粒模型与局部点云配准，图2(e)是颗粒模型参数调节过程，图2(f)是颗粒形心拟合直线L1，图2(g)是位姿求解。
葡萄位姿估计方法包括如下步骤：
S1、利用ZED双目相机在种植园采集葡萄的原始点云数据。
S2、利用深度学习的目标检测Mask R-CNN模型，把葡萄颗粒在复杂的背景进行实例级分割，并对葡萄颗粒区域进行二值化处理，索引葡萄颗粒分割区域的点云数据，从而获取除去背景的葡萄颗粒原始点云数据。
S3、直接从复杂背景中获取的葡萄原始点云数据会附带许多噪点并且点云数据冗余，由此利用统计滤波方法对葡萄颗粒原始点云数据的大尺寸噪声进行处理；利用双边滤波方法对葡萄颗粒原始点云数据的小尺寸噪声进行处理。
S4、葡萄颗粒原始点云数据较多情况下会存在离群点；将葡萄颗粒原始点云数据的离群点删除。
具体地说，所述S4包括如下步骤：
S41、K-邻域搜索及点与邻域点的距离计算：设定葡萄颗粒原始点云数据中各个点为Pi(xi,yi,zi)，i＝0，1，…；检索当前点Pi及搜索距离PiPi最近的K个邻近点Pj，设定为Pi的K-邻域，记为R(Pi)；计算当前点Pi在其邻域内与各个邻近点Pj的距离，并计算平均值D(Pi)与方差F(Pi)：
S42、重复步骤S41，直至遍历完所有葡萄颗粒原始点云数据；设定平均距离阈值Dg与方差阈值Fg；若D(Pi)>Dg且F(Pi)>Fg,则认为当前点Pi为离散点，并从葡萄颗粒原始点云数据中删除。
S5、采用椭球曲面来计算葡萄颗粒外表面的几何外形尺寸。
具体地说，葡萄颗粒的形状近似一个椭球形曲面，可以采用椭球形曲面来替代葡萄颗粒的曲面；设定葡萄颗粒外表面为椭球曲面椭球曲面/>的坐标值为(xQ,yQ,zQ)：
其中,a为椭球曲面与X轴交点到中心距离；b为椭球曲面与Y轴交点到中心的距离；c为椭球曲面与Z轴交点到中心的距离；θ和分别为椭球曲面方程参数；
S6、基于迭代最近点算法点的点云配准：为了使葡萄颗粒的离散点云与离散的葡萄颗粒点云达到最大限度的重合度，需要求取葡萄颗粒的离散点云集与离散的葡萄颗粒点云集之间的最佳变换位置。通过迭代最近点算法求取葡萄颗粒的离散点云集与离散的葡萄颗粒点云集的最佳平移向量T和旋转矩阵R，并对点集移动后的位置进行评估。
具体地说，如图3所示，所述S6步包括如下步骤：
S61、输入滤波后的葡萄颗粒云P与离散的葡萄颗粒点云集O；
S62、设置配准过程中的最大迭代次数max；
S63、利用对应点平均距离最小的刚体变换，求得平移参数T和旋转参数R，从而得到新的变换点集P′；
S64、计算点集P′和离散的葡萄颗粒点云集O之间的距离S，S越小重合度越好：
S65、更新最优值：判断当前迭代的SN的大小：若当前迭代的SN小于现有最小距离Smin,则更新两个点集之间的最小距离Smin＝SN；
S66、判断是否达到最大的迭代次数：若是则输出两个点集的距离Smin，并将距离Smin对应的平移向量T和旋转矩阵R作为最佳平移向量T和旋转矩阵R；否则跳至步骤S63进行下一迭代处理。
S7、基于遗传算法的葡萄颗粒几何模型参数调优；葡萄颗粒的几何模型大小由a,b,c这三个参数决定，不同的参数组合都会得到不同的葡萄颗粒模型。当匹配到的葡萄颗粒几何模型过大或者过小均不能与局部的葡萄颗粒点云完全重合。由此，需要寻找与真实的葡萄颗粒尺寸最为接近的几何模型，可采用遗传算法求解最优的葡萄颗粒模型参数。
具体地说，所述步骤S7包括如下步骤：
S71、输入滤波后的葡萄颗粒点云P；
S72、随机初始化种群，种群规模记为S；采用二进制编码法对染色体进行编码；其中，染色体代表了葡萄颗粒模型的三个参数a,b,c,长度为16；前四位代表参数a,第5～8位代表参数b，第9～16位代表参数c；
S73、计算种群中每个个体的适应度fitvalue；染色体编码解码可得相应参数a,b,c，由参数a,b,c求得对应离散的葡萄颗粒点云集O，将葡萄颗粒局部点云集P与离散的葡萄颗粒点云集O作为迭代最近点算法(ICP)的点云输入，将该算法求解出来的适合性评价得分值的Sm作为个体适应度fitvalue；
S74、把最小的个体适应度记为bestfitvalue，求得对应的最佳个体；
S75、对种群进行遗传处理：选择，交叉以及变异；其中的选择操作采用精英选择策略，调节交叉概率参数与变异概率参数；
S76、通过迭代最近点算法计算每次更新的个体适应度，并计算新一代最佳适应度bestvalue和新一代最佳个体bestindividual；
S77、若出现bestvalue比当前的bestvalue值更小时，当前的bestfvalue和bestindividual都会被新一代的bestfvalue和相应的bestindividual替代；
S78、判断是否达到最大迭代次数，若达到最大迭代次数，执行步骤S79；否则返回到步骤S75进行下一迭代处理；
S79、将最佳个体的染色体解码得到葡萄颗粒模型的三个参数a,b,c。
S8、基于遗传算法求得每个葡萄颗粒的最佳模型，将每个葡萄颗粒的最佳模型的中心点分别记为每个葡萄颗粒的三维形心C(xC,yC,zC)。
S9、葡萄串的三维形心点集设为O1{C1,C2,…,Cn},对于点集O1采用点云库(PCL)中随机样本一致性算法(Ransac)拟合一条用于葡萄串采摘位姿求解的直线L1。
具体地说，所述步骤S9包括如下步骤：
S91、输入葡萄串的三维形心点集O1，并随机设置内群点；
S92、计算适合内群的直线模型；
S93、把其他没选到的点带入建立的直线模型中；
S94、计算直线模型中内群点数量；
S95、判断是否达到最大迭代次数，若达到最大迭代次数，执行步骤S96；否则返回到步骤S91进行下一迭代处理；
S96、比较每次迭代计算内群点数量：将内群点数量最多的直线模型设定为所求的用于葡萄串采摘位姿求解的直线L1，并记录当前直线模型的参数。
S10、采摘机器人末端执行器的位姿求解：建立葡萄坐标系，求解葡萄坐标系在相机坐标系下的位姿便以求得葡萄串相对于相机坐标系的位姿；再经过仿射变换，求得葡萄串相对于采摘机器人的位姿。
具体地说，所述步骤S10中，葡萄串相对于相机坐标系的位姿的求解方法包括如下步骤：
S101、以直线L1在果实区域的直线段中点Pm(xm,ym,zm)为原点，以直线L1朝上的方向矢量为葡萄坐标系的z轴正方向,记该方向矢量为其单位矢量为/>
S102、过直线L2作采摘机器人基座坐标系OXY平面的垂直平面,求解出该平面的法线L2,以直线L2与相机坐标系中的x轴正向一致的方向矢量为葡萄坐标系的y轴正方向，记为其单位矢量为/>
S103、葡萄坐标系中x轴正方向的方向矢量其单位矢量为
S104、葡萄坐标系相对于相机坐标系的位姿为：
上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。
Claims (7)
1.一种基于局部点云的葡萄位姿估计方法，其特征在于：包括如下步骤：
S1、利用相机在种植园采集葡萄的原始点云数据；
S2、利用深度学习的目标检测Mask RCNN模型，把葡萄颗粒在复杂的背景进行实例级分割，并对葡萄颗粒区域进行二值化处理，索引葡萄颗粒分割区域的点云数据，从而获取除去背景的葡萄颗粒原始点云数据；
S3、利用统计滤波方法对葡萄颗粒原始点云数据的大尺寸噪声进行处理；利用双边滤波方法对葡萄颗粒原始点云数据的小尺寸噪声进行处理；
S4、将葡萄颗粒原始点云数据的离群点删除；
S5、采用椭球曲面来计算葡萄颗粒外表面的几何外形尺寸；
S6、基于迭代最近点算法点的点云配准：通过迭代最近点算法求取葡萄颗粒的离散点云集与离散的葡萄颗粒点云集的最佳平移向量T和旋转矩阵R，并对点集移动后的位置进行评估；
S7、基于遗传算法的葡萄颗粒几何模型参数调优；
S8、基于遗传算法求得每个葡萄颗粒的最佳模型，将每个葡萄颗粒的最佳模型的中心点分别记为每个葡萄颗粒的三维形心C(xC,yC,zC)；
S9、葡萄串的三维形心点集设为O1{C1,C2,…,Cn},对于点集O1采用点云库中随机样本一致性算法拟合一条用于葡萄串采摘位姿求解的直线L1；
S10、采摘机器人末端执行器的位姿求解：建立葡萄坐标系，求解葡萄坐标系在相机坐标系下的位姿便以求得葡萄串相对于相机坐标系的位姿；再经过仿射变换，求得葡萄串相对于采摘机器人的位姿。
2.根据权利要求1所述的基于局部点云的葡萄位姿估计方法，其特征在于：所述S4包括如下步骤：
S41、K-邻域搜索及点与邻域点的距离计算：设定葡萄颗粒原始点云数据中各个点为Pi(xi,yi,zi)，i＝0，1，…；检索当前点Pi及搜索距离Pi最近的K个邻近点Pj，设定为Pi的K-邻域，记为R(Pi)；计算当前点Pi在其邻域内与各个邻近点Pj的距离，并计算平均值D(Pi)与方差F(Pi)：
S42、重复步骤S41，直至遍历完所有葡萄颗粒原始点云数据；设定平均距离阈值Dg与方差阈值Fg；若D(Pi)>Dg且F(Pi)>Fg,则认为当前点Pi为离散点，并从葡萄颗粒原始点云数据中删除。
3.根据权利要求1所述的基于局部点云的葡萄位姿估计方法，其特征在于：所述步骤S5，是指：设定葡萄颗粒外表面为椭球曲面椭球曲面/>的坐标值为(xQ,yQ,zQ)：
0≤θ≤π，
其中,a为椭球曲面与X轴交点到中心距离；b为椭球曲面与Y轴交点到中心的距离；c为椭球曲面与Z轴交点到中心的距离；θ和分别为椭球曲面方程参数。
4.根据权利要求1所述的基于局部点云的葡萄位姿估计方法，其特征在于：所述S6包括如下步骤：
S61、输入滤波后的葡萄颗粒云P与离散的葡萄颗粒点云集O；
S62、设置配准过程中的最大迭代次数max；
S63、利用对应点平均距离最小的刚体变换，求得平移向量T和旋转矩阵R，从而得到新的变换点集P′；
S64、计算点集P′和离散的葡萄颗粒点云集O之间的距离S：
S65、更新最优值：判断当前迭代的SN的大小：若当前迭代的SN小于现有最小距离Smin,则更新两个点集之间的最小距离Smin＝SN；
S66、判断是否达到最大的迭代次数：若是则输出两个点集的距离Smin，并将距离Smin对应的平移向量T和旋转矩阵R作为最佳平移向量T和旋转矩阵R；否则跳至步骤S63进行下一迭代处理。
5.根据权利要求4所述的基于局部点云的葡萄位姿估计方法，其特征在于：所述步骤S7包括如下步骤：
S71、输入滤波后的葡萄颗粒点云P；
S72、随机初始化种群，种群规模记为S；采用二进制编码法对染色体进行编码；其中，染色体代表了葡萄颗粒模型的三个参数a,b,c；
S73、计算种群中每个个体的适应度fitvalue；染色体编码解码得相应参数a,b,c，由参数a,b,c求得对应离散的葡萄颗粒点云集O，将葡萄颗粒局部点云集P与离散的葡萄颗粒点云集O作为迭代最近点算法的点云输入，将迭代最近点算法求解出来的适合性评价得分值的Smin作为个体适应度fitvalue；
S74、把最小的个体适应度记为bestfitvalue，求得对应的最佳个体；
S75、对种群进行遗传处理：选择，交叉以及变异；其中的选择操作采用精英选择策略，调节交叉概率参数与变异概率参数；
S76、通过迭代最近点算法计算每次更新的个体适应度，并计算新一代最佳适应度bestvalue和新一代最佳个体bestindividual；
S77、若出现bestvalue比当前的bestvalue值更小时，当前的bestfvalue和bestindividual都会被新一代的bestfvalue和相应的bestindividual替代；
S78、判断是否达到最大迭代次数，若达到最大迭代次数，执行步骤S79；否则返回到步骤S75进行下一迭代处理；
S79、将最佳个体的染色体解码得到葡萄颗粒模型的三个参数a,b,c。
6.根据权利要求5所述的基于局部点云的葡萄位姿估计方法，其特征在于：所述步骤S9包括如下步骤：
S91、输入葡萄串的三维形心点集O1，并随机设置内群点；
S92、计算适合内群的直线模型；
S93、把其他没选到的点带入建立的直线模型中；
S94、计算直线模型中内群点数量；
S95、判断是否达到最大迭代次数，若达到最大迭代次数，执行步骤S96；否则返回到步骤S91进行下一迭代处理；
S96、比较每次迭代计算内群点数量：将内群点数量最多的直线模型设定为所求的用于葡萄串采摘位姿求解的直线L1，并记录当前直线模型的参数。
7.根据权利要求6所述的基于局部点云的葡萄位姿估计方法，其特征在于：所述步骤S10中，葡萄串相对于相机坐标系的位姿的求解方法包括如下步骤：
S101、以直线L1在果实区域的直线段中点Pm(xm,ym,zm)为原点，以直线L1朝上的方向矢量为葡萄坐标系的z轴正方向,记该方向矢量为其单位矢量为/>
S102、过直线L2作采摘机器人基座坐标系OXY平面的垂直平面,求解出该平面的直线L2,以直线L2与相机坐标系中的x轴正向一致的方向矢量为葡萄坐标系的y轴正方向，记为其单位矢量为/>
S103、葡萄坐标系中x轴正方向的方向矢量其单位矢量为/>
S104、葡萄坐标系相对于相机坐标系的位姿为：
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911198014.8A CN111062988B (zh) | 2019-11-29 | 2019-11-29 | 一种基于局部点云的葡萄位姿估计方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911198014.8A CN111062988B (zh) | 2019-11-29 | 2019-11-29 | 一种基于局部点云的葡萄位姿估计方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111062988A CN111062988A (zh) | 2020-04-24 |
| CN111062988B true CN111062988B (zh) | 2024-02-13 |
Family
ID=70299287
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911198014.8A Active CN111062988B (zh) | 2019-11-29 | 2019-11-29 | 一种基于局部点云的葡萄位姿估计方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111062988B (zh) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111583328B (zh) * | 2020-05-06 | 2021-10-22 | 南京农业大学 | 基于几何模型的绿萝叶片外部表型参数的三维估测方法 |
| CN112001955A (zh) * | 2020-08-24 | 2020-11-27 | 深圳市建设综合勘察设计院有限公司 | 一种基于二维投影平面匹配约束的点云配准方法及系统 |
| CN112784735B (zh) * | 2021-01-21 | 2021-10-01 | 重庆文理学院 | 基于单目相机与双目相机识别串型水果母枝的方法 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2011257244A (ja) * | 2010-06-08 | 2011-12-22 | Toshiba Corp | 物体形状検出装置及び方法 |
| CN108876852A (zh) * | 2017-05-09 | 2018-11-23 | 中国科学院沈阳自动化研究所 | 一种基于3d视觉的在线实时物体识别定位方法 |
| WO2019121056A1 (fr) * | 2017-12-21 | 2019-06-27 | Commissariat A L'energie Atomique Et Aux Energies Alternatives | Methode de reconnaissance d'objets dans une scene observee en trois dimensions |
| CN110226413A (zh) * | 2019-06-26 | 2019-09-13 | 佛山科学技术学院 | 一种机器人采摘多串堆叠葡萄的方法 |
| CN110249793A (zh) * | 2019-05-09 | 2019-09-20 | 江苏大学 | 一种棚架葡萄收获的机器人末端-深度相机配置方法与伺服控制方法 |
| CN110298885A (zh) * | 2019-06-18 | 2019-10-01 | 仲恺农业工程学院 | 一种非光滑类球体目标的立体视觉识别方法和定位夹持检测装置及其应用 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB201504360D0 (en) * | 2015-03-16 | 2015-04-29 | Univ Leuven Kath | Automated quality control and selection system |
-
2019
- 2019-11-29 CN CN201911198014.8A patent/CN111062988B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2011257244A (ja) * | 2010-06-08 | 2011-12-22 | Toshiba Corp | 物体形状検出装置及び方法 |
| CN108876852A (zh) * | 2017-05-09 | 2018-11-23 | 中国科学院沈阳自动化研究所 | 一种基于3d视觉的在线实时物体识别定位方法 |
| WO2019121056A1 (fr) * | 2017-12-21 | 2019-06-27 | Commissariat A L'energie Atomique Et Aux Energies Alternatives | Methode de reconnaissance d'objets dans une scene observee en trois dimensions |
| CN110249793A (zh) * | 2019-05-09 | 2019-09-20 | 江苏大学 | 一种棚架葡萄收获的机器人末端-深度相机配置方法与伺服控制方法 |
| CN110298885A (zh) * | 2019-06-18 | 2019-10-01 | 仲恺农业工程学院 | 一种非光滑类球体目标的立体视觉识别方法和定位夹持检测装置及其应用 |
| CN110226413A (zh) * | 2019-06-26 | 2019-09-13 | 佛山科学技术学院 | 一种机器人采摘多串堆叠葡萄的方法 |
Non-Patent Citations (1)
| Title |
|---|
| Vision-based extraction of spatial information in grape clusters for harvesting robots;Lufeng Luo 等;《Biosystems Engineering》;20160915;第151卷;第90-104页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111062988A (zh) | 2020-04-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111062988B (zh) | 一种基于局部点云的葡萄位姿估计方法 | |
| Li et al. | Detection of fruit-bearing branches and localization of litchi clusters for vision-based harvesting robots | |
| CN109345527B (zh) | 一种基于MaskRcnn的膀胱肿瘤检测方法 | |
| CN108921019B (zh) | 一种基于GEI和TripletLoss-DenseNet的步态识别方法 | |
| Wang et al. | A study on long-close distance coordination control strategy for litchi picking | |
| Li et al. | An overlapping-free leaf segmentation method for plant point clouds | |
| CN104881671B (zh) | 一种基于2D‑Gabor的高分遥感影像局部特征提取方法 | |
| CN110111430B (zh) | 一种从三维点云中提取二次曲面的方法 | |
| CN109712160B (zh) | 基于广义熵结合改进的狮群算法实现图像阈值分割方法 | |
| CN112435239A (zh) | 基于MRE-PointNet和自编码器模型的绿萝叶片外形参数估测方法 | |
| WO2022237225A1 (zh) | 针对具有平面基准但不完整的三维扫描点云的在线实时配准方法 | |
| CN113191973B (zh) | 一种基于无监督网络框架的文物点云数据去噪方法 | |
| CN107123138B (zh) | 基于vanilla-R点对剔除策略的点云配准方法 | |
| Li et al. | Development and field evaluation of a robotic harvesting system for plucking high-quality tea | |
| CN114822823B (zh) | 基于云计算和人工智能的融合多维度医疗数据的肿瘤精细分类系统 | |
| Zhu et al. | A method for detecting tomato canopies’ phenotypic traits based on improved skeleton extraction algorithm | |
| CN117408913B (zh) | 待测物体点云去噪方法、系统及装置 | |
| Yu et al. | Pose estimation for augmented reality applications using genetic algorithm | |
| Zhang et al. | Locating Famous Tea's Picking Point Based on Shi-Tomasi Algorithm. | |
| CN114782455B (zh) | 一种用于农机嵌入式设备的棉花行中心线图像提取方法 | |
| CN117152172A (zh) | 一种基于点云数据的输电线路杆塔和电力线提取方法 | |
| Tian et al. | Research on the application of machine vision in tea autonomous picking | |
| CN110570459A (zh) | 一种基于鼠态的运动跟踪方法 | |
| CN114049346B (zh) | 一种基于剪裁YOLOv3-SPP3的柑橘木虱检测与识别方法 | |
| Han et al. | Ellipsoid fitting using variable sample consensus and two-ellipsoid-bounding-counting for locating Lingwu long Jujubes in a natural environment |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |