自动控制 / 系统课时
根据匀速假设构造转移矩阵
根据匀速假设构造转移矩阵
已知小车当前的位置和速度,怎样预测下一次采样时的状态?本篇从一维匀速运动出发,把两条运动方程整理成转移矩阵,并用可运行代码核对采样间隔、状态顺序和单位对结果的影响。
学习目标
- 按位置、速度的状态顺序,推导二阶状态转移矩阵 。
- 用三次 0.2 s 与一次 0.6 s 的预测结果检查实现。
- 识别毫秒与秒混用、时间间隔不固定以及实际加速度带来的误差。
从匀速运动得到矩阵
只考虑一维直线运动,先规定正方向。位置 用 m,速度 用 m/s,状态采用列向量 。下标 是采样序号,实际时间间隔为 ,单位是 s。
匀速假设表示预测区间内加速度为零:位置增加“速度乘以经过时间”,速度保持原值。因此两条运动方程是:
把下一位置和下一速度分别放进矩阵乘法的第一行、第二行,就得到:
这就是 。对理想匀速运动,它是精确的离散传播关系,不要求间隔足够小。该形式也可对照 MathWorks 的运动模型说明。
让系数对应传播路径
矩阵的行对应下一状态,列对应当前状态。第一行的 1 保留旧位置, 将速度变成位移;第二行的 0 表示位置不改变预测速度,1 表示沿用当前速度。右上角具有时间单位,不能把所有系数都理解为无量纲权重。
取 、、,则位移为 ,预测结果是位置 2.6 m、速度 3 m/s。图中的速度分叉解释了它为何同时出现在两条方程中。

状态顺序必须与矩阵一致。若改为 ,矩阵也要改成 ;只交换数组元素、仍保留原矩阵,会把位置当作速度参与计算。
用短步和长步核对实现
从同一初态出发,连续三步 0.2 s 与一步 0.6 s 的总时长相同,纯匀速预测应到达同一位置。下面的完整脚本适用于 Python 3.10 及以上,只使用标准库;二维矩阵逐行与状态相乘,便于检查系数。
from math import isclose
def predict(state, dt):
# state 按位置、速度排列;dt 的单位必须是秒。
if dt < 0:
raise ValueError('本例只接受非负时间间隔')
F = ((1.0, dt), (0.0, 1.0))
# 左乘列向量,两个分量都使用同一个旧状态计算。
return tuple(sum(row[j] * state[j] for j in range(2)) for row in F)
def main():
initial = (2.0, 3.0)
state = initial
for k in range(1, 4):
state = predict(state, 0.2)
print(f't={k * 0.2:.1f}s p={state[0]:.3f}m v={state[1]:.3f}m/s')
# 总时长相同,比较一次长步与三次短步的纯预测结果。
direct = predict(initial, 0.6)
same = all(isclose(a, b, rel_tol=0.0, abs_tol=1e-12)
for a, b in zip(state, direct))
print(f'one 0.6s step: p={direct[0]:.3f}m v={direct[1]:.3f}m/s')
print(f'same_state={same}')
print(f'zero_dt_unchanged={predict(initial, 0.0) == initial}')
if __name__ == '__main__':
main()预期输出:
t=0.2s p=2.600m v=3.000m/s
t=0.4s p=3.200m v=3.000m/s
t=0.6s p=3.800m v=3.000m/s
one 0.6s step: p=3.800m v=3.000m/s
same_state=True
zero_dt_unchanged=True这验证了 ;零间隔时 ,其中 是二阶单位矩阵。代码用绝对容差 比较浮点结果,避免把舍入误差误判为模型错误。
按实际时间戳更新间隔
使用同一时钟基准下、与状态对应的采样时间戳计算间隔。例如采样时间为 0、0.20、0.45 s,两步应分别使用 0.20、0.25 s。从本例初态纯预测得到的位置为 2.60、3.35 m;第二步仍固定使用 0.20 s,会少预测 0.15 m。
时间戳若以毫秒记录,差值 200 必须除以 1000,再传入 predict。直接传入 200 等于预测 200 s,第一步位置会变成 。
示例允许零间隔以核对单位矩阵,并拒绝负间隔。实时处理发现时间倒退时,先检查数据排序和时间基准;不要对间隔取绝对值,否则会把历史观测错误地当成未来观测处理。
出现加速度时会差多少
若真实小车在区间内具有恒定加速度 ,匀速预测会遗漏位置增量 和速度增量 。取 、,真实下一状态为 ,匀速预测仍为 。
当间隔增至 0.4 s,遗漏的位置增量会从 0.04 m 增至 0.16 m。这是运动假设不符造成的偏差,不是矩阵乘法失效。已知加速度应加入运动模型,未知变化可用过程噪声描述;仅把预测计算切成更小步,不能自动补回遗漏的加速度。