Autoware控制模块详细分析

---

  团队博客: 汽车电子社区

---

1. 模块概述

  Autoware控制模块位于src/universe/autoware_universe/control/目录下，是自动驾驶系统的"手和脚"，负责将规划轨迹转换为实际的车辆控制命令。该模块采用分层控制架构，包括横向控制、纵向控制、以及各种安全检查和验证机制，确保车辆能够精确、安全、舒适地跟踪规划轨迹。

2. 模块架构设计

2.1 整体架构

2.2 控制器层次结构

3. 轨迹跟踪节点 (Trajectory Follower)

3.1 核心架构

class TrajectoryFollowerNode : public rclcpp::Node
{
private:
    // 控制器实例
    std::shared_ptr<LateralControllerBase> lateral_controller_;
    std::shared_ptr<LongitudinalControllerBase> longitudinal_controller_;
    
    // 控制模式
    ControlMode control_mode_;
    
    // 输入数据
    Trajectory::ConstSharedPtr current_trajectory_;
    Odometry::ConstSharedPtr current_odometry_;
    SteeringReport::ConstSharedPtr current_steering_;
    AccelWithPedal::ConstSharedPtr current_accel_;
    
    // 输出发布器
    rclcpp::Publisher<ControlCmdStamped>::SharedPtr pub_control_cmd_;
    rclcpp::Publisher<AckermannControlCommand>::SharedPtr pub_ackermann_cmd_;
    
    // 诊断接口
    rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr pub_debug_values_;
    rclcpp::Publisher<MarkerArray>::SharedPtr pub_debug_marker_;
    
    // 参数
    double ctrl_period_;
    bool enable_delay_compensation_;
    bool enable_steering_rate_limit_;
    
public:
    TrajectoryFollowerNode() : Node("trajectory_follower")
    {
        // 1. 初始化控制器
        setupControllers();
        
        // 2. 设置订阅和发布
        setupPubSub();
        
        // 3. 设置定时器
        setupTimer();
        
        // 4. 设置参数回调
        setupParameterCallback();
    }
    
private:
    void onTimer()
    {
        // 1. 检查数据有效性
        if (!isDataReady()) {
            publishStopCommand();
            return;
        }
        
        // 2. 更新控制状态
        updateControlState();
        
        // 3. 计算控制命令
        auto control_cmd = calculateControlCommand();
        
        // 4. 应用安全限制
        auto filtered_cmd = applySafetyFilters(control_cmd);
        
        // 5. 发布控制命令
        publishControlCommand(filtered_cmd);
        
        // 6. 更新诊断信息
        updateDiagnostics(filtered_cmd);
    }
};

3.2 控制状态管理

enum class ControlMode {
    INIT,                   // 初始化状态
    STOPPED,               // 停车状态
    DRIVING,               // 驾驶状态
    EMERGENCY,             // 紧急状态
    MANUAL,                // 手动状态
    OVERRIDE              // 覆盖状态
};

class ControlStateManager {
private:
    ControlMode current_mode_ = ControlMode::INIT;
    ControlMode previous_mode_ = ControlMode::INIT;
    
    // 状态转换条件
    std::map<std::pair<ControlMode, ControlMode>, std::function<bool()>> transition_conditions_;
    
    // 状态处理函数
    std::map<ControlMode, std::function<void()>> state_handlers_;
    
public:
    void update()
    {
        // 1. 检查状态转换
        checkStateTransitions();
        
        // 2. 执行当前状态处理
        executeCurrentState();
        
        // 3. 更新状态历史
        updateStateHistory();
    }
    
    bool requestTransition(ControlMode new_mode)
    {
        auto transition_key = std::make_pair(current_mode_, new_mode);
        auto it = transition_conditions_.find(transition_key);
        
        if (it != transition_conditions_.end() && it->second()) {
            previous_mode_ = current_mode_;
            current_mode_ = new_mode;
            return true;
        }
        
        return false;
    }
    
private:
    void setupTransitions()
    {
        // INIT -> STOPPED
        transition_conditions_[{ControlMode::INIT, ControlMode::STOPPED}] = 
            [this]() { return isInitialzed(); };
        
        // STOPPED -> DRIVING
        transition_conditions_[{ControlMode::STOPPED, ControlMode::DRIVING}] = 
            [this]() { return hasValidTrajectory() && isSystemReady(); };
        
        // DRIVING -> STOPPED
        transition_conditions_[{ControlMode::DRIVING, ControlMode::STOPPED}] = 
            [this]() { return !hasValidTrajectory() || isEmergencyTriggered(); };
        
        // ANY -> EMERGENCY
        transition_conditions_[{ControlMode::INIT, ControlMode::EMERGENCY}] = 
            [this]() { return isEmergencyTriggered(); };
        transition_conditions_[{ControlMode::STOPPED, ControlMode::EMERGENCY}] = 
            [this]() { return isEmergencyTriggered(); };
        transition_conditions_[{ControlMode::DRIVING, ControlMode::EMERGENCY}] = 
            [this]() { return isEmergencyTriggered(); };
    }
};

4. 横向控制器 (Lateral Controllers)

4.1 MPC横向控制器

4.1.1 核心实现

class MPCLateralController : public LateralControllerBase
{
private:
    // MPC求解器
    std::unique_ptr<MPCSolver> mpc_solver_;
    
