参考线平滑-FemPosDeviation-SQP

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 }}{|}^2=|\overrightarrow{P_2{P}_1}+\overrightarrow{P_2{P}_3}{|}^2=(x_1-x_2,y_1-y_2{)}^2+(x_3-x_2,y_3-y_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}{|}^2+|\overrightarrow{P_{r,2}{P}_2}{|}^2+|\overrightarrow{P_{r,3}{P}_3}{|}^2=(x_1-x_{1,r}{)}^2+(y_1-y_{1,r}{)}^2+(x_2-x_{2,r}{)}^2+(y_2-y_{2,r}{)}^2+(x_3-x_{3,r}{)}^2+(y_3-y_{3,r}{)}^2\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,0;0,1],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 − 2 [ X 1 , r , X 2 , r , X 3 , r , ⋯   , X N , r ] [ 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 -2 [X_{1,r}, X_{2,r}, X_{3,r}, \cdots, X_{N,r}] [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−2[X1,r​,X2,r​,X3,r​,⋯,XN,r​][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. 约束条件

2.1 边界约束

只考虑边界约束，即：
$$\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}(2-1)} \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}(2-2)} \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}(2-3)} \end{gathered}$$

2.2 曲率约束

$$\begin{gathered} (x_{i-1}+x_{i+1}-2x_i{)}^2+(y_{i-1}+y_{i+1}-2y_i{)}^2-{\epsilon }_{\kappa ,i}\le (\Delta s^2\times {\kappa }_{max}{)}^2,i=1,2,\cdots ,N-1 \\ {\epsilon }_{\kappa ,i}\ge 0,i=1,2,\cdots ,N-1\text{\hspace{1em}(2-4)} \end{gathered}$$

曲率约束是非线性的，可以使用一阶泰勒展开在平衡点$\bar{X}$(参考点)处进行线性化。令$f=(x_{i-1}+x_{i+1}-2x_i{)}^2+(y_{i-1}+y_{i+1}-2y_i{)}^2$，$f$的梯度向量函数为：
∇ f = [ ∂ f ∂ x i − 1 ∂ f ∂ y i − 1 ∂ f ∂ x i ∂ f ∂ y i ∂ f ∂ x i + 1 ∂ f ∂ y i + 1 ] = [ 2 ( x i − 1 + x i + 1 − 2 x i ) 2 ( y i − 1 + y i + 1 − 2 y i ) − 4 ( x i − 1 + x i + 1 − 2 x i ) − 4 ( y i − 1 + y i + 1 − 2 y i ) 2 ( x i − 1 + x i + 1 − 2 x i ) 2 ( y i − 1 + y i + 1 − 2 y i ) ] (2-5) \nabla f =

= \tag{2-5} ∇f= ​∂xi−1​∂f​∂yi−1​∂f​∂xi​∂f​∂yi​∂f​∂xi+1​∂f​∂yi+1​∂f​​ ​= ​2(xi−1​+xi+1​−2xi​)2(yi−1​+yi+1​−2yi​)−4(xi−1​+xi+1​−2xi​)−4(yi−1​+yi+1​−2yi​)2(xi−1​+xi+1​−2xi​)2(yi−1​+yi+1​−2yi​)​ ​(2-5)
则$f$的一阶泰勒展开式为：
$$f=f(\bar{X})+\nabla f_{X=\bar{X}}^T(X-\bar{X})\text{\hspace{1em}(2-6)}$$
所以，非线性约束$(2-4)$可以转化为线性约束。

3. SQP

通过一阶泰勒展开可以将非线性约束转化为线性约束，然后使用二次规划求解。但是泰勒展开的精度只能保证在平衡点的邻域内，因此当平衡点选取不合适时(可以理解为$|X-\bar{X}|$较大时)，线性化会有较大误差，而序列二次规划(Sequential quadratic programming, SQP)可以解决此问题。序列二次规划将复杂的非线性约束最优化问题转化为一个或者多个二次规划子问题，然后依次迭代求解，在求解二次规划子问题时，会使用上一个二次规划的结果作为非线性约束泰勒展开线性化时的平衡点，使线性化结果逐渐逼近非线性约束，直至求解结果收敛或者达到最大迭代次数。其伪代码可以表示为：

在Apollo的代码中，可以看到其求解流程是符合上述伪代码的，其有二点不同，一是会先进行一次$QP$求解后再进入while循环中，这么做是使用了$OSQP$求解器的updata相关函数接口，对每次迭代时线性化后的约束进行更新，以避免过多的$QP$问题构造，以优化时耗；二是在每次迭代时，会对松弛因子的权重逐渐增大，使曲率约束越来越严格，但其仍然是软约束，所以在迭代结束后会进行曲率校核。

