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

 结果如下：