从阿克曼转向到状态方程：手把手推导自动驾驶中的二自由度车辆模型（附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([ [-(CfCr)/(m*vx), -vx-(a*Cf-b*Cr)/(m*vx)], [-(a*Cf-b*Cr)/(Iz*vx), -(a**2*Cfb**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, N10, dt0.1): 建立基础MPC框架 opti Opti() # 决策变量 x opti.variable(2, N1) u opti.variable(1, N) # 初始状态约束 opti.subject_to(x[:,0] opti.parameter(2,1)) # 动力学约束 for k in range(N): opti.subject_to(x[:,k1] x[:,k] dt*(Ax[:,k] Bu[:,k])) # 成本函数 cost sumsqr(x) 0.1*sumsqr(u) opti.minimize(cost) return opti.to_function(mpc, [opti.p], [u[:,0]])实际工程中模型精度与计算效率需要权衡。对于高速场景可能需要考虑四自由度模型增加纵向和侧倾动力学而对于低速自动泊车等场景简化的运动学模型可能更为合适。