EGO Planner代码解析bspline

1、 int BsplineOptimizer::earlyExit(void *func_data, const double *x, const double *g, const double fx, const double xnorm, const double gnorm, const double step, int n, int k, int ls)

//如果force_stop_type_不为DONT_STOP就返回true，否则返回false。

int BsplineOptimizer::earlyExit(void *func_data, const double *x, const double *g, const double fx, const double xnorm, const double gnorm, const double step, int n, int k, int ls){BsplineOptimizer *opt = reinterpret_cast<BsplineOptimizer *>(func_data);// cout << "k=" << k << endl;// cout << "opt->flag_continue_to_optimize_=" << opt->flag_continue_to_optimize_ << endl;return (opt->force_stop_type_ == STOP_FOR_ERROR || opt->force_stop_type_ == STOP_FOR_REBOUND);}//如果force_stop_type_不为DONT_STOP就返回true，否则返回false。

2、double BsplineOptimizer::costFunctionRebound(void *func_data, const double *x, double *grad, const int n)

//调用combineCostRebound(x,grad,cost,n) 返回cost代价。

double BsplineOptimizer::costFunctionRebound(void *func_data, const double *x, double *grad, const int n){BsplineOptimizer *opt = reinterpret_cast<BsplineOptimizer *>(func_data);double cost;opt->combineCostRebound(x, grad, cost, n);opt->iter_num_ += 1;return cost;}//调用combineCostRebound(x,grad,cost,n) 返回cost。

3、double BsplineOptimizer::costFunctionRefine(void *func_data, const double *x, double *grad, const int n)

//调用combineCostRefine(x,grad,cost,n) 返回加权后的代价cost

double BsplineOptimizer::costFunctionRefine(void *func_data, const double *x, double *grad, const int n){BsplineOptimizer *opt = reinterpret_cast<BsplineOptimizer *>(func_data);double cost;opt->combineCostRefine(x, grad, cost, n);opt->iter_num_ += 1;return cost;}//调用combineCostRefine(x,grad,cost,n) 返回cost

4、void BsplineOptimizer::calcDistanceCostRebound(const Eigen::MatrixXd &q, double &cost,

Eigen::MatrixXd &gradient, int iter_num, double smoothness_cost)

//计算碰撞惩罚，对应论文中Collision Penalty部分公式(5)(6)(7)

 void BsplineOptimizer::calcDistanceCostRebound(const Eigen::MatrixXd &q, double &cost,Eigen::MatrixXd &gradient, int iter_num, double smoothness_cost)//计算碰撞惩罚，对应论文中Collision Penalty部分公式(5)(6)(7){cost = 0.0;//代价int end_idx = q.cols() - order_;//最后的索引double demarcation = cps_.clearance;//控制点的安全距离sfdouble a = 3 * demarcation, b = -3 * pow(demarcation, 2), c = pow(demarcation, 3);//a=3倍的安全距离//b=-3*sf的平方//c=sf的平方force_stop_type_ = DONT_STOP;//停止原因if (iter_num > 3 && smoothness_cost / (cps_.size - 2 * order_) < 0.1)// 0.1 is an experimental value that indicates the trajectory is smooth enough.//0.1是一个实验值，表明轨迹足够平滑。{check_collision_and_rebound();//检查碰撞和对应轨迹弹出}/*** calculate distance cost and gradient ***///计算距离成本和梯度for (auto i = order_; i < end_idx; ++i)//遍历控制点{for (size_t j = 0; j < cps_.direction[i].size(); ++j)//遍历所有基点{double dist = (cps_.points.col(i) - cps_.base_point[i][j]).dot(cps_.direction[i][j]);//距离=(第i个控制点-第i个控制点对应的第j个基点（即控制点到第j个障碍物的距离）)*该方向的方向向量double dist_err = cps_.clearance - dist，;//安全距离-距离，对应论文上的cij,即cij=sf-dijEigen::Vector3d dist_grad = cps_.direction[i][j];//距离梯度为第i个控制点与对应的第j个基点的方向if (dist_err < 0)//如果cij <0{/* do nothing */}else if (dist_err < demarcation)//如果cij小于安全距离（0<cij<sf）{cost += pow(dist_err, 3);//代价为cij的三次方gradient.col(i) += -3.0 * dist_err * dist_err * dist_grad;//该控制点的梯度为=控制点的梯度+(-3*cij*cij*梯度方向)，对应论文计算梯度的第二个公式}else//如果cij>sf,控制点离障碍物的距离小于安全距离{cost += a * dist_err * dist_err + b * dist_err + c;//代价=代价+a*cij*cij+b*cij+c，其中，a=3倍的sf，b=-3*sf的平方,c=sf的平方gradient.col(i) += -(2.0 * a * dist_err + b) * dist_grad;//该控制点的梯度为=控制点的梯度-(2.0 * a * cij + b) * 梯度方向}}}}//计算碰撞惩罚，对应论文中Collision Penalty部分公式(5)(6)(7)

5、void BsplineOptimizer::calcFitnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient)

