也是用了挺久的方法了，屡试不爽
障碍物聚类方法
基本原理
确保路径生成的可行性与鲁棒性，需对处于参考线Frenet坐标系中的静态障碍物进行聚类操作。该策略的核心目标是：
存在车辆无法穿越的空间闭合区域时，对障碍物进行连通性归类处理，约束路径生成过程的可通行域识别行为。
构建在空间障碍冗余扩展模型之上，通过引入带宽参数在Frenet两个维度对原始障碍物边界进行对称拓展。
扩展后的边界作为基础单元参与后续的空间重叠性分析，从而实现弱连接闭合空间的检测与联通体识别。
拓展策略
- S方向：设定拓展边界大于车辆惯性控制距离，以容纳潜在路径偏移带来的冗余路径区域；
- L方向：拓展幅度设为车辆横向物理投影的一半，以排除“可穿越错觉”导致的伪空隙区域。
数据结构
障碍物在SL空间中建模为box包围盒
struct ExpandedBox {
size_t idx;
double s_min, s_max;
double l_min, l_max;
};空间交互关系
bool Overlap1D(double a0, double a1, double b0, double b1) {
return !(a1 < b0 || b1 < a0);
}
bool Overlap2D(const ExpandedBox& a, const ExpandedBox& b) {
return Overlap1D(a.s_min, a.s_max, b.s_min, b.s_max) &&
Overlap1D(a.l_min, a.l_max, b.l_min, b.l_max);
}用于构建障碍物之间的弱连接关系
聚类与连通体识别
障碍物的聚类过程基于空间中构造的隐式无向图结构，其中障碍物为图节点，两个节点间存在边当且仅当其拓展边界产生 2D 空间交集。
流程
- 将所有BOX按
s_min升序排列； - 对每个未访问节点，基于该节点进行BFS，构造其可达域；
- 搜索过程中，采用窗口式滑动机制，仅考察当前节点后续（及少量前向，以避免遗漏）BOX中与当前组空间重叠的节点；
- 所得的可达集构成一类障碍物簇，视作路径规划过程中应一致性避让的刚性障碍体。
路径规划
上述障碍簇形成后，每一类簇被视作整体不可穿越区域，可用于：
- 路径可行域裁剪；
- 动态避让方向一致性约束；
- 提前决策约束传播区域边界。
此机制实质上是一种基于障碍物空间“弱连通性闭包识别”的路径规划前置处理。
效果演示
聚类前
聚类后
障碍簇避让方向判定
流程
对于每一个障碍簇，根据车辆当前S坐标与该组最小S范围分为两类处理方式：
- 前视预判：障碍簇尚未进入车辆行驶影响范围，采取基于路径边界与障碍物位置关系的保守预判；
- 局部精判：车辆已临近或进入该组影响区域，采取基于几何相对位置的细致判定策略。
前视预判
当车辆尚未接近该障碍簇时
- 选取代表障碍物：从组内选择S位置最靠前的障碍物作为分析对象，视作该组的代表。
- 查询路径边界：在该障碍物所处的S位置，从路径边界点序列中，找到第一个的点，返回其左右边界的L坐标。
- 判断中心位置：以该障碍物的L范围中点作为其几何中心，分别计算其到左右边界的距离，判定以车辆最小可通行宽度为准。
- 决策避让方向：
- 若该障碍物更靠近左边界，则判定为右侧避让（即将路径偏向右侧）；
- 否则判定为左侧避让。
- 若路径边界无法查询，统一设为右侧避让。
- 结果应用：将上述决策结果，统一赋予该组所有障碍物，作为后续路径生成阶段的避让方向约束。
局部精判
当车辆已进入或即将进入障碍簇的影响范围时
- 确定关键障碍物：从该组中寻找与车辆当前S位置最接近的障碍物。若多个障碍物距离相等，则选择其中心位置更接近车辆的作为代表。
- 评估参考位置：将车辆当前S值投影至该障碍物的S范围，限制在
[MinS, MaxS]区间内，作为评估位置（用于判断L交互关系）。 - 获取障碍物L边界：读取该障碍物在上述评估位置处的L左右边界，并计算其中心位置。
- 获取车辆L位置：根据车辆在参考线上的SL边界，计算其当前L中心位置。
- 决策避让方向：
- 若车辆L中心在障碍物中心左侧，判定为左避让；
- 否则为右避让。
- 结果应用：将判定结果应用于整个障碍簇。
Comments NOTHING