Apollo代码的实现
PiecewiseJerkProblem::Optimize方法中，首先调用FormulateProblem()构造QP问题的Q，P和A，b，然后进行OSQP设置osqp_setup，最后调用osqp_solve求解 。
bool PiecewiseJerkProblem::Optimize(const int max_iter) {OSQPData* data = https://www.isolves.com/it/cxkf/sf/2022-02-11/FormulateProblem();OSQPSettings* settings = SolverDefaultSettings();settings->max_iter = max_iter;OSQPWorkspace* osqp_work = nullptr;osqp_work = osqp_setup(data, settings);// osqp_setup(&osqp_work, data, settings);osqp_solve(osqp_work);auto status = osqp_work->info->status_val;if (status < 0 || (status != 1 && status != 2)) {AERROR << "failed optimization status:t" << osqp_work->info->status;osqp_cleanup(osqp_work);FreeData(data);c_free(settings);return false;} else if (osqp_work->solution == nullptr) {AERROR << "The solution from OSQP is nullptr";osqp_cleanup(osqp_work);FreeData(data);c_free(settings);return false;}// extract primal resultsx_.resize(num_of_knots_);dx_.resize(num_of_knots_);ddx_.resize(num_of_knots_);for (size_t i = 0; i < num_of_knots_; ++i) {x_.at(i) = osqp_work->solution->x[i] / scale_factor_[0];dx_.at(i) = osqp_work->solution->x[i + num_of_knots_] / scale_factor_[1];ddx_.at(i) =osqp_work->solution->x[i + 2 * num_of_knots_] / scale_factor_[2];}// Cleanuposqp_cleanup(osqp_work);FreeData(data);c_free(settings);return true;} 其中，调用CalculateKernel进行P的构造，调用CalculateAffineConstraint进行A的构造，调用CalculateOffset进行Q的构造 。
OSQPData* PiecewiseJerkProblem::FormulateProblem() {// calculate kernelstd::vector<c_float> P_data;std::vector<c_int> P_indices;std::vector<c_int> P_indptr;CalculateKernel(&P_data, &P_indices, &P_indptr);// calculate affine constraintsstd::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;CalculateAffineConstraint(&A_data, &A_indices, &A_indptr, &lower_bounds,&upper_bounds);// calculate offsetstd::vector<c_float> q;CalculateOffset(&q);OSQPData* data = https://www.isolves.com/it/cxkf/sf/2022-02-11/reinterpret_cast(c_malloc(sizeof(OSQPData)));CHECK_EQ(lower_bounds.size(), upper_bounds.size());size_t kernel_dim = 3 * num_of_knots_;size_t num_affine_constraint = lower_bounds.size();data->n = kernel_dim;data->m = num_affine_constraint;data->P = csc_matrix(kernel_dim, kernel_dim, P_data.size(), CopyData(P_data),CopyData(P_indices), CopyData(P_indptr));data->q = CopyData(q);data->A =csc_matrix(num_affine_constraint, kernel_dim, A_data.size(),CopyData(A_data), CopyData(A_indices), CopyData(A_indptr));data->l = CopyData(lower_bounds);data->u = CopyData(upper_bounds);return data;} 构建P:
参见公式10 。
void PiecewiseJerkPathProblem::CalculateKernel(std::vector<c_float>* P_data,std::vector<c_int>* P_indices,std::vector<c_int>* P_indptr) {const int n = static_cast<int>(num_of_knots_);const int num_of_variables = 3 * n;const int num_of_nonzeros = num_of_variables + (n - 1);std::vector<std::vector<std::pair<c_int, c_float>>> columns(num_of_variables);int value_index = 0;// x(i)^2 * (w_x + w_x_ref[i]), w_x_ref might be a uniform value for all x(i)// or piecewise values for different x(i) 参见公式7for (int i = 0; i < n - 1; ++i) {columns[i].emplace_back(i, (weight_x_ + weight_x_ref_vec_[i]) /(scale_factor_[0] * scale_factor_[0]));++value_index;}// x(n-1)^2 * (w_x + w_x_ref[n-1] + w_end_x)columns[n - 1].emplace_back(n - 1, (weight_x_ + weight_x_ref_vec_[n - 1] + weight_end_state_[0]) /(scale_factor_[0] * scale_factor_[0]));++value_index;// x(i)'^2 * w_dx参见公式8for (int i = 0; i < n - 1; ++i) {columns[n + i].emplace_back(n + i, weight_dx_ / (scale_factor_[1] * scale_factor_[1]));++value_index;}// x(n-1)'^2 * (w_dx + w_end_dx)columns[2 * n - 1].emplace_back(2 * n - 1,(weight_dx_ + weight_end_state_[1]) /(scale_factor_[1] * scale_factor_[1]));++value_index;auto delta_s_square = delta_s_ * delta_s_;// x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)参见公式9的对角线columns[2 * n].emplace_back(2 * n,(weight_ddx_ + weight_dddx_ / delta_s_square) /(scale_factor_[2] * scale_factor_[2]));++value_index;for (int i = 1; i < n - 1; ++i) {columns[2 * n + i].emplace_back(2 * n + i, (weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square) /(scale_factor_[2] * scale_factor_[2]));++value_index;}columns[3 * n - 1].emplace_back(3 * n - 1,(weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) /(scale_factor_[2] * scale_factor_[2]));++value_index;// -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''参见公式9的对角线下面的次对角线for (int i = 0; i < n - 1; ++i) {columns[2 * n + i].emplace_back(2 * n + i + 1,(-2.0 * weight_dddx_ / delta_s_square) /(scale_factor_[2] * scale_factor_[2]));++value_index;}CHECK_EQ(value_index, num_of_nonzeros);int ind_p = 0;for (int i = 0; i < num_of_variables; ++i) {P_indptr->push_back(ind_p);for (const auto& row_data_pair : columns[i]) {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);}