CN110276806B - 用于四自由度并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法 - Google Patents
用于四自由度并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法 Download PDFInfo
- Publication number
- CN110276806B CN110276806B CN201910446270.8A CN201910446270A CN110276806B CN 110276806 B CN110276806 B CN 110276806B CN 201910446270 A CN201910446270 A CN 201910446270A CN 110276806 B CN110276806 B CN 110276806B
- Authority
- CN
- China
- Prior art keywords
- hand
- eye
- coordinate system
- parallel robot
- model
- 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
- 238000004364 calculation method Methods 0.000 title claims abstract description 37
- 230000033001 locomotion Effects 0.000 claims abstract description 177
- 230000007246 mechanism Effects 0.000 claims abstract description 143
- 230000036544 posture Effects 0.000 claims description 110
- 239000011159 matrix material Substances 0.000 claims description 99
- 238000013519 translation Methods 0.000 claims description 54
- 230000014616 translation Effects 0.000 claims description 54
- 230000009466 transformation Effects 0.000 claims description 47
- 238000000034 method Methods 0.000 claims description 40
- 239000013598 vector Substances 0.000 claims description 23
- 235000013399 edible fruits Nutrition 0.000 claims description 22
- 230000006872 improvement Effects 0.000 claims description 13
- 230000008859 change Effects 0.000 claims description 12
- 238000012937 correction Methods 0.000 claims description 10
- 238000010276 construction Methods 0.000 claims description 8
- 230000000007 visual effect Effects 0.000 claims description 8
- 230000003068 static effect Effects 0.000 claims description 6
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 230000009286 beneficial effect Effects 0.000 abstract description 2
- 238000003384 imaging method Methods 0.000 description 8
- 230000008569 process Effects 0.000 description 5
- 238000006243 chemical reaction Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 239000012636 effector Substances 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 235000011449 Rosa Nutrition 0.000 description 2
- 101100233916 Saccharomyces cerevisiae (strain ATCC 204508 / S288c) KAR5 gene Proteins 0.000 description 2
- 238000012271 agricultural production Methods 0.000 description 2
- 238000000354 decomposition reaction Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 101001121408 Homo sapiens L-amino-acid oxidase Proteins 0.000 description 1
- 101000827703 Homo sapiens Polyphosphoinositide phosphatase Proteins 0.000 description 1
- 102100026388 L-amino-acid oxidase Human genes 0.000 description 1
- 102100023591 Polyphosphoinositide phosphatase Human genes 0.000 description 1
- 101100012902 Saccharomyces cerevisiae (strain ATCC 204508 / S288c) FIG2 gene Proteins 0.000 description 1
- 241000219094 Vitaceae Species 0.000 description 1
- 241000219095 Vitis Species 0.000 description 1
- 235000009754 Vitis X bourquina Nutrition 0.000 description 1
- 235000012333 Vitis X labruscana Nutrition 0.000 description 1
- 235000014787 Vitis vinifera Nutrition 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 230000005603 centrifugal distortion Effects 0.000 description 1
- 230000001066 destructive effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 235000021021 grapes Nutrition 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1612—Programme controls characterised by the hand, wrist, grip control
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1679—Programme controls characterised by the tasks executed
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
- B25J9/1697—Vision controlled systems
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T1/00—General purpose image data processing
- G06T1/0014—Image feed-back for automatic industrial control, e.g. robot with camera
-
- 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
- G06T7/85—Stereo camera calibration
-
- 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/10004—Still image; Photographic image
- G06T2207/10012—Stereo images
-
- 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/10024—Color image
-
- 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/30—Subject of image; Context of image processing
- G06T2207/30108—Industrial image inspection
- G06T2207/30164—Workpiece; Machine component
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Health & Medical Sciences (AREA)
- General Health & Medical Sciences (AREA)
- Orthopedic Medicine & Surgery (AREA)
- Image Processing (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种用于四自由度并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法。首先,构建相机固定在机器人本体外的Eye‑to‑hand手眼基础模型,以及手眼系统中基于非线性畸变的立体视觉模型,接着,基于末端夹持机构标定运动间的位姿关系,构建Eye‑to‑hand模型的非平凡解约束,用于剔除标定运动中的无效位姿,以规划并联机器人末端夹持机构的手眼标定运动,最后,采用基于手眼标定获取的机器人运动误差构建具有误差补偿的并联机器人抓取模实现基于立体视觉和4‑R(2‑SS)并联机器人的末端夹持机构的抓取位姿计算。本发明可有效提高四自由度4‑R(2‑SS)并联机器人立体视觉手眼系统的在线手眼标定精度和效率,有利于进一步实现并联机器人的准确快速抓取。
Description
技术领域
本发明涉及机器视觉领域,尤其涉及基于机器视觉和图像处理,针对四自由度4-R(2-SS)(R表示旋转关节,S表示球面副,4-R(2-SS)表示并联机器人由4个运动学结构相同的R(2-SS)支链构成)并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法,用于并联机器人实现基于立体视觉的堆叠串类水果自动分拣。
背景技术
近年来我国的水果产量增长迅猛,传统人工分拣方法已很难满足现代农业生产的需求,基于机器人技术实现水果的自动分拣对农业生产和农产品加工的自动化、规模化、精确化发展具有重要意义。在基于机器人的水果自动分拣过程中,高精度高效率的在线手眼标定是机器人实现准确、快速、无损抓取控制的前提条件。同时,并联机器人因具有精度高、速度快、刚度大等优点,对基于并联机器人的水果自动分拣中在线手眼标定的准确性和快速性提出了更高的要求。目前,手眼系统根据相机与机器人末端执行器之间的位姿关系可分为Eye-to-Hand和Eye-in-Hand。Eye-to-Hand系统的相机固定在机器人本体外的空间中,相机与世界坐标系的位姿保持不变,但与末端执行器之间位姿随机器人的运动而变化。Eye-in-Hand系统的相机安装在机器人末端执行器上,并随机器人一起运动,两者之间的相对位姿保持不变。Eye-in-Hand手眼系统虽然可以移动相机靠近目标物体获取清晰的图像,但难以保证目标物体出现在视场范围内。同时相机在移动时的抖动和拍照时刻的加速度引起的图像拖影对标定和目标检测精度也有一定的影响。而相机相对世界坐标系静止的Eye-to-Hand系统,由于检测精度高、稳定性好,较适用于本发明的工作范围有限的并联机器人水果分拣系统。
传统的手眼标定方法中的模型解算需要机器人在标定时做多方向的旋转运动和大幅度的平移运动。对于具有自由度限制的并联机器人,难以基于传统的手眼模型解算方法求取精确的标定参数。有学者提出限制相机在标定过程中的旋转角度不等于零度的方法,但对于具有旋转自由度限制的机器人,无法精确计算其Z方向平移分量。也有学者利用机器人末端完成三次相互正交的平移动作,对旋转分量线性化从而达到求解条件。但同时平移运动的误差也会给手眼模型解算的精度带来影响。也有学者融合旋转运动约束和平移运动约束构造约束矩阵实现四自由度和六自由度机器人的手眼标定,但其仍未考虑到Z方向平移分量。后期,也有学者对类似于直角坐标机器人和SCARA型水平四轴机器人的具有旋转自由度限制的机器人进行研究,通过设置相机为正交投影姿态,利用二维的手眼标定模型降低手眼标定矩阵的未知参数个数,实现具有旋转自由度限制的机器人标定模型的解算。但其Z方向的平移分量的计算需要基于额外的参考物进行修正,精度受参考物厚度、加工误差等因素的影响较大。
因此,现有传统的手眼标定方法及其针对机器人自由度限制的手眼标定改进方法在精度和速度上均难以满足具有旋转运动限制的四自由度4-R(2-SS)并联机器人分拣系统的手眼标定需求。同时,相机标定误差、机器人运动误差以及无效的机器人标定运动位姿的存在均给实现高精度高效率的4-R(2-SS)并联机器人在线手眼标定和目标的抓取位姿计算带来了困难。基于此,本发明提出一种用于四自由度4-R(2-SS)并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法。根据相机间位姿关系构建立体视觉中各相机与机器人的手眼模型组,以改进单相机的Eye-to-hand基础模型,并对改进后的Eye-to-hand模型进行机器人运动误差补偿,以降低相机标定误差和机器人运动误差对手眼标定精度的影响。并基于标定板和末端夹持机构的垂直约束对手眼标定的垂直分量进行修正,解决现有传统手眼模型解算方法无法实现4-R(2-SS)并联机器人垂直分量准确求取的问题。同时构建Eye-to-hand模型的非平凡解约束,用于剔除标定中的无效运动位姿,以规划并联机器人手眼标定运动,提高在线手眼标定的精度和效率,并进一步实现并联机器人的准确快速抓取。
发明内容
本发明公开了一种用于四自由度4-R(2-SS)并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法。首先,构建相机固定在机器人本体外的Eye-to-hand手眼基础模型,以及手眼系统中基于非线性畸变的立体视觉模型。同时,根据相机间位姿关系构建立体视觉中各相机与机器人的手眼模型组,以改进单相机的Eye-to-hand基础模型;并对改进后的Eye-to-hand模型进行机器人运动误差补偿,以减少相机标定误差和机器人运动误差对4-R(2-SS)并联机器人Eye-to-hand模型精度的影响;其次,根据各相机获取的机器人多次运动的标定数据,基于并联机器人中标定板和末端夹持机构的垂直约束对手眼标定位姿参数中的垂直分量进行修正,实现对具有旋转运动约束的四自由度4-R(2-SS)并联机器人的手眼标定中所有位姿和运动误差的准确求取;接着,基于末端夹持机构标定运动间的位姿关系,构建Eye-to-hand模型的非平凡解约束,用于剔除标定运动中的无效位姿,以规划并联机器人末端夹持机构的手眼标定运动,实现高精度高效率的四自由度4-R(2-SS)并联机器人在线手眼标定;最后,采用基于手眼标定获取的机器人运动误差构建具有误差补偿的并联机器人抓取模型,同时,基于立体视觉模型计算相机坐标系下对象的最优抓取位姿,基于并联机器人运动学方程计算末端夹持机构在并联机器人基础坐标系下的当前位姿,并结合抓取模型和在线手眼标定得到的相机基础坐标系在并联机器人基础坐标系下的位姿,计算末端夹持机构当前位姿与最优抓取位姿之间的转换矩阵,实现基于立体视觉和4-R(2-SS)并联机器人的末端夹持机构的抓取位姿计算。本发明所构建的具有运动误差补偿的立体视觉改进Eye-to-hand模型、基于垂直分量修正的Eye-to-hand模型求解方法、基于Eye-to-hand模型非平凡解约束的机器人标定运动规划方法可有效提高四自由度4-R(2-SS)并联机器人立体视觉手眼系统的在线手眼标定精度和效率,有利于进一步实现并联机器人的准确快速抓取。
本发明的技术方案是采用如下步骤:
(1)具有运动误差补偿的立体视觉Eye-to-hand模型改进:构建相机固定在机器人本体外的Eye-to-hand手眼基础模型,以及手眼系统中基于非线性畸变的立体视觉模型。同时,根据相机间位姿关系构建立体视觉中各相机与机器人的手眼模型组,以改进单相机的Eye-to-hand基础模型;并对改进后的Eye-to-hand模型进行机器人运动误差补偿,以减少相机标定误差和机器人运动误差对4-R(2-SS)并联机器人Eye-to-hand模型精度的影响。
(2)基于垂直分量修正的Eye-to-hand模型求解:根据各相机获取的机器人多次运动的标定数据,基于并联机器人中标定板和末端夹持机构的垂直约束对手眼标定位姿参数中的垂直分量进行修正,实现对具有旋转运动约束的四自由度4-R(2-SS)并联机器人的手眼标定中所有位姿和运动误差的准确求取。
(3)基于Eye-to-hand模型非平凡解约束的4-R(2-SS)并联机器人标定运动规划:基于末端夹持机构标定运动间的位姿关系,构建Eye-to-hand模型的非平凡解约束,用于剔除标定运动中的无效位姿,以规划并联机器人末端夹持机构的手眼标定运动,实现高精度高效率的四自由度4-R(2-SS)并联机器人在线手眼标定。
(4)基于立体视觉和4-R(2-SS)并联机器人的抓取位姿计算:采用基于手眼标定获取的机器人运动误差构建具有误差补偿的并联机器人抓取模型,同时,基于立体视觉模型计算相机坐标系下对象的最优抓取位姿,基于并联机器人运动学方程计算末端夹持机构在并联机器人基础坐标系下的当前位姿,并结合抓取模型和在线手眼标定得到的相机基础坐标系在并联机器人基础坐标系下的位姿,计算末端夹持机构当前位姿与最优抓取位姿之间的转换矩阵,实现基于立体视觉和4-R(2-SS)并联机器人的末端夹持机构的抓取位姿计算。
本发明提出一种用于四自由度4-R(2-SS)并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法,通过采用上述技术方案后,其具有以下有益效果:
1、手眼标定误差主要来源于机器人运动误差引起的末端夹持机构坐标系在并联机器人基础坐标系下的位姿误差以及标定板坐标系在相机坐标系下的位姿误差。因此本发明构建相机固定在机器人本体外的Eye-to-hand手眼基础模型,以及手眼系统中基于非线性畸变的立体视觉模型。同时,根据相机间位姿关系构建立体视觉中各相机与机器人的手眼模型组,以改进单相机的Eye-to-hand基础模型;并对改进后的Eye-to-hand模型进行机器人运动误差补偿,以减少相机标定误差和机器人运动误差对4-R(2-SS)并联机器人Eye-to-hand模型精度的影响。
2、四自由度4-R(2-SS)并联机器人主体包括由4个运动学结构相同的R(2-SS)支链构成的并联机构和末端夹持机构,可实现空间三维平动和绕Z轴的一维转动。现有传统的手眼标定方法及其针对机器人自由度限制的手眼标定改进方法在精度和速度上均难以满足具有旋转运动限制的四自由度4-R(2-SS)并联机器人分拣系统的手眼标定需求。因此,本发明基于4-R(2-SS)并联机器人中标定板和末端夹持机构的垂直约束对基于矩阵向量化和直积的模型求解法求得的手眼标定位姿参数中的垂直分量进行修正,实现对具有旋转运动约束的四自由度4-R(2-SS)并联机器人的手眼标定中所有位姿和误差参数的准确求取。
3、无效的机器人标定运动位姿的存在均给实现高精度高效率的4-R(2-SS)并联机器人在线手眼标定和目标的抓取位姿计算带来了困难。因此本发明基于末端夹持机构标定运动间的位姿关系,构建Eye-to-hand模型的非平凡解约束,用于剔除标定运动中的无效位姿,以规划并联机器人末端夹持机构的手眼标定运动,实现高精度高效率的四自由度4-R(2-SS)并联机器人在线手眼标定。由于基于模型非平凡解约束的规划运动具有运动分布在并联机器人的工作空间中更加均匀、无效标定运动可提前剔除等优点,可提高标定精度,并有效降低在线标定时间,为基于并联机器人的高精度、高效率的实时抓取计算奠定基础。
4、针对基于传统手眼标定方法的手眼标定结果所计算的抓取位姿精度不高,并联机器人运动误差对抓取计算影响较大的问题,本发明基于机器人运动误差对并联机器人的抓取模型进行改进,并结合基于立体视觉模型获取的相机坐标系下对象的最优抓取位姿、基于并联机器人运动学方程求取的末端夹持机构在并联机器人基础坐标系下的当前位姿、以及基于在线手眼标定得到的相机基础坐标系在并联机器人基础坐标系下的位姿,计算末端夹持机构当前位姿与最优抓取位姿之间的转换矩阵,实现基于立体视觉和4-R(2-SS)并联机器人的末端夹持机构的最优抓取位姿计算。
附图说明
以下结合附图和具体实施方式对本发明作进一步详细说明。
图1为本发明用于四自由度4-R(2-SS)并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法的流程图。
图2为相机成像模型。
图3为本发明基于立体视觉和4-R(2-SS)并联机器人的串类水果分拣系统。
图4为本发明4-R(2-SS)并联机器人Eye-to-hand系统坐标关系图。
图5为本发明4-R(2-SS)并联机器人手眼标定平移运动路径规划。
图6为基于本发明规划运动的四自由度4-R(2-SS)并联机器人手眼标定。
图7为四自由度4-R(2-SS)并联机器人的并联机构结构图。
图8为基于模型非平凡解约束的并联机器人规划运动。
图9为标定板图像。其中:图9中的(a)为标定板彩色图像,图9中的(b)为标定板红外图像。
图10为彩色图像中的串类水果所在区域。
具体实施方式
下面结合附图进一步说明书本发明具体实施方式。
本发明公开了一种用于四自由度4-R(2-SS)并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法。首先,构建相机固定在机器人本体外的Eye-to-hand手眼基础模型,以及手眼系统中基于非线性畸变的立体视觉模型。同时,根据相机间位姿关系构建立体视觉中各相机与机器人的手眼模型组,以改进单相机的Eye-to-hand基础模型;并对改进后的Eye-to-hand模型进行机器人运动误差补偿,以减少相机标定误差和机器人运动误差对4-R(2-SS)并联机器人Eye-to-hand模型精度的影响;其次,根据各相机获取的机器人多次运动的标定数据,基于并联机器人中标定板和末端夹持机构的垂直约束对手眼标定位姿参数中的垂直分量进行修正,实现对具有旋转运动约束的四自由度4-R(2-SS)并联机器人的手眼标定中所有位姿和运动误差的准确求取;接着,基于末端夹持机构标定运动间的位姿关系,构建Eye-to-hand模型的非平凡解约束,用于剔除标定运动中的无效位姿,以规划并联机器人末端夹持机构的手眼标定运动,实现高精度高效率的四自由度4-R(2-SS)并联机器人在线手眼标定;最后,采用基于手眼标定获取的机器人运动误差构建具有误差补偿的并联机器人抓取模型,同时,基于立体视觉模型计算相机坐标系下对象的最优抓取位姿,基于并联机器人运动学方程计算末端夹持机构在并联机器人基础坐标系下的当前位姿,并结合抓取模型和在线手眼标定得到的相机基础坐标系在并联机器人基础坐标系下的位姿,计算末端夹持机构当前位姿与最优抓取位姿之间的转换矩阵,实现基于立体视觉和4-R(2-SS)并联机器人的末端夹持机构的抓取位姿计算。本发明所构建的具有运动误差补偿的立体视觉改进Eye-to-hand模型、基于垂直分量修正的Eye-to-hand模型求解方法、基于Eye-to-hand模型非平凡解约束的机器人标定运动规划方法可有效提高四自由度4-R(2-SS)并联机器人立体视觉手眼系统的在线手眼标定精度和效率,有利于进一步实现并联机器人的准确快速抓取。
具体实施方式以本课题组研制的一种新型4-R(2-SS)并联机器人水果分拣系统为例、白罗莎葡萄串为抓取对象进行描述。
参见图1,其具体步骤如下:
1、具有运动误差补偿的立体视觉Eye-to-hand模型改进:构建相机固定在机器人本体外的Eye-to-hand手眼基础模型,以及手眼系统中基于非线性畸变的立体视觉模型。同时,根据相机间位姿关系构建立体视觉中各相机与机器人的手眼模型组,以改进单相机的Eye-to-hand基础模型;并对改进后的Eye-to-hand模型进行机器人运动误差补偿,以减少相机标定误差和机器人运动误差对4-R(2-SS)并联机器人Eye-to-hand模型精度的影响。具体步骤如下:
(1)立体视觉模型的构建:
本发明的四自由度4-R(2-SS)并联机器人主要用于串类水果的自动分拣,为获取串类水果位姿的垂直分量,本发明采用Kinect立体相机获取图像,其中Kinect相机由彩色相机和红外相机组成。首先,参见图2,采用小孔成像原理对彩色相机和红外相机分别建立成像模型。考虑到图像像素坐标系O0-UV的U轴和V轴不垂直的情况,相机的成像模型可表示为:
其中K为相机成像模型的内参矩阵;fu=1/dx,fv=1/dy,dx和dy分别为单个像素的物理尺寸;f为焦距;θ为U轴和V轴的夹角;(u0,v0)为图像坐标系的原点在O0-UV下的坐标;(u,v)为投影点在O0-UV的坐标;(xc,yc,zc)为空间点在相机坐标系下的坐标。
之后,对相机成像模型引入多种非线性的畸变参数,对相机模型进行修正。本发明构建的畸变模型包括主要由视觉系统中透镜的曲面误差导致的径向畸变、主要由透镜光轴与相机光轴不共轴导致的离心畸变、以及由薄棱镜不理想引起的薄棱镜畸变。如式(2)所示,忽略畸变模型的高阶分量,将多种非线性畸变进行综合,取畸变的主要参数建立非线性总畸变的数学模型Dx和Dy。
式(3)即为相机畸变模型,其中向量Dk=[k1 k2 p1 p2 s1 s2]T即为畸变参数向量。
最后,基于双目视觉原理对Kinect相机中的彩色相机和红外相机进行建模,构建立体视觉模型。两个相机在不同时刻同时采集同一标定板图像,基于单目标定获取相机坐标系与标定板坐标系的钢体变换关系,如式(4)所示,其中Rc和Rd分别为彩色相机、红外相机与标定板坐标系的旋转位姿关系;tc和td分别为彩色相机、红外相机与标定板坐标系的平移位姿关系;Mc,Md和Mb分别为空间点在彩色相机坐标系、红外相机坐标系和标定板坐标系下的坐标。
对式(4)进行变换,获取双目视觉中彩色相机和红外相机间的刚体齐次变换矩阵dHc,建立如下立体视觉模型:
其中R和T分别为两相机的旋转和平移关系,具体如式(6)所示。Mc和Md分别表示空间点在彩色相机和红外相机坐标系下的齐次坐标。
(2)Eye-to-hand基础模型的构建:
参见图3,本发明的用于串类水果分拣的四自由度4-R(2-SS)并联机器人主体包括由4个运动学结构相同的R(2-SS)支链构成的并联机构1和末端夹持机构3,可实现空间三维平动和绕Z轴的一维转动。根据串类水果分拣需求,采用Eye-to-Hand系统,将立体相机4安装在机器人外部,不随末端夹持机构3运动,机器人末端夹持机构3在相机视场下运动。为获取串类水果5位姿的垂直分量,采用Kinect立体相机4获取图像。同时,为实现实时在线标定,将采用的圆点标定板2固定在末端夹持机构3上方,并与末端夹持机构3顶部平面保持平行。
在相机坐标系中Od-XdYdZd为Kinect相机中的红外相机坐标系,同时也作为Kinect相机的基础坐标系;Oc-XcYcZc为Kinect相机中的彩色相机坐标系。Ow-XwYwZw为四自由度4-R(2-SS)并联机器人的基础坐标系,同时也作为世界坐标系。Ob-XbYbZb为圆点标定板2坐标系。Og-XgYgZg为并联机器人的末端夹持机构坐标系。Op-XpYpZp为抓取对象的坐标系。末端夹持机构3左右两部分为完全对称的机械结构,Og-XgYgZg的原点位于末端夹持机构3中心,通过精确安装圆点标定板2,使得Ob-XbYbZb和Og-XgYgZg的Z轴共线,XY平面均与水平面平行。设dHb为标定板坐标系在相机基础坐标系下的位姿;dHw为并联机器人基础坐标系在相机基础坐标系下的位姿;wHg为末端夹持机构坐标系在并联机器人基础坐标系下的位姿;gHb为标定板坐标系在末端夹持机构坐标系下的位姿。基于坐标变换链的封闭性得到位姿关系:
dHb=dHw·wHg·gHb (7)
在标定过程中,末端夹持机构3带动标定板2在相机4视场下运动,相机4拍摄不同时刻的标定板图像,进行在线手眼标定。其中,因相机4固定安装,标定板2固定在末端夹持机构3上,因此位姿关系dHw和gHb在运动过程中保持不变。在每个时刻,dHb和wHg均变化。手眼标定的过程即求取dHw的过程。因此,基于末端夹持机构3运动的n个时刻的转换关系可得:
基于式(8),消去固定位姿关系gHb,可得一般转换关系:
设X=dHw,dHb(i+1)和dHbi分别表示在第i+1次和第i次机器人标定运动下标定板坐标系在红外相机坐标系下的位姿,wHg(i+1)和wHgi分别表示在第i+1次和第i次机器人标定运动下末端夹持机构坐标系在并联机器人基础坐标系下的位姿。因此求取dHw的问题可以转换为求解方程AX=XB。设末端夹持机构3在并联机器人基础坐标系下的位姿变化前后的转换关系为Rgg,则有以下关系:
因此,B为末端夹持机构3的位姿变化关系。同理,A为标定板2的位姿变化关系。AX=XB即为本发明的四自由度4-R(2-SS)并联机器人Eye-to-Hand基础模型。
(3)立体视觉Eye-to-hand模型组的构建与改进
本发明基于双目标定结果,结合彩色相机和红外相机的相对位姿对Eye-to-hand基础模型AX=XB进行改进。首先对彩色相机和红外相机分别进行建模,可以得到:
其中,A为标定板的位姿变化关系,B为末端夹持机构的位姿变化关系,X为并联机器人基础坐标系与相机坐标系的位姿关系,分别为标定板2在红外相机坐标系和彩色相机坐标系下的位姿变化关系,cHb(i+1)和cHbi分别表示在第i+1次和第i次机器人标定运动下标定板坐标系在彩色相机坐标系下的位姿,Xd=dHw,Xc=cHw,dHw和cHw分别为并联机器人基础坐标系在红外相机坐标系和彩色相机坐标系下的位姿。基于立体视觉模型(5)对式(11)进行变换,得到改进后的立体视觉Eye-to-hand模型组:
其中,cHd为彩色相机和红外相机间的刚体齐次变换矩阵。
(4)具有运动误差补偿的Eye-to-hand模型改进
手眼标定误差主要来源于机器人运动误差引起的末端夹持机构坐标系在并联机器人基础坐标系下的位姿误差以及标定板坐标系在相机坐标系下的位姿误差。运动误差可以视为由机器人各坐标系微分变换所造成,可基于机器人微分运动模型进行推导。机器人的微分运动可认为由微分转动R(δx,δy,δz)和微分移动T(dx,dy,dz)组成,其中δx,δy,δz分别表示绕x,y,z轴的旋转角度,dx,dy,dz分别表示沿x,y,z轴方向的平移。将微分运动dH考虑到末端夹持机构3的运动中,可以得到新的位姿矩阵H+dH,其中dH可基于式(13)进行计算。
虽然并联机器人Eye-to-hand系统中标定板2固定在末端夹持机构3上方,与末端夹持机构3的运动一致,但在不同的坐标系下,其位姿矩阵并不相同,其微分运动矩阵也不一致。因此,设标定板2在相机坐标系下的微分运动为ΔA,末端夹持机构3在并联机器人基础坐标系下的微分运动为ΔB,基于微分运动对基于立体视觉改进后的Eye-to-hand模型进行并联机器人运动误差补偿,可得具有机器人运动误差补偿的Eye-to-hand模型:
2、基于垂直分量修正的Eye-to-hand模型求解:根据各相机获取的机器人多次运动的标定数据,基于并联机器人中标定板和末端夹持机构的垂直约束对手眼标定位姿参数中的垂直分量进行修正,实现对具有旋转运动约束的四自由度4-R(2-SS)并联机器人的手眼标定中所有位姿和运动误差的准确求取。具体步骤如下:
(1)基于矩阵向量化和直积的Eye-to-hand模型求解
为降低gHb的测量误差对手眼标定模型求解精度的影响,通过消去gHb构建手眼标定模型AX=XB。设有矩阵E=[Eij]m×n和F=[Fij]p×q,则矩阵E和F的直积如式(15)所示,将矩阵E中的每一个元素与矩阵F相乘,得到的直积矩阵大小为mp×nq。
定义矩阵向量化算子vec,如式(16)所示,以矩阵E为例对其进行矩阵向量化,得到大小为mn的,包含所有矩阵E元素的列向量vec(E)mn:
vec(E)=(E11,…,E1n,E21,…,Emn)T (16)
设有三个矩阵E=[Eij]m×m,F=[Fij]m×n和G=[Gij]n×n,则基于矩阵直积和向量化,三个矩阵的乘积满足下式:
在Eye-to-hand模型AX=XB中,矩阵A、B和X均为坐标系间刚性变换的矩阵,可以将矩阵解耦为旋转矩阵和平移向量的表示,如下:
其中,旋转矩阵R、RA和RB为3×3的矩阵,平移向量tA、tB和t为3×1的向量,以X为例,其旋转矩阵和平移向量分别为:
因此,手眼模型方程AX=XB可以表示为:
用方程组的形式表示为:
对式(23)的旋转和平移部分分别进行变换,由于旋转矩阵R、RA和RB均为单位正交阵,因此对公式左右均进行向量化,并基于式(17)进行变换,可以得到:
基于手眼标定中并联机器人末端夹持机构3的两次运动,可以得到:
其中,I为单位矩阵。将式(26)转换为Q·vec(R)=0方程,基于SVD分解对方程求解,并将结果带入式(27),基于最小二乘法求解即可得到旋转矩阵R和平移向量t,最终基于式(20),求解出手眼模型中的转换矩阵X。
(2)基于标定板和末端夹持机构的垂直约束修正并联机器人手眼标定的垂直分量
针对四自由度4-R(2-SS)并联机器人的Eye-to-hand模型,若采用根据解耦方程(23)的基于矩阵向量化和直积的模型求解法,由于四自由度4-R(2-SS)并联机器人的运动约束,无法得到X矩阵中的Z方向的平移量tz。因此,考虑到四自由度4-R(2-SS)并联机器人的结构稳定性,采用垂直分量修正的方法对基于矩阵向量化和直积的模型求解法进行改进,实现四自由度4-R(2-SS)并联机器人Eye-to-hand模型的准确求解。
参见图4,由于并联机器人结构的稳定性以及运动约束,可以使得末端夹持机构坐标系的Z轴与并联机器人基础坐标系的Z轴保持平行。同时由于标定板2安装固定在末端夹持机构3上方,可以使得末端夹持机构坐标系的Z轴与标定板坐标系的Z轴保持重合。手眼标定所需求取的相机坐标系与并联机器人基础坐标系之间的转换关系中的平移分量tz可以通过其他坐标系间的转换关系求得。设基于矩阵向量化和直积的模型求解法求得的并联机器人基础坐标系在相机基础坐标系下的位姿转换矩阵dHw为:
其中,R和t分别表示由相机基础坐标系转换到并联机器人基础坐标系的旋转矩阵和平移向量。Rij,i=1,2,3;j=1,2,3分别表示矩阵R中的元素,(tx,ty,tz)分别表示向量t中的元素。根据齐次转换矩阵dHw中旋转矩阵的正交性,可以得到dHw的逆矩阵wHd为:
相机基础坐标系原点在并联机器人基础坐标系下的Z坐标wzd可根据如下关系计算:
wzd=wzg-bzd-bzg (30)
其中,末端夹持机构3在并联机器人基础坐标系下的垂直平移分量wzg可根据并联机器人运动学正解求得的转换矩阵wHg获得。末端夹持机构3在标定板坐标系下的垂直平移分量bzg可直接测量获得。相机基础坐标系在标定板坐标系下的垂直平移分量bzd可根据视觉标定得到的dHb获得,具体如下:
bzd=-dRb13 dtbx-dRb23 dtby-dRb33 dtbz (32)
其中,dRb和dtb分别表示由相机基础坐标系转换到标定板坐标系的旋转矩阵和平移向量。dRbij,i=1,2,3;j=1,2,3分别表示矩阵dRb中的元素,(dtbx,dtby,dtbz)分别表示向量dtb中的元素。之后基于式(29)和(30)即可求得手眼标定中的Z方向的平移量tz:
至此,通过求得的Z方向的平移量tz对基于矩阵向量化和直积的模型求解法求得的转换矩阵X的垂直平移分量进行修正,实现了四自由度4-R(2-SS)并联机器人手眼标定模型的准确求解。
3、基于Eye-to-hand模型非平凡解约束的4-R(2-SS)并联机器人标定运动规划:基于末端夹持机构3标定运动间的位姿关系,构建Eye-to-hand模型的非平凡解约束,用于剔除标定运动中的无效位姿,以规划并联机器人末端夹持机构3的手眼标定运动,实现高精度高效率的四自由度4-R(2-SS)并联机器人在线手眼标定。具体如下:
对于Eye-to-hand的基础模型AX=XB,为使模型方程具有非平凡解,本发明设定以下三个约束条件:
(1)末端夹持机构3的两次运动之间的位姿转换矩阵满足B≠I,其中I为单位矩阵;
(2)标定板2或末端夹持机构3的两次运动之间旋转角满足θA≠2kπ;
(3)标定板2两次运动之间的位姿转换矩阵A和末端夹持机构3的两次运动之间的位姿转换矩阵B具有公共特征根。
对于约束条件(1),若B=I,则末端夹持机构3的两次运动间没有变化,因此无法对模型进行求解。对于约束(2),若θA=2kπ,则无法求得位姿矩阵X中的平移分量。对于约束(3),AX=XB有非平凡解的充要条件是A,B有公共特征根,证明如下:
设J和L分别为A和B的约旦标准形,且A=TJT-1,B=HLH-1,则有其中Z=T-1XH。设J和L的特征根分别为λi(i=1,2,…,k)和ηj(j=1,2,…,k),由约旦标准形的结构可得,由JZ=ZL所得的K2阶方阵的主对角线各元素为λi-ηj(i=1,2,…,k;j=1,2,…,k)。
必要性:λi≠ηj(i=1,2,…,k;j=1,2,…,k),则对于方程JZ=ZL转换后的齐次方程QY=0中,Q经过行初等变换后得到K2阶单位矩阵,即IY=0。因此IY=0只有平凡解,即JZ=ZL只有平凡解,从而AX=XB只有平凡解。
充分性:若JZ=ZL只有非平凡解,则Q为满秩矩阵,因此Q的对角线各元素λi-ηj≠0(i=1,2,…,k;j=1,2,…,k),即λi≠ηj。证毕
参见图5,首先对并联机器人末端夹持机构3的平移运动路径进行规划。将圆柱形的末端夹持机构分拣运动空间均分为三部分,在每部分以相同的间隔取不同的位置作为理想手眼标定位置。对理想标定位置从下往上进行标号i=1,2,3,…,15,分别对应C1,…C5,B1,…,B5,A1,…,A5。参见图6,末端夹持机构3在不同的理想标定位置附近做随机的小幅度平移运动和大幅度旋转运动,并根据模型非平凡解约束对末端夹持机构3的随机运动进行筛选。将满足模型非平凡解约束的末端夹持机构3的运动姿态用于构建模型方程组,最终实现准确快速的四自由度4-R(2-SS)并联机器人手眼标定。
4、基于立体视觉和4-R(2-SS)并联机器人的抓取位姿计算:采用基于手眼标定获取的机器人运动误差构建具有误差补偿的并联机器人抓取模型,并结合基于立体视觉模型获取的相机坐标系下对象的最优抓取位姿、基于并联机器人运动学方程求取的末端夹持机构3在并联机器人基础坐标系下的当前位姿、以及基于在线手眼标定得到的相机基础坐标系在并联机器人基础坐标系下的位姿计算末端夹持机构3当前位姿与最优抓取位姿之间的转换矩阵,实现基于立体视觉和4-R(2-SS)并联机器人的末端夹持机构3的最优抓取位姿计算。具体步骤如下:
(1)基于立体视觉和4-R(2-SS)并联机器人的抓取模型的构建与改进
在基于并联机器人和立体视觉的串类水果分拣系统中,为实现串类水果对象5的准确稳定抓取,并联机器人的末端夹持机构3需要运动到对象5的位置,以最优抓取位姿对对象5进行抓取。设对象5的最优抓取位姿为Hp,最优抓取位姿在并联机器人的末端夹持机构坐标系下的表示为gHp,为使末端夹持机构3能准确地从当前位姿Hg转换为最优抓取位姿Hp,需要将Hp转换到并联机器人基础坐标系下进行表示wHp。基于Eye-to-Hand模型和视觉模型构建的并联机器人抓取模型如式(34)所示。
其中,末端夹持机构3当前位姿在并联机器人基础坐标系下的表示wHg可由并联机器人的运动学正解获得。相机基础坐标系在并联机器人基础坐标系下的位姿wHd可由Eye-to-Hand模型解算获得。由于对象5的最优抓取位姿来源于相机4获取的图像,因此最优抓取位姿在相机基础坐标系下的位姿dHp可由结合畸变参数的单目相机模型和立体视觉模型获得。
采用如式(14)所示的基于并联机器人运动误差的Eye-to-hand模型得到的手眼标定结果对并联机器人的抓取模型(34)进行改进,得到:
gHp=(wHg+dB)-1·wHd·dHp (35)
(2)基于立体视觉和4-R(2-SS)并联机器人的抓取位姿计算
基于并联机器人运动学方程求取末端夹持机构3在并联机器人基础坐标系下的当前位姿wHg。本发明的四自由度4-R(2-SS)并联机器人运动学方程如下:
其中:i=1,2,3,4。参见图7,(x,y,z)表示P2点在并联机器人基础坐标系下的三维位置;ei=e(cosγi,sinγi,0)T的模表示动平台与静平台的内切圆半径差,即图中O到Ai的向量;γi=(i-1)π/2表示静平台结构角;l1和l2分别表示支链i的主动臂和从动臂的杆长;θi表示主动臂i的转角;s为辅平台相对于动平台沿Z轴方向的位移,s=p(θ/2π),p为丝杠螺距,θ为丝杠转角;且:
基于运动学方程得到的末端夹持机构3的运动位姿(xg,yg,zg,θg)为:
其中c为从动臂末端点P1与动平台末端点P之间的距离,g为P与末端夹持机构3坐标系原点的距离。
基于式(19)、(21)和(39)的变换关系,可将得到的位姿参数(xg,yg,zg,θg)转换为位姿矩阵wHg。
其中:versθ=(1-cosθ)。
之后,对Eye-to-Hand手眼标定得到的X求逆,得到相机基础坐标系在并联机器人基础坐标系下的位姿wHd。根据相机内参(1)、畸变总模型(3)和立体视觉模型(5),采用张正友相机标定法对视觉模型进行标定,计算最优抓取位姿在相机坐标系下的位姿dHp。最后,即可根据如式(35)所示的抓取模型计算末端夹持机构3当前位姿与最优抓取位姿之间的关系gHp,实现串类水果对象的准确稳定抓取。
至此,四自由度4-R(2-SS)并联机器人的在线手眼标定和抓取位姿计算已完成。
实施例
具体实施例以本课题组研制的一种新型4-R(2-SS)并联机器人水果分拣系统为例、白罗莎葡萄串为抓取对象进行描述。其具体实施方式如下:
1、具有运动误差补偿的立体视觉Eye-to-hand模型改进。具体步骤如下:
(1)立体视觉Eye-to-hand模型组的构建与改进。本发明基于双目标定结果,结合彩色相机和红外相机的相对位姿对Eye-to-hand基础模型AX=XB进行改进。首先对彩色相机和红外相机分别进行建模,可以得到:
基于立体视觉模型对式(1)进行变换,得到改进后的立体视觉Eye-to-hand模型组:
(2)具有运动误差补偿的Eye-to-hand模型改进。将微分运动dH考虑到末端夹持机构的运动中,可以得到新的位姿矩阵H+dH,其中dH可基于式(3)进行计算。
基于微分运动对基于立体视觉改进后的Eye-to-hand模型进行并联机器人运动误差补偿,可得具有机器人运动误差补偿的Eye-to-hand模型:
2、基于垂直分量修正的Eye-to-hand模型求解。具体步骤如下:
(1)基于矩阵向量化和直积的Eye-to-hand模型求解。为降低gHb的测量误差对手眼标定模型求解精度的影响,通过消去gHb构建手眼标定模型AX=XB。将矩阵A、B和X解耦为旋转矩阵和平移向量,将手眼模型方程AX=XB表示为:
用方程组的形式表示为:
对式(6)的旋转和平移部分分别进行变换,由于旋转矩阵R、RA和RB均为单位正交阵,因此对公式左右均进行向量化,可以得到:
基于手眼标定中并联机器人末端夹持机构3的两次运动,可以得到:
将式(9)转换为Q·vec(R)=0方程,基于SVD分解对方程求解,并将结果带入式(10),基于最小二乘法求解即可得到旋转矩阵R和平移向量t,最终求解出手眼模型中的转换矩阵X。
(2)基于标定板和末端夹持机构的垂直约束修正并联机器人手眼标定的垂直分量。设基于矩阵向量化和直积的模型求解法求得的并联机器人基础坐标系在相机基础坐标系下的位姿转换矩阵dHw为:
其中,R和t分别表示由相机基础坐标系转换到并联机器人基础坐标系的旋转矩阵和平移向量。Rij,i=1,2,3;j=1,2,3分别表示矩阵R中的元素,(tx,ty,tz)分别表示向量t中的元素。根据齐次转换矩阵dHw中旋转矩阵的正交性,可以得到dHw的逆矩阵wHd为:
相机基础坐标系原点在并联机器人基础坐标系下的Z坐标wzd可根据如下关系计算:
wzd=wzg-bzd-bzg (13)
其中,末端夹持机构在并联机器人基础坐标系下的垂直平移分量wzg可根据并联机器人运动学正解求得的转换矩阵wHg获得。末端夹持机构在标定板坐标系下的垂直平移分量bzg可直接测量获得。相机基础坐标系在标定板坐标系下的垂直平移分量bzd可根据视觉标定得到的dHb获得,具体如下:
bzd=-dRb13 dtbx-dRb23 dtby-dRb33 dtbz (15)
其中,dRb和dtb分别表示由相机基础坐标系转换到标定板坐标系的旋转矩阵和平移向量。dRbij,i=1,2,3;j=1,2,3分别表示矩阵dRb中的元素,(dtbx,dtby,dtbz)分别表示向量dtb中的元素。之后基于式(12)和(13)即可求得手眼标定中的Z方向的平移量tz:
至此,通过求得的Z方向的平移量tz对基于矩阵向量化和直积的模型求解法求得的转换矩阵X的垂直平移分量进行修正,实现了四自由度4-R(2-SS)并联机器人手眼标定模型的准确求解。
3、基于Eye-to-hand模型非平凡解约束的4-R(2-SS)并联机器人标定运动规划。具体如下:
对于Eye-to-hand的基础模型AX=XB,为使模型方程具有非平凡解,本发明设定以下三个约束条件:
(1)末端夹持机构的两次运动之间的位姿转换矩阵满足B≠I,其中I为单位矩阵;
(2)标定板或末端夹持机构的两次运动之间旋转角满足θA≠2kπ;
(3)标定板两次运动之间的位姿转换矩阵A和末端夹持机构的两次运动之间的位姿转换矩阵B具有公共特征根。
首先对并联机器人末端夹持机构的平移运动路径进行规划。将圆柱形的末端夹持机构分拣运动空间均分为三部分,在每部分以相同的间隔取不同的位置作为理想手眼标定位置。对理想标定位置从下往上进行标号i=1,2,3,…,15,分别对应C1,…C5,B1,…,B5,A1,…,A5。如图8所示,末端夹持机构在不同的理想标定位置附近做随机的小幅度平移运动和大幅度旋转运动,并根据模型非平凡解约束对末端夹持机构的随机运动进行筛选。如图9所示。在每个理想标定位置,基于Kinect立体相机中的彩色相机和红外相机分别同时采集标定板的图像。基于标定板图像,将满足模型非平凡解约束的末端夹持机构的运动姿态用于构建模型方程组,最终实现准确快速的四自由度4-R(2-SS)并联机器人手眼标定。
4、基于立体视觉和4-R(2-SS)并联机器人的抓取位姿计算。具体步骤如下:
(1)基于立体视觉和4-R(2-SS)并联机器人的抓取模型的构建与改进。设对象的最优抓取位姿为Hp,最优抓取位姿在并联机器人的末端夹持机构坐标系下的表示为gHp,为使末端夹持机构能准确地从当前位姿Hg转换为最优抓取位姿Hp,需要将Hp转换到并联机器人基础坐标系下进行表示wHp。基于Eye-to-Hand模型和视觉模型构建的并联机器人抓取模型如式(17)所示。
其中,末端夹持机构当前位姿在并联机器人基础坐标系下的表示wHg可由并联机器人的运动学正解获得。相机基础坐标系在并联机器人基础坐标系下的位姿wHd可由Eye-to-Hand模型解算获得。如图10所示,由于对象的最优抓取位姿来源于相机获取的图像,因此最优抓取位姿在相机基础坐标系下的位姿dHp可由结合畸变参数的单目相机模型和立体视觉模型获得。采用基于并联机器人运动误差的Eye-to-hand模型得到的手眼标定结果对并联机器人的抓取模型(17)进行改进,得到:
gHp=(wHg+dB)-1·wHd·dHp (18)
(2)基于立体视觉和4-R(2-SS)并联机器人的抓取位姿计算。基于并联机器人运动学方程求取末端夹持机构在并联机器人基础坐标系下的当前位姿wHg。本发明的四自由度4-R(2-SS)并联机器人运动学方程如下:
其中:i=1,2,3,4。(x,y,z)表示P2点在并联机器人基础坐标系下的三维位置;ei=e(cosγi,sinγi,0)T的模表示动平台与静平台的内切圆半径差;γi=(i-1)π/2表示静平台结构角;l1和l2分别表示支链i的主动臂和从动臂的杆长;θi表示主动臂i的转角;s为辅平台相对于动平台沿Z轴方向的位移,s=p(θ/2π),p为丝杠螺距,θ为丝杠转角;且:
基于运动学方程得到的末端夹持机构的运动位姿(xg,yg,zg,θg)为:
其中c为从动臂末端点P1与动平台末端点P之间的距离,g为P与末端夹持机构坐标系原点的距离。
基于式(22)的变换关系,可将得到的位姿参数(xg,yg,zg,θg)转换为位姿矩阵wHg。
其中:versθ=(1-cosθ)。之后,对Eye-to-Hand手眼标定得到的X求逆,得到相机基础坐标系在并联机器人基础坐标系下的位姿wHd。根据相机内参、畸变总模型和立体视觉模型,采用张正友相机标定法对视觉模型进行标定,计算最优抓取位姿在相机坐标系下的位姿dHp。最后,即可根据如式(18)所示的抓取模型计算末端夹持机构当前位姿与最优抓取位姿之间的关系gHp,实现串类水果对象的准确稳定抓取。
综上,本发明的一种用于四自由度4-R(2-SS)并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法。首先,构建相机固定在机器人本体外的Eye-to-hand手眼基础模型,以及手眼系统中基于非线性畸变的立体视觉模型,同时,根据相机间位姿关系构建立体视觉中各相机与机器人的手眼模型组,以改进单相机的Eye-to-hand基础模型;并对改进后的Eye-to-hand模型进行机器人运动误差补偿,以减少相机标定误差和机器人运动误差对4-R(2-SS)并联机器人Eye-to-hand模型精度的影响;其次,根据各相机获取的机器人多次运动的标定数据,基于并联机器人中标定板和末端夹持机构的垂直约束对手眼标定位姿参数中的垂直分量进行修正,实现对具有旋转运动约束的四自由度4-R(2-SS)并联机器人的手眼标定中所有位姿和运动误差的准确求取;接着,基于末端夹持机构标定运动间的位姿关系,构建Eye-to-hand模型的非平凡解约束,用于剔除标定运动中的无效位姿,以规划并联机器人末端夹持机构的手眼标定运动,实现高精度高效率的四自由度4-R(2-SS)并联机器人在线手眼标定;最后,采用基于手眼标定获取的机器人运动误差构建具有误差补偿的并联机器人抓取模型,同时,基于立体视觉模型计算相机坐标系下对象的最优抓取位姿,基于并联机器人运动学方程计算末端夹持机构在并联机器人基础坐标系下的当前位姿,并结合抓取模型和在线手眼标定得到的相机基础坐标系在并联机器人基础坐标系下的位姿,计算末端夹持机构当前位姿与最优抓取位姿之间的转换矩阵,实现基于立体视觉和4-R(2-SS)并联机器人的末端夹持机构的抓取位姿计算。
应理解上述实施例仅用于说明本发明而不用于限制本发明的范围,在阅读了本发明之后,本领域技术人员对本发明的各种等价形式的修改均落于本申请所附权利要求所限定的范围。
Claims (5)
1.一种用于四自由度4-R(2-SS)并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法,其特征在于,包括如下步骤:
(1)具有运动误差补偿的立体视觉Eye-to-hand模型改进:构建相机固定在机器人本体外的Eye-to-hand手眼基础模型,以及手眼系统中基于非线性畸变的立体视觉模型,同时,根据相机间位姿关系构建立体视觉中各相机与机器人的手眼模型组,以改进单相机的Eye-to-hand基础模型;并对改进后的Eye-to-hand模型进行机器人运动误差补偿;
(2)基于垂直分量修正的Eye-to-hand模型求解:根据各相机获取的机器人多次运动的标定数据,基于并联机器人中标定板和末端夹持机构的垂直约束对手眼标定位姿参数中的垂直分量进行修正,实现对具有旋转运动约束的四自由度4-R(2-SS)并联机器人的手眼标定中所有位姿和运动误差的准确求取;
(3)基于Eye-to-hand模型非平凡解约束的4-R(2-SS)并联机器人标定运动规划:基于末端夹持机构标定运动间的位姿关系,构建Eye-to-hand模型的非平凡解约束,用于剔除标定运动中的无效位姿,以规划并联机器人末端夹持机构的手眼标定运动,实现高精度高效率的四自由度4-R(2-SS)并联机器人在线手眼标定;
(4)基于立体视觉和4-R(2-SS)并联机器人的抓取位姿计算:采用基于手眼标定获取的机器人运动误差构建具有误差补偿的并联机器人抓取模型,同时,基于立体视觉模型计算相机坐标系下对象的最优抓取位姿,基于并联机器人运动学方程计算末端夹持机构在并联机器人基础坐标系下的当前位姿,并结合抓取模型和在线手眼标定得到的相机基础坐标系在并联机器人基础坐标系下的位姿,计算末端夹持机构当前位姿与最优抓取位姿之间的转换矩阵,实现基于立体视觉和4-R(2-SS)并联机器人的末端夹持机构的抓取位姿计算。
2.根据权利要求1所述的一种用于四自由度4-R(2-SS)并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法,其特征在于:步骤(1)中,所述的具有运动误差补偿的立体视觉Eye-to-hand模型改进,具体如下:
2.1,立体视觉Eye-to-hand模型组的构建与改进
基于双目标定结果,结合彩色相机和红外相机的相对位姿对Eye-to-hand基础模型AX=XB进行改进,首先对彩色相机和红外相机分别进行建模,可以得到:
其中,A为标定板的位姿变化关系,B为末端夹持机构的位姿变化关系,X为并联机器人基础坐标系与相机坐标系的位姿关系,分别表示标定板在红外相机坐标系和彩色相机坐标系下的位姿变化关系,Xd=dHw,Xc=cHw,dHw和cHw分别表示并联机器人基础坐标系在红外相机坐标系和彩色相机坐标系下的位姿,dHb(i+1)和dHbi分别表示在第i+1次和第i次机器人标定运动下标定板坐标系在红外相机坐标系下的位姿,cHb(i+1)和cHbi分别表示在第i+1次和第i次机器人标定运动下标定板坐标系在彩色相机坐标系下的位姿,基于立体视觉模型对式(1)进行变换,得到改进后的立体视觉Eye-to-hand模型组:
其中,cHd为彩色相机和红外相机间的刚体齐次变换矩阵;
2.2,具有运动误差补偿的Eye-to-hand模型改进
手眼标定误差主要来源于机器人运动误差引起的末端夹持机构坐标系在并联机器人基础坐标系下的位姿误差以及标定板坐标系在相机坐标系下的位姿误差,运动误差可以视为由机器人各坐标系微分变换所造成,可基于机器人微分运动模型进行推导,机器人的微分运动可认为由微分转动R(δx,δy,δz)和微分移动T(dx,dy,dz)组成,其中δx,δy,δz分别表示绕x,y,z轴的旋转角度,dx,dy,dz分别表示沿x,y,z轴方向的平移,将微分运动dH考虑到末端夹持机构的运动中,可以得到新的位姿矩阵H+dH,其中dH可基于式(3)进行计算;
设标定板在相机坐标系下的微分运动为ΔA,末端夹持机构在并联机器人基础坐标系下的微分运动为ΔB,基于微分运动对基于立体视觉改进后的Eye-to-hand模型进行并联机器人运动误差补偿,可得具有机器人运动误差补偿的Eye-to-hand模型:
3.根据权利要求1所述的一种用于四自由度4-R(2-SS)并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法,其特征在于:步骤(2)中,所述的基于垂直分量修正的Eye-to-hand模型求解,具体如下:
针对四自由度4-R(2-SS)并联机器人的Eye-to-hand模型,考虑到四自由度4-R(2-SS)并联机器人的结构稳定性,采用垂直分量修正的方法对基于矩阵向量化和直积的模型求解法进行改进,实现四自由度4-R(2-SS)并联机器人Eye-to-hand模型的准确求解;
由于并联机器人结构的稳定性以及运动约束,可以使得末端夹持机构坐标系的Z轴与并联机器人基础坐标系的Z轴保持平行,同时由于标定板安装固定在末端夹持机构上方,可以使得末端夹持机构坐标系的Z轴与标定板坐标系的Z轴保持重合,手眼标定所需求取的相机坐标系与并联机器人基础坐标系之间的转换关系中的平移分量tz可以通过其他坐标系间的转换关系求得,设基于矩阵向量化和直积的模型求解法求得的并联机器人基础坐标系在相机基础坐标系下的位姿转换矩阵dHw为:
其中,R和t分别表示由相机基础坐标系转换到并联机器人基础坐标系的旋转矩阵和平移向量,Rij,i=1,2,3;j=1,2,3分别表示矩阵R中的元素,(tx,ty,tz)分别表示向量t中的元素,根据齐次转换矩阵dHw中旋转矩阵的正交性,可以得到dHw的逆矩阵wHd为:
相机基础坐标系原点在并联机器人基础坐标系下的Z坐标wzd可根据如下关系计算:
wzd=wzg-bzd-bzg (7)
其中,末端夹持机构在并联机器人基础坐标系下的垂直平移分量wzg可根据并联机器人运动学正解求得的转换矩阵wHg获得,末端夹持机构在标定板坐标系下的垂直平移分量bzg可直接测量获得,相机基础坐标系在标定板坐标系下的垂直平移分量bzd可根据视觉标定得到的dHb获得,具体如下:
其中,dRb和dtb分别表示由相机基础坐标系转换到标定板坐标系的旋转矩阵和平移向量,dRbij,i=1,2,3;j=1,2,3分别表示矩阵dRb中的元素,(dtbx,dtby,dtbz)分别表示向量dtb中的元素,之后即可求得手眼标定中的Z方向的平移量tz:
至此,通过求得的Z方向的平移量tz对基于矩阵向量化和直积的模型求解法求得的转换矩阵X的垂直平移分量进行修正,实现了四自由度4-R(2-SS)并联机器人手眼标定模型的准确求解。
4.根据权利要求1所述的一种用于四自由度4-R(2-SS)并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法,其特征在于:步骤(3)中,所述的基于Eye-to-hand模型非平凡解约束的4-R(2-SS)并联机器人标定运动规划,具体如下:
对于Eye-to-hand的基础模型AX=XB,为使模型方程具有非平凡解,设定以下三个约束条件:
(1)末端夹持机构的两次运动之间的位姿转换矩阵满足B≠I,其中I为单位矩阵;
(2)标定板或末端夹持机构的两次运动之间旋转角满足θA≠2kπ;
(3)标定板两次运动之间的位姿转换矩阵A和末端夹持机构的两次运动之间的位姿转换矩阵B具有公共特征根;
对于约束条件(1),若B=I,则末端夹持机构的两次运动间没有变化,因此无法对模型进行求解,对于约束(2),若θA=2kπ,则无法求得位姿矩阵X中的平移分量,对于约束(3),AX=XB有非平凡解的充要条件是A,B有公共特征根;
首先对并联机器人末端夹持机构的平移运动路径进行规划,将圆柱形的末端夹持机构分拣运动空间均分为三部分,在每部分以相同的间隔取不同的位置作为理想手眼标定位置;对理想标定位置从下往上进行标号,末端夹持机构在不同的理想标定位置附近做随机的小幅度平移运动和大幅度旋转运动,并根据模型非平凡解约束对末端夹持机构的随机运动进行筛选,将满足模型非平凡解约束的末端夹持机构的运动姿态用于构建模型原点的距离方程组,最终实现准确快速的四自由度4-R(2-SS)并联机器人手眼标定。
5.根据权利要求1所述的一种用于四自由度4-R(2-SS)并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法,其特征在于:步骤(4)中,所述的基于立体视觉和4-R(2-SS)并联机器人的抓取位姿计算,具体如下:
4.1,基于立体视觉和4-R(2-SS)并联机器人的抓取模型的构建与改进
在基于并联机器人和立体视觉的串类水果分拣系统中,为实现串类水果对象的准确稳定抓取,并联机器人的末端夹持机构需要运动到对象的位置,以最优抓取位姿对对象进行抓取,设对象的最优抓取位姿为Hp,最优抓取位姿在并联机器人的末端夹持机构坐标系下的表示为gHp,为使末端夹持机构能准确地从当前位姿Hg转换为最优抓取位姿Hp,需要将Hp转换到并联机器人基础坐标系下进行表示wHp,基于Eye-to-Hand模型和视觉模型构建的并联机器人抓取模型如式(11)所示:
其中,末端夹持机构当前位姿在并联机器人基础坐标系下的表示wHg可由并联机器人的运动学正解获得,相机基础坐标系在并联机器人基础坐标系下的位姿wHd可由Eye-to-Hand模型解算获得,由于对象的最优抓取位姿来源于相机获取的图像,因此最优抓取位姿在相机基础坐标系下的位姿dHp可由结合畸变参数的单目相机模型和立体视觉模型获得;
采用如式(4)所示的基于并联机器人运动误差的Eye-to-hand模型得到的手眼标定结果对并联机器人的抓取模型(11)进行改进,得到:
gHp=(wHg+dB)-1·wHd·dHp (12)
4.2,基于立体视觉和4-R(2-SS)并联机器人的抓取位姿计算
基于并联机器人运动学方程求取末端夹持机构在并联机器人基础坐标系下的当前位姿wHg,四自由度4-R(2-SS)并联机器人运动学方程如下:
其中:i=1,2,3,4,(x,y,z)表示P2点在并联机器人基础坐标系下的三维位置;ei=e(cosγi,sinγi,0)T的模表示动平台与静平台的内切圆半径差;γi=(i-1)π/2表示静平台结构角;l1和l2分别表示支链i的主动臂和从动臂的杆长;θi表示主动臂i的转角;s为辅平台相对于动平台沿Z轴方向的位移,s=p(θ/2π),p为丝杠螺距,θ为丝杠转角;且:
基于运动学方程得到的末端夹持机构的运动位姿(xg,yg,zg,θg)为:
其中c为从动臂末端点P1与动平台末端点P之间的距离,g为P与末端夹持机构坐标系原点的距离;因此,可将得到的位姿参数(xg,yg,zg,θg)转换为位姿矩阵wHg;
之后,对Eye-to-Hand手眼标定得到的X求逆,得到相机基础坐标系在并联机器人基础坐标系下的位姿wHd,根据相机内参、畸变总模型和立体视觉模型,采用张正友相机标定法对视觉模型进行标定,计算最优抓取位姿在相机坐标系下的位姿dHp,最后,即可根据如式(12)所示的抓取模型计算末端夹持机构当前位姿与最优抓取位姿之间的关系gHp,实现串类水果对象的准确稳定抓取。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910446270.8A CN110276806B (zh) | 2019-05-27 | 2019-05-27 | 用于四自由度并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910446270.8A CN110276806B (zh) | 2019-05-27 | 2019-05-27 | 用于四自由度并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110276806A CN110276806A (zh) | 2019-09-24 |
CN110276806B true CN110276806B (zh) | 2023-06-09 |
Family
ID=67959067
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910446270.8A Active CN110276806B (zh) | 2019-05-27 | 2019-05-27 | 用于四自由度并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110276806B (zh) |
Families Citing this family (20)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110695996B (zh) * | 2019-10-14 | 2022-05-31 | 扬州大学 | 一种工业机器人自动手眼标定方法 |
CN111152223A (zh) * | 2020-01-09 | 2020-05-15 | 埃夫特智能装备股份有限公司 | 一种全自动机器人手眼标定方法 |
CN111383272B (zh) * | 2020-02-24 | 2023-07-21 | 江苏大学 | 一种双目视觉水果分拣并联机器人视觉盲区末端位姿检测方法 |
CN111710002B (zh) * | 2020-05-27 | 2022-09-16 | 华中科技大学 | 一种基于Optitrack系统的相机外参标定方法 |
CN111872922B (zh) * | 2020-07-29 | 2021-09-03 | 贵州电网有限责任公司 | 基于3d视觉传感器的三自由度并联机器人手眼标定方法 |
CN112022355B (zh) * | 2020-09-27 | 2022-06-10 | 平安科技(深圳)有限公司 | 基于计算机视觉的手眼标定方法及装置、存储介质 |
CN114310869B (zh) * | 2020-09-29 | 2023-04-25 | 上海科技大学 | 机器人手眼标定方法、系统以及终端 |
CN113146633B (zh) * | 2021-04-23 | 2023-12-19 | 无锡信捷电气股份有限公司 | 一种基于自动贴盒系统的高精度手眼标定方法 |
CN113160334B (zh) * | 2021-04-28 | 2023-04-25 | 北京邮电大学 | 一种基于手眼相机的双机器人系统标定方法 |
CN113618367B (zh) * | 2021-08-19 | 2022-05-03 | 哈尔滨工业大学(深圳) | 基于七自由度并联双模块机器人的多视觉空间装配系统 |
CN113733088B (zh) * | 2021-09-07 | 2024-05-14 | 河南大学 | 一种基于双目视觉的机械臂运动学自标定方法 |
CN115810052A (zh) * | 2021-09-16 | 2023-03-17 | 梅卡曼德(北京)机器人科技有限公司 | 相机的标定方法、装置、电子设备及存储介质 |
CN114083530B (zh) * | 2021-10-22 | 2023-03-31 | 中国科学院自动化研究所 | 工件坐标系标定系统及方法 |
CN114519738A (zh) * | 2022-01-24 | 2022-05-20 | 西北工业大学宁波研究院 | 一种基于icp算法的手眼标定误差修正方法 |
CN114505859B (zh) * | 2022-02-23 | 2023-12-01 | 四川锋准机器人科技有限公司 | 一种种植牙手术机器人末端柔顺控制方法 |
CN114939867A (zh) * | 2022-04-02 | 2022-08-26 | 杭州汇萃智能科技有限公司 | 一种基于立体视觉的机械手外接不规则非对称工具的标定方法及系统 |
CN114654467A (zh) * | 2022-04-08 | 2022-06-24 | 中国电子科技集团公司第十四研究所 | 一种基于视觉定位技术的大负载并联机构的运动控制方法 |
CN114952127B (zh) * | 2022-07-29 | 2022-10-21 | 常州市镭烁光电科技有限公司 | 一种用于焊接的3d视觉相机与机械臂的标定模型和方法 |
CN117262658B (zh) * | 2023-11-10 | 2024-12-03 | 深圳慧智星晨科技有限公司 | 一种存储机构取放通道高精度智能定位及引导的装置和方法 |
CN118544360A (zh) * | 2024-07-25 | 2024-08-27 | 佛山大学 | 基于激光补偿的机器人视觉检测方法、系统、终端及介质 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108098762A (zh) * | 2016-11-24 | 2018-06-01 | 广州映博智能科技有限公司 | 一种基于新型视觉引导的机器人定位装置及方法 |
CN107186708B (zh) * | 2017-04-25 | 2020-05-12 | 珠海智卓投资管理有限公司 | 基于深度学习图像分割技术的手眼伺服机器人抓取系统及方法 |
CN109102547A (zh) * | 2018-07-20 | 2018-12-28 | 上海节卡机器人科技有限公司 | 基于物体识别深度学习模型的机器人抓取位姿估计方法 |
-
2019
- 2019-05-27 CN CN201910446270.8A patent/CN110276806B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN110276806A (zh) | 2019-09-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110276806B (zh) | 用于四自由度并联机器人立体视觉手眼系统的在线手眼标定和抓取位姿计算方法 | |
CN109483516B (zh) | 一种基于空间距离和极线约束的机械臂手眼标定方法 | |
CN109859275B (zh) | 一种基于s-r-s结构的康复机械臂的单目视觉手眼标定方法 | |
WO2021217976A1 (zh) | 一种基于单目视觉定位的机械臂控制方法及装置 | |
CN109859277A (zh) | 一种基于Halcon的机器人视觉系统标定方法 | |
CN110136204B (zh) | 基于双侧远心镜头相机机床位置标定的音膜球顶装配系统 | |
CN111260730B (zh) | 一种利用基准传递原理的可变视轴视觉系统标定方法 | |
CN109465830B (zh) | 机器人单目立体视觉标定系统及方法 | |
CN113751981B (zh) | 基于双目视觉伺服的空间高精度装配方法和系统 | |
CN110722558B (zh) | 机器人的原点校正方法、装置、控制器和存储介质 | |
CN117381800B (zh) | 一种手眼标定方法及系统 | |
CN113500584B (zh) | 一种三自由度并联机器人的末端误差校正系统及方法 | |
CN116026252A (zh) | 一种点云测量方法及系统 | |
CN116309879A (zh) | 一种机器人辅助的多视角三维扫描测量方法 | |
CN115284292A (zh) | 基于激光相机的机械臂手眼标定方法及装置 | |
CN112109072B (zh) | 一种大型稀疏特征托盘精确6d位姿测量和抓取方法 | |
CN111975756B (zh) | 一种3d视觉测量系统的手眼标定系统及方法 | |
TWI708667B (zh) | 運動裝置之位置與姿態校正裝置及系統與方法 | |
CN110405731A (zh) | 一种快速双机械臂基坐标系标定方法 | |
CN115375775A (zh) | 基于前向投影折射模型的水下相机标定方法及装置 | |
CN111028298B (zh) | 一种用于刚体坐标系空间变换标定的汇聚式双目系统 | |
CN118752483A (zh) | 一种刚性连杆-柔性连续体混合机械臂的自标定系统及其实现方法 | |
CN116652970B (zh) | 一种四轴机械臂2d手眼标定方法及系统、存储介质 | |
WO2020010625A1 (zh) | 机器人运动学模型优化方法、系统和存储装置 | |
King et al. | Vision guided robots for automated assembly |
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 |