自动控制 / 系统课时
把估计速度接入简单反馈控制
把估计速度接入简单反馈控制
位置传感器只有带噪读数,直接对相邻位置做差分会放大速度噪声,让加速度指令反复跳变。这里搭建一个单轴运动闭环,在相同目标、传感器噪声和控制器参数下,分别使用原始测量和卡尔曼估计作为反馈,量化响应时间、控制抖动与稳态误差。
学习目标
- 用位置与速度反馈实现带加速度限幅的 PD 控制。
- 复现原始位置差分与卡尔曼速度估计的闭环对比。
- 用响应时间、控制抖动、稳态误差和过冲检验控制效果。
将测量与估计接入同一控制律
把运动对象简化为单轴双积分模型:状态为位置 (m)和速度 (m/s),控制量 为加速度(m/s²)。每拍先根据当前测量算控制量,再推进物理状态,运动规律为 、。
两组实验使用完全相同的 PD 控制律:。其中 是反馈位置, 是反馈速度;np.clip 将加速度限制在 ±4 m/s²,目标位置从 0 阶跃到 1 m。
| 参数 | 固定取值 | 作用 |
|---|---|---|
| 采样间隔与时长 | 0.05 s,12 s | 共 240 次测量与控制 |
| 测量噪声 | 标准差 0.08 m | 只给位置传感器加噪 |
| PD 增益 | KP=4、KD=3 |
位置纠偏与速度阻尼 |
| 卡尔曼观测方差 | R = SIGMA_Z**2 |
匹配位置测量噪声 |
| 卡尔曼过程协方差 | Q = 0.4**2 * np.outer(B, B) |
容忍未建模的加速度 |
| 初始协方差 | np.diag([0.5**2, 1.0**2]) |
给未知初始速度更大不确定性 |
原始方案直接用测量位置 ,速度为 ;滤波方案使用估计位置 和估计速度 。位置测量为 ,真实速度不提供给任何控制器。
在两套独立运动对象上施加同一份预生成噪声序列,避免随机条件不同影响结论。滤波预测必须读入上一拍实际施加、已经限幅的加速度,否则估计模型与运动对象不一致。

