EM planner算法（附代码理解）

CCC：CarCrazeCurator

于 2024-08-09 16:27:10 发布

阅读量2.8k

点赞数 16

文章标签：算法 c++

本文链接：https://blog.csdn.net/LIUMAO99/article/details/141054445

版权

EM Planner算法概述

EM Planner（Expectation-Maximization Planner）是百度Apollo自动驾驶平台中的一种面向L4级别的实时运动规划算法。该算法通过顶层多车道策略选择参考路径，并在Frenet坐标系下进行车道级的路径和速度规划。EM Planner的设计充分考虑了无人车的安全性、舒适性和可扩展性，能够适应高速公路和低速城区等多种场景。该算法通过动态规划（Dynamic Programming）和基于样条的二次规划（Quadratic Programming）实现，展现了较高的可靠性和低耗时性。

EM Planner的工作原理

EM Planner的工作流程包括以下关键步骤：

多车道策略：算法首先通过搜索算法结合代价估算形成变道策略，将多车道策略划分为被动变道和主动变道，后者考虑动态障碍物而做出决策。

路径-速度迭代算法：在Frenet坐标下，EM Planner通过迭代地进行路径和速度最优求解，考虑与障碍物的交互，上一帧的速度规划将帮助下一帧的路径规划。

决策和交通规则约束：交通规则作为硬约束，与障碍物的交互作为软约束。决策模块为规划带来更明确的意图，减少最优求解的搜索空间。

SL和ST投影：在E-step中，障碍物被投影到车道Frenet坐标系，包括静态和动态障碍物。在M-step中，障碍物在速度时间（ST）图中与生成的速度信息进行估计。

DP路径和QP路径：通过Dynamic Programming获得一个粗略的解，并构建一个凸的通道，然后使用基于Quadratic Programming的样条曲线生产光滑路径。

参考线轨迹决策：最后，参考线轨迹决策器根据当前车辆状态、相关约束和每条轨迹的代价，决定最优的轨迹。

1. 多车道策略

在多车道策略中，EM Planner采用了一种分层决策机制，将策略划分为主动变道 (Proactive Lane Changing)和被动变道(Reactive Lane Changing)。主动变道策略通常由导航需求触发，如到达目的地所需的车道变更。被动变道则是在当前车道受阻时（例如，前方有障碍物或车辆），系统自动选择另一条车道以确保安全行驶。这种策略通过结合搜索算法和代价估算，能够智能地预测和选择最佳变道路径，同时考虑动态障碍物的影响，确保决策的安全性和效率性。

主动变道

主动变道通常是由导航系统触发的，例如当车辆需要在前方路口左转或右转时，系统会提前规划并选择正确的车道。这种变道策略需要考虑目的地、交通规则、车道可用性等因素。

void proactiveLaneChange(const NavigationData &navData, VehicleState &vehicleState) {
    // 假设navData包含目的地信息和当前路口信息
    // vehicleState包含车辆当前状态和位置

    // 检查是否需要变道
    if (navData.needsTurn() && vehicleState.getCurrentLane() != navData.getTargetLane()) {
        // 执行变道逻辑
        vehicleState.changeLane(navData.getTargetLane());
    }
}

被动变道

被动变道是在当前车道受阻时自动触发的，例如前方有慢车或障碍物。系统会评估周围车道的情况，并选择一条安全的车道进行变道，以避免碰撞并保持行驶效率。

void reactiveLaneChange(VehicleState &vehicleState, const std::vector<Obstacle> &obstacles) {
    // 假设vehicleState包含车辆当前状态和位置
    // obstacles是一个包含所有障碍物的向量

    // 检查当前车道是否有障碍物
    for (const auto &obstacle : obstacles) {
        if (obstacle.isBlocking(vehicleState.getCurrentLane()) &&
            !obstacle.isTooCloseForSafePassing(vehicleState)) {
            // 寻找安全的车道进行变道
            int safeLane = findSafeLane(vehicleState, obstacles);
            if (safeLane != -1) {
                vehicleState.changeLane(safeLane);
                break;
            }
        }
    }
}

int findSafeLane(const VehicleState &vehicleState, const std::vector<Obstacle> &obstacles) {
    // 遍历所有车道，寻找没有障碍物或障碍物距离安全的车道
    for (int lane = 0; lane < MAX_LANES; ++lane) {
        if (!isLaneBlocked(obstacles, lane)) {
            return lane;
        }
    }
    return -1; // 如果所有车道都被阻塞，返回-1
}

bool isLaneBlocked(const std::vector<Obstacle> &obstacles, int lane) {
    // 检查指定车道是否有障碍物
    for (const auto &obstacle : obstacles) {
        if (obstacle.isInLane(lane)) {
            return true;
        }
    }
    return false;
}