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

Posted 2020-09-30 gxb31415926

平面二自由度机器人运动学分析，包括运动学正解，工作空间，工作空间的边界，运动学逆解及完整的matlab代码。

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;
以上是关于平面二自由度机器人运动学分析的主要内容，如果未能解决你的问题，请参考以下文章
 
 二自由度平面关节机械手动力学方程主要包括哪些项
 SCARA基于Robotics工具箱的四自由度SCARA机器人建模matlab仿真
 6自由度空间机器人课程设计的简要记录
 为啥ANSYS要耦合节点自由度？
 智能追录器——基于人脸识别，图像处理，机器人视觉交叉领域
 V-SLAM综述：VINS-MONO(可观性退化)