一种求UR机械臂逆运动学不受奇异位置影响的方法

　　本文将介绍一种新方法求解UR机械臂的逆运动学。这个方法来自我最近发表在《Mechanism and Machine Theory》上的文章。文末有该文的下载地址，文章中有很多细节将会隐去，各位可以下载原文细读。
　　网上已经有很多文章提供了UR机械臂的逆解方法，这些方法仍存在一些不足。主要是腕部关节的计算公式会受奇异位置的影响，即第四个关节与第六个关节的轴线共线时，这两个关节将难以计算。文章以链接中的UR机械臂为例，讲解求解方法。UR机械臂的图以及DH参数表，都来源于这篇博文。https://blog.csdn.net/fengyu19930920/article/details/81144042/
图1
图2
表1
　　需要注意的是，引用的这篇博文中用的是标准DH方法建立的正向运动学公式。
$T=T_1({\theta }_1)T_2({\theta }_2)T_3({\theta }_3)T_4({\theta }_4)T_5({\theta }_5)T_6({\theta }_6)$（1）

1 逆运动学求解

首先根据文献[1]中的方法，我们可以对公式(1)进行一下变形：
$T_L=T_2({\theta }_2)T_3({\theta }_3)T_4({\theta }_4)Rot(-{\alpha }_4,x_4)$（2）
$T_R=T^{-1}({\theta }_1)T{T}^{-1}({\theta }_6)T^{-1}({\theta }_5)$（3）

TL =[ cos(q2 + q3 + q4), -sin(q2 + q3 + q4), 0, a3*cos(q2 + q3) + a2*cos(q2)
      sin(q2 + q3 + q4),  cos(q2 + q3 + q4), 0, a3*sin(q2 + q3) + a2*sin(q2)
                      0,                  0, 1,                           d4
                      0,                  0, 0,                            1]
   
TR =[ - cos(q1)*(ax*sin(q5) - nx*cos(q5)*cos(q6) + ox*cos(q5)*sin(q6)) - sin(q1)*(ay*sin(q5) - ny*cos(q5)*cos(q6) + oy*cos(q5)*sin(q6)),  cos(q1)*(ax*cos(q5) + nx*cos(q6)*sin(q5) - ox*sin(q5)*sin(q6)) + sin(q1)*(ay*cos(q5) + ny*cos(q6)*sin(q5) - oy*sin(q5)*sin(q6)), - ox*cos(q1)*cos(q6) - nx*cos(q1)*sin(q6) - oy*cos(q6)*sin(q1) - ny*sin(q1)*sin(q6), cos(q1)*(px - ax*d6 + d5*ox*cos(q6) + d5*nx*sin(q6)) + sin(q1)*(py - ay*d6 + d5*oy*cos(q6) + d5*ny*sin(q6))
                                                                                   nz*cos(q5)*cos(q6) - az*sin(q5) - oz*cos(q5)*sin(q6),                                                                            az*cos(q5) + nz*cos(q6)*sin(q5) - oz*sin(q5)*sin(q6),                                                           - oz*cos(q6) - nz*sin(q6),                                                             pz - d1 - az*d6 + d5*oz*cos(q6) + d5*nz*sin(q6)
        cos(q1)*(ay*sin(q5) - ny*cos(q5)*cos(q6) + oy*cos(q5)*sin(q6)) - sin(q1)*(ax*sin(q5) - nx*cos(q5)*cos(q6) + ox*cos(q5)*sin(q6)), sin(q1)*(ax*cos(q5) + nx*cos(q6)*sin(q5) - ox*sin(q5)*sin(q6)) - cos(q1)*(ay*cos(q5) + ny*cos(q6)*sin(q5) - oy*sin(q5)*sin(q6)),   oy*cos(q1)*cos(q6) + ny*cos(q1)*sin(q6) - ox*cos(q6)*sin(q1) - nx*sin(q1)*sin(q6), sin(q1)*(px - ax*d6 + d5*ox*cos(q6) + d5*nx*sin(q6)) - cos(q1)*(py - ay*d6 + d5*oy*cos(q6) + d5*ny*sin(q6))
                                                                                                                                      0,                                                                                                                               0,                                                                                   0,                                                                                                           1];

正运动学方程的变形可以看作是把原来的机械臂拆分为两段子链，分别为$T_L$和$T_R$中。而$T_L$中乘以$Rot(-{\alpha }_4,x_4)$表示将子链$T_L$的末端姿态绕$x_4$轴旋转$-{\alpha }_4$弧度。这样做的目的是为了让子链$T_L$末端姿态的z轴与第四个关节的轴线重合，因为从图中我们可以知道第四个关节和第五个关节的轴线是相互垂直的。另外，采用(2)-(3)的方式变形，可以充分利用机械臂的几何信息。我们可以直观地看出子链$T_L$只包含关节2~关节4，这三个关节组成一个平面结构，它的末端姿态中$z_L=[0,0,1{]}^T$。这样后面推导逆解公式将会变得很方便。

