EGO-Planner文章解读（一）——论文原理和算法实现

在fastplanner中，ESDF 对于评估梯度大小和方向至关重要。然而轨迹优化过程只覆盖了 ESDF 更新范围的非常有限的子空间，计算全图的ESDF是非常冗余的。除此之外，在建图的时候只能看见障碍物的表面，看不见障碍物的后面/内部，在优化的过程中会使用ESDF去产生一个排斥轨迹远离障碍物，在下图所示的例子中，上面的障碍物将轨迹往下推，下面的障碍物将轨迹往上推，会出现轨迹卡在障碍物中的情况。

EGO-Planner中将碰撞项从fastplanner中依据ESDF图进行改进。仅当轨迹撞到新的障碍物时，生成的障碍物信息才会被存储，这使得规划器只提取必要的障碍物信息。然后，如果违反了动力学可行性，我们就会延长时间分配，引入各向异性曲线拟合算法，在保持原始形状的同时调整轨迹的高阶导数。

一、不使用ESDF场产生轨迹推力

在egoplanner这篇论文中，决策变量是控制点Qi。主要思想是，轨迹在优化的过程中，如果轨迹与障碍物发生碰撞，我们就基于这个碰撞对轨迹产生一个反方向的力来把轨迹推出来，远离刚刚碰到的障碍物。

我们对轨迹段进行检查，找到在障碍物内部的控制点Qi，以及与该控制点左右临近的两个不在障碍物内部的控制点。根据这两个不在障碍物内部的控制点，利用A*生成一条远离障碍物的路径。取该控制点的切向量，以该向量为法向量生成一个平面。该平面与障碍物相交，在之前根据左右两个控制点生成的轨迹一侧与障碍物表面的交点为pij点（i代表控制点的索引，j代表障碍物的索引），从Qi到该p点的方向向量为向量v。这样就组成了一个{p,v}对，一个{p,v}对只属于一个控制点Qi。

这里定义第i个控制点Qi到第j个障碍物的距离为dij

$d_{ij}=\left ( Q_{i}-p_{ij} \right )\cdot \mathbf{v_{ij}}$

为了避免轨迹从当前障碍物中逃脱之前生成重复的 {p， v} 对，只有当Qi对于所有的j，dij>0都满足，我们才认为当前障碍物是新发现的。同时，该方法使得只有对最终轨迹有贡献的障碍物才在优化项中进行优化，因此，计算时间显着减少。

二、前端搜索

Fast Planner的前端使用的是Hybrid A * 算法，而Ego Planner并不需要一个与障碍物不发生碰撞的初值轨迹，所以它的前端不需要Hybrid A * 这种比较复杂的算法，它只需要一条不考虑障碍物，满足给定起点和目标点状态的轨迹即可，然后在后端优化中再把轨迹优化成无碰撞的。

前端的路径搜索没有在论文中说明，我们在代码中去解读，对应代码如下：

