Universial robot 运动学

1 正运动学：

1.1 DH方法理解

第i个坐标系固连在第i个连杆的左端。轴i固连于i-1杆，在i-1杆的右端。  i坐标系固定在i杆上，随这i杆转动。

每个连杆有四个参数，第i个连杆：

1. ai = (沿着Xi轴，从Zi移动到Zi+1的距离)  ，即 连杆i的抽象长度！抽象长度由连杆i两端的两个轴：i-1轴和i轴来决定的，两轴的公垂线啊。
2. αi= (从Zi旋转到Zi+1的角度，转轴为Xi，右手定则)，即 连杆i与连杆i+1的夹角。
3. di = (沿着Zi轴，从Xi-1移动至Xi的距离)， Xi-1和Xi都是垂于Zi的！
4. θi = (绕Zi轴，从Xi-1旋转至Xi的角度)，即连杆i-1与连杆i的夹角阿。

前两个参数的作用是建立 i系与i+1系的关系；

后两个参数的作用是建立i-1系与i系的关系； 从而一个连杆的参数起到了承前启后的作用。

因为ai对应距离，通常假设ai大于零，其他的三个参数可正可负。

建立连杆坐标系的步骤：

1：找出各个关节轴，并标出其延长线。

2：如果轴i和i+1不相交，不平行，则找出关节轴i和i+1的公垂线，并以该公垂线与轴i的交点作为i系的原点。

如果两个轴相交，则以交点作为i系的原点.

如果两个轴平行，则寻找令di=0的公垂线作为Xi，Xi与轴的交点为i系的原点。（让di=0只是为了方便）

3：指定Zi沿着轴向，方向随便。

4：规定Xi沿着公垂线的指向，从Zi指向Zi+1, ;如果Zi和Zi+1相交，则Xi垂直于相交轴的平面，方向随便。

需要注意的是，Dh方法建立的坐标系并不是唯一的，首先 Zi是沿着轴向，方向可以随便选；在关节轴相交的情况下(ai=0),由于只要求Xi从Zi指向Zi+1，从而其指向有两种选择；   在关节轴平行的情况下，坐标系i的原点可以随便选择，因为平行轴的公垂线(Xi)有无数条，通常选择令di=0的Xi。另外，当关节为移动关节时，坐标系的选择也有一定的任意性。

DH方法建立坐标时，有一个原则：尽量使连杆参数为0（在符合其规定的几个原则的情况下）。

相邻坐标系之间变换矩阵的推导：

为求得坐标系i和坐标系i+1之间的关系，建立三个中间坐标系，P，Q，R.

Ti-1绕自己的x轴旋转旋转αi-1, 到达R系；（使Zi-1与Zi平行） --- 右手定则

R系沿着自己的x轴正方向移动ai-1  到达Q系；

Q系绕自己的Z轴旋转θi到达P系；--- 右手定则

P系沿着自己的Z轴正方向移动di到达Ti系；

则i系相对于i-1系的变换矩阵(i-1系到i系的变换)： T(i-1,i) = T(i-1,R)*T(R,Q)*T(Q,P)*T(P,i)

T(i-1,R) = Rx(αi-1) =

T(R,Q)= Dx(ai-1) =

T(Q,P)= Rz(θi) =

T(P,i) = Dz(di) =

T(i-1,i) = T(i-1,R)*T(R,Q)*T(Q,P)*T(P,i) =

由ai-1, αi-1, θi，di决定！

1.2 UR 6轴机器人的正运动学

各轴角度都是0的状态：

直观地看一下6个轴：

建立DH坐标系：-- 来自一国外论文<Analytic Inverse Kinematics for the Universal Robots , UR-5/UR-10 Arms>

authosr: Kelsey P. Hawkins, 可以Google， 该论文上的符号有很多错误！！

根据DH参数表可以直接写出T(i-1,i)了，i=1,2,3,4,5,6。

T(0,6) = T(0,1)*T(1,2)*T(2,3)*T(3,4)*T(4,5)*T(5,6);

////////////////////                /////////////////////////                     /////////////////////////

曾经的疑问：1和2坐标系的原点重合的话，如何体现2号轴沿着y1方向的长度？