//计算适应项代价

 void BsplineOptimizer::calcFitnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient)//计算适应项代价{cost = 0.0;//代价为0int end_idx = q.cols() - order_;// def: f = |x*v|^2/a^2 + |x×v|^2/b^2double a2 = 25, b2 = 1;for (auto i = order_ - 1; i < end_idx + 1; ++i){Eigen::Vector3d x = (q.col(i - 1) + 4 * q.col(i) + q.col(i + 1)) / 6.0 - ref_pts_[i - 1];Eigen::Vector3d v = (ref_pts_[i] - ref_pts_[i - 2]).normalized();double xdotv = x.dot(v);Eigen::Vector3d xcrossv = x.cross(v);double f = pow((xdotv), 2) / a2 + pow(xcrossv.norm(), 2) / b2;cost += f;Eigen::Matrix3d m;m << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;Eigen::Vector3d df_dx = 2 * xdotv / a2 * v + 2 / b2 * m * xcrossv;gradient.col(i - 1) += df_dx / 6;gradient.col(i) += 4 * df_dx / 6;gradient.col(i + 1) += df_dx / 6;}}

6、void BsplineOptimizer::calcSmoothnessCost(const Eigen::MatrixXd &q, double &cost,

Eigen::MatrixXd &gradient, bool falg_use_jerk /* = true*/)

//计算平滑代价，对应论文中Smoothness Penalty部分公式(4)

void BsplineOptimizer::calcSmoothnessCost(const Eigen::MatrixXd &q, double &cost,Eigen::MatrixXd &gradient, bool falg_use_jerk /* = true*/)//计算平滑代价，对应论文中Smoothness Penalty部分公式(4){cost = 0.0;if (falg_use_jerk)//加加速度{Eigen::Vector3d jerk, temp_j;for (int i = 0; i < q.cols() - 3; i++)//i=0到Nc-2{/* evaluate jerk */jerk = q.col(i + 3) - 3 * q.col(i + 2) + 3 * q.col(i + 1) - q.col(i);//计算加加速度cost += jerk.squaredNorm();//累加temp_j = 2.0 * jerk;/* jerk gradient *///加加速度梯度gradient.col(i + 0) += -temp_j;gradient.col(i + 1) += 3.0 * temp_j;gradient.col(i + 2) += -3.0 * temp_j;gradient.col(i + 3) += temp_j;}}else{Eigen::Vector3d acc, temp_acc;//加速度for (int i = 0; i < q.cols() - 2; i++)//i=0到Nc-1{/* evaluate acc */acc = q.col(i + 2) - 2 * q.col(i + 1) + q.col(i);cost += acc.squaredNorm();//累加temp_acc = 2.0 * acc;/* acc gradient *///加速度梯度gradient.col(i + 0) += temp_acc;gradient.col(i + 1) += -2.0 * temp_acc;gradient.col(i + 2) += temp_acc;}}}

7、 void BsplineOptimizer::calcFeasibilityCost(const Eigen::MatrixXd &q, double &cost,

Eigen::MatrixXd &gradient)

//计算可行性代价，对应论文中Feasibility Penalty部分公式(8)(9)(10)

//通过在每一维上限制轨迹的高阶导数来确保可行性