    // 车辆模型
    std::shared_ptr<VehicleModelInterface> vehicle_model_;
    
    // 轨迹处理
    std::unique_ptr<MPCTrajectory> mpc_trajectory_;
    
    // 延迟补偿
    std::unique_ptr<SteeringPredictor> steering_predictor_;
    
    // 滤波器
    std::unique_ptr<LowpassFilter> steering_lpf_;
    
    // 参数
    MPCParams params_;
    
    // 状态变量
    MPCState mpc_state_;
    
public:
    LateralOutput compute(
        const Trajectory & trajectory,
        const Odometry & odometry,
        const SteeringReport & steering) override
    {
        // 1. 数据预处理
        auto processed_data = preprocessData(trajectory, odometry, steering);
        
        // 2. MPC求解
        auto mpc_result = solveMPC(processed_data);
        
        // 3. 后处理
        auto lateral_output = postprocessResult(mpc_result, processed_data);
        
        return lateral_output;
    }
    
private:
    struct MPCState {
        double x;              // 横向位置
        double y;              // 纵向位置
        double yaw;            // 航向角
        double vx;             // 纵向速度
        double wz;             // 偏航角速度
        double steer;          // 转向角
        double steer_rate;     // 转向角速度
    };
    
    MPCResult solveMPC(const ProcessedData & data)
    {
        MPCResult result;
        
        // 1. 状态估计
        estimateCurrentState(data);
        
        // 2. 预测时域设置
        setupPredictionHorizon(data);
        
        // 3. 优化问题构建
        auto opt_problem = buildOptimizationProblem(data);
        
        // 4. 约束条件设置
        setupConstraints(opt_problem, data);
        
        // 5. 目标函数设置
        setupObjectiveFunction(opt_problem, data);
        
        // 6. 求解优化问题
        result = solveOptimizationProblem(opt_problem);
        
        return result;
    }
    
    OptProblem buildOptimizationProblem(const ProcessedData & data)
    {
        OptProblem problem;
        
        // 1. 决策变量
        size_t N = params_.prediction_horizon;
        
        // 状态变量：[x, y, yaw, vx, wz, steer]
        problem.addDecisionVariables("state_x", N);
        problem.addDecisionVariables("state_y", N);
        problem.addDecisionVariables("state_yaw", N);
        problem.addDecisionVariables("state_vx", N);
        problem.addDecisionVariables("state_wz", N);
        problem.addDecisionVariables("state_steer", N);
        
        // 控制变量：[steer_rate]
        problem.addDecisionVariables("control_steer_rate", N - 1);
        
        // 2. 变量边界
        setupVariableBounds(problem, N);
        
        return problem;
    }
    
    void setupObjectiveFunction(OptProblem & problem, const ProcessedData & data)
    {
        size_t N = params_.prediction_horizon;
        
        for (size_t i = 0; i < N; ++i) {
            // 1. 轨迹跟踪误差
            double weight_x = params_.weight_lateral_error;
            double weight_y = params_.weight_heading_error;
            
            if (i < data.reference_trajectory.size()) {
                // x方向跟踪
                problem.addQuadraticCost({"state_x", i}, weight_x,
                                       data.reference_trajectory[i].pose.position.x, weight_x);
                
                // y方向跟踪
                problem.addQuadraticCost({"state_y", i}, weight_y,
                                       data.reference_trajectory[i].pose.position.y, weight_y);
                
                // 航向跟踪
                double ref_yaw = tf2::getYaw(data.reference_trajectory[i].pose.orientation);
                problem.addQuadraticCost({"state_yaw", i}, params_.weight_heading_error,
                                       ref_yaw, params_.weight_heading_error);
            }
            
            // 2. 控制输入权重
            if (i < N - 1) {
                problem.addQuadraticCost({"control_steer_rate", i},
                                       params_.weight_steer_rate, 0.0,
                                       params_.weight_steer_rate);
            }
            
            // 3. 终端权重
            if (i == N - 1) {
                problem.addQuadraticCost({"state_steer", i},
                                       params_.weight_terminal_steer, 0.0,
                                       params_.weight_terminal_steer);
            }
        }
    }
    
    void setupConstraints(OptProblem & problem, const ProcessedData & data)
    {
        size_t N = params_.prediction_horizon;
        
        // 1. 车辆动力学约束
        setupVehicleDynamicsConstraints(problem, N);
        
        // 2. 执行器约束
        setupActuatorConstraints(problem, N);
        
        // 3. 安全约束
        setupSafetyConstraints(problem, data, N);
    }
    
    void setupVehicleDynamicsConstraints(OptProblem & problem, size_t N)
    {
        double dt = params_.prediction_step;
        
        for (size_t i = 1; i < N; ++i) {
            // 使用自行车模型
            // x(k+1) = x(k) + vx(k) * cos(yaw(k)) * dt
            problem.addLinearConstraint(
                {"state_x", i}, 1.0,
                {"state_x", i-1}, -1.0,
                {"state_vx", i-1}, -dt * cos_yaw,
                OptConstraintType::EQUAL, 0.0);
            
            // y(k+1) = y(k) + vx(k) * sin(yaw(k)) * dt
            problem.addLinearConstraint(
                {"state_y", i}, 1.0,
                {"state_y", i-1}, -1.0,
                {"state_vx", i-1}, -dt * sin_yaw,
                OptConstraintType::EQUAL, 0.0);
            
            // yaw(k+1) = yaw(k) + wz(k) * dt
            problem.addLinearConstraint(
                {"state_yaw", i}, 1.0,
                {"state_yaw", i-1}, -1.0,
                {"state_wz", i-1}, -dt,
                OptConstraintType::EQUAL, 0.0);
            
            // steer(k+1) = steer(k) + steer_rate(k) * dt
            problem.addLinearConstraint(
                {"state_steer", i}, 1.0,
                {"state_steer", i-1}, -1.0,
                {"control_steer_rate", i-1}, -dt,
                OptConstraintType::EQUAL, 0.0);
        }
    }
};

