从自行车模型到自动驾驶：运动学与动力学的实战代码解析

在自动驾驶技术快速发展的今天，理解车辆运动的基本原理对于算法工程师和研究者至关重要。自行车模型作为车辆运动建模的基础工具，因其简洁性和实用性而广受欢迎。本文将深入探讨自行车模型在运动学和动力学层面的数学原理，并通过Python代码实现，帮助读者从理论走向实践。

1. 自行车模型基础

自行车模型是对四轮车辆的简化表示，它将前后轮各视为一个轮胎，并假设车辆仅在二维平面运动。这种简化虽然牺牲了一些细节，但保留了车辆运动的核心特性，使其成为路径规划和控制算法开发的理想起点。

模型的核心假设包括：

• 忽略垂直方向（Z轴）的运动
• 左右轮胎的运动特性相同，可合并为一个轮胎
• 低速运动，忽略前后轴载荷转移
• 车身和悬架系统为刚性结构
• 前轮驱动转向

关键参数定义表：

符号	含义	单位
δ_f	前轮转向角	rad
δ_r	后轮转向角	rad
β	质心侧偏角	rad
φ	横摆角	rad
v	质心速度	m/s
L_f	质心到前轴距离	m
L_r	质心到后轴距离	m
L	轴距（L_f + L_r）	m
R	转向半径	m

2. 运动学模型推导与实现

运动学模型关注的是车辆运动的几何特性，不考虑力和质量的影响。基于自行车模型，我们可以建立车辆状态随时间变化的微分方程。

2.1 运动学方程推导

在全局坐标系中，车辆状态可表示为位姿向量p = [x, y, θ]^T，其运动学方程为：

ṗ = [ẋ, ẏ, θ̇]^T = v_r [cosθ, sinθ, (tanδ_f)/L]^T

其中v_r是后轮驱动速度，控制输入u = [v_r, δ_f]^T。

Python实现代码：

class KinematicBicycleModel:
    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0, L=2.9, dt=0.1):
        self.x = x      # 全局x坐标
        self.y = y      # 全局y坐标
        self.yaw = yaw  # 航向角
        self.v = v      # 速度
        self.L = L      # 轴距
        self.dt = dt    # 时间步长
        
    def update(self, a, delta):
        """
        更新车辆状态
        :param a: 加速度 (m/s^2)
        :param delta: 前轮转向角 (rad)
        """
        self.x += self.v * math.cos(self.yaw) * self.dt
        self.y += self.v * math.sin(self.yaw) * self.dt
        self.yaw += (self.v / self.L) * math.tan(delta) * self.dt
        self.v += a * self.dt
        
        # 限制转向角和速度范围
        self.yaw = self.normalize_angle(self.yaw)
        self.v = max(0, self.v)  # 不允许倒车
        
    @staticmethod
    def normalize_angle(angle):
        """将角度归一化到[-π, π]区间"""
        while angle > math.pi:
            angle -= 2.0 * math.pi
        while angle < -math.pi:
            angle += 2.0 * math.pi
        return angle

2.2 运动学模型可视化

通过Matplotlib可以直观展示车辆的运动轨迹：

def visualize_vehicle(x, y, yaw, delta, ax, color='b'):
    """绘制车辆在给定位置和方向的示意图"""
    # 车辆轮廓参数
    L = 2.9  # 轴距
    W = 1.8  # 车宽
    
    # 车身轮廓
    car_outline = np.array([
        [-L/2, L/2, L/2, -L/2, -L/2],
        [W/2, W/2, -W/2, -W/2, W/2]
    ])
    
    # 车轮参数
    wheel_len = 0.3
    wheel_width = 0.2
    
    # 旋转矩阵
    rotation = np.array([
        [math.cos(yaw), -math.sin(yaw)],
        [math.sin(yaw), math.cos(yaw)]
    ])
    
    # 变换车身轮廓
    car_outline = rotation @ car_outline
    car_outline[0, :] += x
    car_outline[1, :] += y
    
    # 绘制车身
    ax.plot(car_outline[0, :], car_outline[1, :], color)
    
    # 绘制前轮
    front_wheel = np.array([
        [0, wheel_len],
        [-wheel_width/2, -wheel_width/2]
    ])
    front_wheel = rotation @ front_wheel
    front_wheel[0, :] += x + L/2 * math.cos(yaw)
    front_wheel[1, :] += y + L/2 * math.sin(yaw)
    ax.plot(front_wheel[0, :], front_wheel[1, :], 'k')

