AI 在自动驾驶中的应用

# 模块化自动驾驶系统架构概览
import torch
import torch.nn as nn
from typing import Dict, List, Tuple

class ModularAutonomousSystem(nn.Module):
    def __init__(self):
        super().__init__()
        # 感知模块
        self.perception = PerceptionModule()
        # 定位模块
        self.localization = LocalizationModule()
        # 预测模块
        self.prediction = TrajectoryPredictionModule()
        # 规划模块
        self.planning = MotionPlanningModule()
        # 控制模块
        self.control = ControlModule()

    def forward(self, sensor_data: Dict[str, torch.Tensor],
                map_data: torch.Tensor) -> Dict[str, torch.Tensor]:
        # 感知：目标检测 + 语义分割
        objects = self.perception(sensor_data)
        # 定位：车辆位姿估计
        pose = self.localization(sensor_data, map_data)
        # 预测：周围目标轨迹预测
        predictions = self.prediction(objects, pose)
        # 规划：生成行驶轨迹
        trajectory = self.planning(pose, objects, predictions, map_data)
        # 控制：油门、刹车、转向
        commands = self.control(trajectory, pose)
        return {
            "objects": objects,
            "pose": pose,
            "predictions": predictions,
            "trajectory": trajectory,
            "commands": commands
        }

# 端到端驾驶模型基础架构
import torch
import torch.nn as nn

class EndToEndDrivingModel(nn.Module):
    def __init__(self, num_cameras: int = 6, img_size: int = 256):
        super().__init__()
        # 多视角特征提取
        self.backbone = nn.Sequential(
            nn.Conv2d(3 * num_cameras, 64, kernel_size=7, stride=2, padding=3),
            nn.BatchNorm2d(64),
            nn.ReLU(inplace=True),
            nn.Conv2d(64, 128, kernel_size=3, stride=2, padding=1),
            nn.BatchNorm2d(128),
            nn.ReLU(inplace=True),
            nn.Conv2d(128, 256, kernel_size=3, stride=2, padding=1)
        )
        # BEV 特征转换
        self.bev_encoder = nn.Sequential(
            nn.Conv2d(256, 512, kernel_size=3, padding=1),
            nn.ReLU(),
            nn.Conv2d(512, 512, kernel_size=3, padding=1)
        )
        # 轨迹头
        self.trajectory_head = nn.Sequential(
            nn.AdaptiveAvgPool2d(1),
            nn.Flatten(),
            nn.Linear(512, 256),
            nn.ReLU(),
            nn.Linear(256, 80)  # 20步 x 4维
        )
        # 控制头
        self.control_head = nn.Sequential(
            nn.AdaptiveAvgPool2d(1),
            nn.Flatten(),
            nn.Linear(512, 64),
            nn.ReLU(),
            nn.Linear(64, 3)  # 油门、刹车、转向
        )

    def forward(self, multi_view_images: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]:
        B = multi_view_images.shape[0]
        features = self.backbone(multi_view_images)
        bev_features = self.bev_encoder(features)
        trajectory = self.trajectory_head(bev_features).view(B, 20, 4)
        commands = self.control_head(bev_features)
        return trajectory, commands

# CenterPoint 3D 目标检测核心
import torch
import torch.nn as nn
import torch.nn.functional as F

