# 纵向控制

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

• 速度误差( s p e e d _ e r r o r speed\_error )
• 位置误差( s t a t i o n _ e r r o r 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);


(1) s t a t i o n _ e r r o r = − ( d x ∗ cos ⁡ θ d e s + d y ∗ sin ⁡ θ d e s ) 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);


(2) s p e e d _ e r r o r = V d e s − V ∗ cos ⁡ Δ θ / k speed\_error=V_{des} - V*\cos{\Delta\theta}/k \tag{2}

Apollo所用的PID控制器就是位置型PID控制器：
(3) u ( k ) = K P e ( k ) + K I ∑ i = 1 k e ( i ) + K D [ e ( k ) − e ( k − 1 ) ] 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_;


# 横向控制

## 前馈控制

(4) x ˙ = A x + B u \dot{x}=Ax+Bu\tag{4}

(5) x ˙ = A x + B δ + B 2 φ ˙ d e s \dot{x}=Ax+B\delta+B_2\dot{\varphi}_{des} \tag{5}

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


(6) δ f f = L R + K V a y − k 3 [ ℓ r R − ℓ f 2 C α r m V x 2 R ℓ ] \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;


## 反馈控制

(4) x ˙ = A x + B u \dot{x}=Ax+Bu\tag{4}

• 横向误差 l a t e r a l _ e r r o r lateral\_error
• 横向误差率 l a t e r a l _ e r r o r _ r a t e lateral\_error\_rate
• 航向误差 h e a d i n g _ e r r o r heading\_error
• 航向误差率 h e a d i n g _ e r r o r _ r a t e 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();
}



(7) m a t r i x _ a _ = [ 0 1 0 0 0 − C f + C r m V C f + C r m ℓ r C r − ℓ f C f m V 0 0 0 1 0 ℓ r C r − ℓ f C f I z V ℓ f C f − ℓ r C r I z ℓ r 2 C r − ℓ f 2 C f I z V ] matrix\_a\_ = \begin{bmatrix} 0 &amp; 1 &amp; 0 &amp; 0 \\ 0 &amp; -\frac{C_f+C_r}{mV} &amp; \frac{C_f+C_r}{m} &amp; \frac{\ell_rC_r-\ell_fC_f}{mV} \\ 0 &amp; 0 &amp; 0 &amp; 1 \\ 0 &amp; \frac{\ell_rC_r-\ell_fC_f}{I_zV} &amp; \frac{\ell_fC_f-\ell_rC_r}{I_z} &amp; \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_;


(8) m a t r i x _ b _ = [ 0 c f m 0 ℓ f C f I z ] matrix\_b\_= \begin{bmatrix} 0 \\ \frac{c_f}{m} \\ 0 \\ \frac{\ell_fC_f}{I_z} \end{bmatrix} \tag{8}

(9) J = 1 2 ∫ 0 ∞ ( x T Q x + u T R u ) d t J=\frac{1}{2}\int_0^\infty (x^TQx+u^TRu)dt \tag{9}

1. 选择参数矩阵 Q Q R R
2. 求解Riccati 方程得到矩阵 P P
3. 计算 K = R − 1 B T P 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

(10) { x ′ = O A + B C = x cos ⁡ θ + y sin ⁡ θ y ′ = P C − A B = y cos ⁡ θ − x sin ⁡ θ \begin{cases} x&#x27;=OA+BC=x\cos{\theta}+y\sin{\theta}\\ y&#x27;=PC-AB=y\cos{\theta}-x\sin{\theta} \end{cases}\tag{10}

d x = x − x d e s , d y = y − y d e s 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);


d i s t a n c e = d x 2 + d y 2 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);


(11) s t a t i o n _ e r r o r = − ( d x ∗ cos ⁡ θ d e s + d y ∗ sin ⁡ θ d e s ) 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);


(12) l a t e r a l _ e r r o r = d y ∗ cos ⁡ θ d e s − d x ∗ sin ⁡ θ d e s lateral\_error=dy*\cos{\theta_{des}}-dx*\sin{\theta_{des}} \tag{12}

(13) d i s t a n c e = d x 2 + d y 2 = s t a t i o n _ e r r o r 2 + l a t e r a l _ e r r o r 2 distance=\sqrt{dx^2+dy^2}=\sqrt{station\_error^2+lateral\_error^2}\tag{13}

07-31 2532

09-19 3万+
07-23 5790
05-25 1万+
05-07 5072
07-01 5528
07-30 3551
04-25 1万+
12-11 1万+
01-31 1467
10-26 2459