一种6自由度机械手自主抓取逆解工程算法
【技术领域】
[0001] 本发明属于机器人控制技术领域，具体涉及一种6自由度机械手自主抓取逆解工 程算法。
【背景技术】
[0002] 随着科学技术不断发展，以及机器人取代人工的领域迅速增加，各类机器人的研 制已成为世界各国和军队共同关注的课题。由于机器人的作业目标质量状况不明，放置的 位置随机性较大，抓取过程中随时有意外发生的可能，因此，采用机器人进行作业，降低处 理难度，避免工作人员的意外发生，对提高科技整体技术水平和作业效率具有重要意义。
[0003] 世界上已有的车载机械手型机器人包括履带式"手推车"、"超级手推车" MPR-800 型多功能智能机器人、"安德鲁斯HD-I"机器人、"灵蜥A"和"灵蜥B"排爆机器人、 "RAPT0P-E0D"中型排爆机器人等。而控制系统是机器人中至关重要的一部分，控制系统的 核心是逆运动算法，其好坏程度直接影响着机器人的功能、可靠程度及操作性能等。
[0004] 针对机器人作业环境的特殊情况，可靠性和效率是它的一个重要因素。如果机器 人的控制系统不可靠，将会引入新的不安全因素，不但没能解决问题，反而会促发矛盾问题 的升级。在6自由度的机械手操作时，采用单关节的远程遥控方式，这对机械手末端手爪的 定位抓取造成了极大困难。该种方式对操作人员的训练程度要求高且造成的精神负担较 大，容易疲劳而且操作效率较低。如果采用逆矩阵求解方法，不仅计算复杂，而且计算过程 中多解可能性不断出现，必须靠约束条件去掉增根，且求解不直观。
【发明内容】
[0005] 本发明针对现有技术中机械手的控制系统采用单关节的遥控方式，导致机械手末 端定位抓取困难，造成操作人员的精神负担较大，容易疲劳而且操作效率较低等问题，提供 一种能够实现机械手多关节联动的逆运动求解工程算法。
[0006] 本发明通过以下技术方案实现该目的：
[0007] -种6自由度机械手自主抓取逆解工程算法，包括以下步骤：
[0008] (1)根据实际的目标图像判断抓取目标点（X，Y，Z)，且不考虑目标姿态；
[0009] (2)根据机械手的正解方法，设定6自由度机械手的各关节参数为 (^，9 2, 93, 94, 95, ％)，第4、5、6关节依靠人工调整，因此求出目标点与前3个关节的 关系，即固定后三个关节的初始角度；
[0010] (3)求逆解时先转腰，求出0 i，后面的求解工程为平面求解问题；
[0011] ⑷计算 02，03。
[0012] 进一步，所述步骤（1)中，目标点P(X，Y，Z)由人工设定或者由机器人视觉提供。
[0013] 进一步，所述步骤（2)中，不考虑最后3个关节的旋转，将目标点P(X，Y，Z)定位 于机械手爪中心。在机械手臂上固定一个平面坐标系Ixy}，这是为了将空间坐标系的问题 工程到平面坐标系解决，减少了变量，使得空间变量求解更加简单。该平面坐标系Ixy}的 原点与空间基坐标系{XYZ}的原点重合，即固定在机器人的基坐标系的原点；平面坐标系 {xy}的y轴与空间坐标系{XYZ}的Z轴重合。
[0014] 无论机械臂绕腰关节怎么旋转，坐标系{xy}始终与机械手处于同一平面内，该坐 标系是辅助的动态平面坐标系，那么点P在该坐标系Ixy}中的坐标记为（x，y)，根据几何 学知识可以求得该坐标如下：
[0015]
[0016] 最终要求得点P在机器人基坐标系{XYZ}中的坐标（X，Y，Z)，点P在平面动坐标系 Ixy}与机器人基坐标系{X，Y，Z}满足以下关系式
，所以点P在机器人基坐标 系下的坐标关
[0017] 进一步，所述步骤（3)、（4)中，已知机器人末端的三维坐标（X，Y，Z)的值，求解关 节9 1，9 2，9 3的值。
[0018] 在机器人基坐标系下可以解的0 i的表达式为：0 i=arctan(Y/X)(根据机械臂 的空间位置信息求取角度）；
[0019] 由正解的结论可得
[0020] X2+Y2+Z2=L 12+L22+2L1L 2(cos( 93-9 2)cos(92)_sin( 93-9 2)sin( 9 2)),
[0021 ] X2+Y2+Z2= L 12+L22+2L1L 2cos ( 9 3- 9 2~*~ ?2) = L2
[0022] 可得◎
[0023] 利用三角形的余弦定理可解得
f，根据实际可取
[0024] 将包含9 2, 9 3的y-0-x平面
[0025]
[0026]
[0027] 其中，求出0i，0 2, 0 3后，根据目标姿态，人工调整手爪姿态，实现合理位姿抓取。
[0028] 相对于现有技术，本发明的有益效果为：本发明的6自由度机械手自主抓取逆解 工程算法，在求取各关节角度时，采用先转腰且不考虑手爪姿态，再求取其他关节角度的方 法，先求出腰部回转角度，进而转化为平面问题进行求解其他关节角度，大大提高了求解效 率，并且避免了逆矩阵求解方式带来的复杂性，以及操作人员的精神负担较大、容易疲劳等 问题，进一步提高了抓取效率。
【附图说明】
[0029] 图1是本发明的末端不包括摆动关节的机械手结构示意图。
[0030] 图2是本发明的6自由度机械手运动学正解几何图解示意图。
[0031] 图3是本发明的工程成平面的关节参数求解示意图。
【具体实施方式】
[0032] 以下结合附图及具体实施例对本发明进行详细描述。
[0033] 实施例1。
[0034] 一种6自由度机械手自主抓取逆解工程算法包括以下步骤：
[0035] (1)根据实际的目标图像判断抓取目标点（X，Y，Z)，且不考虑目标姿态；
[0036] (2)根据机械手的正解方法，设定6自由度机械手的各关节参数为 (^，9 2, 93, 94, 95, ％)，第4、5、6关节依靠人工调整，因此求出目标点与前3个关节的 关系，即固定后三个关节的初始角度；
[0037] (3)求逆解时先转腰，求出0 i，后面的求解工程为平面求解问题；
[0038] (4)计算 02,03。
[0039] 进一步，所述步骤（1)中目标点P(X，Y，Z)由人工设定或者由机器人视觉提供。
[0040] 进一步，所述步骤（2)中，不考虑最后3个关节的旋转，将目标点P(X，Y，Z)定位手 爪中心，如图1所示。在机械手臂上固定一个平面坐标系{xy}，这是为了将空间坐标系的问 题工程到平面坐标系解决，减少了变量，使得空间变量求解更加简单。该平面坐标系Ixy} 的原点与空间基坐标系{XYZ}的原点重合，即固定在机器人的基坐标系的原点；平面坐标 系{xy}的y轴与空间坐标系{XYZ}的Z轴重合，坐标系的关系如图2所示。
[0041] 无论机械臂绕腰关节怎么旋转，坐标系Ixy}始终与机械手处于同一平面内，该坐 标系是辅助的动态平面坐标系，那么点P在该坐标系Ixy}中的坐标记为（x，y)，根据几何 学知识可以求得该坐标如下：
[0042]
[0043] 最终要求得点P在机器人基坐标系{XYZ}中的坐标（X，Y，Z)，点P在平面动坐标系 X =^cos：0j Ixy}与机器人基坐标系{X，Y，Z}满足以下关系式贫，所以点P在机器人基坐标 Z. - y 系下的坐标7
[0044] 进一步，所述步骤（3):
[0045] 已知机器人末端的三维坐标（X，Y，Z)的值，求解关节0 d 0 2, 0 3的值，多功能水 下爬行机器人运动学逆解几何图解图如图1所示。
[0046] 在机器人基坐标系下可以解的0 :的表达式0 arctan(Y/X)(根据机械臂的空 间位置信息求取角度）；
[0047] 由正解的结论可得
[0048] X2+Y2+Z2= L 12+L22+2L1L 2(cos( 93-9 2) cos (92) _sin ( 9 3- 9 2) sin (92))
[0049] ，
[0050] X2+Y2+Z2= L 12+L22+2L1L 2cos ( 9 3- 9 2~*~ ? 2) = L2
[0056] 其中，求出0i，0 2, 0 3后，根据目标姿态，人工调整手爪姿态，实现合理位姿抓取。
[0057] 以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并 不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员 来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保 护范围。因此，本发明专利的保护范围应以所附权利要求为准。
【主权项】
1. 一种6自由度机械手自主抓取逆解工程算法，其特征在于，包括W下步骤： (1)根据实际的目标图像判断抓取目标点P狂，Y，幻，且不考虑目标姿态； 似根据机械手的正解方法，设定6自由度机械手的各关节参数为 (0 1，0 2, 0 3, 0 4, 0 5, 0 6)，第4、5、6关节依靠人工调整，因此求出目标点P化Y，口与前3 个关节的关系，即固定后=个关节的初始角度； (3) 求逆解时先转腰，求出0 1的数值，后面的求解工程为平面求解问题； (4) 计算02,03的数值。2. 根据权利要求1所述的6自由度机械手自主抓取逆解工程算法，其特征在于，所述步 骤（1)中目标点P狂，Y，幻由人工设定或者由机器人视觉提供。3. 根据权利要求1所述的6自由度机械手自主抓取逆解工程算法，其特征在于，所述 步骤（2)中，不考虑最后3个关节的旋转，将目标点P〇(，Y，幻定位于机械手爪中屯、，在机械 手臂上固定一个平面坐标系Ixy}，该平面坐标系{xy}的原点与空间基坐标系找Y幻的原点 重合，即固定在机器人的基坐标系的原点；平面坐标系Ixy}的y轴与空间坐标系找Y幻的 Z轴重合； 无论机械臂绕腰关节怎么旋转，坐标系Ixy}始终与机械手处于同一平面内，该坐标系 是辅助的动态平面坐标系，那么点P在该坐标系Ixy}中的坐标记为（x，y)，求得该坐标如 下：最终要求得点P在机器人基坐标系找Y幻中的坐标狂，Y，Z)，点P在平面动坐标系{xy} 与机器人基坐标系化Y，幻满足W下关系式斤W点P在机器人基坐标系下 的坐标为4. 根据权利要求1所述的6自由度机械手自主抓取逆解工程算法，其特征在于，所述步 骤（3)中，已知机器人末端的S维坐标化Y，幻的值，求解关节0 1，0 2, 9 3的值，根据机械 臂的空间位置信息求取角度，在机器人基坐标系下可W求得e1的表达式为： 白1=arctan(Y/X)； 由正解的结论可得0 2,e3的关系式为：5. 根据权利要求1所述的6自由度机械手自主抓取逆解工程算法，其特征在于，求出 0 1，0 2, 0 3后，根据目标姿态，人工调整手爪姿态，实现合理位姿抓取。6. 根据权利要求4所述的6自由度机械手自主抓取逆解工程算法，其特征在于，计算 0 2,e3不使用逆矩阵求解方法，采用先转腰且不考虑手爪姿态，再求取其他关节角度的方 法。
【专利摘要】本发明涉及一种6自由度机械手求逆解算法，在求取各关节角度时，先求出腰部回转角度，进而转化为平面问题，有利于求解效率效率，并且避免了逆矩阵求解方式带来的复杂性，以及操作人员的精神负担较大、容易疲劳等问题，进一步提高了抓取效率。此外，本发明中的目标位置采用双目视觉摄像单元获取或者人工判断，以供上位机控制系统确定所述可疑目标相对于所述车体的三维坐标，并反馈回相应的控制指令，可以进一步实现对目标的精确定位及抓取，从而也进一步提高了抓取效率。
【IPC分类】G06F17/50
【公开号】CN105005656
【申请号】CN201510408520
【发明人】蒋梁中, 张 成
【申请人】广州霞光技研有限公司
【公开日】2015年10月28日
【申请日】2015年7月13日