EM planner算法（附代码理解）

EM Planner算法概述

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

EM Planner的工作原理

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

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

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

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

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

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

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

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());
    }
}

被动变道

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

扫描二维码关注公众号，回复： 17507166 查看本文章

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;
}

2. 路径-速度迭代算法

在Frenet坐标系下，EM Planner采用了路径-速度迭代算法。这一过程涉及两步：首先，算法基于当前车辆状态和环境信息，通过迭代优化求解最优路径。然后，算法利用上一帧的速度规划信息来指导下一帧的路径规划，形成一个闭环反馈机制。这一算法的关键在于，它能够同时考虑路径的平滑性和速度的合理性，以及障碍物的动态变化，通过迭代优化实现路径规划与速度控制的协同优化。

第一步：路径优化

1. 初始化：基于当前车辆状态（位置、速度）和环境信息（静态障碍物、动态障碍物预测），设定初始路径。
2. 成本函数构建：定义一个成本函数，该函数考虑路径的平滑性、与障碍物的最小距离、与参考路径的接近程度等因素。
3. 优化迭代：利用优化算法（如梯度下降、遗传算法、粒子群优化等）迭代调整路径参数，以最小化成本函数。
4. 终止条件：当路径参数变化小于某个阈值或达到最大迭代次数时，结束迭代。

第二步：速度规划

1. 参考速度：基于路径规划的结果，确定一个初步的速度参考，考虑道路限制和车辆动力学约束。
2. 速度调整：考虑前一帧的速度规划信息和当前路径的特性，调整速度以确保车辆能够安全、平顺地行驶。
3. 反馈机制：利用闭环控制策略，根据车辆的实际状态和路径规划结果调整速度，确保车辆能够按照规划的路径行驶。

3. 决策和交通规则约束

EM Planner在规划过程中严格遵守交通规则作为硬约束，确保规划出的路径合法且安全。同时，算法将与障碍物的交互视为软约束，通过动态调整路径和速度，以最小化与障碍物的潜在冲突。这一机制通过决策模块实现，它为整个规划过程提供了明确的意图和目标，从而缩小了最优求解的搜索空间，提高了规划的效率和准确性。

1. 硬约束（Hard Constraints）：

   这些是必须遵守的规则，如交通信号、速度限制、道路边界等。硬约束确保了规划出的路径在法律和安全上是可行的。

   // 确保车辆速度不超过最大限速的硬约束函数
   bool isSpeedWithinLimit(float currentSpeed, float maxSpeed) {
       return currentSpeed <= maxSpeed;
   }
   
   // 确保车辆不越过交通信号灯的硬约束函数
   bool isAtTrafficLight(float vehiclePosition, float lightPosition) {
       // 假设车辆在交通灯位置之前必须停下
       return vehiclePosition <= lightPosition;
   }

2. 软约束（Soft Constraints）：

   这些是建议性规则，如避免与障碍物的潜在冲突。软约束允许算法在不违反硬约束的情况下，对路径进行微调，以提高效率或舒适度。

   // 计算与障碍物的距离，作为软约束的函数
   float calculateObstacleDistance(float vehiclePosition, const std::vector<float>& obstaclePositions) {
       float minDistance = std::numeric_limits<float>::max();
       for (float obstaclePosition : obstaclePositions) {
           minDistance = std::min(minDistance, std::fabs(vehiclePosition - obstaclePosition));
       }
       return minDistance;
   }
   
   // 计算车辆与行人的距离，作为软约束的函数
   float calculatePedestrianDistance(float vehiclePosition, const std::vector<float>& pedestrianPositions) {
       float minDistance = std::numeric_limits<float>::max();
       for (float pedestrianPosition : pedestrianPositions) {
           minDistance = std::min(minDistance, std::fabs(vehiclePosition - pedestrianPosition));
       }
       return minDistance;
   }

3. 动态调整（Dynamic Adjustment）：

   算法能够根据实时环境变化（如交通流量、障碍物出现等）动态调整路径和速度，以适应不断变化的条件。

4. 决策模块（Decision Module）：

   这是算法的核心部分，负责制定策略和目标。决策模块通过分析当前环境和交通状况，确定最优的路径规划方案。

5. 意图和目标（Intentions and Goals）：

   决策模块为规划过程设定明确的意图和目标，如尽快到达目的地、避免拥堵区域等，这有助于缩小搜索空间，提高算法的效率。

6. 最优求解搜索空间（Optimal Solution Search Space）：

   通过设定意图和目标，算法可以排除那些不符合这些条件的潜在解决方案，从而减少需要考虑的选项，加快求解过程。

7. 效率和准确性（Efficiency and Accuracy）：

   通过上述机制，EM Planner能够在保证安全和合法性的同时，提供高效和准确的路径规划结果。

4. SL和ST投影

Frenet坐标系是一种用于自动驾驶车辆路径规划的坐标系统，它将道路表示为一系列沿着道路中心线的点，这些点的集合称为路径。在Frenet坐标系中，每个点由两个坐标表示： 表示沿路径的距离，而表示垂直于路径的距离。EM Planner使用Frenet坐标系进行路径和速度规划，利用SL（纵向位置）和ST（速度时间）投影，在E-step（期望步）和M-step（最大化步）中处理障碍物信息。在E-step中，所有障碍物（包括静态和动态障碍物）被投影到车道Frenet坐标系中，以便于算法理解和处理。在M-step中，障碍物的位置和速度信息被映射到速度时间图（ST图）中，与车辆生成的速度信息进行比较和优化，从而确保规划出的轨迹能够安全地避免障碍物。

Cartesian坐标系到Frenet坐标系的转换

// 定义点结构体，用于存储二维坐标
struct Point {
    double x; // x坐标
    double y; // y坐标
};

// 定义Frenet结构体，用于存储Frenet坐标系下的s, l坐标和道路曲率
struct Frenet {
    double s; // 沿路径的距离
    double l; // 垂直于路径的距离，这里用l代替d
    double yaw; // 道路曲率
};

// 函数实现
Frenet toFrenet(const Point& car_pos, const std::vector<Point>& map_waypoints) {
    // 初始化变量
    double min_l = std::numeric_limits<double>::infinity(); // 最小垂直距离初始化为无穷大
    double closest_x, closest_y; // 最近点的x, y坐标
    double s = 0; // 初始s坐标
    double l = 0; // 初始l坐标
    double closest_len = 0; // 最近点到车辆的距离
    double prev_s = 0; // 上一个点的s坐标
    double prev_yaw = 0; // 上一个点的yaw角度

    // 遍历所有地图路径点
    for (int i = 0; i < map_waypoints.size(); i++) {
        // 计算当前点到车辆的x, y距离
        double x = map_waypoints[i].x - car_pos.x;
        double y = map_waypoints[i].y - car_pos.y;

        // 计算当前点到车辆的距离
        double dist = sqrt(x * x + y * y);

        // 计算与前一个点的距离
        double dist_to_prev = (i > 0) ? 
            sqrt((x - map_waypoints[i - 1].x) * (x - map_waypoints[i - 1].x) + 
                 (y - map_waypoints[i - 1].y) * (y - map_waypoints[i - 1].y)) : 0;

        // 更新s坐标
        s += dist_to_prev;

        // 如果当前点到车辆的距离小于已知的最小垂直距离，则更新最近点信息
        if (dist < min_l) {
            closest_x = x;
            closest_y = y;
            closest_len = dist;
            min_l = dist;
        }

        // 如果不是第一个点，计算道路的曲率和切线角度
        if (i > 0 && dist_to_prev > 0) {
            // 计算当前点与前一个点的切线角度
            double yaw = atan2(y, x);
            prev_yaw = atan2(map_waypoints[i - 1].y - car_pos.y, map_waypoints[i - 1].x - car_pos.x);
            // 计算道路曲率
            prev_yaw = atan2(sin(prev_yaw - yaw), (cos(prev_yaw) * cos(yaw) + sin(prev_yaw) * sin(yaw)));
        }
    }

    // 计算车辆到最近点的垂直距离l
    l = sqrt(closest_x * closest_x + closest_y * closest_y) - closest_len;

    // 计算车辆的切线角度，这里假设道路是直线，所以yaw为0
    double final_yaw = 0;

    // 返回Frenet坐标系下的坐标
    return {s, l, final_yaw};
}

