力控应用

概述

力觉传感的价值最终体现在控制应用中。本节介绍力觉传感在机器人控制中的核心应用：力控抓取、阻抗控制、导纳控制、碰撞检测和力引导装配。

---

力控抓取

抓力调节

目标：施加恰好足够的抓力，既不滑落也不损坏物体。

最小抓力条件（平行夹爪，两指接触）：

\[F_{grip} \geq \frac{F_{load}}{2\mu}\]

其中：

• \(F_{load}\) 为外载荷（通常为物体重力 \(mg\)，加速时为 \(m(g + a)\)）
• \(\mu\) 为手指与物体间的摩擦系数
• 系数 2 因为两个接触面

考虑安全系数：

\[F_{grip} = k_s \cdot \frac{m(g + a_{max})}{2\mu_{min}}\]

典型安全系数 \(k_s = 1.5 \sim 2.0\)。

滑动检测

滑动发生时，力觉传感器会检测到以下特征信号：

基于切向力比：

\[r = \frac{\sqrt{F_x^2 + F_y^2}}{F_z}\]

当 \(r \to \mu\)（接近摩擦锥边界）时，即将发生滑动。

基于振动：

滑动产生高频振动信号（通常 50-500 Hz），可通过触觉传感器的 AC 分量检测：

\[\text{slip\_indicator} = \int_{f_{low}}^{f_{high}} |FFT(F_{tangential}(t))|^2 df > \theta_{slip}\]

抓力自适应控制：

def adaptive_grip_control(F_sensor, F_grip_current):
    """
    基于滑动检测的自适应抓力控制
    """
    F_normal = F_sensor.z
    F_tangential = sqrt(F_sensor.x**2 + F_sensor.y**2)

    # 摩擦比
    friction_ratio = F_tangential / (F_normal + 1e-6)

    # 安全裕度
    safety_margin = mu_estimated - friction_ratio

    if safety_margin < SAFETY_THRESHOLD:
        # 接近滑动，增加抓力
        F_grip_new = F_grip_current + GRIP_INCREMENT
    elif safety_margin > 2 * SAFETY_THRESHOLD:
        # 余量充足，可适当减小抓力（节能/保护物体）
        F_grip_new = F_grip_current - GRIP_DECREMENT
    else:
        F_grip_new = F_grip_current

    return clip(F_grip_new, F_GRIP_MIN, F_GRIP_MAX)

---

阻抗控制（Impedance Control）

基本思想

阻抗控制使机器人末端表现出期望的"弹簧-阻尼器-质量"动态行为：

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

其中：

• \(e = x - x_d\) 为位置误差（当前位置 - 期望位置）
• \(M_d \in \mathbb{R}^{6 \times 6}\) 为期望惯量矩阵
• \(D_d \in \mathbb{R}^{6 \times 6}\) 为期望阻尼矩阵
• \(K_d \in \mathbb{R}^{6 \times 6}\) 为期望刚度矩阵
• \(F_{ext}\) 为外部接触力

物理直觉

参数	增大效果	典型场景
\(K_d\) 大	机器人"硬"，抵抗偏移	精密定位
\(K_d\) 小	机器人"软"，顺从外力	安全交互
\(D_d\) 大	运动被阻尼，响应慢	振动抑制
\(M_d\) 大	惯性大，不易被推动	重载搬运

关节空间阻抗控制

将阻抗关系映射到关节空间：

\[\tau = J^T(q) F_{ext} + g(q) + M(q)\ddot{q}_d + C(q, \dot{q})\dot{q}_d\]

控制律：

\[\tau = g(q) + K_q(q_d - q) + D_q(\dot{q}_d - \dot{q})\]

其中 \(K_q, D_q\) 为关节空间刚度和阻尼矩阵。

笛卡尔空间阻抗控制

更直觉——直接在任务空间设定阻抗行为：

\[\tau = J^T(q) [K_d(x_d - x) + D_d(\dot{x}_d - \dot{x})] + g(q)\]

需要注意：

• 需要精确的运动学模型（\(J(q)\), \(x = f(q)\)）
• 动力学补偿（重力 \(g(q)\)）提高性能
• 奇异性附近需要特殊处理

