示例MATLAB可运行简单程序，建立六自由度机器人的三维动力学仿真模型，采用独立PD控制、重力补偿PD控制完成关节空间轨迹跟踪任务...

以下是一个六自由度机器人的三维动力学仿真模型的示例MATLAB代码：

% 机器人参数
% D-H 参数
a = [0 0.4318 0.0203 0 0 0]; % 机械臂连杆长度 a
d = [0.333 0 0.316 0 0.384 0.107]; % 机械臂连杆长度 d
alpha = [pi/2 0 -pi/2 pi/2 -pi/2 0]; % 机械臂连杆扭曲角 alpha
theta0 = [0 0 0 0 0 0]; % 初始关节角度

% 重力加速度
g = [0 0 -9.8];

% 仿真时间
tspan = [0 10];

% PD控制参数
Kp = diag([100 100 100 100 100 100]);
Kd = diag([10 10 10 10 10 10]);
q_des = [0.5 0.3 -0.5 0.6 -0.8 0.2];

% 仿真
[t, q, qd] = ode45(@robot_dynamics, tspan, [theta0 zeros(1, 6)], [], a, alpha, d, g, Kp, Kd, q_des);

% 画出关节角度随时间的变化曲线
figure;
plot(t, q(:, 1), t, q(:, 2), t, q(:, 3), t, q(:, 4), t, q(:, 5), t, q(:, 6));
legend('q_1', 'q_2', 'q_3', 'q_4', 'q_5', 'q_6');
xlabel('Time (s)');
ylabel('Joint angles (rad)');

% 画出关节速度随时间的变化曲线
figure;
plot(t, qd(:, 1), t, qd(:, 2), t, qd(:, 3), t, qd(:, 4), t, qd(:, 5), t, qd(:, 6));
legend('q_1', 'q_2', 'q_3', 'q_4', 'q_5', 'q_6');
xlabel('Time (s)');
ylabel('Joint velocities (rad/s)');

function dqdt = robot_dynamics(t, q, a, alpha, d, g, Kp, Kd, q_des)
% 计算机器人的动力学模型
% 输入：
% t - 时间
% q - 关节角度和速度
% a, alpha, d - D-H 参数
% g - 重力加速度
% Kp, Kd - PD控制参数
% q_des - 期望关节角度
% 输出：
% dqdt - 关节角度和速度的变化率

theta = q(1:6); % 当前关节角度
theta_dot = q(7:12); % 当前关节角速度

% 计算运动学矩阵
T = eye(4);
T_prev = T;
T_list = {};
for i = 1:6
    T = T * dh_transform(a(i), alpha(i), d(i), theta(i));
    T_list{i} = T;
end

% 计算雅可比矩阵
Jv = zeros(3, 6);
Jw = zeros(3, 6);
for i = 1:6
    p_i = T_list{i}(1:3, 4); % 第i个连杆的末端点位置
    Z_i = T_prev(1:3, 3); % 第i-1个连杆的z轴方向
    Jv(:, i) = cross(Z_i, p_i);
    Jw(:, i) = Z_i;
    T_prev = T_list{i};
end
J = [Jv; Jw];

% 计算逆动力学
M = inertia_matrix(a, alpha, d, theta);
C = centrifugal_coriolis_matrix(a, alpha, d, theta, theta_dot);
G = gravity_vector(a, alpha, d, theta, g);
u = Kp * (q_des - theta) - Kd * theta_dot + G;

theta_dot_dot = inv(M) * (u - C * theta_dot);

dqdt = [theta_dot; theta_dot_dot];
end

function T = dh_transform(a, alpha, d, theta)
% 计算D-H变换矩阵
% 输入：
% a, alpha, d, theta - D-H参数
% 输出：
% T - 变换矩阵

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

function M = inertia_matrix(a, alpha, d, theta)
% 计算惯性矩阵
% 输入：
% a, alpha, d, theta - D-H参数
% 输出：
% M - 惯性矩阵

M = zeros(6, 6);
for i = 1:6
    T_i = dh_transform(a(i), alpha(i), d(i), theta(i));
    R_i = T_i(1:3, 1:3);
    m_i = 10; % 连杆质量
    I_i = [1 0 0; 0 1 0; 0 0 1]; % 连杆惯性矩阵
    J_i = R_i * I_i * R_i'; % 连杆惯性矩阵在基坐标系下的表示
    r_i = T_i(1:3, 4); % 连杆质心在基坐标系下的位置
    G_i = [eye(3) -skew(r_i); zeros(3) eye(3)]; % 连杆质心的位姿矩阵
    M = M + G_i' * J_i * G_i * m_i;
end
end

function C = centrifugal_coriolis_matrix(a, alpha, d, theta, theta_dot)
% 计算离心惯性和科里奥利力矩阵
% 输入：
% a, alpha, d, theta - D-H参数
% theta_dot - 关节角速度
% 输出：
% C - 离心惯性和科里奥利力矩阵

C = zeros(6, 1);
for i = 1:6
    C_i = 0;
    for j = 1:6
        for k = 1:6
            C_i = C_i + 0.5 * inertia_matrix_element(a, alpha, d, theta, i, j, k) ...
                * (theta_dot(j) * theta_dot(k) + (diff(inertia_matrix_element(a, alpha, d, theta, j, k), theta(i)) ...
                + diff(inertia_matrix_element(a, alpha, d, theta, i, j), theta(k)) - diff(inertia_matrix_element(a, alpha, d, theta, i, k), theta(j))) ...
                * theta_dot(i));
        end
    end
    C(i) = C_i;
end
end

function val = inertia_matrix_element(a, alpha, d, theta, i, j, k)
% 计算惯性矩阵中的元素
% 输入：
% a, alpha, d, theta - D-H参数
% i, j, k - 矩阵中的元素下标
% 输出：
% val - 惯性矩阵中的元素值

T_i = dh_transform(a(i), alpha(i), d(i), theta(i));
R_i = T_i(1:3, 1:3);
I_i = [1 0 0; 0 1 0; 0 0 1]; % 连杆惯性矩阵
J_i = R_i * I_i * R_i'; % 连杆惯性矩阵在基坐标系下的表示
val = J_i(j, k);
end

function G = gravity_vector(a, alpha, d, theta, g)
% 计算重力向量
% 输入：
% a, alpha, d, theta - D-H参数
% g - 重力加速度
% 输出：
% G - 重力向量

G = zeros(6, 1);
for i = 1:6
    T_i = dh_transform(a(i), alpha(i), d(i), theta(i));
    r_i = T_i(1:3, 4); % 连杆质心在基坐标系下的位置
    G_i = [g * -1; zeros(3, 1)]; % 连杆上的重力向量
    G = G + T_i' * G_i;
end
end

这个代码计算了机器人的动力学模型，并使用ODE45求解了机器人在PD控制下的关节空间轨迹跟踪任务。可以通过修改PD控制参数和期望关节角度，来探索机器人的运动。