【MATLAB源码】平面3R机器人正运动学MATLAB计算

%% 3R planar robot DIRECT KINEMATICS % 07.2018, @Giuseppe Sensolini clear all clc %% define symbolic variables syms a alpha d theta %% number of joint N = 3; %% DH table of parameters of 2R, same for each joint % assign these parameters in order to compute specific values % | a | alpha | d | theta | DHTABLE = [ sym('a1') 0 0 sym('q1'); sym('a2') 0 0 sym('q2'); sym('a3') 0 0 sym('q3') ]; %% Build the general Denavit-Hartenberg trasformation matrix TDH = [ cos(theta) -sin(theta) 0 a*cos(theta); sin(theta) cos(theta) 0 a*sin(theta); 0 0 1 d; 0 0 0 1 ]; %% Build transformation matrices for each link A = cell(1,N); % For every row in 'DHTABLE' we substitute the right value inside % the general DH matrix for i = 1:N a = DHTABLE(i,1); alpha = DHTABLE(i,2); d = DHTABLE(i,3); theta = DHTABLE(i,4); A{i} = subs(TDH); end %% Direct kinematics disp('Direct kinematics of 2R robot in symbolic form') disp(['Number of joints N = ',num2str(N)]) T = eye(4); for i = 1:N T = T*A{i}; T = simplify(T); end % output direct kinematics matrix TOE = T % output end-effector position p = T(1:3,4) % output xN axis n = T(1:3,1) % output yN axis s = T(1:3,2) % output zN axis a = T(1:3,3) %% end