apollo
apollo copied to clipboard
ApolloAuto
离散点平滑 Fem Smooth - ipopt 曲率约束
在 /apollo/modules/planning/math/discretized_points_smoothing/fem_pos_deviation_ipopt_interface.cc中 for (size_t i = 0; i + 2 < num_of_points_; ++i) { size_t findex = i * 2; size_t mindex = findex + 2; size_t lindex = mindex + 2; g[curvature_constr_start_index_ + i] = ( ((x[findex] + x[lindex]) - 2.0 * x[mindex]) * ((x[findex] + x[lindex]) - 2.0 * x[mindex]) + ((x[findex + 1] + x[lindex + 1]) - 2.0 * x[mindex + 1]) * ((x[findex + 1] + x[lindex + 1]) - 2.0 * x[mindex + 1])) - x[slack_var_start_index_ + i]; }
// b. curvature constraints double ref_total_length = 0.0; auto pre_point = ref_points_.front(); for (size_t i = 1; i < num_of_points_; ++i) { auto cur_point = ref_points_[i]; double x_diff = cur_point.first - pre_point.first; double y_diff = cur_point.second - pre_point.second; ref_total_length += std::sqrt(x_diff * x_diff + y_diff * y_diff); pre_point = cur_point; } double average_delta_s = ref_total_length / static_cast(num_of_points_ - 1); double curvature_constr_upper = average_delta_s * average_delta_s * curvature_constraint;
按照 curvature_const_upper 的公式进行曲率约束的设置，设定曲率约束为 0.2 时，在使用 ipopt 平滑后发现实际上曲率会约束在 0.4 附近。如果需要约束到 0.2，则需要在 curvature_const_upper 的基础上再乘以 0.5，这与理论推导不相符，请问是为什么？
@SongJiayu1 pls format the code or use a github permalink (docs) to make your code more clearly, which will more easy to read.
