impedance-control

name	impedance-control
description	机械臂阻抗控制技能 - 位置控制、力控制、混合力位控制、柔顺控制、碰撞检测
argument-hint	机械臂力控 OR 阻抗控制 OR 力位混合 OR 柔顺控制 OR impedance control
user-invocable	true

机械臂阻抗控制技能

用于实现机械臂的柔顺力控制，涵盖阻抗控制、力位混合控制、碰撞检测和力矩传感器集成

---

何时使用

当需要以下帮助时使用此技能：

• 实现机械臂与环境交互的柔顺控制
• 力控磨削、插拔、装配任务
• 碰撞检测和安全控制
• 混合力位控制（Hybrid Force/Position Control）
• 关节力矩传感器标定

---

快速参考

控制模式对比

机器人控制模式:
├── 位置控制 (Position) → 精确轨迹，跟踪误差小
├── 速度控制 (Velocity) → 响应快，可与力控嵌套
├── 力矩控制 (Torque) → 直接力输出，刚性最低
└── 阻抗控制 (Impedance) ← 本技能重点
    └── 末端力 ↔ 末端速度的动态关系

核心方程

阻抗控制: F = M(ddX) + B(dX) + K(X - Xd)

其中:
- M: 目标惯性矩阵 (Desired Inertia)
- B: 阻尼矩阵 (Damping)
- K: 刚度矩阵 (Stiffness)
- Xd: 期望位置
- X: 实际位置
- F: 末端接触力

ROS2 依赖

sudo apt install -y ros-humble-controller-manager
sudo apt install -y ros-humble-joint-trajectory-controller
sudo apt install -y ros-humble-effort-controllers

---

阻抗控制原理

阻抗控制核心

import numpy as np
from dataclasses import dataclass


@dataclass
class ImpedanceParams:
    """阻抗控制器参数"""
    M: np.ndarray  # 目标惯性矩阵 6x6
    B: np.ndarray  # 阻尼矩阵 6x6
    K: np.ndarray  # 刚度矩阵 6x6


class ImpedanceController:
    """
    阻抗控制器

    将末端执行器的期望动态特性建模为二阶系统
    适用于: 装配、磨削、插拔等接触任务
    """

    def __init__(self, params: ImpedanceParams):
        self.M = params.M
        self.B = params.B
        self.K = params.K

        # 状态
        self.Xd = np.zeros(6)      # 期望位置/姿态
        self.X = np.zeros(6)        # 实际位置/姿态
        self.Xd_dot = np.zeros(6)   # 期望速度
        self.X_dot = np.zeros(6)    # 实际速度
        self.F_ext = np.zeros(6)    # 外部力/力矩

        # 积分项（可选，用于消除静差）
        self.X_error_integral = np.zeros(6)

    def compute_torque(
        self,
        X: np.ndarray,
        X_dot: np.ndarray,
        F_ext: np.ndarray,
        Xd: np.ndarray,
        Xd_dot: np.ndarray,
        Xd_ddot: np.ndarray,
        dt: float
    ) -> np.ndarray:
        """
        计算末端期望力矩

        Args:
            X: 当前末端位姿 (x, y, z, roll, pitch, yaw)
            X_dot: 当前末端速度
            F_ext: 外部接触力（从力矩传感器获取）
            Xd: 期望位姿
            Xd_dot: 期望速度
            Xd_ddot: 期望加速度
            dt: 控制周期

        Returns:
            tau: 关节力矩命令 (n_joints,)
        """
        self.X = X
        self.X_dot = X_dot
        self.F_ext = F_ext
        self.Xd = Xd
        self.Xd_dot = Xd_dot

        # 位置误差
        X_error = Xd - X

        # 积分项（抗静差）
        self.X_error_integral += X_error * dt
        # 积分限幅
        max_integral = 0.05
        self.X_error_integral = np.clip(
            self.X_error_integral, -max_integral, max_integral
        )

        # 阻抗方程: M(ddX_r) + B(dX_r) + K(X_r) = F_ext
        # 其中 X_r = Xd - X（位置误差）
        X_r = X_error
        X_r_dot = Xd_dot - X_dot

        # 期望加速度（从误差计算）
        # M * X_r_ddot + B * X_r_dot + K * X_r = F_ext
        # => X_r_ddot = M^-1 * (F_ext - B * X_r_dot - K * X_r)
        try:
            X_r_ddot = np.linalg.inv(self.M) @ (
                F_ext - self.B @ X_r_dot - self.K @ X_r
            )
        except np.linalg.LinAlgError:
            X_r_ddot = np.zeros(6)

        # 实际末端期望加速度
        X_ddot_desired = Xd_ddot + X_r_ddot

        # 转换为关节空间力矩（需要雅可比矩阵）
        # tau = J^T * F_desired
        # 这里假设已在关节空间直接计算

        return np.zeros(6)  # 占位，需外部提供 Jacobian