Ans：：2坐标系是固连在2号轴上的，可以将2号轴看作是一个无限延申的刚体，固连在刚体的哪个位置上，从“把空间点在2系中的坐标转换为1系下的坐标”的角度来讲是没有关系的，因为固连在不同点上时，同一个空间点在2系下的局部坐标是不同的哦。可以假象一下，该机械臂只有1 ，2 两个关节！

///////////////////              ////////////////////////////                    /////////////////////////

My matlab code:

clear
clc

syms alp0 alp1 alp2 alp3 alp4 alp5 alp6;%alpha0-6
syms t0 t1 t2 t3 t4 t5 t6;%theta0-6，6个轴的旋转角度
syms d0 d1 d2 d3 d4 d5 d6;
syms a0 a1 a2 a3 a4 a5 a6;

T01=[cos(t1) -sin(t1) 0 a0;sin(t1)*cos(alp0) cos(t1)*cos(alp0) -sin(alp0) -sin(alp0)*d2;sin(t1)*sin(alp0) cos(t1)*sin(alp0) cos(alp0) cos(alp0)*d1;0 0 0 1];
T12=[cos(t2) -sin(t2) 0 a1;sin(t2)*cos(alp1) cos(t2)*cos(alp1) -sin(alp1) -sin(alp1)*d2;sin(t2)*sin(alp1) cos(t2)*sin(alp1) cos(alp1) cos(alp1)*d2;0 0 0 1];
T23=[cos(t3) -sin(t3) 0 a2;sin(t3)*cos(alp2) cos(t3)*cos(alp2) -sin(alp2) -sin(alp2)*d3;sin(t3)*sin(alp2) cos(t3)*sin(alp2) cos(alp2) cos(alp2)*d3;0 0 0 1];
T34=[cos(t4) -sin(t4) 0 a3;sin(t4)*cos(alp3) cos(t4)*cos(alp3) -sin(alp3) -sin(alp3)*d4;sin(t4)*sin(alp3) cos(t4)*sin(alp3) cos(alp3) cos(alp3)*d4;0 0 0 1];
T45=[cos(t5) -sin(t5) 0 a4;sin(t5)*cos(alp4) cos(t5)*cos(alp4) -sin(alp4) -sin(alp4)*d5;sin(t5)*sin(alp4) cos(t5)*sin(alp4) cos(alp4) cos(alp4)*d5;0 0 0 1];
T56=[cos(t6) -sin(t6) 0 a5;sin(t6)*cos(alp5) cos(t6)*cos(alp5) -sin(alp5) -sin(alp5)*d6;sin(t6)*sin(alp5) cos(t6)*sin(alp5) cos(alp5) cos(alp5)*d6;0 0 0 1];
T06 = T01*T12*T23*T34*T45*T56;

%用具体数值代替一些符号
T01_num=subs(T01,a0,0); T01_num=subs(T01_num,alp0,0);    T01_num=subs(T01_num,t1,t1);T01_num=subs(T01_num,d1,d1);
T12_num=subs(T12,a1,0); T12_num=subs(T12_num,alp1,pi/2); T12_num=subs(T12_num,t2,t2);T12_num=subs(T12_num,d2,0);
T23_num=subs(T23,a2,a2);T23_num=subs(T23_num,alp2,0);    T23_num=subs(T23_num,t3,t3);T23_num=subs(T23_num,d3,0);
T34_num=subs(T34,a3,a3);T34_num=subs(T34_num,alp3,0);    T34_num=subs(T34_num,t4,t4);T34_num=subs(T34_num,d4,d4);
T45_num=subs(T45,a4,0); T45_num=subs(T45_num,alp4,pi/2); T45_num=subs(T45_num,t5,t5);T45_num=subs(T45_num,d5,d5);
T56_num=subs(T56,a5,0); T56_num=subs(T56_num,alp5,-pi/2);T56_num=subs(T56_num,t6,t6);T56_num=subs(T56_num,d6,d6);
T06_num = T01_num*T12_num*T23_num*T34_num*T45_num*T56_num
%化简各个表达式：
T06_num_sim = simplify(T06_num)