class CenterPoint3D(nn.Module):
    def __init__(self, num_classes: int = 3, voxel_size: float = 0.1):
        super().__init__()
        self.voxel_encoder = VoxelFeatureEncoder()
        self.backbone = SpMiddleFHD(output_channels=128)
        self.head = nn.ModuleDict({
            "heatmap": nn.Conv2d(128, num_classes, kernel_size=3, padding=1),
            "offset": nn.Conv2d(128, 2, kernel_size=3, padding=1),
            "height": nn.Conv2d(128, 1, kernel_size=3, padding=1),
            "dim": nn.Conv2d(128, 3, kernel_size=3, padding=1),
            "rotation": nn.Conv2d(128, 2, kernel_size=3, padding=1)
        })

    def forward(self, point_cloud: torch.Tensor) -> Dict[str, torch.Tensor]:
        voxels, coords = self.voxel_encoder(point_cloud)
        features = self.backbone(voxels, coords)
        heatmap = torch.sigmoid(self.head["heatmap"](features))
        offset = self.head["offset"](features)
        height = self.head["height"](features)
        dim = self.head["dim"](features)
        rotation = self.head["rotation"](features)
        return {
            "heatmap": heatmap,
            "offset": offset,
            "height": height,
            "dim": dim,
            "rotation": rotation
        }

# BEV 语义分割网络
import torch
import torch.nn as nn

class BEVSegmentation(nn.Module):
    def __init__(self, num_classes: int = 10, bev_range: list = [-50, 50, -50, 50]):
        super().__init__()
        self.bev_range = bev_range
        # 多视角图像编码器
        self.image_encoder = nn.Sequential(
            nn.Conv2d(3, 64, 7, 2, 3),
            nn.BatchNorm2d(64),
            nn.ReLU(),
            nn.Conv2d(64, 128, 3, 2, 1),
            nn.BatchNorm2d(128),
            nn.ReLU(),
            nn.Conv2d(128, 256, 3, 2, 1)
        )
        # LSS 深度分布估计
        self.depth_net = nn.Sequential(
            nn.Conv2d(256, 128, 3, padding=1),
            nn.ReLU(),
            nn.Conv2d(128, 80, 1)  # 80 depth bins
        )
        # BEV 融合与分割
        self.bev_head = nn.Sequential(
            nn.Conv2d(256, 512, 3, padding=1),
            nn.BatchNorm2d(512),
            nn.ReLU(),
            nn.Conv2d(512, num_classes, 1)
        )

    def forward(self, images: torch.Tensor, cam_params: torch.Tensor) -> torch.Tensor:
        B, N, C, H, W = images.shape
        images = images.view(B * N, C, H, W)
        features = self.image_encoder(images)
        depth_dist = torch.softmax(self.depth_net(features), dim=1)
        # 简化 LSS 体素池化
        bev_features = self._pool_to_bev(features, depth_dist, cam_params, B, N)
        segmentation = self.bev_head(bev_features)
        return segmentation

    def _pool_to_bev(self, features, depth_dist, cam_params, B, N):
        # 实际实现需要完整的 LSS 几何投影
        return torch.zeros(B, 256, 200, 200, device=features.device)

# BEV 空间多传感器特征融合
import torch
import torch.nn as nn
import torch.nn.functional as F

class BEVFusionModule(nn.Module):
    def __init__(self, lidar_dim: int = 128, camera_dim: int = 256, fusion_dim: int = 256):
        super().__init__()
        # 传感器特征投影到统一维度
        self.lidar_proj = nn.Conv2d(lidar_dim, fusion_dim, 1)
        self.camera_proj = nn.Conv2d(camera_dim, fusion_dim, 1)
        # 交叉注意力融合
        self.lidar_attn = nn.MultiheadAttention(fusion_dim, num_heads=8, batch_first=True)
        self.camera_attn = nn.MultiheadAttention(fusion_dim, num_heads=8, batch_first=True)
        # 融合后处理
        self.fusion_conv = nn.Sequential(
            nn.Conv2d(fusion_dim * 2, fusion_dim, 3, padding=1),
            nn.BatchNorm2d(fusion_dim),
            nn.ReLU(),
            nn.Conv2d(fusion_dim, fusion_dim, 3, padding=1)
        )

    def forward(self, lidar_bev: torch.Tensor, camera_bev: torch.Tensor) -> torch.Tensor:
        B, C, H, W = lidar_bev.shape
        lidar_feat = self.lidar_proj(lidar_bev)
        camera_feat = self.camera_proj(camera_bev)
        # 展平为序列
        lidar_seq = lidar_feat.flatten(2).permute(0, 2, 1)
        camera_seq = camera_feat.flatten(2).permute(0, 2, 1)
        # 交叉注意力
        lidar_enhanced, _ = self.lidar_attn(lidar_seq, camera_seq, camera_seq)
        camera_enhanced, _ = self.camera_attn(camera_seq, lidar_seq, lidar_seq)
        # 恢复空间形状
        lidar_enhanced = lidar_enhanced.permute(0, 2, 1).view(B, -1, H, W)
        camera_enhanced = camera_enhanced.permute(0, 2, 1).view(B, -1, H, W)
        # 拼接融合
        fused = torch.cat([lidar_enhanced, camera_enhanced], dim=1)
        return self.fusion_conv(fused)

