[发明专利]一种用于冗余机械臂可达性分析的逆解求解方法

权利要求书

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

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

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

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

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

2.根据权利要求1所述的用于冗余机械臂可达性分析的逆解求解方法，其特征在于：在所述步骤2中关节角为冗余参数进行逆运动学求解，即给定除关节角q₄任意一个关节角，计算其他关节角；

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

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

由⁰t_sw与⁰R₃(q₁,q₂,q₃)之间的关系得到：

Fsinq₃＝sw_ycosq₁-sw_xsinq₁ (1)

sw_z＝Ecosq₂-Fcosq₃sinq₂ (2)

sw_x,sw_y,sw_z分别是⁰t_sw的x,y,z轴的分量，且E＝d_ewcosq₄+d_se，F＝d_ewsinq₄；

根据式(1)与式(2)，得到关节角q₂与q₃，关于关节角q₁的表达式为：

sinq₃＝(sw_ycosq₁-sw_xsinq₁)/F (3)

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

q₆＝±arccos(⁴R₇(3,3)) (6)

q₅＝arctan2(sign(sinq₆)⁴R₇(2,3),sign(sinq₆)⁴R₇(1,3)) (7)

q₇＝arctan2(-sign(sinq₆)⁴R₇(3,1),sign(sinq₆)⁴R₇(3,2)) (8)

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

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

当以关节角q₂为冗余参数时，由⁰t_sw与⁰R₃(q₁,q₂,q₃)的关系化简得到：

sw_x＝(Esinq₂+Fcosq₂cosq₃)cosq₁-(Fsinq₃)sinq₁ (9)

sw_y＝(Esinq₂+Fcosq₂cosq₃)sinq₁+(Fsinq₃)cosq₁ (10)

sw_z＝-E(cosq₂)+F(sinq₂cosq₃) (11)

此时根据上式得到关节角q₁和q₃，关于关节角q₂的表达式为：

q₃＝±arccos((sw_z+Ecosq₂)/Fsinq₂) (12)

q₁＝arctan2(H_q2(1),H_q2(2)) (13)

其中H_q2的表达式如下：

同上确定变换矩阵⁴R₇后根据反余弦和反正切函数确定关节角q₅,q₆,q₇；

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

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

q₁＝arctan2(H_q2(1),H_q2(2)) (15)

同上确定变换矩阵⁴R₇后根据反余弦和反正切函数，确定关节角q₅,q₆,q₇；关节角q₃为冗余参数求冗余机器人逆解求解完成；

4)关节角q₅,q₆,q₇为冗余参数求冗余机器人逆解：

因为机械臂的对称的，将机械臂关节逆序进行计算，此时⁴R₇的逆当作⁰R₃处理，同样得到分别以关节角q₅,q₆,q₇为冗余参数的逆解。

3.根据权利要求1所述的用于冗余机械臂可达性分析的逆解求解方法，其特征在于：在所述步骤3中，通过定义逆解到关节限位的函数来判断当前逆解是否满足关节限位条件，若超出关节限位，描述当前逆解超出关节极限的大小以及超过关节限；函数的表达式如下：

Max_δ＝argmax_i{|δ₁|,|δ₂|,|δ₃|,|δ₅|,|δ₆|,|δ₇|} (18)

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