autowarefoundation · takayuki5168 · Jul 27, 2023 · Jul 26, 2023
diff --git a/control/mpc_lateral_controller/CMakeLists.txt b/control/mpc_lateral_controller/CMakeLists.txt
@@ -17,7 +17,7 @@ ament_auto_add_library(${MPC_LAT_CON_LIB} SHARED
   src/mpc_trajectory.cpp
   src/mpc_utils.cpp
   src/qp_solver/qp_solver_osqp.cpp
-  src/qp_solver/qp_solver_unconstr_fast.cpp
+  src/qp_solver/qp_solver_unconstraint_fast.cpp
   src/vehicle_model/vehicle_model_bicycle_dynamics.cpp
   src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp
   src/vehicle_model/vehicle_model_bicycle_kinematics.cpp

diff --git a/...ler/qp_solver/qp_solver_unconstr_fast.hpp → ...qp_solver/qp_solver_unconstraint_fast.hpp b/...ler/qp_solver/qp_solver_unconstr_fast.hpp → ...qp_solver/qp_solver_unconstraint_fast.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_
-#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_
+#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_
+#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_
 
 #include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp"
 
@@ -55,4 +55,4 @@ class QPSolverEigenLeastSquareLLT : public QPSolverInterface
     const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override;
 };
 }  // namespace autoware::motion::control::mpc_lateral_controller
-#endif  // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTR_FAST_HPP_
+#endif  // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_
diff --git a/control/mpc_lateral_controller/model_predictive_control_algorithm.md b/control/mpc_lateral_controller/model_predictive_control_algorithm.md
@@ -50,7 +50,7 @@
 x_{2} & = Ax_{1} + Bu_{1} + w_{1} \\
 & = A(Ax_{0} + Bu_{0} + w_{0}) + Bu_{1} + w_{1} \\
 & = A^{2}x_{0} + ABu_{0} + Aw_{0} + Bu_{1} + w_{1} \\
 & = A^{2}x_{0} + \begin{bmatrix}AB & B \end{bmatrix}\begin{bmatrix}u_{0}\\ u_{1} \end{bmatrix} + \begin{bmatrix}A & I \end{bmatrix}\begin{bmatrix}w_{0}\\ w_{1} \end{bmatrix} \tag{3}
 \end{align}
 $$

@@ -125,7 +125,7 @@
 $$
 \begin{align}
 J(U) &= (H(Fx_{0}+GU+SW)-Y_{ref})^{T}Q(H(Fx_{0}+GU+SW)-Y_{ref})+(U-U_{ref})^{T}R(U-U_{ref}) \\
 & =U^{T}(G^{T}H^{T}QHG+R)U+2\left\{(H(Fx_{0}+SW)-Yref)^{T}QHG-U_{ref}^{T}R\right\}U +(\rm{constant}) \tag{10}
 \end{align}
 $$

@@ -182,7 +182,7 @@

 There are three expressions in the update equations that are subject to linear approximation: the lateral deviation (or lateral coordinate) $y$, the heading angle (or the heading angle error) $\theta$, and the steering $\delta$. We can make a small angle assumption on the heading angle $\theta$.

 In the path tracking problem, the curvature of the trajectory $\kappa_{r}$ is known in advance. At the lower speeds, the Ackermann formula approximates the reference steering angle $\theta_{r}$(this value corresponds to the $U_{ref}$ mentioned above). The Ackerman steering expression can be written as;

 $$
 \begin{align}
@@ -202,7 +202,7 @@

 $$
 \begin{align}
 \tan\delta &\simeq \tan\delta_{r} + \frac{\text{d}\tan\delta}{\text{d}\delta} \Biggm|_{\delta=\delta_{r}}\Delta\delta \\
 &= \tan \delta_{r} + \frac{1}{\cos^{2}\delta_{r}}\Delta\delta \\
 &= \tan \delta_{r} + \frac{1}{\cos^{2}\delta_{r}}\left(\delta-\delta_{r}\right) \\
 &= \tan \delta_{r} - \frac{\delta_{r}}{\cos^{2}\delta_{r}} + \frac{1}{\cos^{2}\delta_{r}}\delta
@@ -344,7 +344,7 @@
 \end{align}
 $$
 
-Discretisizing $\dot{u}$ as $\left(u_{k} - u_{k-1}\right)/\text{d}t$ and multiply both sides by dt the resulting constraint become linear and convex
+Discretizing $\dot{u}$ as $\left(u_{k} - u_{k-1}\right)/\text{d}t$ and multiply both sides by dt the resulting constraint become linear and convex
 
 $$
 \begin{align}

diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp
@@ -16,7 +16,7 @@
 
 #include "motion_utils/motion_utils.hpp"
 #include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp"
-#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp"
+#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp"
 #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp"
 #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp"
 #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp"

diff --git a/...src/qp_solver/qp_solver_unconstr_fast.cpp → ...qp_solver/qp_solver_unconstraint_fast.cpp b/...src/qp_solver/qp_solver_unconstr_fast.cpp → ...qp_solver/qp_solver_unconstraint_fast.cpp
@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp"
+#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp"
 
 #include <Eigen/Dense>
 

diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp
@@ -15,7 +15,7 @@
 #include "gtest/gtest.h"
 #include "mpc_lateral_controller/mpc.hpp"
 #include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp"
-#include "mpc_lateral_controller/qp_solver/qp_solver_unconstr_fast.hpp"
+#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp"
 #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp"
 #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp"
 #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp"