MATLAB机械臂建模

利用matlab中的robotics toolbox工具，建立机械臂的frame

操作流程：

1. 给出各个关节的旋转矩阵的初始角度，给出各个操作臂的长度，设置base的坐标。
2. 给出所有旋转矩阵
3. 给出在各自坐标系下的方向向量
4. 将所有的方向向量向基坐标系下转换
5. 将转换后的各个位置坐标存入数组内并绘制在基座标系下的图像

代码：

SL1=Link([0       0          0.180    -pi/2        0     ],'standard'); 
SL2=Link([0       0          0.600     0           0     ],'standard');
SL3=Link([0       0          0.130    -pi/2        0     ],'standard');
SL4=Link([0       0.630       0        pi/2        0     ],'standard');
SL5=Link([0       0           0       -pi/2        0     ],'standard');
SL6=Link([0       0.1075      0        0           0     ],'standard');
starobot=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','Robot');
figure(1)；
teach(starobot);

另一种较为简单：

clear
clc

R01=[ cos(a)  -sin(a)  0; sin(a)  cos(a)   0; 0        0       1];
R12=[ cos(b)  0   sin(b); 0       1   0;-sin(b) 0   cos(b)];
R23=[ cos(c)  0   sin(c);0       1   0;-sin(c) 0   cos(c)];
R34=[ cos(d)  0   sin(d);0       1   0; -sin(d) 0   cos(d)];
R45=[ cos(f)  -sin(f)  0;sin(f)  cos(f)   0;0         0      1];

L1=3;
L2=3;
L3=3;
L4=3;
L5=3;
L6=3;
O_0=[0;0;0];

r_OA_0 = [0;0;L1];
r_AB_1 = [0;0;L2];
r_BC_2 = [0;0;L3];
r_CD_3 = [0;0;L4];
r_DE_4 = [0;0;L5];
r_EF_5 = [0;0;L6];

r_OB_0 = r_OA_0 + R01*r_AB_1;
r_OC_0 = r_OB_0 + R01*R12*r_BC_2;
r_OD_0 = r_OC_0 + R01*R12*R23*r_CD_3;
r_OE_0 = r_OD_0 + R01*R12*R23*R34*r_DE_4;
r_OF_0 = r_OE_0 + R01*R12*R23*R34*R45*r_EF_5;

Link1 = [O_0 r_OA_0];
Link2 = [r_OA_0, r_OB_0];
Link3 = [r_OB_0, r_OC_0];
Link4 = [r_OC_0, r_OD_0];
Link5 = [r_OD_0, r_OE_0];
Link6 = [r_OE_0, r_OF_0];

hold on
grid on
view(45, 30);
axis square;
plot3(Link1(1,:), Link1(2,:), Link1(3,:), 'LineWidth', 3, 'color', 'r');
plot3(Link2(1,:), Link2(2,:), Link2(3,:), 'LineWidth', 3, 'color', 'b');
plot3(Link3(1,:), Link3(2,:), Link3(3,:), 'LineWidth', 3, 'color', 'r');
plot3(Link4(1,:), Link4(2,:), Link4(3,:), 'LineWidth', 3, 'color', 'b');
plot3(Link5(1,:), Link5(2,:), Link5(3,:), 'LineWidth', 3, 'color', 'r');
plot3(Link6(1,:), Link6(2,:), Link6(3,:), 'LineWidth', 3, 'color', 'b');