Apollo control 模块源码解析

Apollo control模块接收车辆的底盘(chassis),定位(localization),行驶轨迹(planning)信息,输出车辆的控制信息(SteeringWheelAngle,acceleration,throttle)等信息.

         首先,控制模块的入口还是在modules/control/main.cc下,使用APOLLO_MAIN(Apollo::control::Control)进入整个apollo的环境中.APOLLO_MAIN()函数的定义是在modules/common/apollo_app.h中,       

#define APOLLO_MAIN(APP)                                       \
  int main(int argc, char **argv) {                            \
    google::InitGoogleLogging(argv[0]);                        \
    google::ParseCommandLineFlags(&argc, &argv, true);         \
    signal(SIGINT, apollo::common::apollo_app_sigint_handler); \
    APP apollo_app_;                                           \
    ros::init(argc, argv, apollo_app_.Name());                 \
    apollo_app_.Spin();                                        \
    return 0;                                                  \
  }

#endif  // MODULES_COMMON_APOLLO_APP_H_

 

这是主函数的真正入口,首先使用初始化Google的日志工具,使用signal(SIGINT,Apollo::common::Apollo_app_sigint_handler)接收到终止信息SIGINT(用户在linux的terminal中输出Ctrl-c后发出)后,调用ros::shutdown()关闭ROS.使用宏定义展开定义一个control的对象 Apollo::control::Controlapollo_app_ 然后初始化ros节点.功能的入口在control对象的apollo_app_.Spin()函数.最终返回0,结束程序的运行.

 

         然后我们分析下Apollo::control::Control类型,类型的声明和定义发生在modules/control/control.h和contro.cc中,在Apollo框架中,任何主体的功能模块都是继承于apollo::common::ApolloApp类型,control也不例外.所以首先执行的是ApolloApp中的Spin()函数,让我们看一下Spin()函数都做了些什么吧(在modules/common/apollo_app.cc中)

int ApolloApp::Spin() {
  std::unique_ptr<ros::AsyncSpinner> spinner;
  if (callback_thread_num_ > 1) {
    spinner = std::unique_ptr<ros::AsyncSpinner>(
        new ros::AsyncSpinner(callback_thread_num_));
  }
  auto status = Init();
  if (!status.ok()) {
    AERROR << Name() << " Init failed: " << status;
    return -1;
  }
  status = Start();
  if (!status.ok()) {
    AERROR << Name() << " Start failed: " << status;
    return -2;
  }
  ExportFlags();
  if (spinner) {
    spinner->start();
  } else {
    ros::spin();
  }
  ros::waitForShutdown();
  Stop();
  AINFO << Name() << " exited.";
  return 0;
}

std::unique_ptr<ros::AsyncSpinner> spinner 创建一个ros多线程,用来执行模块中的回调函数,接下来依次执行Init(),Start(),ExporeFlags(),如果spinner创建成功,就开始在另一独立线程中执行回调函数.然后等待shutdown,再执行Stop()函数,最后退出返回.而关键的Init(),Start()函数是纯虚函数,需要在子类中重写定义的,也就是说,程序到这个地方,就转到control的Init()中了,然后让我们视线转移到modules/common/control/control.cc中，

Status Control::Init() {
  init_time_ = Clock::NowInSeconds();

  AINFO << "Control init, starting ...";
  CHECK(common::util::GetProtoFromFile(FLAGS_control_conf_file, &control_conf_))
      << "Unable to load control conf file: " + FLAGS_control_conf_file;

  AINFO << "Conf file: " << FLAGS_control_conf_file << " is loaded.";

  AdapterManager::Init(FLAGS_control_adapter_config_filename);

  common::monitor::MonitorLogBuffer buffer(&monitor_logger_);

  // set controller
  if (!controller_agent_.Init(&control_conf_).ok()) {
    std::string error_msg = "Control init controller failed! Stopping...";
    buffer.ERROR(error_msg);
    return Status(ErrorCode::CONTROL_INIT_ERROR, error_msg);
  }

  // lock it in case for after sub, init_vehicle not ready, but msg trigger
  // come
  CHECK(AdapterManager::GetLocalization())
      << "Localization is not initialized.";

  CHECK(AdapterManager::GetChassis()) << "Chassis is not initialized.";

  CHECK(AdapterManager::GetPlanning()) << "Planning is not initialized.";

  CHECK(AdapterManager::GetPad()) << "Pad is not initialized.";

  CHECK(AdapterManager::GetMonitor()) << "Monitor is not initialized.";

  CHECK(AdapterManager::GetControlCommand())
      << "ControlCommand publisher is not initialized.";

  AdapterManager::AddPadCallback(&Control::OnPad, this);
  AdapterManager::AddMonitorCallback(&Control::OnMonitor, this);

  return Status::OK();
}

 

