Robotics正运动学求解仿真（附代码和解释）

最近小虎在课程设计上用到了机器人学正运动学的仿真学习，利用D-H参数法求解正运动学问题。在利用MATLAB仿真中，通过设置不同的D-H参数以及位置的数量，可以实现多关节不同的位姿位置变换。另外，通过参数连续化，可以近似拟合出动态的运动。

目录

• 结果
• 代码分析
• • D-H参数表
  • Build_3DOFRobot.m
  • DHfk3Dof.m
  • DH_Matrix.m
  • DrawCylinder.m
  • Connect3D.m
  • Mov_3DOF_Rob.m
• 完整代码——已存github

结果

静态的四位置图：

动态仿真图：

代码分析

代码部分分为了6个程序，分别是设置初始参数、D-H求解、D-H参数绘图、3D圆柱关节绘制、3D杆件绘制、移动主程序。代码分析无删减。

D-H参数表

objects	$\theta$	$d$	$a$	$\alpha$
1	$0$	0	50	$90^0$
2	$20^0$	100	0	$90^0$
3	$30^0$	120	0	$0$

比如关节2到3的变换可以就简化为：

在这里的实际例子是：
$${}^2{T}_3=A_3=\left[\begin{array}{cccc}0.9397& 0& 0.3420& 0\\ 0.3420& 0& -0.9397& 0\\ 0& -1& 0& 100\\ 0& 0& 0& 1\end{array}\right]$$

Build_3DOFRobot.m

参数设置，D-H参数表。
有些变换要进行角度弧度的转变，因此设有ToDeg和ToRad。

ToDeg = 180/pi;
ToRad = pi/180; 
UZ = [0 0 1]';

构建结构变量，以便设置D-H参数表，$\theta$在调用DH求解时候设置，这里暂且设置为零。

Link= struct('name','Body' , 'th', 0, 'dz', 0, 'dx', 0, 'alf',90*ToRad,'az',UZ); 
Link(1)= struct('name','Base' , 'th', 0*ToRad, 'dz', 0, 'dx', 0, 'alf',0*ToRad,'az',UZ);
Link(2) = struct('name','J1' , 'th', 0*ToRad, 'dz', 50, 'dx', 0, 'alf',90*ToRad,'az',UZ);
Link(3) = struct('name','J2' , 'th', 0*ToRad, 'dz', 0, 'dx', 100, 'alf',90*ToRad,'az',UZ); 
Link(4) = struct('name','J3' , 'th', 0*ToRad, 'dz', 0, 'dx', 120, 'alf',0*ToRad,'az',UZ);

DHfk3Dof.m

D-H参数绘图。
调用D-H参数表，设置绘图尺寸（关节圆柱），将角度代入。

function DHfk3Dof(th1,th2,th3,fcla)
global Link;
Build_3DOFRobot;
radius = 10; 
len= 30;
joint_col = 0; 
plot3(0,0,0,'ro');
Link(2).th=th1*pi/180; 
Link(3).th=th2*pi/180; 
Link(4).th=th3*pi/180;

求解D-H参数矩阵。

for i=1:4 
    DH_Matrix(i);
end

绘图。

for i=2:4 
    Link(i).A=Link(i-1).A*Link(i).A; 
    Link(i).p= Link(i).A(:,4); 
    Link(i).n= Link(i).A(:,1); 
    Link(i).o= Link(i).A(:,2); 
    Link(i).a= Link(i).A(:,3);
    Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)]; 
    Connect3D(Link(i-1).p,Link(i).p,'b',2); hold on;
DrawCylinder(Link(i-1).p, Link(i-1).R * Link(i).az, radius,len, joint_col); 
hold on;
end

DH_Matrix.m

D-H矩阵求解。

function DH_Matrix(i) 
global Link 
C=cos(Link(i).th); 
S=sin(Link(i).th); 
Ca=cos(Link(i).alf);
Sa=sin(Link(i).alf); 
a=Link(i).dx; 
d=Link(i).dz; 

Link(i).n=[C,S,0,0]'; 
Link(i).o=[-1*S*Ca,C*Ca,Sa,0]'; 
Link(i).a=[S*Sa, -1*C*Sa,Ca,0]';
Link(i).p=[a*C,a*S,d,1]'; 
Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)]; 
Link(i).A=[Link(i).n,Link(i).o,Link(i).a,Link(i).p];

DrawCylinder.m

画关节圆柱。

function h = DrawCylinder(pos, az, radius,len, col)
%rotation matrix
az0 = [0;0;1];
ax  = cross(az0,az);
ax_n = norm(ax);
if ax_n < eps 
	rot = eye(3);
else
    ax = ax/ax_n;
    ay = cross(az,ax);
    ay = ay/norm(ay);
    rot = [ax ay az];
end

%********** make cylinder
% col = [0 0.5 0];  % cylinder color

a = 20;    % number of side faces
theta = (0:a)/a * 2*pi;

x = [radius; radius]* cos(theta);
y = [radius; radius] * sin(theta);
z = [len/2; -len/2] * ones(1,a+1);
cc = col*ones(size(x));

for n=1:size(x,1)
   xyz = [x(n,:);y(n,:);z(n,:)];
   xyz2 = rot * xyz;
   x2(n,:) = xyz2(1,:);
   y2(n,:) = xyz2(2,:);
   z2(n,:) = xyz2(3,:);
end

%************* draw
% side faces
h = surf(x2+pos(1),y2+pos(2),z2+pos(3),cc);

for n=1:2
	patch(x2(n,:)+pos(1),y2(n,:)+pos(2),z2(n,:)+pos(3),cc(n,:));
end	

Connect3D.m

画连杆。

function Connect3D(p1,p2,option,pt)
h = plot3([p1(1) p2(1)],[p1(2) p2(2)],[p1(3) p2(3)],option);
set(h,'LineWidth',pt)
end

Mov_3DOF_Rob.m

移动主程序，这里还设了三个$\theta$的值。

close all; 
th1=0;
th2=20; 
th3=30;
DHfk3Dof(th1,th2,th3,0); 
view(134,12); 

动态分析：

pause; 
stp=5;
for i=0:stp:180 
    DHfk3Dof(th1+i,th2,th3,1);
end

完整代码——已存github

见我的github:Robotics-motion-simulation