# 传感器标定与时间同步工具
import numpy as np
from scipy.spatial.transform import Rotation

class SensorCalibration:
    def __init__(self):
        self.extrinsics = {}  # 传感器外参
        self.time_offsets = {}  # 时间偏移

    def set_extrinsic(self, sensor_name: str, R: np.ndarray, t: np.ndarray):
        T = np.eye(4)
        T[:3, :3] = R
        T[:3, 3] = t
        self.extrinsics[sensor_name] = T

    def transform_point(self, point: np.ndarray, from_sensor: str, to_sensor: str) -> np.ndarray:
        T_from = self.extrinsics[from_sensor]
        T_to = np.linalg.inv(self.extrinsics[to_sensor])
        T = T_to @ T_from
        point_h = np.append(point, 1.0)
        return (T @ point_h)[:3]

    def synchronize_timestamps(self, timestamps: dict, reference: str = "lidar") -> dict:
        synced = {}
        ref_time = timestamps[reference]
        for sensor, ts in timestamps.items():
            offset = self.time_offsets.get(sensor, 0.0)
            synced[sensor] = ref_time + offset
        return synced

    def calibrate_lidar_camera(self, lidar_points: np.ndarray,
                                camera_points: np.ndarray) -> tuple:
        from scipy.optimize import minimize
        def objective(params):
            R = Rotation.from_euler("xyz", params[:3]).as_matrix()
            t = params[3:]
            T = np.eye(4)
            T[:3, :3] = R
            T[:3, 3] = t
            projected = (T @ np.hstack([lidar_points, np.ones((len(lidar_points), 1))].T).T).T
            return np.mean((projected[:, :3] - camera_points) ** 2)
        result = minimize(objective, np.zeros(6), method="Nelder-Mead")
        R_opt = Rotation.from_euler("xyz", result.x[:3]).as_matrix()
        return R_opt, result.x[3:]

# 基于 Transformer 的多模态轨迹预测
import torch
import torch.nn as nn
import math

class TrajectoryTransformer(nn.Module):
    def __init__(self, num_modes: int = 6, horizon: int = 80, hidden_dim: int = 128):
        super().__init__()
        self.num_modes = num_modes
        self.horizon = horizon
        # 历史编码
        self.history_encoder = nn.Sequential(
            nn.Linear(6, hidden_dim),  # x, y, vx, vy, heading, type
            nn.LayerNorm(hidden_dim),
            nn.ReLU()
        )
        # Transformer 编码器
        encoder_layer = nn.TransformerEncoderLayer(
            d_model=hidden_dim, nhead=8,
            dim_feedforward=512, dropout=0.1, batch_first=True
        )
        self.transformer_encoder = nn.TransformerEncoder(encoder_layer, num_layers=4)
        # 多模态解码头
        self.mode_query = nn.Parameter(torch.randn(num_modes, hidden_dim))
        self.decoder = nn.Sequential(
            nn.Linear(hidden_dim, hidden_dim),
            nn.ReLU(),
            nn.Linear(hidden_dim, horizon * 2)  # (x, y) per step
        )
        # 概率评分头
        self.prob_head = nn.Sequential(
            nn.Linear(hidden_dim, 1)
        )

    def forward(self, agent_history: torch.Tensor,
                map_context: torch.Tensor) -> tuple:
        B, N, T, _ = agent_history.shape
        # 编码历史
        agent_features = self.history_encoder(agent_history).view(B * N, T, -1)
        encoded = self.transformer_encoder(agent_features)
        # 聚合
        context = encoded.mean(dim=1).view(B, N, -1).mean(dim=1)
        # 多模态解码
        mode_features = self.mode_query.unsqueeze(0).expand(B, -1, -1)
        mode_features = mode_features + context.unsqueeze(1)
        trajectories = self.decoder(mode_features).view(B, self.num_modes, self.horizon, 2)
        probabilities = torch.softmax(self.prob_head(mode_features).squeeze(-1), dim=-1)
        return trajectories, probabilities

