単純なモデル予測制御を実装しました。
予測モデルは、差動二輪ロボットをベースにしています。
最適化のためのソルバーは、ceres-solver/ceres-solverを使用しています。
sudo apt install cmake gnuplot libgoogle-glog-dev libeigen3-dev libceres-dev
git clone [email protected]:uhobeike/simple_model_predictive_control.git
cd simple_model_predictive_control
mkdir build && cd build
cmake .. && make && cd -
./build/simple_mpc
iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time
0 1.032580e+03 0.00e+00 1.00e+00 0.00e+00 0.00e+00 1.00e+04 0 3.27e-03 3.37e-03
1 7.124342e+01 9.61e+02 3.14e+00 3.46e-01 9.31e-01 2.78e+04 1 6.00e-03 9.42e-03
2 3.173822e+01 3.95e+01 3.25e+00 6.66e-01 5.55e-01 2.79e+04 1 6.21e-03 1.57e-02
3 1.329102e+00 3.04e+01 3.57e+00 4.33e-01 9.58e-01 8.36e+04 1 7.13e-03 2.28e-02
4 1.512905e-01 1.18e+00 1.73e+00 1.94e-01 8.86e-01 1.55e+05 1 5.87e-03 2.87e-02
5 1.240095e-02 1.39e-01 2.00e-01 1.65e-01 9.18e-01 3.73e+05 1 6.47e-03 3.52e-02
6 3.172385e-03 9.23e-03 1.54e-01 1.31e-01 7.44e-01 4.22e+05 1 5.81e-03 4.10e-02
7 3.842388e-04 2.79e-03 2.31e-02 2.10e-01 8.79e-01 7.47e+05 1 5.52e-03 4.66e-02
8 1.774499e-04 2.07e-04 9.84e-03 1.92e-01 5.38e-01 7.47e+05 1 6.68e-03 5.33e-02
9 1.773121e-04 1.38e-07 9.84e-03 6.62e-03 7.76e-04 3.74e+05 1 8.64e-03 6.19e-02
10 1.773056e-04 1.44e-07 9.84e-03 6.63e-03 8.13e-04 9.34e+04 1 9.77e-03 7.17e-02
11 1.772665e-04 1.83e-07 9.74e-03 6.70e-03 1.03e-03 4.69e+04 1 1.14e-02 8.32e-02
12 1.765845e-04 6.82e-07 9.18e-03 3.95e-02 3.85e-03 2.37e+04 1 1.01e-02 9.33e-02
13 1.743448e-04 2.24e-06 9.26e-03 3.60e-02 1.27e-02 1.23e+04 1 8.72e-03 1.02e-01
14 1.728743e-04 1.47e-06 9.40e-03 3.11e-02 8.43e-03 6.31e+03 1 8.28e-03 1.10e-01
15 1.721621e-04 7.12e-07 9.55e-03 2.77e-02 4.12e-03 3.19e+03 1 8.24e-03 1.19e-01
16 1.713394e-04 8.23e-07 9.67e-03 2.57e-02 4.78e-03 1.62e+03 1 8.30e-03 1.27e-01
17 1.677175e-04 3.62e-06 1.00e-02 1.18e-01 2.11e-02 8.62e+02 1 5.56e-03 1.32e-01
18 1.721724e-04 -4.45e-06 1.00e-02 1.38e-01 -2.66e-02 4.31e+02 1 2.94e-02 1.62e-01
19 1.293387e-04 3.84e-05 8.21e-03 1.38e-01 2.29e-01 3.72e+02 1 5.75e-03 1.68e-01
20 1.248041e-04 4.53e-06 8.13e-03 3.96e-01 3.51e-02 2.06e+02 1 5.72e-03 1.73e-01
21 8.394738e-05 4.09e-05 5.91e-03 5.42e-02 3.28e-01 1.98e+02 1 5.68e-03 1.79e-01
22 7.593300e-05 8.01e-06 5.54e-03 4.08e-02 9.59e-02 1.30e+02 1 6.00e-03 1.85e-01
23 5.214254e-05 2.38e-05 4.09e-03 2.30e-02 3.15e-01 1.23e+02 1 5.92e-03 1.91e-01
24 4.569738e-05 6.45e-06 3.73e-03 1.32e-02 1.25e-01 8.67e+01 1 6.03e-03 1.97e-01
25 3.267195e-05 1.30e-05 2.84e-03 8.74e-03 2.88e-01 8.06e+01 1 6.14e-03 2.03e-01
26 2.828736e-05 4.38e-06 2.55e-03 7.43e-03 1.36e-01 5.82e+01 1 6.03e-03 2.09e-01
27 2.048230e-05 7.81e-06 1.96e-03 6.24e-03 2.81e-01 5.36e+01 1 5.81e-03 2.15e-01
28 1.760299e-05 2.88e-06 1.75e-03 6.21e-03 1.44e-01 3.94e+01 1 5.90e-03 2.21e-01
29 1.274234e-05 4.86e-06 1.33e-03 5.55e-03 2.84e-01 3.65e+01 1 5.76e-03 2.27e-01
30 1.086540e-05 1.88e-06 1.17e-03 5.75e-03 1.53e-01 2.73e+01 1 5.83e-03 2.33e-01
31 7.862919e-06 3.00e-06 8.88e-04 5.19e-03 2.89e-01 2.54e+01 1 5.88e-03 2.39e-01
32 6.688322e-06 1.17e-06 7.76e-04 5.47e-03 1.58e-01 1.93e+01 1 5.75e-03 2.44e-01
33 4.878867e-06 1.81e-06 5.82e-04 4.81e-03 2.91e-01 1.80e+01 1 5.88e-03 2.50e-01
34 4.185668e-06 6.93e-07 5.07e-04 5.00e-03 1.56e-01 1.35e+01 1 5.84e-03 2.56e-01
35 3.093451e-06 1.09e-06 3.75e-04 4.12e-03 2.91e-01 1.26e+01 1 5.70e-03 2.62e-01
36 2.712019e-06 3.81e-07 3.29e-04 4.20e-03 1.41e-01 9.22e+00 1 5.85e-03 2.68e-01
37 2.029543e-06 6.82e-07 2.58e-04 3.16e-03 2.97e-01 8.64e+00 1 5.85e-03 2.74e-01
38 1.838976e-06 1.91e-07 2.56e-04 3.24e-03 1.16e-01 5.94e+00 1 5.66e-03 2.79e-01
39 1.392916e-06 4.46e-07 1.98e-04 2.23e-03 3.13e-01 5.65e+00 1 5.73e-03 2.85e-01
40 1.308650e-06 8.43e-08 1.96e-04 2.43e-03 8.31e-02 3.58e+00 1 5.73e-03 2.91e-01
41 1.018832e-06 2.90e-07 1.41e-04 1.61e-03 3.29e-01 3.44e+00 1 5.60e-03 2.96e-01
42 9.805994e-07 3.82e-08 1.31e-04 1.71e-03 6.06e-02 2.05e+00 1 5.67e-03 3.02e-01
43 8.120806e-07 1.69e-07 9.06e-05 1.26e-03 3.15e-01 1.95e+00 1 5.89e-03 3.08e-01
44 7.897351e-07 2.23e-08 7.40e-05 1.45e-03 5.55e-02 1.15e+00 1 5.70e-03 3.14e-01
45 7.024238e-07 8.73e-08 5.59e-05 1.04e-03 2.73e-01 1.05e+00 1 5.67e-03 3.19e-01
46 6.850532e-07 1.74e-08 4.27e-05 1.62e-03 6.65e-02 6.34e-01 1 5.66e-03 3.25e-01
47 6.450376e-07 4.00e-08 3.53e-05 9.89e-04 2.05e-01 5.26e-01 1 5.60e-03 3.31e-01
48 6.310723e-07 1.40e-08 2.69e-05 1.50e-03 8.44e-02 3.34e-01 1 5.66e-03 3.36e-01
49 6.149139e-07 1.62e-08 2.34e-05 1.06e-03 1.34e-01 2.40e-01 1 5.68e-03 3.42e-01
50 6.064663e-07 8.45e-09 1.93e-05 1.13e-03 8.82e-02 1.54e-01 1 5.61e-03 3.48e-01
51 6.002221e-07 6.24e-09 1.76e-05 8.61e-04 9.15e-02 9.97e-02 1 5.64e-03 3.53e-01
52 5.964460e-07 3.78e-09 1.61e-05 8.85e-04 7.83e-02 6.23e-02 1 5.77e-03 3.59e-01
53 5.940756e-07 2.37e-09 1.53e-05 7.42e-04 7.37e-02 3.85e-02 1 5.94e-03 3.65e-01
54 5.926468e-07 1.43e-09 1.47e-05 4.35e-04 6.89e-02 2.35e-02 1 6.86e-03 3.72e-01
55 5.917875e-07 8.59e-10 1.43e-05 2.31e-04 6.62e-02 1.42e-02 1 5.71e-03 3.78e-01
56 5.912719e-07 5.16e-10 1.41e-05 1.33e-04 6.46e-02 8.55e-03 1 6.31e-03 3.84e-01
57 5.909629e-07 3.09e-10 1.40e-05 7.89e-05 6.37e-02 5.13e-03 1 6.44e-03 3.91e-01
58 5.907779e-07 1.85e-10 1.39e-05 4.72e-05 6.31e-02 3.08e-03 1 5.69e-03 3.96e-01
59 5.906672e-07 1.11e-10 1.39e-05 2.83e-05 6.27e-02 1.85e-03 1 6.87e-03 4.03e-01
60 5.906009e-07 6.63e-11 1.38e-05 1.69e-05 6.25e-02 1.10e-03 1 6.19e-03 4.09e-01
61 5.905612e-07 3.97e-11 1.38e-05 1.01e-05 6.24e-02 6.62e-04 1 6.04e-03 4.15e-01
62 5.905374e-07 2.37e-11 1.38e-05 6.07e-06 6.23e-02 3.96e-04 1 5.97e-03 4.21e-01
63 5.905232e-07 1.42e-11 1.38e-05 3.63e-06 6.23e-02 2.37e-04 1 5.73e-03 4.27e-01
64 5.905147e-07 8.50e-12 1.38e-05 2.17e-06 6.23e-02 1.42e-04 1 5.65e-03 4.33e-01
65 5.905096e-07 5.09e-12 1.38e-05 1.30e-06 6.23e-02 8.49e-05 1 5.88e-03 4.39e-01
66 5.905066e-07 3.04e-12 1.38e-05 7.79e-07 6.22e-02 5.08e-05 1 5.60e-03 4.44e-01
67 5.905048e-07 1.82e-12 1.38e-05 4.66e-07 6.22e-02 3.04e-05 1 5.68e-03 4.50e-01
68 5.905037e-07 1.09e-12 1.38e-05 2.79e-07 6.22e-02 1.82e-05 1 5.67e-03 4.56e-01
69 5.905030e-07 6.52e-13 1.38e-05 1.67e-07 6.22e-02 1.09e-05 1 5.52e-03 4.61e-01
Ceres Solver Report: Iterations: 70, Initial cost: 1.032580e+03, Final cost: 5.905030e-07, Termination: CONVERGENCE
Initial velocity: 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, omega: 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
Final velocity: 0, 0.135049, 0.149686, 0.147858, 0.134244, 0.133518, 0.131609, 0.128317, 0.124225, 0.120296, 0.116756, 0.110159, 0.104737, 0.0965443, 0.11373, 0.100285, 0.0991691, 0.100067, 0.101409, 0.103747, 0.106785, 0.110393, 0.114405, 0.118741, 0.123498, 0.111281, 0.144437, 0.132301, 0.134725, 0.137329, 0.139179, 0.140103, 0.1405, 0.141048, 0.141598, 0.139138, 0.135506, 0.129373, 0.133489, 0.124917, omega: 0.563069, 0.271689, 0.0538583, -0.0763308, -0.0786377, -0.0415507, -0.0275034, -0.0382909, -0.0498934, -0.057907, -0.0645367, -0.0720145, -0.0803636, -0.0893672, -0.0942228, -0.0980076, -0.0992069, -0.097782, -0.0916037, -0.0842563, -0.0765681, -0.068881, -0.0611952, -0.053523, -0.0455831, -0.0356321, -0.0330142, -0.0256039, -0.0133146, -0.0112215, -0.0102517, -0.00657962, 0.0104808, 0.0237446, 0.0204859, 0.00376261, 0.0307743, -0.0268024, 0.0936738, 0,
React
Typescript
Django
Laravel
Facebook
Microsoft
Google
Alibaba
D3
Tencent