Skip to content

Commit 84ddac6

Browse files
masayukiainokosuke55pre-commit-ci[bot]asei-proxima
authored
feat(smart_mpc_trajectory_follower): enhance performance with LSTM and compensation, add compensator outside of MPC (autowarefoundation#7696)
* update smart mpc package * Update control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/data_collection_utils.py Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com> * Update control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/data_collection_utils.py Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com> * Update control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com> * Update control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com> * Update control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/pure_pursuit_gain_updater.py Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com> * Update control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com> * Update control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/pure_pursuit_gain_updater.py Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com> * style(pre-commit): autofix * modified commentout Signed-off-by: masayukiaino <masayuki.aino@proxima-ai-tech.com> * fixed duplicate conditions in run_auto_test.py Signed-off-by: masayukiaino <masayuki.aino@proxima-ai-tech.com> * doc: add description of kernel density estimation * rename some parameters and remove unnecessary parameters Signed-off-by: masayukiaino <masayuki.aino@proxima-ai-tech.com> * style(pre-commit): autofix * Fixed links in README.md * add a sample of trained models Signed-off-by: masayukiaino <masayuki.aino@proxima-ai-tech.com> * style(pre-commit): autofix * doc: remove japanese comment * update README.md * add whitespace * Update control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/pure_pursuit_gain_updater.py Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com> * Update control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com> * style(pre-commit): autofix * Some files were refactored Signed-off-by: masayukiaino <masayuki.aino@proxima-ai-tech.com> * const on member functions that do not change member variables Signed-off-by: masayukiaino <masayuki.aino@proxima-ai-tech.com> * bug fixed --------- Signed-off-by: masayukiaino <masayuki.aino@proxima-ai-tech.com> Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: asei-proxima <asei.inoue@proxima-ai-tech.com>
1 parent 3e07609 commit 84ddac6

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

47 files changed

+7614
-2403
lines changed
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
11
# Files generated when installing smart_mpc_trajectory_follower
22
build/
33
*.egg-info/
4+
.cspell.json

control/autoware_smart_mpc_trajectory_follower/README.md

+189-66
Large diffs are not rendered by default.
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,11 @@
11
mpc_parameter:
22
system:
3-
mode: mppi_ilqr # option: ilqr, mppi, mppi_ilqr
3+
mode: ilqr # option: ilqr, mppi, mppi_ilqr
44
mpc_setting:
55
ctrl_time_step: 0.03333
66
mpc_freq: 3
77
N: 50
8-
steer_ctrl_queue_size: 50
8+
steer_ctrl_queue_size: 17
99
steer_ctrl_queue_size_core: 15
1010
acc_ctrl_queue_size: 12
1111
nx_0: 6
@@ -14,21 +14,21 @@ mpc_parameter:
1414
cost_parameters:
1515
Q: [0.0, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
1616
Q_c: [1e+2, 1e+8, 1e+6, 1e+3, 1.0, 1.0, 1.0, 1.0]
17-
Q_f: [1e+2, 1e+8, 1e+2, 1e+8, 1.0, 1.0, 1.0, 1.0]
17+
Q_f: [1e+2, 1e+8, 1e+2, 1e+7, 1.0, 1.0, 1.0, 1.0]
1818
R: [10.0, 1000.0]
1919
acc_lim_weight: 100.0
2020
steer_lim_weight: 100.0
2121
acc_rate_lim_weight: 10000.0
2222
steer_rate_lim_weight: 10000.0
23-
min_steer_rate_transform_for_start: 0.01
24-
power_steer_rate_transform_for_start: 5
25-
coef_steer_rate_transform_for_start: 3.0
26-
min_loose_lateral_cost: 0.00001
27-
power_loose_lateral_cost: 10
28-
threshold_loose_lateral_cost: 0.2
29-
min_loose_yaw_cost: 0.00001
30-
power_loose_yaw_cost: 1
31-
threshold_loose_yaw_cost: 0.1
23+
vel_steer_cost_coef_table: [0.01, 0.01, 0.08, 0.36, 1.0]
24+
vel_steer_table: [0.01, 0.1, 0.2, 0.27, 0.33]
25+
lateral_cost_coef_table: [0.00001, 0.0005, 0.06, 0.27, 1.0]
26+
lateral_error_table: [0.01, 0.1, 0.15, 0.175, 0.2]
27+
yaw_cost_coef_table: [0.00001, 1.0]
28+
yaw_error_table: [0.00001, 0.1]
29+
steer_rate_cost_table: [10.0, 5.0, 1.0]
30+
curvature_table: [0.01, 0.03, 0.05]
31+
use_max_curvature: true
3232
ilqr:
3333
ls_step: 0.9
3434
max_iter_ls: 10
@@ -40,17 +40,31 @@ mpc_parameter:
4040
max_iter_mppi: 2
4141
sample_num: 100
4242
mppi_tol: 0.5
43-
mppi_step: 20
43+
mppi_step: 7
44+
pure_pursuit:
45+
acc_kp: 0.5
46+
lookahead_time: 3.0
47+
min_lookahead: 10.0
48+
steer_kp_param: 2.0
49+
steer_kd_param: 2.0
50+
naive_pure_pursuit:
51+
acc_kp: 0.5
52+
lookahead_coef: 1.0
53+
lookahead_intercept: 5.0
4454
preprocessing:
4555
reference_horizon: 50
4656
cap_pred_error: [0.5, 2.0]
4757
use_sg_for_nominal_inputs: true
4858
sg_deg_for_nominal_inputs: 0
4959
sg_window_size_for_nominal_inputs: 10
50-
to_be_deprecated:
51-
tighten_horizon: 20
52-
min_tighten_steer_rate: 1.0
53-
power_tighten_steer_rate_by_lateral_error: 1
54-
threshold_tighten_steer_rate_by_lateral_error: 0.05
55-
power_tighten_steer_rate_by_yaw_error: 1
56-
threshold_tighten_steer_rate_by_yaw_error: 0.05
60+
compensation:
61+
acc_fb_decay: 0.1
62+
acc_fb_gain: 1.0
63+
acc_fb_sec_order_ratio: 1.0
64+
max_error_acc: 1e-3
65+
steer_fb_decay: 0.001
66+
steer_fb_gain: 1.0
67+
steer_fb_sec_order_ratio: 1.0
68+
max_error_steer: 1e-2
69+
limit:
70+
read_limit_file: false

control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml

+34
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,8 @@ trained_model_parameter:
55
max_train_data_size: 10000
66
error_decay: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
77
use_trained_model_diff: true
8+
minimum_steer_diff: 0.03
9+
reflect_only_poly_diff: false
810
use_sg_for_trained_model_diff: true
911
sg_deg_for_trained_model_diff: 0
1012
sg_window_size_for_trained_model_diff: 25
@@ -17,6 +19,12 @@ trained_model_parameter:
1719
use_theta_noise: false
1820
use_acc_noise: false
1921
use_steer_noise: false
22+
memory_for_training:
23+
use_memory_for_training: true
24+
use_memory_diff: true
25+
use_sg_for_memory_diff: true
26+
sg_deg_for_memory_diff: 0
27+
sg_window_size_for_memory_diff: 10
2028
smoothing:
2129
acc_sigma_for_learning: 5.0
2230
steer_sigma_for_learning: 5.0
@@ -28,3 +36,29 @@ trained_model_parameter:
2836
theta_out_sigma_for_learning: 10.0
2937
acc_out_sigma_for_learning: 5.0
3038
steer_out_sigma_for_learning: 5.0
39+
normalize:
40+
vel_normalize: 0.2
41+
acc_normalize: 1.0
42+
steer_normalize: 3.0
43+
weight:
44+
NN_x_weight: 1.0
45+
NN_y_weight: 1.0
46+
NN_v_weight: 1.0
47+
NN_yaw_weight: 2.0
48+
NN_acc_weight: 1.0
49+
NN_steer_weight: 2.0
50+
NN_x_weight_diff: 0.001
51+
NN_y_weight_diff: 0.01
52+
NN_v_weight_diff: 0.001
53+
NN_yaw_weight_diff: 0.1
54+
NN_acc_weight_diff: 0.1
55+
NN_steer_weight_diff: 0.1
56+
NN_x_weight_two_diff: 0.0001
57+
NN_y_weight_two_diff: 0.001
58+
NN_v_weight_two_diff: 0.00001
59+
NN_yaw_weight_two_diff: 0.01
60+
NN_acc_weight_two_diff: 0.01
61+
NN_steer_weight_two_diff: 0.01
62+
finalize_x_weight: 10.0
63+
finalize_y_weight: 10.0
64+
finalize_v_weight: 10.0

control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/.gitignore

+1
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@ train_drive_*.png
88
test_feedforward_*/
99
test_pure_pursuit_*/
1010
test_python_*/
11+
test_*/
1112
python_sim_log_*/
1213
sim_setting.json
1314
auto_test_result_*.csv

0 commit comments

Comments
 (0)