百度Apollo5.0控制模块代码学习（五）横向控制

1 横向控制流程图

在之前我分析了横向控制的相关代码，在此进行总结，并对其中的一些疑问进行解释。
首先根据对横向控制的代码进行分析，整理了如下的控制流程图：

2 跟踪点确定

和纵向控制中一样，我们可以用下图表示代码中涉及的点
点1：车辆目前的位置；
点2：距离车辆最近的点；
点5：前视点（0.8s之后的点）；

从以下代码中可以看出，系统的状态向量为matrix_state_。

  const double steer_angle_feedback = -(matrix_k_ * matrix_state_)(0, 0) * 180 /
                                      M_PI * steer_ratio_ /
                                      steer_single_direction_max_degree_ * 100;

而matrix_state_如下：

  if (control_conf_->lat_controller_conf().enable_look_ahead_back_control() &&
      std::fabs(vehicle_state->linear_velocity()) <= low_speed_bound_) {
    matrix_state_(0, 0) = debug->lateral_error_feedback();
    matrix_state_(2, 0) = debug->heading_error_feedback();
  } else {
    matrix_state_(0, 0) = debug->lateral_error();
    matrix_state_(2, 0) = debug->heading_error();
  }
  matrix_state_(1, 0) = debug->lateral_error_rate();
  matrix_state_(3, 0) = debug->heading_error_rate();

以上几个值分别在下述代码中找到：

 734行： debug->set_lateral_error_feedback(lateral_error_feedback);
 721行：debug->set_heading_error_feedback(heading_error_feedback);
 696行：debug->set_lateral_error(lateral_error);
 704行：  debug->set_heading_error(heading_error);
 745行：debug->set_lateral_error_rate(lateral_error_dot);
 759行：debug->set_heading_error_rate(debug->heading_rate() -
                                debug->ref_heading_rate());

1，首先看：lateral_error：

double lateral_error = cos_target_heading * dy - sin_target_heading * dx;

  const double dx = x - target_point.path_point().x();
  const double dy = y - target_point.path_point().y();

因此可以知道，横向偏差求取的关键点为target_point，target_point求取具体实现为如下代码。
如果只允许查询时间最近规划点，或允许导航模式但不允许导航模式下位置更新，则target_point为距离当前车辆时间0.8s之后时间最接近的规划点，即点3，否则，target_point为距离当前车辆距离最近的规划点，即点2。
注：本工程中target_point为物理位置距离当前车辆最近的规划点，即点2。

  if (FLAGS_query_time_nearest_point_only) {
    target_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(
        Clock::NowInSeconds() + query_relative_time_);
  } else {
    if (FLAGS_use_navigation_mode &&
        !FLAGS_enable_navigation_mode_position_update) {
      target_point = trajectory_analyzer.QueryNearestPointByAbsoluteTime(
          Clock::NowInSeconds() + query_relative_time_);
    } else {
      target_point = trajectory_analyzer.QueryNearestPointByPosition(x, y);
    }
  }

2，看heading_error：
heading_error的求取过程如下代码，由此可知，求取的关键点同样为target_point，在本工程中，同样为点2。

 debug->set_ref_heading(target_point.path_point().theta());
  double heading_error =
      common::math::NormalizeAngle(theta - debug->ref_heading());

因此，本工程中的跟踪点为点2，即距离当前车辆位置最近的规划点，这一点与纵向控制中的跟踪点一致（纵向控制中跟踪点为时间距离当前车辆时间最近的规划点，预瞄点为距离最近点和下一点之间的插值法得到。）。

3 横向控制原理图

通过之前的整理，可以画出如下的横向控制原理图：