bool FemPosDeviationSqpOsqpInterface::Solve() {
  // Sanity Check
  if (ref_points_.empty()) {
    AERROR << "reference points empty, solver early terminates";
    return false;
  }

  if (ref_points_.size() != bounds_around_refs_.size()) {
    AERROR << "ref_points and bounds size not equal, solver early terminates";
    return false;
  }

  if (ref_points_.size() < 3) {
    AERROR << "ref_points size smaller than 3, solver early terminates";
    return false;
  }

  if (ref_points_.size() > std::numeric_limits::max()) {
    AERROR << "ref_points size too large, solver early terminates";
    return false;
  }

  // Calculate optimization states definitions
  num_of_points_ = static_cast(ref_points_.size());
  num_of_pos_variables_ = num_of_points_ * 2;
  num_of_slack_variables_ = num_of_points_ - 2;
  num_of_variables_ = num_of_pos_variables_ + num_of_slack_variables_;

  num_of_variable_constraints_ = num_of_variables_;
  num_of_curvature_constraints_ = num_of_points_ - 2;
  num_of_constraints_ =
      num_of_variable_constraints_ + num_of_curvature_constraints_;

  // Set primal warm start
  std::vector primal_warm_start;
  SetPrimalWarmStart(ref_points_, &primal_warm_start);

  // Calculate kernel
  std::vector P_data;
  std::vector P_indices;
  std::vector P_indptr;
  CalculateKernel(&P_data, &P_indices, &P_indptr);

  // Calculate offset
  std::vector q;
  CalculateOffset(&q);

  // Calculate affine constraints
  std::vector A_data;
  std::vector A_indices;
  std::vector A_indptr;
  std::vector lower_bounds;
  std::vector upper_bounds;
  CalculateAffineConstraint(ref_points_, &A_data, &A_indices, &A_indptr,
                            &lower_bounds, &upper_bounds);

  // Load matrices and vectors into OSQPData
  OSQPData* data = reinterpret_cast(c_malloc(sizeof(OSQPData)));
  data->n = num_of_variables_;
  data->m = num_of_constraints_;
  data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(),
                       P_indices.data(), P_indptr.data());
  data->q = q.data();
  data->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(),
                       A_indices.data(), A_indptr.data());
  data->l = lower_bounds.data();
  data->u = upper_bounds.data();

  // Define osqp solver settings
  OSQPSettings* settings =
      reinterpret_cast(c_malloc(sizeof(OSQPSettings)));
  osqp_set_default_settings(settings);
  settings->max_iter = max_iter_;
  settings->time_limit = time_limit_;
  settings->verbose = verbose_;
  settings->scaled_termination = scaled_termination_;
  settings->warm_start = warm_start_;
  settings->polish = true;
  settings->eps_abs = 1e-5;
  settings->eps_rel = 1e-5;
  settings->eps_prim_inf = 1e-5;
  settings->eps_dual_inf = 1e-5;

  // Define osqp workspace
  OSQPWorkspace* work = nullptr;
  // osqp_setup(&work, data, settings);
  work = osqp_setup(data, settings);

  // Initial solution
  bool initial_solve_res = OptimizeWithOsqp(primal_warm_start, &work);

  if (!initial_solve_res) {
    AERROR << "initial iteration solving fails";
    osqp_cleanup(work);
    c_free(data->A);
    c_free(data->P);
    c_free(data);
    c_free(settings);
    return false;
  }

  // Sequential solution

  int pen_itr = 0;
  double ctol = 0.0;
  double original_slack_penalty = weight_curvature_constraint_slack_var_;
  double last_fvalue = work->info->obj_val;

  while (pen_itr < sqp_pen_max_iter_) {
    int sub_itr = 1;
    bool fconverged = false;

    while (sub_itr < sqp_sub_max_iter_) {
      SetPrimalWarmStart(opt_xy_, &primal_warm_start);
      CalculateOffset(&q);
      CalculateAffineConstraint(opt_xy_, &A_data, &A_indices, &A_indptr,
                                &lower_bounds, &upper_bounds);
      osqp_update_lin_cost(work, q.data());
      osqp_update_A(work, A_data.data(), OSQP_NULL, A_data.size());
      osqp_update_bounds(work, lower_bounds.data(), upper_bounds.data());

      bool iterative_solve_res = OptimizeWithOsqp(primal_warm_start, &work);
      if (!iterative_solve_res) {
        AERROR << "iteration at " << sub_itr
               << ", solving fails with max sub iter " << sqp_sub_max_iter_;
        weight_curvature_constraint_slack_var_ = original_slack_penalty;
        osqp_cleanup(work);
        c_free(data->A);
        c_free(data->P);
        c_free(data);
        c_free(settings);
        return false;
      }

      double cur_fvalue = work->info->obj_val;
      double ftol = std::abs((last_fvalue - cur_fvalue) / last_fvalue);

      if (ftol < sqp_ftol_) {
        ADEBUG << "merit function value converges at sub itr num " << sub_itr;
        ADEBUG << "merit function value converges to " << cur_fvalue
               << ", with ftol " << ftol << ", under max_ftol " << sqp_ftol_;
        fconverged = true;
        break;
      }

      last_fvalue = cur_fvalue;
      ++sub_itr;
    }

    if (!fconverged) {
      AERROR << "Max number of iteration reached";
      weight_curvature_constraint_slack_var_ = original_slack_penalty;
      osqp_cleanup(work);
      c_free(data->A);
      c_free(data->P);
      c_free(data);
      c_free(settings);
      return false;
    }

    ctol = CalculateConstraintViolation(opt_xy_);

    ADEBUG << "ctol is " << ctol << ", at pen itr " << pen_itr;

    if (ctol < sqp_ctol_) {
      ADEBUG << "constraint satisfied at pen itr num " << pen_itr;
      ADEBUG << "constraint voilation value drops to " << ctol
             << ", under max_ctol " << sqp_ctol_;
      weight_curvature_constraint_slack_var_ = original_slack_penalty;
      osqp_cleanup(work);
      c_free(data->A);
      c_free(data->P);
      c_free(data);
      c_free(settings);
      return true;
    }

    weight_curvature_constraint_slack_var_ *= 10;
    ++pen_itr;
  }

  ADEBUG << "constraint not satisfied with total itr num " << pen_itr;
  ADEBUG << "constraint voilation value drops to " << ctol
         << ", higher than max_ctol " << sqp_ctol_;
  weight_curvature_constraint_slack_var_ = original_slack_penalty;
  osqp_cleanup(work);
  c_free(data->A);
  c_free(data->P);
  c_free(data);
  c_free(settings);
  return true;
}
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
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192