一种用于冗余机械臂可达性分析的逆解求解方法与流程

本发明属于机械臂运动学领域，具体涉及一种用于冗余机械臂可达性分析的逆解求解方法。

背景技术

目前，七自由度机械臂因为其存在冗余的特征，在保证末端任务实现的情况下关节层仍然有一个自由度可以独立运动，可用于实现操作度优化、关节力矩优化、避障等任务，这种类型的机器人已经成为当前机器人研究的热点与重点。

然而冗余的存在给七自由度机械臂逆解的求解带来困难，通过查询文献和综合分析发现，尽管国内外对七自由度机械臂逆解的研究较多，通常分为数值解和解析解求解，但在可达性分析中需考虑关节极限的情况下求解全局所有姿态的逆解时，数值解求解方法因其是否能快速准确得到逆解依赖于初解的选择故不适用，当前常用的解析解求解基于臂形角的逆解求解因其需首先给定臂形角的值才能得到逆解，而给定臂形角的大小亦不能保证逆解满足关节限位条件，故也不适用。基于单个关节角为冗余参数的解析解逆解求解亦不能保证得到的逆解满足关节限位条件。

在可达性分析中，我们通常首先划分机械臂的六维工作空间，然后进行逆解求解计算，需在无人为判断的情况下求解全局姿态是否存在满足关节限位的可达逆解。如今存在的在考虑关节极限的情况下判断全局的机械臂的姿态的逆解的可能可行的方案在shimizum,kakuyah,yoonwk,etal.analyticalinversekinematiccomputationfor7-dofredundantmanipulatorswithjointlimitsanditsapplicationtoredundancyresolution[j].ieeetransactionsonrobotics,2008,24(5):1131-1142.得到分析，其得到了臂形角与关节极限的关系，求得所有满足关节限位条件的臂形角。然而该方法得到关节极限与臂形角的关系情况较多、较复杂，且在求解得到每个关节的关节极限对应的臂形角的并集时较为困难，通常需人为干预进行并集计算，故该方法也不适合。

技术实现要素：

为了克服上述现有技术的不足，本发明提供了一种用于冗余机械臂可达性分析的逆解求解方法。考虑关节角为冗余参数求解逆解时可能不能得到满足关节限位的逆解，判断超出关节限位最大的关节角，利用冗余机械臂的自运动，得到靠近关节限位的更新后的关节角，重新进行逆解计算，直到求得满足关节限位条件的可行逆解。

本发明所采用的技术方案是：

一种用于冗余机械臂可达性分析的逆解求解方法，包括以下步骤：

步骤1：采用标准d-h方法建立机械臂的连杆坐标关系，机械臂的前三个旋转关节(q1,q2,q3)的关节轴线相交于一点s，故将前三个关节等效为一个虚拟的球关节；机械臂的后三个旋转关节(q5,q6,q7)的关节轴线亦相交于一点w，故也将后三个关节等效为一个虚拟的球关节；第四个关节角q4是一个旋转关节，其连杆坐标系中心为点w；机械臂的末端与第七个关节连杆坐标系重合；根据机械臂运动学得到逆运动学计算所需的相关参数的关系；

步骤2：对于srs构型的机械臂，末端姿态固定，关节空间仍然能够独立运动，这种运动称为冗余机器人的自运动，然而此时两个虚拟的球关节中心的位置能够唯一确定，根据三角形的余弦公式，能够得到关节角q4，再以一个关节角为冗余参数进行逆运动学求解，即给定除关节角q4外任意一个关节角，计算其他关节角；

步骤3：对于给定的末端姿态，首先以关节角q3＝0为冗余参数求解一组逆解，在考虑关节限位的情况下，该组逆解或许不满足关节限位条件，通过定义所得逆解到关节极限位置关系的函数来判断当前逆解是否满足关节限位条件，若超出关节限位，该函数的值能够描述当前逆解超出关节限位的大小以及超过关节限位的方向，即是大于关节极限的最大值或小于关节极限的最小值；

