平面二自由度机器人运动学分析

1 %运动学正解
2 function [x, y, c] = planar_robot_forward_kinematics(L1, L2, theta1, theta2)
3 x = L1 * cos(theta1) + L2 * cos(theta1 + theta2);
4 y = L1 * sin(theta1) + L2 * sin(theta1 + theta2);
5 c = theta1 + theta2;
6 end

 1 %运动学逆解
 2 function [theta1, theta2] = planar_robot_inverse_kinematics(L1, L2, x, y, handcoor)
 3 c2 = (x^2 + y^2 - L1^2 -L2^2) / (2 * L1 * L2);%若(x,y)在工作空间里，则c2必在[-1,1]里，但由于计算精度，c2的绝对值可能稍微大于1
 4 temp = 1 - c2^2;
 5 if(temp < 0)
 6     temp = 0;
 7 end
 8 if(handcoor == 0)   %right handcoor
 9     theta2 = atan2(sqrt(temp), c2);
10 else    %left handcoor
11     theta2 = atan2(-sqrt(temp), c2);
12 end
13 s2 = sin(theta2);
14 theta1 = atan2(y, x) - atan2(L2 * s2, L1 + L2 * c2);
15 end

1 %画圆弧
2 function draw_arc(x0, y0, r, theta1, theta2, options)
3 deltaTheta = 0.1 * pi / 180;
4 theta = theta1 : deltaTheta : theta2;
5 x = x0 + r * cos(theta);
6 y = y0 + r * sin(theta);
7 plot(x, y, ‘LineStyle‘, options.LineStyle, ‘Color‘, options.Color, ‘LineWidth‘, options.LineWidth);
8 axis equal;
9 end

 1 %画工作空间
 2 function draw_planar_robot_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)
 3 thetaL1 = thetaLimit1(1);
 4 thetaU1 = thetaLimit1(2);
 5 thetaL2 = thetaLimit2(1);
 6 thetaU2 = thetaLimit2(2);
 7 
 8 hold on;
 9 if(handcoor == 0) %right handcoor
