Skip to content

A
apollo

Pinoxchio / apollo
与 Fork 源项目一致

从无法访问的项目Fork

通知 2
Star 0
Fork 0
A
apollo

体验新版 GitCode，发现更多精彩内容 >>

提交 94c5fb09 编写于 作者: Y Yajia Zhang 提交者： Qi Luo
浏览文件
操作

planning: refactor piecewise jerk problem

上级 65656eb2
隐藏空白更改
内联 并排
Showing with 72 addition and 123 deletion
modules/planning/math/piecewise_jerk/piecewise_jerk_problem.cc
浏览文件 @ 94c5fb09
......@@ -26,12 +26,11 @@ namespace apollo {
namespace planning {
namespace {
constexpr double kMaxVariableRange = 1e10;
constexpr double kMaxVariableRange = 1.0e10;
} // namespace
void PiecewiseJerkProblem::InitProblem(const size_t num_of_knots,
const double delta_s,
const std::array<double, 5>& w,
const std::array<double, 3>& x_init,
const std::array<double, 3>& x_end) {
CHECK_GE(num_of_knots, 2);
......@@ -40,14 +39,7 @@ void PiecewiseJerkProblem::InitProblem(const size_t num_of_knots,
x_init_ = x_init;
x_end_ = x_end;
weight_.x_w = w[0];
weight_.x_derivative_w = w[1];
weight_.x_second_order_derivative_w = w[2];
weight_.x_third_order_derivative_w = w[3];
weight_.x_ref_w = w[4];
delta_s_ = delta_s;
delta_s_sq_ = delta_s * delta_s;
x_bounds_.resize(num_of_knots_,
std::make_pair(-kMaxVariableRange, kMaxVariableRange));
......@@ -57,9 +49,6 @@ void PiecewiseJerkProblem::InitProblem(const size_t num_of_knots,
ddx_bounds_.resize(num_of_knots_,
std::make_pair(-kMaxVariableRange, kMaxVariableRange));
x_ref_.resize(num_of_knots_, 0.0);
penalty_dx_.resize(num_of_knots_, 0.0);
}
bool PiecewiseJerkProblem::OptimizeWithOsqp(
......@@ -81,7 +70,7 @@ bool PiecewiseJerkProblem::OptimizeWithOsqp(
data->l = lower_bounds.data();
data->u = upper_bounds.data();
CHECK_EQ(upper_bounds.size(), lower_bounds.size());
CHECK_EQ(lower_bounds.size(), upper_bounds.size());
*work = osqp_setup(data, settings);
......@@ -104,16 +93,14 @@ bool PiecewiseJerkProblem::OptimizeWithOsqp(
}
void PiecewiseJerkProblem::SetZeroOrderReference(std::vector<double> x_ref) {
if (x_ref.size() == num_of_knots_) {
x_ref_ = std::move(x_ref);
}
CHECK_EQ(x_ref.size(), num_of_knots_);
x_ref_ = std::move(x_ref);
}
void PiecewiseJerkProblem::SetFirstOrderPenalty(
std::vector<double> penalty_dx) {
if (penalty_dx.size() == num_of_knots_) {
penalty_dx_ = std::move(penalty_dx);
}
CHECK_EQ(penalty_dx.size(), num_of_knots_);
penalty_dx_ = std::move(penalty_dx);
}
void PiecewiseJerkProblem::SetZeroOrderBounds(
......@@ -129,9 +116,9 @@ void PiecewiseJerkProblem::SetFirstOrderBounds(
}
void PiecewiseJerkProblem::SetSecondOrderBounds(
std::vector<std::pair<double, double>> d2x_bounds) {
CHECK_EQ(d2x_bounds.size(), num_of_knots_);
ddx_bounds_ = std::move(d2x_bounds);
std::vector<std::pair<double, double>> ddx_bounds) {
CHECK_EQ(ddx_bounds.size(), num_of_knots_);
ddx_bounds_ = std::move(ddx_bounds);
}
void PiecewiseJerkProblem::SetZeroOrderBounds(const double x_lower_bound,
......@@ -158,43 +145,6 @@ void PiecewiseJerkProblem::SetSecondOrderBounds(const double ddx_lower_bound,
}
}
void PiecewiseJerkProblem::ProcessBound(
const std::vector<std::tuple<double, double, double>>& src,
std::vector<std::pair<double, double>>* dst) {
DCHECK_NOTNULL(dst);
*dst = std::vector<std::pair<double, double>>(
num_of_knots_, std::make_pair(-kMaxVariableRange, kMaxVariableRange));
for (size_t i = 0; i < src.size(); ++i) {
size_t index = static_cast<size_t>(std::get<0>(src[i]) / delta_s_ + 0.5);
if (index < dst->size()) {
dst->at(index).first =
std::max(dst->at(index).first, std::get<1>(src[i]));
dst->at(index).second =
std::min(dst->at(index).second, std::get<2>(src[i]));
}
}
}
// x_bounds: tuple(s, lower_bounds, upper_bounds)
void PiecewiseJerkProblem::SetVariableBounds(
const std::vector<std::tuple<double, double, double>>& x_bounds) {
ProcessBound(x_bounds, &x_bounds_);
}
// dx_bounds: tuple(s, lower_bounds, upper_bounds)
void PiecewiseJerkProblem::SetVariableDerivativeBounds(
const std::vector<std::tuple<double, double, double>>& dx_bounds) {
ProcessBound(dx_bounds, &dx_bounds_);
}
// ddx_bounds: tuple(s, lower_bounds, upper_bounds)
void PiecewiseJerkProblem::SetVariableSecondOrderDerivativeBounds(
const std::vector<std::tuple<double, double, double>>& ddx_bounds) {
ProcessBound(ddx_bounds, &ddx_bounds_);
}
bool PiecewiseJerkProblem::Optimize(const int max_iter) {
// calculate kernel
std::vector<c_float> P_data;
......@@ -277,36 +227,35 @@ void PiecewiseJerkProblem::CalculateKernel(std::vector<c_float>* P_data,
// x(i)^2 * (w_x + w_ref)
for (int i = 0; i < N; ++i) {
columns[i].emplace_back(i, (weight_.x_w + weight_.x_ref_w));
columns[i].emplace_back(i, (weight_x_ + weight_x_reference_));
++value_index;
}
// x(i)'^2 * w_dx
for (int i = 0; i < N; ++i) {
columns[N + i].emplace_back(N + i, weight_.x_derivative_w);
columns[N + i].emplace_back(N + i, weight_dx_);
++value_index;
}
auto delta_s_sq_ = delta_s_ * delta_s_;
// x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
columns[2 * N].emplace_back(
2 * N, weight_.x_second_order_derivative_w +
weight_.x_third_order_derivative_w / delta_s_sq_);
2 * N, weight_ddx_ + weight_dddx_ / delta_s_sq_);
++value_index;
for (int i = 1; i < N - 1; ++i) {
columns[2 * N + i].emplace_back(
2 * N + i, weight_.x_second_order_derivative_w +
2.0 * weight_.x_third_order_derivative_w / delta_s_sq_);
2 * N + i, weight_ddx_ + 2.0 * weight_dddx_ / delta_s_sq_);
++value_index;
}
columns[3 * N - 1].emplace_back(
3 * N - 1, weight_.x_second_order_derivative_w +
weight_.x_third_order_derivative_w / delta_s_sq_);
3 * N - 1, weight_ddx_ + weight_dddx_ / delta_s_sq_);
++value_index;
// -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
for (int i = 0; i < N - 1; ++i) {
columns[2 * N + i].emplace_back(
2 * N + i + 1, -2.0 * weight_.x_third_order_derivative_w / delta_s_sq_);
2 * N + i + 1, -2.0 * weight_dddx_ / delta_s_sq_);
++value_index;
}
......@@ -362,10 +311,8 @@ void PiecewiseJerkProblem::CalculateAffineConstraint(
for (int i = 0; i + 1 < N; ++i) {
columns[2 * N + i].emplace_back(constraint_index, -1.0);
columns[2 * N + i + 1].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) =
-max_x_third_order_derivative_ * delta_s_;
upper_bounds->at(constraint_index) =
max_x_third_order_derivative_ * delta_s_;
lower_bounds->at(constraint_index) = -dddx_bound_ * delta_s_;
upper_bounds->at(constraint_index) = dddx_bound_ * delta_s_;
++constraint_index;
}
......@@ -381,6 +328,7 @@ void PiecewiseJerkProblem::CalculateAffineConstraint(
}
// x(i+1) - x(i) - x(i)'*delta_s - 1/3*x(i)''*delta_s^2 - 1/6*x(i)''*delta_s^2
auto delta_s_sq_ = delta_s_ * delta_s_;
for (int i = 0; i + 1 < N; ++i) {
columns[i].emplace_back(constraint_index, -1.0);
columns[i + 1].emplace_back(constraint_index, 1.0);
......@@ -429,11 +377,11 @@ void PiecewiseJerkProblem::CalculateOffset(std::vector<c_float>* q) {
const int kNumParam = 3 * N;
q->resize(kNumParam);
for (int i = 0; i < N; ++i) {
q->at(i) += -2.0 * weight_.x_ref_w * x_ref_[i];
q->at(i) += -2.0 * weight_x_reference_ * x_ref_[i];
}
q->at(N - 1) += -2.0 * weight_.x_w * x_end_[0];
q->at(N * 2 - 1) += -2.0 * weight_.x_derivative_w * x_end_[1];
q->at(N * 3 - 1) += -2.0 * weight_.x_second_order_derivative_w * x_end_[2];
q->at(N - 1) += -2.0 * weight_x_ * x_end_[0];
q->at(N * 2 - 1) += -2.0 * weight_dx_ * x_end_[1];
q->at(N * 3 - 1) += -2.0 * weight_ddx_ * x_end_[2];
}
} // namespace planning
......
modules/planning/math/piecewise_jerk/piecewise_jerk_problem.h
浏览文件 @ 94c5fb09
......@@ -65,18 +65,15 @@ class PiecewiseJerkProblem {
* -- w[4]: default reference line weight, (x_bounds[k].first +
* x_bounds[k].second)/2
*/
virtual void InitProblem(const size_t num_var, const double delta_s,
const std::array<double, 5>& w,
const std::array<double, 3>& x_init = {0.0, 0.0,
0.0},
const std::array<double, 3>& x_end = {0.0, 0.0,
0.0});
virtual void ResetInitConditions(const std::array<double, 3>& x_init) {
virtual void InitProblem(const size_t num_of_knots, const double delta_s,
const std::array<double, 3>& x_init,
const std::array<double, 3>& x_end);
virtual void SetInitCondition(const std::array<double, 3>& x_init) {
x_init_ = x_init;
}
virtual void ResetEndConditions(const std::array<double, 3>& x_end) {
virtual void SetEndCondition(const std::array<double, 3>& x_end) {
x_end_ = x_end;
}
......@@ -84,39 +81,46 @@ class PiecewiseJerkProblem {
void SetFirstOrderPenalty(std::vector<double> penalty_dx);
void SetZeroOrderBounds(std::vector<std::pair<double, double>> x_bounds);
void SetFirstOrderBounds(std::vector<std::pair<double, double>> dx_bounds);
void SetSecondOrderBounds(std::vector<std::pair<double, double>> d2x_bounds);
void SetZeroOrderBounds(std::vector<std::pair<double, double>> x_bounds);
void SetZeroOrderBounds(const double x_lower_bound,
const double x_upper_bound);
void SetFirstOrderBounds(std::vector<std::pair<double, double>> dx_bounds);
void SetFirstOrderBounds(const double dx_lower_bound,
const double dx_upper_bound);
void SetSecondOrderBounds(std::vector<std::pair<double, double>> d2x_bounds);
void SetSecondOrderBounds(const double ddx_lower_bound,
const double ddx_upper_bound);
void SetThirdOrderBound(const double dddx_bound) {
max_x_third_order_derivative_ = dddx_bound;
dddx_bound_ = dddx_bound;
}
// x_bounds: tuple(s, lower_bounds, upper_bounds)
// s doesn't need to be sorted
virtual void SetVariableBounds(
const std::vector<std::tuple<double, double, double>>& x_bounds);
void set_weight_x(const double weight_x) {
weight_x_ = weight_x;
}
// dx_bounds: tuple(s, lower_bounds, upper_bounds)
// s doesn't need to be sorted
virtual void SetVariableDerivativeBounds(
const std::vector<std::tuple<double, double, double>>& dx_bounds);
void set_weight_dx(const double weight_dx) {
weight_dx_ = weight_dx;
}
void set_weight_ddx(const double weight_ddx) {
weight_ddx_ = weight_ddx;
}
// ddx_bounds: tuple(s, lower_bounds, upper_bounds)
// s doesn't need to be sorted
virtual void SetVariableSecondOrderDerivativeBounds(
const std::vector<std::tuple<double, double, double>>& ddx_bounds);
void set_weight_dddx(const double weight_dddx) {
weight_dddx_ = weight_dddx;
}
void set_weight_x_reference(const double weight_x_reference) {
weight_x_reference_ = weight_x_reference;
}
virtual bool Optimize(const int max_iter = 4000);
......@@ -150,10 +154,6 @@ class PiecewiseJerkProblem {
std::vector<c_float>& q, OSQPData* data, OSQPWorkspace** work, // NOLINT
OSQPSettings* settings);
virtual void ProcessBound(
const std::vector<std::tuple<double, double, double>>& src,
std::vector<std::pair<double, double>>* dst);
protected:
size_t num_of_knots_ = 0;
......@@ -166,22 +166,23 @@ class PiecewiseJerkProblem {
std::array<double, 3> x_end_;
std::vector<double> x_ref_;
std::vector<double> penalty_dx_;
std::vector<std::pair<double, double>> x_bounds_;
std::vector<std::pair<double, double>> dx_bounds_;
std::vector<std::pair<double, double>> ddx_bounds_;
double dddx_bound_ = 0.0;
double weight_x_ = 0.0;
double weight_dx_ = 0.0;
double weight_ddx_ = 0.0;
struct {
double x_w = 0.0;
double x_derivative_w = 0.0;
double x_second_order_derivative_w = 0.0;
double x_third_order_derivative_w = 0.0;
double x_ref_w = 0.0;
} weight_;
double weight_dddx_ = 0.0;
double max_x_third_order_derivative_ = 0.0;
double weight_x_reference_ = 0.0;
double delta_s_ = 1.0;
double delta_s_sq_ = 1.0;
};
} // namespace planning
......
modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_optimizer.cc
浏览文件 @ 94c5fb09
......@@ -97,12 +97,13 @@ Status PiecewiseJerkSpeedOptimizer::Process(
path_time_qp->SetSecondOrderBounds(veh_param.max_deceleration(),
veh_param.max_acceleration());
path_time_qp->SetThirdOrderBound(FLAGS_longitudinal_jerk_bound);
// TODO(Hongyi): delete this when ready to use vehicle_params
path_time_qp->SetSecondOrderBounds(-4.4, 2.0);
path_time_qp->SetDesireDerivative(FLAGS_default_cruise_speed);
// Update STBoundary
std::vector<std::tuple<double, double, double>> x_bounds;
std::vector<std::pair<double, double>> s_bounds;
for (int i = 0; i < num_of_knots; ++i) {
double curr_t = i * delta_t;
double s_lower_bound = 0.0;
......@@ -135,14 +136,14 @@ Status PiecewiseJerkSpeedOptimizer::Process(
speed_data->clear();
return Status(ErrorCode::PLANNING_ERROR, msg);
}
x_bounds.emplace_back(curr_t, s_lower_bound, s_upper_bound);
s_bounds.emplace_back(s_lower_bound, s_upper_bound);
}
path_time_qp->SetVariableBounds(x_bounds);
path_time_qp->SetZeroOrderBounds(std::move(s_bounds));
// Update SpeedBoundary and ref_s
std::vector<double> x_ref;
std::vector<double> penalty_dx;
std::vector<std::tuple<double, double, double>> dx_bounds;
std::vector<std::pair<double, double>> s_dot_bounds;
const SpeedLimit& speed_limit = st_graph_data.speed_limit();
for (int i = 0; i < num_of_knots; ++i) {
double curr_t = i * delta_t;
......@@ -154,18 +155,17 @@ Status PiecewiseJerkSpeedOptimizer::Process(
// get curvature
PathPoint path_point;
path_data.GetPathPointWithPathS(path_s, &path_point);
penalty_dx.emplace_back(fabs(path_point.kappa()) *
penalty_dx.emplace_back(std::fabs(path_point.kappa()) *
piecewise_jerk_speed_config.kappa_penalty_weight());
// get v_upper_bound
const double v_lower_bound = 0.0;
double v_upper_bound = FLAGS_planning_upper_speed_limit;
v_upper_bound = speed_limit.GetSpeedLimitByS(path_s);
dx_bounds.emplace_back(curr_t, v_lower_bound,
std::fmax(v_upper_bound, 0.0));
s_dot_bounds.emplace_back(v_lower_bound, std::fmax(v_upper_bound, 0.0));
}
path_time_qp->SetZeroOrderReference(x_ref);
path_time_qp->SetFirstOrderPenalty(penalty_dx);
path_time_qp->SetVariableDerivativeBounds(dx_bounds);
path_time_qp->SetFirstOrderBounds(s_dot_bounds);
// Sovle the problem
if (!path_time_qp->Optimize()) {
......
Markdown is supported
0% 或 .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑！
想要评论请 注册 或