机械臂控制方法

一、五种控制模式与对比总览

1. 位置控制

2. 速度控制

3. 力控制

4. 导纳控制

5. 阻抗控制

重力补偿与摩擦力补偿的作用总结

• 重力补偿：消除连杆重力产生的静态力矩。在位置/速度控制中作为前馈提高精度；在力控、阻抗控制中必须精确补偿，否则重力会被控制器视为外部干扰或导致控制偏差。
• 摩擦力补偿：补偿关节摩擦力（静摩擦、库仑摩擦、粘滞摩擦）。在低速运动、力控、阻抗控制中尤为重要，可减小死区、提高力控带宽和轨迹跟踪精度。

五种算法对比总表

结论

重力补偿和摩擦力补偿是高性能机器人控制的基础，尤其是在力控和阻抗控制中直接决定了系统的稳定性和精度。位置控制虽然对补偿依赖较低，但在高精度场合仍需加入。选择控制算法时，应根据硬件能力（是否有力传感器、是否支持力矩控制）、任务需求（精度、柔顺）以及模型精度综合决定。

二、阻抗控制和导纳控制详细对比

阻抗控制和导纳控制是机器人柔顺控制的两大核心方法，它们从互为对偶的角度解决同一个问题：如何让机器人在与环境交互时表现出期望的柔顺行为。两者在控制架构、硬件要求、动态特性、适用场景上存在本质区别，理解这些差异是选择正确控制方案的关键。

1. 核心特点对比

2. 阻抗控制

2.1 原理简述

阻抗控制通过位置/速度误差产生期望力，直接输出力矩驱动电机。它使机械臂表现得像一个虚拟的弹簧-阻尼-质量系统，主动抵抗偏离轨迹的力。

2.2 优点

• 高动态响应：直接控制力矩，无中间位置环延迟，适合高速、高动态场景。
• 无需力传感器：仅需编码器反馈，降低硬件成本。
• 自然柔顺：通过调节阻抗参数 $K_d$，可直接设定末端“软硬”程度。
• 统一框架：同时实现轨迹跟踪与力交互，无需模式切换。

2.3 缺点

• 硬件要求高：需要开放力矩接口的机器人（协作机器人、直驱电机），传统工业机器人难以应用。
• 模型依赖强：需要精确的动力学模型（重力、摩擦力、惯性力补偿），否则会产生偏差。
• 稳定性敏感：在刚性环境（高 $K_e$）中，若参数设置不当易振荡。
• 参数整定复杂：$M_d, B_d, K_d$ 相互耦合，需综合考虑。

2.4 适用场景

• 协作机器人（如 Franka、UR、KUKA LBR）：原生支持力矩控制，适合人机交互。
• 人形机器人/外骨骼：需要高动态响应和自然柔顺。
• 医疗手术机器人：需精确力控但避免额外传感器。
• 高速装配：需要快速响应接触力变化。

3. 导纳控制

3.1 原理简述

导纳控制通过外力计算位置修正量，输出给底层位置控制器。它使机械臂“顺应”外力，表现为被动柔顺，同时可设定期望力实现主动力控。

3.2 优点

• 易于实现：可直接叠加在现有工业机器人位置环上，无需改造底层。
• 稳定性好：底层高增益位置环提供了天然的稳定性，适合刚性环境。
• 力控直观：可直接设定期望力 $F_d$，实现恒力控制。
• 参数物理意义清晰：$K_d$ 决定柔顺度，$B_d$ 决定阻尼，便于工程整定。

3.3 缺点

• 动态响应受限：受底层位置环带宽限制，响应速度不如阻抗控制。
• 需要力传感器：通常需要末端六维力传感器或精确的力矩估计，增加成本。
• 存在稳态误差：在恒力控制时，力误差与 $K_d$ 和 $K_e$ 有关，需积分消除。
• 位置环依赖：若底层位置环精度低或延迟大，会影响整体性能。

3.4 适用场景

• 工业机器人改造：在已有位置控制机器人上增加柔顺能力。
• 精密装配（轴孔装配、电子元件插接）：利用导纳的自动对中特性。
• 打磨/抛光：结合参考轨迹与恒力控制。
• 拖动示教：设 $F_d=0$，实现轻便拖拽。

4. 详细优缺点对比

5. 选择指南：何时用阻抗，何时用导纳？

5.1 优先选择阻抗控制的情况

• ✅ 机器人本身支持力矩/电流控制（协作机器人、直驱电机）。
• ✅ 需要高动态响应（如高速装配、动态抓取）。
• ✅ 希望降低成本，避免购买昂贵的力传感器。
• ✅ 应用场景环境刚度变化大（如与人体交互），阻抗控制可通过参数调节适应。
• ✅ 有精确的动力学模型（已做充分辨识）。

5.2 优先选择导纳控制的情况

• ✅ 机器人是传统工业机器人，仅支持位置控制（如发那科、安川、库卡等）。
• ✅ 需要高精度力跟踪（如恒力打磨、精密压装），力传感器直接测量更可靠。
• ✅ 环境刚度高且稳定（如金属对金属接触），导纳控制稳定性更好。
• ✅ 希望快速实现柔顺功能，无需改造底层控制器。
• ✅ 对响应速度要求不高（如慢速装配、拖动示教）。

6. 组合使用与混合架构

在实际复杂任务中，两者可以混合使用或分层结合：

6.1 自由空间用阻抗，接触空间用导纳

• 自由空间：阻抗控制提供高动态轨迹跟踪。
• 接触空间：切换到导纳控制，实现精确力控。

6.2 阻抗控制 + 力传感器外环

• 内环：阻抗控制保证稳定性和柔顺性。
• 外环：力传感器修正期望轨迹，提高力控精度。

6.3 导纳控制 + 高带宽位置环

