matlab练习程序(机械臂DH模型)
之前有写过二维机械臂正逆运动学运算,不过一般机械臂都是三维的,通常可以用DH模型来表示。
DH模型其坐标系和参数定义如下图:
坐标系定义:
X:当前关节指向下一个关节的方向。
Z:当前关节绕动轴的方向。
Y:垂直于XZ平面的方向。
每个关节都由四个参数[a,d,alpha,theta]定义:
a:上一个关节和当前关节延X方向的距离。
d:上一个关节和当前关节延Z方向的距离。
alpha:延X方向上一个关节和当前关节Z轴间夹角。
theta:绕Z轴当前关节的旋转角度。
最后可以根据四个参数写出转移矩阵:
根据转移矩阵即可得到机械臂的正向坐标变换和最终位置,即机械臂正向运动学。
根据正向变换和最终位置即可计算得到旋转参数,即机械臂逆向运动学。
有robot工具箱可以用工具箱求解,没有工具箱可以用最优化方法求解,当然也可以直接计算,这里用最优化方式求解。
matlab代码如下:
clear all;close all;clc; warning off; %建立机器人模型PUMA560 % theta d a alpha offset % SL1=Link([0 0 0 -pi/2 0 ],'standard'); % SL2=Link([0 0 0.432 0 0 ],'standard'); % SL3=Link([0 0.149 0.02 pi/2 0 ],'standard'); % SL4=Link([0 0.433 0 -pi/2 0 ],'standard'); % SL5=Link([0 0 0 pi/2 0 ],'standard'); % SL6=Link([0 0 0 0 0 ],'standard'); % p560=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','puma560'); T=eye(4); T(1,4) = 0.5; T(2,4) = -0.2; T(3,4) = -0.4; % q = p560.ikine(T); %robot工具箱逆运动学求解 % p560.plot(q); % endPos = p560.fkine(q); %robot工具箱正运动学求解 options.Algorithm = 'levenberg-marquardt'; par = zeros(1,6); f = @(par) func(par,T); par= lsqnonlin(f,par,[],[],options); %执行优化 % figure; % p560.plot(par); endPos = model(par) %DH模型 function endPos = model(q) % d a alpha theta DHTable = [ 0 0 -pi/2 q(1); 0 0.432 0 q(2); 0.149 0.02 pi/2 q(3); 0.433 0 -pi/2 q(4); 0 0 pi/2 q(5); 0 0 0 q(6)]; endPos = eye(4); for i = 1:size(DHTable,1) theta = DHTable(i,4); alpha = DHTable(i,3); d = DHTable(i,1); a = DHTable(i,2); T = [cos(theta) -sin(theta)*cos(alpha) sin(theta)*sin(alpha) a*cos(theta); sin(theta) cos(theta)*cos(alpha) -cos(theta)*sin(alpha) a*sin(theta); 0 sin(alpha) cos(alpha) d; 0 0 0 1]; endPos = endPos * T; end end function re = func(par,T) endPos = model(par); re = T - endPos; end
结果如下: