路径交叉点跟踪问题

src/autonomy/pl/src/pl.cpp

@@ -33,6 +33,8 @@ WRC_GPS_Point GPSPointQueue[WRC_MAX_POINTS_IN_PATH];
 WRC_MC_NAV_INFO g_NavInfo;
 struct_rtk g_rtk;
 
+static int g_last_road_pos = -1;  // 跟踪上一次的最近点索引，用于单调性约束
+
 int mode = 0;
 int last_mode = 0;
 
@@ -72,6 +74,7 @@ int ntzx_GPS_posit(double Cur_navAngle, double Cur_lonti, double Cur_lati, doubl
 }
 
 // Find_Nearest_Point
+// 线性前向搜索 + 严格单调性约束：拒绝向后跳动，防止交叉点索引混乱
 int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_info, WRC_GPS_Point* Road_info)
 {
     if (start_index < 0)
@@ -79,14 +82,26 @@ int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_in
         return -1;
     }
 
+    // 钳位 start_index 到有效范围
+    if (start_index >= GpsPointNum)
+    {
+        start_index = GpsPointNum - 1;
+    }
+
     int Nearest_point_index = start_index;  // 初始化为起始索引
     double min_distance = ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
                                           Road_info[start_index].LongitudeInfo, Road_info[start_index].LatitudeInfo);
 
-    // 从 start_index + 1 开始搜索
+    // 线性前向搜索（不环绕），到达路径末尾停止
     for (int i = 1; i < GpsPointNum; i++)
     {
-        int current_index = (start_index + i) % GpsPointNum;  // 环绕访问
+        int current_index = start_index + i;
+
+        // 到达路径末尾，停止搜索
+        if (current_index >= GpsPointNum)
+        {
+            break;
+        }
 
         double current_distance =
             ntzx_GPS_length(Cur_GPS_info.Longitude_degree, Cur_GPS_info.Latitude_degree,
@@ -94,17 +109,46 @@ int Road_Planning_Find_Nearest_Point(int start_index, WRC_MC_NAV_INFO Cur_GPS_in
 
         if (current_distance < min_distance)
         {
+            // === 航向角过滤：排除交叉路径上航向不匹配的段 ===
+            double heading_diff = std::fmod(std::fabs(Cur_GPS_info.Yaw_rad - Road_info[current_index].Heading), 360.0);
+            if (heading_diff > 180.0) heading_diff = 360.0 - heading_diff;
+
+            if (heading_diff > 45.0)
+            {
+                continue;  // 航向不匹配，跳过此交叉段候选点
+            }
+
             min_distance = current_distance;
             Nearest_point_index = current_index;  // 更新最近点的索引
 
             // 如果距离小于 0.8 m，提前退出
             if (min_distance < 0.8)
             {
-                return Nearest_point_index;
+                break;
             }
         }
     }
 
+    // === 严格单调性约束 ===
+    // 车辆只往前走，不允许回头，因此索引不允许向后跳动。
+    // 当路径存在空间交叉点时，几何最近点可能跳回另一段路径，
+    // 此处强制拒绝任何向后跳动。
+    if (g_last_road_pos >= 0 && Nearest_point_index < g_last_road_pos)
+    {
+        // 候选点在上次位置之前，拒绝它，保持前进
+        if (g_last_road_pos + 1 < GpsPointNum)
+        {
+            Nearest_point_index = g_last_road_pos + 1;
+        }
+        else
+        {
+            Nearest_point_index = g_last_road_pos;
+        }
+
+        LOG_WARN("单调约束: 拒绝向后跳 (候选=%d, 上次=%d), 保持前进", Nearest_point_index, g_last_road_pos);
+    }
+
+    g_last_road_pos = Nearest_point_index;
     return Nearest_point_index;  // 返回最近点的索引
 }
 /*
@@ -304,6 +348,7 @@ void PL_ProcThread()
                 road_pos = 0;
                 des_pos = 3;
                 direction_pos = 20;
+                g_last_road_pos = -1;  // 重置单调性约束，允许在新路径上自由搜索
                 task_status = TaskStatus::RUNNING;
                 need_reload_path = false;
                 LOG_INFO("#################### 开始执行新路径! ####################");