void BsplineOptimizer::calcFeasibilityCost(const Eigen::MatrixXd &q, double &cost,Eigen::MatrixXd &gradient)//计算可行性代价，对应论文中Feasibility Penalty部分公式(8)(9)(10)//通过在每一维上限制轨迹的高阶导数来确保可行性{//#define SECOND_DERIVATIVE_CONTINOUS,二阶导数连续#ifdef SECOND_DERIVATIVE_CONTINOUScost = 0.0;double demarcation = 1.0; // 1m/s, 1m/s/s  安全范围double ar = 3 * demarcation, br = -3 * pow(demarcation, 2), cr = pow(demarcation, 3);double al = ar, bl = -br, cl = cr;/* abbreviation */double ts, ts_inv2, ts_inv3;ts = bspline_interval_;//ts为b样条的间隔时间ts_inv2 = 1 / ts / ts;//加速度ts_inv3 = 1 / ts / ts / ts;//加加速度/* velocity feasibility *///速度可行性for (int i = 0; i < q.cols() - 1; i++){Eigen::Vector3d vi = (q.col(i + 1) - q.col(i)) / ts;//速度为后一个控制点-前一个控制点再除以时间tsfor (int j = 0; j < 3; j++){if (vi(j) > max_vel_ + demarcation)//如果速度大于最大速度+安全速度1m/s{double diff = vi(j) - max_vel_;//差值为当前速度-最大速度cost += (ar * diff * diff + br * diff + cr) * ts_inv3; //代价// multiply ts_inv3 to make vel and acc has similar magnitude//乘以ts_inv3，使vel和acc具有相似的幅值double grad = (2.0 * ar * diff + br) / ts * ts_inv3;//计算梯度gradient(j, i + 0) += -grad;gradient(j, i + 1) += grad;}else if (vi(j) > max_vel_)//如果速度大于最大速度{double diff = vi(j) - max_vel_;cost += pow(diff, 3) * ts_inv3;;double grad = 3 * diff * diff / ts * ts_inv3;;gradient(j, i + 0) += -grad;gradient(j, i + 1) += grad;}else if (vi(j) < -(max_vel_ + demarcation)){double diff = vi(j) + max_vel_;cost += (al * diff * diff + bl * diff + cl) * ts_inv3;double grad = (2.0 * al * diff + bl) / ts * ts_inv3;gradient(j, i + 0) += -grad;gradient(j, i + 1) += grad;}else if (vi(j) < -max_vel_){double diff = vi(j) + max_vel_;cost += -pow(diff, 3) * ts_inv3;double grad = -3 * diff * diff / ts * ts_inv3;gradient(j, i + 0) += -grad;gradient(j, i + 1) += grad;}else{/* nothing happened */}}}/* acceleration feasibility *///加速度可行性for (int i = 0; i < q.cols() - 2; i++){Eigen::Vector3d ai = (q.col(i + 2) - 2 * q.col(i + 1) + q.col(i)) * ts_inv2;//加速度=(第三个控制点-2*第二个控制点+当前控制点）* 1 / ts / tsfor (int j = 0; j < 3; j++){if (ai(j) > max_acc_ + demarcation)//角速度>最大加速度+安全加速度1m/s{double diff = ai(j) - max_acc_;cost += ar * diff * diff + br * diff + cr;//相应代价double grad = (2.0 * ar * diff + br) * ts_inv2;gradient(j, i + 0) += grad;gradient(j, i + 1) += -2 * grad;gradient(j, i + 2) += grad;}else if (ai(j) > max_acc_){double diff = ai(j) - max_acc_;cost += pow(diff, 3);double grad = 3 * diff * diff * ts_inv2;gradient(j, i + 0) += grad;gradient(j, i + 1) += -2 * grad;gradient(j, i + 2) += grad;}else if (ai(j) < -(max_acc_ + demarcation)){double diff = ai(j) + max_acc_;cost += al * diff * diff + bl * diff + cl;double grad = (2.0 * al * diff + bl) * ts_inv2;gradient(j, i + 0) += grad;gradient(j, i + 1) += -2 * grad;gradient(j, i + 2) += grad;}else if (ai(j) < -max_acc_){double diff = ai(j) + max_acc_;cost += -pow(diff, 3);double grad = -3 * diff * diff * ts_inv2;gradient(j, i + 0) += grad;gradient(j, i + 1) += -2 * grad;gradient(j, i + 2) += grad;}else{/* nothing happened */}}}#elsecost = 0.0;/* abbreviation */double ts, /*vm2, am2, */ ts_inv2;// vm2 = max_vel_ * max_vel_;// am2 = max_acc_ * max_acc_;ts = bspline_interval_;ts_inv2 = 1 / ts / ts;/* velocity feasibility *///速度可行性for (int i = 0; i < q.cols() - 1; i++)//i=1到Nc,对应论文中Feasibility Penalty部分公式(8){Eigen::Vector3d vi = (q.col(i + 1) - q.col(i)) / ts;//对应论文中公式(2)//cout << "temp_v * vi=" ;for (int j = 0; j < 3; j++)//xyz三轴上分别计算速度{if (vi(j) > max_vel_)//每个轴上的速度>最大速度{// cout << "fuck VEL" << endl;// cout << vi(j) << endl;cost += pow(vi(j) - max_vel_, 2) * ts_inv2;//累加从i=1到Nc，计算WvF(Vi)// multiply ts_inv3 to make vel and acc has similar magnitude// * ts_inv2使vel和acc具有相似的幅值,即乘以一个权重Wvgradient(j, i + 0) += -2 * (vi(j) - max_vel_) / ts * ts_inv2;gradient(j, i + 1) += 2 * (vi(j) - max_vel_) / ts * ts_inv2;}else if (vi(j) < -max_vel_){cost += pow(vi(j) + max_vel_, 2) * ts_inv2;gradient(j, i + 0) += -2 * (vi(j) + max_vel_) / ts * ts_inv2;gradient(j, i + 1) += 2 * (vi(j) + max_vel_) / ts * ts_inv2;}else{/* code */}}}/* acceleration feasibility *///加速度可行性for (int i = 0; i < q.cols() - 2; i++)//i=1到Nc-1,对应论文中Feasibility Penalty部分公式(8){Eigen::Vector3d ai = (q.col(i + 2) - 2 * q.col(i + 1) + q.col(i)) * ts_inv2;//计算速度//cout << "temp_a * ai=" ;for (int j = 0; j < 3; j++)//计算xyz每个轴上的加速度{if (ai(j) > max_acc_)//加速度>最大加速度{// cout << "fuck ACC" << endl;// cout << ai(j) << endl;cost += pow(ai(j) - max_acc_, 2);//累加从i=1到Nc-1，计算WaF(Ai)gradient(j, i + 0) += 2 * (ai(j) - max_acc_) * ts_inv2;gradient(j, i + 1) += -4 * (ai(j) - max_acc_) * ts_inv2;gradient(j, i + 2) += 2 * (ai(j) - max_acc_) * ts_inv2;}else if (ai(j) < -max_acc_){cost += pow(ai(j) + max_acc_, 2);gradient(j, i + 0) += 2 * (ai(j) + max_acc_) * ts_inv2;gradient(j, i + 1) += -4 * (ai(j) + max_acc_) * ts_inv2;gradient(j, i + 2) += 2 * (ai(j) + max_acc_) * ts_inv2;}else{/* code */}}//cout << endl;}#endif}

