CN107066645A - 一种七自由度偏置机械臂逆解方法 - Google Patents

一种七自由度偏置机械臂逆解方法 Download PDF

Info

Publication number
CN107066645A
CN107066645A CN201611088649.9A CN201611088649A CN107066645A CN 107066645 A CN107066645 A CN 107066645A CN 201611088649 A CN201611088649 A CN 201611088649A CN 107066645 A CN107066645 A CN 107066645A
Authority
CN
China
Prior art keywords
arm
angle
coordinate system
biasing mechanism
joint
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.)
Pending
Application number
CN201611088649.9A
Other languages
English (en)
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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN201611088649.9A priority Critical patent/CN107066645A/zh
Publication of CN107066645A publication Critical patent/CN107066645A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/10—Geometric CAD
    • G06F30/17—Mechanical parametric or variational design
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Abstract

本发明涉及一种七自由度偏置机械臂逆解方法，建立机械臂末端位姿的4×4的齐次变换矩阵0T7；定义臂型角和臂型面，计算偏置机械臂肘部关节角θ4、肩部关节角和腕部关节角；根据偏置机械臂与非偏置机械臂逆解的关系，可求偏置机械臂的逆解。本方法在障碍回避、奇异处理、关节力矩优化等方面具有较大的优势；简化了七自由度关节机器人的逆解求解过程，提高了计算效率。采用本方法在机器人工作空间优化、关节型机器人运动路径规划等方面能提供形象的空间感，能直观的表达机械臂的臂型。

Description