步骤4：通过上述的函数定义判断得到此时距离关节限位最远的关节角qj，以一定的步长更新该关节角的大小，使其靠近关节极限，得到新的关节角再以为冗余机械臂的冗余参数进行逆运动学求解得到一组新的逆解，用步骤3中定义的函数判断该组逆解是否满足关节限位条件；若得到可行逆解，则求解过程结束，给定姿态是机械臂可达的姿态；若该组逆解亦不满足关节限位的条件，则重复步骤4直到求解得到满足关节限位条件的可行逆解或者迭代次数超出设定的最大迭代次数，则此时认为该给定的机械臂末端姿态是不可达。

在所述步骤2中关节角为冗余参数进行逆运动学求解，即给定除关节角q4任意一个关节角，计算其他关节角；

对于给定的末端姿态，两个虚拟球关节的中心位置即能够确定且机械臂的连杆长度已经得到，因此根据三角形的余弦公式，首先判断关节角q4的大小，且其不随机械臂自运动而改变；同时虚拟球关节的球心连线的位置矢量⁰tsw与虚拟球关节的方向变换关系⁰r3(q1,q2,q3)之间的关系能够得到，用于求解关节角为冗余参数的逆解，具体步骤如下：

1)关节角q1为冗余参数求冗余机器人逆解：

由⁰tsw与⁰r3(q1,q2,q3)之间的关系得到：

fsinq3＝swycosq1-swxsinq1(1)

swz＝ecosq2-fcosq3sinq2(2)

swx,swy,swz分别是⁰tsw的x,y,z轴的分量，且e＝dewcosq4+dse，f＝dewsinq4；

根据式(1)与式(2)，得到关节角q2与q3，关于关节角q1的表达式为：

sinq3＝(swycosq1-swxsinq1)/f(3)

根据连杆的传递关系，得到另一个虚拟球关节的方向变换矩阵⁴r7，又因为其关于q5,q6,q7的表达式如下：

q6＝±arccos(⁴r7(3,3))(6)

q5＝arctan2(sign(sinq6)⁴r7(2,3),sign(sinq6)⁴r7(1,3))(7)

q7＝arctan2(-sign(sinq6)⁴r7(3,1),sign(sinq6)⁴r7(3,2))(8)

因此，完成基于关节角q1为冗余参数的逆解求解；

2)关节角q2为冗余参数求冗余机器人逆解：

当以关节角q2为冗余参数时，由⁰tsw与⁰r3(q1,q2,q3)的关系化简得到：

swx＝(esinq2+fcosq2cosq3)cosq1-(fsinq3)sinq1(9)

swy＝(esinq2+fcosq2cosq3)sinq1+(fsinq3)cosq1(10)

swz＝-e(cosq2)+f(sinq2cosq3)(11)

此时根据上式得到关节角q1和q3，关于关节角q2的表达式为：

q3＝±arccos((swz+ecosq2)/fsinq2)(12)

q1＝arctan2(hq2(1),hq2(2))(13)

其中hq2的表达式如下：

同上确定变换矩阵⁴r7后根据反余弦和反正切函数确定关节角q5,q6,q7；

3)关节角q3为冗余参数求冗余机器人逆解：

当以关节角q3为冗余参数时，根据式(9)，式(10)与式(11)，q1,q2也计算得到：

q1＝arctan2(hq2(1),hq2(2))(15)

同上确定变换矩阵⁴r7后根据反余弦和反正切函数，确定关节角q5,q6,q7；关节角q3为冗余参数求冗余机器人逆解求解完成；

4)关节角q5,q6,q7为冗余参数求冗余机器人逆解：

因为机械臂的对称的，将机械臂关节逆序进行计算，此时⁴r7的逆当作⁰r3处理，同样得到分别以关节角q5,q6,q7为冗余参数的逆解。

在所述步骤3中，通过定义逆解到关节限位的函数来判断当前逆解是否满足关节限位条件，若超出关节限位，描述当前逆解超出关节极限的大小以及超过关节限；函数的表达式如下：

