Autoware.Universe规划控制模块实战指南：从零构建自动驾驶决策系统

1次阅读

共计 2033 个字符，预计需要花费 6 分钟才能阅读完成。

传统自动驾驶规划控制方案在复杂城市场景中常面临三大挑战：动态障碍物避让反应滞后、多目标决策逻辑耦合度高、紧急制动场景下的轨迹抖动。Autoware.Universe 通过模块化架构将问题分解为三层：

行为决策层 ：基于有限状态机(FSM) 处理变道、跟车等高层指令，采用 ROS2 Action 机制实现异步任务调度
运动规划层：使用 Conformal Lattice 算法在 Frenet 坐标系下生成候选轨迹，避免笛卡尔空间的曲率突变
控制执行层：通过 MPC 控制器将规划轨迹转化为转向 / 油门指令，支持控制频率高达 100Hz

规划控制模块的核心通信流程如下图所示（此处应有序列图描述）：

@startuml
participant "Behavior Planner" as BP
participant "Motion Planner" as MP
participant "Controller" as CTL

BP -> MP : /planning/mission (Mission.msg)
MP -> CTL : /planning/trajectory (Trajectory.msg)
CTL -> MP : /control/reference (Feedback.msg)
@enduml

关键消息字段设计：

/planning/trajectory：
header.stamp：采用 ROS2 的 builtin_interfaces/Time 时间戳
points[]：轨迹点数组，每个点包含：
- pose (geometry_msgs/Pose)：世界坐标系下的位姿
- velocity (float32)：目标速度(m/s)
- acceleration (float32)：期望加速度(m/s²)

以下示例展示如何实现红绿灯决策状态机（Python 与 C ++ 混合示例）：

// 状态枚举定义 (C++14)
enum class TrafficLightState {
  APPROACHING,
  STOPPED,
  PASSING
};

// Frenet 坐标系转换关键代码
void convertToFrenet(const Path &path, const Pose &pose, FrenetPoint *frenet) {
  // 1. 寻找最近路径点
  auto closest_idx = findClosestWaypoint(path, pose);

  // 2. 计算横向偏移（数学公式）// s = ∫_0^t v(t)dt 
  // d = ||p - p_closest|| * sin(θ_diff)
  ...
}

# Python 状态转移逻辑
def update_state(self):
  if self.current_state == TrafficLightState.APPROACHING:
    if self._check_red_light():
      self._publish_stop_command()
      self.current_state = TrafficLightState.STOPPED

# ROS2 QoS 配置（避免消息丢失）qos_profile = QoSProfile(
  depth=10,
  reliability=QoSReliabilityPolicy.RELIABLE,
  durability=QoSDurabilityPolicy.TRANSIENT_LOCAL
)

在 Intel i7-1185G7 平台上的测试数据：