自动驾驶轨迹规划DFTPAV：非结构化环境下的时空联合优化

1. 项目概述与背景

今天要拆解的是来自浙江大学FAST实验室高飞团队的自动驾驶轨迹规划项目DFTPAV（Dynamic Feasible Trajectory Planner for Autonomous Vehicles）。这个项目主要解决非结构化环境（如停车场、施工区域等缺乏明确车道线的场景）下的自动驾驶轨迹规划问题。我在实际部署和测试过程中发现，相比传统方法，这套方案在复杂场景的实时性和轨迹平滑性上有显著提升。

项目代码结构清晰，采用ROS框架实现模块化设计。核心创新点在于时空联合优化算法，能够在动态障碍物存在的环境中生成符合车辆动力学约束的可行轨迹。下面我会结合代码结构、运行逻辑和实际调试经验，带大家深入理解这套系统的设计精髓。

2. 环境配置与运行准备

2.1 基础环境搭建

项目需要ROS melodic或noetic环境（推荐Ubuntu 18.04/20.04）。在配置环境时，建议使用以下命令安装依赖：

bash复制sudo apt-get install ros-$ROS_DISTRO-navigation ros-$ROS_DISTRO-rviz \
ros-$ROS_DISTRO-hector-slam ros-$ROS_DISTRO-ackermann-msgs \
protobuf-compiler libode-dev

注意：如果使用Ubuntu 20.04，需要手动安装libode8（默认源中版本不兼容），建议从源码编译安装。

2.2 代码编译要点

克隆仓库后，编译过程有几个关键点需要注意：

1. Protobuf文件需要先编译：

bash复制cd src/Plan/traj_planner/proto
protoc --cpp_out=. minco_config.proto

2. 第三方库odeint-v2需要手动链接：

bash复制sudo ln -s thirdparty/odeint-v2/include/odeint /usr/local/include/

3. 编译时建议使用以下命令避免内存不足：

bash复制catkin_make -j2  # 限制并行编译线程数

3. 代码架构深度解析

3.1 核心模块划分

项目采用经典的感知-决策-规划架构，代码主要分为两大模块：

3.2 Plan模块实现细节

3.2.1 轨迹规划核心流程

1. 路径搜索层（kino_astar.cpp）：

   • 使用改进的Kinodynamic A*算法
   • 考虑车辆运动学约束（最小转弯半径等）
   • 输出粗粒度可行路径

2. 轨迹优化层（traj_optimizer.cpp）：

   cpp复制void MINCOptimizer::optimizeTrajectory(
       const Eigen::MatrixXd& iniState,
       const Eigen::MatrixXd& finState,
       const std::vector<Eigen::Vector3d>& waypoints) {
       // 使用MINCO方法进行时空优化
       // 包含动力学约束、障碍物避碰、舒适性代价等
   }
   

3. 管理调度层（traj_manager.cpp）：

   • 处理ROS消息回调
   • 协调各子模块执行顺序
   • 异常状态处理

3.2.2 关键算法实现

项目中采用的MINCO（Minimum Control Effort）优化算法有以下几个创新点：

1. 时空解耦的代价函数设计：

   math复制J = w_t·J_time + w_s·J_smooth + w_o·J_obs + w_d·J_dyn
   

2. 基于B样条的轨迹表示：

   • 允许局部调整而不影响全局轨迹
   • 天然满足连续性约束

3. 障碍物处理采用ESDF（欧几里得符号距离场）：

   cpp复制void MapAdapter::updateESDF() {
       // 构建3D距离场用于快速碰撞检测
   }
   

3.3 Sim模块设计剖析

3.3.1 物理仿真器

phy_simulator_parking.launch启动的仿真器包含：

• 车辆动力学模型（基于ODE物理引擎）
• 轮胎-地面接触模型
• 执行器延迟模拟

3.3.2 行为规划器

behavior_planner实现了一套有限状态机：

mermaid复制stateDiagram
    [*] --> IDLE
    IDLE --> CRUISE: 收到目标点
    CRUISE --> LANE_CHANGE: 检测到慢车
    LANE_CHANGE --> CRUISE: 完成换道
    CRUISE --> PARKING: 到达停车区

注：实际代码中状态转换条件更为复杂，包含22种过渡条件判断

4. 实战调试经验分享

4.1 参数调优指南

在config/minco_config.pb.txt中有几个关键参数需要特别注意：

4.2 常见问题排查

1. 规划轨迹抖动：

   • 检查ESDF地图更新频率（建议≥10Hz）
   • 调整traj_optimizer中的平滑项权重
   • 确认IMU数据时间戳对齐

2. 仿真中车辆跑偏：

   bash复制rostopic echo /vehicle/odom  # 检查里程计数据
   rosrun rqt_reconfigure rqt_reconfigure  # 调整轮胎摩擦系数
   

3. 启动时崩溃：

   • 确认protobuf版本一致（要求3.12+）
   • 检查ROS时间同步：

     bash复制rosparam set /use_sim_time true
     

5. 进阶开发建议

5.1 自定义场景扩展

要添加新场景，需要修改以下文件：

1. src/Sim/playgrounds/下新增场景定义
2. 在planning_integrated/launch中创建新launch文件
3. 更新map.launch中的地图路径

5.2 算法改进方向

1. 实时性优化：

   • 将A搜索替换为Hybrid A
   • 采用GPU加速ESDF计算

2. 规划效果提升：

   • 加入学习型代价函数
   • 融合语义信息（如交通标志）

3. 我在实际项目中验证过的有效改进：

   diff复制// 在traj_optimizer.cpp中添加
   + double consider_ground_slope(const Eigen::Vector3d& pos) {
   +   // 考虑地面坡度对轨迹的影响
   + }
   

这套代码架构设计非常清晰，特别适合作为自动驾驶规划算法的研究平台。通过阅读和修改这个项目，我深刻理解了时空联合规划的技术要点。建议感兴趣的读者可以从停车场场景入手，逐步扩展到更复杂的城市道路环境。