一种七自由度偏置机械臂逆解方法与流程

本发明属于一种拟人七自由度偏置机械臂逆解方法，涉及拟人七自由度机械臂逆解过程中机械臂初始姿态计算的一种方法，应用对象包括空间机械臂和地面机械臂，可应用于空间机械臂的任务规划，尤其更适用于障碍物规避，空间立体感更加形象。

背景技术：

随着航天事业的发展，空间设施越来越多，空间机械臂在未来将越来越重要的作用。由于冗余机械臂在障碍回避、奇异处理、关节力矩优化等方面具有较大的优势，根据在轨任务的需求分析采用冗余度机械臂的重要性，然后从奇异消除、工作空间优化、运动学简化、关节运动范围、机械制造可实现性等方面进行臂型论证，最后采取在最优六自由度机械臂基础上，在肩部增加一个关节，构成“3+1+3”配置的臂型方案，该机械表的肩部和腕部三个关节轴线相较于一点，运动学方程简单，先解出非偏置机械臂(srs)的逆解，在利用偏置机械臂与非偏置机械臂(ssrms)逆解之间的对应关系可得到偏置机械臂解析形式的运动学逆解。机械臂在执行笛卡尔空间轨迹的运动时，需要根据期望的末端位姿计算关节角，由于七自由度机械臂的位置逆解存在无穷中情况，为了降低求解难度和存在逆解的可能情况，首先规定三号坐标系的坐标原点在参考平面上时的姿态为初始姿态，并作为参考臂型，此时各量的右上标取为0加以区分，参考系标识在左上角；并引入壁型角的概念，从而求出期望位姿下的逆解。本发明是以冗余机械臂为研究对象，为相关运动学问题的研究奠定理论基础，并且该发明顺应了我国大力发展机器人行业，尤其发展空间机器人及其在轨服务技术的趋势，研究成果对未来空间机器人在轨服务的实际应用具有一定的理论和实际意义。

技术实现要素：

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种七自由度偏置机械臂逆解方法，结合几何图像，让人选择臂型是更加形象，更有利于臂型设计和障碍规避。

技术方案

一种七自由度偏置机械臂逆解方法，其特征在于步骤如下：

步骤1：建立机械臂末端位姿的4×4的齐次变换矩阵⁰t7

以基座为起始端，在关节部位建立0号坐标系，依次编号至末端执行器处建立7号坐标系，共建立0、1、2、3、4、5、6和7个关节部位的坐标系；所述的坐标系为d-h坐标系；

⁰p3为3号坐标系原点在0号坐标系中的表示，d1d2d3d4d5d6d7分别为1-7号坐标系之间z轴的偏移长度，a1a2a3a4a5a6a7分别为相邻坐标系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′1q′2q′3q′4q′5q′6q′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的齐次变换矩阵⁰t7；定义臂型角和臂型面，计算偏置机械臂肘部关节角θ4、肩部关节角和腕部关节角；根据偏置机械臂与非偏置机械臂逆解的关系，可求偏置机械臂的逆解。本方法在障碍回避、奇异处理、关节力矩优化等方面具有较大的优势；简化了七自由度关节机器人的逆解求解过程，提高了计算效率。采用本方法在机器人工作空间优化、关节型机器人运动路径规划等方面能提供形象的空间感，能直观的表达机械臂的臂型。

附图说明

图1：计算七自由度机械臂逆解流程图

图2：臂型面定义

图3：臂型角定义

图4：任意状态下各向量关系图

图5：初始状态下各向量关系图

图6：初始姿态下机械臂的姿态图

图7：机械臂臂型示意图

具体实施方式

现结合实施例、附图对本发明作进一步描述：

步骤一：建立机械臂末端位姿的4×4的齐次变换矩阵⁰t7

⁰t7为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＝[001]^t，则矢量v与点w所组成的平面为参考平面。令w＝w-s、e＝e-s。则有：

e＝⁰p3-[00d1]^t(3)

⁰p3为3号坐标系原点在0号坐标系中的表示，d1d2d3d4d5d6d7分别为1-7号坐标系之间z轴的偏移长度，a1a2a3a4a5a6a7分别为相邻坐标系z轴的垂直距离，α1α2α3α4α5α6α7分别相邻坐标系前一个坐标系z轴绕自己x轴旋转α角与后一个坐标系z轴重合，θ1θ2θ3θ4θ5θ6θ7分别为各个关节旋转的角度。当d2d6为0时为偏置机械臂对应的非偏置机械臂臂型。

臂型角便是参考面绕着向量w按右手螺旋定则旋转到与当前臂型面重合时，所转的角度，即为臂型角ψ。

矢量e在矢量w上的投影为：