• 若底层位置环带宽足够高（>200Hz），导纳控制的动态响应可接近阻抗控制。

7. 总结

选择哪种控制方式，本质上取决于机器人硬件能力（是否支持力矩控制、是否有力传感器）和任务需求（动态响应 vs. 力控精度）。在现代机器人系统中，两者并非对立，而是可以优势互补，共同构建高性能的柔顺控制系统。

三、导纳控制：期望轨迹与期望力详解

导纳控制通过参考轨迹（$x_r, \dot{x}_r, \ddot{x}_r$）和期望力（$F_d$）这两个核心输入，将机械臂的运动规划和力交互统一在一个框架内。参考轨迹定义了“想怎么运动”，期望力定义了“想施加多大的力”，二者通过导纳模型相互作用，使机械臂既能精确跟踪轨迹（无接触时），又能主动施加或顺应接触力（有接触时）。

下面从公式、各输入作用、协同机制、工程实例四个方面详细展开。

1. 导纳控制的基本方程

导纳控制的标准形式（在任务空间）为：

其中：

• $x_c, \dot{x}_c, \ddot{x}_c$：修正后的期望轨迹（最终发送给底层位置控制器的指令）
• $x_r, \dot{x}_r, \ddot{x}_r$：参考轨迹（由上层规划器提供）
• $F_{ext}$：实际检测到的外部接触力
• $F_d$：期望的接触力（用户设定）
• $M_d, B_d, K_d$：目标惯量、阻尼、刚度（可调参数）

导纳控制的核心思想是：实际力与期望力的偏差驱动一个虚拟的质量-弹簧-阻尼系统，产生位置修正量 $\Delta x = x_c - x_r$，从而改变机械臂的指令轨迹。

2. 参考轨迹 $x_r, \dot{x}_r, \ddot{x}_r$ 的作用

参考轨迹是导纳控制的“标称运动”，它告诉机械臂在没有外力（或外力恰好等于期望力）时应该怎样运动。

2.1 定义“零修正”时的运动

当 $F_{ext} = F_d$ 时，方程右边为零，导纳模型的稳态解为 $x_c = x_r$。此时：

• 导纳层不对参考轨迹做任何修改，直接将其传给底层位置控制器。
• 机械臂按照参考轨迹精确运动，表现出纯位置控制的行为。

意义：在自由空间或恒力接触达到稳态时，机械臂严格遵循规划的路径，保证了轨迹跟踪精度。

2.2 提供运动基准

参考轨迹包含了位置、速度、加速度三个层次的信息，它们共同决定了“标称”的动力学：

• $x_r$：确定机械臂在空间中应该到达的位置。
• $\dot{x}_r$：提供速度参考，影响导纳模型的动态响应（尤其是在离散实现中）。
• $\ddot{x}_r$：在需要前馈补偿时，可用于改善跟踪性能（虽然导纳模型本身会生成加速度，但部分实现会使用 $\ddot{x}_r$ 作为前馈）。

2.3 与修正量的叠加

最终指令为 $x_c = x_r + \Delta x$。参考轨迹是基础，$\Delta x$ 是在此基础上的“弹性偏移”。参考轨迹决定了机械臂的“意愿”，而修正量反映了对外力的“妥协”。

3. 期望力 $F_d$ 的作用

期望力 $F_d$ 是导纳控制的“力控目标”，它定义了机械臂与环境之间期望的相互作用力。

3.1 设定力平衡点

导纳模型响应的驱动力是 $F_{ext} - F_d$。当 $F_{ext} = F_d$ 时，驱动力为零，无位置修正。这意味着：

• 机械臂将保持当前轨迹不变，同时维持与环境的接触力恰好为 $F_d$。
• 如果 $F_d = 0$，则无外力时机械臂严格走轨迹，有外力时产生顺应性位移（被动柔顺）。
• 如果 $F_d \neq 0$，则机械臂会主动施加力去达到该期望值（主动力控）。

3.2 实现主动力跟踪

在恒定力任务（如打磨、抛光）中，设定一个非零的 $F_d$（例如法向接触力10N），导纳控制会自动调整位置修正量，使实际接触力始终趋近 $F_d$：

• 当 $F_{ext} < F_d$ 时，$F_{ext} - F_d$ 为负，导纳模型产生正向修正（机械臂向前压），增加接触力。
• 当 $F_{ext} > F_d$ 时，产生负向修正（机械臂回退），减小接触力。

最终稳态时，$F_{ext} \approx F_d$，实现恒力控制。

3.3 作为“虚拟力引导”输入

当参考轨迹静止（如 $x_r$ 为常数）且 $F_d$ 非零时，导纳控制会产生持续的位置修正，驱动机械臂主动运动，直至接触力达到期望值。这可用于：

• 遥操作：操作杆的力作为 $F_d$，机械臂跟随操作力运动。
• 力引导装配：在轴孔装配中，设定期望侧向力为零，使机械臂自动对中。

4. 参考轨迹与期望力的协同作用

4.1 统一框架下的无缝切换

导纳控制将轨迹跟踪与力控融合在一个方程中，无需模式切换：

• 自由空间：若设定 $F_d = 0$ 且无外力，则 $x_c = x_r$，机械臂精确走轨迹。
• 接触空间：有外力时，$x_c = x_r + (F_{ext} - F_d)/K_d$（稳态），轨迹被修正，同时实际力趋近 $F_d$。

这种统一性避免了力/位混合控制中坐标系分解和模式切换带来的复杂性。

4.2 参数化调节行为

通过调整 $M_d, B_d, K_d$ 和 $F_d$，可以独立设定轨迹跟踪的刚度和力控的硬度：

• 增大 $K_d$：位置修正量减小，轨迹跟踪更“硬”，但力控灵敏度降低。
• 增大 $F_d$：机械臂主动施力更大，适用于重载打磨等场景。
• 增大 $B_d$：修正过程更平滑，抑制振荡。

