Apollo5.5规划代码分析（3）_apollo slack

/*
 * @brief:
 * This class solve an optimization problem:
 * Y
 * |
 * |                       P(x1, y1)  P(x2, y2)
 * |            P(x0, y0)                       ... P(x(k-1), y(k-1))
 * |P(start)
 * |
 * |________________________________________________________ X
 *
 *
 * Given an initial set of points from 0 to k-1,  The goal is to find a set of
 * points which makes the line P(start), P0, P(1) ... P(k-1) "smooth".
 */
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

fem_pos_deviation_smoother.h

#pragma once

#include <utility>
#include <vector>

#include "modules/planning/proto/fem_pos_deviation_smoother_config.pb.h"

namespace apollo {
namespace planning {
class FemPosDeviationSmoother {
 public:
  explicit FemPosDeviationSmoother(const FemPosDeviationSmootherConfig& config);

  bool Solve(const std::vector<std::pair<double, double>>& raw_point2d,
             const std::vector<double>& bounds, std::vector<double>* opt_x,
             std::vector<double>* opt_y);

  bool QpWithOsqp(const std::vector<std::pair<double, double>>& raw_point2d,
                  const std::vector<double>& bounds, std::vector<double>* opt_x,
                  std::vector<double>* opt_y);

  bool NlpWithIpopt(const std::vector<std::pair<double, double>>& raw_point2d,
                    const std::vector<double>& bounds,
                    std::vector<double>* opt_x, std::vector<double>* opt_y);

  bool SqpWithOsqp(const std::vector<std::pair<double, double>>& raw_point2d,
                   const std::vector<double>& bounds,
                   std::vector<double>* opt_x, std::vector<double>* opt_y);

 private:
  FemPosDeviationSmootherConfig config_;
};
}  // namespace planning
}  // namespace apollo
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

fem_pos_deviation_smoother_config.proto

syntax = "proto2";

package apollo.planning;

message FemPosDeviationSmootherConfig {
  optional double weight_fem_pos_deviation = 2 [default = 1.0e10];
  optional double weight_ref_deviation = 3 [default = 1.0];
  optional double weight_path_length = 4 [default = 1.0];
  optional bool apply_curvature_constraint = 5 [default = false];
  optional double weight_curvature_constraint_slack_var = 6 [default = 1.0e2];
  optional double curvature_constraint = 7 [default = 0.2];
  optional bool use_sqp = 8 [default = false];
  optional double sqp_ftol = 9 [default = 1e-4];
  optional double sqp_ctol = 10 [default = 1e-3];
  optional int32 sqp_pen_max_iter = 11 [default = 10];
  optional int32 sqp_sub_max_iter = 12 [default = 100];

  // osqp settings
  optional int32 max_iter = 100 [default = 500];
  // time_limit set to be 0.0 meaning no time limit
  optional double time_limit = 101 [default = 0.0];
  optional bool verbose = 102 [default = false];
  optional bool scaled_termination = 103 [default = true];
  optional bool warm_start = 104 [default = true];

  // ipopt settings
  optional int32 print_level = 200 [default = 0];
  optional int32 max_num_of_iterations = 201 [default = 500];
  optional int32 acceptable_num_of_iterations = 202 [default = 15];
  optional double tol = 203 [default = 1e-8];
  optional double acceptable_tol = 204 [default = 1e-1];
}

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