一种七自由度偏置机械臂逆解方法
技术领域
本发明属于一种拟人七自由度偏置机械臂逆解方法，涉及拟人七自由度机械臂逆解过程中机械臂初始姿态计算的一种方法，应用对象包括空间机械臂和地面机械臂，可应用于空间机械臂的任务规划，尤其更适用于障碍物规避，空间立体感更加形象。
背景技术
随着航天事业的发展，空间设施越来越多，空间机械臂在未来将越来越重要的作用。由于冗余机械臂在障碍回避、奇异处理、关节力矩优化等方面具有较大的优势，根据在轨任务的需求分析采用冗余度机械臂的重要性，然后从奇异消除、工作空间优化、运动学简化、关节运动范围、机械制造可实现性等方面进行臂型论证，最后采取在最优六自由度机械臂基础上，在肩部增加一个关节，构成“3+1+3”配置的臂型方案，该机械表的肩部和腕部三个关节轴线相较于一点，运动学方程简单，先解出非偏置机械臂(SRS)的逆解，在利用偏置机械臂与非偏置机械臂(SSRMS)逆解之间的对应关系可得到偏置机械臂解析形式的运动学逆解。机械臂在执行笛卡尔空间轨迹的运动时，需要根据期望的末端位姿计算关节角，由于七自由度机械臂的位置逆解存在无穷中情况，为了降低求解难度和存在逆解的可能情况，首先规定三号坐标系的坐标原点在参考平面上时的姿态为初始姿态，并作为参考臂型，此时各量的右上标取为0加以区分，参考系标识在左上角；并引入壁型角的概念，从而求出期望位姿下的逆解。本发明是以冗余机械臂为研究对象，为相关运动学问题的研究奠定理论基础，并且该发明顺应了我国大力发展机器人行业，尤其发展空间机器人及其在轨服务技术的趋势，研究成果对未来空间机器人在轨服务的实际应用具有一定的理论和实际意义。
发明内容
要解决的技术问题
为了避免现有技术的不足之处，本发明提出一种七自由度偏置机械臂逆解方法，结合几何图像，让人选择臂型是更加形象，更有利于臂型设计和障碍规避。
技术方案
一种七自由度偏置机械臂逆解方法，其特征在于步骤如下：
步骤1：建立机械臂末端位姿的4×4的齐次变换矩阵0T7
以基座为起始端，在关节部位建立0号坐标系，依次编号至末端执行器处建立7号坐标系，共建立0、1、2、3、4、5、6和7个关节部位的坐标系；所述的坐标系为D-H坐标系；
0P3为3号坐标系原点在0号坐标系中的表示，d1 d2 d3 d4 d5 d6 d7分别为1-7号坐标系之间z轴的偏移长度，a1 a2 a3 a4 a5 a6 a7分别为相邻坐标系z轴的垂直距离，α1 α2 α3α4 α5 α6 α7分别相邻坐标系前一个坐标系z轴绕自己x轴旋转α角与后一个坐标系z轴重合，θ1 θ2 θ3 θ4 θ5 θ6 θ7分别为各个关节旋转的角度。当d2 d6为0时为偏置机械臂对应的非偏置机械臂臂型；
在上述矩阵中变量nx、ny、nz为7号坐标系x轴单位向量在7号坐标系中的分量；sx、sy、sz为7号坐标系y轴的单位向量在0号坐标系中的分量；ax、ay、az为7号坐标系z轴单位向量在0号坐标系的分量；px、py、pz为7号坐标系的原点在0号坐标系中的表示；
步骤2、臂型角ψ的定义与参数化：臂型角是参考面绕着向量w按右手螺旋定则旋转到与当前臂型面重合时所转的角度；
其中：分别为向量w、p、k单位向量，采用sψ,cψ，si,ci表示sinψ、cosψ、sinθi、cosθi，i＝1,2，3,4,5,6,7；V为与关节1的旋转轴平行的单位矢量；
步骤3、偏置机械臂肘部关节角的计算θ4：
SW为机械臂的斜边长；
步骤4、计算偏置机械臂肩部关节角和腕部关节角：偏置机械臂肩部关节角包括θ1θ2 θ3，腕部关节角包括θ5 θ6 θ7；
先求非偏置机械臂逆解关节角θ′1 θ′2 θ′3 θ′4 θ′5 θ′6 θ′7：
θ1＝θ′1
θ2＝θ′2
θ6＝θ′6
θ7＝θ′7
A＝-d2+d6[(axs1-ayc1)c7-(nxs1-nyc1)s7]+pxs1-pyc1-d7(sxs1-syc1)
B＝d6[c7((axc1+ays1)c2+azs2)-s7((nxc1+nys1)c2+nzs2)]-
d7[(sxc1+sys1)c2+szs2]+(pxc1+pys1)c2+(pz-d1)s2
θ3＝ar tan(A(a3+a4c4)-Ba4s4,B(a3+a4c4)+Aa4s4)
θ5＝θ′3+θ′4+θ′5-θ3-θ4
再偏置机械臂的逆解为：
θ1＝θ′1
θ2＝θ′2
θ6＝θ′6
θ7＝θ′7
A＝-d2+d6[(axs1-ayc1)c7-(nxs1-nyc1)s7]+pxs1-pyc1-d7(sxs1-syc1)
B＝d6[c7((axc1+ays1)c2+azs2)-s7((nxc1+nys1)c2+nzs2)]-
d7[(sxc1+sys1)c2+szs2]+(pxc1+pys1)c2+(pz-d1)s2
θ3＝ar tan(A(a3+a4c4)-Ba4s4,B(a3+a4c4)+Aa4s4)
θ5＝θ′3+θ′4+θ′5-θ3-θ4
θ′4、α、θ2 θ4 θ6分别取两种情况，因此偏置七自由度机械臂的逆解有32种情况；
步骤5、选择合理的一种逆解：
设偏置机械臂初始机械臂姿态角为q＝[q′1 q′2 q′3 q′4 q′5 q′6 q′7]，根据2-范数原理求出距初始姿态角最小的一组逆解，方法如下：
计算腕部关节和肩部关节
腕部两个关节的计算：
θ6＝±acos(-(aw23sinψ+bw23cosψ+cw23))
θ7＝atan2((aw33sinψ+bw33cosψ+cw33)s6,(aw13sinψ+bw13cosψ+cw13)s6)
肩部两个关节的计算：
θ1＝atan2((as23sinψ+bs23cosψ+cs23)s2,(as13sinψ+bs13cosψ+cs13)s2)
θ3＝atan2(-(as32sinψ+bs32cosψ+cs32)s2,(as31sinψ+bs31cosψ+cs31)s2)
式中的qij(i＝1、2、3…32，j＝1、2…7)代表32种7个关节的解，Qij(i＝1、2、3…32，j＝1、2…7)每组逆解关节与初始关节差的绝对值；
选出ΔQi最小的对应一组解，为所求逆解。
有益效果
本发明提出的一种七自由度偏置机械臂逆解方法，建立机械臂末端位姿的4×4的齐次变换矩阵0T7；定义臂型角和臂型面，计算偏置机械臂肘部关节角θ4、肩部关节角和腕部关节角；根据偏置机械臂与非偏置机械臂逆解的关系，可求偏置机械臂的逆解。本方法在障碍回避、奇异处理、关节力矩优化等方面具有较大的优势；简化了七自由度关节机器人的逆解求解过程，提高了计算效率。采用本方法在机器人工作空间优化、关节型机器人运动路径规划等方面能提供形象的空间感，能直观的表达机械臂的臂型。
附图说明
图1：计算七自由度机械臂逆解流程图
图2：臂型面定义
图3：臂型角定义
图4：任意状态下各向量关系图
图5：初始状态下各向量关系图
图6：初始姿态下机械臂的姿态图
图7：机械臂臂型示意图
具体实施方式
现结合实施例、附图对本发明作进一步描述：
步骤一：建立机械臂末端位姿的4×4的齐次变换矩阵0T7
0T7为4×4的齐次变换矩阵，即为机械臂末端位姿，可写成如下形式：
以基座为起始端，在关节部位建立0号坐标系，依次编号至末端执行器处建立7号坐标系，共建立0、1、2、3、4、5、6和7个关节部位的坐标系；所述的坐标系为D-H坐标系；
在上述矩阵中变量nx、ny、nz为7号坐标系x轴单位向量在7号坐标系中的分量；sx、sy、sz为7号坐标系y轴的单位向量在0号坐标系中的分量；ax、ay、az为7号坐标系z轴单位向量在0号坐标系的分量；px、py、pz为7号坐标系的原点在0号坐标系中的表示；
步骤二：臂型角ψ的定义与参数化
选取关节1和关节2旋转轴线的交点作为S点，3号坐标系的坐标原点为E点， 关节6和关节7旋转轴线的交点为W点，各点的位置矢量(从参考坐标系原点指向该点的矢量)分别表示为S、E、W。点S、E、W所组成的平面SEW即为当前臂型面。
定义与关节1的旋转轴平行的单位矢量为V，即V＝[0 0 1]T，则矢量V与点W所组成的平面为参考平面。令w＝W-S、e＝E-S。则有：
e＝0P3-[0 0 d1]T (3)
0P3为3号坐标系原点在0号坐标系中的表示，d1 d2 d3 d4 d5 d6 d7分别为1-7号坐标系之间z轴的偏移长度，a1 a2 a3 a4 a5 a6 a7分别为相邻坐标系z轴的垂直距离，α1 α2 α3α4 α5 α6 α7分别相邻坐标系前一个坐标系z轴绕自己x轴旋转α角与后一个坐标系z轴重合，θ1 θ2 θ3 θ4 θ5 θ6 θ7分别为各个关节旋转的角度。当d2 d6为0时为偏置机械臂对应的非偏置机械臂臂型。
臂型角便是参考面绕着向量w按右手螺旋定则旋转到与当前臂型面重合时，所转的角度，即为臂型角ψ。
矢量e在矢量w上的投影为：
当前臂型面内与向量w垂直的单位矢量可表示为：
参考平面内与矢量w垂直的单位向量可表示为：
根据式(5)和式(6)以及矢量点乘和叉乘的性质可得：
因此：
分别为向量w、p、k单位向量，以sψ,cψ，si,ci表示sinψ cosψ sinθi cosθii＝1,2，3,4,5,6,7
点S⊥为点S沿关节3(z2)轴线延长d3长度的点，与E、W点构成一平面。根据该机械臂的臂型特征可知，直线SS⊥与面S⊥EW垂直。如图所示，我们规定三号坐标系的坐标原点在参考平面上时各量的右上标取为0加以区分，参考系标识在左上角。
步骤三：偏置机械臂肘部关节角的计算θ4
上面定义了臂型面及臂型角，下面将推导给定臂型角下的各关节角的计算公式。
在直角三角形S⊥WS中由勾股定理得：
三角形S⊥EW另两边的长度S⊥E＝a3、EW＝a4。根据余弦定律有：
因此：
由此可见，轴关节角θ4与臂型角ψ没有关系，对于给定的末端位姿，有两组取值。
步骤四：计算偏置机械臂肩部关节角和腕部关节角
偏置机械臂肩部关节角包括θ1 θ2 θ3，腕部关节角包括θ5 θ6 θ7，欲求偏置机械臂逆解关节角必须先求非偏置机械臂逆解关节角，非偏置机械臂关节角用θ′1 θ′2 θ′3 θ′4θ′5 θ′6 θ′7表示。
θ1＝θ′1
θ2＝θ′2
θ6＝θ′6
θ7＝θ′7
A＝-d2+d6[(axs1-ayc1)c7-(nxs1-nyc1)s7]+pxs1-pyc1-d7(sxs1-syc1)
B＝d6[c7((axc1+ays1)c2+azs2)-s7((nxc1+nys1)c2+nzs2)]-
d7[(sxc1+sys1)c2+szs2]+(pxc1+pys1)c2+(pz-d1)s2
θ3＝ar tan(A(a3+a4c4)-Ba4s4,B(a3+a4c4)+Aa4s4)
θ5＝θ′3+θ′4+θ′5-θ3-θ4
1)：计算非偏置初始姿态
其中
规定，三号坐标系的坐标原点在参考平面上时各量的右上标取为0加以区分，参考系标示在左上角，式(12)中为当臂型角ψ为0时3号坐标系在0号坐标系的方向余璇矩阵，分别为当3号坐标系原点在参考平面上时其的x y z轴的单位矢量在0号坐标系的表示，式(13)中为3号坐标系原点在参考平面时矢量e和4号坐标系x轴的单位矢量在0号坐标系的表达
由DH坐标系建系规则可知：
d3z3+a3x3＝e (14)
z3 x3为第3号坐标系的z x轴的单位矢量。
由式(14)可得：
式(15)中0e0为已知量，证明如下：
向量e可以看作是由向量w绕向量l(次向量是V与ω叉乘所得)旋转α角度所得。此时则有：
0e＝R(l,α)·w (16)
其中0e为当第3号坐标系原点在参考平面上时矢量e的表示，其中l为：
l＝V×w (17)
R(l,α)＝I3+[ul×]sin(α)+[ul×]2(1-cos(α)) (18)
[ul×]表示向量l单位化后的叉乘因子，I3为三阶单位矩阵。
由式(16)可得：
0e0＝0e＝R(l,α)·w (19)
式(19)中只有α是未知的，α的求解过程如下：
在直角三角形SS⊥E中，由勾股定理可得：
在ΔSE0W中由余弦定理可得:
将式(20)带入式(21)可得：
在ΔSE0W中，由于即，边E0W不是ΔSE0W中的最长边，所以α≤90°，故有：
至此，α求解完毕，α角可取两种情况，即0e0为已知量证明完毕。
由DH坐标系建系规则可知：
w-e＝a4x4 (24)
式(24)中x4为第4号坐标系x轴的单位矢量。
由方向余弦表示的姿态变换关系可知：
3x4＝[c4 s4 0]T (25)
0x4＝0R3·3x4＝[0x3 0y3 0z3]3x4 (26)
式(25)中3x4为4号坐标系的x轴的单位矢量在3号坐标系的表示，式0x4为4号坐标系的x轴的单位矢量在0号坐标系的表示，0R3为3号坐标系相对于0号坐标系的方向余弦矩阵，0x3 0y3 0z3分别为3号坐标系的x y z轴的单位矢量在0号坐标系的表示。由式(24)、式(25)和式(26)可得：
由于w、a4和0e0都是已知量，故也是已知量，其中表示当臂型角为0时，4号坐标系的x轴单位矢量在0号坐标系的表示。
式(15)和式(27)左右两边分别叉乘且结合三轴单位正交矢量的关系即，x×y＝z、y×z＝x、z×x＝y可得：
解由式(15)、式(27)和式(29)组成的方程组：
可得：
故：
即初始姿态有上述算法得到。
2)非偏置机械臂腕部关节和肩部关节计算方法
非偏置机械臂的肩部关节包括θ′1 θ′2 θ′3，腕部关节角包括θ′5 θ′6 θ′7，肘部关节为θ′4
腕部关节角
θ′2＝±acos(-(as33sinψ+bs33cosψ+cs33))
θ′1＝atan2((as23sinψ+bs23cosψ+cs23)s2,(as13sinψ+bs13cosψ+cs13)s2)
θ′3＝atan2(-(as32sinψ+bs32cosψ+cs32)s2,(as31sinψ+bs31cosψ+cs31)s2)
上公式由下面所得
三号坐标系在零号坐标系中绕w旋转ψ角后的姿态变换矩阵为：
0Rψ＝I3+[0uw×]sinψ+[0uw×]2(1-cosψ) (33)
其中，I3为3×3的单位阵，[0uw×]为向量w的斜对称阵。由式(33)知三号坐标系在零号坐标下的姿态可表示为：
将方程(33)带入方程(34)可得：
0R3＝Assinψ+Bscosψ+Cs (35)
其中，
根据姿态变换矩阵的定义，可得：
由式(35)和式(36)可得：
c2＝-(as33sinψ+bs33cosψ+cs33) (37)
根据式(37)，可求解θ2，有两组值：
θ′2＝±acos(-(as33sinψ+bs33cosψ+cs33)) (38)
相应于θ2的取值结合式(35)和式(36)，可求解出θ1和θ3：
θ′1＝atan2((as23sinψ+bs23cosψ+cs23)/s2,(as13sinψ+bs13cosψ+cs13)/s2) (39)
θ′3＝atan2(-(as32sinψ+bs32cosψ+cs32)/s2,(as31sinψ+bs31cosψ+cs31)/s2) (40)
式(37)—式(40)中asij(i、j＝1、2、3)分别为As矩阵中的元素，bsij(i、j＝1、2、3)分别为Bs中的元素，csij(i、j＝1、2、3)分别为Cs矩阵中的元素。
需要说明的是，计算θ1、θ3的式(39)和(40)中，每一项均除以相同的系数s2′，但这是不能随便约掉的，因为它的符号影响了相应角度所处的象限位置，但为了避免由于s2＝0导致计算中出现无穷大的数而无法正常运算，可将处于s2改造成乘以s2而不影响计算结果，即：
θ′1＝atan2((as23sinψ+bs23cosψ+cs23)s2,(as13sinψ+bs13cosψ+cs13)s2) (41)
θ′3＝atan2(-(as32sinψ+bs32cosψ+cs32)s2,(as31sinψ+bs31cosψ+cs31)s2) (42)
肩部关节角与肘部关节：
θ′6＝±acos(-(aw23sinψ+bw23cosψ+cw23))
θ′4＝atan2((aw22sinψ+bw22cosψ+cw22)s6,(aw21sinψ+bw21cosψ+cw21)s6)
θ′5＝(θ′4+θ′5)-θ′4
θ′7＝atan2((aw33sinψ+bw33cosψ+cw33)s6,(aw13sinψ+bw13cosψ+cw13)s6)
结合式0T7＝0T1 1T2…6T7＝fkine(θ1,θ2,…,θ7)和式0R3＝Assinψ+Bscosψ+Cs可知四号 坐标系在七号坐标系下的表示为：
其中：
由姿态变换方式得到的四号坐标系在七号坐标系下的表示为：
由方程(43)和方程(44)并采用类似于计算肩部关节角的处理方式，有：
θ′6＝±acos(-(aw23sinψ+bw23cosψ+cw23)) (45)
θ′4＝atan2((aw22sinψ+bw22cosψ+cw22)s6,(aw21sinψ+bw21cosψ+cw21)s6) (46)
θ′4+θ′5＝arctan((aw22sψ+bw22cψ+cw22)s6,(aw21sψ+bw21cψ+cw21)s6) (45)
θ′5＝(θ′4+θ′5)-θ′4 (48)
θ′7＝atan2((aw33sinψ+bw33cosψ+cw33)s6,(aw13sinψ+bw13cosψ+cw13)s6) (49)
式(46)—式(50)中awij(i、j＝1、2、3)分别为Aw矩阵中的元素，bwij(i、j＝1、2、3)分别为Bw中的元素，cwij(i、j＝1、2、3)分别为Cw矩阵中的元素。
综上非偏置七自由度机械臂的逆解共有16种结果。
根据偏置机械臂与非偏置机械臂逆解的关系，可求偏置机械臂的逆解，对应关系如下表所示。
表1偏置机械臂型与非偏置机械臂型关节解析表达式关系
综上可得偏置机械臂的逆解为：
θ1＝θ′1
θ2＝θ′2
θ6＝θ′6
θ7＝θ′7
A＝-d2+d6[(axs1-ayc1)c7-(nxs1-nyc1)s7]+pxs1-pyc1-d7(sxs1-syc1)
B＝d6[c7((axc1+ays1)c2+azs2)-s7((nxc1+nys1)c2+nzs2)]-
d7[(sxc1+sys1)c2+szs2]+(pxc1+pys1)c2+(pz-d1)s2
θ3＝ar tan(A(a3+a4c4)-Ba4s4,B(a3+a4c4)+Aa4s4)
θ5＝θ′3+θ′4+θ′5-θ3-θ4
θ′4、α、θ2 θ4 θ6分别可取两种情况，因此偏置七自由度机械臂的逆解有32种情况。
步骤五：选择合理的一种逆解
设偏置机械臂初始机械臂姿态角为q＝[q′1 q′2 q′3 q′4 q′5 q′6 q′7]，根据2-范数原理求出距初始姿态角最小的一组逆解，方法如下
式(50)中的qij(i＝1、2、3…32，j＝1、2…7)代表32种7个关节的解，Qij(i＝1、2、3…32，j＝1、2…7)每组逆解关节与初始关节差的绝对值。
选出ΔQi最小的对应一组解，即为所求逆解。
具体实施方法
假设七自由度机械臂的DH参数为如下表所示，机械臂初始姿态如图6所示
表2 SSRMS型机械臂的D-H参数
当机械臂末端位姿为
当求逆解时应当考虑障碍物规避问题，在末端位姿为上式的情况下，如图7所示。
当壁型角为60°时可能有障碍物挡住机械臂，此时我们可以找到合适的壁型角规避机械臂在运动时碰触到障碍物。即取壁型角为30°，即根据上述算法编程求解，可得到 32种逆解，这时取在当前臂型变化最小的一组逆解
表3 32种机械臂逆解
经编程正运动学验证，上表32种逆解是正确的。
根据2-范数原理求得目标位姿与初始姿态变化最小的一组逆解，结果如下表所示。
表4相对于初始姿态变化最小的逆解
即逆解求出。

