文章目录
团队博客: 汽车电子社区
1. 模块概述
Autoware规划模块位于src/universe/autoware_universe/planning/目录下，是自动驾驶系统的"大脑"，负责根据感知信息和地图数据生成安全、舒适的行驶轨迹。该模块采用分层架构，从全局任务规划到局部运动规划，为车辆提供完整的驾驶决策能力。
2. 模块架构设计
2.1 整体架构
2.2 分层规划体系
3. 任务规划模块 (Mission Planner)
3.1 核心架构
class MissionPlanner : public rclcpp::Node
{
private:
ArrivalChecker arrival_checker_; // 到达检查器
pluginlib::ClassLoader<PlannerPlugin> plugin_loader_; // 插件加载器
std::shared_ptr<PlannerPlugin> planner_; // 规划插件
LaneletRoute::ConstSharedPtr current_route_; // 当前路由
RouteState state_; // 路由状态
// 订阅话题
rclcpp::Subscription<Odometry>::SharedPtr sub_odometry_;
rclcpp::Subscription<LaneletMapBin>::SharedPtr sub_vector_map_;
rclcpp::Subscription<OperationModeState>::SharedPtr sub_operation_mode_;
// 发布话题
rclcpp::Publisher<LaneletRoute>::SharedPtr pub_route_;
rclcpp::Publisher<RouteState>::SharedPtr pub_state_;
rclcpp::Publisher<MarkerArray>::SharedPtr pub_debug_marker_;
// 服务接口
rclcpp::Service<ClearRoute>::SharedPtr srv_clear_route_;
rclcpp::Service<SetLaneletRoute>::SharedPtr srv_set_lanelet_route_;
rclcpp::Service<SetWaypointRoute>::SharedPtr srv_set_waypoint_route_;
};
3.2 路由状态管理
enum class RouteState {
UNSET, // 未设置路由
SET, // 已设置路由
DRIVING, // 正在驾驶
ARRIVED, // 已到达目标
CHANGING_ROUTE // 正在改变路由
};
class MissionPlanner {
private:
void change_state(RouteState new_state, const std::string & reason)
{
RouteState old_state = state_;
state_ = new_state;
RCLCPP_INFO(
get_logger(),
"Route state changed: %s -> %s (%s)",
toString(old_state).c_str(),
toString(new_state).c_str(),
reason.c_str());
publishRouteState();
}
public:
void update()
{
switch (state_) {
case RouteState::SET:
if (isRouteReceived()) {
change_state(RouteState::DRIVING, "Route received");
}
break;
case RouteState::DRIVING:
if (arrival_checker_.hasArrived(current_route_, current_pose_)) {
change_state(RouteState::ARRIVED, "Arrived at destination");
}
break;
case RouteState::ARRIVED:
// 保持到达状态，等待新的路由
break;
}
}
};
3.3 安全重路由检查
class MissionPlanner {
private:
bool check_reroute_safety(const LaneletRoute & new_route)
{
// 1. 检查路由连续性
if (!isRouteContinuous(current_route_, new_route)) {
RCLCPP_WARN(get_logger(), "New route is not continuous with current route");
return false;
}
// 2. 检查重路由距离
double reroute_distance = calculateRerouteDistance(current_route_, new_route);
if (reroute_distance < minimum_reroute_length_) {
RCLCPP_WARN(get_logger(), "Reroute distance too short: %f", reroute_distance);
return false;
}
// 3. 检查时间间隔
auto time_since_last_reroute = std::chrono::steady_clock::now() - last_reroute_time_;
if (time_since_last_reroute < std::chrono::seconds(reroute_time_threshold_)) {
RCLCPP_WARN(get_logger(), "Reroute too frequent");
return false;
}
// 4. 检查自动驾驶模式限制
if (!allow_reroute_in_autonomous_mode_ &&
current_operation_mode_ == OperationModeState::AUTONOMOUS) {
RCLCPP_WARN(get_logger(), "Reroute not allowed in autonomous mode");
return false;
}
return true;
}
};
3.4 到达检查算法
class ArrivalChecker {
public:
bool hasArrived(
const LaneletRoute::ConstSharedPtr & route,
const geometry_msgs::msg::Pose & current_pose)
{
if (!route || route->segments.empty()) {
return false;
}
// 1. 获取目标点
const auto & goal_pose = route->goal_pose;
// 2. 计算到目标的距离
double distance = calcDistance2D(current_pose.position, goal_pose.position);
// 3. 计算航向差
double yaw_current = tf2::getYaw(current_pose.orientation);
double yaw_goal = tf2::getYaw(goal_pose.orientation);
double yaw_diff = normalizeRadian(yaw_goal - yaw_current);
// 4. 判断是否到达
bool position_arrived = distance < arrival_distance_threshold_;
bool heading_arrived = std::abs(yaw_diff) < arrival_yaw_threshold_;
return position_arrived && heading_arrived;
}
private:
double arrival_distance_threshold_ = 2.0; // 到达距离阈值 [m]
double arrival_yaw_threshold_ = 0.1745; // 到达航向阈值 [rad] (10deg)
};
4. 行为规划模块 (Behavior Planner)
4.1 行为路径规划器 (Behavior Path Planner)
4.1.1 模块管理器架构
class BehaviorPathPlannerNode : public rclcpp::Node
{
private:
std::unique_ptr<BehaviorPathPlannerManager> manager_ptr_;
std::unique_ptr<SceneModuleManagerInterface> scene_manager_;
// 输入数据
PlannerData planner_data_;
// 输出接口
rclcpp::Publisher<PathWithLaneId>::SharedPtr pub_path_;
rclcpp::Publisher<TurnIndicatorsCommand>::SharedPtr pub_turn_indicators_;
rclcpp::Publisher<PoseStamped>::SharedPtr pub_modified_goal_;
// 调试接口
rclcpp::Publisher<MarkerArray>::SharedPtr pub_debug_marker_;
rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr pub_debug_values_;
};
4.1.2 场景模块管理
class PlannerManager {
private:
std::vector<std::shared_ptr<SceneModuleManagerInterface>> manager_ptrs_;
pluginlib::ClassLoader<SceneModuleManagerInterface> plugin_loader_;
// 模块优先级
std::map<std::string, int> module_priorities_;
public:
void setupModules(const std::vector<std::string> & module_names)
{
for (const auto & module_name : module_names) {
try {
auto module_ptr = plugin_loader_.createSharedInstance(module_name);
module_ptr->init(this);
manager_ptrs_.push_back(module_ptr);
RCLCPP_INFO(get_logger(), "Loaded module: %s", module_name.c_str());
} catch (const pluginlib::PluginlibException & ex) {
RCLCPP_ERROR(get_logger(), "Failed to load module %s: %s",
module_name.c_str(), ex.what());
}
}
}
PathWithLaneId generatePlan(const PlannerData & data)
{
// 按优先级排序模块
auto sorted_modules = sortModulesByPriority();
PathWithLaneId result_path = data.reference_path;
for (auto & module : sorted_modules) {
if (module->isExecutionRequested(data)) {
auto module_result = module->plan(data, result_path);
if (module_result.path.points.empty()) {
RCLCPP_WARN(get_logger(), "Module %s returned empty path",
module->getModuleName().c_str());
continue;
}
result_path = module_result.path;
if (module_result.execution_succeeded) {
module->setExecutionRequest(true);
}
} else {
module->setExecutionRequest(false);
}
}
return result_path;
}
};
4.1.3 变道模块实现
class LaneChangeModule : public SceneModuleInterface
{
private:
enum class State {
WAITING_FOR_EXECUTION, // 等待执行
PREPARING_LANE_CHANGE, // 准备变道
EXECUTING_LANE_CHANGE, // 执行变道
COMPLETED_LANE_CHANGE // 完成变道
};
State state_ = State::WAITING_FOR_EXECUTION;
// 变道参数
LaneChangeParameters params_;
// 安全检查器
std::shared_ptr<SafetyChecker> safety_checker_;
// 轨迹生成器
std::shared_ptr<LaneChangeTrajectoryGenerator> trajectory_generator_;
public:
bool isExecutionRequested(const PlannerData & data) override
{
if (state_ != State::WAITING_FOR_EXECUTION) {
return true; // 已启动的变道继续执行
}
// 检查变道需求
return isLaneChangeRequired(data) &&
isLaneChangeSafe(data) &&
isLaneChangeAvailable(data);
}
SceneModuleOutput plan(const PlannerData & data, const PathWithLaneId & reference_path) override
{
switch (state_) {
case State::WAITING_FOR_EXECUTION:
if (isExecutionRequested(data)) {
state_ = State::PREPARING_LANE_CHANGE;
return prepareLaneChange(data);
}
break;
case State::PREPARING_LANE_CHANGE:
if (isReadyToExecute(data)) {
state_ = State::EXECUTING_LANE_CHANGE;
return executeLaneChange(data);
} else {
return prepareLaneChange(data);
}
break;
case State::EXECUTING_LANE_CHANGE:
if (isLaneChangeCompleted(data)) {
state_ = State::COMPLETED_LANE_CHANGE;
return completeLaneChange(data);
} else {
return executeLaneChange(data);
}
break;
case State::COMPLETED_LANE_CHANGE:
// 变道完成，返回空路径
return SceneModuleOutput{};
}
return SceneModuleOutput{};
}
private:
bool isLaneChangeRequired(const PlannerData & data)
{
// 1. 检查是否在目标车道上
if (isOnPreferredLane(data)) {
return false;
}
// 2. 检查距离目标车道的距离
double distance_to_target_lane =
getDistanceToTargetLane(data.current_pose, data.route_handler);
if (distance_to_target_lane > params_.minimum_lane_change_length) {
return false;
}
// 3. 检查到交通规则元素的距离
double distance_to_regulatory_element =
getDistanceToRegulatoryElement(data.current_pose, data.route_handler);
if (distance_to_regulatory_element < params_.minimum_lane_change_length) {
return false;
}
return true;
}
bool isLaneChangeSafe(const PlannerData & data)
{
// 1. 检查后方交通状况
auto rear_vehicles = getRearVehicles(data.predicted_objects, data.current_lane);
if (!isSafeFromRearVehicles(rear_vehicles, data.current_twist)) {
return false;
}
// 2. 检查目标车道交通状况
auto target_lane_vehicles = getTargetLaneVehicles(data.predicted_objects, data.route_handler);
if (!isSafeFromTargetLaneVehicles(target_lane_vehicles)) {
return false;
}
// 3. 检查盲区
if (!isSafeFromBlindSpots(data)) {
return false;
}
return true;
}
};
4.1.4 变道轨迹生成
class LaneChangeTrajectoryGenerator {
public:
PathWithLaneId generateLaneChangeTrajectory(
const PlannerData & data,
const LaneChangeInfo & info)
{
PathWithLaneId trajectory;
// 1. 计算变道起始点和结束点
auto start_point = calcLaneChangeStartPoint(data, info);
auto end_point = calcLaneChangeEndPoint(data, info);
// 2. 生成候选轨迹
auto candidate_trajectories = generateCandidateTrajectories(start_point, end_point);
// 3. 评估轨迹
double best_score = std::numeric_limits<double>::lowest();
PathWithLaneId best_trajectory;
for (const auto & candidate : candidate_trajectories) {
double score = evaluateTrajectory(candidate, data, info);
if (score > best_score) {
best_score = score;
best_trajectory = candidate;
}
}
// 4. 后处理
return postprocessTrajectory(best_trajectory);
}
private:
std::vector<PathWithLaneId> generateCandidateTrajectories(
const Pose & start_point,
const Pose & end_point)
{
std::vector<PathWithLaneId> candidates;
// 1. 多种横向偏移
for (double lateral_offset = 1.0; lateral_offset <= 4.0; lateral_offset += 0.5) {
// 2. 多种纵向长度
for (double longitudinal_length = 20.0; longitudinal_length <= 50.0; longitudinal_length += 5.0) {
// 3. 多种速度规划
for (double velocity = 5.0; velocity <= 15.0; velocity += 2.5) {
auto trajectory = generateTrajectory(
start_point, end_point, lateral_offset, longitudinal_length, velocity);
candidates.push_back(trajectory);
}
}
}
return candidates;
}
double evaluateTrajectory(
const PathWithLaneId & trajectory,
const PlannerData & data,
const LaneChangeInfo & info)
{
double score = 0.0;
// 1. 舒适性评估
double comfort_score = evaluateComfort(trajectory);
score += params_.comfort_weight * comfort_score;
// 2. 安全性评估
double safety_score = evaluateSafety(trajectory, data);
score += params_.safety_weight * safety_score;
// 3. 效率评估
double efficiency_score = evaluateEfficiency(trajectory, info);
score += params_.efficiency_weight * efficiency_score;
// 4. 违规评估
double violation_score = evaluateViolations(trajectory, data);
score -= params_.violation_penalty * violation_score;
return score;
}
};
4.2 行为速度规划器 (Behavior Velocity Planner)
4.2.1 模块管理架构
class BehaviorVelocityPlannerNode : public rclcpp::Node
{
private:
// 各场景模块
std::shared_ptr<IntersectionModule> intersection_module_;
std::shared_ptr<CrosswalkModule> crosswalk_module_;
std::shared_ptr<BlindSpotModule> blind_spot_module_;
std::shared_ptr<DetectionAreaModule> detection_area_module_;
std::shared_ptr<OcclusionSpotModule> occlusion_spot_module_;
std::shared_ptr<RunOutModule> run_out_module_;
std::shared_ptr<NoDrivableLaneModule> no_drivable_lane_module_;
std::shared_ptr<NoStoppingAreaModule> no_stopping_area_module_;
// 模块状态管理
std::map<std::string, bool> module_activation_;
// 数据管理
PlannerData planner_data_;
public:
void plan()
{
// 1. 生成参考速度规划
auto velocity_plan = generateReferenceVelocityPlan(planner_data_);
// 2. 按优先级应用各模块的速度限制
auto sorted_modules = getSortedModulesByPriority();
for (auto & [name, module] : sorted_modules) {
if (module_activation_[name]) {
auto module_result = module->plan(velocity_plan, planner_data_);
if (module_result.safe) {
velocity_plan = module_result.velocity_plan;
module->setApproval(true);
} else {
module->setApproval(false);
}
}
}
// 3. 发布结果
publishVelocityPlan(velocity_plan);
}
};
4.2.2 交叉路口处理模块
class IntersectionModule {
private:
enum class State {
STOP, // 停车状态
GO, // 通行状态
APPROACH // 接近状态
};
State state_ = State::APPROACH;
// 安全检查器
std::shared_ptr<IntersectionSafetyChecker> safety_checker_;
// 轨迹生成器
std::shared_ptr<IntersectionTrajectoryGenerator> trajectory_generator_;
public:
BehaviorVelocityModuleOutput plan(
const Trajectory & reference_velocity_plan,
const PlannerData & data)
{
switch (state_) {
case State::APPROACH:
return handleApproachState(reference_velocity_plan, data);
case State::STOP:
return handleStopState(reference_velocity_plan, data);
case State::GO:
return handleGoState(reference_velocity_plan, data);
}
return BehaviorVelocityModuleOutput{reference_velocity_plan, true};
}
private:
BehaviorVelocityModuleOutput handleApproachState(
const Trajectory & reference_velocity_plan,
const PlannerData & data)
{
// 1. 检查是否需要停车
auto decision_result = makeDecision(data.object_manager);
if (decision_result.is_stop_required) {
// 2. 生成停车轨迹
auto stop_trajectory = generateStopTrajectory(
reference_velocity_plan, decision_result.stop_point);
// 3. 检查是否已到停车线
if (hasArrivedAtStopLine(data.current_pose, stop_trajectory)) {
state_ = State::STOP;
}
return BehaviorVelocityModuleOutput{stop_trajectory, true};
} else {
// 安全通行
state_ = State::GO;
return BehaviorVelocityModuleOutput{reference_velocity_plan, true};
}
}
BehaviorVelocityModuleOutput handleStopState(
const Trajectory & reference_velocity_plan,
const PlannerData & data)
{
// 1. 检查是否可以通行
auto decision_result = makeDecision(data.object_manager);
if (decision_result.is_safe_to_go) {
// 2. 生成通行轨迹
auto go_trajectory = generateGoTrajectory(reference_velocity_plan);
state_ = State::GO;
return BehaviorVelocityModuleOutput{go_trajectory, true};
} else {
// 继续停车
auto stop_trajectory = maintainStopTrajectory(reference_velocity_plan);
return BehaviorVelocityModuleOutput{stop_trajectory, true};
}
}
struct DecisionResult {
bool is_stop_required;
bool is_safe_to_go;
geometry_msgs::msg::Point stop_point;
std::vector<std::string> reason_messages;
};
DecisionResult makeDecision(const ObjectManager & object_manager)
{
DecisionResult result;
result.is_stop_required = false;
result.is_safe_to_go = true;
// 1. 检查交叉路口内车辆
auto intersection_vehicles = object_manager.getIntersectionVehicles();
for (const auto & vehicle : intersection_vehicles) {
double ttc = calculateTTC(vehicle, object_manager.getEgoVelocity());
if (ttc < params_.collision_time_margin) {
result.is_stop_required = true;
result.is_safe_to_go = false;
result.reason_messages.push_back(
"Vehicle in intersection with TTC: " + std::to_string(ttc));
break;
}
}
// 2. 检查预测轨迹中的冲突
auto predicted_conflicts = checkPredictedTrajectoryConflicts(object_manager);
if (!predicted_conflicts.empty()) {
result.is_stop_required = true;
result.is_safe_to_go = false;
result.reason_messages.push_back("Predicted trajectory conflicts detected");
}
// 3. 检查行人和自行车
auto vulnerable_road_users = object_manager.getVulnerableRoadUsers();
for (const auto & vru : vulnerable_road_users) {
double distance = calculateDistance2D(vru.pose.position, object_manager.getEgoPose());
if (distance < params_.vru_distance_threshold) {
result.is_stop_required = true;
result.is_safe_to_go = false;
result.reason_messages.push_back("Vulnerable road user nearby");
}
}
// 4. 计算停车点
if (result.is_stop_required) {
result.stop_point = calculateStopPoint(object_manager);
}
return result;
}
};
4.2.3 遮挡点检测与处理
class OcclusionSpotModule {
private:
std::vector<OcclusionSpot> occlusion_spots_;
public:
BehaviorVelocityModuleOutput plan(
const Trajectory & reference_velocity_plan,
const PlannerData & data)
{
// 1. 检测遮挡点
occlusion_spots_ = detectOcclusionSpots(data.occupancy_grid, data.current_pose);
// 2. 评估风险
auto risk_assessment = assessOcclusionRisks(occlusion_spots_, data.current_velocity);
// 3. 生成速度限制
auto velocity_plan = applyRiskBasedSpeedLimits(
reference_velocity_plan, risk_assessment);
return BehaviorVelocityModuleOutput{velocity_plan, true};
}
private:
std::vector<OcclusionSpot> detectOcclusionSpots(
const nav_msgs::msg::OccupancyGrid & occupancy_grid,
const geometry_msgs::msg::Pose & current_pose)
{
std::vector<OcclusionSpot> occlusion_spots;
// 1. 沿着参考轨迹搜索遮挡点
for (double distance = params_.min_detection_distance;
distance <= params_.max_detection_distance;
distance += params_.detection_step) {
auto check_point = calcPointAhead(current_pose, distance);
// 2. 检查该点的可视性
if (isOccludedFromSensor(check_point, occupancy_grid, current_pose)) {
OcclusionSpot occlusion_spot;
occlusion_spot.position = check_point;
occlusion_spot.distance_from_ego = distance;
occlusion_spot.detection_probability = calcDetectionProbability(distance);
occlusion_spot.confidence = calcConfidenceLevel(distance);
occlusion_spots.push_back(occlusion_spot);
}
}
return occlusion_spots;
}
std::vector<RiskAssessment> assessOcclusionRisks(
const std::vector<OcclusionSpot> & occlusion_spots,
const double current_velocity)
{
std::vector<RiskAssessment> risk_assessments;
for (const auto & occlusion_spot : occlusion_spots) {
RiskAssessment assessment;
// 1. 计算风险概率
assessment.risk_probability = calcRiskProbability(
occlusion_spot.distance_from_ego, current_velocity);
// 2. 计算风险严重性
assessment.risk_severity = calcRiskSeverity(occlusion_spot);
// 3. 计算总体风险
assessment.overall_risk = assessment.risk_probability * assessment.risk_severity;
// 4. 计算安全速度
assessment.safe_speed = calcSafeSpeed(assessment.overall_risk, current_velocity);
risk_assessments.push_back(assessment);
}
return risk_assessments;
}
double calcRiskPredictiveBraking(
const OcclusionSpot & occlusion_spot,
const double current_velocity)
{
// 基于预测性制动的风险计算
// 1. 计算到达遮挡点的时间
double tta = occlusion_spot.distance_from_ego / current_velocity;
// 2. 计算遮挡区域的大小
double occlusion_size = calcOcclusionSize(occlusion_spot);
// 3. 计算隐藏对象出现的概率
double emergence_probability = calcEmergenceProbability(occlusion_spot);
// 4. 计算碰撞概率
double collision_probability =
emergence_probability * occlusion_spot.detection_probability;
// 5. 计算风险值
double risk_value = collision_probability *
std::exp(-tta / params_.time_decay_factor) * occlusion_size;
return risk_value;
}
Trajectory applyRiskBasedSpeedLimits(
const Trajectory & reference_trajectory,
const std::vector<RiskAssessment> & risk_assessments)
{
Trajectory modified_trajectory = reference_trajectory;
for (auto & point : modified_trajectory.points) {
double min_speed_limit = std::numeric_limits<double>::max();
// 为每个轨迹点找到最小的速度限制
for (const auto & assessment : risk_assessments) {
double distance = calcDistance2D(point.pose.position, assessment.occlusion_position);
if (distance < params_.influence_distance) {
double speed_limit = assessment.safe_speed;
// 根据距离调整速度限制
double distance_factor = 1.0 - (distance / params_.influence_distance);
speed_limit = interpolateSpeed(
reference_velocity, speed_limit, distance_factor);
min_speed_limit = std::min(min_speed_limit, speed_limit);
}
}
if (min_speed_limit != std::numeric_limits<double>::max()) {
point.longitudinal_velocity_mps =
std::min(point.longitudinal_velocity_mps, min_speed_limit);
}
}
return modified_trajectory;
}
};
5. 运动规划模块 (Motion Planning)
5.1 障碍物巡航规划器
5.1.1 PID控制器实现
class PIDBasedPlanner {
private:
struct PIDGain {
double kp; // 比例增益
double ki; // 积分增益
double kd; // 微分增益
// 输出限制
double max_out;
double min_out;
// 各项限制
double max_p_effort;
double min_p_effort;
double max_i_effort;
double min_i_effort;
double max_d_effort;
double min_d_effort;
} pid_gain_;
// 状态变量
double integral_error_ = 0.0;
double prev_error_ = 0.0;
std::chrono::steady_clock::time_point prev_time_;
public:
double calculate(
const double target_distance,
const double current_distance,
const double current_velocity,
const std::chrono::steady_clock::time_point & current_time)
{
// 1. 计算误差
double error = target_distance - current_distance;
// 2. 计算时间差
double dt = std::chrono::duration<double>(current_time - prev_time_).count();
if (dt <= 0.0) {
return current_velocity; // 时间差无效，保持当前速度
}
// 3. 计算各项输出
double p_term = pid_gain_.kp * error;
integral_error_ += error * dt;
double i_term = pid_gain_.ki * integral_error_;
double derivative = (error - prev_error_) / dt;
double d_term = pid_gain_.kd * derivative;
// 4. 应用限制
p_term = std::clamp(p_term, pid_gain_.min_p_effort, pid_gain_.max_p_effort);
i_term = std::clamp(i_term, pid_gain_.min_i_effort, pid_gain_.max_i_effort);
d_term = std::clamp(d_term, pid_gain_.min_d_effort, pid_gain_.max_d_effort);
// 5. 计算总输出
double output = p_term + i_term + d_term;
output = std::clamp(output, pid_gain_.min_out, pid_gain_.max_out);
// 6. 更新状态
prev_error_ = error;
prev_time_ = current_time;
return output;
}
void reset()
{
integral_error_ = 0.0;
prev_error_ = 0.0;
prev_time_ = std::chrono::steady_clock::now();
}
};
5.1.2 优化控制器实现
class OptimizationBasedPlanner {
private:
struct OptimizationData {
std::vector<double> time_points;
std::vector<double> current_velocities;
std::vector<double> target_distances;
std::vector<double> obstacle_velocities;
std::vector<double> confidence_levels;
};
struct OptimizationResult {
std::vector<double> optimized_velocities;
std::vector<double> optimized_accelerations;
bool optimization_successful;
std::string error_message;
};
public:
OptimizationResult optimize(const OptimizationData & data)
{
OptimizationResult result;
try {
// 1. 设置优化问题
auto problem = setupOptimizationProblem(data);
// 2. 设置约束条件
setupConstraints(problem, data);
// 3. 设置目标函数
setupObjectiveFunction(problem, data);
// 4. 求解优化问题
auto solution = solveOptimizationProblem(problem);
// 5. 提取结果
result.optimized_velocities = solution.velocities;
result.optimized_accelerations = solution.accelerations;
result.optimization_successful = true;
} catch (const std::exception & e) {
result.optimization_successful = false;
result.error_message = e.what();
}
return result;
}
private:
OptProblem setupOptimizationProblem(const OptimizationData & data)
{
OptProblem problem;
// 1. 决策变量：速度和加速度
size_t num_points = data.time_points.size();
// 速度变量
problem.addDecisionVariables("velocity", num_points);
// 加速度变量
problem.addDecisionVariables("acceleration", num_points - 1);
// 2. 设置变量边界
for (size_t i = 0; i < num_points; ++i) {
problem.setVariableBounds("velocity", i, 0.0, params_.max_velocity);
}
for (size_t i = 0; i < num_points - 1; ++i) {
problem.setVariableBounds("acceleration", i,
params_.min_acceleration, params_.max_acceleration);
}
return problem;
}
void setupConstraints(OptProblem & problem, const OptimizationData & data)
{
// 1. 运动学约束
setupKinematicConstraints(problem, data);
// 2. 安全约束
setupSafetyConstraints(problem, data);
// 3. 舒适性约束
setupComfortConstraints(problem, data);
}
void setupKinematicConstraints(OptProblem & problem, const OptimizationData & data)
{
size_t num_points = data.time_points.size();
for (size_t i = 1; i < num_points; ++i) {
double dt = data.time_points[i] - data.time_points[i-1];
// v(i) = v(i-1) + a(i-1) * dt
problem.addLinearConstraint(
{"velocity", i}, 1.0,
{"velocity", i-1}, -1.0,
{"acceleration", i-1}, -dt,
OptConstraintType::EQUAL, 0.0);
}
}
void setupSafetyConstraints(OptProblem & problem, const OptimizationData & data)
{
for (size_t i = 0; i < num_points; ++i) {
// 安全距离约束
double min_safe_distance = calcSafeFollowingDistance(
data.current_velocities[i], data.obstacle_velocities[i]);
// s_target - v * dt >= min_safe_distance
problem.addLinearConstraint(
{"velocity", i}, -data.time_points[i],
OptConstraintType::GREATER_EQUAL, min_safe_distance);
}
}
void setupObjectiveFunction(OptProblem & problem, const OptimizationData & data)
{
// 1. 跟踪误差最小化
for (size_t i = 0; i < num_points; ++i) {
double weight = params_.tracking_error_weight;
if (i < data.confidence_levels.size()) {
weight *= data.confidence_levels[i]; // 根据置信度调整权重
}
// v * dt - target_distance
double coeff = data.time_points[i];
problem.addQuadraticCost({"velocity", i}, coeff,
data.target_distances[i], weight);
}
// 2. 加速度变化最小化
for (size_t i = 0; i < num_points - 1; ++i) {
problem.addQuadraticCost({"acceleration", i}, 1.0, 0.0,
params_.acceleration_weight);
}
// 3. 加加速度最小化
for (size_t i = 1; i < num_points - 1; ++i) {
double dt = data.time_points[i+1] - data.time_points[i];
// (a(i) - a(i-1)) / dt
problem.addQuadraticCost({"acceleration", i}, 1.0/dt,
{"acceleration", i-1}, -1.0/dt, 0.0,
params_.jerk_weight);
}
}
double calcSafeFollowingDistance(const double ego_velocity, const double obj_velocity)
{
// 安全距离 = 车辆长度 + 反应距离 + 制动距离
double reaction_distance = ego_velocity * params_.reaction_time;
double braking_distance = std::pow(ego_velocity, 2) / (2.0 * params_.max_deceleration);
double object_braking_distance = std::pow(obj_velocity, 2) / (2.0 * params_.max_deceleration);
return params_.vehicle_length + reaction_distance +
(braking_distance - object_braking_distance);
}
};
5.2 路径优化模块
5.2.1 MPT优化器
class MPTOptimizer {
private:
// 车辆模型参数
struct VehicleModel {
double wheel_base;
double max_steering_angle;
double max_curvature;
// 车辆圆形表示
int num_circles;
std::vector<double> circle_radiuses;
std::vector<double> circle_longitudinal_offsets;
};
VehicleModel vehicle_model_;
public:
Trajectory optimize(
const Trajectory & reference_trajectory,
const PlannerData & planner_data)
{
// 1. 离散化参考轨迹
auto discretized_trajectory = discretizeTrajectory(reference_trajectory);
// 2. 设置优化问题
auto opt_problem = setupMPTOptimization(discretized_trajectory, planner_data);
// 3. 求解优化问题
auto solution = solveMPTProblem(opt_problem);
// 4. 重构连续轨迹
auto optimized_trajectory = reconstructTrajectory(solution);
return optimized_trajectory;
}
private:
OptProblem setupMPTOptimization(
const DiscretizedTrajectory & reference_trajectory,
const PlannerData & planner_data)
{
OptProblem problem;
size_t num_points = reference_trajectory.size();
// 1. 决策变量
// 状态变量：[x, y, yaw, curvature]
problem.addDecisionVariables("state_x", num_points);
problem.addDecisionVariables("state_y", num_points);
problem.addDecisionVariables("state_yaw", num_points);
problem.addDecisionVariables("state_curvature", num_points);
// 控制变量：[curvature_rate]
problem.addDecisionVariables("control_curvature_rate", num_points - 1);
// 2. 目标函数
setupMPTObjectiveFunction(problem, reference_trajectory);
// 3. 约束条件
setupMPTConstraints(problem, reference_trajectory, planner_data);
return problem;
}
void setupMPTObjectiveFunction(
OptProblem & problem,
const DiscretizedTrajectory & reference_trajectory)
{
size_t num_points = reference_trajectory.size();
for (size_t i = 0; i < num_points; ++i) {
// 1. 轨迹跟踪权重
double tracking_weight = params_.tracking_error_weight;
// x方向跟踪误差
problem.addQuadraticCost({"state_x", i}, tracking_weight,
reference_trajectory[i].x, tracking_weight);
// y方向跟踪误差
problem.addQuadraticCost({"state_y", i}, tracking_weight,
reference_trajectory[i].y, tracking_weight);
// 航向跟踪误差
problem.addQuadraticCost({"state_yaw", i}, params_.heading_weight,
reference_trajectory[i].yaw, params_.heading_weight);
// 2. 控制输入权重
if (i < num_points - 1) {
problem.addQuadraticCost({"control_curvature_rate", i},
params_.curvature_rate_weight, 0.0,
params_.curvature_rate_weight);
}
}
// 3. 软约束惩罚项
setupSoftConstraintPenalties(problem, reference_trajectory);
}
void setupMPTConstraints(
OptProblem & problem,
const DiscretizedTrajectory & reference_trajectory,
const PlannerData & planner_data)
{
size_t num_points = reference_trajectory.size();
// 1. 运动学约束
setupKinematicConstraints(problem, num_points);
// 2. 车辆圆形约束
setupVehicleCircleConstraints(problem, planner_data);
// 3. 边界约束
setupBoundaryConstraints(problem, reference_trajectory);
}
void setupVehicleCircleConstraints(
OptProblem & problem,
const PlannerData & planner_data)
{
// 获取可行驶区域
auto drivable_area = planner_data.drivable_area;
for (size_t i = 0; i < num_points; ++i) {
for (int circle_idx = 0; circle_idx < vehicle_model_.num_circles; ++circle_idx) {
// 计算每个车辆圆形中心的位置
double offset_x = vehicle_model_.circle_longitudinal_offsets[circle_idx];
double radius = vehicle_model_.circle_radiuses[circle_idx];
// 圆心位置 = 车辆位置 + 航向偏移
double circle_x = state_x + offset_x * cos(state_yaw);
double circle_y = state_y + offset_x * sin(state_yaw);
// 约束：圆心在可行驶区域内
for (const auto & boundary_point : drivable_area.boundary) {
// (circle_x - boundary_x)^2 + (circle_y - boundary_y)^2 >= radius^2
problem.addQuadraticConstraint(
{"state_x", i}, 2.0, boundary_point.x, -offset_x * cos(boundary_point.y),
{"state_y", i}, 2.0, boundary_point.y, -offset_x * sin(boundary_point.y),
OptConstraintType::GREATER_EQUAL, std::pow(radius, 2));
}
}
}
}
void setupSoftConstraintPenalties(
OptProblem & problem,
const DiscretizedTrajectory & reference_trajectory)
{
// 1. 可行驶区域约束软化
for (size_t i = 0; i < num_points; ++i) {
for (int circle_idx = 0; circle_idx < vehicle_model_.num_circles; ++circle_idx) {
// 添加松弛变量
problem.addDecisionVariables("slack_" + std::to_string(i) + "_" + std::to_string(circle_idx), 1);
// 松弛变量惩罚
problem.addQuadraticCost({"slack_" + std::to_string(i) + "_" + std::to_string(circle_idx), 0},
params_.boundary_penalty_weight, 0.0,
params_.boundary_penalty_weight);
// 将松弛变量添加到约束中
problem.addLinearConstraint({"slack_" + std::to_string(i) + "_" + std::to_string(circle_idx), 0},
1.0, OptConstraintType::GREATER_EQUAL, 0.0);
}
}
// 2. 控制输入约束软化
for (size_t i = 0; i < num_points - 1; ++i) {
problem.addDecisionVariables("control_slack_" + std::to_string(i), 1);
problem.addQuadraticCost({"control_slack_" + std::to_string(i), 0},
params_.control_penalty_weight, 0.0,
params_.control_penalty_weight);
}
}
};
6. 轨迹验证模块
6.1 规划验证器
class PlanningValidator : public rclcpp::Node
{
private:
ValidationHandlingOption handling_option_;
// 验证检查器
std::vector<std::shared_ptr<ValidationChecker>> checkers_;
// 历史数据
Trajectory last_valid_trajectory_;
std::chrono::steady_clock::time_point last_valid_time_;
public:
Trajectory validateTrajectory(const Trajectory & input_trajectory)
{
ValidationReport report;
// 1. 执行所有检查
for (auto & checker : checkers_) {
auto checker_result = checker->check(input_trajectory);
report.addCheckerResult(checker->getName(), checker_result);
}
// 2. 汇总验证结果
bool is_valid = report.overallValid();
if (!is_valid) {
RCLCPP_WARN(get_logger(), "Trajectory validation failed: %s",
report.getErrorMessage().c_str());
// 3. 根据处理选项处理失效轨迹
switch (handling_option_) {
case ValidationHandlingOption::PUBLISH_AS_IS:
RCLCPP_INFO(get_logger(), "Publishing invalid trajectory as-is");
return input_trajectory;
case ValidationHandlingOption::STOP_PUBLISHING:
RCLCPP_WARN(get_logger(), "Stop publishing trajectory");
return Trajectory{};
case ValidationHandlingOption::PUBLISH_LAST_VALID:
RCLCPP_WARN(get_logger(), "Publishing last valid trajectory");
return last_valid_trajectory_;
}
}
// 4. 更新有效轨迹
last_valid_trajectory_ = input_trajectory;
last_valid_time_ = std::chrono::steady_clock::now();
return input_trajectory;
}
private:
void setupCheckers()
{
// 1. 字段有效性检查
checkers_.push_back(std::make_shared<FieldValidityChecker>());
// 2. 轨迹点间距检查
checkers_.push_back(std::make_shared<TrajectoryPointDistanceChecker>());
// 3. 曲率合理性检查
checkers_.push_back(std::make_shared<CurvatureSanityChecker>());
// 4. 相对角度变化率检查
checkers_.push_back(std::make_shared<RelativeAngleChecker>());
// 5. 横向加速度检查
checkers_.push_back(std::make_shared<LateralAccelerationChecker>());
// 6. 纵向加速度检查
checkers_.push_back(std::make_shared<LongitudinalAccelerationChecker>());
// 7. 转向角检查
checkers_.push_back(std::make_shared<SteeringAngleChecker>());
// 8. 转向角变化率检查
checkers_.push_back(std::make_shared<SteeringAngleRateChecker>());
// 9. 速度偏差检查
checkers_.push_back(std::make_shared<VelocityDeviationChecker>());
// 10. 距离偏差检查
checkers_.push_back(std::make_shared<DistanceDeviationChecker>());
// 11. 前向轨迹长度检查
checkers_.push_back(std::make_shared<ForwardTrajectoryLengthChecker>());
}
};
6.2 具体验证检查器
6.2.1 动力学检查器
class LateralAccelerationChecker : public ValidationChecker {
public:
CheckerResult check(const Trajectory & trajectory) override
{
CheckerResult result;
result.checker_name = "LateralAccelerationChecker";
result.is_valid = true;
for (size_t i = 0; i < trajectory.points.size(); ++i) {
const auto & point = trajectory.points[i];
// 计算横向加速度：a_lat = v² * κ
double velocity = point.longitudinal_velocity_mps;
double curvature = calculateCurvature(trajectory, i);
double lateral_acceleration = std::pow(velocity, 2) * curvature;
// 检查横向加速度是否在允许范围内
if (std::abs(lateral_acceleration) > params_.max_lateral_acceleration) {
result.is_valid = false;
result.error_messages.push_back(
"Point " + std::to_string(i) +
": Lateral acceleration " + std::to_string(lateral_acceleration) +
" exceeds limit " + std::to_string(params_.max_lateral_acceleration));
}
}
return result;
}
private:
double calculateCurvature(const Trajectory & trajectory, size_t index)
{
if (index == 0 || index >= trajectory.points.size() - 1) {
return 0.0;
}
const auto & prev_point = trajectory.points[index - 1];
const auto & curr_point = trajectory.points[index];
const auto & next_point = trajectory.points[index + 1];
// 使用三点法计算曲率
double dx1 = curr_point.pose.position.x - prev_point.pose.position.x;
double dy1 = curr_point.pose.position.y - prev_point.pose.position.y;
double dx2 = next_point.pose.position.x - curr_point.pose.position.x;
double dy2 = next_point.pose.position.y - curr_point.pose.position.y;
double cross_product = dx1 * dy2 - dy1 * dx2;
double dot_product = dx1 * dx2 + dy1 * dy2;
double distance1 = std::sqrt(dx1 * dx1 + dy1 * dy1);
double distance2 = std::sqrt(dx2 * dx2 + dy2 * dy2);
if (distance1 < 1e-6 || distance2 < 1e-6) {
return 0.0;
}
double angle = std::atan2(cross_product, dot_product);
double avg_distance = (distance1 + distance2) / 2.0;
return std::abs(angle) / avg_distance;
}
};
6.2.2 速度一致性检查器
class VelocityDeviationChecker : public ValidationChecker {
public:
CheckerResult check(const Trajectory & trajectory) override
{
CheckerResult result;
result.checker_name = "VelocityDeviationChecker";
result.is_valid = true;
for (size_t i = 0; i < trajectory.points.size(); ++i) {
const auto & point = trajectory.points[i];
// 计算期望速度（基于加速度和加加速度约束）
double expected_velocity = calculateExpectedVelocity(trajectory, i);
// 计算速度偏差
double velocity_deviation = std::abs(point.longitudinal_velocity_mps - expected_velocity);
// 检查速度偏差是否在允许范围内
if (velocity_deviation > params_.max_velocity_deviation) {
result.is_valid = false;
result.error_messages.push_back(
"Point " + std::to_string(i) +
": Velocity deviation " + std::to_string(velocity_deviation) +
" exceeds limit " + std::to_string(params_.max_velocity_deviation));
}
}
return result;
}
private:
double calculateExpectedVelocity(const Trajectory & trajectory, size_t index)
{
if (index == 0) {
return trajectory.points[0].longitudinal_velocity_mps;
}
const auto & prev_point = trajectory.points[index - 1];
const auto & curr_point = trajectory.points[index];
// 计算两点间距离
double distance = calcDistance2D(prev_point.pose.position, curr_point.pose.position);
// 计算时间间隔（基于速度）
double dt;
if (curr_point.longitudinal_velocity_mps > 0.1) {
dt = distance / curr_point.longitudinal_velocity_mps;
} else {
dt = 0.1; // 最小时间间隔
}
// 基于加速度约束计算期望速度
double max_accelerated_velocity =
prev_point.longitudinal_velocity_mps + params_.max_acceleration * dt;
double max_decelerated_velocity =
prev_point.longitudinal_velocity_mps + params_.max_deceleration * dt;
return std::clamp(curr_point.longitudinal_velocity_mps,
max_decelerated_velocity, max_accelerated_velocity);
}
};
7. 自由空间规划器
7.1 Hybrid A*算法实现
class HybridAStarPlanner {
private:
// 网格地图
std::shared_ptr<GridMap> grid_map_;
// 搜索节点
struct SearchNode {
double x, y, theta; // 位置和朝向
double g_cost; // 从起点到当前节点的代价
double h_cost; // 从当前节点到目标的启发式代价
double f_cost() const { return g_cost + h_cost; }
size_t parent_index; // 父节点索引
double steering_angle; // 转向角
double velocity; // 速度
// 重载比较运算符用于优先队列
bool operator<(const SearchNode & other) const {
return f_cost() > other.f_cost(); // 注意：优先队列是大顶堆
}
};
// 搜索参数
struct SearchParameters {
double wheel_base; // 轴距
double max_steering_angle; // 最大转向角
double min_turning_radius; // 最小转弯半径
double grid_resolution; // 网格分辨率
double theta_resolution; // 角度分辨率
double backward_penalty; // 倒车惩罚
double steering_penalty; // 转向惩罚
double reversing_penalty; // 反向惩罚
double goal_tolerance; // 目标容差
} params_;
public:
Path plan(
const Pose & start_pose,
const Pose & goal_pose,
const GridMap & grid_map)
{
grid_map_ = std::make_shared<GridMap>(grid_map);
// 1. 初始化搜索
std::priority_queue<SearchNode> open_set;
std::vector<SearchNode> closed_set;
std::unordered_map<size_t, size_t> node_index_map;
// 2. 创建起始节点
SearchNode start_node = createStartNode(start_pose, goal_pose);
open_set.push(start_node);
// 3. A*搜索
while (!open_set.empty()) {
// 获取f代价最小的节点
SearchNode current = open_set.top();
open_set.pop();
// 检查是否到达目标
if (isGoalReached(current, goal_pose)) {
return reconstructPath(current, closed_set);
}
// 生成后继节点
auto successors = generateSuccessors(current, grid_map_);
for (auto & successor : successors) {
// 检查是否在闭合集中
size_t successor_key = getNodeKey(successor);
if (node_index_map.find(successor_key) != node_index_map.end()) {
continue;
}
// 添加到开放集
open_set.push(successor);
node_index_map[successor_key] = closed_set.size();
closed_set.push_back(successor);
}
}
// 4. 搜索失败
RCLCPP_WARN(get_logger(), "Hybrid A* search failed");
return Path{};
}
private:
std::vector<SearchNode> generateSuccessors(
const SearchNode & current,
const GridMap & grid_map)
{
std::vector<SearchNode> successors;
// 1. 前进运动
for (double steering_angle : getSteeringAngles()) {
for (double velocity : getVelocities()) {
auto successor = generateMotion(current, steering_angle, velocity, true);
// 检查碰撞
if (!checkCollision(successor, grid_map)) {
successors.push_back(successor);
}
}
}
// 2. 后退运动
for (double steering_angle : getSteeringAngles()) {
for (double velocity : getVelocities()) {
auto successor = generateMotion(current, steering_angle, velocity, false);
// 检查碰撞
if (!checkCollision(successor, grid_map)) {
successors.push_back(successor);
}
}
}
return successors;
}
SearchNode generateMotion(
const SearchNode & current,
double steering_angle,
double velocity,
bool forward)
{
// 使用自行车模型生成运动
double dt = params_.time_step;
double distance = std::abs(velocity) * dt;
SearchNode next = current;
if (forward) {
next.velocity = velocity;
next.steering_angle = steering_angle;
} else {
next.velocity = -velocity;
next.steering_angle = -steering_angle;
}
// 运动学方程
double turning_radius = params_.wheel_base / std::tan(std::abs(next.steering_angle));
double delta_theta = distance / turning_radius;
next.theta = current.theta + (next.steering_angle > 0 ? delta_theta : -delta_theta);
next.x = current.x + turning_radius * (std::sin(next.theta) - std::sin(current.theta));
next.y = current.y + turning_radius * (-std::cos(next.theta) + std::cos(current.theta));
// 计算代价
next.g_cost = current.g_cost + calculateStepCost(current, next, steering_angle);
next.h_cost = calculateHeuristic(next, goal_pose_);
next.parent_index = closed_set.size();
return next;
}
double calculateStepCost(
const SearchNode & from,
const SearchNode & to,
double steering_angle)
{
// 1. 基础距离代价
double distance_cost = std::sqrt(
std::pow(to.x - from.x, 2) + std::pow(to.y - from.y, 2));
// 2. 转向惩罚
double steering_cost = std::abs(steering_angle) * params_.steering_penalty;
// 3. 倒车惩罚
double reversing_cost = (to.velocity < 0) ? params_.reversing_penalty : 0.0;
// 4. 后退惩罚（改变方向）
double backward_cost = ((from.velocity * to.velocity) < 0) ? params_.backward_penalty : 0.0;
return distance_cost + steering_cost + reversing_cost + backward_cost;
}
double calculateHeuristic(const SearchNode & node, const Pose & goal)
{
// 使用A*启发式函数
double distance = std::sqrt(
std::pow(node.x - goal.position.x, 2) +
std::pow(node.y - goal.position.y, 2));
// 考虑朝向差异
double angle_diff = normalizeAngle(node.theta - tf2::getYaw(goal.orientation));
double angle_cost = std::abs(angle_diff) * params_.angle_weight;
return distance + angle_cost;
}
bool checkCollision(const SearchNode & node, const GridMap & grid_map)
{
// 将车辆近似为矩形
double vehicle_length = params_.vehicle_length;
double vehicle_width = params_.vehicle_width;
// 获取车辆四个角的坐标
std::vector<Point2d> corners = getVehicleCorners(
node.x, node.y, node.theta, vehicle_length, vehicle_width);
// 检查每个角是否在障碍物上
for (const auto & corner : corners) {
if (grid_map.isOccupied(corner.x, corner.y)) {
return true;
}
}
// 检查车辆边缘
for (size_t i = 0; i < corners.size(); ++i) {
size_t next_i = (i + 1) % corners.size();
if (grid_map.isLineOccupied(corners[i], corners[next_i])) {
return true;
}
}
return false;
}
};
8. 性能优化和实时保证
8.1 时间限制机制
class TimeLimitedPlanner {
private:
std::chrono::milliseconds max_processing_time_{50}; // 50ms最大处理时间
public:
PlanningResult planWithTimeLimit(const PlanningInput & input)
{
auto start_time = std::chrono::steady_clock::now();
// 1. 尝试完整算法
try {
auto result = planFullAlgorithm(input);
auto elapsed_time = std::chrono::steady_clock::now() - start_time;
if (elapsed_time < max_processing_time_) {
return result;
}
} catch (const std::exception & e) {
RCLCPP_WARN(get_logger(), "Full algorithm failed: %s", e.what());
}
// 2. 尝试简化算法
try {
auto result = planSimplifiedAlgorithm(input);
auto elapsed_time = std::chrono::steady_clock::now() - start_time;
if (elapsed_time < max_processing_time_) {
RCLCPP_INFO(get_logger(), "Used simplified algorithm");
return result;
}
} catch (const std::exception & e) {
RCLCPP_ERROR(get_logger(), "Simplified algorithm failed: %s", e.what());
}
// 3. 使用紧急规划
RCLCPP_WARN(get_logger(), "Using emergency planning");
return planEmergencyTrajectory(input);
}
private:
Trajectory planEmergencyTrajectory(const PlanningInput & input)
{
Trajectory emergency_trajectory;
// 生成最简单的应急轨迹：直线减速停车
for (double t = 0.0; t <= 2.0; t += 0.1) {
TrajectoryPoint point;
// 线性减速
double velocity = std::max(0.0, input.current_velocity - 2.0 * t);
// 累积距离
double distance = input.current_velocity * t - 1.0 * t * t;
// 生成轨迹点
point.pose = extrapolatePose(input.current_pose, distance, 0.0);
point.longitudinal_velocity_mps = velocity;
point.acceleration_mps2 = -2.0;
emergency_trajectory.points.push_back(point);
}
return emergency_trajectory;
}
};
8.2 内存优化
class MemoryOptimizedPlanner {
private:
// 对象池
std::vector<std::unique_ptr<PlanningData>> data_pool_;
std::queue<std::unique_ptr<PlanningData>> available_data_;
std::mutex pool_mutex_;
// 缓存系统
std::unordered_map<size_t, CachedTrajectory> trajectory_cache_;
std::chrono::milliseconds cache_ttl_{100};
public:
std::unique_ptr<PlanningData> acquirePlanningData()
{
std::lock_guard<std::mutex> lock(pool_mutex_);
if (available_data_.empty()) {
// 创建新的数据对象
return std::make_unique<PlanningData>();
}
// 重用现有对象
auto data = std::move(available_data_.front());
available_data_.pop();
data->reset(); // 重置状态
return data;
}
void releasePlanningData(std::unique_ptr<PlanningData> data)
{
std::lock_guard<std::mutex> lock(pool_mutex_);
available_data_.push(std::move(data));
}
std::optional<Trajectory> getCachedTrajectory(size_t key)
{
auto it = trajectory_cache_.find(key);
if (it == trajectory_cache_.end()) {
return std::nullopt;
}
// 检查过期
auto now = std::chrono::steady_clock::now();
if (now - it->second.timestamp > cache_ttl_) {
trajectory_cache_.erase(it);
return std::nullopt;
}
return it->second.trajectory;
}
void cacheTrajectory(size_t key, const Trajectory & trajectory)
{
trajectory_cache_[key] = {
trajectory,
std::chrono::steady_clock::now()
};
// 限制缓存大小
if (trajectory_cache_.size() > 1000) {
auto oldest = trajectory_cache_.begin();
for (auto it = trajectory_cache_.begin(); it != trajectory_cache_.end(); ++it) {
if (it->second.timestamp < oldest->second.timestamp) {
oldest = it;
}
}
trajectory_cache_.erase(oldest);
}
}
};
9. 总结
Autoware规划模块展现了现代自动驾驶规划系统的先进性和完整性：
9.1 技术特点
1. 分层架构: 清晰的功能分层，从全局到局部逐级细化
2. 模块化设计: 每个功能模块独立，便于维护和扩展
3. 多算法支持: 支持多种规划算法和优化方法
4. 实时保证: 通过时间限制和算法简化确保实时性
5. 安全验证: 多层次的安全验证和故障处理
9.2 核心优势
- 全面性: 覆盖从任务规划到运动优化的完整规划流程
- 灵活性: 支持多种驾驶场景和规划策略
- 可靠性: 完善的错误处理和验证机制
- 可扩展性: 插件架构支持新算法的快速集成
9.3 应用价值
该规划模块为自动驾驶车辆提供了安全、舒适、高效的驾驶决策能力，是自动驾驶系统的核心组件。其先进的算法和完善的架构设计使其成为自动驾驶领域的重要参考实现。
扫一扫
5316
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
