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>

Author: 4 people

control/autoware_smart_mpc_trajectory_follower/.gitignore

@@ -1,3 +1,4 @@
 # Files generated when installing smart_mpc_trajectory_follower
 build/
 *.egg-info/
+.cspell.json

control/autoware_smart_mpc_trajectory_follower/README.md

@@ -1,11 +1,11 @@
 mpc_parameter:
   system:
-    mode: mppi_ilqr # option: ilqr, mppi, mppi_ilqr
+    mode: ilqr # option: ilqr, mppi, mppi_ilqr
   mpc_setting:
     ctrl_time_step: 0.03333
     mpc_freq: 3
     N: 50
-    steer_ctrl_queue_size: 50
+    steer_ctrl_queue_size: 17
     steer_ctrl_queue_size_core: 15
     acc_ctrl_queue_size: 12
     nx_0: 6
@@ -14,21 +14,21 @@ mpc_parameter:
   cost_parameters:
     Q: [0.0, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
     Q_c: [1e+2, 1e+8, 1e+6, 1e+3, 1.0, 1.0, 1.0, 1.0]
-    Q_f: [1e+2, 1e+8, 1e+2, 1e+8, 1.0, 1.0, 1.0, 1.0]
+    Q_f: [1e+2, 1e+8, 1e+2, 1e+7, 1.0, 1.0, 1.0, 1.0]
     R: [10.0, 1000.0]
     acc_lim_weight: 100.0
     steer_lim_weight: 100.0
     acc_rate_lim_weight: 10000.0
     steer_rate_lim_weight: 10000.0
-    min_steer_rate_transform_for_start: 0.01
-    power_steer_rate_transform_for_start: 5
-    coef_steer_rate_transform_for_start: 3.0
-    min_loose_lateral_cost: 0.00001
-    power_loose_lateral_cost: 10
-    threshold_loose_lateral_cost: 0.2
-    min_loose_yaw_cost: 0.00001
-    power_loose_yaw_cost: 1
-    threshold_loose_yaw_cost: 0.1
+    vel_steer_cost_coef_table: [0.01, 0.01, 0.08, 0.36, 1.0]
+    vel_steer_table: [0.01, 0.1, 0.2, 0.27, 0.33]
+    lateral_cost_coef_table: [0.00001, 0.0005, 0.06, 0.27, 1.0]
+    lateral_error_table: [0.01, 0.1, 0.15, 0.175, 0.2]
+    yaw_cost_coef_table: [0.00001, 1.0]
+    yaw_error_table: [0.00001, 0.1]
+    steer_rate_cost_table: [10.0, 5.0, 1.0]
+    curvature_table: [0.01, 0.03, 0.05]
+    use_max_curvature: true
   ilqr:
     ls_step: 0.9
     max_iter_ls: 10
@@ -40,17 +40,31 @@ mpc_parameter:
     max_iter_mppi: 2
     sample_num: 100
     mppi_tol: 0.5
-    mppi_step: 20
+    mppi_step: 7
+  pure_pursuit:
+    acc_kp: 0.5
+    lookahead_time: 3.0
+    min_lookahead: 10.0
+    steer_kp_param: 2.0
+    steer_kd_param: 2.0
+  naive_pure_pursuit:
+    acc_kp: 0.5
+    lookahead_coef: 1.0
+    lookahead_intercept: 5.0
   preprocessing:
     reference_horizon: 50
     cap_pred_error: [0.5, 2.0]
     use_sg_for_nominal_inputs: true
     sg_deg_for_nominal_inputs: 0
     sg_window_size_for_nominal_inputs: 10
-  to_be_deprecated:
-    tighten_horizon: 20
-    min_tighten_steer_rate: 1.0
-    power_tighten_steer_rate_by_lateral_error: 1
-    threshold_tighten_steer_rate_by_lateral_error: 0.05
-    power_tighten_steer_rate_by_yaw_error: 1
-    threshold_tighten_steer_rate_by_yaw_error: 0.05
+  compensation:
+    acc_fb_decay: 0.1
+    acc_fb_gain: 1.0
+    acc_fb_sec_order_ratio: 1.0
+    max_error_acc: 1e-3
+    steer_fb_decay: 0.001
+    steer_fb_gain: 1.0
+    steer_fb_sec_order_ratio: 1.0
+    max_error_steer: 1e-2
+  limit:
+    read_limit_file: false

control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml

@@ -5,6 +5,8 @@ trained_model_parameter:
     max_train_data_size: 10000
     error_decay: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
     use_trained_model_diff: true
+    minimum_steer_diff: 0.03
+    reflect_only_poly_diff: false
     use_sg_for_trained_model_diff: true
     sg_deg_for_trained_model_diff: 0
     sg_window_size_for_trained_model_diff: 25
@@ -17,6 +19,12 @@ trained_model_parameter:
     use_theta_noise: false
     use_acc_noise: false
     use_steer_noise: false
+  memory_for_training:
+    use_memory_for_training: true
+    use_memory_diff: true
+    use_sg_for_memory_diff: true
+    sg_deg_for_memory_diff: 0
+    sg_window_size_for_memory_diff: 10
   smoothing:
     acc_sigma_for_learning: 5.0
     steer_sigma_for_learning: 5.0
@@ -28,3 +36,29 @@ trained_model_parameter:
     theta_out_sigma_for_learning: 10.0
     acc_out_sigma_for_learning: 5.0
     steer_out_sigma_for_learning: 5.0
+  normalize:
+    vel_normalize: 0.2
+    acc_normalize: 1.0
+    steer_normalize: 3.0
+  weight:
+    NN_x_weight: 1.0
+    NN_y_weight: 1.0
+    NN_v_weight: 1.0
+    NN_yaw_weight: 2.0
+    NN_acc_weight: 1.0
+    NN_steer_weight: 2.0
+    NN_x_weight_diff: 0.001
+    NN_y_weight_diff: 0.01
+    NN_v_weight_diff: 0.001
+    NN_yaw_weight_diff: 0.1
+    NN_acc_weight_diff: 0.1
+    NN_steer_weight_diff: 0.1
+    NN_x_weight_two_diff: 0.0001
+    NN_y_weight_two_diff: 0.001
+    NN_v_weight_two_diff: 0.00001
+    NN_yaw_weight_two_diff: 0.01
+    NN_acc_weight_two_diff: 0.01
+    NN_steer_weight_two_diff: 0.01
+    finalize_x_weight: 10.0
+    finalize_y_weight: 10.0
+    finalize_v_weight: 10.0

control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml

@@ -8,6 +8,7 @@ train_drive_*.png
 test_feedforward_*/
 test_pure_pursuit_*/
 test_python_*/
+test_*/
 python_sim_log_*/
 sim_setting.json
 auto_test_result_*.csv