%求逆解用到的一些参数：
syms Nx Ny Nz Ox Oy Oz Ax Ay Az Px Py Pz
T06_known = [Nx Ox Ax Px;Ny Oy Ay Py;Nz Oz Az Pz;0 0 0 1]%已知末端位姿求各轴角度
%求解theta1 5 6：
T01_num_inv=simplify( inv(T01_num) );
T56_num_inv=simplify( inv(T56_num) );
Left = simplify( T01_num_inv*T06_known*T56_num_inv )
T15_num= simplify( T12_num*T23_num*T34_num*T45_num )

%求解theta2 3 4
T45_num_inv = simplify( inv(T45_num) );
Left_2 = simplify( T01_num_inv*T06_known*T56_num_inv*T45_num_inv )
T14_num= simplify( T12_num*T23_num*T34_num)

结果，

T01_num =
[ cos(t1), -sin(t1), 0,  0]
[ sin(t1),  cos(t1), 0,  0]
[       0,        0, 1, d1]
[       0,        0, 0,  1]

T12_num =
[ cos(t2), -sin(t2),  0, 0]
[       0,        0, -1, 0]
[ sin(t2),  cos(t2),  0, 0]
[       0,        0,  0, 1]

T23_num =
[ cos(t3), -sin(t3), 0, a2]
[ sin(t3),  cos(t3), 0,  0]
[       0,        0, 1,  0]
[       0,        0, 0,  1]

T34_num =
[ cos(t4), -sin(t4), 0, a3]
[ sin(t4),  cos(t4), 0,  0]
[       0,        0, 1, d4]
[       0,        0, 0,  1]

T45_num =
[ cos(t5), -sin(t5),  0,   0]
[       0,        0, -1, -d5]
[ sin(t5),  cos(t5),  0,   0]
[       0,        0,  0,   1]

T56_num =
[  cos(t6), -sin(t6), 0,  0]
[        0,        0, 1, d6]
[ -sin(t6), -cos(t6), 0,  0]
[        0,        0, 0,  1]

进一步化简T06_num_sim：

//matlab数组的下标是从1开始的
//旋转矩阵
T06_num_sim(1,1)=c6s1s5 + c1c5c6c234 - c1s6s234
T06_num_sim(2,1)=c5*c6*c234*s1 - s1*s6*s234 - c1*c6*s5
T06_num_sim(3,1)=c234s6+s234c5c6

T06_num_sim(1,2)=-c1c6s234 - s1s5s6 - s6c1c234c5
T06_num_sim(2,2)=c1*s5*s6 - c6*s1*s234 - c5*c234*s1*s6
T06_num_sim(3,2)=c234c6-s234c5s6

T06_num_sim(1,3)=c5s1 - c1c234s5
T06_num_sim(2,3)=-c1*c5 - c234*s1*s5
T06_num_sim(3,3)=-s5*s234

//末端位置
T06_num_sim(1,4)= -( a3*c1*s2*s3 - a2*c1*c2 - c5*d6*s1 - c1*d5*s234 - d4*s1 - a3*c1*c2*c3 + c1*c234*d6*s5 )

T06_num_sim(2,4)=-( c1*d4 + c1*c5*d6 - a2*c2*s1 - d5*s1*s234 + c234*d6*s1*s5 + a3*s1*s2*s3 - a3*c2*c3*s1 )

T06_num_sim(3,4)=d1-d5c234+a3s23+a2s2-d6s5s234

用上述方法建模求得的最后结果的末端位置px py pz的值与git上的UR代码计算的差一个负号，原因还没有探索；另外的3*3旋转矩阵的计算结果与git UR上是否一致还没有检查。

git UR: https://github.com/ros-industrial/universal_robot/blob/kinetic-devel/ur_kinematics/src/ur_kin.cpp 中的ur_kinematics函数。

2 运动学逆解：

运动学逆解有数值迭代法，解析法。解析法 包括矢量代数法、几何法、矩阵法和四元数法等，优点是可以得到全部解，缺点在于难度较

大，方法通用性不强。数值解法直接求解约束方程组，可以通过迭代运算求得任何机构的实数解，但通常不能得到全部解，一般而言，初值的选取及搜索算法对收敛性和精度影响较大。

