自动驾驶车辆运动学模型 Python 实现单车模型 3 个核心方程与 2 种仿真场景在自动驾驶算法开发中车辆运动学模型是路径跟踪和轨迹规划的基础。不同于复杂的动力学模型运动学模型更关注几何关系适合低速场景下的控制算法验证。本文将用Python实现一个精简的单车模型并通过8字绕环和泊车轨迹两个典型场景展示其应用价值。1. 单车模型的核心方程推导单车模型Bicycle Model是四轮车辆的简化表示它将前后轮各视为单个车轮。这种简化在低速场景下通常小于5m/s具有足够精度且能大幅降低计算复杂度。模型建立基于三个关键假设忽略轮胎侧偏特性即车轮转向方向即为速度方向仅考虑平面运动X/Y位置和横摆角后轮转向角为零δr0核心运动学方程推导结果如下# 状态更新方程 x_dot v * cos(φ) # X轴速度分量 y_dot v * sin(φ) # Y轴速度分量 φ_dot (v * tan(δf)) / L # 横摆角速度其中关键参数定义符号物理意义单位v车辆质心速度m/sφ车辆横摆角radδf前轮转向角radL车辆轴距m注意实际编程时需要处理角度单位转换特别是当使用度数输入转向角时需先转换为弧度制。2. Python 类实现与数值积分我们采用面向对象方式封装模型使用欧拉积分法进行状态更新。这种离散化方法虽然简单但对于演示目的已经足够。import numpy as np import matplotlib.pyplot as plt class BicycleModel: def __init__(self, L2.8, dt0.1): :param L: 轴距(m) :param dt: 仿真步长(s) self.L L self.dt dt self.reset_state() def reset_state(self): 重置车辆状态 self.x 0 # X坐标(m) self.y 0 # Y坐标(m) self.phi 0 # 横摆角(rad) self.v 0 # 速度(m/s) def update(self, v, delta): 状态更新 :param v: 速度指令(m/s) :param delta: 前轮转角(rad) # 限制转向角度在物理合理范围内 delta np.clip(delta, -np.pi/4, np.pi/4) # 欧拉积分 self.x v * np.cos(self.phi) * self.dt self.y v * np.sin(self.phi) * self.dt self.phi (v * np.tan(delta) / self.L) * self.dt self.v v return self.x, self.y, self.phi关键实现细节使用np.clip限制转向角在±45°范围内模拟真实车辆的机械限制状态变量采用国际单位制米、弧度提供reset_state方法方便多次仿真3. 8字绕环场景仿真8字轨迹是验证控制算法的经典场景能同时测试转向系统的左右对称性能。我们通过给定速度和前轮转角序列来生成参考轨迹。def simulate_figure8(): model BicycleModel(L2.8, dt0.05) traj [] # 参数配置 duration 60 # 总时长(s) steps int(duration / model.dt) v 2.0 # 恒定速度(m/s) for t in np.linspace(0, duration, steps): # 生成周期性转向指令 delta 0.5 * np.sin(2 * np.pi * t / 20) model.update(v, delta) traj.append((model.x, model.y)) # 可视化 x, y zip(*traj) plt.figure(figsize(10, 6)) plt.plot(x, y, label车辆轨迹) plt.xlabel(X位置 (m)) plt.ylabel(Y位置 (m)) plt.title(8字绕环仿真轨迹) plt.grid(True) plt.axis(equal) plt.legend() plt.show()执行结果将呈现对称的8字形轨迹。调整delta的振幅和频率可以改变8字的大小和形状这为后续开发轨迹跟踪控制器提供了测试基准。4. 自动泊车场景实现平行泊车是城市自动驾驶的典型场景。我们设计一个分阶段控制策略初始定位阶段车辆以恒定速度直线行驶到起始位置倒车转向阶段最大转向角倒车回正调整阶段反向转向完成泊入def simulate_parking(): model BicycleModel(L2.8, dt0.1) traj [] # 阶段1直线行驶3秒 for _ in range(30): model.update(1.5, 0) traj.append((model.x, model.y, model.phi)) # 阶段2最大转向角倒车 for _ in range(25): model.update(-1.0, np.pi/4) traj.append((model.x, model.y, model.phi)) # 阶段3反向转向调整 for _ in range(25): model.update(-1.0, -np.pi/4) traj.append((model.x, model.y, model.phi)) # 可视化 x, y, phi zip(*traj) plt.figure(figsize(12, 6)) plt.plot(x, y, label泊车轨迹) # 绘制最终姿态 end_idx -1 plt.quiver(x[end_idx], y[end_idx], np.cos(phi[end_idx]), np.sin(phi[end_idx]), scale10, colorr, label最终朝向) plt.xlabel(X位置 (m)) plt.ylabel(Y位置 (m)) plt.title(平行泊车仿真) plt.grid(True) plt.axis(equal) plt.legend() plt.show()泊车轨迹分析红色箭头表示车辆最终朝向通过调整各阶段的持续时间和转向角可以优化泊车最终位置实际应用中需要加入障碍物检测和轨迹修正逻辑5. 模型扩展与工程实践建议基础单车模型在实际工程中通常需要以下扩展1. 增加状态约束# 在update方法中添加速度限制 v np.clip(v, -3.0, 10.0) # 限制倒车和前进最大速度 # 添加转向速率限制 max_delta_rate np.pi/8 # 最大转向速度(rad/s) delta np.clip(delta, last_delta - max_delta_rate, last_delta max_delta_rate)2. 多坐标系转换工具def vehicle_to_world(x, y, phi, points): 将车辆坐标系下的点转换到世界坐标系 :param points: Nx2数组车辆坐标系下的坐标 :return: 世界坐标系下的坐标 rot np.array([[np.cos(phi), -np.sin(phi)], [np.sin(phi), np.cos(phi)]]) return (rot points.T).T np.array([x, y])3. 可视化增强def plot_vehicle(x, y, phi, colorb): 绘制车辆矩形表示 car_length 4.0 car_width 2.0 # 定义车辆轮廓车辆坐标系 corners np.array([ [car_length/2, car_width/2], [car_length/2, -car_width/2], [-car_length/2, -car_width/2], [-car_length/2, car_width/2] ]) # 转换到世界坐标系 world_corners vehicle_to_world(x, y, phi, corners) plt.plot(*zip(*world_corners, world_corners[0]), colorcolor)在实车调试中运动学模型的主要局限在于高速场景下轮胎侧偏效应显著。这时需要升级到动力学模型但单车模型仍可作为快速原型设计和算法验证的有力工具。
自动驾驶车辆运动学模型 Python 实现：单车模型 3 个核心方程与 2 种仿真场景
自动驾驶车辆运动学模型 Python 实现单车模型 3 个核心方程与 2 种仿真场景在自动驾驶算法开发中车辆运动学模型是路径跟踪和轨迹规划的基础。不同于复杂的动力学模型运动学模型更关注几何关系适合低速场景下的控制算法验证。本文将用Python实现一个精简的单车模型并通过8字绕环和泊车轨迹两个典型场景展示其应用价值。1. 单车模型的核心方程推导单车模型Bicycle Model是四轮车辆的简化表示它将前后轮各视为单个车轮。这种简化在低速场景下通常小于5m/s具有足够精度且能大幅降低计算复杂度。模型建立基于三个关键假设忽略轮胎侧偏特性即车轮转向方向即为速度方向仅考虑平面运动X/Y位置和横摆角后轮转向角为零δr0核心运动学方程推导结果如下# 状态更新方程 x_dot v * cos(φ) # X轴速度分量 y_dot v * sin(φ) # Y轴速度分量 φ_dot (v * tan(δf)) / L # 横摆角速度其中关键参数定义符号物理意义单位v车辆质心速度m/sφ车辆横摆角radδf前轮转向角radL车辆轴距m注意实际编程时需要处理角度单位转换特别是当使用度数输入转向角时需先转换为弧度制。2. Python 类实现与数值积分我们采用面向对象方式封装模型使用欧拉积分法进行状态更新。这种离散化方法虽然简单但对于演示目的已经足够。import numpy as np import matplotlib.pyplot as plt class BicycleModel: def __init__(self, L2.8, dt0.1): :param L: 轴距(m) :param dt: 仿真步长(s) self.L L self.dt dt self.reset_state() def reset_state(self): 重置车辆状态 self.x 0 # X坐标(m) self.y 0 # Y坐标(m) self.phi 0 # 横摆角(rad) self.v 0 # 速度(m/s) def update(self, v, delta): 状态更新 :param v: 速度指令(m/s) :param delta: 前轮转角(rad) # 限制转向角度在物理合理范围内 delta np.clip(delta, -np.pi/4, np.pi/4) # 欧拉积分 self.x v * np.cos(self.phi) * self.dt self.y v * np.sin(self.phi) * self.dt self.phi (v * np.tan(delta) / self.L) * self.dt self.v v return self.x, self.y, self.phi关键实现细节使用np.clip限制转向角在±45°范围内模拟真实车辆的机械限制状态变量采用国际单位制米、弧度提供reset_state方法方便多次仿真3. 8字绕环场景仿真8字轨迹是验证控制算法的经典场景能同时测试转向系统的左右对称性能。我们通过给定速度和前轮转角序列来生成参考轨迹。def simulate_figure8(): model BicycleModel(L2.8, dt0.05) traj [] # 参数配置 duration 60 # 总时长(s) steps int(duration / model.dt) v 2.0 # 恒定速度(m/s) for t in np.linspace(0, duration, steps): # 生成周期性转向指令 delta 0.5 * np.sin(2 * np.pi * t / 20) model.update(v, delta) traj.append((model.x, model.y)) # 可视化 x, y zip(*traj) plt.figure(figsize(10, 6)) plt.plot(x, y, label车辆轨迹) plt.xlabel(X位置 (m)) plt.ylabel(Y位置 (m)) plt.title(8字绕环仿真轨迹) plt.grid(True) plt.axis(equal) plt.legend() plt.show()执行结果将呈现对称的8字形轨迹。调整delta的振幅和频率可以改变8字的大小和形状这为后续开发轨迹跟踪控制器提供了测试基准。4. 自动泊车场景实现平行泊车是城市自动驾驶的典型场景。我们设计一个分阶段控制策略初始定位阶段车辆以恒定速度直线行驶到起始位置倒车转向阶段最大转向角倒车回正调整阶段反向转向完成泊入def simulate_parking(): model BicycleModel(L2.8, dt0.1) traj [] # 阶段1直线行驶3秒 for _ in range(30): model.update(1.5, 0) traj.append((model.x, model.y, model.phi)) # 阶段2最大转向角倒车 for _ in range(25): model.update(-1.0, np.pi/4) traj.append((model.x, model.y, model.phi)) # 阶段3反向转向调整 for _ in range(25): model.update(-1.0, -np.pi/4) traj.append((model.x, model.y, model.phi)) # 可视化 x, y, phi zip(*traj) plt.figure(figsize(12, 6)) plt.plot(x, y, label泊车轨迹) # 绘制最终姿态 end_idx -1 plt.quiver(x[end_idx], y[end_idx], np.cos(phi[end_idx]), np.sin(phi[end_idx]), scale10, colorr, label最终朝向) plt.xlabel(X位置 (m)) plt.ylabel(Y位置 (m)) plt.title(平行泊车仿真) plt.grid(True) plt.axis(equal) plt.legend() plt.show()泊车轨迹分析红色箭头表示车辆最终朝向通过调整各阶段的持续时间和转向角可以优化泊车最终位置实际应用中需要加入障碍物检测和轨迹修正逻辑5. 模型扩展与工程实践建议基础单车模型在实际工程中通常需要以下扩展1. 增加状态约束# 在update方法中添加速度限制 v np.clip(v, -3.0, 10.0) # 限制倒车和前进最大速度 # 添加转向速率限制 max_delta_rate np.pi/8 # 最大转向速度(rad/s) delta np.clip(delta, last_delta - max_delta_rate, last_delta max_delta_rate)2. 多坐标系转换工具def vehicle_to_world(x, y, phi, points): 将车辆坐标系下的点转换到世界坐标系 :param points: Nx2数组车辆坐标系下的坐标 :return: 世界坐标系下的坐标 rot np.array([[np.cos(phi), -np.sin(phi)], [np.sin(phi), np.cos(phi)]]) return (rot points.T).T np.array([x, y])3. 可视化增强def plot_vehicle(x, y, phi, colorb): 绘制车辆矩形表示 car_length 4.0 car_width 2.0 # 定义车辆轮廓车辆坐标系 corners np.array([ [car_length/2, car_width/2], [car_length/2, -car_width/2], [-car_length/2, -car_width/2], [-car_length/2, car_width/2] ]) # 转换到世界坐标系 world_corners vehicle_to_world(x, y, phi, corners) plt.plot(*zip(*world_corners, world_corners[0]), colorcolor)在实车调试中运动学模型的主要局限在于高速场景下轮胎侧偏效应显著。这时需要升级到动力学模型但单车模型仍可作为快速原型设计和算法验证的有力工具。
