Coursera自动驾驶课程实战：用Python从零搭建自行车运动学模型（附代码）

自动驾驶技术的核心在于对车辆运动的精确建模与控制。作为Coursera自动驾驶专项课程的实践延伸，本文将带您用Python从零实现自行车运动学模型，涵盖后轴、前轴和重心三种参考点选择，并通过Jupyter Notebook可视化运动轨迹。不同于单纯的理论推导，我们将聚焦于如何将数学公式转化为可运行代码，并解决实际调试中的坐标系转换难题。

1. 环境准备与基础概念

在开始编码前，我们需要配置合适的开发环境并理解自行车模型的基本假设。自行车模型是车辆运动学建模的简化形式，它将四轮车辆简化为两轮模型，保留了转向和前进的基本运动特性。

开发环境配置：

# 推荐使用Anaconda创建虚拟环境
conda create -n vehicle_model python=3.8
conda activate vehicle_model

# 安装必要库
pip install numpy matplotlib ipykernel scipy

自行车模型的三个关键假设：

1. 车辆仅在二维平面运动（忽略垂直方向变化）
2. 前后轮通过刚性连接，无悬挂系统影响
3. 轮胎与地面始终保持纯滚动接触（无滑动）

运动学与动力学区别：

• 运动学：研究物体运动的几何特性，不考虑力和质量
• 动力学：研究力与运动的关系，考虑质量、力矩等因素

提示：本文专注于运动学建模，适合作为动力学建模的前导知识。实际自动驾驶系统需要结合两者。

2. 坐标系转换原理与实现

车辆运动涉及多个坐标系转换，这是建模中最易出错的环节。我们需要明确以下坐标系：

1. 世界坐标系（World Frame）：固定的全局参考系
2. 车身坐标系（Body Frame）：固定在车辆上的局部参考系
3. 轮轴坐标系（Wheel Frame）：各车轮的独立参考系

坐标转换矩阵实现：

import numpy as np

def rotation_matrix(theta):
    """创建2D旋转矩阵"""
    return np.array([
        [np.cos(theta), -np.sin(theta)],
        [np.sin(theta),  np.cos(theta)]
    ])

def transform_point(point, translation, rotation):
    """将点从一个坐标系转换到另一个坐标系"""
    R = rotation_matrix(rotation)
    return R @ point + translation

常见的坐标系转换问题及解决方案：

• 角度单位混淆（弧度vs角度）：统一使用弧度制
• 旋转方向定义：确定正方向约定（通常逆时针为正）
• 链式转换顺序：明确转换的先后关系

3. 后轴参考点模型实现

后轴参考点是自行车模型中最常用的建模方式，其核心思想是将后轮中心作为车辆位置参考。

运动学方程：

ẋ = v * cos(θ)
ẏ = v * sin(θ)
θ̇ = v * tan(δ) / L

其中：

• (x,y)为后轴中心坐标
• θ为车辆朝向角
• v为后轴中心速度
• δ为前轮转向角
• L为轴距（前后轮距离）

Python实现：

class RearWheelBicycleModel:
    def __init__(self, L=2.5):
        self.L = L  # 轴距
        
    def update(self, state, v, delta, dt):
        x, y, theta = state
        new_theta = theta + (v * np.tan(delta) / self.L) * dt
        new_x = x + v * np.cos(theta) * dt
        new_y = y + v * np.sin(theta) * dt
        return np.array([new_x, new_y, new_theta])

轨迹模拟示例：

def simulate_trajectory(model, initial_state, controls, dt=0.1):
    states = [initial_state]
    for v, delta in controls:
        new_state = model.update(states[-1], v, delta, dt)
        states.append(new_state)
    return np.array(states)

后轴模型的优缺点分析：

• 优点：计算简单，直观反映车辆运动
• 缺点：在低速大转向角时可能出现奇点

4. 前轴与重心参考点模型

4.1 前轴参考点模型

前轴模型将参考点置于前轮中心，适用于需要精确控制转向的场景。

运动学方程：

ẋ = v * cos(θ + δ)
ẏ = v * sin(θ + δ)
θ̇ = v * sin(δ) / L

Python实现差异：

class FrontWheelBicycleModel:
    def update(self, state, v, delta, dt):
        x, y, theta = state
        new_theta = theta + (v * np.sin(delta) / self.L) * dt
        new_x = x + v * np.cos(theta + delta) * dt
        new_y = y + v * np.sin(theta + delta) * dt
        return np.array([new_x, new_y, new_theta])

4.2 重心参考点模型

重心模型更接近真实车辆行为，引入了侧偏角β的概念。

运动学方程：

ẋ = v * cos(θ + β)
ẏ = v * sin(θ + β)
θ̇ = v * cos(β) * tan(δ) / L
β = arctan(lr * tan(δ) / L)

其中lr为重心到后轴距离

Python实现：

class COGBicycleModel:
    def __init__(self, L=2.5, lr=1.2):
        self.L = L
        self.lr = lr  # 重心到后轴距离
        
    def update(self, state, v, delta, dt):
        x, y, theta = state
        beta = np.arctan(self.lr * np.tan(delta) / self.L)
        new_theta = theta + (v * np.cos(beta) * np.tan(delta) / self.L) * dt
        new_x = x + v * np.cos(theta + beta) * dt
        new_y = y + v * np.sin(theta + beta) * dt
        return np.array([new_x, new_y, new_theta])