相比传统的六自由度机器人，UR 机器人具有各个关节都能整周回转的优点，且由于UR机器人机构特点满足机器人机构学的 Pieper 准则，因此其运动学逆解具有封闭解的优势，使运动控制更为容易。

Pieper准则是：机器人的三个相邻关节轴交于一点或三轴线平行。

对于6自由度的机器人来说，运动学反解非常复杂，一般没有封闭解(就是解析解)。Pieper方法就是在此基础上进行研究发现，如果机器人满足两个充分条件中的一个，就会得到解析解，这两个条件是：

　　（1）三个相邻关节轴相交于一点；

　　（2）三个相邻关节轴相互平行。

现在的大多数商品化机器人都满足封闭解的两个充分条件之一。如PUMA和STANFORD机器人满足第一条件，而ASEA和MINIMOVER机器人满足第二条件。以PUMA560机器人为例，它的最后3个关节轴相交于一点。我们运用Pieper方法解出它的封闭解，从求解的过程中我们也可以发现，这种求解方法也适用于带有移动关节的机器人。

先看数学基础：

2.1 两个简单的数学方法

2.1.1 求角度

这个逆运动学算法求解的角度范围是

因为标准的反正切arctan的值域是

所以不能使用，这里介绍一个改进的反正切求法 Atan2(y, x)（Matlab里有这个函数）,它的值域可以满足要求。

C 语言里 double atan2(double y, double x) 返回的是原点至点(x,y)的方位角，即与 x 轴的夹角。也可以理解为复数 x+yi 的辐角。返回值的单位为弧度，取值范围为(-pi, pi]。在数学坐标系中，结果为正表示从 X 轴逆时针旋转的角度，结果为负表示从 X 轴顺时针旋转的角度。 要注意的是，函数atan2(y,x)中参数的顺序是倒置的，atan2(y,x)计算的值相当于点(x,y)的角度值。

2.1.2 解方程

首先进行三角恒等变换，令

其中：

然后带入原方程：

则

上式中d3应该是d;  

前提应该是{ Px*Px + Py * Py - d*d >= 0 且 Px*Px + Py * Py >0} 在该前提下解才存在。

2.2 解析法求解逆运动学（用矩阵的变换）：

T(0,6)= T(0,1)*T(1,2)*T(2,3)*T(3,4)*T(4,5)*T(5,6);//这里T(i,i+1)表示i系至i+1系的变换矩阵

记已知末端位姿矩阵 T_know(0,6) =   

[ Nx, Ox, Ax, Px]

[ Ny, Oy, Ay, Py ]

[ Nz, Oz, Az, Pz]

[  0,    0,    0,   1]

求解关节角theta1,5,6:

2.2.1 求解第一关节角theta1:

变换公式:    inv(T(0,1)) * T_know(0,6) * inv(*T(5,6)) = T(1,2)*T(2,3)*T(3,4)*T(4,5)  //inv表示求逆阵   ------------------------（1）

式子（1）左侧 =

[ cos(t6)*(Nx*cos(t1) + Ny*sin(t1)) - sin(t6)*(Ox*cos(t1) + Oy*sin(t1)), Ax*cos(t1) + Ay*sin(t1), - cos(t6)*(Ox*cos(t1) + Oy*sin(t1)) - sin(t6)*(Nx*cos(t1) + Ny*sin(t1)), Px*cos(t1) - d6*(Ax*cos(t1) + Ay*sin(t1)) + Py*sin(t1)]
[ cos(t6)*(Ny*cos(t1) - Nx*sin(t1)) - sin(t6)*(Oy*cos(t1) - Ox*sin(t1)), Ay*cos(t1) - Ax*sin(t1), - cos(t6)*(Oy*cos(t1) - Ox*sin(t1)) - sin(t6)*(Ny*cos(t1) - Nx*sin(t1)), Py*cos(t1) - d6*(Ay*cos(t1) - Ax*sin(t1)) - Px*sin(t1)]
[                                               Nz*cos(t6) - Oz*sin(t6),                      Az,                                               - Oz*cos(t6) - Nz*sin(t6),                                        Pz - d1 - Az*d6]
[                                                                     0,                       0,                                                                       0,                                                      1]