4.3 工程实例：恒力打磨

• 参考轨迹：规划工具沿工件表面的路径（位置、速度）。
• 期望力：$F_d = 10\,\text{N}$（法向）。
• 效果：
  • 无接触时，机械臂沿路径运动（无修正）。
  • 接触工件后，导纳控制根据 $F_{ext} - F_d$ 实时调整法向位置，使接触力稳定在10N附近，同时切向仍按参考轨迹移动。

5. 与阻抗控制的对比

关键区别：导纳控制明确将期望力 $F_d$ 作为输入，更适合需要直接设定接触力的任务（如恒力打磨）；阻抗控制则通过调整阻抗参数来间接影响力交互，无需力传感器，但力跟踪精度较低。

6. 总结

• 参考轨迹 $x_r, \dot{x}_r, \ddot{x}_r$：定义标称运动，是机械臂“想走的路”。无外力或外力恰好等于期望力时，机械臂严格按此运动，保证了轨迹跟踪精度。
• 期望力 $F_d$：定义期望的接触力，是机械臂“想施加的力”。通过导纳模型，实际力与期望力的偏差被转换为位置修正，实现主动力跟踪或被动柔顺。
• 二者结合：使导纳控制成为一个统一的框架，既能实现高精度轨迹跟踪（自由空间），又能实现恒力控制（接触空间），且二者平滑过渡，无需切换控制策略。

在实际应用中，合理设置参考轨迹和期望力，配合导纳参数整定，即可让机械臂完成从自由运动到复杂接触任务的连续操作。

四、阻抗控制：期望轨迹详解

在阻抗控制中，期望轨迹 $x_r, \dot{x}_r, \ddot{x}_r$ 是控制器的核心输入，它们共同定义了机械臂“想达到的运动状态”。阻抗控制通过比较期望轨迹与实际状态，计算出需要输出的力矩，使机械臂在主动跟踪轨迹的同时，表现出可调的柔顺特性。

下面从控制律公式出发，逐一解析三个期望量的具体作用。

1. 阻抗控制的基本方程

阻抗控制的标准形式（在任务空间）为：

其中：

• $x_r, \dot{x}_r, \ddot{x}_r$：期望轨迹（位置、速度、加速度）
• $x, \dot{x}, \ddot{x}$：实际状态（测量或估计）
• $M_d, B_d, K_d$：目标惯量、阻尼、刚度（对角矩阵）
• $F_{cmd}$：期望输出的力（经雅可比映射为关节力矩）

三个期望量分别对应控制律中的三项，各自承担不同的控制职责。

2. 期望位置 $x_r$ 的作用

作用：与当前实际位置 $x$ 比较，生成位置误差项 $K_d (x_r - x)$。

• 物理意义：相当于一个虚拟弹簧。当机械臂偏离期望位置时，这一项会产生一个正比于偏差的回复力，试图将机械臂“拉”回轨迹。
• 轨迹跟踪核心：在自由空间（无外力）中，这项主导了轨迹跟踪行为。偏差越大，回复力越大，保证稳态误差趋近于零（若 $K_d > 0$）。
• 刚度调节：$K_d$ 的大小决定了轨迹跟踪的“硬度”。高刚度 → 位置跟踪精度高，但对外力抵抗强烈；低刚度 → 允许偏离，柔顺性好。

直观理解：就像一根弹簧连接着期望位置和实际位置，弹簧的刚度就是 $K_d$。

3. 期望速度 $\dot{x}_r$ 的作用

作用：与实际速度 $\dot{x}$ 比较，生成速度误差项 $B_d (\dot{x}_r - \dot{x})$。

• 物理意义：相当于一个虚拟阻尼器。当机械臂运动速度与期望不一致时，产生一个与速度差成正比的力，用于抑制振荡并提高动态响应。
• 稳定性保障：如果没有阻尼项（$B_d = 0$），系统会像一个无阻尼弹簧，在轨迹附近持续振荡。合适的阻尼可以使系统快速收敛且无超调。
• 速度前馈：在期望速度变化时，这项帮助机械臂“跟上”速度变化，减少因惯量导致的滞后。

直观理解：就像给弹簧系统加了一个阻尼器，吸收振动，使运动平滑。

4. 期望加速度 $\ddot{x}_r$ 的作用

作用：作为前馈项 $M_d \ddot{x}_r$ 的一部分（与 $M_d \ddot{x}$ 结合后形成 $M_d (\ddot{x}_r - \ddot{x})$）。

• 物理意义：相当于一个虚拟质量。该项直接根据期望加速度输出力，用于克服机械臂自身的惯量，使机械臂能够快速响应轨迹的加速度变化。
• 提升跟踪性能：如果没有加速度前馈，阻抗控制完全依赖误差反馈来驱动机器人。当期望轨迹快速变化（如急停、高速拐弯）时，反馈滞后会导致较大的动态误差。加入 $M_d \ddot{x}_r$ 后，控制器可以“预判”需要的力，显著减小跟踪误差。
• 惯量匹配：$M_d$ 通常设置为接近机械臂实际惯量，或根据任务调整（如降低惯量使机械臂更“轻”）。

直观理解：就像提前知道要加速多少，直接输出对应的力，而不是等误差出现后再纠正。

5. 三者的协同作用

将阻抗控制类比为 “弹簧-阻尼-质量”系统 连接在一个移动的基座上：

• 期望轨迹 $x_r(t)$ 是这个基座的运动轨迹。
• 实际位置 $x(t)$ 是质量块（机械臂）的位置。
• 弹簧（$K_d$）和阻尼器（$B_d$）连接在基座和质量块之间。
• 质量（$M_d$）是质量块的虚拟质量。

运动机制：