Claims (1)

1.一种七自由度偏置机械臂逆解方法，其特征在于步骤如下：
步骤1：建立机械臂末端位姿的4×4的齐次变换矩阵0T7
以基座为起始端，在关节部位建立0号坐标系，依次编号至末端执行器处建立7号坐标系，共建立0、1、2、3、4、5、6和7个关节部位的坐标系；所述的坐标系为D-H坐标系；
0P3为3号坐标系原点在0号坐标系中的表示，d1 d2 d3 d4 d5 d6 d7分别为1-7号坐标系之间z轴的偏移长度，a1 a2 a3 a4 a5 a6 a7分别为相邻坐标系z轴的垂直距离，α1 α2 α3 α4α5 α6 α7分别相邻坐标系前一个坐标系z轴绕自己x轴旋转α角与后一个坐标系z轴重合，θ1θ2 θ3 θ4 θ5 θ6 θ7分别为各个关节旋转的角度。当d2d6为0时为偏置机械臂对应的非偏置机械臂臂型；
在上述矩阵中变量nx、ny、nz为7号坐标系x轴单位向量在7号坐标系中的分量；sx、sy、sz为7号坐标系y轴的单位向量在0号坐标系中的分量；ax、ay、az为7号坐标系z轴单位向量在0号坐标系的分量；px、py、pz为7号坐标系的原点在0号坐标系中的表示；
步骤2、臂型角ψ的定义与参数化：臂型角是参考面绕着向量w按右手螺旋定则旋转到与当前臂型面重合时所转的角度；
其中：分别为向量w、p、k单位向量，采用sψ,cψ，si,ci表示sinψ、cosψ、sinθi、cosθi，i＝1,2，3,4,5,6,7；V为与关节1的旋转轴平行的单位矢量；
步骤3、偏置机械臂肘部关节角的计算θ4：
SW为机械臂的斜边长；
步骤4、计算偏置机械臂肩部关节角和腕部关节角：偏置机械臂肩部关节角包括θ1θ2θ3，腕部关节角包括θ5θ6θ7；
先求非偏置机械臂逆解关节角θ′1 θ′2 θ′3 θ′4 θ′5 θ′6 θ′7：
θ1＝θ′1
θ2＝θ′2
θ6＝θ′6
θ7＝θ′7
A＝-d2+d6[(axs1-ayc1)c7-(nxs1-nyc1)s7]+pxs1-pyc1-d7(sxs1-syc1)
B＝d6[c7((axc1+ays1)c2+azs2)-s7((nxc1+nys1)c2+nzs2)]-
d7[(sxc1+sys1)c2+szs2]+(pxc1+pys1)c2+(pz-d1)s2
θ3＝artan(A(a3+a4c4)-Ba4s4,B(a3+a4c4)+Aa4s4)
θ5＝θ′3+θ′4+θ′5-θ3-θ4
再偏置机械臂的逆解为：
θ1＝θ′1
θ2＝θ′2
θ6＝θ′6
θ7＝θ′7
A＝-d2+d6[(axs1-ayc1)c7-(nxs1-nyc1)s7]+pxs1-pyc1-d7(sxs1-syc1)
B＝d6[c7((axc1+ays1)c2+azs2)-s7((nxc1+nys1)c2+nzs2)]-
d7[(sxc1+sys1)c2+szs2]+(pxc1+pys1)c2+(pz-d1)s2
θ3＝artan(A(a3+a4c4)-Ba4s4,B(a3+a4c4)+Aa4s4)
θ5＝θ′3+θ′4+θ′5-θ3-θ4
θ′4、α、θ2 θ4 θ6分别取两种情况，因此偏置七自由度机械臂的逆解有32种情况；
步骤5、选择合理的一种逆解：
设偏置机械臂初始机械臂姿态角为q＝[q′1 q'2 q'3 q'4 q'5 q'6 q'7]，根据2-范数原理求出距初始姿态角最小的一组逆解，方法如下：
计算腕部关节和肩部关节
腕部两个关节的计算：
θ6＝±acos(-(aw23sinψ+bw23cosψ+cw23))
θ7＝atan2((aw33sinψ+bw33cosψ+cw33)s6,(aw13sinψ+bw13cosψ+cw13)s6)
肩部两个关节的计算：
θ1＝atan2((as23sinψ+bs23cosψ+cs23)s2,(as13sinψ+bs13cosψ+cs13)s2)
θ3＝atan2(-(as32sinψ+bs32cosψ+cs32)s2,(as31sinψ+bs31cosψ+cs31)s2)
式中的qij(i＝1、2、3…32，j＝1、2…7)代表32种7个关节的解，Qij(i＝1、2、3…32，j＝1、2…7)每组逆解关节与初始关节差的绝对值；
选出ΔQi最小的对应一组解，为所求逆解。
CN201611088649.9A 2016-12-01 2016-12-01 一种七自由度偏置机械臂逆解方法 Pending CN107066645A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201611088649.9A CN107066645A (zh) 2016-12-01 2016-12-01 一种七自由度偏置机械臂逆解方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201611088649.9A CN107066645A (zh) 2016-12-01 2016-12-01 一种七自由度偏置机械臂逆解方法

