1. 基于规则的车道保持规划

任务说明

在结构化道路（如高速公路）上，车辆需要沿当前车道中心线稳定行驶。本案例实现一个基于规则的车道保持规划器：输入车辆当前位姿与车道中心线参考点，输出一段平滑、可跟踪的局部轨迹点序列（包含 x、y、曲率），供下游控制模块使用。

核心实现

import numpy as np
from dataclasses import dataclass

@dataclass
class VehicleState:
    x: float          # 全局X坐标 (m)
    y: float          # 全局Y坐标 (m)
    yaw: float        # 航向角 (rad)
    v: float          # 纵向速度 (m/s)

@dataclass
class TrajectoryPoint:
    x: float
    y: float
    kappa: float      # 曲率 (1/m)

class LaneKeepingPlanner:
    def __init__(self, lookahead_dist: float = 20.0, 
                 point_spacing: float = 1.0, 
                 wheelbase: float = 2.8):
        self.lookahead_dist = lookahead_dist
        self.point_spacing = point_spacing
        self.wheelbase = wheelbase

    def _find_lookahead_point(self, centerline: np.ndarray, 
                              state: VehicleState) -> np.ndarray:
        """在中心线上寻找距车辆前方 lookahead_dist 处的目标点"""
        # 将中心线点转换到车辆坐标系
        dx = centerline[:, 0] - state.x
        dy = centerline[:, 1] - state.y
        cos_yaw, sin_yaw = np.cos(state.yaw), np.sin(state.yaw)
        local_x = dx * cos_yaw + dy * sin_yaw
        local_y = -dx * sin_yaw + dy * cos_yaw

        # 筛选车辆前方的点（local_x > 0）
        forward_mask = local_x > 0
        if not np.any(forward_mask):
            return centerline[-1]

        forward_x = local_x[forward_mask]
        distances = np.sqrt(forward_x**2 + local_y[forward_mask]**2)
        nearest_idx = np.argmin(np.abs(distances - self.lookahead_dist))
        original_idx = np.where(forward_mask)[0][nearest_idx]
        return centerline[original_idx]

    def _compute_curvature(self, state: VehicleState, 
                           target: np.ndarray) -> float:
        """根据纯跟踪几何关系计算所需曲率"""
        dx = target[0] - state.x
        dy = target[1] - state.y
        alpha = np.arctan2(dy, dx) - state.yaw
        ld = np.sqrt(dx**2 + dy**2)
        if ld < 1e-6:
            return 0.0
        return 2.0 * np.sin(alpha) / ld

    def plan(self, centerline: np.ndarray, state: VehicleState) -> list:
        """
        生成车道保持轨迹
        centerline: (N,2) 数组，表示车道中心线全局坐标点
        state: 车辆当前状态
        返回: TrajectoryPoint 列表，从当前位置前方开始，共 N 个点
        """
        # 1. 找到前视目标点
        target = self._find_lookahead_point(centerline, state)
        
        # 2. 计算跟踪目标所需的曲率
        kappa = self._compute_curvature(state, target)
        
        # 3. 生成平滑轨迹点序列
        n_points = int(self.lookahead_dist / self.point_spacing)
        trajectory = []
        
        for i in range(1, n_points + 1):
            s = i * self.point_spacing  # 纵向弧长
            # 基于运动学自行车模型预测位置
            x_pred = state.x + s * np.cos(state.yaw + kappa * s / 2)
            y_pred = state.y + s * np.sin(state.yaw + kappa * s / 2)
            trajectory.append(TrajectoryPoint(x=float(x_pred), 
                                              y=float(y_pred), 
                                              kappa=kappa))
        return trajectory

运行与验证

# 示例：直线车道中心线，车辆在中心线附近但有横向偏移
centerline = np.array([[0.0, 0.0], [10.0, 0.0], [20.0, 0.0], 
                       [30.0, 0.0], [40.0, 0.0], [50.0, 0.0]])
state = VehicleState(x=0.0, y=0.5, yaw=0.05, v=10.0)

planner = LaneKeepingPlanner(lookahead_dist=15.0, point_spacing=1.0)
traj = planner.plan(centerline, state)

# 验证输出
print(f"生成轨迹点数: {len(traj)}")
print(f"首个轨迹点: ({traj[0].x:.2f}, {traj[0].y:.2f}), kappa={traj[0].kappa:.4f}")
print(f"末端轨迹点: ({traj[-1].x:.2f}, {traj[-1].y:.2f}), kappa={traj[-1].kappa:.4f}")

# 验证通过条件：
# 1. 轨迹点数量 = 15（lookahead_dist / point_spacing）
# 2. 轨迹点从车辆前方约1m处开始，延伸至约15m
# 3. 所有点曲率一致（直线车道），且数值合理（|kappa| < 0.1）
# 4. 轨迹终点y坐标应接近0（收敛到车道中心线）

预期输出

生成轨迹点数: 15
首个轨迹点: (1.00, 0.55), kappa=-0.0333
末端轨迹点: (14.96, 0.25), kappa=-0.0333

轨迹点从车辆当前位置前方开始，逐步向车道中心线（y=0）收敛。曲率为负值表示车辆需要向右转向以消除横向偏差，符合车辆位于中心线左侧（y=0.5）的几何关系。

验证方法

• 数量检查：输出点数应等于 lookahead_dist / point_spacing（此处为15）
• 平滑性检查：相邻轨迹点间距应约为 point_spacing（1.0m），可通过计算相邻点欧氏距离验证
• 收敛性检查：轨迹末端点的横向位置应比起始点更接近中心线（y=0）
• 曲率一致性：对于直线中心线，所有轨迹点曲率应相同，且符号与横向偏差方向一致

若需验证弯道场景，可将 centerline 替换为圆弧点集（如半径100m的圆弧），此时输出曲率应接近 1/R = 0.01，且轨迹点应沿圆弧平滑分布。