maxδ＝argmaxi{|δ1|,|δ2|,|δ3|,|δ5|,|δ6|,|δ7|}(18)

式中表示第i个关节的左极限，表示第i个关节的右极限；变量maxδ的定义是用来得到6个关节角离关节极限最远的关节，若得到的δi不全为0，则判定该逆解不满足关节限位条件，使用函数maxδ得到离关节限位最大的关节角j更新得到下次迭代的关节角qj(k+1)＝qj(k)+sign(δj)δqj，δqj为更新的关节角的大小，并使用qj(k+1)作为冗余参数继续进行逆解求解。

采用上述技术方案，本发明与现有技术相比有这样的有益效果：

本发明方法无需给定初解，在考虑关节极限的情况下可直接判断全局机械臂逆解是否可达。该方法的特点是随意给定末端姿态即可判断该姿态是否是满足关节限位条件的可达位姿。在求解机械臂的可达性时，首先划分六维空间的位姿，使用该方法可以没有人为干预的情况下判断所给的位姿是否是可达的。

附图说明

图1是本发明一种用于冗余机械臂可达性分析的逆解求解方法算法流程图。

图2是七自由度机械臂的连杆坐标示意图。

图3是七自由度机械臂简图及其自运动示意图。

图4是关节限位的描述图。

具体实施方式

下面结合附图以及具体实施方式对本发明做进一步描述：

如图1所示，一种用于冗余机械臂可达性分析的逆解求解方法，实现过程如下：

步骤1：采用标准d-h方法建立机械臂的连杆坐标关系，其连杆坐标关系如图2所示，且机械臂的前三个旋转关节(q1,q2,q3)的关节轴线相交于一点s，故可以将前三个关节等效为一个虚拟的球关节；机械臂的后三个旋转关节(q5,q6,q7)的关节轴线亦相交于一点w，故也可以将后三个关节等效为一个虚拟的球关节；第四个关节角q4是一个旋转关节，其连杆坐标系中心为w；机械臂的末端与第七个关节连杆坐标系重合；等效后的机械臂可以简化为两个球关节和一个旋转关节，如图3所示。根据机械臂运动学得到逆运动学计算所需的相关参数的关系。

步骤2：如图3所示，对于这种构型的机械臂，末端姿态固定，关节空间仍然可以独立运动，这种运动我们称为冗余机器人的自运动，在末端不动情况下机械臂关节配置因自运动可以由bsew变为bse’w。此时两个虚拟的球关节中心的位置s与w可以唯一确定，根据三角形的余弦公式，关节角q4可以得到，再以一个关节角为冗余参数进行逆运动学求解，即给定除关节角q4任意一个关节角，计算其他关节角。

步骤3：对于给定的末端姿态⁰r7,⁰t7，首先以关节角q3＝0为冗余参数求解一组逆解，在考虑关节限位的情况下，该组逆解可能不满足关节限位条件，通过定义所得逆解到关节限位关系的函数来判断当前逆解是否满足关节限位条件，若超出关节限位，该函数的值可以描述当前逆解超出关节限位的大小以及超过关节限位的方向(即是大于关节极限的最大值或小于关节极限的最小值)。

步骤4：用步骤3中定义的函数判断该组逆解是否满足关节限位条件。若得到可行逆解，则求解过程结束，给定姿态是机械臂可达的姿态；若该组逆解不满足关节限位的条件，通过上述的函数定义我们可以判断得到此时距离关节限位最远的关节角qj，以一定的步长更新该关节角的大小，使其靠近关节极限，得到新的关节角再以为冗余机械臂的冗余参数进行逆运动学求解得到一组新的逆解，则重复步骤4直到求解得到满足关节限位条件的可行逆解或者迭代次数超出设定的最大迭代次数，则此时认为该给定的机械臂末端姿态是不可达。

进一步地，在步骤2中关节角为冗余参数进行逆运动学求解，即给定除关节角q4任意一个关节角，计算其他关节角。

