From 9b362206a70e10a4e68efc08953a48edbec46eeb Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 30 Oct 2023 14:41:30 +0900 Subject: [PATCH] feat: adjust the steering rate and accel limits (#686) Signed-off-by: tomoya.kimura --- .../trajectory_follower/1/lateral/mpc.param.yaml | 4 ++-- .../10/lateral/mpc.param.yaml | 4 ++-- .../trajectory_follower/2/lateral/mpc.param.yaml | 4 ++-- .../trajectory_follower/3/lateral/mpc.param.yaml | 4 ++-- .../trajectory_follower/4/lateral/mpc.param.yaml | 4 ++-- .../trajectory_follower/5/lateral/mpc.param.yaml | 4 ++-- .../trajectory_follower/6/lateral/mpc.param.yaml | 4 ++-- .../trajectory_follower/7/lateral/mpc.param.yaml | 4 ++-- .../trajectory_follower/9/lateral/mpc.param.yaml | 4 ++-- .../default/lateral/mpc.param.yaml | 4 ++-- .../vehicle_cmd_gate/vehicle_cmd_gate.param.yaml | 16 ++++++++-------- 11 files changed, 28 insertions(+), 28 deletions(-) diff --git a/autoware_launch/config/control/trajectory_follower/1/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/1/lateral/mpc.param.yaml index 2c00852e69..bc4b336a30 100644 --- a/autoware_launch/config/control/trajectory_follower/1/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/1/lateral/mpc.param.yaml @@ -49,8 +49,8 @@ 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: [20.0, 20.0, 20.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] + 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] diff --git a/autoware_launch/config/control/trajectory_follower/10/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/10/lateral/mpc.param.yaml index 2c00852e69..bc4b336a30 100644 --- a/autoware_launch/config/control/trajectory_follower/10/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/10/lateral/mpc.param.yaml @@ -49,8 +49,8 @@ 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: [20.0, 20.0, 20.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] + 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] diff --git a/autoware_launch/config/control/trajectory_follower/2/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/2/lateral/mpc.param.yaml index 2c00852e69..bc4b336a30 100644 --- a/autoware_launch/config/control/trajectory_follower/2/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/2/lateral/mpc.param.yaml @@ -49,8 +49,8 @@ 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: [20.0, 20.0, 20.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] + 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] diff --git a/autoware_launch/config/control/trajectory_follower/3/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/3/lateral/mpc.param.yaml index 2c00852e69..bc4b336a30 100644 --- a/autoware_launch/config/control/trajectory_follower/3/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/3/lateral/mpc.param.yaml @@ -49,8 +49,8 @@ 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: [20.0, 20.0, 20.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] + 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] diff --git a/autoware_launch/config/control/trajectory_follower/4/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/4/lateral/mpc.param.yaml index fad9a654bc..d85c9ae81a 100644 --- a/autoware_launch/config/control/trajectory_follower/4/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/4/lateral/mpc.param.yaml @@ -49,8 +49,8 @@ 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: [20.0, 20.0, 20.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] + 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] diff --git a/autoware_launch/config/control/trajectory_follower/5/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/5/lateral/mpc.param.yaml index 303cc344d7..6fba1855b9 100644 --- a/autoware_launch/config/control/trajectory_follower/5/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/5/lateral/mpc.param.yaml @@ -49,8 +49,8 @@ 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: [20.0, 20.0, 20.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] + 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] diff --git a/autoware_launch/config/control/trajectory_follower/6/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/6/lateral/mpc.param.yaml index 2c00852e69..bc4b336a30 100644 --- a/autoware_launch/config/control/trajectory_follower/6/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/6/lateral/mpc.param.yaml @@ -49,8 +49,8 @@ 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: [20.0, 20.0, 20.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] + 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] diff --git a/autoware_launch/config/control/trajectory_follower/7/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/7/lateral/mpc.param.yaml index 2c00852e69..bc4b336a30 100644 --- a/autoware_launch/config/control/trajectory_follower/7/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/7/lateral/mpc.param.yaml @@ -49,8 +49,8 @@ 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: [20.0, 20.0, 20.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] + 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] diff --git a/autoware_launch/config/control/trajectory_follower/9/lateral/mpc.param.yaml b/autoware_launch/config/control/trajectory_follower/9/lateral/mpc.param.yaml index 2c00852e69..bc4b336a30 100644 --- a/autoware_launch/config/control/trajectory_follower/9/lateral/mpc.param.yaml +++ b/autoware_launch/config/control/trajectory_follower/9/lateral/mpc.param.yaml @@ -49,8 +49,8 @@ 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: [20.0, 20.0, 20.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] + 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] 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 68fba6fbed..ed67097364 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 @@ -49,8 +49,8 @@ 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: [20.0, 20.0, 20.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] + 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] diff --git a/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml index 191e894622..5d2c16f8a3 100644 --- a/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml +++ b/autoware_launch/config/control/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml @@ -14,14 +14,14 @@ stop_check_duration: 1.0 nominal: vel_lim: 25.0 - reference_speed_points: [20.0, 30.0] - steer_lim: [1.0, 0.8] - steer_rate_lim: [1.0, 0.8] - lon_acc_lim: [5.0, 4.0] - lon_jerk_lim: [5.0, 4.0] - lat_acc_lim: [5.0, 4.0] - lat_jerk_lim: [7.0, 6.0] - actual_steer_diff_lim: [1.0, 0.8] + reference_speed_points: [0.0, 1.388, 2.778, 4.167, 5.556, 6.944, 8.333, 9.722, 11.11] + steer_lim: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.025, 0.025] + steer_rate_lim: [0.5 , 0.5 , 0.4 , 0.3 , 0.15 ,0.07 , 0.05, 0.035, 0.035] + lon_acc_lim: [100.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0] + lon_jerk_lim: [100.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0] + lat_acc_lim: [5.0, 4.0, 4.0, 4.0, 4.0, 4.0, 4.0, 4.0, 4.0] + lat_jerk_lim: [0.869, 0.869, 0.869, 1.47, 1.74, 1.36, 1.30, 1.78, 2.32] + actual_steer_diff_lim: [1.0, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8] on_transition: vel_lim: 50.0 reference_speed_points: [20.0, 30.0]