diff --git a/autoware_launch/config/control/trajectory_follower/default/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/default/lateral/mpc.param.yaml index 7820c562e8..2bbff7ff0e 100644 --- a/autoware_launch/config/control/trajectory_follower/default/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/default/lateral/mpc.param.yaml @@ -7,13 +7,13 @@ admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value # -- path smoothing -- - enable_path_smoothing: false # flag for path smoothing + enable_path_smoothing: true # flag for path smoothing path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) # -- trajectory extending -- - extend_trajectory_for_end_yaw_control: false # flag of trajectory extending for terminal yaw control + extend_trajectory_for_end_yaw_control: true # flag of trajectory extending for terminal yaw control # -- mpc optimization -- qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) @@ -23,19 +23,19 @@ mpc_weight_heading_error: 0.0 # heading error weight in matrix Q mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q mpc_weight_steering_input: 1.0 # steering error weight in matrix R - mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R + mpc_weight_steering_input_squared_vel: 1.00 # steering error * velocity weight in matrix R mpc_weight_lat_jerk: 0.1 # lateral jerk weight in matrix R mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R - mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point + mpc_low_curvature_weight_lat_error: 0.05 # lateral error weight in matrix Q in low curvature point mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point - mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point + mpc_low_curvature_weight_steering_input_squared_vel: 2.00 # steering error * velocity weight in matrix R in low curvature point mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point - mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03) + mpc_low_curvature_thresh_curvature: 0.03 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03) mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero @@ -45,22 +45,22 @@ # -- vehicle model -- vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics - input_delay: 0.24 # steering input delay time for delay compensation - vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] - steer_rate_lim_dps_list_by_curvature: [40.0, 50.0, 60.0] # steering angle rate limit list depending on curvature [deg/s] + input_delay: 0.1 # steering input delay time for delay compensation + vehicle_model_steer_tau: 0.1 # steering dynamics time constant (1d approximation) [s] + steer_rate_lim_dps_list_by_curvature: [20.0, 20.0, 20.0] # steering angle rate limit list depending on curvature [deg/s] curvature_list_for_steer_rate_lim: [0.001, 0.002, 0.01] # curvature list for steering angle rate limit interpolation in ascending order [/m] - steer_rate_lim_dps_list_by_velocity: [60.0, 50.0, 40.0] # steering angle rate limit list depending on velocity [deg/s] - velocity_list_for_steer_rate_lim: [10.0, 15.0, 20.0] # velocity list for steering angle rate limit interpolation in ascending order [m/s] - acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] + steer_rate_lim_dps_list_by_velocity: [18.0, 18.0, 15.0, 12.0, 5.0, 2.25, 1.5 , 1.5 , 1.5] # steering angle rate limit list depending on velocity [deg/s] [72, 72, 36, 18, 9, 3.6, 3.6] + velocity_list_for_steer_rate_lim: [0.0, 1.388, 2.778, 4.167, 5.556, 6.944, 8.333, 9.722, 11.11] # velocity list for steering angle rate limit interpolation in ascending order [m/s] + acceleration_limit: 1.0 # acceleration limit for trajectory velocity modification [m/ss] velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] # -- lowpass filter for noise reduction -- - steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz] + steering_lpf_cutoff_hz: 10.0 # cutoff frequency of lowpass filter for steering command [Hz] error_deriv_lpf_cutoff_hz: 5.0 # stop state: steering command is kept in the previous value in the stop state. - stop_state_entry_ego_speed: 0.001 - stop_state_entry_target_speed: 0.001 + stop_state_entry_ego_speed: 0.2 + stop_state_entry_target_speed: 0.2 converged_steer_rad: 0.1 keep_steer_control_until_converged: true new_traj_duration_time: 1.0