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

任务目标

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

核心实现

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
    yaw: float
    kappa: float  # 曲率 (1/m)

class LaneKeepPlanner:
    def __init__(self, lookahead_dist: float = 20.0, dt: float = 0.2,
                 horizon: int = 10, wheelbase: float = 2.8):
        self.Ld = lookahead_dist      # 预瞄距离
        self.dt = dt                  # 采样间隔
        self.horizon = horizon        # 轨迹点数
        self.L = wheelbase            # 轴距

    def _find_target_point(self, state: VehicleState, centerline: np.ndarray):
        """在中心线上寻找距离车辆前方Ld处的目标点"""
        # centerline: (N,2) 每行为[x,y]
        dx = centerline[:,0] - state.x
        dy = centerline[:,1] - state.y
        dist = np.hypot(dx, dy)
        # 筛选车辆前方的点（投影到航向方向为正）
        forward = dx * np.cos(state.yaw) + dy * np.sin(state.yaw)
        candidates = np.where((dist > self.Ld*0.5) & (forward > 0))[0]
        if len(candidates) == 0:
            # 回退：取最近点
            idx = int(np.argmin(dist))
        else:
            # 取距离最接近Ld的前方点
            idx = candidates[int(np.argmin(np.abs(dist[candidates] - self.Ld)))]
        return centerline[idx]

    def _pure_pursuit_control(self, state: VehicleState, target: np.ndarray):
        """纯跟踪算法计算前轮转角"""
        dx = target[0] - state.x
        dy = target[1] - state.y
        alpha = np.arctan2(dy, dx) - state.yaw
        # 归一化角度到[-pi, pi]
        alpha = np.arctan2(np.sin(alpha), np.cos(alpha))
        if abs(alpha) < 1e-6:
            return 0.0
        delta = np.arctan2(2.0 * self.L * np.sin(alpha), self.Ld)
        return np.clip(delta, -0.6, 0.6)  # 前轮转角限幅

    def generate_trajectory(self, state: VehicleState, centerline: np.ndarray):
        """
        生成车道保持轨迹
        返回: list[TrajectoryPoint], 长度=horizon
        """
        traj = []
        cur_state = VehicleState(state.x, state.y, state.yaw, state.v)

        for _ in range(self.horizon):
            target = self._find_target_point(cur_state, centerline)
            delta = self._pure_pursuit_control(cur_state, target)

            # 运动学自行车模型更新
            curvature = np.tan(delta) / self.L
            cur_state.x += cur_state.v * np.cos(cur_state.yaw) * self.dt
            cur_state.y += cur_state.v * np.sin(cur_state.yaw) * self.dt
            cur_state.yaw += cur_state.v * curvature * self.dt
            cur_state.yaw = np.arctan2(np.sin(cur_state.yaw), np.cos(cur_state.yaw))

            traj.append(TrajectoryPoint(
                x=cur_state.x, y=cur_state.y,
                yaw=cur_state.yaw, kappa=curvature
            ))
        return traj

运行与验证

依赖：仅需 numpy。

最小验证脚本：

# 构造直线车道中心线（沿X轴）
centerline = np.array([[i*1.0, 0.0] for i in range(200)])

# 车辆初始状态：位于车道中心线左侧0.5m，航向偏0.1rad
state = VehicleState(x=0.0, y=-0.5, yaw=0.1, v=10.0)

planner = LaneKeepPlanner()
traj = planner.generate_trajectory(state, centerline)

# 验证条件
assert len(traj) == 10, "轨迹点数应为10"
# 1. 终点横向偏差应收敛到0附近（车道保持）
final_lateral = traj[-1].y
print(f"终点横向偏差: {final_lateral:.3f} m")
# 2. 曲率应连续且不超过限幅对应值
kappas = [p.kappa for p in traj]
print(f"曲率范围: [{min(kappas):.4f}, {max(kappas):.4f}] 1/m")
# 3. 航向角应平滑变化（无跳变）
yaw_diffs = np.diff([p.yaw for p in traj])
print(f"航向角最大变化: {np.max(np.abs(yaw_diffs)):.4f} rad/步")

通过标准：

• 轨迹点数等于设定的 horizon（10个）；
• 终点横向偏差值小于0.2m（表明车辆已收敛回车道中心线）；
• 相邻点航向角变化小于0.05 rad（轨迹平滑无突变）；
• 曲率值在合理范围内（|κ| ≤ tan(0.6)/2.8 ≈ 0.24 1/m）。

若初始横向偏差增大或车道出现弯道，可适当增大 lookahead_dist 以改善跟踪稳定性。

关键说明：规划器采用纯跟踪几何方法计算前轮转角，并用自行车模型递推预测未来轨迹。核心逻辑集中在 _find_target_point（选取预瞄点）与 _pure_pursuit_control（计算转角）中，二者共同保证轨迹收敛于车道中心线。若需适应弯道场景，只需将弯道中心线点集传入 centerline 参数即可，无需修改算法内部逻辑。