1. 单自由度阻抗控制基础
任务目标
实现单自由度质量-弹簧-阻尼系统的阻抗控制，使机器人末端呈现期望的刚度和阻尼特性。给定期望位置轨迹，控制器根据接触力偏差调整实际运动，使系统动态行为等效于期望的阻抗模型。
阻抗控制原理
单自由度阻抗控制的目标是让机器人末端呈现期望的动力学特性：
M_d·(ẍ - ẍ_d) + B_d·(ẋ - ẋ_d) + K_d·(x - x_d) = F_ext其中M_d、B_d、K_d分别为期望惯量、阻尼和刚度，x_d为期望位置，F_ext为环境接触力。当F_ext=0时，系统跟踪期望轨迹；当存在接触力时，位置偏差与力满足上述阻抗关系。
控制律推导
对于单自由度系统，若实际动力学为M·ẍ = F_act + F_ext，则阻抗控制律为：
F_act = M·ẍ_d + M·M_d⁻¹·(B_d·(ẋ_d - ẋ) + K_d·(x_d - x) - F_ext) - F_ext当M_d = M时简化为：
F_act = M·ẍ_d + B_d·(ẋ_d - ẋ) + K_d·(x_d - x) - F_ext关键完整代码
import numpy as np
import matplotlib.pyplot as plt
class ImpedanceControl1DOF:
"""单自由度阻抗控制器仿真"""
def __init__(self, M=1.0, B_d=20.0, K_d=100.0, dt=0.001):
self.M = M # 实际质量
self.B_d = B_d # 期望阻尼
self.K_d = K_d # 期望刚度
self.dt = dt
def control(self, x, x_dot, x_d, xd_dot, xdd_dot, F_ext):
"""计算控制力"""
# 阻抗控制律（M_d = M）
F_act = self.M * xdd_dot + self.B_d * (xd_dot - x_dot) \
+ self.K_d * (x_d - x) - F_ext
return F_act
def simulate(self, x0=0.0, xd0=0.0, duration=2.0,
x_d_func=None, F_ext_func=None):
"""仿真主循环"""
dt = self.dt
n_steps = int(duration / dt)
# 状态初始化
x = x0
x_dot = xd0
# 记录轨迹
t_arr = np.arange(0, duration, dt)
x_arr = np.zeros(n_steps)
x_d_arr = np.zeros(n_steps)
F_ext_arr = np.zeros(n_steps)
F_act_arr = np.zeros(n_steps)
for i in range(n_steps):
t = t_arr[i]
# 期望轨迹（默认阶跃到0.1m）
if x_d_func is None:
x_d = 0.1 if t >= 0.1 else 0.0
xd_dot = 0.0
xdd_dot = 0.0
else:
x_d, xd_dot, xdd_dot = x_d_func(t)
# 环境接触力（默认无接触）
if F_ext_func is None:
F_ext = 0.0
else:
F_ext = F_ext_func(t, x)
# 计算控制力
F_act = self.control(x, x_dot, x_d, xd_dot, xdd_dot, F_ext)
# 更新状态（实际动力学：M·ẍ = F_act + F_ext）
xdd = (F_act + F_ext) / self.M
x_dot += xdd * dt
x += x_dot * dt
# 记录
x_arr[i] = x
x_d_arr[i] = x_d
F_ext_arr[i] = F_ext
F_act_arr[i] = F_act
return t_arr, x_arr, x_d_arr, F_ext_arr, F_act_arr
# ===== 仿真验证 =====
if __name__ == "__main__":
# 创建控制器
controller = ImpedanceControl1DOF(M=1.0, B_d=20.0, K_d=100.0, dt=0.001)
# 场景1：自由空间阶跃跟踪（无接触力）
t1, x1, xd1, F1, Fa1 = controller.simulate(duration=2.0)
# 场景2：接触刚性墙壁（x=0.08m处）
def wall_force(t, x):
k_wall = 10000.0
return k_wall * (x - 0.08) if x > 0.08 else 0.0
t2, x2, xd2, F2, Fa2 = controller.simulate(duration=2.0, F_ext_func=wall_force)
# 绘制结果
fig, axes = plt.subplots(2, 2, figsize=(12, 8))
# 场景1：位置跟踪
axes[0,0].plot(t1, xd1, 'r--', label='期望位置')
axes[0,0].plot(t1, x1, 'b-', label='实际位置')
axes[0,0].set_xlabel('时间 (s)')
axes[0,0].set_ylabel('位置 (m)')
axes[0,0].set_title('自由空间阶跃跟踪')
axes[0,0].legend()
axes[0,0].grid(True)
# 场景1：控制力
axes[0,1].plot(t1, Fa1, 'g-')
axes[0,1].set_xlabel('时间 (s)')
axes[0,1].set_ylabel('控制力 (N)')
axes[0,1].set_title('自由空间控制力')
axes[0,1].grid(True)
# 场景2：位置跟踪（含接触）
axes[1,0].plot(t2, xd2, 'r--', label='期望位置')
axes[1,0].plot(t2, x2, 'b-', label='实际位置')
axes[1,0].axhline(y=0.08, color='k', linestyle=':', label='墙壁位置')
axes[1,0].set_xlabel('时间 (s)')
axes[1,0].set_ylabel('位置 (m)')
axes[1,0].set_title('接触刚性墙壁')
axes[1,0].legend()
axes[1,0].grid(True)
# 场景2：接触力
axes[1,1].plot(t2, F2, 'm-')
axes[1,1].set_xlabel('时间 (s)')
axes[1,1].set_ylabel('接触力 (N)')
axes[1,1].set_title('环境接触力')
axes[1,1].grid(True)
plt.tight_layout()
plt.show()
# 输出稳态误差
idx_ss = int(1.5 / controller.dt) # 1.5s后视为稳态
err_free = abs(x1[-1] - xd1[-1])
err_contact = abs(x2[-1] - xd2[-1])
print(f"自由空间稳态误差: {err_free:.6f} m")
print(f"接触墙壁稳态位置: {x2[-1]:.6f} m (期望0.1m)")
print(f"接触力稳态值: {F2[-1]:.2f} N")运行方式
pip install numpy matplotlib
python impedance_control_1dof.py验证方法与判定标准
场景1（自由空间）：期望位置从0阶跃到0.1m，实际位置应平滑跟踪，无超调或少量超调后收敛。稳态误差应趋近于0（由于纯刚性控制，理论上稳态误差为0）。
场景2（接触墙壁）：墙壁位于x=0.08m处，期望位置仍为0.1m。由于墙壁限制，实际位置应稳定在0.08m附近，接触力稳态值应满足：
F_ext = K_d × (x_d - x) = 100 × (0.1 - 0.08) = 2.0 N即接触力稳态值应约为2.0N，验证阻抗关系F = K_d·Δx成立。
通过条件：场景1稳态误差小于0.001m；场景2稳态位置在0.08±0.001m范围内，接触力在2.0±0.1N范围内。
关键参数说明
- K_d（期望刚度）：决定位置偏差与接触力的比例关系，K_d越大，相同力产生的变形越小
- B_d（期望阻尼）：决定系统收敛速度和超调量，B_d过小会导致振荡
- M_d（期望惯量）：本实现中取M_d=M，简化控制律；若M_d≠M需使用完整控制律