CN108453727B - 基于椭圆特征的机械臂末端位姿误差校正方法及系统 - Google Patents
基于椭圆特征的机械臂末端位姿误差校正方法及系统 Download PDFInfo
- Publication number
- CN108453727B CN108453727B CN201810026683.6A CN201810026683A CN108453727B CN 108453727 B CN108453727 B CN 108453727B CN 201810026683 A CN201810026683 A CN 201810026683A CN 108453727 B CN108453727 B CN 108453727B
- Authority
- CN
- China
- Prior art keywords
- joint
- actuator
- mechanical arm
- pose
- target
- 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
- 238000000034 method Methods 0.000 title claims abstract description 60
- 210000001503 joint Anatomy 0.000 claims description 65
- 239000011159 matrix material Substances 0.000 claims description 28
- 230000000007 visual effect Effects 0.000 claims description 28
- 230000009466 transformation Effects 0.000 claims description 25
- 238000004422 calculation algorithm Methods 0.000 claims description 17
- 238000012545 processing Methods 0.000 claims description 13
- 238000005070 sampling Methods 0.000 claims description 13
- 238000012937 correction Methods 0.000 claims description 12
- 230000003287 optical effect Effects 0.000 claims description 11
- 238000012216 screening Methods 0.000 claims description 10
- 238000013519 translation Methods 0.000 claims description 9
- 239000012636 effector Substances 0.000 description 16
- 238000004364 calculation method Methods 0.000 description 11
- 230000008569 process Effects 0.000 description 11
- 238000005457 optimization Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 6
- 239000013598 vector Substances 0.000 description 4
- 230000009286 beneficial effect Effects 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 238000003860 storage Methods 0.000 description 3
- 238000005259 measurement Methods 0.000 description 2
- 230000001131 transforming effect Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- DMSMPAJRVJJAGA-UHFFFAOYSA-N benzo[d]isothiazol-3-one Chemical compound C1=CC=C2C(=O)NSC2=C1 DMSMPAJRVJJAGA-UHFFFAOYSA-N 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 238000009924 canning Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000019771 cognition Effects 0.000 description 1
- 238000005520 cutting process Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 230000002401 inhibitory effect Effects 0.000 description 1
- 239000002184 metal Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000004513 sizing Methods 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 210000001364 upper extremity Anatomy 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/1679—Programme controls characterised by the tasks executed
- B25J9/1692—Calibration of manipulator
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Image Analysis (AREA)
- Image Processing (AREA)
Abstract
本发明涉及一种基于椭圆特征的机械臂末端位姿误差校正方法,包括获取圆口目标罐的图像,从图像中提取目标椭圆;根据目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系;根据相对位置和轴向关系以及机械臂构形约束,确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量;根据位姿偏差量调整机械臂,直至执行器的位姿偏差量小于预设值。该方法解决了单个圆特征定位存在二义性且无法确定轴向旋转姿态的问题,可为机械臂收集样品放入目标罐中提供精确引导,校正因局部柔性或关节间隙引起的控制误差。本发明还提供一种基于椭圆特征的机械臂末端位姿误差校正系统。
Description
技术领域
本发明涉及机械臂控制技术领域,尤其涉及一种基于椭圆特征的机械臂末端位姿误差校正方法及系统。
背景技术
随着科学技术的发展,机械臂不但在地面的应用越来越广泛,在地外天体表面着陆探测中也发挥越来越重要的作用,尤其对于地外天体表面的采样操作具有无可替代性。
地外天体表面着陆采样与返回是世界航天大国探索深空和拓展人类认知外星球和太阳系的重要手段。机械臂是地外天体表面着陆采样任务中必不可少的关键设备,能够按照预定的程序自主进行采样任务或者在地面遥操作中心的控制下进行采样作业,可以代替航天员完成各种采样和样品装罐操作,实现地外天体表面的无人化采样。
由于地外天体表面采样的机械臂构形多为类人上肢的多自由度细长结构,在采样操作中存在一定的柔性,同时机械臂关节存在间隙误差,使得机械臂末端的执行器重复定位精度不高。机械臂末端的视觉相机能够获取目标图像,利用图像特征解算目标位置,从而引导机械臂末端的执行器逐步逼近目标位置。然而,现有技术中通常选取图像特征解算出目标与机械臂末端执行器之间的相对位置,根据位置偏差实现机械臂控制,但这种方式要求必须选取能够唯一表示目标的位置状态的图像特征,且该方式仅关注机械臂末端的位置或位姿偏差量,无法实现对机械臂关节角度偏差量的精准控制。
发明内容
本发明所要解决的技术问题是针对现有技术的不足,提供一种基于椭圆特征的机械臂末端位姿误差校正方法及系统。
本发明解决上述技术问题的技术方案如下:一种基于椭圆特征的机械臂末端位姿误差校正方法,包括:
通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像,从所述圆口目标罐的图像中提取目标椭圆;
根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系;
根据所述相对位置和轴向关系以及机械臂构形约束,确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量;
根据所述执行器当前位姿、机械臂各关节的关节角度以及所述位姿偏差量调整机械臂,直至执行器的位姿偏差量小于预设值。
本发明的有益效果是:该方法利用机械臂末端的视觉相机获取目标罐的图像,从图像中获取单个目标椭圆,根据目标椭圆的椭圆特征确定机械臂末端执行器与目标罐的相对位置和轴向关系,进而确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量,根据位姿偏差量设计机械臂操作调整策略。与现有的视觉引导的误差校正方法相比,该方法能够利用机械臂末端视觉相机获取图像中的单个椭圆特征和机械臂构形先验信息计算目标罐的位置和姿态,不仅考虑机械臂末端位姿偏差,还考虑机械臂各关节的偏差量,解决了单个椭圆特征定位存在二义性且无法确定轴向旋转姿态的困难,可为机械臂抓取样品并放入目标罐中提供精确引导,校正因局部柔性或关节间隙引起的控制误差,使遥操作人员准确掌握机械臂末端执行器与目标罐的相对状态和机械臂各关节的当前状态,进而有效控制机械臂的精准操作,提高操作精度。
在上述技术方案的基础上,本发明还可以做如下改进。
进一步,所述从所述圆口目标罐的图像中提取目标椭圆包括:提取所述图像中的轮廓,通过弧段筛选算法从所述轮廓中剔除无效弧段,选择有效弧段,利用最小二乘算法对每个有效弧段进行椭圆拟合,合并交叠椭圆,剔除无效椭圆,获取目标椭圆。
采用上述进一步方案的有益效果是,通过上述方法可以快速获取图像中的椭圆,且对边界进行多次拟合,合并交叠椭圆,剔除无效椭圆,抑制图像噪声的影响,提高定位精度,准确获取目标椭圆。
进一步,所述根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系包括:
获取所述目标椭圆的椭圆方程描述参数,利用所述椭圆方程描述参数求取经过所述目标椭圆的椭锥面标准方程;
利用所述椭锥面标准方程求取与所述圆口目标罐的罐口尺寸相同的两个截面,根据所述两个截面确定圆口目标罐罐口与视觉相机之间的两组第一相对位置和第一轴向关系;
根据手眼标定关系将所述两组第一相对位置和第一轴向关系转换为圆口目标罐罐口与执行器之间的两组第二相对位置和第二轴向关系。
采用上述进一步方案的有益效果是,通过椭锥面标准方程和椭锥面截取等,能够以解析解的方式求得圆口目标罐的相对位置和轴向关系,求解中不引入随机误差,减少了定位解算中的误差源,提高了椭圆定位的整体解算精度。
进一步,所述根据所述相对位置和轴向关系以及机械臂构形约束,确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量包括:
根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且第一关节以外的所有关节轴相互平行的机械臂构形约束,确定机械臂各关节的关节角度和执行器位姿,得到两组执行器位姿及其对应的关节角度;其中,所述末轴垂面为与末端关节轴垂直的执行器所在的平面;
将所述两组执行器位姿及其对应的关节角度与已知的执行器目标位姿及其对应的关节角度作差求得执行器的位姿偏差量及其对应关节角度的偏差量,舍弃两组执行器位姿及其对应关节角度中偏差量大的一组,将偏差量小的一组执行器位姿作为执行器当前位姿,相应地获得当前机械臂各关节的关节角度;将所述执行器当前位姿与执行器目标位姿的偏差量作为执行器的位姿偏差量。
采用上述进一步方案的有益效果是,通过引入机械臂构形约束,能够利用椭圆定位得到的轴向信息解算机械臂的各关节角和执行器位姿,消除椭圆特征定位轴向时存在的姿态不确定性问题,进一步利用机械臂关节角和执行器位姿先验信息消除单一椭圆特征定位的二义性问题,从而得到唯一的相对位置和姿态解。
进一步,所述根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且第一关节以外的所有关节轴相互平行的机械臂构形约束,确定机械臂各关节的关节角度和执行器位姿,得到两组执行器位姿及其对应的关节角度包括:
根据机械臂第一关节以外的所有关节的轴线垂直于第一关节的特性,在末轴垂面上构建简化的机械臂等效模型;将所述第二轴向关系分解为末轴垂面的投影分量和法线分量,建立两组分量与关节角的变换方程,解算第一关节的关节角度和第一关节以外的所有关节的关节角度之和,进而计算执行器相对于机械臂基座的旋转变换矩阵,得到执行器姿态;
利用所述旋转变换矩阵、圆口目标罐相对于机械臂基座的平移关系和所述第二相对位置,确定所述执行器相对于机械臂基座的相对位置,得到执行器位置;其中,所述执行器位姿包括执行器姿态和执行器位置;
根据中间关节的关节角度共面的构形特点,将中间关节的关节角度投影到所述末轴垂面上,构建中间关节的关节角度与机械臂末端坐标位置的等式方程,解算中间关节的关节角度,进而求得末端关节的关节角度。
采用上述进一步方案的有益效果是,结合机械臂的构形特点,将机械臂逆运动学求解中的各关节的关节角度进行解耦处理,将多元联合方程组求解问题化简为多个一元方程的递推求解,简化了求解过程、降低了求解难度,使得仅依靠位置信息和不完全的姿态信息(轴向信息)即能完成所有关节角的逆运动学求解。
进一步,所述通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像时,确定所述视觉相机的光轴方向与圆口目标罐罐口法线方向的夹角,使所述夹角与机械臂执行器的姿态偏差量的差值大于预设值;其中所述执行器的位姿偏差量包括位置偏差量和姿态偏差量。
采用上述进一步方案的有益效果是,通过这一设定能够确保执行器姿态误差量能够被椭圆特征准确反映,不发生因光轴与目标罐罐口法线夹角太小导致执行器姿态误差量难以分辨的情况,确保了椭圆特征定位的唯一性和可靠性。
本发明解决上述技术问题的另一技术方案如下:一种基于椭圆特征的机械臂末端位姿误差校正系统,包括:
图像获取模块,用于通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像;
处理模块,用于从所述圆口目标罐的图像中提取目标椭圆;
用于根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系;
用于根据所述相对位置和轴向关系以及机械臂构形约束,确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量;
控制模块,用于根据所述执行器当前位姿、机械臂各关节的关节角度以及所述位姿偏差量调整机械臂,直至执行器的位姿偏差量小于预设值。
本发明的有益效果是:该系统利用机械臂末端的视觉相机获取目标罐的图像,从图像中获取单个目标椭圆,根据目标椭圆的椭圆特征确定机械臂末端执行器与目标罐的相对位置和轴向关系,进而确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量,根据位姿偏差量设计机械臂操作调整策略。与现有的视觉引导的误差校正相比,该系统能够利用机械臂末端视觉相机获取图像中的单个椭圆特征和机械臂构形先验信息计算目标罐的位置和姿态,不仅考虑机械臂末端位姿偏差,还考虑机械臂各关节的偏差量,解决了单个椭圆特征定位存在二义性且无法确定轴向旋转姿态的困难,可为机械臂抓取样品并放入目标罐中提供精确引导,校正因局部柔性或关节间隙引起的控制误差,使遥操作人员准确掌握机械臂末端执行器与目标罐的相对状态和机械臂各关节的当前状态,进而有效控制机械臂的精准操作,提高操作精度。
在上述技术方案的基础上,本发明还可以做如下改进。
进一步,所述处理模块具体用于:提取所述图像中的轮廓,通过弧段筛选算法从所述轮廓中剔除无效弧段,选择有效弧段,利用最小二乘算法对每个有效弧段进行椭圆拟合,合并交叠椭圆,剔除无效椭圆,获取目标椭圆。
采用上述进一步方案的有益效果是,通过上述方法可以快速获取图像中的椭圆,且对边界进行多次拟合,合并交叠椭圆,剔除无效椭圆,抑制图像噪声的影响,提高定位精度,准确获取目标椭圆。
进一步,所述处理模块具体用于:
获取所述目标椭圆的椭圆方程描述参数,利用所述椭圆方程描述参数求取经过所述目标椭圆的椭锥面标准方程;
利用所述椭锥面标准方程求取与所述圆口目标罐的罐口尺寸相同的两个截面,根据所述两个截面确定圆口目标罐罐口与视觉相机之间的两组第一相对位置和第一轴向关系;
根据手眼标定关系将所述第一相对位置和第一轴向关系转换为圆口目标罐罐口与执行器之间的两组第二相对位置和第二轴向关系。
采用上述进一步方案的有益效果是,通过椭锥面标准方程和椭锥面截取等,能够以解析解的方式求得圆口目标罐的相对位置和轴向关系,求解中不引入随机误差,减少了定位解算中的误差源,提高了椭圆定位的整体解算精度。
进一步,所述处理模块具体用于:
根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且第一关节以外的所有关节轴相互平行的机械臂构形约束,确定机械臂各关节的关节角度和执行器位姿,得到两组执行器位姿及其对应的关节角度;其中,所述末轴垂面为与末端关节轴垂直的执行器所在的平面;
将所述两组执行器位姿及其对应的关节角度与已知的执行器目标位姿及其对应的关节角度作差求得执行器的位姿偏差量及其对应关节角度的偏差量,舍弃两组执行器位姿及其对应关节角度中偏差量大的一组,将偏差量小的一组执行器位姿作为执行器当前位姿,相应地获得当前机械臂各关节的关节角度;将所述执行器当前位姿与执行器目标位姿的偏差量作为执行器的位姿偏差量。
采用上述进一步方案的有益效果是,通过引入机械臂构形约束,能够利用椭圆定位得到的轴向信息解算机械臂的各关节角和执行器位姿,消除椭圆特征定位轴向时存在的姿态不确定性问题,进一步利用机械臂关节角和执行器位姿先验信息消除单一椭圆特征定位的二义性问题,从而得到唯一的相对位置和姿态解。
附图说明
图1是为机械臂、执行器、视觉相机和目标罐结构示意图;
图2是基于椭圆特征的机械臂末端位姿误差校正方法流程图;
图3是对图像中边缘点构成的曲线进行抽样和分割点求取辅助图;
图4是对图像中多个弧段拟合椭圆的合并示意图;
图5是空间目标圆向像平面的投影示意图;
图6是机械臂构形、视觉相机、圆口目标罐及尺寸标注图;
图7是简化的等效机械臂构形、视觉相机、圆口目标罐及尺寸标注图;
图8是机械臂基座及各关节中心、相机、圆口目标罐的坐标系建立图;
图9是机械臂的俯视投影及其坐标轴描述图;
图10是机械臂的侧视投影及其坐标轴描述图;
图11是基于椭圆特征的机械臂末端位姿误差校正系统示意图。
具体实施方式
以下结合附图对本发明的原理和特征进行描述,所举实施例只用于解释本发明,并非用于限定本发明的范围。
图1为机械臂、执行器、视觉相机和目标罐结构示意图。其中执行器固定在机械臂末端,视觉相机固定在执行器上,目标罐固定于视觉相机的拍摄范围内。
图2给出了本发明实施例提供的一种基于椭圆特征的机械臂末端位姿误差校正方法的示意性流程图。如图2所示,该方法包括:
S1,通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像,从所述圆口目标罐的图像中提取目标椭圆;
S2,根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系;
S3,根据所述相对位置和轴向关系以及机械臂构形约束,确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量;
S4,根据所述执行器当前位姿、机械臂各关节的关节角度以及所述位姿偏差量调整机械臂,直至执行器的位姿偏差量小于预设值。
上述实施例中,该方法利用机械臂末端的视觉相机获取目标罐的图像,从图像中获取单个目标椭圆,根据目标椭圆的椭圆特征确定机械臂末端执行器与目标罐的相对位置和轴向关系,进而确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量,根据位姿偏差量设计机械臂操作调整策略。与现有的视觉引导的误差校正方法相比,该方法能够利用机械臂末端视觉相机获取图像中的单个椭圆特征和机械臂构形先验信息计算目标罐的位置和姿态,不仅考虑机械臂末端位姿偏差,还考虑机械臂各关节的偏差量,解决了单个椭圆特征定位存在二义性且无法确定轴向旋转姿态的困难,可为机械臂抓取样品并放入目标罐中提供精确引导,校正因局部柔性或关节间隙引起的控制误差,使遥操作人员准确掌握机械臂末端执行器与目标罐的相对状态和机械臂各关节的当前状态,进而有效控制机械臂的精准操作,提高操作精度。
现有技术中利用单个椭圆特征解算目标罐的位姿,会得到两个对称的解,这称为椭圆特征定位的二义性。由于圆形目标罐的对称特征,相机从不同角度看,能够得到相同形状的椭圆,这使得在根据椭圆特征定位时,仅能解算轴向法线,不能完全确定圆形目标罐与相机的姿态关系。
该实施例中,利用单个椭圆特征和机械臂构形信息能够计算得到目标罐唯一的位置和姿态,主要是由于在利用椭圆特征定位解算中,考虑了机械臂的构形约束,使得最终的位置和姿态解唯一。
在基于视觉特征的操作定位中,图像特征的选择和确定模型直接影响最终定位的精度,合理选择目标特征和构建确定模型是实现视觉引导精确操作的关键,对实现地外天体表面采样具有重要意义。
可选地,作为本发明另外一个实施例,S1,通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像,从所述圆口目标罐的图像中提取目标椭圆包括:通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像,提取所述图像中的轮廓,通过弧段筛选算法从所述轮廓中剔除无效弧段,选择有效弧段,利用最小二乘算法对每个有效弧段进行椭圆拟合,合并交叠椭圆、剔除无效椭圆,获取目标椭圆。
该实施例中,通过上述方法可以快速获取图像中的椭圆,且对边界进行多次拟合,合并交叠椭圆,剔除无效椭圆,抑制图像噪声的影响,提高定位精度,准确获取目标椭圆。
弧段筛选算法对图像中提取出的边缘点进行编组和筛选,舍去无效弧段。具体包括如下三个步骤:第一步是边缘跟踪连接与细化,第二步是对连接和细化的曲线进行抽样和分割,第三步是对无效弧段进行剔除。结合图3对弧段筛选算法介绍如下:
第一步是边缘跟踪连接与细化:在圆口目标罐的图像中搜出一个边缘点,再以该边缘点为起点在周围8邻域进行边缘点搜寻,加入到边缘序列点中构建曲线。如果周围8邻域搜得的边缘点数量大于2,判断边缘是否有分叉,如果边缘有分叉,则将分叉边缘定义为一个新的边缘曲线,如果边缘无分叉则删除多余的边缘点。
第二步是对连接和细化的曲线进行抽样和分割:首先对离散点组成的弧段以固定的弧长ΔS(可令ΔS=3)进行抽样,得到新的离散点弧段,如图3所示。然后对离散点弧段进行分割点搜寻分割弧段,方法如下:如果相邻两点连线的角度差大于给定阈值δc即|θ1-θ2|>δc,或者相邻两点连线的斜率单调变化即(k2-k1)(k3-k2)<0,表达为坐标Pi(xi,yi)的形式为(Δx12Δy23-Δy12Δx23)(Δx23Δy34-Δy23Δx34)<0,其中,Δxij=xj-xi,Δyij=yj-yi,则在P3位置对弧段进行分割。其中Pi(xi,yi)表示离散点的坐标,β1表示直线P1P2与直线P2P3的夹角,β2表示直线P2P3与直线P3P4的夹角,k1,k2,k3分别表示直线P1P2、直线P2P3和直线P3P4的斜率。
基于弧段筛选算法得到多个有效弧段,对每个弧段采用最小二乘算法拟合椭圆,求取椭圆方程描述参数的方法描述如下。椭圆方程可通过带约束的二次曲线进行描述,其一般形式为:
s.t.表示优化问题的约束(subjected to),优化问题的数学表达方式。令Θ=[AB C D E F],u=[x2 2xy y2 2x 2y 1],上述公式中的系数Θ可以根据比例因子不同产生无数组解,均不影响u的取值。为了实现Θ解的唯一性,可指定一个尺度因子,如F=1或者||Θ||=1。拟合求解过程给出如下。
对公式(1-3)中的优化问题进行求解,通过引入拉格朗日因子λ,将带约束的优化问题转化为无约束问题,并对二次目标函数进行求导运算可得:
令S=UTU,考虑U、S和H的特殊性,我们进行矩阵分块得:
根据上述分块关系,可重新构建矩阵方程:
S1Θ1+S2Θ2=λH1Θ1, (1-5)
由于H1为非奇异矩阵,公式(1-7)可写成:
公式(1-4)中的等式约束重写为:
Θ1H1Θ1=1. (1-9)
令则椭圆拟合求解问题转化为求解MΘ1=λΘ1的问题,即求取矩阵M的特征值的问题。考虑到Θ1H1Θ1=1对特征值非负性的约束,取M的非负特征值对应的特征向量作为Θ1的解。根据求得Θ2,从而得到椭圆曲线一般方程的系数Θ,即得到二次曲线椭圆方程的描述参数A,B,C,D,E,F。
为了抑制图像噪声的影响,提高定位精度,可对边界进行多次拟合。即第一次拟合后,将每个边界点代入上式,计算残差。然后将残差较大的一部分点剔除,再对剩余的点进行二次椭圆拟合。该过程可以重复若干次,直到均方差小于某一阈值为止。
基于上述的二次曲线椭圆方程的描述参数,可以求解椭圆方程的标准形式为:
椭圆方程的标准形式包含椭圆中心(x0,y0)、椭圆方向角θ和长短半轴a、b共5个参数。利用二次曲线椭圆方程的描述参数A,B,C,D,E,F计算出这5个参数的计算公式如下,其中尺度因子F=1:
其中,(x0,y0)为拟合椭圆中心坐标,θ为长半轴与图像坐标系x轴夹角。根据式(1-1)、(1-12)进行变换得:
根据式(1-13)得到的参数可以求出椭圆的长、短半轴a和b如下。
在得到(x0,y0)、θ和长短半轴a、b后,可利用多个有效弧段各自拟合出的椭圆的相似关系,合并同一椭圆的弧段为一弧段组。合并方法结合图4介绍如下。为每个有效弧段定义旋转坐标系,坐标系原点定义为拟合椭圆的中心,坐标系起始轴定义为与图像x轴平行方向,定义逆时针旋转方向为正,顺时针旋转方向为负,定义旋转角度范围为[-2π,2π]。图4中的三个弧段i,j,k对应的旋转角度范围分别为Γi[αi1,αi2]、Γj[αj1,αj2]、Γk[αk1,αk2],对应的拟合椭圆参数分别为Ωi(xi,yi,ai,bi,θi)、Ωj(xj,yj,aj,bj,θj)、Ωk(xk,yk,ak,bk,θk)。如果满足||ΩH-ΩG||2≤ξΩ且其中H,G∈{i,j,k},ξΩ为给定的阈值,则将弧段H和弧段G合并为同一组。
对弧段组内的多个弧段应用上述的最小二乘拟合算法获得新的椭圆,选择弧段组内弧段覆盖角度最大的椭圆作为最终的目标罐口椭圆。
上述过程中假设图像中背景较为简单,仅存在目标罐口为圆形特征,不存在其他的形状以及尺寸近似的干扰椭圆,则能够选出唯一的目标罐口椭圆。对于复杂背景下存在干扰椭圆的情况,则可以通过引入机械臂末端位姿的近似位置作为先验信息,根据透视投影原理计算目标椭圆的大致位置、方向与尺寸,筛选掉偏离较大的椭圆,保留唯一的目标罐口椭圆。
可选地,作为本发明另一个实施例,S2,根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系包括:
获取所述目标椭圆的椭圆方程描述参数,利用所述椭圆方程描述参数求取经过所述目标椭圆的椭锥面标准方程;
利用所述椭锥面标准方程求取与所述圆口目标罐的罐口尺寸相同的两个截面,根据所述两个截面确定圆口目标罐罐口与视觉相机之间的两组第一相对位置和第一轴向关系;
根据手眼标定关系将所述第一相对位置和第一轴向关系转换为圆口目标罐罐口与执行器之间的两组第二相对位置和第二轴向关系。
该实施例中,通过椭锥面标准方程和椭锥面截取等,能够以解析解的方式求得圆口目标罐的位置和轴向关系,求解中不引入随机误差,减少了定位解算中的误差源,提高了椭圆定位的整体解算精度。
图5呈现了空间目标圆向像平面的投影示意图。在空间中尺寸相等的两个目标圆能够投影到像平面中形成同一椭圆图像,因此利用图像椭圆方程和目标罐罐口尺寸求解罐口位姿可得到两组解。下面结合图5对S2步骤中圆口目标罐罐口与相机之间的相对位置和轴向关系的解算过程进行介绍。
首先,圆口目标罐罐口圆在图像中成像满足透视投影原理,设uj为图像中椭圆弧线上的点,则满足以下公式:
其中,uj=(uj,vj,1)T表示第j个观测点投影到相机图像平面的点坐标,Ω=(t,θ)表示相机坐标系相对于世界坐标系的平移和旋转, 分别表示第j个月面观测点在相机坐标系和世界坐标系中的齐次坐标,表示从月面观测点到图像的透视投影变换,表示与投影坐标相关的畸变参数矩阵,定义如下:
椭圆弧线点uj在图像平面内满足椭圆方程:
其中A,B,C,D,E,F为步骤S1中求得的椭圆描述参数,转换成矩阵的形式:
将公式(2-1)带入公式(2-3)可得过相机光心和空间圆形成的椭圆锥的曲面方程:
在相机坐标系下,圆锥的表达式比较复杂,切割平面不变描述和计算。因此先将相机坐标系空间转换到标准空间计算,计算结果再转换到相机坐标系中。旋转过程固定椭圆锥原点仍在摄像机光心处。由Q为对称矩阵可知,存在正交矩阵P将Q对角化得:
PTQP=diag(λ1,λ2,λ3),
λ1Xcs+λ2Ycs+λ3Zcs=0. (2-6)
由公式(2-6)可知,三个系数λ1,λ2,λ3必然有两个同号,且与另一个异号。由于λ1,λ2,λ3和特征向量矩阵P的正负取值的问题,使得λ1,λ2,λ3有多组解。设Q的特征值为κ1,κ2,κ3,与只对应的特征向量为v1,v2,v3。设κ1,κ2同号且|κ1|>|κ2|,则λ1=κ1,λ2=κ2,λ3=κ3。令P=[p1,p2,p3],若则p1=v1,否则p1=-v1。p2=v2,p2=v2×v3。
基于标准空间中的椭圆锥曲面方程,可求得圆心位置和圆所在平面的法向量为:
将公式(2-7)和公式(2-8)的结果转换到摄像机坐标系得:
即为目标罐罐口与相机之间的相对位置和轴向关系。令分别表示末端执行器坐标系到相机坐标系的齐次变换矩阵和旋转矩阵,将上述相对位置和轴向关系变换到机械臂末端执行器坐标系得到目标罐罐口与末端执行器之间的两组相对位置和轴向关系:
可选地,作为本发明另一个实施例,S3,根据所述相对位置和轴向关系以及机械臂构形约束,确定执行器的精确位姿和机械臂各关节的角度包括:
根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且第一关节以外的所有关节轴相互平行的机械臂构形约束,确定机械臂各关节的关节角度和执行器位姿,得到两组执行器位姿及其对应的关节角度;其中,所述末轴垂面为与末端关节轴垂直的执行器所在的平面;
将所述两组执行器位姿及其对应的关节角度与已知的执行器目标位姿及其对应的关节角度作差求得执行器的位姿偏差量及其对应关节角度的偏差量,舍弃两组执行器位姿及其对应关节角度中偏差量大的一组,将偏差量小的一组执行器位姿作为执行器当前位姿,相应地获得当前机械臂各关节的关节角度;将所述执行器当前位姿与执行器目标位姿的偏差量作为执行器的位姿偏差量。
该实施例中,通过引入机械臂构形约束,能够利用椭圆定位得到的轴向信息解算机械臂的各关节角和执行器位姿,消除椭圆特征定位轴向时存在的姿态不确定性问题,进一步利用机械臂关节角和执行器位姿先验信息消除单一椭圆特征定位的二义性问题,从而得到唯一的相对位置和姿态解。
可选地,作为本发明另一个实施例,根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且第一关节以外的所有关节轴相互平行的机械臂构形约束,确定机械臂各关节的关节角度和执行器位姿,得到两组执行器位姿及其对应的关节角度包括:
根据机械臂第一关节以外的所有关节的轴线垂直于第一关节的特性,在末轴垂面上构建简化的机械臂等效模型;将所述第二轴向关系分解为末轴垂面的投影分量和法线分量,建立两组分量与关节角的变换方程,解算第一关节的关节角度和第一关节以外的所有关节的关节角度之和,进而计算执行器相对于机械臂基座的旋转变换矩阵,得到执行器姿态;
利用所述旋转变换矩阵、圆口目标罐相对于机械臂基座的平移关系和所述第二相对位置,确定所述执行器相对于机械臂基座的相对位置,得到执行器位置;其中,所述执行器位姿包括执行器姿态和执行器位置;
根据中间关节的关节角度共面的构形特点,将中间关节的关节角度投影到所述末轴垂面上,构建中间关节的关节角度与机械臂末端坐标位置的等式方程,解算中间关节的关节角度,进而求得末端关节的关节角度。其中,第一关节以外的所有关节为第一关节以外的其他关节。其中,中间关节为除第一关节和末端关节以外的其他关节。
该实施例中,结合机械臂的构形特点,将机械臂逆运动学求解中的各关节的关节角度进行解耦处理,将多元联合方程组求解问题化简为多个一元方程的递推求解,简化了求解过程、降低了求解难度,使得仅依靠位置信息和不完全的姿态信息(轴向信息)即能完成所有关节角的逆运动学求解。
利用机械臂构形约束和视觉测量的执行器中心坐标计算机械臂各关节角度的方法分为三步实现。以下以四自由度机械臂做如下说明:首先根据机械臂二、三、四关节轴线垂直于第一关节的特性,在与第四关节轴垂直的执行器所在平面(末轴垂面)上构建简化的机械臂等效模型,将第二轴向关系分解为该平面的投影分量和法线分量,建立两组分量与关节角的变换方程,解算第一关节的关节角度和二、三、四关节的关节角度之和,进而计算机械臂末端执行器相对于机械臂基座的旋转变换矩阵,即机械臂末端执行器姿态;再利用该旋转变换矩阵和圆口目标罐相对于机械臂基座的平移关系和所述第二相对位置,确定执行器相对于机械臂基座的位置,即执行器位置;最后根据第二、三关节的关节角度共面的构形特点,将第二、三关节的关节角度投影到与第四关节轴垂直的执行器所在平面(末轴垂面)上,构建第二、三关节的关节角度与机械臂末端坐标位置的等式方程,解算第二关节和第三关节的关节角度,进而求得第四关节的关节角度。具体介绍如下。
结合图6、图7介绍机械臂的构形及其简化的等效构形如下。如图6所示,机械臂第二、第三、第四关节轴相互平行且与第一关节轴垂直,从而可以将图6中的原始的机械臂构形等效为图7中的简化模型。设第一、二、三、四关节的关节角分别为θ1,θ2,θ3,θ4,关节中心之间在沿关节轴向方向偏移分别为d1,d2,d3,d4,在垂直于关节轴向方向的偏移分别为a1,a2,a3,a4。其中d1,d2,d3,d4、a1,a2,a3,a4均为已知量,a1=a4=0,θ1,θ2,θ3,θ4为运动可变量。由于机械臂为细长金属结构,其第一、第二、第三关节在运动过程中负荷较重,可能存在间隙使得θ1,θ2,θ3,θ4与实际控制量产生偏差;同时一、二关节和二、三关节之间的臂杆为细长臂杆,可能发生微小形变,使得a2,a3与实测值存在微小偏差。在图7的简化等效模型中,第一、四关节与原始机械臂构形相同,第二、三关节与连杆简化成为两根连杆,连杆1为可伸缩连杆,长度可变,连杆2为定长连杆,长度为d2+d3。该简化更加有利于后续对机械臂各关节角的解耦求解。
下面结合图8至图10,介绍根据视觉测量的执行器中心坐标求解准确的θ1,θ2,θ3,θ4,校正机械臂运动偏差的方法如下。在机械臂基座、四个关节中心、相机光心和圆口目标罐罐口中心建立坐标系如图8所示。第一关节中心坐标系由基座坐标系沿z0轴平移距离d1得到,随着第一关节的运动x1轴相对于x0轴发生旋转。第二、三、四关节中心坐标系的z轴相互平行且始终与z0(z1)垂直,随着各个关节的运动第二、三、四关节中心坐标系的x轴之间夹角发生变化。设机械臂末端执行器坐标系与第四关节中心坐标系重合。定义和分别为机械臂基座坐标系到第i关节中心坐标系、第i关节中心坐标系到第j关节中心坐标系、机械臂基座坐标系到圆口目标罐坐标系、机械臂基座坐标系到末端执行器坐标系和末端执行器坐标系到相机坐标系的齐次变换矩阵,分别为对应五类坐标系变换的旋转矩阵, 为对应五类坐标系变换的平移向量。上述变量中,和均通过预先标定的方式确定为已知量,而和可通过机械臂的构形状态近似计算得到,然而由于存在机械臂关节末端位姿误差和臂杆柔性误差,使得求得的和存在一定误差,需要根据测量结果校正计算得到。下面介
根据步骤2中的测量相机坐标系中的计算结果【公式(2-10)和公式(2-11)】,将其变换到机械臂末端执行器坐标系得:
由式(3-1)-(3-3)可知:
对上述矩阵方程展开得:
上述方程中的后两个等式通过三角函数关系可变换得到关于sinθ1和sinθs的一元二次方程,根据一元二次方程求根公式分别求得θ1和θs,各有2个解。再根据第一个等式筛选得到唯一的θ1和θs,从而求得旋转变换矩阵根据公式(3-5)即可求得
将已知量带入公式(3-7)并进行展开运算得:
将公式(3-8)中的前两个等式联立构建关于θ2和θ2+θ3为自变量的方程组,消去θ2+θ3可得到关于θ2的一元二次方程,可得到2组解,比对2组解与已知的机械臂构形状态之间的相似关系,即可求得唯一的θ2,进而求得θ3,利用θ4=θs-θ2-θ3得到θ4。从而完成了对θ1,θ2,θ3,θ4的求解。
由于在公式(2-9)和(2-10)中,目标罐罐口中心坐标和法线为两组解,因此求得的θ1,θ2,θ3,θ4也有两组解。将这两组解分别与机械臂目标状态进行比较计算偏差,选取偏差量较小的一组解作为求得的精确位姿。根据机械臂状态与精确位姿偏差量,向减小偏差的方向调整机械臂运动。其中,机械臂目标状态是指机械臂末端执行器抵达目标位姿时对应的机械臂各关节的关节角度,可以根据机械臂末端执行器的位姿利用逆运动学求解得到。
可选地,作为本发明另一个实施例,通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像时,确定所述视觉相机的光轴方向与圆口目标罐罐口法线方向的夹角,使所述夹角与执行器的姿态偏差量的差值大于预设值;其中所述执行器的位姿偏差量包括位置偏差量和姿态偏差量。
该实施例中,通过固定在末端执行器末端的手眼相机拍摄圆口目标罐图像时,要求手眼相机的光轴方向与目标罐口法线方向夹角远大于执行器的姿态偏差量,如可大于执行器姿态偏差量的5倍。该要求能够保证S3步骤中根据末端执行器位姿偏差量大小,舍弃偏差大的一组,只保留唯一解的正确性。通过这一设定能够确保执行器姿态误差量能够被椭圆特征准确反映,不发生因光轴与目标罐罐口法线夹角太小导致执行器姿态误差量难以分辨的情况,确保了椭圆特征定位的唯一性和可靠性。
需要说明的是,上述实施例中的机械臂可以为如图1所示的机械臂,该机械臂可以为四自由度机械臂,其四个关节皆为旋转关节,第二、三、四关节平行,与水平多关节机械臂SCARA的三个旋转关节构造相同,并与第一轴垂直。一、二关节和三、四关节组合紧凑,关节轴距离极小,二、三关节和三、四关节之间通过长臂连杆连接。末端执行器安装固定于四自由度机械臂的第四关节处,安装固定方向与第四关节垂直,能够绕第四关节转动;视觉相机可以为手眼相机,安装于执行器上,轴线与执行器轴线平行;圆口目标罐安装于机械臂可达的范围内,固定安装,罐口裸露向上,能够被机械臂末端的视觉相机观测,罐口尺寸已知。
上文结合图1至图10,详细描述了根据本发明实施例提供基于椭圆特征的机械臂末端位姿误差校正方法。下面结合图11,详细描述本发明实施例提供基于椭圆特征的机械臂末端位姿误差校正系统。
图11给出了本发明实施例提供的一种基于椭圆特征的机械臂末端位姿误差校正系统的示意性结构框图。如图11所示,该系统包括图像获取模块、处理模块和控制模块。
图像获取模块通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像;处理模块从所述圆口目标罐的图像中提取目标椭圆;根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系;根据所述相对位置和轴向关系以及机械臂构形约束,确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量;控制模块根据所述执行器当前位姿、机械臂各关节的关节角度以及所述位姿偏差量调整机械臂,直至执行器的位姿偏差量小于预设值。
该实施例中,利用机械臂末端的视觉相机获取目标罐的图像,从图像中获取单个目标椭圆,根据目标椭圆的椭圆特征确定机械臂末端执行器与目标罐的相对位置和轴向关系,进而确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量,根据位姿偏差量设计机械臂操作调整策略。与现有的视觉引导的误差校正方法相比,该方法能够利用机械臂末端视觉相机获取图像中的单个椭圆特征和机械臂构形先验信息计算目标罐的位置和姿态,不仅考虑机械臂末端位姿偏差,还考虑机械臂各关节的偏差量,解决了单个椭圆特征定位存在二义性且无法确定轴向旋转姿态的困难,可为机械臂抓取样品并放入目标罐中提供精确引导,校正因局部柔性或关节间隙引起的控制误差,使遥操作人员准确掌握机械臂末端执行器与目标罐的相对状态和机械臂各关节的当前状态,进而有效控制机械臂的精准操作,提高操作精度。
可选地,作为本发明另外一个实施例,所述处理模块具体用于:提取所述图像中的轮廓,通过弧段筛选算法从所述轮廓中剔除无效弧段,选择有效弧段,利用最小二乘算法对每个有效弧段进行椭圆拟合,合并交叠椭圆,剔除无效椭圆,获取目标椭圆。
可选地,作为本发明另一个实施例,所述处理模块具体用于:获取所述目标椭圆的椭圆方程描述参数,利用所述椭圆方程描述参数求取经过所述目标椭圆的椭锥面标准方程;利用所述椭锥面标准方程求取与所述圆口目标罐的罐口尺寸相同的两个截面,根据所述两个截面确定圆口目标罐罐口与视觉相机之间的两组第一相对位置和第一轴向关系;根据手眼标定关系将所述第一相对位置和第一轴向关系转换为圆口目标罐罐口与执行器之间的两组第二相对位置和第二轴向关系。
可选地,作为本发明另一个实施例,所述处理模块具体用于:根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且第一关节以外的所有关节轴相互平行的机械臂构形约束,确定机械臂各关节的关节角度和执行器位姿,得到两组执行器位姿及其对应的关节角度;其中,所述末轴垂面为与末端关节轴垂直的执行器所在的平面;将所述两组执行器位姿及其对应的关节角度与已知的执行器目标位姿及其对应的关节角度作差求得执行器的位姿偏差量及其对应关节角度的偏差量,舍弃两组执行器位姿及其对应关节角度中偏差量大的一组,将偏差量小的一组执行器位姿作为执行器当前位姿,相应地获得当前机械臂各关节的关节角度;将所述执行器当前位姿与执行器目标位姿的偏差量作为执行器的位姿偏差量。
可选地,作为本发明另一个实施例,处理模块具体用于,根据机械臂第一关节以外的所有关节的轴线垂直于第一关节的特性,在末轴垂面上构建简化的机械臂等效模型;将所述第二轴向关系分解为末轴垂面的投影分量和法线分量,建立两组分量与关节角的变换方程,解算第一关节的关节角度和第一关节以外的所有关节的关节角度之和,进而计算执行器相对于机械臂基座的旋转变换矩阵,得到执行器姿态;利用所述旋转变换矩阵、圆口目标罐相对于机械臂基座的平移关系和所述第二相对位置,确定所述执行器相对于机械臂基座的相对位置,得到执行器位置;其中,所述执行器位姿包括执行器姿态和执行器位置;根据中间关节的关节角度共面的构形特点,将中间关节的关节角度投影到所述末轴垂面上,构建中间关节的关节角度与机械臂末端坐标位置的等式方程,解算中间关节的关节角度,进而求得末端关节的关节角度。
可选地,作为本发明另一个实施例,图像获取模块通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像时,确定所述视觉相机的光轴方向与圆口目标罐罐口法线方向的夹角,使所述夹角与执行器的姿态偏差量的差值大于预设值;其中所述执行器的位姿偏差量包括位置偏差量和姿态偏差量。
所属领域的技术人员可以清楚地了解到,为了描述的方便和简洁,上述描述的装置和单元的具体工作过程,可以参考前述方法实施例中的对应过程,在此不再赘述。
在本申请所提供的几个实施例中,应该理解到,所揭露的装置和方法,可以通过其它的方式实现。例如,以上所描述的装置实施例仅仅是示意性的,例如,单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个单元或组件可以结合或者可以集成到另一个系统,或一些特征可以忽略,或不执行。
作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本发明实施例方案的目的。
另外,在本发明各个实施例中的各功能单元可以集成在一个处理单元中,也可以是各个单元单独物理存在,也可以是两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现,也可以采用软件功能单元的形式实现。
集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本发明的技术方案本质上或者说对现有技术做出贡献的部分,或者该技术方案的全部或部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行本发明各个实施例方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(ROM,Read-Only Memory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述仅为本发明的较佳实施例,并不用以限制本发明,凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (5)
1.一种基于椭圆特征的机械臂末端位姿误差校正方法,其特征在于,包括:
通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像,从所述圆口目标罐的图像中提取目标椭圆;
根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系,具体包括:获取所述目标椭圆的椭圆方程描述参数,利用所述椭圆方程描述参数求取经过所述目标椭圆的椭锥面标准方程;利用所述椭锥面标准方程求取与所述圆口目标罐的罐口尺寸相同的两个截面,根据所述两个截面确定圆口目标罐罐口与视觉相机之间的两组第一相对位置和第一轴向关系;根据手眼标定关系将所述两组第一相对位置和第一轴向关系转换为圆口目标罐罐口与执行器之间的两组第二相对位置和第二轴向关系;
根据所述相对位置和轴向关系以及机械臂构形约束,确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量,具体包括:根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且第一关节以外的所有关节轴相互平行的机械臂构形约束,确定机械臂各关节的关节角度和执行器位姿,得到两组执行器位姿及其对应的关节角度;其中,所述末轴垂面为与末端关节轴垂直的执行器所在的平面;
将所述两组执行器位姿及其对应的关节角度与已知的执行器目标位姿及其对应的关节角度作差求得执行器的位姿偏差量及其对应关节角度的偏差量,舍弃两组执行器位姿及其对应关节角度中偏差量大的一组,将偏差量小的一组执行器位姿作为执行器当前位姿,相应地获得当前机械臂各关节的关节角度;将所述执行器当前位姿与执行器目标位姿的偏差量作为执行器的位姿偏差量;
所述根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且第一关节以外的所有关节轴相互平行的机械臂构形约束,确定机械臂各关节的关节角度和执行器位姿,得到两组执行器位姿及其对应的关节角度包括:
根据机械臂第一关节以外的所有关节的轴线垂直于第一关节轴线的特性,在末轴垂面上构建简化的机械臂等效模型;将所述第二轴向关系分解为末轴垂面的投影分量和法线分量,建立两组分量与关节角的变换方程,解算第一关节的关节角度和第一关节以外的所有关节的关节角度之和,进而计算执行器相对于机械臂基座的旋转变换矩阵,得到执行器姿态;利用所述旋转变换矩阵、圆口目标罐相对于机械臂基座的平移关系和所述第二相对位置,确定所述执行器相对于机械臂基座的相对位置,得到执行器位置;其中,所述执行器位姿包括执行器姿态和执行器位置;根据中间关节的关节角度共面的构形特点,将中间关节的关节角度投影到所述末轴垂面上,构建中间关节的关节角度与机械臂末端坐标位置的等式方程,解算中间关节的关节角度,进而求得末端关节的关节角度;
根据所述执行器当前位姿、机械臂各关节的关节角度以及所述位姿偏差量调整机械臂,直至执行器的位姿偏差量小于预设值。
2.根据权利要求 1 所述的方法,其特征在于,所述从所述圆口目标罐的图像中提取目标椭圆包括:提取所述图像中的轮廓,通过弧段筛选算法从所述轮廓中剔除无效弧段,选择有效弧段,具体包括如下三个步骤:第一步是边缘跟踪连接与细化,第二步是对连接和细化的曲线进行抽样和分割,第三步是对无效弧段进行剔除;利用最小二乘算法对每个有效弧段进行椭圆拟合,合并交叠椭圆,剔除无效椭圆,获取目标椭圆。
3.根据权利要求1至2任一项所述的方法,其特征在于,所述通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像时,确定所述视觉相机的光轴方向与圆口目标罐罐口法线方向的夹角,使所述夹角与执行器的姿态偏差量的差值大于预设值;其中所述执行器的位姿偏差量包括位置偏差量和姿态偏差量。
4.一种基于椭圆特征的机械臂末端位姿误差校正系统,其特征在于,包括:
图像获取模块,用于通过固定于机械臂末端的执行器上的视觉相机获取圆口目标罐的图像;
处理模块,用于从所述圆口目标罐的图像中提取目标椭圆;
根据所述目标椭圆的椭圆特征确定圆口目标罐罐口与执行器之间的相对位置和轴向关系,具体包括:获取所述目标椭圆的椭圆方程描述参数,利用所述椭圆方程描述参数求取经过所述目标椭圆的椭锥面标准方程;利用所述椭锥面标准方程求取与所述圆口目标罐的罐口尺寸相同的两个截面,根据所述两个截面确定圆口目标罐罐口与视觉相机之间的两组第一相对位置和第一轴向关系;根据手眼标定关系将所述第一相对位置和第一轴向关系转换为圆口目标罐罐口与执行器之间的两组第二相对位置和第二轴向关系;
根据所述相对位置和轴向关系以及机械臂构形约束,确定执行器当前位姿、机械臂各关节的关节角度以及执行器的位姿偏差量,具体包括:根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且第一关节以外的所有关节轴相互平行的机械臂构形约束,确定机械臂各关节的关节角度和执行器位姿,得到两组执行器位姿及其对应的关节角度;其中,所述末轴垂面为与末端关节轴垂直的执行器所在的平面;
将所述两组执行器位姿及其对应的关节角度与已知的执行器目标位姿及其对应的关节角度作差求得执行器的位姿偏差量及其对应关节角度的偏差量,舍弃两组执行器位姿及其对应关节角度中偏差量大的一组,将偏差量小的一组执行器位姿作为执行器当前位姿,相应地获得当前机械臂各关节的关节角度;将所述执行器当前位姿与执行器目标位姿的偏差量作为执行器的位姿偏差量;
所述根据所述两组第二相对位置和第二轴向关系以及末轴垂面与第一关节的轴平行且相对距离已知且第一关节以外的所有关节轴相互平行的机械臂构形约束,确定机械臂各关节的关节角度和执行器位姿,得到两组执行器位姿及其对应的关节角度包括:
根据机械臂第一关节以外的所有关节的轴线垂直于第一关节轴线的特性,在末轴垂面上构建简化的机械臂等效模型;将所述第二轴向关系分解为末轴垂面的投影分量和法线分量,建立两组分量与关节角的变换方程,解算第一关节的关节角度和第一关节以外的所有关节的关节角度之和,进而计算执行器相对于机械臂基座的旋转变换矩阵,得到执行器姿态;利用所述旋转变换矩阵、圆口目标罐相对于机械臂基座的平移关系和所述第二相对位置,确定所述执行器相对于机械臂基座的相对位置,得到执行器位置;其中,所述执行器位姿包括执行器姿态和执行器位置;根据中间关节的关节角度共面的构形特点,将中间关节的关节角度投影到所述末轴垂面上,构建中间关节的关节角度与机械臂末端坐标位置的等式方程,解算中间关节的关节角度,进而求得末端关节的关节角度;
控制模块,用于根据所述执行器当前位姿、机械臂各关节的关节角度以及所述位姿偏差量调整机械臂,直至执行器的位姿偏差量小于预设值。
5.根据权利要求 4 所述的系统,其特征在于,所述处理模块具体用于:提取所述图像中的轮廓,通过弧段筛选算法从所述轮廓中剔除无效弧段,选择有效弧段,具体包括如下三个步骤:第一步是边缘跟踪连接与细化,第二步是对连接和细化的曲线进行抽样和分割,第三步是对无效弧段进行剔除;利用最小二乘算法对每个有效弧段进行椭圆拟合,合并交叠椭圆,剔除无效椭圆,获取目标椭圆。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810026683.6A CN108453727B (zh) | 2018-01-11 | 2018-01-11 | 基于椭圆特征的机械臂末端位姿误差校正方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810026683.6A CN108453727B (zh) | 2018-01-11 | 2018-01-11 | 基于椭圆特征的机械臂末端位姿误差校正方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108453727A CN108453727A (zh) | 2018-08-28 |
CN108453727B true CN108453727B (zh) | 2020-08-25 |
Family
ID=63220621
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810026683.6A Active CN108453727B (zh) | 2018-01-11 | 2018-01-11 | 基于椭圆特征的机械臂末端位姿误差校正方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108453727B (zh) |
Families Citing this family (22)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109176488B (zh) * | 2018-09-28 | 2021-09-10 | 哈尔滨工业大学(深圳) | 一种柔性机器人运动学标定方法及系统 |
CN109376405B (zh) * | 2018-09-29 | 2023-10-27 | 贵州华烽电器有限公司 | 一种飞行员操纵装置中传动链系间隙的计算方法 |
CN109648563B (zh) * | 2019-01-02 | 2021-10-29 | 南京邮电大学 | 串联机器人运动控制方法及计算机存储介质 |
CN110009680B (zh) * | 2019-02-28 | 2022-04-22 | 中国人民解放军国防科技大学 | 基于圆特征及异面特征点的单目图像位置、姿态测量方法 |
CN110116407B (zh) * | 2019-04-26 | 2021-03-30 | 哈尔滨工业大学(深圳) | 柔性机器人位姿测量方法及装置 |
CN110148187B (zh) * | 2019-06-04 | 2021-03-12 | 郑州大学 | 一种SCARA机械手Eye-in-Hand的高精度手眼标定方法和系统 |
CN110906863B (zh) * | 2019-10-30 | 2022-01-28 | 成都绝影智能科技有限公司 | 一种用于线结构光传感器的手眼标定系统及标定方法 |
CN114078158A (zh) * | 2020-08-14 | 2022-02-22 | 边辕视觉科技(上海)有限公司 | 一种自动化获取目标物体特征点参数的方法 |
CN111815634A (zh) * | 2020-09-09 | 2020-10-23 | 苏州浪潮智能科技有限公司 | 基于机器视觉内存对位接插方法与系统、设备、存储介质 |
CN112476489B (zh) * | 2020-11-13 | 2021-10-22 | 哈尔滨工业大学(深圳) | 基于自然特征的柔性机械臂同步测量方法及系统 |
CN112706164B (zh) * | 2020-12-18 | 2022-05-24 | 深圳市大富智慧健康科技有限公司 | 机械臂初始位姿自动矫正方法、装置、设备及存储介质 |
CN112837368B (zh) * | 2021-01-28 | 2023-03-24 | 中南大学 | 提高料面成像面积的高炉内窥镜安装位姿配置方法及系统 |
JP2022125537A (ja) * | 2021-02-17 | 2022-08-29 | セイコーエプソン株式会社 | 校正方法 |
CN113768630A (zh) * | 2021-08-06 | 2021-12-10 | 武汉中科医疗科技工业技术研究院有限公司 | 主手夹持机构、主手控制台、手术机器人及主从对齐方法 |
CN113787518B (zh) * | 2021-09-06 | 2022-11-11 | 武汉库柏特科技有限公司 | 一种机器人末端姿态控制方法、装置、设备及存储介质 |
CN113640726B (zh) * | 2021-10-19 | 2021-12-21 | 青岛杰瑞自动化有限公司 | 一种双轴磁强计的多方位椭圆拟合标定方法及系统 |
CN113985255A (zh) * | 2021-10-29 | 2022-01-28 | 北京航星科技有限公司 | 一种电路板静态测试系统及测试方法 |
CN115493499B (zh) * | 2021-12-30 | 2024-04-19 | 北京航天飞行控制中心 | 一种圆柱体或类圆柱体的装配方法及系统 |
CN115042175B (zh) * | 2022-06-10 | 2024-07-16 | 合肥工业大学 | 一种机器人机械臂末端姿态的调整方法 |
CN115416018B (zh) * | 2022-08-17 | 2024-03-15 | 雅客智慧(北京)科技有限公司 | 末端执行器形变补偿方法、装置、电子设备和存储介质 |
CN117123922A (zh) * | 2023-10-26 | 2023-11-28 | 上海方菱计算机软件有限公司 | 一种椭圆管寻中方法、系统、电子设备、存储介质 |
CN118438456B (zh) * | 2024-07-03 | 2024-09-03 | 杭州三坛医疗科技有限公司 | 一种姿态定位方法、装置、电子设备及存储介质 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2014184526A (ja) * | 2013-03-25 | 2014-10-02 | Canon Inc | ロボット装置、ロボット制御方法、プログラム及び記録媒体 |
CN104101331A (zh) * | 2014-07-24 | 2014-10-15 | 合肥工业大学 | 基于全光场相机的非合作目标位姿测量 |
CN104180808A (zh) * | 2014-08-05 | 2014-12-03 | 南京航空航天大学 | 一种用于自主空中加油的圆形锥套视觉位姿解算方法 |
CN105537824A (zh) * | 2016-01-27 | 2016-05-04 | 华南理工大学 | 一种基于机械臂手眼协调自主焊接控制方法 |
CN105643642A (zh) * | 2014-12-02 | 2016-06-08 | 宁夏巨能机器人系统有限公司 | 一种机器臂的定位方法 |
CN106737855A (zh) * | 2016-08-22 | 2017-05-31 | 南京理工大学 | 一种综合位姿误差模型与刚度补偿的机器人精度补偿方法 |
-
2018
- 2018-01-11 CN CN201810026683.6A patent/CN108453727B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2014184526A (ja) * | 2013-03-25 | 2014-10-02 | Canon Inc | ロボット装置、ロボット制御方法、プログラム及び記録媒体 |
CN104101331A (zh) * | 2014-07-24 | 2014-10-15 | 合肥工业大学 | 基于全光场相机的非合作目标位姿测量 |
CN104180808A (zh) * | 2014-08-05 | 2014-12-03 | 南京航空航天大学 | 一种用于自主空中加油的圆形锥套视觉位姿解算方法 |
CN105643642A (zh) * | 2014-12-02 | 2016-06-08 | 宁夏巨能机器人系统有限公司 | 一种机器臂的定位方法 |
CN105537824A (zh) * | 2016-01-27 | 2016-05-04 | 华南理工大学 | 一种基于机械臂手眼协调自主焊接控制方法 |
CN106737855A (zh) * | 2016-08-22 | 2017-05-31 | 南京理工大学 | 一种综合位姿误差模型与刚度补偿的机器人精度补偿方法 |
Also Published As
Publication number | Publication date |
---|---|
CN108453727A (zh) | 2018-08-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108453727B (zh) | 基于椭圆特征的机械臂末端位姿误差校正方法及系统 | |
CN108765489B (zh) | 一种基于组合靶标的位姿计算方法、系统、介质及设备 | |
Peng et al. | A pose measurement method of a space noncooperative target based on maximum outer contour recognition | |
CN109919984A (zh) | 一种基于局部特征描述子的点云自动配准方法 | |
CN111775146A (zh) | 一种工业机械臂多工位作业下的视觉对准方法 | |
CN110116407A (zh) | 柔性机器人位姿测量方法及装置 | |
CN110503713B (zh) | 一种基于轨迹平面法向量和圆心结合的旋转轴估计方法 | |
CN111383272B (zh) | 一种双目视觉水果分拣并联机器人视觉盲区末端位姿检测方法 | |
CN109115184B (zh) | 基于非合作目标协同测量方法及系统 | |
CN110308459B (zh) | 不依赖模型的非合作卫星相对位姿测量方法 | |
Guo et al. | Real-time measurement and estimation of the 3D geometry and motion parameters for spatially unknown moving targets | |
CN107167116A (zh) | 一种空间圆弧位姿的视觉检测方法 | |
CN115546289A (zh) | 一种基于机器人的复杂结构件三维形貌测量方法 | |
CN116309879A (zh) | 一种机器人辅助的多视角三维扫描测量方法 | |
CN110245367B (zh) | 光机集成分析面形误差处理方法 | |
Pokorny et al. | Grasp moduli spaces and spherical harmonics | |
CN114581515B (zh) | 一种基于最优路径转换的多相机标定参数优化方法 | |
Chen et al. | A new pose estimation method for non-cooperative spacecraft based on point cloud | |
Takeishi et al. | Simultaneous estimation of shape and motion of an asteroid for automatic navigation | |
CN111899303A (zh) | 一种新的考虑空间逆投影约束的特征匹配与相对定位方法 | |
Deng et al. | An improved posture evaluation method for cylindrical intersecting holes on large aerospace components based on monocular vision | |
Kong et al. | Online measurement method for assembly pose of gear structure based on monocular vision | |
Chen et al. | The trajectory planning system for spraying robot based on k-means clustering and NURBS curve optimization | |
Li et al. | Industrial Robot Hand–Eye Calibration Combining Data Augmentation and Actor-Critic Network | |
CN111366162B (zh) | 基于太阳帆板投影与模板匹配的小天体探测器位姿估计方法 |
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 |