自动控制 / 系统课时
如何从位置读数推断运动速度
如何从位置读数推断运动速度
位置传感器每隔0.1秒返回一个读数,却不直接报告速度。你将把一维运动拆成位置、速度两个状态,用同一组带噪数据比较相邻差分与多点拟合,明确状态估计应输出什么,以及怎样检查结果是否可信。
学习目标
- 区分真实位置、位置测量与位置和速度的估计值。
- 解释差分速度的抖动来源,用六个位置读数计算速度。
- 明确在线估计的信息范围,识别匀速假设失效的情况。
一个位置读数为什么不够
设目标沿直轨运动,向右为正。第次采样的真实位置为,单位为米;真实速度为,单位为米每秒。把两者合成状态,上标表示转置,即将这两个量排成列向量。
传感器只返回,其中是位置测量误差。读数为1.20米时,目标可能静止,也可能正向或反向运动;缺少时间上的变化信息,仅凭这个读数无法确定速度。帽号如表示估计值。
先采用短时匀速模型:,。若无噪声,0秒的位置为1.00米,0.1秒为1.20米,便能算出速度为2.00米每秒。速度影响后续位置,因此能从连续位置读数中推断。genui{"citation":{"ref":"turn2view0"}}

相邻差分为什么会抖动
最直接的做法是用两次位置读数之差除以采样间隔。代入测量误差后,可见速度误差也被这个间隔除了一次:
第一项是采样区间内的真实平均速度;在匀速条件下,它才等于各时刻的速度。图中的读数1.02米和1.17米给出1.50米每秒,比真实速度2.00米每秒少了0.50米每秒。
若相邻位置误差独立、零均值,标准差均为,由方差相加可得差分噪声标准差。取米、秒,得到约0.283米每秒;间隔缩为0.01秒则约为2.83米每秒。缩短间隔并不保证单次差分更准。
用六个读数拟合运动速度
假设0~0.5秒内速度基本不变,用直线同时解释六个读数:截距表示初始位置,斜率表示速度。最小二乘拟合让各读数与直线之间的竖直残差平方和最小,利用整段数据估计同一个速度。
下面是完整的Python 3.10及以上版本示例,无需第三方依赖。保存为position_velocity_demo.py后执行python position_velocity_demo.py。误差序列是人为固定的演示数据,不代表已经验证过的噪声分布。
# 固定误差序列便于复现,真值只用于构造测量和验收。
times = [k / 10 for k in range(6)]
errors = [0.02, -0.03, 0.01, 0.04, -0.02, 0.03]
truth = [1.0 + 2.0 * t for t in times]
readings = [p + e for p, e in zip(truth, errors)]
# 相邻差分必须除以对应的时间间隔。
raw_v = [
(readings[k] - readings[k - 1]) / (times[k] - times[k - 1])
for k in range(1, len(times))
]
# 用全部读数拟合 p(t) = a + v*t,估计器只使用时间和读数。
t_bar = sum(times) / len(times)
z_bar = sum(readings) / len(readings)
denominator = sum((t - t_bar) ** 2 for t in times)
v_hat = sum(
(t - t_bar) * (z - z_bar) for t, z in zip(times, readings)
) / denominator
a_hat = z_bar - v_hat * t_bar
p_hat = a_hat + v_hat * times[-1]
print('位置读数(m):', ' '.join(f'{z:.2f}' for z in readings))
print('相邻差分(m/s):', ' '.join(f'{v:.2f}' for v in raw_v))
print(f'拟合速度: {v_hat:.3f} m/s')
print(f'末时刻位置估计: {p_hat:.3f} m')
print(f'速度绝对误差: {abs(v_hat - 2.0):.3f} m/s')
print(f'末时刻位置绝对误差: {abs(p_hat - truth[-1]):.3f} m')预期输出:
位置读数(m): 1.02 1.17 1.41 1.64 1.78 2.03
相邻差分(m/s): 1.50 2.40 2.30 1.40 2.50
拟合速度: 2.031 m/s
末时刻位置估计: 2.016 m
速度绝对误差: 0.031 m/s
末时刻位置绝对误差: 0.016 m到0.5秒才收齐六点,此时拟合速度为2.031米每秒,位置估计为2.016米。末次差分为2.50米每秒,末次位置读数为2.03米;这组数据的拟合结果更接近真值,但单次实验不能保证所有数据都改善。
把算例变成在线估计目标
上例是一段数据的直线拟合基线。卡尔曼滤波会递推结合运动模型和新测量,维护位置、速度估计以及表示不确定性的误差协方差;真实运动偏离模型与传感器测量误差需要分别描述。genui{"citation":{"refs":["turn2view0","turn2view1"]}}
把接口约定为:输入当前时间戳与位置读数,输出同一时刻的位置、速度估计。验收时检查以下三点:
- 不使用未来读数:0.3秒的在线输出只能使用截至0.3秒的数据。用全段六点回算的早期位置,不能当成当时已经得到的结果。
- 分别核对两种误差:本组固定数据在0.5秒处的位置绝对误差小于0.02米、速度绝对误差小于0.05米每秒,即通过复现检查;这些阈值不是通用性能保证。
- 保留真实时间间隔:时间戳必须严格递增。若两次采样实际相隔0.12秒,应除以0.12,而不是仍按0.1计算。
还要检查运动假设:若无噪声目标在0.5秒内从2米每秒匀加速到4米每秒,这六个时刻的位置直线拟合斜率是3米每秒,不能代表末时刻的4米每秒。用于跟踪变速目标时,应检验模型和估计响应速度,不能只看输出是否平滑。