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) 返回cost4、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;//控制点的安全距离sf
double 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-dij
Eigen::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;//代价为0
int end_idx = q.cols() - order_;
// def: f = |x*v|^2/a^2 + |x×v|^2/b^2
double 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_CONTINOUS
cost = 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;
//速度为后一个控制点-前一个控制点再除以时间ts
for (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 / ts
for (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 */
}
}
}
#else
cost = 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具有相似的幅值,即乘以一个权重Wv
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 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 1
for (int j = segment_ids[i].first; j <= segment_ids[i].second; ++j)
cps_.flag_temp[j] = false;
// step 2
int 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 computation
double 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 3
if (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());
}
}
else
ROS_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啊 看完太不容易了 ！！
