feat(robotaxi): 实现路线擦除与车辆位置投影算法

hejianhao

2026-03-27 04aed36e4ffbddd2b05521b12ec71ce094246c69

feat(robotaxi): 实现路线擦除与车辆位置投影算法

- 新增 `getProjectedPoint` 工具函数，用于计算车辆在线段上的投影点，提升位置吸附精度
- 优化 `erasePassedRouteSegments` 逻辑：当路线发生较大变化时自动回退至全量重绘，并返回投影点作为车辆位置
- 调整状态4/6的路线更新逻辑，使用投影点而非原始首点更新车辆位置，使轨迹更贴合路线

 src/robotaxi/pages/begin-route/index.tsx

@@ -31,6 +31,7 @@
  convertAdcodeToCityCode,
  getCityInfoWithId,
  getDistance,
  getProjectedPoint,
  mapOrderStatusToTripStatus,
  reportEvent
} from '../../../utils/Robotaxi-public-method';
@@ -508,20 +509,18 @@
  // 使用 eraseLines 擦除已行驶路段（状态4/6专用）
  const erasePassedRouteSegments = (newPoints: any[], served?: number) => {
    const oldPoints = currentFullRouteRef.current;
    if (!oldPoints?.length || !newPoints?.length) return;
    if (!oldPoints?.length || !newPoints?.length) return newPoints?.[0];

    const newFirst = newPoints[0];
    let bestMatchIndex = -1;
    let bestDistance = Infinity;
    const startIndex = lastErasedIndexRef.current;

    // 如果后端提供了明确的已行驶点数 served (作为增量：这次少了几个点)
    if (typeof served === 'number' && !Number.isNaN(served) && served > 0) {
      // 既然 served 是这次少了多少个点，那么新的擦除索引 = 上次的索引 + served
    if (typeof served === 'number' && !Number.isNaN(served) && served >= 0) {
      bestMatchIndex = startIndex + served;
      // 确保不超过数组范围
      bestMatchIndex = Math.min(bestMatchIndex, oldPoints.length - 1);
    } else {
      let bestDistance = Infinity;
      // 降级使用距离匹配逻辑，移除不合理的退出条件，使匹配更加敏感
      for (let i = startIndex; i < oldPoints.length; i++) {
        const dist = getDistance(
@@ -535,6 +534,16 @@
        // 如果找到了非常近的点（例如10米内），就可以认为这就是车辆当前位置所在的索引，提前结束查找
        if (bestDistance < 10) break;
      }
    }

    if (bestMatchIndex < startIndex) {
      // 路线发生较大变化，回退为全量重绘
      currentFullRouteRef.current = [...newPoints];
      lastErasedIndexRef.current = 0;
      lastRoutePointRef.current = null;
      lastRouteUpdateAtRef.current = 0;
      setRoutePoints([...newPoints]);
      return newPoints[0];
    }

    // 只要找到了比之前更大的索引，就进行擦除（无论距离多远，只要是往后走就应该擦除）
@@ -563,14 +572,13 @@
      });

      lastErasedIndexRef.current = bestMatchIndex;
    } else {
      // 路线发生较大变化，回退为全量重绘
      currentFullRouteRef.current = [...newPoints];
      lastErasedIndexRef.current = 0;
      lastRoutePointRef.current = null;
      lastRouteUpdateAtRef.current = 0;
      setRoutePoints([...newPoints]);
    }

    const nextIndex = Math.min(bestMatchIndex + 1, oldPoints.length - 1);
    const pointA = oldPoints[bestMatchIndex];
    const pointB = oldPoints[nextIndex];

    return getProjectedPoint(newFirst, pointA, pointB);
  };

  // 更新车辆位置（移除节流，实时更新）
@@ -951,7 +959,7 @@
      // 状态4/6：使用 eraseLines 擦除已行驶路段，不再全量重绘
      const isEraseStatus = [4, 6].includes(tripStatusRef.current);
      if (isEraseStatus) {
        const firstPoint = points?.[0];
        let vehiclePoint = points?.[0];
        if (orderRouteInitializedRef.current && currentFullRouteRef.current.length > 0) {
          // 后续更新：擦除已行驶路段
          let servedCount = 0;
@@ -971,7 +979,7 @@
          } catch (e) {
            console.error('解析 served 数据失败', e);
          }
          erasePassedRouteSegments(points, servedCount);
          vehiclePoint = erasePassedRouteSegments(points, servedCount) || points?.[0];
        } else {
          // 首次绘制：全量渲染并存储完整路线
          orderRouteInitializedRef.current = true;
@@ -981,8 +989,8 @@
            applyRoutePoints(points);
          });
        }
        if (firstPoint && typeof firstPoint.latitude === 'number' && typeof firstPoint.longitude === 'number') {
          updateVehicleLocationThrottled(firstPoint.latitude, firstPoint.longitude, true);
        if (vehiclePoint && typeof vehiclePoint.latitude === 'number' && typeof vehiclePoint.longitude === 'number') {
          updateVehicleLocationThrottled(vehiclePoint.latitude, vehiclePoint.longitude, true);
        }
        return;
      }

 src/utils/Robotaxi-public-method.ts

@@ -19,6 +19,44 @@
  return s * 1000;
};

/**
 * 计算点 P 在线段 AB 上的投影点（吸附算法）
 * @param p 真实点 {latitude, longitude}
 * @param a 线段起点 {latitude, longitude}
 * @param b 线段终点 {latitude, longitude}
 * @returns 投影点 {latitude, longitude}
 */
export const getProjectedPoint = (p: any, a: any, b: any) => {
  if (!p || !a || !b) return p;
  if (a.latitude === b.latitude && a.longitude === b.longitude) return a;

  const x0 = p.longitude, y0 = p.latitude;
  const x1 = a.longitude, y1 = a.latitude;
  const x2 = b.longitude, y2 = b.latitude;

  // 向量 AB
  const dx = x2 - x1;
  const dy = y2 - y1;
  // 向量 AP
  const ex = x0 - x1;
  const ey = y0 - y1;

  // 计算投影比例 t
  const dotProduct = ex * dx + ey * dy;
  const lengthSquared = dx * dx + dy * dy;
  const t = dotProduct / lengthSquared;

  // 如果投影点落在线段外，则取最近的端点
  if (t <= 0) return { latitude: a.latitude, longitude: a.longitude };
  if (t >= 1) return { latitude: b.latitude, longitude: b.longitude };

  // 返回投影点
  return {
    latitude: y1 + t * dy,
    longitude: x1 + t * dx
  };
};

// 判断是否需要更新路线（距离大于100米或时间超过10秒）
export const shouldUpdateRoute = (lastLat: number, lastLng: number, newLat: number, newLng: number, lastUpdateTime: number): boolean => {
  const distance = getDistance(lastLat, lastLng, newLat, newLng);