devbox
小桥流水78
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

机器人运动学及轨迹规划— (3) 逆运动学方程求解_求解逆运动学 方程

作者：小桥流水78 | 2024-08-17 14:57:35

赞

踩

求解逆运动学 方程

       上节建立了机器人的DH参数和正运动学方程，本节对该IRB2600机器人求逆过程进行逐步推导，包括所有的运算过程和Matlab代码。        

       对于六自由度的工业机械臂，一般存在八组逆解，即机器人到达笛卡尔空间坐标系中的任意一点都存在八种姿态（在不考虑关节限位的情况下）。工业机器人的逆运动学求解，常用的有数值解和解析解，其中数值解是采用差值（最小二乘法）或逼近的方式得到的一组近似解，通常计算量较大，对于冗余自由度机器人较为适用。而一般的工业机器人在设计的时候会满足pieper准则（后三轴的轴线交于一点：腕部），这种情况下采用解析法（反变换法）来求逆通常较为简单。

       首先定义前文中相邻关节的齐次变换矩阵和变量：

  1. syms c1 c2 c3 c4 c5 c6  s1 s2 s3 s4 s5 s6 a1 a2 a3 d1 d4 d6 nx ny nz ox oy oz ax ay az wx wy wz
  2. T01=[c1,-s1,0,0; s1,c1,0,0; 0,0,1,d1; 0,0,0,1];
  3. T12=[s2,c2,0,a1; 0,0,1,0; c2,-s2,0,0; 0,0,0,1];
  4. T23=[c3,-s3,0,a2; s3,c3,0,0; 0,0,1,0; 0,0,0,1];
  5. T34=[c4,-s4,0,a3; 0,0,1,d4; -s4,-c4,0,0; 0,0,0,1];
  6. T45=[c5,-s5,0,0; 0,0,-1,0; s5,c5,0,0; 0,0,0,1];
  7. T56=[c6,-s6,0,0; 0,0,1,0; -s6,-c6,0,0; 0,0,0,1];
  8.  
  9. T06=[nx,ox,ax,wx; ny,oy,ay,wy; nz,oz,az,wz; 0,0,0,1];

         然后连乘T01*T12*T23*T34*T45*T56得到一个超级长的变换矩阵，还是很容易化简的。

  1. T01*T12*T23*T34*T45*T56
  2.  
  3. ans =
  4.  
  5. [c6*(s5*(c1*c2*c3 - c1*s2*s3) + c5*(s1*s4 + c4*(c1*c2*s3 + c1*c3*s2))) + s6*(c4*s1 - s4*(c1*c2*s3 + c1*c3*s2)),   c6*(c4*s1 - s4*(c1*c2*s3 + c1*c3*s2)) - s6*(s5*(c1*c2*c3 - c1*s2*s3) + c5*(s1*s4 + c4*(c1*c2*s3 + c1*c3*s2))), c5*(c1*c2*c3 - c1*s2*s3) - s5*(s1*s4 + c4*(c1*c2*s3 + c1*c3*s2)), a1*c1 + a3*(c1*c2*s3 + c1*c3*s2) + d4*(c1*c2*c3 - c1*s2*s3) + a2*c1*s2]
  6. [c6*(s5*(c2*c3*s1 - s1*s2*s3) - c5*(c1*s4 - c4*(c2*s1*s3 + c3*s1*s2))) - s6*(c1*c4 + s4*(c2*s1*s3 + c3*s1*s2)), - s6*(s5*(c2*c3*s1 - s1*s2*s3) - c5*(c1*s4 - c4*(c2*s1*s3 + c3*s1*s2))) - c6*(c1*c4 + s4*(c2*s1*s3 + c3*s1*s2)), c5*(c2*c3*s1 - s1*s2*s3) + s5*(c1*s4 - c4*(c2*s1*s3 + c3*s1*s2)), a1*s1 + a3*(c2*s1*s3 + c3*s1*s2) + d4*(c2*c3*s1 - s1*s2*s3) + a2*s1*s2]
  7. [                                    - c6*(s5*(c2*s3 + c3*s2) - c4*c5*(c2*c3 - s2*s3)) - s4*s6*(c2*c3 - s2*s3),                                         s6*(s5*(c2*s3 + c3*s2) - c4*c5*(c2*c3 - s2*s3)) - c6*s4*(c2*c3 - s2*s3),                     - c5*(c2*s3 + c3*s2) - c4*s5*(c2*c3 - s2*s3),                   d1 + a2*c2 + a3*(c2*c3 - s2*s3) - d4*(c2*s3 + c3*s2)]
  8. [                                                                                                            0,                                                                                                               0,                                                                0,                                                                      1]

       在刚体的位姿描述中可以得到T06=

       因为六轴坐标系建立在腕部，将px py pz 用wx wy wz表示。

       我们通过等式T06=T01*T12*T23*T34*T45*T56可以看出，上面Matlab求得的wx wy wz分别为

       可以得到    θ1=atan2(wy,wx) ，同理通过上式可以求得机器人的前三个关节角度，具体推导过程见结尾附图。

       后三个关节角度需要进行矩阵的反变换，将等式T06=T01*T12*T23*T34*T45*T56的两端同时乘以(T03)^{-1}得T36=(T03)^{-1}T06=T34*T45*T56.

       其中(T03)^{-1}为

  1. inv(T01*T12*T23)
  2.  
  3. ans =
  4.  
  5. [(c1*c2*s3 + c1*c3*s2)/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2), (c2*s1*s3 + c3*s1*s2)/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2),  (c2*c3 - s2*s3)/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2), -(c2*c3*d1 + a1*c2*s3 + a1*c3*s2 - d1*s2*s3 + a2*c2^2*c3 + a2*c3*s2^2)/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2)]
  6. [(c1*c2*c3 - c1*s2*s3)/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2),  (s1*(c2*c3 - s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2), -(c2*s3 + c3*s2)/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2),  (a2*s2^2*s3 - a1*c2*c3 + c2*d1*s3 + c3*d1*s2 + a1*s2*s3 + a2*c2^2*s3)/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2)]
  7. [                                                                                                                                            -s1/(c1^2 + s1^2),                                                                                                                                              c1/(c1^2 + s1^2),                                                                0,                                                                                                                      0]
  8. [                                                                                                                                                            0,                                                                                                                                                             0,                                                                0,                                                                                                                      1]

         T06为 

  1. T06=[nx  ox  ax  wx
  2.      ny  oy  ay  wy
  3.      nz  oz  az  wz 
  4.      0   0   0   1];

        等式左边 (T03)^{-1}T06得到一个超级超级长的矩阵，定义为A.

  1.  inv(T01*T12*T23)*T06
  2.  
  3. ans =
  4.  
  5. [(nx*(c1*c2*s3 + c1*c3*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) + (nz*(c2*c3 - s2*s3))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (ny*(c2*s1*s3 + c3*s1*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2), (ox*(c1*c2*s3 + c1*c3*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) + (oz*(c2*c3 - s2*s3))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (oy*(c2*s1*s3 + c3*s1*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2), (ax*(c1*c2*s3 + c1*c3*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) + (az*(c2*c3 - s2*s3))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (ay*(c2*s1*s3 + c3*s1*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2), (wx*(c1*c2*s3 + c1*c3*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) - (c2*c3*d1 + a1*c2*s3 + a1*c3*s2 - d1*s2*s3 + a2*c2^2*c3 + a2*c3*s2^2)/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (wz*(c2*c3 - s2*s3))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (wy*(c2*s1*s3 + c3*s1*s2))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2)]
  6. [   (nx*(c1*c2*c3 - c1*s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) - (nz*(c2*s3 + c3*s2))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (ny*s1*(c2*c3 - s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2),    (ox*(c1*c2*c3 - c1*s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) - (oz*(c2*s3 + c3*s2))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (oy*s1*(c2*c3 - s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2),    (ax*(c1*c2*c3 - c1*s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) - (az*(c2*s3 + c3*s2))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (ay*s1*(c2*c3 - s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2),    (a2*s2^2*s3 - a1*c2*c3 + c2*d1*s3 + c3*d1*s2 + a1*s2*s3 + a2*c2^2*s3)/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (wx*(c1*c2*c3 - c1*s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2) - (wz*(c2*s3 + c3*s2))/(c2^2*c3^2 + c2^2*s3^2 + c3^2*s2^2 + s2^2*s3^2) + (s1*wy*(c2*c3 - s2*s3))/(c1^2*c2^2*c3^2 + c1^2*c2^2*s3^2 + c1^2*c3^2*s2^2 + c1^2*s2^2*s3^2 + c2^2*c3^2*s1^2 + c2^2*s1^2*s3^2 + c3^2*s1^2*s2^2 + s1^2*s2^2*s3^2)]
  7. [                                                                                                                                                                                                                                                                                                                                                                 (c1*ny)/(c1^2 + s1^2) - (nx*s1)/(c1^2 + s1^2),                                                                                                                                                                                                                                                                                                                                                                  (c1*oy)/(c1^2 + s1^2) - (ox*s1)/(c1^2 + s1^2),                                                                                                                                                                                                                                                                                                                                                                  (ay*c1)/(c1^2 + s1^2) - (ax*s1)/(c1^2 + s1^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          (c1*wy)/(c1^2 + s1^2) - (s1*wx)/(c1^2 + s1^2)]
  8. [                                                                                                                                                                                                                                                                                                                                                                                                             0,                                                                                                                                                                                                                                                                                                                                                                                                              0,                                                                                                                                                                                                                                                                                                                                                                                                              0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      1]

         然后等式右边的T34*T45*T56定义为B.

  1. T34*T45*T56
  2.  
  3. ans =
  4.  
  5. [  c4*c5*c6 - s4*s6, - c6*s4 - c4*c5*s6, -c4*s5, a3]
  6. [             c6*s5,             -s5*s6,     c5, d4]
  7. [- c4*s6 - c5*c6*s4,   c5*s4*s6 - c4*c6,  s4*s5,  0]
  8. [                 0,                  0,      0,  1]

令A(2,3)=B(2,3),化简求得θ5.

令A(1,3)=B(1,3),A(3,3)=B(3,3),化简求得θ4.

令A(2,1)=B(2,1),A(2,2)=B(2,2),化简求得θ6.

详细计算过程如下：

1、其中T12和T56的修改是因为在机器人的2轴和6轴均有一定角度的初始偏置，经计算验证得到的结论是：无论是否将DH参数中的初始偏置角度代入，均不影响逆解方程的结果。

2、对于θ2的角度计算用到了以下万能公式：

将计算得到的逆解通式输入Matlab:

  1. function [ikine_t]=mymodikine(Tbe)
  2. %    ti     di        ai-1      alphai-1 
  3. MDH=[0      0.445     0              0;
  4.      0      0         0.150      -pi/2;
  5.      0      0         0.700          0;
  6.      0      0.795     0.115       -pi/2;
  7.      0      0         0            pi/2;
  8.      0      0         0          -pi/2];
  9.  nx=Tbe(1,1); ny=Tbe(2,1); nz=Tbe(3,1); 
  10.  ox=Tbe(1,2); oy=Tbe(2,2); oz=Tbe(3,2); 
  11.  ax=Tbe(1,3); ay=Tbe(2,3); az=Tbe(3,3); 
  12.  wx=Tbe(1,4); wy=Tbe(2,4); wz=Tbe(3,4);
  13.  d1=0.445; d4=0.795;  
  14.  a1=0.150;  a2=0.700;   a3=0.115;
  15.  %t1
  16.  t11=atan2(wy,wx);
  17.  t12=atan2(-wy,-wx);
  18.  %t2
  19.  k11=wx/cos(t11)-a1;
  20.  k12=wx/cos(t12)-a1;
  21.  
  22.  k3=wz-d1;
  23.  K11=(k11*k11+k3*k3+a2*a2-d4*d4-a3*a3)/2/a2;
  24.  K12=(k12*k12+k3*k3+a2*a2-d4*d4-a3*a3)/2/a2;
  25.  t21=atan2(K11/(sqrt(k11*k11+k3*k3)),sqrt((k11*k11+k3*k3-K11*K11)/(k11*k11+k3*k3)))-atan2(k3,k11);
  26.  t22=atan2(K12/(sqrt(k12*k12+k3*k3)),sqrt((k12*k12+k3*k3-K12*K12)/(k12*k12+k3*k3)))-atan2(k3,k12);
  27.  t23=atan2(K11/(sqrt(k11*k11+k3*k3)),-sqrt((k11*k11+k3*k3-K11*K11)/(k11*k11+k3*k3)))-atan2(k3,k11);
  28.  t24=atan2(K12/(sqrt(k12*k12+k3*k3)),-sqrt((k12*k12+k3*k3-K12*K12)/(k12*k12+k3*k3)))-atan2(k3,k12);
  29.  
  30.  
  31.  
  32.  %t3
  33.  A1=(a3*(k11-a2*sin(t21))-d4*(k3-a2*cos(t21)))/(d4*d4+a3*a3);
  34.  B1=(a3*(k3-a2*cos(t21))+d4*(k11-a2*sin(t21)))/(d4*d4+a3*a3); 
  35.  A2=(a3*(k12-a2*sin(t22))-d4*(k3-a2*cos(t22)))/(d4*d4+a3*a3);
  36.  B2=(a3*(k3-a2*cos(t22))+d4*(k12-a2*sin(t22)))/(d4*d4+a3*a3);
  37.  A3=(a3*(k11-a2*sin(t23))-d4*(k3-a2*cos(t23)))/(d4*d4+a3*a3);
  38.  B3=(a3*(k3-a2*cos(t23))+d4*(k11-a2*sin(t23)))/(d4*d4+a3*a3); 
  39.  A4=(a3*(k12-a2*sin(t24))-d4*(k3-a2*cos(t24)))/(d4*d4+a3*a3);
  40.  B4=(a3*(k3-a2*cos(t24))+d4*(k12-a2*sin(t24)))/(d4*d4+a3*a3);
  41.  
  42.  
  43.  t31=atan2(A1,B1)-t21;
  44.  t32=atan2(A2,B2)-t22;
  45.  t33=atan2(A3,B3)-t23;
  46.  t34=atan2(A4,B4)-t24;
  47.  
  48.  
  49.  
  50.   %t5
  51.   r51=ax*cos(t11)*cos(t21+t31)-az*sin(t21+t31)+ay*sin(t11)*cos(t21+t31);
  52.   r52=ax*cos(t12)*cos(t22+t32)-az*sin(t22+t32)+ay*sin(t12)*cos(t22+t32);
  53.   r53=ax*cos(t11)*cos(t23+t33)-az*sin(t23+t33)+ay*sin(t11)*cos(t23+t33);
  54.   r54=ax*cos(t12)*cos(t24+t34)-az*sin(t24+t34)+ay*sin(t12)*cos(t24+t34);
  55.   t51=atan2(-sqrt(1-r51*r51),r51);
  56.   t52=atan2(-sqrt(1-r52*r52),r52);
  57.   t53=atan2(-sqrt(1-r53*r53),r53);
  58.   t54=atan2(-sqrt(1-r54*r54),r54);
  59.   t55=atan2(sqrt(1-r51*r51),r51);
  60.   t56=atan2(sqrt(1-r52*r52),r52);
  61.   t57=atan2(sqrt(1-r53*r53),r53);
  62.   t58=atan2(sqrt(1-r54*r54),r54);
  63.  
  64.   
  65.   %t4
  66.   t41=atan2(ax*sin(t11)-ay*cos(t11),ax*cos(t11)*sin(t21+t31)+az*cos(t21+t31)+ay*sin(t11)*sin(t21+t31));
  67.   t42=atan2(ax*sin(t12)-ay*cos(t12),ax*cos(t12)*sin(t22+t32)+az*cos(t22+t32)+ay*sin(t12)*sin(t22+t32));
  68.   t43=atan2(ax*sin(t11)-ay*cos(t11),ax*cos(t11)*sin(t23+t33)+az*cos(t23+t33)+ay*sin(t11)*sin(t23+t33));
  69.   t44=atan2(ax*sin(t12)-ay*cos(t12),ax*cos(t12)*sin(t24+t34)+az*cos(t24+t34)+ay*sin(t12)*sin(t24+t34));
  70.   t45=atan2(-(ax*sin(t11)-ay*cos(t11)),-(ax*cos(t11)*sin(t21+t31)+az*cos(t21+t31)+ay*sin(t11)*sin(t21+t31)));
  71.   t46=atan2(-(ax*sin(t12)-ay*cos(t12)),-(ax*cos(t12)*sin(t22+t32)+az*cos(t22+t32)+ay*sin(t12)*sin(t22+t32)));
  72.   t47=atan2(-(ax*sin(t11)-ay*cos(t11)),-(ax*cos(t11)*sin(t23+t33)+az*cos(t23+t33)+ay*sin(t11)*sin(t23+t33)));
  73.   t48=atan2(-(ax*sin(t12)-ay*cos(t12)),-(ax*cos(t12)*sin(t24+t34)+az*cos(t24+t34)+ay*sin(t12)*sin(t24+t34)));
  74.   
  75.   %t6
  76.   
  77.   t61=atan2(ox*cos(t11)*cos(t21+t31)-oz*sin(t21+t31)+oy*sin(t11)*cos(t21+t31),-(nx*cos(t11)*cos(t21+t31)-nz*sin(t21+t31)+ny*sin(t11)*cos(t21+t31)));
  78.   t62=atan2(ox*cos(t12)*cos(t22+t32)-oz*sin(t22+t32)+oy*sin(t12)*cos(t22+t32),-(nx*cos(t12)*cos(t22+t32)-nz*sin(t22+t32)+ny*sin(t12)*cos(t22+t32)));
  79.   t63=atan2(ox*cos(t11)*cos(t23+t33)-oz*sin(t23+t33)+oy*sin(t11)*cos(t23+t33),-(nx*cos(t11)*cos(t23+t33)-nz*sin(t23+t33)+ny*sin(t11)*cos(t23+t33)));
  80.   t64=atan2(ox*cos(t12)*cos(t24+t34)-oz*sin(t24+t34)+oy*sin(t12)*cos(t24+t34),-(nx*cos(t12)*cos(t24+t34)-nz*sin(t24+t34)+ny*sin(t12)*cos(t24+t34)));
  81.   t65=atan2(-(ox*cos(t11)*cos(t21+t31)-oz*sin(t21+t31)+oy*sin(t11)*cos(t21+t31)),(nx*cos(t11)*cos(t21+t31)-nz*sin(t21+t31)+ny*sin(t11)*cos(t21+t31)));
  82.   t66=atan2(-(ox*cos(t12)*cos(t22+t32)-oz*sin(t22+t32)+oy*sin(t12)*cos(t22+t32)),(nx*cos(t12)*cos(t22+t32)-nz*sin(t22+t32)+ny*sin(t12)*cos(t22+t32)));
  83.   t67=atan2(-(ox*cos(t11)*cos(t23+t33)-oz*sin(t23+t33)+oy*sin(t11)*cos(t23+t33)),(nx*cos(t11)*cos(t23+t33)-nz*sin(t23+t33)+ny*sin(t11)*cos(t23+t33)));
  84.   t68=atan2(-(ox*cos(t12)*cos(t24+t34)-oz*sin(t24+t34)+oy*sin(t12)*cos(t24+t34)),(nx*cos(t12)*cos(t24+t34)-nz*sin(t24+t34)+ny*sin(t12)*cos(t24+t34)));
  85.  
  86.  
  87.   ikine_t=[ t11 t21 t31 t41 t51 t61
  88.             t12 t22 t32 t42 t52 t62
  89.             t11 t23 t33 t43 t53 t63
  90.             t12 t24 t34 t44 t54 t64
  91.             t11 t21 t31 t45 t55 t65
  92.             t12 t22 t32 t46 t56 t66
  93.             t11 t23 t33 t47 t57 t67
  94.             t12 t24 t34 t48 t58 t68]*57.3;

联合上一节的正运动学方程将各关节设置为（25°，3°，10°，-45°， -10°，120°），得到此时腕部的齐次变换矩阵为

  1. theta1 = theta(1,1);
  2. theta2 = theta(1,2);
  3. theta3 = theta(1,3);
  4. theta4 = theta(1,4);
  5. theta5 = theta(1,5);
  6. theta6 = theta(1,6);
  7. %    theta        d       a         alpha
  8. MDH=[theta1       0.445      0              0;
  9.      theta2-pi/2    0        0.150      -pi/2;
  10.      theta3         0        0.700          0;
  11.      theta4        0.795     0.115      -pi/2;
  12.      theta5         0        0           pi/2;
  13.      theta6-pi      0        0         -pi/2];
  14.  
  15.  T01=[cos(MDH(1,1))         -sin(MDH(1,1))           0                    0;
  16.       sin(MDH(1,1))         cos(MDH(1,1))            0                    0;
  17.       0                     0                        1             MDH(1,2);
  18.       0                     0                        0                   1];
  19.  T12=[cos(MDH(2,1))         -sin(MDH(2,1))           0             MDH(2,3);
  20.       0                     0                        1                    0;
  21.       -sin(MDH(2,1))        -cos(MDH(2,1))           0                    0;
  22.       0                     0                        0                   1];
  23.  T23=[cos(MDH(3,1))         -sin(MDH(3,1))           0             MDH(3,3);
  24.       sin(MDH(3,1))         cos(MDH(3,1))            0                    0;
  25.       0                     0                        1                    0;
  26.       0                     0                        0                   1];
  27.  T34=[cos(MDH(4,1))         -sin(MDH(4,1))           0             MDH(4,3);
  28.       0                     0                        1             MDH(4,2);
  29.       -sin(MDH(4,1))        -cos(MDH(4,1))           0                    0
  30.       0                     0                        0                   1];
  31.  T45=[cos(MDH(5,1))         -sin(MDH(5,1))           0                    0;
  32.       0                     0                        -1                   0;
  33.       sin(MDH(5,1))         cos(MDH(5,1))            0                    0;
  34.       0                     0                        0                   1];
  35.  T56=[cos(MDH(6,1))         -sin(MDH(6,1))           0                    0;
  36.       0                     0                        1                    0;
  37.       -sin(MDH(6,1))        -cos(MDH(6,1))           0                    0;
  38.       0                     0                        0                   1];
  39. % 腕部到法兰
  40.  T6TCP=[1                   0                        0                    0;
  41.         0                   1                        0                    0;
  42.         0                   0                        1                0.085;
  43.         0                   0                        0                   1];
  44.     
  45. %   T06=T01*T12*T23*T34*T45*T56*T6TCP;
  46. T06=T01*T12*T23*T34*T45*T56;
  47.   
  48. theta=[25*pi/180,3*pi/180,10*pi/180,-45*pi/180,-10*pi/180,120*pi/180];
  49. T06
  50.  
  51. %//
  52.  
  53.  
  54. %输出结果
  55.  
  56. T06 =
  57.  
  58.    -0.5365   -0.0432    0.8428    0.8946
  59.     0.8097    0.2552    0.5285    0.4172
  60.    -0.2379    0.9659   -0.1019    1.0773
  61.          0         0         0    1.0000

将求得的齐次变换矩阵代入逆解得到： 

  1. MDH=[0      0.445     0              0;
  2.      0      0         0.150      -pi/2;
  3.      0      0         0.700          0;
  4.      0      0.795     0.115       -pi/2;
  5.      0      0         0            pi/2;
  6.      0      0         0          -pi/2];
  7.  nx=Tbe(1,1); ny=Tbe(2,1); nz=Tbe(3,1); 
  8.  ox=Tbe(1,2); oy=Tbe(2,2); oz=Tbe(3,2); 
  9.  ax=Tbe(1,3); ay=Tbe(2,3); az=Tbe(3,3); 
  10.  wx=Tbe(1,4); wy=Tbe(2,4); wz=Tbe(3,4);
  11.  d1=0.445; d4=0.795;  
  12.  a1=0.150;  a2=0.700;   a3=0.115;
  13.  %t1
  14.  t11=atan2(wy,wx);
  15.  t12=atan2(-wy,-wx);
  16.  %t2
  17.  k11=wx/cos(t11)-a1;
  18.  k12=wx/cos(t12)-a1;
  19.  
  20.  k3=wz-d1;
  21.  K11=(k11*k11+k3*k3+a2*a2-d4*d4-a3*a3)/2/a2;
  22.  K12=(k12*k12+k3*k3+a2*a2-d4*d4-a3*a3)/2/a2;
  23.  t21=atan2(K11/(sqrt(k11*k11+k3*k3)),sqrt((k11*k11+k3*k3-K11*K11)/(k11*k11+k3*k3)))-atan2(k3,k11);
  24.  t22=atan2(K12/(sqrt(k12*k12+k3*k3)),sqrt((k12*k12+k3*k3-K12*K12)/(k12*k12+k3*k3)))-atan2(k3,k12);
  25.  t23=atan2(K11/(sqrt(k11*k11+k3*k3)),-sqrt((k11*k11+k3*k3-K11*K11)/(k11*k11+k3*k3)))-atan2(k3,k11);
  26.  t24=atan2(K12/(sqrt(k12*k12+k3*k3)),-sqrt((k12*k12+k3*k3-K12*K12)/(k12*k12+k3*k3)))-atan2(k3,k12);
  27.  
  28.  
  29.  
  30.  %t3
  31.  A1=(a3*(k11-a2*sin(t21))-d4*(k3-a2*cos(t21)))/(d4*d4+a3*a3);
  32.  B1=(a3*(k3-a2*cos(t21))+d4*(k11-a2*sin(t21)))/(d4*d4+a3*a3); 
  33.  A2=(a3*(k12-a2*sin(t22))-d4*(k3-a2*cos(t22)))/(d4*d4+a3*a3);
  34.  B2=(a3*(k3-a2*cos(t22))+d4*(k12-a2*sin(t22)))/(d4*d4+a3*a3);
  35.  A3=(a3*(k11-a2*sin(t23))-d4*(k3-a2*cos(t23)))/(d4*d4+a3*a3);
  36.  B3=(a3*(k3-a2*cos(t23))+d4*(k11-a2*sin(t23)))/(d4*d4+a3*a3); 
  37.  A4=(a3*(k12-a2*sin(t24))-d4*(k3-a2*cos(t24)))/(d4*d4+a3*a3);
  38.  B4=(a3*(k3-a2*cos(t24))+d4*(k12-a2*sin(t24)))/(d4*d4+a3*a3);
  39.  
  40.  
  41.  t31=atan2(A1,B1)-t21;
  42.  t32=atan2(A2,B2)-t22;
  43.  t33=atan2(A3,B3)-t23;
  44.  t34=atan2(A4,B4)-t24;
  45.  
  46.  
  47.  
  48.   %t5
  49.   r51=ax*cos(t11)*cos(t21+t31)-az*sin(t21+t31)+ay*sin(t11)*cos(t21+t31);
  50.   r52=ax*cos(t12)*cos(t22+t32)-az*sin(t22+t32)+ay*sin(t12)*cos(t22+t32);
  51.   r53=ax*cos(t11)*cos(t23+t33)-az*sin(t23+t33)+ay*sin(t11)*cos(t23+t33);
  52.   r54=ax*cos(t12)*cos(t24+t34)-az*sin(t24+t34)+ay*sin(t12)*cos(t24+t34);
  53.   t51=atan2(-sqrt(1-r51*r51),r51);
  54.   t52=atan2(-sqrt(1-r52*r52),r52);
  55.   t53=atan2(-sqrt(1-r53*r53),r53);
  56.   t54=atan2(-sqrt(1-r54*r54),r54);
  57.   t55=atan2(sqrt(1-r51*r51),r51);
  58.   t56=atan2(sqrt(1-r52*r52),r52);
  59.   t57=atan2(sqrt(1-r53*r53),r53);
  60.   t58=atan2(sqrt(1-r54*r54),r54);
  61.  
  62.   
  63.   %t4
  64.   t41=atan2(ax*sin(t11)-ay*cos(t11),ax*cos(t11)*sin(t21+t31)+az*cos(t21+t31)+ay*sin(t11)*sin(t21+t31));
  65.   t42=atan2(ax*sin(t12)-ay*cos(t12),ax*cos(t12)*sin(t22+t32)+az*cos(t22+t32)+ay*sin(t12)*sin(t22+t32));
  66.   t43=atan2(ax*sin(t11)-ay*cos(t11),ax*cos(t11)*sin(t23+t33)+az*cos(t23+t33)+ay*sin(t11)*sin(t23+t33));
  67.   t44=atan2(ax*sin(t12)-ay*cos(t12),ax*cos(t12)*sin(t24+t34)+az*cos(t24+t34)+ay*sin(t12)*sin(t24+t34));
  68.   t45=atan2(-(ax*sin(t11)-ay*cos(t11)),-(ax*cos(t11)*sin(t21+t31)+az*cos(t21+t31)+ay*sin(t11)*sin(t21+t31)));
  69.   t46=atan2(-(ax*sin(t12)-ay*cos(t12)),-(ax*cos(t12)*sin(t22+t32)+az*cos(t22+t32)+ay*sin(t12)*sin(t22+t32)));
  70.   t47=atan2(-(ax*sin(t11)-ay*cos(t11)),-(ax*cos(t11)*sin(t23+t33)+az*cos(t23+t33)+ay*sin(t11)*sin(t23+t33)));
  71.   t48=atan2(-(ax*sin(t12)-ay*cos(t12)),-(ax*cos(t12)*sin(t24+t34)+az*cos(t24+t34)+ay*sin(t12)*sin(t24+t34)));
  72.   
  73.   %t6
  74.   
  75.   t61=atan2(ox*cos(t11)*cos(t21+t31)-oz*sin(t21+t31)+oy*sin(t11)*cos(t21+t31),-(nx*cos(t11)*cos(t21+t31)-nz*sin(t21+t31)+ny*sin(t11)*cos(t21+t31)));
  76.   t62=atan2(ox*cos(t12)*cos(t22+t32)-oz*sin(t22+t32)+oy*sin(t12)*cos(t22+t32),-(nx*cos(t12)*cos(t22+t32)-nz*sin(t22+t32)+ny*sin(t12)*cos(t22+t32)));
  77.   t63=atan2(ox*cos(t11)*cos(t23+t33)-oz*sin(t23+t33)+oy*sin(t11)*cos(t23+t33),-(nx*cos(t11)*cos(t23+t33)-nz*sin(t23+t33)+ny*sin(t11)*cos(t23+t33)));
  78.   t64=atan2(ox*cos(t12)*cos(t24+t34)-oz*sin(t24+t34)+oy*sin(t12)*cos(t24+t34),-(nx*cos(t12)*cos(t24+t34)-nz*sin(t24+t34)+ny*sin(t12)*cos(t24+t34)));
  79.   t65=atan2(-(ox*cos(t11)*cos(t21+t31)-oz*sin(t21+t31)+oy*sin(t11)*cos(t21+t31)),(nx*cos(t11)*cos(t21+t31)-nz*sin(t21+t31)+ny*sin(t11)*cos(t21+t31)));
  80.   t66=atan2(-(ox*cos(t12)*cos(t22+t32)-oz*sin(t22+t32)+oy*sin(t12)*cos(t22+t32)),(nx*cos(t12)*cos(t22+t32)-nz*sin(t22+t32)+ny*sin(t12)*cos(t22+t32)));
  81.   t67=atan2(-(ox*cos(t11)*cos(t23+t33)-oz*sin(t23+t33)+oy*sin(t11)*cos(t23+t33)),(nx*cos(t11)*cos(t23+t33)-nz*sin(t23+t33)+ny*sin(t11)*cos(t23+t33)));
  82.   t68=atan2(-(ox*cos(t12)*cos(t24+t34)-oz*sin(t24+t34)+oy*sin(t12)*cos(t24+t34)),(nx*cos(t12)*cos(t24+t34)-nz*sin(t24+t34)+ny*sin(t12)*cos(t24+t34)));
  83.  
  84.  
  85.   ikine_t=[ t11 t21 t31 t41 t51 t61
  86.             t12 t22 t32 t42 t52 t62
  87.             t11 t23 t33 t43 t53 t63
  88.             t12 t24 t34 t44 t54 t64
  89.             t11 t21 t31 t45 t55 t65
  90.             t12 t22 t32 t46 t56 t66
  91.             t11 t23 t33 t47 t57 t67
  92.             t12 t24 t34 t48 t58 t68]*57.3;
  93.  
  94. Tbe=[-0.5365   -0.0432    0.8428    0.8946
  95.       0.8097    0.2552    0.5285    0.4172
  96.      -0.2379    0.9659   -0.1019    1.0773
  97.       0         0         0         1.0000];
  98.  
  99. ikine_t
  100.  
  101. %///
  102. %输出结果
  103.  
  104. ikine_t =
  105.  
  106.    25.0040    2.9975   10.0005  -45.0108   -9.9961  120.0217
  107.  -155.0093  -93.3535  -21.4919    7.4515  -71.2102 -106.9829
  108.    25.0040  102.8794 -173.5506 -172.7646  -76.6684 -106.2506
  109.  -155.0093  -28.5017 -142.0582   25.0538  -16.8536 -128.6682
  110.    25.0040    2.9975   10.0005  135.0024    9.9961  -59.9916
  111.  -155.0093  -93.3535  -21.4919 -172.5617   71.2102   73.0304
  112.    25.0040  102.8794 -173.5506    7.2486   76.6684   73.7627
  113.  -155.0093  -28.5017 -142.0582 -154.9594   16.8536   51.3451

 在RobotStudio中示教相同点位对比验证，各组轴配置与逆解的关节角度一致。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/小桥流水78/article/detail/993189
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号