Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
3940e41d
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,发现更多精彩内容 >>
提交
3940e41d
编写于
11月 02, 2017
作者:
Q
Qi Luo
提交者:
Yifei Jiang
11月 02, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Control: Update filter for test
上级
bee1a12d
变更
3
隐藏空白更改
内联
并排
Showing
3 changed file
with
14 addition
and
63 deletion
+14
-63
modules/control/controller/lat_controller.cc
modules/control/controller/lat_controller.cc
+12
-59
modules/control/controller/lat_controller.h
modules/control/controller/lat_controller.h
+1
-3
modules/control/controller/lat_controller_test.cc
modules/control/controller/lat_controller_test.cc
+1
-1
未找到文件。
modules/control/controller/lat_controller.cc
浏览文件 @
3940e41d
...
...
@@ -375,62 +375,6 @@ Status LatController::Reset() {
return
Status
::
OK
();
}
// state = [Lateral Error, Lateral Error Rate, Heading Error, Heading Error
// Rate, Preview Lateral1, Preview Lateral2, ...]
void
LatController
::
UpdateState
(
SimpleLateralDebug
*
debug
)
{
TrajectoryPoint
traj_point
;
const
auto
&
position
=
VehicleState
::
instance
()
->
ComputeCOMPosition
(
lr_
);
double
raw_lateral_error
=
GetLateralError
(
position
,
&
traj_point
);
// lateral_error_ = lateral_rate_filter_.Filter(raw_lateral_error);
debug
->
set_lateral_error
(
lateral_error_filter_
.
Update
(
raw_lateral_error
));
// ref_curvature_ = traj_point.kappa();
debug
->
set_curvature
(
traj_point
.
path_point
().
kappa
());
// ref_heading_ = traj_point.theta;
debug
->
set_ref_heading
(
traj_point
.
path_point
().
theta
());
// heading_error_ =
// common::math::NormalizeAngle(VehicleState::instance()->heading() -
// ref_heading_);
debug
->
set_heading_error
(
common
::
math
::
NormalizeAngle
(
VehicleState
::
instance
()
->
heading
()
-
traj_point
.
path_point
().
theta
()));
// Reverse heading error if vehicle is going in reverse
if
(
VehicleState
::
instance
()
->
gear
()
==
canbus
::
Chassis
::
GEAR_REVERSE
)
{
debug
->
set_heading_error
(
-
debug
->
heading_error
());
}
// heading_error_rate_ = (heading_error_ - previous_heading_error_) / ts_;
debug
->
set_heading_error_rate
(
(
debug
->
heading_error
()
-
previous_heading_error_
)
/
ts_
);
// lateral_error_rate_ = (lateral_error_ - previous_lateral_error_) / ts_;
debug
->
set_lateral_error_rate
(
(
debug
->
lateral_error
()
-
previous_lateral_error_
)
/
ts_
);
// Prepare for next iteration.
previous_heading_error_
=
debug
->
heading_error
();
previous_lateral_error_
=
debug
->
lateral_error
();
// State matrix update;
// First four elements are fixed;
matrix_state_
(
0
,
0
)
=
debug
->
lateral_error
();
matrix_state_
(
1
,
0
)
=
debug
->
lateral_error_rate
();
matrix_state_
(
2
,
0
)
=
debug
->
heading_error
();
matrix_state_
(
3
,
0
)
=
debug
->
heading_error_rate
();
// Next elements are depending on preview window size;
for
(
int
i
=
0
;
i
<
preview_window_
;
++
i
)
{
double
preview_time
=
ts_
*
(
i
+
1
);
const
auto
&
future_position_estimate
=
VehicleState
::
instance
()
->
EstimateFuturePosition
(
preview_time
);
double
preview_lateral
=
GetLateralError
(
future_position_estimate
,
nullptr
);
matrix_state_
(
basic_state_size_
+
i
,
0
)
=
preview_lateral
;
}
// preview matrix update;
}
void
LatController
::
UpdateStateAnalyticalMatching
(
SimpleLateralDebug
*
debug
)
{
if
(
FLAGS_use_relative_position
)
{
ComputeLateralErrors
(
0.0
,
0.0
,
VehicleState
::
instance
()
->
heading
(),
...
...
@@ -542,7 +486,7 @@ double LatController::GetLateralError(const common::math::Vec2d &point,
void
LatController
::
ComputeLateralErrors
(
const
double
x
,
const
double
y
,
const
double
theta
,
const
double
linear_v
,
const
double
angular_v
,
const
TrajectoryAnalyzer
&
trajectory_analyzer
,
SimpleLateralDebug
*
debug
)
const
{
SimpleLateralDebug
*
debug
)
{
// TODO(QiL): change this to conf.
::
apollo
::
common
::
TrajectoryPoint
target_point
;
if
(
FLAGS_use_relative_position
)
{
...
...
@@ -562,8 +506,17 @@ void LatController::ComputeLateralErrors(
double
cos_matched_theta
=
std
::
cos
(
target_point
.
path_point
().
theta
());
double
sin_matched_theta
=
std
::
sin
(
target_point
.
path_point
().
theta
());
// d_error = cos_matched_theta * dy - sin_matched_theta * dx;
debug
->
set_lateral_error
(
cos_matched_theta
*
dy
-
sin_matched_theta
*
dx
);
// lateral_error_ = lateral_rate_filter_.Filter(raw_lateral_error
);
// TODO(QiL): Code reformat when done with test
double
raw_lateral_error
=
cos_matched_theta
*
dy
-
sin_matched_theta
*
dx
;
if
(
FLAGS_use_relative_position
)
{
double
filtered_lateral_error
=
lateral_error_filter_
.
Update
(
raw_lateral_error
);
debug
->
set_lateral_error
(
filtered_lateral_error
);
}
else
{
debug
->
set_lateral_error
(
raw_lateral_error
);
}
double
delta_theta
=
common
::
math
::
NormalizeAngle
(
theta
-
target_point
.
path_point
().
theta
());
double
sin_delta_theta
=
std
::
sin
(
delta_theta
);
...
...
@@ -577,7 +530,7 @@ void LatController::ComputeLateralErrors(
debug
->
set_heading_error_rate
(
angular_v
-
target_point
.
path_point
().
kappa
()
*
target_point
.
v
());
// matched_theta =
target_point
.path_point().theta();
// matched_theta =
3
.path_point().theta();
debug
->
set_ref_heading
(
target_point
.
path_point
().
theta
());
// matched_kappa = target_point.path_point().kappa();
debug
->
set_curvature
(
target_point
.
path_point
().
kappa
());
...
...
modules/control/controller/lat_controller.h
浏览文件 @
3940e41d
...
...
@@ -101,8 +101,6 @@ class LatController : public Controller {
std
::
string
Name
()
const
override
;
protected:
void
UpdateState
(
SimpleLateralDebug
*
debug
);
void
UpdateStateAnalyticalMatching
(
SimpleLateralDebug
*
debug
);
void
UpdateMatrix
();
...
...
@@ -118,7 +116,7 @@ class LatController : public Controller {
void
ComputeLateralErrors
(
const
double
x
,
const
double
y
,
const
double
theta
,
const
double
linear_v
,
const
double
angular_v
,
const
TrajectoryAnalyzer
&
trajectory_analyzer
,
SimpleLateralDebug
*
debug
)
const
;
SimpleLateralDebug
*
debug
);
bool
LoadControlConf
(
const
ControlConf
*
control_conf
);
void
InitializeFilters
(
const
ControlConf
*
control_conf
);
void
LoadLatGainScheduler
(
const
LatControllerConf
&
lat_controller_conf
);
...
...
modules/control/controller/lat_controller_test.cc
浏览文件 @
3940e41d
...
...
@@ -57,7 +57,7 @@ class LatControllerTest : public ::testing::Test, LatController {
void
ComputeLateralErrors
(
const
double
x
,
const
double
y
,
const
double
theta
,
const
double
linear_v
,
const
double
angular_v
,
const
TrajectoryAnalyzer
&
trajectory_analyzer
,
SimpleLateralDebug
*
debug
)
const
{
SimpleLateralDebug
*
debug
)
{
LatController
::
ComputeLateralErrors
(
x
,
y
,
theta
,
linear_v
,
angular_v
,
trajectory_analyzer
,
debug
);
}
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录