5. DP路径和QP路径

EM Planner采用Dynamic Programming（动态规划）和Quadratic Programming（二次规划）两种方法生成路径。首先，通过DP算法获得一个初步的、粗略的路径解，构建出一个大致的行驶轨迹。接着，算法使用基于QP的样条曲线生成光滑路径，通过优化路径的平滑性和连续性，提高车辆行驶的舒适度和安全性。这一过程确保了路径规划的高效性和路径的平滑性，是EM Planner算法中的关键步骤之一。

Dynamic Programming（动态规划）

1. 初步路径生成：通过将整个路径规划问题分解为一系列较小的子问题，DP能够逐步构建出一个粗略的路径解。每个子问题只考虑从当前位置到目标位置的局部最优路径。

2. 状态空间定义：在路径规划中，状态空间通常由车辆的位置和时间（或其他相关参数）定义。

3. 递归关系：DP通过定义状态转移方程来找到从一个状态到另一个状态的最优路径。这种递归方法允许算法利用之前的计算结果来简化当前的计算。

4. 边界条件：DP需要定义边界条件，即当车辆到达目标位置时的成本或路径。

5. 优化计算：DP通过回溯过程找到全局最优解，这种方法在计算上可能比较耗时，但对于初步路径生成是有效的。

// 动态规划路径优化
std::pair<double, double> optimizePath(const VehicleState& vehicle, const std::vector<Obstacle>& obstacles, const std::vector<Obstacle>& map_waypoints) {
    const int MAX_ITERATIONS = 100;
    const double STEP_SIZE = 1.0; // 根据实际情况调整步长
    const double THRESHOLD = 1e-6;
    const double MAX_S = 100.0; // 最大s坐标值
    const double MAX_L = 10.0;  // 最大l坐标值

    auto frenet_coordinates = toFrenet(vehicle, map_waypoints);
    double s = frenet_coordinates.first;
    double l = frenet_coordinates.second;
    double cost = std::numeric_limits<double>::max();

    std::vector<Eigen::Vector2d> control_points; // 用于多项式插值的控制点

    for (int iter = 0; iter < MAX_ITERATIONS; ++iter) {
        double prev_cost = cost;
        for (double ds = -STEP_SIZE; ds <= STEP_SIZE; ds += STEP_SIZE) {
            for (double dl = -STEP_SIZE; dl <= STEP_SIZE; dl += STEP_SIZE) {
                double new_s = s + ds;
                double new_l = l + dl;

                // 添加边界条件检查
                if (new_s < 0 || new_s > MAX_S || new_l < 0 || new_l > MAX_L) continue;

                double new_cost = costFunction(vehicle, obstacles, new_s, new_l);
                if (new_cost < cost) {
                    s = new_s;
                    l = new_l;
                    cost = new_cost;
                    control_points.push_back(Eigen::Vector2d(s, l)); // 收集控制点
                }
            }
        }

        if (fabs(prev_cost - cost) < THRESHOLD) {
            break; // 如果成本变化小于阈值，则停止迭代
        }
    }

    // 使用多项式插值进行平滑处理
    std::vector<Eigen::Vector2d> smooth_path = generateSmoothPath(control_points);

    // 从平滑后的路径中提取最终的s和l坐标
    // 这里假设我们取平滑路径的最后一个点作为最终路径点
    if (!smooth_path.empty()) {
        Eigen::Vector2d final_point = smooth_path.back();
        s = final_point.x();
        l = final_point.y();
    }

    return {s, l};
}