8、 bool BsplineOptimizer::check_collision_and_rebound(void)

//判断是否需要检查障碍物和将轨迹从障碍物中弹出,与计算Pij出代码

bool BsplineOptimizer::check_collision_and_rebound(void)//判断是否需要检查障碍物和将轨迹从障碍物中弹出,与计算Pij出代码一致{int end_idx = cps_.size - order_;/*** Check and segment the initial trajectory according to obstacles ***///根据障碍物检查并分段初始轨迹int in_id, out_id;vector<std::pair<int, int>> segment_ids;bool flag_new_obs_valid = false;int i_end = end_idx - (end_idx - order_) / 3;for (int i = order_ - 1; i <= i_end; ++i){bool occ = grid_map_->getInflateOccupancy(cps_.points.col(i));/*** check if the new collision will be valid ***///检查新障碍物是否有效if (occ){for (size_t k = 0; k < cps_.direction[i].size(); ++k){cout.precision(2);if ((cps_.points.col(i) - cps_.base_point[i][k]).dot(cps_.direction[i][k]) < 1 * grid_map_->getResolution()) // current point is outside all the collision_points.{occ = false; // Not really takes effect, just for better hunman understanding.break;}}}if (occ){flag_new_obs_valid = true;int j;for (j = i - 1; j >= 0; --j){occ = grid_map_->getInflateOccupancy(cps_.points.col(j));if (!occ){in_id = j;break;}}if (j < 0) // fail to get the obs free point，无法获取obs空闲点{ROS_ERROR("ERROR! the drone is in obstacle. This should not happen.");in_id = 0;}for (j = i + 1; j < cps_.size; ++j){occ = grid_map_->getInflateOccupancy(cps_.points.col(j));if (!occ){out_id = j;break;}}if (j >= cps_.size) // fail to get the obs free point{ROS_WARN("WARN! terminal point of the current trajectory is in obstacle, skip this planning.");force_stop_type_ = STOP_FOR_ERROR;return false;}i = j + 1;segment_ids.push_back(std::pair<int, int>(in_id, out_id));}}if (flag_new_obs_valid){vector<vector<Eigen::Vector3d>> a_star_pathes;for (size_t i = 0; i < segment_ids.size(); ++i){/*** a star search ***/Eigen::Vector3d in(cps_.points.col(segment_ids[i].first)), out(cps_.points.col(segment_ids[i].second));if (a_star_->AstarSearch(/*(in-out).norm()/10+0.05*/ 0.1, in, out)){a_star_pathes.push_back(a_star_->getPath());}else{ROS_ERROR("a star error");segment_ids.erase(segment_ids.begin() + i);i--;}}/*** Assign parameters to each segment ***/for (size_t i = 0; i < segment_ids.size(); ++i){// step 1for (int j = segment_ids[i].first; j <= segment_ids[i].second; ++j)cps_.flag_temp[j] = false;// step 2int got_intersection_id = -1;for (int j = segment_ids[i].first + 1; j < segment_ids[i].second; ++j){Eigen::Vector3d ctrl_pts_law(cps_.points.col(j + 1) - cps_.points.col(j - 1)), intersection_point;int Astar_id = a_star_pathes[i].size() / 2, last_Astar_id; // Let "Astar_id = id_of_the_most_far_away_Astar_point" will be better, but it needs more computationdouble val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law), last_val = val;while (Astar_id >= 0 && Astar_id < (int)a_star_pathes[i].size()){last_Astar_id = Astar_id;if (val >= 0)--Astar_id;else++Astar_id;val = (a_star_pathes[i][Astar_id] - cps_.points.col(j)).dot(ctrl_pts_law);// cout << val << endl;if (val * last_val <= 0 && (abs(val) > 0 || abs(last_val) > 0)) // val = last_val = 0.0 is not allowed{intersection_point =a_star_pathes[i][Astar_id] +((a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id]) *(ctrl_pts_law.dot(cps_.points.col(j) - a_star_pathes[i][Astar_id]) / ctrl_pts_law.dot(a_star_pathes[i][Astar_id] - a_star_pathes[i][last_Astar_id])) // = t);got_intersection_id = j;break;}}if (got_intersection_id >= 0){cps_.flag_temp[j] = true;double length = (intersection_point - cps_.points.col(j)).norm();if (length > 1e-5){for (double a = length; a >= 0.0; a -= grid_map_->getResolution()){bool occ = grid_map_->getInflateOccupancy((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j));if (occ || a < grid_map_->getResolution()){if (occ)a += grid_map_->getResolution();cps_.base_point[j].push_back((a / length) * intersection_point + (1 - a / length) * cps_.points.col(j));cps_.direction[j].push_back((intersection_point - cps_.points.col(j)).normalized());break;}}}else{got_intersection_id = -1;}}}//step 3if (got_intersection_id >= 0){for (int j = got_intersection_id + 1; j <= segment_ids[i].second; ++j)if (!cps_.flag_temp[j]){cps_.base_point[j].push_back(cps_.base_point[j - 1].back());cps_.direction[j].push_back(cps_.direction[j - 1].back());}for (int j = got_intersection_id - 1; j >= segment_ids[i].first; --j)if (!cps_.flag_temp[j]){cps_.base_point[j].push_back(cps_.base_point[j + 1].back());cps_.direction[j].push_back(cps_.direction[j + 1].back());}}elseROS_WARN("Failed to generate direction. It doesn't matter.");}force_stop_type_ = STOP_FOR_REBOUND;return true;}return false;}

