b)

主程序：

1 % 采用几何法求解
2 clc;
3 clear;
4 T_0_H = input('请输入T_0_H:\n'); % Hand 坐标系 to 基坐标系
5 r11 = T_0_H(1,1); r12 = T_0_H(1,2); r13 = T_0_H(1,3); x = T_0_H(1,4);
6 r21 = T_0_H(2,1); r22 = T_0_H(2,2); r23 = T_0_H(2,3); y = T_0_H(2,4);
7 r31 = T_0_H(3,1); r32 = T_0_H(3,2); r33 = T_0_H(3,3); z = T_0_H(3,4);
8 L1 = 4; L2 = 3; L3 = 2; % 固定长度参数
9 c2 = ((x-L3*r11)^2 + (y-L3*r21)^2 - L1^2 - L2^2) / (2*L1*L2);
10 if c2>1 || c2 <-1
11 disp(' ');
12 msg = 'cos(θ2)超限，无解！';
13 error(msg);
14 end
15 theta2 = acos(c2);
16 [theta1,theta3] = get_theta(r11,x,r21,y,L1,L2,L3,theta2);
17 T_0_H_out1 = recheck(theta1,theta2,theta3,L1,L2,L3);
18 disp(' ');
19 if theta2 ~= 0
20 theta2_2 = -theta2;
21 [theta1_2,theta3_2] = get_theta(r11,x,r21,y,L1,L2,L3,theta2_2);
22 T_0_H_out2 = recheck(theta1_2,theta2_2,theta3_2,L1,L2,L3);
23 disp('有两组解:');
24 fprintf('解1: θ1 = %f°, θ2 = %f°, θ3 = %f°\n',rad2deg(theta1), rad2deg(theta2),
rad2deg(theta3));
25 disp('循环校验结果:');
26 disp(' ');
27 disp(T_0_H_out1);
28 fprintf('解2: θ1 = %f°, θ2 = %f°, θ3 = %f°\n',rad2deg(theta1_2), rad2deg(theta2_2),
rad2deg(theta3_2));
29 disp('循环校验结果:');
30 disp(T_0_H_out2);
31 else
32 disp('只有一组解:');
33 fprintf('解: θ1 = %f°, θ2 = %f°, θ3 = %f°\n',rad2deg(theta1), rad2deg(theta2),
rad2deg(theta3));
34 disp('循环校验结果:');
35 disp(T_0_H_out1);
36 end
37 function [theta1,theta3] = get_theta(r11,x,r21,y,L1,L2,L3,theta2)
38 beta = atan2(y-L3*r21, x-L3*r11);
39 c_phi = ((x-L3*r11)^2 + (y-L3*r21)^2 + L1^2 - L2^2) / (2*L1*sqrt((x-L3*r11)^2 + (y-
L3*r21)^2));
40 psi = acos(c_phi);
41 if theta2 < 0
42 theta1 = beta + psi;
43 else
44 theta1 = beta - psi;
45 end
46 phi = atan2(r21, r11);
47 theta3 = phi - theta1 - theta2;
48 end
49
50 function T_O_H_out = recheck(theta1,theta2,theta3,L1,L2,L3)
51 phi = theta1+theta2+theta3;
52 T_O_3 = [cos(phi),-sin(phi), 0.0,L1*cos(theta1)+L2*cos(theta1+theta2);
53 sin(phi), cos(phi), 0.0,L1*sin(theta1)+L2*sin(theta1+theta2);
54 0.0, 0.0, 1.0, 0.0;
55 0.0, 0.0, 0.0, 1.0; ];
56 T_3_H = [1,0,0,L3;
57 0,1,0,0;
58 0,0,1,0;
59 0,0,0,1;];
60 T_O_H_out = T_O_3*T_3_H;
61 end

输出：
 c)

主程序：

1 clc;
2 clear;
3 % DH参数: [THETA D A ALPHA],[关节角,连杆偏距,连杆长度,连杆扭转角]
4 % modified: 使用改进DH参数建模，因为课本上坐标系建立在连杆「首端」
5 % standard: 使用标准DH参数建模，这种情况下坐标系建立在连杆「末端」
6 L1 = 4; L2 = 3; L3 = 2; % 固定长度参数
7 L(1) = Link([0,0,0,0],'modified');
8 L(2) = Link([0,0,L1,0],'modified');
9 L(3) = Link([0,0,L2,0],'modified');
10 myrobot = SerialLink(L,'tool',[L3 0 0]);% 连接连杆,工具变换矩阵[L3 0 0]
11 T_0_H = input('请输入T_0_H:\n'); % Hand 坐标系 to 基坐标系
12 q0 = [0,0,0];
13 angle = myrobot.ikine(T_0_H,q0,'mask',[1 1 0 0 0 0]);
14 fprintf('解: θ1 = %f°, θ2 = %f°, θ3 = %f°\n',rad2deg(angle(1)), rad2deg(angle(2)),
rad2deg(angle(3)));
15 disp(' ');
16 T_0_H_out = myrobot.fkine(angle);
17 disp('循环校验结果:');
18 disp(T_0_H_out);

输出：