UR5机械臂PID轨迹跟踪控制控制，六自由度机械臂simscape物理仿真，需要可以提供DH参数表，坐标系表示，三维模型，可以导出角度，角速度，角加速度以及力矩，误差曲线图
机械臂轨迹跟踪这事儿听起来高大上，其实核心就两件事——建模准不准和控制算法硬不硬。今天咱们拿UR5开刀，手把手在Simscape里搭个能看见关节力矩变化的物理仿真，顺便验证下老生常谈的PID到底能不能打。
先上硬货，UR5的DH参数得扒清楚。别被那些花里胡哨的坐标系吓到，直接上MATLAB数组更实在：
dh = [0 pi/2 0.089159; 0.425 0 0; 0.392 0 0; 0 pi/2 0.10915; 0 -pi/2 0.09465; 0 0 0.0823]; % 单位：米/弧度注意第三列其实是连杆偏距，第一行那个0.089米对应UR5基座到第二个关节的竖直距离。有兄弟问为啥要转来转去的坐标系？说白了就是让每个关节的运动学计算能套用统一公式，省得每次重新推导。
Simscape建模时有个坑——默认的刚体惯性参数可能和真实UR5对不上。咱们直接在Solid块右键导入STEP模型，这时候会弹出一个让人头大的惯性矩阵。别慌，记住这个口诀：质量误差不超过10%，转动惯量主要看对角线元素，实在搞不定就偷个懒用默认值先跑起来。
UR5机械臂PID轨迹跟踪控制控制，六自由度机械臂simscape物理仿真，需要可以提供DH参数表，坐标系表示，三维模型，可以导出角度，角速度，角加速度以及力矩，误差曲线图
轨迹跟踪的核心在于PID三个参数的配合。给各位看看我调试时用的Simulink PID模块参数：
Kp = diag([800 600 500 400 300 200]); % 六个关节比例系数 Ki = diag([5 4 3 2 1 0.5]); % 别笑，最后一个关节确实需要弱积分 Kd = diag([50 40 30 20 10 5]); % 阻尼系数看实际振动情况这里有个骚操作——把积分项限幅设置成扭矩最大值的20%，防止出现关节抱死的情况。比如UR5的肘关节最大扭矩约150N·m，那积分上限就设在30N·m左右。
仿真跑起来后重点看这几个信号：
- 关节角度误差（Desired - Actual）
- 电机输出扭矩曲线
- 末端执行器位姿偏差
用这个脚本导出数据并画图：
simout = sim('ur5_pid_tracking'); error = get(simout.logsout,'joint_error').Values; plot(error.Time, error.Data(:,1), 'LineWidth',2); hold on % 其他关节类似... xlabel('时间(s)');ylabel('角度误差(rad)');典型情况下会看到前0.5秒有个剧烈震荡，这时候别急着调参数，先检查接触力设置——是不是机械臂突然加载导致动力学突变？把接触刚度从1e6降到1e4 N/m试试，仿真会更稳定。
遇到轨迹拐点处误差飙升怎么办？临时加大微分项不是长久之计。更聪明的做法是给期望轨迹加个低通滤波，相当于给目标轨迹"打码"，让机械臂有个缓冲时间。代码实现就两行：
t = 0:0.001:10; qd_smooth = lowpass(qd_raw, 5, 1000); % 截止频率5Hz最后说个血泪教训：仿真能跑≠真机可用。务必检查电机模型里的扭矩饱和模块是否启用，现实的伺服电机可不会任你1000N·m的指令随便发。把扭矩限制设到UR5规格的120%，这样得到的误差曲线才靠谱。
)
)