Apollo 11.0
自动驾驶开放平台
Public 成员函数 | 所有成员列表
apollo::planning::LateralOSQPOptimizer类 参考

#include <lateral_osqp_optimizer.h>

类 apollo::planning::LateralOSQPOptimizer 继承关系图:
apollo::planning::LateralOSQPOptimizer 的协作图:

Public 成员函数

 LateralOSQPOptimizer ()=default
 
virtual ~LateralOSQPOptimizer ()=default
 
bool optimize (const std::array< double, 3 > &d_state, const double delta_s, const std::vector< std::pair< double, double > > &d_bounds) override
 
- Public 成员函数 继承自 apollo::planning::LateralQPOptimizer
 LateralQPOptimizer ()=default
 
virtual ~LateralQPOptimizer ()=default
 
virtual PiecewiseJerkTrajectory1d GetOptimalTrajectory () const
 
virtual std::vector< common::FrenetFramePoint > GetFrenetFramePath () const
 

额外继承的成员函数

- Protected 属性 继承自 apollo::planning::LateralQPOptimizer
double delta_s_ = FLAGS_default_delta_s_lateral_optimization
 
std::vector< double > opt_d_
 
std::vector< double > opt_d_prime_
 
std::vector< double > opt_d_pprime_
 

详细描述

在文件 lateral_osqp_optimizer.h 第 34 行定义.

构造及析构函数说明

◆ LateralOSQPOptimizer()

apollo::planning::LateralOSQPOptimizer::LateralOSQPOptimizer ( )
default

◆ ~LateralOSQPOptimizer()

virtual apollo::planning::LateralOSQPOptimizer::~LateralOSQPOptimizer ( )
virtualdefault

成员函数说明

◆ optimize()

bool apollo::planning::LateralOSQPOptimizer::optimize ( const std::array< double, 3 > &  d_state,
const double  delta_s,
const std::vector< std::pair< double, double > > &  d_bounds 
)
overridevirtual

实现了 apollo::planning::LateralQPOptimizer.

在文件 lateral_osqp_optimizer.cc 第 26 行定义.

