用 matlab 代码实现 6 自由度机器人的速度雅可比矩阵的构 造，并建立雅可比矩阵函数，要求：输入参数为 alpha、a、d、theta， 返回 6x6 速度雅可比矩阵；

下面是用 Matlab 实现 6 自由度机器人的速度雅可比矩阵的构造的代码，并建立雅可比矩阵函数。

function Jv = VelocityJacobian(alpha, a, d, theta)
% alpha, a, d, theta 分别为机器人 DH 参数中的四个变量，单位为弧度和米
% Jv 为 6x6 的速度雅可比矩阵

% 机器人的 DH 参数
DH = [alpha(1) a(1) d(1) theta(1);
      alpha(2) a(2) d(2) theta(2);
      alpha(3) a(3) d(3) theta(3);
      alpha(4) a(4) d(4) theta(4);
      alpha(5) a(5) d(5) theta(5);
      alpha(6) a(6) d(6) theta(6)];

% 初始化基座系下和末端执行器系下的旋转矩阵和平移矩阵
T = cell(1,7);
T{1} = eye(4);
R = cell(1,6);
P = cell(1,6);

% 计算各关节的旋转矩阵和平移矩阵
for i = 1:6
    A = [cos(DH(i,4)) -sin(DH(i,4))*cos(DH(i,1)) sin(DH(i,4))*sin(DH(i,1)) DH(i,2)*cos(DH(i,4));
         sin(DH(i,4)) cos(DH(i,4))*cos(DH(i,1)) -cos(DH(i,4))*sin(DH(i,1)) DH(i,2)*sin(DH(i,4));
         0 sin(DH(i,1)) cos(DH(i,1)) DH(i,3);
         0 0 0 1];
    T{i+1} = T{i} * A;
    R{i} = T{i+1}(1:3,1:3);
    P{i} = T{i+1}(1:3,4);
end

% 初始化雅可比矩阵
Jv = zeros(6,6);

% 计算末端执行器系下的位置矢量和旋转矢量
P6 = P{6};
R6 = R{6};

% 计算每个关节对末端执行器系下的位置矢量和旋转矢量的影响
for i = 1:6
    Ji = [cross(R{i}(:,3),P6-P{i});
          R{i}(:,3)];
    Jv(:,i) = Ji;
end

使用示例：

% 6 自由度机器人的 DH 参数
alpha = [0 -pi/2 0 0 pi/2 -pi/2];
a = [0 0.5 0 1 0 0];
d = [0 0 0.5 0 0 0.1];
theta = [0 pi/3 -pi/6 pi/4 pi/2 0];

% 计算速度雅可比矩阵
Jv = VelocityJacobian(alpha, a, d, theta);

以上代码实现了输入机器人 DH 参数，计算出 6 自由度机器人的速度雅可比矩阵的功能。