式子（1）右侧 =

[ cos(t2 + t3 + t4)*cos(t5), -cos(t2 + t3 + t4)*sin(t5),  sin(t2 + t3 + t4), a3*cos(t2 + t3) + a2*cos(t2) + d5*sin(t2 + t3 + t4)]
[                  -sin(t5),                   -cos(t5),                  0,                                                 -d4]
[ sin(t2 + t3 + t4)*cos(t5), -sin(t2 + t3 + t4)*sin(t5), -cos(t2 + t3 + t4), a3*sin(t2 + t3) + a2*sin(t2) - d5*cos(t2 + t3 + t4)]
[                         0,                          0,                  0,                                                   1]

式子（1）左右两侧的第二行最后一列相等：

Py*c1 - d6*(Ay*c1 - Ax*s1) - Px*s1 = -d4

设m = (d6*Ay - Py), n =  d6*Ax -Px,

则 m*c1 - n*s1 = d4

根据前面介绍的解方程的方法：

2.2.1 求解theta5:

式子（1）左右两侧的第2行,第2列相等：

Ax*s1 - Ay*c1 = c5

从而 theta5 =+arccos(Ax*s1 - Ay*c1) 或 - arccos(Ax*s1 - Ay*c1)

2.2.1 求解theta6:

式子（1）左右两侧的第2行,第1列相等：

-s5 = c6*(Ny*c1 - Nx*s1) - s6*(Oy*c1 - Ox*s1)

记 Ny*c1 - Nx*s1 = n;     Oy*c1 - Ox*s1 = m

则-s5 = c6*n - s6*m

带入公式得

theta6 = atan2(n,m) - atan2(-s5, 0)   -----(根据旋单位转矩阵的性质，m*m+n*n-s5*s5=0)

= atan2(n,m) + atan2(s5, 0)

根据前面公式，解存在的条件是m*m+n*n-s5*s5 >= 0 且 m*m+n*n > 0， 这里已经是m*m+n*n-s5*s5=0，所以解存在的条件是s5*s5>0 即 s5不等于0；

然后，一般的资料上会进一步写成：theta6 = atan2(n/s5,m/s5)  （可以成立吗？？？）

对于atan2(y,x),当 x与y同时为0时，无解，写成这样是为了说明s5=0时，解不存在。

（--------atan2()的性质，画图可知 atan2(y,x) = -atan2(-y,x)；）

求解2，3，4关节角

变换公式:    inv(T(0,1)) * T_know(0,6) * inv(*T(5,6))* inv(*T(4,5))  =  T(1,2)*T(2,3)*T(3,4)  //inv表示求逆阵   ------------------------（2）

式子（2）左侧 =

[ cos(t5)*(cos(t6)*(Nx*cos(t1) + Ny*sin(t1)) - sin(t6)*(Ox*cos(t1) + Oy*sin(t1))) - sin(t5)*(Ax*cos(t1) + Ay*sin(t1)), cos(t6)*(Ox*cos(t1) + Oy*sin(t1)) + sin(t6)*(Nx*cos(t1) + Ny*sin(t1)), sin(t5)*(cos(t6)*(Nx*cos(t1) + Ny*sin(t1)) - sin(t6)*(Ox*cos(t1) + Oy*sin(t1))) + cos(t5)*(Ax*cos(t1) + Ay*sin(t1)), Px*cos(t1) - d6*(Ax*cos(t1) + Ay*sin(t1)) + d5*(cos(t6)*(Ox*cos(t1) + Oy*sin(t1)) + sin(t6)*(Nx*cos(t1) + Ny*sin(t1))) + Py*sin(t1)]
[ cos(t5)*(cos(t6)*(Ny*cos(t1) - Nx*sin(t1)) - sin(t6)*(Oy*cos(t1) - Ox*sin(t1))) - sin(t5)*(Ay*cos(t1) - Ax*sin(t1)), cos(t6)*(Oy*cos(t1) - Ox*sin(t1)) + sin(t6)*(Ny*cos(t1) - Nx*sin(t1)), sin(t5)*(cos(t6)*(Ny*cos(t1) - Nx*sin(t1)) - sin(t6)*(Oy*cos(t1) - Ox*sin(t1))) + cos(t5)*(Ay*cos(t1) - Ax*sin(t1)), Py*cos(t1) - d6*(Ay*cos(t1) - Ax*sin(t1)) + d5*(cos(t6)*(Oy*cos(t1) - Ox*sin(t1)) + sin(t6)*(Ny*cos(t1) - Nx*sin(t1))) - Px*sin(t1)]
[                                                                      cos(t5)*(Nz*cos(t6) - Oz*sin(t6)) - Az*sin(t5),                                               Oz*cos(t6) + Nz*sin(t6),                                                                      Az*cos(t5) + sin(t5)*(Nz*cos(t6) - Oz*sin(t6)),                                                                                      Pz - d1 - Az*d6 + d5*(Oz*cos(t6) + Nz*sin(t6))]
[                                                                                                                   0,                                                                     0,                                                                                                                   0,                                                                                                                                   1]

