From eb1094d12f798168c6cccaf6e1a61ee6b60cfd5d Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 17 Oct 2023 20:42:09 +0900 Subject: [PATCH] fix(mpc): update steering rate limit in mpc (#672) Signed-off-by: Takamasa Horibe --- .../config/control/trajectory_follower/1/lateral/mpc.param.yaml | 2 +- .../control/trajectory_follower/10/lateral/mpc.param.yaml | 2 +- .../config/control/trajectory_follower/2/lateral/mpc.param.yaml | 2 +- .../config/control/trajectory_follower/3/lateral/mpc.param.yaml | 2 +- .../config/control/trajectory_follower/4/lateral/mpc.param.yaml | 2 +- .../config/control/trajectory_follower/5/lateral/mpc.param.yaml | 2 +- .../config/control/trajectory_follower/6/lateral/mpc.param.yaml | 2 +- .../config/control/trajectory_follower/7/lateral/mpc.param.yaml | 2 +- .../config/control/trajectory_follower/9/lateral/mpc.param.yaml | 2 +- .../control/trajectory_follower/default/lateral/mpc.param.yaml | 2 +- 10 files changed, 10 insertions(+), 10 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 1abc587490..e4c6245242 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,7 +49,7 @@ 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: [11.25, 11.25, 9. , 6.75, 4.5 , 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] + steer_rate_lim_dps_list_by_velocity: [15.0, 15.0, 13.0, 10.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 1abc587490..e4c6245242 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,7 +49,7 @@ 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: [11.25, 11.25, 9. , 6.75, 4.5 , 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] + steer_rate_lim_dps_list_by_velocity: [15.0, 15.0, 13.0, 10.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 1abc587490..e4c6245242 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,7 +49,7 @@ 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: [11.25, 11.25, 9. , 6.75, 4.5 , 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] + steer_rate_lim_dps_list_by_velocity: [15.0, 15.0, 13.0, 10.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 1abc587490..e4c6245242 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,7 +49,7 @@ 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: [11.25, 11.25, 9. , 6.75, 4.5 , 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] + steer_rate_lim_dps_list_by_velocity: [15.0, 15.0, 13.0, 10.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 362698de73..5d2100033a 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,7 +49,7 @@ 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: [11.25, 11.25, 9. , 6.75, 4.5 , 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] + steer_rate_lim_dps_list_by_velocity: [15.0, 15.0, 13.0, 10.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 6d96103a22..a863e5f207 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,7 +49,7 @@ 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: [11.25, 11.25, 9. , 6.75, 4.5 , 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] + steer_rate_lim_dps_list_by_velocity: [15.0, 15.0, 13.0, 10.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 1abc587490..e4c6245242 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,7 +49,7 @@ 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: [11.25, 11.25, 9. , 6.75, 4.5 , 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] + steer_rate_lim_dps_list_by_velocity: [15.0, 15.0, 13.0, 10.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 1abc587490..e4c6245242 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,7 +49,7 @@ 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: [11.25, 11.25, 9. , 6.75, 4.5 , 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] + steer_rate_lim_dps_list_by_velocity: [15.0, 15.0, 13.0, 10.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 1abc587490..e4c6245242 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,7 +49,7 @@ 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: [11.25, 11.25, 9. , 6.75, 4.5 , 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] + steer_rate_lim_dps_list_by_velocity: [15.0, 15.0, 13.0, 10.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 ed86e8dec5..c8aab56c77 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,7 +49,7 @@ 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: [11.25, 11.25, 9. , 6.75, 4.5 , 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] + steer_rate_lim_dps_list_by_velocity: [15.0, 15.0, 13.0, 10.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]