简介这份资源面向机器人控制课程与自动化专业学生聚焦二自由度机器人的位置PD控制实验内容涵盖机器人几何模型与动力学模型搭建、控制器设计及参数整定方法。文档基于Matlab、Simulink和Robot工具箱展开给出了二连杆参数配置、运动学与动力学建模关键代码并对比了简单PD控制、PD加重力补偿等控制方案的优劣能帮助读者理解机器人控制实验从建模到仿真的完整流程与实现细节。资源包为单个PDF文件大小约245KB便于阅读、打印和反复查阅。该资源已有1221人浏览学习适合正在完成相关课程设计、毕业设计或入门机器人控制的读者参考借鉴也可作为教师备课与实验指导的辅助材料。1. 机器人控制实验的起点为什么先做二自由度位置PD控制第一次走进二自由度机器人实验台之前很多人默认“位置控制”就是把目标角度填进去电机自然会转到该停的位置。真正跑起来才发现不是抖成筛子就是慢到像没吃饭。二自由度机器人的位置PD控制是机器人控制里最容易被低估的一块控制律只有两项却能暴露力矩饱和、传动间隙、微分噪声和重力补偿这四类工程问题。这篇文章以一个小型平面两连杆机械臂为对象从动力学建模、仿真回路、增益整定到实验台排错走一遍适合正在做课程实验、入门机器人控制的工程师也适合把PD当“落后”算法而没细调过底层的人。2. 二自由度机器人位置PD控制的建模与仿真环境2.1 关节空间PD控制与末端位置控制的关系二自由度机器人的“位置控制”可以分两个层面一是让每个关节转到一个给定角度二是让末端到达笛卡尔空间某个坐标点。实际实验里最可靠的落地方式是先做关节空间PD控制。原因很简单末端笛卡尔误差到关节力矩的转换依赖雅可比矩阵遇到奇异位形会出现控制发散而关节空间的误差和控制量都是标量物理意义清楚调试时能直接看到每个关节的行为。把两个关节都在关节空间锁准末端的位姿自然就准了。反过来如果盲目在笛卡尔空间做PD两个关节的增益会互相耦合可能末端看着在逼近目标但关节内部在较劲。所以我的建议是第一阶段只做关节空间的位置PD控制目标角度由逆运动学算出来或手动给定先让关节“听话”。2.2 动力学模型与控制律要仿真二自由度机器人的位置PD控制至少要有一个动力学模型。这里选最常见的平面两连杆点质量模型第一段质量集中在连杆1末端第二段质量集中在连杆2末端忽略摩擦和电机动力学。方程形式为[ M(q)\ddot{q} C(q,\dot{q})\dot{q} G(q) \tau ]其中 (M(q)) 是惯性矩阵(C(q,\dot{q})) 是科氏力和离心力项(G(q)) 是重力项。PD控制律加一个重力前馈[ \tau K_p (q_d - q) - K_d \dot{q} G(q) ]这里 (q_d) 是目标角度(K_p) 是比例增益(K_d) 是微分增益。注意这里的微分项用的是关节速度 (\dot{q})而不是位置误差的导数两者在目标不变时等价但用关节速度可以在轨迹跟踪时避免“目标移动引起的虚假误差导数”。之所以要加 (G(q))是因为纯PD控制靠误差产生力矩想要克服重力就必须保留一个静态误差。把这个误差减小到零需要无限大的 (K_p)这在实验台上不可行。加上重力补偿后稳态误差理论上为零。2.3 用Python搭一个最小的二自由度机器人控制回路下面这段代码把动力学、PD控制、欧拉积分三部分合在一起可以直接跑出两个关节的阶跃响应。仿真步长取1ms时长3秒。import numpy as np import matplotlib.pyplot as plt # 平面二自由度机械臂参数点质量模型 L1 L2 1.0 # 连杆长度单位 m M1 M2 1.0 # 集中质量单位 kg G 9.81 def gravity(q): 重力矩项与速度无关 q1, q2 q return np.array([ (M1 M2) * G * L1 * np.cos(q1) M2 * G * L2 * np.cos(q1 q2), M2 * G * L2 * np.cos(q1 q2) ]) def dynamics(q, dq, tau): 正向动力学ddq M^{-1}(tau - C*dq - G) q1, q2 q dq1, dq2 dq M11 (M1 M2) * L1**2 M2 * L2**2 2 * M2 * L1 * L2 * np.cos(q2) M12 M2 * L2**2 M2 * L1 * L2 * np.cos(q2) M22 M2 * L2**2 M np.array([[M11, M12], [M12, M22]]) C np.array([ [-M2 * L1 * L2 * np.sin(q2) * dq2, -M2 * L1 * L2 * np.sin(q2) * (dq1 dq2)], [ M2 * L1 * L2 * np.sin(q2) * dq1, 0.0] ]) G_vec gravity(q) return np.linalg.solve(M, tau - C dq - G_vec) def pd_control(q, dq, q_d, Kp, Kd): 关节空间PD控制带重力补偿 e q_d - q tau Kp * e - Kd * dq gravity(q) return tau # 控制目标两个关节分别转到 0.6 rad 和 0.9 rad q_d np.array([0.6, 0.9]) Kp np.array([80.0, 60.0]) Kd np.array([15.0, 10.0]) # 仿真循环欧拉积分 dt 0.001 T 3.0 steps int(T / dt) q np.array([0.0, 0.0]) dq np.array([0.0, 0.0]) q_log [] for _ in range(steps): tau pd_control(q, dq, q_d, Kp, Kd) ddq dynamics(q, dq, tau) dq dq ddq * dt q q dq * dt q_log.append(q.copy()) q_log np.array(q_log) t np.arange(0, T, dt) plt.plot(t, q_log[:, 0], labeljoint 1) plt.plot(t, q_log[:, 1], labeljoint 2) plt.plot(t, np.ones_like(t) * q_d[0], --, colorgray) plt.plot(t, np.ones_like(t) * q_d[1], :, colorgray) plt.legend() plt.xlabel(t/s) plt.ylabel(angle/rad) plt.show()代码里的dynamics通过np.linalg.solve解线性方程组得到加速度这一步替代了手推的逆向动力学。pd_control把 (K_p) 和 (K_d) 作为向量逐元素作用在两个关节上并对每个关节单独补偿重力矩。仿真采用显式欧拉步长1ms足够稳定如果把步长拉到10ms以上惯性矩阵和科氏力项的数值刚度会导致响应出现不真实的发散。下表是这组仿真中使用的主要参数。实际实验时这些参数应当从机械设计文档或辨识结果中取得而不是靠猜。参数含义仿真值(L_1, L_2)连杆长度1.0 m(M_1, M_2)集中质量1.0 kg(G)重力加速度9.81 m/s²(K_p)关节1/2比例增益80, 60 N·m/rad(K_d)关节1/2微分增益15, 10 N·m·s/rad(dt)仿真/控制周期1 ms跑完这段代码关节1会在约1秒内收敛到0.6 rad关节2略快一点因为它的惯量更小。如果你看到明显超调说明 (K_p) 相对 (K_d) 偏大这正是下一章要处理的问题。3. 位置PD控制参数整定二自由度机器人的增益怎么设3.1 把PD控制看成二阶系统二自由度机器人的两个关节并非完全解耦但在某一瞬时可以把关节i的惯性视为常数 (J_i)于是PD控制近似为二阶系统[ J_i \ddot{\theta}i K{p_i} (\theta_{d_i} - \theta_i) - K_{d_i} \dot{\theta}_i ]对应的自然频率和阻尼比[ \omega_{n_i} \sqrt{\frac{K_{p_i}}{J_i}}, \quad \zeta_i \frac{K_{d_i}}{2\sqrt{K_{p_i} J_i}} ]这个近似只在科氏力不大、目标点附近的角度变化小时成立但足够指导调参方向。增大 (K_p) 会提高自然频率让响应变快增大 (K_d) 会增加阻尼压住超调。如果 (K_d) 太大系统会像泡在粘稠液体里响应迟钝而且高频噪声会被放大到电流指令上。下表整理了增益失衡的典型表现。这组观察在仿真和实验台上几乎一致可以作为调试时的快速对照表。增益组合典型现象原因(K_p) 过小收敛慢、稳态误差大比例力矩不足(K_p) 过大超调、振荡甚至发散自然频率过高阻尼相对不足(K_d) 过小持续振荡振幅逐渐衰减阻尼比过低(K_d) 过大响应迟钝、电机发热、噪声大微分项放大量测噪声两关节 (K_p) 不匹配末端轨迹画弧两关节不同步自然频率差异过大3.2 用阶跃扫描脚本比较增益与响应在上一节的仿真代码基础上封装一个run_sim函数对同一起始位置和同一目标位置扫描不同增益直接看关节1的阶跃响应。def run_sim(q_d, Kp, Kd, q0, dt0.001, T3.0): 输入目标角度和增益返回完整角度轨迹 q, dq q0.copy(), np.zeros_like(q0) history [] for _ in range(int(T / dt)): tau pd_control(q, dq, q_d, Kp, Kd) ddq dynamics(q, dq, tau) dq dq ddq * dt q q dq * dt history.append(q.copy()) return np.array(history) q0 np.zeros(2) q_d np.array([1.0, 1.0]) # 固定关节2扫描关节1的比例/微分增益 for kp1, kd1 in [(80, 15), (200, 15), (200, 40)]: q_log run_sim(q_d, np.array([kp1, 60.0]), np.array([kd1, 10.0]), q0) plt.plot(q_log[:, 0], labelfKp{kp1}, Kd{kd1}) plt.axhline(q_d[0], colorgray, linestyle--) plt.legend() plt.xlabel(t/s) plt.ylabel(joint1 angle/rad) plt.show()这段代码把之前写在主程序里的仿真循环收进了函数每次调用都用相同的起始条件和目标角度。对比三条曲线可以看到(80, 15)上升慢但没有超调(200, 15)上升快但出现明显超调大约在1.2秒附近回到稳态(200, 40)在保持上升速度的同时把超调压了下来但收敛末期略微变慢。要在数值上量化“超调量”可以直接计算max(q[:,0]) - q_d[0]。实验报告里通常还要求记录2% 调节时间也就是误差进入稳态值正负2%范围后不再出去的起始时刻。这两个指标配合起来才能说“这一组增益比另一组好”。3.3 二自由度机器人位置PD控制的整定流程实际调参不需要解析求解最优增益按下面的顺序做往往比乱试快得多先把两个关节的 (K_p) 和 (K_d) 都设成较小的值比如仿真里用的80/60实验台上从一个更保守的起点开始。只给关节1下发阶跃目标把关节2的目标设成当前位置避免关节间的耦合干扰判断。以100%到20%的步长增大关节1的 (K_p)直到响应开始出现第一次超调记下这个值。保持 (K_p) 不变从0开始增大 (K_d)每增加一次就重新跑阶跃直到超调消失且收敛时间没有明显变长。换到关节2重复步骤2到4最后再让两个关节同时运动观察轨迹是否有明显画弧。注意仿真里 (K_p) 和 (K_d) 可以设得很大因为电机模型不存在饱和与死区。实验台上超过驱动器电流上限后控制力矩会被截断等效增益下降响应会出现“爬坡”现象。这个差别是后面实验路上最容易踩的坑。提示如果实验台有减速器(K_p) 和 (K_d) 的物理单位要换算到电机侧。常见错误是在电机驱动器里直接设关节侧增益导致实际反馈被减速比放大系统刚开机就啸叫。4. 从仿真到实验二自由度机器人位置PD控制的落地点与坑4.1 实验台架与控制调度二自由度机器人实验台的典型构成是两个直流伺服电机配增量式编码器、减速器驱动器工作在电流模式或速度模式控制器通过数据采集卡或嵌入式板卡读取编码器并输出指令。控制循环的调度方式直接决定PD控制质量。控制循环要固定周期运行常见做法是在实时线程或中断里执行下面的序列# 主控制循环固定周期 2ms while not stop: t0 time.perf_counter() q read_encoder() # 读取两个关节角度单位 rad dq filtered_velocity(q) # 用滤波后的速度避免差分噪声 tau pd_control(q, dq, q_d, Kp, Kd) write_motor(tau) # 写入驱动器电流/电压指令 wait_until(t0 0.002) # 保证循环周期不漂移read_encoder返回的是经过编码器计数转换后的弧度值filtered_velocity不是简单差分而是先差分再低通下面的小节专门讲这个问题。write_motor在电流模式下输出力矩值在电压模式下需要再换算成PWM占空比或电压指令。控制周期越短PD控制可用的 (K_p) 和 (K_d) 范围越大。周期从10ms缩短到1ms通常能把可用比例增益提高三到五倍因为数字控制的等效相位滞后随周期缩短而减小。很多台架跑不出高速响应问题不在电机而在控制周期太大。4.2 差分求速度与低通滤波增量式编码器得到的是离散角度序列用前向差分 ((q[k] - q[k-1]) / T_s) 算速度会把量化噪声放大。以1024线编码器为例4倍频后每圈4096个脉冲转角分辨率约0.0015 rad在1ms周期下差分速度分辨率只有1.5 rad/s直接进入微分项就会造成高频抖动。解决办法是先用一阶低通滤波处理差分速度def filtered_velocity(q_raw, dq_filtered, alpha0.3): 对差分速度做低通alpha 越大响应越快噪声也越大 T_s 0.002 dq_diff (q_raw - q_raw_prev) / T_s dq_filtered alpha * dq_diff (1.0 - alpha) * dq_filtered return dq_filtered这个函数里alpha是滤波系数。0.3是一个比较常见的折中滤波后噪声明显下降但速度估计会滞后约一个控制周期。滞后会对PD控制的阻尼项造成相位损失等效于让 (K_d) 变小所以实验时经常需要在“噪声大”和“阻尼失效”之间找平衡。下表给出一组经验值alpha噪声衰减滞后程度适用场景0.1很强明显高减速比、低速度场景0.3中等一至两个控制周期通用实验台0.5较弱约一个周期低噪声编码器、高响应要求如果实验台使用绝对值编码器角度精度更高可以适当增大alpha。如果使用无刷电机加霍尔传感器速度反馈本身就有较大纹波建议把alpha设在0.2以下。4.3 二自由度机器人位置PD控制的五个典型问题把仿真代码原封不动搬到实验台时常见的问题通常不在PD算法本身而在外围环节。下面这张排查表把现象、原因和排查方向放在一起。现象可能原因排查方向总有固定角度偏差重力补偿不准或未补偿分别正向/反向运动记录稳态误差差低频振荡频率约几赫兹(K_d) 不足或控制周期过大增大 (K_d) 或缩短控制周期高频抖动电机发烫微分噪声、驱动器电流饱和降低alpha减小 (K_d)末端轨迹画弧两个关节固有频率不匹配让两关节的阻尼比接近而非增益接近换负载后突然振荡惯量变化导致阻尼比下降按实际负荷重新整定 (K_d)最后一个常见问题是机械谐振。二自由度机械臂的连杆存在柔性模态PD控制带宽接近谐振频率时会激发结构振荡。判断方法是把 (K_p) 从较小值逐渐增大当听到尖锐啸叫或高频振动时说明已经接近闭环谐振边界此时不是继续调 (K_d)而是降低 (K_p) 或加入陷波滤波器。5. 位置PD控制进阶重力前馈、轨迹跟踪与性能验证5.1 从点到点阶跃到连续轨迹的前馈位置PD控制在点到点阶跃时表现不错但让末端走连续轨迹比如画圆或正弦跟踪纯PD会始终带着相位延迟。常见做法是在PD控制律中加入期望加速度前馈构成计算力矩控制[ \tau M(q)(\ddot{q}_d K_p e K_d \dot{e}) C(q,\dot{q})\dot{q} G(q) ]这里的 (e q_d - q)(\dot{e} \dot{q}_d - \dot{q})。与纯PD相比(M(q)\ddot{q}_d) 提前补偿了关节加速所需的力矩让跟随误差大幅减小。实现时要把动力学里的M、C、G都计算出来代码骨架如下def computed_torque_control(q, dq, q_d, dq_d, ddq_d, Kp, Kd): 计算力矩控制前馈加反馈适合轨迹跟踪 M inertia_matrix(q) # 从动力学模型获得 C coriolis_matrix(q, dq) # 科氏力/离心力 G_vec gravity(q) # 重力项 e q_d - q de dq_d - dq tau M (ddq_d Kp * e Kd * de) C dq G_vec return tau实验台上如果无法准确辨识全套动力学参数可以只保留重力补偿和期望速度前馈 (K_{ff} \dot{q}_d)这比纯PD更稳也比完整计算力矩控制更容易实现。对于二自由度机器人重力项在低速运动中占比最大优先补偿重力往往能把跟踪误差降低一半以上。5.2 用RMSE和最大误差验证控制性能连续轨迹跟踪不能只看阶跃响应要记录整个轨迹里的跟踪误差。常用指标是均方根误差和最大绝对误差[ RMSE \sqrt{\frac{1}{N} \sum_{k1}^{N} (q_{d_k} - q_k)^2} ]在Python里可以这样算def tracking_error(q_log, q_d_log): 返回每个关节的 RMSE 和最大绝对误差 q_log np.asarray(q_log) q_d_log np.asarray(q_d_log) e q_d_log - q_log rmse np.sqrt(np.mean(e**2, axis0)) max_err np.max(np.abs(e), axis0) return rmse, max_err用正弦轨迹测试时建议让两个关节分别走不同频率比如关节1是0.5Hz关节2是0.8Hz这样能观察耦合影响。把PD增益从阶跃整定值开始先加前馈看误差是否对称下降再去调(K_d)消除动态滞后。如果RMSE已经小于编码器分辨率的2倍再调增益的收益就不大了。5.3 PD控制可以作为机器人外部控制模板的底层在工业机器人外部控制模板里常见架构是上层负责轨迹规划和速度前瞻底层用PD或PID做关节电流环和速度环。二自由度实验台上练出来的调试思路可以直接迁移过去先定控制周期再整定增益最后加前馈。唯一的差别是工业机器人通常有更完善的摩擦补偿和力矩限幅但你在实验台上遇到的那个“末段爬行”问题在工业现场一样存在。所以当你发现两个关节都精确停在目标角度但末端画了一段奇怪的弧线不要急着怀疑运动学逆解先看看两个关节的阻尼比是否接近。位置PD控制调到最后考验的不是公式而是你对每个环节延迟和噪声的敏感度。本文还有配套的精品资源点击获取 二自由度机器人位置PD控制：建模仿真、增益整定与实验排错指南
简介这份资源面向机器人控制课程与自动化专业学生聚焦二自由度机器人的位置PD控制实验内容涵盖机器人几何模型与动力学模型搭建、控制器设计及参数整定方法。文档基于Matlab、Simulink和Robot工具箱展开给出了二连杆参数配置、运动学与动力学建模关键代码并对比了简单PD控制、PD加重力补偿等控制方案的优劣能帮助读者理解机器人控制实验从建模到仿真的完整流程与实现细节。资源包为单个PDF文件大小约245KB便于阅读、打印和反复查阅。该资源已有1221人浏览学习适合正在完成相关课程设计、毕业设计或入门机器人控制的读者参考借鉴也可作为教师备课与实验指导的辅助材料。1. 机器人控制实验的起点为什么先做二自由度位置PD控制第一次走进二自由度机器人实验台之前很多人默认“位置控制”就是把目标角度填进去电机自然会转到该停的位置。真正跑起来才发现不是抖成筛子就是慢到像没吃饭。二自由度机器人的位置PD控制是机器人控制里最容易被低估的一块控制律只有两项却能暴露力矩饱和、传动间隙、微分噪声和重力补偿这四类工程问题。这篇文章以一个小型平面两连杆机械臂为对象从动力学建模、仿真回路、增益整定到实验台排错走一遍适合正在做课程实验、入门机器人控制的工程师也适合把PD当“落后”算法而没细调过底层的人。2. 二自由度机器人位置PD控制的建模与仿真环境2.1 关节空间PD控制与末端位置控制的关系二自由度机器人的“位置控制”可以分两个层面一是让每个关节转到一个给定角度二是让末端到达笛卡尔空间某个坐标点。实际实验里最可靠的落地方式是先做关节空间PD控制。原因很简单末端笛卡尔误差到关节力矩的转换依赖雅可比矩阵遇到奇异位形会出现控制发散而关节空间的误差和控制量都是标量物理意义清楚调试时能直接看到每个关节的行为。把两个关节都在关节空间锁准末端的位姿自然就准了。反过来如果盲目在笛卡尔空间做PD两个关节的增益会互相耦合可能末端看着在逼近目标但关节内部在较劲。所以我的建议是第一阶段只做关节空间的位置PD控制目标角度由逆运动学算出来或手动给定先让关节“听话”。2.2 动力学模型与控制律要仿真二自由度机器人的位置PD控制至少要有一个动力学模型。这里选最常见的平面两连杆点质量模型第一段质量集中在连杆1末端第二段质量集中在连杆2末端忽略摩擦和电机动力学。方程形式为[ M(q)\ddot{q} C(q,\dot{q})\dot{q} G(q) \tau ]其中 (M(q)) 是惯性矩阵(C(q,\dot{q})) 是科氏力和离心力项(G(q)) 是重力项。PD控制律加一个重力前馈[ \tau K_p (q_d - q) - K_d \dot{q} G(q) ]这里 (q_d) 是目标角度(K_p) 是比例增益(K_d) 是微分增益。注意这里的微分项用的是关节速度 (\dot{q})而不是位置误差的导数两者在目标不变时等价但用关节速度可以在轨迹跟踪时避免“目标移动引起的虚假误差导数”。之所以要加 (G(q))是因为纯PD控制靠误差产生力矩想要克服重力就必须保留一个静态误差。把这个误差减小到零需要无限大的 (K_p)这在实验台上不可行。加上重力补偿后稳态误差理论上为零。2.3 用Python搭一个最小的二自由度机器人控制回路下面这段代码把动力学、PD控制、欧拉积分三部分合在一起可以直接跑出两个关节的阶跃响应。仿真步长取1ms时长3秒。import numpy as np import matplotlib.pyplot as plt # 平面二自由度机械臂参数点质量模型 L1 L2 1.0 # 连杆长度单位 m M1 M2 1.0 # 集中质量单位 kg G 9.81 def gravity(q): 重力矩项与速度无关 q1, q2 q return np.array([ (M1 M2) * G * L1 * np.cos(q1) M2 * G * L2 * np.cos(q1 q2), M2 * G * L2 * np.cos(q1 q2) ]) def dynamics(q, dq, tau): 正向动力学ddq M^{-1}(tau - C*dq - G) q1, q2 q dq1, dq2 dq M11 (M1 M2) * L1**2 M2 * L2**2 2 * M2 * L1 * L2 * np.cos(q2) M12 M2 * L2**2 M2 * L1 * L2 * np.cos(q2) M22 M2 * L2**2 M np.array([[M11, M12], [M12, M22]]) C np.array([ [-M2 * L1 * L2 * np.sin(q2) * dq2, -M2 * L1 * L2 * np.sin(q2) * (dq1 dq2)], [ M2 * L1 * L2 * np.sin(q2) * dq1, 0.0] ]) G_vec gravity(q) return np.linalg.solve(M, tau - C dq - G_vec) def pd_control(q, dq, q_d, Kp, Kd): 关节空间PD控制带重力补偿 e q_d - q tau Kp * e - Kd * dq gravity(q) return tau # 控制目标两个关节分别转到 0.6 rad 和 0.9 rad q_d np.array([0.6, 0.9]) Kp np.array([80.0, 60.0]) Kd np.array([15.0, 10.0]) # 仿真循环欧拉积分 dt 0.001 T 3.0 steps int(T / dt) q np.array([0.0, 0.0]) dq np.array([0.0, 0.0]) q_log [] for _ in range(steps): tau pd_control(q, dq, q_d, Kp, Kd) ddq dynamics(q, dq, tau) dq dq ddq * dt q q dq * dt q_log.append(q.copy()) q_log np.array(q_log) t np.arange(0, T, dt) plt.plot(t, q_log[:, 0], labeljoint 1) plt.plot(t, q_log[:, 1], labeljoint 2) plt.plot(t, np.ones_like(t) * q_d[0], --, colorgray) plt.plot(t, np.ones_like(t) * q_d[1], :, colorgray) plt.legend() plt.xlabel(t/s) plt.ylabel(angle/rad) plt.show()代码里的dynamics通过np.linalg.solve解线性方程组得到加速度这一步替代了手推的逆向动力学。pd_control把 (K_p) 和 (K_d) 作为向量逐元素作用在两个关节上并对每个关节单独补偿重力矩。仿真采用显式欧拉步长1ms足够稳定如果把步长拉到10ms以上惯性矩阵和科氏力项的数值刚度会导致响应出现不真实的发散。下表是这组仿真中使用的主要参数。实际实验时这些参数应当从机械设计文档或辨识结果中取得而不是靠猜。参数含义仿真值(L_1, L_2)连杆长度1.0 m(M_1, M_2)集中质量1.0 kg(G)重力加速度9.81 m/s²(K_p)关节1/2比例增益80, 60 N·m/rad(K_d)关节1/2微分增益15, 10 N·m·s/rad(dt)仿真/控制周期1 ms跑完这段代码关节1会在约1秒内收敛到0.6 rad关节2略快一点因为它的惯量更小。如果你看到明显超调说明 (K_p) 相对 (K_d) 偏大这正是下一章要处理的问题。3. 位置PD控制参数整定二自由度机器人的增益怎么设3.1 把PD控制看成二阶系统二自由度机器人的两个关节并非完全解耦但在某一瞬时可以把关节i的惯性视为常数 (J_i)于是PD控制近似为二阶系统[ J_i \ddot{\theta}i K{p_i} (\theta_{d_i} - \theta_i) - K_{d_i} \dot{\theta}_i ]对应的自然频率和阻尼比[ \omega_{n_i} \sqrt{\frac{K_{p_i}}{J_i}}, \quad \zeta_i \frac{K_{d_i}}{2\sqrt{K_{p_i} J_i}} ]这个近似只在科氏力不大、目标点附近的角度变化小时成立但足够指导调参方向。增大 (K_p) 会提高自然频率让响应变快增大 (K_d) 会增加阻尼压住超调。如果 (K_d) 太大系统会像泡在粘稠液体里响应迟钝而且高频噪声会被放大到电流指令上。下表整理了增益失衡的典型表现。这组观察在仿真和实验台上几乎一致可以作为调试时的快速对照表。增益组合典型现象原因(K_p) 过小收敛慢、稳态误差大比例力矩不足(K_p) 过大超调、振荡甚至发散自然频率过高阻尼相对不足(K_d) 过小持续振荡振幅逐渐衰减阻尼比过低(K_d) 过大响应迟钝、电机发热、噪声大微分项放大量测噪声两关节 (K_p) 不匹配末端轨迹画弧两关节不同步自然频率差异过大3.2 用阶跃扫描脚本比较增益与响应在上一节的仿真代码基础上封装一个run_sim函数对同一起始位置和同一目标位置扫描不同增益直接看关节1的阶跃响应。def run_sim(q_d, Kp, Kd, q0, dt0.001, T3.0): 输入目标角度和增益返回完整角度轨迹 q, dq q0.copy(), np.zeros_like(q0) history [] for _ in range(int(T / dt)): tau pd_control(q, dq, q_d, Kp, Kd) ddq dynamics(q, dq, tau) dq dq ddq * dt q q dq * dt history.append(q.copy()) return np.array(history) q0 np.zeros(2) q_d np.array([1.0, 1.0]) # 固定关节2扫描关节1的比例/微分增益 for kp1, kd1 in [(80, 15), (200, 15), (200, 40)]: q_log run_sim(q_d, np.array([kp1, 60.0]), np.array([kd1, 10.0]), q0) plt.plot(q_log[:, 0], labelfKp{kp1}, Kd{kd1}) plt.axhline(q_d[0], colorgray, linestyle--) plt.legend() plt.xlabel(t/s) plt.ylabel(joint1 angle/rad) plt.show()这段代码把之前写在主程序里的仿真循环收进了函数每次调用都用相同的起始条件和目标角度。对比三条曲线可以看到(80, 15)上升慢但没有超调(200, 15)上升快但出现明显超调大约在1.2秒附近回到稳态(200, 40)在保持上升速度的同时把超调压了下来但收敛末期略微变慢。要在数值上量化“超调量”可以直接计算max(q[:,0]) - q_d[0]。实验报告里通常还要求记录2% 调节时间也就是误差进入稳态值正负2%范围后不再出去的起始时刻。这两个指标配合起来才能说“这一组增益比另一组好”。3.3 二自由度机器人位置PD控制的整定流程实际调参不需要解析求解最优增益按下面的顺序做往往比乱试快得多先把两个关节的 (K_p) 和 (K_d) 都设成较小的值比如仿真里用的80/60实验台上从一个更保守的起点开始。只给关节1下发阶跃目标把关节2的目标设成当前位置避免关节间的耦合干扰判断。以100%到20%的步长增大关节1的 (K_p)直到响应开始出现第一次超调记下这个值。保持 (K_p) 不变从0开始增大 (K_d)每增加一次就重新跑阶跃直到超调消失且收敛时间没有明显变长。换到关节2重复步骤2到4最后再让两个关节同时运动观察轨迹是否有明显画弧。注意仿真里 (K_p) 和 (K_d) 可以设得很大因为电机模型不存在饱和与死区。实验台上超过驱动器电流上限后控制力矩会被截断等效增益下降响应会出现“爬坡”现象。这个差别是后面实验路上最容易踩的坑。提示如果实验台有减速器(K_p) 和 (K_d) 的物理单位要换算到电机侧。常见错误是在电机驱动器里直接设关节侧增益导致实际反馈被减速比放大系统刚开机就啸叫。4. 从仿真到实验二自由度机器人位置PD控制的落地点与坑4.1 实验台架与控制调度二自由度机器人实验台的典型构成是两个直流伺服电机配增量式编码器、减速器驱动器工作在电流模式或速度模式控制器通过数据采集卡或嵌入式板卡读取编码器并输出指令。控制循环的调度方式直接决定PD控制质量。控制循环要固定周期运行常见做法是在实时线程或中断里执行下面的序列# 主控制循环固定周期 2ms while not stop: t0 time.perf_counter() q read_encoder() # 读取两个关节角度单位 rad dq filtered_velocity(q) # 用滤波后的速度避免差分噪声 tau pd_control(q, dq, q_d, Kp, Kd) write_motor(tau) # 写入驱动器电流/电压指令 wait_until(t0 0.002) # 保证循环周期不漂移read_encoder返回的是经过编码器计数转换后的弧度值filtered_velocity不是简单差分而是先差分再低通下面的小节专门讲这个问题。write_motor在电流模式下输出力矩值在电压模式下需要再换算成PWM占空比或电压指令。控制周期越短PD控制可用的 (K_p) 和 (K_d) 范围越大。周期从10ms缩短到1ms通常能把可用比例增益提高三到五倍因为数字控制的等效相位滞后随周期缩短而减小。很多台架跑不出高速响应问题不在电机而在控制周期太大。4.2 差分求速度与低通滤波增量式编码器得到的是离散角度序列用前向差分 ((q[k] - q[k-1]) / T_s) 算速度会把量化噪声放大。以1024线编码器为例4倍频后每圈4096个脉冲转角分辨率约0.0015 rad在1ms周期下差分速度分辨率只有1.5 rad/s直接进入微分项就会造成高频抖动。解决办法是先用一阶低通滤波处理差分速度def filtered_velocity(q_raw, dq_filtered, alpha0.3): 对差分速度做低通alpha 越大响应越快噪声也越大 T_s 0.002 dq_diff (q_raw - q_raw_prev) / T_s dq_filtered alpha * dq_diff (1.0 - alpha) * dq_filtered return dq_filtered这个函数里alpha是滤波系数。0.3是一个比较常见的折中滤波后噪声明显下降但速度估计会滞后约一个控制周期。滞后会对PD控制的阻尼项造成相位损失等效于让 (K_d) 变小所以实验时经常需要在“噪声大”和“阻尼失效”之间找平衡。下表给出一组经验值alpha噪声衰减滞后程度适用场景0.1很强明显高减速比、低速度场景0.3中等一至两个控制周期通用实验台0.5较弱约一个周期低噪声编码器、高响应要求如果实验台使用绝对值编码器角度精度更高可以适当增大alpha。如果使用无刷电机加霍尔传感器速度反馈本身就有较大纹波建议把alpha设在0.2以下。4.3 二自由度机器人位置PD控制的五个典型问题把仿真代码原封不动搬到实验台时常见的问题通常不在PD算法本身而在外围环节。下面这张排查表把现象、原因和排查方向放在一起。现象可能原因排查方向总有固定角度偏差重力补偿不准或未补偿分别正向/反向运动记录稳态误差差低频振荡频率约几赫兹(K_d) 不足或控制周期过大增大 (K_d) 或缩短控制周期高频抖动电机发烫微分噪声、驱动器电流饱和降低alpha减小 (K_d)末端轨迹画弧两个关节固有频率不匹配让两关节的阻尼比接近而非增益接近换负载后突然振荡惯量变化导致阻尼比下降按实际负荷重新整定 (K_d)最后一个常见问题是机械谐振。二自由度机械臂的连杆存在柔性模态PD控制带宽接近谐振频率时会激发结构振荡。判断方法是把 (K_p) 从较小值逐渐增大当听到尖锐啸叫或高频振动时说明已经接近闭环谐振边界此时不是继续调 (K_d)而是降低 (K_p) 或加入陷波滤波器。5. 位置PD控制进阶重力前馈、轨迹跟踪与性能验证5.1 从点到点阶跃到连续轨迹的前馈位置PD控制在点到点阶跃时表现不错但让末端走连续轨迹比如画圆或正弦跟踪纯PD会始终带着相位延迟。常见做法是在PD控制律中加入期望加速度前馈构成计算力矩控制[ \tau M(q)(\ddot{q}_d K_p e K_d \dot{e}) C(q,\dot{q})\dot{q} G(q) ]这里的 (e q_d - q)(\dot{e} \dot{q}_d - \dot{q})。与纯PD相比(M(q)\ddot{q}_d) 提前补偿了关节加速所需的力矩让跟随误差大幅减小。实现时要把动力学里的M、C、G都计算出来代码骨架如下def computed_torque_control(q, dq, q_d, dq_d, ddq_d, Kp, Kd): 计算力矩控制前馈加反馈适合轨迹跟踪 M inertia_matrix(q) # 从动力学模型获得 C coriolis_matrix(q, dq) # 科氏力/离心力 G_vec gravity(q) # 重力项 e q_d - q de dq_d - dq tau M (ddq_d Kp * e Kd * de) C dq G_vec return tau实验台上如果无法准确辨识全套动力学参数可以只保留重力补偿和期望速度前馈 (K_{ff} \dot{q}_d)这比纯PD更稳也比完整计算力矩控制更容易实现。对于二自由度机器人重力项在低速运动中占比最大优先补偿重力往往能把跟踪误差降低一半以上。5.2 用RMSE和最大误差验证控制性能连续轨迹跟踪不能只看阶跃响应要记录整个轨迹里的跟踪误差。常用指标是均方根误差和最大绝对误差[ RMSE \sqrt{\frac{1}{N} \sum_{k1}^{N} (q_{d_k} - q_k)^2} ]在Python里可以这样算def tracking_error(q_log, q_d_log): 返回每个关节的 RMSE 和最大绝对误差 q_log np.asarray(q_log) q_d_log np.asarray(q_d_log) e q_d_log - q_log rmse np.sqrt(np.mean(e**2, axis0)) max_err np.max(np.abs(e), axis0) return rmse, max_err用正弦轨迹测试时建议让两个关节分别走不同频率比如关节1是0.5Hz关节2是0.8Hz这样能观察耦合影响。把PD增益从阶跃整定值开始先加前馈看误差是否对称下降再去调(K_d)消除动态滞后。如果RMSE已经小于编码器分辨率的2倍再调增益的收益就不大了。5.3 PD控制可以作为机器人外部控制模板的底层在工业机器人外部控制模板里常见架构是上层负责轨迹规划和速度前瞻底层用PD或PID做关节电流环和速度环。二自由度实验台上练出来的调试思路可以直接迁移过去先定控制周期再整定增益最后加前馈。唯一的差别是工业机器人通常有更完善的摩擦补偿和力矩限幅但你在实验台上遇到的那个“末段爬行”问题在工业现场一样存在。所以当你发现两个关节都精确停在目标角度但末端画了一段奇怪的弧线不要急着怀疑运动学逆解先看看两个关节的阻尼比是否接近。位置PD控制调到最后考验的不是公式而是你对每个环节延迟和噪声的敏感度。本文还有配套的精品资源点击获取