对于给定的末端姿态，两个虚拟球关节的中心位置即可以确定且机械臂的连杆长度已经得到，因此根据三角形的余弦公式可以首先判断关节角q4的大小，且其不随机械臂自运动而改变。同时虚拟球关节的球心连线的位置矢量⁰tsw与虚拟球关节的方向变换关系⁰r3(q1,q2,q3)之间的关系可以得到，可以用于求解关节角为冗余参数的逆解。

1)关节角q1为冗余参数求冗余机器人逆解：

由⁰tsw与⁰r3(q1,q2,q3)之间的关系可以得到：

fsinq3＝swycosq1-swxsinq1(1)

swz＝ecosq2-fcosq3sinq2(2)

swx,swy,swz分别是⁰tsw的x,y,z轴的分量，且e＝dewcosq4+dse，f＝dewsinq4。

根据上式(1)与式(2)，可以得到关节角q2与q3,关于关节角q1的表达式

sinq3＝(swycosq1-swxsinq1)/f(3)

根据连杆的传递关系，我们可以得到另一个虚拟球关节的方向变换矩阵⁴r7，又因为其关于q5,q6,q7的表达式可以如下：

q6＝±arccos(⁴r7(3,3))(6)

q5＝arctan2(sign(sinq6)⁴r7(2,3),sign(sinq6)⁴r7(1,3))(7)

q7＝arctan2(-sign(sinq6)⁴r7(3,1),sign(sinq6)⁴r7(3,2))(8)

因此，完成基于关节角q1为冗余参数的逆解求解。

2)关节角q2为冗余参数求冗余机器人逆解：

当以关节角q2为冗余参数时，由⁰tsw与⁰r3(q1,q2,q3)的关系化简得到：

swx＝(esinq2+fcosq2cosq3)cosq1-(fsinq3)sinq1(9)

swy＝(esinq2+fcosq2cosq3)sinq1+(fsinq3)cosq1(10)

swz＝e(cosq2)-f(sinq2cosq3)(11)

此时可根据上式得到关节角q1和q3，关于关节角q2的表达式为：

q3＝±arccos((swz+ecosq2)/fsinq2)(12)

q1＝arctan2(hq2(1),hq2(2))(13)

其中hq2的表达式如下：

同上确定变换矩阵⁴r7后根据反余弦和反正切函数可以确定关节角q5,q6,q7。

3)关节角q3为冗余参数求冗余机器人逆解：

当以关节角q3为冗余参数时，根据式(9)，式(10)与式(11)，q1,q2也可计算得到：

q1＝arctan2(hq2(1),hq2(2))(15)

同上确定变换矩阵⁴r7后根据反余弦和反正切函数可以确定关节角q5,q6,q7。关节角q3为冗余参数求冗余机器人逆解求解完成。

4)关节角q5,q6,q7为冗余参数求冗余机器人逆解：

因为机械臂的对称的，可以将机械臂关节逆序进行计算，此时⁴r7的逆当作0r3处理，同样可以得到分别以关节角q5,q6,q7为冗余参数的逆解。

进一步地，在步骤3中，通过定义逆解到关节限位的函数来判断当前逆解是否满足关节限位条件，若超出关节限位，描述当前逆解超出关节极限的大小以及超过关节限，该函数的表达式如下，函数随值随qi变化如图4所示。

maxδ＝argmax{|δ1|,|δ2|,|δ3|,|δ5|,|δ6|,|δ7|}(18)

式(17)中表示第i个关节的左极限，表示第i个关节的右极限。变量maxδ的定义是用来得到6个关节角离关节极限最远的关节。若得到的δi不全为0，则判定该逆解不满足关节限位条件，使用函数maxδ得到离关节限位最大的关节角j更新得到下次迭代的关节角qj(k+1)＝qj(k)+sign(δj)δqj，δqj为更新的关节角的大小，并使用qj(k+1)作为冗余参数继续进行逆解求解。