Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
74e36906
A
apollo
项目概览
Pinoxchio
/
apollo
与 Fork 源项目一致
从无法访问的项目Fork
通知
2
Star
0
Fork
0
代码
文件
提交
分支
Tags
贡献者
分支图
Diff
Issue
0
列表
看板
标记
里程碑
合并请求
0
Wiki
0
Wiki
分析
仓库
DevOps
项目成员
Pages
A
apollo
项目概览
项目概览
详情
发布
仓库
仓库
文件
提交
分支
标签
贡献者
分支图
比较
Issue
0
Issue
0
列表
看板
标记
里程碑
合并请求
0
合并请求
0
Pages
分析
分析
仓库分析
DevOps
Wiki
0
Wiki
成员
成员
收起侧边栏
关闭侧边栏
动态
分支图
创建新Issue
提交
Issue看板
体验新版 GitCode,发现更多精彩内容 >>
提交
74e36906
编写于
7月 18, 2017
作者:
Q
Qi Luo
提交者:
Jiangtao Hu
7月 18, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Add initial mpc solver and test files
上级
5b48f858
变更
4
隐藏空白更改
内联
并排
Showing
4 changed file
with
367 addition
and
0 deletion
+367
-0
modules/common/math/BUILD
modules/common/math/BUILD
+27
-0
modules/common/math/mpc_solver.cc
modules/common/math/mpc_solver.cc
+171
-0
modules/common/math/mpc_solver.h
modules/common/math/mpc_solver.h
+74
-0
modules/common/math/mpc_solver_test.cc
modules/common/math/mpc_solver_test.cc
+95
-0
未找到文件。
modules/common/math/BUILD
浏览文件 @
74e36906
...
@@ -16,6 +16,7 @@ cc_library(
...
@@ -16,6 +16,7 @@ cc_library(
":linear_interpolation"
,
":linear_interpolation"
,
":lqr"
,
":lqr"
,
":math_utils"
,
":math_utils"
,
":mpc"
,
":polygon2d"
,
":polygon2d"
,
":quaternion"
,
":quaternion"
,
":search"
,
":search"
,
...
@@ -263,6 +264,32 @@ cc_library(
...
@@ -263,6 +264,32 @@ cc_library(
],
],
)
)
cc_library
(
name
=
"mpc"
,
srcs
=
[
"mpc_solver.cc"
,
],
hdrs
=
[
"mpc_solver.h"
,
],
deps
=
[
"//modules/common:log"
,
"@eigen//:eigen"
,
],
)
cc_test
(
name
=
"mpc_test"
,
size
=
"small"
,
srcs
=
[
"mpc_solver_test.cc"
,
],
deps
=
[
":mpc"
,
"@gtest//:main"
,
],
)
cc_test
(
cc_test
(
name
=
"math_utils_test"
,
name
=
"math_utils_test"
,
size
=
"small"
,
size
=
"small"
,
...
...
modules/common/math/mpc_solver.cc
0 → 100644
浏览文件 @
74e36906
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* 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
* 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
* limitations under the License.
*****************************************************************************/
#include "modules/common/math/mpc_solver.h"
#include <algorithm>
#include "modules/common/log.h"
namespace
apollo
{
namespace
common
{
namespace
math
{
using
Matrix
=
Eigen
::
MatrixXd
;
// Linear MPC solver, for single actuator
void
SolveLinearMPC
(
const
Matrix
&
matrix_a
,
const
Matrix
&
matrix_b
,
const
Matrix
&
matrix_c
,
const
Matrix
&
matrix_q
,
const
Matrix
&
matrix_r
,
const
Matrix
&
matrix_lower
,
const
Matrix
&
matrix_upper
,
const
Matrix
&
matrix_initial_state
,
const
std
::
vector
<
Matrix
>
&
reference
,
const
double
eps
,
const
int
max_iter
,
std
::
vector
<
Matrix
>
*
control
)
{
if
(
matrix_a
.
rows
()
!=
matrix_a
.
cols
()
||
matrix_b
.
rows
()
!=
matrix_a
.
rows
()
||
matrix_lower
.
rows
()
!=
matrix_upper
.
rows
())
{
AERROR
<<
"One or more matrices have incompatible dimensions. \
Aborting.
\n
"
;
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
>
matrix_a_power
(
control
->
size
());
// Compute power of matrix_a
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
)
{
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
;
}
}
// 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
];
}
// compute matrix_ll, matrix_uu, matrix_qq, matrix_rr together
for
(
unsigned
int
i
=
0
;
i
<
control
->
size
();
++
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
(),
\
matrix_q
.
rows
(),
matrix_q
.
rows
())
=
matrix_q
;
matrix_rr
.
block
(
i
*
matrix_r
.
rows
(),
i
*
matrix_r
.
rows
(),
\
matrix_r
.
rows
(),
matrix_r
.
rows
())
=
matrix_r
;
}
// 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
)
{
(
*
control
)[
i
]
=
matrix_v
.
block
(
i
*
(
*
control
)[
0
].
rows
(),
0
,
(
*
control
)[
0
].
rows
(),
1
);
}
}
void
SolveQPSMO
(
const
Matrix
&
matrix_q
,
const
Matrix
&
matrix_b
,
const
Matrix
&
matrix_lower
,
const
Matrix
&
matrix_upper
,
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
Matrix
matrix_df
=
matrix_q
*
(
*
matrix_v
)
+
matrix_b
;
Matrix
matrix_qq
=
matrix_q
.
inverse
();
for
(
int
iter
=
0
;
iter
<
max_iter
;
++
iter
)
{
double
max_df
=
0
;
int
best_r
=
0
;
for
(
int
r
=
0
;
r
<
matrix_q
.
rows
();
++
r
)
{
if
((
*
matrix_v
)(
r
)
<=
matrix_lower
(
r
)
&&
matrix_df
(
r
)
>
0
)
{
continue
;
}
else
if
((
*
matrix_v
)(
r
)
>=
matrix_upper
(
r
)
&&
matrix_df
(
r
)
<
0
)
{
continue
;
}
else
if
(
std
::
abs
(
matrix_df
(
r
))
>
max_df
)
{
best_r
=
r
;
max_df
=
std
::
abs
(
matrix_df
(
r
));
}
}
int
r
=
best_r
;
{
const
double
old_alpha
=
(
*
matrix_v
)(
r
);
(
*
matrix_v
)(
r
)
=
-
(
matrix_df
(
r
)
-
matrix_q
(
r
,
r
)
*
(
*
matrix_v
)(
r
))
*
matrix_qq
(
r
);
if
((
*
matrix_v
)(
r
)
<
matrix_lower
(
r
))
{
(
*
matrix_v
)(
r
)
=
matrix_lower
(
r
);
}
else
if
((
*
matrix_v
)(
r
)
>
matrix_upper
(
r
))
{
(
*
matrix_v
)(
r
)
=
matrix_upper
(
r
);
}
const
double
delta
=
old_alpha
-
(
*
matrix_v
)(
r
);
// Gradient update.
for
(
int
k
=
0
;
k
<
matrix_df
.
rows
();
++
k
)
{
matrix_df
(
k
)
-=
matrix_q
(
r
,
k
)
*
delta
;
}
}
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
;
}
}
}
}
// namespace math
}
// namespace common
}
// namespace apollo
modules/common/math/mpc_solver.h
0 → 100644
浏览文件 @
74e36906
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* 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
* 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
* limitations under the License.
*****************************************************************************/
/**
* @file mpc_solver.h
* @brief Convert mpc problem to qp based problem and solve.
*/
#ifndef MODULES_CONTROL_COMMON_MPC_SOLVER_H_
#define MODULES_CONTROL_COMMON_MPC_SOLVER_H_
#include "Eigen/Core"
#include "Eigen/LU"
/**
* @namespace apollo::common::math
* @brief apollo::common::math
*/
namespace
apollo
{
namespace
common
{
namespace
math
{
/**
* @brief Solver for discrete-time model predictive control problem.
* @param A The system dynamic matrix
* @param B The control matrix
* @param Q The cost matrix for system state
* @param R The cost matrix for control output
* @param tolerance The numerical tolerance for solving
* Algebraic Riccati equation (ARE)
* @param max_num_iteration The maximum iterations for solving ARE
* @param ptr_K The feedback control matrix (pointer)
*/
void
SolveLinearMPC
(
const
Eigen
::
MatrixXd
&
matrix_a
,
const
Eigen
::
MatrixXd
&
matrix_b
,
const
Eigen
::
MatrixXd
&
matrix_c
,
const
Eigen
::
MatrixXd
&
matrix_q
,
const
Eigen
::
MatrixXd
&
matrix_r
,
const
Eigen
::
MatrixXd
&
matrix_lower
,
const
Eigen
::
MatrixXd
&
matrix_upper
,
const
Eigen
::
MatrixXd
&
_matrix_initial_state
,
const
std
::
vector
<
Eigen
::
MatrixXd
>
&
reference
,
const
double
eps
,
const
int
max_iter
,
std
::
vector
<
Eigen
::
MatrixXd
>
*
control
);
void
SolveQPSMO
(
const
Eigen
::
MatrixXd
&
matrix_q
,
const
Eigen
::
MatrixXd
&
matrix_b
,
const
Eigen
::
MatrixXd
&
matrix_lower
,
const
Eigen
::
MatrixXd
&
matrix_upper
,
const
double
&
eps
,
const
int
&
max_iter
,
Eigen
::
MatrixXd
*
matrix_alpha
);
}
// namespace math
}
// namespace common
}
// namespace apollo
#endif // MODULES_CONTROL_COMMON_MPC_SOLVER_H_
modules/common/math/mpc_solver_test.cc
0 → 100644
浏览文件 @
74e36906
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* 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
* 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
* limitations under the License.
*****************************************************************************/
#include "modules/common/math/mpc_solver.h"
#include "gtest/gtest.h"
namespace
apollo
{
namespace
common
{
namespace
math
{
TEST
(
MPCSolverTest
,
MPC
)
{
const
int
STATES
=
4
;
const
int
CONTROLS
=
1
;
const
int
PREVIEW_HORIZON
=
30
;
const
int
CONTROL_HORIZON
=
30
;
const
double
EPS
=
0.01
;
const
int
MAX_ITER
=
100
;
Eigen
::
MatrixXd
A
(
STATES
,
STATES
);
A
<<
1
,
0
,
1
,
0
,
0
,
1
,
0
,
1
,
0
,
0
,
1
,
0
,
0
,
0
,
0
,
1
;
Eigen
::
MatrixXd
B
(
STATES
,
CONTROLS
);
B
<<
0
,
0
,
1
,
0
;
Eigen
::
MatrixXd
C
(
STATES
,
1
);
C
<<
0
,
0
,
0
,
0.1
;
Eigen
::
MatrixXd
Q
(
STATES
,
STATES
);
Q
<<
1
,
0
,
0
,
0
,
0
,
1
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
,
0
;
Eigen
::
MatrixXd
R
(
CONTROLS
,
1
);
R
<<
1
;
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
;
Eigen
::
MatrixXd
initial_state
(
STATES
,
1
);
initial_state
<<
0
,
0
,
0
,
0
;
Eigen
::
MatrixXd
reference_state
(
STATES
,
1
);
reference_state
<<
200
,
200
,
0
,
0
;
std
::
vector
<
Eigen
::
MatrixXd
>
reference
(
PREVIEW_HORIZON
,
reference_state
);
Eigen
::
MatrixXd
control_matrix
(
CONTROLS
,
1
);
control_matrix
<<
0
;
std
::
vector
<
Eigen
::
MatrixXd
>
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
));
}
}
}
// namespace math
}
// namespace common
}
// namespace apollo
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录