自动控制 / 系统课时

用输入矩阵表达加速度作用

用输入矩阵表达加速度作用

已知小车将在下一采样间隔加速时,仅靠当前位置和速度外推会漏掉额外位移。本篇从匀加速公式构造输入矩阵,用 NumPy 验证一次预测,并明确哪些加速度信息可以写进输入、哪些误差应交给过程噪声描述。

学习目标

  • 为位置、速度两维状态构造 2×12\times1 的输入矩阵 BB。
  • 算清持续 0.1 s 的已知加速度分别增加多少位置和速度。
  • 区分已知输入 uku_k 与未知过程噪声 wkw_k,避免在预测中重复计入加速度。

从匀加速运动构造输入矩阵

规定状态顺序为 xk=[pk,vk]Tx_k=[p_k,v_k]^T,分别表示位置和速度。采样间隔为 Δt=tk+1−tk>0\Delta t=t_{k+1}-t_k>0;uku_k 表示在区间 [tk,tk+1)[t_k,t_{k+1}) 内保持恒定的已知加速度,单位为 m/s²。不同采样区间可以使用不同的加速度。

忽略扰动时,匀加速公式给出:

pk+1=pk+vkΔt+12ukΔt2vk+1=vk+ukΔt\begin{aligned} p_{k+1}&=p_k+v_k\Delta t+\frac{1}{2}u_k\Delta t^2\\ v_{k+1}&=v_k+u_k\Delta t \end{aligned}

将原状态与输入分别收集,就得到 xk+1=Fxk+Bukx_{k+1}=Fx_k+Bu_k,其中输入矩阵负责把一个加速度映射成两个状态分量的变化:

F=[1Δt01],B=[Δt2/2Δt]F=\begin{bmatrix}1&\Delta t\\0&1\end{bmatrix}, \qquad B=\begin{bmatrix}\Delta t^2/2\\\Delta t\end{bmatrix}

这里 FF 为 2×22\times2,BB 为 2×12\times1,uku_k 为标量。这种输入写法保留位置、速度两个状态,不必把已知加速度再添加成第三个状态。genui{"citation":{"ref":"turn3view1"}}

BB 第一行的单位是 s²,乘加速度后得到额外位移;第二行的单位是 s,乘后得到速度增量。位置项中的 1/21/2 来自速度增量在区间内从零线性增长,平均增量只有末值的一半;漏掉它会把加速度产生的额外位移算大一倍。

拆开原速度与加速度的贡献

设当前位置估计为 2 m、速度估计为 3 m/s,下一段 Δt=0.1\Delta t=0.1 s,已知加速度 uk=2u_k=2 m/s²。此时 B=[0.005,0.1]TB=[0.005,0.1]^T。记当前估计为 x^k∣k\hat{x}_{k|k},它使用了截至时刻 kk 的信息。

计算项 位置分量(m) 速度分量(m/s)
原状态外推 Fx^k∣kF\hat{x}_{k\mid k} 2.30 3.00
输入贡献 BukBu_k 0.01 0.20
两项相加后的预测 2.31 3.20

已知加速度映射为位置与速度增量,再与原状态外推结果相加

原速度已经贡献了 0.30 m 位移,输入只补充另外的 0.01 m。计算位置时必须使用区间起点的速度;若先把速度更新为 3.20 m/s,再套原位置公式,会重复计入加速度影响,错误地得到 2.33 m。

用 NumPy 验证一拍预测

使用 Python 3.10+ 和已安装 NumPy 的环境。把代码保存为 predict_with_acceleration.py,运行 python predict_with_acceleration.py。脚本完整实现一次状态均值预测;观测校正与协方差更新不在本示例内。

F @ x 表示矩阵乘法。这里 u 是标量,所以输入项使用 B * u;不能写 B @ u,因为 NumPy 的矩阵乘法不接受标量操作数。genui{"citation":{"ref":"turn2view2"}}

python
import numpy as np

