RL-Frenet Trajectory Planning in CARLA - 项目架构与核心算法
一、项目概述
本项目是一个基于 CARLA 自动驾驶模拟器的端到端深度强化学习框架，结合了 Frenet 坐标系轨迹规划 和 深度强化学习算法，实现了自动驾驶车辆的长期和短期规划。
二、项目架构
2.1 整体架构图
┌─────────────────────────────────────────────────────────────────────────────┐
│ CARLA Simulator │
│ ┌─────────────────────────────────────────────────────────────────────┐ │
│ │ World (地图、障碍物、交通参与者) │ │
│ └─────────────────────────────────────────────────────────────────────┘ │
└───────────────────────────────┬─────────────────────────────────────────────┘
│ (Observation: 车辆状态、环境感知)
▼
┌─────────────────────────────────────────────────────────────────────────────┐
│ RL Agent (强化学习代理) │
│ │
│ ┌─────────────────────────────────────────────────────────────────────┐ │
│ │ Gym Environment Wrapper │ │
│ │ - 状态空间定义 (Observation Space) │ │
│ │ - 动作空间定义 (Action Space) │ │
│ │ - 奖励函数设计 (Reward Function) │ │
│ └─────────────────────────────────────────────────────────────────────┘ │
│ │ │
│ ▼ │
│ ┌─────────────────────────────────────────────────────────────────────┐ │
│ │ Reinforcement Learning Algorithms │ │
│ │ ┌─────────┐ ┌─────────┐ ┌─────────┐ ┌─────────┐ ┌─────────┐ │ │
│ │ │ DDPG │ │ TRPO │ │ A2C │ │ PPO2 │ │ ACER │ │ │
│ │ └────┬────┘ └────┬────┘ └────┬────┘ └────┬────┘ └────┬────┘ │ │
│ │ └───────────┴───────────┴───────────┴───────────┘ │ │
│ │ │ │ │
│ │ ▼ │ │
│ │ ┌─────────────────────────────────────────────────────────────┐ │ │
│ │ │ Policy Network │ │ │
│ │ │ - MLP/CNN 特征提取 │ │ │
│ │ │ - Actor: 动作输出 (车道变更、目标速度) │ │ │
│ │ │ - Critic: 值函数评估 │ │ │
│ │ └─────────────────────────────────────────────────────────────┘ │ │
│ └─────────────────────────────────────────────────────────────────────┘ │
│ │ (Action: 车道变更、目标速度) │
▼
┌─────────────────────────────────────────────────────────────────────────────┐
│ Local Planner (局部规划器) │
│ │
│ ┌─────────────────────────────────────────────────────────────────────┐ │
│ │ Frenet Optimal Trajectory Planner │ │
│ │ ┌─────────────────────────────────────────────────────────────┐ │ │
│ │ │ 1. Cubic Spline 全局路径拟合 │ │ │
│ │ │ 2. Frenet 状态估计 (s, s_d, s_dd, d, d_d, d_dd) │ │ │
│ │ │ 3. 候选轨迹生成 (五次/四次多项式) │ │ │
│ │ │ 4. 碰撞检测与可行性筛选 │ │ │
│ │ │ 5. 最优轨迹选择 (代价函数) │ │ │
│ │ └─────────────────────────────────────────────────────────────┘ │ │
│ └─────────────────────────────────────────────────────────────────────┘ │
│ │ (最优轨迹: 全局坐标路径) │
▼
┌─────────────────────────────────────────────────────────────────────────────┐
│ Low Level Controller (低级控制器) │
│ │
│ ┌─────────────────────────────────────────────────────────────────────┐ │
│ │ VehiclePIDController │ │
│ │ ┌──────────────────┐ ┌──────────────────┐ │ │
│ │ │ PIDLongitudinal │ │ PIDLateral │ │ │
│ │ │ - 油门/刹车控制 │ │ - 方向盘控制 │ │ │
│ │ └──────────────────┘ └──────────────────┘ │ │
│ └─────────────────────────────────────────────────────────────────────┘ │
│ │ (Control Command) │
▼
┌─────────────────────────────────────────────────────────────────────────────┐
│ CARLA Vehicle Actor │
│ - apply_control() 执行控制指令 │
│ - get_location() 获取车辆位置 │
│ - get_transform() 获取车辆变换 │
└─────────────────────────────────────────────────────────────────────────────┘2.2 文件夹结构
RL-frenet-trajectory-planning-in-CARLA/
├── agents/ # 核心代理模块
│ ├── __init__.py
│ ├── local_planner/ # 局部轨迹规划器
│ │ ├── cubic_spline_planner.py # 三次样条曲线拟合
│ │ └── frenet_optimal_trajectory.py # Frenet最优轨迹生成
│ ├── low_level_controller/ # 低级控制器
│ │ └── controller.py # PID控制器实现
│ └── reinforcement_learning/ # 强化学习模块
│ └── stable_baselines/ # stable-baselines库集成
│ ├── __init__.py # RL算法导出
│ ├── a2c/ # A2C算法实现
│ ├── acer/ # ACER算法实现
│ ├── acktr/ # ACKTR算法实现
│ ├── common/ # 通用组件
│ │ ├── policies.py # 策略网络定义
│ │ ├── base_class.py # RL模型基类
│ │ ├── distributions.py # 概率分布
│ │ ├── runners.py # 环境运行器
│ │ └── vec_env/ # 向量环境
│ └── ... # 其他算法
├── tools/ # 工具模块
│ ├── cfgs/ # 配置文件
│ │ └── config.yaml # 环境与RL配置
│ └── modules.py # 模块包装器
├── carla_gym/ # CARLA Gym环境接口
│ └── envs/ # 环境定义
├── run.py # 训练/测试入口
├── monitor_plot.py # 结果可视化
├── config.py # 配置加载
└── README.md # 项目说明三、核心算法组件详解
3.1 Cubic Spline Planner (三次样条规划器)
文件位置 ：agents/local_planner/cubic_spline_planner.py
作用：将离散的全局路径点拟合成平滑的连续曲线，用于后续的 Frenet 坐标系转换和轨迹生成。
核心类：
| 类名 | 功能 | 关键方法 |
|---|---|---|
Spline |
一维三次样条 | calc(t) - 计算位置 |
calcd(t) - 计算一阶导数 |
||
calcdd(t) - 计算二阶导数 |
||
Spline2D |
二维三次样条 | calc_position(s) - 计算(x,y)位置 |
calc_yaw(s) - 计算航向角 |
||
calc_curvature(s) - 计算曲率 |
||
Spline3D |
三维三次样条 | calc_position(s) - 计算(x,y,z)位置 |
calc_pitch(s) - 计算俯仰角 |
算法原理：
- 给定一系列控制点
(x, y)，通过求解线性方程组得到样条曲线系数 - 使用自然边界条件（端点二阶导数为0）
- 曲线表达式：
y(x) = a + bx + cx² + dx³
关键代码 ：Spline2D.calc_yaw() 通过一阶导数计算航向角：
python
def calc_yaw(self, s):
dx = self.sx.calcd(s)
dy = self.sy.calcd(s)
yaw = math.atan2(dy, dx)
return yaw3.2 Frenet Optimal Trajectory Planner (Frenet最优轨迹规划器)
文件位置 ：agents/local_planner/frenet_optimal_trajectory.py
作用：在 Frenet 坐标系下生成多条候选轨迹，通过代价函数选择最优轨迹。
核心类：
| 类名 | 功能 | 关键属性 |
|---|---|---|
quintic_polynomial |
五次多项式（横向运动） | a0-a5 - 多项式系数 |
quartic_polynomial |
四次多项式（纵向运动） | a0-a4 - 多项式系数 |
Frenet_path |
Frenet路径数据结构 | s,d - Frenet坐标；x,y - 全局坐标 |
FrenetPlanner |
Frenet规划器主类 | 轨迹生成、碰撞检测、最优选择 |
Frenet 坐标系：
s：沿参考路径的纵向距离d：垂直于参考路径的横向偏移- 状态向量：
[s, s_d, s_dd, d, d_d, d_dd]
轨迹生成流程：
1. 全局路径拟合 (Spline3D)
↓
2. Frenet状态估计 (基于车辆当前位置)
↓
3. 候选轨迹生成 (横向五次多项式 + 纵向四次多项式)
↓
4. Frenet→全局坐标转换
↓
5. 曲率计算
↓
6. 可行性筛选 (速度/加速度/曲率/碰撞检测)
↓
7. 最优轨迹选择 (代价函数最小化)代价函数：
cf = KLAT * cd + KLON * cv
cd (横向代价) = KJ * Jp + KT * Ti + KD * (d_final - target_d)²
cv (纵向代价) = KJ * Js + KT * Ti + KD * (v_final - target_v)²
其中：
- Jp/Js：横向/纵向加加速度的平方和 (舒适性)
- Ti：轨迹时间 (效率)
- d_final - target_d：横向偏移误差 (准确性)
- v_final - target_v：速度误差 (准确性)关键方法：
| 方法 | 功能 |
|---|---|
estimate_frenet_state() |
根据车辆状态估计当前 Frenet 坐标 |
calc_frenet_paths() |
生成候选 Frenet 轨迹集合 |
calc_global_paths() |
Frenet→全局坐标转换 |
check_collision() |
碰撞检测 |
frenet_optimal_planning() |
完整的最优轨迹规划流程 |
run_step_single_path() |
基于 RL 动作生成单条轨迹 |
3.3 Low Level Controller (低级控制器)
文件位置 ：agents/low_level_controller/controller.py
作用：将规划器输出的轨迹转换为车辆控制指令（油门、刹车、方向盘）。
核心类：
| 类名 | 功能 | 参数 |
|---|---|---|
VehiclePIDController |
车辆PID控制器组合 | 整合横向+纵向控制 |
PIDLongitudinalController |
纵向PID控制器 | K_P=40.0, K_D=0.1, K_I=4 |
PIDLateralController |
横向PID控制器 | K_P=0.3, K_D=0.0, K_I=0.0 |
PIDCrossTrackController |
横向偏差PID控制 | 可配置参数 |
IntelligentDriverModel |
智能驾驶员模型(巡航控制) | 基于IDM模型 |
纵向控制逻辑：
python
def _pid_control(self, target_speed, current_speed):
_e = (target_speed - current_speed) # 速度误差
_de = (e_buffer[-1] - e_buffer[-2]) / dt # 误差变化率
_ie = sum(e_buffer) * dt # 误差积分
throttle = K_P * _e + K_D * _de / dt + K_I * _ie * dt
return clip(throttle, 0.0, 1.0)横向控制逻辑：
- 通过计算车辆朝向向量与目标点向量的夹角作为控制误差
- 使用叉积判断左右方向
- 输出方向盘转角 -1, 1
3.4 Reinforcement Learning Algorithms (强化学习算法)
文件位置 ：agents/reinforcement_learning/stable_baselines/
支持算法：
| 算法 | 类型 | 适用场景 |
|---|---|---|
| DDPG | 连续动作空间 | 连续控制任务 |
| TRPO | 策略优化 | 稳定收敛 |
| A2C | 优势演员-评论家 | 高效训练 |
| PPO2 | 近端策略优化 | 稳定高效 |
| ACER | 样本高效 | 减少样本消耗 |
A2C 算法核心实现 (a2c/a2c.py)：
损失函数：
loss = pg_loss - entropy * ent_coef + vf_loss * vf_coef
其中：
- pg_loss = -E[advantage * log_prob] (策略梯度损失)
- vf_loss = MSE(value_prediction, target_value) (值函数损失)
- entropy = 策略熵 (鼓励探索)训练流程：
1. 收集 n_steps 步的轨迹数据
2. 计算 discounted rewards (GAE)
3. 计算优势函数 advantage = rewards - values
4. 更新策略网络和值网络
5. 重复直到达到总步数策略网络 (common/policies.py)：
| 网络类型 | 功能 |
|---|---|
nature_cnn |
Nature论文CNN结构 (图像处理) |
sequence_1d_cnn |
1D序列CNN (时序数据) |
sequence_1d_cnn_ego_bypass_tc |
带ego旁路的1D CNN |
mlp_extractor |
MLP特征提取器 |
四、数据流与交互流程
4.1 训练模式数据流
CARLA环境 → Observation → Gym环境包装 → RL算法 → Action
↑ │
│ ▼
apply_control ← ControlCommand ← LowLevelController ← Trajectory ← LocalPlanner4.2 关键数据结构
Observation (状态空间)：
- 车辆自身状态：位置、速度、加速度、航向角
- 周围车辆状态：相对距离、相对速度
- Frenet 状态：s, s_d, s_dd, d, d_d, d_dd
- 全局路径信息：目标路径点
Action (动作空间)：
change_lane：车道变更 (-1=左, 0=保持, 1=右)target_speed：目标速度
Reward (奖励函数)：
- 前进奖励：沿路径方向的位移
- 效率奖励：速度接近目标速度
- 舒适性奖励：平滑驾驶（惩罚大加速度）
- 安全性奖励：碰撞惩罚
- 车道保持奖励：保持在车道内
五、关键配置与参数
5.1 配置文件位置
tools/cfgs/config.yaml：环境和 RL 算法配置
5.2 核心参数
CARLA 仿真参数：
| 参数 | 默认值 | 说明 |
|---|---|---|
| DT | 0.05 | 仿真时间步长(秒) |
| LANE_WIDTH | 3.5 | 车道宽度(米) |
局部规划器参数：
| 参数 | 默认值 | 说明 |
|---|---|---|
| MAX_SPEED | 150/3.6 | 最大速度(米/秒) |
| MAX_ACCEL | 4.0 | 最大加速度(米/秒²) |
| MAX_CURVATURE | 1.0 | 最大曲率(1/米) |
| MAXT | 6.0 | 最大预测时间(秒) |
| MINT | 3.0 | 最小预测时间(秒) |
代价函数权重：
| 参数 | 默认值 | 说明 |
|---|---|---|
| KJ | 0.1 | 加加速度权重 |
| KT | 0.1 | 时间权重 |
| KD | 1.0 | 偏移权重 |
| KLAT | 1.0 | 横向代价权重 |
| KLON | 1.0 | 纵向代价权重 |
六、运行方式
6.1 训练命令
bash
# Terminal 1: 启动 CARLA 服务器
./CarlaUE4.sh -carla-server -fps=20 -world-port=2000
# Terminal 2: 启动训练
python3 run.py --cfg_file=tools/cfgs/config.yaml --agent_id=1 --env=CarlaGymEnv-v16.2 测试命令
bash
# 使用预训练模型测试
python3 run.py --agent_id=1 --env=CarlaGymEnv-v1 --test --play_mode=16.3 预训练模型
| Agent ID | 算法 |
|---|---|
| 1 | DDPG |
| 2 | TRPO |
| 3 | A2C |
| 4 | PPO2 |
七、核心设计亮点
-
Frenet 坐标系规划：将复杂的二维轨迹规划分解为独立的纵向和横向规划问题，简化了问题复杂度
-
多项式轨迹生成：使用五次/四次多项式保证轨迹及其导数的连续性，提高驾驶舒适性
-
端到端 RL 集成：将 RL 的高层决策（车道变更、目标速度）与传统规划器结合，兼顾学习能力和安全性
-
模块化设计：各组件职责清晰，易于替换和扩展
-
Gym 环境封装：符合 OpenAI Gym 标准接口，便于切换不同 RL 算法
八、参考文献
-
Werling, M., et al. "Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame." 2010.
-
Moghadam, M., et al. "An End-to-end Deep Reinforcement Learning Approach for the Long-term Short-term Planning on the Frenet Space." arXiv:2011.13098, 2020.
-
Schulman, J., et al. "Proximal Policy Optimization Algorithms." arXiv:1707.06347, 2017.
-
Lillicrap, T. P., et al. "Continuous Control with Deep Reinforcement Learning." arXiv:1509.02971, 2015.