当前位置：首页 > news >正文

机械臂柔顺控制：阻抗控制、导纳控制和力位混合控制

news 来源：原创 2025/5/10 8:19:38

下面详细介绍机械臂的柔顺控制，包括阻抗控制、导纳控制和力位混合控制，并附带代码示例。

什么是柔顺控制？

在传统的机器人控制中，我们通常关注的是精确的位置或轨迹跟踪。然而，在许多与环境交互的任务中（例如装配、打磨、擦拭等），机器人需要具备一定的“柔顺性”，即能够根据外部环境的力/力矩做出适当的响应，而不是一味地追求位置精度。柔顺控制就是为了实现这种与环境的柔顺交互而设计的控制方法。

柔顺控制的核心思想：

不直接控制位置，而是控制力与位置之间的关系。
模拟弹簧-阻尼系统的行为。

1. 阻抗控制 (Impedance Control)

原理： 阻抗控制的核心思想是建立末端执行器位置/姿态与力/力矩之间的动态关系，类似于一个虚拟的弹簧-阻尼系统。
- 目标： 控制机器人末端执行器与环境之间的力/力矩关系，使其表现出期望的阻抗特性（质量、阻尼、刚度）。
- 公式：
```
F = M(x_d - x)'' + D(x_d - x)' + K(x_d - x) 
```
  其中：
  - F 是末端执行器受到的力/力矩。
  - M 是期望的惯性矩阵（质量）。
  - D 是期望的阻尼矩阵。
  - K 是期望的刚度矩阵。
  - x_d 是期望的位置/姿态。
  - x 是实际的位置/姿态。
  - ' 和 '' 分别表示一阶和二阶导数（速度和加速度）。
优点：
- 概念简单，易于理解。
- 可以同时控制力和位置。
缺点：
- 需要精确的机器人动力学模型。
- 对环境的刚度变化比较敏感。
代码示例 (Python, 伪代码):

import numpy as npclass ImpedanceController:def __init__(self, M, D, K):self.M = M  # 惯性矩阵self.D = D  # 阻尼矩阵self.K = K  # 刚度矩阵self.x_d = np.zeros(6)  # 期望位置/姿态 (x, y, z, roll, pitch, yaw)self.x_d_dot = np.zeros(6)  # 期望速度self.x_d_ddot = np.zeros(6)  # 期望加速度def set_desired(self, x_d, x_d_dot=None, x_d_ddot=None):self.x_d = x_dif x_d_dot is not None:self.x_d_dot = x_d_dotif x_d_ddot is not None:self.x_d_ddot = x_d_ddotdef compute_force(self, x, x_dot, x_ddot):# 阻抗控制律F = self.M @ (self.x_d_ddot - x_ddot) + self.D @ (self.x_d_dot - x_dot) + self.K @ (self.x_d - x)return F# 使用示例
M = np.eye(6) * 10  # 示例惯性矩阵
D = np.eye(6) * 50  # 示例阻尼矩阵
K = np.eye(6) * 100  # 示例刚度矩阵controller = ImpedanceController(M, D, K)# 假设我们有一个机器人模型和传感器，可以获取当前位置、速度和加速度
# robot_model = ...
# sensor = ...# 循环控制
while True:# 获取当前状态# x = robot_model.get_position()# x_dot = robot_model.get_velocity()# x_ddot = sensor.get_acceleration()x = np.random.rand(6)x_dot = np.random.rand(6)x_ddot = np.random.rand(6)# 设置期望位置x_d = np.array([1, 0, 0, 0, 0, 0])  # 示例期望位置controller.set_desired(x_d)# 计算控制力F = controller.compute_force(x, x_dot, x_ddot)# 将控制力应用到机器人# robot_model.apply_force(F)print(F)

2. 导纳控制 (Admittance Control)

原理： 导纳控制是阻抗控制的对偶形式。它建立的是力/力矩与位置/姿态之间的动态关系。
- 目标： 根据外部施加的力/力矩，控制机器人末端执行器的运动（位置、速度、加速度），使其表现出期望的导纳特性（质量、阻尼、刚度的倒数）。
- 公式：
```
M_a x'' + D_a x' + K_a x = F_ext
```
  其中：
  - M_a 是期望的导纳矩阵（质量）。
  - D_a 是期望的导纳矩阵（阻尼）。
  - K_a 是期望的导纳矩阵（刚度）。
  - x 是实际的位置/姿态。
  - F_ext 是外部施加的力/力矩。
  - ' 和 '' 分别表示一阶和二阶导数（速度和加速度）。