• 当基座运动时（期望轨迹变化），弹簧被拉伸或压缩，产生力拉动质量块跟随基座运动。
• 阻尼器吸收振动，使运动平滑。
• 质量项决定了质量块对加速度的响应特性。

如果基座突然加速（$\ddot{x}_r$ 大），质量块会通过弹簧感受到力，但同时质量项 $M_d \ddot{x}_r$ 会直接输出一个前馈力，帮助质量块“预判”并更快地跟上基座。

6. 与导纳控制的对比

7. 工程实现中的注意事项

7.1 轨迹规划器的要求

阻抗控制需要完整的轨迹信息（位置、速度、加速度），因此上层轨迹规划器必须生成二阶平滑的轨迹。常用方法：

• S曲线规划：加速度连续，速度平滑，位置为三阶可导。
• 多项式规划：常用五次多项式保证加速度连续。
• 梯形速度规划：加速度不连续，需滤波或离散化处理。

若加速度不连续或噪声大，直接使用 $\ddot{x}_r$ 会导致力矩指令突变，引起振动。

7.2 实际加速度 $\ddot{x}$ 的处理

方程中的 $\ddot{x}$ 是实际加速度，但编码器二次差分噪声极大。工程实现中通常：

• 忽略 $\ddot{x}$ 项：简化为 $F_{cmd} = M_d \ddot{x}_r + B_d (\dot{x}_r - \dot{x}) + K_d (x_r - x)$，此时 $M_d$ 仅作为前馈增益。
• 使用观测器：通过状态观测器（如卡尔曼滤波）估计加速度，降低噪声影响。

7.3 参数整定顺序

1. 先定刚度 $K_d$：根据任务精度需求确定轨迹跟踪的“硬度”。
2. 再定阻尼 $B_d$：取临界阻尼 $B_d = 2\sqrt{M_d K_d}$ 为基准，再微调。
3. 最后定惯量 $M_d$：通常设为接近机械臂实际惯量，若希望响应更快可适当降低。

8. 总结

期望轨迹 $x_r, \dot{x}_r, \ddot{x}_r$ 在阻抗控制中的作用可以概括为：

它们是机械臂“想要成为的状态”，通过弹簧、阻尼、质量三项分别产生位置回复力、速度阻尼力和加速度前馈力，共同驱动机器人精确跟踪轨迹，同时保持可调的柔顺性。

• $x_r$：提供位置基准，产生弹簧回复力，决定稳态精度与刚度。
• $\dot{x}_r$：提供速度基准，产生阻尼力，决定动态稳定性与响应平滑性。
• $\ddot{x}_r$：提供加速度前馈，克服惯量，提升高速轨迹跟踪性能。

三者缺一不可，共同构成了阻抗控制既能高精度跟踪轨迹、又能柔顺响应外力的能力基础。

五、导纳控制实现力控

导纳控制是通过“位置控制”来实现“力控制”的，其核心机制在于利用环境的物理约束（刚度）和导纳模型的内建关系，将期望的力转化为实际的位置修正，从而间接达到期望的力值。

简单来说：导纳控制器并不直接命令机械臂“使出多大的力”，而是命令机械臂“移动到某个位置”，通过让机械臂与环境的物理接触产生反作用力，并利用力反馈闭环来调整这个位置，使接触力收敛到期望值。

下面从物理原理、数学推导、动态过程及工程实现四个维度详细拆解。

1. 物理原理：环境刚度作为“力传感器”

当机械臂与刚性环境接触时，接触力本质上是由机械臂末端位置与环境表面位置的侵入量产生的。这就像一个弹簧：你压得越深，力就越大。

• 环境模型：假设环境是线弹性的，其刚度为 $K_e$，环境表面的位置为 $x_e$。
• 接触力：当机械臂实际位置 $x$ 超过环境表面时，接触力为： $$F_{ext} = K_e \cdot (x - x_e) \quad (\text{当 } x > x_e \text{ 时})$$

关键洞察：只要知道环境刚度 $K_e$ 和期望力 $F_d$，理论上可以计算出需要压入的深度 $\Delta x = F_d / K_e$。但实际中 $K_e$ 往往是未知且变化的，因此需要闭环控制。

2. 数学推导：从期望力到位置修正

导纳控制的核心方程将期望力 $F_d$ 与实际力 $F_{ext}$ 的偏差转换为位置修正：

2.1 稳态分析

在一维稳态情况下（速度、加速度为零），导纳方程简化为：

其中：

• $x_r$：参考轨迹位置
• $x_c$：修正后的指令位置
• $K_d$：导纳刚度
• $F_{ext}$：实际接触力
• $F_d$：期望力

整理得：

解读：

• 当 $F_{ext} < F_d$ 时，括号内为负，$x_c < x_r$（看起来指令位置减小了，即回退）。但为什么回退反而能增大接触力？因为回退减少了侵入量，但实际接触力 $F_{ext}$ 是由环境刚度 $K_e$ 和实际位置 $x$ 决定的，而 $x$ 由底层位置控制器跟踪 $x_c$。这里有一个关键：修正量符号需要结合环境位置 $x_e$ 理解。

2.2 更直观的推导（考虑环境）

假设参考轨迹 $x_r$ 已经处于接触状态（即 $x_r > x_e$）。实际位置 $x$ 由底层位置控制器尽可能精确地跟踪 $x_c$，因此近似有 $x \approx x_c$。

实际接触力：

将 $x_c$ 的表达式代入：

整理得：

对于典型导纳控制，通常选择 $K_d \ll K_e$（即导纳刚度远小于环境刚度），使得 $K_e/K_d$ 很大，此时：

结论：只要导纳刚度 $K_d$ 远小于环境刚度 $K_e$，稳态实际力就会趋近期望力 $F_d$。

3. 动态过程：如何通过位置变化“找到”期望力

