# 机械臂控制方法
## 一、五种控制模式与对比总览
### 1. 位置控制
| 维度 | 内容 |
|------|------|
| **目标输入** | 期望位置 $ x_d $（笛卡尔空间或关节空间） |
| **观测输入** | 实际位置 $ x $（编码器） |
| **控制输出** | 关节力矩 $ \tau $ 或速度指令（取决于内环） |
| **核心公式** | 常用PID控制：
$ \tau_{cmd} = K_p e + K_i \int e\, dt + K_d \dot{e} $
$ e = x_d - x $ |
| **参数整定** | 整定 $ K_p, K_i, K_d $ 以获得快速且无超调的响应。常用Ziegler-Nichols或手动试凑。 |
| **重力补偿与摩擦力补偿** | **重力补偿**：通常作为前馈项加入，用于抵消连杆重力产生的静态力矩，尤其在关节坐标控制中必不可少，可减小稳态误差。
**摩擦力补偿**：可选，在低速或高精度定位时加入，补偿静摩擦和库仑摩擦，防止爬行现象。 |
| **优点** | 简单、成熟、稳态精度高，适用于自由空间高精度轨迹跟踪。 |
| **缺点** | 无法处理接触力，与环境接触时会产生很大的冲击力。 |
| **适用场景** | 无接触运动：搬运、喷涂、点胶、焊接。 |
---
### 2. 速度控制
| 维度 | 内容 |
|------|------|
| **目标输入** | 期望速度 $ \dot{x}_d $（笛卡尔或关节） |
| **观测输入** | 实际速度 $ \dot{x} $（编码器差分或观测器） |
| **控制输出** | 关节力矩 $ \tau $ |
| **核心公式** | 速度环PI：
$ \tau_{cmd} = K_{pv} (\dot{x}_d - \dot{x}) + K_{iv} \int (\dot{x}_d - \dot{x}) dt $ |
| **参数整定** | 调节 $ K_{pv} $ 和 $ K_{iv} $ 使速度响应快速且无稳态误差，通常采用临界比例法。 |
| **重力补偿与摩擦力补偿** | **重力补偿**：通常必需，否则重力会导致速度环产生持续偏差。
**摩擦力补偿**：非常关键，尤其在高精度速度跟踪或零速时，需加入摩擦模型（静摩擦+库仑摩擦+粘滞摩擦）的前馈补偿，以减小速度过零时的死区。 |
| **优点** | 响应快，可作为位置控制的内环，提高系统带宽。 |
| **缺点** | 单独使用时有位置累积误差；对摩擦力敏感。 |
| **适用场景** | 速度跟踪任务（如喷涂、切割）、位置环的内环。 |
---
### 3. 力控制
| 维度 | 内容 |
|------|------|
| **目标输入** | 期望力 $ F_d $（笛卡尔空间） |
| **观测输入** | 实际接触力 $ F $（力传感器） |
| **控制输出** | 关节力矩 $ \tau $ |
| **核心公式** | 力环PID：
$ F_{cmd} = K_{pf} (F_d - F) + K_{if} \int (F_d - F) dt + K_{df} (\dot{F}_d - \dot{F}) $
$ \tau = J^T F_{cmd} + \tau_{comp} $ |
| **参数整定** | 先降低位置环增益，再整定力环参数。比例决定力控刚度，积分消除稳态误差，微分提高稳定性。 |
| **重力补偿与摩擦力补偿** | **重力补偿**：必须精确补偿，否则重力会被误判为接触力，导致力控偏差。
**摩擦力补偿**：同样重要，关节摩擦会干扰力传感器读数或影响力环输出，需用摩擦模型前馈消除。 |
| **优点** | 可实现精确的力跟踪，适合恒力接触任务。 |
| **缺点** | 对力传感器依赖强，在刚性环境中易失稳，无法处理位置约束。 |
| **适用场景** | 精密装配（压装）、打磨抛光、医疗手术。 |
---
### 4. 导纳控制
| 维度 | 内容 |
|------|------|
| **目标输入** | 参考轨迹 $ x_r, \dot{x}_r, \ddot{x}_r $ + 期望力 $ F_d $（可选） |
| **观测输入** | 实际外力 $ F_{ext} $（力传感器或关节力矩估计） |
| **控制输出** | 修正后的期望位置 $ x_{cmd} $（发送给底层位置控制器） |
| **核心公式** | $ M_d (\ddot{x}_{cmd} - \ddot{x}_r) + B_d (\dot{x}_{cmd} - \dot{x}_r) + K_d (x_{cmd} - x_r) = F_{ext} - F_d $
离散化得 $ x_{cmd} = x_r + \Delta x $ |
| **参数整定** | 整定 $ M_d, B_d, K_d $。刚度 $ K_d $ 决定柔顺程度，阻尼 $ B_d $ 抑制振荡，惯量 $ M_d $ 影响动态响应。 |
| **重力补偿与摩擦力补偿** | **重力补偿**：通常由底层位置控制器（如伺服驱动器）完成，导纳层本身不直接补偿重力，但若采用关节力矩估计外力，则必须包含精确的重力与摩擦力模型，否则外力估计不准。
**摩擦力补偿**：同样在底层位置环或外力估计中实现，保证力测量的准确性。 |
| **优点** | 实现简单，可直接叠加在工业机器人位置环上；稳定性好。 |
| **缺点** | 响应受底层位置环限制；依赖力传感器或精确的力矩估计。 |
| **适用场景** | 工业机器人拖动示教、人机协作、精密装配。 |
---
### 5. 阻抗控制
| 维度 | 内容 |
|------|------|
| **目标输入** | 期望轨迹 $ x_r, \dot{x}_r, \ddot{x}_r $ |
| **观测输入** | 实际位置 $ x $、速度 $ \dot{x} $（编码器） |
| **控制输出** | 关节力矩 $ \tau $ |
| **核心公式** | $ F_{cmd} = M_d (\ddot{x}_r - \ddot{x}) + B_d (\dot{x}_r - \dot{x}) + K_d (x_r - x) $
$ \tau = J^T F_{cmd} + \tau_{grav} + \tau_{fric} $ |
| **参数整定** | 整定 $ M_d, B_d, K_d $ 使系统在接触与自由空间均稳定。通常先定刚度，再定阻尼至临界阻尼。 |
| **重力补偿与摩擦力补偿** | **重力补偿**：**必不可少**。阻抗控制输出力矩直接驱动电机，必须加入精确的重力补偿项 $ \tau_{grav} $，否则机械臂会因重力而下垂，无法跟踪轨迹。
**摩擦力补偿**：强烈推荐，尤其对于谐波减速器等非线性摩擦明显的关节，加入摩擦前馈可显著提高低速轨迹跟踪性能和力控精度。 |
| **优点** | 高动态响应，无需力传感器，同时实现轨迹跟踪与柔顺交互。 |
| **缺点** | 需要开放力矩接口的机器人，对动力学模型精度要求高。 |
| **适用场景** | 协作机器人、人机交互、仿人机器人、外骨骼。 |
---
### 重力补偿与摩擦力补偿的作用总结
- **重力补偿**：消除连杆重力产生的静态力矩。在位置/速度控制中作为前馈提高精度；在力控、阻抗控制中**必须精确补偿**，否则重力会被控制器视为外部干扰或导致控制偏差。
- **摩擦力补偿**：补偿关节摩擦力（静摩擦、库仑摩擦、粘滞摩擦）。在低速运动、力控、阻抗控制中尤为重要，可减小死区、提高力控带宽和轨迹跟踪精度。
---
### 五种算法对比总表
| 算法 | 目标输入 | 观测输入 | 控制输出 | 核心公式 | 重力补偿 | 摩擦力补偿 | 优点 | 缺点 | 适用场景 |
|------|---------|---------|---------|---------|---------|---------|------|------|---------|
| **位置控制** | $ x_d $ | $ x $ | $ \tau $ | PID | 前馈可选 | 可选 | 简单、高精度 | 接触力不可控 | 自由空间轨迹跟踪 |
| **速度控制** | $ \dot{x}_d $ | $ \dot{x} $ | $ \tau $ | PI | 必需 | 强烈推荐 | 响应快 | 位置累积误差 | 速度跟踪、内环 |
| **力控制** | $ F_d $ | $ F $ | $ \tau $ | PID力环 | 必需 | 必需 | 精确力跟踪 | 依赖传感器、易失稳 | 恒力接触任务 |
| **导纳控制** | $ x_r, F_d $ | $ F_{ext} $ | $ x_{cmd} $ | 导纳模型 | 底层实现 | 底层实现 | 易实现、稳定 | 动态受限 | 工业机器人柔顺任务 |
| **阻抗控制** | $ x_r $ | $ x, \dot{x} $ | $ \tau $ | 阻抗模型 | **必需** | **强烈推荐** | 高动态、无需力传感器 | 需力矩接口、模型精度 | 协作机器人、高性能柔顺 |
---
### 结论
重力补偿和摩擦力补偿是高性能机器人控制的基础，尤其是在力控和阻抗控制中直接决定了系统的稳定性和精度。位置控制虽然对补偿依赖较低，但在高精度场合仍需加入。选择控制算法时，应根据硬件能力（是否有力传感器、是否支持力矩控制）、任务需求（精度、柔顺）以及模型精度综合决定。
## 二、阻抗控制和导纳控制详细对比
阻抗控制和导纳控制是机器人柔顺控制的两大核心方法，它们从**互为对偶**的角度解决同一个问题：如何让机器人在与环境交互时表现出期望的柔顺行为。两者在**控制架构、硬件要求、动态特性、适用场景**上存在本质区别，理解这些差异是选择正确控制方案的关键。
---
### 1. 核心特点对比
| 维度 | 阻抗控制 | 导纳控制 |
|------|----------|----------|
| **控制架构** | 内环：力矩/电流控制
外环：阻抗模型（位置误差→力） | 内环：位置控制
外环：导纳模型（力→位置修正） |
| **输入** | 期望轨迹 $x_r, \dot{x}_r, \ddot{x}_r$ | 期望轨迹 $x_r$ + 期望力 $F_d$（可选） |
| **输出** | 关节力矩 $\tau$ | 修正后的位置指令 $x_c$ |
| **力检测** | 无需力传感器（通过位置误差间接感知） | 需要力传感器或精确的力矩估计 |
| **动力学补偿** | 必需（重力、摩擦力、惯性力） | 底层位置环负责，导纳层通常不直接补偿 |
| **动态响应** | 高（直接力矩控制，带宽可达数百Hz） | 中（受底层位置环带宽限制） |
| **稳定性** | 高刚度环境易振荡，需精确参数 | 稳定性好，尤其适合刚性环境 |
| **参数整定** | $M_d, B_d, K_d$ 直接影响动态特性 | $M_d, B_d, K_d$ 主要影响修正响应 |
---
### 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. 详细优缺点对比
| 对比项 | 阻抗控制 | 导纳控制 |
|--------|----------|----------|
| **硬件要求** | 力矩接口、高精度编码器 | 位置接口、力传感器 |
| **模型精度** | 高（需精确动力学补偿） | 中（底层位置环鲁棒性强） |
| **力控精度** | 中（通过误差间接） | 高（直接测量力） |
| **响应速度** | 快（ms级） | 中（10ms级） |
| **稳定性边界** | 窄（高刚度环境需谨慎） | 宽（底层位置环稳定） |
| **参数整定** | 复杂（三者耦合） | 相对简单（物理意义清晰） |
| **成本** | 中（无传感器，但需高性能驱动） | 高（力传感器昂贵） |
| **典型带宽** | 50–200 Hz | 10–50 Hz |
| **适用自由度** | 冗余/非冗余均可 | 冗余/非冗余均可 |
---
### 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. 导纳控制的基本方程
导纳控制的标准形式（在任务空间）为：
$$
M_d (\ddot{x}_c - \ddot{x}_r) + B_d (\dot{x}_c - \dot{x}_r) + K_d (x_c - x_r) = F_{ext} - F_d
$$
其中：
- $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. 与阻抗控制的对比
| 维度 | 导纳控制 | 阻抗控制 |
|------|----------|----------|
| **输入** | 参考轨迹 $x_r, \dot{x}_r, \ddot{x}_r$ + 期望力 $F_d$ | 参考轨迹 $x_r, \dot{x}_r, \ddot{x}_r$ |
| **输出** | 修正后的位置指令 $x_c$ | 力矩指令 $\tau$ |
| **自由空间行为** | 若 $F_{ext}=0$ 且 $F_d=0$，则精确跟踪参考轨迹 | 主动消除轨迹误差，跟踪参考轨迹 |
| **接触空间行为** | 根据 $F_{ext} - F_d$ 修正轨迹，实现力跟踪 | 根据位置误差产生阻抗力，柔顺响应 |
| **力传感器需求** | 通常需要 | 不需要（通过误差间接感知） |
**关键区别**：导纳控制明确将期望力 $F_d$ 作为输入，更适合需要直接设定接触力的任务（如恒力打磨）；阻抗控制则通过调整阻抗参数来间接影响力交互，无需力传感器，但力跟踪精度较低。
---
### 6. 总结
- **参考轨迹 $x_r, \dot{x}_r, \ddot{x}_r$**：定义标称运动，是机械臂“想走的路”。无外力或外力恰好等于期望力时，机械臂严格按此运动，保证了轨迹跟踪精度。
- **期望力 $F_d$**：定义期望的接触力，是机械臂“想施加的力”。通过导纳模型，实际力与期望力的偏差被转换为位置修正，实现主动力跟踪或被动柔顺。
- **二者结合**：使导纳控制成为一个统一的框架，既能实现高精度轨迹跟踪（自由空间），又能实现恒力控制（接触空间），且二者平滑过渡，无需切换控制策略。
在实际应用中，合理设置参考轨迹和期望力，配合导纳参数整定，即可让机械臂完成从自由运动到复杂接触任务的连续操作。
---
## 四、阻抗控制：期望轨迹详解
在阻抗控制中，期望轨迹 $ x_r, \dot{x}_r, \ddot{x}_r $ 是控制器的**核心输入**，它们共同定义了机械臂“想达到的运动状态”。阻抗控制通过比较期望轨迹与实际状态，计算出需要输出的力矩，使机械臂在**主动跟踪轨迹**的同时，表现出可调的柔顺特性。
下面从控制律公式出发，逐一解析三个期望量的具体作用。
---
### 1. 阻抗控制的基本方程
阻抗控制的标准形式（在任务空间）为：
$$
F_{cmd} = M_d (\ddot{x}_r - \ddot{x}) + B_d (\dot{x}_r - \dot{x}) + K_d (x_r - x)
$$
其中：
- $ 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. 与导纳控制的对比
| 维度 | 阻抗控制 | 导纳控制 |
|------|----------|----------|
| **期望轨迹的作用** | 直接作为控制器的输入，与反馈比较产生误差，进而输出力矩 | 作为参考基准，外力产生修正量叠加后形成位置指令 |
| **无外力时的行为** | 主动出力消除误差，精确跟踪轨迹 | 无修正，底层位置控制器跟踪轨迹 |
| **轨迹跟踪性能** | 可独立设计 $ M_d, B_d, K_d $ 优化动态响应 | 受底层位置环限制，跟踪性能取决于位置环带宽 |
| **是否需要加速度** | 需要 $ \ddot{x}_r $ 作为前馈，提升动态性能 | 通常不需要，导纳模型内部可生成加速度 |
---
### 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 稳态分析
在一维稳态情况下（速度、加速度为零），导纳方程简化为：
$$
K_d (x_c - x_r) = F_{ext} - F_d
$$
其中：
- $x_r$：参考轨迹位置
- $x_c$：修正后的指令位置
- $K_d$：导纳刚度
- $F_{ext}$：实际接触力
- $F_d$：期望力
整理得：
$$
x_c = x_r + \frac{F_{ext} - F_d}{K_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$。
实际接触力：
$$
F_{ext} = K_e (x - x_e) \approx K_e (x_c - x_e)
$$
将 $x_c$ 的表达式代入：
$$
F_{ext} \approx K_e \left( x_r - x_e + \frac{F_{ext} - F_d}{K_d} \right)
$$
整理得：
$$
F_{ext} \left(1 - \frac{K_e}{K_d}\right) \approx K_e (x_r - x_e) - \frac{K_e}{K_d} F_d
$$
对于典型导纳控制，通常选择 $K_d \ll K_e$（即导纳刚度远小于环境刚度），使得 $K_e/K_d$ 很大，此时：
$$
F_{ext} \cdot \left(-\frac{K_e}{K_d}\right) \approx -\frac{K_e}{K_d} F_d \quad \Rightarrow \quad F_{ext} \approx F_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$ 进行限幅：
$$
\Delta x = \text{clip}\left(\frac{F_d - F_{ext}}{K_d}, -\Delta x_{max}, \Delta x_{max}\right)
$$
#### 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$ 和末端侵入量决定：
$$
F_{ext} = K_e \cdot (x - x_e)
$$
其中 $x$ 为实际位置，$x_e$ 为环境表面位置。
阻抗控制定义了期望的末端动态特性：
$$
M_d (\ddot{x}_r - \ddot{x}) + B_d (\dot{x}_r - \dot{x}) + K_d (x_r - x) = F_{ext}
$$
在**稳态**（$\dot{x}=0, \ddot{x}=0$）下，简化为：
$$
K_d (x_r - x) = F_{ext}
$$
联立环境模型，可得：
$$
F_{ext} = \frac{K_d K_e}{K_d + K_e} (x_r - x_e)
$$
由此可知：
- 稳态接触力由期望轨迹 $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 修正期望轨迹（最常用）
$$
x_{r,new} = x_{r,original} + K_f (F_d - F_{ext})
$$
其中 $K_f$ 为力控增益。将修正后的期望轨迹输入阻抗控制器，阻抗控制再根据位置误差输出力矩。
##### 3.2.2 修正期望刚度（自适应阻抗控制）
$$
K_d(t) = K_{d0} + K_{adapt} \int (F_d - F_{ext}) dt
$$
通过在线调整阻抗刚度，使接触力收敛。
#### 3.3 控制律示例
完整的控制律为：
$$
\tau = J^T \left[ M_d (\ddot{x}_r - \ddot{x}) + B_d (\dot{x}_r - \dot{x}) + K_d (x_r - x) \right] + \tau_{grav} + \tau_{fric}
$$
其中 $x_r$ 由力外环实时更新：
$$
x_r(t) = x_{r0}(t) + K_f \int_0^t (F_d - F_{ext}) dt
$$
（积分器消除稳态误差）
#### 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. 两种方法的对比
| 维度 | 方法一：轨迹规划（无传感器） | 方法二：力传感器外环 |
|------|------------------------------|------------------------|
| **力传感器** | 不需要 | 需要 |
| **力控精度** | 中（依赖 $K_e$ 估计） | 高（直接反馈） |
| **响应速度** | 慢（位置环限制） | 中（力环+阻抗环） |
| **稳态误差** | 可能存在（需积分） | 可消除（积分外环） |
| **参数整定** | 简单 | 较复杂（双环耦合） |
| **适应能力** | 环境变化需重调 | 自动适应 |
| **典型应用** | 低成本力控、简单装配 | 精密打磨、医疗手术 |
---
### 6. 工程实现示例（方法二：力传感器外环）
```python
# 初始化
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$）。
**工作原理**：
- 传感器内部由多个应变片组成的弹性体结构（如十字梁、圆筒形）。
- 当外力作用于传感器时，弹性体产生微小形变，应变片电阻值变化。
- 通过惠斯通电桥将电阻变化转换为电压信号。
- 利用标定矩阵（解耦矩阵）将电压信号转换为六维力/力矩数据。
**核心公式**：
$$
\begin{bmatrix} F_x \\ F_y \\ F_z \\ M_x \\ M_y \\ M_z \end{bmatrix} = \mathbf{C} \cdot \begin{bmatrix} V_1 \\ V_2 \\ \vdots \\ V_n \end{bmatrix}
$$
其中：
- $\mathbf{C}$ 为 $6 \times n$ 的解耦矩阵（通过标定获得，$n$ 为应变片通道数，通常 $n \geq 6$）
- $V_i$ 为各通道的电压信号
#### 1.2 算法公式（数据处理）
实际使用时，原始数据需经过**零点漂移补偿**和**温度补偿**：
$$
F_{comp} = F_{raw} - F_{offset} - K_T \cdot (T - T_0)
$$
其中 $K_T$ 为温度补偿系数，$T$ 为当前温度。
#### 1.3 优缺点
| 维度 | 内容 |
|------|------|
| **优点** | ① 测量精度高（分辨率可达0.01N，精度0.1%~1%FS）
② 直接测量末端力，无需动力学模型
③ 各向同性好，解耦后各通道独立
④ 带宽高（可达1kHz以上） |
| **缺点** | ① 成本高（数万至数十万元）
② 易过载损坏（通常只有3~5倍安全过载）
③ 增加末端惯量，影响动态性能
④ 线缆管理复杂，可能干扰运动
⑤ 存在零点漂移，需定期校准 |
#### 1.4 适用场景
- 精密装配（轴孔装配、电子元件插接）
- 恒力打磨、抛光
- 医疗手术机器人
- 力控研究与算法验证
- 高精度力/位混合控制
**典型产品**：ATI Omega系列、Robotiq FT300、坤维KWR系列
---
### 2. 关节力矩传感器
#### 2.1 算法原理
关节力矩传感器集成在机器人**关节内部**，通常安装在减速器与连杆之间，直接测量每个关节的输出力矩，再通过机器人运动学推算末端受力。
**工作原理**：
- 每个关节配置应变式或磁致伸缩式力矩传感器。
- 传感器测量关节输出轴的扭转形变，得到关节力矩 $\tau_j$（$j=1,\dots,n$，$n$ 为关节数）。
- 通过机器人**动力学模型**和**雅可比矩阵**，从关节力矩中分离出外力引起的部分，并映射到末端六维力。
**核心公式**：
关节总力矩由以下部分构成：
$$
\tau_{total} = \tau_{ext} + \tau_{gravity} + \tau_{friction} + \tau_{inertia} + \tau_{Coriolis}
$$
其中：
- $\tau_{total}$：传感器测量的关节总力矩
- $\tau_{ext}$：外力引起的关节力矩（待求）
- $\tau_{gravity}$：重力补偿项（与关节位置 $q$ 有关）
- $\tau_{friction}$：摩擦力矩（速度、温度相关）
- $\tau_{inertia} + \tau_{Coriolis}$：惯性力和科里奥利力项（与加速度、速度相关）
分离出 $\tau_{ext}$ 后，通过雅可比矩阵映射到末端力：
$$
F_{ext} = (J^T)^+ \cdot \tau_{ext}
$$
其中 $J$ 为雅可比矩阵，$(J^T)^+$ 为 $J^T$ 的伪逆（当机械臂冗余时使用）。
#### 2.2 算法公式（外力分离）
在实际控制器中，外力矩的计算通常为：
$$
\tau_{ext} = \tau_{sensor} - \tau_{gravity}(q) - \tau_{friction}(\dot{q}) - M(q)\ddot{q} - C(q,\dot{q})\dot{q}
$$
各补偿项需基于精确的动力学模型实时计算。
#### 2.3 优缺点
| 维度 | 内容 |
|------|------|
| **优点** | ① 集成度高，无外部线缆
② 可直接测量关节力矩，不受末端工具影响
③ 适合协作机器人，实现拖拽示教和碰撞检测
④ 可同时用于力控和安全保护 |
| **缺点** | ① 仅高端协作机器人（如Franka、KUKA LBR）配置
② 需要精确的动力学补偿（重力、摩擦力、惯性力）
③ 对冲击敏感，易损坏
④ 成本高，维修复杂
⑤ 无法直接测量末端纯力矩（如绕工具坐标系的扭转） |
#### 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_{motor} = K_t \cdot I_q
$$
外力矩估算：
$$
\tau_{ext} = \tau_{motor} - \tau_{gravity}(q) - \tau_{friction}(\dot{q}) - M(q)\ddot{q} - C(q,\dot{q})\dot{q}
$$
其中：
- $\tau_{gravity}(q)$：重力力矩（取决于关节位置）
- $\tau_{friction}(\dot{q})$：摩擦力矩（通常包含库仑摩擦和粘滞摩擦）
- $M(q)\ddot{q}$：惯性力矩
- $C(q,\dot{q})\dot{q}$：科里奥利力和离心力
得到 $\tau_{ext}$ 后，通过雅可比映射得到末端力：
$$
F_{ext} = (J^T)^+ \cdot \tau_{ext}
$$
#### 3.2 算法公式（简化实现）
在实际工程中，为降低计算量，常采用简化模型：
$$
\tau_{ext} \approx K_t I - \tau_{gravity}(q) - \tau_{friction}(\dot{q})
$$
忽略惯性力和科里奥利力（适用于低速运动场景）。
#### 3.3 优缺点
| 维度 | 内容 |
|------|------|
| **优点** | ① 无需额外传感器，成本低
② 易于集成，无硬件改动
③ 可直接利用现有驱动器电流信号
④ 适用于已有工业机器人改造 |
| **缺点** | ① 依赖精确的动力学模型（惯性参数、摩擦模型）
② 低速时摩擦力占主导，信噪比差
③ 无法测量静态外力（摩擦力补偿误差导致漂移）
④ 受温度影响（电机常数 $K_t$ 随温度变化）
⑤ 无法测量纯外力矩（如绕末端轴旋转） |
#### 3.4 适用场景
- 拖动示教（如UR的“零力模式”）
- 低成本力控方案
- 碰撞检测（阈值保护）
- 不需要高精度力控的柔顺任务
**典型产品**：UR协作机器人（通过关节电流估算）、各类工业机器人（需二次开发）
---
### 4. 三种方法对比总结
| 维度 | 末端六维力传感器 | 关节力矩传感器 | 电流/力矩估计 |
|------|------------------|----------------|----------------|
| **测量位置** | 末端法兰与工具之间 | 关节内部（减速器与连杆之间） | 电机驱动器（间接） |
| **测量物理量** | 末端六维力/力矩 | 关节力矩（一维/关节） | 电机电流（转换为力矩） |
| **精度** | 最高（0.1% FS） | 高（1% FS） | 中（5~10% 典型值） |
| **成本** | 高 | 高 | 低（无额外硬件） |
| **带宽** | 高（>1kHz） | 中（500Hz~1kHz） | 高（电流环带宽 >1kHz） |
| **动力学模型依赖** | 无 | 中（需重力/摩擦力补偿） | 高（需完整动力学模型） |
| **温度影响** | 有（需温度补偿） | 有（应变片温漂） | 大（$K_t$ 温漂） |
| **易用性** | 即插即用，需标定 | 集成在机器人中 | 需建模与调试 |
| **维护** | 易损，可更换 | 维修复杂 | 无硬件维护 |
| **适用场景** | 高精度力控、装配、打磨 | 协作机器人、人机交互 | 拖动示教、低成本柔顺 |
---
### 5. 选型建议
- **需要最高精度力控**（如医疗手术、微装配）→ **末端六维力传感器**
- **协作机器人，集成度高，避免外部线缆**（如人机协作）→ **关节力矩传感器**
- **成本敏感，已有工业机器人改造**（如拖动示教）→ **电流/力矩估计**
- **动态性能要求极高**（如高速打磨）→ **末端六维力传感器** 或 **高带宽电流估计+模型补偿**
在实际系统中，常采用**组合方案**：例如使用末端六维力传感器实现高精度力控，同时用电流估计进行碰撞检测和安全保护，提高系统的鲁棒性和安全性。
## 八、重力估计和摩擦力估计
重力估计和摩擦力估计是机器人实现精确力控制、阻抗控制、以及无传感器力估计（如电流/力矩估计）的**核心基础**。它们的精度直接决定了外力估算的准确性，进而影响柔顺控制的性能。
下面分别详细阐述两者的算法原理、数学模型、辨识方法及工程实现。
---
### 一、重力估计
重力估计的目的是计算机械臂在任意姿态下，各关节因连杆重量而产生的**静态力矩**，以便在控制中将其补偿掉，使控制器只响应外力。
#### 1. 算法原理
重力估计基于**拉格朗日动力学**或**牛顿-欧拉递推法**，将机械臂视为由多个连杆组成的树状结构。每个连杆有其质量、质心位置和惯性张量。重力对每个关节产生的力矩等于该关节下游所有连杆的重力合力对该关节轴的力矩。
核心思想：**将机械臂的每个连杆视为刚体，计算其在重力场中产生的静力矩。**
#### 2. 数学模型
##### 2.1 单连杆重力矩
对于一个孤立连杆，重力作用在质心处，对关节轴的力矩为：
$$
\tau_{grav,i} = m_i \cdot \mathbf{g}^T \cdot \frac{\partial \mathbf{p}_{com,i}}{\partial q_i}
$$
其中：
- $m_i$：连杆 $i$ 的质量
- $\mathbf{g}$：重力加速度向量（通常为 $[0, 0, -g]^T$ 在世界坐标系中）
- $\mathbf{p}_{com,i}$：连杆 $i$ 质心在基坐标系中的位置
- $q_i$：关节 $i$ 的角度
##### 2.2 完整机械臂的重力矩
对于多连杆机械臂，每个关节 $i$ 的重力矩等于所有下游连杆的重力对该关节轴的贡献之和：
$$
\tau_{grav,i} = \sum_{j=i}^{n} \left[ m_j \cdot \mathbf{g}^T \cdot \frac{\partial \mathbf{p}_{com,j}}{\partial q_i} \right]
$$
其中 $n$ 为总关节数。
更常用的形式是使用**雅可比矩阵的质心部分**：
$$
\tau_{grav} = \sum_{j=1}^{n} J_{v,j}^T \cdot (m_j \mathbf{g})
$$
其中 $J_{v,j}$ 为连杆 $j$ 质心的平动雅可比矩阵。
##### 2.3 标准动力学方程中的重力项
在完整的机器人动力学方程中，重力项 $\mathbf{G}(q)$ 是关节位置 $q$ 的函数：
$$
\mathbf{G}(q) = \frac{\partial P(q)}{\partial q}
$$
其中 $P(q)$ 为机械臂的势能函数。
**具体表达式**（以6轴串联机器人为例）：
$$
\mathbf{G}(q) =
\begin{bmatrix}
\tau_{grav,1}(q_1, q_2, \dots, q_6) \\
\tau_{grav,2}(q_2, \dots, q_6) \\
\vdots \\
\tau_{grav,6}(q_6)
\end{bmatrix}
$$
#### 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. 工程实现
在实际控制器中，重力补偿通常以**前馈**形式加入：
```python
## 控制循环中
q = get_joint_positions() ## 当前关节角度
tau_grav = compute_gravity(q) ## 计算重力矩（查表或实时计算）
tau_cmd = tau_feedback + tau_grav ## 叠加到控制输出
```
**优化方法**：
- **离线查表**：预先计算各姿态的重力矩，运行时插值，速度快但占用内存。
- **实时计算**：使用递推牛顿-欧拉算法，计算量小（$O(n)$），精度高。
#### 5. 误差来源与补偿
| 误差来源 | 影响 | 补偿方法 |
|----------|------|----------|
| 动力学参数不准确 | 重力矩偏差 | 参数辨识优化 |
| 负载变化（抓取工件） | 末端质量改变 | 在线负载辨识 |
| 温度影响（减速器效率） | 重力矩偏移 | 自适应补偿 |
| 基座倾角（如移动机器人） | 重力方向变化 | 倾角传感器反馈 |
---
### 二、摩擦力估计
摩擦力是影响低速力控精度和动态响应的重要非线性因素。摩擦力估计的目的是准确建模并补偿关节处的摩擦转矩。
#### 1. 算法原理
摩擦力产生于关节中的**轴承、减速器、密封件**等接触面之间的相对运动。摩擦力与运动速度、温度、润滑状态、负载等因素密切相关，表现出复杂的非线性特性。
**核心思想**：建立摩擦力模型，根据关节速度 $\dot{q}$ 预测摩擦力矩，并在控制中以前馈形式抵消，或在电流估计中将其从电机力矩中减去。
#### 2. 数学模型
##### 2.1 静态摩擦模型（最常用）
**库仑摩擦 + 粘滞摩擦 + 静摩擦**：
$$
\tau_{fric}(\dot{q}) = \tau_c \cdot \text{sgn}(\dot{q}) + \tau_v \cdot \dot{q} + \tau_s \cdot e^{-(|\dot{q}|/v_s)^2} \cdot \text{sgn}(\dot{q})
$$
其中：
- $\tau_c$：库仑摩擦力矩（常数，与速度方向相反）
- $\tau_v$：粘滞摩擦系数（线性项）
- $\tau_s$：静摩擦力矩（最大静摩擦）
- $v_s$：Stribeck速度（静摩擦到库仑摩擦的过渡速度）
- $\text{sgn}(\dot{q})$：符号函数
**简化版（工业常用）**：
$$
\tau_{fric}(\dot{q}) = \tau_c \cdot \text{sgn}(\dot{q}) + \tau_v \cdot \dot{q}
$$
##### 2.2 动态摩擦模型（高精度）
**LuGre模型**（最经典）：
$$
\frac{dz}{dt} = \dot{q} - \frac{|\dot{q}|}{g(\dot{q})} z
$$
$$
g(\dot{q}) = \tau_c + (\tau_s - \tau_c) e^{-(|\dot{q}|/v_s)^2}
$$
$$
\tau_{fric} = \sigma_0 z + \sigma_1 \frac{dz}{dt} + \tau_v \dot{q}
$$
其中：
- $z$：内部状态变量（鬃毛平均形变）
- $\sigma_0$：鬃毛刚度
- $\sigma_1$：鬃毛阻尼
- 该模型能描述预滑动位移、Stribeck效应、摩擦滞后等复杂现象。
##### 2.3 摩擦力矩的分解
在关节总力矩中，摩擦力矩通常与其他项叠加：
$$
\tau_{motor} = \tau_{inertia} + \tau_{gravity} + \tau_{fric} + \tau_{ext}
$$
#### 3. 参数辨识方法
##### 3.1 恒定速度法（简单）
- 控制机械臂以恒定速度 $\dot{q}_k$ 运动。
- 记录各关节的稳态力矩 $\tau_{steady}$。
- 忽略惯性项，有：
$$
\tau_{steady} = \tau_{fric}(\dot{q}_k) + \tau_{gravity}(q) + \tau_{ext}
$$
- 通过多个速度点测量，拟合出 $\tau_c, \tau_v$。
##### 3.2 最小二乘辨识（完整）
**步骤**：
1. 设计激励轨迹（覆盖正反方向、不同速度）。
2. 采集 $q, \dot{q}, \ddot{q}, \tau$ 数据。
3. 构建摩擦力模型，通过优化算法（如非线性最小二乘）求解模型参数。
4. 常用损失函数：
$$
\min \sum_k \| \tau_{fric,model}(\dot{q}_k) - \tau_{fric,meas}(\dot{q}_k) \|^2
$$
##### 3.3 递推最小二乘（在线辨识）
在运行过程中实时更新摩擦参数，适应温度变化和磨损：
$$
\hat{\theta}(k) = \hat{\theta}(k-1) + K(k) [y(k) - \phi^T(k) \hat{\theta}(k-1)]
$$
#### 4. 工程实现
在控制系统中，摩擦力补偿通常以**前馈**形式加入：
```python
## 控制循环中
q_dot = get_joint_velocities()
tau_fric = compute_friction(q_dot) ## 根据速度查表或公式计算
tau_cmd = tau_feedback + tau_grav + tau_fric ## 叠加补偿
```
**特殊处理**：
- **零速处理**：当 $|\dot{q}| < \epsilon$ 时，不进行摩擦补偿（避免抖动），或使用平滑切换函数（如双曲正切）。
- **方向切换**：速度过零时，摩擦符号突变，需配合高带宽控制避免冲击。
#### 5. 误差来源与补偿
| 误差来源 | 影响 | 补偿方法 |
|----------|------|----------|
| 温度变化 | 摩擦系数变化 | 在线自适应辨识 |
| 负载变化 | 关节正压力变化 | 负载相关的摩擦模型 |
| 速度波动 | 动态摩擦滞后 | 使用LuGre等动态模型 |
| 方向非对称 | 正反向摩擦不同 | 分别辨识正向和反向参数 |
---
### 三、重力与摩擦力的联合作用
在实际的**电流/力矩估计**或**力控制**中，重力与摩擦力需要同时补偿。外力矩的估算公式为：
$$
\tau_{ext} = \tau_{motor} - \tau_{grav}(q) - \tau_{fric}(\dot{q}) - M(q)\ddot{q} - C(q,\dot{q})\dot{q}
$$
其中：
- $\tau_{motor}$：电机输出力矩（由电流估算）
- $\tau_{grav}(q)$：重力补偿（关键项）
- $\tau_{fric}(\dot{q})$：摩擦力补偿（低速时尤其重要）
- $M(q)\ddot{q} + C(q,\dot{q})\dot{q}$：惯性力和科里奥利力（高速时需补偿）
#### 工程实现架构
```
电机电流 I → 力矩估算 τ_motor
↓
关节位置 q, 速度 q_dot, 加速度 q_ddot
↓
┌─────────────┼─────────────┐
↓ ↓ ↓
重力模型 摩擦模型 惯性模型
↓ ↓ ↓
τ_grav(q) τ_fric(q_dot) τ_inertia(q,q_dot,q_ddot)
↓
τ_ext = τ_motor - Σ补偿项
↓
雅可比映射 → 末端力 F_ext
```
---
### 四、总结
| 项目 | 重力估计 | 摩擦力估计 |
|------|----------|------------|
| **物理本质** | 连杆重力对关节的静态力矩 | 接触面相对运动的阻力 |
| **主要影响因素** | 关节位置 $q$、负载质量 | 关节速度 $\dot{q}$、温度、负载 |
| **数学模型** | 基于拉格朗日动力学 | 库仑+粘滞模型 / LuGre模型 |
| **辨识方法** | 最小二乘、静力法 | 恒定速度法、非线性最小二乘 |
| **补偿方式** | 前馈（位置相关） | 前馈（速度相关） |
| **精度要求** | 高（影响力控稳态误差） | 高（影响低速力控响应） |
| **工程难点** | 负载变化、基座倾角 | 温度变化、方向非对称 |
精确的重力与摩擦力估计是实现高质量柔顺控制的基础。在工程实践中，通常采用**离线辨识 + 在线自适应**的组合策略，以应对模型参数随工况变化的问题。
## 九、惯性参数估计
质量估计在机器人控制中通常指**动力学参数辨识**中的核心部分——估计连杆的**质量**、**质心位置**和**惯性张量**，或是在线估计**末端负载质量**。准确的动力学参数是实现高精度重力补偿、惯性前馈和阻抗控制的基础。
下面从**物理意义**、**数学模型**、**辨识方法**、**在线/离线策略**及**工程应用**五个方面详细阐述。
---
### 1. 质量估计的物理意义
在机器人动力学中，每个连杆的惯性参数包括：
- **质量** $ m_i $
- **质心位置** $ \mathbf{r}_i = [x_i, y_i, z_i]^T $（在连杆坐标系下）
- **惯性张量** $ \mathbf{I}_i $（3×3 对称矩阵，包含转动惯量和惯性积）
这些参数统称为**惯性参数**。它们决定了：
- 机械臂在不同姿态下的**重力矩**
- 运动过程中所需的**惯性力**和**科里奥利力**
- 与外部环境交互时的**动态响应**
**质量估计**的目标就是通过实验数据，反推出这些参数。
---
### 2. 数学模型
#### 2.1 完整动力学方程
串联机器人的动力学方程通常表示为：
$$
\boldsymbol{\tau} = \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}}) \dot{\mathbf{q}} + \mathbf{G}(\mathbf{q}) + \boldsymbol{\tau}_{fric}(\dot{\mathbf{q}}) + \boldsymbol{\tau}_{ext}
$$
其中：
- $\mathbf{M}(\mathbf{q})$：质量矩阵（惯性项）
- $\mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})$：科里奥利力和离心力项
- $\mathbf{G}(\mathbf{q})$：重力项
- $\boldsymbol{\tau}_{fric}$：摩擦力矩
- $\boldsymbol{\tau}_{ext}$：外力矩
质量估计就是要确定 $\mathbf{M}(\mathbf{q})$、$\mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})$、$\mathbf{G}(\mathbf{q})$ 中所包含的惯性参数。
#### 2.2 线性化形式
动力学方程对惯性参数是**线性**的，可以写为：
$$
\boldsymbol{\tau} = \mathbf{Y}(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}) \cdot \mathbf{\Phi}
$$
其中：
- $\mathbf{Y}$：回归矩阵，由关节位置、速度、加速度计算得到
- $\mathbf{\Phi}$：基础惯性参数向量（所有待辨识参数）
**基础惯性参数**是经重组后可唯一辨识的最小参数集，通常包括：
- 各连杆质量
- 质心坐标与质量的乘积（$m x, m y, m z$）
- 惯性张量的部分元素组合
#### 2.3 关节空间的参数化
对于第 $i$ 个关节，力矩可表示为：
$$
\tau_i = \sum_{j=1}^{n} \left[ Y_{ij1}(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}) \cdot \phi_{j1} + Y_{ij2} \cdot \phi_{j2} + \dots \right]
$$
典型的基参数如：
- $\phi_{i1} = m_i$（质量）
- $\phi_{i2} = m_i \cdot x_i$（质量 × 质心x）
- $\phi_{i3} = m_i \cdot y_i$
- $\phi_{i4} = m_i \cdot z_i$
- $\phi_{i5} = I_{xx,i} - I_{yy,i}$（惯性张量组合）
- ...
---
### 3. 辨识方法（离线）
#### 3.1 激励轨迹设计
为了使所有参数可辨识，需要设计**持续激励**的轨迹，通常使用：
- **傅里叶级数轨迹**：每个关节的轨迹表示为：
$$
q_i(t) = q_{i0} + \sum_{k=1}^{N} \left[ a_{ik} \sin(\omega_k t) + b_{ik} \cos(\omega_k t) \right]
$$
频率 $\omega_k$ 取为基频的整数倍，避免重复。
- **优化设计**：通过优化算法选择傅里叶系数，使回归矩阵 $\mathbf{Y}$ 的条件数最小，保证数值稳定性。
#### 3.2 数据采集与预处理
1. 控制机械臂沿激励轨迹运动，采集：
- 关节位置 $\mathbf{q}$（编码器）
- 关节速度 $\dot{\mathbf{q}}$（差分或观测器）
- 关节加速度 $\ddot{\mathbf{q}}$（二次差分或观测器，常需滤波）
- 关节力矩 $\boldsymbol{\tau}$（电流估算或力矩传感器）
2. 数据滤波：使用低通滤波器（如Butterworth）去除高频噪声，注意相位延迟补偿。
#### 3.3 线性最小二乘辨识
将采集到的 $N$ 组数据代入线性模型：
$$
\boldsymbol{\tau}_k = \mathbf{Y}_k \cdot \mathbf{\Phi} \quad (k = 1, \dots, N)
$$
构建超定方程组：
$$
\begin{bmatrix} \boldsymbol{\tau}_1 \\ \vdots \\ \boldsymbol{\tau}_N \end{bmatrix}=
\begin{bmatrix} \mathbf{Y}_1 \\ \vdots \\ \mathbf{Y}_N \end{bmatrix}
\cdot \mathbf{\Phi}
$$
使用**加权最小二乘**求解：
$$
\mathbf{\Phi} = (\mathbf{Y}^T \mathbf{W} \mathbf{Y})^{-1} \mathbf{Y}^T \mathbf{W} \boldsymbol{\tau}
$$
其中 $\mathbf{W}$ 为权重矩阵，可根据各关节力矩的信噪比设置。
#### 3.4 摩擦力与惯性力的分离
由于摩擦力也包含在力矩信号中，通常需要同时辨识：
$$
\boldsymbol{\tau} = \mathbf{Y}_{inertia}(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}) \cdot \mathbf{\Phi}_{inertia} + \mathbf{Y}_{fric}(\dot{\mathbf{q}}) \cdot \mathbf{\Phi}_{fric}
$$
通过**组合辨识**，将摩擦力模型参数与惯性参数同时求解。
---
### 4. 在线质量估计
在线估计主要用于**负载质量变化**的场景（如抓取不同重量的工件），实时更新末端质量。
#### 4.1 基于重力矩的估计
当机械臂低速运动（可忽略惯性力）且静止时，关节力矩平衡：
$$
\tau_i = \tau_{grav,i}(q, m_{load}) + \tau_{fric,i}(\dot{q})
$$
通过多姿态测量，利用最小二乘在线拟合负载质量 $m_{load}$。
#### 4.2 基于递推最小二乘（RLS）
构建线性回归模型：
$$
\tau_{measured} - \tau_{grav,robot}(q) = \mathbf{Y}_{load}(q, \dot{q}, \ddot{q}) \cdot m_{load}
$$
使用**递推最小二乘**实时更新：
$$
\hat{m}_{load}(k) = \hat{m}_{load}(k-1) + K(k) [y(k) - \phi^T(k) \hat{m}_{load}(k-1)]
$$
#### 4.3 基于卡尔曼滤波
将负载质量作为状态变量，与机器人状态联合估计：
$$
\mathbf{x} = [\mathbf{q}, \dot{\mathbf{q}}, m_{load}]^T
$$
利用无迹卡尔曼滤波或扩展卡尔曼滤波，融合编码器与力矩数据，实现鲁棒估计。
---
### 5. 质量估计在控制中的作用
| 控制环节 | 质量估计的作用 |
|----------|----------------|
| **重力补偿** | 精确的重力矩计算，消除稳态误差 |
| **惯性前馈** | 利用估计的 $M(q)$ 计算前馈力矩，提升轨迹跟踪精度 |
| **阻抗控制** | 设置 $M_d$ 时参考实际惯量，避免参数不匹配导致的振荡 |
| **导纳控制** | 外力估算时需扣除重力项，质量估计精度直接影响力控精度 |
| **碰撞检测** | 通过估计的惯性参数计算理论力矩，与实际比较，检测外力 |
---
### 6. 工程实现要点
#### 6.1 离线辨识流程
```
设计激励轨迹 → 执行轨迹采集数据 → 数据滤波与预处理 →
构建线性模型 → 最小二乘辨识 → 验证模型精度
```
#### 6.2 负载估计流程（在线）
```
抓取未知负载 → 低速运动 → 采集多组力矩与位置数据 →
RLS更新质量估计 → 更新控制器中的重力项
```
#### 6.3 注意事项
- **可辨识性**：某些惯性参数组合不可独立辨识（如质量与质心耦合），需使用**基础参数**。
- **激励充分性**：轨迹需覆盖足够大的关节空间和速度范围，避免奇异。
- **摩擦力补偿**：若未准确补偿摩擦力，会严重影响质量估计精度。
- **温度影响**：电机常数 $K_t$ 随温度变化，需实时校准或采用关节力矩传感器。
---
### 7. 总结
| 维度 | 内容 |
|------|------|
| **目标** | 估计机器人各连杆质量、质心、惯量，或末端负载质量 |
| **数学基础** | 动力学方程线性化：$\tau = \mathbf{Y} \cdot \Phi$ |
| **离线方法** | 傅里叶激励 + 加权最小二乘 |
| **在线方法** | 递推最小二乘、卡尔曼滤波 |
| **关键难点** | 激励轨迹设计、摩擦分离、参数可辨识性 |
| **控制意义** | 精确的重力补偿、惯性前馈、力控基础 |
精确的质量估计是实现高性能柔顺控制的基石。在实际应用中，通常先通过离线辨识获得机器人本体的惯性参数，再在线估计负载变化，使控制器在抓取不同工件时仍能保持高精度的力控和轨迹跟踪性能。
## 十、零空间控制与导纳控制、阻抗控制
零空间控制与导纳控制、阻抗控制是机器人控制中**不同层面**的概念，它们解决不同的问题，但可以**深度融合**，实现更高级的柔顺行为。简单来说：
- **阻抗/导纳控制**：定义机械臂末端与环境交互的**柔顺行为**（如何响应外力）。
- **零空间控制**：利用冗余机械臂的**内部自由度**，在不影响末端运动的前提下，实现**附加任务**（如避奇异、关节限位、优化姿态）。
二者结合时，阻抗/导纳控制负责末端的“主任务”（力/位交互），零空间控制负责关节空间的“次任务”（内部优化），共同构成完整的控制架构。
---
### 1. 三者各自的定位
| 控制方法 | 控制目标 | 作用域 | 核心思想 |
|----------|----------|--------|----------|
| **阻抗控制** | 定义末端力-位动态关系 | 末端笛卡尔空间 | 通过力/位误差产生力矩，使末端表现期望的刚度/阻尼 |
| **导纳控制** | 定义末端力-位动态关系 | 末端笛卡尔空间 | 测量外力，修正期望位置，间接实现柔顺 |
| **零空间控制** | 利用冗余自由度执行附加任务 | 关节空间（零空间） | 在保证末端主任务不变的前提下，优化关节运动 |
---
### 2. 零空间控制的数学原理
#### 2.1 冗余机械臂
当机械臂的自由度数 $n$ 大于完成任务所需的自由度 $m$（通常 $m=6$ 或更少）时，称为**冗余机械臂**。例如7轴协作机器人（$n=7, m=6$）有1个冗余度。
#### 2.2 速度层面的零空间
末端速度与关节速度的关系：
$$
\dot{x} = J(q) \dot{q}
$$
其中 $J$ 为雅可比矩阵（$m \times n$）。
当 $n > m$ 时，方程有无穷多解。通解为：
$$
\dot{q} = J^+ \dot{x} + (I - J^+ J) \dot{q}_0
$$
其中：
- $J^+$：雅可比伪逆（最小范数解）
- $(I - J^+ J)$：零空间投影矩阵
- $\dot{q}_0$：任意关节速度向量
**关键**：$(I - J^+ J) \dot{q}_0$ 这一项在末端不产生任何速度（因为 $J (I - J^+ J)=0$），因此可以自由选择 $\dot{q}_0$ 来实现附加任务，而不影响末端运动。
#### 2.3 力/力矩层面的零空间（对偶）
根据虚功原理，关节力矩与末端力的关系：
$$
\tau = J^T F
$$
对于冗余机械臂，存在**零空间力矩**：
$$
\tau = J^T F + (I - J^T J^{+T}) \tau_0
$$
其中 $(I - J^T J^{+T}) \tau_0$ 是零空间力矩，它在末端不产生任何净力，只影响关节内部受力分布。
---
### 3. 阻抗/导纳控制与零空间的结合
#### 3.1 核心架构
在实际控制中，阻抗/导纳控制通常用于计算**末端期望力/力矩**，而零空间控制则在此基础上添加**关节空间的附加任务**。整体控制律为：
$$
\tau = \underbrace{J^T F_{imp} + \tau_{grav} + \tau_{fric}}_{\text{主任务（末端阻抗/导纳）}} + \underbrace{(I - J^T J^{+T}) \tau_{null}}_{\text{零空间任务}}
$$
其中：
- $F_{imp}$：阻抗控制计算的末端期望力，或导纳控制生成的末端力指令
- $\tau_{null}$：零空间期望力矩（由附加任务生成）
#### 3.2 典型结合方式
##### （1）阻抗控制 + 零空间
- **主任务**：阻抗控制定义末端刚度/阻尼，实现柔顺交互。
- **零空间任务**：利用冗余度进行：
- **关节限位避障**：当某关节接近限位时，零空间产生远离限位的运动。
- **避奇异**：远离奇异构型。
- **最小化力矩**：优化关节力矩分配。
- **姿态优化**：在人机协作中，调整肘部位置以避免碰撞。
##### （2）导纳控制 + 零空间
- **主任务**：导纳控制通过外力修正末端位置指令，实现柔顺。
- **零空间任务**：由于导纳控制输出的是位置指令，零空间控制通常作用于**速度层**：
$$
\dot{q}_{cmd} = J^+ \dot{x}_{cmd} + (I - J^+ J) \dot{q}_{null}
$$
其中 $\dot{x}_{cmd}$ 由导纳模型生成，$\dot{q}_{null}$ 由附加任务生成。
---
### 4. 工程实现架构
```
┌─────────────────────────────────────────────────────────┐
│ 上层任务规划 │
│ • 期望轨迹 x_r, F_d │
│ • 零空间目标（关节限位、避奇异等） │
└─────────────────────────────────────────────────────────┘
↓
┌──────────────────────────┼──────────────────────────────┐
│ ↓ │
│ ┌─────────────────┐ │ ┌─────────────────┐ │
│ │ 阻抗/导纳控制 │ │ │ 零空间控制 │ │
│ │ (末端柔顺) │ │ │ (关节优化) │ │
│ │ │ │ │ │ │
│ │ F_imp = M_d ë │ │ │ τ_null = ... │ │
│ │ + B_d ė │ │ │ │ │
│ │ + K_d e │ │ │ 或 q̇_null │ │
│ └─────────────────┘ │ └─────────────────┘ │
│ ↓ │ ↓ │
│ └──────────────┼───────────┘ │
│ ↓ │
│ τ_total = J^T F_imp + τ_grav + (I - J^T J^+T) τ_null │
│ 或 │
│ q̇_cmd = J^+ ẋ_cmd + (I - J^+ J) q̇_null │
└─────────────────────────────────────────────────────────┘
↓
底层伺服控制
```
---
### 5. 实例：7轴协作机器人的柔顺搬运
**场景**：7轴机械臂抓取工件，与人协作搬运，同时避免碰撞。
- **主任务（阻抗控制）**：
- 设置末端低刚度（$K_d = 500 N/m$），使机械臂顺从人施加的力。
- 阻抗控制计算末端期望力 $F_{imp}$。
- **零空间任务**：
- **肘部避碰**：检测到肘部靠近障碍物，零空间生成远离障碍物的关节运动。
- **关节限位**：某关节接近限位（如 ±170°），零空间生成反向运动。
- **优化舒适度**：保持肘部在人体侧面，避免阻挡视线。
- **综合控制**：
$$
\tau = J^T F_{imp} + \tau_{grav} + (I - J^T J^{+T}) (\tau_{limit} + \tau_{collision})
$$
末端跟随人力运动（柔顺），同时关节自动调整，避免限位和碰撞。
---
### 6. 关键区别与联系总结
| 维度 | 阻抗/导纳控制 | 零空间控制 |
|------|--------------|------------|
| **控制目标** | 末端力/位柔顺行为 | 关节空间内部优化 |
| **作用空间** | 笛卡尔空间（末端） | 关节零空间 |
| **是否影响末端** | 直接影响末端运动/力 | **不影响**末端运动（理论上） |
| **数学工具** | 二阶阻抗/导纳模型 | 雅可比伪逆、零空间投影 |
| **典型应用** | 人机交互、装配、打磨 | 冗余机器人避奇异、关节限位、避障 |
| **与主任务关系** | 主任务 | 次任务（在主任务零空间内执行） |
---
### 7. 理论联系：分层控制框架
从更宏观的角度，三者可以统一到**分层控制框架**中：
1. **第一层（末端任务层）**：阻抗或导纳控制定义末端行为，生成期望力 $F_{imp}$ 或期望速度 $\dot{x}_{cmd}$。
2. **第二层（零空间任务层）**：在满足末端任务的前提下，利用零空间优化关节运动。
3. **第三层（底层伺服层）**：执行最终的力矩或速度指令。
这种分层架构使得机械臂既能实现柔顺的末端交互，又能充分利用冗余自由度提升操作灵活性和安全性。
---
### 8. 总结
- **阻抗/导纳控制**与**零空间控制**是**互补而非竞争**的关系。
- 阻抗/导纳控制处理**末端与环境交互的柔顺性**（“做什么”）。
- 零空间控制处理**冗余自由度的高效利用**（“如何做得更好”）。
- 二者结合时，通常以阻抗/导纳控制生成的主任务力矩/速度为基础，在其零空间上叠加附加任务的修正量，实现“**柔顺末端 + 灵活关节**”的协同控制。
这种融合架构在现代协作机器人、人形机器人、医疗机器人等领域已成为标准配置，使得机器人在保证安全柔顺交互的同时，还能优化运动性能、规避约束、提升作业效率。
## 十一、拖动示教实现方法
拖动示教（也称直接示教）是指操作人员直接手持机械臂末端，引导其运动，机器人记录运动轨迹，之后可复现。这种操作方式要求机器人处于“柔顺”状态，既能够被人手轻松推动，又能在停止时稳定停留，不产生反冲。
实现拖动示教主要有两种控制方式：**导纳控制** 和 **阻抗控制**，以及工程中常用的 **零力控制**（通常基于电流环+动力学补偿）。下面详细介绍这三种方式的原理、实现方法和特点。
---
### 1. 导纳控制实现拖动示教
导纳控制通过测量外力，修正期望位置，使机械臂表现出“轻”和“顺从”的特性。
#### 1.1 原理
- 检测外力 $F_{ext}$（通过末端力传感器或关节力矩估计）
- 导纳模型将外力转换为位置修正量 $\Delta x$
- 最终指令位置 $x_{cmd} = x_r + \Delta x$
- 在拖动示教中，通常将参考轨迹 $x_r$ 设为当前实际位置（或恒为零），使机械臂完全跟随外力
#### 1.2 核心公式（一维简化）
$$
x_{cmd} = x_{current} + \frac{F_{ext}}{K_d}
$$
其中 $K_d$ 为导纳刚度。**$K_d$ 越小，机械臂越容易被推动**（同样的力产生更大的位移）。
#### 1.3 实现步骤
1. **力检测**：实时读取末端力传感器或估算关节外力矩。
2. **重力补偿**：减去重力对力传感器或电流的影响，使传感器只响应人手的推力。
3. **导纳计算**：根据外力 $F_{ext}$ 和导纳参数 $K_d$（通常也加入阻尼 $B_d$ 提高稳定性），计算位置修正量。
4. **位置指令下发**：将修正后的位置发送给底层位置控制器。
5. **轨迹记录**：在拖动过程中，以固定频率记录关键点或连续路径。
#### 1.4 特点
- **优点**：实现简单，可直接在现有位置控制机器人上叠加；稳定性好。
- **缺点**：响应速度受底层位置环限制；需要精确的力传感器或力矩估计。
---
### 2. 阻抗控制实现拖动示教
阻抗控制通过位置误差产生力矩，使机械臂主动顺应外力，实现拖动示教。
#### 2.1 原理
- 设定期望的阻抗参数（低刚度 $K_d$、适当阻尼 $B_d$、低惯量 $M_d$）
- 实际位置与期望位置的误差 $e = x_r - x$ 通过阻抗模型产生期望力 $F_{imp}$
- 在拖动示教中，通常将期望位置 $x_r$ 设为当前实际位置（即零误差期望），或设为恒定值
- 输出力矩 $\tau = J^T F_{imp} + \tau_{grav} + \tau_{fric}$
#### 2.2 核心公式
$$
F_{imp} = K_d (x_r - x) + B_d (0 - \dot{x}) + M_d (0 - \ddot{x})
$$
当 $x_r$ 设为当前实际位置时，位置误差为零，但速度、加速度误差产生阻尼力和惯性力，使人感觉机械臂有“惯性”和“粘性”。
#### 2.3 实现步骤
1. **设置阻抗参数**：刚度 $K_d$ 设为零或极小值（使位置误差不产生回复力），阻尼 $B_d$ 和惯量 $M_d$ 根据期望的“手感”调整。
2. **重力与摩擦力补偿**：精确补偿重力、科里奥利力和摩擦力，使电机只响应人手施加的外力。
3. **力矩控制**：输出力矩指令，直接驱动电机。
4. **轨迹记录**：同导纳控制。
#### 2.4 特点
- **优点**：动态响应快，无需额外力传感器（依靠电流环+动力学模型）。
- **缺点**：需要开放力矩接口的机器人；对动力学模型精度要求高。
---
### 3. 零力控制（电流环+动力学补偿）
零力控制是阻抗控制的一种工程简化实现，常用于协作机器人（如UR、Franka）的拖动示教模式。
#### 3.1 原理
- 通过电机电流估算关节力矩
- 实时计算并减去重力、摩擦力、惯性力等所需的力矩
- 使关节的净力矩为零，操作者只需克服很小的残余力即可推动机械臂
#### 3.2 核心公式
$$
\tau_{cmd} = \tau_{gravity}(q) + \tau_{friction}(\dot{q}) + \tau_{inertia}(q,\dot{q},\ddot{q})
$$
当控制器输出正好等于这些补偿力矩时，电机不产生额外的驱动力，机械臂处于“零力”状态。操作者施加的外力只需克服剩余的静摩擦和惯量。
#### 3.3 实现步骤
1. **动力学建模**：精确辨识各连杆的质量、质心、惯量，以及摩擦力模型。
2. **实时计算**：根据当前关节位置、速度、加速度，计算重力、摩擦力、惯性力补偿值。
3. **电流指令**：将补偿值转换为电机电流指令，叠加到驱动器。
4. **微调**：根据拖动手感调整补偿参数（如增加摩擦力前馈以减小阻力）。
5. **轨迹记录**：在拖动过程中记录关节角度序列。
#### 3.4 特点
- **优点**：无需额外力传感器；实现成本低；响应快。
- **缺点**：依赖精确的动力学模型；低速时摩擦力补偿不准确，可能有“颗粒感”。
---
### 4. 三种方式对比
| 方式 | 力检测方式 | 输出 | 响应速度 | 硬件要求 | 适用场景 |
|------|-----------|------|----------|----------|----------|
| **导纳控制** | 力传感器或力矩估计 | 位置指令 | 中（受位置环限制） | 有力传感器或可估计外力 | 工业机器人改造、高精度拖动 |
| **阻抗控制** | 无（通过位置误差间接） | 力矩指令 | 快 | 开放力矩接口、高精度模型 | 协作机器人、高性能拖动 |
| **零力控制** | 无（电流环+模型） | 力矩指令 | 快 | 精确动力学模型 | 协作机器人、低成本拖动 |
---
### 5. 工程实现要点
#### 5.1 重力补偿是关键
无论哪种方式，**精确的重力补偿**是实现良好拖动手感的基础。否则，机械臂会因自身重量而下垂或上翘，操作者需要持续用力托举。
- **离线辨识**：通过激励轨迹辨识各连杆质量、质心位置。
- **在线补偿**：实时计算重力矩并叠加到输出。
#### 5.2 摩擦力补偿提升手感
摩擦力补偿可以减少“颗粒感”和“粘滞感”，使拖动更平滑。
- 常用模型：库仑摩擦 + 粘滞摩擦
- 注意低速时静摩擦的补偿，避免过零时的抖动
#### 5.3 参数整定
- **导纳控制**：刚度 $K_d$ 设为 50–200 N/m（越小越轻），阻尼 $B_d$ 设为临界阻尼的 0.5–1 倍。
- **阻抗控制**：刚度 $K_d$ 设为零或很小，阻尼 $B_d$ 设为 10–50 N·s/m，惯量 $M_d$ 设为 0.1–0.5 kg。
- **零力控制**：直接调节补偿系数（如重力补偿比例 0.95–1.05），微调至机械臂“漂浮”感最佳。
#### 5.4 安全保护
- 设置最大速度、最大力矩限制。
- 急停按钮需随时可用。
- 在拖动过程中，实时监控电机电流，防止过载。
---
### 6. 实现流程（以导纳控制为例）
```python
# 初始化
K_d = 100 # 导纳刚度 (N/m)
B_d = 20 # 导纳阻尼 (N·s/m)
x_current = 0 # 当前实际位置
while True:
# 1. 读取外力（传感器或估计）
F_ext = read_force()
# 2. 重力补偿（减去工具重力在传感器坐标系的分量）
F_hand = F_ext - F_gravity_tool(q)
# 3. 导纳模型计算位置修正（离散化）
# 简化为：delta_x = F_hand / K_d
delta_x = F_hand / K_d
# 4. 更新指令位置
x_cmd = x_current + delta_x
# 5. 发送给底层位置控制器
send_position_command(x_cmd)
# 6. 更新当前实际位置
x_current = read_actual_position()
# 7. 记录轨迹点（可选）
record_trajectory_point(x_current)
```
---
### 7. 总结
拖动示教主要通过 **导纳控制**、**阻抗控制** 或 **零力控制** 实现，其核心思想都是让机械臂在操作者施加的外力作用下“顺从”运动，同时消除自身重力、摩擦力的影响。
- **工业机器人改造**：推荐使用导纳控制，配合末端力传感器，叠加在现有位置环上。
- **协作机器人原生实现**：通常使用阻抗控制或零力控制，利用关节电流和动力学模型，无需额外传感器。
- **低成本方案**：零力控制+动力学模型，但需要较精确的模型辨识。
无论哪种方式，精确的重力补偿和合适的参数整定都是实现“轻、滑、稳”拖动手感的关键。