式子（2）右侧 =

[ cos(t2 + t3 + t4), -sin(t2 + t3 + t4),  0, a3*cos(t2 + t3) + a2*cos(t2)]
[                 0,                  0, -1,                          -d4]
[ sin(t2 + t3 + t4),  cos(t2 + t3 + t4),  0, a3*sin(t2 + t3) + a2*sin(t2)]
[                 0,                  0,  0,                            1]

-----求解关节角3 (此时已经解出了 theta1 5 6 )

左右两侧矩阵的第1行第4列相等：

Px*c1 - d6*(Ax*c1+Ay*s1) + d5*( c6*(Ox*c1+Oy*s1) + s6*(Nx*c1+Ny*s1) ) +Py*s1 = a3*c23 + a2*c2   -------------------(3)

左右两侧矩阵的第3行第4列相等：

Pz - d1 - Az*d6 + d5*(Oz*c6 + Nz*s6)  =  a3*s23 + a2*s2  -----------------------------------------------------------------------------(4)

由于theta1，5，6在前面已经解出来了，(3),(4)式左侧可以计算出来，记(3)式左侧为m, (4)式左侧为n, 则：

m = a3*c23 + a2*c2------------------------------------------------------------------------------------------------------------------------------  (5)

n = a3*s23 + a2*s2  -------------------------------------------------------------------------------------------------------------------------------(6)

(5)(6)两式平方相加：

m*m + n*n = a2*a2 + a3*a3 + 2*a2*a3*(c2*c23 + s2*s23)

= a2*a2 + a3*a3 + 2*a2*a3*c3

从而 theta3 = +/- arccos[ (m*m + n*n - a2*a2 - a3*a3 )/(2*a2*a3) ] ;

前提：[ ]内部的表达式绝对值要小于等于1，即： (m*m + n*n) <= (a2 + a3)^2;

-----求解关节角2 (此时已经解出了 theta1 5 6 3)

将(5)式展开： m = c2*(a3*c3 + a2) - s2*a3*s3 -----------------------------------------------------------------------------------------(7)

将(6)式展开： n =  c2*(a3*s3) + s2*(a2 + a3*c3)---------------------------------------------------------------------------------------(8)

将s2，c2看作未知数，可以解先线性方程得到：

s2 = [ ( a2*c3 + a2)*n - a3*s3*m ] / (a2*a2 + a3*a3 + 2*a2*a3*c3)

c2 = (m + s2*a3*s3) / (a3*c3 + a2)

从而 theta2 = Atan2(s2, c2)

-----求解关节角4 (此时已经解出了 theta1 5 6 3 2)

(2)式左右两侧第一行第二列相等：

-s234 = c6*(Ox*c1+Oy*s1) + s6*(Nx*c1 + Ny*s1)   --- 左侧记作m

c234 = Oz*c6 + Nz*s6 ---------------------------------------- 左侧记作n

从而 theta4 = Atan2(-m,n)  - theta2 -theta3

整理求解公式：

theta1:    m = (d6*Ay - Py), n =  d6*Ax -Px,

theta5:  +arccos(Ax*s1 - Ay*c1) 或 - arccos(Ax*s1 - Ay*c1)

theta6: 