4.1.2 车辆模型

class VehicleModelBicycleDynamics : public VehicleModelInterface
{
private:
    // 车辆参数
    double wheelbase_;           // 轴距
    double mass_;               // 质量
    double inertia_;            // 转动惯量
    double lf_;                 // 前轴到质心距离
    double lr_;                 // 后轴到质心距离
    double cf_;                 // 前轮侧偏刚度
    double cr_;                 // 后轮侧偏刚度
    
    // 状态维度
    static constexpr int STATE_DIM = 4;  // [x, y, yaw, vx]
    static constexpr int INPUT_DIM = 1;  // [steer]
    
public:
    Eigen::MatrixXd getStateMatrix(double vx, double steer) const override
    {
        Eigen::MatrixXd A = Eigen::MatrixXd::Zero(STATE_DIM, STATE_DIM);
        
        if (std::abs(vx) < 1e-6) {
            return A;  // 避免除零
        }
        
        double cos_yaw = std::cos(mpc_state_.yaw);
        double sin_yaw = std::sin(mpc_state_.yaw);
        
        // 线性化自行车模型
        double denominator = mass_ * vx * vx - 
            (lf_ * cf_ - lr_ * cr_) * wheelbase_;
        
        // x状态方程
        A(0, 2) = -vx * sin_yaw;
        A(0, 3) = cos_yaw;
        
        // y状态方程
        A(1, 2) = vx * cos_yaw;
        A(1, 3) = sin_yaw;
        
        // yaw状态方程
        A(2, 2) = -(cf_ + cr_) / (mass_ * vx);
        A(2, 3) = (cf_ * lf_ - cr_ * lr_) / (mass_ * vx * vx) - 1.0;
        A(2, 4) = cf_ / mass_;
        
        // vx状态方程
        A(3, 1) = vx * (cf_ + cr_) / (mass_ * vx);
        A(3, 3) = -(cf_ * lf_ - cr_ * lr_) / (mass_ * vx);
        
        return A;
    }
    
    Eigen::MatrixXd getInputMatrix(double vx) const override
    {
        Eigen::MatrixXd B = Eigen::MatrixXd::Zero(STATE_DIM, INPUT_DIM);
        
        if (std::abs(vx) < 1e-6) {
            return B;
        }
        
        // x输入矩阵
        B(0, 0) = 0.0;
        
        // y输入矩阵
        B(1, 0) = 0.0;
        
        // yaw输入矩阵
        B(2, 0) = cf_ * wheelbase_ / (mass_ * vx * vx);
        
        // vx输入矩阵
        B(3, 0) = -cf_ * lf_ / (mass_ * vx);
        
        return B;
    }
    
    Eigen::MatrixXd getOutputMatrix() const override
    {
        Eigen::MatrixXd C = Eigen::MatrixXd::Zero(2, STATE_DIM);
        
        // 输出：[y, yaw]
        C(0, 1) = 1.0;  // y
        C(1, 2) = 1.0;  // yaw
        
        return C;
    }
};

4.2 Pure Pursuit横向控制器

4.2.1 核心算法

class PurePursuitLateralController : public LateralControllerBase
{
private:
    // Pure Pursuit参数
    double lookahead_distance_gain_;
    double min_lookahead_distance_;
    double max_lookahead_distance_;
    
    // 收敛参数
    double convergence_threshold_;
    double steering_proportional_gain_;
    
    // 轨迹处理
    size_t current_trajectory_index_;
    bool found_closest_point_;
    
public:
    LateralOutput compute(
        const Trajectory & trajectory,
        const Odometry & odometry,
        const SteeringReport & steering) override
    {
        LateralOutput output;
        
        // 1. 查找最近点
        auto closest_point = findClosestTrajectoryPoint(trajectory, odometry);
        
        // 2. 计算前瞻距离
        double lookahead_distance = calculateLookaheadDistance(odometry);
        
        // 3. 查找前瞻点
        auto lookahead_point = findLookaheadPoint(
            trajectory, closest_point, lookahead_distance);
        
        // 4. 计算曲率
        double curvature = calculatePurePursuitCurvature(
            odometry, lookahead_point);
        
        // 5. 转换为转向角
        double steering_angle = convertCurvatureToSteeringAngle(curvature);
        
        // 6. 应用转向限制
        steering_angle = applySteeringLimits(steering_angle, steering);
        
        output.steering_angle = steering_angle;
        output.lookahead_distance = lookahead_distance;
        output.curvature = curvature;
        
        return output;
    }
    
private:
    double calculateLookaheadDistance(const Odometry & odometry)
    {
        // 基于速度的自适应前瞻距离
        double base_distance = lookahead_distance_gain_ * odometry->twist.twist.linear.x;
        
        // 限制在最小和最大值之间
        double lookahead_distance = std::clamp(base_distance,
                                             min_lookahead_distance_,
                                             max_lookahead_distance_);
        
        // 考虑轨迹曲率
        if (current_trajectory_index_ < current_trajectory_->points.size()) {
            double trajectory_curvature = calculateTrajectoryCurvature(
                current_trajectory_index_);
            
            // 曲率越大，前瞻距离越小
            double curvature_factor = 1.0 / (1.0 + std::abs(trajectory_curvature));
            lookahead_distance *= curvature_factor;
        }
        
        return lookahead_distance;
    }
    