# 交互感知图神经网络预测
import torch
import torch.nn as nn
import torch.nn.functional as F

class InteractionGraphPredictor(nn.Module):
    def __init__(self, num_agents: int = 20, hidden_dim: int = 64, num_modes: int = 6):
        super().__init__()
        self.num_agents = num_agents
        self.num_modes = num_modes
        self.node_encoder = nn.Linear(5, hidden_dim)
        self.edge_encoder = nn.Linear(3, hidden_dim)
        # 图注意力层
        self.gat_layer1 = nn.MultiheadAttention(hidden_dim, num_heads=4, batch_first=True)
        self.gat_layer2 = nn.MultiheadAttention(hidden_dim, num_heads=4, batch_first=True)
        # 轨迹输出
        self.trajectory_decoder = nn.Sequential(
            nn.Linear(hidden_dim, 128),
            nn.ReLU(),
            nn.Linear(128, 80 * 2)  # 80 timesteps
        )
        self.confidence_decoder = nn.Linear(hidden_dim, num_modes)

    def build_adjacency(self, positions: torch.Tensor, threshold: float = 50.0) -> torch.Tensor:
        B, N, _ = positions.shape
        dist_matrix = torch.cdist(positions, positions)
        adjacency = (dist_matrix < threshold).float()
        # 掩码自连接
        adjacency += torch.eye(N, device=positions.device).unsqueeze(0)
        return adjacency

    def forward(self, agent_states: torch.Tensor) -> tuple:
        B, N, T, _ = agent_states.shape
        # 节点特征
        node_feat = self.node_encoder(agent_states[:, :, -1, :])
        # 交互建模
        enhanced1, _ = self.gat_layer1(node_feat, node_feat, node_feat)
        enhanced2, _ = self.gat_layer2(enhanced1, enhanced1, enhanced1)
        # 输出
        trajectories = self.trajectory_decoder(enhanced2).view(B, N, 80, 2)
        confidence = torch.softmax(self.confidence_decoder(enhanced2.mean(dim=1)), dim=-1)
        return trajectories, confidence

# 强化学习驾驶策略 - PPO 实现
import torch
import torch.nn as nn
import torch.nn.functional as F
from torch.distributions import Normal