提示：在实际应用中，运动学模型适用于低速场景（通常<5m/s）。当车速较高时，需要考虑轮胎侧偏等动力学效应。

3. 动力学模型深入解析

动力学模型进一步考虑了力和力矩对车辆运动的影响，特别是轮胎与地面间的相互作用。这对于高速行驶或精确控制场景尤为重要。

3.1 线性侧偏力假设

采用最简单的线性侧偏力模型：

F_α = C_α * α

其中：

• F_α是侧偏力
• α是侧偏角
• C_α是侧偏刚度（通常为负值）

3.2 动力学方程推导

基于牛顿第二定律和转动定律，建立动力学方程：

# 状态空间表示
A = np.array([
    [0, 1, 0, 0],
    [0, 2*(C_αf + C_αr)/(m*v_x), 0, 2*(C_αf*lf - C_αr*lr)/(m*v_x) - v_x],
    [0, 0, 0, 1],
    [0, 2*(C_αf*lf - C_αr*lr)/(I*v_x), 0, 2*(C_αf*lf² + C_αr*lr²)/(I*v_x)]
])

B = np.array([
    [0],
    [-2*C_αf/m],
    [0],
    [-2*C_αf*lf/I]
])

3.3 动力学模型Python实现

class DynamicBicycleModel:
    def __init__(self, params):
        """
        初始化动力学模型
        :param params: 包含车辆参数的字典
        """
        self.m = params['mass']          # 质量 (kg)
        self.Iz = params['inertia']      # 绕z轴转动惯量 (kg·m²)
        self.lf = params['lf']           # 质心到前轴距离 (m)
        self.lr = params['lr']           # 质心到后轴距离 (m)
        self.Cf = params['Cf']           # 前轮侧偏刚度 (N/rad)
        self.Cr = params['Cr']           # 后轮侧偏刚度 (N/rad)
        
        # 状态变量 [y, ẏ, ψ, ψ̇]
        self.state = np.zeros(4)
        self.time = 0
        
    def update(self, delta, vx, dt):
        """
        更新动力学模型状态
        :param delta: 前轮转向角 (rad)
        :param vx: 纵向速度 (m/s)
        :param dt: 时间步长 (s)
        """
        y, y_dot, psi, psi_dot = self.state
        
        # 计算侧偏角
        alpha_f = delta - (y_dot + self.lf * psi_dot) / vx
        alpha_r = -(y_dot - self.lr * psi_dot) / vx
        
        # 计算侧偏力
        Fyf = self.Cf * alpha_f
        Fyr = self.Cr * alpha_r
        
        # 状态导数
        y_ddot = (Fyf + Fyr) / self.m - vx * psi_dot
        psi_ddot = (self.lf * Fyf - self.lr * Fyr) / self.Iz
        
        # 更新状态 (欧拉积分)
        self.state += np.array([
            y_dot * dt,
            y_ddot * dt,
            psi_dot * dt,
            psi_ddot * dt
        ])
        
        self.time += dt
        
        return self.state

4. 模型应用与路径跟踪

将自行车模型应用于路径跟踪是自动驾驶中的常见任务。这里介绍基于模型预测控制(MPC)的实现思路。

4.1 误差模型建立

定义横向位移误差e1和航向角误差e2：

