Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
2ebd4352
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,发现更多精彩内容 >>
提交
2ebd4352
编写于
7月 18, 2017
作者:
Q
Qi Luo
提交者:
Jiangtao Hu
7月 18, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Update mpc solver and test file
上级
74e36906
变更
2
隐藏空白更改
内联
并排
Showing
2 changed file
with
101 addition
and
49 deletion
+101
-49
modules/common/math/mpc_solver.cc
modules/common/math/mpc_solver.cc
+37
-40
modules/common/math/mpc_solver_test.cc
modules/common/math/mpc_solver_test.cc
+64
-9
未找到文件。
modules/common/math/mpc_solver.cc
浏览文件 @
2ebd4352
...
@@ -6,8 +6,7 @@
...
@@ -6,8 +6,7 @@
* You may obtain a copy of the License at
* You may obtain a copy of the License at
*
*
* http://www.apache.org/licenses/LICENSE-2.0
* 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,
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* See the License for the specific language governing permissions and
...
@@ -25,7 +24,8 @@ namespace math {
...
@@ -25,7 +24,8 @@ namespace math {
using
Matrix
=
Eigen
::
MatrixXd
;
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
,
void
SolveLinearMPC
(
const
Matrix
&
matrix_a
,
const
Matrix
&
matrix_b
,
const
Matrix
&
matrix_b
,
const
Matrix
&
matrix_c
,
const
Matrix
&
matrix_c
,
...
@@ -46,50 +46,50 @@ void SolveLinearMPC(const Matrix &matrix_a,
...
@@ -46,50 +46,50 @@ void SolveLinearMPC(const Matrix &matrix_a,
return
;
return
;
}
}
// Initialize matrix_k, matrix_m, matrix_t and matrix_v, matrix_qq, matrix_rr, vector of matrix A power
const
unsigned
int
horizon
=
reference
.
size
();
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
);
// Update augment reference matrix_t
Matrix
matrix_t
=
matrix_m
;
Matrix
matrix_t
=
Matrix
::
Zero
(
matrix_b
.
rows
()
*
horizon
,
1
);
Matrix
matrix_v
=
Matrix
::
Zero
((
*
control
)[
0
].
rows
()
*
control
->
size
(),
1
);
for
(
unsigned
int
j
=
0
;
j
<
horizon
;
++
j
)
{
Matrix
matrix_qq
=
Matrix
::
Zero
(
matrix_k
.
rows
(),
matrix_k
.
rows
());
matrix_t
.
block
(
j
*
reference
[
0
].
size
(),
0
,
reference
[
0
].
size
(),
1
)
=
reference
[
j
];
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
);
// Update augment control matrix_v
std
::
vector
<
Matrix
>
matrix_a_power
(
control
->
size
());
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
>
matrix_a_power
(
horizon
);
matrix_a_power
[
0
]
=
matrix_a
;
matrix_a_power
[
0
]
=
matrix_a
;
for
(
unsigned
int
i
=
1
;
i
<
matrix_a_power
.
size
();
++
i
)
{
for
(
unsigned
int
i
=
1
;
i
<
matrix_a_power
.
size
();
++
i
)
{
matrix_a_power
[
i
]
=
matrix_a
*
matrix_a_power
[
i
-
1
];
matrix_a_power
[
i
]
=
matrix_a
*
matrix_a_power
[
i
-
1
];
}
}
// Compute matrix_k
Matrix
matrix_k
=
Matrix
::
Zero
(
matrix_b
.
rows
()
*
horizon
,
matrix_b
.
cols
()
*
control
->
size
());
for
(
unsigned
int
r
=
0
;
r
<
control
->
size
()
;
++
r
)
{
for
(
unsigned
int
r
=
0
;
r
<
horizon
;
++
r
)
{
for
(
unsigned
int
c
=
0
;
c
<=
r
;
++
c
)
{
for
(
unsigned
int
c
=
0
;
c
<=
r
;
++
c
)
{
matrix_k
.
block
(
r
*
matrix_b
.
rows
(),
c
*
matrix_b
.
cols
(),
matrix_b
.
rows
(),
\
matrix_k
.
block
(
r
*
matrix_b
.
rows
(),
c
*
matrix_b
.
cols
(),
matrix_b
.
rows
(),
\
matrix_b
.
cols
())
=
matrix_a_power
[
r
-
c
]
*
matrix_b
;
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
// Compute matrix_m
matrix_m
.
block
(
0
,
0
,
matrix_a
.
rows
(),
1
)
=
matrix_a
*
matrix_initial_state
+
matrix_c
;
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
)
{
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
;
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 togethe
r
//
Compute matrix_ll, matrix_uu, matrix_qq, matrix_r
r
for
(
unsigned
int
i
=
0
;
i
<
control
->
size
()
;
++
i
)
{
for
(
unsigned
int
i
=
0
;
i
<
horizon
;
++
i
)
{
matrix_ll
.
block
(
i
*
(
*
control
)[
0
].
rows
(),
0
,
(
*
control
)[
0
].
rows
(),
1
)
=
matrix_lower
;
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_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_qq
.
block
(
i
*
matrix_q
.
rows
(),
i
*
matrix_q
.
rows
(),
\
...
@@ -98,13 +98,13 @@ void SolveLinearMPC(const Matrix &matrix_a,
...
@@ -98,13 +98,13 @@ void SolveLinearMPC(const Matrix &matrix_a,
matrix_r
.
rows
(),
matrix_r
.
rows
())
=
matrix_r
;
matrix_r
.
rows
(),
matrix_r
.
rows
())
=
matrix_r
;
}
}
//
u
pdate matrix_m1, matrix_m2, convert MPC problem to QP problem done
//
U
pdate 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_m1
=
matrix_k
.
transpose
()
*
matrix_qq
*
matrix_k
+
matrix_rr
;
Matrix
matrix_m2
=
matrix_k
.
transpose
()
*
matrix_qq
*
(
matrix_m
-
matrix_t
);
Matrix
matrix_m2
=
matrix_k
.
transpose
()
*
matrix_qq
*
(
matrix_m
-
matrix_t
);
// Method 2: QP_SMO_Solver
// Method 2: QP_SMO_Solver
SolveQPSMO
(
matrix_m1
,
matrix_m2
,
matrix_ll
,
matrix_uu
,
eps
,
max_iter
,
&
matrix_v
);
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
);
(
*
control
)[
i
]
=
matrix_v
.
block
(
i
*
(
*
control
)[
0
].
rows
(),
0
,
(
*
control
)[
0
].
rows
(),
1
);
}
}
}
}
...
@@ -117,10 +117,10 @@ void SolveQPSMO (
...
@@ -117,10 +117,10 @@ void SolveQPSMO (
const
double
&
eps
,
const
double
&
eps
,
const
int
&
max_iter
,
const
int
&
max_iter
,
Matrix
*
matrix_v
)
{
Matrix
*
matrix_v
)
{
// Warning: Skipped the sanity check since this is designed for solely used by mpc_solver
, if you want to
// Warning: Skipped the sanity check since this is designed for solely used by mpc_solver
//
use it for other purpose, force sanity check at the beginning
//
TODO: Replace this with available QP solver
Matrix
matrix_df
=
matrix_q
*
(
*
matrix_v
)
+
matrix_b
;
Matrix
matrix_df
=
matrix_q
*
(
*
matrix_v
)
+
matrix_b
;
Matrix
matrix_q
q
=
matrix_q
.
inverse
();
Matrix
matrix_q
_inv
=
matrix_q
.
inverse
();
for
(
int
iter
=
0
;
iter
<
max_iter
;
++
iter
)
{
for
(
int
iter
=
0
;
iter
<
max_iter
;
++
iter
)
{
double
max_df
=
0
;
double
max_df
=
0
;
int
best_r
=
0
;
int
best_r
=
0
;
...
@@ -143,7 +143,7 @@ void SolveQPSMO (
...
@@ -143,7 +143,7 @@ void SolveQPSMO (
{
{
const
double
old_alpha
=
(
*
matrix_v
)(
r
);
const
double
old_alpha
=
(
*
matrix_v
)(
r
);
(
*
matrix_v
)(
r
)
=
-
(
matrix_df
(
r
)
-
matrix_q
(
r
,
r
)
*
(
*
matrix_v
)(
r
))
(
*
matrix_v
)(
r
)
=
-
(
matrix_df
(
r
)
-
matrix_q
(
r
,
r
)
*
(
*
matrix_v
)(
r
))
*
matrix_q
q
(
r
);
*
matrix_q
_inv
(
r
);
if
((
*
matrix_v
)(
r
)
<
matrix_lower
(
r
))
{
if
((
*
matrix_v
)(
r
)
<
matrix_lower
(
r
))
{
(
*
matrix_v
)(
r
)
=
matrix_lower
(
r
);
(
*
matrix_v
)(
r
)
=
matrix_lower
(
r
);
}
}
...
@@ -158,10 +158,7 @@ void SolveQPSMO (
...
@@ -158,10 +158,7 @@ void SolveQPSMO (
}
}
}
}
AERROR
<<
"max_df is: "
<<
max_df
<<
std
::
endl
;
if
(
max_df
<
eps
)
{
if
(
max_df
<
eps
)
{
AERROR
<<
"max_df is less than eps: "
<<
max_df
<<
std
::
endl
;
AERROR
<<
"iter now is: "
<<
iter
<<
std
::
endl
;
break
;
break
;
}
}
}
}
...
...
modules/common/math/mpc_solver_test.cc
浏览文件 @
2ebd4352
...
@@ -26,8 +26,7 @@ namespace math {
...
@@ -26,8 +26,7 @@ namespace math {
TEST
(
MPCSolverTest
,
MPC
)
{
TEST
(
MPCSolverTest
,
MPC
)
{
const
int
STATES
=
4
;
const
int
STATES
=
4
;
const
int
CONTROLS
=
1
;
const
int
CONTROLS
=
1
;
const
int
PREVIEW_HORIZON
=
30
;
const
int
HORIZON
=
10
;
const
int
CONTROL_HORIZON
=
30
;
const
double
EPS
=
0.01
;
const
double
EPS
=
0.01
;
const
int
MAX_ITER
=
100
;
const
int
MAX_ITER
=
100
;
...
@@ -61,7 +60,6 @@ TEST(MPCSolverTest, MPC) {
...
@@ -61,7 +60,6 @@ TEST(MPCSolverTest, MPC) {
Eigen
::
MatrixXd
lower_bound
(
CONTROLS
,
1
);
Eigen
::
MatrixXd
lower_bound
(
CONTROLS
,
1
);
lower_bound
<<
-
0.2
;
lower_bound
<<
-
0.2
;
std
::
cout
<<
"lower_bound size: "
<<
lower_bound
.
size
()
<<
std
::
endl
;
Eigen
::
MatrixXd
upper_bound
(
CONTROLS
,
1
);
Eigen
::
MatrixXd
upper_bound
(
CONTROLS
,
1
);
upper_bound
<<
0.6
;
upper_bound
<<
0.6
;
...
@@ -77,18 +75,75 @@ TEST(MPCSolverTest, MPC) {
...
@@ -77,18 +75,75 @@ TEST(MPCSolverTest, MPC) {
0
,
0
,
0
;
0
;
std
::
vector
<
Eigen
::
MatrixXd
>
reference
(
PREVIEW_
HORIZON
,
reference_state
);
std
::
vector
<
Eigen
::
MatrixXd
>
reference
(
HORIZON
,
reference_state
);
Eigen
::
MatrixXd
control_matrix
(
CONTROLS
,
1
);
Eigen
::
MatrixXd
control_matrix
(
CONTROLS
,
1
);
control_matrix
<<
0
;
control_matrix
<<
0
;
std
::
vector
<
Eigen
::
MatrixXd
>
control
(
CONTROL_HORIZON
,
control_matrix
);
std
::
vector
<
Eigen
::
MatrixXd
>
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
)
{
for
(
unsigned
int
i
=
0
;
i
<
control
.
size
();
++
i
)
{
EXPECT_FLOAT_EQ
(
0.6
,
control
[
0
](
0
));
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
<
Eigen
::
MatrixXd
>
reference1
(
HORIZON
,
reference_state1
);
control_matrix
<<
0
;
std
::
vector
<
Eigen
::
MatrixXd
>
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
<
Eigen
::
MatrixXd
>
reference2
(
HORIZON
,
reference_state2
);
control_matrix
<<
0
;
std
::
vector
<
Eigen
::
MatrixXd
>
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 math
}
// namespace common
}
// namespace common
}
// namespace apollo
}
// namespace apollo
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录