示例MATLAB程序，采用Matlab SimMechanics(新版为Multibody)工具包建立六自由度机器人的三维动力学仿真模型，采用独立PD控制、重力补偿PD控制完成关节空间轨迹跟踪任务

下面是一个使用 SimMechanics 建立六自由度机器人的三维动力学仿真模型并使用独立 PD 控制完成关节空间轨迹跟踪任务的示例 MATLAB 程序：

% 建立机器人模型
robot = createRobot();

% 设定轨迹跟踪任务的目标关节角度
goal = [0.5, 0.6, 0.7, 0.8, 0.9, 1.0];

% 设定 PD 控制参数
kp = 10;
kd = 1;

% 仿真步数
steps = 100;

% 初始化关节角度
q = zeros(steps, 6);

% 开始仿真
for i = 1:steps
    % 计算当前关节角度的误差
    error = goal - q(i,:);
    
    % 计算关节角速度
    qd = kp * error + kd * (error - q(i-1,:));
    
    % 更新关节角度
    q(i+1,:) = q(i,:) + qd;
    
    % 更新机器人的位置
    updateRobot(robot, q(i,:));
end

% 绘制关节角度的轨迹
plot(q);

若要使用重力补偿 PD 控制，则在计算关节角速度时，需要额外加入重力补偿项，即：

% 计算重力补偿力
g = gravityCompensation(robot, q(i,:));

% 计算关节角速度
qd = kp * error + kd * (error - q(i-1,:)) + g;

需要注意的是，上述程序中的 `createRobot` 和 `updateRobot` 函数是需要您自己编写的。可以参考 SimMech