10     options.LineStyle = ‘-‘;
11     options.Color=‘g‘;
12     options.LineWidth = 3;
13     
14     x0 = 0;
15     y0 = 0;
16     r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
17     alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
18     thetaStart = thetaL1 + alpha;
19     thetaEnd = thetaU1 + alpha;
20     draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
21     
22     x0 = 0;
23     y0 = 0;
24     r = L1 + L2;
25     thetaStart = thetaL1;
26     thetaEnd = thetaU1;
27     draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
28     
29     x0 = L1 * cos(thetaU1);
30     y0 = L1 * sin(thetaU1);
31     r = L2;
32     thetaStart = thetaU1;
33     thetaEnd = thetaU1 + thetaU2;
34     draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
35     
36     x0 = L1 * cos(thetaL1);
37     y0 = L1 * sin(thetaL1);
38     r = L2;
39     thetaStart = thetaL1;
40     thetaEnd = thetaL1 + thetaU2;
41     draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
42     
43     title(‘Workspace in right handcoor‘, ‘fontsize‘, 16);
44 else  %left handcoor
45     options.LineStyle = ‘-‘;
46     options.Color=‘b‘;
47     options.LineWidth = 3;
48     
49     x0 = 0;
50     y0 = 0;
51     r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
52     alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
53     thetaStart = thetaL1 - alpha;
54     thetaEnd = thetaU1 - alpha;
55     draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
56     
57     x0 = 0;
58     y0 = 0;
59     r = L1 + L2;
60     thetaStart = thetaL1;
61     thetaEnd = thetaU1;
62     draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
63     
64     x0 = L1 * cos(thetaU1);
65     y0 = L1 * sin(thetaU1);
66     r = L2;
67     thetaStart = thetaU1 + thetaL2;
68     thetaEnd = thetaU1;
69     draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
70     
71     x0 = L1 * cos(thetaL1);
72     y0 = L1 * sin(thetaL1);
73     r = L2;
74     thetaStart = thetaL1 + thetaL2;
75     thetaEnd = thetaL1;
76     draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
77     title(‘Workspace in left handcoor‘, ‘fontsize‘, 16);
78 end
79 set(gcf, ‘color‘, ‘w‘);
80 axis off;
81 end

  1 %画工作空间草图
  2 function draw_planar_robot_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)
  3 
  4 thetaL1 = thetaLimit1(1);
  5 thetaU1 = thetaLimit1(2);
  6 thetaL2 = thetaLimit2(1);
  7 thetaU2 = thetaLimit2(2);
  8 
  9 hold on;
 10 if(handcoor == 0) %right handcoor
 11     options.LineStyle = ‘-‘;
 12     options.Color=‘g‘;
 13     options.LineWidth = 3;
 14     
 15     x0 = 0;
 16     y0 = 0;
 17     r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
 18     alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
 19     thetaStart = thetaL1 + alpha;
 20     thetaEnd = thetaU1 + alpha;
 21     draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
 22     
 23     x0 = 0;
 24     y0 = 0;
 25     r = L1 + L2;
 26     thetaStart = thetaL1;
 27     thetaEnd = thetaU1;
 28     draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
 29     
 30     x0 = L1 * cos(thetaU1);
 31     y0 = L1 * sin(thetaU1);
 32     r = L2;
 33     thetaStart = thetaU1;
 34     thetaEnd = thetaU1 + thetaU2;
 35     draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
 36     
 37     x0 = L1 * cos(thetaL1);
 38     y0 = L1 * sin(thetaL1);
 39     r = L2;
 40     thetaStart = thetaL1;
 41     thetaEnd = thetaL1 + thetaU2;
 42     draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
 43     
 44     %-------------
 45     options.LineStyle = ‘--‘;
 46     options.Color=‘r‘;
 47     options.LineWidth = 0.5;
 48     
 49     x0 = 0;
 50     y0 = 0;
 51     r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
 52     thetaStart = 0;
 53     thetaEnd = 2 * pi;
 54     draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
 55     
 56     r = L1 + L2;
 57     draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
 58     
 59     x0 = L1 * cos(thetaU1);
 60     y0 = L1 * sin(thetaU1);
 61     r = L2;
 62     draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
 63     
 64     x0 = L1 * cos(thetaL1);
 65     y0 = L1 * sin(thetaL1);
 66     draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
 67     
 68     xA1 = L1 * cos(thetaL1);
 69     yA1 = L1 * sin(thetaL1);
 70     xB1 = xA1 + L2 * cos(thetaL1 + thetaU2);
 71     yB1 = yA1 + L2 * sin(thetaL1 + thetaU2);
 72     xA2 = L1 * cos(thetaU1);
 73     yA2 = L1 * sin(thetaU1);
 74     xB2 = xA2 + L2 * cos(thetaU1 + thetaU2);
 75     yB2 = yA2 + L2 * sin(thetaU1 + thetaU2);
 76     xC1 = (L1 + L2) * cos(thetaL1);
 77     yC1 = (L1 + L2) * sin(thetaL1);
 78     xC2 = (L1 + L2) * cos(thetaU1);
 79     yC2 = (L1 + L2) * sin(thetaU1);
 80     
 81     plot([0, xA1, xB1], [0, yA1, yB1], ‘lineStyle‘, ‘-‘, ‘color‘, ‘k‘, ‘lineWidth‘, 3);
 82     plot([0, xA2, xB2], [0, yA2, yB2], ‘lineStyle‘, ‘:‘, ‘color‘, ‘k‘, ‘lineWidth‘, 3);
 83     
 84     fontsize = 15;
 85     delta = 25;
 86     text(0, 0, ‘O‘, ‘Fontsize‘, fontsize);
 87     text(xA1, yA1 - delta, ‘A_1‘, ‘fontsize‘, fontsize);
 88     text(xB1, yB1 - delta, ‘B_1‘, ‘fontsize‘, fontsize);
 89     text(xA2, yA2 + delta, ‘A_2‘, ‘fontsize‘, fontsize);
 90     text(xB2, yB2 - delta, ‘B_2‘, ‘fontsize‘, fontsize);
 91     text(xC1, yC1, ‘C_1‘, ‘fontsize‘, fontsize);
 92     text(xC2, yC2, ‘C_2‘, ‘fontsize‘, fontsize);
 93     title(‘Workspace sketch in right handcoor‘, ‘fontsize‘, 16);
 94     
 95 else  %left handcoor
 96     options.LineStyle = ‘-‘;
 97     options.Color=‘b‘;
 98     options.LineWidth = 3;
 99     