9、bool BsplineOptimizer::BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double ts)

//B样条优化轨迹，将轨迹推出障碍物，得到最优的无碰撞轨迹

//设置时间间隔t，调用rebound_optimize()将轨迹推出障碍物，得到最优的无碰撞轨迹，并将其控制点赋值给optimal_points。

bool BsplineOptimizer::BsplineOptimizeTrajRebound(Eigen::MatrixXd &optimal_points, double ts)//B样条优化轨迹，将轨迹推出障碍物，得到最优的无碰撞轨迹{setBsplineInterval(ts);//设置B样条的间隔bool flag_success = rebound_optimize();optimal_points = cps_.points;return flag_success;//设置时间间隔ts//调用rebound_optimize()将轨迹推出障碍物//得到最优的无碰撞轨迹，并将其控制点赋值给optimal_points。}

10、 bool BsplineOptimizer::BsplineOptimizeTrajRefine(const Eigen::MatrixXd &init_points, const double ts, Eigen::MatrixXd &optimal_points)

//B样条优化轨迹，重新分配时间，到最优的动力学可行轨迹

//设置初始控制点init_points、时间间隔ts，调用refine_optimize()重新分配时间，得到最优的动力学可行轨迹，并将其控制点赋值给optimal_points。

bool BsplineOptimizer::BsplineOptimizeTrajRefine(const Eigen::MatrixXd &init_points, const double ts, Eigen::MatrixXd &optimal_points)//B样条优化轨迹，重新分配时间，到最优的动力学可行轨迹{setControlPoints(init_points);setBsplineInterval(ts);bool flag_success = refine_optimize();optimal_points = cps_.points;return flag_success;//设置初始控制点init_points、时间间隔ts//调用refine_optimize()重新分配时间//得到最优的动力学可行轨迹，并将其控制点赋值给optimal_points}