Publications (1)

Publication Number Publication Date
CN107066645A true CN107066645A (zh) 2017-08-18

Family

ID=59619089

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201611088649.9A Pending CN107066645A (zh) 2016-12-01 2016-12-01 一种七自由度偏置机械臂逆解方法

Country Status (1)

Country Link
CN (1) CN107066645A (zh)

Cited By (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107685330A (zh) * 2017-10-18 2018-02-13 佛山华数机器人有限公司 一种六自由度手腕偏置串联机器人的运动学逆解求解方法
CN108241339A (zh) * 2017-12-27 2018-07-03 北京航空航天大学 仿人机械臂的运动求解和构型控制方法
CN108638055A (zh) * 2018-04-11 2018-10-12 北京控制工程研究所 一种七自由度空间机械臂自主避障规划方法
CN108789406A (zh) * 2018-06-05 2018-11-13 上海大学 一种用于冗余机械臂可达性分析的逆解求解方法
CN109333520A (zh) * 2018-10-23 2019-02-15 广州霞光技研有限公司 一种四轴scara机器人逆解方法
CN109623823A (zh) * 2018-12-29 2019-04-16 深圳前海达闼云端智能科技有限公司 逆解计算偏置机械臂关节角度值的方法、装置及计算设备
CN109702751A (zh) * 2019-03-01 2019-05-03 中国科学院电子学研究所 一种七自由度串联机械臂的位置级逆解方法
CN110712203A (zh) * 2019-09-26 2020-01-21 苏州苏相机器人智能装备有限公司 一种7自由度机械臂逆运动学求解算法
WO2020034417A1 (zh) * 2018-08-16 2020-02-20 居鹤华 基于轴不变量多轴机器人d-h系及d-h参数确定方法
WO2020034416A1 (zh) * 2018-08-16 2020-02-20 居鹤华 基于轴不变量的通用7r机械臂逆解建模与解算方法
CN112828916A (zh) * 2021-01-20 2021-05-25 清华大学深圳国际研究生院 冗余机械臂遥操作组合交互装置及冗余机械臂遥操作系统
CN113378349A (zh) * 2021-03-25 2021-09-10 北京航空航天大学 S-r-s结构七自由度机械臂逆运动学解析解的数值稳定算法
CN113580135A (zh) * 2021-08-09 2021-11-02 华中科技大学 一种有偏置七轴机器人实时逆解算法
CN113858202A (zh) * 2021-09-29 2021-12-31 中铁工程装备集团有限公司 一种锚杆台车钻臂的逆解分析方法、装置、设备及介质
CN114670190A (zh) * 2022-03-08 2022-06-28 西北工业大学 一种基于解析数值混合法的冗余机械臂逆运动学方法
CN115081147A (zh) * 2022-07-20 2022-09-20 上海龙慧医疗科技有限公司 用于扩展无偏置七轴机械臂末端手术工具瞄准范围的方法
WO2022199059A1 (zh) * 2021-03-23 2022-09-29 深圳市优必选科技股份有限公司 机械臂控制方法、装置、作业控制设备及可读存储介质
CN116038702A (zh) * 2022-12-30 2023-05-02 成都卡诺普机器人技术股份有限公司 一种七轴机器人逆解方法及七轴机器人

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110093119A1 (en) * 2009-10-16 2011-04-21 Samsung Electronics Co., Ltd. Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same
CN104635762A (zh) * 2015-01-13 2015-05-20 北京航空航天大学 一种面向srs仿人手臂的自运动角计算方法
CN104866722A (zh) * 2015-05-26 2015-08-26 宁波韦尔德斯凯勒智能科技有限公司 一种七轴工业机械臂的逆动学求解方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110093119A1 (en) * 2009-10-16 2011-04-21 Samsung Electronics Co., Ltd. Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same
CN104635762A (zh) * 2015-01-13 2015-05-20 北京航空航天大学 一种面向srs仿人手臂的自运动角计算方法
CN104866722A (zh) * 2015-05-26 2015-08-26 宁波韦尔德斯凯勒智能科技有限公司 一种七轴工业机械臂的逆动学求解方法

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
LEI YAN 等: "Analytical inverse kinematics of a class of redundant manipulator based on dual arm-angle parameterization", 《2014 IEEE INTERNATIONAL CONFERENCE ON SYSTEMS, MAN, AND CYBERNETICS (SMC)》 *
徐文福 等: "偏置式冗余空间机械臂逆运动学求解的参数化方法", 《宇航学报》 *
邢葆轶: "基于QNX的七自由度机械臂控制系统设计", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
闫文彬 等: "一种七自由度冗余机械臂的逆运动学优化算法", 《系统科学与数学》 *
闫磊: "拟人机器人参数化逆运动学及在轨装配的协调规划研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
韩建海主编: "《工业机器人（第三版）》", 31 July 2015, 华中科技大学出版社 *

Cited By (25)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107685330A (zh) * 2017-10-18 2018-02-13 佛山华数机器人有限公司 一种六自由度手腕偏置串联机器人的运动学逆解求解方法
CN108241339A (zh) * 2017-12-27 2018-07-03 北京航空航天大学 仿人机械臂的运动求解和构型控制方法
CN108638055B (zh) * 2018-04-11 2020-07-14 北京控制工程研究所 一种七自由度空间机械臂自主避障规划方法
CN108638055A (zh) * 2018-04-11 2018-10-12 北京控制工程研究所 一种七自由度空间机械臂自主避障规划方法
CN108789406A (zh) * 2018-06-05 2018-11-13 上海大学 一种用于冗余机械臂可达性分析的逆解求解方法
WO2020034416A1 (zh) * 2018-08-16 2020-02-20 居鹤华 基于轴不变量的通用7r机械臂逆解建模与解算方法
WO2020034417A1 (zh) * 2018-08-16 2020-02-20 居鹤华 基于轴不变量多轴机器人d-h系及d-h参数确定方法
CN109333520A (zh) * 2018-10-23 2019-02-15 广州霞光技研有限公司 一种四轴scara机器人逆解方法
CN109623823B (zh) * 2018-12-29 2022-02-08 达闼机器人有限公司 逆解计算偏置机械臂关节角度值的方法、装置及计算设备
CN109623823A (zh) * 2018-12-29 2019-04-16 深圳前海达闼云端智能科技有限公司 逆解计算偏置机械臂关节角度值的方法、装置及计算设备
CN109702751A (zh) * 2019-03-01 2019-05-03 中国科学院电子学研究所 一种七自由度串联机械臂的位置级逆解方法
CN110712203A (zh) * 2019-09-26 2020-01-21 苏州苏相机器人智能装备有限公司 一种7自由度机械臂逆运动学求解算法
CN110712203B (zh) * 2019-09-26 2021-03-26 苏州苏相机器人智能装备有限公司 一种7自由度机械臂逆运动学求解算法
CN112828916A (zh) * 2021-01-20 2021-05-25 清华大学深圳国际研究生院 冗余机械臂遥操作组合交互装置及冗余机械臂遥操作系统
CN112828916B (zh) * 2021-01-20 2022-07-08 清华大学深圳国际研究生院 冗余机械臂遥操作组合交互装置及冗余机械臂遥操作系统
WO2022199059A1 (zh) * 2021-03-23 2022-09-29 深圳市优必选科技股份有限公司 机械臂控制方法、装置、作业控制设备及可读存储介质
CN113378349A (zh) * 2021-03-25 2021-09-10 北京航空航天大学 S-r-s结构七自由度机械臂逆运动学解析解的数值稳定算法
CN113378349B (zh) * 2021-03-25 2022-05-20 北京航空航天大学 S-r-s结构七自由度机械臂逆运动学解析解的数值稳定算法
CN113580135A (zh) * 2021-08-09 2021-11-02 华中科技大学 一种有偏置七轴机器人实时逆解算法
CN113858202A (zh) * 2021-09-29 2021-12-31 中铁工程装备集团有限公司 一种锚杆台车钻臂的逆解分析方法、装置、设备及介质
CN114670190A (zh) * 2022-03-08 2022-06-28 西北工业大学 一种基于解析数值混合法的冗余机械臂逆运动学方法
CN114670190B (zh) * 2022-03-08 2023-10-24 西北工业大学 一种基于解析数值混合法的冗余机械臂逆运动学方法
CN115081147A (zh) * 2022-07-20 2022-09-20 上海龙慧医疗科技有限公司 用于扩展无偏置七轴机械臂末端手术工具瞄准范围的方法
CN116038702A (zh) * 2022-12-30 2023-05-02 成都卡诺普机器人技术股份有限公司 一种七轴机器人逆解方法及七轴机器人
CN116038702B (zh) * 2022-12-30 2023-12-19 成都卡诺普机器人技术股份有限公司 一种七轴机器人逆解方法及七轴机器人

Similar Documents

Publication Publication Date Title
CN107066645A (zh) 一种七自由度偏置机械臂逆解方法
Maciejewski Fault tolerant properties of kinematically redundant manipulators
Xu et al. Dual arm-angle parameterisation and its applications for analytical inverse kinematics of redundant manipulators
Sun et al. Analytical inverse kinematic solution using the DH method for a 6-DOF robot
CN103481288B (zh) 一种5关节机器人末端工具位姿控制方法
CN109702751A (zh) 一种七自由度串联机械臂的位置级逆解方法
CN105643619B (zh) 一种采用框架描述的工业机器人工具位姿控制方法
Corinaldi et al. Singularity-free path-planning of dexterous pointing tasks for a class of spherical parallel mechanisms
CN107727026A (zh) 面向双工业机器人协同工作的工件坐标系的标定方法
Yan et al. Analytical inverse kinematics of a class of redundant manipulator based on dual arm-angle parameterization
Shimizu Analytical inverse kinematics for 5-DOF humanoid manipulator under arbitrarily specified unconstrained orientation of end-effector
Moradi et al. Joint limit analysis and elbow movement minimization for redundant manipulators using closed form method
Li et al. Solving inverse kinematics model for 7-DoF robot arms based on space vector
Hayawi Analytical inverse kinematics algorithm of a 5-DOF robot arm
Wang et al. An online motion planning algorithm for a 7DOF redundant manipulator
Taki et al. A novel singularity-consistent inverse kinematics decomposition for SRS type manipulators
Chen et al. An analytical solution of inverse kinematics for a 7-DOF redundant manipulator
Chen et al. A general analytical algorithm for collaborative robot (cobot) with 6 degree of freedom (DOF)
Kim et al. Kinematics analysis and motion planning for a 7-DOF redundant industrial robot manipulator
Liu et al. A TE-E optimal planning technique based on screw theory for robot trajectory in workspace
Bruyninckx Robot kinematics and dynamics
Harish et al. Manipulability Index of a Parallel Robot Manipulator
Zhang et al. A new analytical inverse kinematics model for seven degrees of freedom redundant manipulators
Du et al. Dexterity analysis for omni-directional wheeled mobile manipulator based on double quaternion
Zhou et al. Research on kinematics solution of 7-axis redundant robot based on self-motion

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
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20170818

WD01 Invention patent application deemed withdrawn after publication