一种六自由度串联机器人运动学逆解的求解方法

1.一种六自由度串联机器人运动学逆解的求解方法，其特征在于，当机器人连杆的后三个关节的轴线交于一点时，其步骤包括：
(1)建立连杆坐标系：将机器人连杆放入坐标系，固定端为基坐标，依次往后的六个关节轴旋转角度分别设为变量θ1，θ2，θ3，θ4，θ5，θ6；
(2)利用几何法，即根据Paden-Kahan子问题思想建立与后三个关节相关的函数SubProb_3R(ξ4，ξ5，ξ6，p，q)求解θ4，θ5，θ6的值，其中ξ4，ξ5，ξ6为三个零节距的单位运动旋量，p，q为空间两点；
(3)利用矩阵乘法变换 此时已知θ4，θ5，θ6，通过θ1，θ2，θ3三个
变量构造三角函数方程，利用代数消元法即可求得θ1，θ2，θ3；所述步骤(2)中，ξ4，ξ5，ξ6的三个空间转轴之间的关系是ξ4空间垂直于ξ5和ξ6，ξ5空间平行于ξ6；对θ4，θ5，θ6三个旋转角度的求解包括对一个单轴旋转角度θ6的求解和对一个双轴旋转角度θ4，θ5的求解；
对一个单轴旋转角度θ6的求解步骤包括：
(1)设d为转轴上一点，定义u＝ p-d，v＝q-d，根据位置不变原则有：
则
T
(2)定义u′，v′为u，v在垂直于转轴ξ的平面上的投影，则u′＝u-ωωu，v′＝T
v-ωωv，其中ω为关节旋转的法线；
T
(3)当且仅当u，v在轴上的投影等长时，在轴ω上垂直的平面上投影也等长，即ωuT
＝ωv，||u′||＝||v′||，那么根据式子 和投影矢量u′，v′求得旋转角
度，即对应第六关节的旋转角度θ6；
对一个双轴旋转角度θ4，θ5的求解步骤包括：
(1)假定将点p绕给定轴ξ5旋转θ5，再绕给定轴ξ4旋转θ4到点q重合；
(2) 令q1是转轴 ξ 4上的任一点，由距离保持不变原则得到：
令δ＝||q-q1||，则 令q2是转轴ξ5上
任一点，并定义u＝p-q2，v＝q1-q2，则
(3)将上述两点向垂直于转轴ξ5的平面做投影，并定义u′，v′为u，v在垂直于转轴T T
ξ5的平面上的投影，则u′＝u-ω2ω2u，v′＝v-ω2ω2v，其中ω2为关节旋转的法线，同
2 2 T 2
样对δ投影，得u′ ＝δ -||ω2(p-q1)||，即
-1 T T
(4)设θ为矢量u′与v′之间的夹角，则θ＝tan 2(ω(u′×v′)，u′ v′)，
2 2 2
利用余弦定理求解φ＝θ-θ5，有||u′||+||v′||-2||u′||||v′||cosφ＝δ，因此，
(5)已知θ5，则由 p求得p1，再根据 计算θ4，得双轴旋转角
度，即分别对应第四和第五关节的旋转角度θ4和θ5。
2.一种六自由度串联机器人运动学逆解的求解方法，其特征在于，当机器人连杆的后三个关节的轴线不交于一点时，其步骤包括：
(1)建立连杆坐标系：将机器人连杆放入坐标系，固定端为基坐标，依次往后的六个关节轴旋转角度分别设为变量θ1，θ2，θ3，θ4，θ5，θ6；
(2)假设θ6为已知值，利用几何法和代数消元法求取θ1，θ2，θ3，θ4，θ5得到θ1＝β1，θ2＝β2，θ3＝β3，θ4＝β4，θ5＝β5，其中β1，β2，β3，β4，β5为解析解表达式；
(3)根据六自由度机器人运动学正解的求解公式，得到正解表达式
(4)将机器人连杆的结果位姿与初始位姿进行对比，得表达式 其中
为初始位姿；
(5)利用优化算法以γ为目标，调整θ6的值使其满足|γ|≤σ，其中σ为误差阈值，得到θ1，θ2，θ3，θ4，θ5，θ6的数字解。
3.根据权利要求2所述的一种六自由度串联机器人运动学逆解的求解方法，其特征在于，所述步骤(5)中，所述优化算法为禁忌搜索法或爬山法。
4.根据权利要求3所述的一种六自由度串联机器人运动学逆解的求解方法，其特征在于，所述步骤(5)具体包括以下步骤：
(5a)设θ6为具体数值；
(5b)根据步骤(2)～(4)得到γ的值；
(5c)对比|γ|与误差阈值σ的大小；
(5d)若|γ|≤σ，则该具体数值为θ6的数字解，若|γ|＞σ，则根据禁忌搜索法或者爬山法重新设定θ6为具体数值；
(5e)重复执行步骤(5b)～(5d)，直至得到符合要求的θ6的数字解，此时，该θ6对应的由步骤(2)得到的θ1，θ2，θ3，θ4，θ5的值即为其相应的数字解。