class DrivingPPOAgent(nn.Module):
    def __init__(self, state_dim: int = 64, action_dim: int = 3):
        super().__init__()
        # 共享特征提取
        self.feature_extractor = nn.Sequential(
            nn.Linear(state_dim, 256),
            nn.ReLU(),
            nn.Linear(256, 128),
            nn.ReLU()
        )
        # Actor 网络
        self.actor_mean = nn.Sequential(
            nn.Linear(128, 64),
            nn.ReLU(),
            nn.Linear(64, action_dim),
            nn.Tanh()
        )
        self.actor_logstd = nn.Parameter(torch.zeros(action_dim))
        # Critic 网络
        self.critic = nn.Sequential(
            nn.Linear(128, 64),
            nn.ReLU(),
            nn.Linear(64, 1)
        )

    def forward(self, state: torch.Tensor) -> tuple:
        features = self.feature_extractor(state)
        action_mean = self.actor_mean(features)
        action_std = torch.exp(self.actor_logstd).expand_as(action_mean)
        dist = Normal(action_mean, action_std)
        action = dist.sample()
        log_prob = dist.log_prob(action).sum(dim=-1)
        value = self.critic(features).squeeze(-1)
        return action, log_prob, value

    def evaluate(self, state: torch.Tensor, action: torch.Tensor) -> tuple:
        features = self.feature_extractor(state)
        action_mean = self.actor_mean(features)
        action_std = torch.exp(self.actor_logstd).expand_as(action_mean)
        dist = Normal(action_mean, action_std)
        log_prob = dist.log_prob(action).sum(dim=-1)
        entropy = dist.entropy().sum(dim=-1)
        value = self.critic(features).squeeze(-1)
        return log_prob, entropy, value

# 模型预测控制 - 轨迹优化
import numpy as np
from scipy.optimize import minimize

class MPCPlanner:
    def __init__(self, horizon: int = 20, dt: float = 0.1):
        self.horizon = horizon
        self.dt = dt
        self.vehicle_length = 4.5
        self.vehicle_width = 1.8

    def bicycle_model(self, state: np.ndarray, control: np.ndarray) -> np.ndarray:
        x, y, v, theta = state
        delta, a = control
        # 运动学自行车模型
        x_dot = v * np.cos(theta)
        y_dot = v * np.sin(theta)
        v_dot = a
        theta_dot = v * np.tan(delta) / 2.7  # 轴距 2.7m
        return np.array([x_dot, y_dot, v_dot, theta_dot])

    def optimize_trajectory(self, initial_state: np.ndarray,
                             reference_path: np.ndarray,
                             obstacles: list) -> np.ndarray:
        n_controls = self.horizon * 2
        def cost(controls):
            controls = controls.reshape(self.horizon, 2)
            state = initial_state.copy()
            total_cost = 0.0
            for t in range(self.horizon):
                state = state + self.bicycle_model(state, controls[t]) * self.dt
                # 跟踪误差
                ref_pos = reference_path[min(t, len(reference_path)-1)]
                tracking_error = (state[0] - ref_pos[0])2 + (state[1] - ref_pos[1])2
                total_cost += 10.0 * tracking_error
                # 控制平滑
                if t > 0:
                    total_cost += 0.1 * np.sum(controls[t] - controls[t-1])2
                # 碰撞惩罚
                for obs_x, obs_y, obs_r in obstacles:
                    dist = np.sqrt((state[0]-obs_x)2 + (state[1]-obs_y)2)
                    if dist < obs_r + 1.0:
                        total_cost += 1000.0 * (obs_r + 1.0 - dist)2
            return total_cost
        result = minimize(cost, np.zeros(n_controls),
                         method="SLSQP",
                         bounds=[(-0.5, 0.5)] * self.horizon + [(-3.0, 3.0)] * self.horizon)
        return result.x.reshape(self.horizon, 2)

# CARLA 自动化测试框架
import carla
import numpy as np

