CN108481324B - 一种八轴多功能机械臂的逆解工程及其碰撞检测算法 - Google Patents

一种八轴多功能机械臂的逆解工程及其碰撞检测算法 Download PDF

Info

Publication number
CN108481324B
CN108481324B CN201810378543.5A CN201810378543A CN108481324B CN 108481324 B CN108481324 B CN 108481324B CN 201810378543 A CN201810378543 A CN 201810378543A CN 108481324 B CN108481324 B CN 108481324B
Authority
CN
China
Prior art keywords
joint
inverse solution
algorithm
mechanical arm
attitude
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
Application number
CN201810378543.5A
Other languages
English (en)
Other versions
CN108481324A (zh
Inventor
燕胜
宋云涛
程勇
史善爽
熊宇
杨松竹
潘洪涛
郝志伟
李阳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hefei Institutes of Physical Science of CAS
Original Assignee
Hefei Institutes of Physical Science of CAS
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Hefei Institutes of Physical Science of CAS filed Critical Hefei Institutes of Physical Science of CAS
Priority to CN201810378543.5A priority Critical patent/CN108481324B/zh
Publication of CN108481324A publication Critical patent/CN108481324A/zh
Application granted granted Critical
Publication of CN108481324B publication Critical patent/CN108481324B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/1602—Programme controls characterised by the control system, structure, architecture
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/0095—Means or methods for testing manipulators

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

本发明公开了一种八轴多功能机械臂的逆解工程及其碰撞检测算法，包括位置精度逆解算法、方位角逆解算法、碰撞检测算法三个部分。方位角逆解算法根据目标姿态Z轴方向向量及空间位置坐标求解第七关节空间位置坐标，以及验证位置精度逆解算法得到的前段关节逆解，并求解出满足末端目标姿态的后段关节逆解。位置精度逆解算法通过关节之间的约束，简化逆解求解过程。碰撞检测算法通过简化多功能机械臂的特殊工作空间，提高逆解工程的运算效率，该算法嵌入到位置精度逆解算法，验证前段六个关节的空间合理性，以及全关节逆解的空间合理性。本发明降低了八轴机械臂逆解工程的运算量，提高了运算效率，提高了逆解空间合理性的验证效率。

Description

一种八轴多功能机械臂的逆解工程及其碰撞检测算法
技术领域
本发明涉及机械臂运动算法领域，具体是一种八轴多功能机械臂的逆解工程及其碰撞检测算法。
背景技术
在中国热核聚变反应堆工程的常规维护中，八轴多功能机械臂扮演着重要的角色，承担着装置维护的重大责任。为了争取到更多的聚变反应实验时间，需要尽可能快的以及尽可能好的完成日常维护作业。多功能机械臂的末端搭载着末端执行器，是反应堆维护工作的直接操作者。为了保证维护过程中的精度控制，需要多功能机械臂保证一个精确地末端定位精度，为末端执行器提供一个精确定位的支撑平台。
八轴机械臂的运动学逆解是一个计算量大且复杂的过程，需要根据实际运动情况进行简化及解的筛选。通过简化和筛选才能尽快的得到符合空间要求及机械臂构型的运动学逆解。
发明内容
本发明的目的是提供一种逆解工程及其碰撞检测算法，尤其是涉及一种八轴多功能机械臂的逆解工程及其碰撞检测算法。
为了达到上述目的，本发明所采用的技术方案为：
一种八轴多功能机械臂的逆解工程及其碰撞检测算法，其特征在于：包括方位角逆解算法、位置精度逆解算法、碰撞检测算法，其中：
(1)、方位角逆解算法其过程如下：
(1.1)、目标姿态位置矩阵TEND由姿态矩阵REND和位置矢量PEND组成，其中
Figure BDA0001640412820000011
RIND＝[xEND yEND zEND]，
Figure BDA0001640412820000012
则第七关节空间位置坐标 PV＝PEND-(dV+dg)*zEND；
其中，px、py、pz、指该点在空间中的三维坐标；d7、d8是建立D-H坐标系过程中产生的对应的参数，根据机器人关节坐标系的建立得出，d1-d8参数表如表1所示：
表1 D-H坐标系过程中d1-d8参数
Figure BDA0001640412820000021
(1.2)、将第七关节空间位置坐标P7导入位置精度逆解算法，可以求出前段关节逆解Pos1To5＝[d1 θ2 θ3 θ4 θ5]，通过碰撞检测算法验证前段关节逆解的可行性，得到满足要求的前段关节逆解 Pos1To5；其中d1建立D-H坐标系过程中产生的对应的参数；θ2-θ5 表示的是八轴多功能机械臂中第二-第五关节的转动变量，这些编号对应的是关节坐标系的下标，转动方向为绕着各个坐标系的Z轴转动；
(1.3)、通过关节逆解Pos1To5＝[Pos1To5 0]得到第六关节的姿态位置矩阵
Figure BDA0001640412820000022
根据D-H法建立的关节坐标系，满足P6＝P7，θ7＝arccos(z6*zEND/(|z6|*|zEND|))，其中z6为第六关节Z轴方向向量，R6指的是第六关节的坐标转换后的方向矩阵、P6指的则是转换后的位置向量，P7指的是经过前7个关节坐标系转换之后的坐标中心点的位置向量，Zend指的是机器人末端点目标姿态的Z轴向量，θ7是八轴多功能机械臂中第七关节的转动变量；
(1.4)、由第六关节Z轴方向向量z6和目标姿态Z轴方向向量 zEND得到法向量Vpubllc1，方向符合右手准则；
(1.5)、通过第六关节Y轴方向向量y6与法向量Vpubllc1的夹角θjud＝arccos(Z6*ZEND/(|Z6|*|ZEND|))来判定θ6的值：
Figure BDA0001640412820000031
θ6是八轴多功能机械臂中第六关节的转动变量；
(1.6)、通过前段关节逆解Pos1To7＝[di θ2 θ3 θ4 θ5 θ6 θ7] 得到第七关节的姿态位置矩阵
Figure BDA0001640412820000032
其中第七关节X轴方向向量x7和目标姿态X轴方向向量xEND，得到法向量Vpubllc2，方向符合右手准则，通过 Vpubllc2与zEND的方向关系判定θ8的值：
Figure BDA0001640412820000041
其中x7为X轴方向向量，R7为第七关节的坐标转换后的方向矩阵， P7为经过前7个关节坐标系转换之后的坐标中心点的位置向量，θ8 是八轴多功能机械臂中第八关节的转动变量；
(2)、位置精度逆解算法其过程如下：
(2.1)、根据前三个关节的位置相互约束关系进行逆解算法的简化：
Figure BDA0001640412820000042
Figure BDA0001640412820000043
由d1划分三种情况：d1∈[0，2705)，d1∈[2705，4455)，d1＝4455，分别对应三个子算法：全臂约束逆解算法、半臂约束逆解算法、平动约束逆解算法；
(2.2)、通过方位角逆解算法得到第七关节空间位置坐标P7，按照d1递增的顺序进行逆解，依次运行全臂约束逆解算法、半臂约束逆解算法、平动约束逆解算法，在上一个算法无法求得解的情况下运行下一个算法，如果得到解，则运行方位角逆解算法，求得最终的解；
(2.3)、运行全臂约束逆解算法，输入关节变量 Pos1To6＝[d1 π/2-π/2 θ4 θ5 0]得到第六关节的姿态位置矩阵
Figure BDA0001640412820000051
其中
Figure BDA0001640412820000052
且P6＝P7，通过方程联立求解得到d1，θ4，θ5，然后判定d1是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；
(2.4)、运行半臂约束逆解算法，输入关节变量 Pos1To5＝[d1 π/2 θ3 θ4 θ5 0]得到第六关节的姿态位置矩阵
Figure BDA0001640412820000053
其中
Figure BDA0001640412820000054
且P6＝P7，由于机械臂存在冗余自由度，为了降低逆解求解过程运算量，间隔5°遍历θ3∈[-π，0]，再通过方程联立求解得到d1，θ4，θ5，然后判定d1是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；
(2.5)、运行平动约束逆解算法，输入关节变量 Pos1To5＝[4455 θ2 θ3 θ4 θ5 0]得到第六关节的姿态位置矩阵
Figure BDA0001640412820000055
其中
Figure BDA0001640412820000056
且P6＝P7，由于机械臂存在冗余自由度，为了降低逆解求解过程运算量，间隔5°遍历θ5∈[-π，0]，由pz(θ4,θ5)得到θ4，再通过方程联立求解得到d1，θ2，θ3，然后判定d1是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；
(3)、碰撞检测算法其过程如下：
(3.1)、简化机械臂环状工作空间，空间切面折线以极坐标方程曲线代替，极点位于空间切面折线的中心位置，该位置由待检测点决定，环状臂 极坐标方程：
l(θ)＝(0.000277500372481*θ10-0.008523502454628*θ9
+0.106922940971428*θ9-0.696566524761767
*θ7+2.447743902396733*θ6
-4.160347429809979*θ5+1.503776922304815
*θ4+3.856518561349923*θ3
-3.237876202808929*θ2+1.010407153490720
*θ1+1.623553179189534)*(1.0e+03)
安全域极坐标方程：lsafe(θ)＝0.62*l(θ)；
(3.2)、根据关节变量求得各关节的空间位置坐标：
Figure BDA0001640412820000061
在Pi与Pi+1之间等间距取100个待检测点，
Figure BDA0001640412820000062
Figure BDA0001640412820000063
(3.3)、根据i遍历多个指定关节，根据k遍历临近指定关节之间的待监测点，待检测点与环状空间中心轴组成平面T1，相对环状空间中心轴与机械臂基座标原点组成的竖直平面T2，确定T1与T2的角度θT1T2，极点Ppolεr相对环状空间中心轴坐标系的位置坐标(center_x、center_y、center_z)：
center_x＝5738.4*cos(theta_center_axis)；
center_y＝5738.4*sin(theta_center_axis)；
center_z＝0
待检测点
Figure BDA0001640412820000071
相对极点Ppolar的空间向量
Figure BDA0001640412820000072
得到极坐标半径
Figure BDA0001640412820000073
和与xoy平面的夹角即极角
Figure BDA0001640412820000074
导入安全域极坐标方程：
Figure BDA0001640412820000075
在
Figure BDA0001640412820000076
的情况下，待检测点
Figure BDA0001640412820000077
通过碰撞检测。
本发明通过八轴多功能机械臂的逆解工程及碰撞检测算法通过方位角逆解算法降低了八轴机械臂逆解工程的运算量，位置精度逆解算法通过拆解复杂情况提高了运算效率，碰撞检测算法通过特殊空间曲线拟合提高了逆解空间合理性的验证效率。
附图说明
图1为本发明的八轴多功能机械臂的关节坐标系轴侧方向示意图；
图2为本发明的机械臂工作示意图的轴侧方向示意图；
图3为本发明的机械臂工作示意图的左侧方向示意图；
图4为本发明的机械臂工作示意图的俯视方向示意图；
图5为本发明的避障算法的截面折线的拟合曲线示意图；
图6为本发明的避障算法的安全曲线示意图；
图7为本发明的目标工作装置环境剖视图；
图8为本发明的目标工作装置截面坐标系示意图；
图9为本发明的八轴多功能机械臂的工作示意图；
图10为本发明的八轴多功能机械臂的轴侧方向示意图；
图11为本发明的算法程序流程图；
图中标号：1、切面拟合曲线极坐标原点；2、机械臂基座标原点；3、目标工作装置切面采集点；4、目标工作装置切面简化折线；5、目标工作装置；6、机械臂基座；7、八轴机械臂；8、末端执行器。
具体实施方式
如图11所示，一种八轴多功能机械臂的逆解工程及其碰撞检测算法，包括方位角逆解算法、位置精度逆解算法、碰撞检测算法，如图1、图2、图3、图4 所示，本发明首先根据D-H法建立八轴机械臂的关节坐标系。图8表示了极坐标系与机器人极坐标系的相对位置关系。
如图7、图9、图10，表示了机器人及该机器人应用的装置，八轴机械臂7 安装在机械臂基座6上，末端执行器8安装在八轴机械臂7的末端，机械臂基座 6实现与大装置的连接，其中：
(1)、方位角逆解算法其过程如下：
(1.1)、目标姿态位置矩阵TEND由姿态矩阵REND和位置矢量PEND组成，其中
Figure BDA0001640412820000081
RIND＝[xEND yEND zEND，
Figure BDA0001640412820000082
则第七关节空间位置坐标 PV＝PEND-(dV+dg)*zEND；
其中，px、py、pz、指该点在空间中的三维坐标；d7、d8是建立D-H坐标系过程中产生的对应的参数，根据机器人关节坐标系的建立得出，d1-d8参数表如表1所示：
表1 D-H坐标系过程中d1-d8参数
Figure BDA0001640412820000083
Figure BDA0001640412820000091
(1.2)、将第七关节空间位置坐标P7导入位置精度逆解算法，可以求出前段关节逆解Pos1To5＝[d1 θ2 θ3 θ4 θ5]，通过碰撞检测算法验证前段关节逆解的可行性，得到满足要求的前段关节逆解 Pos1To5；其中d1建立D-H坐标系过程中产生的对应的参数；θ2-θ5 表示的是八轴多功能机械臂中第二-第五关节的转动变量，这些编号对应的是关节坐标系的下标，转动方向为绕着各个坐标系的Z轴转动；
(1.3)、通过关节逆解Pos1To5＝[Pos1To5 0]得到第六关节的姿态位置矩阵
Figure BDA0001640412820000092
根据D-H法建立的关节坐标系，满足P6＝P7，θ7＝arccos(z6*zEND/(|z6|*|zEND|))，其中z6为第六关节Z轴方向向量，zEND为目标姿态Z轴方向向量；
其中z6为第六关节Z轴方向向量，R6指的是第六关节的坐标转换后的方向矩阵、P6指的则是转换后的位置向量，P7指的是经过前7 个关节坐标系转换之后的坐标中心点的位置向量，Zend指的是机器人末端点目标姿态的Z轴向量，θ7是八轴多功能机械臂中第七关节的转动变量；
(1.4)、由第六关节Z轴方向向量z6和目标姿态Z轴方向向量 zEND得到法向量Vpublic1，方向符合右手准则；
(1.5)、通过第六关节Y轴方向向量y6与法向量Vpublic1的夹角θjud＝arccos(z6*zEND/(|z6|*|zEND|))来判定θ6的值：
Figure BDA0001640412820000101
θ6是八轴多功能机械臂中第六关节的转动变量；
(1.6)、通过前段关节逆解Pos1To7＝[d1 θ2 θ3 θ4 θ5 θ6 θ7] 得到第七关节的姿态位置矩阵
Figure BDA0001640412820000102
其中第七关节X轴方向向量x7和目标姿态X轴方向向量xEND，得到法向量Vpublic2，方向符合右手准则，通过 Vpublic2与zEND的方向关系判定θ8的值：
Figure BDA0001640412820000111
其中x7为X轴方向向量，R7为第七关节的坐标转换后的方向矩阵， P7为经过前7个关节坐标系转换之后的坐标中心点的位置向量，θ8 是八轴多功能机械臂中第八关节的转动变量；
(2)、位置精度逆解算法其过程如下：
(2.1)、根据前三个关节的位置相互约束关系进行逆解算法的简化：
Figure BDA0001640412820000112
Figure BDA0001640412820000113
由d1划分三种情况：d1∈[0，2705)，d1∈[2705，4455)，d1＝4455，分别对应三个子算法：全臂约束逆解算法、半臂约束逆解算法、平动约束逆解算法；
(2.2)、通过方位角逆解算法得到第七关节空间位置坐标P7，按照d1递增的顺序进行逆解，依次运行全臂约束逆解算法、半臂约束逆解算法、平动约束逆解算法，在上一个算法无法求得解的情况下运行下一个算法，如果得到解，则运行方位角逆解算法，求得最终的解；
(2.3)、运行全臂约束逆解算法，输入关节变量 Pos1To6＝[d1 π/2 -π/2 θ4 θ5 0]得到第六关节的姿态位置矩阵
Figure BDA0001640412820000121
其中
Figure BDA0001640412820000122
且P6＝P7，通过方程联立求解得到d1，θ4，θ5，然后判定d1是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；
(2.4)、运行半臂约束逆解算法，输入关节变量 Pos1To5＝[d1 π/2 θ3 θ4 θ5 0]得到第六关节的姿态位置矩阵
Figure BDA0001640412820000123
其中
Figure BDA0001640412820000124
且P6＝P7，由于机械臂存在冗余自由度，为了降低逆解求解过程运算量，间隔5°遍历θ3∈[-π，0]，再通过方程联立求解得到d1，θ4，θ5，然后判定d1是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；
(2.5)、运行平动约束逆解算法，输入关节变量 Pos1To5＝[4455 θ2 θ3 θ4 θ5 0]得到第六关节的姿态位置矩阵
Figure BDA0001640412820000125
其中
Figure BDA0001640412820000126
且P6＝P7，由于机械臂存在冗余自由度，为了降低逆解求解过程运算量，间隔5°遍历θ5∈[-π，0]，由pz(θ4，θ5)得到θ4，再通过方程联立求解得到d1，θ2，θ3，然后判定d1是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；
(3)、碰撞检测算法其过程如下：
(3.1)、a)如图6的out-curve所示，为简化后的工作空间截面，图5的o ut-curve所示，为简化后的工作空间截面的折线图(极角-极坐标半径)，对折线图进行曲线拟合，得到图5的fitting-curve，对应图6的fitting-curve。简化机械臂环状工作空间，空间切面折线以极坐标方程曲线(图6的 fitting-curve)代替，然后进行极坐标半径的缩放，缩放比例为0. 62，如图6的inner-curve，极点位于空间切面折线的中心位置，该位置由待检测点决定，环状臂 (图6的fitting-curve)极坐标方程： l(θ)＝(0.000277500372481*θ10-0.008523502454628*θ9
+0.106922940971428*θ9-0.696566524761767
*θ7+2.447743902396733*θ6
-4.160347429809979*θ5+1.503776922304815
*θ4+3.856518561349923*θ3
-3.237876202808929*θ2+1.010407153490720
*θ1+1.623553179189534)*(1.0e+03)
安全域极坐标方程：lsafe(θ)＝0.62*l(θ)；
(3.2)、根据关节变量求得各关节的空间位置坐标：
Figure BDA0001640412820000131
在Pi与Pi+1之间等间距取100个待检测点，
Figure BDA0001640412820000132
Figure BDA0001640412820000133
(3.3)、根据i遍历多个指定关节，根据k遍历临近指定关节之间的待监测点，待检测点与环状空间中心轴组成平面T1，相对环状空间中心轴与机械臂基座标原点组成的竖直平面T2，确定T1与T2的角度θT1T2，极点Ppolεr相对环状空间中心轴坐标系的位置坐标(center_x、center_y、center_z)：
center_x＝5738.4*cos(theta_center_axis)；
center_y＝5738.4*sin(theta_center_axis)；
center_z＝0
待检测点
Figure BDA0001640412820000141
相对极点Ppolar的空间向量
Figure BDA0001640412820000142
得到极坐标半径
Figure BDA0001640412820000143
和与xoy平面的夹角即极角
Figure BDA0001640412820000144
导入安全域极坐标方程：
Figure BDA0001640412820000145
在
Figure BDA0001640412820000146
的情况下，待检测点
Figure BDA0001640412820000147
通过碰撞检测。
如图9所示为八轴多功能机械臂的工作示意图，搭载前端执行器8为目标工作装置开展维护作业。
本发明通过八轴多功能机械臂的逆解工程及碰撞检测算法通过方位角逆解算法降低了八轴机械臂逆解工程的运算量，位置精度逆解算法通过拆解复杂情况提高了运算效率，碰撞检测算法通过特殊空间曲线拟合提高了逆解空间合理性的验证效率。
以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是本发明的原理，在不脱离本发明精神和范围的前提下本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明的范围内。本发明要求的保护范围由所附的权利要求书及其等同物界定。

Claims (1)

1.一种八轴多功能机械臂的逆解工程及其碰撞检测算法，其特征在于：包括方位角逆解算法、位置精度逆解算法、碰撞检测算法，其中：
(1)、方位角逆解算法其过程如下：
(1.1)、目标姿态位置矩阵TEND由姿态矩阵REND和位置矢量PEND组成，其中
Figure FDA0002934827370000011
Figure FDA0002934827370000012
REND＝[xEND yEND zEND]，
Figure FDA0002934827370000013
则第七关节空间位置坐标P7＝PEND-(d7+d8)*zEND；
其中，px、py、pz指该点在空间中的三维坐标；d7、d8是建立D-H坐标系过程中产生的对应的参数，根据机器人关节坐标系的建立得出；d1-d8参数表如表1所示：
表1 D-H坐标系过程中d1-d8参数
Figure FDA0002934827370000014
(1.2)、将第七关节空间位置坐标P7导入位置精度逆解算法，可以求出前段关节逆解Pos1To5＝[d1 θ2 θ3 θ4 θ5]，通过碰撞检测算法验证前段关节逆解的可行性，得到满足要求的前段关节逆解Pos1To5；其中d1建立D-H坐标系过程中产生的对应的参数；θ2-θ5表示的是八轴多功能机械臂中第二-第五关节的转动变量，这些编号对应的是关节坐标系的下标，转动方向为绕着各个坐标系的Z轴转动；
(1.3)、通过关节逆解Pos1To5＝[Pos1To5 0]得到第六关节的姿态位置矩阵
Figure FDA0002934827370000015
根据D-H法建立的关节坐标系，满足P6＝P7，θ7＝arccos(z6*zEND/(|z6|*|zEND|))，其中z6为第六关节Z轴方向向量，R6指的是第六关节的坐标转换后的方向矩阵、P6指的则是转换后的位置向量，P7指的是经过前7个关节坐标系转换之后的坐标中心点的位置向量，Zend指的是机器人末端点目标姿态的Z轴向量，θ7是八轴多功能机械臂中第七关节的转动变量；
(1.4)、由第六关节Z轴方向向量z6和目标姿态Z轴方向向量zEND得到法向量Vpublic1，方向符合右手准则；
(1.5)、通过第六关节Y轴方向向量y6与法向量Vpublic1的夹角θjud＝arccos(z6*zEND/(|z6|*|zEND|))来判定θ6的值：
Figure FDA0002934827370000021
θ6是八轴多功能机械臂中第六关节的转动变量；
(1.6)、通过前段关节逆解Pos1To7＝[d1 θ2 θ3 θ4 θ5 θ6 θ7]得到第七关节的姿态位置矩阵
Figure FDA0002934827370000022
其中第七关节X轴方向向量x7和目标姿态X轴方向向量xEND，得到法向量Vpublic2，方向符合右手准则，通过Vpublic2与zEND的方向关系判定θ8的值：
Figure DEST_PATH_FDA0001640412810000031
其中x7为X轴方向向量，R7为第七关节的坐标转换后的方向矩阵，P7为经过前7个关节坐标系转换之后的坐标中心点的位置向量，θ8是八轴多功能机械臂中第八关节的转动变量；
(2)、位置精度逆解算法其过程如下：
(2.1)、根据前三个关节的位置相互约束关系进行逆解算法的简化：
Figure FDA0002934827370000024
Figure FDA0002934827370000025
由d1划分三种情况：d1∈[0，2705)，d1∈[2705，4455)，d1＝4455，分别对应三个子算法：全臂约束逆解算法、半臂约束逆解算法、平动约束逆解算法；
(2.2)、通过方位角逆解算法得到第七关节空间位置坐标P7，按照d1递增的顺序进行逆解，依次运行全臂约束逆解算法、半臂约束逆解算法、平动约束逆解算法，在上一个算法无法求得解的情况下运行下一个算法，如果得到解，则运行方位角逆解算法，求得最终的解；
(2.3)、运行全臂约束逆解算法，输入关节变量Pos1To6＝[d1 π/2 -π/2 θ4 θ5 0]得到第六关节的姿态位置矩阵
Figure FDA0002934827370000031
其中
Figure FDA0002934827370000032
且P6＝P7，通过方程联立求解得到d1，θ4，θ5，然后判定d1是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；
(2.4)、运行半臂约束逆解算法，输入关节变量Pos1To5＝[d1 π/2 θ3 θ4 θ5 0]得到第六关节的姿态位置矩阵
Figure FDA0002934827370000033
其中
Figure FDA0002934827370000034
且P6＝P7，由于机械臂存在冗余自由度，为了降低逆解求解过程运算量，间隔5°遍历θ3∈[-π，0]，再通过方程联立求解得到d1，θ4，θ5，然后判定d1是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；
(2.5)、运行平动约束逆解算法，输入关节变量Pos1To5＝[4455 θ2 θ3 θ4 θ5 0]得到第六关节的姿态位置矩阵
Figure FDA0002934827370000035
其中
Figure FDA0002934827370000036
Figure FDA0002934827370000037
且P6＝P7，由于机械臂存在冗余自由度，为了降低逆解求解过程运算量，间隔5°遍历θ5∈[-π，0]，由pz(θ4，θ5)得到θ4，再通过方程联立求解得到d1，θ2，θ3，然后判定d1是否在算法初始条件范围内，判定通过后导入碰撞检测算法，验证机械臂姿态的空间可行性；
(3)、碰撞检测算法其过程如下：
(3.1)、简化机械臂环状工作空间，空间切面折线以极坐标方程曲线代替，极点位于空间切面折线的中心位置，该位置由待检测点决定，环状臂极坐标方程：
l(θ)＝(0.000277500372481*θ10-0.008523502454628*θ9+0.106922940971428*θ8-0.696566524761767*θ7+2.447743902396733*θ6-4.160347429809979*θ5+1.503776922304815*θ4+3.856518561349923*θ3-3.237876202808929*θ2+1.010407153490720*θ1+1.623553179189534)*(1.0e+03)
安全域极坐标方程：lsafe(θ)＝0.62*l(θ)；
(3.2)、根据关节变量求得各关节的空间位置坐标：
Figure FDA0002934827370000041
在Pi与Pi+1之间等间距取100个待检测点，
Figure FDA0002934827370000042
Figure FDA0002934827370000043
(3.3)、根据i遍历多个指定关节，根据k遍历临近指定关节之间的待监测点，待检测点与环状空间中心轴组成平面T1，相对环状空间中心轴与机械臂基座标原点组成的竖直平面T2，确定T1与T2的角度θT1T2，极点Ppolar相对环状空间中心轴坐标系的位置坐标(center_x、center_y、center_z)：
center_x＝5738.4*cos(theta_center_axis)；
center_y＝5738.4*sin(theta_center_axis)；
center_z＝0
待检测点
Figure FDA0002934827370000044
相对极点Ppolar的空间向量
Figure FDA0002934827370000045
得到极坐标半径
Figure FDA0002934827370000046
和与xoy平面的夹角即极角
Figure FDA0002934827370000047
导入安全域极坐标方程：
Figure FDA0002934827370000048
在
Figure FDA0002934827370000049
的情况下，待检测点
Figure FDA00029348273700000410
通过碰撞检测。
CN201810378543.5A 2018-04-25 2018-04-25 一种八轴多功能机械臂的逆解工程及其碰撞检测算法 Active CN108481324B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810378543.5A CN108481324B (zh) 2018-04-25 2018-04-25 一种八轴多功能机械臂的逆解工程及其碰撞检测算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810378543.5A CN108481324B (zh) 2018-04-25 2018-04-25 一种八轴多功能机械臂的逆解工程及其碰撞检测算法

Publications (2)

Publication Number Publication Date
CN108481324A CN108481324A (zh) 2018-09-04
CN108481324B true CN108481324B (zh) 2021-04-20

Family

ID=63313030

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810378543.5A Active CN108481324B (zh) 2018-04-25 2018-04-25 一种八轴多功能机械臂的逆解工程及其碰撞检测算法

Country Status (1)

Country Link
CN (1) CN108481324B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111844005B (zh) * 2020-07-08 2022-06-28 哈尔滨工业大学 一种应用于隧道湿喷的2r-p-2r-p-2r机械臂运动规划方法
CN113119131B (zh) * 2021-05-08 2022-08-16 北京壹点灵动科技有限公司 机器人的控制方法、装置、计算机可读存储介质及处理器
WO2023274000A1 (zh) * 2021-06-29 2023-01-05 武汉联影智融医疗科技有限公司 一种机器人系统及其评价方法、控制方法
CN116922384A (zh) * 2023-07-21 2023-10-24 北京纳通医用机器人科技有限公司 机械臂控制方法、装置、设备及存储介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB2417090A (en) * 2003-04-28 2006-02-15 Stephen James Crampton CMM arm with exoskeleton
JP4498061B2 (ja) * 2004-08-18 2010-07-07 株式会社ダイヘン 溶接ロボット制御装置
KR101278836B1 (ko) * 2011-12-12 2013-07-01 인하대학교 산학협력단 인간형 관절 구성의 ６축 로봇 팔 제어 장치 및 방법
CN103499922A (zh) * 2013-09-16 2014-01-08 北京邮电大学 一种基于fpga的七自由度空间机械臂运动学实时解算方法
CN204498793U (zh) * 2015-01-23 2015-07-29 桂林电子科技大学 一种用于检测果实成熟度及采摘的柔性机械手
CN106845037A (zh) * 2017-03-21 2017-06-13 山东科技大学 一种五自由度串联机器人的逆运动学通用求解方法
CN107729637A (zh) * 2017-10-09 2018-02-23 燕山大学 基于臂形角区间的冗余自由度机械臂运动规划及评价方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB2417090A (en) * 2003-04-28 2006-02-15 Stephen James Crampton CMM arm with exoskeleton
JP4498061B2 (ja) * 2004-08-18 2010-07-07 株式会社ダイヘン 溶接ロボット制御装置
KR101278836B1 (ko) * 2011-12-12 2013-07-01 인하대학교 산학협력단 인간형 관절 구성의 ６축 로봇 팔 제어 장치 및 방법
CN103499922A (zh) * 2013-09-16 2014-01-08 北京邮电大学 一种基于fpga的七自由度空间机械臂运动学实时解算方法
CN204498793U (zh) * 2015-01-23 2015-07-29 桂林电子科技大学 一种用于检测果实成熟度及采摘的柔性机械手
CN106845037A (zh) * 2017-03-21 2017-06-13 山东科技大学 一种五自由度串联机器人的逆运动学通用求解方法
CN107729637A (zh) * 2017-10-09 2018-02-23 燕山大学 基于臂形角区间的冗余自由度机械臂运动规划及评价方法

Also Published As

Publication number Publication date
CN108481324A (zh) 2018-09-04

Similar Documents

Publication Publication Date Title
CN108481324B (zh) 一种八轴多功能机械臂的逆解工程及其碰撞检测算法
CN108286949B (zh) 一种可移动式三维检测机器人系统
JP5365379B2 (ja) ロボットシステム及びロボットシステムのキャリブレーション方法
Gong et al. Analytical inverse kinematics and self-motion application for 7-DOF redundant manipulator
CN109623206B (zh) 用于机器人管道焊接中优化离线规划的焊枪位姿的方法
CN109664317B (zh) 机器人的物体抓取系统及方法
CN107791248B (zh) 基于不满足Pieper准则的六自由度串联机器人的控制方法
CN113043286B (zh) 一种多自由度机械臂实时避障路径规划系统及方法
Mu et al. A hybrid obstacle-avoidance method of spatial hyper-redundant manipulators for servicing in confined space
CN111791234A (zh) 一种狭窄空间内多机器人空间工作位置防撞控制算法
Lakhal et al. Control of a hyper-redundant robot for quality inspection in additive manufacturing for construction
CN110788446A (zh) 基于激光传感器的变姿态实时焊缝跟踪方法
Zhang et al. Optimal motion planning of all position autonomous mobile welding robot system for fillet seams
CN113799130A (zh) 一种人机协作装配中的机器人位姿标定方法
Xu et al. Advances in multi-robotic welding techniques: A review
Dharmawan et al. Robot base placement and kinematic evaluation of 6R serial manipulators to achieve collision-free welding of large intersecting cylindrical pipes
CN111515954B (zh) 一种机械臂高质量运动路径生成方法
Ranjan et al. Identification and control of NAO humanoid robot to grasp an object using monocular vision
Tongloy et al. An image-based visual servo control system based on an eye-in-hand monocular camera for autonomous robotic grasping
Shanmugasundar et al. Method of Trajectory Generation of a Generic Robot using Bresenham’ s Circle Algorithm
Zhang et al. Motorized Measurement of Deformation on Surface of Revolution with 2D Laser Profiler
Zhong et al. Analysis and experiment of workpiece quality detection based on industrial robot
Lina et al. Modeling, control algorithm, and cloud technology application of an automated SAW platform for large-size and thick-wall workpieces
Cheng et al. A Cube Workpiece Positioning Method Based on Point Structured Light Scanning
Yang et al. Collision avoidance trajectory planning for a dual-robot system: using a modified APF method

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