/*** STEP 1: INIT ***/double ts = (start_pt - local_target_pt).norm() > 0.1 ? pp_.ctrl_pt_dist / pp_.max_vel_ * 1.2 : pp_.ctrl_pt_dist / pp_.max_vel_ * 5; // pp_.ctrl_pt_dist / pp_.max_vel_ is too tense, and will surely exceed the acc/vel limits//ts是时间间隔，这里是判断起点距终点是否接近，距离目标点较远时，初始轨迹的时间间隔稍长；距离目标点较近时，时间间隔较短，生成更紧密的控制点vector<Eigen::Vector3d> point_set, start_end_derivatives;static bool flag_first_call = true, flag_force_polynomial = false;bool flag_regenerate = false;do{point_set.clear();start_end_derivatives.clear();flag_regenerate = false;if (flag_first_call || flag_polyInit || flag_force_polynomial /*|| ( start_pt - local_target_pt ).norm() < 1.0*/) // Initial path generated from a min-snap traj by order.//三个标志位，首次使用，强制使用多项式，{flag_first_call = false;flag_force_polynomial = false;PolynomialTraj gl_traj;double dist = (start_pt - local_target_pt).norm();//轨迹距离double time = pow(pp_.max_vel_, 2) / pp_.max_acc_ > dist ? sqrt(dist / pp_.max_acc_) : (dist - pow(pp_.max_vel_, 2) / pp_.max_acc_) / pp_.max_vel_ + 2 * pp_.max_vel_ / pp_.max_acc_;//轨迹时间，如果起点和目标点距离小于速度-加速度约束的临界值，用加速度时间，如果距离较远，采用加速-匀速-减速时间，time为总时间if (!flag_randomPolyTraj)//不需要随机中间点{gl_traj = PolynomialTraj::one_segment_traj_gen(start_pt, start_vel, start_acc, local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), time);//直接使用起点和终点的mininum snap轨迹}else//需要随机中间点{Eigen::Vector3d horizen_dir = ((start_pt - local_target_pt).cross(Eigen::Vector3d(0, 0, 1))).normalized();Eigen::Vector3d vertical_dir = ((start_pt - local_target_pt).cross(horizen_dir)).normalized();Eigen::Vector3d random_inserted_pt = (start_pt + local_target_pt) / 2 +//随机生成中间点(((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * horizen_dir * 0.8 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989) +(((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * vertical_dir * 0.4 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989);Eigen::MatrixXd pos(3, 3);//轨迹点pos.col(0) = start_pt;pos.col(1) = random_inserted_pt;pos.col(2) = local_target_pt;Eigen::VectorXd t(2);t(0) = t(1) = time / 2;gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, local_target_vel, start_acc, Eigen::Vector3d::Zero(), t);//使用mininum snap生成轨迹}double t;bool flag_too_far;ts *= 1.5; // ts will be divided by 1.5 in the next//将时间变为之前的1.5倍，放宽一点，do{ts /= 1.5;//在每次循环中缩小时间间隔point_set.clear();flag_too_far = false;Eigen::Vector3d last_pt = gl_traj.evaluate(0);for (t = 0; t < time; t += ts){Eigen::Vector3d pt = gl_traj.evaluate(t);//每隔ts采样一个点ptif ((last_pt - pt).norm() > pp_.ctrl_pt_dist * 1.5)//计算pt与上一个点的距离，如果超过了pp_.ctrl_pt_dist * 1.5，说明轨迹不够平滑，终止循环{flag_too_far = true;break;}last_pt = pt;point_set.push_back(pt);//将pt加入点集中}} while (flag_too_far || point_set.size() < 7); // To make sure the initial path has enough points.//采样多项式轨迹点，直到满足控制点密度约束//存在两个轨迹点的间距超过阈值，点的数量太少，会重新进行采样t -= ts;//t是采样终止的时间点，将时间t修正到轨迹的实际终点start_end_derivatives.push_back(gl_traj.evaluateVel(0));//起点速度start_end_derivatives.push_back(local_target_vel);//终点速度start_end_derivatives.push_back(gl_traj.evaluateAcc(0));//起点加速度start_end_derivatives.push_back(gl_traj.evaluateAcc(t));//终点加速度}else // Initial path generated from previous trajectory.//不是第一次生成轨迹，从上一次生成的轨迹提取点集，用作当前规划的初始路径{double t;double t_cur = (ros::Time::now() - local_data_.start_time_).toSec();//计算起始时间，当前时间相对于轨迹开始时间移动了多少秒，此时开始为起点时间vector<double> pseudo_arc_length;//记录轨迹点之间的累积弧长，用于插值采样vector<Eigen::Vector3d> segment_point;//提取的轨迹点集合pseudo_arc_length.push_back(0.0);//起始点弧长为0for (t = t_cur; t < local_data_.duration_ + 1e-3; t += ts)//以固定步长ts来遍历时间{segment_point.push_back(local_data_.position_traj_.evaluateDeBoorT(t));//调用 evaluateDeBoorT(t) 函数，从B样条轨迹中计算时间 t 对应的轨迹点位置if (t > t_cur)//跳过起点，只有当当前时间大于起点时间时才计算弧长{pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm() + pseudo_arc_length.back());//(segment_point.back() - segment_point[segment_point.size() - 2]).norm()当前位置减去前一个点的位置的弧长，加入到pseudo_arc_length累积弧长中}}t -= ts;//由于 t 在最后一次循环中被多加了一个时间步长 ts，需要减去 ts，回退到最后一个有效采样时间点double poly_time = (local_data_.position_traj_.evaluateDeBoorT(t) - local_target_pt).norm() / pp_.max_vel_ * 2;//估计从当前轨迹末点到目标点的时间if (poly_time > ts)//时间大于时间间隔ts，当前点到目标点的轨迹较长，且需要根据该时间来生成新的轨迹，否则就不需要生成新的轨迹{PolynomialTraj gl_traj = PolynomialTraj::one_segment_traj_gen(local_data_.position_traj_.evaluateDeBoorT(t),local_data_.velocity_traj_.evaluateDeBoorT(t),local_data_.acceleration_traj_.evaluateDeBoorT(t),local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), poly_time);//根据当前轨迹的末端状态（位置、速度、加速度）和目标点的状态（位置、速度）生成一个单段的多项式轨迹for (t = ts; t < poly_time; t += ts)//使用时间步长 ts 对生成的多项式轨迹 gl_traj 进行采样{if (!pseudo_arc_length.empty()){segment_point.push_back(gl_traj.evaluate(t));//计算给定时间 t 时的轨迹点（位置）pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm() + pseudo_arc_length.back());//计算每个采样点与前一个采样点之间的距离，并将该距离加到 pseudo_arc_length 中}else//若弧长轨迹为空，则是轨迹生成失败{ROS_ERROR("pseudo_arc_length is empty, return!");continous_failures_count_++;return false;}}}double sample_length = 0;//记录当前的弧长。初始化为0double cps_dist = pp_.ctrl_pt_dist * 1.5; // cps_dist will be divided by 1.5 in the next//表示当前控制点之间的距离，将预定距离放大1.5倍size_t id = 0;//标识 pseudo_arc_length 中的索引，初始化为 0do{cps_dist /= 1.5;//每次循环将 cps_dist 缩小为上一次的 1/1.5point_set.clear();sample_length = 0;id = 0;//上面三步是进行一个重置while ((id <= pseudo_arc_length.size() - 2) && sample_length <= pseudo_arc_length.back())//遍历完所有的 pseudo_arc_length 或者 sample_length 超过了总弧长{if (sample_length >= pseudo_arc_length[id] && sample_length < pseudo_arc_length[id + 1])//当前 sample_length 位于两个点的伪弧长之间{point_set.push_back((sample_length - pseudo_arc_length[id]) / (pseudo_arc_length[id + 1] - pseudo_arc_length[id]) * segment_point[id + 1] +(pseudo_arc_length[id + 1] - sample_length) / (pseudo_arc_length[id + 1] - pseudo_arc_length[id]) * segment_point[id]);//线性插值计算该点的位置sample_length += cps_dist;//每次成功计算出一个新的轨迹点后，sample_length 增加 cps_dist，进行下一个点的采样}elseid++;}point_set.push_back(local_target_pt);} while (point_set.size() < 7); // If the start point is very close to end point, this will help//直到 point_set 的大小达到至少 7 个点start_end_derivatives.push_back(local_data_.velocity_traj_.evaluateDeBoorT(t_cur));start_end_derivatives.push_back(local_target_vel);start_end_derivatives.push_back(local_data_.acceleration_traj_.evaluateDeBoorT(t_cur));start_end_derivatives.push_back(Eigen::Vector3d::Zero());//保存轨迹在起点和终点的导数（速度和加速度），这些信息将用于轨迹的优化和重新规划if (point_set.size() > pp_.planning_horizen_ / pp_.ctrl_pt_dist * 3) // The initial path is unnormally too long!//检查轨迹点，不能太长{flag_force_polynomial = true;flag_regenerate = true;}}} while (flag_regenerate);Eigen::MatrixXd ctrl_pts;UniformBspline::parameterizeToBspline(ts, point_set, start_end_derivatives, ctrl_pts);//将轨迹变为控制点vector<vector<Eigen::Vector3d>> a_star_pathes;a_star_pathes = bspline_optimizer_rebound_->initControlPoints(ctrl_pts, true);//使用 B 样条优化器初始化控制点，并根据这些控制点生成多段路径，存储在 a_star_pathest_init = ros::Time::now() - t_start;//记录下初始化经过多少时间static int vis_id = 0;//可视化visualization_->displayInitPathList(point_set, 0.2, 0);visualization_->displayAStarList(a_star_pathes, vis_id);t_start = ros::Time::now();//更新t_start记录优化时间