e1 = y - y_desired
e2 = ψ - ψ_desired

对应的误差动力学方程：

# 误差状态空间矩阵
A_error = np.array([
    [0, 1, 0, 0],
    [0, 2*(Cf + Cr)/(m*vx), -2*(Cf + Cr)/m, 2*(Cf*lf - Cr*lr)/(m*vx)],
    [0, 0, 0, 1],
    [0, 2*(Cf*lf - Cr*lr)/(Iz*vx), 2*(Cr*lr - Cf*lf)/Iz, 2*(Cf*lf² + Cr*lr²)/(Iz*vx)]
])

B_error = np.array([
    [0],
    [-2*Cf/m],
    [0],
    [-2*Cf*lf/Iz]
])

4.2 MPC控制器实现

class MPCController:
    def __init__(self, model, horizon=10, dt=0.1):
        self.model = model
        self.horizon = horizon  # 预测时域
        self.dt = dt            # 时间步长
        
    def solve(self, x0, ref_path):
        """
        求解MPC优化问题
        :param x0: 初始状态
        :param ref_path: 参考路径
        :return: 最优控制序列
        """
        # 构建优化问题
        opti = casadi.Opti()
        
        # 决策变量
        X = opti.variable(4, self.horizon+1)  # 状态
        U = opti.variable(1, self.horizon)    # 控制量(转向角)
        
        # 初始条件约束
        opti.subject_to(X[:,0] == x0)
        
        # 动力学约束
        for k in range(self.horizon):
            x_next = self.model.dynamics(X[:,k], U[:,k], self.dt)
            opti.subject_to(X[:,k+1] == x_next)
            
        # 控制量约束
        opti.subject_to(opti.bounded(-0.6, U, 0.6))  # 转向角限制
        
        # 成本函数
        cost = 0
        for k in range(self.horizon):
            # 跟踪误差成本
            cost += X[0,k]**2 + 0.1*X[1,k]**2 + X[2,k]**2 + 0.1*X[3,k]**2
            # 控制量变化成本
            if k > 0:
                cost += 0.01*(U[:,k] - U[:,k-1])**2
                
        # 终端成本
        cost += 10*X[0,-1]**2 + X[2,-1]**2
        
        # 求解
        opti.minimize(cost)
        opti.solver('ipopt')
        sol = opti.solve()
        
        return sol.value(U[:,0])  # 返回第一个控制量

注意：实际应用中需要根据车辆特性和控制需求调整成本函数权重和约束条件。过强的约束可能导致优化问题不可行，而过弱的约束可能导致控制效果不佳。

5. 高级话题与性能优化

5.1 模型参数辨识

准确的模型参数对控制性能至关重要。可以通过实验数据对关键参数进行辨识：

def parameter_estimation(data):
    """
    基于实验数据估计车辆参数
    :param data: 包含时间序列数据的字典
    :return: 估计的参数
    """
    # 构建优化问题
    opti = casadi.Opti()
    
    # 待估计参数
    m = opti.variable()    # 质量
    Iz = opti.variable()   # 转动惯量
    Cf = opti.variable()   # 前轮侧偏刚度
    Cr = opti.variable()   # 后轮侧偏刚度
    
    # 参数约束 (物理意义限制)
    opti.subject_to(m > 1000)
    opti.subject_to(Iz > 100)
    opti.subject_to(Cf < -10000)
    opti.subject_to(Cr < -10000)
    
    # 成本函数 (最小化预测误差)
    cost = 0
    for i in range(len(data['t'])-1):
        # 获取当前状态和控制量
        x = data['x'][i,:]
        u = data['u'][i]
        dt = data['t'][i+1] - data['t'][i]
        
        # 预测下一步状态
        alpha_f = u - (x[1] + lf*x[3])/x[4]
        alpha_r = -(x[1] - lr*x[3])/x[4]
        Fyf = Cf * alpha_f
        Fyr = Cr * alpha_r
        
        y_ddot = (Fyf + Fyr)/m - x[4]*x[3]
        psi_ddot = (lf*Fyf - lr*Fyr)/Iz
        
        # 预测状态
        x_pred = x + [x[1], y_ddot, x[3], psi_ddot, 0] * dt
        
        # 误差项
        cost += sumsqr(x_pred[:4] - data['x'][i+1,:4])
    
    # 求解
    opti.minimize(cost)
    opti.solver('ipopt')
    sol = opti.solve()
    
    return sol.value(m), sol.value(Iz), sol.value(Cf), sol.value(Cr)

