Apollo 10.0
自动驾驶开放平台
mpc_controller.h
浏览该文件的文档.
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
22#pragma once
23
24#include <fstream>
25#include <memory>
26#include <string>
27
28#include "Eigen/Core"
29
30#include "modules/common_msgs/config_msgs/vehicle_config.pb.h"
31#include "modules/control/controllers/mpc_controller/proto/mpc_controller.pb.h"
32
44
49namespace apollo {
50namespace control {
51
57class MPCController : public ControlTask {
58 public:
63
67 virtual ~MPCController();
68
74 common::Status Init(std::shared_ptr<DependencyInjector> injector) override;
75
86 const localization::LocalizationEstimate *localization,
87 const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory,
88 ControlCommand *cmd) override;
89
94 common::Status Reset() override;
95
99 void Stop() override;
100
105 std::string Name() const override;
106
107 protected:
108 void UpdateState(SimpleMPCDebug *debug);
109
110 void UpdateMatrix(SimpleMPCDebug *debug);
111
113
114 void ComputeLateralErrors(const double x, const double y, const double theta,
115 const double linear_v, const double angular_v,
116 const double linear_a,
117 const TrajectoryAnalyzer &trajectory_analyzer,
118 SimpleMPCDebug *debug);
119
120 void ComputeLongitudinalErrors(const TrajectoryAnalyzer *trajectory,
121 SimpleMPCDebug *debug);
122
123 void ComputeLongitudinalErrors(const TrajectoryAnalyzer *trajectory_analyzer,
124 const double preview_time, const double ts,
125 SimpleMPCDebug *debug);
126
127 void GetPathRemain(
128 const planning::ADCTrajectory *planning_published_trajectory,
129 SimpleMPCDebug *debug);
130
131 bool LoadControlConf();
132
133 void InitializeFilters();
134
135 void LogInitParameters();
136
137 void ProcessLogs(const SimpleMPCDebug *debug, const canbus::Chassis *chassis);
138
139 void CloseLogFile();
140
141 double Wheel2SteerPct(const double wheel_angle);
142
143 // vehicle parameter
145
146 // a proxy to analyze the planning trajectory
148
150
152
153 std::unique_ptr<Interpolation2D> control_interpolation_;
154
155 MPCControllerConf control_conf_;
157 // the following parameters are vehicle physics related.
158 // control time interval
159 double ts_ = 0.0;
160 // corner stiffness; front
161 double cf_ = 0.0;
162 // corner stiffness; rear
163 double cr_ = 0.0;
164 // distance between front and rear wheel center
165 double wheelbase_ = 0.0;
166 // mass of the vehicle
167 double mass_ = 0.0;
168 // distance from front wheel center to COM
169 double lf_ = 0.0;
170 // distance from rear wheel center to COM
171 double lr_ = 0.0;
172 // rotational inertia
173 double iz_ = 0.0;
174 // the ratio between the turn of the steering wheel and the turn of the wheels
175 double steer_ratio_ = 0.0;
176 // the maximum turn of steer
178 // the maximum turn of vehicle wheel
180
181 // limit steering to maximum theoretical lateral acceleration
182 double max_lat_acc_ = 0.0;
183
184 // number of states, includes
185 // lateral error, lateral error rate, heading error, heading error rate,
186 // station error, velocity error,
187 const int basic_state_size_ = 6;
188
189 const int controls_ = 2;
190
191 const int horizon_ = 10;
192 // vehicle state matrix
193 Eigen::MatrixXd matrix_a_;
194 // vehicle state matrix (discrete-time)
195 Eigen::MatrixXd matrix_ad_;
196
197 // control matrix
198 Eigen::MatrixXd matrix_b_;
199 // control matrix (discrete-time)
200 Eigen::MatrixXd matrix_bd_;
201
202 // offset matrix
203 Eigen::MatrixXd matrix_c_;
204 // offset matrix (discrete-time)
205 Eigen::MatrixXd matrix_cd_;
206
207 // gain matrix
208 Eigen::MatrixXd matrix_k_;
209 // control authority weighting matrix
210 Eigen::MatrixXd matrix_r_;
211 // updated control authority weighting matrix
212 Eigen::MatrixXd matrix_r_updated_;
213 // state weighting matrix
214 Eigen::MatrixXd matrix_q_;
215 // updated state weighting matrix
216 Eigen::MatrixXd matrix_q_updated_;
217 // vehicle state matrix coefficients
218 Eigen::MatrixXd matrix_a_coeff_;
219 // 4 by 1 matrix; state matrix
220 Eigen::MatrixXd matrix_state_;
221
222 // heading error of last control cycle
224 // lateral distance to reference trajectory of last control cycle
226
227 // lateral dynamic variables for computing the differential valute to
228 // estimate acceleration and jerk
230
233
236
237 // longitudinal dynamic variables for computing the differential valute to
238 // estimate acceleration and jerk
241
242 // parameters for mpc solver; number of iterations
244 // parameters for mpc solver; threshold for computation
245 double mpc_eps_ = 0.0;
246
248
250
251 std::unique_ptr<Interpolation1D> lat_err_interpolation_;
252
253 std::unique_ptr<Interpolation1D> heading_err_interpolation_;
254
255 std::unique_ptr<Interpolation1D> feedforwardterm_interpolation_;
256
257 std::unique_ptr<Interpolation1D> steer_weight_interpolation_;
258
259 // for logging purpose
260 std::ofstream mpc_log_file_;
261
262 const std::string name_;
263
265
267
269
271
272 double brake_lowerbound_ = 0.0;
273
275
277
278 double max_acceleration_ = 0.0;
279
280 double max_deceleration_ = 0.0;
281
282 // MeanFilter heading_error_filter_;
284
285 // MeanFilter lateral_error_filter;
287
288 // Lead/Lag controller
289 bool enable_leadlag_ = false;
291
293
294 // Enable the feedback-gain-related compensation components in the feedfoward
295 // term for steering control
297
298 // Limitation for judging if the unconstrained analytical control is close
299 // enough to the solver's output with constraint
301
302 // Look-ahead controller
304
305 double low_speed_bound_ = 0.0;
306
307 double low_speed_window_ = 0.0;
308
309 // number of control cycles look ahead (preview controller)
311
312 // longitudial length for look-ahead lateral error estimation during forward
313 // driving and look-back lateral error estimation during backward driving
314 // (look-ahead controller)
319
322
324
325 double preview_time_ = 0.0;
326
327 bool use_preview_ = false;
328
330
331 // PID controller
334
336 std::shared_ptr<DependencyInjector> injector_;
337};
338
339// 1.2 当前类声明为插件
342
343} // namespace control
344} // namespace apollo
The DigitalFilter class is used to pass signals with a frequency lower than a certain cutoff frequenc...
The MeanFilter class is used to smoothen a series of noisy numbers, such as sensor data or the output...
Definition mean_filter.h:45
A general class to denote the return status of an API call.
Definition status.h:43
A lead/lag controller for speed and steering using defualt integral hold
MPCController, combined lateral and longitudinal controllers
PIDController acceleration_lookup_pid_controller_
TrajectoryAnalyzer trajectory_analyzer_
void FeedforwardUpdate(SimpleMPCDebug *debug)
common::Status ComputeControlCommand(const localization::LocalizationEstimate *localization, const canbus::Chassis *chassis, const planning::ADCTrajectory *trajectory, ControlCommand *cmd) override
compute steering target and throttle/ brake based on current vehicle status and target trajectory
void GetPathRemain(const planning::ADCTrajectory *planning_published_trajectory, SimpleMPCDebug *debug)
void ProcessLogs(const SimpleMPCDebug *debug, const canbus::Chassis *chassis)
void ComputeLateralErrors(const double x, const double y, const double theta, const double linear_v, const double angular_v, const double linear_a, const TrajectoryAnalyzer &trajectory_analyzer, SimpleMPCDebug *debug)
void UpdateMatrix(SimpleMPCDebug *debug)
std::unique_ptr< Interpolation1D > heading_err_interpolation_
LeadlagController leadlag_controller_
common::MeanFilter lateral_error_filter_
std::unique_ptr< Interpolation1D > feedforwardterm_interpolation_
std::unique_ptr< Interpolation2D > control_interpolation_
void Stop() override
stop MPC controller
std::unique_ptr< Interpolation1D > lat_err_interpolation_
common::DigitalFilter digital_filter_pitch_angle_
std::unique_ptr< Interpolation1D > steer_weight_interpolation_
calibration_table calibration_table_
double Wheel2SteerPct(const double wheel_angle)
common::VehicleParam vehicle_param_
common::MeanFilter heading_error_filter_
common::Status Reset() override
reset MPC Controller
std::shared_ptr< DependencyInjector > injector_
std::string Name() const override
MPC controller name
virtual ~MPCController()
destructor
common::Status Init(std::shared_ptr< DependencyInjector > injector) override
initialize MPC Controller
common::DigitalFilter digital_filter_
void UpdateState(SimpleMPCDebug *debug)
void ComputeLongitudinalErrors(const TrajectoryAnalyzer *trajectory, SimpleMPCDebug *debug)
A proportional-integral-derivative controller for speed and steering using defualt integral hold
process point query and conversion related to trajectory
Defines the Controller base class.
#define CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(name, base)
Defines the DigitalFilter class.
Functions to generate coefficients for digital filter.
Defines the MeanFilter class.
class register implement
Definition arena_queue.h:37
Defines the PIDBCController class.
Defines the TrajectoryAnalyzer class.