实际的控制循环通过迭代搜索的方式让力收敛到期望值。以恒力打磨为例：

1. 初始状态：机械臂刚接触工件，实际力 $F_{ext} = 0$，期望力 $F_d = 10N$。
2. 第一个周期：力偏差 $F_{ext} - F_d = -10N$。导纳模型计算位置修正量 $\Delta x = -10N / K_d$（假设 $K_d = 1000N/m$，则 $\Delta x = -0.01m$，即向 - 方向移动 1cm）。
   • 直觉困惑：为什么向负方向（回退）移动反而能增加接触力？
   • 关键：这里的 $\Delta x$ 是相对于参考轨迹 $x_r$ 的修正。假设参考轨迹 $x_r$ 已经规划到工件内部某个深度，修正量为负意味着指令位置比参考轨迹更靠外（即减少侵入量）。但如果实际接触力小于期望力，应该是侵入量不足，为何要减少侵入量？
   • 纠正：上述推导中修正量的符号与期望力的关系取决于导纳方程的形式。在实际常用的导纳控制实现中，方程常写为： $$M_d (\ddot{x}_c - \ddot{x}_r) + B_d (\dot{x}_c - \dot{x}_r) + K_d (x_c - x_r) = F_d - F_{ext}$$ 此时稳态为： $$x_c = x_r + \frac{F_d - F_{ext}}{K_d}$$ 当 $F_{ext} < F_d$ 时，修正量为正，指令位置向正方向（深入环境）移动，从而增加接触力。

正确的动态过程（采用 $F_d - F_{ext}$ 的形式）：

• 周期1：$F_{ext}=0, F_d=10N$，偏差 $10N$，修正量 $\Delta x = 10/K_d = +0.01m$（假设 $K_d=1000$）。指令位置比参考轨迹深入 1cm，实际接触力增大。
• 周期2：接触力增大到 $8N$，偏差 $2N$，修正量 $0.002m$，继续深入。
• 周期3：接触力增大到 $9.5N$，偏差 $0.5N$，修正量 $0.0005m$。
• 稳态：接触力逐渐逼近 $10N$，修正量趋于 0，指令位置稳定在某个深度，使 $F_{ext}=F_d$。

4. 参数 $K_d$ 的关键作用

从稳态关系 $x_c = x_r + (F_d - F_{ext})/K_d$ 可以看出：

• $K_d$ 决定了力控的“硬度”：
  • $K_d$ 很大（如 10000 N/m）时，很小的力偏差就会产生较大的位置修正，系统响应快，但容易振荡，且稳态时位置修正量小，对模型误差敏感。
  • $K_d$ 很小（如 500 N/m）时，力偏差产生的位置修正量较大，系统响应较慢，但更稳定，且对未知环境刚度有更好的适应性。

稳定性条件：为保证力控稳定，通常要求 $K_d < K_e$（导纳刚度小于环境刚度）。若 $K_d > K_e$，机械臂与环境组成的系统可能发生振荡。

5. 工程实现中的关键细节

5.1 力控方向的确定

• 在需要力控的方向上，设置非零的 $F_d$，并选择较小的 $K_d$。
• 在不需要力控的方向（如切向），设置 $F_d = 0$，并选择较大的 $K_d$，以保持轨迹跟踪精度。

5.2 饱和与限幅

为防止单周期修正过大导致冲击，工程上会对 $\Delta x$ 进行限幅：

5.3 与参考轨迹的关系

• 如果参考轨迹 $x_r$ 本身已经规划到工件内部，则最终指令 $x_c = x_r + \Delta x$ 可能超出安全范围，需与上层轨迹规划协同。
• 更好的做法：在恒力控制阶段，将参考轨迹 $x_r$ 设为恒速运动（如沿表面移动），法向力控由导纳修正自动调节。

5.4 力控与位置控的平滑过渡

• 自由空间运动时，设定 $F_d = 0$，机械臂按参考轨迹运动。
• 检测到接触后，逐步引入非零 $F_d$ 或切换导纳参数，避免冲击。

6. 直观类比：用手压弹簧秤

想象你用手（机械臂）压一个弹簧秤（环境），目标是让秤显示 10N（$F_d$），但你只能控制手的位置（位置控制）。

1. 你把手放在某个位置（参考轨迹 $x_r$），秤显示 0N。
2. 你通过一个“导纳控制器”来决策：如果秤显示的力小于 10N，你就把手向下移动一点（正修正量）；如果大于 10N，你就向上移动一点（负修正量）。
3. 每次移动的量与力偏差成正比（比例系数 $1/K_d$）。
4. 经过几次调整，你最终会找到一个位置，使秤稳定显示 10N。

这个过程中，你从未直接命令手“使出10N的力”，你只是不断地调整手的位置，利用弹簧秤的物理特性（环境刚度）来产生所需的力。导纳控制正是将这个“人”的决策过程形式化为数学算法。

7. 总结

导纳控制通过位置控制实现目标力 $F_d$ 的完整逻辑链：

1. 检测：测量实际接触力 $F_{ext}$。
2. 比较：计算力偏差 $F_d - F_{ext}$。
3. 映射：通过导纳模型（虚拟弹簧）将力偏差转换为位置修正量 $\Delta x = (F_d - F_{ext})/K_d$（稳态简化）。
4. 执行：将修正量叠加到参考轨迹上，得到新的指令位置 $x_c = x_r + \Delta x$。
5. 驱动：底层高精度位置控制器驱动机器臂到达 $x_c$。
6. 物理效应：由于环境刚度 $K_e$ 的存在，实际接触力 $F_{ext} = K_e (x - x_e)$ 随之变化。
7. 闭环：重复步骤1-6，直到 $F_{ext}$ 收敛到 $F_d$。

