Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
a3d37ef2
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,发现更多精彩内容 >>
提交
a3d37ef2
编写于
11月 08, 2019
作者:
K
kevin-y-wang
提交者:
Xiangquan Xiao
11月 11, 2019
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Control: refactor Mrac controller initialization with ERROR STATUS
上级
2ae93a3d
变更
2
显示空白变更内容
内联
并排
Showing
2 changed file
with
36 addition
and
52 deletion
+36
-52
modules/control/common/mrac_controller.cc
modules/control/common/mrac_controller.cc
+32
-49
modules/control/common/mrac_controller.h
modules/control/common/mrac_controller.h
+4
-3
未找到文件。
modules/control/common/mrac_controller.cc
浏览文件 @
a3d37ef2
...
@@ -17,6 +17,7 @@
...
@@ -17,6 +17,7 @@
#include "modules/control/common/mrac_controller.h"
#include "modules/control/common/mrac_controller.h"
#include <cmath>
#include <cmath>
#include <string>
#include <vector>
#include <vector>
#include "Eigen/Dense"
#include "Eigen/Dense"
...
@@ -36,22 +37,16 @@ using Matrix = Eigen::MatrixXd;
...
@@ -36,22 +37,16 @@ using Matrix = Eigen::MatrixXd;
double
MracController
::
Control
(
const
double
command
,
const
Matrix
state
,
double
MracController
::
Control
(
const
double
command
,
const
Matrix
state
,
const
double
input_limit
,
const
double
input_limit
,
const
double
input_rate_limit
)
{
const
double
input_rate_limit
)
{
// check if the reference/adaption model well set up during the initialization
// check if the current sampling time is valid and the reference/adaption
if
(
!
reference_model_enabled_
||
!
adaption_model_enabled_
)
{
// model well set up during the initialization
AWARN
<<
"MRAC: model build failed; will work as a unity compensator. The "
if
(
ts_
<=
0.0
||
!
reference_model_enabled_
||
!
adaption_model_enabled_
)
{
AERROR
<<
"MRAC: model build failed; will work as a unity compensator. The "
"reference_model building status: "
"reference_model building status: "
<<
reference_model_enabled_
<<
reference_model_enabled_
<<
"; The adaption_model building status: "
<<
"; The adaption_model building status: "
<<
adaption_model_enabled
_
;
<<
adaption_model_enabled_
<<
"; Current sampling time: "
<<
ts
_
;
return
command
;
// treat the mrac as a unity proportional controller
return
command
;
// treat the mrac as a unity proportional controller
}
}
// check if the current sampling time is valid
if
(
ts_
<=
0.0
)
{
AWARN
<<
"MRAC: current sampling time <= 0, will use the last control, ts_: "
<<
ts_
;
return
control_previous_
;
}
// update the state in the real actuation system
// update the state in the real actuation system
state_action_
.
col
(
0
)
=
state
;
state_action_
.
col
(
0
)
=
state
;
...
@@ -152,19 +147,13 @@ void MracController::Init(const MracConf &mrac_conf, const double dt) {
...
@@ -152,19 +147,13 @@ void MracController::Init(const MracConf &mrac_conf, const double dt) {
// Initialize the reference model parameters
// Initialize the reference model parameters
matrix_a_reference_
=
Matrix
::
Zero
(
model_order_
,
model_order_
);
matrix_a_reference_
=
Matrix
::
Zero
(
model_order_
,
model_order_
);
matrix_b_reference_
=
Matrix
::
Zero
(
model_order_
,
1
);
matrix_b_reference_
=
Matrix
::
Zero
(
model_order_
,
1
);
if
(
SetReferenceModel
(
mrac_conf
).
ok
())
{
reference_model_enabled_
=
BuildReferenceModel
();
(
SetReferenceModel
(
mrac_conf
).
ok
()
&&
BuildReferenceModel
().
ok
());
}
else
{
reference_model_enabled_
=
false
;
}
// Initialize the adaption model parameters
// Initialize the adaption model parameters
matrix_p_adaption_
=
Matrix
::
Zero
(
model_order_
,
model_order_
);
matrix_p_adaption_
=
Matrix
::
Zero
(
model_order_
,
model_order_
);
matrix_b_adaption_
=
Matrix
::
Zero
(
model_order_
,
1
);
matrix_b_adaption_
=
Matrix
::
Zero
(
model_order_
,
1
);
if
(
SetAdaptionModel
(
mrac_conf
).
ok
())
{
adaption_model_enabled_
=
BuildAdaptionModel
();
(
SetAdaptionModel
(
mrac_conf
).
ok
()
&&
BuildAdaptionModel
().
ok
());
}
else
{
adaption_model_enabled_
=
false
;
}
}
}
Status
MracController
::
SetReferenceModel
(
const
MracConf
&
mrac_conf
)
{
Status
MracController
::
SetReferenceModel
(
const
MracConf
&
mrac_conf
)
{
...
@@ -221,22 +210,13 @@ Status MracController::SetAdaptionModel(const MracConf &mrac_conf) {
...
@@ -221,22 +210,13 @@ Status MracController::SetAdaptionModel(const MracConf &mrac_conf) {
return
Status
::
OK
();
return
Status
::
OK
();
}
}
void
MracController
::
BuildReferenceModel
()
{
Status
MracController
::
BuildReferenceModel
()
{
reference_model_enabled_
=
true
;
if
(
ts_
<=
0.0
)
{
AWARN
<<
"ts_ <= 0, continuous-discrete transformation for reference model "
"failed, ts_: "
<<
ts_
;
reference_model_enabled_
=
false
;
return
;
}
if
(
model_order_
>
2
)
{
if
(
model_order_
>
2
)
{
AWARN
<<
"reference model order beyond the designed range, "
const
auto
error_msg
=
"model_order: "
StrCat
(
"mrac controller error: reference model order "
,
model_order_
,
<<
model_order_
;
" is beyond the designed range"
)
;
reference_model_enabled_
=
false
;
AERROR
<<
error_msg
;
return
;
return
Status
(
ErrorCode
::
CONTROL_INIT_ERROR
,
error_msg
)
;
}
}
if
(
model_order_
==
1
)
{
if
(
model_order_
==
1
)
{
matrix_a_reference_
(
0
,
0
)
=
-
1.0
/
tau_reference_
;
matrix_a_reference_
(
0
,
0
)
=
-
1.0
/
tau_reference_
;
...
@@ -247,16 +227,16 @@ void MracController::BuildReferenceModel() {
...
@@ -247,16 +227,16 @@ void MracController::BuildReferenceModel() {
matrix_a_reference_
(
1
,
1
)
=
-
2
*
zeta_reference_
*
wn_reference_
;
matrix_a_reference_
(
1
,
1
)
=
-
2
*
zeta_reference_
*
wn_reference_
;
matrix_b_reference_
(
1
,
0
)
=
wn_reference_
*
wn_reference_
;
matrix_b_reference_
(
1
,
0
)
=
wn_reference_
*
wn_reference_
;
}
}
return
Status
::
OK
();
}
}
void
MracController
::
BuildAdaptionModel
()
{
Status
MracController
::
BuildAdaptionModel
()
{
adaption_model_enabled_
=
true
;
if
(
model_order_
>
2
)
{
if
(
model_order_
>
2
)
{
AWARN
<<
"Adaption model order beyond the designed range, "
const
auto
error_msg
=
"model_order: "
StrCat
(
"mrac controller error: adaption model order "
,
model_order_
,
<<
model_order_
;
" is beyond the designed range"
)
;
adaption_model_enabled_
=
false
;
AERROR
<<
error_msg
;
return
;
return
Status
(
ErrorCode
::
CONTROL_INIT_ERROR
,
error_msg
)
;
}
}
if
(
model_order_
==
1
)
{
if
(
model_order_
==
1
)
{
matrix_b_adaption_
(
0
,
0
)
=
1.0
;
matrix_b_adaption_
(
0
,
0
)
=
1.0
;
...
@@ -264,10 +244,13 @@ void MracController::BuildAdaptionModel() {
...
@@ -264,10 +244,13 @@ void MracController::BuildAdaptionModel() {
matrix_b_adaption_
(
1
,
0
)
=
wn_reference_
*
wn_reference_
;
matrix_b_adaption_
(
1
,
0
)
=
wn_reference_
*
wn_reference_
;
}
}
if
(
!
CheckLyapunovPD
(
matrix_a_reference_
,
matrix_p_adaption_
))
{
if
(
!
CheckLyapunovPD
(
matrix_a_reference_
,
matrix_p_adaption_
))
{
AWARN
<<
"Solution of the algebraic Lyapunov equation is not symmetric "
const
std
::
string
error_msg
=
"positive definite"
;
"Solution of the algebraic Lyapunov equation is not symmetric positive "
adaption_model_enabled_
=
false
;
"definite"
;
AERROR
<<
error_msg
;
return
Status
(
ErrorCode
::
CONTROL_INIT_ERROR
,
error_msg
);
}
}
return
Status
::
OK
();
}
}
bool
MracController
::
CheckLyapunovPD
(
const
Matrix
matrix_a
,
bool
MracController
::
CheckLyapunovPD
(
const
Matrix
matrix_a
,
...
...
modules/control/common/mrac_controller.h
浏览文件 @
a3d37ef2
...
@@ -66,15 +66,16 @@ class MracController {
...
@@ -66,15 +66,16 @@ class MracController {
/**
/**
* @brief build mrac (1st or 2nd) order reference model in the discrete-time
* @brief build mrac (1st or 2nd) order reference model in the discrete-time
form, with the bilinear transform (trapezoidal integration) method
form, with the bilinear transform (trapezoidal integration) method
* @return Status reference model initialization status
*/
*/
void
BuildReferenceModel
();
common
::
Status
BuildReferenceModel
();
/**
/**
* @brief build mrac (1st or 2nd) order adaptive dynamic model in the
* @brief build mrac (1st or 2nd) order adaptive dynamic model in the
* discrete-time form
* discrete-time form
* @
param none
* @
return Status adaption model initialization status
*/
*/
void
BuildAdaptionModel
();
common
::
Status
BuildAdaptionModel
();
/**
/**
* @brief check if the solution of the algebraic Lyapunov Equation is
* @brief check if the solution of the algebraic Lyapunov Equation is
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录