5.2 实时性能优化

对于实时控制系统，计算效率至关重要。以下优化策略值得考虑：

1. 热启动：利用上一控制周期的解作为当前优化的初始猜测
2. 代码生成：将优化问题编译为高效C代码
3. 简化模型：在长预测时域使用简化模型
4. 并行计算：利用多核处理器并行计算不同场景

# 使用ACADOS求解器的示例
def setup_acados_solver(model, N=20, Tf=2.0):
    ocp = AcadosOcp()
    
    # 模型设置
    ocp.model = model
    
    # 维度设置
    ocp.dims.N = N
    nx = model.x.size()[0]
    nu = model.u.size()[0]
    
    # 成本函数
    Q = np.diag([10, 0.1, 10, 0.1])  # 状态权重
    R = np.diag([0.01])              # 控制量权重
    
    ocp.cost.W = scipy.linalg.block_diag(Q, R)
    ocp.cost.W_e = Q
    
    # 约束
    ocp.constraints.lbu = np.array([-0.6])
    ocp.constraints.ubu = np.array([0.6])
    ocp.constraints.idxbu = np.array([0])
    
    # 求解器选项
    ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
    ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
    ocp.solver_options.integrator_type = 'ERK'
    ocp.solver_options.nlp_solver_type = 'SQP'
    
    # 创建求解器
    acados_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')
    
    return acados_solver

6. 实际应用挑战与解决方案

在将自行车模型应用于实际自动驾驶系统时，会遇到多种挑战：

常见挑战及解决方案表：

挑战	可能原因	解决方案
模型预测偏差大	参数不准确/模型简化过度	在线参数估计/模型自适应
控制器震荡	权重设置不当/时域过短	调整成本函数/增加阻尼项
计算延迟	优化问题复杂	简化模型/代码优化/硬件加速
执行器饱和	控制量超出物理限制	约束处理/抗饱和补偿
路面条件变化	摩擦系数变化	参数自适应/鲁棒控制

一个实用的解决方案是采用分层控制架构：

1. 路径规划层：生成全局参考路径
2. 轨迹生成层：考虑动力学约束生成可行轨迹
3. 控制层：基于自行车模型实现精确跟踪
4. 执行层：处理底层执行器控制

class HierarchicalController:
    def __init__(self):
        self.planner = PathPlanner()
        self.trajectory_gen = TrajectoryGenerator()
        self.mpc = MPCController()
        self.low_level = LowLevelController()
        
    def run_step(self, vehicle_state, perception):
        # 路径规划
        global_path = self.planner.plan(vehicle_state, perception)
        
        # 轨迹生成
        trajectory = self.trajectory_gen.generate(
            vehicle_state, 
            global_path,
            constraints={'max_accel': 2.0, 'max_steer': 0.6}
        )
        
        # MPC控制
        steer_cmd = self.mpc.solve(vehicle_state, trajectory)
        
        # 底层控制
        actuator_cmd = self.low_level.convert(steer_cmd)
        
        return actuator_cmd

在开发过程中，充分的测试验证必不可少。建议采用以下测试策略：

1. 单元测试：验证各个模型组件
2. 闭环仿真：使用CARLA等仿真平台测试整体性能
3. 硬件在环：验证与真实ECU的交互
4. 实车测试：逐步扩大测试场景复杂度