theta6 = atan2(n,m) + atan2(s5, 0)   ---  解存在的条件是s5*s5>0 即 s5不等于0；

Ny*c1 - Nx*s1 = n;     Oy*c1 - Ox*s1 = m

然后，一般的资料上会进一步写成：theta6 = atan2(n/s5,m/s5)  （可以成立吗？？？）

theta3 = +/- arccos[ (m*m + n*n - a2*a2 - a3*a3 )/(2*a2*a3) ] ;

前提：[ ]内部的表达式绝对值要小于等于1，即： (m*m + n*n) <= (a2 + a3)^2;

--------------------------------------------------------------------------------------------

theta2 = Atan2(s2, c2)

           s2 = [ ( a2*c3 + a2)*n - a3*s3*m ] / (a2*a2 + a3*a3 + 2*a2*a3*c3)

           c2 = (m + s2*a3*s3) / (a3*c3 + a2)

theta4 = Atan2(-m,n)  - theta2 -theta3

-s234 = c6*(Ox*c1+Oy*s1) + s6*(Nx*c1 + Ny*s1)   --- 左侧记作m

c234 = Oz*c6 + Nz*s6 ---------------------------------------- 左侧记作n

奇异位置：

theta1: 

matlab robot tool box学习：

%From internet:机器人工具箱中的比运动学函数并不精确，同时机器人通常有多组逆解，而ikine函数只能求出一组。
%对满足pieper条件的机器人，最好自己求出他的解析解，利用解析解来求得多组逆解，不仅速度快，而且更精确。

%尝试了将变量定义成sym，传入Link函数，但是正运动学计算的结果有乱码！
%用SerialLink.ikine Numerical inverse kinematics 求逆解时 我遇到了matlab提示迭代错误，solution diverging at step 304, try reducing alpha。

%帮助文档中提到用SerialLink.ikine_sym可计算符号逆解，但是in experimental  --
%感觉这个工具箱的作用最主要是画图画轨迹
clc;
clear;
%startup_rvc;
%syms d1 d4 d5 d6 a2 a3;   syms t1 t2 t3 t4 t5 t6;
d1 = 100;d4 = 100;d5 = 100;d6 = 100;  a2 = 100;a3 = 100;

% d:link offset, a:link length, alpha:link twist; 

%L1 = Link( 'd', d1, 'a', 0, 'alpha', pi/2, 'sigma',0,'mdh',1);

L1 = Link('revolute', 'd', d1, 'a', 0.0, 'alpha', pi/2, 'modified');
L2 = Link('revolute', 'd', 0.0, 'a', a2, 'alpha',  0,   'modified');
L3 = Link('revolute', 'd', 0.0, 'a', a3, 'alpha',  0,   'modified');

L4 = Link('revolute', 'd', d4, 'a', 0.0, 'alpha',  pi/2,   'modified');
L5 = Link('revolute', 'd', d5, 'a', 0.0, 'alpha',  -pi/2,   'modified');
L6 = Link('revolute', 'd', d6, 'a', 0.0, 'alpha',  0,   'modified');%a 和alpha都为零，因为后面没有轴了。

robot=SerialLink([L1 L2 L3 L4 L5 L6]);
robot.name = 'abc';
robot.display()

thetas = [1.8 1 1.6 1 0.3 0.6];
pos1 = robot.fkine(thetas) %forward kinematics
%robot.fkine([t1 t2 t3 t4 t5 t6])

Q0=[1 1 1 1 1 1];
%thetas1 = robot.ikine(pos1,Q0)  %逆运动学，该函数接受初始估计值Q0作为迭代的开始,第三个参数还可以指定步长等。

%robot.plot([pi/4 pi/3 pi/3 pi/3 pi/3 pi/3])%画出机械臂的形态
for i=1:1:50
    %robot.plot([i 2*i 3*i pi/3 pi/3 pi/3])
    pos = robot.fkine([i 2*i 3*i pi/3 pi/3 pi/3]); %forward kinematics
    plot3(pos(1),pos(2),pos(3),'r.','MarkerSize',15.0);%画图, 也可调用robot.plot进行画图，但是运行速度很慢
    hold on
end

ref:

https://blog.csdn.net/fengyu19930920/article/details/81144042