100     x0 = 0;
101     y0 = 0;
102     r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
103     alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
104     thetaStart = thetaL1 - alpha;
105     thetaEnd = thetaU1 - alpha;
106     draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
107     
108     x0 = 0;
109     y0 = 0;
110     r = L1 + L2;
111     thetaStart = thetaL1;
112     thetaEnd = thetaU1;
113     draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
114     
115     x0 = L1 * cos(thetaU1);
116     y0 = L1 * sin(thetaU1);
117     r = L2;
118     thetaStart = thetaU1 + thetaL2;
119     thetaEnd = thetaU1;
120     draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
121     
122     x0 = L1 * cos(thetaL1);
123     y0 = L1 * sin(thetaL1);
124     r = L2;
125     thetaStart = thetaL1 + thetaL2;
126     thetaEnd = thetaL1;
127     draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
128     
129     %-------------
130     options.LineStyle = ‘--‘;
131     options.Color=‘r‘;
132     options.LineWidth = 0.5;
133     
134     x0 = 0;
135     y0 = 0;
136     r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
137     thetaStart = 0;
138     thetaEnd = 2 * pi;
139     draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
140     
141     r = L1 + L2;
142     draw_arc(x0, y0, r, thetaStart, thetaEnd, options)
143     
144     x0 = L1 * cos(thetaU1);
145     y0 = L1 * sin(thetaU1);
146     r = L2;
147     draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
148     
149     x0 = L1 * cos(thetaL1);
150     y0 = L1 * sin(thetaL1);
151     draw_arc(x0, y0, r, thetaStart, thetaEnd, options);
152     
153     xA1 = L1 * cos(thetaL1);
154     yA1 = L1 * sin(thetaL1);
155     xB1 = xA1 + L2 * cos(thetaL1 + thetaL2);
156     yB1 = yA1 + L2 * sin(thetaL1 + thetaL2);
157     xA2 = L1 * cos(thetaU1);
158     yA2 = L1 * sin(thetaU1);
159     xB2 = xA2 + L2 * cos(thetaU1 + thetaL2);
160     yB2 = yA2 + L2 * sin(thetaU1 + thetaL2);
161     xC1 = (L1 + L2) * cos(thetaL1);
162     yC1 = (L1 + L2) * sin(thetaL1);
163     xC2 = (L1 + L2) * cos(thetaU1);
164     yC2 = (L1 + L2) * sin(thetaU1);
165     
166     plot([0, xA1, xB1], [0, yA1, yB1], ‘lineStyle‘, ‘-‘, ‘color‘, ‘k‘, ‘lineWidth‘, 3);
167     plot([0, xA2, xB2], [0, yA2, yB2], ‘lineStyle‘, ‘:‘, ‘color‘, ‘k‘, ‘lineWidth‘, 3);
168     
169     fontsize = 15;
170     delta = 25;
171     text(0, 0, ‘O‘, ‘fontsize‘, fontsize);
172     text(xA1, yA1 - delta, ‘A_1‘, ‘fontsize‘, fontsize);
173     text(xB1, yB1 + delta, ‘B_1‘, ‘fontsize‘, fontsize);
174     text(xA2, yA2 + delta, ‘A_2‘, ‘fontsize‘, fontsize);
175     text(xB2, yB2 - delta, ‘B_2‘, ‘fontsize‘, fontsize);
176     text(xC1, yC1, ‘C_1‘, ‘fontsize‘, fontsize);
177     text(xC2, yC2, ‘C_2‘, ‘fontsize‘, fontsize);
178     title(‘Workspace sketch in left handcoor‘, ‘fontsize‘, 16);
179 end
180 set(gcf, ‘color‘, ‘w‘);
181 axis off;
182 end

 1 %求边界点
 2 function p = solve_planar_robot_boundary_point( L1, L2, thetaLimit1, thetaLimit2, handcoor, x0, y0, s)
 3 
 4 thetaL1 = thetaLimit1(1);
 5 thetaU1 = thetaLimit1(2);
 6 thetaL2 = thetaLimit2(1);
 7 thetaU2 = thetaLimit2(2);
 8 
 9 xA1 = L1 * cos(thetaL1);
