开发者说 | 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中。
| 1 2 3 4 5 6 7 8 9 10 11 12 | #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::Control apollo_app_ 然后初始化ROS节点，功能的入口在Control对象的apollo_app_.Spin()函数。最终返回0，结束程序的运行。
然后我们分析下Apollo::control::Control类型,类型的声明和定义发生在modules/control/control.h和control.cc中，在Apollo框架中，任何主体的功能模块都是继承于apollo::common::ApolloApp类型，Control也不例外。所以首先执行的是ApolloApp中的Spin()函数，让我们看一下Spin()函数都做了些什么吧(在modules/common/apollo_app.cc中)。
| 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 | 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中。
| 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | 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文件,确定该模块的订阅话题和输出话题。
| 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 | 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()函数中。
| 1 | 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中。
| 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 | 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)注册控制器。
| 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 | 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中修改）。
| 1 2 | 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中。
| 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 | 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()中执行了哪些动作：
| 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 | 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()，把带有preview matrix添加到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.cc)
计算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
Apollo
自Apollo平台开放已来，我们收到了大量开发者的咨询和反馈，越来越多开发者基于Apollo擦出了更多的火花，并愿意将自己的成果贡献出来，这充分体现了Apollo『贡献越多，获得越多』的开源精神。为此我们开设了『开发者说』板块，希望开发者们能够踊跃投稿，更好地为广大自动驾驶开发者营造一个共享交流的平台！
* 以上内容为开发者投稿，不代表百度官方言论。
本文首发于CSDN博客，已获得开发者授权，原文地址：
https://blog.csdn.net/yue597215286/article/details/80201200
更多相关模块代码请戳【阅读原文】访问Apollo GitHub。