从阿克曼转向到状态方程：手把手推导自动驾驶中的二自由度车辆模型（附Python代码）
从阿克曼转向到状态方程：手把手推导自动驾驶中的二自由度车辆模型（附Python代码）
在自动驾驶系统的开发中，车辆模型的精确度直接影响控制算法的性能。许多工程师在学习LQR或MPC控制器设计时，往往卡在如何将物理车辆抽象为数学模型这一关键环节。本文将从一个具体的工程概念——阿克曼转向几何出发，逐步推导出可用于控制器设计的二自由度车辆状态空间方程，并通过Python代码实现模型验证，构建完整的"理论-代码"闭环。
1. 阿克曼转向几何：车辆运动学的起点
阿克曼转向原理是理解车辆转向行为的基础。当车辆低速转弯时，内侧轮需要比外侧轮转动更大的角度，以确保所有车轮绕同一瞬时中心旋转。这种几何关系可以用简单的三角函数表示：
def calculate_ackermann_angle(steering_angle, wheelbase, track_width): """计算内外轮阿克曼转向角度""" inner_angle = math.atan(wheelbase / (wheelbase/math.tan(steering_angle) - track_width/2)) outer_angle = math.atan(wheelbase / (wheelbase/math.tan(steering_angle) + track_width/2)) return inner_angle, outer_angle在低速假设下（通常<5m/s），我们可以忽略轮胎侧向力影响，建立简化运动学模型：
- x方向速度：
ẋ = v·cos(θ) - y方向速度：
ẏ = v·sin(θ) - 横摆角速度：
ω = v·tan(δ)/L
注意：此模型假设无轮胎侧滑，且转向角δ较小（通常<15°）
2. 动力学扩展：引入轮胎侧偏特性
当车速提高时，轮胎会产生侧偏角，此时必须考虑动力学效应。建立车辆受力平衡方程时，需要分别在x、y和横摆三个方向分析：
| 力/力矩 | 表达式 |
|---|---|
| x方向 | m(ẍ - vyφ̇) = Fxr + Fxfcosδ - Fyfsinδ |
| y方向 | m(ÿ + vxφ̇) = Fyr + Fyfcosδ + Fxfsinδ |
| 横摆 | Izφ̈ = (Fyfcosδ + Fxfsinδ)a - Fyrb |
其中关键参数包括：
Fyf, Fyr：前后轴侧偏力Cf, Cr：前后轮胎侧偏刚度β：质心侧偏角
轮胎侧偏力采用线性模型：
def lateral_force(slip_angle, cornering_stiffness): """计算轮胎侧偏力""" return -cornering_stiffness * slip_angle3. 状态空间方程推导
将动力学方程线性化后，可表示为标准状态空间形式：
ẋ = Ax + Bu y = Cx + Du具体推导步骤：
- 选择状态变量：
x = [vy φ̇]ᵀ - 确定输入变量：
u = δ - 线性化处理小角度假设
- 整理矩阵系数：
def build_state_space_matrix(vx, Cf, Cr, m, Iz, a, b): """构建状态空间矩阵""" A = np.array([ [-(Cf+Cr)/(m*vx), -vx-(a*Cf-b*Cr)/(m*vx)], [-(a*Cf-b*Cr)/(Iz*vx), -(a**2*Cf+b**2*Cr)/(Iz*vx)] ]) B = np.array([ [Cf/m], [a*Cf/Iz] ]) return A, B4. 模型验证与仿真
通过Python实现完整仿真流程：
import numpy as np from scipy.integrate import odeint def vehicle_dynamics(x, t, u, params): """二自由度车辆动力学模型""" vy, r = x # 侧向速度，横摆角速度 delta = u # 前轮转角 vx = params['vx'] # 纵向速度假设恒定 # 状态方程计算 dvy = (params['Cf']*(delta - (vy + params['a']*r)/vx) + params['Cr']*(-(vy - params['b']*r)/vx))/params['m'] - vx*r dr = (params['a']*params['Cf']*(delta - (vy + params['a']*r)/vx) - params['b']*params['Cr']*(-(vy - params['b']*r)/vx))/params['Iz'] return [dvy, dr] # 仿真参数 params = { 'm': 1500, # 质量(kg) 'Iz': 2500, # 横摆转动惯量(kg·m²) 'a': 1.2, # 前轴到质心距离(m) 'b': 1.6, # 后轴到质心距离(m) 'Cf': 80000, # 前轮侧偏刚度(N/rad) 'Cr': 100000, # 后轮侧偏刚度(N/rad) 'vx': 20 # 纵向速度(m/s) } # 仿真步长 t = np.linspace(0, 10, 1000) # 阶跃转向输入 u = np.deg2rad(5) * (t > 1).astype(float) # 初始条件 x0 = [0, 0] # 数值积分 sol = odeint(vehicle_dynamics, x0, t, args=(u, params))仿真结果可绘制侧向速度和横摆角速度响应曲线，验证模型合理性。实际项目中，还需要考虑：
- 轮胎非线性特性（如Pacejka模型）
- 载荷转移影响
- 悬架动力学耦合效应
5. 控制器设计衔接
得到的状态空间模型可直接用于LQR控制器设计：
from scipy.linalg import solve_continuous_are def design_lqr_controller(A, B, Q, R): """设计LQR控制器""" P = solve_continuous_are(A, B, Q, R) K = np.linalg.inv(R) @ B.T @ P return K # 权重矩阵选择 Q = np.diag([1, 10]) # 侧向速度、横摆角速度权重 R = np.array([[0.1]]) # 转向输入权重 A, B = build_state_space_matrix(params['vx'], params['Cf'], params['Cr'], params['m'], params['Iz'], params['a'], params['b']) K = design_lqr_controller(A, B, Q, R)在模型预测控制(MPC)中，该模型可作为预测模型的核心部分，通过离散化后用于滚动优化：
from casadi import * def mpc_controller_setup(A, B, N=10, dt=0.1): """建立基础MPC框架""" opti = Opti() # 决策变量 x = opti.variable(2, N+1) u = opti.variable(1, N) # 初始状态约束 opti.subject_to(x[:,0] == opti.parameter(2,1)) # 动力学约束 for k in range(N): opti.subject_to(x[:,k+1] == x[:,k] + dt*(A@x[:,k] + B@u[:,k])) # 成本函数 cost = sumsqr(x) + 0.1*sumsqr(u) opti.minimize(cost) return opti.to_function('mpc', [opti.p], [u[:,0]])实际工程中，模型精度与计算效率需要权衡。对于高速场景，可能需要考虑四自由度模型（增加纵向和侧倾动力学）；而对于低速自动泊车等场景，简化的运动学模型可能更为合适。