控制：让真实机器人稳定、准确又柔顺地行动

PID：从误差出发的基本语言

单关节位置目标为 \(q_d(t)\)，实际位置为 \(q(t)\)，误差 \(e=q_d-q\)。PID 控制为

\[u(t)=K_p e(t)+K_i\int_0^t e(\tau)d\tau+K_d\dot e(t).\]

比例项 \(K_p e\) 像弹簧，误差越大作用越强；积分项累积长期偏差，可消除重力或摩擦造成的静差；微分项根据误差变化提供阻尼，减少超调。实际微分常对测量做带通或低通处理，避免高频噪声被放大。

增益不能脱离采样频率、负载与机构刚度讨论。\(K_p\) 太低会软、慢，太高会振荡并放大柔性；\(K_i\) 太高会在执行器饱和时持续积累，造成解除饱和后的大幅过冲，需做 anti-windup；\(K_d\) 太高对噪声敏感。调参前先确认反馈符号、单位、采样时间和执行器限幅。

前馈与计算力矩

已知期望轨迹和动力学时，可先补偿大部分所需力矩：

\[\boldsymbol\tau= \mathbf M(\mathbf q)\mathbf v+ \mathbf C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}+ \mathbf g(\mathbf q),\]

\[\mathbf v=\ddot{\mathbf q}_d+ \mathbf K_d(\dot{\mathbf q}_d-\dot{\mathbf q})+ \mathbf K_p(\mathbf q_d-\mathbf q).\]

若模型准确，非线性动力学被近似抵消，闭环误差表现得像设计好的二阶系统。模型有误时，反馈修正残差。计算力矩控制对力矩接口、状态估计和模型质量要求较高；很多工业机器人只开放位置或速度接口，上层只能生成平滑参考，让厂商底层伺服完成跟踪。

图 1　P 看现在，I 记过去，D 预警变化。

阻抗、导纳与力控制

机器人自由运动时关注位置，接触环境时还要控制交互关系。阻抗控制指定末端位姿误差与交互力之间的期望动力学：

\[\mathbf M_d\ddot{\mathbf e}+ \mathbf D_d\dot{\mathbf e}+ \mathbf K_d\mathbf e=\mathbf F_{ext}.\]

\(\mathbf e=\mathbf x-\mathbf x_d\) 是实际与期望末端位姿误差；\(\mathbf M_d,\mathbf D_d,\mathbf K_d\) 分别是期望惯性、阻尼和刚度。直觉上，机器人表现成一组可设计的质量—弹簧—阻尼器。刚度高，位置保持更硬；刚度低，受力后更愿意让步。

导纳控制反过来读取外力，计算应该产生的位置或速度变化，适合底层是高性能位置控制的刚性机器人。直接力控制则让测得力 \(\mathbf F\) 跟踪目标 \(\mathbf F_d\)。擦拭桌面可沿切平面控制运动、沿法向控制力，形成混合位置—力控制。方向选择依赖接触几何，若法向估错，控制器可能一边沿错误方向加力，一边滑离目标。

柔顺不自动等于安全。传感器饱和、接触时延、刚度切换和工具尖锐边缘仍能造成危险；安全还需要速度、力、功率、工作空间和停止链的独立限制。

图 2　接触任务需要选择机器人对误差和外力的关系。

MPC：把未来一小段时间纳入当前动作

模型预测控制在每个控制周期求一个有限时域优化：

\[\min_{\mathbf u_{0:N-1}} \sum_{k=0}^{N-1} \left( \|\mathbf x_k-\mathbf x_k^{ref}\|_{\mathbf Q}^2+ \|\mathbf u_k\|_{\mathbf R}^2 \right) +\|\mathbf x_N-\mathbf x_N^{ref}\|_{\mathbf Q_f}^2\]

\[\text{s.t. }\mathbf x_{k+1}=f(\mathbf x_k,\mathbf u_k), \quad \mathbf x_k\in\mathcal X, \quad \mathbf u_k\in\mathcal U.\]

只执行求得序列的第一个控制量，下一周期用新状态重新求解，这叫滚动时域。MPC 能直接处理输入、状态和接触约束，也能预见刹车距离；代价是实时优化、模型误差和调参复杂度。若求解器超时，应有上一次可行解、简化控制器或安全停车作为降级路径。

全身控制：让多个任务共享一副身体

人形或移动操作机器人同时要保持接触、平衡、手部轨迹、视线和姿态。全身控制常把关节加速度、力矩和接触力作为变量，在动力学与接触约束下求解二次规划：

\[\min_{\ddot{\mathbf q},\boldsymbol\tau,\boldsymbol\lambda} \sum_i w_i\|\mathbf J_i\ddot{\mathbf q}+\dot{\mathbf J}_i\dot{\mathbf q}-\ddot{\mathbf x}_i^*\|^2\]

受制于全身动力学、摩擦锥、关节与力矩上下限。\(i\) 表示不同任务，\(w_i\) 是权重；严格任务优先级也可用分层优化，保证低优先任务不破坏高优先任务。权重法计算方便，但权重相近时任务会互相妥协；层级法语义清楚，求解复杂度更高。

全身控制器接收的期望若彼此矛盾，例如手必须到达远处且双脚不能动、质心又必须保持在支撑域内，优化器可能无解或把误差压到意料之外的任务上。系统要监控约束松弛和任务误差，不能只观察“求解器返回成功”。

图 3　优化控制同时考虑未来与约束，全身控制同时协调多个任务。

稳定性、时延与饱和

控制设计最终关心闭环稳定性。Lyapunov 方法寻找一个类似能量的函数 \(V(\mathbf x)>0\)，并证明沿系统轨迹 \(\dot V(\mathbf x)\le0\)，从而说明状态不会发散，并在更强条件下收敛。工程上即使没有完整证明，也应做增益裕度、采样抖动、时延、负载变化与饱和测试。

时延会吃掉相位裕度。视觉闭环若 100 ms 才更新，不能直接用关节伺服那样的高增益；网络控制还会有抖动和丢包。执行器饱和意味着控制器算出的命令无法实现，积分器、状态预测和 MPC 都应知道这个边界。紧急停止属于独立安全链，不能依赖高级控制软件仍正常运行。

工程例子：机械臂插销时越修正越卡

位置控制要求插销沿理想轴线直插，孔位实际偏 1 mm。高刚度控制把横向误差变成较大接触力，摩擦让插销楔住；继续增加推力会损伤零件。可改用分阶段策略：自由空间用位置控制接近，检测接触后切换低横向刚度的阻抗控制，沿轴向限制推力，并用小幅搜索或视觉/力觉估计孔轴。

切换本身也要连续。期望位姿、积分状态和力传感器零点若在切换瞬间不一致，会产生冲击。失败条件应包括最大力、最大搜索范围、插入深度进展和时间；超过阈值退出接触并重定位。