当前位置：网站首页>EGO Planner代码解析bspline_optimizer部分(3)
EGO Planner代码解析bspline_optimizer部分(3)
2022-07-03 18:55:00 【X uuuer.】
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啊 看完太不容易了 ！！
边栏推荐
- High concurrency Architecture - read write separation
- Does SQL always report foreign key errors when creating tables?
- 平淡的生活里除了有扎破皮肤的刺，还有那些原本让你魂牵梦绕的诗与远方
- 组策略中开机脚本与登录脚本所使用的用户身份
- math_泰勒公式
- Mysql45 lecture learning notes (II)
- VLAN experiment
- What does foo mean in programming?
- leetcode：11. 盛最多水的容器【雙指針 + 貪心 + 去除最短板】
- SSM integration - joint debugging of front and rear protocols (list function, add function, add function status processing, modify function, delete function)
猜你喜欢
What is SQL get connection
SSM integration - joint debugging of front and rear protocols (list function, add function, add function status processing, modify function, delete function)
leetcode：11. 盛最多水的容器【雙指針 + 貪心 + 去除最短板】
Integrated easy to pay secondary domain name distribution system
Record: pymysql is used in pycharm to connect to the database
我们做了一个智能零售结算平台
Xception for deeplab v3+ (including super detailed code comments and original drawing of the paper)
Getting started with JDBC
Caddy server agent
2022.02.11
随机推荐
Help change the socket position of PCB part
Read the paper glodyne global topology preserving dynamic network embedding
EGO Planner代码解析bspline_optimizer部分(2)
SQL: special update operation
math_泰勒公式
The more you talk, the more your stupidity will be exposed.
达梦数据库的物理备份和还原简解
Zero length array
[combinatorics] dislocation problem (recursive formula | general term formula | derivation process)*
High concurrency Architecture - separate databases and tables
php-fpm的max_chindren的一些误区
__ Weak and__ The difference between blocks
leetcode：11. 盛最多水的容器【雙指針 + 貪心 + 去除最短板】
How does GCN use large convolution instead of small convolution? (the explanation of the paper includes super detailed notes + Chinese English comparison + pictures)
2022.02.11
Introduction to SSH Remote execution command
Add control at the top of compose lazycolumn
Record: install MySQL on ubuntu18.04
SSM整合-前后台协议联调（列表功能、添加功能、添加功能状态处理、修改功能、删除功能）
Change is the eternal theme