三种模型的对比：

特性	后轴模型	前轴模型	重心模型
计算复杂度	低	中	高
低速精度	一般	较好	最好
转向敏感性	高	中	低
适用场景	高速路径跟踪	精确转向控制	全工况模拟

5. 轨迹可视化与调试技巧

使用Matplotlib实现交互式轨迹可视化：

import matplotlib.pyplot as plt
from matplotlib.widgets import Slider

def plot_trajectory(states, title):
    plt.figure(figsize=(10, 6))
    plt.plot(states[:,0], states[:,1], 'b-', linewidth=2)
    plt.xlabel('X position (m)')
    plt.ylabel('Y position (m)')
    plt.title(title)
    plt.axis('equal')
    plt.grid(True)
    
    # 绘制初始和最终朝向
    draw_vehicle(states[0], 'g')
    draw_vehicle(states[-1], 'r')
    plt.show()

def draw_vehicle(state, color):
    x, y, theta = state
    plt.plot(x, y, color+'o', markersize=10)
    plt.arrow(x, y, 0.5*np.cos(theta), 0.5*np.sin(theta), 
              head_width=0.2, head_length=0.3, fc=color, ec=color)

常见调试问题解决：

1. 轨迹发散或不连续：

   • 检查时间步长dt是否过大
   • 验证角度单位（确保使用弧度）
   • 检查三角函数参数顺序

2. 转向响应异常：

   • 确认转向角δ的范围（通常限制在±30度内）
   • 检查轴距L的取值是否合理

3. 数值不稳定：

   • 增加状态更新的频率（减小dt）
   • 对极端输入值进行限幅处理

注意：实际车辆运动存在物理限制，如最大转向角、加速度限制等，在仿真中应考虑这些约束。

6. 进阶应用：与控制器集成

运动学模型常作为控制器设计的基础。以下是一个简单的纯追踪控制器示例：

class PurePursuitController:
    def __init__(self, L=2.5, lookahead=3.0):
        self.L = L
        self.lookahead = lookahead
        
    def compute_steering(self, state, path):
        x, y, theta = state
        # 寻找路径上最近点
        distances = np.sqrt((path[:,0]-x)**2 + (path[:,1]-y)**2)
        idx = np.argmin(distances)
        
        # 选择前视点
        lookahead_idx = min(idx + 5, len(path)-1)
        target = path[lookahead_idx]
        
        # 计算转向角
        alpha = np.arctan2(target[1]-y, target[0]-x) - theta
        delta = np.arctan(2 * self.L * np.sin(alpha) / self.lookahead)
        return delta

控制器与模型的集成示例：

def closed_loop_simulation(model, controller, path, initial_state, dt=0.1):
    states = [initial_state]
    current_state = initial_state
    
    for _ in range(100):  # 模拟100步
        v = 2.0  # 固定速度
        delta = controller.compute_steering(current_state, path)
        current_state = model.update(current_state, v, delta, dt)
        states.append(current_state)
        
    return np.array(states)

7. 性能优化与工程实践

在实际应用中，我们需要考虑代码的执行效率和数值稳定性：

性能优化技巧：

1. 使用向量化运算替代循环
2. 预计算不变参数
3. 利用Numba加速数值计算

from numba import jit

@jit(nopython=True)
def update_state_numba(state, v, delta, dt, L):
    x, y, theta = state
    new_theta = theta + (v * np.tan(delta) / L) * dt
    new_x = x + v * np.cos(theta) * dt
    new_y = y + v * np.sin(theta) * dt
    return np.array([new_x, new_y, new_theta])

工程实践建议：

• 为模型参数添加合理的默认值
• 实现输入参数的验证检查
• 添加详细的文档字符串
• 创建单元测试验证核心功能

def test_rear_wheel_model():
    model = RearWheelBicycleModel(L=2.5)
    state = np.array([0, 0, 0])
    
    # 直行测试
    new_state = model.update(state, v=1.0, delta=0, dt=1.0)
    assert np.allclose(new_state, [1, 0, 0])
    
    # 转向测试
    new_state = model.update(state, v=1.0, delta=np.radians(10), dt=1.0)
    expected_theta = np.tan(np.radians(10))/2.5
    assert abs(new_state[2] - expected_theta) < 1e-6

8. 扩展思考与实际应用

掌握了基础自行车模型后，可以考虑以下扩展方向：

1. 加入动力学因素：

   • 轮胎滑移模型
   • 质量分布影响
   • 悬挂系统效应

2. 复杂场景建模：

   • 坡道行驶
   • 低附着路面
   • 紧急避障

3. 与感知系统集成：

   • 结合视觉或激光雷达数据
   • 实时参数估计
   • 自适应模型调整

实际工程中，运动学模型常用于：

• 路径规划算法的可行性检查
• 控制器设计的基准测试
• 传感器数据的时间对齐
• 系统级的快速原型开发