1.1 求解${\theta }_1$

由前面的变形可以知道，子链$T_L$末端姿态的分向量与关节2至4无关，位置$P_{LZ}$也与这三个关节无关，而子链$T_L$中对应的部分至于关节1和关节6有关，因此可以得到下列等式：
　　$P_{LZ}=P_{RZ}$
　　$s_1(p_x-a_x{d}_6+d_5{o}_x{c}_6)+d_5{n}_x{s}_6)-c_1(p_y-a_y{d}_6+d_5{o}_y{c}_6+d_5{n}_y{s}_6)=d_4$ （4）
　　
　　$z_L{z}_R=0$
　　$(o_y{c}_6+n_y{s}_6)c_1-(o_x{c}_6+n_x{s}_6)s_1=0$ （5）
　　将(5)代入(4)中可以消掉${\theta }_6$，得到只含${\theta }_1$的三角函数式子：
　　　　$s_1(p_x-a_x{d}_6)-c_1(p_y-a_y{d}_6)=d_4$ （6）
解得${\theta }_1$,

A1=px - ax*d6;
B1=py - ay*d6;
q1=atan2(d4,k1*sqrt(A1^2+B1^2-d4^2))+atan2(B1,A1)

${\theta }_1$表达式中根号部分，表示目标位姿点是否在机械臂的工作空间内，若不在，则根号内为负号无解。$k1=\pm 1$。代码中的q表示$\theta$，下同。

1.2 求解${\theta }_6$

根据两个子链末端的z轴互相垂直$z_L\perp z_R$,有
$o_y{c}_1{c}_6+n_y{c}_1{s}_6-o_x{c}_6{s}_1-n_x{s}_1{s}_6＝\mathrm{０}$ （7）
解得

A6=ny*cos(q1) - nx*sin(q1);
B6=oy*cos(q1) - ox*sin(q1);
q6=atan2(0,k2)-atan2(B6,A6)

式中$k2=\pm 1$。

1.3 求解${\theta }_2$和${\theta }_3$

由$P_{Lx}=P_{Rx}$ 和$P_{Ly}=P_{Ry}$ 得到方程组　
$a_3{c}_{23}+a_2{c}_2=c_1(p_x-a_x{d}_6+d_5{o}_x{c}_6+d_5{n}_x{s}_6)+s_1(p_y-a_y{d}_6+d_5{o}_y{c}_6+d_5{n}_y{s}_6)$ （8）
$a_3{s}_{23}+a_2{s}_2=p_z-d_1-a_z{d}_6+d_5{o}_z{c}_6+d_5{n}_z{s}_6$ （9）
首先将式(8)和(9)两边同时平方相加可得到${\theta }_3$，

p1= cos(q1)*(px - ax*d6 + d5*ox*cos(q6) + d5*nx*sin(q6)) + sin(q1)*(py - ay*d6 + d5*oy*cos(q6) + d5*ny*sin(q6));
p2= pz - d1 - az*d6 + d5*oz*cos(q6) + d5*nz*sin(q6);

c3=(p1^2+p2^2-a2^2 - a3^2)/(2*a2*a3);
s3=k3*sqrt(1-c3^2);
q3=atan2(s3,c3)

式中$p_1和p_2$分别表示式(8)和(9)等号右边的部分。$k3=\pm 1$。

然后将式(8)和(9)中$c_{23}和s_{23}$都移到等号右边，接着两边平方相加，得到

A23=2*a3*p1;
B23=2*a3*p2;
C23=a3^2 + p1^2 + p2^2-a2^2;
q23=atan2(k3*sqrt(A23^2+B23^2-C23^2),C23)+atan2(B23,A23)

式中$p_1和p_2,k3$同上。

最后${\theta }_2={\theta }_{23}-{\theta }_3$。

1.4 求解${\theta }_5$

根据参考文献[1]中的条件式(9)有，${\mathbf{z}}_{\mathbf{L}}^{\mathbf{T}}{\mathbf{y}}_{\mathbf{R}}=1$。之前我们知道$z_L=[0,0,1{]}^T$，因此可以很容易得到式子
$s_1(a_x{c}_5+n_x{c}_6{s}_5-o_x{s}_5{s}_6)-c_1(a_y{c}_5+n_y{c}_6{s}_5-o_y{s}_5{s}_6)=1$ （10）
由该式可以得到${\theta }_5$，

A5=( - ny*cos(q1)*cos(q6) + nx*cos(q6)*sin(q1) + oy*cos(q1)*sin(q6) - ox*sin(q1)*sin(q6));
B5= ax*sin(q1) - ay*cos(q1);
q5=atan2(0,1)+atan2(A5,B5);