class JointImpedanceController:
    """
    关节空间阻抗控制
    （更常用，计算更简单）
    """

    def __init__(
        self,
        n_joints: int,
        Kp: np.ndarray = None,
        Kd: np.ndarray = None
    ):
        self.n = n_joints
        # 关节空间 PD 增益（等价于阻抗）
        self.Kp = Kp if Kp is not None else np.diag([50] * n_joints)
        self.Kd = Kd if Kd is not None else np.diag([10] * n_joints)

        self.q_desired = np.zeros(n_joints)
        self.q_dot_desired = np.zeros(n_joints)
        self.tau_ext = np.zeros(n_joints)  # 外部力矩

    def compute(
        self,
        q: np.ndarray,
        q_dot: np.ndarray,
        q_ddot: np.ndarray,
        q_desired: np.ndarray,
        tau_ext: np.ndarray
    ) -> np.ndarray:
        """
        计算关节力矩命令

        Args:
            q: 当前关节位置
            q_dot: 当前关节速度
            q_ddot: 当前关节加速度
            q_desired: 期望关节位置
            tau_ext: 外部力矩（重力补偿后的净力矩）

        Returns:
            tau_cmd: 力矩命令
        """
        # 误差
        q_error = q_desired - q
        q_dot_error = self.q_dot_desired - q_dot

        # 阻抗控制力矩 = Kp * error + Kd * derror + tau_ext
        tau_cmd = self.Kp @ q_error + self.Kd @ q_dot_error + tau_ext

        # 输出限幅（保护关节）
        max_torque = 100.0  # Nm
        tau_cmd = np.clip(tau_cmd, -max_torque, max_torque)

        return tau_cmd

---

混合力位控制 (Hybrid Force/Position Control)

Mason 1981 框架

import numpy as np


class HybridForcePositionController:
    """
    混合力位控制器 (Hybrid Force/Position Control)

    在不同方向上同时进行力控制和位置控制
    典型应用: 擦拭平面（法向力控，平面内位置控制）
    """

    def __init__(self, n_joints: int):
        self.n = n_joints

        # 选择矩阵 (Task Space Selection Matrix)
        # S[i] = 1 表示沿该方向力控制
        # S[i] = 0 表示沿该方向位置控制
        self.S = np.zeros(6)  # [Fx, Fy, Fz, Mx, My, Mz]

        # 位置控制器
        self.Kp_pos = np.diag([20] * 6)
        self.Kd_pos = np.diag([5] * 6)

        # 力控制器
        self.Kp_force = np.diag([5] * 6)
        self.Ki_force = np.diag([0.5] * 6)
        self.Kd_force = np.diag([0.1] * 6)

        # 积分状态
        self.force_integral = np.zeros(6)

        # 期望力
        self.F_desired = np.zeros(6)
        self.X_desired = np.zeros(6)

    def set_force_control_directions(self, directions: list):
        """设置力控制方向"""
        .S = np.zeros()
         d  directions:
            .S[d] = 

     ():
        
         d  directions:
             .S[d] == :
                  

     () -> np.ndarray:
        
        
        X_error = Xd - X
        X_dot_error = -X_dot  
        F_pos = .Kp_pos @ X_error + .Kd_pos @ X_dot_error

        
        F_error = Fd - F_ext
        .force_integral += F_error * dt
        
        .force_integral = np.clip(.force_integral, -, )
        F_force = .Kp_force @ F_error + .Ki_force @ .force_integral

        
        S_pos =  - .S  
        F_cmd = np.zeros()
         i  ():
             .S[i] > :
                F_cmd[i] = F_force[i]  
            :
                F_cmd[i] = F_pos[i]     

         F_cmd

