2. 导纳控制基础
任务目标
实现单自由度导纳控制算法：给定机器人末端期望位置 \(x_d\) 与实测外力 \(f_{ext}\)，通过导纳模型计算位置修正量 \(\Delta x\)，使机器人表现出期望的惯性-阻尼-刚度特性。最终验证稳态下外力与位置偏差满足 \(f_{ext} = K \cdot \Delta x\) 的关系。
导纳控制原理
单自由度导纳控制的核心是将机器人等效为质量-阻尼-弹簧系统：
\[ M \cdot \ddot{e} + B \cdot \dot{e} + K \cdot e = f_{ext} \]
其中 \(e = x_d - x_{actual}\) 为位置修正量（即 \(\Delta x\)），\(M\)、\(B\)、\(K\) 分别为期望惯量、阻尼和刚度。当外力恒定且系统达到稳态时，\(\ddot{e}=0\)、\(\dot{e}=0\)，此时力-位置关系简化为：
\[ f_{ext} = K \cdot e \]
本案例采用前向欧拉法对上述二阶微分方程进行离散化数值求解，实时计算位置修正量。
完整实现代码
import numpy as np
import matplotlib.pyplot as plt
class AdmittanceControl1D:
"""单自由度导纳控制器"""
def __init__(self, M=1.0, B=10.0, K=100.0, dt=0.001):
self.M = M # 期望惯量 (kg)
self.B = B # 期望阻尼 (N·s/m)
self.K = K # 期望刚度 (N/m)
self.dt = dt # 控制周期 (s)
self.e = 0.0 # 位置修正量 (m)
self.e_dot = 0.0 # 修正量速度 (m/s)
def reset(self):
"""重置内部状态"""
self.e = 0.0
self.e_dot = 0.0
def update(self, f_ext):
"""
根据外力更新位置修正量
f_ext: 实测外力 (N)，正方向为机器人期望运动方向
返回: 修正后的期望位置 x_cmd = x_d + e
"""
# 由动力学方程解出加速度: e_ddot = (f_ext - B*e_dot - K*e) / M
e_ddot = (f_ext - self.B * self.e_dot - self.K * self.e) / self.M
# 前向欧拉积分
self.e_dot += e_ddot * self.dt
self.e += self.e_dot * self.dt
return self.e
# ========== 验证部分 ==========
if __name__ == "__main__":
# 控制器参数
M, B, K = 1.0, 10.0, 100.0
dt = 0.001
controller = AdmittanceControl1D(M, B, K, dt)
# 仿真参数
sim_time = 5.0 # 仿真时长 (s)
steps = int(sim_time / dt)
f_ext = 20.0 # 恒定外力 (N)
# 记录数据
time_axis = np.arange(steps) * dt
e_history = np.zeros(steps)
# 仿真循环
for i in range(steps):
e_history[i] = controller.update(f_ext)
# 理论稳态值: e_ss = f_ext / K
e_ss_theory = f_ext / K
# 输出结果
print(f"期望刚度 K = {K} N/m")
print(f"施加外力 f_ext = {f_ext} N")
print(f"理论稳态修正量 e_ss = {e_ss_theory:.6f} m")
print(f"仿真最终修正量 e_end = {e_history[-1]:.6f} m")
print(f"稳态误差 = {abs(e_history[-1] - e_ss_theory):.2e} m")
# 绘制响应曲线
plt.figure(figsize=(8, 4))
plt.plot(time_axis, e_history, linewidth=2)
plt.axhline(y=e_ss_theory, color='r', linestyle='--', label=f'理论稳态值 {e_ss_theory:.4f} m')
plt.xlabel('时间 (s)')
plt.ylabel('位置修正量 e (m)')
plt.title('单自由度导纳控制响应')
plt.legend()
plt.grid(True)
plt.tight_layout()
plt.savefig('admittance_response.png', dpi=100)
plt.show()
运行方式
- 安装依赖：
pip install numpy matplotlib - 将上述代码保存为
admittance_1d.py - 在终端执行：
python admittance_1d.py
验证方法与判定标准
程序运行后应观察到：
- 控制台输出中，仿真最终修正量与理论稳态值的差值小于 \(10^{-4}\) m（即稳态误差接近零）；
- 生成的响应曲线显示位置修正量从 0 开始，经过短暂瞬态后收敛到红色虚线标注的理论稳态值；
- 稳态时满足 \(f_{ext} = K \cdot e\)，即 \(20.0 \, \text{N} = 100 \, \text{N/m} \times 0.2 \, \text{m}\)。
若满足以上条件，则说明导纳控制算法实现正确，力-位置关系得到验证。
关键参数说明
- M（惯量）：影响响应速度，M 越小响应越快但可能引入振荡；
- B（阻尼）：影响收敛特性，B 过小会导致超调振荡；
- K（刚度）：决定稳态时外力与位置偏差的比例关系，是力-位置验证的核心参数；
- dt（控制周期）：需满足数值稳定性条件，一般取 1ms 以内。
常见问题
- 稳态误差偏大：检查仿真时长是否足够（建议 ≥ 5s），或增大阻尼 B 加速收敛；
- 数值发散：减小 dt 至 0.0005s 或降低 M 值；
- 外力方向约定：本实现中外力正方向使机器人沿正方向偏移，若实际外力方向相反，需取负值输入。