机械臂控制02——导纳控制和阻抗控制

Blog

机械臂控制02——导纳控制和阻抗控制

2026-07-17

1. 问题引出

一般无刷电机都具备三种不同的控制模式：

位置模式
速度控制
力矩控制

在具体的机械臂产品上，一般都对外提供了位置控制的接口，也就是说我们的控制器直接给机械臂的各个电机发送位置指令，机械臂即可达到目标位置。

然后我们继续设想一些控制场景，

机械臂在某个位置保持不动
让机械臂往前伸一段距离
让机械臂擦玻璃拿着抹布擦玻璃
…

上述场景基本上简单理解起来只需要计算每个点击需要的角度，并且把位置指令下发到电机就可以了。

但是如果存在外力干扰，则就会变的比较复杂了，比如：

保持不动的时候有人推他了会怎么样？会看谁的力气大？
让机械臂往前移动一段距离时，如果中途遇到了阻碍会怎么样？会把障碍物顶碎还是把机器臂顶坏？
擦玻璃过程中如果玻璃不平，比如有凹陷或者突起，会不会擦不到或者把玻璃顶碎？

基于以上问题，我们需要对我们控制机械臂的过程进行控制上的优化。

优化的目标是：把刚性的机械臂通过控制算法将其模拟成柔性的。

2. 导纳控制

导纳控制的期望目标是：设定一个期望的运动目标之后，如果收到了外界阻力，我希望机械臂变成和弹簧一样的特性，有人推我就让一让，有人拉我就跟着走，这样就可以让机械臂变得很有柔性，而不是硬抗阻力。

导纳控制中一共需要三个参数

刚性
阻尼
质量

最终的计算公式为：

m(a_d - a_e)+d(v_d-v_e)+k(x_d-x_e)=F_d-F_e

其中：

m：质量系数， $a_d$ ：期望加速度， $a_e$ ：实际加速度
d：阻尼系数， $v_d$ ：期望速度， $v_e$ ：实际速度
k：刚性系数， $x_d$ ：期望位置， $x_e$ ：实际位置
Fd：期望受到的外部力，Fe：实际受到的外部力

关于参数的符号：
定义运动方向为正，以下仿真中定义 X 轴正方向为正。
阻力方向和运动方向一致。收到外力阻碍运动，则阻力符号为正；外力利于运动，则外力符号为正；

2.1 刚性

如果不考虑柔顺控制的话，正常机械臂位置模式下基本可以认为是绝对刚性的。也就是说发送目标位置后，想让他偏离目标位置只有一个可能，就是外部使用更大的力强制把机械臂的位置推偏。

这里让机械臂模拟弹簧特性， $F = k * x$ ，其中 F 为弹簧受到的外部力，k 为弹簧的弹性系数，x 为收到外力后弹簧末端相对于原始位置的偏移距离。

也就是

k(x_d-x_e)=F_d-F_e

由此可计算得，机械臂运动过程中实际下发的位置会根据期望位置调整为：

x_e=x_d-(F_d-F_e)/k

使用python对上述公式进行仿真

import numpy as np
import matplotlib.pyplot as plt

# ==================== 中文显示 ====================
plt.rcParams['font.sans-serif'] = ['SimHei']   # 使用黑体显示中文
plt.rcParams['axes.unicode_minus'] = False     # 解决负号'-'显示为方块的问题

# ==================== 仿真设置 ====================
dt = 0.01                                  # 时间步长 (秒)
total_time = 5                             # 总时长 (秒)
steps = int(total_time / dt)               # 仿真点数
time = np.linspace(0, total_time, steps)   # 时间点 (方便画图)

# ==================== 期望轨迹 ====================
v_desired = 0.5               # 期望速度0.5m/s
x_desired = v_desired * time  # 期望轨迹

# ==================== 参数 ====================
K = 100.0                    # 刚度 (Stiffness): 越大，反弹力越强，越像硬弹簧

# ==================== 仿真过程 ====================
force_ext = np.zeros(steps)  # 外力 F_ext
x_dif = np.zeros(steps)      # 外力导致的偏移轨迹
x_ext = np.zeros(steps)      # 实际最终计算的轨迹

# 迭代仿真
for i in range(steps):
    t = time[i]
    
    # 添加外力 (模拟在 t=2s 时撞到墙，受到反向推力)
    if t > 2.0 and t < 2.5:
        force_ext[i] = -50.0  # 施加一个向左的冲击力

    # 核心算法：导纳控制离散化计算
    # 1. 计算外力刚性偏移
    x_dif[i] = (0 - force_ext[i]) / K
    # 2. 最终轨迹 = 期望轨迹 + 外力柔性偏移轨迹
    x_ext[i] = x_desired[i] - x_dif[i]
    
# 画图显示
# ================= 绘图可视化 =================
plt.figure(figsize=(12, 8))

# 图1: 外力输入
plt.subplot(3, 1, 1)
plt.plot(time, force_ext, 'r-', linewidth=2)
plt.title('External Force Input (F_ext)', fontsize=14)
plt.ylabel('Force (N)')
plt.grid(True)
plt.axvline(x=2.0, color='k', linestyle='--', alpha=0.3)

