# 纵向控制

Apollo纵向控制的工作原理如图1所示。它主要由“位移-速度闭环PID控制器”、“速度-加速度闭环PID控制器”和“速度-加速度-刹车/油门”开环控制器构成。

• 速度误差($speed\_error$)
• 位置误差($station\_error$)

void TrajectoryAnalyzer::ToTrajectoryFrame(const double x, const double y,
const double theta, const double v,
const PathPoint &ref_point,
double *ptr_s, double *ptr_s_dot,
double *ptr_d,
double *ptr_d_dot)


// reference: Optimal trajectory generation for dynamic street scenarios in a
// Moritz Werling, Julius Ziegler, SÃ¶ren Kammel and Sebastian Thrun, ICRA 2010
// similar to the method in this paper without the assumption the "normal"
// vector
// (from vehicle position to ref_point position) and reference heading are
// perpendicular.


// trajectory_analyzer.cc
double dx = x - ref_point.x();
double dy = y - ref_point.y();

double dot_rd_nd = dx * cos_ref_theta + dy * sin_ref_theta;
*ptr_s = ref_point.s() + dot_rd_nd;

// lon_controller.cc
// s_matched即上面的ptr_s
debug->set_station_error(reference_point.path_point().s() - s_matched);


$station\_error=-(dx*\cos{\theta_{des}} + dy*\sin{\theta_{des}}) \tag{1}$

// trajectory_analyzer.cc
double cross_rd_nd = cos_ref_theta * dy - sin_ref_theta * dx;
*ptr_d = cross_rd_nd;

double delta_theta = theta - ref_point.theta();
double cos_delta_theta = std::cos(delta_theta);
double sin_delta_theta = std::sin(delta_theta);

double one_minus_kappa_r_d = 1 - ref_point.kappa() * (*ptr_d);
if (one_minus_kappa_r_d <= 0.0) {
AERROR << "TrajectoryAnalyzer::ToTrajectoryFrame "
"found fatal reference and actual difference. "
"Control output might be unstable:"
<< " ref_point.kappa:" << ref_point.kappa()
<< " ref_point.x:" << ref_point.x()
<< " ref_point.y:" << ref_point.y() << " car x:" << x
<< " car y:" << y << " *ptr_d:" << *ptr_d
<< " one_minus_kappa_r_d:" << one_minus_kappa_r_d;
// currently set to a small value to avoid control crash.
one_minus_kappa_r_d = 0.01;
}

*ptr_s_dot = v * cos_delta_theta / one_minus_kappa_r_d;

// lon_controller.cc
// s_dot_matched即上面的ptr_s_dot
debug->set_speed_error(reference_point.v() - s_dot_matched);


$speed\_error=V_{des} - V*\cos{\Delta\theta}/k \tag{2}$

Apollo所用的PID控制器就是位置型PID控制器：
$u(k)=K_Pe(k)+K_I\sum_{i=1}^k e(i)+K_D[e(k)-e(k-1)] \tag{3}$

// 微分项
diff = (error - previous_error_) / dt;
// 积分项
integral_ += error * dt * ki_;
// PID控制器输出
output = error * kp_ + integral_ + diff * kd_;


# 横向控制

## 前馈控制

$\dot{x}=Ax+Bu\tag{4}$

$\dot{x}=Ax+B\delta+B_2\dot{\varphi}_{des} \tag{5}$

const double steer_angle_feedforward = ComputeFeedForward(debug->curvature());


$\delta_{ff}=\frac{L}{R}+K_Va_y-k_3[\frac{\ell_r}{R}-\frac{\ell_f}{2C_{\alpha r}}\frac{m{V_x}^2}{R\ell}] \tag{6}$

const double steer_angle_feedforwardterm =
(wheelbase_ * ref_curvature + kv * v * v * ref_curvature -
matrix_k_(0, 2) *
(lr_ * ref_curvature -
lf_ * mass_ * v * v * ref_curvature / 2 / cr_ / wheelbase_)) *
180 / M_PI * steer_transmission_ratio_ /
steer_single_direction_max_degree_ * 100;


