4、一维导纳控制器实现：离散化方法
好，咱们接着往下走。上一节我们把一维导纳的连续域模型讲清楚了，说白了就是那个经典的二阶微分方程：
M * ẍ_d + B * ẋ_d + K * x_d = F_ext但问题来了——我们的控制器是跑在数字芯片上的，不是模拟电路。你没法直接解微分方程，得把它变成差分方程。这个过程，就叫离散化。
我个人习惯把离散化比作「翻译」——把连续时间的语言，翻译成离散时间能听懂的话。翻译得好不好，直接决定了你的机器人动起来是丝滑还是抽搐。
4.1 三种离散化方法
离散化方法有很多，但做导纳控制，你掌握这三种就够了：前向欧拉、后向欧拉、双线性变换（也叫Tustin变换）。
咱们先看一个通用思路。假设我们有一个连续传递函数：
G(s) = X(s) / F(s) = 1 / (M*s² + B*s + K)我们要把它变成离散的 G(z)。怎么做？用不同的近似公式把 s 替换掉。
| 方法 | 替换公式 | 特点 |
|---|---|---|
| 前向欧拉 | s ≈ (z - 1) / T | 简单，但可能不稳定 |
| 后向欧拉 | s ≈ (z - 1) / (T * z) | 稳定，精度一般 |
| 双线性变换 | s ≈ (2/T) * (z - 1) / (z + 1) | 精度最高，最常用 |
其中 T 是采样周期。嗯，这里要注意：采样周期选多大，直接影响离散化效果。我见过有人用1kHz的采样率去做力控，结果抖得不行——说白了就是T太大了，离散化误差累积到不可接受。
4.2 前向欧拉法
前向欧拉是最直观的方法。你把导数近似成前向差分：
ẋ(t) ≈ (x[k+1] - x[k]) / T
ẍ(t) ≈ (x[k+1] - 2*x[k] + x[k-1]) / T²代入导纳方程，整理后得到：
x[k+1] = (2*M + B*T) / (M + B*T + K*T²) * x[k]
- M / (M + B*T + K*T²) * x[k-1]
+ T² / (M + B*T + K*T²) * F_ext[k]
注意：前向欧拉法有条件稳定。如果你的系统刚度K很大，或者采样周期T偏大，很容易发散。我曾经在一个高刚度场景下试过，结果位置直接飞出去了……嗯，从那以后我再也不敢在高刚度下用前向欧拉。
4.3 后向欧拉法
后向欧拉用的是后向差分：
ẋ(t) ≈ (x[k] - x[k-1]) / T
ẍ(t) ≈ (x[k] - 2*x[k-1] + x[k-2]) / T²代入后得到：
x[k] = (2*M + B*T) / (M + B*T + K*T²) * x[k-1]
- M / (M + B*T + K*T²) * x[k-2]
+ T² / (M + B*T + K*T²) * F_ext[k]后向欧拉最大的好处是——无条件稳定。不管你参数怎么选，它都不会发散。但代价是精度稍微差一点，尤其在高频段会有相位滞后。
4.4 双线性变换法（Tustin）
双线性变换是我个人最推荐的方法。它用梯形近似代替矩形近似，精度高得多。
替换公式是：
s = (2/T) * (z - 1) / (z + 1)代入导纳传递函数，经过一顿代数运算（我就不展开了，你直接用结果就行），得到：
x[k] = (4*M/T² - K) / (4*M/T² + 2*B/T + K) * x[k-1]
+ (2*B/T - 4*M/T²) / (4*M/T² + 2*B/T + K) * x[k-2]
+ (1/T²) / (4*M/T² + 2*B/T + K) * (F_ext[k] + 2*F_ext[k-1] + F_ext[k-2])
核心建议：如果你的控制周期在1ms以上（即采样频率≥1kHz），优先用双线性变换。它兼顾了稳定性和精度，是工业界最常用的离散化方法。
4.5 一维导纳控制器伪代码
好，理论讲完了，咱们看看代码怎么写。下面是用双线性变换法的一维导纳控制器伪代码：
// 一维导纳控制器 - 双线性变换法
// 输入：外力 F_ext，上一时刻状态
// 输出：期望位置 x_des
// 参数初始化
M = 1.0 // 惯性
B = 10.0 // 阻尼
K = 100.0 // 刚度
T = 0.001 // 采样周期 1ms
// 计算系数
a0 = 4*M/T² + 2*B/T + K
a1 = (4*M/T² - K) / a0
a2 = (2*B/T - 4*M/T²) / a0
b0 = 1 / (T² * a0)
// 状态变量
x_prev1 = 0 // x[k-1]
x_prev2 = 0 // x[k-2]
f_prev1 = 0 // F_ext[k-1]
f_prev2 = 0 // F_ext[k-2]
// 主循环
while (true) {
// 读取当前外力
f_curr = readForceSensor()
// 计算期望位置
x_curr = a1 * x_prev1 + a2 * x_prev2
+ b0 * (f_curr + 2*f_prev1 + f_prev2)
// 更新状态
x_prev2 = x_prev1
x_prev1 = x_curr
f_prev2 = f_prev1
f_prev1 = f_curr
// 发送期望位置给底层位置控制器
sendPositionCommand(x_curr)
// 等待下一个控制周期
wait(T)
}4.6 Python实现
下面是完整的Python实现。我建议你直接跑一下，感受一下不同参数的效果。
import numpy as np
import matplotlib.pyplot as plt
class OneDimAdmittanceController:
"""一维导纳控制器 - 双线性变换法"""
def __init__(self, M=1.0, B=10.0, K=100.0, T=0.001):
self.M = M
self.B = B
self.K = K
self.T = T
# 计算离散化系数
a0 = 4*M/(T**2) + 2*B/T + K
self.a1 = (4*M/(T**2) - K) / a0
self.a2 = (2*B/T - 4*M/(T**2)) / a0
self.b0 = 1 / (T**2 * a0)
# 初始化状态
self.reset()
def reset(self):
"""重置状态变量"""
self.x_prev1 = 0.0
self.x_prev2 = 0.0
self.f_prev1 = 0.0
self.f_prev2 = 0.0
def update(self, f_curr):
"""
更新导纳控制器
输入: f_curr - 当前外力
返回: x_curr - 期望位置
"""
x_curr = (self.a1 * self.x_prev1 +
self.a2 * self.x_prev2 +
self.b0 * (f_curr + 2*self.f_prev1 + self.f_prev2))
# 更新状态
self.x_prev2 = self.x_prev1
self.x_prev1 = x_curr
self.f_prev2 = self.f_prev1
self.f_prev1 = f_curr
return x_curr
# 测试代码
if __name__ == "__main__":
# 创建控制器
adm = OneDimAdmittanceController(M=1.0, B=20.0, K=50.0, T=0.001)
# 模拟外力输入：阶跃力
t = np.arange(0, 2.0, 0.001)
force = np.zeros_like(t)
force[500:] = 10.0 # 0.5s时施加10N阶跃力
# 记录响应
position = []
for f in force:
pos = adm.update(f)
position.append(pos)
# 绘制结果
plt.figure(figsize=(10, 6))
plt.subplot(2, 1, 1)
plt.plot(t, force)
plt.ylabel('Force (N)')
plt.grid(True)
plt.subplot(2, 1, 2)
plt.plot(t, position)
plt.ylabel('Position (m)')
plt.xlabel('Time (s)')
plt.grid(True)
plt.show()
小技巧：调试的时候，先把K设成0，看看系统是不是像个纯质量-阻尼系统。如果响应正常，再慢慢加刚度。这样能快速定位问题。
4.7 三种方法对比总结
最后，我用一张表总结一下三种方法的适用场景：
| 方法 | 稳定性 | 精度 | 推荐场景 |
|---|---|---|---|
| 前向欧拉 | 有条件稳定 | 低 | 低刚度、高采样率 |
| 后向欧拉 | 无条件稳定 | 中 | 对稳定性要求高、精度要求不高 |
| 双线性变换 | 无条件稳定 | 高 | 通用场景，强烈推荐 |
你想想看，做机器人控制，稳定永远是第一位的。所以我个人建议：默认用双线性变换，除非你有特殊理由（比如计算资源极度受限）。
好，这一节就到这儿。代码你可以直接拿去用，改改参数就能适配你自己的项目。