10 yA1 = L1 * sin(thetaL1);
11 xA2 = L1 * cos(thetaU1);
12 yA2 = L1 * sin(thetaU1);
13 xC1 = (L1 + L2) * cos(thetaL1);
14 yC1 = (L1 + L2) * sin(thetaL1);
15 xC2 = (L1 + L2) * cos(thetaU1);
16 yC2 = (L1 + L2) * sin(thetaU1);
17 
18 if(handcoor == 0) %right handcoor
19     r = [sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2)), L1 + L2, L2, L2];
20     xB1 = xA1 + L2 * cos(thetaL1 + thetaU2);
21     yB1 = yA1 + L2 * sin(thetaL1 + thetaU2);
22     xB2 = xA2 + L2 * cos(thetaU1 + thetaU2);
23     yB2 = yA2 + L2 * sin(thetaU1 + thetaU2);
24     pp = [xB1, yB1, xB2, yB2; xC1, yC1, xC2, yC2; xC2, yC2, xB2, yB2; xC1, yC1, xB1, yB1];
25 else  %left handcoor
26     r = [sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2)), L1 + L2, L2, L2];
27     xB1 = xA1 + L2 * cos(thetaL1 + thetaL2);
28     yB1 = yA1 + L2 * sin(thetaL1 + thetaL2);
29     xB2 = xA2 + L2 * cos(thetaU1 + thetaL2);
30     yB2 = yA2 + L2 * sin(thetaU1 + thetaL2);
31     pp = [xB1, yB1, xB2, yB2; xC1, yC1, xC2, yC2; xB2, yB2, xC2, yC2; xB1, yB1, xC1, yC1];
32 end
33 xc = [0, 0, L1 * cos(thetaU1), L1 * cos(thetaL1)];
34 yc = [0, 0, L1 * sin(thetaU1), L1 * sin(thetaL1)];
35 
36 p = zeros(8,2);
37 k = 0;
38 if(abs(s(1) - 0) < eps)
39     temp = r.^2 - (x0 - xc).^2;
40     for i = 1 : 4
41         if(temp(i) >= 0)
42             xp1 = x0;
43             yp1 = yc(i) + sqrt(temp(i));
44             xp2 = x0;
45             yp2 = yc(i) - sqrt(temp(i));
46             if(dot([xp1 - x0; yp1 - y0], s) > 0 && (pp(i, 3) - xp1) * (pp(i, 2) - yp1) - (pp(i, 1) - xp1) * (pp(i, 4) - yp1) > 0)
47                 k = k + 1;
48                 p(k, :) = [xp1, yp1];
49             end
50             if(dot([xp2 - x0; yp2 - y0], s) > 0 && (pp(i, 3) - xp2) * (pp(i, 2) - yp2) - (pp(i, 1) - xp2) * (pp(i, 4) - yp2) > 0)
51                 k = k + 1;
52                 p(k, :) = [xp2, yp2];
53             end
54         end
55     end
56 else
57     a = (s(2) / s(1))^2 + 1;
58     b = 2 * (s(2) / s(1)) * (y0 - yc - x0 * (s(2) / s(1))) - 2 * xc;
59     c = xc.^2 - r.^2 - 2 * y0 * yc + y0^2 + yc.^2 + (s(2) / s(1))^2 * x0^2 + 2 * (s(2) / s(1)) * x0 * (yc - y0);
60     delta = b.^2 - 4 * a * c;
61     for i = 1 : 4
62         if(delta(i) >= 0)
63             x = roots([a b(i) c(i)]);
64             xp1 = x(1);
65             yp1 = s(2) / s(1) * (x(1) - x0) + y0;
66             xp2 = x(2);
67             yp2 = s(2) / s(1) * (x(2) - x0) + y0;
68             if(dot([xp1 - x0; yp1 - y0], s) > 0 && (pp(i, 3) - xp1) * (pp(i, 2) - yp1) - (pp(i, 1) - xp1) * (pp(i, 4) - yp1) > 0)
69                 k = k + 1;
70                 p(k, :) = [xp1, yp1];
71             end
72             if(dot([xp2 - x0; yp2 - y0], s) > 0 && (pp(i, 3) - xp2) * (pp(i, 2) - yp2) - (pp(i, 1) - xp2) * (pp(i, 4) - yp2) > 0)
73                 k = k + 1;
74                 p(k, :) = [xp2, yp2];
75             end
76         end
77     end
78 end
79 
80 distance = sqrt((p(1 : k, 1) - x0).^2 + (p(1 : k, 2) - y0).^2);
81 [~, index] = min(distance);
82 p = p(index, :);
83 end

 1 %画工作空间(示例)
 2 clc;
 3 clear;
 4 close all;
 5 L1 = 200;
 6 L2 = 200;
 7 thetaLimit1 = [-135, 135] * pi / 180;
 8 thetaLimit2 = [-145, 145] * pi / 180;
 9 
10 figure(1);
11 handcoor = 0;
12 draw_planar_robot_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)
13 figure(2);
14 handcoor = 1;
15 draw_planar_robot_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)
16 
17 figure(3);
18 handcoor = 0;
19 draw_planar_robot_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)
20 figure(4);
21 handcoor = 1;
22 draw_planar_robot_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)

 1 %求边界点(示例)
 2 clc;
 3 clear;
 4 close all;
 5 L1 = 200;
 6 L2 = 200;
 7 thetaLimit1 = [-135, 135] * pi / 180;
 8 thetaLimit2 = [-145, 145] * pi / 180;
 9 
10 figure(1);
11 handcoor = 1;
12 draw_planar_robot_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor);
13 while (1)
14     [x0, y0] = ginput(1);
15     [x1, y1] = ginput(1);
16     s = [x1 - x0; y1 - y0];
17     p = solve_planar_robot_boundary_point(L1, L2, thetaLimit1, thetaLimit2, handcoor, x0, y0, s);
18     plot([x0, p(1)], [y0, p(2)], ‘--o‘)
19 end