    double calculatePurePursuitCurvature(
        const Odometry & odometry,
        const TrajectoryPoint & lookahead_point)
    {
        // 车辆位置
        double ego_x = odometry->pose.pose.position.x;
        double ego_y = odometry->pose.pose.position.y;
        double ego_yaw = tf2::getYaw(odometry->pose.pose.orientation);
        
        // 前瞻点位置
        double target_x = lookahead_point.pose.position.x;
        double target_y = lookahead_point.pose.position.y;
        
        // 转换到车辆坐标系
        double dx = target_x - ego_x;
        double dy = target_y - ego_y;
        
        double transformed_x = dx * std::cos(-ego_yaw) - dy * std::sin(-ego_yaw);
        double transformed_y = dx * std::sin(-ego_yaw) + dy * std::cos(-ego_yaw);
        
        // Pure Pursuit公式
        double curvature = 2.0 * transformed_y / 
            (transformed_x * transformed_x + transformed_y * transformed_y);
        
        return curvature;
    }
    
    double convertCurvatureToSteeringAngle(double curvature)
    {
        // 转向角 = curvature * wheelbase
        double steering_angle = curvature * params_.wheelbase;
        
        // 应用转向角限制
        steering_angle = std::clamp(steering_angle,
                                   -params_.max_steering_angle,
                                   params_.max_steering_angle);
        
        return steering_angle;
    }
    
    TrajectoryPoint findLookaheadPoint(
        const Trajectory & trajectory,
        const TrajectoryPoint & closest_point,
        double lookahead_distance)
    {
        // 从最近点开始搜索
        size_t search_index = std::distance(trajectory.points.begin(),
                                          std::find(trajectory.points.begin(),
                                                  trajectory.points.end(),
                                                  closest_point));
        
        double accumulated_distance = 0.0;
        TrajectoryPoint lookahead_point = closest_point;
        
        // 沿轨迹搜索前瞻点
        for (size_t i = search_index; i < trajectory.points.size(); ++i) {
            if (i > search_index) {
                double segment_distance = calculateDistance(
                    trajectory.points[i-1].pose.position,
                    trajectory.points[i].pose.position);
                
                accumulated_distance += segment_distance;
                
                if (accumulated_distance >= lookahead_distance) {
                    // 插值找到精确的前瞻点
                    double excess_distance = accumulated_distance - lookahead_distance;
                    lookahead_point = interpolateTrajectoryPoint(
                        trajectory.points[i-1], trajectory.points[i], 
                        segment_distance - excess_distance);
                    break;
                }
            }
            
            lookahead_point = trajectory.points[i];
        }
        
        return lookahead_point;
    }
};

5. 纵向控制器 (Longitudinal Controllers)

5.1 PID纵向控制器

5.1.1 核心实现

class PIDLongitudinalController : public LongitudinalControllerBase
{
private:
    // PID控制器
    std::unique_ptr<PIDController> velocity_pid_;
    std::unique_ptr<PIDController> acceleration_pid_;
    
    // 平滑停止控制器
    std::unique_ptr<SmoothStopController> smooth_stop_controller_;
    
    // 状态机
    LongitudinalState current_state_;
    
    // 过滤器
    std::unique_ptr<LowpassFilter> acceleration_filter_;
    
    // 参数
    LongitudinalParams params_;
    
    // 历史数据
    std::deque<TrajectoryPoint> trajectory_history_;
    
public:
    LongitudinalOutput compute(
        const Trajectory & trajectory,
        const Odometry & odometry,
        const AccelWithPedal & accel) override
    {
        LongitudinalOutput output;
        
        // 1. 状态机更新
        updateStateMachine(trajectory, odometry);
        
        // 2. 根据状态计算控制
        switch (current_state_) {
            case LongitudinalState::STOP:
                output = computeStopControl();
                break;
                
            case LongitudinalState::STARTING:
                output = computeStartingControl(trajectory, odometry);
                break;
                
            case LongitudinalState::CRUISING:
                output = computeCruisingControl(trajectory, odometry);
                break;
                
            case LongitudinalState::STOPPING:
                output = computeStoppingControl(trajectory, odometry);
                break;
                
            case LongitudinalState::EMERGENCY:
                output = computeEmergencyControl();
                break;
        }
        
        // 3. 应用安全限制
        output = applySafetyLimits(output);
        
        // 4. 后处理
        output = postprocessOutput(output);
        
        return output;
    }
    
private:
    enum class LongitudinalState {
        STOP,                   // 停车状态
        STARTING,              // 启动状态
        CRUISING,              // 巡航状态
        STOPPING,              // 停车中状态
        EMERGENCY              // 紧急状态
    };
    
    LongitudinalOutput computeCruisingControl(
        const Trajectory & trajectory,
        const Odometry & odometry)
    {
        LongitudinalOutput output;
        
        // 1. 获取目标速度和加速度
        auto target_motion = getTargetMotion(trajectory, odometry);
        
        // 2. 计算速度误差
        double velocity_error = target_motion.velocity - odometry->twist.twist.linear.x;
        
        // 3. PID速度控制
        double acceleration_cmd = velocity_pid_->calculate(
            velocity_error, params_.control_period);
        
        // 4. 添加前馈加速度
        acceleration_cmd += target_motion.acceleration;
        
        // 5. 应用加速度限制
        acceleration_cmd = std::clamp(acceleration_cmd,
                                     params_.min_acceleration,
                                     params_.max_acceleration);
        
        // 6. 转换为油门/刹车
        auto brake_accel = calculateBrakeAcceleration(acceleration_cmd);
        auto throttle = calculateThrottle(acceleration_cmd);
        
        output.acceleration = acceleration_cmd;
        output.brake_acceleration = brake_accel;
        output.throttle = throttle;
        
        return output;
    }
    
    LongitudinalOutput computeStoppingControl(
        const Trajectory & trajectory,
        const Odometry & odometry)
    {
        LongitudinalOutput output;
        
        // 1. 使用平滑停止控制器
        auto smooth_stop_result = smooth_stop_controller_->compute(
            trajectory, odometry);
        
        if (smooth_stop_result.is_smooth_stop_required) {
            output.acceleration = smooth_stop_result.acceleration;
            output.brake_acceleration = smooth_stop_result.brake_acceleration;
            output.throttle = 0.0;
        } else {
            // 切换到完全停车状态
            current_state_ = LongitudinalState::STOP;
            return computeStopControl();
        }
        
        return output;
    }
    
    LongitudinalOutput computeEmergencyControl()
    {
        LongitudinalOutput output;
        
        // 紧急制动：最大减速度
        output.acceleration = params_.emergency_deceleration;
        output.brake_acceleration = params_.emergency_deceleration;
        output.throttle = 0.0;
        
        return output;
    }
};

5.1.2 平滑停止控制器

class SmoothStopController
{
private:
    // 平滑停止参数
    double stop_distance_;
    double stop_velocity_;
    double stop_acceleration_;
    double stop_jerk_;
    
    // 状态
    bool is_smooth_stop_active_;
    double initial_velocity_;
    double initial_distance_;
    std::chrono::steady_clock::time_point start_time_;
    
public:
    SmoothStopResult compute(
        const Trajectory & trajectory,
        const Odometry & odometry)
    {
        SmoothStopResult result;
        
        // 1. 检查是否需要平滑停止
        if (!isSmoothStopRequired(trajectory, odometry)) {
            result.is_smooth_stop_required = false;
            is_smooth_stop_active_ = false;
            return result;
        }
        
        // 2. 初始化平滑停止
        if (!is_smooth_stop_active_) {
            initializeSmoothStop(trajectory, odometry);
        }
        
        // 3. 计算当前时刻的加速度
        result.acceleration = calculateSmoothStopAcceleration();
        result.brake_acceleration = result.acceleration;
        result.throttle = 0.0;
        result.is_smooth_stop_required = true;
        
        // 4. 检查是否完成
        if (isSmoothStopCompleted(odometry)) {
            is_smooth_stop_active_ = false;
        }
        
        return result;
    }
    
private:
    struct SmoothStopResult {
        bool is_smooth_stop_required;
        double acceleration;
        double brake_acceleration;
        double throttle;
    };
    
    bool isSmoothStopRequired(
        const Trajectory & trajectory,
        const Odometry & odometry)
    {
        // 1. 检查轨迹终点距离
        double distance_to_end = calculateDistanceToTrajectoryEnd(
            trajectory, odometry);
        
        // 2. 检查当前速度
        double current_velocity = odometry->twist.twist.linear.x;
        
        // 3. 判断是否需要平滑停止
        if (distance_to_end < stop_distance_ && current_velocity > stop_velocity_) {
            return true;
        }
        
        return false;
    }
    
    void initializeSmoothStop(
        const Trajectory & trajectory,
        const Odometry & odometry)
    {
        is_smooth_stop_active_ = true;
        start_time_ = std::chrono::steady_clock::now();
        initial_velocity_ = odometry->twist.twist.linear.x;
        initial_distance_ = calculateDistanceToTrajectoryEnd(trajectory, odometry);
    }
    
    double calculateSmoothStopAcceleration()
    {
        auto current_time = std::chrono::steady_clock::now();
        double elapsed_time = std::chrono::duration<double>(
            current_time - start_time_).count();
        
        // 使用5次多项式生成平滑的加速度曲线
        // a(t) = -6jerk * (t/T)^2 * (1 - t/T)^2
        double t = elapsed_time;
        double T = calculateStopTime(initial_velocity_, stop_velocity_, initial_distance_);
        
        if (t >= T) {
            return stop_acceleration_;
        }
        
        double normalized_time = t / T;
        double acceleration = -6.0 * stop_jerk_ * 
            std::pow(normalized_time, 2) * 
            std::pow(1.0 - normalized_time, 2);
        
        return std::clamp(acceleration, stop_acceleration_, 0.0);
    }
    