变刚度控制

根据任务需求在线调整刚度：

\[K_d(t) = \begin{cases} K_{high} & \text{自由空间运动} \\ K_{low} & \text{接触状态} \end{cases}\]

平滑切换：

\[K_d(t) = K_{low} + (K_{high} - K_{low}) \cdot \sigma(-\alpha \|F_{ext}\|)\]

其中 \(\sigma\) 为 sigmoid 函数。

---

导纳控制（Admittance Control）

与阻抗控制的区别

方面	阻抗控制	导纳控制
输入	位置偏差 → 输出力	外力 → 输出位置偏差
因果关系	\(F = Z \cdot v\)（力 = 阻抗 × 速度）	\(v = Y \cdot F\)（速度 = 导纳 × 力）
适用场景	力矩可控的关节	位置控制的机器人
传感器需求	可不需要力传感器	必须有力传感器
内环	力矩控制	位置/速度控制

导纳控制器

力传感器测量外力 \(F_{ext}\)，导纳控制器计算位置修正：

\[M_d \ddot{x}_{adj} + D_d \dot{x}_{adj} + K_d x_{adj} = F_{ext}\]

然后将修正量叠加到期望轨迹：

\[x_{cmd} = x_{desired} + x_{adj}\]

class AdmittanceController:
    def __init__(self, M, D, K, dt):
        self.M = M  # 期望惯量
        self.D = D  # 期望阻尼
        self.K = K  # 期望刚度
        self.dt = dt
        self.x_adj = np.zeros(6)
        self.dx_adj = np.zeros(6)

    def update(self, F_ext, x_desired):
        # 导纳动力学: M * ddx + D * dx + K * x = F_ext
        ddx_adj = np.linalg.solve(self.M,
            F_ext - self.D @ self.dx_adj - self.K @ self.x_adj)

        # 积分
        self.dx_adj += ddx_adj * self.dt
        self.x_adj += self.dx_adj * self.dt

        # 叠加到期望位置
        x_cmd = x_desired + self.x_adj
        return x_cmd

应用：人机协作搬运

人推动机器人末端，机器人顺从移动：

• \(K_d = 0\)（无弹簧回复力，自由移动）
• \(D_d\) 适中（阻尼提供稳定性和"重量感"）
• \(M_d\) 小（惯量小，轻松推动）

\[D_d \dot{x}_{adj} = F_{ext} \quad \Rightarrow \quad \dot{x}_{adj} = \frac{F_{ext}}{D_d}\]

这就是最简单的导纳控制——人施力，机器人按比例运动。

---

碰撞检测

基于动量观测器

动量观测器（Momentum Observer）是一种无需外部力传感器的碰撞检测方法（但有力传感器更准确）。

广义动量：

\[p = M(q)\dot{q}\]

动量变化率：

\[\dot{p} = M(q)\ddot{q} + \dot{M}(q)\dot{q} = \tau + J^T F_{ext} - C(q,\dot{q})\dot{q} - g(q) + \dot{M}\dot{q}\]

利用性质 \(\dot{M} - 2C\) 为反对称矩阵：

\[\dot{p} = \tau + J^T F_{ext} + C^T(q,\dot{q})\dot{q} - g(q)\]

观测器：