11、bool BsplineOptimizer::rebound_optimize()

//进行优化，将轨迹推出障碍物

//使用L-BFGS方法对目标函数进行优化，得到光滑、无碰撞、动力学可行的轨迹

bool BsplineOptimizer::rebound_optimize()//进行优化，将轨迹推出障碍物//使用L-BFGS方法对目标函数进行优化，得到光滑、无碰撞、动力学可行的轨迹{iter_num_ = 0;int start_id = order_;int end_id = this->cps_.size - order_;variable_num_ = 3 * (end_id - start_id);double final_cost;ros::Time t0 = ros::Time::now(), t1, t2;int restart_nums = 0, rebound_times = 0;;bool flag_force_return, flag_occ, success;new_lambda2_ = lambda2_;constexpr int MAX_RESART_NUMS_SET = 3;do{/* ---------- prepare ---------- */min_cost_ = std::numeric_limits<double>::max();iter_num_ = 0;flag_force_return = false;flag_occ = false;success = false;double q[variable_num_];memcpy(q, cps_.points.data() + 3 * start_id, variable_num_ * sizeof(q[0]));lbfgs::lbfgs_parameter_t lbfgs_params;lbfgs::lbfgs_load_default_parameters(&lbfgs_params);lbfgs_params.mem_size = 16;lbfgs_params.max_iterations = 200;lbfgs_params.g_epsilon = 0.01;/* ---------- optimize ---------- */t1 = ros::Time::now();int result = lbfgs::lbfgs_optimize(variable_num_, q, &final_cost, BsplineOptimizer::costFunctionRebound, NULL, BsplineOptimizer::earlyExit, this, &lbfgs_params);t2 = ros::Time::now();double time_ms = (t2 - t1).toSec() * 1000;double total_time_ms = (t2 - t0).toSec() * 1000;/* ---------- success temporary, check collision again ---------- */if (result == lbfgs::LBFGS_CONVERGENCE ||result == lbfgs::LBFGSERR_MAXIMUMITERATION ||result == lbfgs::LBFGS_ALREADY_MINIMIZED ||result == lbfgs::LBFGS_STOP){//ROS_WARN("Solver error in planning!, return = %s", lbfgs::lbfgs_strerror(result));flag_force_return = false;UniformBspline traj = UniformBspline(cps_.points, 3, bspline_interval_);double tm, tmp;traj.getTimeSpan(tm, tmp);double t_step = (tmp - tm) / ((traj.evaluateDeBoorT(tmp) - traj.evaluateDeBoorT(tm)).norm() / grid_map_->getResolution());for (double t = tm; t < tmp * 2 / 3; t += t_step) // Only check the closest 2/3 partition of the whole trajectory.{flag_occ = grid_map_->getInflateOccupancy(traj.evaluateDeBoorT(t));if (flag_occ){//cout << "hit_obs, t=" << t << " P=" << traj.evaluateDeBoorT(t).transpose() << endl;if (t <= bspline_interval_) // First 3 control points in obstacles!{cout << cps_.points.col(1).transpose() << "\n"<< cps_.points.col(2).transpose() << "\n"<< cps_.points.col(3).transpose() << "\n"<< cps_.points.col(4).transpose() << endl;ROS_WARN("First 3 control points in obstacles! return false, t=%f", t);return false;}break;}}if (!flag_occ){printf("\033[32miter(+1)=%d,time(ms)=%5.3f,total_t(ms)=%5.3f,cost=%5.3f\n\033[0m", iter_num_, time_ms, total_time_ms, final_cost);success = true;}else // restart{restart_nums++;initControlPoints(cps_.points, false);new_lambda2_ *= 2;printf("\033[32miter(+1)=%d,time(ms)=%5.3f,keep optimizing\n\033[0m", iter_num_, time_ms);}}else if (result == lbfgs::LBFGSERR_CANCELED){flag_force_return = true;rebound_times++;cout << "iter=" << iter_num_ << ",time(ms)=" << time_ms << ",rebound." << endl;}else{ROS_WARN("Solver error. Return = %d, %s. Skip this planning.", result, lbfgs::lbfgs_strerror(result));// while (ros::ok());}} while ((flag_occ && restart_nums < MAX_RESART_NUMS_SET) ||(flag_force_return && force_stop_type_ == STOP_FOR_REBOUND && rebound_times <= 20));return success;}