在Init()中前面的check的参数common::util::GetProtoFromFile(FLAGS_control_conf_file,&control_conf_)中,利用了util中的GetProtoFromFile()函数,目的是将第一个参数对应的文件输入到第二个参数中,以供程序使用,前面以Flag开头的参数都是google的gflag风格,对应的文件在每个模块的common/[module_name]_gflag.cc,相比control就是在modules/control/common/control_gflag.cc中可以找到.在里面,我们发现,control_con_file对应的文件为:”modules/control/conf/lincoln.pb.txt”所以这一步,我们把control的配置文件从lincoln.pb.txt中读到control_conf_中,也就是说,在调试过程中,我们如果需要更改配置参数,直接在lincoln.pb.txt中修改,然后直接重启该模块就OK了.(但是有一个问题，lincoln.pb.txt是如何来的？这其实是来自于modules/calibration/data/mkz8/calibration_table.pb.txt,当你在dreamview中选择车辆的时候，lincoln.pb.txt会继承于calibration_table.pb.txt)然后使用AdapterManager中的Init()方法对control中的adapter_config_filename初始化,,也就是说,读取control/conf/adapter.conf文件,确定该模块的订阅话题和输出话题.

 

config {
  type: LOCALIZATION
  mode: RECEIVE_ONLY
  message_history_limit: 10
}
config {
  type: PAD
  mode: RECEIVE_ONLY
  message_history_limit: 10
}
config {
  type: PLANNING_TRAJECTORY
  mode: RECEIVE_ONLY
  message_history_limit: 10
}
config {
  type: CHASSIS
  mode: RECEIVE_ONLY
  message_history_limit: 10
}
config {
  type: CONTROL_COMMAND
  mode: PUBLISH_ONLY
}
config {
  type: MONITOR
  mode: DUPLEX
  message_history_limit: 1
}
is_ros: true

在这里面,我们看到control模块订阅的话题有:localiza,pad,planning_trajectory,chassis,发布消息的话题为control_command.再返回control的Init()函数中,

 if (!controller_agent_.Init(&control_conf_).ok()) 

执行controller_agent_.Init(&control_conf_)函数,其意义是指明使用哪一个controller,并使用其中的方法计算control_command中的信息.controller_agent是apollo的一大特色,使用代理让我们很方便的更改我们使用的控制器,并自动完成初始化,检查等一系列工作.下面介绍下controller_agent的相关功能.

         controller_agent的定义存在于modules/controller.controller_agent.cc中.

Status ControllerAgent::Init(const ControlConf *control_conf) {
  RegisterControllers(control_conf);
  CHECK(InitializeConf(control_conf).ok()) << "Fail to initialize config.";
  for (auto &controller : controller_list_) {
    if (controller == NULL || !controller->Init(control_conf_).ok()) {
      if (controller != NULL) {
        AERROR << "Controller <" << controller->Name() << "> init failed!";
        return Status(ErrorCode::CONTROL_INIT_ERROR,
                      "Failed to init Controller:" + controller->Name());
      } else {
        return Status(ErrorCode::CONTROL_INIT_ERROR,
                      "Failed to init Controller");
      }
    }
    AINFO << "Controller <" << controller->Name() << "> init done!";
  }
  return Status::OK();
}

在ControllerAgent::Init(const ControlCong *control_conf)中,首先使用内部成员函数RegisterControllers(control_conf)注册控制器

void ControllerAgent::RegisterControllers(const ControlConf *control_conf) {
  AINFO << "Only support MPC controller or Lat + Lon controllers as of now";
  for (auto active_controller : control_conf->active_controllers()) {
    switch (active_controller) {
      case ControlConf::MPC_CONTROLLER:
        controller_factory_.Register(
            ControlConf::MPC_CONTROLLER,
            []() -> Controller * { return new MPCController(); });
        break;
      case ControlConf::LAT_CONTROLLER:
        controller_factory_.Register(
            ControlConf::LAT_CONTROLLER,
            []() -> Controller * { return new LatController(); });
        break;
      case ControlConf::LON_CONTROLLER:
        controller_factory_.Register(
            ControlConf::LON_CONTROLLER,
            []() -> Controller * { return new LonController(); });
        break;
      default:
        AERROR << "Unknown active controller type:" << active_controller;
    }
  }
}

 

