1. 搭建端到端规划控制仿真环境

任务目标

构建一个轻量级Python仿真环境，包含：自行车模型车辆动力学、结构化道路（车道线）、静态障碍物，以及供后续端到端模型调用的交互接口（重置、步进、获取观测）。本案例不涉及神经网络训练，只交付可验证的仿真底座。

环境依赖

• Python ≥ 3.8
• NumPy ≥ 1.21
• Matplotlib ≥ 3.5（仅用于可视化验证）

核心实现

以下代码完整实现了仿真环境。车辆状态为 [x, y, heading, v]，控制量为 [加速度, 前轮转角]。道路为双向四车道（总宽14米），障碍物以矩形表示。交互接口为 reset()、step(action)、get_observation()。

import numpy as np

class VehicleDynamics:
    """自行车模型：状态 [x, y, heading, v]，控制 [accel, steer]"""
    def __init__(self, wheelbase=2.5):
        self.L = wheelbase

    def step(self, state, action, dt=0.1):
        x, y, theta, v = state
        accel, delta = action
        # 限制控制量范围
        accel = np.clip(accel, -3.0, 3.0)
        delta = np.clip(delta, -0.5, 0.5)
        # 自行车模型离散更新
        x_new = x + v * np.cos(theta) * dt
        y_new = y + v * np.sin(theta) * dt
        theta_new = theta + (v / self.L) * np.tan(delta) * dt
        v_new = v + accel * dt
        v_new = np.clip(v_new, 0.0, 20.0)  # 不倒车，限速
        return np.array([x_new, y_new, theta_new, v_new])

class SimulationEnv:
    """端到端规划控制仿真环境"""
    def __init__(self, dt=0.1):
        self.dt = dt
        self.vehicle = VehicleDynamics()
        # 道路参数：双向四车道，单车道宽3.5m
        self.road_width = 14.0
        self.road_length = 100.0
        self.lane_center_y = [-5.25, -1.75, 1.75, 5.25]  # 四条车道中心线y坐标
        # 障碍物：[x, y, width, length] 矩形中心坐标与尺寸
        self.obstacles = np.array([
            [30.0, -1.75, 2.0, 4.0],
            [55.0, 1.75, 2.0, 4.0],
            [70.0, -5.25, 2.0, 4.0]
        ])
        self.state = None

    def reset(self, start_state=None):
        """重置环境，默认起点在左侧车道中心"""
        if start_state is None:
            self.state = np.array([0.0, -5.25, 0.0, 5.0])
        else:
            self.state = np.array(start_state, dtype=float)
        return self.get_observation()

    def step(self, action):
        """执行控制动作，返回 (obs, done, info)"""
        self.state = self.vehicle.step(self.state, action, self.dt)
        done = self._check_done()
        info = {"offroad": self._check_offroad(),
                "collision": self._check_collision()}
        return self.get_observation(), done, info

    def get_observation(self):
        """返回观测：车辆状态 + 前方最近障碍物相对位置（若有）"""
        obs = self.state.copy()
        # 寻找前方最近障碍物
        min_dist = float('inf')
        rel_pos = np.array([0.0, 0.0])
        for obs_rect in self.obstacles:
            ox, oy, ow, ol = obs_rect
            dx = ox - self.state[0]
            if dx > 0:  # 只考虑前方
                dy = oy - self.state[1]
                dist = np.hypot(dx, dy)
                if dist < min_dist:
                    min_dist = dist
                    rel_pos = np.array([dx, dy])
        if min_dist < float('inf'):
            obs = np.concatenate([obs, rel_pos])
        else:
            obs = np.concatenate([obs, np.array([self.road_length, 0.0])])
        return obs

    def _check_offroad(self):
        """是否偏离道路（y超出道路边界）"""
        return abs(self.state[1]) > self.road_width / 2

    def _check_collision(self):
        """是否与障碍物碰撞（简化为圆心距离判断）"""
        for ox, oy, ow, ol in self.obstacles:
            dx = self.state[0] - ox
            dy = self.state[1] - oy
            if abs(dx) < ol/2 + 1.0 and abs(dy) < ow/2 + 1.0:
                return True
        return False

    def _check_done(self):
        """终止条件：出界、碰撞或到达道路尽头"""
        if self._check_offroad() or self._check_collision():
            return True
        return self.state[0] > self.road_length

# 可视化验证函数（仅用于确认环境正确性）
def visualize(env, traj):
    import matplotlib.pyplot as plt
    fig, ax = plt.subplots(figsize=(10, 4))
    # 画道路边界
    ax.add_patch(plt.Rectangle((-2, -7), env.road_length+4, 14,
                                fill=True, facecolor='lightgray', alpha=0.5))
    # 画车道线
    for y in [-3.5, 0, 3.5]:
        ax.axhline(y, color='white', linestyle='--', linewidth=1)
    # 画障碍物
    for ox, oy, ow, ol in env.obstacles:
        ax.add_patch(plt.Rectangle((ox-ol/2, oy-ow/2), ol, ow,
                                    fill=True, facecolor='red', alpha=0.7))
    # 画轨迹
    traj = np.array(traj)
    ax.plot(traj[:, 0], traj[:, 1], 'b-', linewidth=2, label='轨迹')
    ax.plot(traj[0, 0], traj[0, 1], 'go', markersize=8, label='起点')
    ax.set_xlim(-2, env.road_length+2)
    ax.set_ylim(-8, 8)
    ax.set_aspect('equal')
    ax.legend()
    plt.show()

if __name__ == "__main__":
    # 最小验证：执行一次直线行驶并可视化
    env = SimulationEnv()
    obs = env.reset()
    traj = [env.state.copy()]
    # 简单控制策略：保持当前速度，不转向
    for _ in range(50):
        obs, done, info = env.step([0.0, 0.0])
        traj.append(env.state.copy())
        if done:
            break
    print(f"最终状态: {env.state}")
    print(f"观测维度: {len(obs)}")
    visualize(env, traj)

运行与验证

1. 将上述代码保存为 sim_env.py，直接运行：python sim_env.py。
2. 预期输出：终端打印最终状态（约 [5.0, -5.25, 0.0, 5.0]，因50步×0.1s×5m/s=25m，实际x≈5.0），观测维度为6（4维状态+2维相对障碍物位置）。
3. 可视化窗口应显示：灰色道路、红色障碍物、蓝色直线轨迹从起点(0,-5.25)沿车道中心延伸，不穿越障碍物。

通过条件：程序无报错运行；轨迹保持在车道内（y恒为-5.25）；车辆x坐标随步进增加；观测向量维度为6。若满足以上条件，则仿真环境搭建成功，可供后续端到端模型调用。

接口说明：后续端到端模型只需调用 reset() 获取初始观测，循环调用 step(action) 传入 [加速度, 前轮转角]，即可获得下一时刻观测与终止信号。观测向量为6维，前4维为车辆自身状态，后2维为前方最近障碍物的相对位置（若前方无障碍物，则返回道路尽头坐标）。