ego(4)---检测B样条轨迹的障碍物进入点与退出点
障碍物进出点检测的作用
在经过 B 样条的控制点采样后，接下来是绕障的环节，绕障使用的是 Astar ，但在使用 Astar 之前，需要进行障碍物进出点的检测与标记。通俗点讲，这部分的作用就是为 Astar 绕障碍做前置准备。
检测进出点的步骤
采样步长计算
ego 中使用采样步长，或者叫分辨率来检测是否经过障碍物：
double step_size = grid_map_->getResolution() / ((init_points.col(0) - init_points.rightCols(1)).norm() / (init_points.cols() - 1)) / 2;grid_map_->getResolution() 表示栅格地图的分辨率，step_size 用来确认采样步长，保证采样点的密度不低于栅格精度，就可以避免越过障碍物。
步长计算：栅格分辨率 / (轨迹总长度 / 控制点数量) / 2。
检测是否碰撞
在 ego 中采用了一个小状态机来标记是否发生碰撞，使用内外两层检测，外层只检测初始轨迹的关键区间(中间的 2/3 段，认为首尾段无障碍)。
内层根据分辨率来逐个插值采样，然后判断是否是进入点或退出点，当进入点与退出点都找到时，才会记录。
在检测判断时，都会检测连续状态的次数，在 ego 中是使用的这个变量：
constexpr int ENOUGH_INTERVAL = 2; // 碰撞次数，超过此阈值才会认为碰撞到障碍物检测逻辑示意
检测逻辑示意图：
进入退出点示意图：
对应的 ego 代码(添加了部分注释)：
/*** Segment the initial trajectory according to obstacles ***/
/** 定位初始轨迹的障碍段 **/
constexpr int ENOUGH_INTERVAL = 2; // 碰撞次数，超过此阈值才会认为碰撞到障碍物
// 计算轨迹采样步长，确保采样密度足够，不会越过障碍物 步长计算：栅格分辨率 / (轨迹总长度/控制点数量)
double step_size = grid_map_->getResolution() / ((init_points.col(0) - init_points.rightCols(1)).norm() / (init_points.cols() - 1)) / 2;
int in_id, out_id;
vector<std::pair<int, int>> segment_ids;
int same_occ_state_times = ENOUGH_INTERVAL + 1;
bool occ, last_occ = false;
bool flag_got_start = false, flag_got_end = false, flag_got_end_maybe = false;// 遍历初始轨迹的关键区间(仅检查中间的 2/3 段控制点，认为首尾段无障碍)
int i_end = (int)init_points.cols() - order_ - ((int)init_points.cols() - 2 * order_) / 3; // only check closed 2/3 points.
for (int i = order_; i <= i_end; ++i)
{// 采样当前控制点与下一个控制点之间的轨迹for (double a = 1.0; a >= 0.0; a -= step_size){// 计算采样点坐标，然后判断采样点是否在障碍物区域内 采样点的线性插值类似低通滤波的公式，根据分辨率插值采样，求解出采样点坐标occ = grid_map_->getInflateOccupancy(a * init_points.col(i - 1) + (1 - a) * init_points.col(i));// cout << setprecision(5);// cout << (a * init_points.col(i-1) + (1-a) * init_points.col(i)).transpose() << " occ1=" << occ << endl;// 上次不是碰撞点，当前是，则认为碰撞状态切换，检测是否是进入点if (occ && !last_occ){if (same_occ_state_times > ENOUGH_INTERVAL || i == order_){in_id = i - 1; // 进入点坐标对应前面的坐标点flag_got_start = true; // 标记已经获取到进入点}same_occ_state_times = 0;flag_got_end_maybe = false; // terminate in advance}// 上次是碰撞点，当前不是，检测是否是退出点else if (!occ && last_occ){out_id = i;flag_got_end_maybe = true; // 标记可能是退出点same_occ_state_times = 0;}else{++same_occ_state_times; // 连续相同状态检测}// 确认退出点：连续 ENOUGH_INTERVAL 次未占用，避免临时跳出if (flag_got_end_maybe && (same_occ_state_times > ENOUGH_INTERVAL || (i == (int)init_points.cols() - order_))){flag_got_end_maybe = false;flag_got_end = true; // 标记获取到退出点}last_occ = occ;// 记录障碍段的进入点与退出点if (flag_got_start && flag_got_end){flag_got_start = false;flag_got_end = false;segment_ids.push_back(std::pair<int, int>(in_id, out_id));}}
}