这RegisterControllers中,从control_conf中读取到配置文件中我们**的controller(也就是说，我们需要使用什么类型的控制器，我们需要在modules/control/conf/lincoln.pb.txt中修改

 

active_controllers: LAT_CONTROLLER
active_controllers: LON_CONTROLLER

,依次使用apollo的工厂模式注册返回一个controller.完成注册后再次检测是否初始化成功,里面有很多日志文件的输出,这样设计,特别规范,能够很方便我们在日志文件中找到程序运行失败的原因.

         当代理注册完成之后,返回到control的Init()函数中,依次检查下control模块所需要的localization,chassis,planning,pad,monitor是否能够得到信息,以便确保输入信息的正确性.截止到此,我们已经完成了Spin()中的Init()函数,然后执行的将是Start()函数，让我们再次把视线转移到modules/control/control.cc中

Status Control::Start() {
  // set initial vehicle state by cmd
  // need to sleep, because advertised channel is not ready immediately
  // simple test shows a short delay of 80 ms or so
  AINFO << "Control resetting vehicle state, sleeping for 1000 ms ...";
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));

  // should init_vehicle first, let car enter work status, then use status msg
  // trigger control

  AINFO << "Control default driving action is "
        << DrivingAction_Name(control_conf_.action());
  pad_msg_.set_action(control_conf_.action());

  timer_ = AdapterManager::CreateTimer(
      ros::Duration(control_conf_.control_period()), &Control::OnTimer, this);

  AINFO << "Control init done!";

  common::monitor::MonitorLogBuffer buffer(&monitor_logger_);
  buffer.INFO("control started");

  return Status::OK();
}

         在Start函数中,最重要的就是timer_= AdapterManager::CreateTimer(ros::Duration(control_conf_.control_period()), &Control::OnTimer, this)函数,这个函数定义了回调函数的入口,和回调函数的执行周期,也就是说OnTimer()函数会周期性的执行,具体执行周期,在配置文件lincoln.pb.txt中有定义:control_period:0.01.也就是说control模块会以100hz的频率向外输出control_command信息.

         然后让我们看下Ontimer()中执行了哪些动作:

void Control::OnTimer(const ros::TimerEvent &) {
  double start_timestamp = Clock::NowInSeconds();

  if (FLAGS_is_control_test_mode && FLAGS_control_test_duration > 0 &&
      (start_timestamp - init_time_) > FLAGS_control_test_duration) {
    AERROR << "Control finished testing. exit";
    ros::shutdown();
  }

  ControlCommand control_command;

  Status status = ProduceControlCommand(&control_command);
  AERROR_IF(!status.ok()) << "Failed to produce control command:"
                          << status.error_message();

  double end_timestamp = Clock::NowInSeconds();

  if (pad_received_) {
    control_command.mutable_pad_msg()->CopyFrom(pad_msg_);
    pad_received_ = false;
  }

  const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
  control_command.mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  control_command.mutable_latency_stats()->set_total_time_exceeded(
      time_diff_ms < control_conf_.control_period());
  ADEBUG << "control cycle time is: " << time_diff_ms << " ms.";
  status.Save(control_command.mutable_header()->mutable_status());

  SendCmd(&control_command);
}

除去检查信息的,最重要的一点就是ProduceControlCommand(&control_command)函数.这个成员函数中,最重要的一点函数就是controller_agent_.ComputeControlCommand(...)函数,这个函数将使用我们注册过的controller,调用其ComputeControlCommand()函数计算control_command信息.然后返回到Ontimer中其最后调用内部成员函数SendCmd(&control_command)函数将控制信息在话题/apollo/control中.

         到此为止,我们知道了control的模块的工作流程了.下面着重介绍下Apollo目前在使用的LQR控制器的策略,其源文件位于modules/control/controller/lat_controller.h和lat_controller.cc中.研究其工作流程,可以从其ComputeControlCommand()作为入口,依次看各个程序运行的关系即可

         下面讨论下preview_window_= 0(默认为0,目前还没有测试,下期更新)的控制策略

横向控制的重点在于ComputeControlCommand(

const localization::LocalizationEstimate *localization,

const canbus::Chassis *chassis,

const planning:ADCTrajectory *planning_published_trajectory,

ControlCommand *cmd)

