逆向运动学的封闭形式解的解析法

在这里向大家分享基于标准D-H参数法的求解逆向运动的过程，一些教材上有详细的推算过程，在这里直接推送一个计算公式。下篇博客分享一些选择角度的想法(一起讨论)。
首先解机械臂的逆向运动学有多种方法
一种是后三个关节(4,5,6)满足piper准则(后三个轴线交于一点)有封闭形式解。
这里有两种方法，①是解析法 ②是投影法。满足这个准则的机械臂有很多如puma560(经典),还有我的高仿机械臂(这里主要是指Z轴会重合在某一点)。
另外一种是不满足上面的准则的，但又都是转动矩阵，就是使用数值解法。
即是基于雅可比矩阵的迭代法。

后来我发现我设定的打横的关节的Z轴指向纸面外，与puma560的方向不一样，为了学习方便，我还是决定换回来(指向纸面内)，就仿照着puma560的方向画了一个，除了α的数据，其他都没有变化。如图：

在这副图的基础上，使用标准D-H法的变换顺序，可以得到六个变换矩阵：`
以上便是全部变换矩阵
还有就是一些逆矩阵是需要的

T1=[c1   0       s1    0;     
        s1    0      -c1   0;     
        0       1      0    d1;     
        0       0      0      1;]%1-2a1为0，所以省略
T2=[c2  -s2   0    a2*c2;   
        s2    c2     0  a2*s2;       
         0       0       1    0;      
         0      0        0    1;] %2-3   
T3=[c3   0    s3   a3*c3;   
         s3    0     -c3  a3*s3;     
         0       1       0    0;     
          0      0      0      1;]%3-4
T4=[c4     0   -   s4   0;   
      s4      0       c4   0;     
       0       -1      0   d4;    
       0       0        0  1;]%4-5
T5=[c5   0    s5   0;     
       s5    0     -c5  0;      
        0      1       0    0;     
         0     0      0    1;]%5-6
T6=[c6  -s6       0    0;     
        s6    c6      0  0;        
         0      0       1   d6;     
         0      0       0   1;]%6-7`

这里不是代码，是逆矩阵的列举
inv(T1)=[ c1  0 -s1 0;
          s1  0  c1  0;
          0   -1  0   d1;
          0    0  0   1; ]
inv(T6)=[ c6 s6  0  0;
         -s6 c6  0  0;
           0  0  1  -d6;
           0  0  0   1;]
还有就是T=[nx ox ax px;
           ny  oy ay py;
           nz  oz az pz;
           o   o   o  1;]
           
          

求θ1

方法是inv(T1)(T6)inv(T)=T2*T3*T4*T5
[(c1*nx+s1*ny)*c6-(c1*ox+s1*oy)*s6      (c1*nx+s1*ny)*s6+(c1*ox+s1*oy)*c6       c1*ax+s1*ay      -d6*(c1*ax+s1*ay)+c1*px+s1*py ;
 -nz*c6+oz*s6                                 -nz*s6-oz*c6                       -az             d6*az-pz+d1                ;
 (-s1*nx+c1*ny)*c6-(s1*ox+c1*oy)s6     (-s1*nx+c1*ny)*s6+(-s1*ox+c1*oy)*c6       -s1*ax+ca*ay    d6(s1*ax-c1*ay)-s1*px+c1*py;
              0                                    0                               0                  1                       ]
 =======
 [  xxxxxxxxxxxxxxxxxxxxxx                    -c2*c3*c4+s2*s3*s4              xxxxxxxxx          a2*c2-d4*s23+a3*c23
    xxxxxxxxxxxxxxxxxxxxxx                   -s2*c3*s4-s3*c2*s4               xxxxxxxxxxxx        a2*s2+d4*c23*a3*s23
     xxxxxxxxxxxxxxxxx                       xxxxxxxxxxxxxxxxxx                  s4*s5                 0
      0                                             0                             0                    1                    ]
      
      

然后就是上面的等式的第三行第四列相等

d6(s1*ax-c1*ay)-s1*px+c1*py=0
就能得到这个式子求出
  θ1 = arctan((-d6*ay+py)/(-ax*d6+px)) 
  有两个值 一个正 一个负

θ3
令上面的等式的第一行第一列和第一行第三行相等 ；

k1=-d6*(c1*ax+s1*ay)+c1*px+s1*py=a2*c2-d4*s23+a3*c23
k2=d6*az-pz+d1=a2*s2+d4*c23*a3*s23
计算k1^2+K^2=[a2*c2-d4*s23+a3*c23]^2+[a2*s2+d4*c23*a3*s23]^2
    通过计算可以约去c2和s2, 方法是拆开c23和s23
      得到的结果
      k1=(-d6*(c1*ax+s1*ay))+c1*px+s1*py;
      k2=d6*az-pz+d1;
      K=a3*c3-d4*s3
      a2^2+a3^2+d4^2-2a2*d4*s3+2*a2*a3*c3=k1^2+k2^2
      K=(k1^2+k2^2-(a2^2+a3^2+d4^2))/2*a2;
      最后计算
      θ3：atan2(a3,d4)-atan2(K,+/-(sqrt(a3^2+d4^2-K^2)))%有正负

同理可以求θ2

{a2*s2+d4*c23*a3*s23}+a2*c2-d4*s23+a3*c23=k1+k2
经过计算可以的到 
                y=d4*(c3-s3)+a3*(c3+s3)+a2;
                x=d4*(s3-c3)+a3*(s3-c3)-a2;
                c2*y+s2*x=K1+k2
                同样可以得到
                θ2：atan2(k1+k2,-(sqrt(y^2+x^2-(k1+k2)^2)))-atan2(y,x)
                由角1和角2 决定，有四种可能。

求θ6
由上面的等式可以得到

求θ4

(-s1*nx+c1*ny)*s6+(-s1*ox+c1*oy)*c6 =-c4
也是利用等式相等的方法 
   θ4= acos((s1*nx-c1*ny)*s6+(s1*ox-c1*oy)*c6)
   这里在后面我觉得 角4绝对不能为0；

θ5

-s1*ax+c1*ay=s4s5
根据这个式子，我们可以求出 角5的大小：atan((-s1*ax+c1*ay)/s4)  这里说明 s4不能为0,及角四不能为0,难道这里是奇异点。(有待验证）

根据这些式子，可以在Matlab 中验证。
由于有多解，需要自己去判断，我想的就是 抄那个matlab 的函数的程序