自动控制 / 系统课时

根据匀速假设构造转移矩阵

根据匀速假设构造转移矩阵

已知小车当前的位置和速度,怎样预测下一次采样时的状态?本篇从一维匀速运动出发,把两条运动方程整理成转移矩阵,并用可运行代码核对采样间隔、状态顺序和单位对结果的影响。

学习目标

  • 按位置、速度的状态顺序,推导二阶状态转移矩阵 FF。
  • 用三次 0.2 s 与一次 0.6 s 的预测结果检查实现。
  • 识别毫秒与秒混用、时间间隔不固定以及实际加速度带来的误差。

从匀速运动得到矩阵

只考虑一维直线运动,先规定正方向。位置 pkp_k 用 m,速度 vkv_k 用 m/s,状态采用列向量 xk=[pk,vk]Tx_k=[p_k,v_k]^T。下标 kk 是采样序号,实际时间间隔为 Δtk=tk+1−tk\Delta t_k=t_{k+1}-t_k,单位是 s。

匀速假设表示预测区间内加速度为零:位置增加“速度乘以经过时间”,速度保持原值。因此两条运动方程是:

pk+1=pk+Δtkvk,vk+1=vk.\begin{aligned} p_{k+1}&=p_k+\Delta t_k v_k,\\ v_{k+1}&=v_k. \end{aligned}

把下一位置和下一速度分别放进矩阵乘法的第一行、第二行,就得到:

[pk+1vk+1]=[1Δtk01]⏟Fk[pkvk].\begin{bmatrix}p_{k+1}\\v_{k+1}\end{bmatrix} = \underbrace{\begin{bmatrix}1&\Delta t_k\\0&1\end{bmatrix}}_{F_k} \begin{bmatrix}p_k\\v_k\end{bmatrix}.

这就是 xk+1=Fkxkx_{k+1}=F_kx_k。对理想匀速运动,它是精确的离散传播关系,不要求间隔足够小。该形式也可对照 MathWorks 的运动模型说明。

让系数对应传播路径

矩阵的行对应下一状态,列对应当前状态。第一行的 1 保留旧位置,Δtk\Delta t_k 将速度变成位移;第二行的 0 表示位置不改变预测速度,1 表示沿用当前速度。右上角具有时间单位,不能把所有系数都理解为无量纲权重。

取 p0=2 mp_0=2\,\mathrm{m}、v0=3 m/sv_0=3\,\mathrm{m/s}、Δt=0.2 s\Delta t=0.2\,\mathrm{s},则位移为 3×0.2=0.6 m3\times0.2=0.6\,\mathrm{m},预测结果是位置 2.6 m、速度 3 m/s。图中的速度分叉解释了它为何同时出现在两条方程中。

速度通过位移项影响下一位置,预测速度保持不变

状态顺序必须与矩阵一致。若改为 [vk,pk]T[v_k,p_k]^T,矩阵也要改成 [10Δtk1]\begin{bmatrix}1&0\\\Delta t_k&1\end{bmatrix};只交换数组元素、仍保留原矩阵,会把位置当作速度参与计算。

用短步和长步核对实现

从同一初态出发,连续三步 0.2 s 与一步 0.6 s 的总时长相同,纯匀速预测应到达同一位置。下面的完整脚本适用于 Python 3.10 及以上,只使用标准库;二维矩阵逐行与状态相乘,便于检查系数。

python
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()

预期输出:

text
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

这验证了 F(0.2)3=F(0.6)F(0.2)^3=F(0.6);零间隔时 F(0)=IF(0)=I,其中 II 是二阶单位矩阵。代码用绝对容差 10−1210^{-12} 比较浮点结果,避免把舍入误差误判为模型错误。

按实际时间戳更新间隔

使用同一时钟基准下、与状态对应的采样时间戳计算间隔。例如采样时间为 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,第一步位置会变成 2+200×3=602 m2+200\times3=602\,\mathrm{m}。

示例允许零间隔以核对单位矩阵,并拒绝负间隔。实时处理发现时间倒退时,先检查数据排序和时间基准;不要对间隔取绝对值,否则会把历史观测错误地当成未来观测处理。

出现加速度时会差多少

若真实小车在区间内具有恒定加速度 aa,匀速预测会遗漏位置增量 12aΔt2\tfrac12a\Delta t^2 和速度增量 aΔta\Delta t。取 a=2 m/s2a=2\,\mathrm{m/s^2}、Δt=0.2 s\Delta t=0.2\,\mathrm{s},真实下一状态为 [2.64,3.4]T[2.64,3.4]^T,匀速预测仍为 [2.6,3]T[2.6,3]^T。

当间隔增至 0.4 s,遗漏的位置增量会从 0.04 m 增至 0.16 m。这是运动假设不符造成的偏差,不是矩阵乘法失效。已知加速度应加入运动模型,未知变化可用过程噪声描述;仅把预测计算切成更小步,不能自动补回遗漏的加速度。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

根据匀速假设构造转移矩阵

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通