参考线平滑-FemPosDeviation-OSQP

FemPosDeviation参考线平滑方法是离散点平滑方法，Fem是Finite element estimate的意思。

1. 优化目标

1.1 平滑性

参考线平滑的首要目标当然是平滑性，使用向量的模$|\overrightarrow{P_2{P}_2^{\prime }}|$来表示，显然$|\overrightarrow{P_2{P}_2^{\prime }}|$越小，三个点$P_1,P_2,P_3$越接近一条直线，越平滑。
$$J_{smooth}=|\overrightarrow{P_2{P}_2^{\prime }}|=|\overrightarrow{P_2{P}_1}+\overrightarrow{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}\text{\hspace{1em}(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[

\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−2010​010−201​−2040−20​0−2040−2​10−2010​010−201​ ​[x1​,y1​,x2​,y2​,x3​,y3​]T(1-2)

1.2 几何性

平滑后的参考线，希望能够保留原始道路的几何信息，不会把弯道的处的参考线平滑成一条直线。使用平滑后点与原始点的距离来表示。
$$J_{deviation}=|\overrightarrow{P_{r,1}{P}_1}|+|\overrightarrow{P_{r,2}{P}_2}|+|\overrightarrow{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})|\text{\hspace{1em}(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[

\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​] ​100000​010000​001000​000100​000010​000001​ ​[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_{length}=|\overrightarrow{P_1{P}_2}{|}^2+|\overrightarrow{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\text{\hspace{1em}(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[

\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−1000​010−100​−1020−10​0−1020−1​00−1010​000−101​ ​[x1​,y1​,x2​,y2​,x3​,y3​]T(1-6)

因此，参考线平滑的优化目标可以定义为：
$$J=w_{smooth}\ast J_{smooth}+w_{deviation}\ast J_{deviation}+w_{length}\ast J_{length}\text{\hspace{1em}(1-7)}$$

上述是按照$3$个点进行举例的，当平滑点的数目为$N\ge 3$时，同时为了简化公式表达，令$X_i=[x_i,y_i],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[

\right] [X_1, X_2, X_3, \cdots, X_N]^T \tag{1-8} Jsmooth​=[X1​,X2​,X3​,⋯,XN​] ​I−2IIO⋮OOO​−2I5I−4II⋮OOO​I−4I6I−4I⋮OOO​OI−4I6I⋮OOO​⋯⋯⋯⋯⋱⋯⋯⋯​OOOO⋮6I−4IO​OOOO⋮−4I5I−2I​OOOO⋮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[

\right] [X_1, X_2, X_3, \cdots, X_N]^T \tag{1-9} Jdeviation​=[X1​,X2​,X3​,⋯,XN​] ​IO⋮O​OI⋮O​⋯⋯⋱O​OO⋮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[

\right] [X_1, X_2, X_3, \cdots, X_N]^T \tag{1-10} Jlength​=[X1​,X2​,X3​,⋯,XN​] ​I−IO⋮OO​−I2I−I⋮OO​O−I2I⋮OO​⋯⋯⋯⋱⋯⋯​OOO⋮2I−I​OOO⋮−II​ ​[X1​,X2​,X3​,⋯,XN​]T(1-10)

2. 约束条件

只考虑边界约束，即：
$$\begin{gathered} x_{i,lower}\le x_i\le x_{i,upper} \\ {y}_{i,lower}\le y_i\le y_{i,upper}\text{\hspace{1em}(1-11)} \end{gathered}$$
可以转化为：
$$\begin{gathered} x_{i,r}-bound\le x_i\le x_{i,r}+bound \\ {y}_{i,r}-bound\le y_i\le y_{i,r}+bound\text{\hspace{1em}(1-12)} \end{gathered}$$
对参考线的起点和终点进行约束，令其等于原始参考线上的点：
$$\begin{gathered} x_{1,r}\le x_1\le x_{1,r} \\ {y}_{1,r}\le y_1\le y_{1,r}\text{\hspace{1em}(1-13)} \end{gathered}$$

3. 代码

FemPosDeviation算法使用OSQP二次规划求解器进行求解，其代码实现在modules/planning/math/discretized_points_smoothing/FemPosDeviationOsqpInterface.cc中。

其对优化目标函数的实现如下：

void FemPosDeviationOsqpInterface::CalculateKernel(
    std::vector* P_data, std::vector* P_indices,
    std::vector* 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>> 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);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97

void FemPosDeviationOsqpInterface::CalculateOffset(std::vector* 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);
  }
}
1
2
3
4
5
6
7

对约束的代码实现如下：

void FemPosDeviationOsqpInterface::CalculateAffineConstraint(
    std::vector* A_data, std::vector* A_indices,
    std::vector* A_indptr, std::vector* lower_bounds,
    std::vector* 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]);
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21