class AutonomousTestRunner:
    def __init__(self, host: str = "localhost", port: int = 2000):
        self.client = carla.Client(host, port)
        self.client.set_timeout(10.0)
        self.world = self.client.get_world()
        self.results = []

    def run_test_scenario(self, scenario_name: str,
                           autopilot_class, num_episodes: int = 100) -> dict:
        scenario = self._load_scenario(scenario_name)
        successes = 0
        infractions = {"collision": 0, "red_light": 0, "route_deviation": 0}
        for ep in range(num_episodes):
            env = self._setup_environment(scenario, ep)
            agent = autopilot_class()
            done = False
            reward_sum = 0.0
            while not done:
                state = self._get_state(env)
                action = agent.get_action(state)
                next_state, reward, done, info = self._step(env, action)
                reward_sum += reward
                if info.get("collision"):
                    infractions["collision"] += 1
                    done = True
                if info.get("red_light"):
                    infractions["red_light"] += 1
            if reward_sum > 0:
                successes += 1
        return {
            "scenario": scenario_name,
            "success_rate": successes / num_episodes,
            "infractions": infractions,
            "total_episodes": num_episodes
        }

    def _load_scenario(self, name):
        # 从场景库加载
        return {"name": name}

    def _setup_environment(self, scenario, seed):
        return {"scenario": scenario, "seed": seed}

    def _get_state(self, env):
        return np.random.randn(64)

    def _step(self, env, action):
        return None, 0.0, True, {}

# 基于扩散模型的场景生成
import torch
import torch.nn as nn

class ScenarioDiffusionModel(nn.Module):
    def __init__(self, num_agents: int = 20, horizon: int = 80, hidden_dim: int = 128):
        super().__init__()
        self.num_agents = num_agents
        self.horizon = horizon
        # 场景编码
        self.scene_encoder = nn.Sequential(
            nn.Linear(num_agents * horizon * 4, hidden_dim * 4),
            nn.LayerNorm(hidden_dim * 4),
            nn.ReLU(),
            nn.Linear(hidden_dim * 4, hidden_dim)
        )
        # 扩散去噪网络
        self.denoise_net = nn.Sequential(
            nn.Linear(hidden_dim + num_agents * horizon * 4, hidden_dim * 2),
            nn.ReLU(),
            nn.Linear(hidden_dim * 2, hidden_dim),
            nn.ReLU(),
            nn.Linear(hidden_dim, num_agents * horizon * 4)
        )
        self.noise_schedule = torch.linspace(1e-4, 0.02, 1000)

    def forward_diffusion(self, x0: torch.Tensor, t: int) -> tuple:
        beta_t = self.noise_schedule[t]
        alpha_t = 1.0 - beta_t
        noise = torch.randn_like(x0)
        xt = torch.sqrt(torch.tensor(alpha_t)) * x0 + torch.sqrt(torch.tensor(1 - alpha_t)) * noise
        return xt, noise

    def denoise(self, xt: torch.Tensor, t: int, context: torch.Tensor) -> torch.Tensor:
        t_emb = self._timestep_embedding(t, context.shape[0])
        combined = torch.cat([xt, context, t_emb], dim=-1)
        return self.denoise_net(combined)

    def sample(self, context: torch.Tensor, num_steps: int = 50) -> torch.Tensor:
        B = context.shape[0]
        x = torch.randn(B, self.num_agents * self.horizon * 4, device=context.device)
        for t in reversed(range(num_steps)):
            noise_pred = self.denoise(x, t, context)
            x = x - noise_pred * self.noise_schedule[t]
        return x.view(B, self.num_agents, self.horizon, 4)

    def _timestep_embedding(self, t: int, batch_size: int) -> torch.Tensor:
        return torch.full((batch_size, 1), t / 1000.0)

# 多任务端到端驾驶模型训练
import torch
import torch.nn as nn
import torch.nn.functional as F

