diff --git a/modules/common/math/mpc_solver.cc b/modules/common/math/mpc_solver.cc index 33451d787dd96397a3f4b16d011908156198ce06..72425b127557da7b2884ac22da80c687878c5b4d 100644 --- a/modules/common/math/mpc_solver.cc +++ b/modules/common/math/mpc_solver.cc @@ -6,8 +6,7 @@ * You may obtain a copy of the License at * * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software + * * Unless required by applicable law or agreed to in writing, software * distributed under the License is distributed on an "AS IS" BASIS, * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * See the License for the specific language governing permissions and @@ -25,7 +24,8 @@ namespace math { using Matrix = Eigen::MatrixXd; -// Linear MPC solver, for single actuator +// discrete linear predictive control solver, with control format +// x(i + 1) = A * x(i) + B * u (i) + C void SolveLinearMPC(const Matrix &matrix_a, const Matrix &matrix_b, const Matrix &matrix_c, @@ -46,50 +46,50 @@ void SolveLinearMPC(const Matrix &matrix_a, return; } - // Initialize matrix_k, matrix_m, matrix_t and matrix_v, matrix_qq, matrix_rr, vector of matrix A power - Matrix matrix_k = Matrix::Zero(matrix_b.rows() * control->size(), matrix_b.cols() * control->size()); - Matrix matrix_m = Matrix::Zero(matrix_b.rows() * control->size(), 1); - Matrix matrix_t = matrix_m; - Matrix matrix_v = Matrix::Zero((*control)[0].rows() * control->size(), 1); - Matrix matrix_qq = Matrix::Zero(matrix_k.rows(), matrix_k.rows()); - Matrix matrix_rr = Matrix::Zero(matrix_k.cols(), matrix_k.cols()); - Matrix matrix_ll = Matrix::Zero(control->size() * matrix_lower.rows(), 1); - Matrix matrix_uu = Matrix::Zero(control->size() * matrix_upper.rows(), 1); - std::vector matrix_a_power(control->size()); + const unsigned int horizon = reference.size(); + + // Update augment reference matrix_t + Matrix matrix_t = Matrix::Zero(matrix_b.rows() * horizon, 1); + for (unsigned int j = 0; j < horizon; ++j) { + matrix_t.block(j * reference[0].size(), 0, reference[0].size(), 1) = reference[j]; + } + + // Update augment control matrix_v + Matrix matrix_v = Matrix::Zero((*control)[0].rows() * horizon, 1); + for (unsigned int j = 0; j < horizon; ++j) { + matrix_v.block(j * (*control)[0].rows(), 0, (*control)[0].rows(), 1) = (*control)[j]; + } - // Compute power of matrix_a + std::vector matrix_a_power(horizon); matrix_a_power[0] = matrix_a; for (unsigned int i = 1; i < matrix_a_power.size(); ++i) { matrix_a_power[i] = matrix_a * matrix_a_power[i-1]; } - // Compute matrix_k - for (unsigned int r = 0; r < control->size(); ++r) { + Matrix matrix_k = Matrix::Zero(matrix_b.rows() * horizon, matrix_b.cols() * control->size()); + for (unsigned int r = 0; r < horizon; ++r) { for (unsigned int c = 0; c <= r; ++c) { matrix_k.block(r * matrix_b.rows(), c * matrix_b.cols(), matrix_b.rows(), \ matrix_b.cols()) = matrix_a_power[r-c] * matrix_b; } } + // Initialize matrix_k, matrix_m, matrix_t and matrix_v, matrix_qq, matrix_rr, vector of matrix A power + Matrix matrix_m = Matrix::Zero(matrix_b.rows() * horizon, 1); + Matrix matrix_qq = Matrix::Zero(matrix_k.rows(), matrix_k.rows()); + Matrix matrix_rr = Matrix::Zero(matrix_k.cols(), matrix_k.cols()); + Matrix matrix_ll = Matrix::Zero(horizon * matrix_lower.rows(), 1); + Matrix matrix_uu = Matrix::Zero(horizon * matrix_upper.rows(), 1); + // Compute matrix_m matrix_m.block(0, 0, matrix_a.rows(), 1) = matrix_a * matrix_initial_state + matrix_c; - for (unsigned int i = 1; i < control->size(); ++i) { - matrix_m.block(i * matrix_a.rows(), 0, matrix_a.rows(), 1) = matrix_a * matrix_m.block((i-1) * matrix_a.rows(), 0, matrix_a.rows(), 1) + matrix_c; - } - - // compute matrix_t - for (unsigned int j = 0; j < reference.size(); ++j) { - matrix_t.block(j * reference[0].size(), 0, reference[0].size(), 1) = reference[j]; - } - - // compute matrix_v - for (unsigned int j = 0; j < control->size(); ++j) { - // matrix_v.block(j * control->size(), 0, control->size(), 1) = control[j]; - matrix_v.block(j * (*control)[0].rows(), 0, (*control)[0].rows(), 1) = (*control)[j]; + for (unsigned int i = 1; i < horizon; ++i) { + matrix_m.block(i * matrix_a.rows(), 0, matrix_a.rows(), 1) = matrix_a \ + * matrix_m.block((i-1) * matrix_a.rows(), 0, matrix_a.rows(), 1) + matrix_c; } - // compute matrix_ll, matrix_uu, matrix_qq, matrix_rr together - for (unsigned int i = 0; i < control->size(); ++i) { + // Compute matrix_ll, matrix_uu, matrix_qq, matrix_rr + for (unsigned int i = 0; i < horizon; ++i) { matrix_ll.block(i * (*control)[0].rows(), 0, (*control)[0].rows(), 1) = matrix_lower; matrix_uu.block(i * (*control)[0].rows(), 0, (*control)[0].rows(), 1) = matrix_upper; matrix_qq.block(i * matrix_q.rows(), i * matrix_q.rows(), \ @@ -98,13 +98,13 @@ void SolveLinearMPC(const Matrix &matrix_a, matrix_r.rows(), matrix_r.rows()) = matrix_r; } - // update matrix_m1, matrix_m2, convert MPC problem to QP problem done + // Update matrix_m1, matrix_m2, convert MPC problem to QP problem done Matrix matrix_m1 = matrix_k.transpose() * matrix_qq * matrix_k + matrix_rr; Matrix matrix_m2 = matrix_k.transpose() * matrix_qq * (matrix_m - matrix_t); // Method 2: QP_SMO_Solver SolveQPSMO(matrix_m1, matrix_m2, matrix_ll, matrix_uu, eps, max_iter, &matrix_v); - for (unsigned int i = 0; i < control->size(); ++i) { + for (unsigned int i = 0; i < horizon; ++i) { (*control)[i] = matrix_v.block(i * (*control)[0].rows(), 0, (*control)[0].rows(), 1); } } @@ -117,10 +117,10 @@ void SolveQPSMO ( const double& eps, const int& max_iter, Matrix* matrix_v) { - // Warning: Skipped the sanity check since this is designed for solely used by mpc_solver, if you want to - // use it for other purpose, force sanity check at the beginning + // Warning: Skipped the sanity check since this is designed for solely used by mpc_solver + // TODO: Replace this with available QP solver Matrix matrix_df = matrix_q * (* matrix_v) + matrix_b; - Matrix matrix_qq = matrix_q.inverse(); + Matrix matrix_q_inv = matrix_q.inverse(); for (int iter = 0; iter < max_iter; ++iter) { double max_df = 0; int best_r = 0; @@ -143,7 +143,7 @@ void SolveQPSMO ( { const double old_alpha = (*matrix_v)(r); (*matrix_v)(r) = -(matrix_df(r) - matrix_q(r, r) * (*matrix_v)(r)) - * matrix_qq(r); + * matrix_q_inv(r); if ((*matrix_v)(r) < matrix_lower(r)) { (*matrix_v)(r) = matrix_lower(r); } @@ -158,10 +158,7 @@ void SolveQPSMO ( } } - AERROR << "max_df is: " << max_df << std::endl; if (max_df < eps) { - AERROR << "max_df is less than eps: " << max_df << std::endl; - AERROR << "iter now is: " << iter << std::endl; break; } } diff --git a/modules/common/math/mpc_solver_test.cc b/modules/common/math/mpc_solver_test.cc index 8f2f590d4e868e5cdb2918827bdac54ab6dc0064..c91745cd149b47b81ec522da530cffcba39983b3 100644 --- a/modules/common/math/mpc_solver_test.cc +++ b/modules/common/math/mpc_solver_test.cc @@ -26,8 +26,7 @@ namespace math { TEST(MPCSolverTest, MPC) { const int STATES = 4; const int CONTROLS = 1; - const int PREVIEW_HORIZON = 30; - const int CONTROL_HORIZON = 30; + const int HORIZON = 10; const double EPS = 0.01; const int MAX_ITER = 100; @@ -61,7 +60,6 @@ TEST(MPCSolverTest, MPC) { Eigen::MatrixXd lower_bound(CONTROLS, 1); lower_bound << -0.2; - std::cout << "lower_bound size: " << lower_bound.size() << std::endl; Eigen::MatrixXd upper_bound(CONTROLS, 1); upper_bound << 0.6; @@ -77,18 +75,75 @@ TEST(MPCSolverTest, MPC) { 0, 0; - std::vector reference(PREVIEW_HORIZON, reference_state); + std::vector reference(HORIZON, reference_state); Eigen::MatrixXd control_matrix(CONTROLS, 1); control_matrix << 0; - std::vector control(CONTROL_HORIZON, control_matrix); - solve_linear_mpc(A, B, C, Q, R, lower_bound, upper_bound, initial_state, reference, EPS, MAX_ITER, &control); - for (int i = 0; i < control.size(); ++i) { - EXPECT_FLOAT_EQ(0.6, control[0](0)); + std::vector control(HORIZON, control_matrix); + + for (unsigned int i = 0; i < control.size(); ++i) { + for (unsigned int i = 1; i < control.size(); ++i) { + control[i-1] = control[i]; + } + SolveLinearMPC(A, B, C, Q, R, lower_bound, upper_bound, initial_state, reference, EPS, MAX_ITER, &control); + EXPECT_FLOAT_EQ(upper_bound(0), control[0](0)); + initial_state = A * initial_state + B * control[0](0) + C; + } + + Eigen::MatrixXd initial_state1(STATES, 1); + initial_state1 << 300, + 300, + 0, + 0; + + Eigen::MatrixXd reference_state1(STATES, 1); + reference_state1 << 200, + 200, + 0, + 0; + + std::vector reference1(HORIZON, reference_state1); + + control_matrix << 0; + std::vector control1(HORIZON, control_matrix); + + for (unsigned int i = 0; i < control1.size(); ++i) { + for (unsigned int i = 1; i < control.size(); ++i) { + control1[i-1] = control1[i]; + } + SolveLinearMPC(A, B, C, Q, R, lower_bound, upper_bound, initial_state1, reference1, EPS, MAX_ITER, &control1); + EXPECT_FLOAT_EQ(lower_bound(0), control1[0](0)); + initial_state1 = A * initial_state1 + B * control1[0](0) + C; } -} + Eigen::MatrixXd initial_state2(STATES, 1); + initial_state2 << 300, + 300, + 0, + 0; + + Eigen::MatrixXd reference_state2(STATES, 1); + reference_state2 << 300, + 300, + 0, + 0; + + std::vector reference2(HORIZON, reference_state2); + + control_matrix << 0; + std::vector control2(HORIZON, control_matrix); + + for (unsigned int i = 0; i < control2.size(); ++i) { + for (unsigned int i = 1; i < control.size(); ++i) { + control2[i-1] = control2[i]; + } + SolveLinearMPC(A, B, C, Q, R, lower_bound, upper_bound, initial_state2, reference2, EPS, MAX_ITER, &control2); + EXPECT_FLOAT_EQ(0.0, control2[0](0)); + initial_state2 = A * initial_state2 + B * control2[0](0) + C; + } + +} } // namespace math } // namespace common } // namespace apollo