    double calculateStopTime(
        double initial_velocity,
        double final_velocity,
        double distance)
    {
        // 基于运动学方程计算停止时间
        // s = (v0 + vf) * t / 2
        double avg_velocity = (initial_velocity + final_velocity) / 2.0;
        return distance / avg_velocity;
    }
};

5.2 PID控制器实现

class PIDController
{
private:
    struct PIDParams {
        double kp;              // 比例增益
        double ki;              // 积分增益
        double kd;              // 微分增益
        
        // 限制参数
        double max_output;      // 最大输出
        double min_output;      // 最小输出
        double max_integral;    // 最大积分项
        
        // 滤波参数
        double lpf_cutoff_hz;   // 低通滤波器截止频率
    } params_;
    
    // 状态变量
    double integral_error_ = 0.0;
    double prev_error_ = 0.0;
    std::chrono::steady_clock::time_point prev_time_;
    
    // 滤波器
    std::unique_ptr<LowpassFilter> derivative_filter_;
    
public:
    double calculate(double error, double dt)
    {
        // 1. 计算各项输出
        double p_term = params_.kp * error;
        
        // 积分项（抗饱和）
        integral_error_ += error * dt;
        integral_error_ = std::clamp(integral_error_, 
                                   -params_.max_integral, 
                                   params_.max_integral);
        double i_term = params_.ki * integral_error_;
        
        // 微分项（带滤波）
        double derivative = (error - prev_error_) / dt;
        double filtered_derivative = derivative_filter_->filter(derivative);
        double d_term = params_.kd * filtered_derivative;
        
        // 2. 计算总输出
        double output = p_term + i_term + d_term;
        
        // 3. 输出限制
        output = std::clamp(output, params_.min_output, params_.max_output);
        
        // 4. 更新状态
        prev_error_ = error;
        
        return output;
    }
    
    void reset()
    {
        integral_error_ = 0.0;
        prev_error_ = 0.0;
        derivative_filter_->reset();
    }
    
    void setGains(double kp, double ki, double kd)
    {
        params_.kp = kp;
        params_.ki = ki;
        params_.kd = kd;
    }
    
    void setOutputLimits(double min_output, double max_output)
    {
        params_.min_output = min_output;
        params_.max_output = max_output;
    }
};

6. 安全监控和验证

6.1 车辆命令网关

class VehicleCmdGate : public rclcpp::Node
{
private:
    // 命令来源
    enum class CmdSource {
        TRAJECTORY_FOLLOWER,     // 轨迹跟踪器
        EXTERNAL_CMD,           // 外部命令
        REMOTE_CONTROL,         // 遥控
        MANUAL_CONTROL          // 手动控制
    };
    
    // 当前命令源
    CmdSource current_cmd_source_ = CmdSource::TRAJECTORY_FOLLOWER;
    
    // 命令过滤器
    std::unique_ptr<VehicleCmdFilter> cmd_filter_;
    
    // 暂停接口
    std::unique_ptr<ADAPIPauseInterface> pause_interface_;
    
    // 安全检查器
    std::vector<std::shared_ptr<SafetyChecker>> safety_checkers_;
    
    // 命令历史
    std::deque<ControlCmd> cmd_history_;
    
public:
    void onControlCommand(const ControlCmd::ConstSharedPtr msg)
    {
        // 1. 命令来源识别
        auto source = identifyCommandSource(msg);
        
        // 2. 命令优先级检查
        if (!hasCommandPriority(source)) {
            RCLCPP_WARN(get_logger(), "Command from %s has lower priority", 
                       toString(source).c_str());
            return;
        }
        
        // 3. 安全过滤
        auto filtered_cmd = cmd_filter_->filter(*msg);
        
        // 4. 安全检查
        if (!performSafetyChecks(filtered_cmd)) {
            RCLCPP_ERROR(get_logger(), "Safety check failed, command rejected");
            return;
        }
        
        // 5. 命令验证
        if (!validateCommand(filtered_cmd)) {
            RCLCPP_ERROR(get_logger(), "Command validation failed");
            return;
        }
        
        // 6. 更新命令源
        current_cmd_source_ = source;
        
        // 7. 发布命令
        publishFilteredCommand(filtered_cmd);
        
        // 8. 更新历史
        updateCommandHistory(filtered_cmd);
    }
    
private:
    bool performSafetyChecks(const ControlCmd & cmd)
    {
        for (auto & checker : safety_checkers_) {
            auto result = checker->check(cmd);
            
            if (!result.is_safe) {
                RCLCPP_WARN(get_logger(), "Safety check '%s' failed: %s",
                           checker->getName().c_str(), 
                           result.error_message.c_str());
                return false;
            }
        }
        
        return true;
    }
    
    bool validateCommand(const ControlCmd & cmd)
    {
        // 1. 检查命令完整性
        if (!hasValidFields(cmd)) {
            return false;
        }
        
        // 2. 检查数值范围
        if (!isInRange(cmd)) {
            return false;
        }
        
        // 3. 检查变化率
        if (!isChangeRateValid(cmd)) {
            return false;
        }
        
        return true;
    }
    
