2、车辆运动学模型：自行车模型推导、状态空间表示、离散化方法、前轮转角与曲率约束
各位同学，咱们今天聊聊车辆运动学模型。说实话，这是整个混合A星算法的基础中的基础。你想想看，如果连车怎么动都描述不清楚，那路径规划就是空中楼阁。
我个人习惯把运动学模型比作「车的物理身份证」。它告诉我们：给定一个控制输入，车下一时刻会跑到哪里。嗯，咱们从最经典的自行车模型开始讲起。
2.1 自行车模型推导
为什么叫自行车模型？说白了，就是把四轮汽车简化成两轮自行车。前轮负责转向，后轮负责驱动。我在项目中遇到过不少新手，一上来就搞复杂的四轮模型，结果调参调到崩溃。其实对于泊车这种低速场景，自行车模型完全够用。
推导过程其实不复杂。你看这张图（虽然这里没有图，但请你脑补一下）：
- 后轮中心为基准点 (x, y)
- 车辆航向角为 θ
- 前轮转角为 δ
- 轴距为 L
核心假设就两条：
- 车辆在平面上运动，没有垂向跳动
- 轮胎侧偏角忽略不计（低速时成立）
那么，后轮中心的运动方程就是：
dx/dt = v * cos(θ)
dy/dt = v * sin(θ)
dθ/dt = v * tan(δ) / L这里 v 是后轮速度。注意，前轮转角 δ 是有范围的，一般乘用车在 ±35° 左右。我曾经见过一个同学把 δ 设到 45°，结果仿真里车直接原地打转——嗯，物理上不可能。
核心公式记忆点：
曲率 κ = tan(δ) / L
说白了，前轮转角越大，曲率越大，转弯半径越小。
2.2 状态空间表示
有了微分方程，咱们就可以写成状态空间形式了。我个人习惯用向量来表示：
状态向量: s = [x, y, θ, δ, v]^T
控制输入: u = [a, ω]^T
其中 a 是加速度，ω 是前轮转角速度你可能会问：为什么要把 δ 和 v 也放进状态里？因为在实际控制中，我们不能瞬间改变速度和转角，它们有惯性。我在做实车测试时就踩过这个坑——仿真里一脚油门车速就变了，但真车上加速需要时间。
完整的连续时间状态空间方程：
s_dot = f(s, u) =
[
v * cos(θ),
v * sin(θ),
v * tan(δ) / L,
a,
ω
]我的小技巧： 写代码时把状态向量定义成 numpy 数组，索引用常量命名，比如 IDX_X=0, IDX_Y=1... 这样代码可读性会好很多。
2.3 离散化方法
连续模型不能直接用在计算机里，得离散化。常用的方法有三种：
| 方法 | 公式 | 精度 | 我推荐吗？ |
|---|---|---|---|
| 欧拉法 | s_{k+1} = s_k + dt * f(s_k, u_k) | O(dt) | 不推荐，太粗糙 |
| 龙格-库塔法(RK4) | 四步加权平均 | O(dt^4) | 精度高，但计算量大 |
| 半隐式欧拉法 | 先更新角度，再更新位置 | O(dt^2) | ✅ 我的首选 |
为什么我推荐半隐式欧拉法？因为它在精度和计算量之间取得了很好的平衡。具体做法是：
# 半隐式欧拉法离散化
def discrete_step(s, u, dt):
x, y, theta, delta, v = s
a, omega = u
# 先更新角度和速度
theta_new = theta + v * tan(delta) / L * dt
v_new = v + a * dt
delta_new = delta + omega * dt
# 再用更新后的角度算位置
x_new = x + v_new * cos(theta_new) * dt
y_new = y + v_new * sin(theta_new) * dt
return [x_new, y_new, theta_new, delta_new, v_new]注意！ 离散化步长 dt 不能太大。我一般取 0.1~0.2 秒。太大时，曲率变化会失真，路径会「跳变」。太小呢，搜索树节点太多，实时性变差。
2.4 前轮转角与曲率约束
这部分是工程落地的关键。理论模型再漂亮，不考虑约束就是废纸一张。
前轮转角约束：
- 最大转角 δ_max：一般乘用车 30°~40°，卡车可能只有 25°
- 转角变化率 ω_max：方向盘不能打太快，一般 0.5~1.0 rad/s
曲率约束：
- 最小转弯半径 R_min = L / tan(δ_max)
- 曲率 κ_max = 1 / R_min
我在项目中遇到过一个问题：用混合A星搜索时，生成的路径曲率变化太剧烈，实车方向盘嘎嘎响。后来我加了一个曲率变化率约束，问题就解决了。
避坑指南：
我曾经在泊车场景中忽略了「方向盘回正速度」这个约束。结果路径规划出来很漂亮，但实车执行时方向盘跟不上，直接撞到路肩上。嗯，从那以后我每次都会检查 ω_max 是否合理。
最后，给一个实用的约束检查函数：
def check_constraints(s, u):
delta = s[3]
omega = u[1]
# 转角约束
if abs(delta) > DELTA_MAX:
return False
# 转角变化率约束
if abs(omega) > OMEGA_MAX:
return False
# 曲率约束
kappa = tan(delta) / L
if abs(kappa) > KAPPA_MAX:
return False
return True好了，这一章的内容就这些。说白了，运动学模型就是给车画了个「运动边界」。你理解了它，后面的混合A星搜索才能有的放矢。下一章咱们聊聊如何用这个模型生成平滑的泊车路径。