Robotics正運動學求解仿真（附代碼和解釋）

摘要

最近小虎在課程設計上用到了機器人學正運動學的仿真學習，利用D-H參數法求解正運動學問題。在利用MATLAB仿真中，通過設置不同的D-H參數以及位置的數量，可以實現多關節不同的位姿位置變換。另外，通過參數連續化，可以近似擬合出動態的運動。

結果

靜態的四位置圖：

動態仿真圖：

代碼分析

代碼部分分爲了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的變換可以就簡化爲：

在這裏的實際例子是：
$^2T_3=A_3=\begin{bmatrix} 0.9397 & 0 & 0.3420& 0\\ 0.3420 & 0 & -0.9397& 0\\ 0 & -1 & 0& 100\\ 0 & 0 & 0& 1 \end{bmatrix}$

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

完整代碼

其實就在代碼分析中。見CSDN資源下載，如果超過20積分，請評論區留下郵箱小虎有空將免費發送。
https://download.csdn.net/download/Davidietop/12313360