一种六自由度串联机器人运动学逆解的求解方法\n技术领域\n[0001] 本发明涉及一种机器人运动学的求解方法，具体地讲，尤其是涉及一种六自由度串联机器人运动学逆解的求解方法。\n背景技术\n[0002] 目前，对六自由度串联机器人运动学逆解的求解方法是将机器人各轴旋转角度设为变量带入正运动学方程\n中，根据矩阵乘法原理Tc＝Ta·Tb，其中Tb＝(Ta)-1·Tc可推导出运动学方程\n方程左边数据已知，整理方程可将含有θ1的部分移动到方程的左边得到\n通过类似方法结合三角函数\n相关和差化积公式可以分离变量求解每个轴的角度。此方法主要利用等式原理在机器人连杆的姿态和位置的矩阵里构造含一个变量的方程，从而求解。此方法缺点在于任意构造方程可能导致方程系数行列式的秩小于阶数，无法得到对应解析解，而且对于复杂机构的三角函数关系求解有更多冗余，要得到其中能够唯一表达解析解的方程相当困难。\n发明内容\n[0003] 本发明的目的在于提供一种六自由度串联机器人运动学逆解的求解方法，解决目前求解方法中任意构造方程导致方程系数行列式的秩小于阶数而无法得到对应解析解的缺陷。\n[0004] 为了实现上述目的，本发明采用的技术方案如下：\n[0005] 一种六自由度串联机器人运动学逆解的求解方法，当机器人连杆的后三个关节的轴线交于一点时，其步骤包括：\n[0006] (1)建立连杆坐标系：将机器人连杆放入坐标系，固定端为基坐标，依次往后的六个关节轴旋转角度分别设为变量θ1，θ2，θ3，θ4，θ5，θ6；\n[0007] (2)当机器人连杆的后三个关节的轴线交于一点时，利用几何法，即根据Paden-Kahan子问题思想建立与后三个关节相关的函数SubProb_3R(ξ4，ξ5，ξ6，p，q)求解θ4，θ5，θ6的值，其中ξ4，ξ5，ξ6为三个零节距的单位运动旋量，p，q为空间两点；\n[0008] (3)利用矩阵乘法变换 此时已知θ4，θ5，θ6，通过θ1，θ2，\nθ3三个变量构造三角函数方程，利用代数消元法即可求得θ1，θ2，θ3。\n[0009] 进一步地，所述步骤(2)中，ξ4，ξ5，ξ6的三个空间转轴之间的关系是ξ4空间垂直于ξ5和ξ6，ξ5空间平行于ξ6；对θ4，θ5，θ6三个旋转角度的求解包括对一个单轴旋转角度θ6的求解和对一个双轴旋转角度θ4，θ5的求解。\n[0010] 更进一步地，对一个单轴旋转角度θ6的求解步骤包括：\n[0011] (1)设d为转轴上一点，定义u＝p-d，v＝p-d，根据位置不变原则有：\n则\n[0012] (2)定义u′，v′为u，v在垂直于转轴ξ6的平面上的投影，则u′＝u-ωωTu，v′＝v-ωωTv，其中ω为关节旋转的法线；\n[0013] (3)当且仅当u，v在轴上的投影等长时，在轴ω上垂直的平面上投影也等长，即T T\nωu＝ωv，||u′||＝||v′||，那么根据式子 和投影矢量u′，v′求得旋\n转角度，即对应第六关节的旋转角度θ6。\n[0014] 再进一步地，对一个双轴旋转角度θ4，θ5的求解步骤包括：\n[0015] (1)假定将点p绕给定轴ξ5旋转θ5，再绕给定轴ξ4旋转θ4到点q重合；\n[0016] (2)令 q1是转轴ξ 4上的任一点，由距离保持不变原则得到：\n令δ＝||q-q1||，则 令q2是转轴ξ5上\n任一点，并定义u＝p-q2，v＝q1-q2，则\n[0017] (3)将上述两点向垂直于转轴ξ5的平面做投影，并定义u′，v′为u，v在垂直于T T\n转轴ξ5的平面上的投影，则u′＝u-ω2ω2u，v′＝v-ω2ω2v，其中ω2为关节旋转的法线，同样对δ投影，得u′2＝δ2-||ω2T(p-q1)||2，即\n-1 T T\n[0018] (4)设θ为矢量u′与v′之间的夹角，则θ＝tan 2(ω(u′×v′)，u′ v′)，\n2 2 2\n利用余弦定理求解φ＝θ-θ5，有||u′||+||v′||-2||u′||||v′||cosφ＝δ，因此，\n[0019] (5)已知θ5，则由 求得p1，再根据 计算θ4，得双轴旋\n转角度，即分别对应第四和第五关节的旋转角度θ4和θ5。\n[0020] 一种六自由度串联机器人运动学逆解的求解方法，当机器人连杆的后三个关节的轴线不交于一点时，其步骤包括：\n[0021] (1)建立连杆坐标系：将机器人连杆放入坐标系，固定端为基坐标，依次往后的六个关节轴旋转角度分别设为变量θ1，θ2，θ3，θ4，θ5，θ6；\n[0022] (2)假设θ6为已知值，利用几何法和代数消元法求取θ1，θ2，θ3，θ4，θ5得到θ1＝β1，θ2＝β2，θ3＝β3，θ4＝β4，θ5＝β5，其中β1，β2，β3，β4，β5为解析解表达式；\n[0023] (3)根据六自由度机器人运动学正解的求解公式，得到正解表达式\n[0024] \n[0025] (4)将机器人连杆的结果位姿与初始位姿进行对比，得表达式 其\n中 为初始位姿；\n[0026] (5)利用优化算法以γ为目标，调整θ6的值使其满足|γ|≤σ，其中σ为误差阈值，得到θ1，θ2，θ3，θ4，θ5，θ6的数字解。\n[0027] 进一步地，所述优化算法为禁忌搜索法或爬山法。\n[0028] 更进一步地，所述步骤(5)具体包括以下步骤：\n[0029] (5a)设θ6为具体数值；\n[0030] (5b)根据步骤(2)～(4)得到γ的值；\n[0031] (5c)对比|γ|与误差阈值σ的大小；\n[0032] (5d)若|γ|≤σ，则该具体数值为θ6的数字解，若|γ|＞σ，则根据禁忌搜索法或者爬山法重新设定θ6为具体数值；\n[0033] (5e)重复执行步骤(5b)～(5d)，直至得到符合要求的θ6的数字解，此时，该θ6对应的由步骤(2)得到的θ1，θ2，θ3，θ4，θ5的值即为其相应的数字解。\n[0034] 与现有技术相比，本发明的有益效果是：\n[0035] (1)本发明构思巧妙，以几何法和代数消元法综合求解代替了原有的单一求解方式，避免了任意构造方程导致方程系数行列式的秩小于阶数的问题。\n[0036] (2)本发明利用基于Paden-Kahan子问题的几何法求解出三个轴的解，再利用代数消元法求解剩余的三个轴的解，减少了直接利用三角函数关系求解计算的冗余，从而提高了求解效率。\n[0037] (3)本发明利用几何法的优点，有效地避免了复杂机构的三角函数关系使用消元法不容易将二元一次方程转化为一元一次方程的问题，使所得的解析解更加准确，从而得到对应唯一的解析解。\n[0038] (4)本发明通过结合禁忌搜索法，还可以求得五个轴的解析解表达式和一个轴的数字解，使机器人连杆末端为非正交球型或者非正交非球型的结构时求解困难的情况得以改善。\n[0039] (5)本发明利用几何法、代数消元法、以及禁忌搜索法的结合运用，能够求解出连杆机器人的各种情况，大大增加了本发明的适用范围。\n附图说明\n[0040] 图1是本发明-实施例1中利用代数消元法建立连杆坐标系的结构示意图。\n[0041] 图2是图1中连杆坐标系的示意图。\n[0042] 图3是本发明-实施例1中利用几何法求解的原理示意图。\n[0043] 图4是图3中单轴旋转角度求解的原理示意图。\n[0044] 图5是图3中双轴旋转角度求解的原理示意图。\n[0045] 图6是本发明-实施例2的连杆结构示意图。\n具体实施方式\n[0046] 下面结合附图和具体的实施例对本发明作进一步说明。本发明的实施方式包括但不限于下列实施例。\n[0047] 实施例1\n[0048] 如图1和图2所示，以The Unimation PUMA560机器人为例，建立连杆坐标系，将机器人连杆放入坐标系，固定端为基坐标，依次往后的六个关节轴旋转角度分别设为变量θ1，θ2，θ3，θ4，θ5，θ6；并写出Denavit-Hartenberg参数表如下：\n[0049] PUMA 560机器人的连杆参数\n[0050] \n[0051] 已知初始位形的旋转角度值，记作θs，设G(θt)为机器人位于初始位形时惯性坐标系的刚体变换，则每个关节构造一个单位运动旋量，用G(θi)表示第i个关节的旋转运动，此时除了第i个关节之外的所有其他关节均固定于初始位形；\n[0052] The Unimation PUMA560机器人的结构特点为，前三个轴控制末端法兰的空间位置，后三个轴控制末端法兰的空间姿态，即机器人连杆的后三个关节的轴线交于一点。如图3所示，利用几何法，即根据Paden-Kahan子问题思想建立与后三个关节相关的函数SubProb_3R(ξ4，ξ5，ξ6，p，q）求解θ4，θ5，θ6的值，其中ξ4，ξ5，ξ6为三个零节距的单位运动旋量，p，q为空间两点，ξ4，ξ5，ξ6的三个空间转轴之间的关系是ξ4空间垂直于ξ5和ξ6，ξ5空间平行于ξ6；对θ4，θ5，θ6三个旋转角度的求解包括对一个单轴旋转角度θ6的求解和对一个双轴旋转角度θ4，θ5的求解。\n[0053] 如图4所示，对一个单轴旋转角度θ6的求解步骤包括：\n[0054] (1)设d为转轴上一点，定义u＝p-d，v＝q-d，根据位置不变原则有：\n则\n[0055] (2)定义u′，v′为u，v在垂直于转轴ξ6的平面上的投影，则u′＝u-ωωTu，T\nv′＝v-ωωv，其中ω为关节旋转的法线；\n[0056] (3)当且仅当u，v在轴上的投影等长时，在轴ω上垂直的平面上投影也T T\n等长，即ωu＝ωv，||u′||＝||v′||，那么根据式子 和投影矢\n量u′，v′求得旋转角度，即对应第六关节的旋转角度θ6。如果u′≠0，则有-1 T ′T\n故θ＝tan 2(ω(u′×v′)，u v′)；如果u′＝0，\n则存在无穷多个解，此时p＝q且两点都在旋转轴上。\n[0057] 如图5所示，对一个双轴旋转角度θ4，θ5的求解步骤包括：\n[0058] (1)假定将点p绕给定轴ξ5旋转θ5，再绕给定轴ξ4旋转θ4到点q重合；\n[0059] (2)令 q1是转轴ξ 4上的任一点，由距离保持不变原则得到：\n令δ＝||q-q1||，则 令q2是转轴ξ5上\n任一点，并定义u＝p-q2，v＝q1-q2，则\n[0060] (3)将上述两点向垂直于转轴ξ5的平面做投影，并定义u′，v′为u，v在垂直于T T\n转轴ξ5的平面上的投影，则u′＝u-ω2ω2u，v′＝v-ω2ω2v，其中ω2为关节旋转的法\n2 2 T 2\n线，同样对δ投影，得u′ ＝δ -||ω2(p-q1)||，即\n-1 T T\n[0061] (4)设θ为矢量u′与v′之间的夹角，则θ＝tan 2(ω(u′×v′)，u′ v′，\n2 2 2\n利用余弦定理求解φ＝θ-θ5，有||u′||+||v′||-2||u′||||v′||cosφ＝δ，因此， 此式有无解取决于半径为||u′||的圆与半径\n2\n为δ′ 的圆的交点数目；\n[0062] (5)已知θ5，则由 求得p1，再根据 计算θ4，计算方法与\n求单轴角度类似，求得双轴旋转角度，即分别对应第四和第五关节的旋转角度θ4和θ5。\n[0063] 再根据矩阵齐次关系通用式：\n[0064] \n[0065] 结合参数可以得到每一个连杆变换矩阵：\n[0066] \n[0067] \n[0068] \n[0069] 根据\n可以得\n到 的值，将上式变换为：\n[0070] \n[0071] 由于已经通过几何法计算出θ4，θ5，θ6三个数据，联立 利用代\n数消元法即可求出θ1，θ2，θ3。\n[0072] 实施例2\n[0073] 如图6所示，以Kawasaki EE10机器人结构为例说明禁忌搜索法的应用，本实施例与实施例1的区别在于，当机器人需要控制的手腕为非正交球型手腕或者非正交非球型手腕的结构，即机器人连杆的后三个关节的轴线不交于一点时，直接利用几何法和代数消元法求运动学逆解非常困难，因此在计算过程中引入禁忌搜索法。其步骤包括：\n[0074] (1)建立连杆坐标系：将机器人连杆放入坐标系，固定端为基坐标，依次往后的六个关节轴旋转角度分别设为变量θ1，θ2，θ3，θ4，θ5，θ6；\n[0075] (2)假设θ6为已知值，利用几何法和代数消元法求取θ1，θ2，θ3，θ4，θ5得到θ1＝β1，θ2＝β2，θ3＝β3，θ4＝β4，θ5＝β5，其中β1，β2，β3，β4，β5为解析解表达式；\n[0076] (3)根据六自由度机器人运动学正解的求解公式，得到正解表达式\n[0077] \n[0078] (4)将机器人连杆的结果位姿与初始位姿进行对比，得表达式 其\n中 为初始位姿；\n[0079] (5)由于设计机器人结构过程中机器人的行程范围已经确定，所以必然存在能够满足结果位姿逆解的关节值，故利用禁忌搜索法或爬山法等优化算法以γ为目标，调整θ6的值使其满足|γ|≤σ，其中σ为误差阈值，得到θ1，θ2，θ3，θ4，θ5，θ6的数字解，具体步骤包括：\n[0080] (5a)设θ6为具体数值；\n[0081] (5b)根据步骤(2)～(4)得到γ的值；\n[0082] (5c)对比|γ|与误差阈值σ的大小；\n[0083] (5d)若|γ|≤σ，则该具体数值为θ6的数字解，若|γ|＞σ，则根据禁忌搜索法或者爬山法优化侧重点，重新设定θ6为具体数值；\n[0084] (5e)重复执行步骤(5b)～(5d)，直至得到符合要求的θ6的数字解，此时，该θ6对应的由步骤(2)得到的θ1，θ2，θ3，θ4，θ5的值即为其相应的数字解。\n[0085] 按照上述实施例，便可很好地实现本发明。