首先计算时间间隔ts，这里如果如果 UAV 距离终点较远 (> 0.1m)，ts 取 1.2 倍的最大速度分辨率，如果距离较近 (≤ 0.1m)，ts 缩短 5 倍，以保证 UAV 运动更加精准。

double ts = (start_pt - local_target_pt).norm() > 0.1 ? pp_.ctrl_pt_dist / pp_.max_vel_ * 1.2 : pp_.ctrl_pt_dist / pp_.max_vel_ * 5;

然后进入轨迹点的生成部分，分为第一次生成和从上一次生成的轨迹提取点集两种情况

1.第一次生成

double dist = (start_pt - local_target_pt).norm();//轨迹距离
double time = pow(pp_.max_vel_, 2) / pp_.max_acc_ > dist ? sqrt(dist / pp_.max_acc_) : (dist - pow(pp_.max_vel_, 2) / pp_.max_acc_) / pp_.max_vel_ + 2 * pp_.max_vel_ / pp_.max_acc_;

dist记录起终点的距离，time记录轨迹花费时间（如果 UAV 距离较短，采用加速-减速运动，如果 UAV 距离较长，采用加速-匀速-减速运动）。

这里分为两种情况，第一种情况是不需要使用随机的中间点

1.不需要使用随机的中间点

if (!flag_randomPolyTraj)//不需要随机中间点{gl_traj = PolynomialTraj::one_segment_traj_gen(start_pt, start_vel, start_acc, local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), time);//直接使用起点和终点的mininum snap轨迹}

直接通过初始点位置速度加速度和目标点的位置速度加速度（为0）和轨迹执行的时间生成轨迹（mininum snap方式生成的轨迹）。

2.随机生成中间点生成轨迹

第二种情况是要随机生成中间点生成轨迹。