28 {
29 std::vector<c_float> P_data;
30 std::vector<c_int> P_indices;
31 std::vector<c_int> P_indptr;
32 CalculateKernel(d_bounds, &P_data, &P_indices, &P_indptr);
33 delta_s_ = delta_s;
34 const int num_var = static_cast<int>(d_bounds.size());
35 const int kNumParam = 3 * static_cast<int>(d_bounds.size());
36 const int kNumConstraint = kNumParam + 3 * (num_var - 1) + 3;
37 c_float lower_bounds[kNumConstraint];
38 c_float upper_bounds[kNumConstraint];
39
40 const int prime_offset = num_var;
41 const int pprime_offset = 2 * num_var;
42
43 std::vector<std::vector<std::pair<c_int, c_float>>> columns;
44 columns.resize(kNumParam);
45
46 int constraint_index = 0;
47
48 // d_i+1'' - d_i''
49 for (int i = 0; i + 1 < num_var; ++i) {
50 columns[pprime_offset + i].emplace_back(constraint_index, -1.0);
51 columns[pprime_offset + i + 1].emplace_back(constraint_index, 1.0);
52
53 lower_bounds[constraint_index] =
54 -FLAGS_lateral_third_order_derivative_max * delta_s_;
55 upper_bounds[constraint_index] =
56 FLAGS_lateral_third_order_derivative_max * delta_s_;
57 ++constraint_index;
58 }
59
60 // d_i+1' - d_i' - 0.5 * ds * (d_i'' + d_i+1'')
61 for (int i = 0; i + 1 < num_var; ++i) {
62 columns[prime_offset + i].emplace_back(constraint_index, -1.0);
63 columns[prime_offset + i + 1].emplace_back(constraint_index, 1.0);
64 columns[pprime_offset + i].emplace_back(constraint_index, -0.5 * delta_s_);
65 columns[pprime_offset + i + 1].emplace_back(constraint_index,
66 -0.5 * delta_s_);
67
68 lower_bounds[constraint_index] = 0.0;
69 upper_bounds[constraint_index] = 0.0;
70 ++constraint_index;
71 }
72
73 // d_i+1 - d_i - d_i' * ds - 1/3 * d_i'' * ds^2 - 1/6 * d_i+1'' * ds^2
74 for (int i = 0; i + 1 < num_var; ++i) {
75 columns[i].emplace_back(constraint_index, -1.0);
76 columns[i + 1].emplace_back(constraint_index, 1.0);
77 columns[prime_offset + i].emplace_back(constraint_index, -delta_s_);
78 columns[pprime_offset + i].emplace_back(constraint_index,
79 -delta_s_ * delta_s_ / 3.0);
80 columns[pprime_offset + i + 1].emplace_back(constraint_index,
81 -delta_s_ * delta_s_ / 6.0);
82
83 lower_bounds[constraint_index] = 0.0;
84 upper_bounds[constraint_index] = 0.0;
85 ++constraint_index;
86 }
87
88 columns[0].emplace_back(constraint_index, 1.0);
89 lower_bounds[constraint_index] = d_state[0];
90 upper_bounds[constraint_index] = d_state[0];
91 ++constraint_index;
92
93 columns[prime_offset].emplace_back(constraint_index, 1.0);
94 lower_bounds[constraint_index] = d_state[1];
95 upper_bounds[constraint_index] = d_state[1];
96 ++constraint_index;
97
98 columns[pprime_offset].emplace_back(constraint_index, 1.0);
99 lower_bounds[constraint_index] = d_state[2];
100 upper_bounds[constraint_index] = d_state[2];
101 ++constraint_index;
102
103 const double LARGE_VALUE = 2.0;
104 for (int i = 0; i < kNumParam; ++i) {
105 columns[i].emplace_back(constraint_index, 1.0);
106 if (i < num_var) {
107 lower_bounds[constraint_index] = d_bounds[i].first;
108 upper_bounds[constraint_index] = d_bounds[i].second;
109 } else {
110 lower_bounds[constraint_index] = -LARGE_VALUE;
111 upper_bounds[constraint_index] = LARGE_VALUE;
112 }
113 ++constraint_index;
114 }
115
116 CHECK_EQ(constraint_index, kNumConstraint);
117
118 // change affine_constraint to CSC format
119 std::vector<c_float> A_data;
120 std::vector<c_int> A_indices;
121 std::vector<c_int> A_indptr;
122 int ind_p = 0;
123 for (int j = 0; j < kNumParam; ++j) {
124 A_indptr.push_back(ind_p);
125 for (const auto& row_data_pair : columns[j]) {
126 A_data.push_back(row_data_pair.second);
127 A_indices.push_back(row_data_pair.first);
128 ++ind_p;
129 }
130 }
131 A_indptr.push_back(ind_p);
132
133 // offset
134 double q[kNumParam];
135 for (int i = 0; i < kNumParam; ++i) {
136 if (i < num_var) {
137 q[i] = -2.0 * FLAGS_weight_lateral_obstacle_distance *
138 (d_bounds[i].first + d_bounds[i].second);
139 } else {
140 q[i] = 0.0;
141 }
142 }
143
144 // Problem settings
145 OSQPSettings* settings =
146 reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
147
148 // Define Solver settings as default
149 osqp_set_default_settings(settings);
150 settings->alpha = 1.0; // Change alpha parameter
151 settings->eps_abs = 1.0e-05;
152 settings->eps_rel = 1.0e-05;
153 settings->max_iter = 5000;
154 settings->polish = true;
155 settings->verbose = FLAGS_enable_osqp_debug;
156
157 // Populate data
158 OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
159 data->n = kNumParam;
160 data->m = kNumConstraint;
161 data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(),
162 P_indices.data(), P_indptr.data());
163 data->q = q;
164 data->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(),
165 A_indices.data(), A_indptr.data());
166 data->l = lower_bounds;
167 data->u = upper_bounds;
168
169 // Workspace
170 OSQPWorkspace* work = nullptr;
171 // osqp_setup(&work, data, settings);
172 work = osqp_setup(data, settings);
173
174 // Solve Problem
175 osqp_solve(work);
176
177 // extract primal results
178 for (int i = 0; i < num_var; ++i) {
179 opt_d_.push_back(work->solution->x[i]);
180 opt_d_prime_.push_back(work->solution->x[i + num_var]);
181 opt_d_pprime_.push_back(work->solution->x[i + 2 * num_var]);
182 }
183 opt_d_prime_[num_var - 1] = 0.0;
184 opt_d_pprime_[num_var - 1] = 0.0;
185
186 // Cleanup
187 osqp_cleanup(work);
188 c_free(data->A);
189 c_free(data->P);
190 c_free(data);
191 c_free(settings);
192
193 return true;
194}
该类的文档由以下文件生成: