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

好的，以下是用 MDH 参数构造 6 自由度机器人速度雅可比矩阵的 Matlab 代码实现：

function Jv = velocity_jacobian(alpha, a, d, theta)
% alpha, a, d, theta: 6个关节的 DH 参数
% Jv: 6x6 速度雅可比矩阵

% DH 参数表
dh_params = [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 = eye(4);
for i = 1:6
    T = T * dh_transformation(dh_params(i,1), dh_params(i,2), dh_params(i,3), dh_params(i,4));
end
R = T(1:3,1:3);
P = T(1:3,4);

% 计算末端速度雅可比矩阵
Jv = zeros(6,6);
for i = 1:6
    % 计算第i个关节轴在基座标系下的方向向量
    z = [0; 0; 1];
    for j = 1:i-1
        R_temp = dh_transformation(dh_params(j,1), dh_params(j,2), dh_params(j,3), dh_params(j,4));
        z = R_temp(1:3,1:3) * z;
    end
    % 计算第i个关节轴在基座标系下的位置向量
    p = [0; 0; 0];
    for j = 1:i-1
        R_temp = dh_transformation(dh_params(j,1), dh_params(j,2), dh_params(j,3), dh_params(j,4));
        p = p + R_temp(1:3,1:3) * [0; 0; dh_params(j,3)];
    end
    % 计算第i个关节轴的角速度和线速度雅可比矩阵
    Jw = z;
    Jv(:,i) = [cross(Jw, P-p); Jw];
end

end


function T = dh_transformation(alpha, a, d, theta)
% 返回 DH 参数为 alpha, a, d, theta 的变换矩阵

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];

end

代码中，`alpha`、`a`、`d`、`theta` 为 6 个关节的 DH 参数，`dh_transformation()` 函数用于计算 DH 变换矩阵，`velocity_jacobian()` 函数用于计算速度雅可比矩阵。最后得到的 `Jv` 矩阵即为 6 自由度机器人的速度雅可比矩阵。