需要注意的是，由文献[1]中的方法得到之前的四个关节角后，剩余两个角都将是唯一对应，求解过程中带有正负符号的部分直接等于0。大家可以自己把两个子链的末端姿态画出来看就能明白，我在我的硕士毕业论文中也给出了证明，因为刚毕业，现在还没在知网上出版。各位先记住这个结论即可，当然也可以尝试证明，我用的是三余弦定理证明的。

1.5 求解${\theta }_4$

${\theta }_4$的求解也可以按照文献[1]中的条件式(8)来，但是我们观察两个子链姿态中$x_L和x_R$的表达式，会发现
$c_{234}=-c_1(a_x{s}_5-n_x{c}_5{c}_6+o_x{c}_5{s}_6)-s_1(a_y{s}_5-n_y{c}_5{c}_6+o_y{c}_5{s}_6)$（11）
$s_{234}=n_z{c}_5{c}_6-a_z{s}_5-o_z{c}_5{s}_6$（12）
解得，

A4= nz*cos(q5)*cos(q6) - az*sin(q5) - oz*cos(q5)*sin(q6);
B4=-cos(q1)*(ax*sin(q5) - nx*cos(q5)*cos(q6) + ox*cos(q5)*sin(q6)) - sin(q1)*(ay*sin(q5) - ny*cos(q5)*cos(q6) + oy*cos(q5)*sin(q6));
q234=atan2(A4,B4);
q4=q234-q23;

至此UR机械臂的所有逆解都求出来了。接下来我们验证奇异位置时能否得到逆运动学解。

2. 验证求奇异位置逆解

从图以及建立的DH参数表可以知道，${\theta }_5=0$时，关节4与关节6的轴线是共线的，此时UR机械臂处于腕部奇异位置。我们令目标位姿由以下关节角组合得到${\theta }_d=[1,1,1,1,0,1]$,代入正向运动学公式得到目标位姿$T_d$

Td =

   1.0e+02 *

  -0.003531651555686   0.004089021333016   0.008414709848079   1.324431340991907
  -0.005500221413615   0.006368273410318  -0.005403023058681  -1.480707090264286
  -0.007568024953079  -0.006536436208636   0.000000000000000  -5.311352944124976
                   0                   0                   0   0.010000000000000

它所对应的所有逆解为

AQ =

   0.095363758584843   0.850264373394017   1.736680792136842  -2.586945165530858  -0.904636241415157  -2.283185307179587
   0.095363758584843   2.492335031398676  -1.736680792136842  -0.755654239261834  -0.904636241415157  -2.283185307179587
   0.095363758584843   1.166295711457801   1.000859439511193   0.974437502620798   0.904636241415157   0.858407346410206
   0.095363758584843   2.123333020453949  -1.000859439511193   2.019119072647037   0.904636241415157   0.858407346410206
   1.000000000000001   1.006084796303796   0.999365806146063   1.038416022677409  -0.000000000000001   0.956133374872731
   1.000000000000001   1.961706157981659  -0.999365806146063   2.081526273291673  -0.000000000000001   0.956133374872731
   1.000000000000001   0.666163375380177   1.737955362927514  -2.501844766770216   0.000000000000001  -2.185459278717062
   1.000000000000001   2.309386429160488  -1.737955362927514  -0.669157094695499   0.000000000000001  -2.185459278717062

这些解对应的位姿与目标位姿之间的误差为

delta =

   1.0e-12 *

   0.236088728961565   0.130244884890050   0.236088466354214   0.212690026999602   0.153055985103290   0.360631166302336   0.102475995454713   0.170530682241551

从结果可以看出，求解的位姿误差范数达到10的负-12次方数量级，且求解公式不受奇异位置的影响。

3. 代码

正向运动学的代码。

3.1 标准DH方法的齐次变换矩阵

function [ T ] = stdtrans( q,d,a,alfa )
%TRANS 此处显示有关此函数的摘要
%   此处显示详细说明
T=[cos(q) -sin(q)*cos(alfa)   sin(q)*sin(alfa)  a*cos(q);
   sin(q)  cos(q)*cos(alfa)  -cos(q)*sin(alfa)  a*sin(q);
       0         sin(alfa)    cos(alfa)         d;
    0 0 0 1];
end

3.2 正向运动学

function [T] = FW(q)

alfa=pi/2;
d1=89.459;
a2=-425 ;
a3=-392.25 ;
d4=109.15;
d5=94.65;
d6=82.3;
T1=stdtrans(q(1),     d1,  0, alfa);
T2=stdtrans(q(2),     0,  a2, 0);
T3=stdtrans(q(3),     0,  a3, 0);
T4=stdtrans(q(4),     d4,  0, alfa);
T5=stdtrans(q(5),     d5,  0, -alfa);
T6=stdtrans(q(6),     d6,  0, 0);

