From 2c5de0c496e99e575cfa4ea5084bec1607e5123f Mon Sep 17 00:00:00 2001 From: sho yoshida <73815070+SSSyoshida@users.noreply.github.com> Date: Mon, 27 Nov 2023 11:20:06 +0900 Subject: [PATCH] add awsim_jpt parameter from JPT2 (#702) --- .../awsim_jpt/lateral/mpc.param.yaml | 76 +++++++++++++++++++ .../awsim_jpt/longitudinal/pid.param.yaml | 74 ++++++++++++++++++ 2 files changed, 150 insertions(+) create mode 100644 autoware_launch/config/control/trajectory_follower/awsim_jpt/lateral/mpc.param.yaml create mode 100644 autoware_launch/config/control/trajectory_follower/awsim_jpt/longitudinal/pid.param.yaml diff --git a/autoware_launch/config/control/trajectory_follower/awsim_jpt/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/awsim_jpt/lateral/mpc.param.yaml new file mode 100644 index 0000000000..bc4b336a30 --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/awsim_jpt/lateral/mpc.param.yaml @@ -0,0 +1,76 @@ +/**: + ros__parameters: + # -- system -- + traj_resample_dist: 0.1 # path resampling interval [m] + use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) + admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value + admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value + + # -- 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: true # flag of trajectory extending for terminal yaw control + + # -- mpc optimization -- + qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) + mpc_prediction_horizon: 50 # prediction horizon step + mpc_prediction_dt: 0.1 # prediction horizon period [s] + mpc_weight_lat_error: 0.2 # lateral error weight in matrix Q + mpc_weight_heading_error: 0.0 # heading error weight in matrix Q + mpc_weight_heading_error_squared_vel: 0.03 # 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: 1.00 # steering error * velocity weight in matrix R + mpc_weight_lat_jerk: 0.0 # 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.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.03 # 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: 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.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 + mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration + mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing + mpc_min_prediction_length: 5.0 # minimum prediction length + + # -- vehicle model -- + vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics + 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: [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: 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.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 + new_traj_end_dist: 0.3 + mpc_converged_threshold_rps: 0.01 # threshold of mpc convergence check [rad/s] + + # steer offset + steering_offset: + enable_auto_steering_offset_removal: true + update_vel_threshold: 5.56 + update_steer_threshold: 0.035 + average_num: 1000 + steering_offset_limit: 0.02 diff --git a/autoware_launch/config/control/trajectory_follower/awsim_jpt/longitudinal/pid.param.yaml b/autoware_launch/config/control/trajectory_follower/awsim_jpt/longitudinal/pid.param.yaml new file mode 100644 index 0000000000..8c98424e6a --- /dev/null +++ b/autoware_launch/config/control/trajectory_follower/awsim_jpt/longitudinal/pid.param.yaml @@ -0,0 +1,74 @@ +/**: + ros__parameters: + delay_compensation_time: 0.40 + + enable_smooth_stop: true + enable_overshoot_emergency: false + enable_large_tracking_error_emergency: false + enable_slope_compensation: true + enable_keep_stopped_until_steer_convergence: true + + # state transition + drive_state_stop_dist: 0.5 + drive_state_offset_stop_dist: 1.0 + stopping_state_stop_dist: 0.5 + stopped_state_entry_duration_time: 0.1 + stopped_state_entry_vel: 0.01 + stopped_state_entry_acc: 0.1 + emergency_state_overshoot_stop_dist: 1.5 + emergency_state_traj_trans_dev: 3.0 + emergency_state_traj_rot_dev: 0.7854 + + # drive state + kp: 2.0 + ki: 0.02 + kd: 0.0 + max_out: 1.0 + min_out: -5.0 + max_p_effort: 0.5 + min_p_effort: -5.0 + max_i_effort: 0.3 + min_i_effort: -0.3 + max_d_effort: 0.0 + min_d_effort: 0.0 + lpf_vel_error_gain: 0.9 + current_vel_threshold_pid_integration: 0.5 + enable_brake_keeping_before_stop: false + brake_keeping_acc: -0.2 + + # smooth stop state + smooth_stop_max_strong_acc: -0.8 + smooth_stop_min_strong_acc: -1.3 + smooth_stop_weak_acc: -0.6 + smooth_stop_weak_stop_acc: -0.8 + smooth_stop_strong_stop_acc: -3.4 + smooth_stop_max_fast_vel: 0.5 + smooth_stop_min_running_vel: 0.01 + smooth_stop_min_running_acc: 0.01 + smooth_stop_weak_stop_time: 0.8 + smooth_stop_weak_stop_dist: -0.3 + smooth_stop_strong_stop_dist: -0.5 + + # stopped state + stopped_vel: 0.0 + stopped_acc: -3.4 + stopped_jerk: -5.0 + + # emergency state + emergency_vel: 0.0 + emergency_acc: -5.0 + emergency_jerk: -3.0 + + # acceleration limit + max_acc: 3.0 + min_acc: -5.0 + + # jerk limit + max_jerk: 3.5 + min_jerk: -5.0 + + # pitch + use_trajectory_for_pitch_calculation: false + lpf_pitch_gain: 0.95 + max_pitch_rad: 0.1 + min_pitch_rad: -0.1