本质：导纳控制利用“位置-力”的物理转换关系（$F = K_e \cdot \Delta x$），通过调节位置来间接控制力，使原本只能进行位置控制的机械臂具备了精确的力控能力。这种“以位置换力”的思想，是导纳控制能够在工业机器人上广泛应用的根本原因。

六、阻抗控制实现力控

阻抗控制实现指定值的力控（即让机械臂以恒定的期望力 $F_d$ 接触环境）与传统力控制不同，它不直接以力偏差作为控制输入，而是通过规划期望轨迹和调整阻抗参数，使稳态接触力收敛到目标值。

阻抗控制的核心思想是：让机械臂表现得像一个虚拟的弹簧-阻尼-质量系统，通过设定这个虚拟系统的平衡点（期望轨迹）和刚度（$K_d$），间接控制接触力。

下面从原理机制、两种实现方法、参数作用以及工程要点四个方面详细说明。

1. 原理机制：力与位置的内在关系

当机械臂与刚性环境接触时，接触力由环境刚度 $K_e$ 和末端侵入量决定：

其中 $x$ 为实际位置，$x_e$ 为环境表面位置。

阻抗控制定义了期望的末端动态特性：

在稳态（$\dot{x}=0, \ddot{x}=0$）下，简化为：

联立环境模型，可得：

由此可知：

• 稳态接触力由期望轨迹 $x_r$ 和环境刚度 $K_e$、阻抗刚度 $K_d$ 共同决定。
• 要实现指定的力 $F_d$，可以通过调整期望轨迹 $x_r$ 或阻抗刚度 $K_d$ 来达到。

2. 方法一：基于期望轨迹规划（无传感器力控）

这是阻抗控制实现力控的经典方法，无需力传感器，仅需编码器反馈。

2.1 原理

通过规划期望轨迹 $x_r$ 使其深入环境内部，利用环境刚度产生接触力。阻抗控制通过调节 $K_d$ 来改变末端“硬度”，从而影响力的大小。

2.2 实现步骤

1. 设定目标力 $F_d$。
2. 估计环境刚度 $K_e$（可通过离线辨识或在线估计）。
3. 计算期望轨迹： $$x_r = x_e + \frac{F_d}{K_e} \cdot \left(1 + \frac{K_d}{K_e}\right)$$ 或更常用的是设定一个力控方向上的期望位置偏移： $$x_r = x_{r0} + \Delta x_{force}$$ 其中 $\Delta x_{force}$ 通过力误差的积分或比例关系调整。
4. 阻抗控制执行：机械臂在阻抗控制下跟踪 $x_r$，稳态时实际力 $F_{ext}$ 会趋近于 $F_d$。

2.3 特点

• 优点：无需力传感器；实现简单；利用环境刚度自然产生力。
• 缺点：力控精度受 $K_e$ 估计误差影响；环境刚度变化时需要重新调整；响应较慢（依赖位置环）。

3. 方法二：阻抗控制 + 力传感器外环

在阻抗控制框架中引入力传感器反馈，形成力外环 + 阻抗内环的结构，实现更精确的力跟踪。

3.1 原理

• 内环：阻抗控制保持期望的动态特性（保证稳定性）。
• 外环：根据力误差修正期望轨迹或阻抗参数，使实际力趋近期望力。

3.2 实现方式

3.2.1 修正期望轨迹（最常用）

其中 $K_f$ 为力控增益。将修正后的期望轨迹输入阻抗控制器，阻抗控制再根据位置误差输出力矩。

3.2.2 修正期望刚度（自适应阻抗控制）

通过在线调整阻抗刚度，使接触力收敛。

3.3 控制律示例

完整的控制律为：

其中 $x_r$ 由力外环实时更新：

（积分器消除稳态误差）

3.4 特点

• 优点：力控精度高，可消除稳态误差；适应环境刚度变化。
• 缺点：需要力传感器；参数整定复杂（力环与阻抗环耦合）。

4. 参数对力控的影响

4.1 阻抗刚度 $K_d$

• 力控特性：$K_d$ 决定了末端“硬度”。在基于轨迹规划的方法中，$K_d$ 越小，力控对 $x_r$ 的响应越灵敏，但稳态误差可能增大；在力传感器外环方法中，$K_d$ 影响力控环的稳定性。
• 选择建议：恒力任务通常选择较低的 $K_d$（如 500–2000 N/m），使末端较软，对外力变化敏感。

4.2 环境刚度 $K_e$

• 影响：$K_e$ 未知或变化时，单纯轨迹规划方法的力控精度会下降。
• 对策：采用力传感器外环，或在线估计 $K_e$ 并调整 $x_r$。

4.3 阻尼 $B_d$

• 作用：抑制力控过程中的振荡。力控任务中建议取临界阻尼或略过阻尼（$\zeta \ge 1$）。

4.4 力控增益 $K_f$

• 作用：力传感器外环的比例增益。过大易振荡，过小响应慢。
• 整定：先整定阻抗内环至稳定，再逐步增加 $K_f$，观察力响应。

5. 两种方法的对比

6. 工程实现示例（方法二：力传感器外环）

# 初始化
M_d = 1.0      # 目标惯量 (kg)
B_d = 50.0     # 目标阻尼 (N·s/m)
K_d = 1000.0   # 目标刚度 (N/m)
K_f = 0.01     # 力控增益 (m/N)
x_r = 0.0      # 期望轨迹
F_d = 10.0     # 期望力 (N)

while True:
    # 1. 读取实际状态
    x, v, a = read_encoder()
    F_ext = read_force_sensor()
    
    # 2. 力外环：修正期望轨迹
    e_f = F_d - F_ext
    x_r += K_f * e_f * dt   # 积分修正
    
    # 3. 阻抗控制：计算期望力
    e = x_r - x
    e_dot = 0 - v
    F_cmd = M_d * (0 - a) + B_d * e_dot + K_d * e  # 假设期望加速度为0
    
    # 4. 动力学补偿 + 力矩输出
    tau_cmd = J.T @ F_cmd + tau_grav + tau_fric
    send_torque(tau_cmd)
    
    wait(dt)

