自动控制 / 系统课时
用输入矩阵表达加速度作用
用输入矩阵表达加速度作用
已知小车将在下一采样间隔加速时,仅靠当前位置和速度外推会漏掉额外位移。本篇从匀加速公式构造输入矩阵,用 NumPy 验证一次预测,并明确哪些加速度信息可以写进输入、哪些误差应交给过程噪声描述。
学习目标
- 为位置、速度两维状态构造 的输入矩阵 。
- 算清持续 0.1 s 的已知加速度分别增加多少位置和速度。
- 区分已知输入 与未知过程噪声 ,避免在预测中重复计入加速度。
从匀加速运动构造输入矩阵
规定状态顺序为 ,分别表示位置和速度。采样间隔为 ; 表示在区间 内保持恒定的已知加速度,单位为 m/s²。不同采样区间可以使用不同的加速度。
忽略扰动时,匀加速公式给出:
将原状态与输入分别收集,就得到 ,其中输入矩阵负责把一个加速度映射成两个状态分量的变化:
这里 为 , 为 , 为标量。这种输入写法保留位置、速度两个状态,不必把已知加速度再添加成第三个状态。genui{"citation":{"ref":"turn3view1"}}
第一行的单位是 s²,乘加速度后得到额外位移;第二行的单位是 s,乘后得到速度增量。位置项中的 来自速度增量在区间内从零线性增长,平均增量只有末值的一半;漏掉它会把加速度产生的额外位移算大一倍。
拆开原速度与加速度的贡献
设当前位置估计为 2 m、速度估计为 3 m/s,下一段 s,已知加速度 m/s²。此时 。记当前估计为 ,它使用了截至时刻 的信息。
| 计算项 | 位置分量(m) | 速度分量(m/s) |
|---|---|---|
| 原状态外推 | 2.30 | 3.00 |
| 输入贡献 | 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"}}
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()预期输出:
无输入预测: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),程序未必报错,但结果已经不再是位置、速度列向量;示例中的形状断言会拦住这种错误。
把已知输入与随机过程噪声分开
存在未建模扰动时,真实状态模型写成 。这里 是两维随机状态增量,假设其均值为零; 是它的协方差。已知输入与未知扰动必须分开建模。genui{"citation":{"ref":"turn3view0"}}
因此,下一时刻的状态均值预测为:
预测器不知道本次 的具体值,不能抽一份随机数加进估计。随机抽样适合生成仿真真值;滤波器用 记录预测的不确定性。令 为状态估计误差的协方差,在过程噪声与当前估计误差独立时:
准确已知的输入同时作用于真实状态和预测状态,相减后会抵消,所以不会单独增加误差协方差。不要把 或 加到 上,也不要把已知的 2 m/s² 再当作随机加速度重复加入 。
若实际加速度为 ,且误差在每段内恒定、段间独立、均值为零、方差为 ,并与当前估计误差独立,那么这部分状态噪声为 ,对应 。这是区间恒定随机加速度模型,不能直接套用到连续白噪声加速度模型。genui{"citation":{"ref":"turn2view1"}}
例如 m/s²、 s 时,这部分噪声的位置标准差为 0.0025 m,速度标准差为 0.05 m/s。若 已经包含这一来源,不要再加一次 。
接入系统前检查时间与输入含义
- 时间单位与索引:100 ms 要换算成 0.1 s; 必须对应从 到 的那段运动。若间隔改为 0.2 s,要同时重算 和 ,此时 。
- 输入的物理含义:本模型接收加速度,不能直接代入力或油门百分比。若已知净外力为 20 N、质量为 10 kg,才可按 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。若仍出现额外增量,就检查是否在其他位置重复加入了加速度。