三关节机械臂求正解逆解

求解思路

三连杆如图所示
D-H参数表

+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|        L1|          0|     1.5708|          0|
|  2|         q2|          0|        L2|          0|          0|
|  3|         q3|          0|        L3|          0|          0|
+---+-----------+-----------+-----------+-----------+-----------+
 

正解：
由DH矩阵相乘可以得到。

逆解：求三个关节的转角th1 th2 th3
已知末端坐标及欧拉角和杆长 x y z R P Y L1 L2 L3 
 为了减少变量个数，alpha及 offset作为已知参数给出
由末端坐标及欧拉角求得末端的变换矩阵 T
[ cos(P)*cos(Y), cos(Y)*sin(P)*sin(R) - cos(R)*sin(Y), sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P), x]
[ cos(P)*sin(Y), cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y), cos(R)*sin(P)*sin(Y) - cos(Y)*sin(R), y]
[       -sin(P),                        cos(P)*sin(R),                        cos(P)*cos(R), z]
[             0,                                    0,                                    0, 1]

又由T = T1*T2*T3
       inv(T1)*T=T2*T3
inv(T1)为T1的逆。

T1 = 		[ cos(th1), 0,  sin(th1),  0]	
            [ sin(th1), 0, -cos(th1),  0]
			[        0, 1,         0, L1]
			[        0, 0,         0,  1]
T2=		 [ cos(th2), -sin(th2), 0, L2*cos(th2)]
		[ sin(th2),  cos(th2), 0, L2*sin(th2)]
		[        0,         0, 1,           0]
		[        0,         0, 0,           1]
		
T3 =	[ cos(th3), -sin(th3), 0, L3*cos(th3)]
		[ sin(th3),  cos(th3), 0, L3*sin(th3)]
		[        0,         0, 1,           0]
		[        0,         0, 0,           1]

T1的逆  [ cos(th1),  sin(th1), 0,    0;
           0,        0,        1,  -L1;
         sin(th1), -cos(th1), 0,   0;
          0,        0,        0,   1];

可以求得    inv(T1)*T
[ cos(P)*cos(Y)*cos(th1) + cos(P)*sin(Y)*sin(th1),   sin(th1)*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y)) - cos(th1)*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R)), cos(th1)*(sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P)) - sin(th1)*(cos(Y)*sin(R) - cos(R)*sin(P)*sin(Y)), x*cos(th1) + y*sin(th1)]
[                                         -sin(P),                                                                                       cos(P)*sin(R),                                                                                     cos(P)*cos(R),                  z - L1]
[ cos(P)*cos(Y)*sin(th1) - cos(P)*sin(Y)*cos(th1), - cos(th1)*(cos(R)*cos(Y) + sin(P)*sin(R)*sin(Y)) - sin(th1)*(cos(R)*sin(Y) - cos(Y)*sin(P)*sin(R)), cos(th1)*(cos(Y)*sin(R) - cos(R)*sin(P)*sin(Y)) + sin(th1)*(sin(R)*sin(Y) + cos(R)*cos(Y)*sin(P)), x*sin(th1) - y*cos(th1)]
[                                               0,                                                                                                   0,                                                                                                 0,                       1]

求得   T2*T3
[ cos(th2 + th3), -sin(th2 + th3), 0, L3*cos(th2 + th3) + L2*cos(th2)]
[ sin(th2 + th3),  cos(th2 + th3), 0, L3*sin(th2 + th3) + L2*sin(th2)]
[              0,               0, 1,                               0]
[              0,               0, 0,                               1]


对比两者
由两者3行4列相等      x*sin(th1) - y*cos(th1) = 0
可得：th1 = atan(y/x)
由2行4列  与 1行4列 联立解方程
令  m1 = z - L1;  n1 =  x*cos(th1) + y*sin(th1);
则化简为
m1 - L2*sin(th2) = L3*sin(th2 + th3)
n1 -  L2*cos(th2) = L3*cos(th2 + th3)
两边平方相加，求解
令
m2 = 2*m1*L2;
n2 = 2*n1*L2;
k =m1^2+n1^2+L2^2-L3^2;
则 
th2 =acos(( 2*n2*k+sqrt(4*n2^2*k^2-4*(k^2-m2^2)*(m2^2+n2^2)) ) / ( 2*(m2^2+n2^2) ));
或者 th2 =acos(( 2*n2*k-sqrt(4*n2^2*k^2-4*(k^2-m2^2)*(m2^2+n2^2)) ) / ( 2*(m2^2+n2^2) ));
th3 =asin((m1 -L2*sin(th2))/L3) -th2;

则由x y z  可以求得 三个关节的转动角度
存在多解