7. 与导纳控制力控的区别

8. 总结

阻抗控制实现指定值的力控有两种主流方法：

1. 无传感器方法：通过规划期望轨迹深入环境内部，利用环境刚度和阻抗刚度产生目标力。简单但精度受限，适合环境刚度已知且稳定的场景。

2. 力传感器外环方法：在阻抗控制外叠加力反馈环，实时修正期望轨迹或阻抗参数，实现高精度力跟踪。复杂但适应性强，适合精密力控任务。

无论哪种方法，阻抗控制实现力控的本质都是通过调节位置（或位置误差）来间接控制力，这与直接力控制（力环直接输出力矩）有根本不同。选择何种方法，取决于硬件能力（是否有力传感器、是否支持力矩控制）、精度要求和环境特性。

七、外部受力检测方法

末端六维力传感器、关节力矩传感器、电流/力矩估计这三种机械臂受力检测方法的算法原理、核心公式、优缺点及适用场景。

1. 末端六维力传感器

1.1 算法原理

六维力传感器通常安装在机械臂末端法兰与工具之间，直接测量工具与环境相互作用的三个力分量（$F_x, F_y, F_z$）和三个力矩分量（$M_x, M_y, M_z$）。

工作原理：

• 传感器内部由多个应变片组成的弹性体结构（如十字梁、圆筒形）。
• 当外力作用于传感器时，弹性体产生微小形变，应变片电阻值变化。
• 通过惠斯通电桥将电阻变化转换为电压信号。
• 利用标定矩阵（解耦矩阵）将电压信号转换为六维力/力矩数据。

核心公式：

其中：

• $\mathbf{C}$ 为 $6 \times n$ 的解耦矩阵（通过标定获得，$n$ 为应变片通道数，通常 $n \geq 6$）
• $V_i$ 为各通道的电压信号

1.2 算法公式（数据处理）

实际使用时，原始数据需经过零点漂移补偿和温度补偿：

其中 $K_T$ 为温度补偿系数，$T$ 为当前温度。

1.3 优缺点

1.4 适用场景

• 精密装配（轴孔装配、电子元件插接）
• 恒力打磨、抛光
• 医疗手术机器人
• 力控研究与算法验证
• 高精度力/位混合控制

典型产品：ATI Omega系列、Robotiq FT300、坤维KWR系列

2. 关节力矩传感器

2.1 算法原理

关节力矩传感器集成在机器人关节内部，通常安装在减速器与连杆之间，直接测量每个关节的输出力矩，再通过机器人运动学推算末端受力。

工作原理：

• 每个关节配置应变式或磁致伸缩式力矩传感器。
• 传感器测量关节输出轴的扭转形变，得到关节力矩 $\tau_j$（$j=1,\dots,n$，$n$ 为关节数）。
• 通过机器人动力学模型和雅可比矩阵，从关节力矩中分离出外力引起的部分，并映射到末端六维力。

核心公式：

关节总力矩由以下部分构成：

其中：

• $\tau_{total}$：传感器测量的关节总力矩
• $\tau_{ext}$：外力引起的关节力矩（待求）
• $\tau_{gravity}$：重力补偿项（与关节位置 $q$ 有关）
• $\tau_{friction}$：摩擦力矩（速度、温度相关）
• $\tau_{inertia} + \tau_{Coriolis}$：惯性力和科里奥利力项（与加速度、速度相关）

分离出 $\tau_{ext}$ 后，通过雅可比矩阵映射到末端力：

其中 $J$ 为雅可比矩阵，$(J^T)^+$ 为 $J^T$ 的伪逆（当机械臂冗余时使用）。

2.2 算法公式（外力分离）

在实际控制器中，外力矩的计算通常为：

各补偿项需基于精确的动力学模型实时计算。

2.3 优缺点

2.4 适用场景

• 协作机器人（人机交互、拖动示教）
• 碰撞检测与安全保护
• 关节空间力控（如阻抗控制）
• 装配任务（配合末端工具）

典型产品：Franka Emika Panda、KUKA LBR iiwa、UR（通过电流估计，非直接关节力矩传感器）

3. 电流/力矩估计（无传感器方案）

3.1 算法原理

利用电机驱动器的电流反馈和机器人动力学模型，估算关节外力矩，进而推算末端受力，无需额外硬件传感器。

工作原理：

• 电机输出力矩与电流成正比：$\tau_{motor} = K_t \cdot I$，其中 $K_t$ 为电机力矩常数，$I$ 为电流（经矢量控制解耦后的q轴电流）。
• 通过动力学模型计算理论上的驱动力矩（包括重力、摩擦力、惯性力等）。
• 实际电机力矩与理论力矩的差值即为外力矩 $\tau_{ext}$。

核心公式：

电机输出力矩：

外力矩估算：

其中：

• $\tau_{gravity}(q)$：重力力矩（取决于关节位置）
• $\tau_{friction}(\dot{q})$：摩擦力矩（通常包含库仑摩擦和粘滞摩擦）
• $M(q)\ddot{q}$：惯性力矩
• $C(q,\dot{q})\dot{q}$：科里奥利力和离心力

得到 $\tau_{ext}$ 后，通过雅可比映射得到末端力：

3.2 算法公式（简化实现）

在实际工程中，为降低计算量，常采用简化模型：

忽略惯性力和科里奥利力（适用于低速运动场景）。

3.3 优缺点

3.4 适用场景

• 拖动示教（如UR的“零力模式”）
• 低成本力控方案
• 碰撞检测（阈值保护）
• 不需要高精度力控的柔顺任务

