Limit aggressive steering based on vehicle speed (#206)

modules/control/conf/lincoln.pb.txt

@@ -30,6 +30,7 @@ lat_controller_conf {
   steer_transmission_ratio: 16
   steer_single_direction_max_degree: 470
   max_iteration: 150
+  max_lateral_acceleration: 5.0
 }
 lon_controller_conf {
   ts: 0.01

modules/control/controller/lat_controller.cc

@@ -98,6 +98,8 @@ bool LatController::LoadControlConf(const ControlConf* control_conf) {
       control_conf->lat_controller_conf().steer_transmission_ratio();
   steer_single_direction_max_degree_ =
       control_conf->lat_controller_conf().steer_single_direction_max_degree();
+  max_lat_acc_ = control_conf->lat_controller_conf().max_lateral_acceleration();
+
   double mass_fl = control_conf->lat_controller_conf().mass_fl();
   double mass_fr = control_conf->lat_controller_conf().mass_fr();
   double mass_rl = control_conf->lat_controller_conf().mass_rl();
@@ -265,11 +267,21 @@ Status LatController::ComputeControlCommand(
 
   double steer_angle_feedforward = ComputeFeedForward(debug->curvature());
   double steer_angle = steer_angle_feedback + steer_angle_feedforward;
+
   // Clamp the steer angle to -100.0 to 100.0
   steer_angle = apollo::common::math::Clamp(steer_angle, -100.0, 100.0);
 
-  steer_angle = digital_filter_.Filter(steer_angle);
-  cmd->set_steering_target(steer_angle);
+  double steer_limit = std::atan(max_lat_acc_ * wheelbase_ /
+       (vehicle_state_.linear_velocity() * vehicle_state_.linear_velocity())) *
+       steer_transmission_ratio_ * 180 / M_PI /
+       steer_single_direction_max_degree_;
+
+  // Clamp the steer angle
+  double steer_angle_limited = apollo::common::math::Clamp(steer_angle,
+                               -steer_limit, steer_limit);
+
+  steer_angle_limited = digital_filter_.Filter(steer_angle_limited);
+  cmd->set_steering_target(steer_angle_limited);
   cmd->set_steering_rate(FLAGS_steer_angle_rate);
 
   // compute extra information for logging and debugging
@@ -303,6 +315,7 @@ Status LatController::ComputeControlCommand(
   debug->set_steer_angle_feedback(steer_angle_feedback);
   debug->set_steering_position(chassis->steering_percentage());
   debug->set_ref_speed(vehicle_state_.linear_velocity());
+  debug->set_steer_angle_limited(steer_angle_limited);
   ProcessLogs(debug, chassis);
   return Status::OK();
 }

modules/control/controller/lat_controller.h

@@ -150,6 +150,9 @@ class LatController : public Controller {
   // the maximum turn of steer
   double steer_single_direction_max_degree_ = 0.0;
 
+  // limit steering to maximum theoretical lateral acceleration
+  double max_lat_acc_ = 0.0;
+
   // number of control cycles look ahead (preview controller)
   int preview_window_ = 0;
   // number of states without previews, includes

modules/control/proto/control_cmd.proto

@@ -100,6 +100,7 @@ message SimpleLateralDebug {
   optional double steer_angle_feedback = 14;
   optional double steering_position = 15;
   optional double ref_speed = 16;
+  optional double steer_angle_limited = 17;
 }
 
 message InputDebug {

modules/control/proto/lat_controller_conf.proto

@@ -22,4 +22,5 @@ message LatControllerConf {
   optional int32 steer_transmission_ratio = 14;
   optional int32 steer_single_direction_max_degree = 15;  // in degree
   optional int32 max_iteration = 16;  // maximum iteration for lqr solve
+  optional double max_lateral_acceleration = 17;  // limit aggressive steering
 }