Skip to content
体验新版
项目
组织
正在加载...
登录
切换导航
打开侧边栏
Pinoxchio
apollo
提交
f9d8e733
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,发现更多精彩内容 >>
提交
f9d8e733
编写于
9月 06, 2017
作者:
H
henryhu6
提交者:
lianglia-apollo
9月 06, 2017
浏览文件
操作
浏览文件
下载
电子邮件补丁
差异文件
Control: Use dynamic time interval
上级
1347236d
变更
17
隐藏空白更改
内联
并排
Showing
17 changed file
with
76 addition
and
290 deletion
+76
-290
modules/control/conf/lincoln.pb.txt
modules/control/conf/lincoln.pb.txt
+2
-2
modules/control/controller/BUILD
modules/control/controller/BUILD
+0
-2
modules/control/controller/lat_controller.cc
modules/control/controller/lat_controller.cc
+11
-11
modules/control/controller/lat_controller.h
modules/control/controller/lat_controller.h
+2
-3
modules/control/controller/lon_controller.cc
modules/control/controller/lon_controller.cc
+13
-22
modules/control/controller/lon_controller.h
modules/control/controller/lon_controller.h
+4
-6
modules/control/controller/lon_controller_test.cc
modules/control/controller/lon_controller_test.cc
+1
-2
modules/control/filters/BUILD
modules/control/filters/BUILD
+0
-25
modules/control/filters/digital_filter.cc
modules/control/filters/digital_filter.cc
+24
-15
modules/control/filters/digital_filter.h
modules/control/filters/digital_filter.h
+2
-16
modules/control/filters/digital_filter_coefficients.cc
modules/control/filters/digital_filter_coefficients.cc
+0
-52
modules/control/filters/digital_filter_coefficients.h
modules/control/filters/digital_filter_coefficients.h
+0
-48
modules/control/filters/digital_filter_coefficients_test.cc
modules/control/filters/digital_filter_coefficients_test.cc
+0
-45
modules/control/filters/digital_filter_test.cc
modules/control/filters/digital_filter_test.cc
+13
-36
modules/control/proto/lat_controller_conf.proto
modules/control/proto/lat_controller_conf.proto
+1
-2
modules/control/proto/lon_controller_conf.proto
modules/control/proto/lon_controller_conf.proto
+1
-1
modules/control/testdata/conf/lincoln.pb.txt
modules/control/testdata/conf/lincoln.pb.txt
+2
-2
未找到文件。
modules/control/conf/lincoln.pb.txt
浏览文件 @
f9d8e733
...
...
@@ -13,7 +13,7 @@ max_steering_percentage_allowed: 100
minimum_speed_resolution: 0.2
lat_controller_conf {
ts: 0.01
preview_
window:
0
preview_
time: 0.
0
cf: 155494.663
cr: 155494.663
mass_fl: 520
...
...
@@ -80,7 +80,7 @@ lon_controller_conf {
throttle_deadzone: 18.0
speed_controller_input_limit: 2.0
station_error_limit: 2.0
preview_
window: 20.
0
preview_
time: 0.2
0
standstill_acceleration: -3.0
station_pid_conf {
integrator_enable: false
...
...
modules/control/controller/BUILD
浏览文件 @
f9d8e733
...
...
@@ -33,7 +33,6 @@ cc_library(
"//modules/control/common:interpolation_1d"
,
"//modules/control/common:trajectory_analyzer"
,
"//modules/control/filters:digital_filter"
,
"//modules/control/filters:digital_filter_coefficients"
,
"//modules/control/filters:mean_filter"
,
"//modules/control/proto:control_proto"
,
"@eigen//:eigen"
,
...
...
@@ -59,7 +58,6 @@ cc_library(
"//modules/control/common:pid_controller"
,
"//modules/control/common:trajectory_analyzer"
,
"//modules/control/filters:digital_filter"
,
"//modules/control/filters:digital_filter_coefficients"
,
"//modules/localization/common:localization_common"
,
],
)
...
...
modules/control/controller/lat_controller.cc
浏览文件 @
f9d8e733
...
...
@@ -100,7 +100,7 @@ bool LatController::LoadControlConf(const ControlConf *control_conf) {
CHECK_GT
(
ts_
,
0.0
)
<<
"[LatController] Invalid control update interval."
;
cf_
=
control_conf
->
lat_controller_conf
().
cf
();
cr_
=
control_conf
->
lat_controller_conf
().
cr
();
preview_
window_
=
control_conf
->
lat_controller_conf
().
preview_window
();
preview_
time_
=
control_conf
->
lat_controller_conf
().
preview_time
();
wheelbase_
=
vehicle_param_
.
wheel_base
();
steer_transmission_ratio_
=
vehicle_param_
.
steer_ratio
();
steer_single_direction_max_degree_
=
...
...
@@ -156,11 +156,8 @@ void LatController::LogInitParameters() {
void
LatController
::
InitializeFilters
(
const
ControlConf
*
control_conf
)
{
// Low pass filter
std
::
vector
<
double
>
den
(
3
,
0.0
);
std
::
vector
<
double
>
num
(
3
,
0.0
);
LpfCoefficients
(
ts_
,
control_conf
->
lat_controller_conf
().
cutoff_freq
(),
&
den
,
&
num
);
digital_filter_
.
set_coefficients
(
den
,
num
);
digital_filter_
.
set_coefficients
(
ts_
,
control_conf
->
lat_controller_conf
().
cutoff_freq
());
// Mean filters
/**
heading_rate_filter_ = MeanFilter(
...
...
@@ -177,7 +174,7 @@ Status LatController::Init(const ControlConf *control_conf) {
"failed to load control_conf"
);
}
// Matrix init operations.
int
matrix_size
=
basic_state_size_
+
preview_window_
;
int
matrix_size
=
basic_state_size_
+
std
::
floor
(
preview_time_
/
ts_
)
;
matrix_a_
=
Matrix
::
Zero
(
basic_state_size_
,
basic_state_size_
);
matrix_ad_
=
Matrix
::
Zero
(
basic_state_size_
,
basic_state_size_
);
matrix_adc_
=
Matrix
::
Zero
(
matrix_size
,
matrix_size
);
...
...
@@ -423,7 +420,8 @@ void LatController::UpdateState(SimpleLateralDebug *debug) {
matrix_state_
(
3
,
0
)
=
debug
->
heading_error_rate
();
// Next elements are depending on preview window size;
for
(
int
i
=
0
;
i
<
preview_window_
;
++
i
)
{
int
preview_window
=
std
::
floor
(
preview_time_
/
ts_
);
for
(
int
i
=
0
;
i
<
preview_window
;
++
i
)
{
double
preview_time
=
ts_
*
(
i
+
1
);
const
auto
&
future_position_estimate
=
VehicleState
::
instance
()
->
EstimateFuturePosition
(
preview_time
);
...
...
@@ -454,7 +452,8 @@ void LatController::UpdateStateAnalyticalMatching(SimpleLateralDebug *debug) {
matrix_state_
(
3
,
0
)
=
debug
->
heading_error_rate
();
// Next elements are depending on preview window size;
for
(
int
i
=
0
;
i
<
preview_window_
;
++
i
)
{
int
preview_window
=
std
::
floor
(
preview_time_
/
ts_
);
for
(
int
i
=
0
;
i
<
preview_window
;
++
i
)
{
double
preview_time
=
ts_
*
(
i
+
1
);
auto
preview_point
=
trajectory_analyzer_
.
QueryNearestPointByRelativeTime
(
preview_time
);
...
...
@@ -488,10 +487,11 @@ void LatController::UpdateMatrixCompound() {
// Initialize preview matrix
matrix_adc_
.
block
(
0
,
0
,
basic_state_size_
,
basic_state_size_
)
=
matrix_ad_
;
matrix_bdc_
.
block
(
0
,
0
,
basic_state_size_
,
1
)
=
matrix_bd_
;
if
(
preview_
window_
>
0
)
{
if
(
preview_
time_
>
0.
0
)
{
matrix_bdc_
(
matrix_bdc_
.
rows
()
-
1
,
0
)
=
1
;
// Update augument A matrix;
for
(
int
i
=
0
;
i
<
preview_window_
-
1
;
++
i
)
{
int
preview_window
=
std
::
floor
(
preview_time_
/
ts_
);
for
(
int
i
=
0
;
i
<
preview_window
-
1
;
++
i
)
{
matrix_adc_
(
basic_state_size_
+
i
,
basic_state_size_
+
1
+
i
)
=
1
;
}
}
...
...
modules/control/controller/lat_controller.h
浏览文件 @
f9d8e733
...
...
@@ -33,7 +33,6 @@
#include "modules/control/common/trajectory_analyzer.h"
#include "modules/control/controller/controller.h"
#include "modules/control/filters/digital_filter.h"
#include "modules/control/filters/digital_filter_coefficients.h"
#include "modules/control/filters/mean_filter.h"
/**
...
...
@@ -159,8 +158,8 @@ class LatController : public Controller {
// limit steering to maximum theoretical lateral acceleration
double
max_lat_acc_
=
0.0
;
//
number of control cycles look ahead
(preview controller)
int
preview_window
_
=
0
;
//
look ahead time
(preview controller)
double
preview_time
_
=
0
;
// number of states without previews, includes
// lateral error, lateral error rate, heading error, heading error rate
const
int
basic_state_size_
=
4
;
...
...
modules/control/controller/lon_controller.cc
浏览文件 @
f9d8e733
...
...
@@ -103,22 +103,18 @@ Status LonController::Init(const ControlConf *control_conf) {
station_pid_controller_
.
Init
(
lon_controller_conf
.
station_pid_conf
());
speed_pid_controller_
.
Init
(
lon_controller_conf
.
low_speed_pid_conf
());
SetDigitalFilterPitchAngle
(
lon_controller_conf
);
digital_filter_pitch_angle_
.
set_coefficients
(
lon_controller_conf
.
ts
(),
lon_controller_conf
.
pitch_angle_filter_conf
().
cutoff_freq
());
LoadControlCalibrationTable
(
lon_controller_conf
);
previous_control_time_
=
Clock
::
NowInSecond
();
controller_initialized_
=
true
;
return
Status
::
OK
();
}
void
LonController
::
SetDigitalFilterPitchAngle
(
const
LonControllerConf
&
lon_controller_conf
)
{
double
cutoff_freq
=
lon_controller_conf
.
pitch_angle_filter_conf
().
cutoff_freq
();
double
ts
=
lon_controller_conf
.
ts
();
SetDigitalFilter
(
ts
,
cutoff_freq
,
&
digital_filter_pitch_angle_
);
}
void
LonController
::
LoadControlCalibrationTable
(
const
LonControllerConf
&
lon_controller_conf
)
{
const
auto
&
control_table
=
lon_controller_conf
.
calibration_table
();
...
...
@@ -144,6 +140,10 @@ Status LonController::ComputeControlCommand(
localization_
=
localization
;
chassis_
=
chassis
;
current_control_time_
=
Clock
::
NowInSecond
();
dt_
=
current_control_time_
-
previous_control_time_
;
previous_control_time_
=
current_control_time_
;
trajectory_message_
=
planning_published_trajectory
;
if
(
!
control_interpolation_
)
{
AERROR
<<
"Fail to initialize calibration table."
;
...
...
@@ -164,8 +164,7 @@ Status LonController::ComputeControlCommand(
double
brake_cmd
=
0.0
;
double
throttle_cmd
=
0.0
;
double
ts
=
lon_controller_conf
.
ts
();
double
preview_time
=
lon_controller_conf
.
preview_window
()
*
ts
;
double
preview_time
=
lon_controller_conf
.
preview_time
();
if
(
preview_time
<
0.0
)
{
const
auto
error_msg
=
apollo
::
common
::
util
::
StrCat
(
...
...
@@ -186,7 +185,7 @@ Status LonController::ComputeControlCommand(
debug
->
station_error
(),
-
station_error_limit
,
station_error_limit
);
}
double
speed_offset
=
station_pid_controller_
.
Control
(
station_error_limited
,
ts
);
station_pid_controller_
.
Control
(
station_error_limited
,
dt_
);
double
speed_controller_input
=
0.0
;
double
speed_controller_input_limit
=
...
...
@@ -206,11 +205,11 @@ Status LonController::ComputeControlCommand(
lon_controller_conf
.
switch_speed
())
{
speed_pid_controller_
.
SetPID
(
lon_controller_conf
.
low_speed_pid_conf
());
acceleration_cmd_closeloop
=
speed_pid_controller_
.
Control
(
speed_controller_input_limited
,
ts
);
speed_pid_controller_
.
Control
(
speed_controller_input_limited
,
dt_
);
}
else
{
speed_pid_controller_
.
SetPID
(
lon_controller_conf
.
high_speed_pid_conf
());
acceleration_cmd_closeloop
=
speed_pid_controller_
.
Control
(
speed_controller_input_limited
,
ts
);
speed_pid_controller_
.
Control
(
speed_controller_input_limited
,
dt_
);
}
double
acceleration_cmd
=
...
...
@@ -342,13 +341,5 @@ void LonController::ComputeLongitudinalErrors(
debug
->
set_preview_acceleration_reference
(
preview_point
.
a
());
}
void
LonController
::
SetDigitalFilter
(
double
ts
,
double
cutoff_freq
,
DigitalFilter
*
digital_filter
)
{
std
::
vector
<
double
>
denominators
;
std
::
vector
<
double
>
numerators
;
LpfCoefficients
(
ts
,
cutoff_freq
,
&
denominators
,
&
numerators
);
digital_filter
->
set_coefficients
(
denominators
,
numerators
);
}
}
// namespace control
}
// namespace apollo
modules/control/controller/lon_controller.h
浏览文件 @
f9d8e733
...
...
@@ -33,7 +33,6 @@
#include "modules/control/common/trajectory_analyzer.h"
#include "modules/control/controller/controller.h"
#include "modules/control/filters/digital_filter.h"
#include "modules/control/filters/digital_filter_coefficients.h"
/**
* @namespace apollo::control
...
...
@@ -104,14 +103,9 @@ class LonController : public Controller {
SimpleLongitudinalDebug
*
debug
);
private:
void
SetDigitalFilterPitchAngle
(
const
LonControllerConf
&
lon_controller_conf
);
void
LoadControlCalibrationTable
(
const
LonControllerConf
&
lon_controller_conf
);
void
SetDigitalFilter
(
double
ts
,
double
cutoff_freq
,
DigitalFilter
*
digital_filter
);
void
CloseLogFile
();
const
::
apollo
::
localization
::
LocalizationEstimate
*
localization_
=
nullptr
;
...
...
@@ -131,6 +125,10 @@ class LonController : public Controller {
DigitalFilter
digital_filter_pitch_angle_
;
double
current_control_time_
=
0.0
;
double
previous_control_time_
=
0.0
;
double
dt_
=
0.0
;
const
ControlConf
*
control_conf_
=
nullptr
;
};
}
// namespace control
...
...
modules/control/controller/lon_controller_test.cc
浏览文件 @
f9d8e733
...
...
@@ -121,8 +121,7 @@ TEST_F(LonControllerTest, ComputeLongitudinalErrors) {
vehicle_state
->
Update
(
localization_pb
,
chassis_pb
);
TrajectoryAnalyzer
trajectory_analyzer
(
&
trajectory_pb
);
double
ts
=
longitudinal_conf_
.
ts
();
double
preview_time
=
longitudinal_conf_
.
preview_window
()
*
ts
;
double
preview_time
=
longitudinal_conf_
.
preview_time
();
SimpleLongitudinalDebug
debug
;
ComputeLongitudinalErrors
(
&
trajectory_analyzer
,
preview_time
,
&
debug
);
...
...
modules/control/filters/BUILD
浏览文件 @
f9d8e733
...
...
@@ -11,7 +11,6 @@ cc_library(
"digital_filter.h"
,
],
deps
=
[
":digital_filter_coefficients"
,
"//modules/common:log"
,
],
)
...
...
@@ -29,21 +28,10 @@ cc_library(
],
)
cc_library
(
name
=
"digital_filter_coefficients"
,
srcs
=
[
"digital_filter_coefficients.cc"
,
],
hdrs
=
[
"digital_filter_coefficients.h"
,
],
)
cc_library
(
name
=
"filters"
,
deps
=
[
":digital_filter"
,
":digital_filter_coefficients"
,
":mean_filter"
,
],
)
...
...
@@ -56,7 +44,6 @@ cc_test(
],
deps
=
[
":digital_filter"
,
":digital_filter_coefficients"
,
"//modules/common"
,
"@gtest//:main"
,
"@ros//:ros_common"
,
...
...
@@ -75,16 +62,4 @@ cc_test(
],
)
cc_test
(
name
=
"digital_filter_coefficients_test"
,
size
=
"small"
,
srcs
=
[
"digital_filter_coefficients_test.cc"
,
],
deps
=
[
":digital_filter_coefficients"
,
"@gtest//:main"
,
],
)
cpplint
()
modules/control/filters/digital_filter.cc
浏览文件 @
f9d8e733
...
...
@@ -29,27 +29,36 @@ const double kDoubleEpsilon = 1.0e-6;
namespace
apollo
{
namespace
control
{
DigitalFilter
::
DigitalFilter
(
const
std
::
vector
<
double
>
&
denominators
,
const
std
::
vector
<
double
>
&
numerators
)
{
DigitalFilter
::
DigitalFilter
(
const
double
ts
,
const
double
cutoff_freq
)
{
dead_zone_
=
0.0
;
last_
=
0.0
;
set_coefficients
(
denominators
,
numerators
);
}
void
DigitalFilter
::
set_denominators
(
const
std
::
vector
<
double
>
&
denominators
)
{
denominators_
=
denominators
;
y_values_
.
resize
(
denominators_
.
size
(),
0.0
);
set_coefficients
(
ts
,
cutoff_freq
);
}
void
DigitalFilter
::
set_numerators
(
const
std
::
vector
<
double
>
&
numerators
)
{
numerators_
=
numerators
;
x_values_
.
resize
(
numerators_
.
size
(),
0.0
);
}
void
DigitalFilter
::
set_coefficients
(
const
double
ts
,
const
double
cutoff_freq
)
{
denominators_
.
clear
();
numerators_
.
clear
();
denominators_
.
reserve
(
3
);
numerators_
.
reserve
(
3
);
double
wa
=
2.0
*
M_PI
*
cutoff_freq
;
// Analog frequency in rad/s
double
alpha
=
wa
*
ts
/
2.0
;
// tan(Wd/2), Wd is discrete frequency
double
alpha_sqr
=
alpha
*
alpha
;
double
tmp_term
=
std
::
sqrt
(
2.0
)
*
alpha
+
alpha_sqr
;
double
gain
=
alpha_sqr
/
(
1.0
+
tmp_term
);
void
DigitalFilter
::
set_coefficients
(
const
std
::
vector
<
double
>
&
denominators
,
const
std
::
vector
<
double
>
&
numerators
)
{
set_denominators
(
denominators
);
set_numerators
(
numerators
);
denominators_
.
push_back
(
1.0
);
denominators_
.
push_back
(
2.0
*
(
alpha_sqr
-
1.0
)
/
(
1.0
+
tmp_term
));
denominators_
.
push_back
((
1.0
-
std
::
sqrt
(
2.0
)
*
alpha
+
alpha_sqr
)
/
(
1.0
+
tmp_term
));
numerators_
.
push_back
(
gain
);
numerators_
.
push_back
(
2.0
*
gain
);
numerators_
.
push_back
(
gain
);
x_values_
.
resize
(
numerators_
.
size
(),
0.0
);
y_values_
.
resize
(
denominators_
.
size
(),
0.0
);
}
void
DigitalFilter
::
set_dead_zone
(
const
double
deadzone
)
{
...
...
modules/control/filters/digital_filter.h
浏览文件 @
f9d8e733
...
...
@@ -47,8 +47,7 @@ class DigitalFilter {
* @param denominators The denominators of the DigitalFilter.
* @param numerators The numerators of the DigitalFilter.
*/
DigitalFilter
(
const
std
::
vector
<
double
>
&
denominators
,
const
std
::
vector
<
double
>
&
numerators
);
DigitalFilter
(
const
double
ts
,
const
double
cutoff_freq
);
/**
* @brief Default destructor.
...
...
@@ -71,25 +70,12 @@ class DigitalFilter {
* Output: y[0]
*/
/**
* @brief set denominators by an input vector
* @param denominators The denominators of filter
*/
void
set_denominators
(
const
std
::
vector
<
double
>
&
denominators
);
/**
* @brief set numerators by an input vector
* @param numerators The numerators of filter
*/
void
set_numerators
(
const
std
::
vector
<
double
>
&
numerators
);
/**
* @brief set denominators and numerators
* @param denominators The denominators of filter
* @param numerators The numerators of filter
*/
void
set_coefficients
(
const
std
::
vector
<
double
>
&
denominators
,
const
std
::
vector
<
double
>
&
numerators
);
void
set_coefficients
(
const
double
ts
,
const
double
cutoff_freq
);
/**
* @brief set filter deadzone
...
...
modules/control/filters/digital_filter_coefficients.cc
已删除
100644 → 0
浏览文件 @
1347236d
/******************************************************************************
* 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/control/filters/digital_filter_coefficients.h"
#include <cmath>
#include <vector>
namespace
apollo
{
namespace
control
{
void
LpfCoefficients
(
const
double
ts
,
const
double
cutoff_freq
,
std
::
vector
<
double
>
*
denominators
,
std
::
vector
<
double
>
*
numerators
)
{
denominators
->
clear
();
numerators
->
clear
();
denominators
->
reserve
(
3
);
numerators
->
reserve
(
3
);
double
wa
=
2.0
*
M_PI
*
cutoff_freq
;
// Analog frequency in rad/s
double
alpha
=
wa
*
ts
/
2.0
;
// tan(Wd/2), Wd is discrete frequency
double
alpha_sqr
=
alpha
*
alpha
;
double
tmp_term
=
std
::
sqrt
(
2.0
)
*
alpha
+
alpha_sqr
;
double
gain
=
alpha_sqr
/
(
1.0
+
tmp_term
);
denominators
->
push_back
(
1.0
);
denominators
->
push_back
(
2.0
*
(
alpha_sqr
-
1.0
)
/
(
1.0
+
tmp_term
));
denominators
->
push_back
((
1.0
-
std
::
sqrt
(
2.0
)
*
alpha
+
alpha_sqr
)
/
(
1.0
+
tmp_term
));
numerators
->
push_back
(
gain
);
numerators
->
push_back
(
2.0
*
gain
);
numerators
->
push_back
(
gain
);
return
;
}
}
// namespace control
}
// namespace apollo
modules/control/filters/digital_filter_coefficients.h
已删除
100644 → 0
浏览文件 @
1347236d
/******************************************************************************
* 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 digital_filter_coefficients.h
* @brief Functions to generate coefficients for digital filter.
*/
#ifndef MODULES_CONTROL_FILTERS_DIGITAL_FILTER_COEFFICIENTS_H_
#define MODULES_CONTROL_FILTERS_DIGITAL_FILTER_COEFFICIENTS_H_
#include <vector>
/**
* @namespace apollo::control
* @brief apollo::control
*/
namespace
apollo
{
namespace
control
{
/**
* @brief Get low-pass coefficients for digital filter.
* @param ts Time interval between signals.
* @param cutoff_freq Cutoff of frequency to filter high-frequency signals out.
* @param denominators Denominator coefficients for digital filter.
* @param numerators Numerator coefficients for digital filter.
*/
void
LpfCoefficients
(
const
double
ts
,
const
double
cutoff_freq
,
std
::
vector
<
double
>
*
denominators
,
std
::
vector
<
double
>
*
numerators
);
}
// namespace control
}
// namespace apollo
#endif // MODULES_CONTROL_FILTERS_DIGITAL_FILTER_COEFFICIENTS_H_
modules/control/filters/digital_filter_coefficients_test.cc
已删除
100644 → 0
浏览文件 @
1347236d
/******************************************************************************
* 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/control/filters/digital_filter_coefficients.h"
#include "gtest/gtest.h"
namespace
apollo
{
namespace
control
{
class
DigitalFilterCoefficientsTest
:
public
::
testing
::
Test
{
public:
virtual
void
SetUp
()
{}
};
TEST_F
(
DigitalFilterCoefficientsTest
,
LpfCoefficients
)
{
double
ts
=
0.01
;
double
cutoff_freq
=
20
;
std
::
vector
<
double
>
den
;
std
::
vector
<
double
>
num
;
LpfCoefficients
(
ts
,
cutoff_freq
,
&
den
,
&
num
);
EXPECT_EQ
(
den
.
size
(),
3
);
EXPECT_EQ
(
num
.
size
(),
3
);
EXPECT_NEAR
(
num
[
0
],
0.1729
,
0.01
);
EXPECT_NEAR
(
num
[
1
],
0.3458
,
0.01
);
EXPECT_NEAR
(
num
[
2
],
0.1729
,
0.01
);
EXPECT_NEAR
(
den
[
0
],
1.0
,
0.01
);
EXPECT_NEAR
(
den
[
2
],
0.2217
,
0.01
);
}
}
// namespace control
}
// namespace apollo
modules/control/filters/digital_filter_test.cc
浏览文件 @
f9d8e733
...
...
@@ -30,43 +30,24 @@ class DigitalFilterTest : public ::testing::Test {
TEST_F
(
DigitalFilterTest
,
SetGet
)
{
DigitalFilter
digital_filter
;
std
::
vector
<
double
>
numerators
=
{
1.0
,
2.0
,
3.0
};
std
::
vector
<
double
>
denominators
=
{
4.0
,
5.0
,
6.0
};
digital_filter
.
set_denominators
(
denominators
);
digital_filter
.
set_numerators
(
numerators
);
digital_filter
.
set_coefficients
(
0.01
,
20
);
std
::
vector
<
double
>
denominators_got
=
digital_filter
.
denominators
();
std
::
vector
<
double
>
numerators_got
=
digital_filter
.
numerators
();
EXPECT_EQ
(
numerators_got
.
size
(),
numerators
.
size
());
EXPECT_EQ
(
denominators_got
.
size
(),
denominators
.
size
());
for
(
size_t
i
=
0
;
i
<
numerators
.
size
();
++
i
)
{
EXPECT_DOUBLE_EQ
(
numerators_got
[
i
],
numerators
[
i
]);
}
for
(
size_t
i
=
0
;
i
<
denominators
.
size
();
++
i
)
{
EXPECT_DOUBLE_EQ
(
denominators_got
[
i
],
denominators
[
i
]);
}
digital_filter
.
set_coefficients
(
denominators
,
numerators
);
denominators_got
.
clear
();
denominators_got
=
digital_filter
.
denominators
();
numerators_got
.
clear
();
numerators_got
=
digital_filter
.
numerators
();
EXPECT_EQ
(
numerators_got
.
size
(),
numerators
.
size
());
EXPECT_EQ
(
denominators_got
.
size
(),
denominators
.
size
());
for
(
size_t
i
=
0
;
i
<
numerators
.
size
();
++
i
)
{
EXPECT_DOUBLE_EQ
(
numerators_got
[
i
],
numerators
[
i
]);
}
for
(
size_t
i
=
0
;
i
<
denominators
.
size
();
++
i
)
{
EXPECT_DOUBLE_EQ
(
denominators_got
[
i
],
denominators
[
i
]);
}
EXPECT_EQ
(
numerators_got
.
size
(),
3
);
EXPECT_EQ
(
denominators_got
.
size
(),
3
);
EXPECT_NEAR
(
numerators_got
[
0
],
0.1729
,
0.01
);
EXPECT_NEAR
(
numerators_got
[
1
],
0.3458
,
0.01
);
EXPECT_NEAR
(
numerators_got
[
2
],
0.1729
,
0.01
);
EXPECT_NEAR
(
denominators_got
[
0
],
1.0
,
0.01
);
EXPECT_NEAR
(
denominators_got
[
2
],
0.2217
,
0.01
);
double
dead_zone
=
1.0
;
digital_filter
.
set_dead_zone
(
dead_zone
);
EXPECT_FLOAT_EQ
(
digital_filter
.
dead_zone
(),
dead_zone
);
}
TEST_F
(
DigitalFilterTest
,
FilterOff
)
{
std
::
vector
<
double
>
numerators
=
{
0.0
,
0.0
,
0.0
};
std
::
vector
<
double
>
denominators
=
{
1.0
,
0.0
,
0.0
};
DigitalFilter
digital_filter
(
denominators
,
numerators
);
DigitalFilter
digital_filter
(
0.1
,
0
);
const
std
::
vector
<
double
>
step_input
(
100
,
1.0
);
std
::
vector
<
double
>
rand_input
(
100
,
1.0
);
...
...
@@ -86,21 +67,17 @@ TEST_F(DigitalFilterTest, FilterOff) {
}
TEST_F
(
DigitalFilterTest
,
MovingAverage
)
{
std
::
vector
<
double
>
numerators
=
{
0.25
,
0.25
,
0.25
,
0.25
};
std
::
vector
<
double
>
denominators
=
{
1.0
,
0.0
,
0.0
};
DigitalFilter
digital_filter
;
digital_filter
.
set_numerators
(
numerators
);
digital_filter
.
set_denominators
(
denominators
);
DigitalFilter
digital_filter
(
0.25
,
1
);
const
std
::
vector
<
double
>
step_input
(
100
,
1.0
);
// Check step input, transients.
for
(
size_t
i
=
0
;
i
<
4
;
++
i
)
{
double
expected_filter_out
=
(
i
+
1
)
*
0.25
;
EXPECT_
FLOAT_EQ
(
digital_filter
.
Filter
(
step_input
[
i
]),
expected_filter_out
);
EXPECT_
NEAR
(
digital_filter
.
Filter
(
step_input
[
i
]),
expected_filter_out
,
0.5
);
}
// Check step input, steady state
for
(
size_t
i
=
4
;
i
<
step_input
.
size
();
++
i
)
{
EXPECT_
FLOAT_EQ
(
digital_filter
.
Filter
(
step_input
[
i
]),
1.0
);
EXPECT_
NEAR
(
digital_filter
.
Filter
(
step_input
[
i
]),
1.0
,
0.1
);
}
}
...
...
modules/control/proto/lat_controller_conf.proto
浏览文件 @
f9d8e733
...
...
@@ -5,8 +5,7 @@ package apollo.control;
// simple optimal steer control param
message
LatControllerConf
{
optional
double
ts
=
1
;
// sample time (dt) 0.01 now, configurable
// preview window n, preview time = preview window * ts
optional
int32
preview_window
=
2
;
optional
double
preview_time
=
2
;
optional
double
cf
=
3
;
optional
double
cr
=
4
;
// N/rad
optional
int32
mass_fl
=
5
;
...
...
modules/control/proto/lon_controller_conf.proto
浏览文件 @
f9d8e733
...
...
@@ -18,7 +18,7 @@ message LonControllerConf {
optional
double
throttle_deadzone
=
3
;
optional
double
speed_controller_input_limit
=
4
;
optional
double
station_error_limit
=
5
;
optional
double
preview_
window
=
6
;
optional
double
preview_
time
=
6
;
optional
double
standstill_acceleration
=
7
;
optional
PidConf
station_pid_conf
=
8
;
...
...
modules/control/testdata/conf/lincoln.pb.txt
浏览文件 @
f9d8e733
...
...
@@ -12,7 +12,7 @@ active_controllers: LON_CONTROLLER
max_steering_percentage_allowed: 100
lat_controller_conf {
ts: 0.01
preview_
window:
0
preview_
time: 0.
0
cf: 155494.663
cr: 155494.663
mass_fl: 520
...
...
@@ -80,7 +80,7 @@ lon_controller_conf {
throttle_deadzone: 0.2
speed_controller_input_limit: 3.0
station_error_limit: 3.0
preview_
window: 40.
0
preview_
time: 0.4
0
standstill_acceleration: -3.0
station_pid_conf {
integrator_enable: false
...
...
编辑
预览
Markdown
is supported
0%
请重试
或
添加新附件
.
添加附件
取消
You are about to add
0
people
to the discussion. Proceed with caution.
先完成此消息的编辑!
取消
想要评论请
注册
或
登录