    bool isChangeRateValid(const ControlCmd & cmd)
    {
        if (cmd_history_.empty()) {
            return true;
        }
        
        const auto & prev_cmd = cmd_history_.back();
        auto time_diff = cmd.header.stamp - prev_cmd.header.stamp;
        
        if (time_diff.seconds() <= 0.0) {
            return false;  // 时间戳无效
        }
        
        // 检查转向角变化率
        double steer_rate = std::abs(cmd.steering_angle - prev_cmd.steering_angle) / 
                           time_diff.seconds();
        if (steer_rate > params_.max_steering_rate) {
            RCLCPP_WARN(get_logger(), "Steering rate %f exceeds limit %f",
                       steer_rate, params_.max_steering_rate);
            return false;
        }
        
        // 检查加速度变化率
        double accel_rate = std::abs(cmd.acceleration - prev_cmd.acceleration) / 
                           time_diff.seconds();
        if (accel_rate > params_.max_acceleration_rate) {
            RCLCPP_WARN(get_logger(), "Acceleration rate %f exceeds limit %f",
                       accel_rate, params_.max_acceleration_rate);
            return false;
        }
        
        return true;
    }
};

6.2 控制验证器

class ControlValidator : public rclcpp::Node
{
private:
    // 验证检查器
    std::vector<std::shared_ptr<ValidationChecker>> checkers_;
    
    // 诊断发布器
    rclcpp::Publisher<DiagnosticArray>::SharedPtr pub_diagnostics_;
    
    // 调试发布器
    rclcpp::Publisher<MarkerArray>::SharedPtr pub_debug_markers_;
    rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr pub_debug_values_;
    
    // 验证历史
    std::deque<ValidationReport> validation_history_;
    
public:
    void onControlCommand(const ControlCmd::ConstSharedPtr cmd)
    {
        ValidationReport report;
        
        // 1. 执行所有验证检查
        for (auto & checker : checkers_) {
            auto checker_result = checker->check(*cmd);
            report.addCheckerResult(checker->getName(), checker_result);
        }
        
        // 2. 计算总体评分
        report.overall_score = calculateOverallScore(report);
        report.is_valid = report.overall_score > params_.validation_threshold;
        
        // 3. 发布诊断信息
        publishDiagnostics(report);
        
        // 4. 更新历史
        updateValidationHistory(report);
        
        // 5. 调试可视化
        publishDebugVisualization(*cmd, report);
    }
    
private:
    void setupCheckers()
    {
        // 1. 基础控制检查
        checkers_.push_back(std::make_shared<ControlRangeChecker>());
        checkers_.push_back(std::make_shared<ControlRateChecker>());
        
        // 2. 车辆动力学检查
        checkers_.push_back(std::make_shared<VehicleDynamicsChecker>());
        checkers_.push_back(std::make_shared<TireForceChecker>());
        
        // 3. 舒适性检查
        checkers_.push_back(std::make_shared<ComfortChecker>());
        checkers_.push_back(std::make_shared<JerkChecker>());
        
        // 4. 安全性检查
        checkers_.push_back(std::make_shared<StabilityChecker>());
        checkers_.push_back(std::make_shared<LaneKeepingChecker>());
        
        // 5. 性能检查
        checkers_.push_back(std::make_shared<TrackingPerformanceChecker>());
        checkers_.push_back(std::make_shared<ControlDelayChecker>());
    }
};

6.3 自主紧急制动

class AutonomousEmergencyBraking : public rclcpp::Node
{
private:
    // AEB状态
    enum class AEBState {
        INACTIVE,               // 未激活
        WARNING,                // 警告状态
        TRIGGERED,              // 触发状态
        RECOVERY               // 恢复状态
    };
    
    AEBState current_state_ = AEBState::INACTIVE;
    
    // 碰撞检测器
    std::shared_ptr<CollisionDetector> collision_detector_;
    
    // 风险评估器
    std::shared_ptr<RiskAssessment> risk_assessment_;
    
    // 制动策略
    std::shared_ptr<BrakeStrategy> brake_strategy_;
    
    // 参数
    AEBParams params_;
    
public:
    void onPredictedObjects(const PredictedObjects::ConstSharedPtr objects)
    {
        // 1. 风险评估
        auto risk_assessment = risk_assessment_->assess(*objects);
        
        // 2. 状态更新
        updateAEBState(risk_assessment);
        
        // 3. 根据状态执行操作
        switch (current_state_) {
            case AEBState::INACTIVE:
                // 监控状态，无需操作
                break;
                
            case AEBState::WARNING:
                handleWarningState(risk_assessment);
                break;
                
            case AEBState::TRIGGERED:
                handleTriggeredState(risk_assessment);
                break;
                
            case AEBState::RECOVERY:
                handleRecoveryState(risk_assessment);
                break;
        }
        
        // 4. 发布状态
        publishAEBState();
    }
    
private:
    void updateAEBState(const RiskAssessment & risk)
    {
        AEBState new_state = current_state_;
        
        switch (current_state_) {
            case AEBState::INACTIVE:
                if (risk.max_risk > params_.warning_threshold) {
                    new_state = AEBState::WARNING;
                }
                break;
                
            case AEBState::WARNING:
                if (risk.max_risk > params_.trigger_threshold) {
                    new_state = AEBState::TRIGGERED;
                } else if (risk.max_risk < params_.recovery_threshold) {
                    new_state = AEBState::RECOVERY;
                }
                break;
                
            case AEBState::TRIGGERED:
                if (risk.max_risk < params_.recovery_threshold) {
                    new_state = AEBState::RECOVERY;
                }
                break;
                
            case AEBState::RECOVERY:
                if (risk.max_risk > params_.warning_threshold) {
                    new_state = AEBState::WARNING;
                } else if (risk.max_risk < params_.inactive_threshold) {
                    new_state = AEBState::INACTIVE;
                }
                break;
        }
        
        if (new_state != current_state_) {
            RCLCPP_INFO(get_logger(), "AEB state changed: %s -> %s",
                       toString(current_state_).c_str(),
                       toString(new_state).c_str());
            current_state_ = new_state;
        }
    }
    
