adc_mpc_controller.cc 29 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737
  1. /******************************************************************************
  2. * Copyright 2017 The Apollo Authors. All Rights Reserved.
  3. *
  4. * Licensed under the Apache License, Version 2.0 (the "License");
  5. * you may not use this file except in compliance with the License.
  6. * You may obtain a copy of the License at
  7. *
  8. * http://www.apache.org/licenses/LICENSE-2.0
  9. *
  10. * Unless required by applicable law or agreed to in writing, software
  11. * distributed under the License is distributed on an "AS IS" BASIS,
  12. * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13. * See the License for the specific language governing permissions and
  14. * limitations under the License.
  15. *****************************************************************************/
  16. #include "modules/control/controller/mpc_controller.h"
  17. #include <algorithm>
  18. #include <iomanip>
  19. #include <limits>
  20. #include <utility>
  21. #include <vector>
  22. #include "Eigen/LU"
  23. #include "absl/strings/str_cat.h"
  24. #include "cyber/common/log.h"
  25. #include "cyber/time/clock.h"
  26. #include "modules/common/configs/vehicle_config_helper.h"
  27. #include "modules/common/math/math_utils.h"
  28. #include "modules/common/math/mpc_osqp.h"
  29. #include "modules/control/common/control_gflags.h"
  30. namespace apollo {
  31. namespace control {
  32. using apollo::common::ErrorCode;
  33. using apollo::common::Status;
  34. using apollo::common::TrajectoryPoint;
  35. using apollo::common::VehicleStateProvider;
  36. using apollo::cyber::Clock;
  37. using Matrix = Eigen::MatrixXd;
  38. using apollo::common::VehicleConfigHelper;
  39. namespace {
  40. std::string GetLogFileName() {
  41. time_t raw_time;
  42. char name_buffer[80];
  43. std::time(&raw_time);
  44. std::tm time_tm;
  45. localtime_r(&raw_time, &time_tm);
  46. strftime(name_buffer, sizeof(name_buffer),
  47. "/tmp/mpc_controller_%F_%H%M%S.csv", &time_tm);
  48. return std::string(name_buffer);
  49. }
  50. void WriteHeaders(std::ofstream &file_stream) {}
  51. } // namespace
  52. MPCController::MPCController() : name_("MPC Controller") {
  53. if (FLAGS_enable_csv_debug) {
  54. mpc_log_file_.open(GetLogFileName());
  55. mpc_log_file_ << std::fixed;
  56. mpc_log_file_ << std::setprecision(6);
  57. WriteHeaders(mpc_log_file_);
  58. }
  59. AINFO << "Using " << name_;
  60. }
  61. MPCController::~MPCController() { CloseLogFile(); }
  62. bool MPCController::LoadControlConf(const ControlConf *control_conf) {
  63. if (!control_conf) {
  64. AERROR << "[MPCController] control_conf = nullptr";
  65. return false;
  66. }
  67. vehicle_param_ = VehicleConfigHelper::Instance()->GetConfig().vehicle_param();
  68. ts_ = control_conf->mpc_controller_conf().ts();
  69. if (ts_ <= 0.0) {
  70. AERROR << "[MPCController] Invalid control update interval.";
  71. return false;
  72. }
  73. cf_ = control_conf->mpc_controller_conf().cf();
  74. cr_ = control_conf->mpc_controller_conf().cr();
  75. wheelbase_ = vehicle_param_.wheel_base();
  76. steer_ratio_ = vehicle_param_.steer_ratio();
  77. steer_single_direction_max_degree_ =
  78. vehicle_param_.max_steer_angle() * 180 / M_PI;
  79. max_lat_acc_ = control_conf->mpc_controller_conf().max_lateral_acceleration();
  80. // TODO(Shu, Qi, Yu): add sanity check for conf values
  81. // steering ratio should be positive
  82. static constexpr double kEpsilon = 1e-6;
  83. if (std::isnan(steer_ratio_) || steer_ratio_ < kEpsilon) {
  84. AERROR << "[MPCController] steer_ratio = 0";
  85. return false;
  86. }
  87. wheel_single_direction_max_degree_ =
  88. steer_single_direction_max_degree_ / steer_ratio_ / 180 * M_PI;
  89. max_acceleration_ = vehicle_param_.max_acceleration();
  90. max_deceleration_ = vehicle_param_.max_deceleration();
  91. const double mass_fl = control_conf->mpc_controller_conf().mass_fl();
  92. const double mass_fr = control_conf->mpc_controller_conf().mass_fr();
  93. const double mass_rl = control_conf->mpc_controller_conf().mass_rl();
  94. const double mass_rr = control_conf->mpc_controller_conf().mass_rr();
  95. const double mass_front = mass_fl + mass_fr;
  96. const double mass_rear = mass_rl + mass_rr;
  97. mass_ = mass_front + mass_rear;
  98. lf_ = wheelbase_ * (1.0 - mass_front / mass_);
  99. lr_ = wheelbase_ * (1.0 - mass_rear / mass_);
  100. iz_ = lf_ * lf_ * mass_front + lr_ * lr_ * mass_rear;
  101. mpc_eps_ = control_conf->mpc_controller_conf().eps();
  102. mpc_max_iteration_ = control_conf->mpc_controller_conf().max_iteration();
  103. throttle_lowerbound_ =
  104. std::max(vehicle_param_.throttle_deadzone(),
  105. control_conf->mpc_controller_conf().throttle_minimum_action());
  106. brake_lowerbound_ =
  107. std::max(vehicle_param_.brake_deadzone(),
  108. control_conf->mpc_controller_conf().brake_minimum_action());
  109. minimum_speed_protection_ = control_conf->minimum_speed_protection();
  110. max_acceleration_when_stopped_ =
  111. control_conf->max_acceleration_when_stopped();
  112. max_abs_speed_when_stopped_ = vehicle_param_.max_abs_speed_when_stopped();
  113. standstill_acceleration_ =
  114. control_conf->mpc_controller_conf().standstill_acceleration();
  115. enable_mpc_feedforward_compensation_ =
  116. control_conf->mpc_controller_conf().enable_mpc_feedforward_compensation();
  117. unconstrained_control_diff_limit_ =
  118. control_conf->mpc_controller_conf().unconstrained_control_diff_limit();
  119. LoadControlCalibrationTable(control_conf->mpc_controller_conf());
  120. ADEBUG << "MPC conf loaded";
  121. return true;
  122. }
  123. void MPCController::ProcessLogs(const SimpleMPCDebug *debug,
  124. const canbus::Chassis *chassis) {
  125. // TODO(QiL): Add debug information
  126. }
  127. void MPCController::LogInitParameters() {
  128. ADEBUG << name_ << " begin.";
  129. ADEBUG << "[MPCController parameters]"
  130. << " mass_: " << mass_ << ","
  131. << " iz_: " << iz_ << ","
  132. << " lf_: " << lf_ << ","
  133. << " lr_: " << lr_;
  134. }
  135. void MPCController::InitializeFilters(const ControlConf *control_conf) {
  136. // Low pass filter
  137. std::vector<double> den(3, 0.0);
  138. std::vector<double> num(3, 0.0);
  139. common::LpfCoefficients(
  140. ts_, control_conf->mpc_controller_conf().cutoff_freq(), &den, &num);
  141. digital_filter_.set_coefficients(den, num);
  142. lateral_error_filter_ = common::MeanFilter(static_cast<std::uint_fast8_t>(
  143. control_conf->mpc_controller_conf().mean_filter_window_size()));
  144. heading_error_filter_ = common::MeanFilter(static_cast<std::uint_fast8_t>(
  145. control_conf->mpc_controller_conf().mean_filter_window_size()));
  146. }
  147. Status MPCController::Init(std::shared_ptr<DependencyInjector> injector,
  148. const ControlConf *control_conf) {
  149. if (!LoadControlConf(control_conf)) {
  150. AERROR << "failed to load control conf";
  151. return Status(ErrorCode::CONTROL_COMPUTE_ERROR,
  152. "failed to load control_conf");
  153. }
  154. injector_ = injector;
  155. // Matrix init operations.
  156. matrix_a_ = Matrix::Zero(basic_state_size_, basic_state_size_);
  157. matrix_ad_ = Matrix::Zero(basic_state_size_, basic_state_size_);
  158. matrix_a_(0, 1) = 1.0;
  159. matrix_a_(1, 2) = (cf_ + cr_) / mass_;
  160. matrix_a_(2, 3) = 1.0;
  161. matrix_a_(3, 2) = (lf_ * cf_ - lr_ * cr_) / iz_;
  162. matrix_a_(4, 5) = 1.0;
  163. matrix_a_(5, 5) = 0.0;
  164. // TODO(QiL): expand the model to accommodate more combined states.
  165. matrix_a_coeff_ = Matrix::Zero(basic_state_size_, basic_state_size_);
  166. matrix_a_coeff_(1, 1) = -(cf_ + cr_) / mass_;
  167. matrix_a_coeff_(1, 3) = (lr_ * cr_ - lf_ * cf_) / mass_;
  168. matrix_a_coeff_(2, 3) = 1.0;
  169. matrix_a_coeff_(3, 1) = (lr_ * cr_ - lf_ * cf_) / iz_;
  170. matrix_a_coeff_(3, 3) = -1.0 * (lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / iz_;
  171. matrix_b_ = Matrix::Zero(basic_state_size_, controls_);
  172. matrix_bd_ = Matrix::Zero(basic_state_size_, controls_);
  173. matrix_b_(1, 0) = cf_ / mass_;
  174. matrix_b_(3, 0) = lf_ * cf_ / iz_;
  175. matrix_b_(4, 1) = 0.0;
  176. matrix_b_(5, 1) = -1.0;
  177. matrix_bd_ = matrix_b_ * ts_;
  178. matrix_c_ = Matrix::Zero(basic_state_size_, 1);
  179. matrix_cd_ = Matrix::Zero(basic_state_size_, 1);
  180. matrix_state_ = Matrix::Zero(basic_state_size_, 1);
  181. matrix_k_ = Matrix::Zero(1, basic_state_size_);
  182. matrix_r_ = Matrix::Identity(controls_, controls_);
  183. matrix_q_ = Matrix::Zero(basic_state_size_, basic_state_size_);
  184. int r_param_size = control_conf->mpc_controller_conf().matrix_r_size();
  185. for (int i = 0; i < r_param_size; ++i) {
  186. matrix_r_(i, i) = control_conf->mpc_controller_conf().matrix_r(i);
  187. }
  188. int q_param_size = control_conf->mpc_controller_conf().matrix_q_size();
  189. if (basic_state_size_ != q_param_size) {
  190. const auto error_msg =
  191. absl::StrCat("MPC controller error: matrix_q size: ", q_param_size,
  192. " in parameter file not equal to basic_state_size_: ",
  193. basic_state_size_);
  194. AERROR << error_msg;
  195. return Status(ErrorCode::CONTROL_COMPUTE_ERROR, error_msg);
  196. }
  197. for (int i = 0; i < q_param_size; ++i) {
  198. matrix_q_(i, i) = control_conf->mpc_controller_conf().matrix_q(i);
  199. }
  200. // Update matrix_q_updated_ and matrix_r_updated_
  201. matrix_r_updated_ = matrix_r_;
  202. matrix_q_updated_ = matrix_q_;
  203. InitializeFilters(control_conf);
  204. LoadMPCGainScheduler(control_conf->mpc_controller_conf());
  205. LogInitParameters();
  206. ADEBUG << "[MPCController] init done!";
  207. return Status::OK();
  208. }
  209. void MPCController::CloseLogFile() {
  210. if (FLAGS_enable_csv_debug && mpc_log_file_.is_open()) {
  211. mpc_log_file_.close();
  212. }
  213. }
  214. double MPCController::Wheel2SteerPct(const double wheel_angle) {
  215. return wheel_angle / wheel_single_direction_max_degree_ * 100;
  216. }
  217. void MPCController::Stop() { CloseLogFile(); }
  218. std::string MPCController::Name() const { return name_; }
  219. void MPCController::LoadMPCGainScheduler(
  220. const MPCControllerConf &mpc_controller_conf) {
  221. const auto &lat_err_gain_scheduler =
  222. mpc_controller_conf.lat_err_gain_scheduler();
  223. const auto &heading_err_gain_scheduler =
  224. mpc_controller_conf.heading_err_gain_scheduler();
  225. const auto &feedforwardterm_gain_scheduler =
  226. mpc_controller_conf.feedforwardterm_gain_scheduler();
  227. const auto &steer_weight_gain_scheduler =
  228. mpc_controller_conf.steer_weight_gain_scheduler();
  229. ADEBUG << "MPC control gain scheduler loaded";
  230. Interpolation1D::DataType xy1, xy2, xy3, xy4;
  231. for (const auto &scheduler : lat_err_gain_scheduler.scheduler()) {
  232. xy1.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
  233. }
  234. for (const auto &scheduler : heading_err_gain_scheduler.scheduler()) {
  235. xy2.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
  236. }
  237. for (const auto &scheduler : feedforwardterm_gain_scheduler.scheduler()) {
  238. xy3.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
  239. }
  240. for (const auto &scheduler : steer_weight_gain_scheduler.scheduler()) {
  241. xy4.push_back(std::make_pair(scheduler.speed(), scheduler.ratio()));
  242. }
  243. lat_err_interpolation_.reset(new Interpolation1D);
  244. ACHECK(lat_err_interpolation_->Init(xy1))
  245. << "Fail to load lateral error gain scheduler for MPC controller";
  246. heading_err_interpolation_.reset(new Interpolation1D);
  247. ACHECK(heading_err_interpolation_->Init(xy2))
  248. << "Fail to load heading error gain scheduler for MPC controller";
  249. feedforwardterm_interpolation_.reset(new Interpolation1D);
  250. ACHECK(feedforwardterm_interpolation_->Init(xy3))
  251. << "Fail to load feed forward term gain scheduler for MPC controller";
  252. steer_weight_interpolation_.reset(new Interpolation1D);
  253. ACHECK(steer_weight_interpolation_->Init(xy4))
  254. << "Fail to load steer weight gain scheduler for MPC controller";
  255. }
  256. Status MPCController::ComputeControlCommand(
  257. const localization::LocalizationEstimate *localization,
  258. const canbus::Chassis *chassis,
  259. const planning::ADCTrajectory *planning_published_trajectory,
  260. ControlCommand *cmd) {
  261. trajectory_analyzer_ =
  262. std::move(TrajectoryAnalyzer(planning_published_trajectory));
  263. SimpleMPCDebug *debug = cmd->mutable_debug()->mutable_simple_mpc_debug();
  264. debug->Clear();
  265. ComputeLongitudinalErrors(&trajectory_analyzer_, debug);
  266. // Update state
  267. UpdateState(debug);
  268. UpdateMatrix(debug);
  269. FeedforwardUpdate(debug);
  270. auto vehicle_state = injector_->vehicle_state();
  271. // Add gain scheduler for higher speed steering
  272. if (FLAGS_enable_gain_scheduler) {
  273. matrix_q_updated_(0, 0) =
  274. matrix_q_(0, 0) *
  275. lat_err_interpolation_->Interpolate(vehicle_state->linear_velocity());
  276. matrix_q_updated_(2, 2) =
  277. matrix_q_(2, 2) * heading_err_interpolation_->Interpolate(
  278. vehicle_state->linear_velocity());
  279. steer_angle_feedforwardterm_updated_ =
  280. steer_angle_feedforwardterm_ *
  281. feedforwardterm_interpolation_->Interpolate(
  282. vehicle_state->linear_velocity());
  283. matrix_r_updated_(0, 0) =
  284. matrix_r_(0, 0) * steer_weight_interpolation_->Interpolate(
  285. vehicle_state->linear_velocity());
  286. } else {
  287. matrix_q_updated_ = matrix_q_;
  288. matrix_r_updated_ = matrix_r_;
  289. steer_angle_feedforwardterm_updated_ = steer_angle_feedforwardterm_;
  290. }
  291. debug->add_matrix_q_updated(matrix_q_updated_(0, 0));
  292. debug->add_matrix_q_updated(matrix_q_updated_(1, 1));
  293. debug->add_matrix_q_updated(matrix_q_updated_(2, 2));
  294. debug->add_matrix_q_updated(matrix_q_updated_(3, 3));
  295. debug->add_matrix_r_updated(matrix_r_updated_(0, 0));
  296. debug->add_matrix_r_updated(matrix_r_updated_(1, 1));
  297. Matrix control_matrix = Matrix::Zero(controls_, 1);
  298. std::vector<Matrix> control(horizon_, control_matrix);
  299. Matrix control_gain_matrix = Matrix::Zero(controls_, basic_state_size_);
  300. std::vector<Matrix> control_gain(horizon_, control_gain_matrix);
  301. Matrix addition_gain_matrix = Matrix::Zero(controls_, 1);
  302. std::vector<Matrix> addition_gain(horizon_, addition_gain_matrix);
  303. Matrix reference_state = Matrix::Zero(basic_state_size_, 1);
  304. std::vector<Matrix> reference(horizon_, reference_state);
  305. Matrix lower_bound(controls_, 1);
  306. lower_bound << -wheel_single_direction_max_degree_, max_deceleration_;
  307. Matrix upper_bound(controls_, 1);
  308. upper_bound << wheel_single_direction_max_degree_, max_acceleration_;
  309. const double max = std::numeric_limits<double>::max();
  310. Matrix lower_state_bound(basic_state_size_, 1);
  311. Matrix upper_state_bound(basic_state_size_, 1);
  312. // lateral_error, lateral_error_rate, heading_error, heading_error_rate
  313. // station_error, station_error_rate
  314. lower_state_bound << -1.0 * max, -1.0 * max, -1.0 * M_PI, -1.0 * max,
  315. -1.0 * max, -1.0 * max;
  316. upper_state_bound << max, max, M_PI, max, max, max;
  317. #ifdef ADCTEST
  318. double mpc_start_timestamp = 0;
  319. #else
  320. double mpc_start_timestamp = Clock::NowInSeconds();
  321. #endif
  322. double steer_angle_feedback = 0.0;
  323. double acc_feedback = 0.0;
  324. double steer_angle_ff_compensation = 0.0;
  325. double unconstrained_control_diff = 0.0;
  326. double control_gain_truncation_ratio = 0.0;
  327. double unconstrained_control = 0.0;
  328. const double v = injector_->vehicle_state()->linear_velocity();
  329. std::vector<double> control_cmd(controls_, 0);
  330. apollo::common::math::MpcOsqp mpc_osqp(
  331. matrix_ad_, matrix_bd_, matrix_q_updated_, matrix_r_updated_,
  332. matrix_state_, lower_bound, upper_bound, lower_state_bound,
  333. upper_state_bound, reference_state, mpc_max_iteration_, horizon_,
  334. mpc_eps_);
  335. if (!mpc_osqp.Solve(&control_cmd)) {
  336. AERROR << "MPC OSQP solver failed";
  337. } else {
  338. ADEBUG << "MPC OSQP problem solved! ";
  339. control[0](0, 0) = control_cmd.at(0);
  340. control[0](1, 0) = control_cmd.at(1);
  341. }
  342. steer_angle_feedback = Wheel2SteerPct(control[0](0, 0));
  343. acc_feedback = control[0](1, 0);
  344. for (int i = 0; i < basic_state_size_; ++i) {
  345. unconstrained_control += control_gain[0](0, i) * matrix_state_(i, 0);
  346. }
  347. unconstrained_control += addition_gain[0](0, 0) * v * debug->curvature();
  348. if (enable_mpc_feedforward_compensation_) {
  349. unconstrained_control_diff =
  350. Wheel2SteerPct(control[0](0, 0) - unconstrained_control);
  351. if (fabs(unconstrained_control_diff) <= unconstrained_control_diff_limit_) {
  352. steer_angle_ff_compensation =
  353. Wheel2SteerPct(debug->curvature() *
  354. (control_gain[0](0, 2) *
  355. (lr_ - lf_ / cr_ * mass_ * v * v / wheelbase_) -
  356. addition_gain[0](0, 0) * v));
  357. } else {
  358. control_gain_truncation_ratio = control[0](0, 0) / unconstrained_control;
  359. steer_angle_ff_compensation =
  360. Wheel2SteerPct(debug->curvature() *
  361. (control_gain[0](0, 2) *
  362. (lr_ - lf_ / cr_ * mass_ * v * v / wheelbase_) -
  363. addition_gain[0](0, 0) * v) *
  364. control_gain_truncation_ratio);
  365. }
  366. } else {
  367. steer_angle_ff_compensation = 0.0;
  368. }
  369. #ifdef ADCTEST
  370. double mpc_end_timestamp = 0;
  371. #else
  372. double mpc_end_timestamp = Clock::NowInSeconds();
  373. #endif
  374. ADEBUG << "MPC core algorithm: calculation time is: "
  375. << (mpc_end_timestamp - mpc_start_timestamp) * 1000 << " ms.";
  376. // TODO(QiL): evaluate whether need to add spline smoothing after the result
  377. double steer_angle = steer_angle_feedback +
  378. steer_angle_feedforwardterm_updated_ +
  379. steer_angle_ff_compensation;
  380. if (FLAGS_set_steer_limit) {
  381. const double steer_limit = std::atan(max_lat_acc_ * wheelbase_ /
  382. (vehicle_state->linear_velocity() *
  383. vehicle_state->linear_velocity())) *
  384. steer_ratio_ * 180 / M_PI /
  385. steer_single_direction_max_degree_ * 100;
  386. // Clamp the steer angle with steer limitations at current speed
  387. double steer_angle_limited =
  388. common::math::Clamp(steer_angle, -steer_limit, steer_limit);
  389. steer_angle_limited = digital_filter_.Filter(steer_angle_limited);
  390. steer_angle = steer_angle_limited;
  391. debug->set_steer_angle_limited(steer_angle_limited);
  392. }
  393. steer_angle = digital_filter_.Filter(steer_angle);
  394. // Clamp the steer angle to -100.0 to 100.0
  395. steer_angle = common::math::Clamp(steer_angle, -100.0, 100.0);
  396. cmd->set_steering_target(steer_angle);
  397. debug->set_acceleration_cmd_closeloop(acc_feedback);
  398. double acceleration_cmd = acc_feedback + debug->acceleration_reference();
  399. // TODO(QiL): add pitch angle feed forward to accommodate for 3D control
  400. if ((planning_published_trajectory->trajectory_type() ==
  401. apollo::planning::ADCTrajectory::NORMAL) &&
  402. (std::fabs(debug->acceleration_reference()) <=
  403. max_acceleration_when_stopped_ &&
  404. std::fabs(debug->speed_reference()) <= max_abs_speed_when_stopped_)) {
  405. acceleration_cmd =
  406. (chassis->gear_location() == canbus::Chassis::GEAR_REVERSE)
  407. ? std::max(acceleration_cmd, -standstill_acceleration_)
  408. : std::min(acceleration_cmd, standstill_acceleration_);
  409. ADEBUG << "Stop location reached";
  410. debug->set_is_full_stop(true);
  411. }
  412. // TODO(Yu): study the necessity of path_remain and add it to MPC if needed
  413. debug->set_acceleration_cmd(acceleration_cmd);
  414. double calibration_value = 0.0;
  415. if (FLAGS_use_preview_speed_for_table) {
  416. calibration_value = control_interpolation_->Interpolate(
  417. std::make_pair(debug->speed_reference(), acceleration_cmd));
  418. } else {
  419. calibration_value = control_interpolation_->Interpolate(std::make_pair(
  420. injector_->vehicle_state()->linear_velocity(), acceleration_cmd));
  421. }
  422. debug->set_calibration_value(calibration_value);
  423. double throttle_cmd = 0.0;
  424. double brake_cmd = 0.0;
  425. if (calibration_value >= 0) {
  426. throttle_cmd = std::max(calibration_value, throttle_lowerbound_);
  427. brake_cmd = 0.0;
  428. } else {
  429. throttle_cmd = 0.0;
  430. brake_cmd = std::max(-calibration_value, brake_lowerbound_);
  431. }
  432. cmd->set_steering_rate(FLAGS_steer_angle_rate);
  433. // if the car is driven by acceleration, disgard the cmd->throttle and brake
  434. cmd->set_throttle(throttle_cmd);
  435. cmd->set_brake(brake_cmd);
  436. cmd->set_acceleration(acceleration_cmd);
  437. debug->set_heading(vehicle_state->heading());
  438. debug->set_steering_position(chassis->steering_percentage());
  439. debug->set_steer_angle(steer_angle);
  440. debug->set_steer_angle_feedforward(steer_angle_feedforwardterm_updated_);
  441. debug->set_steer_angle_feedforward_compensation(steer_angle_ff_compensation);
  442. debug->set_steer_unconstrained_control_diff(unconstrained_control_diff);
  443. debug->set_steer_angle_feedback(steer_angle_feedback);
  444. debug->set_steering_position(chassis->steering_percentage());
  445. if (std::fabs(vehicle_state->linear_velocity()) <=
  446. vehicle_param_.max_abs_speed_when_stopped() ||
  447. chassis->gear_location() == planning_published_trajectory->gear() ||
  448. chassis->gear_location() == canbus::Chassis::GEAR_NEUTRAL) {
  449. cmd->set_gear_location(planning_published_trajectory->gear());
  450. } else {
  451. cmd->set_gear_location(chassis->gear_location());
  452. }
  453. ProcessLogs(debug, chassis);
  454. return Status::OK();
  455. }
  456. Status MPCController::Reset() {
  457. previous_heading_error_ = 0.0;
  458. previous_lateral_error_ = 0.0;
  459. return Status::OK();
  460. }
  461. void MPCController::LoadControlCalibrationTable(
  462. const MPCControllerConf &mpc_controller_conf) {
  463. const auto &control_table = mpc_controller_conf.calibration_table();
  464. ADEBUG << "Control calibration table loaded";
  465. ADEBUG << "Control calibration table size is "
  466. << control_table.calibration_size();
  467. Interpolation2D::DataType xyz;
  468. for (const auto &calibration : control_table.calibration()) {
  469. xyz.push_back(std::make_tuple(calibration.speed(),
  470. calibration.acceleration(),
  471. calibration.command()));
  472. }
  473. control_interpolation_.reset(new Interpolation2D);
  474. ACHECK(control_interpolation_->Init(xyz))
  475. << "Fail to load control calibration table";
  476. }
  477. void MPCController::UpdateState(SimpleMPCDebug *debug) {
  478. const auto &com = injector_->vehicle_state()->ComputeCOMPosition(lr_);
  479. ComputeLateralErrors(com.x(), com.y(), injector_->vehicle_state()->heading(),
  480. injector_->vehicle_state()->linear_velocity(),
  481. injector_->vehicle_state()->angular_velocity(),
  482. injector_->vehicle_state()->linear_acceleration(),
  483. trajectory_analyzer_, debug);
  484. // State matrix update;
  485. matrix_state_(0, 0) = debug->lateral_error();
  486. matrix_state_(1, 0) = debug->lateral_error_rate();
  487. matrix_state_(2, 0) = debug->heading_error();
  488. matrix_state_(3, 0) = debug->heading_error_rate();
  489. matrix_state_(4, 0) = debug->station_error();
  490. matrix_state_(5, 0) = debug->speed_error();
  491. }
  492. void MPCController::UpdateMatrix(SimpleMPCDebug *debug) {
  493. const double v = std::max(injector_->vehicle_state()->linear_velocity(),
  494. minimum_speed_protection_);
  495. matrix_a_(1, 1) = matrix_a_coeff_(1, 1) / v;
  496. matrix_a_(1, 3) = matrix_a_coeff_(1, 3) / v;
  497. matrix_a_(3, 1) = matrix_a_coeff_(3, 1) / v;
  498. matrix_a_(3, 3) = matrix_a_coeff_(3, 3) / v;
  499. Matrix matrix_i = Matrix::Identity(matrix_a_.cols(), matrix_a_.cols());
  500. matrix_ad_ = (matrix_i - ts_ * 0.5 * matrix_a_).inverse() *
  501. (matrix_i + ts_ * 0.5 * matrix_a_);
  502. matrix_c_(1, 0) = (lr_ * cr_ - lf_ * cf_) / mass_ / v - v;
  503. matrix_c_(3, 0) = -(lf_ * lf_ * cf_ + lr_ * lr_ * cr_) / iz_ / v;
  504. matrix_cd_ = matrix_c_ * debug->ref_heading_rate() * ts_;
  505. }
  506. void MPCController::FeedforwardUpdate(SimpleMPCDebug *debug) {
  507. const double v = injector_->vehicle_state()->linear_velocity();
  508. const double kv =
  509. lr_ * mass_ / 2 / cf_ / wheelbase_ - lf_ * mass_ / 2 / cr_ / wheelbase_;
  510. steer_angle_feedforwardterm_ = Wheel2SteerPct(
  511. wheelbase_ * debug->curvature() + kv * v * v * debug->curvature());
  512. }
  513. void MPCController::ComputeLateralErrors(
  514. const double x, const double y, const double theta, const double linear_v,
  515. const double angular_v, const double linear_a,
  516. const TrajectoryAnalyzer &trajectory_analyzer, SimpleMPCDebug *debug) {
  517. const auto matched_point =
  518. trajectory_analyzer.QueryNearestPointByPosition(x, y);
  519. const double dx = x - matched_point.path_point().x();
  520. const double dy = y - matched_point.path_point().y();
  521. const double cos_matched_theta = std::cos(matched_point.path_point().theta());
  522. const double sin_matched_theta = std::sin(matched_point.path_point().theta());
  523. // d_error = cos_matched_theta * dy - sin_matched_theta * dx;
  524. debug->set_lateral_error(cos_matched_theta * dy - sin_matched_theta * dx);
  525. // matched_theta = matched_point.path_point().theta();
  526. debug->set_ref_heading(matched_point.path_point().theta());
  527. const double delta_theta =
  528. common::math::NormalizeAngle(theta - debug->ref_heading());
  529. debug->set_heading_error(delta_theta);
  530. const double sin_delta_theta = std::sin(delta_theta);
  531. // d_error_dot = chassis_v * sin_delta_theta;
  532. double lateral_error_dot = linear_v * sin_delta_theta;
  533. double lateral_error_dot_dot = linear_a * sin_delta_theta;
  534. if (FLAGS_reverse_heading_control) {
  535. if (injector_->vehicle_state()->gear() == canbus::Chassis::GEAR_REVERSE) {
  536. lateral_error_dot = -lateral_error_dot;
  537. lateral_error_dot_dot = -lateral_error_dot_dot;
  538. }
  539. }
  540. debug->set_lateral_error_rate(lateral_error_dot);
  541. debug->set_lateral_acceleration(lateral_error_dot_dot);
  542. debug->set_lateral_jerk(
  543. (debug->lateral_acceleration() - previous_lateral_acceleration_) / ts_);
  544. previous_lateral_acceleration_ = debug->lateral_acceleration();
  545. // matched_kappa = matched_point.path_point().kappa();
  546. debug->set_curvature(matched_point.path_point().kappa());
  547. // theta_error = delta_theta;
  548. debug->set_heading_error(delta_theta);
  549. // theta_error_dot = angular_v - matched_point.path_point().kappa() *
  550. // matched_point.v();
  551. debug->set_heading_rate(angular_v);
  552. debug->set_ref_heading_rate(debug->curvature() * matched_point.v());
  553. debug->set_heading_error_rate(debug->heading_rate() -
  554. debug->ref_heading_rate());
  555. debug->set_heading_acceleration(
  556. (debug->heading_rate() - previous_heading_rate_) / ts_);
  557. debug->set_ref_heading_acceleration(
  558. (debug->ref_heading_rate() - previous_ref_heading_rate_) / ts_);
  559. debug->set_heading_error_acceleration(debug->heading_acceleration() -
  560. debug->ref_heading_acceleration());
  561. previous_heading_rate_ = debug->heading_rate();
  562. previous_ref_heading_rate_ = debug->ref_heading_rate();
  563. debug->set_heading_jerk(
  564. (debug->heading_acceleration() - previous_heading_acceleration_) / ts_);
  565. debug->set_ref_heading_jerk(
  566. (debug->ref_heading_acceleration() - previous_ref_heading_acceleration_) /
  567. ts_);
  568. debug->set_heading_error_jerk(debug->heading_jerk() -
  569. debug->ref_heading_jerk());
  570. previous_heading_acceleration_ = debug->heading_acceleration();
  571. previous_ref_heading_acceleration_ = debug->ref_heading_acceleration();
  572. }
  573. void MPCController::ComputeLongitudinalErrors(
  574. const TrajectoryAnalyzer *trajectory_analyzer, SimpleMPCDebug *debug) {
  575. // the decomposed vehicle motion onto Frenet frame
  576. // s: longitudinal accumulated distance along reference trajectory
  577. // s_dot: longitudinal velocity along reference trajectory
  578. // d: lateral distance w.r.t. reference trajectory
  579. // d_dot: lateral distance change rate, i.e. dd/dt
  580. double s_matched = 0.0;
  581. double s_dot_matched = 0.0;
  582. double d_matched = 0.0;
  583. double d_dot_matched = 0.0;
  584. const auto matched_point = trajectory_analyzer->QueryMatchedPathPoint(
  585. injector_->vehicle_state()->x(), injector_->vehicle_state()->y());
  586. trajectory_analyzer->ToTrajectoryFrame(
  587. injector_->vehicle_state()->x(), injector_->vehicle_state()->y(),
  588. injector_->vehicle_state()->heading(),
  589. injector_->vehicle_state()->linear_velocity(), matched_point, &s_matched,
  590. &s_dot_matched, &d_matched, &d_dot_matched);
  591. #ifndef ADCTEST
  592. const double current_control_time = Clock::NowInSeconds();
  593. #else
  594. const double current_control_time = 0;
  595. #endif
  596. TrajectoryPoint reference_point =
  597. trajectory_analyzer->QueryNearestPointByAbsoluteTime(
  598. current_control_time);
  599. ADEBUG << "matched point:" << matched_point.DebugString();
  600. ADEBUG << "reference point:" << reference_point.DebugString();
  601. const double linear_v = injector_->vehicle_state()->linear_velocity();
  602. const double linear_a = injector_->vehicle_state()->linear_acceleration();
  603. double heading_error = common::math::NormalizeAngle(
  604. injector_->vehicle_state()->heading() - matched_point.theta());
  605. double lon_speed = linear_v * std::cos(heading_error);
  606. double lon_acceleration = linear_a * std::cos(heading_error);
  607. double one_minus_kappa_lat_error = 1 - reference_point.path_point().kappa() *
  608. linear_v * std::sin(heading_error);
  609. debug->set_station_reference(reference_point.path_point().s());
  610. debug->set_station_feedback(s_matched);
  611. debug->set_station_error(reference_point.path_point().s() - s_matched);
  612. debug->set_speed_reference(reference_point.v());
  613. debug->set_speed_feedback(lon_speed);
  614. debug->set_speed_error(reference_point.v() - s_dot_matched);
  615. debug->set_acceleration_reference(reference_point.a());
  616. debug->set_acceleration_feedback(lon_acceleration);
  617. debug->set_acceleration_error(reference_point.a() -
  618. lon_acceleration / one_minus_kappa_lat_error);
  619. double jerk_reference =
  620. (debug->acceleration_reference() - previous_acceleration_reference_) /
  621. ts_;
  622. double lon_jerk =
  623. (debug->acceleration_feedback() - previous_acceleration_) / ts_;
  624. debug->set_jerk_reference(jerk_reference);
  625. debug->set_jerk_feedback(lon_jerk);
  626. debug->set_jerk_error(jerk_reference - lon_jerk / one_minus_kappa_lat_error);
  627. previous_acceleration_reference_ = debug->acceleration_reference();
  628. previous_acceleration_ = debug->acceleration_feedback();
  629. }
  630. } // namespace control
  631. } // namespace apollo