dh参数逆运动学_DH参数法建立机器人的运动学正解

DH参数法建立机器人的运动学正解

运用DH参数法时坐标系建立的两个约定：

(1)x_i与z_(i-1)垂直

(2)x_i与z_(i-1)相交

坐标系i与坐标系i-1的其次变换矩阵为：

a为两z轴的距离，d为两x轴的距离。

alpha与theta的正方向约定为：

下面举三个例子：

a、平面二自由度机器人

clear;

clc;

syms theta1 alpha1 a1 d1 theta2 alpha2 a2 d2 a b theta d;

A1=[cos(theta1),-sin(theta1)*cos(alpha1),sin(theta1)*sin(alpha1),a1*cos(theta1);...

sin(theta1),cos(theta1)*cos(alpha1),-cos(theta1)*sin(alpha1),a1*sin(theta1);...

0,sin(alpha1),cos(alpha1),d1;...

0,0,0,1];

A2=[cos(theta2),-sin(theta2)*cos(alpha2),sin(theta2)*sin(alpha2),a2*cos(theta2);...

sin(theta2),cos(theta2)*cos(alpha2),-cos(theta2)*sin(alpha2),a2*sin(theta2);...

0,sin(alpha2),cos(alpha2),d2;...

0,0,0,1];

L=sqrt(a^2+b^2);

beta=atan(b/a);

a1=L;

alpha1=sym(0);

d1=sym(0);

theta1=theta;

a2=d;

alpha2=sym(0);

d2=sym(0);

theta2=-beta;

T=eval_r(A1*A2)

求得运动学正解为：

b、平面三自由度机器人

clear;

clc;

syms theta1 alpha1 a1 d1 theta2 alpha2 a2 d2 theta3 alpha3 a3 d3 d;

A1=[cos(theta1),-sin(theta1)*cos(alpha1),sin(theta1)*sin(alpha1),a1*cos(theta1);...

sin(theta1),cos(theta1)*cos(alpha1),-cos(theta1)*sin(alpha1),a1*sin(theta1);...

0,sin(alpha1),cos(alpha1),d1;...

0,0,0,1];

A2=[cos(theta2),-sin(theta2)*cos(alpha2),sin(theta2)*sin(alpha2),a2*cos(theta2);...

sin(theta2),cos(theta2)*cos(alpha2),-cos(theta2)*sin(alpha2),a2*sin(theta2);...

0,sin(alpha2),cos(alpha2),d2;...

0,0,0,1];

A3=[cos(theta3),-sin(theta3)*cos(alpha3),sin(theta3)*sin(alpha3),a3*cos(theta3);...

sin(theta3),cos(theta3)*cos(alpha3),-cos(theta3)*sin(alpha3),a3*sin(theta3);...

0,sin(alpha3),cos(alpha3),d3;...

0,0,0,1];

alpha1=sym(0);

d1=sym(0);

a2=d;

alpha2=sym(0);

d2=sym(0);

theta2=sym(0);

alpha3=sym(0);

d3=sym(0);

theta3=-theta3;

T=eval_r(A1*A2*A3)

求得运动学正解为：

c、六自由度机器人

clear;

clc;

syms theta1 alpha1 a1 d1 theta2 alpha2 a2 d2 theta3 alpha3 a3 d3 ...

theta4 alpha4 a4 d4 theta5 alpha5 a5 d5 theta6 alpha6 a6 d6;

A1=[cos(theta1),-sin(theta1)*cos(alpha1),sin(theta1)*sin(alpha1),a1*cos(theta1);...

sin(theta1),cos(theta1)*cos(alpha1),-cos(theta1)*sin(alpha1),a1*sin(theta1);...

0,sin(alpha1),cos(alpha1),d1;...

0,0,0,1];

A2=[cos(theta2),-sin(theta2)*cos(alpha2),sin(theta2)*sin(alpha2),a2*cos(theta2);...

sin(theta2),cos(theta2)*cos(alpha2),-cos(theta2)*sin(alpha2),a2*sin(theta2);...

0,sin(alpha2),cos(alpha2),d2;...

0,0,0,1];

A3=[cos(theta3),-sin(theta3)*cos(alpha3),sin(theta3)*sin(alpha3),a3*cos(theta3);...

sin(theta3),cos(theta3)*cos(alpha3),-cos(theta3)*sin(alpha3),a3*sin(theta3);...

0,sin(alpha3),cos(alpha3),d3;...

0,0,0,1];

A4=[cos(theta4),-sin(theta4)*cos(alpha4),sin(theta4)*sin(alpha4),a4*cos(theta4);...

sin(theta4),cos(theta4)*cos(alpha4),-cos(theta4)*sin(alpha4),a4*sin(theta4);...

0,sin(alpha4),cos(alpha4),d4;...

0,0,0,1];

A5=[cos(theta5),-sin(theta5)*cos(alpha5),sin(theta5)*sin(alpha5),a5*cos(theta5);...

sin(theta5),cos(theta5)*cos(alpha5),-cos(theta5)*sin(alpha5),a5*sin(theta5);...

0,sin(alpha5),cos(alpha5),d5;...

0,0,0,1];

A6=[cos(theta6),-sin(theta6)*cos(alpha6),sin(theta6)*sin(alpha6),a6*cos(theta6);...

sin(theta6),cos(theta6)*cos(alpha6),-cos(theta6)*sin(alpha6),a6*sin(theta6);...

0,sin(alpha6),cos(alpha6),d6;...

0,0,0,1];

a1=sym(0);

alpha1=sym(-pi/2);

alpha2=sym(0);

d2=sym(0);

a3=sym(0);

alpha3=sym(-pi/2);

d3=sym(0);

a4=sym(0);

alpha4=sym(-pi/2);

d4=sym(0);

a5=sym(0);

alpha5=sym(pi/2);

d5=sym(0);

a6=sym(0);

alpha6=sym(0);

T=simplify(eval_r(A1*A2*A3*A4*A5*A6))

注：以上的eval_r为eval(不知道为什么，保存后“eval”就变成“eval_r”了)

from:http://blog.sina.com.cn/u/2707887295