Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
bab0e80e
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,体验更适合开发者的 AI 搜索 >>
提交
bab0e80e
编写于
7月 18, 2018
作者:
Y
YajiaZhang
提交者:
Yajia Zhang
7月 18, 2018
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
control: minor code restructure for mpc controller
上级
a096054d
变更
2
隐藏空白更改
内联
并排
Showing
2 changed file
with
9 addition
and
33 deletion
+9
-33
modules/control/controller/mpc_controller.cc
modules/control/controller/mpc_controller.cc
+6
-28
modules/control/controller/mpc_controller.h
modules/control/controller/mpc_controller.h
+3
-5
未找到文件。
modules/control/controller/mpc_controller.cc
浏览文件 @
bab0e80e
...
...
@@ -159,7 +159,7 @@ Status MPCController::Init(const ControlConf *control_conf) {
matrix_a_
(
3
,
2
)
=
(
lf_
*
cf_
-
lr_
*
cr_
)
/
iz_
;
matrix_a_
(
4
,
5
)
=
1.0
;
matrix_a_
(
5
,
5
)
=
0.0
;
// TODO(QiL): expand the model to accom
en
date more combined states.
// TODO(QiL): expand the model to accom
mo
date more combined states.
matrix_a_coeff_
=
Matrix
::
Zero
(
basic_state_size_
,
basic_state_size_
);
matrix_a_coeff_
(
1
,
1
)
=
-
(
cf_
+
cr_
)
/
mass_
;
...
...
@@ -261,7 +261,7 @@ void MPCController::LoadMPCGainScheduler(
feedforwardterm_interpolation_
.
reset
(
new
Interpolation1D
);
CHECK
(
feedforwardterm_interpolation_
->
Init
(
xy2
))
<<
"Fail to load feed
forward
term gain scheduler for MPC controller"
;
<<
"Fail to load feed
forward
term gain scheduler for MPC controller"
;
steer_weight_interpolation_
.
reset
(
new
Interpolation1D
);
CHECK
(
steer_weight_interpolation_
->
Init
(
xy2
))
...
...
@@ -284,13 +284,13 @@ Status MPCController::ComputeControlCommand(
ComputeLongitudinalErrors
(
&
trajectory_analyzer_
,
debug
);
// Update state
UpdateState
AnalyticalMatching
(
debug
);
UpdateState
(
debug
);
UpdateMatrix
(
debug
);
FeedforwardUpdate
(
debug
);
// Add gain sheduler for higher speed steering
// Add gain s
c
heduler for higher speed steering
if
(
FLAGS_enable_gain_scheduler
)
{
matrix_q_updated_
(
0
,
0
)
=
matrix_q_
(
0
,
0
)
*
...
...
@@ -391,7 +391,7 @@ Status MPCController::ComputeControlCommand(
debug
->
set_acceleration_cmd_closeloop
(
acc_feedback
);
double
acceleration_cmd
=
acc_feedback
+
debug
->
acceleration_reference
();
// TODO(QiL): add pitch angle feed
forward to accomen
date for 3D control
// TODO(QiL): add pitch angle feed
forward to accommo
date for 3D control
debug
->
set_is_full_stop
(
false
);
if
(
std
::
fabs
(
debug
->
acceleration_reference
())
<=
...
...
@@ -475,7 +475,7 @@ void MPCController::LoadControlCalibrationTable(
<<
"Fail to load control calibration table"
;
}
void
MPCController
::
UpdateState
AnalyticalMatching
(
SimpleMPCDebug
*
debug
)
{
void
MPCController
::
UpdateState
(
SimpleMPCDebug
*
debug
)
{
const
auto
&
com
=
VehicleStateProvider
::
instance
()
->
ComputeCOMPosition
(
lr_
);
ComputeLateralErrors
(
com
.
x
(),
com
.
y
(),
VehicleStateProvider
::
instance
()
->
heading
(),
...
...
@@ -509,28 +509,6 @@ void MPCController::UpdateMatrix(SimpleMPCDebug *debug) {
matrix_cd_
=
matrix_c_
*
debug
->
heading_error_rate
()
*
ts_
;
}
/*
* SL coordinate system:
* left to the ref_line, L is +
* right to the ref_line, L is -
*/
double
MPCController
::
GetLateralError
(
const
common
::
math
::
Vec2d
&
point
,
TrajectoryPoint
*
traj_point
)
const
{
const
auto
closest
=
trajectory_analyzer_
.
QueryNearestPointByPosition
(
point
.
x
(),
point
.
y
());
const
double
point_angle
=
std
::
atan2
(
point
.
y
()
-
closest
.
path_point
().
y
(),
point
.
x
()
-
closest
.
path_point
().
x
());
const
double
point2path_angle
=
point_angle
-
closest
.
path_point
().
theta
();
if
(
traj_point
!=
nullptr
)
{
*
traj_point
=
closest
;
}
const
double
dx
=
closest
.
path_point
().
x
()
-
point
.
x
();
const
double
dy
=
closest
.
path_point
().
y
()
-
point
.
y
();
return
std
::
sin
(
point2path_angle
)
*
std
::
sqrt
(
dx
*
dx
+
dy
*
dy
);
}
void
MPCController
::
FeedforwardUpdate
(
SimpleMPCDebug
*
debug
)
{
steer_angle_feedforwardterm_
=
(
wheelbase_
*
debug
->
curvature
())
*
180
/
M_PI
*
steer_transmission_ratio_
/
...
...
modules/control/controller/mpc_controller.h
浏览文件 @
bab0e80e
...
...
@@ -101,16 +101,12 @@ class MPCController : public Controller {
std
::
string
Name
()
const
override
;
protected:
void
UpdateState
AnalyticalMatching
(
SimpleMPCDebug
*
debug
);
void
UpdateState
(
SimpleMPCDebug
*
debug
);
void
UpdateMatrix
(
SimpleMPCDebug
*
debug
);
void
FeedforwardUpdate
(
SimpleMPCDebug
*
debug
);
double
GetLateralError
(
const
common
::
math
::
Vec2d
&
point
,
apollo
::
common
::
TrajectoryPoint
*
trajectory_point
)
const
;
void
ComputeLateralErrors
(
const
double
x
,
const
double
y
,
const
double
theta
,
const
double
linear_v
,
const
double
angular_v
,
const
TrajectoryAnalyzer
&
trajectory_analyzer
,
...
...
@@ -120,7 +116,9 @@ class MPCController : public Controller {
SimpleMPCDebug
*
debug
);
bool
LoadControlConf
(
const
ControlConf
*
control_conf
);
void
InitializeFilters
(
const
ControlConf
*
control_conf
);
void
LogInitParameters
();
void
ProcessLogs
(
const
SimpleMPCDebug
*
debug
,
const
canbus
::
Chassis
*
chassis
);
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录