## 反馈控制

$\dot{x}=Ax+Bu\tag{4}$

• 横向误差 $lateral\_error$
• 横向误差率 $lateral\_error\_rate$
• 航向误差 $heading\_error$
• 航向误差率 $heading\_error\_rate$

  matrix_state_ = Matrix::Zero(matrix_size, 1);

matrix_state_(0, 0) = debug->lateral_error();
matrix_state_(1, 0) = debug->lateral_error_rate();


  matrix_a_ = 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_coeff_ = Matrix::Zero(matrix_size, matrix_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_a_coeff_用于更新状态矩阵matrix_a_

void LatController::UpdateMatrix() {
const double v =
std::max(VehicleStateProvider::instance()->linear_velocity(), 0.2);
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_) *
(matrix_i - ts_ * 0.5 * matrix_a_).inverse();
}



$matrix\_a\_ = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{C_f+C_r}{mV} & \frac{C_f+C_r}{m} & \frac{\ell_rC_r-\ell_fC_f}{mV} \\ 0 & 0 & 0 & 1 \\ 0 & \frac{\ell_rC_r-\ell_fC_f}{I_zV} & \frac{\ell_fC_f-\ell_rC_r}{I_z} & \frac{\ell_r^2C_r-\ell_f^2C_f}{IzV} \end{bmatrix} \tag{7}$

  matrix_b_ = Matrix::Zero(basic_state_size_, 1);
matrix_bd_ = Matrix::Zero(basic_state_size_, 1);
matrix_bdc_ = Matrix::Zero(matrix_size, 1);
matrix_b_(1, 0) = cf_ / mass_;
matrix_b_(3, 0) = lf_ * cf_ / iz_;
matrix_bd_ = matrix_b_ * ts_;

matrix_bdc_.block(0, 0, basic_state_size_, 1) = matrix_bd_;


$matrix\_b\_= \begin{bmatrix} 0 \\ \frac{c_f}{m} \\ 0 \\ \frac{\ell_fC_f}{I_z} \end{bmatrix} \tag{8}$

$J=\frac{1}{2}\int_0^\infty (x^TQx+u^TRu)dt \tag{9}$

1. 选择参数矩阵$Q$$R$
2. 求解Riccati 方程得到矩阵$P$
3. 计算$K=R^{-1}B^TP$

LQR的求解过程可参看LQR，感兴趣的可以研究一下，matlab、python等也提供的都有用于求解LQR模型的函数。

Apollo中对Q、R矩阵的定义如下：

  lqr_eps_ = control_conf->lat_controller_conf().eps();
lqr_max_iteration_ = control_conf->lat_controller_conf().max_iteration();

query_relative_time_ = control_conf->query_relative_time();

matrix_k_ = Matrix::Zero(1, matrix_size);
matrix_r_ = Matrix::Identity(1, 1);
matrix_q_ = Matrix::Zero(matrix_size, matrix_size);

int q_param_size = control_conf->lat_controller_conf().matrix_q_size();
for (int i = 0; i < q_param_size; ++i) {
matrix_q_(i, i) = control_conf->lat_controller_conf().matrix_q(i);
}

matrix_q_updated_ = matrix_q_;


  if (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) *
VehicleStateProvider::instance()->linear_velocity());
matrix_r_, lqr_eps_, lqr_max_iteration_,
&matrix_k_);
} else {
matrix_r_, lqr_eps_, lqr_max_iteration_,
&matrix_k_);
}


  // feedback = - K * state
// Convert vehicle steer angle from rad to degree and then to steer degree
// then to 100% ratio
const double steer_angle_feedback = -(matrix_k_ * matrix_state_)(0, 0) * 180 /
M_PI * steer_transmission_ratio_ /
steer_single_direction_max_degree_ * 100;

const double steer_angle_feedforward = ComputeFeedForward(debug->curvature());

// Clamp the steer angle to -100.0 to 100.0
double steer_angle = common::math::Clamp(
steer_angle_feedback + steer_angle_feedforward, -100.0, 100.0);


