RL-frenet-trajectory-planning-in-CARLA-拓冰建站

RL-Frenet Trajectory Planning in CARLA - 项目架构与核心算法

一、项目概述

本项目是一个基于 CARLA 自动驾驶模拟器的端到端深度强化学习框架，结合了Frenet 坐标系轨迹规划和深度强化学习算法，实现了自动驾驶车辆的长期和短期规划。

核心论文：An End-to-end Deep Reinforcement Learning Approach for the Long-term Short-term Planning on the Frenet Space

二、项目架构

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()通过一阶导数计算航向角：

defcalc_yaw(self,s):dx=self.sx.calcd(s)dy=self.sy.calcd(s)yaw=math.atan2(dy,dx)returnyaw

3.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模型

纵向控制逻辑：

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*dtreturnclip(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 ← LocalPlanner

4.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 训练命令

# 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-v1

6.2 测试命令

# 使用预训练模型测试python3 run.py--agent_id=1--env=CarlaGymEnv-v1--test--play_mode=1

6.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.