def main():
    dt = 0.1
    u = 2.0
    # 状态和输入矩阵都使用二维列向量,避免广播出 2×2 矩阵。
    x = np.array([[2.0], [3.0]])
    F = np.array([[1.0, dt], [0.0, 1.0]])
    B = np.array([[0.5 * dt**2], [dt]])

    # 分开计算原速度的贡献与已知加速度带来的增量。
    base = F @ x
    delta = B * u
    predicted = base + delta
    print(f'无输入预测:p={base[0, 0]:.3f} m, v={base[1, 0]:.3f} m/s')
    print(f'输入新增:dp={delta[0, 0]:.3f} m, dv={delta[1, 0]:.3f} m/s')
    print(f'最终预测:p={predicted[0, 0]:.3f} m, v={predicted[1, 0]:.3f} m/s')
    print(f'结果形状:{predicted.shape}')

    # 用运动学手算值核对数值,形状单独检查以防误用广播。
    assert predicted.shape == (2, 1)
    np.testing.assert_allclose(predicted, [[2.31], [3.2]])
    print('检查通过')

if __name__ == '__main__':
    main()

预期输出:

text
无输入预测:p=2.300 m, v=3.000 m/s
输入新增:dp=0.010 m, dv=0.200 m/s
最终预测:p=2.310 m, v=3.200 m/s
结果形状:(2, 1)
检查通过

不要把 x 改成形状为 (2,) 的数组,却保留 B 为 (2, 1)。这会让加法广播成 (2, 2),程序未必报错,但结果已经不再是位置、速度列向量;示例中的形状断言会拦住这种错误。

把已知输入与随机过程噪声分开

存在未建模扰动时,真实状态模型写成 xk+1=Fxk+Buk+wkx_{k+1}=Fx_k+Bu_k+w_k。这里 wkw_k 是两维随机状态增量,假设其均值为零;QQ 是它的协方差。已知输入与未知扰动必须分开建模。genui{"citation":{"ref":"turn3view0"}}

因此,下一时刻的状态均值预测为:

x^k+1∣k=Fx^k∣k+Buk\hat{x}_{k+1|k}=F\hat{x}_{k|k}+Bu_k

预测器不知道本次 wkw_k 的具体值,不能抽一份随机数加进估计。随机抽样适合生成仿真真值;滤波器用 QQ 记录预测的不确定性。令 PP 为状态估计误差的协方差,在过程噪声与当前估计误差独立时:

Pk+1∣k=FPk∣kFT+QP_{k+1|k}=FP_{k|k}F^T+Q

准确已知的输入同时作用于真实状态和预测状态,相减后会抵消,所以不会单独增加误差协方差。不要把 BukBu_k 或 (Buk)(Buk)T(Bu_k)(Bu_k)^T 加到 PP 上,也不要把已知的 2 m/s² 再当作随机加速度重复加入 QQ。

若实际加速度为 uk+εku_k+\varepsilon_k,且误差在每段内恒定、段间独立、均值为零、方差为 σa2\sigma_a^2,并与当前估计误差独立,那么这部分状态噪声为 BεkB\varepsilon_k,对应 Qa=Bσa2BTQ_a=B\sigma_a^2B^T。这是区间恒定随机加速度模型,不能直接套用到连续白噪声加速度模型。genui{"citation":{"ref":"turn2view1"}}

例如 σa=0.5\sigma_a=0.5 m/s²、Δt=0.1\Delta t=0.1 s 时,这部分噪声的位置标准差为 0.0025 m,速度标准差为 0.05 m/s。若 QQ 已经包含这一来源,不要再加一次 QaQ_a。

接入系统前检查时间与输入含义

  • 时间单位与索引:100 ms 要换算成 0.1 s;uku_k 必须对应从 tkt_k 到 tk+1t_{k+1} 的那段运动。若间隔改为 0.2 s,要同时重算 FF 和 BB,此时 B=[0.02,0.2]TB=[0.02,0.2]^T。
  • 输入的物理含义:本模型接收加速度,不能直接代入力或油门百分比。若已知净外力为 20 N、质量为 10 kg,才可按 u=20/10=2u=20/10=2 m/s² 换算后使用。
  • 命令与实际执行:发出 2 m/s² 的命令不代表实际加速度就是 2 m/s²。若执行器限幅到 1 m/s²,应使用限幅后的已知输入;剩余误差需另行建模。持续偏置不能仅靠增大零均值噪声的方差来消除。

把脚本中的输入改为 u = 0.0,并将数值断言的期望值改为 [[2.3], [3.0]],输入贡献应归零,最终预测退回位置 2.30 m、速度 3.00 m/s。若仍出现额外增量,就检查是否在其他位置重复加入了加速度。

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

目录与阅读设置

阅读设置

17

记录一个想法

用输入矩阵表达加速度作用

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

登录哪都通