---

ROS2 力矩控制节点

力矩控制器

#!/usr/bin/env python3
"""机械臂力矩控制节点"""

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from geometry_msgs.msg import WrenchStamped
import numpy as np


class TorqueControlNode(Node):
    def __init__(self):
        super().__init__('torque_control_node')

        # 参数
        self.declare_parameter('n_joints', 6)
        self.declare_parameter('control_mode', 'impedance')  # impedance / torque
        self.n = self.get_parameter('n_joints').value

        # 阻抗参数
        self.Kp = np.diag([30.0] * self.n)
        self.Kd = np.diag([8.0] * self.n)

        # 关节状态
        self.q = np.zeros(self.n)
        self.q_dot = np.zeros(self.n)
        self.tau_ext = np.zeros(self.n)

        # 目标
        self.q_desired = np.zeros(.n)
        .tau_desired = np.zeros(.n)

        
        .joint_state_sub = .create_subscription(
            JointState,
            ,
            .joint_state_callback,
            
        )
        .ft_sub = .create_subscription(
            WrenchStamped,
            ,
            .ft_callback,
            
        )
        .target_sub = .create_subscription(
            JointState,
            ,
            .target_callback,
            
        )

        
        .tau_pub = .create_publisher(
            JointState,
            ,
            
        )

        
        .timer = .create_timer(, .control_loop)  

        .get_logger().info()

     ():
         (msg.position) >= .n:
            .q = np.array(msg.position[:.n])
         (msg.velocity) >= .n:
            .q_dot = np.array(msg.velocity[:.n])

     ():
        
        .tau_ext[] = msg.wrench.torque.x
        .tau_ext[] = msg.wrench.torque.y
        .tau_ext[] = msg.wrench.torque.z
        

     ():
         (msg.position) >= .n:
            .q_desired = np.array(msg.position[:.n])

     ():
        
        
        q_error = .q_desired - .q
        q_dot_error = -.q_dot

        
        tau_cmd = .Kp @ q_error + .Kd @ q_dot_error - .tau_ext * 

        
        cmd = JointState()
        cmd.header.stamp = .get_clock().now().to_msg()
        cmd.name = [  i  (.n)]
        cmd.effort = tau_cmd.tolist()
        .tau_pub.publish(cmd)

---

力矩传感器标定

重力补偿 + 零点标定

class ForceTorqueCalibrator:
    """力矩传感器标定"""

    def __init__(self, n_joints: int = 6):
        self.n = n_joints
        self.wrench_bias = np.zeros(6)  # 零点偏移
        self.wrench_transform = np.eye(6)  # 坐标系变换矩阵
        self.gravity_direction = np.array([0, 0, -9.81])

    def calibrate_zero(self, samples: int = 100):
        """
        零点标定 - 机器人处于无负载姿态时采集

        Args:
            samples: 采样次数
        """
        raw_data = []

        for _ in range(samples):
            # 读取当前力矩
            wrench = self.read_wrench()
            raw_data.append(wrench)

        raw_data = np.array(raw_data)

        # 取中位数作为零点（抗异常值）
        self.wrench_bias = np.median(raw_data, axis=0)

        return self.wrench_bias

    def calibrate_transform(
        self,
        joint_positions: list,
        wrench_readings: list
    ):
        """
        标定坐标系变换

        使用最小二乘法求解变换矩阵
        关节力矩 = transform @ wrench + bias
        """
        n = len(joint_positions)
        A = np.zeros((n * 6, 42))  
        b = np.zeros(n * )

         i, (q, w)  ((joint_positions, wrench_readings)):
            
            A[i*:(i+)*, :] = ._build_regression_row(w)
            b[i*:(i+)*] = q

        
        x, _, _, _ = np.linalg.lstsq(A, b, rcond=)

        
        .wrench_transform = x.reshape(, )
        .wrench_bias = x.reshape(, )[:, ]

         .wrench_transform

     () -> np.ndarray:
        
        
        wrench = raw_wrench - .wrench_bias

        
        joint_torque = .wrench_transform @ np.append(wrench, )

         joint_torque

