百度Apollo5.0控制模块代码学习（七）MPC控制
2024-06-14 06:36:38
1 流程图
2代码分析
以下内容对代码进行分析：
以下代码是加载MPC控制的相关配置参数；
bool MPCController::LoadControlConf(const ControlConf *control_conf) {if (!control_conf) {AERROR << "[MPCController] control_conf = nullptr";return false;}vehicle_param_ = VehicleConfigHelper::Instance()->GetConfig().vehicle_param();ts_ = control_conf->mpc_controller_conf().ts();CHECK_GT(ts_, 0.0) << "[MPCController] Invalid control update interval.";cf_ = control_conf->mpc_controller_conf().cf();cr_ = control_conf->mpc_controller_conf().cr();wheelbase_ = vehicle_param_.wheel_base();steer_ratio_ = vehicle_param_.steer_ratio();steer_single_direction_max_degree_ =vehicle_param_.max_steer_angle() * 180 / M_PI;max_lat_acc_ = control_conf->mpc_controller_conf().max_lateral_acceleration();wheel_single_direction_max_degree_ =steer_single_direction_max_degree_ / steer_ratio_ / 180 * M_PI;max_acceleration_ = vehicle_param_.max_acceleration();max_deceleration_ = vehicle_param_.max_deceleration();const double mass_fl = control_conf->mpc_controller_conf().mass_fl();const double mass_fr = control_conf->mpc_controller_conf().mass_fr();const double mass_rl = control_conf->mpc_controller_conf().mass_rl();const double mass_rr = control_conf->mpc_controller_conf().mass_rr();const double mass_front = mass_fl + mass_fr;const double mass_rear = mass_rl + mass_rr;mass_ = mass_front + mass_rear;lf_ = wheelbase_ * (1.0 - mass_front / mass_);lr_ = wheelbase_ * (1.0 - mass_rear / mass_);iz_ = lf_ * lf_ * mass_front + lr_ * lr_ * mass_rear;mpc_eps_ = control_conf->mpc_controller_conf().eps();mpc_max_iteration_ = control_conf->mpc_controller_conf().max_iteration();throttle_lowerbound_ =std::max(vehicle_param_.throttle_deadzone(),control_conf->mpc_controller_conf().throttle_minimum_action());brake_lowerbound_ =std::max(vehicle_param_.brake_deadzone(),control_conf->mpc_controller_conf().brake_minimum_action());minimum_speed_protection_ = control_conf->minimum_speed_protection();max_acceleration_when_stopped_ =control_conf->max_acceleration_when_stopped();max_abs_speed_when_stopped_ = vehicle_param_.max_abs_speed_when_stopped();standstill_acceleration_ =control_conf->mpc_controller_conf().standstill_acceleration();enable_mpc_feedforward_compensation_ =control_conf->mpc_controller_conf().enable_mpc_feedforward_compensation();unconstraint_control_diff_limit_ =control_conf->mpc_controller_conf().unconstraint_control_diff_limit();LoadControlCalibrationTable(control_conf->mpc_controller_conf());AINFO << "MPC conf loaded";return true;
}
以下代码是对MPC控制器进行初始化，包括矩阵A，矩阵B，矩阵C，矩阵K，矩阵R，矩阵Q，加载MPC增益调度序列，初始化滤波器，
Status MPCController::Init(const ControlConf *control_conf) {if (!LoadControlConf(control_conf)) {AERROR << "failed to load control conf";return Status(ErrorCode::CONTROL_COMPUTE_ERROR,"failed to load control_conf");}// Matrix init operations.matrix_a_ = Matrix::Zero(basic_state_size_, basic_state_size_);matrix_ad_ = Matrix::Zero(basic_state_size_, basic_state_size_);matrix_a_(0, 1) = 1.0;matrix_a_(1, 2) = (cf_ + cr_) / mass_;matrix_a_(2, 3) = 1.0;matrix_a_(3, 2) = (lf_ * cf_ - lr_ * cr_) / iz_;matrix_a_(4, 5) = 1.0;matrix_a_(5, 5) = 0.0;// TODO(QiL): expand the model to accommodate more combined states.matrix_a_coeff_ = Matrix::Zero(basic_state_size_, basic_state_size_);matrix_a_coeff_(1, 1) = -(cf_ + cr_) / mass_;matrix_a_coeff_(1, 3) = (lr_ * cr_ - lf_ * cf_) / mass_;matrix_a_coeff_(2, 3) = 1.0;matrix_a_coeff_(3, 1) = (lr_ * cr_ - lf_ * cf_) / iz_;matrix_a_coeff_(3, 3) = -1.0 * (lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / iz_;matrix_b_ = Matrix::Zero(basic_state_size_, controls_);matrix_bd_ = Matrix::Zero(basic_state_size_, controls_);matrix_b_(1, 0) = cf_ / mass_;matrix_b_(3, 0) = lf_ * cf_ / iz_;matrix_b_(4, 1) = 0.0;matrix_b_(5, 1) = -1.0;matrix_bd_ = matrix_b_ * ts_;matrix_c_ = Matrix::Zero(basic_state_size_, 1);matrix_c_(5, 0) = 1.0;matrix_cd_ = Matrix::Zero(basic_state_size_, 1);matrix_state_ = Matrix::Zero(basic_state_size_, 1);matrix_k_ = Matrix::Zero(1, basic_state_size_);matrix_r_ = Matrix::Identity(controls_, controls_);matrix_q_ = Matrix::Zero(basic_state_size_, basic_state_size_);int r_param_size = control_conf->mpc_controller_conf().matrix_r_size();for (int i = 0; i < r_param_size; ++i) {matrix_r_(i, i) = control_conf->mpc_controller_conf().matrix_r(i);}int q_param_size = control_conf->mpc_controller_conf().matrix_q_size();if (basic_state_size_ != q_param_size) {const auto error_msg = common::util::StrCat("MPC controller error: matrix_q size: ", q_param_size," in parameter file not equal to basic_state_size_: ",basic_state_size_);AERROR << error_msg;return Status(ErrorCode::CONTROL_COMPUTE_ERROR, error_msg);}for (int i = 0; i < q_param_size; ++i) {matrix_q_(i, i) = control_conf->mpc_controller_conf().matrix_q(i);}// Update matrix_q_updated_ and matrix_r_updated_matrix_r_updated_ = matrix_r_;matrix_q_updated_ = matrix_q_;InitializeFilters(control_conf);LoadMPCGainScheduler(control_conf->mpc_controller_conf());LogInitParameters();AINFO << "[MPCController] init done!";return Status::OK();
}
下列代码就是加载MPC增益序列，具体有以下序列：
1，横向偏差增益序列；
2，航向偏差增益序列；
3，前馈项增益序列；
4，转向加权增益序列；
void MPCController::LoadMPCGainScheduler(const MPCControllerConf &mpc_controller_conf) {const auto &lat_err_gain_scheduler =mpc_controller_conf.lat_err_gain_scheduler();const auto &heading_err_gain_scheduler =mpc_controller_conf.heading_err_gain_scheduler();const auto &feedforwardterm_gain_scheduler =mpc_controller_conf.feedforwardterm_gain_scheduler();const auto &steer_weight_gain_scheduler =mpc_controller_conf.steer_weight_gain_scheduler();AINFO << "MPC control gain scheduler loaded";Interpolation1D::DataType xy1, xy2, xy3, xy4;for (const auto &scheduler : lat_err_gain_scheduler.scheduler()) {xy1.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));}for (const auto &scheduler : heading_err_gain_scheduler.scheduler()) {xy2.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));}for (const auto &scheduler : feedforwardterm_gain_scheduler.scheduler()) {xy3.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));}for (const auto &scheduler : steer_weight_gain_scheduler.scheduler()) {xy4.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));}lat_err_interpolation_.reset(new Interpolation1D);CHECK(lat_err_interpolation_->Init(xy1))<< "Fail to load lateral error gain scheduler for MPC controller";heading_err_interpolation_.reset(new Interpolation1D);CHECK(heading_err_interpolation_->Init(xy2))<< "Fail to load heading error gain scheduler for MPC controller";feedforwardterm_interpolation_.reset(new Interpolation1D);CHECK(feedforwardterm_interpolation_->Init(xy2))<< "Fail to load feed forward term gain scheduler for MPC controller";steer_weight_interpolation_.reset(new Interpolation1D);CHECK(steer_weight_interpolation_->Init(xy2))<< "Fail to load steer weight gain scheduler for MPC controller";
}
以下代码为计算控制命令，
输入：当前车辆状态，目标轨迹
输出：方向盘转向角度，油门，制动
Status MPCController::ComputeControlCommand(const localization::LocalizationEstimate *localization,const canbus::Chassis *chassis,const planning::ADCTrajectory *planning_published_trajectory,ControlCommand *cmd) {trajectory_analyzer_ =std::move(TrajectoryAnalyzer(planning_published_trajectory));SimpleMPCDebug *debug = cmd->mutable_debug()->mutable_simple_mpc_debug();debug->Clear();计算纵向偏差，实现如下所示：
ComputeLongitudinalErrors(&trajectory_analyzer_, debug);
计算纵向偏差。
1，首先求得一个matched_point，求取的具体方法可参考纵向控制。
2，然后 求得车辆当前位置到matched_point点的横向偏差，横向车速，纵向偏差，纵向车速。
3，求得reference_point，该点是时间距离当前车辆时间点最近的规划点。
4，计算航向角偏差，即当前车辆航向角和matched_point点的航向角差值。
5，计算车辆到matched_point的纵向车速偏差和纵向加速度偏差以及到reference_point的曲率变化；
6，设置各种偏差值，后面会用到；
void MPCController::ComputeLongitudinalErrors(const TrajectoryAnalyzer *trajectory_analyzer, SimpleMPCDebug *debug) {// the decomposed vehicle motion onto Frenet frame// s: longitudinal accumulated distance along reference trajectory// s_dot: longitudinal velocity along reference trajectory// d: lateral distance w.r.t. reference trajectory// d_dot: lateral distance change rate, i.e. dd/dtdouble s_matched = 0.0;double s_dot_matched = 0.0;double d_matched = 0.0;double d_dot_matched = 0.0;const auto matched_point = trajectory_analyzer->QueryMatchedPathPoint(VehicleStateProvider::Instance()->x(),VehicleStateProvider::Instance()->y());trajectory_analyzer->ToTrajectoryFrame(VehicleStateProvider::Instance()->x(),VehicleStateProvider::Instance()->y(),VehicleStateProvider::Instance()->heading(),VehicleStateProvider::Instance()->linear_velocity(), matched_point,&s_matched, &s_dot_matched, &d_matched, &d_dot_matched);const double current_control_time = Clock::NowInSeconds();TrajectoryPoint reference_point =trajectory_analyzer->QueryNearestPointByAbsoluteTime(current_control_time);ADEBUG << "matched point:" << matched_point.DebugString();ADEBUG << "reference point:" << reference_point.DebugString();const double linear_v = VehicleStateProvider::Instance()->linear_velocity();const double linear_a =VehicleStateProvider::Instance()->linear_acceleration();double heading_error = common::math::NormalizeAngle(VehicleStateProvider::Instance()->heading() - matched_point.theta());double lon_speed = linear_v * std::cos(heading_error);double lon_acceleration = linear_a * std::cos(heading_error);double one_minus_kappa_lat_error = 1 - reference_point.path_point().kappa() *linear_v * std::sin(heading_error);debug->set_station_reference(reference_point.path_point().s());debug->set_station_feedback(s_matched);debug->set_station_error(reference_point.path_point().s() - s_matched);debug->set_speed_reference(reference_point.v());debug->set_speed_feedback(lon_speed);debug->set_speed_error(reference_point.v() - s_dot_matched);debug->set_acceleration_reference(reference_point.a());debug->set_acceleration_feedback(lon_acceleration);debug->set_acceleration_error(reference_point.a() -lon_acceleration / one_minus_kappa_lat_error);double jerk_reference =(debug->acceleration_reference() - previous_acceleration_reference_) /ts_;double lon_jerk =(debug->acceleration_feedback() - previous_acceleration_) / ts_;debug->set_jerk_reference(jerk_reference);debug->set_jerk_feedback(lon_jerk);debug->set_jerk_error(jerk_reference - lon_jerk / one_minus_kappa_lat_error);previous_acceleration_reference_ = debug->acceleration_reference();previous_acceleration_ = debug->acceleration_feedback();
}
更新状态变量；
// Update stateUpdateState(debug);
更新状态变量的具体代码如下：
首先计算车辆横向偏差，然后设置系统的状态参数如下：
1，横向偏差：当前车辆位置到距离车辆最近轨迹点（追踪点）的横向偏差。
2，横向偏差变化率：线车速与（当前车辆航向角-最近轨迹点航向角）的正弦。
3，航向角偏差：当前车辆航向角-最近轨迹点航向角。
4，航向角偏差变化率：当前车辆角速度-追踪点曲率*追踪点线车速（即追踪点的角速度）。
5，位置偏差：当前车辆到纵向偏差计算中的reference_point走过的距离-当前车辆到纵向matched_point的纵向距离。
6，速度偏差：reference_point的车速-纵向matched_point中的纵向车速。
void MPCController::UpdateState(SimpleMPCDebug *debug) {const auto &com = VehicleStateProvider::Instance()->ComputeCOMPosition(lr_);ComputeLateralErrors(com.x(), com.y(),VehicleStateProvider::Instance()->heading(),VehicleStateProvider::Instance()->linear_velocity(),VehicleStateProvider::Instance()->angular_velocity(),VehicleStateProvider::Instance()->linear_acceleration(),trajectory_analyzer_, debug);// State matrix update;matrix_state_(0, 0) = debug->lateral_error();matrix_state_(1, 0) = debug->lateral_error_rate();matrix_state_(2, 0) = debug->heading_error();matrix_state_(3, 0) = debug->heading_error_rate();matrix_state_(4, 0) = debug->station_error();matrix_state_(5, 0) = debug->speed_error();
}
横向偏差计算的具体实现如下：
1，首先计算距离车辆最近的轨迹点matched_point，这个点和纵向控制中的matched_point不一样；
2，计算车辆当前位置到横向matched_point的横向偏差，航向角偏差，横向车速偏差，横向加速度偏差。
void MPCController::ComputeLateralErrors(const double x, const double y, const double theta, const double linear_v,const double angular_v, const double linear_a,const TrajectoryAnalyzer &trajectory_analyzer, SimpleMPCDebug *debug) {const auto matched_point =trajectory_analyzer.QueryNearestPointByPosition(x, y);const double dx = x - matched_point.path_point().x();const double dy = y - matched_point.path_point().y();const double cos_matched_theta = std::cos(matched_point.path_point().theta());const double sin_matched_theta = std::sin(matched_point.path_point().theta());// d_error = cos_matched_theta * dy - sin_matched_theta * dx;debug->set_lateral_error(cos_matched_theta * dy - sin_matched_theta * dx);// matched_theta = matched_point.path_point().theta();debug->set_ref_heading(matched_point.path_point().theta());const double delta_theta =common::math::NormalizeAngle(theta - debug->ref_heading());debug->set_heading_error(delta_theta);const double sin_delta_theta = std::sin(delta_theta);// d_error_dot = chassis_v * sin_delta_theta;double lateral_error_dot = linear_v * sin_delta_theta;double lateral_error_dot_dot = linear_a * sin_delta_theta;if (FLAGS_reverse_heading_control) {if (VehicleStateProvider::Instance()->gear() ==canbus::Chassis::GEAR_REVERSE) {lateral_error_dot = -lateral_error_dot;lateral_error_dot_dot = -lateral_error_dot_dot;}}debug->set_lateral_error_rate(lateral_error_dot);debug->set_lateral_acceleration(lateral_error_dot_dot);debug->set_lateral_jerk((debug->lateral_acceleration() - previous_lateral_acceleration_) / ts_);previous_lateral_acceleration_ = debug->lateral_acceleration();// matched_kappa = matched_point.path_point().kappa();debug->set_curvature(matched_point.path_point().kappa());// theta_error = delta_theta;debug->set_heading_error(delta_theta);// theta_error_dot = angular_v - matched_point.path_point().kappa() *// matched_point.v();debug->set_heading_rate(angular_v);debug->set_ref_heading_rate(debug->curvature() * matched_point.v());debug->set_heading_error_rate(debug->heading_rate() -debug->ref_heading_rate());debug->set_heading_acceleration((debug->heading_rate() - previous_heading_rate_) / ts_);debug->set_ref_heading_acceleration((debug->ref_heading_rate() - previous_ref_heading_rate_) / ts_);debug->set_heading_error_acceleration(debug->heading_acceleration() -debug->ref_heading_acceleration());previous_heading_rate_ = debug->heading_rate();previous_ref_heading_rate_ = debug->ref_heading_rate();debug->set_heading_jerk((debug->heading_acceleration() - previous_heading_acceleration_) / ts_);debug->set_ref_heading_jerk((debug->ref_heading_acceleration() - previous_ref_heading_acceleration_) /ts_);debug->set_heading_error_jerk(debug->heading_jerk() -debug->ref_heading_jerk());previous_heading_acceleration_ = debug->heading_acceleration();previous_ref_heading_acceleration_ = debug->ref_heading_acceleration();
}
更新系数矩阵
UpdateMatrix(debug);
更新系数矩阵的具体代码实现如下：
更新矩阵A，并采用双线性变换离散法进行离散化处理；
更新矩阵C，并将其离散化处理；
void MPCController::UpdateMatrix(SimpleMPCDebug *debug) {const double v = std::max(VehicleStateProvider::Instance()->linear_velocity(),minimum_speed_protection_);matrix_a_(1, 1) = matrix_a_coeff_(1, 1) / v;matrix_a_(1, 3) = matrix_a_coeff_(1, 3) / v;matrix_a_(3, 1) = matrix_a_coeff_(3, 1) / v;matrix_a_(3, 3) = matrix_a_coeff_(3, 3) / v;Matrix matrix_i = Matrix::Identity(matrix_a_.cols(), matrix_a_.cols());matrix_ad_ = (matrix_i - ts_ * 0.5 * matrix_a_).inverse() *(matrix_i + ts_ * 0.5 * matrix_a_);matrix_c_(1, 0) = (lr_ * cr_ - lf_ * cf_) / mass_ / v - v;matrix_c_(3, 0) = -(lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / iz_ / v;matrix_cd_ = matrix_c_ * debug->heading_error_rate() * ts_;
}
计算前馈控制量,
FeedforwardUpdate(debug);
具体代码实现如下:首先计算前轮转角，然后将前轮转角转换成方向盘转角转动百分比。
void MPCController::FeedforwardUpdate(SimpleMPCDebug *debug) {const double v = VehicleStateProvider::Instance()->linear_velocity();const double kv =lr_ * mass_ / 2 / cf_ / wheelbase_ - lf_ * mass_ / 2 / cr_ / wheelbase_;steer_angle_feedforwardterm_ = Wheel2SteerPct(wheelbase_ * debug->curvature() + kv * v * v * debug->curvature());
}
如果需要增加对于高速转向的增益序列，则需要将Q，R矩阵的相应参数乘以车速。
并更新Q，R矩阵。
// Add gain scheduler for higher speed steeringif (FLAGS_enable_gain_scheduler) {matrix_q_updated_(0, 0) =matrix_q_(0, 0) *lat_err_interpolation_->Interpolate(VehicleStateProvider::Instance()->linear_velocity());matrix_q_updated_(2, 2) =matrix_q_(2, 2) *heading_err_interpolation_->Interpolate(VehicleStateProvider::Instance()->linear_velocity());steer_angle_feedforwardterm_updated_ =steer_angle_feedforwardterm_ *feedforwardterm_interpolation_->Interpolate(VehicleStateProvider::Instance()->linear_velocity());matrix_r_updated_(0, 0) =matrix_r_(0, 0) *steer_weight_interpolation_->Interpolate(VehicleStateProvider::Instance()->linear_velocity());} else {matrix_q_updated_ = matrix_q_;matrix_r_updated_ = matrix_r_;steer_angle_feedforwardterm_updated_ = steer_angle_feedforwardterm_;}debug->add_matrix_q_updated(matrix_q_updated_(0, 0));debug->add_matrix_q_updated(matrix_q_updated_(1, 1));debug->add_matrix_q_updated(matrix_q_updated_(2, 2));debug->add_matrix_q_updated(matrix_q_updated_(3, 3));debug->add_matrix_r_updated(matrix_r_updated_(0, 0));debug->add_matrix_r_updated(matrix_r_updated_(1, 1));
设置以下矩阵：
预测时序，参考时序等。
Matrix control_matrix = Matrix::Zero(controls_, 1);std::vector<Matrix> control(horizon_, control_matrix);Matrix control_gain_matrix = Matrix::Zero(controls_, basic_state_size_);std::vector<Matrix> control_gain(horizon_, control_gain_matrix);Matrix addition_gain_matrix = Matrix::Zero(controls_, 1);std::vector<Matrix> addition_gain(horizon_, addition_gain_matrix);Matrix reference_state = Matrix::Zero(basic_state_size_, 1);std::vector<Matrix> reference(horizon_, reference_state);Matrix lower_bound(controls_, 1);lower_bound << -wheel_single_direction_max_degree_, max_deceleration_;Matrix upper_bound(controls_, 1);upper_bound << wheel_single_direction_max_degree_, max_acceleration_;之后为解MPC控制方程。
MPC控制器解法和论文《Model Predictive Control of a Mobile Robot Using Linearization》(Walter Fetter Lages etc.)一致。在这里就不展开说明了，后续会详细分析。
double mpc_start_timestamp = Clock::NowInSeconds();double steer_angle_feedback = 0.0;double acc_feedback = 0.0;double steer_angle_ff_compensation = 0.0;double unconstraint_control_diff = 0.0;double control_gain_truncation_ratio = 0.0;double unconstraint_control = 0.0;const double v = VehicleStateProvider::Instance()->linear_velocity();if (!common::math::SolveLinearMPC(matrix_ad_, matrix_bd_, matrix_cd_,matrix_q_updated_, matrix_r_updated_,lower_bound, upper_bound, matrix_state_,reference, mpc_eps_, mpc_max_iteration_,&control, &control_gain, &addition_gain)) {AERROR << "MPC solver failed";}
MPC算法求得两个最优值：车轮转角，加速度。
之将车轮转角转换成方向盘转角。
求出控制量的约束值。
else {ADEBUG << "MPC problem solved! ";steer_angle_feedback = Wheel2SteerPct(control[0](0, 0));acc_feedback = control[0](1, 0);for (int i = 0; i < basic_state_size_; ++i) {unconstraint_control += control_gain[0](0, i) * matrix_state_(i, 0);}unconstraint_control += addition_gain[0](0, 0) * v * debug->curvature();
如果允许对MPC控制进行前馈补偿，则不受约束的控制量差值=正常控制量-约束控制量，如果该差值小于差值限制值，则计算方向盘转角补偿量。否则补偿量等于0。
if (enable_mpc_feedforward_compensation_) {unconstraint_control_diff =Wheel2SteerPct(control[0](0, 0) - unconstraint_control);if (fabs(unconstraint_control_diff) <= unconstraint_control_diff_limit_) {steer_angle_ff_compensation =Wheel2SteerPct(debug->curvature() *(control_gain[0](0, 2) *(lr_ - lf_ / cr_ * mass_ * v * v / wheelbase_) -addition_gain[0](0, 0) * v));} else {control_gain_truncation_ratio = control[0](0, 0) / unconstraint_control;steer_angle_ff_compensation =Wheel2SteerPct(debug->curvature() *(control_gain[0](0, 2) *(lr_ - lf_ / cr_ * mass_ * v * v / wheelbase_) -addition_gain[0](0, 0) * v) *control_gain_truncation_ratio);}} else {steer_angle_ff_compensation = 0.0;}}double mpc_end_timestamp = Clock::NowInSeconds();ADEBUG << "MPC core algorithm: calculation time is: "<< (mpc_end_timestamp - mpc_start_timestamp) * 1000 << " ms.";
方向盘转角=MPC计算的方向盘转角+大车速下的方向盘转角增加量+方向盘转角补偿量。
// TODO(QiL): evaluate whether need to add spline smoothing after the resultdouble steer_angle = steer_angle_feedback +steer_angle_feedforwardterm_updated_ +steer_angle_ff_compensation;
如果允许设置方向盘转角限制值，则限制值为最大纵向加速度*轴距/速度平方转换成方向盘转角。
之后对其进行滤波处理。
if (FLAGS_set_steer_limit) {const double steer_limit =std::atan(max_lat_acc_ * wheelbase_ /(VehicleStateProvider::Instance()->linear_velocity() *VehicleStateProvider::Instance()->linear_velocity())) *steer_ratio_ * 180 / M_PI / steer_single_direction_max_degree_ * 100;// Clamp the steer angle with steer limitations at current speeddouble steer_angle_limited =common::math::Clamp(steer_angle, -steer_limit, steer_limit);steer_angle_limited = digital_filter_.Filter(steer_angle_limited);steer_angle = steer_angle_limited;debug->set_steer_angle_limited(steer_angle_limited);}steer_angle = digital_filter_.Filter(steer_angle);// Clamp the steer angle to -100.0 to 100.0steer_angle = common::math::Clamp(steer_angle, -100.0, 100.0);
设置方向盘转动角度。
cmd->set_steering_target(steer_angle);
加速命令=MPC求出的加速反馈+reference_point点的加速度；
debug->set_acceleration_cmd_closeloop(acc_feedback);double acceleration_cmd = acc_feedback + debug->acceleration_reference();// TODO(QiL): add pitch angle feed forward to accommodate for 3D control
如果满足停车条件，则加速度为最大停车加速度。
if ((planning_published_trajectory->trajectory_type() ==apollo::planning::ADCTrajectory::NORMAL) &&(std::fabs(debug->acceleration_reference()) <=max_acceleration_when_stopped_ &&std::fabs(debug->speed_reference()) <= max_abs_speed_when_stopped_)) {acceleration_cmd =(chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)? std::max(acceleration_cmd, -standstill_acceleration_): std::min(acceleration_cmd, standstill_acceleration_);ADEBUG << "Stop location reached";debug->set_is_full_stop(true);}// TODO(Yu): study the necessity of path_remain and add it to MPC if needed
设置加速度命令。
debug->set_acceleration_cmd(acceleration_cmd);
根据加速度命令，查找标定表得到标定值。
double calibration_value = 0.0;if (FLAGS_use_preview_speed_for_table) {calibration_value = control_interpolation_->Interpolate(std::make_pair(debug->speed_reference(), acceleration_cmd));} else {calibration_value = control_interpolation_->Interpolate(std::make_pair(VehicleStateProvider::Instance()->linear_velocity(), acceleration_cmd));}
设置标定值。
根据标定值分别计算油门/刹车值。
debug->set_calibration_value(calibration_value);double throttle_cmd = 0.0;double brake_cmd = 0.0;if (calibration_value >= 0) {throttle_cmd = std::max(calibration_value, throttle_lowerbound_);brake_cmd = 0.0;} else {throttle_cmd = 0.0;brake_cmd = std::max(-calibration_value, brake_lowerbound_);}cmd->set_steering_rate(FLAGS_steer_angle_rate);// if the car is driven by acceleration, disgard the cmd->throttle and brakecmd->set_throttle(throttle_cmd);cmd->set_brake(brake_cmd);cmd->set_acceleration(acceleration_cmd);debug->set_heading(VehicleStateProvider::Instance()->heading());debug->set_steering_position(chassis->steering_percentage());debug->set_steer_angle(steer_angle);debug->set_steer_angle_feedforward(steer_angle_feedforwardterm_updated_);debug->set_steer_angle_feedforward_compensation(steer_angle_ff_compensation);debug->set_steer_unconstraint_control_diff(unconstraint_control_diff);debug->set_steer_angle_feedback(steer_angle_feedback);debug->set_steering_position(chassis->steering_percentage());if (std::fabs(VehicleStateProvider::Instance()->linear_velocity()) <=vehicle_param_.max_abs_speed_when_stopped() ||chassis->gear_location() == planning_published_trajectory->gear() ||chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {cmd->set_gear_location(planning_published_trajectory->gear());} else {cmd->set_gear_location(chassis->gear_location());}ProcessLogs(debug, chassis);return Status::OK();
}
百度Apollo5.0控制模块代码学习（七）MPC控制相关推荐
- 百度Apollo5.0控制模块代码学习（八）MPC控制
1 序言 对MPC控制算法,参照论文<Model Predictive Control of a Mobile Robot Using Linearization>进行整理.其中很多内容还 ...
- Apollo代码学习(七)—MPC与LQR比较
Apollo代码学习-MPC与LQR比较 前言 研究对象 状态方程 工作时域 目标函数 求解方法 前言 Apollo中用到了PID.MPC和LQR三种控制器,其中,MPC和LQR控制器在状态方程的形式 ...
- 搭建百度unit2.0测试代码（Java）
1.主类:App.java package haidong.haidong;import java.io.BufferedReader; import java.io.InputStreamReade ...
- Python学习(七) 流程控制if语句
在Python中流程控制if语句采用如下格式: if expression : statement elif expression : statement elif expression : stat ...
- Apollo代码学习(五)—横纵向控制
Apollo代码学习-横纵向控制 前言 纵向控制 横向控制 前馈控制 注意 反馈控制 总结 补充 2018.11.28 前言 在我的第一篇博文:Apollo代码学习(一)-控制模块概述中,对横纵向控制 ...
- Apollo代码学习(六)—模型预测控制(MPC)
Apollo代码学习-模型预测控制 前言 模型预测控制 预测模型 线性化 单车模型 滚动优化 反馈矫正 总结 前言 非专业选手,此篇博文内容基于书本和网络资源整理,可能理解的较为狭隘,起点较低,就事论 ...
- （转）MyBatis框架的学习(七)——MyBatis逆向工程自动生成代码
http://blog.csdn.net/yerenyuan_pku/article/details/71909325 什么是逆向工程 MyBatis的一个主要的特点就是需要程序员自己编写sql,那么 ...
- Apollo代码学习(六)—模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码
Apollo代码学习(六)-模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码
- VTM10.0代码学习5：coding_unit()cu_pred_data()
此系列是为了记录自己学习VTM10.0的过程和锻炼表达能力,主要是从解码端进行入手.由于本人水平有限,出现的错误恳请大家指正,欢迎与大家一起交流进步. 上一篇博客(VTM10.0代码学习4)讲述了将语 ...
- VTM10.0代码学习10：EncGOP_compressGOP()
此系列是为了记录自己学习VTM10.0的过程,目前正在看编码端.主要的参考文档有JVET-S2001-vH和JVET-S2002-v1.由于本人水平有限,出现的错误恳请大家指正,欢迎与大家一起交流进步 ...
最新文章
- 收到猎头信息的一些感想
- DevExpress ASP.NET 使用经验谈（9）-Dev控件客户端事件 ClientSideEvents
- 再过五分钟，你就懂Oracle的数据库对象了
- Linux配置sudo
- python读取多个文件夹_如何从python中的文件夹中读取多个NetCDF文件
- 深入理解Spring Redis的使用 (九)、通过Redis 实现 分布式锁 的 BUG，以及和数据库加锁的性能测试...
- 搞不明白为什么大家都在学习 k8s
- 双向板受力特点_弹性减震球形钢支座/双向弹簧铰支座特性
- 诱人的 TypeScript 视频教程（69 个视频）
- matlab 图像 变成二维,matlab三维图像变二维
- HDU - 1728 逃离迷宫 【BFS】
- ios实现类似魔兽小地图功能 在
- 第四讲 身份认证技术
- pyqt5显示日期与时间
- 智慧高速再“提速”，数字化+智能化成建设焦点
- linux otg u盘,如何使用OTG手机功能U盘？
- linux服务器开机管理,Linux服务器开机自动启动服务或脚本的方法
- 谷歌地球 GoogleEarth软件介绍
- CSAPP实验记录（2）--------- Bomb
- MySQL分区总结_mysql分区表小结2