12、bool BsplineOptimizer::refine_optimize()

//再优化

//使用L-BFGS方法对目标函数进行优化，得到重新分配时间后，光滑、拟合较好、动力学可行的轨迹。

 bool BsplineOptimizer::refine_optimize()//再优化//使用L-BFGS方法对目标函数进行优化//得到重新分配时间后，光滑、拟合较好、动力学可行的轨迹。{iter_num_ = 0;int start_id = order_;int end_id = this->cps_.points.cols() - order_;variable_num_ = 3 * (end_id - start_id);double q[variable_num_];double final_cost;memcpy(q, cps_.points.data() + 3 * start_id, variable_num_ * sizeof(q[0]));double origin_lambda4 = lambda4_;bool flag_safe = true;int iter_count = 0;do{lbfgs::lbfgs_parameter_t lbfgs_params;lbfgs::lbfgs_load_default_parameters(&lbfgs_params);lbfgs_params.mem_size = 16;lbfgs_params.max_iterations = 200;lbfgs_params.g_epsilon = 0.001;int result = lbfgs::lbfgs_optimize(variable_num_, q, &final_cost, BsplineOptimizer::costFunctionRefine, NULL, NULL, this, &lbfgs_params);if (result == lbfgs::LBFGS_CONVERGENCE ||result == lbfgs::LBFGSERR_MAXIMUMITERATION ||result == lbfgs::LBFGS_ALREADY_MINIMIZED ||result == lbfgs::LBFGS_STOP){//pass}else{ROS_ERROR("Solver error in refining!, return = %d, %s", result, lbfgs::lbfgs_strerror(result));}UniformBspline traj = UniformBspline(cps_.points, 3, bspline_interval_);double tm, tmp;traj.getTimeSpan(tm, tmp);double t_step = (tmp - tm) / ((traj.evaluateDeBoorT(tmp) - traj.evaluateDeBoorT(tm)).norm() / grid_map_->getResolution()); // Step size is defined as the maximum size that can passes throgth every gird.for (double t = tm; t < tmp * 2 / 3; t += t_step){if (grid_map_->getInflateOccupancy(traj.evaluateDeBoorT(t))){// cout << "Refined traj hit_obs, t=" << t << " P=" << traj.evaluateDeBoorT(t).transpose() << endl;Eigen::MatrixXd ref_pts(ref_pts_.size(), 3);for (size_t i = 0; i < ref_pts_.size(); i++){ref_pts.row(i) = ref_pts_[i].transpose();}flag_safe = false;break;}}if (!flag_safe)lambda4_ *= 2;iter_count++;} while (!flag_safe && iter_count <= 0);lambda4_ = origin_lambda4;//cout << "iter_num_=" << iter_num_ << endl;return flag_safe;}

13、void BsplineOptimizer::combineCostRebound(const double *x, double *grad, double &f_combine, const int n)

//求得轨迹的光滑项、碰撞项、动力学可行项的加权和f_combine以及梯度方向grad

void BsplineOptimizer::combineCostRebound(const double *x, double *grad, double &f_combine, const int n)//求得轨迹的光滑项、碰撞项、动力学可行项的加权和f_combine以及梯度方向grad{memcpy(cps_.points.data() + 3 * order_, x, n * sizeof(x[0]));/* ---------- evaluate cost and gradient ---------- */double f_smoothness, f_distance, f_feasibility;Eigen::MatrixXd g_smoothness = Eigen::MatrixXd::Zero(3, cps_.size);Eigen::MatrixXd g_distance = Eigen::MatrixXd::Zero(3, cps_.size);Eigen::MatrixXd g_feasibility = Eigen::MatrixXd::Zero(3, cps_.size);calcSmoothnessCost(cps_.points, f_smoothness, g_smoothness);calcDistanceCostRebound(cps_.points, f_distance, g_distance, iter_num_, f_smoothness);calcFeasibilityCost(cps_.points, f_feasibility, g_feasibility);f_combine = lambda1_ * f_smoothness + new_lambda2_ * f_distance + lambda3_ * f_feasibility;//printf("origin %f %f %f %f\n", f_smoothness, f_distance, f_feasibility, f_combine);Eigen::MatrixXd grad_3D = lambda1_ * g_smoothness + new_lambda2_ * g_distance + lambda3_ * g_feasibility;memcpy(grad, grad_3D.data() + 3 * order_, n * sizeof(grad[0]));}

14、 void BsplineOptimizer::combineCostRefine(const double *x, double *grad, double &f_combine, const int n)