// 计算成本函数
double costFunction(const VehicleState& vehicle, const std::vector<Obstacle>& obstacles, double s, double l) {
    double cost = 0.0;
    for (const Obstacle& obs : obstacles) {
        double dist_to_obs = hypot(s - obs.s, l - obs.l);
        cost += 1.0 / std::max(dist_to_obs, 1e-6); // 避免除零
    }
    return cost;
}

// 定义一个辅助函数，用于转换控制点到Eigen::Vector2d数组
Eigen::MatrixXd controlPointsToEigen(const std::vector<Eigen::Vector2d>& control_points) {
    Eigen::MatrixXd eigen_control_points(control_points.size(), 2);
    for (int i = 0; i < control_points.size(); ++i) {
        eigen_control_points.row(i) << control_points[i].x(), control_points[i].y();
    }
    return eigen_control_points;
}

// 多项式插值函数实现
std::vector<Eigen::Vector2d> generateSmoothPath(const std::vector<Eigen::Vector2d>& control_points) {
    // 将控制点转换为Eigen矩阵
    Eigen::MatrixXd eigen_control_points = controlPointsToEigen(control_points);

    // 定义样条参数
    const double tension = 0.0; // 张紧参数，0表示自然样条
    const double bias = 0.0;    // 偏差参数，0表示无偏差
    const double smoothness = 1.0; // 平滑度参数

    // 创建三次样条曲线
    Eigen::Spline3d spline(Eigen::Dynamic, tension, bias, smoothness);
    
    // 设置样条的控制点
    spline.setControlPoints(eigen_control_points.transpose());

    // 定义插值点的数量，这里我们假设在最小和最大s坐标之间均匀采样
    double min_s = eigen_control_points.col(0).minCoeff();
    double max_s = eigen_control_points.col(0).maxCoeff();
    const int num_points = 100; // 插值点的数量
    std::vector<Eigen::Vector2d> smooth_path;

    // 计算插值点
    for (int i = 0; i <= num_points; ++i) {
        double s = min_s + i * (max_s - min_s) / num_points;
        Eigen::Vector3d point_on_spline = spline(s);
        smooth_path.emplace_back(point_on_spline(0), point_on_spline(1));
    }

    return smooth_path;
}

Quadratic Programming（二次规划）

1. 平滑路径生成：在DP生成的初步路径基础上，QP可以用来生成更加平滑和连续的路径。这通常涉及到最小化路径曲率或加速度变化。

2. 目标函数：QP的目标函数通常是路径的平滑性度量，如路径的二阶导数的平方和，这代表了路径的曲率。

3. 约束条件：QP的约束条件可能包括车辆动力学限制、避免碰撞的边界以及必须遵守的交通规则。

4. 数值优化：QP问题通常使用数值优化方法求解，如梯度下降、牛顿法或内点法等。

5. 实时性：与DP相比，QP可以更快地求解，使其适合于需要快速响应的实时路径规划场景。