运行两组可复现的运动仿真
准备 Python 3.10+ 和 NumPy 1.24+,使用 python -m pip install numpy 安装依赖。将下方代码保存为 control_compare.py,运行 python control_compare.py。程序每次先测量、再计算并限幅控制量、最后更新真实运动状态;两组只改变反馈信号的来源。
import numpy as np
DT = 0.05
STEPS = int(12.0 / DT)
TARGET = 1.0
SIGMA_Z = 0.08
KP, KD, U_MAX = 4.0, 3.0, 4.0
A = np.array([[1.0, DT], [0.0, 1.0]])
B = np.array([0.5 * DT**2, DT])
Q = 0.4**2 * np.outer(B, B) # 未建模加速度的协方差
R = SIGMA_Z**2
noise = np.random.default_rng(7).normal(0.0, SIGMA_Z, STEPS)
def simulate(filtered):
state = np.zeros(2) # 真实位置、速度
estimate = np.zeros(2)
P = np.diag([0.5**2, 1.0**2])
last_z, last_u = 0.0, 0.0
positions, controls = [], []
for k in range(STEPS):
z = state[0] + noise[k]
if filtered:
# 用上一拍实际施加的加速度进行预测
prior = estimate if k == 0 else A @ estimate + B * last_u
P_prior = P if k == 0 else A @ P @ A.T + Q
K = P_prior[:, 0] / (P_prior[0, 0] + R)
estimate = prior + K * (z - prior[0])
I_KH = np.eye(2) - np.outer(K, [1.0, 0.0])
P = I_KH @ P_prior @ I_KH.T + R * np.outer(K, K)
pos, vel = estimate
else:
# 原始位置差分在短采样间隔下会放大噪声
pos = z
vel = 0.0 if k == 0 else (z - last_z) / DT
u = float(np.clip(KP * (TARGET - pos) - KD * vel,
-U_MAX, U_MAX))
positions.append(state[0])
controls.append(u)
state = A @ state + B * u
last_z, last_u = z, u
positions = np.array(positions)
controls = np.array(controls)
tail = int(2.0 / DT)
reach = np.flatnonzero(positions >= 0.9 * TARGET)
t90 = float(reach[0] * DT) if reach.size else float("nan")
mae = float(np.mean(np.abs(TARGET - positions[-tail:])))
jitter = float(np.std(controls[-tail:]))
overshoot = float(max(0.0, positions.max() - TARGET))
return t90, mae, jitter, overshoot
for name, filtered in [("raw", False), ("kf", True)]:
t90, mae, jitter, overshoot = simulate(filtered)
print(f"{name:3} t90={t90:.2f}s "
f"MAE={mae:.3f}m jitter={jitter:.3f}m/s^2 "
f"overshoot={overshoot:.3f}m")预期输出(固定随机种子 7,数值按打印精度取整):
raw t90=1.45s MAE=0.080m jitter=3.363m/s^2 overshoot=0.238m
kf t90=1.25s MAE=0.038m jitter=0.260m/s^2 overshoot=0.098m滤波器使用状态 、已知输入 和位置观测 。P 的更新采用 Joseph 形式,避免直接相减引入非对称或负方差。这里真实运动没有额外施加过程扰动,Q 只是滤波器对潜在模型误差预留的不确定性,用来先隔离测量噪声的影响。
用四项指标检验闭环表现
- 响应时间
t90:真实位置第一次达到目标 90% 的时间,不是滤波算法本身的计算耗时。本例从 1.45 s 缩短到 1.25 s。 - 稳态误差
MAE:最后 2 s 内真实位置与 1 m 目标之差的平均绝对值,从 0.080 m 降到 0.038 m;这是有限窗口指标,不能等同于数学上的零稳态偏差。 - 控制抖动
jitter:最后 2 s 控制加速度的标准差,从 3.363 降到 0.260 m/s²,约减少 92%。大幅高频控制变化会增加执行器负担。 - 过冲
overshoot:真实位置超过 1 m 的最大距离,从 0.238 m 降到 0.098 m;数值越小不代表所有任务越好,还需同时检查到达时间。
差分速度会放大高频位置噪声:本例其理论噪声标准差约为 m/s,再乘速度增益 3 就容易让未限幅的加速度指令剧烈变化。因此,直接测量反馈组即使套用同样的 PD 参数,也经常碰到 ±4 m/s² 的限幅。滤波组在这个测试条件下同时获得较小的指令抖动与更快的到达时间,但这并非所有模型和噪声条件下都成立。
识别滤波延迟与实际接入风险
先只检验速度估计的作用:把滤波分支的 pos, vel = estimate 改为 pos, vel = z, estimate[1],这样两组都用原始位置,差异仅来自速度反馈。然后保持相同随机种子、总仿真时长和末尾 2 s 评估窗口,重新比较四项指标。
若加速、刹车时估计速度明显滞后,检查 Q 是否过小;可将 Q = 0.4**2 * np.outer(B, B) 中的 0.4 分别改为 0.1、0.8 试验。较小的过程不确定性通常更平滑,但对未建模加速更迟钝;较大值通常响应更快,却更容易把测量噪声带入控制。实际系统还需测量执行器延迟,并把实际生效的限幅后指令送回预测器,不能只使用未执行的理想指令。
本例可把滤波组的验收门槛设为:t90 不超过 1.5 s、MAE 不超过 0.05 m、jitter 不超过 0.5 m/s²、overshoot 不超过 0.15 m,运行结果均满足。将二维跟踪器接入时,可对 、 两轴分别计算反馈加速度;若总加速度上限是 4 m/s²,必须对二维加速度向量按模长统一限幅,不能仅将每个轴都夹到 ±4,否则对角线方向可能达到 5.66 m/s²。