//求得延长了分配时长后轨迹的光滑项、拟合项、动力学可行项的加权和f_combine以及梯度方向grad

  void BsplineOptimizer::combineCostRefine(const double *x, double *grad, double &f_combine, const int n)//求得延长了分配时长后轨迹的光滑项、拟合项、动力学可行项的加权和f_combine以及梯度方向grad{memcpy(cps_.points.data() + 3 * order_, x, n * sizeof(x[0]));/* ---------- evaluate cost and gradient ---------- */double f_smoothness, f_fitness, f_feasibility;Eigen::MatrixXd g_smoothness = Eigen::MatrixXd::Zero(3, cps_.points.cols());Eigen::MatrixXd g_fitness = Eigen::MatrixXd::Zero(3, cps_.points.cols());Eigen::MatrixXd g_feasibility = Eigen::MatrixXd::Zero(3, cps_.points.cols());//time_satrt = ros::Time::now();calcSmoothnessCost(cps_.points, f_smoothness, g_smoothness);calcFitnessCost(cps_.points, f_fitness, g_fitness);calcFeasibilityCost(cps_.points, f_feasibility, g_feasibility);/* ---------- convert to solver format...---------- */f_combine = lambda1_ * f_smoothness + lambda4_ * f_fitness + lambda3_ * f_feasibility;// printf("origin %f %f %f %f\n", f_smoothness, f_fitness, f_feasibility, f_combine);Eigen::MatrixXd grad_3D = lambda1_ * g_smoothness + lambda4_ * g_fitness + lambda3_ * g_feasibility;memcpy(grad, grad_3D.data() + 3 * order_, n * sizeof(grad[0]));}} // namespace ego_planner

啊看完太不容易了！！

EGO Planner代码解析bspline_optimizer部分(3)相关推荐

Baidu Apollo代码解析之EM Planner中的QP Speed Optimizer 1
大家好,我已经把CSDN上的博客迁移到了知乎上,欢迎大家在知乎关注我的专栏慢慢悠悠小马车(https://zhuanlan.zhihu.com/duangduangduang).希望大家可以多多交流, ...
代码解析之自行车模型在Apollo规划中的应用
大家好,我已经把CSDN上的博客迁移到了知乎上,欢迎大家在知乎关注我的专栏慢慢悠悠小马车(https://zhuanlan.zhihu.com/duangduangduang).希望大家可以多多交流, ...
matrix_multiply代码解析
matrix_multiply代码解析关于matrix_multiply 程序执行代码里两个矩阵的乘法,并将相乘结果打印在屏幕上. 示例的主要目的是展现怎么实现一个自定义CPU计算任务. 参考:ht ...
CornerNet代码解析——损失函数
CornerNet代码解析--损失函数文章目录 CornerNet代码解析--损失函数前言总体损失 1.Heatmap的损失 2.Embedding的损失 3.Offset的损失前言今天要解 ...
视觉SLAM开源算法ORB-SLAM3 原理与代码解析
来源:深蓝学院,文稿整理者:何常鑫,审核&修改:刘国庆本文总结于上交感知与导航研究所科研助理--刘国庆关于[视觉SLAM开源算法ORB-SLAM3 原理与代码解析]的公开课. ORB-SLA ...
java获取object属性值_java反射获取一个object属性值代码解析
有些时候你明明知道这个object里面是什么,但是因为种种原因,你不能将它转化成一个对象,只是想单纯地提取出这个object里的一些东西,这个时候就需要用反射了. 假如你这个类是这样的: privat ...
python中的doc_基于Python获取docx/doc文件内容代码解析
这篇文章主要介绍了基于Python获取docx/doc文件内容代码解析,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友可以参考下整体思路: 下载文件并修改后缀 ...
mongoose框架示例代码解析（一）
mongoose框架示例代码解析(一) 参考: Mongoose Networking Library Documentation(Server) Mongoose Networking Librar ...
ViBe算法原理和代码解析
ViBe - a powerful technique for background detection and subtraction in video sequences 算法官网:http:// ...
【Android 逆向】使用 Python 代码解析 ELF 文件 ( PyCharm 中进行断点调试 | ELFFile 实例对象分析 )
文章目录一.PyCharm 中进行断点调试二.ELFFile 实例对象分析一.PyCharm 中进行断点调试在上一篇博客 [Android 逆向]使用 Python 代码解析 ELF 文件 ( ...

EGO Planner代码解析bspline_optimizer部分(3)

EGO Planner代码解析bspline_optimizer部分(3)相关推荐

最新文章

热门文章