\[\hat{r}(t) = K_I \left[ p(t) - p(0) - \int_0^t (\tau + C^T \dot{q} - g + \hat{r}) d\tau' \right]\]

其中 \(K_I > 0\) 为观测器增益。稳态时：

\[\hat{r} \to J^T F_{ext} = \hat{\tau}_{ext}\]

即观测器输出收敛到外部力矩。

碰撞判断

\[\|\hat{\tau}_{ext}\| > \tau_{threshold} \quad \Rightarrow \quad \text{碰撞检测触发}\]

碰撞响应策略：

级别	条件	响应
警告	\(\|\hat{\tau}_{ext}\| > \tau_1\)	减速
软停止	\(\|\hat{\tau}_{ext}\| > \tau_2\)	停止运动
紧急停止	\(\|\hat{\tau}_{ext}\| > \tau_3\)	切断电机驱动
回退	软停止后	沿碰撞方向后退

提升灵敏度

碰撞检测的灵敏度取决于：

1. 模型精度：摩擦模型不准 → 残差大 → 灵敏度低
2. 传感器噪声：编码器噪声 → 阈值不能设太低
3. 采样率：越高越好（1 kHz 以上推荐）
4. 滤波：低通滤波减少误报但增加延迟

---

力引导装配

轴孔装配（Peg-in-Hole）

经典的力控装配任务，六维力矩传感器的杀手级应用。

挑战：

• 间隙小（可能仅 0.01-0.1 mm）
• 纯位置控制无法完成（定位精度不够）
• 需要力反馈来引导对准

三阶段策略：

阶段 1：接近与搜索

螺旋搜索，寻找孔的位置：

\[x(t) = x_0 + r(t)\cos(\omega t), \quad y(t) = y_0 + r(t)\sin(\omega t)\]

\[r(t) = r_0 + v_r \cdot t\]

同时施加恒定的下压力 \(F_z = F_{search}\)。

检测条件：当 \(F_z\) 突然减小（轴进入孔口）→ 进入下一阶段。

阶段 2：对准

根据力矩信号调整姿态：

\[\Delta\theta_x = -K_\theta \cdot M_y, \quad \Delta\theta_y = K_\theta \cdot M_x\]

力矩 \(M_x, M_y\) 反映了轴与孔的角度偏差。

收敛条件：\(\sqrt{M_x^2 + M_y^2} < M_{tolerance}\)

阶段 3：插入

保持姿态，沿 Z 方向施加恒力：

\[F_z = F_{insert} = \text{const}\]

同时横向力归零（导纳控制）：

\[\dot{x}_{adj} = \frac{1}{D} F_x, \quad \dot{y}_{adj} = \frac{1}{D} F_y\]

完成条件：\(z > z_{target}\) 或 \(F_z > F_{max}\)（卡死）。

表面跟踪

力传感器使机器人能够沿未知表面移动，保持恒定接触力：

\[\text{法向}: \quad F_n = F_{desired} \quad \text{(力控)}\]

\[\text{切向}: \quad v_t = v_{desired} \quad \text{(速度控)}\]

混合力/位置控制：

\[\tau = J^T [S_f K_f (F_d - F) + S_p K_p (x_d - x)]\]

其中：

• \(S_f = \text{diag}(0,0,1,0,0,0)\) — 力控方向选择矩阵
• \(S_p = I - S_f\) — 位置控方向选择矩阵
• 法向（z）使用力控，切向（x, y）使用位置控

螺钉拧紧

力矩控制的典型应用：

1. 自由拧入阶段：速度控制，力矩较小
2. 贴合阶段：力矩开始上升
3. 拧紧阶段：力矩控制，达到目标力矩

\[M_z(t) \to M_{target} \quad \text{(e.g., 5 N·m)}\]

过拧保护：\(M_z > M_{max} \Rightarrow \text{停止}\)

---

力控 + 学习

学习阻抗参数

传统方法需要手动调节 \(K_d, D_d, M_d\)，强化学习可以自动学习：

\[\pi(s) = \{x_d, K_d, D_d\}\]

状态 \(s\) 包含位置、速度、力信息。

残差力学习

在模型基础控制上叠加学习的力修正：

\[F_{total} = F_{model}(q, \dot{q}, \ddot{q}) + F_{residual}^{NN}(q, \dot{q}, F_{ext})\]

---

相关内容

• 控制理论 — 控制理论基础
• 六维力矩传感器 — 力矩传感器硬件
• 力觉传感综述 — 力觉传感全貌

---

参考资源

• Siciliano, B. et al., Robotics: Modelling, Planning and Control, Ch. 9
• Hogan, N., "Impedance Control: An Approach to Manipulation," 1985
• De Luca, A. et al., "Collision Detection and Safe Reaction with the DLR-III Lightweight Manipulator Arm," IROS, 2006
• Inoue, T. et al., "Robotic Peg-in-Hole Assembly: State of the Art Review," Advanced Robotics, 2017

---

评论