├── README.md ├── 第5章代码 ├── 力矩.jpg ├── 关节角.jpg ├── 笛卡尔.jpg ├── 行列式.jpg ├── 关节角速度.jpg ├── myjacobian.m └── chapter5.m ├── 第6章代码 ├── back.m ├── newton_om.m ├── myjacobian.m ├── mylink.m └── chapter6.m ├── 第4章代码 ├── theta2to1.m ├── invkine.m └── chapter4.m └── 第3章代码 ├── chapter3.m └── mylink.m /README.md: -------------------------------------------------------------------------------- 1 | # robots 2 | The homework of robot learning base. 3 | -------------------------------------------------------------------------------- /第5章代码/力矩.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kid-yang233/robots/HEAD/第5章代码/力矩.jpg -------------------------------------------------------------------------------- /第5章代码/关节角.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kid-yang233/robots/HEAD/第5章代码/关节角.jpg -------------------------------------------------------------------------------- /第5章代码/笛卡尔.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kid-yang233/robots/HEAD/第5章代码/笛卡尔.jpg -------------------------------------------------------------------------------- /第5章代码/行列式.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kid-yang233/robots/HEAD/第5章代码/行列式.jpg -------------------------------------------------------------------------------- /第5章代码/关节角速度.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kid-yang233/robots/HEAD/第5章代码/关节角速度.jpg -------------------------------------------------------------------------------- /第6章代码/back.m: -------------------------------------------------------------------------------- 1 | function F = back(Ni,ni_1,Ri_1,Pi,Fi,fi_1,Pci) 2 | fi = Ri_1*fi_1 + Fi; 3 | ni = Ni + Ri_1*ni_1 + cross(Pi,Ri_1*fi_1) + cross(Pci,Fi); 4 | F = [fi ni]; 5 | end -------------------------------------------------------------------------------- /第4章代码/theta2to1.m: -------------------------------------------------------------------------------- 1 | function theta1 = theta2to1(theta2, x, y) 2 | 3 | L1 = 4; L2 = 3; 4 | 5 | k1 = L1 + L2 * cos(theta2); 6 | k2 = L2 * sin(theta2); 7 | 8 | % r = sqrt( k1^2 + k2^2 ); 9 | % gamma = atan2(k2, k1); 10 | 11 | if x == 0 && y == 0 12 | theta1 = inf; 13 | else 14 | theta1 = atan2(y, x) - atan2(k2, k1); 15 | end 16 | 17 | end -------------------------------------------------------------------------------- /第6章代码/newton_om.m: -------------------------------------------------------------------------------- 1 | function T = newton_om(Ri,Wi,dWi,dthetai,ddthetai,Pi1,mi1,Ii1,dvi,Pci) 2 | 3 | Wi1 = Ri*Wi + dthetai*[0;0;1]; 4 | dWi1 = Ri*dWi + cross(Ri*dWi,dthetai*[0;0;1]) + ddthetai*[0;0;1]; 5 | dvi1 = Ri*(cross(dWi,Pi1)+cross(Wi,cross(Wi,Pi1))+dvi); 6 | dcvi1 = cross(dWi1,Pci)+cross(Wi1,cross(Wi1,Pci)) + dvi1; 7 | 8 | Fi1 = mi1 * dcvi1; 9 | Ni1 = Ii1*dWi1 + cross(Wi1,Ii1*Wi1); 10 | 11 | T = [ Wi1 dWi1 dvi1 Fi1 Ni1 ]; 12 | 13 | end -------------------------------------------------------------------------------- /第5章代码/myjacobian.m: -------------------------------------------------------------------------------- 1 | function J = myjacobian(Theta, L1, L2, L3) 2 | 3 | t1 = Theta(1); 4 | t2 = Theta(2); 5 | t3 = Theta(3); 6 | 7 | J = [ 8 | -L1*sin(t1)-L2*sin(t1+t2)-L3*sin(t1+t2+t3), -L2*sin(t1+t2)-L3*sin(t1+t2+t3), -L3*sin(t1+t2+t3) ; 9 | L1*cos(t1)+L2*cos(t1+t2)+L3*cos(t1+t2+t3), L2*cos(t1+t2)+L3*cos(t1+t2+t3), L3*cos(t1+t2+t3); 10 | 1, 1, 1 11 | ]; 12 | 13 | 14 | 15 | 16 | end -------------------------------------------------------------------------------- /第6章代码/myjacobian.m: -------------------------------------------------------------------------------- 1 | function J = myjacobian(Theta, L1, L2, L3) 2 | 3 | t1 = Theta(1); 4 | t2 = Theta(2); 5 | t3 = Theta(3); 6 | 7 | J = [ 8 | -L1*sin(t1)-L2*sin(t1+t2)-L3*sin(t1+t2+t3), -L2*sin(t1+t2)-L3*sin(t1+t2+t3), -L3*sin(t1+t2+t3) ; 9 | L1*cos(t1)+L2*cos(t1+t2)+L3*cos(t1+t2+t3), L2*cos(t1+t2)+L3*cos(t1+t2+t3), L3*cos(t1+t2+t3); 10 | 1, 1, 1 11 | ]; 12 | 13 | 14 | 15 | 16 | end -------------------------------------------------------------------------------- /第4章代码/invkine.m: -------------------------------------------------------------------------------- 1 | function thetas = invkine(T0toH) 2 | 3 | if sum( size(T0toH) == [4,4] ) ~= 2 4 | error("The input matrix should be in size (4, 4)"); 5 | end 6 | L1 = 4; L2 = 3; L3 = 2; 7 | THto3 = [ 8 | 1 0 0 -L3 9 | 0 1 0 0 10 | 0 0 1 0 11 | 0 0 0 1 12 | ]; 13 | T0to3 = T0toH * THto3; 14 | 15 | x = T0to3(1, 4); 16 | y = T0to3(2, 4); 17 | c2 = (x^2+y^2-L1^2-L2^2)/(2*L1*L2); 18 | s2_1 = sqrt(1 - c2^2); 19 | s2_2 = -1*sqrt(1-c2^2); 20 | theta2_1 = atan2(s2_1, c2); 21 | theta2_2 = atan2(s2_2, c2); 22 | 23 | theta1_1 = theta2to1(theta2_1, x, y); 24 | theta1_2 = theta2to1(theta2_2, x, y); 25 | 26 | theta3_1 = atan2( T0to3(2,1), T0to3(1,1) ) - theta1_1 - theta2_1; 27 | theta3_2 = atan2(T0to3(2,1), T0to3(1,1)) - theta1_2 - theta2_2; 28 | 29 | thetas = [ 30 | theta1_1, theta2_1, theta3_1; 31 | theta1_2, theta2_2, theta3_2 32 | ]; 33 | 34 | 35 | end -------------------------------------------------------------------------------- /第3章代码/chapter3.m: -------------------------------------------------------------------------------- 1 | clear, clc; 2 | % 已知参数 3 | l1 = 4; 4 | l2 = 3; 5 | l3 = 2; 6 | syms theta1 theta2 theta3; 7 | 8 | % alpha a d theta isrevolute 9 | L1 = mylink(0, 0, 0, 0, 1); 10 | L2 = mylink(0, l1, 0, 0, 1); 11 | L3 = mylink(0, l2, 0, 0, 1); 12 | 13 | L1.transMatrix(theta1); 14 | L2.transMatrix(theta2); 15 | L3.transMatrix(theta3); 16 | 17 | T_3H = [ 1 0 0 l3 18 | 0 1 0 0 19 | 0 0 1 0 20 | 0 0 0 1]; 21 | 22 | H1 = L1.transMatrix(0)*L2.transMatrix(0)* L3.transMatrix(0) * T_3H; 23 | H2 = L1.transMatrix(pi/18)*L2.transMatrix(pi/9)* L3.transMatrix(pi/6) * T_3H; 24 | H3 = L1.transMatrix(pi/2)*L2.transMatrix(pi/2)* L3.transMatrix(pi/2) * T_3H; 25 | 26 | 27 | LH = Link([0, 0, l3, 0], 'modified'); 28 | LH.A(0); 29 | L(1)=Link([0,0,0,0],'modified'); 30 | L(2)=Link([0,0,4,0],'modified'); 31 | L(3)=Link([0,0,3,0],'modified'); 32 | threeLink = SerialLink(L,'name','ThreeLink'); 33 | 34 | T1 = threeLink.fkine([0,0,0]) 35 | T2 = threeLink.fkine([pi/18,pi/9,pi/6]) 36 | T3 = threeLink.fkine([pi/2,pi/2,pi/2]) 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /第3章代码/mylink.m: -------------------------------------------------------------------------------- 1 | classdef mylink 2 | %mylink 自定义的一个连杆类，用来存放D-H参数 3 | % 此处显示详细说明 4 | 5 | properties 6 | alpha % 连杆扭转角 7 | a % 连杆长度 8 | d % 连杆偏距 9 | theta % 关节角 10 | isrevolute % 是转动关节还是移动关节 11 | end 12 | 13 | methods 14 | function obj = mylink(alpha_,a_, d_, theta_, isrevolute_) 15 | % 构造函数 16 | % 利用此函数初始化类对象 17 | obj.alpha = alpha_; 18 | obj.a = a_; 19 | obj.d = d_; 20 | obj.theta = theta_; 21 | if isrevolute_ == 1 || isrevolute == 0 22 | obj.isrevolute = isrevolute_; 23 | else 24 | error('Arg isrevolute of mylink constructor must be 0 or 1'); 25 | end 26 | end 27 | 28 | function T = transMatrix(self,value) 29 | %transMatrix 计算此连杆的齐次变换矩阵 30 | sa = sin(self.alpha); 31 | ca = cos(self.alpha); 32 | if self.isrevolute == 1 33 | st = sin(value); ct = cos(value); 34 | dd = self.d; 35 | else 36 | st = sin(self.theta); ct = cos(self.theta); 37 | dd = value; 38 | end % 判断是否是转动关节 39 | % 计算齐次变换矩阵 40 | T = [ ct -st 0 self.a 41 | st*ca ct*ca -sa -sa*dd 42 | st*sa ct*sa ca ca*dd 43 | 0 0 0 1 ]; 44 | end % function transMatrix 45 | end 46 | end 47 | 48 | -------------------------------------------------------------------------------- /第6章代码/mylink.m: -------------------------------------------------------------------------------- 1 | classdef mylink 2 | %mylink 自定义的一个连杆类，用来存放D-H参数 3 | % 此处显示详细说明 4 | 5 | properties 6 | alpha % 连杆扭转角 7 | a % 连杆长度 8 | d % 连杆偏距 9 | theta % 关节角 10 | isrevolute % 是转动关节还是移动关节 11 | end 12 | 13 | methods 14 | function obj = mylink(alpha_,a_, d_, theta_, isrevolute_) 15 | % 构造函数 16 | % 利用此函数初始化类对象 17 | obj.alpha = alpha_; 18 | obj.a = a_; 19 | obj.d = d_; 20 | obj.theta = theta_; 21 | if isrevolute_ == 1 || isrevolute == 0 22 | obj.isrevolute = isrevolute_; 23 | else 24 | error('Arg isrevolute of mylink constructor must be 0 or 1'); 25 | end 26 | end 27 | 28 | function T = transMatrix(self,value) 29 | %transMatrix 计算此连杆的齐次变换矩阵 30 | sa = sin(self.alpha); 31 | ca = cos(self.alpha); 32 | if self.isrevolute == 1 33 | st = sin(value); ct = cos(value); 34 | dd = self.d; 35 | else 36 | st = sin(self.theta); ct = cos(self.theta); 37 | dd = value; 38 | end % 判断是否是转动关节 39 | % 计算齐次变换矩阵 40 | T = [ ct -st 0 self.a 41 | st*ca ct*ca -sa -sa*dd 42 | st*sa ct*sa ca ca*dd 43 | 0 0 0 1 ]; 44 | end % function transMatrix 45 | end 46 | end 47 | 48 | -------------------------------------------------------------------------------- /第4章代码/chapter4.m: -------------------------------------------------------------------------------- 1 | clc, clear 2 | L1 = 4; L2 = 3; L3 = 2; 3 | % DH = [ THETA D A ALPHA] 4 | link1 = Link([0, 0, 0, 0], 'modified'); 5 | link2 = Link([0, 0, L1, 0], 'modified'); 6 | link3 = Link([0, 0, L2, 0], 'modified'); 7 | bot = SerialLink([link1 link2 link3], 'name', 'T0to3'); 8 | 9 | T0toH = [ 10 | 1 0 0 9 11 | 0 1 0 0 12 | 0 0 1 0 13 | 0 0 0 1 14 | ]; 15 | % 逆运动学解，具体过程见函数invkine 16 | tt = invkine(T0toH); 17 | THto3 = [ 18 | 1 0 0 -L3 19 | 0 1 0 0 20 | 0 0 1 0 21 | 0 0 0 1 22 | ]; 23 | T0to3 = T0toH * THto3; 24 | bot.ikine(T0to3, 'mask', [1 1 0 0 0 1]) 25 | 26 | T0toH = [ 27 | 0.5 -0.866 0 7.5373 28 | 0.866 0.5 0 3.9266 29 | 0 0 1 0 30 | 0 0 0 1 31 | ]; 32 | % 逆运动学解，具体过程见函数invkine 33 | tt = invkine(T0toH); 34 | THto3 = [ 35 | 1 0 0 -L3 36 | 0 1 0 0 37 | 0 0 1 0 38 | 0 0 0 1 39 | ]; 40 | T0to3 = T0toH * THto3; 41 | bot.ikine(T0to3, 'mask', [1 1 0 0 0 1]) 42 | 43 | 44 | T0toH = [ 45 | 0 1 0 -3 46 | -1 0 0 2 47 | 0 0 1 0 48 | 0 0 0 1 49 | ]; 50 | % 逆运动学解，具体过程见函数invkine 51 | tt = invkine(T0toH); 52 | THto3 = [ 53 | 1 0 0 -L3 54 | 0 1 0 0 55 | 0 0 1 0 56 | 0 0 0 1 57 | ]; 58 | T0to3 = T0toH * THto3; 59 | bot.ikine(T0to3, 'mask', [1 1 0 0 0 1]) 60 | 61 | %无解 62 | T0toH = [ 63 | 0.866 -0.5 0 -3.1245 64 | -0.5 0.866 0 8.1674 65 | 0 0 1 0 66 | 0 0 0 1 67 | ]; 68 | % 逆运动学解，具体过程见函数invkine 69 | %tt = invkine(T0toH); 70 | THto3 = [ 71 | 1 0 0 -L3 72 | 0 1 0 0 73 | 0 0 1 0 74 | 0 0 0 1 75 | ]; 76 | T0to3 = T0toH * THto3; 77 | bot.ikine(T0to3, 'mask', [1 1 0 0 0 1]) 78 | 79 | -------------------------------------------------------------------------------- /第5章代码/chapter5.m: -------------------------------------------------------------------------------- 1 | clear; 2 | L1 = 4; L2 = 3; L3 = 2; 3 | Theta0 = [10; 20; 30]*pi/180; 4 | delta_t = 0.1; 5 | 6 | % 每一步中的雅可比矩阵 7 | J = myjacobian(Theta0, L1, L2, L3); 8 | % 期望速度 9 | V_c = [0.2;-0.3;-0.2]; 10 | 11 | % 期望力和力矩 12 | W = [1; 2; 3]; 13 | 14 | 15 | % 每一步中的角度 16 | Theta = Theta0; 17 | 18 | % 存放角度 19 | Angle = Theta0; 20 | % 存放关节的角速度 21 | Omega = [0;0;0]; 22 | 23 | % 机械臂模型 24 | link1 = Link([0, 0, 0, 0], 'modified'); 25 | link2 = Link([0, 0, L1, 0], 'modified'); 26 | link3 = Link([0, 0, L2, 0], 'modified'); 27 | linkH = Link([0, 0, L3, 0], 'modified'); 28 | bot = SerialLink([link1 link2 link3 linkH], 'name', 'T0toH'); 29 | T = bot.fkine([Theta', 0]); 30 | 31 | % 存放末端的位置和姿态 32 | X = T.t; 33 | phi = Theta(1) + Theta(2) + Theta(3); 34 | 35 | % 存放雅可比矩阵的行列式 36 | J_det = [det(J)]; 37 | 38 | % 存放关节力矩 39 | tao = [J'*W]; 40 | 41 | for t = 0+delta_t:delta_t:5 42 | J = myjacobian(Theta, L1, L2, L3); 43 | if det(J) == 0 44 | print("singular point\n"); 45 | continue; 46 | else 47 | dTheta = inv(J)*V_c; 48 | Theta = Theta + dTheta*delta_t; 49 | Omega = [Omega, dTheta]; 50 | Angle = [Angle, Theta]; 51 | 52 | T = bot.fkine([Theta', 0]); 53 | X = [X, T.t]; 54 | phi = [phi, Theta(1) + Theta(2) + Theta(3)]; 55 | 56 | J_det = [J_det, det(J)]; 57 | 58 | tao = [tao, J'*W]; 59 | end 60 | end 61 | 62 | 63 | t = 0:delta_t:5; 64 | %plot(t, Angle) 65 | %title("关节角与时间的关系") 66 | %legend("theta1", "theta2", "theta3") 67 | %xlabel("时间/s"); ylabel("角度/rad") 68 | 69 | %plot(t, Omega) 70 | %title("关节角速度与时间的关系") 71 | %legend("w1", "w2", "w3") 72 | %xlabel("时间/s"); ylabel("角速度/(rad·s^{-1})") 73 | 74 | %plot(t, [X(1:2, :); phi]) 75 | %title("三个笛卡尔分量与实践的关系") 76 | %legend("x", "y", "phi") 77 | %xlabel("时间/s"); ylabel("m 或 rad") 78 | 79 | %plot(t, J_det) 80 | %title("雅可比矩阵行列式与时间的关系") 81 | 82 | %plot(t, tao) 83 | %title("关节力矩与时间的关系") 84 | %legend("tau1", "tau2", "tau3") 85 | %xlabel("时间/s"); ylabel("力或力矩/（N或N·m）") 86 | 87 | angle_0 = Angle(:, 1); 88 | angle_end = Angle(:, end); 89 | 90 | J_0 = bot.jacob0([angle_0' 0]); 91 | myjacobian(angle_0, L1, L2, L3) 92 | 93 | J_end = bot.jacob0([angle_end' 0]); 94 | myjacobian(angle_end, L1, L2, L3) -------------------------------------------------------------------------------- /第6章代码/chapter6.m: -------------------------------------------------------------------------------- 1 | clear; 2 | l1 = 4; l2 = 3; l3 = 2; 3 | Theta = [10; 20; 30]*pi/180; 4 | m1=20;m2=15;m3=10; 5 | I1=0.5;I2=0.2;I3=0.1; 6 | 7 | dtheta = [1;2;3]; 8 | ddtheta = [0.5;1;1.5]; 9 | G = [0;-9.81;0]; 10 | Omega = [0;0;0]; 11 | 12 | L1 = mylink(0, 0, 0, 0, 1); 13 | L2 = mylink(0, l1, 0, 0, 1); 14 | L3 = mylink(0, l2, 0, 0, 1); 15 | T01 = L1.transMatrix(Theta(1));R01 = T01(1:3,1:3);R10 =inv(R01); 16 | T12 = L2.transMatrix(Theta(2));R12 = T12(1:3,1:3);R21 =inv(R12); 17 | T23 = L3.transMatrix(Theta(3));R23 = T23(1:3,1:3);R32 =inv(R23); 18 | Th = [ 1 0 0 l3 19 | 0 1 0 0 20 | 0 0 1 0 21 | 0 0 0 1];R3h = Th(1:3,1:3);Rh3 =inv(R3h); 22 | T_1 = newton_om(R10,Omega,[0;0;0],dtheta(1),ddtheta(1),[0;0;0],m1,I1,G,[l1/2;0;0]); 23 | W1 = T_1(:,1); dW1=T_1(:,2);dv1=T_1(:,3);F1= T_1(:,4);N1= T_1(:,5); 24 | T_2 = newton_om(R21,W1,dW1,dtheta(2),ddtheta(2),[l1;0;0],m2,I2,dv1,[l2/2;0;0]); 25 | W2 = T_2(:,1); dW2=T_2(:,2);dv2=T_2(:,3);F2= T_2(:,4);N2= T_2(:,5); 26 | T_3 = newton_om(R32,W2,dW2,dtheta(3),ddtheta(3),[l2;0;0],m3,I3,dv2,[l3/2;0;0]); 27 | W3 = T_3(:,1); dW3=T_3(:,2);dv3=T_3(:,3);F3= T_3(:,4);N3= T_3(:,5); 28 | 29 | f4 = [0;0;0]; 30 | n4 = [0;0;0]; 31 | F_3 = back(N3,n4,Rh3,[l3;0;0],F3,f4,[l3/2;0;0]); 32 | f3 = F_3(:,1);n3 = F_3(:,2); 33 | 34 | F_2 = back(N2,n3,R23,[l2;0;0],F2,f3,[l2/2;0;0]); 35 | f2 = F_2(:,1);n2 = F_2(:,2); 36 | 37 | F_1 = back(N1,n2,R12,[l1;0;0],F1,f2,[l1/2;0;0]); 38 | f1 = F_1(:,1);n1 = F_1(:,2); 39 | 40 | tau3 = n3.' * [0;0;1]; 41 | tau2 = n2.' * [0;0;1]; 42 | tau1 = n1.' * [0;0;1]; 43 | [tau1 tau2 tau3] 44 | 45 | 46 | theta = [pi/18,pi/9,pi/6]'; % 初始角度 47 | thetaDot = [1,2,3]'; % 初始角速度 48 | v00Dot = [0;-9.81;0]; 49 | thetaDotDot = [0.5,1,1.5]'; 50 | L(1) = Link('alpha',0,'a',0,'d',0,'modified'); 51 | L(2) = Link('alpha',0,'a',l1,'d',0,'modified'); 52 | L(3) = Link('alpha',0,'a',l2,'d',0,'modified'); 53 | % 连杆质量 54 | L(1).m = m1; 55 | L(2).m = m2; 56 | L(3).m = m3; 57 | % 连杆质心位置 58 | L(1).r = [l1/2;0;0]; 59 | L(2).r = [l2/2;0;0]; 60 | L(3).r = [l3/2;0;0]; 61 | % 连杆惯性张量，式（6.28) 62 | L(1).I = [0,0,0; 0,0,0; 0,0,I1]; 63 | L(2).I = [0,0,0; 0,0,0; 0,0,I2]; 64 | L(3).I = [0,0,0; 0,0,0; 0,0,I3]; 65 | robot=SerialLink(L, 'name', 'robot'); 66 | TAU_1G_rtb = robot.rne(theta',thetaDot',thetaDotDot', 'gravity', v00Dot) 67 | TAU_GL_rtb = robot.gravload(theta', v00Dot); 68 | 69 | 70 | 71 | --------------------------------------------------------------------------------