From a505fb24dfa40be07f852bacf22c8adf88c1f9ff Mon Sep 17 00:00:00 2001 From: Shumpei Wakabayashi <42209144+shmpwk@users.noreply.github.com> Date: Thu, 8 Aug 2024 07:56:49 +0900 Subject: [PATCH] chore(control): tune 60kmph param (#895) * chore: 20240625 60kmph Signed-off-by: Shumpei Wakabayashi * chore Signed-off-by: Shumpei Wakabayashi * 202406251140 Signed-off-by: shumpei.wakabayashi * 202406251144 Signed-off-by: shumpei.wakabayashi * 202406251125 Signed-off-by: shumpei.wakabayashi * 202406251310 Signed-off-by: shumpei.wakabayashi * 202406251415 Signed-off-by: shumpei.wakabayashi * 202406251454 Signed-off-by: shumpei.wakabayashi * 202406251627 Signed-off-by: shumpei.wakabayashi * 202406260020 Signed-off-by: Shumpei Wakabayashi * Update autoware_launch/config/control/trajectory_follower/9/lateral/mpc.param.yaml * Update autoware_launch/config/control/trajectory_follower/9/lateral/mpc.param.yaml * 2 Signed-off-by: Shumpei Wakabayashi * 2 Signed-off-by: Shumpei Wakabayashi --------- Signed-off-by: Shumpei Wakabayashi Signed-off-by: shumpei.wakabayashi --- .../2/lateral/mpc.param.yaml | 8 ++++---- .../9/lateral/mpc.param.yaml | 8 ++++---- .../vehicle_cmd_gate.param.yaml | 18 +++++++++--------- .../scenario_planning/common/common.param.yaml | 10 +++++----- 4 files changed, 22 insertions(+), 22 deletions(-) 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 f66fe2a33b..8480d3849b 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 @@ -24,10 +24,10 @@ 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.1 # lateral jerk weight in matrix R + mpc_weight_lat_jerk: 0.075 # 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_lat_error: 0.1 # 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 @@ -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: [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] + 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, 1.5, 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, 12.5, 13.89, 15.28, 16.67] # 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 1287392647..888c9489d6 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 @@ -24,10 +24,10 @@ 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.075 # lateral jerk weight in matrix R + mpc_weight_lat_jerk: 0.075 # 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.5 # lateral error weight in matrix Q in low curvature point + mpc_low_curvature_weight_lat_error: 0.1 # 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 @@ -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: [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] + 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, 1.5, 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, 12.5, 13.89, 15.28, 16.67] # 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 a9f3b04e79..3477739e9d 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 @@ -16,16 +16,16 @@ stop_check_duration: 1.0 nominal: vel_lim: 25.0 - reference_speed_points: [0.1, 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: [10.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0] - lon_jerk_lim: [90.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] + reference_speed_points: [0.1, 1.388, 2.778, 4.167, 5.556, 6.944, 8.333, 9.722, 11.11, 12.5, 13.89, 15.28, 16.8] + steer_lim: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.025, 0.025, 0.025, 0.025, 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, 0.035, 0.035, 0.035, 0.035] + lon_acc_lim: [10.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0, 6.0] + lon_jerk_lim: [90.0, 6.0, 6.0, 6.0, 6.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, 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, 2.5, 2.5, 2.5, 2.5] + actual_steer_diff_lim: [1.0, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8, 0.8] on_transition: - vel_lim: 50.0 + vel_lim: 60.0 reference_speed_points: [20.0, 30.0] steer_lim: [1.0, 0.8] steer_rate_lim: [1.0, 0.8] diff --git a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml index e4e2e56783..84c42211e6 100644 --- a/autoware_launch/config/planning/scenario_planning/common/common.param.yaml +++ b/autoware_launch/config/planning/scenario_planning/common/common.param.yaml @@ -1,17 +1,17 @@ /**: ros__parameters: - max_vel: 11.1 # max velocity limit [m/s] + max_vel: 16.8 # max velocity limit [m/s] # constraints param for normal driving normal: min_acc: -0.5 # min deceleration [m/ss] - max_acc: 1.0 # max acceleration [m/ss] + max_acc: 1.5 # max acceleration [m/ss] min_jerk: -0.5 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] + max_jerk: 1.5 # max jerk [m/sss] # constraints to be observed limit: min_acc: -2.5 # min deceleration limit [m/ss] - max_acc: 1.0 # max acceleration limit [m/ss] + max_acc: 2.0 # max acceleration limit [m/ss] min_jerk: -1.5 # min jerk limit [m/sss] - max_jerk: 1.5 # max jerk limit [m/sss] + max_jerk: 2.0 # max jerk limit [m/sss]