# 补充 2018.11.28

$\begin{cases} x'=OA+BC=x\cos{\theta}+y\sin{\theta}\\ y'=PC-AB=y\cos{\theta}-x\sin{\theta} \end{cases}\tag{10}$

$dx=x-x_{des},dy=y-y_{des}$

  // 纵向控制
double dx = x - ref_point.x();
double dy = y - ref_point.y();

double cos_ref_theta = std::cos(ref_point.theta());
double sin_ref_theta = std::sin(ref_point.theta());
// 横向控制
const double dx = x - target_point.path_point().x();
const double dy = y - target_point.path_point().y();

const double cos_matched_theta = std::cos(target_point.path_point().theta());
const double sin_matched_theta = std::sin(target_point.path_point().theta());


  TrajectoryPoint target_point;
// 绝对时间
target_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(
current_timestamp + query_relative_time_);
// 相对位置
target_point = trajectory_analyzer.QueryNearestPointByPosition(x, y);


$distance=\sqrt{dx^2+dy^2}$

  // the sin of diff angle between vector (cos_ref_theta, sin_ref_theta) and
// (dx, dy)
double cross_rd_nd = cos_ref_theta * dy - sin_ref_theta * dx;
*ptr_d = cross_rd_nd;

// the cos of diff angle between vector (cos_ref_theta, sin_ref_theta) and
// (dx, dy)
double dot_rd_nd = dx * cos_ref_theta + dy * sin_ref_theta;
*ptr_s = ref_point.s() + dot_rd_nd;

double delta_theta = theta - ref_point.theta();
double cos_delta_theta = std::cos(delta_theta);
double sin_delta_theta = std::sin(delta_theta);

*ptr_d_dot = v * sin_delta_theta;

double one_minus_kappa_r_d = 1 - ref_point.kappa() * (*ptr_d);
if (one_minus_kappa_r_d <= 0.0) {
AERROR << "TrajectoryAnalyzer::ToTrajectoryFrame "
"found fatal reference and actual difference. "
"Control output might be unstable:"
<< " ref_point.kappa:" << ref_point.kappa()
<< " ref_point.x:" << ref_point.x()
<< " ref_point.y:" << ref_point.y() << " car x:" << x
<< " car y:" << y << " *ptr_d:" << *ptr_d
<< " one_minus_kappa_r_d:" << one_minus_kappa_r_d;
// currently set to a small value to avoid control crash.
one_minus_kappa_r_d = 0.01;
}

*ptr_s_dot = v * cos_delta_theta / one_minus_kappa_r_d;

// 位置误差, 其中s_matched = ptr_s
debug->set_station_error(reference_point.path_point().s() - s_matched);
// 速度误差, 其中s_dot_matched = ptr_s_dot
debug->set_speed_error(reference_point.v() - s_dot_matched);


$station\_error=-(dx*\cos{\theta_{des}+dy*\sin{\theta_{des}}}) \tag{11}$

  const double cos_target_heading = std::cos(target_point.path_point().theta());

lateral_error = lateral_error_filter_.Update(lateral_error);
}

debug->set_lateral_error(lateral_error);


$lateral\_error=dy*\cos{\theta_{des}}-dx*\sin{\theta_{des}} \tag{12}$

$distance=\sqrt{dx^2+dy^2}=\sqrt{station\_error^2+lateral\_error^2}\tag{13}$

07-26 1686

07-30 1273

11-20 2万+

07-31 1057

05-07 2910

09-19 1万+

07-30 1914

10-11 2万+

01-04 8980

11-02 2万+

06-09 2441

11-29 1万+

10-23 8430

07-23 2791

12-17 1万+

01-23 1万+

07-25 2392

12-05 1589

03-25 4万+

05-25 9万+

04-30 3万+

04-09 3万+

08-02 900

01-06 960

03-01 2万+

10-26 1525

08-27 8536

03-23 2万+

#### 曾经废寝忘食学到的技术，现在都没用了......

©️2019 CSDN 皮肤主题: 游动-白 设计师: 上身试试