    void handleTriggeredState(const RiskAssessment & risk)
    {
        // 1. 计算制动力度
        auto brake_command = brake_strategy_->calculateBrake(risk);
        
        // 2. 发布紧急制动命令
        publishEmergencyBrakeCommand(brake_command);
        
        // 3. 更新诊断信息
        updateDiagnostics(risk);
        
        // 4. 记录事件
        logAE BEvent(risk);
    }
};

7. 控制性能分析

7.1 跟踪误差分析

class ControlPerformanceAnalysis : public rclcpp::Node
{
private:
    // 性能指标计算器
    std::shared_ptr<TrackingMetricsCalculator> tracking_metrics_;
    std::shared_ptr<ComfortMetricsCalculator> comfort_metrics_;
    std::shared_ptr<SafetyMetricsCalculator> safety_metrics_;
    
    // 数据存储
    std::deque<PerformanceData> performance_history_;
    
    // 分析窗口
    std::chrono::seconds analysis_window_{30};
    
public:
    void onControlCommand(const ControlCmd::ConstSharedPtr cmd)
    {
        // 1. 记录控制数据
        recordControlData(*cmd);
        
        // 2. 计算性能指标
        if (hasEnoughData()) {
            auto metrics = calculatePerformanceMetrics();
            
            // 3. 发布性能报告
            publishPerformanceReport(metrics);
            
            // 4. 更新历史记录
            updatePerformanceHistory(metrics);
        }
    }
    
private:
    struct PerformanceMetrics {
        // 跟踪性能
        double lateral_error_rms;          // 横向误差均方根
        double longitudinal_error_rms;     // 纵向误差均方根
        double heading_error_rms;          // 航向误差均方根
        
        // 控制性能
        double steering_control_effort;     // 转向控制努力
        double acceleration_control_effort; // 加速度控制努力
        
        // 舒适性指标
        double lateral_acceleration_rms;   // 横向加速度均方根
        double longitudinal_jerk_rms;      // 纵向加加速度均方根
        
        // 安全指标
        double min_time_to_collision;       // 最小碰撞时间
        double lane_departure_rate;        // 偏离车道率
        
        // 效率指标
        double average_velocity;           // 平均速度
        double travel_time;                 // 行驶时间
    };
    
    PerformanceMetrics calculatePerformanceMetrics()
    {
        PerformanceMetrics metrics;
        
        // 1. 跟踪性能计算
        metrics = tracking_metrics_->calculate(performance_history_);
        
        // 2. 控制性能计算
        metrics.steering_control_effort = calculateControlEffort("steering");
        metrics.acceleration_control_effort = calculateControlEffort("acceleration");
        
        // 3. 舒适性指标计算
        metrics = comfort_metrics_->calculate(performance_history_);
        
        // 4. 安全指标计算
        metrics = safety_metrics_->calculate(performance_history_);
        
        // 5. 效率指标计算
        metrics.average_velocity = calculateAverageVelocity();
        metrics.travel_time = calculateTravelTime();
        
        return metrics;
    }
    
    double calculateControlEffort(const std::string & control_type)
    {
        double total_effort = 0.0;
        size_t count = 0;
        
        for (const auto & data : performance_history_) {
            if (control_type == "steering") {
                total_effort += std::abs(data.control_cmd.steering_angle);
            } else if (control_type == "acceleration") {
                total_effort += std::abs(data.control_cmd.acceleration);
            }
            ++count;
        }
        
        return count > 0 ? total_effort / count : 0.0;
    }
};

8. 总结

  Autoware控制模块展现了现代自动驾驶控制系统的先进性和完整性：

8.1 技术特点

  1. 分层控制架构: 横向和纵向控制的独立设计和协调
  2. 多控制器支持: MPC、PID、Pure Pursuit等多种控制算法
  3. 安全机制: 多层次的安全检查和紧急制动
  4. 性能优化: 实时性保证和资源优化
  5. 状态管理: 完善的状态机和转换逻辑

8.2 核心优势

  - 高精度控制: 先进的MPC算法实现厘米级跟踪精度
  - 舒适驾乘: 平滑的加速度和转向控制
  - 安全可靠: 多重安全验证和故障处理机制
  - 实时响应: 50Hz的高频控制输出
  - 可扩展性: 模块化设计便于功能扩展

8.3 应用价值

  该控制模块为自动驾驶车辆提供了精确、安全、舒适的轨迹跟踪能力，是自动驾驶系统的执行基础。其先进的控制算法和完善的安全机制使其成为自动驾驶领域的重要参考实现。

  控制系统的设计充分体现了现代控制理论的工程应用，结合了模型预测控制、最优控制、鲁棒控制等先进技术，为自动驾驶的产业化应用奠定了坚实基础。