# 图2: 导纳模型内部状态 (偏移量)
plt.subplot(3, 1, 2)
plt.plot(time, x_dif, 'b-', label='Admittance Offset (x_d)', linewidth=2)
plt.title('Admittance Model Response (Offset from Trajectory)', fontsize=14)
plt.ylabel('Position Offset (m)')
plt.legend()
plt.grid(True)

# 图3: 最终结果对比
plt.subplot(3, 1, 3)
plt.plot(time, x_desired, 'g--', label='Original Reference (x_e)', linewidth=2)
plt.plot(time, x_ext, 'm-', label='Final Target (x_e + x_d)', linewidth=3)
plt.title('Final Target Position Generation', fontsize=14)
plt.xlabel('Time (s)')
plt.ylabel('Position (m)')
plt.legend()
plt.grid(True)

plt.tight_layout()
# plt.savefig('output.png')
plt.show()

最终得到的仿真路径如下

可以看到如果施加外力的话，最终计算的轨迹会比期望轨迹有一段固定比例的偏移，可以防止硬抗外部作用力，但是变化过程并不顺滑。

2.2 阻尼

纯刚性系统中，收到外力后，计算的位置的变化几乎是瞬移的，没有运动过程，但是机械臂移动的位置当然不可能会是瞬移过去的。

因此计算位置时，计算的位置要模拟出机械臂在移动过程中收到一点阻力，不能直接瞬间移动的情况，这样移动的路径就会丝滑很多。

添加阻尼项后的公式如下

d(v_d-v_e)+k(x_d-x_e)=F_d-F_e

可以得出速度偏差 $$ d\Delta v+k\Delta x=F_d-F_e

\\Delta v=(F_d-F_e - k*\Delta x)/d $$

再通过速度偏差计算实际的位置

\\\Delta x=\Delta v*d_t=x_d-x_e \\x_e=x_d-\Delta x

使用python对上述公式进行仿真

import numpy as np
import matplotlib.pyplot as plt

# ==================== 中文显示 ====================
plt.rcParams['font.sans-serif'] = ['SimHei']   # 使用黑体显示中文
plt.rcParams['axes.unicode_minus'] = False     # 解决负号'-'显示为方块的问题

# ==================== 仿真设置 ====================
dt = 0.01                                  # 时间步长 (秒)
total_time = 5                             # 总时长 (秒)
steps = int(total_time / dt)               # 仿真点数
time = np.linspace(0, total_time, steps)   # 时间点 (方便画图)

# ==================== 期望轨迹 ====================
v_desired = 0.5               # 期望速度0.5m/s
x_desired = v_desired * time  # 期望轨迹

# ==================== 参数 ====================
K = 100.0                    # 刚度 (Stiffness)
D = 50.0                     # 阻尼 (Damping)

# ==================== 仿真过程 ====================
force_ext = np.zeros(steps)  # 外力 F_ext
x_dif = np.zeros(steps)      # 外力导致的偏移轨迹 (即 ε)
x_ext = np.zeros(steps)      # 实际最终计算的轨迹

eps = 0.0                    # 当前偏移量 (状态变量)

# 迭代仿真
for i in range(steps):
    t = time[i]
    
    # 添加外力 (模拟在 t=2s 时撞到墙，受到反向推力)
    if t > 2.0 and t < 2.5:
        force_ext[i] = -50.0  # 施加一个向左的冲击力

    # 核心算法：导纳控制离散化计算
    # 1. 计算速度偏差 根据 d(v_d-v_e)+k(x_d-x_e)=F_d-F_e
    deps = (0 - force_ext[i] - K * eps) / D
    # 2. 根据速度偏差计算位置偏差
    if i < steps - 1:
        eps += dt * deps
    # 3. 存储位置偏差
    x_dif[i] = eps
    # 4. 计算实际位置 ( 实际位置 = 期望位置 - 位置偏差)
    x_ext[i] = x_desired[i] - eps  # 因为 ε = x_d - x_r，所以 x_r = x_d - ε

# ================= 绘图可视化 =================
plt.figure(figsize=(12, 8))

# 图1: 外力输入
plt.subplot(3, 1, 1)
plt.plot(time, force_ext, 'r-', linewidth=2)
plt.title('External Force Input (F_ext)', fontsize=14)
plt.ylabel('Force (N)')
plt.grid(True)
plt.axvline(x=2.0, color='k', linestyle='--', alpha=0.3)

# 图2: 导纳模型内部状态 (偏移量 ε)
plt.subplot(3, 1, 2)
plt.plot(time, x_dif, 'b-', label='Admittance Offset (ε)', linewidth=2)
plt.title('Admittance Model Response (Offset from Trajectory)', fontsize=14)
plt.ylabel('Position Offset (m)')
plt.legend()
plt.grid(True)