优点：
- 不需要精确的机器人动力学模型。
- 对环境刚度的变化不敏感。
缺点：
- 难以同时精确控制力和位置。
代码示例 (Python, 伪代码):

import numpy as npclass AdmittanceController:def __init__(self, M_a, D_a, K_a):self.M_a = M_a  # 导纳矩阵 (质量)self.D_a = D_a  # 导纳矩阵 (阻尼)self.K_a = K_a  # 导纳矩阵 (刚度)self.x = np.zeros(6)  # 当前位置/姿态self.x_dot = np.zeros(6)  # 当前速度self.x_ddot = np.zeros(6)self.x_last = np.zeros(6)self.x_dot_last = np.zeros(6)self.dt = 0.01 #积分间隔def compute_motion(self, F_ext):# 导纳控制律 (数值积分实现)# x'' = M_a^-1 (F_ext - D_a x' - K_a x)x_ddot = np.linalg.inv(self.M_a) @ (F_ext - self.D_a @ self.x_dot - self.K_a @ self.x)# 欧拉积分计算速度和位置x_dot = self.x_dot + x_ddot * self.dtx = self.x + x_dot * self.dtself.x_last = self.x.copy()self.x_dot_last = self.x_dot.copy()self.x_dot = x_dotself.x = xreturn x, x_dot, x_ddot# 使用示例
M_a = np.eye(6) * 0.1  # 示例导纳矩阵 (质量)
D_a = np.eye(6) * 0.5  # 示例导纳矩阵 (阻尼)
K_a = np.eye(6) * 1.0  # 示例导纳矩阵 (刚度)controller = AdmittanceController(M_a, D_a, K_a)# 假设我们有一个力传感器可以获取外部施加的力
# force_sensor = ...# 循环控制
while True:# 获取外部力# F_ext = force_sensor.get_force()F_ext = np.random.rand(6)# 计算期望的运动x, x_dot, x_ddot = controller.compute_motion(F_ext)# 将期望的运动发送给机器人底层控制器 (例如位置控制器)# robot_controller.set_target(x, x_dot)print(x)

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

原理： 力位混合控制将任务空间分解为力控制子空间和位置控制子空间，分别进行力和位置的控制。
- 目标： 在某些方向上控制力，在另一些方向上控制位置。
- 方法： 使用选择矩阵 S 来指定哪些方向进行力控制，哪些方向进行位置控制。S 是一个对角矩阵，对角线上的元素为 1 表示力控制，为 0 表示位置控制。
优点：
- 可以适应复杂的任务需求，例如在装配过程中，沿着轴向进行力控制，在垂直于轴向的方向进行位置控制。
缺点：
- 需要对任务进行分解，设计选择矩阵。
- 控制器的设计和调试比较复杂。
代码示例（伪代码）

import numpy as np
class HybridForcePositionController:def __init__(self, S, force_controller, position_controller):self.S = S  # 选择矩阵self.force_controller = force_controller  # 力控制器 (例如阻抗控制器)self.position_controller = position_controller  # 位置控制器 (例如PD控制器)def compute_control(self, x, x_dot, F_ext, x_d, x_d_dot):# 力控制部分F_c = self.S @ self.force_controller.compute_force(x,x_dot, F_ext)# 位置控制部分tau_p = (np.eye(6) - self.S) @ self.position_controller.compute_torque(x, x_dot, x_d, x_d_dot)# 总的控制输出control_output = F_c + tau_preturn control_output
#使用示例
# 假设我们有一个6自由度机械臂，需要在z轴方向进行力控制，其他方向进行位置控制
S = np.diag([0, 0, 1, 0, 0, 0])  # 选择矩阵# 创建力控制器和位置控制器 (这里使用伪代码)
force_controller = ImpedanceController(M=np.eye(6), D=np.eye(6)*10, K=np.eye(6))
position_controller = PDController(Kp=np.eye(6)*100, Kd=np.eye(6)*20)# 创建混合控制器
hybrid_controller = HybridForcePositionController(S, force_controller, position_controller)# 循环控制
while True:x = np.random.rand(6)x_dot = np.random.rand(6)F_ext = np.random.rand(6)x_d = np.random.rand(6)x_d_dot = np.random.rand(6)# 获取当前状态和期望状态# ...# 计算控制输出control_output = hybrid_controller.compute_control(x, x_dot, F_ext, x_d, x_d_dot)# 将控制输出应用到机器人# ...print(control_output)