当前臂型面内与向量w垂直的单位矢量可表示为：

参考平面内与矢量w垂直的单位向量可表示为：

根据式(5)和式(6)以及矢量点乘和叉乘的性质可得：

因此：

分别为向量w、p、k单位向量，以sψ,cψ，si,ci表示sinψcosψsinθicosθ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＝artan(a(a3+a4c4)-ba4s4,b(a3+a4c4)+aa4s4)

θ5＝θ′3+θ′4+θ′5-θ3-θ4

1)：计算非偏置初始姿态

其中

规定，三号坐标系的坐标原点在参考平面上时各量的右上标取为0加以区分，参考系标示在左上角，式(12)中为当臂型角ψ为0时3号坐标系在0号坐标系的方向余璇矩阵，分别为当3号坐标系原点在参考平面上时其的xyz轴的单位矢量在0号坐标系的表示，式(13)中为3号坐标系原点在参考平面时矢量e和4号坐标系x轴的单位矢量在0号坐标系的表达

由dh坐标系建系规则可知：

d3z3+a3x3＝e(14)

z3x3为第3号坐标系的zx轴的单位矢量。

由式(14)可得：

式(15)中⁰e⁰为已知量，证明如下：

向量e可以看作是由向量w绕向量l(次向量是v与ω叉乘所得)旋转α角度所得。此时则有：

⁰e＝r(l,α)·w(16)

其中⁰e为当第3号坐标系原点在参考平面上时矢量e的表示，其中l为：

l＝v×w(17)

r(l,α)＝i3+[ul×]sin(α)+[ul×]²(1-cos(α))(18)

[ul×]表示向量l单位化后的叉乘因子，i3为三阶单位矩阵。

由式(16)可得：

⁰e⁰＝⁰e＝r(l,α)·w(19)

式(19)中只有α是未知的，α的求解过程如下：

在直角三角形ss⊥e中，由勾股定理可得：

在δse0w中由余弦定理可得:

将式(20)带入式(21)可得：

在δse0w中，由于即，边e0w不是δse0w中的最长边，所以α≤90°，故有：

至此，α求解完毕，α角可取两种情况，即⁰e⁰为已知量证明完毕。

由dh坐标系建系规则可知：

w-e＝a4x4(24)

式(24)中x4为第4号坐标系x轴的单位矢量。

由方向余弦表示的姿态变换关系可知：

³x4＝[c4s40]^t(25)

⁰x4＝⁰r3·³x4＝[⁰x3⁰y3⁰z3]³x4(26)

式(25)中³x4为4号坐标系的x轴的单位矢量在3号坐标系的表示，式⁰x4为4号坐标系的x轴的单位矢量在0号坐标系的表示，⁰r3为3号坐标系相对于0号坐标系的方向余弦矩阵，⁰x3⁰y3⁰z3分别为3号坐标系的xyz轴的单位矢量在0号坐标系的表示。由式(24)、式(25)和式(26)可得：

由于w、a4和⁰e⁰都是已知量，故也是已知量，其中表示当臂型角为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旋转ψ角后的姿态变换矩阵为：

⁰rψ＝i3+[⁰uw×]sinψ+[⁰uw×]²(1-cosψ)(33)

其中，i3为3×3的单位阵，[⁰uw×]为向量w的斜对称阵。由式(33)知三号坐标系在零号坐标下的姿态可表示为：

将方程(33)带入方程(34)可得：

⁰r3＝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)

结合式⁰t7＝⁰t1¹t2…⁶t7＝fkine(θ1,θ2,…,θ7)和式⁰r3＝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＝artan(a(a3+a4c4)-ba4s4,b(a3+a4c4)+aa4s4)

θ5＝θ′3+θ′4+θ′5-θ3-θ4

θ′4、α、θ2θ4θ6分别可取两种情况，因此偏置七自由度机械臂的逆解有32种情况。

步骤五：选择合理的一种逆解

设偏置机械臂初始机械臂姿态角为q＝[q′1q′2q′3q′4q′5q′6q′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所示

表2ssrms型机械臂的d-h参数

当机械臂末端位姿为

当求逆解时应当考虑障碍物规避问题，在末端位姿为上式的情况下，如图7所示。

当壁型角为60°时可能有障碍物挡住机械臂，此时我们可以找到合适的壁型角规避机械臂在运动时碰触到障碍物。即取壁型角为30°，即根据上述算法编程求解，可得到32种逆解，这时取在当前臂型变化最小的一组逆解

表332种机械臂逆解

经编程正运动学验证，上表32种逆解是正确的。

根据2-范数原理求得目标位姿与初始姿态变化最小的一组逆解，结果如下表所示。

表4相对于初始姿态变化最小的逆解

即逆解求出。