class UniADUnified(nn.Module):
    def __init__(self, num_classes: int = 10, num_modes: int = 6):
        super().__init__()
        # BEV 特征提取（共享）
        self.bev_encoder = BEVEncoder()
        # 检测头
        self.detect_head = nn.Sequential(
            nn.Conv2d(256, 128, 3, padding=1),
            nn.ReLU(),
            nn.Conv2d(128, num_classes + 4, 1)  # cls + bbox
        )
        # 跟踪头
        self.track_head = nn.Sequential(
            nn.Conv2d(256, 128, 3, padding=1),
            nn.ReLU(),
            nn.Conv2d(128, 64, 1)  # track embedding
        )
        # 预测头
        self.predict_head = TrajectoryHead(num_modes=num_modes)
        # 规划头
        self.plan_head = nn.Sequential(
            nn.AdaptiveAvgPool2d(1),
            nn.Flatten(),
            nn.Linear(256, 128),
            nn.ReLU(),
            nn.Linear(128, 80 * 2)  # 规划轨迹
        )

    def forward(self, multi_view_images: torch.Tensor,
                targets: dict = None) -> dict:
        bev_features = self.bev_encoder(multi_view_images)
        # 多任务输出
        detection = self.detect_head(bev_features)
        tracking = self.track_head(bev_features)
        prediction = self.predict_head(bev_features)
        planning = self.plan_head(bev_features).view(-1, 80, 2)
        outputs = {
            "detection": detection,
            "tracking": tracking,
            "prediction": prediction,
            "planning": planning
        }
        if targets is not None:
            loss = self._compute_loss(outputs, targets)
            outputs["loss"] = loss
        return outputs

    def _compute_loss(self, outputs: dict, targets: dict) -> torch.Tensor:
        det_loss = F.cross_entropy(outputs["detection"], targets["detection_labels"]) / 10.0
        track_loss = F.mse_loss(outputs["tracking"], targets["tracking_emb"]) / 100.0
        pred_loss = F.l1_loss(outputs["prediction"], targets["prediction_traj"]) / 5.0
        plan_loss = F.mse_loss(outputs["planning"], targets["planning_traj"]) / 2.0
        return det_loss + track_loss + pred_loss + plan_loss

# 部署与推理服务
import torch
import numpy as np
from dataclasses import dataclass
import time

@dataclass
class DrivingCommand:
    steering: float
    throttle: float
    brake: float
    planned_trajectory: np.ndarray

class DrivingInferenceEngine:
    def __init__(self, model_path: str, device: str = "cuda"):
        self.device = torch.device(device)
        self.model = torch.jit.load(model_path)
        self.model.to(self.device)
        self.model.eval()
        self.latency_buffer = []

    def infer(self, camera_images: np.ndarray,
              ego_state: np.ndarray,
              navigation_goal: np.ndarray) -> DrivingCommand:
        start_time = time.time()
        # 预处理
        tensor_images = self._preprocess_images(camera_images)
        tensor_ego = torch.tensor(ego_state, device=self.device).float()
        # 推理
        with torch.no_grad():
            plan_traj = self.model(tensor_images, tensor_ego)
        # 轨迹转控制指令
        command = self._trajectory_to_control(plan_traj.cpu().numpy(), ego_state)
        latency = time.time() - start_time
        self.latency_buffer.append(latency)
        if len(self.latency_buffer) > 100:
            self.latency_buffer.pop(0)
        return command

    def _preprocess_images(self, images: np.ndarray) -> torch.Tensor:
        tensor = torch.tensor(images, device=self.device).float() / 255.0
        return tensor.permute(0, 3, 1, 2).unsqueeze(0)

    def _trajectory_to_control(self, trajectory: np.ndarray,
                                ego_state: np.ndarray) -> DrivingCommand:
        # 纯追踪控制器
        lookahead_idx = 10
        target_point = trajectory[lookahead_idx]
        dx = target_point[0] - ego_state[0]
        dy = target_point[1] - ego_state[1]
        steering = np.arctan2(dy, dx) * 0.5
        speed = np.sqrt(dx2 + dy2) / (lookahead_idx * 0.1)
        throttle = min(max(speed * 0.3, 0.0), 1.0)
        brake = max(0.0, 1.0 - speed * 0.5)
        return DrivingCommand(
            steering=float(np.clip(steering, -1.0, 1.0)),
            throttle=float(throttle),
            brake=float(brake),
            planned_trajectory=trajectory
        )

    @property
    def avg_latency(self) -> float:
        return np.mean(self.latency_buffer) if self.latency_buffer else 0.0