FemPosDeviation参考线平滑方法是离散点平滑方法，Fem是Finite element estimate的意思。
1. 优化目标
1.1 平滑性
参考线平滑的首要目标当然是平滑性，使用向量的模
∣
P
2
P
2
′
⃗
∣
| \vec{P_2 P^{\prime}_2}|
∣P2P2′∣来表示，显然
∣
P
2
P
2
′
⃗
∣
| \vec{P_2 P^{\prime}_2}|
∣P2P2′∣越小，三个点
P
1
,
P
2
,
P
3
P_1,P_2,P_3
P1,P2,P3越接近一条直线，越平滑。
J
s
m
o
o
t
h
=
∣
P
2
P
2
′
⃗
∣
=
∣
P
2
P
1
⃗
+
P
2
P
3
⃗
∣
=
∣
(
x
1
−
x
2
,
y
1
−
y
2
)
+
(
x
3
−
x
2
,
y
3
−
y
2
)
∣
=
(
x
1
+
x
3
−
2
x
2
)
2
+
(
y
1
+
y
3
−
2
y
2
)
2
(1-1)
J_{smooth} = | \vec{P_2 P^{\prime}_2}| = | \vec{P_2 P_1} + \vec{P_2 P_3} | = | (x_1 - x_2, y_1 - y_2) + (x_3 - x_2, y_3 - y_2) | = \sqrt{(x_1 + x_3 - 2x_2)^2 + (y_1 + y_3 - 2y_2)^2} \tag{1-1}
Jsmooth=∣P2P2′∣=∣P2P1+P2P3∣=∣(x1−x2,y1−y2)+(x3−x2,y3−y2)∣=(x1+x3−2x2)2+(y1+y3−2y2)2(1-1)
J s m o o t h = [ x 1 , y 1 , x 2 , y 2 , x 3 , y 3 ] [ 1 0 − 2 0 1 0 0 1 0 − 2 0 1 − 2 0 4 0 − 2 0 0 − 2 0 4 0 − 2 1 0 − 2 0 1 0 0 1 0 − 2 0 1 ] [ x 1 , y 1 , x 2 , y 2 , x 3 , y 3 ] T (1-2) J_{smooth} = [x_1, y_1, x_2, y_2, x_3, y_3] \left[\begin{matrix} 1 & 0 & -2 & 0 & 1 & 0 \\ 0 & 1 & 0 & -2 & 0 & 1 \\ -2 & 0 & 4 & 0 & -2 & 0 \\ 0 & -2 & 0 & 4 & 0 & -2 \\ 1 & 0 & -2 & 0 & 1 & 0 \\ 0 & 1 & 0 & -2 & 0 & 1 \end{matrix}\right] [x_1, y_1, x_2, y_2, x_3, y_3]^T \tag{1-2} Jsmooth=[x1,y1,x2,y2,x3,y3] 10−2010010−201−2040−200−2040−210−2010010−201 [x1,y1,x2,y2,x3,y3]T(1-2)
1.2 几何性
平滑后的参考线，希望能够保留原始道路的几何信息，不会把弯道的处的参考线平滑成一条直线。使用平滑后点与原始点的距离来表示。
J
d
e
v
i
a
t
i
o
n
=
∣
P
r
,
1
P
1
⃗
∣
+
∣
P
r
,
2
P
2
⃗
∣
+
∣
P
r
,
3
P
3
⃗
∣
=
∣
(
x
1
−
x
1
,
r
,
y
1
−
y
1
,
r
)
∣
+
∣
(
x
2
−
x
2
,
r
,
y
2
−
y
2
,
r
)
∣
+
∣
(
x
3
−
x
3
,
r
,
y
3
−
y
3
,
r
)
∣
(1-3)
J_{deviation} = | \vec{P_{r,1} P_1}| + | \vec{P_{r,2} P_2}| + | \vec{P_{r,3} P_3}| = | (x_1 - x_{1,r}, y_1 - y_{1,r}) | + | (x_2 - x_{2,r}, y_2 - y_{2,r}) | + | (x_3 - x_{3,r}, y_3 - y_{3,r}) | \tag{1-3}
Jdeviation=∣Pr,1P1∣+∣Pr,2P2∣+∣Pr,3P3∣=∣(x1−x1,r,y1−y1,r)∣+∣(x2−x2,r,y2−y2,r)∣+∣(x3−x3,r,y3−y3,r)∣(1-3)
J d e v i a t i o n = [ x 1 , y 1 , x 2 , y 2 , x 3 , y 3 ] [ 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 ] [ x 1 , y 1 , x 2 , y 2 , x 3 , y 3 ] T − 2 [ x 1 , r , y 1 , r , x 2 , r , y 2 , r , x 3 , r , y 3 , r ] [ x 1 , y 1 , x 2 , y 2 , x 3 , y 3 ] T (1-4) J_{deviation} = [x_1, y_1, x_2, y_2, x_3, y_3] \left[\begin{matrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}\right] [x_1, y_1, x_2, y_2, x_3, y_3]^T \\ -2[x_{1,r}, y_{1,r}, x_{2,r}, y_{2,r}, x_{3,r}, y_{3,r}] [x_1, y_1, x_2, y_2, x_3, y_3]^T \tag{1-4} Jdeviation=[x1,y1,x2,y2,x3,y3] 100000010000001000000100000010000001 [x1,y1,x2,y2,x3,y3]T−2[x1,r,y1,r,x2,r,y2,r,x3,r,y3,r][x1,y1,x2,y2,x3,y3]T(1-4)
1.3 均匀性
平滑后的参考线的每两个相邻点之间的长度尽量均匀一直。
J
l
e
n
g
t
h
=
∣
P
1
P
2
⃗
∣
2
+
∣
P
2
P
3
⃗
∣
2
=
(
x
2
−
x
1
)
2
+
(
y
2
−
y
1
)
2
+
(
x
3
−
x
2
)
2
+
(
y
3
−
y
2
)
2
(1-5)
J_{length} = | \vec{P_1 P_2}|^2 + | \vec{P_2 P_3}|^2 = (x_2 - x_1)^2 + (y_2 - y_1)^2 + (x_3 - x_2)^2 + (y_3 - y_2)^2 \tag{1-5}
Jlength=∣P1P2∣2+∣P2P3∣2=(x2−x1)2+(y2−y1)2+(x3−x2)2+(y3−y2)2(1-5)
J l e n g t h = [ x 1 , y 1 , x 2 , y 2 , x 3 , y 3 ] [ 1 0 − 1 0 0 0 0 1 0 − 1 0 0 − 1 0 2 0 − 1 0 0 − 1 0 2 0 − 1 0 0 − 1 0 1 0 0 0 0 − 1 0 1 ] [ x 1 , y 1 , x 2 , y 2 , x 3 , y 3 ] T (1-6) J_{length} = [x_1, y_1, x_2, y_2, x_3, y_3] \left[\begin{matrix} 1 & 0 & -1 & 0 & 0 & 0 \\ 0 & 1 & 0 & -1 & 0 & 0 \\ -1 & 0 & 2 & 0 & -1 & 0 \\ 0 & -1 & 0 & 2 & 0 & -1 \\ 0 & 0 & -1 & 0 & 1 & 0 \\ 0 & 0 & 0 & -1 & 0 & 1 \end{matrix}\right] [x_1, y_1, x_2, y_2, x_3, y_3]^T \tag{1-6} Jlength=[x1,y1,x2,y2,x3,y3] 10−1000010−100−1020−100−1020−100−1010000−101 [x1,y1,x2,y2,x3,y3]T(1-6)
因此，参考线平滑的优化目标可以定义为：
J
=
w
s
m
o
o
t
h
∗
J
s
m
o
o
t
h
+
w
d
e
v
i
a
t
i
o
n
∗
J
d
e
v
i
a
t
i
o
n
+
w
l
e
n
g
t
h
∗
J
l
e
n
g
t
h
(1-7)
J = w_{smooth} * J_{smooth} + w_{deviation} * J_{deviation} + w_{length} * J_{length} \tag{1-7}
J=wsmooth∗Jsmooth+wdeviation∗Jdeviation+wlength∗Jlength(1-7)
上述是按照
3
3
3个点进行举例的，当平滑点的数目为
N
≥
3
N \geq 3
N≥3时，同时为了简化公式表达，令
X
i
=
[
x
i
,
y
i
]
,
I
=
[
1
,
1
;
0
,
0
]
,
O
=
[
0
,
0
;
0
,
0
]
X_i = [x_i, y_i],I = [1,1;0,0],O=[0,0;0,0]
Xi=[xi,yi],I=[1,1;0,0],O=[0,0;0,0]可得：
J
s
m
o
o
t
h
=
[
X
1
,
X
2
,
X
3
,
⋯
,
X
N
]
[
I
−
2
I
I
O
⋯
O
O
O
−
2
I
5
I
−
4
I
I
⋯
O
O
O
I
−
4
I
6
I
−
4
I
⋯
O
O
O
O
I
−
4
I
6
I
⋯
O
O
O
⋮
⋮
⋮
⋮
⋱
⋮
⋮
⋮
O
O
O
O
⋯
6
I
−
4
I
I
O
O
O
O
⋯
−
4
I
5
I
−
2
I
O
O
O
O
⋯
O
−
2
I
I
]
[
X
1
,
X
2
,
X
3
,
⋯
,
X
N
]
T
(1-8)
J_{smooth} = [X_1, X_2, X_3, \cdots, X_N] \left[\begin{matrix} I & -2I & I & O & \cdots & O & O & O \\ -2I & 5I & -4I & I & \cdots & O & O & O \\ I & -4I & 6I & -4I & \cdots & O & O & O \\ O & I & -4I & 6I & \cdots & O & O & O \\ \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots \\ O & O & O & O & \cdots & 6I & -4I & I \\ O & O & O & O & \cdots & -4I & 5I &-2I \\ O & O & O & O & \cdots & O & -2I &I \end{matrix}\right] [X_1, X_2, X_3, \cdots, X_N]^T \tag{1-8}
Jsmooth=[X1,X2,X3,⋯,XN]
I−2IIO⋮OOO−2I5I−4II⋮OOOI−4I6I−4I⋮OOOOI−4I6I⋮OOO⋯⋯⋯⋯⋱⋯⋯⋯OOOO⋮6I−4IOOOOO⋮−4I5I−2IOOOO⋮I−2II
[X1,X2,X3,⋯,XN]T(1-8)
J d e v i a t i o n = [ X 1 , X 2 , X 3 , ⋯ , X N ] [ I O ⋯ O O I ⋯ O ⋮ ⋮ ⋱ ⋮ O O O I ] [ X 1 , X 2 , X 3 , ⋯ , X N ] T (1-9) J_{deviation} = [X_1, X_2, X_3, \cdots, X_N] \left[\begin{matrix} I & O & \cdots & O \\ O & I & \cdots & O \\ \vdots & \vdots & \ddots & \vdots\\ O & O & O & I \end{matrix}\right] [X_1, X_2, X_3, \cdots, X_N]^T \tag{1-9} Jdeviation=[X1,X2,X3,⋯,XN] IO⋮OOI⋮O⋯⋯⋱OOO⋮I [X1,X2,X3,⋯,XN]T(1-9)
J l e n g t h = [ X 1 , X 2 , X 3 , ⋯ , X N ] [ I − I O ⋯ O O − I 2 I − I ⋯ O O O − I 2 I ⋯ O O ⋮ ⋮ ⋮ ⋱ ⋮ ⋮ O O O ⋯ 2 I − I O O O ⋯ − I I ] [ X 1 , X 2 , X 3 , ⋯ , X N ] T (1-10) J_{length} = [X_1, X_2, X_3, \cdots, X_N] \left[\begin{matrix} I & -I & O & \cdots & O & O\\ -I & 2I & -I & \cdots & O & O\\ O & -I & 2I & \cdots & O & O\\ \vdots & \vdots & \vdots & \ddots & \vdots & \vdots\\ O & O & O &\cdots & 2I & -I \\ O & O & O &\cdots & -I & I \end{matrix}\right] [X_1, X_2, X_3, \cdots, X_N]^T \tag{1-10} Jlength=[X1,X2,X3,⋯,XN] I−IO⋮OO−I2I−I⋮OOO−I2I⋮OO⋯⋯⋯⋱⋯⋯OOO⋮2I−IOOO⋮−II [X1,X2,X3,⋯,XN]T(1-10)
2. 约束条件
只考虑边界约束，即：
x
i
,
l
o
w
e
r
≤
x
i
≤
x
i
,
u
p
p
e
r
y
i
,
l
o
w
e
r
≤
y
i
≤
y
i
,
u
p
p
e
r
(1-11)
x_{i,lower} \leq x_i \leq x_{i,upper} \\ y_{i,lower} \leq y_i \leq y_{i,upper} \tag{1-11}
xi,lower≤xi≤xi,upperyi,lower≤yi≤yi,upper(1-11)
可以转化为：
x
i
,
r
−
b
o
u
n
d
≤
x
i
≤
x
i
,
r
+
b
o
u
n
d
y
i
,
r
−
b
o
u
n
d
≤
y
i
≤
y
i
,
r
+
b
o
u
n
d
(1-12)
x_{i,r} - bound \leq x_i \leq x_{i,r} + bound \\ y_{i,r} - bound \leq y_i \leq y_{i,r} + bound \tag{1-12}
xi,r−bound≤xi≤xi,r+boundyi,r−bound≤yi≤yi,r+bound(1-12)
对参考线的起点和终点进行约束，令其等于原始参考线上的点：
x
1
,
r
≤
x
1
≤
x
1
,
r
y
1
,
r
≤
y
1
≤
y
1
,
r
(1-13)
x_{1,r} \leq x_1 \leq x_{1,r} \\ y_{1,r} \leq y_1 \leq y_{1,r} \tag{1-13}
x1,r≤x1≤x1,ry1,r≤y1≤y1,r(1-13)
3. 代码
FemPosDeviation算法使用OSQP二次规划求解器进行求解，其代码实现在modules/planning/math/discretized_points_smoothing/FemPosDeviationOsqpInterface.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);
}
void FemPosDeviationOsqpInterface::CalculateOffset(std::vector<c_float>* q) {
for (int i = 0; i < num_of_points_; ++i) {
const auto& ref_point_xy = ref_points_[i];
q->push_back(-2.0 * weight_ref_deviation_ * ref_point_xy.first);
q->push_back(-2.0 * weight_ref_deviation_ * ref_point_xy.second);
}
}
对约束的代码实现如下：
void FemPosDeviationOsqpInterface::CalculateAffineConstraint(
std::vector<c_float>* A_data, std::vector<c_int>* A_indices,
std::vector<c_int>* A_indptr, std::vector<c_float>* lower_bounds,
std::vector<c_float>* upper_bounds) {
int ind_A = 0;
for (int i = 0; i < num_of_variables_; ++i) {
A_data->push_back(1.0);
A_indices->push_back(i);
A_indptr->push_back(ind_A);
++ind_A;
}
A_indptr->push_back(ind_A);
for (int i = 0; i < num_of_points_; ++i) {
const auto& ref_point_xy = ref_points_[i];
upper_bounds->push_back(ref_point_xy.first + bounds_around_refs_[i]);
upper_bounds->push_back(ref_point_xy.second + bounds_around_refs_[i]);
lower_bounds->push_back(ref_point_xy.first - bounds_around_refs_[i]);
lower_bounds->push_back(ref_point_xy.second - bounds_around_refs_[i]);
}
}
641
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