---

碰撞检测

class CollisionDetector:
    """关节空间碰撞检测"""

    def __init__(self, n_joints: int):
        self.n = n_joints

        # 碰撞阈值（根据正常操作力矩设定）
        self.torque_threshold = np.array([30.0] * n_joints)  # Nm

        # 碰撞标志
        self.collision_detected = False

        # 碰撞前的正常力矩估计（EMA）
        self.tau_normal_estimate = np.zeros(n_joints)
        self.alpha = 0.95  # EMA 平滑因子

    def detect(
        self,
        tau_cmd: np.ndarray,
        tau_actual: np.ndarray
    ) -> bool:
        """
        检测是否发生碰撞

        Args:
            tau_cmd: 发送的力矩命令
            tau_actual: 实际测量的力矩

        Returns:
            True if collision detected
        """
        # 计算力矩残差
        tau_residual = np.abs(tau_actual - tau_cmd)

        # 更新正常力矩估计
        self.torque_normal_estimate = (
            self.alpha * self.torque_normal_estimate +
            (1 - self.alpha) * tau_cmd
        )

        # 动态阈值：正常力矩的 2 倍或固定阈值
        dynamic_threshold = np.maximum(
            2.0 * np.abs(self.torque_normal_estimate),
            self.torque_threshold
        )

        # 碰撞判定：残差超过阈值
        self.collision_detected = np.(tau_residual > dynamic_threshold)

         .collision_detected

     () -> np.ndarray:
        
        
         np.sign(tau_residual)


 :
    

     ():
        .collision_joint = -
        .collision_direction = 

     ():
        
        
         

        
         

     () -> np.ndarray:
        
         q_dot *   

---

ROS2 配置

Controllers YAML

effort_controllers:
  ros__parameters:
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
    command_interfaces:
      - effort
    state_interfaces:
      - position
      - velocity
      - effort

impedance_controller:
  ros__parameters:
    n_joints: 6
    Kp: [30.0, 30.0, 25.0, 20.0, 15.0, 10.0]
    Kd: [8.0, 8.0, 6.0, 5.0, 4.0, 3.0]
    max_torque: [100.0, 100.0, 80.0, 50.0, 30.0, 30.0]

Launch

# launch/torque_control.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        Node(
            package='controller_manager',
            executable='spawner',
            arguments=['effort_controller'],
        ),
        Node(
            package='torque_control',
            executable='torque_control_node',
            name='torque_control_node',
            parameters=[{
                'n_joints': 6,
                'control_mode': 'impedance',
            }],
        ),
    ])

---

故障排查

问题	原因	解决方案
机械臂振荡	Kp/Kd 增益过高	减小 Kd，或增加 Kp
响应迟缓	刚度太低	增加 Kp，降低阻尼比
力控制不稳	力传感器噪声大	增加低通滤波，减小微分项
碰撞后不停止	阈值设置不当	标定正常力矩，重新设置阈值
启动抖动	初始误差大	增加启动缓冲，渐进达到目标
关节异响	力矩超限摩擦	减小最大力矩限幅

调试命令

# 监听关节力矩
ros2 topic echo /joint_states

# 监听力矩传感器
ros2 topic echo /ft_sensor/wrench

# 发送力矩命令测试
ros2 topic pub /joint_effort_controller/commands std_msgs.msg/Float64MultiArray \
  'data: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]' --once

# 录制数据进行后分析
ros2 bag record /joint_states /ft_sensor/wrench -o torque_data

---

相关技能

• manipulator/motion-control — 机械臂运动控制基础
• manipulator/motion-control/grasp-planning — 抓取规划
• manipulator/motion-control/trajectory — 轨迹规划
• manipulator/force-control/force-position-hybrid — 力位混合控制
• manipulator/force-control/impedance-control — 阻抗控制（本文）
• manipulator/force-control — 力控技能目录