典型产品：UR协作机器人（通过关节电流估算）、各类工业机器人（需二次开发）

4. 三种方法对比总结

5. 选型建议

• 需要最高精度力控（如医疗手术、微装配）→ 末端六维力传感器
• 协作机器人，集成度高，避免外部线缆（如人机协作）→ 关节力矩传感器
• 成本敏感，已有工业机器人改造（如拖动示教）→ 电流/力矩估计
• 动态性能要求极高（如高速打磨）→ 末端六维力传感器 或 高带宽电流估计+模型补偿

在实际系统中，常采用组合方案：例如使用末端六维力传感器实现高精度力控，同时用电流估计进行碰撞检测和安全保护，提高系统的鲁棒性和安全性。

八、重力估计和摩擦力估计

重力估计和摩擦力估计是机器人实现精确力控制、阻抗控制、以及无传感器力估计（如电流/力矩估计）的核心基础。它们的精度直接决定了外力估算的准确性，进而影响柔顺控制的性能。

下面分别详细阐述两者的算法原理、数学模型、辨识方法及工程实现。

一、重力估计

重力估计的目的是计算机械臂在任意姿态下，各关节因连杆重量而产生的静态力矩，以便在控制中将其补偿掉，使控制器只响应外力。

1. 算法原理

重力估计基于拉格朗日动力学或牛顿-欧拉递推法，将机械臂视为由多个连杆组成的树状结构。每个连杆有其质量、质心位置和惯性张量。重力对每个关节产生的力矩等于该关节下游所有连杆的重力合力对该关节轴的力矩。

核心思想：将机械臂的每个连杆视为刚体，计算其在重力场中产生的静力矩。

2. 数学模型

2.1 单连杆重力矩

对于一个孤立连杆，重力作用在质心处，对关节轴的力矩为：

其中：

• $m_i$：连杆 $i$ 的质量
• $\mathbf{g}$：重力加速度向量（通常为 $[0, 0, -g]^T$ 在世界坐标系中）
• $\mathbf{p}_{com,i}$：连杆 $i$ 质心在基坐标系中的位置
• $q_i$：关节 $i$ 的角度

2.2 完整机械臂的重力矩

对于多连杆机械臂，每个关节 $i$ 的重力矩等于所有下游连杆的重力对该关节轴的贡献之和：

其中 $n$ 为总关节数。

更常用的形式是使用雅可比矩阵的质心部分：

其中 $J_{v,j}$ 为连杆 $j$ 质心的平动雅可比矩阵。

2.3 标准动力学方程中的重力项

在完整的机器人动力学方程中，重力项 $\mathbf{G}(q)$ 是关节位置 $q$ 的函数：

其中 $P(q)$ 为机械臂的势能函数。

具体表达式（以6轴串联机器人为例）：

3. 参数辨识方法

重力估计的精度依赖于动力学参数（质量、质心位置、惯性张量）的准确性。这些参数通常通过辨识实验获得。

3.1 最小二乘辨识

步骤：

1. 设计激励轨迹（如正弦扫描、傅里叶级数轨迹），使机械臂充分运动。
2. 采集关节位置 $q$、速度 $\dot{q}$、加速度 $\ddot{q}$ 和驱动力矩 $\tau$。
3. 构建线性回归模型： $$\tau = \mathbf{Y}(q, \dot{q}, \ddot{q}) \cdot \mathbf{\Phi}$$ 其中 $\mathbf{Y}$ 为回归矩阵，$\mathbf{\Phi}$ 为待辨识的动力学参数向量（包括质量、质心、惯性项）。
4. 使用加权最小二乘法求解： $$\mathbf{\Phi} = (\mathbf{Y}^T \mathbf{W} \mathbf{Y})^{-1} \mathbf{Y}^T \mathbf{W} \tau$$ 其中 $\mathbf{W}$ 为权重矩阵（通常取信号信噪比的倒数）。

3.2 静力法（简单辨识）

对于重力项，可通过静态标定获得：

• 将机械臂置于多个不同姿态 $q_k$（至少 $n$ 个）。
• 记录每个姿态下的关节力矩 $\tau_{static,k}$（电机保持力矩）。
• 由于静态时惯性力、科里奥利力为零，且摩擦力可视为常数，有： $$\tau_{static} = \mathbf{G}(q) + \tau_{friction\_coulomb} \cdot \text{sign}(\dot{q}) + \tau_{offset}$$
• 通过多姿态数据拟合，分离出重力项。

4. 工程实现

在实际控制器中，重力补偿通常以前馈形式加入：

## 控制循环中
q = get_joint_positions()          ## 当前关节角度
tau_grav = compute_gravity(q)      ## 计算重力矩（查表或实时计算）
tau_cmd = tau_feedback + tau_grav  ## 叠加到控制输出

优化方法：

• 离线查表：预先计算各姿态的重力矩，运行时插值，速度快但占用内存。
• 实时计算：使用递推牛顿-欧拉算法，计算量小（$O(n)$），精度高。

5. 误差来源与补偿

二、摩擦力估计

摩擦力是影响低速力控精度和动态响应的重要非线性因素。摩擦力估计的目的是准确建模并补偿关节处的摩擦转矩。

1. 算法原理

摩擦力产生于关节中的轴承、减速器、密封件等接触面之间的相对运动。摩擦力与运动速度、温度、润滑状态、负载等因素密切相关，表现出复杂的非线性特性。

核心思想：建立摩擦力模型，根据关节速度 $\dot{q}$ 预测摩擦力矩，并在控制中以前馈形式抵消，或在电流估计中将其从电机力矩中减去。

2. 数学模型

2.1 静态摩擦模型（最常用）

库仑摩擦 + 粘滞摩擦 + 静摩擦：