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

以下是一个简单的示例MATLAB程序，它建立了一个六自由度机器人的三维动力学仿真模型，并使用独立PD控制和重力补偿PD控制来完成关节空间轨迹跟踪任务。

% 机器人参数
% 长度单位 m, 质量单位 kg
L1 = 0.2; L2 = 0.2; L3 = 0.2;
m1 = 2; m2 = 2; m3 = 2;
g = 9.8;

% 建立机器人模型
robot = SerialLink([
    Revolute('d', 0, 'a', 0, 'alpha', pi/2, 'm', m1, 'r', [0 0 0], 'I', [0 0 0], 'G', 1, 'Jm', 0, 'B', 0, 'qlim', [-pi pi])
    Revolute('d', 0, 'a', L1, 'alpha', 0, 'm', m2, 'r', [0 L1/2 0], 'I', [0 0 0], 'G', 1, 'Jm', 0, 'B', 0, 'qlim', [-pi pi])
    Revolute('d', 0, 'a', L2, 'alpha', 0, 'm', m3, 'r', [0 L2/2 0], 'I', [0 0 0], 'G', 1, 'Jm', 0, 'B', 0, 'qlim', [-pi pi])
]);

% 关节空间轨迹
t = linspace(0, 2*pi, 100);
q1 = sin(t);
q2 = cos(t);
q3 = sin(2*t);

% PD控制参数
kp = 100*eye(3);
kd = 10*eye(3);

% 初始化状态
q = [0 0 0];
qd = [0 0 0];
qdd = [0 0 0];

% 仿真
for i = 1:length(t)
    % 计算重力项
    tau_g = robot.gravload(q);
    
    % 计算惯性项和科里奥利项
    [M, C] = robot.inertia(q);
    
    % PD控制
    e = [q1(i) q2(i) q3(i)] - q;
    ed = [0 0 0] - qd;
    tau_pd = kp*e' + kd*ed';
    
    % 计算加速度
    qdd = inv(M)*(tau_pd - C*qd' - tau_g');
    
    % 更新状态
    qdd = min(max(qdd, -10), 10);
    qd = qd + qdd*0.01;
    qd = min(max(qd, -10), 10);
    q = q + qd*0.01;
    q = min(max(q, -pi), pi);
    
    % 绘图
    robot.plot(q, 'trail', 'r-');
    axis([-0.5 0.5 -0.5 0.5 -0.5 0.5]);
    drawnow;
end

该程序使用SerialLink类建立了一个三轴机器人的模型，并利用PD控制方程计算关节加速度。程序通过更新状态来模拟机器人的运动，并可以在每个时间步骤中进行绘图以实现动态仿真。