首先,使用函数UpdateStateAnalyticalMatching(debug)计算了车辆距轨迹点(planning给出的)的横向位置(使用ComputeLateralErrors()),然后将latral_error()、latral_error_rate()、heading_error()、heading_error_rate()分别放在了matrix_state_的第1,2,3,4行。即:

 

然后，使用UpdateMatrix(),对matrix_a_进行计算,matrix_a_= matrix_A_coeff_()/v,即

 

 

 

经过matrix_a_ = matrix_A_coeff_()/v后,

新建一个单位矩阵matrix_i;

又经历

对状态矩阵进行了离散化,具体参考资料

https://en.wikipedia.org/wiki/Discretization#Approximations

 

 

最后经历UpdateMatrixCompound(),把带有previewmatrix添加到matrix_adc_,这里没有用到preview matrix,暂时不用管,只是这个函数完成了把matrix_ad_的值赋给matrix_adc_,把matrix_bd_的值赋给matrix_bdc_.

 

最后使用LQR求解器对k矩阵进行求解,在此求解器中,把matrix_adc_当做是LQR的状态矩阵A,把matrix_bdc_当做是输入控制矩阵B.Apollo中LQR求解器总共有7个参数,依次分别为:

①A:车辆状态矩阵(复合矩阵),4*4,当preview_window_= 0 时;

②B:输入控制矩阵(复合矩阵),4*4,当preview_window_= 0 时;

③Q:系统状态的常矩阵,也是LQR中的Q,状态权重矩阵;

④R:控制输出常矩阵,也是LQR中的R

⑤tolerance:LQR计算阈值,求解AlgebraicRiccati方程的数值公差,是一个double类型,默认为0.01

⑥max_num_iteration:计算LQR的最大迭代次数

⑦ptr_k:反馈控制函数,也是LQR计算的结果

 

下面描述一下apollo中的LQR控制器的工作流程:

(详细见modules/common/math/linear_quadratic_regulator.c)

 

计算P的值:

          

反复迭代,直到迭代次数超过max_num_iteration(默认为150)的数大或者前后两个P值内部差值小于tolerance(默认为lqr_eps_=0.01).结束循环.

         最后计算K矩阵:

          

Apollo中前轮转角使用了前馈+反馈的机制进行横向控制

反馈的转角为:

         steer_angle_feedback= -(matrix_k*matrix_state)(0,0)

前馈的前轮转角为:

         先计算

          

          

附录:apollo中control中lat_controller.h中的成员变量:

ts_:离散时长 默认值为0.01

cf_:前轮侧偏刚度

cr_:后轮侧偏刚度

wheelbase_:轴距 

mass_:车重

lf_:前轮到质心的距离

lr_:后轮到质心的距离

iz_:转动惯量

steer_tranmission_ratio_:转向比

steer_single_direction_max_degree_:最大方向盘转角

max_lat_aa_:横向加速度限值(理论值) 

preview_window_:向前预测的控制周期数(预测控制器) 默认为0

basic_state_size_: 基本状态的大小,默认为4,包括横向误差,横向误差速率,航向角误差,航向角误差速率

matrix_a_ 车辆状态矩阵默认为4*4

matrix_ad_ 车辆状态矩阵(离散) 默认为4*4

matrix_adc_ 车辆状态矩阵(复合矩阵)  在preview_window_=0时为4*4

matrix_b_ 控制矩阵 4*1

matrix_bd_ 控制矩阵(离散) 4*1

matrix_bdc_ 控制矩阵(复合) 在preview_window_=0时为4*1

matrix_k_ 增益矩阵 1*4 在preview_window_ = 0 时为1*4

matrix_r_ 控制权重矩阵默认为1*1且为单位阵

matrix_q_ 状态权重矩阵在preview_window_ = 0 时为4*4

matrix_q_updated_ 状态权重更新矩阵

matrix_a_coeff_ 状态矩阵系数 在preview_window_ = 0 时为4*4

matrix_state_ 四合一矩阵,状态矩阵 在preview_window_ = 0 时为4*1

 

previous_heading_error_: 最后控制周期的航向偏差

previous_lateral_error_: 最后控制周期内与参考轨迹的横向距离

lqr_max_iteration_: LQR循环次数默认为150

lqr_eps_:  LQR计算阈值 默认数值为0.01