else//需要随机中间点
{Eigen::Vector3d horizen_dir = ((start_pt - local_target_pt).cross(Eigen::Vector3d(0, 0, 1))).normalized();Eigen::Vector3d vertical_dir = ((start_pt - local_target_pt).cross(horizen_dir)).normalized();Eigen::Vector3d random_inserted_pt = (start_pt + local_target_pt) / 2 +//随机生成中间点(((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * horizen_dir * 0.8 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989) +(((double)rand()) / RAND_MAX - 0.5) * (start_pt - local_target_pt).norm() * vertical_dir * 0.4 * (-0.978 / (continous_failures_count_ + 0.989) + 0.989);Eigen::MatrixXd pos(3, 3);//轨迹点pos.col(0) = start_pt;pos.col(1) = random_inserted_pt;pos.col(2) = local_target_pt;Eigen::VectorXd t(2);t(0) = t(1) = time / 2;gl_traj = PolynomialTraj::minSnapTraj(pos, start_vel, local_target_vel, start_acc, Eigen::Vector3d::Zero(), t);//使用mininum snap生成轨迹
}

首先计算了水平和垂直两个方向的向量horizen_dir和vertical_dir，(start_pt + local_target_pt) / 2，得到起点和终点的中点。

((double)rand()) / RAND_MAX - 0.5生成一个(0,1)的随机数，再减去0.5使其范围为(-0.5,0.5)。再* (start_pt - local_target_pt).norm() * horizen_dir * 0.8，乘以水平方向的距离和水平方向的方向向量，范围最大为0.8。(-0.978 / (continous_failures_count_ + 0.989) + 0.989)这里是用于调整偏移项的大小，失败次数多偏移量会变小，失败次数少偏移量会变大。

三个点分为了两端轨迹，每一段时间为time的一半。

调用minSnapTraj函数生成最终轨迹。

3.采样轨迹点

生成轨迹后，对轨迹点进行采样，

