无人驾驶算法——Baidu Apollo代码解析之ReferenceLine Smoother参考线平滑
作者：互联网
无人驾驶算法——Baidu Apollo代码解析之ReferenceLine Smoother参考线平滑
Date: 2020/12/15
Editor:萧潇子（Jesse）
Contact: 1223167600@qq.com
Apollo 参考线平滑类
Apollo主要的参考线平滑类有三个：QpSplineReferenceLineSmoother、SpiralReferenceLineSmoother和DiscretePointsReferenceLineSmoother。
reference_line_provider.cc
ReferenceLineProvider::ReferenceLineProvider(
const common::VehicleStateProvider *vehicle_state_provider,
const hdmap::HDMap *base_map,
const std::shared_ptr<relative_map::MapMsg> &relative_map)
: vehicle_state_provider_(vehicle_state_provider) {
if (!FLAGS_use_navigation_mode) {
pnc_map_ = std::make_unique<hdmap::PncMap>(base_map);
relative_map_ = nullptr;
} else {
pnc_map_ = nullptr;
relative_map_ = relative_map;
}
ACHECK(cyber::common::GetProtoFromFile(FLAGS_smoother_config_filename,
&smoother_config_))
<< "Failed to load smoother config file "
<< FLAGS_smoother_config_filename;
//参考线平滑的几种方式
if (smoother_config_.has_qp_spline()) {
smoother_.reset(new QpSplineReferenceLineSmoother(smoother_config_));
} else if (smoother_config_.has_spiral()) {
smoother_.reset(new SpiralReferenceLineSmoother(smoother_config_));
} else if (smoother_config_.has_discrete_points()) {
//散点平滑
smoother_.reset(new DiscretePointsReferenceLineSmoother(smoother_config_));
} else {
ACHECK(false) << "unknown smoother config "
<< smoother_config_.DebugString();
}
is_initialized_ = true;
}
其中DiscretePointsReferenceLineSmoother中包含了两个方法：CosThetaSmoother和FemPosDeviationSmoother。本文主要介绍散点平滑的建模过程（代码中公式的物理意义以及推导）
代价函数
cos_theta_ipopt_interface.cc
//初始化点
bool CosThetaIpoptInterface::get_starting_point(int n, bool init_x, double* x,
bool init_z, double* z_L,
double* z_U, int m,
bool init_lambda,
double* lambda) {
CHECK_EQ(static_cast<size_t>(n), num_of_variables_);
std::random_device rd;
std::default_random_engine gen = std::default_random_engine(rd());
std::normal_distribution<> dis{0, 0.05};
//在原始参考点基础上添加随机数初始化点
for (size_t i = 0; i < num_of_points_; ++i) {
size_t index = i << 1;
x[index] = ref_points_[i].first + dis(gen);
x[index + 1] = ref_points_[i].second + dis(gen);
}
return true;
}
//代价函数包含2部分
bool CosThetaIpoptInterface::eval_f(int n, const double* x, bool new_x,
double& obj_value) {
CHECK_EQ(static_cast<size_t>(n), num_of_variables_);
if (use_automatic_differentiation_) {
eval_obj(n, x, &obj_value);
return true;
}
//第1部分——与参考线的距离
obj_value = 0.0;
for (size_t i = 0; i < num_of_points_; ++i) {
size_t index = i << 1;
obj_value +=
(x[index] - ref_points_[i].first) * (x[index] - ref_points_[i].first) +
(x[index + 1] - ref_points_[i].second) *
(x[index + 1] - ref_points_[i].second);
}
//第2部分——cos_theta
for (size_t i = 0; i < num_of_points_ - 2; i++) {
size_t findex = i << 1;
size_t mindex = findex + 2;
size_t lindex = mindex + 2;
obj_value -=
weight_cos_included_angle_ *
(((x[mindex] - x[findex]) * (x[lindex] - x[mindex])) +
((x[mindex + 1] - x[findex + 1]) * (x[lindex + 1] - x[mindex + 1]))) /
std::sqrt((x[mindex] - x[findex]) * (x[mindex] - x[findex]) +
(x[mindex + 1] - x[findex + 1]) *
(x[mindex + 1] - x[findex + 1])) /
std::sqrt((x[lindex] - x[mindex]) * (x[lindex] - x[mindex]) +
(x[lindex + 1] - x[mindex + 1]) *
(x[lindex + 1] - x[mindex + 1]));
}
}
//第3部分——总长度
for (size_t i = 0; i < num_of_points_ - 1; ++i) {
size_t findex = i << 1;
size_t nindex = findex + 2;
*obj_value +=
weight_length_ *
((x[findex] - x[nindex]) * (x[findex] - x[nindex]) +
(x[findex + 1] - x[nindex + 1]) * (x[findex + 1] - x[nindex + 1]));
}
return true;
}
以 n n n个点为例子 P 0 , P 1 , P 2 , P 3 , ⋯ , P n P_0, P_1, P_2, P_3, \cdots, P_n P0,P1,P2,P3,⋯,Pn
第1部分——参考点距离代价
o
b
j
v
a
l
u
e
1
=
∑
i
=
0
n
−
1
(
x
i
−
x
i
−
r
e
f
)
2
+
(
y
i
−
y
i
−
r
e
f
)
2
obj_{value}1 = \sum^{n-1}_{i=0}(x_i-x_{i-ref})^2 + (y_i-y_{i-ref})^2
objvalue1=i=0∑n−1(xi−xi−ref)2+(yi−yi−ref)2
第2部分——平滑性代价
o
b
j
v
a
l
u
e
2
=
−
∑
i
=
0
n
−
2
(
x
i
+
1
−
x
i
)
×
(
x
i
+
2
−
x
i
+
1
)
+
(
y
i
+
1
−
y
i
)
×
(
y
i
+
2
−
y
i
+
1
)
(
x
i
+
1
−
x
i
)
2
+
(
y
i
+
1
−
y
i
)
2
(
x
i
+
2
−
x
i
+
1
)
2
+
(
y
i
+
2
−
y
i
+
1
)
2
obj_{value}2 = -\sum^{n-2}_{i=0}\frac{(x_{i+1} - x_i)\times (x_{i+2} - x_{i+1}) + (y_{i+1} - y_{i})\times (y_{i+2} - y_{i+1}) }{\sqrt{(x_{i+1} - x_{i})^2+(y_{i+1} - y_{i})^2} \sqrt{(x_{i+2} - x_{i+1})^2+(y_{i+2} - y_{i+1})^2}}
objvalue2=−i=0∑n−2(xi+1−xi)2+(yi+1−yi)2
(xi+2−xi+1)2+(yi+2−yi+1)2
(xi+1−xi)×(xi+2−xi+1)+(yi+1−yi)×(yi+2−yi+1)
第3部分——总长度代价
o
b
j
v
a
l
u
e
3
=
∑
i
=
0
n
−
1
(
x
i
−
x
i
+
1
)
2
+
(
y
i
−
y
i
+
1
)
2
obj_{value}3 = \sum^{n-1}_{i=0}(x_i-x_{i+1})^2 + (y_i-y_{i+1})^2
objvalue3=i=0∑n−1(xi−xi+1)2+(yi−yi+1)2
总代价函数：
o
b
j
v
a
l
u
e
=
w
1
×
o
b
j
v
a
l
u
e
1
+
w
2
×
o
b
j
v
a
l
u
e
2
+
w
3
×
o
b
j
v
a
l
u
e
3
obj_{value} = w_1 \times obj_{value}1 + w_2 \times obj_{value}2 + w_3 \times obj_{value}3
objvalue=w1×objvalue1+w2×objvalue2+w3×objvalue3
这里解释一下第2部分代价函数代码中的公式的意义：
cos_theta方法： = 前面是 - 号
连续3点
P
0
,
P
1
,
P
2
P_0, P_1, P_2
P0,P1,P2 如上图所示，其中向量
P
0
P
1
⃗
=
(
x
1
−
x
0
,
y
1
−
y
0
)
\vec{P_0P_1} = (x_1-x_0, y_1-y_0)
P0P1
=(x1−x0,y1−y0)和
P
1
P
2
⃗
=
(
x
2
−
x
1
,
y
2
−
y
1
)
\vec{P_1P_2} = (x_2-x_1, y_2-y_1)
P1P2
=(x2−x1,y2−y1)之间的夹角
θ
\theta
θ
c
o
s
θ
=
P
0
P
1
⃗
⋅
P
1
P
2
⃗
∣
P
0
P
1
⃗
∣
∣
P
1
P
2
⃗
∣
=
(
x
1
−
x
0
)
×
(
x
2
−
x
1
)
+
(
y
1
−
y
0
)
×
(
y
2
−
y
1
)
/
(
x
1
−
x
0
)
2
+
(
y
1
−
y
0
)
2
/
(
x
2
−
x
1
)
2
+
(
y
2
−
y
1
)
2
\begin{aligned} cos\theta &= \frac{\vec{P_0P_1} \cdot \vec{P_1P_2}}{|\vec{P_0P_1}| |\vec{P_1P_2}|}\\ & = (x_1 - x_0)\times (x_2 - x_1) + (y_1 - y_0)\times (y_2 - y_1) / \sqrt{(x_1 - x_0)^2+(y_1 - y_0)^2} / \sqrt{(x_2 - x_1)^2+(y_2 - y_1)^2} \end{aligned}
cosθ=∣P0P1
∣∣P1P2
∣P0P1
⋅P1P2
=(x1−x0)×(x2−x1)+(y1−y0)×(y2−y1)/(x1−x0)2+(y1−y0)2
/(x2−x1)2+(y2−y1)2
c
o
s
θ
cos\theta
cosθ值越大，
θ
\theta
θ越小，
P
0
,
P
1
,
P
2
P_0, P_1, P_2
P0,P1,P2 越接近直线，曲线越平滑
fem_pos_deviation_ipopt_interface.cc
/** Template to return the objective value */
template <class T>
bool FemPosDeviationIpoptInterface::eval_obj(int n, const T* x, T* obj_value) {
*obj_value = 0.0;
// 第1部分——参考点距离代价
for (size_t i = 0; i < num_of_points_; ++i) {
size_t index = i * 2;
*obj_value +=
weight_ref_deviation_ *
((x[index] - ref_points_[i].first) * (x[index] - ref_points_[i].first) +
(x[index + 1] - ref_points_[i].second) *
(x[index + 1] - ref_points_[i].second));
}
// 第2部分——平滑性代价
for (size_t i = 0; i + 2 < num_of_points_; ++i) {
size_t findex = i * 2;
size_t mindex = findex + 2;
size_t lindex = mindex + 2;
*obj_value += weight_fem_pos_deviation_ *
(((x[findex] + x[lindex]) - 2.0 * x[mindex]) *
((x[findex] + x[lindex]) - 2.0 * x[mindex]) +
((x[findex + 1] + x[lindex + 1]) - 2.0 * x[mindex + 1]) *
((x[findex + 1] + x[lindex + 1]) - 2.0 * x[mindex + 1]));
}
// 第3部分——总长度代价
for (size_t i = 0; i + 1 < num_of_points_; ++i) {
size_t findex = i * 2;
size_t nindex = findex + 2;
*obj_value +=
weight_path_length_ *
((x[findex] - x[nindex]) * (x[findex] - x[nindex]) +
(x[findex + 1] - x[nindex + 1]) * (x[findex + 1] - x[nindex + 1]));
}
// 第4部分——对于松弛变量的惩罚
for (size_t i = slack_var_start_index_; i < slack_var_end_index_; ++i) {
*obj_value += weight_curvature_constraint_slack_var_ * x[i];
}
return true;
}
代价函数：
以 n n n个点为例子 P 0 , P 1 , P 2 , P 3 , ⋯ , P n P_0, P_1, P_2, P_3, \cdots, P_n P0,P1,P2,P3,⋯,Pn
第1部分——参考点距离代价
o
b
j
v
a
l
u
e
1
=
∑
i
=
0
n
−
1
(
x
i
−
x
i
−
r
e
f
)
2
+
(
y
i
−
y
i
−
r
e
f
)
2
obj_{value}1 = \sum^{n-1}_{i=0}(x_i-x_{i-ref})^2 + (y_i-y_{i-ref})^2
objvalue1=i=0∑n−1(xi−xi−ref)2+(yi−yi−ref)2
第2部分——平滑性代价
o
b
j
v
a
l
u
e
2
=
∑
i
=
0
n
−
2
(
x
i
+
x
i
+
2
−
2
×
x
i
+
1
)
2
+
(
y
i
+
y
i
+
2
−
2
×
y
i
+
1
)
2
obj_{value}2 = \sum^{n-2}_{i=0}(x_i+x_{i+2}-2 \times x_{i+1})^2+(y_{i}+y_{i+2}-2 \times y_{i+1})^2
objvalue2=i=0∑n−2(xi+xi+2−2×xi+1)2+(yi+yi+2−2×yi+1)2
第3部分——总长度代价
o
b
j
v
a
l
u
e
3
=
∑
i
=
0
n
−
1
(
x
i
−
x
i
+
1
)
2
+
(
y
i
−
y
i
+
1
)
2
obj_{value}3 = \sum^{n-1}_{i=0}(x_i-x_{i+1})^2 + (y_i-y_{i+1})^2
objvalue3=i=0∑n−1(xi−xi+1)2+(yi−yi+1)2
第4部分——对于松弛变量的惩罚也是常规做法，可以在后面约束的部分看到松弛变量的意义。
总代价函数：
o
b
j
v
a
l
u
e
=
w
1
×
o
b
j
v
a
l
u
e
1
+
w
2
×
o
b
j
v
a
l
u
e
2
+
w
3
×
o
b
j
v
a
l
u
e
3
obj_{value} = w_1 \times obj_{value}1 + w_2 \times obj_{value}2 + w_3 \times obj_{value}3
objvalue=w1×objvalue1+w2×objvalue2+w3×objvalue3
这里解释一下第2部分代价函数代码中的公式的意义：
Fem_pos_deviation方法：= 前面是 + 号
(
x
i
+
x
i
+
2
−
2
×
x
i
+
1
)
2
+
(
y
i
+
y
i
+
2
−
2
×
y
i
+
1
)
2
(x_i+x_{i+2}-2 \times x_{i+1})^2+(y_{i}+y_{i+2}-2 \times y_{i+1})^2
(xi+xi+2−2×xi+1)2+(yi+yi+2−2×yi+1)2
其中上式的物理意义如上图中 P 1 P ’ ⃗ \vec{P_1P’} P1P’ 向量的模的平方，它可以理解为：向量 P 1 P 0 ⃗ \vec{P_1P_0} P1P0 和 向量 P 1 P 2 ⃗ \vec{P_1P_2} P1P2 相加结果新的向量 P 1 P ’ ⃗ \vec{P_1P’} P1P’ 的模平方。如果这三个点在一条直线上，那么这个值最小；三个点组成的两个线段的夹角 θ \theta θ越大，即曲线越平滑。
约束
约束条件：
/** Template to compute contraints */
template <class T>
bool FemPosDeviationIpoptInterface::eval_constraints(int n, const T* x, int m,
T* g) {
// a. 位置约束
for (size_t i = 0; i < num_of_points_; ++i) {
size_t index = i * 2;
g[index] = x[index];
g[index + 1] = x[index + 1];
}
// b. 曲率约束
for (size_t i = 0; i + 2 < num_of_points_; ++i) {
size_t findex = i * 2;
size_t mindex = findex + 2;
size_t lindex = mindex + 2;
g[curvature_constr_start_index_ + i] =
(((x[findex] + x[lindex]) - 2.0 * x[mindex]) *
((x[findex] + x[lindex]) - 2.0 * x[mindex]) +
((x[findex + 1] + x[lindex + 1]) - 2.0 * x[mindex + 1]) *
((x[findex + 1] + x[lindex + 1]) - 2.0 * x[mindex + 1])) -
x[slack_var_start_index_ + i];
}
// c. 松弛变量的约束
size_t slack_var_index = 0;
for (size_t i = slack_constr_start_index_; i < slack_constr_end_index_; ++i) {
g[i] = x[slack_var_start_index_ + slack_var_index];
++slack_var_index;
}
return true;
}
边界条件：
bool FemPosDeviationIpoptInterface::get_bounds_info(int n, double* x_l,
double* x_u, int m,
double* g_l, double* g_u) {
CHECK_EQ(static_cast<size_t>(n), num_of_variables_);
CHECK_EQ(static_cast<size_t>(m), num_of_constraints_);
// 状态变量
// a. for x, y
for (size_t i = 0; i < num_of_points_; ++i) {
size_t index = i * 2;
// x
x_l[index] = -1e20;
x_u[index] = 1e20;
// y
x_l[index + 1] = -1e20;
x_u[index + 1] = 1e20;
}
// b. for slack var
for (size_t i = slack_var_start_index_; i < slack_var_end_index_; ++i) {
x_l[i] = -1e20;
x_u[i] = 1e20;
}
// 约束
// a. 点的上下左右边界约束
for (size_t i = 0; i < num_of_points_; ++i) {
size_t index = i * 2;
// x
g_l[index] = ref_points_[i].first - bounds_around_refs_[i];
g_u[index] = ref_points_[i].first + bounds_around_refs_[i];
// y
g_l[index + 1] = ref_points_[i].second - bounds_around_refs_[i];
g_u[index + 1] = ref_points_[i].second + bounds_around_refs_[i];
}
// b. 曲率约束上下界
double ref_total_length = 0.0;
auto pre_point = ref_points_.front();
for (size_t i = 1; i < num_of_points_; ++i) {
auto cur_point = ref_points_[i];
double x_diff = cur_point.first - pre_point.first;
double y_diff = cur_point.second - pre_point.second;
ref_total_length += std::sqrt(x_diff * x_diff + y_diff * y_diff);
pre_point = cur_point;
}
double average_delta_s =
ref_total_length / static_cast<double>(num_of_points_ - 1);
double curvature_constr_upper =
average_delta_s * average_delta_s * curvature_constraint_;
for (size_t i = curvature_constr_start_index_;
i < curvature_constr_end_index_; ++i) {
g_l[i] = -1e20;
g_u[i] = curvature_constr_upper * curvature_constr_upper;
}
// c. 松弛变量约束
for (size_t i = slack_constr_start_index_; i < slack_constr_end_index_; ++i) {
g_l[i] = 0.0;
g_u[i] = 1e20;
}
return true;
}
这里解释一下对曲率约束的上届：
对于除去首尾的每个anchor point（因为需要三个点计算曲率，故首尾无法计算），需要满足：
(
x
i
+
x
i
+
2
−
2
×
x
i
+
1
)
2
+
(
y
i
+
y
i
+
2
−
2
×
y
i
+
1
)
2
−
s
t
a
c
k
i
≤
(
c
u
r
v
a
t
u
r
e
_
c
o
n
s
t
r
_
u
p
p
e
r
)
2
c
u
r
v
a
t
u
r
e
_
c
o
n
s
t
r
_
u
p
p
e
r
=
a
v
e
r
a
g
e
_
d
e
l
t
a
_
s
×
a
v
e
r
a
g
e
_
d
e
l
t
a
_
s
×
c
u
r
v
a
t
u
r
e
_
c
o
n
s
t
r
a
i
n
t
_
(x_i+x_{i+2}-2 \times x_{i+1})^2+(y_{i}+y_{i+2}-2 \times y_{i+1})^2 - stack_i \leq (curvature\_constr\_upper)^2 \\\\ curvature\_constr\_upper = average\_delta\_s \times average\_delta\_s \times curvature\_constraint\_
(xi+xi+2−2×xi+1)2+(yi+yi+2−2×yi+1)2−stacki≤(curvature_constr_upper)2curvature_constr_upper=average_delta_s×average_delta_s×curvature_constraint_
该约束是之前提到的那个矢量的模的平方减去松弛变量。首先计算了原始的anchor points之间的平均距离average_delta_s，定义最大曲率curvature_constraint = 0.2，该约束的上界设置为
a
v
e
r
a
g
e
_
d
e
l
t
a
_
s
2
×
c
u
r
v
a
t
u
r
e
_
c
o
n
s
t
r
a
i
n
t
average\_delta\_s^2 \times curvature\_constraint
average_delta_s2×curvature_constraint。该约束没有下界，而计算其上界的具体物理意义如下图所示，黄色线段可以理解为松弛变量。
如图所示，假设
P
0
,
P
1
,
P
2
P_0, P_1, P_2
P0,P1,P2三点处在同一个圆上，当
θ
1
\theta_1
θ1较小时，向量
P
1
P
0
⃗
\vec{P_1P_0}
P1P0
和 向量
P
1
P
2
⃗
\vec{P_1P_2}
P1P2
的模近似等于弧长，因此有：
θ
1
=
d
S
R
\theta_1 = \frac{dS} {R}
θ1=RdS
根据
O
−
P
0
−
P
1
O-P_0-P_1
O−P0−P1等腰三角形几何关系有：
θ
2
=
π
−
θ
1
2
\theta_2 = \frac{\pi - \theta_1}{2}
θ2=2π−θ1
由于
∣
P
1
P
0
⃗
∣
=
∣
P
1
P
2
⃗
∣
|\vec{P_1P_0}|=|\vec{P_1P_2}|
∣P1P0
∣=∣P1P2
∣，所以
C
C
C是
P
1
P
3
P_1P_3
P1P3的中点，有
∣
P
1
C
⃗
∣
=
∣
C
P
3
⃗
∣
|\vec{P_1C}|=|\vec{CP_3}|
∣P1C
∣=∣CP3
∣关系，由此得：
∣
P
1
P
3
⃗
∣
=
2
P
1
C
|\vec{P_1P_3}| = 2P_1C
∣P1P3
∣=2P1C
根据
C
−
P
0
−
P
1
C-P_0-P_1
C−P0−P1直角三角形几何关系有：
P
1
C
=
d
S
×
c
o
s
(
θ
2
)
P_1C = dS \times cos(\theta_2)
P1C=dS×cos(θ2)
把
P
1
C
P_1C
P1C带入
∣
P
1
P
3
⃗
∣
|\vec{P_1P_3}|
∣P1P3
∣得：
∣
P
1
P
3
⃗
∣
=
2
P
1
C
=
2
×
d
S
×
c
o
s
(
θ
2
)
\begin{aligned} |\vec{P_1P_3}| &= 2P_1C\\ & = 2 \times dS \times cos(\theta_2) \end{aligned}
∣P1P3
∣=2P1C=2×dS×cos(θ2)
把
θ
2
\theta_2
θ2带入得：
∣
P
1
P
3
⃗
∣
=
2
×
d
S
×
c
o
s
(
θ
2
)
=
2
×
d
S
×
c
o
s
(
π
−
θ
1
2
)
=
2
×
d
S
×
c
o
s
(
π
2
−
θ
1
2
)
=
2
×
d
S
×
s
i
n
(
θ
1
2
)
=
2
×
d
S
×
θ
1
2
=
d
S
×
θ
1
\begin{aligned} |\vec{P_1P_3}| & = 2 \times dS \times cos(\theta_2)\\ & = 2 \times dS \times cos(\frac{\pi - \theta_1}{2})\\ & = 2 \times dS \times cos(\frac{\pi}{2} - \frac{\theta_1}{2})\\ & = 2 \times dS \times sin(\frac{\theta_1}{2})\\ & = 2 \times dS \times \frac{\theta_1}{2}\\ & = dS \times \theta_1\\ \end{aligned}
∣P1P3
∣=2×dS×cos(θ2)=2×dS×cos(2π−θ1)=2×dS×cos(2π−2θ1)=2×dS×sin(2θ1)=2×dS×2θ1=dS×θ1
把
θ
1
\theta_1
θ1带入得：
∣
P
1
P
3
⃗
∣
=
d
S
×
θ
1
=
d
S
×
d
S
R
=
d
S
2
×
1
R
=
d
S
2
×
c
u
r
\begin{aligned} |\vec{P_1P_3}| & = dS \times \theta_1\\ & = dS \times \frac{dS} {R}\\ & = dS^2 \times \frac{1} {R}\\ & = dS^2 \times cur\\ \end{aligned}
∣P1P3
∣=dS×θ1=dS×RdS=dS2×R1=dS2×cur
fem_pos_deviation_osqp_interface.cc和fem_pos_deviation_sqp_osqp_interface.cc文件中二次规划代价函数矩阵形式如下：
void FemPosDeviationOsqpInterface::CalculateKernel(
std::vector<c_float>* P_data, std::vector<c_int>* P_indices,
std::vector<c_int>* P_indptr) {
CHECK_GT(num_of_variables_, 4);
// Three quadratic penalties are involved:
// 1. Penalty x on distance between middle point and point by finite element
// estimate;
// 2. Penalty y on path length;
// 3. Penalty z on difference between points and reference points
// General formulation of P matrix is as below(with 6 points as an example):
// I is a two by two identity matrix, X, Y, Z represents x * I, y * I, z * I
// 0 is a two by two zero matrix
// |X+Y+Z, -2X-Y, X, 0, 0, 0 |
// |0, 5X+2Y+Z, -4X-Y, X, 0, 0 |
// |0, 0, 6X+2Y+Z, -4X-Y, X, 0 |
// |0, 0, 0, 6X+2Y+Z, -4X-Y, X |
// |0, 0, 0, 0, 5X+2Y+Z, -2X-Y|
// |0, 0, 0, 0, 0, X+Y+Z|
// Only upper triangle needs to be filled
std::vector<std::vector<std::pair<c_int, c_float>>> columns;
columns.resize(num_of_variables_);
int col_num = 0;
for (int col = 0; col < 2; ++col) {
columns[col].emplace_back(col, weight_fem_pos_deviation_ +
weight_path_length_ +
weight_ref_deviation_);
++col_num;
}
for (int col = 2; col < 4; ++col) {
columns[col].emplace_back(
col - 2, -2.0 * weight_fem_pos_deviation_ - weight_path_length_);
columns[col].emplace_back(col, 5.0 * weight_fem_pos_deviation_ +
2.0 * weight_path_length_ +
weight_ref_deviation_);
++col_num;
}
int second_point_from_last_index = num_of_points_ - 2;
for (int point_index = 2; point_index < second_point_from_last_index;
++point_index) {
int col_index = point_index * 2;
for (int col = 0; col < 2; ++col) {
col_index += col;
columns[col_index].emplace_back(col_index - 4, weight_fem_pos_deviation_);
columns[col_index].emplace_back(
col_index - 2,
-4.0 * weight_fem_pos_deviation_ - weight_path_length_);
columns[col_index].emplace_back(
col_index, 6.0 * weight_fem_pos_deviation_ +
2.0 * weight_path_length_ + weight_ref_deviation_);
++col_num;
}
}
int second_point_col_from_last_col = num_of_variables_ - 4;
int last_point_col_from_last_col = num_of_variables_ - 2;
for (int col = second_point_col_from_last_col;
col < last_point_col_from_last_col; ++col) {
columns[col].emplace_back(col - 4, weight_fem_pos_deviation_);
columns[col].emplace_back(
col - 2, -4.0 * weight_fem_pos_deviation_ - weight_path_length_);
columns[col].emplace_back(col, 5.0 * weight_fem_pos_deviation_ +
2.0 * weight_path_length_ +
weight_ref_deviation_);
++col_num;
}
for (int col = last_point_col_from_last_col; col < num_of_variables_; ++col) {
columns[col].emplace_back(col - 4, weight_fem_pos_deviation_);
columns[col].emplace_back(
col - 2, -2.0 * weight_fem_pos_deviation_ - weight_path_length_);
columns[col].emplace_back(col, weight_fem_pos_deviation_ +
weight_path_length_ +
weight_ref_deviation_);
++col_num;
}
CHECK_EQ(col_num, num_of_variables_);
int ind_p = 0;
for (int i = 0; i < col_num; ++i) {
P_indptr->push_back(ind_p);
for (const auto& row_data_pair : columns[i]) {
// Rescale by 2.0 as the quadratic term in osqp default qp problem setup
// is set as (1/2) * x' * P * x
P_data->push_back(row_data_pair.second * 2.0);
P_indices->push_back(row_data_pair.first);
++ind_p;
}
}
P_indptr->push_back(ind_p);
}
代价函数——二次规划代价函数矩阵形式
以 n n n个点为例子 P 0 , P 1 , P 2 , P 3 , ⋯ , P n P_0, P_1, P_2, P_3, \cdots, P_n P0,P1,P2,P3,⋯,Pn
第1部分——参考点距离代价
o
b
j
v
a
l
u
e
1
=
∑
i
=
0
n
−
1
(
x
i
−
x
i
−
r
e
f
)
2
+
(
y
i
−
y
i
−
r
e
f
)
2
obj_{value}1 = \sum^{n-1}_{i=0}(x_i-x_{i-ref})^2 + (y_i-y_{i-ref})^2
objvalue1=i=0∑n−1(xi−xi−ref)2+(yi−yi−ref)2
第2部分——平滑性代价
o
b
j
v
a
l
u
e
2
=
∑
i
=
0
n
−
2
(
x
i
+
x
i
+
2
−
2
×
x
i
+
1
)
2
+
(
y
i
+
y
i
+
2
−
2
×
y
i
+
1
)
2
obj_{value}2 = \sum^{n-2}_{i=0}(x_i+x_{i+2}-2 \times x_{i+1})^2+(y_{i}+y_{i+2}-2 \times y_{i+1})^2
objvalue2=i=0∑n−2(xi+xi+2−2×xi+1)2+(yi+yi+2−2×yi+1)2
第3部分——总长度代价
o
b
j
v
a
l
u
e
3
=
∑
i
=
0
n
−
1
(
x
i
−
x
i
+
1
)
2
+
(
y
i
−
y
i
+
1
)
2
obj_{value}3 = \sum^{n-1}_{i=0}(x_i-x_{i+1})^2 + (y_i-y_{i+1})^2
objvalue3=i=0∑n−1(xi−xi+1)2+(yi−yi+1)2
总代价函数：
o
b
j
v
a
l
u
e
=
w
1
×
o
b
j
v
a
l
u
e
1
+
w
2
×
o
b
j
v
a
l
u
e
2
+
w
3
×
o
b
j
v
a
l
u
e
3
obj_{value} = w_1 \times obj_{value}1 + w_2 \times obj_{value}2 + w_3 \times obj_{value}3
objvalue=w1×objvalue1+w2×objvalue2+w3×objvalue3
其中
w
1
w
2
w
3
w_1 w_2 w_3
w1w2w3分别代表上述三种代价的权重，经过化简得到矩阵形式：
o
b
s
v
a
l
u
e
=
1
2
x
T
P
x
+
q
T
x
obs_{value} = \frac{1}{2}x^TPx + q^T x
obsvalue=21xTPx+qTx
其中
P
P
P和
q
q
q矩阵形式如下：
P
=
W
2
+
W
3
+
W
1
−
2
W
2
−
W
3
W
2
0
0
0
0
−
2
W
2
−
W
3
5
W
2
+
2
W
3
+
W
1
−
4
W
2
−
W
3
W
2
0
0
0
W
2
−
4
W
2
−
W
3
6
W
2
+
2
W
3
+
W
1
−
4
W
2
−
W
3
W
2
0
0
0
⋱
⋱
⋱
⋱
⋱
0
0
0
W
2
−
4
W
2
−
W
3
6
W
2
+
2
W
3
+
W
1
−
4
W
2
−
W
3
W
2
0
0
0
W
2
−
4
W
2
−
W
3
5
W
2
+
2
W
3
+
W
1
−
2
W
2
−
W
3
0
0
0
0
W
2
−
2
W
2
−
W
3
W
2
+
W
3
+
W
1
P = \begin{matrix} W_2+W_3+W_1 & -2W_2-W_3 & W_2 & 0 & 0 &0 &0\\ -2W_2-W_3 & 5W_2+2W_3+W_1 & -4W_2-W_3 & W_2 & 0 & 0 & 0\\ W_2 & -4W_2-W_3 & 6W_2+2W_3+W_1 & -4W_2-W_3 & W_2 & 0 & 0\\ 0 & \ddots & \ddots & \ddots & \ddots & \ddots & 0 \\ 0 & 0 & W_2 & -4W_2-W_3 & 6W_2+2W_3+W_1 & -4W_2-W_3 & W_2 \\ 0 & 0 & 0 & W_2 & -4W_2-W_3 & 5W_2+2W_3+W_1 & -2W_2-W_3 \\ 0 & 0 & 0 & 0 & W_2 & -2W_2-W_3 & W_2+W_3+W_1 \end{matrix}
P=W2+W3+W1−2W2−W3W20000−2W2−W35W2+2W3+W1−4W2−W3⋱000W2−4W2−W36W2+2W3+W1⋱W2000W2−4W2−W3⋱−4W2−W3W2000W2⋱6W2+2W3+W1−4W2−W3W2000⋱−4W2−W35W2+2W3+W1−2W2−W30000W2−2W2−W3W2+W3+W1
q = − 2 W 1 X r e f q = -2 W_1X_{ref} q=−2W1Xref
约束方程矩阵形式：
l
≤
A
x
≤
u
l \leq Ax \leq u
l≤Ax≤u
A
A
A为单位矩阵
I
n
×
n
I_{n \times n }
In×n，
l
l
l和
u
u
u为
x
y
xy
xy设置的上下界
标签：Baidu,index,yi,Smoother,value,times,ReferenceLine,ref,col 来源： https://blog.csdn.net/renyushuai900/article/details/111191413