T=T1*T2*T3*T4*T5*T6;
end

3.3 逆运动学

function [Q] = IK(T,K)

d1=89.459;
a2=-425 ;
a3=-392.25 ;
d4=109.15;
d5=94.65;
d6=82.3;
k1=K(1);
k2=K(2);
k3=K(3);


px=T(1,4); py=T(2,4); pz=T(3,4);
ax=T(1,3); ay=T(2,3); az=T(3,3);
ox=T(1,2); oy=T(2,2); oz=T(3,2);
nx=T(1,1); ny=T(2,1); nz=T(3,1);


A1=px - ax*d6;
B1=py - ay*d6;
q1=atan2(d4,k1*sqrt(A1^2+B1^2-d4^2))+atan2(B1,A1);

% %  + (ny*cos(q1) - nx*sin(q1))*sin(q6)+(oy*cos(q1)- ox*sin(q1))*cos(q6)

A6=ny*cos(q1) - nx*sin(q1);
B6=oy*cos(q1) - ox*sin(q1);
q6=atan2(0,k2)-atan2(B6,A6);

%%%%%%%%%%%%%%%
p1= cos(q1)*(px - ax*d6 + d5*ox*cos(q6) + d5*nx*sin(q6)) + sin(q1)*(py - ay*d6 + d5*oy*cos(q6) + d5*ny*sin(q6));
p2= pz - d1 - az*d6 + d5*oz*cos(q6) + d5*nz*sin(q6);


c3=(p1^2+p2^2-a2^2 - a3^2)/(2*a2*a3);
s3=k3*sqrt(1-c3^2);
q3=atan2(s3,c3);

% % % a3^2 - 2*cos(q2 + q3)*a3*p1 - 2*sin(q2 + q3)*a3*p2 + p1^2 + p2^2-a2^2
A23=2*a3*p1;
B23=2*a3*p2;
C23=a3^2 + p1^2 + p2^2-a2^2;
q23=atan2(k3*sqrt(A23^2+B23^2-C23^2),C23)+atan2(B23,A23);

%%%%%%%%%%%%%%%
q2=q23-q3;


% % % ax*cos(q5)*sin(q1) - ay*cos(q1)*cos(q5) - ny*cos(q1)*cos(q6)*sin(q5) + nx*cos(q6)*sin(q1)*sin(q5) + oy*cos(q1)*sin(q5)*sin(q6) - ox*sin(q1)*sin(q5)*sin(q6)
%%%%%%%%%%%%%%%
% % (ax*sin(q1) - ay*cos(q1))*cos(q5)+( - ny*cos(q1)*cos(q6) + nx*cos(q6)*sin(q1) + oy*cos(q1)*sin(q6) - ox*sin(q1)*sin(q6))*sin(q5)
A5=( - ny*cos(q1)*cos(q6) + nx*cos(q6)*sin(q1) + oy*cos(q1)*sin(q6) - ox*sin(q1)*sin(q6));
B5= ax*sin(q1) - ay*cos(q1);
q5=atan2(0,1)+atan2(A5,B5);



%%%%%%%%%%%%%
A4= nz*cos(q5)*cos(q6) - az*sin(q5) - oz*cos(q5)*sin(q6);
B4=-cos(q1)*(ax*sin(q5) - nx*cos(q5)*cos(q6) + ox*cos(q5)*sin(q6)) - sin(q1)*(ay*sin(q5) - ny*cos(q5)*cos(q6) + oy*cos(q5)*sin(q6));
q234=atan2(A4,B4);

q4=q234-q23;

Q=[q1 q2 q3 q4 q5 q6];
end

3.4 总运行m程序

clear
clc

K=[1  1  1
   1  1 -1
   1 -1  1 
   1 -1 -1
   -1  1  1
   -1  1 -1
   -1 -1  1 
   -1 -1 -1];
   
   
Td=FW([1 1 1 1 0 1])

for i=1:8
Q=IK(Td,K(i,:));

%%%%%%将各个关节角的值转换到[-pi,pi]
for j=1:6
    if Q(j)<-pi
      Q(j)=Q(j)+2*pi;
    elseif Q(j)>pi 
      Q(j)=Q(j)-2*pi;
    else
       Q(j)=Q(j);
    end
end

T1=FW(Q);
delta(i)=norm(T1-Td);
AQ(i,:)=Q;

end

AQ

4 参考文献

[1] Xiao F, Li G, Jiang D, et al. An effective and unified method to derive the inverse kinematics formulas of general six-DOF manipulator with simple geometry[J]. Mechanism and Machine Theory, 2021, 159: 104265.

这篇论文目前是online阶段，希望里面的内容能帮助国内各位同行。也衷心希望各位写论文时，提到逆运动学的部分能把这篇引用进去。有问题的朋友可以留言，谢谢各位！