# 图3: 最终结果对比
plt.subplot(3, 1, 3)
plt.plot(time, x_desired, 'g--', label='Original Reference (x_d)', linewidth=2)
plt.plot(time, x_ext, 'm-', label='Final Target (x_d - ε)', linewidth=3)
plt.title('Final Target Position Generation', fontsize=14)
plt.xlabel('Time (s)')
plt.ylabel('Position (m)')
plt.legend()
plt.grid(True)

plt.tight_layout()
# plt.savefig('output.png')
plt.show()

相较于只有刚性参数的图，目标位置变化不会再发生突变了，位置变化已经柔和一点了，但是在力施加和取消的瞬间，轨迹的曲线会突然有个尖峰，也就是速度变化其实会有个突变。

2.3 质量

纯刚性系统：位置会发生突变。因此添加阻尼，不让位置突变。
阻尼系统：速度会发生突变。因此添加质量惯性，不让速度突变。
刚性阻尼质量系统：加速度会发生突变。哈哈哈哈哈哈，但是够用了。

添加质量参数的公式如下：

m(a_d - a_r)+d(v_d-v_r)+k(x_d-x_r)=F_d-F_e

和上面的推导过程一样，

使用python对上述公式进行仿真

import numpy as np
import matplotlib.pyplot as plt

# ==================== 中文显示 ====================
plt.rcParams['font.sans-serif'] = ['SimHei']   # 使用黑体显示中文
plt.rcParams['axes.unicode_minus'] = False     # 解决负号'-'显示为方块的问题

# ==================== 仿真设置 ====================
dt = 0.01                                  # 时间步长 (秒)
total_time = 5                             # 总时长 (秒)
steps = int(total_time / dt)               # 仿真点数
time = np.linspace(0, total_time, steps)   # 时间点 (方便画图)

# ==================== 期望轨迹 ====================
v_desired = 0.5               # 期望速度0.5m/s
x_desired = v_desired * time  # 期望轨迹

# ==================== 参数 ====================
K = 100.0                    # 刚度 (Stiffness)
D = 50.0                     # 阻尼 (Damping)
M = 10.0                     # 质量 (Mass)

# ==================== 仿真过程 ====================
force_ext = np.zeros(steps)  # 外力 F_ext
x_dif = np.zeros(steps)      # 外力导致的偏移轨迹 (即 ε)
x_ext = np.zeros(steps)      # 实际最终计算的轨迹

eps = 0.0                    # 当前位置偏差 (状态变量)
deps = 0.0                   # 当前速度偏差 (状态变量)

# 迭代仿真
for i in range(steps):
    t = time[i]
    
    # 添加外力 (模拟在 t=2s 时撞到墙，受到反向推力)
    if t > 2.0 and t < 2.5:
        force_ext[i] = -50.0  # 施加一个向左的冲击力

    # 核心算法：导纳控制离散化计算 (二阶系统)
    # 根据 M*ddeps + D*deps + K*eps = -F_ext
    # 1. 计算加速度偏差 ddeps
    ddeps = (0 - force_ext[i] - D * deps - K * eps) / M
    # 2. 更新速度偏差 (欧拉积分)
    if i < steps - 1:
        deps += dt * ddeps
        eps  += dt * deps   # 注意：用更新后的 deps 更新 eps
    # 3. 存储位置偏差
    x_dif[i] = eps
    # 4. 计算实际位置 ( 实际位置 = 期望位置 - 位置偏差)
    x_ext[i] = x_desired[i] - eps  # 因为 ε = x_d - x_r，所以 x_r = x_d - ε

# ================= 绘图可视化 =================
plt.figure(figsize=(12, 8))

# 图1: 外力输入
plt.subplot(3, 1, 1)
plt.plot(time, force_ext, 'r-', linewidth=2)
plt.title('External Force Input (F_ext)', fontsize=14)
plt.ylabel('Force (N)')
plt.grid(True)
plt.axvline(x=2.0, color='k', linestyle='--', alpha=0.3)

# 图2: 导纳模型内部状态 (偏移量 ε)
plt.subplot(3, 1, 2)
plt.plot(time, x_dif, 'b-', label='Admittance Offset (ε)', linewidth=2)
plt.title('Admittance Model Response (Offset from Trajectory)', fontsize=14)
plt.ylabel('Position Offset (m)')
plt.legend()
plt.grid(True)

# 图3: 最终结果对比
plt.subplot(3, 1, 3)
plt.plot(time, x_desired, 'g--', label='Original Reference (x_d)', linewidth=2)
plt.plot(time, x_ext, 'm-', label='Final Target (x_d - ε)', linewidth=3)
plt.title('Final Target Position Generation', fontsize=14)
plt.xlabel('Time (s)')
plt.ylabel('Position (m)')
plt.legend()
plt.grid(True)

plt.tight_layout()
# plt.savefig('output.png')
plt.show()

可以看到加上质量参数后，当施加外力时，因为存在惯性，速度之后慢慢提升，因此位置的变化就丝滑的多了。

3. 阻抗控制

机械臂控制01——运动学解算 ThreadX内核移植01