// 速度规划函数，基于二次规划求解
double planSpeedWithQP(const VehicleState& vehicle, const std::pair<double, double>& frenet_position, const std::vector<Obstacle>& obstacles) {
    const int N = 10; // 时间步数
    const double TIME_STEP = 0.1; // 时间步长
    const double MAX_SPEED = 30; // 最大速度
    const double MIN_SPEED = 0; // 最小速度
    const double MAX_ACCEL = 5; // 最大加速度
    const double MIN_ACCEL = -5; // 最小加速度

    // 定义QP问题的维度
    const int n = N; // 状态变量数，即速度
    const int m = 2 * N; // 约束数，每个时间步的上下限约束

    // 定义QP问题的矩阵和向量
    Eigen::MatrixXd P(n, n); // 目标函数的二次项系数矩阵，对角阵
    Eigen::VectorXd q(n); // 目标函数的线性项系数向量
    Eigen::MatrixXd A(m, n); // 约束矩阵
    Eigen::VectorXd l(m); // 约束下限向量
    Eigen::VectorXd u(m); // 约束上限向量

    // 初始化QP矩阵和向量
    P.setIdentity();
    q.setZero();
    A.setZero();
    l.setZero();
    u.setZero();

    // 设置QP矩阵和向量
    // 设置QP矩阵和向量
    // 目标函数：最小化加速的平方
    for (int i = 0; i < n - 1; ++i) {
        P(i, i) = 1.0 / (TIME_STEP * TIME_STEP);
        P(i + 1, i) = -2.0 / (TIME_STEP * TIME_STEP);
        P(i + 1, i + 1) = 1.0 / (TIME_STEP * TIME_STEP);
    }

    // 约束：速度上下限
    for (int i = 0; i < N; i++) {
        A(i, i) = 1.0;
        A(N + i, i) = -1.0;
        l(i) = MIN_SPEED;
        u(i) = MAX_SPEED;
        l(N + i) = MIN_ACCEL;
        u(N + i) = MAX_ACCEL;
    }

    // 设置初始状态
    Eigen::VectorXd x0(n);
    x0.setZero();
    x0(0) = vehicle.v; // 当前速度

    // 设置QP求解器
    OSQPData data = {};
    OSQPSettings settings = {};
    osqp_setup(&settings, &data);
    osqp_set_P(&settings, P.data());
    osqp_set_A(&settings, A.data());
    osqp_set_q(&settings, q.data());
    osqp_set_l(&settings, l.data());
    osqp_set_u(&settings, u.data());
    osqp_update_settings(&settings, OSQP_DEFAULT);
    osqp_setup(&data, &settings);

    // 求解QP问题
    osqp_solve(&data);

    // 获取结果
    double final_speed = x0(n - 1);

    // 清理资源
    osqp_cleanup(&data, &settings);

    return final_speed;
}

6. 参考线轨迹决策

最后，参考线轨迹决策模块根据当前车辆状态、交通规则约束、障碍物信息以及每条轨迹的代价，综合评估并决定最优的行驶轨迹。这一决策过程考虑了车辆的动态特性、环境的动态变化以及交通规则的约束，确保了规划出的轨迹既符合安全标准，也满足舒适性和效率的要求。

// 决策模块中用于评估轨迹代价的函数
float evaluateTrajectoryCost(const Trajectory& trajectory, float maxSpeed, const std::vector<float>& obstacles) {
    // 检查速度是否超过最大限速
    for (float position : trajectory.positions) {
        // 这里假设有一个函数calculateSpeed(position)来计算当前位置的速度
        float speed = calculateSpeed(position); // 需要实现calculateSpeed函数
        if (speed > maxSpeed) {
            return std::numeric_limits<float>::infinity(); // 如果速度超过限制，返回无穷大代价
        }
    }

    // 检查是否避开了所有障碍物
    for (float obstacle : obstacles) {
        for (float position : trajectory.positions) {
            if (position == obstacle) { // 简化检查，实际可能需要更复杂的距离计算
                return std::numeric_limits<float>::infinity(); // 如果轨迹与障碍物相交，返回无穷大代价
            }
        }
    }

    // 如果轨迹满足所有约束条件，返回一个合理的非无穷大代价
    // 这里只是一个示例值，实际中应根据具体情况计算
    return 1.0f; // 可以是任何合理的初始代价值
}

EM Planner算法与端到端自动驾驶的结合

将EM Planner算法与端到端自动驾驶技术结合，是自动驾驶领域迈向更高智能化水平的关键一步。EM Planner的精细路径规划与速度控制能力，与端到端技术的环境感知和决策制定优势互补，共同构建了一个既高效又安全的自动驾驶系统。这种结合不仅提高了系统的适应性和鲁棒性，还促进了自动驾驶从理论到实践的无缝衔接，为实现全自动驾驶的愿景提供了坚实的技术支撑。随着技术的不断进步和应用场景的拓展，EM Planner与端到端自动驾驶的融合将更加深入，有望引领自动驾驶领域的未来发展方向，为智慧出行创造更多可能。