do{ts /= 1.5;//在每次循环中缩小时间间隔point_set.clear();flag_too_far = false;Eigen::Vector3d last_pt = gl_traj.evaluate(0);for (t = 0; t < time; t += ts){Eigen::Vector3d pt = gl_traj.evaluate(t);//每隔ts采样一个点ptif ((last_pt - pt).norm() > pp_.ctrl_pt_dist * 1.5)//计算pt与上一个点的距离，如果超过了pp_.ctrl_pt_dist * 1.5，说明轨迹不够平滑，终止循环{flag_too_far = true;break;}last_pt = pt;point_set.push_back(pt);//将pt加入点集中}} while (flag_too_far || point_set.size() < 7);

每进一次循环，逐步缩小采样的时间间隔ts，在内部的for循环中进行采样，采样的点存放在容器point_set中，当轨迹点足够多 (point_set.size() >= 7)或者轨迹平滑 (flag_too_far == false)时采样结束。

start_end_derivatives.push_back(gl_traj.evaluateVel(0)); // 起点速度
start_end_derivatives.push_back(local_target_vel);       // 终点速度
start_end_derivatives.push_back(gl_traj.evaluateAcc(0)); // 起点加速度
start_end_derivatives.push_back(gl_traj.evaluateAcc(t)); // 终点加速度

这里将起点和终点的速度和加速度存放在start_end_derivatives容器中。

2.基于上一次轨迹

从上一次生成的轨迹提取点集

double t_cur = (ros::Time::now() - local_data_.start_time_).toSec();
vector<double> pseudo_arc_length;
vector<Eigen::Vector3d> segment_point;
pseudo_arc_length.push_back(0.0);

计算当前时刻 t_cur，表示当前时间距离上一条轨迹起点的时长，用于确定从哪里开始提取轨。pseudo_arc_length：存储轨迹点从开始到当前点的累积弧长，用于插值计算。

segment_point：存储轨迹中的采样点。

1.轨迹点采样

首先进行轨迹点采样

for (t = t_cur; t < local_data_.duration_ + 1e-3; t += ts)//以固定步长ts来遍历时间{segment_point.push_back(local_data_.position_traj_.evaluateDeBoorT(t));//调用 evaluateDeBoorT(t) 函数，从B样条轨迹中计算时间 t 对应的轨迹点位置if (t > t_cur)//跳过起点，只有当当前时间大于起点时间时才计算弧长{pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm() + pseudo_arc_length.back());//(segment_point.back() - segment_point[segment_point.size() - 2]).norm()当前位置减去前一个点的位置的弧长，加入到pseudo_arc_length累积弧长中}}

此时t已经是大于等于local_data_.duration了，需要减去一个ts到最后一次有效采样点的时间点。

t -= ts;//由于 t 在最后一次循环中被多加了一个时间步长 ts，需要减去 ts，回退到最后一个有效采样时间点double poly_time = (local_data_.position_traj_.evaluateDeBoorT(t) - local_target_pt).norm() / pp_.max_vel_ * 2;//估计从当前轨迹末点到目标点的时间

此时t在最后一个有效采样点位置上，计算一些从当前t的点到目标点所需要的时间poly_time。

if (poly_time > ts)
{PolynomialTraj gl_traj = PolynomialTraj::one_segment_traj_gen(local_data_.position_traj_.evaluateDeBoorT(t),local_data_.velocity_traj_.evaluateDeBoorT(t),local_data_.acceleration_traj_.evaluateDeBoorT(t),local_target_pt, local_target_vel, Eigen::Vector3d::Zero(), poly_time);

如果到目标点的时间太长了，就需要额外生成一段轨迹，轨迹起点：当前轨迹的最后一个采样点的位置（evaluateDeBoorT(t)），轨迹终点：目标点 local_target_pt，利用之前同样的mininum snap直接生成一段轨迹。

for (t = ts; t < poly_time; t += ts)//使用时间步长 ts 对生成的多项式轨迹 gl_traj 进行采样{if (!pseudo_arc_length.empty()){segment_point.push_back(gl_traj.evaluate(t));//计算给定时间 t 时的轨迹点（位置）pseudo_arc_length.push_back((segment_point.back() - segment_point[segment_point.size() - 2]).norm() + pseudo_arc_length.back());//计算每个采样点与前一个采样点之间的距离，并将该距离加到 pseudo_arc_length 中}else//若弧长轨迹为空，则是轨迹生成失败{ROS_ERROR("pseudo_arc_length is empty, return!");continous_failures_count_++;return false;}}

对最后一个采样点到目标点这段轨迹，重新使用时间步长ts进行补充采样。

2.插值采样

然后进行基于弧长的等距插值采样

double sample_length = 0;
double cps_dist = pp_.ctrl_pt_dist * 1.5;
size_t id = 0;

这里是一些变量值

sample_length = 0        记录当前的弧长。初始化为0
cps_dist = pp_.ctrl_pt_dist * 1.5        cps_dist will be divided by 1.5 in the next//表示当前控制点之间的距离，将预定距离放大1.5倍
id = 0        标识 pseudo_arc_length 中的索引，初始化为 0

接着进入do-while循环采样

do{cps_dist /= 1.5;//每次循环将 cps_dist 缩小为上一次的 1/1.5point_set.clear();sample_length = 0;id = 0;//上面三步是进行一个重置while ((id <= pseudo_arc_length.size() - 2) && sample_length <= pseudo_arc_length.back())//遍历完所有的 pseudo_arc_length 或者 sample_length 超过了总弧长{if (sample_length >= pseudo_arc_length[id] && sample_length < pseudo_arc_length[id + 1])//当前 sample_length 位于两个点的伪弧长之间{point_set.push_back((sample_length - pseudo_arc_length[id]) / (pseudo_arc_length[id + 1] - pseudo_arc_length[id]) * segment_point[id + 1] +(pseudo_arc_length[id + 1] - sample_length) / (pseudo_arc_length[id + 1] - pseudo_arc_length[id]) * segment_point[id]);//线性插值计算该点的位置sample_length += cps_dist;//每次成功计算出一个新的轨迹点后，sample_length 增加 cps_dist，进行下一个点的采样}elseid++;}point_set.push_back(local_target_pt);} while (point_set.size() < 7); // If the start point is very close to end point, this will help//直到 point_set 的大小达到至少 7 个点

这里使用的是一个线性插值的公式：

这里我用gpt给出了一个计算的例子：

start_end_derivatives.push_back(local_data_.velocity_traj_.evaluateDeBoorT(t_cur));
start_end_derivatives.push_back(local_target_vel);
start_end_derivatives.push_back(local_data_.acceleration_traj_.evaluateDeBoorT(t_cur));
start_end_derivatives.push_back(Eigen::Vector3d::Zero());//保存轨迹在起点和终点的导数（速度和加速度），这些信息将用于轨迹的优化和重新规划

这里同样记录下来起始点和终点的速度加速度到start_end_derivatives。

以上获得了前端搜索的轨迹点，首先我们要将其转化为B样条的控制点。

Eigen::MatrixXd ctrl_pts;
UniformBspline::parameterizeToBspline(ts, point_set, start_end_derivatives, ctrl_pts);

使用和fastplanner一致的parameterizeToBspline函数，解线性方程Ap=b，将轨迹点转为控制点，便于之后进行优化和时间的重分配。

三、后端优化

egoplanner的代价函数为：

第一项Js代表平滑项，第二项Jc代表障碍物项（安全性检查），第三项Jd代表动力学可行项。

1.平滑项

这里平滑项是对轨迹的加速度a和jerk进行惩罚，由于B样条的凸包特性，最小化 Bspline 轨迹的二阶和三阶导数的控制点就足以减少沿整个曲线的这些导数。

这里导数的控制点计算公式为：

代码中的实现如下：

  void BsplineOptimizer::calcSmoothnessCost(const Eigen::MatrixXd &q, double &cost,Eigen::MatrixXd &gradient, bool falg_use_jerk /* = true*/)
//q是控制点的坐标数据，cost存放代价，gradient存放梯度，falg_use_jerk标志位是否使用jerk{cost = 0.0;if (falg_use_jerk)//是否使用jerk{Eigen::Vector3d jerk, temp_j;for (int i = 0; i < q.cols() - 3; i++){/* evaluate jerk */jerk = q.col(i + 3) - 3 * q.col(i + 2) + 3 * q.col(i + 1) - q.col(i);//jerk的计算公式，jerk=qi+3-3*qi+2+3qi+1-qicost += jerk.squaredNorm();//cost加上jerk的二范数，平方项temp_j = 2.0 * jerk;//对jerk^2求导，2*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//不使用jerk，只使用加速度项A{Eigen::Vector3d acc, temp_acc;for (int i = 0; i < q.cols() - 2; i++){/* evaluate acc */acc = q.col(i + 2) - 2 * q.col(i + 1) + q.col(i);//加速度项A的计算公式A=qi+2-2*qi+1+qicost += acc.squaredNorm();//cost加上加速度的平方temp_acc = 2.0 * acc;//平方项求导后，2*A/* acc gradient */gradient.col(i + 0) += temp_acc;gradient.col(i + 1) += -2.0 * temp_acc;gradient.col(i + 2) += temp_acc;//对应的控制点的梯度}}}

利用&cost和&gradient来记录各个项对cost和梯度值的贡献，q是传入的控制点。

之后遍历控制点，计算加速度A和Jerk项的值，并计算其对控制点Qi的梯度值。gradient的col(i)代表Qi的梯度值。

2.碰撞项的代价

碰撞惩罚将控制点推离障碍物。这是通过采用安全间隙 sf 并用 dij < sf 惩罚控制点来实现的。为了进一步促进优化，我们构造了一个两次连续可微的惩罚函数 jc，并在 dij 减小时抑制其斜率，从而产生分段函数

这里的dij是我们在之前解决无ESDF产生推力里面计算的第i个控制点Qi到第j个障碍物的距离为dij

$d_{ij}=\left ( Q_{i}-p_{ij} \right )\cdot \mathbf{v_{ij}}$

sf为安全距离的阈值，根据dij和安全阈值sf的差值不同，对其施加不同方式的惩罚。

其中 jc（i， j）是 Qi 上 {p， v}j 对产生的成本值。每个 Qi 的成本是独立评估的，并从所有相应的 {p， v}j 对中累积。因此，如果控制点发现更多障碍物，则会获得更高的轨迹变形权重。每个Qi点的代价为：

Np 是属于 Qi 的 {p， v}j 对的数量。

因此，这里的总代价是控制点惩罚的累加值：

由于jc是dij的函数，而dij是Qi的函数，通过链式求导法则可以直接求导该惩罚项对控制点的导数：

内层循环是单个控制点的所有{p,v}，外层循环是所有的控制点。

代码的实现如下：

  void BsplineOptimizer::calcDistanceCostRebound(const Eigen::MatrixXd &q, double &cost,Eigen::MatrixXd &gradient, int iter_num, double smoothness_cost)//输入控制点矩阵q，代价cost，梯度gradient，迭代次数iter_num，轨迹的平滑度代价smoothness_cost{cost = 0.0;int end_idx = q.cols() - order_;//end点的idx，不优化末尾的pb个点double demarcation = cps_.clearance;//距离阈值，sfdouble a = 3 * demarcation, b = -3 * pow(demarcation, 2), c = pow(demarcation, 3);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.//当迭代次数超过 3 且平滑度代价足够小（即轨迹已经足够平滑）时，检查是否需要回弹。{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)//遍历每个控制点的pv对{double dist = (cps_.points.col(i) - cps_.base_point[i][j]).dot(cps_.direction[i][j]);//dij=(Qij-pij)*vijdouble dist_err = cps_.clearance - dist;//cij=sf-dijEigen::Vector3d dist_grad = cps_.direction[i][j];//dij对Qij的梯度if (dist_err < 0)//cij小于0不做惩罚{/* do nothing */}else if (dist_err < demarcation)//0<=cij<=sf，三次项的惩罚，cij^3{cost += pow(dist_err, 3);gradient.col(i) += -3.0 * dist_err * dist_err * dist_grad;//梯度为-3*cij^2*dist_grad}else//cij大于sf，二次函数惩罚{cost += a * dist_err * dist_err + b * dist_err + c;//cost项gradient.col(i) += -(2.0 * a * dist_err + b) * dist_grad;//梯度}}}}

3.动力学可行项

在每个维度上限制轨迹的高阶导数可以确保动力学可行性。同样由于凸包特性，约束控制点的导数足以约束整个 B 样条。

F函数是三个维度的f()函数的和，f()函数的定义如下：

这里传入的cr就是导数控制点某个维度的值，根据其超出限制的多少进行不同项的惩罚。a1， b1， c1， a2， b2， c2 通过满足函数的二阶连续性求得，cm是设定的最大值，cj是二次区间和三次区间的分裂点。λ<1-ϵ，是一个弹性系数，其中，ϵ 的值远小于1。同样这里很容易实现对控制点Qi的导数。

代码的实现如下：

  void BsplineOptimizer::calcFeasibilityCost(const Eigen::MatrixXd &q, double &cost,Eigen::MatrixXd &gradient){//#define SECOND_DERIVATIVE_CONTINOUS#ifdef SECOND_DERIVATIVE_CONTINOUS//两种方式进行速度加速度超限的惩罚，函数选择的不一样cost = 0.0;double demarcation = 1.0; // 1m/s, 1m/s/sdouble 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_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;for (int j = 0; j < 3; j++){if (vi(j) > max_vel_ + demarcation)//使用复杂函数进行惩罚{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 magnitudedouble 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;for (int j = 0; j < 3; j++){if (ai(j) > max_acc_ + demarcation){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++){Eigen::Vector3d vi = (q.col(i + 1) - q.col(i)) / ts;//计算速度控制点//cout << "temp_v * vi=" ;for (int j = 0; j < 3; j++)//遍历该点的三个维度{if (vi(j) > max_vel_)//超限时进行惩罚{// cout << "fuck VEL" << endl;// cout << vi(j) << endl;cost += pow(vi(j) - max_vel_, 2) * ts_inv2; // multiply ts_inv3 to make vel and acc has similar magnitudegradient(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++){Eigen::Vector3d ai = (q.col(i + 2) - 2 * q.col(i + 1) + q.col(i)) * ts_inv2;//计算加速度点Ai//cout << "temp_a * ai=" ;for (int j = 0; j < 3; j++)//三个维度{if (ai(j) > max_acc_){// cout << "fuck ACC" << endl;// cout << ai(j) << endl;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 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}

这里的代码和fastplanner中后端的一样，只有惩罚函数不同，这里提供了两种惩罚函数方式，简单和复杂。

然后使用和fastplanner一样的（可以看我之前解读fastplanner的博客）combineCostRefine函数，将三项惩罚项的代价和导数汇总，便于进行迭代优化。

四、时间重分配

在优化之前分配准确的时间曲线是不合理的，因为规划者不知道那时最终轨迹的信息。因此，额外的时间重新分配程序对于确保动态可行性至关重要。在fastplanner中，将轨迹参数化为非均匀 B 样条，并在某些段超过导数限制时迭代延长时间跨度。

但是在之前fastplanner博客中的关于B样条曲线的讲述中，我们可以知道时间跨度Δt和控制点是相互影响的，因此在fastplanner中对于时间跨度的修改会使得B样条的控制点发生变化使得轨迹发生改变，与我们之前搜索和优化的最优路径形状差距较大。

在egoplanner中，根据之前优化得到的安全轨迹 Φs 以合理的时间重新分配重新生成均匀的 B 样条轨迹 Φf。然后，提出了一种各向异性曲线拟合方法，使 Φf 自由优化其控制点以满足高阶导数约束，同时保持与 Φs 几乎相同的形状。（ Φs表示时间优化前的轨迹， Φf表示时间优化后的轨迹）

1.均匀B样条时间间隔改变

首先我们要计算一个超出限制的比例因子re，re计算如下：i是对控制点索引的遍历，r是对三个维度xyz的遍历，vm，am和jm分别是这几个量的最大限制。

re 表示我们应该将 Φf 相对于 Φs 的时间分配延长多少。

这里我们来看控制点导数Vi，Ai，Ji的计算公式：

Vi、Aj 和 Jk 分别与Δt，Δt 的平方和Δt 的立方成反比，通过re，我们将现有时间扩大为新的时间节点向量，其中时间间隔Δt变为Δt'：

现在是时间向量发生了变化：

因此新的控制点Qi'也与之前的Qi不一样了，我们需要重新求解控制点Qi’，并将其作为初始值在后续进行优化。

我们主要要求新的轨迹Φf与之前的轨迹Φs尽量一致，首先我们表示Φf和Φs：

为了保证两个轨迹尽量一致，我们希望新轨迹Φf在时间点 $r_{e}t$ 与原来的轨迹Φs在对应的时间点t位置一致，同时初始和终末状态也是一样的，这里假设初始时间点t1为0，终点时间点为tm。

根据每个时间点的位置可以促成m+1个约束，再把起点和终点速度和加速度一致4个约束加上，可以构成矩阵A，向量b由各个时间点的位置信息和起终点的速度加速度信息组成。通过求解线性方程AQ'=b，可以求出新的控制点Q'。这里可以闭式求解：

代码中实现如下：

  void EGOPlannerManager::reparamBspline(UniformBspline &bspline, vector<Eigen::Vector3d> &start_end_derivative, double ratio,Eigen::MatrixXd &ctrl_pts, double &dt, double &time_inc)/**/{double time_origin = bspline.getTimeSum();//获得时间的总长度int seg_num = bspline.getControlPoint().cols() - 3;//控制点的数量// double length = bspline.getLength(0.1);// int seg_num = ceil(length / pp_.ctrl_pt_dist);bspline.lengthenTime(ratio);//根据输入的 ratio 调整轨迹的时间分布double duration = bspline.getTimeSum();//获取调整后的总时间（getTimeSum() 获取整个轨迹的总时长dt = duration / double(seg_num);//计算每个轨迹段的时间步长 dt，即每个控制点之间的时间间隔time_inc = duration - time_origin;//计算总的时间增量vector<Eigen::Vector3d> point_set;for (double time = 0.0; time <= duration + 1e-4; time += dt){point_set.push_back(bspline.evaluateDeBoorT(time));//从时间 0 开始，到新的总时间 duration 为止，按时间步长 dt 采样轨迹点}UniformBspline::parameterizeToBspline(dt, point_set, start_end_derivative, ctrl_pts);//将轨迹点变为控制点}

其参数变量：

bspline：输入的原始轨迹对象，类型为 UniformBspline。
start_end_derivative：包含起点和终点的速度与加速度导数，用于计算时间分配。
ratio：时间分配的调整比例，影响轨迹上各段的时间分布。
ctrl_pts：输出的控制点矩阵，存储通过重新参数化后的轨迹控制点。
dt：每个轨迹段的时间步长，即每个控制点之间的时间间隔。
time_inc：总的时间增量，即轨迹调整前后的时间差。
time_origin：原始轨迹的总时间长度
seg_num：获取轨迹的控制点数量，减去3是因为3是阶数pb，减去开头的pb个点，这是由初始状态决定的

bspline.lengthenTime(ratio)使用ratio对轨迹进行时间调整，

double duration = bspline.getTimeSum();
dt = duration / double(seg_num);
time_inc = duration - time_origin;

计算调整后的总时间 duration。
计算时间步长 dt，即每个轨迹段的时间间隔，通过将调整后的总时间 duration 除以控制点数量 seg_num 来得到
计算时间增量 time_inc，即调整后总时间与原始时间的差值

vector<Eigen::Vector3d> point_set;
for (double time = 0.0; time <= duration + 1e-4; time += dt)
{point_set.push_back(bspline.evaluateDeBoorT(time));
}

在调整后的轨迹上按时间步长dt 采样轨迹点，从时间 0 开始，到新的总时间duration为止。每次采样一个轨迹点，并将其存储在point_set中。

和fastplanner一样，将轨迹点转为控制点

UniformBspline::parameterizeToBspline(dt, point_set, start_end_derivative, ctrl_pts);

2.时间分配优化

求解出新的控制点Q'后，将其作为初值进行优化，优化的代价函数为：（决策变量仍然为控制点）

这里有三项代价项，前两项Js和Jd是和之前优化问题一样的平滑项和动力学可行项，第三项Jf是曲线拟合项，即要保证优化后的曲线尽量和之前的曲线保持形状一致。

这里我们主要来看拟合项Jf，这里采用的是一种椭球形的惩罚项，分为轴向和径向。

其中T 和T ′是Φs和Φf的轨迹持续时间，α ∈ [0，1]。我们希望优化时，新的轨迹在新的扩展的时间上面与旧的轨迹对应的时间上的位置尽可能的相同，旧轨迹即原轨迹Φs，它是由后端优化得到的，已经考虑了避障，是无碰撞的轨迹，所以只有新的轨迹与该轨迹的位置尽可能的相同，也就可以实现无碰撞，所以这里优化的目标函数里不需要包含避障项。因此，在与速度垂直的方向上，新的轨迹与原轨迹尽可能的贴合。而沿着速度的方向，可以不贴合的那么好，放松一点，来使得轨迹更加平滑，所以轴向和径向惩罚项的权重不一致。

这里da和dr分别代表轴向和径向的方向向量，代价函数为该方向向量的二范数，并且带有椭圆形的权重系数。

优化在BsplineOptimizeTrajRefine函数中进行，该函数调用了refine_optimize()函数，其核心内容是代价函数costFunctionRefine。

costFunctionRefine和之前的cost函数主要不同是拟合项Jf，实现函数如下：

void BsplineOptimizer::calcFitnessCost(const Eigen::MatrixXd &q, double &cost, Eigen::MatrixXd &gradient){cost = 0.0;int 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)//历了所有控制点，从 order_ - 1 到 end_idx，对于每个控制点，计算该点与参考路径之间的误差，此处使用了一个加权平均来计算当前位置误差{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();//v是参考路径的切向量，即参考路径上相邻两点之间的方向double xdotv = x.dot(v);//公式daEigen::Vector3d xcrossv = x.cross(v);//公式drdouble f = pow((xdotv), 2) / a2 + pow(xcrossv.norm(), 2) / b2;//da/a^2+dr/b^2cost += 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;//对这几个控制点的偏导数值}}

主要是遍历了需要优化的控制点，计算方向向量da和dr，并计算其对应的代价f，累加f实现积分，同时求解f对对应控制点的导数值，累加在gradient容器中。