第1课: 自动驾驶概述

import numpy as np from dataclasses import dataclass from typing import List, Tuple import time @dataclass class SensorData: timestamp: float lidar_points: np.ndarray radar_detections: list imu_accel: Tuple[float,float,float] gps_position: Tuple[float,float] @dataclass class DetectedObject: obj_type: str position: Tuple[float,float] velocity: Tuple[float,float] confidence: float @dataclass class TrajPoint: x: float; y: float; theta: float; v: float; t: float @dataclass class ControlCmd: steering: float; throttle: float; brake: float def simulate_sensors(t): n = 3000 ang = np.random.uniform(-np.pi, np.pi, n) dst = np.random.uniform(1, 80, n) lidar = np.column_stack([dst*np.cos(ang), dst*np.sin(ang), np.where(np.random.random(n)>0.4, np.random.uniform(-0.3,0.3,n), np.random.uniform(0,2.0,n)), np.random.uniform(0.1,1.0,n)]) radar = [(25+t*0.5, -2.0, 0.08), (40-t*0.3, 1.5, -0.15)] return SensorData(t, lidar, radar, (0.1,0,9.81), (31.23+1e-5*t, 121.47+1e-5*t)) def perceive(sensor): objects = [] for d, v, a in sensor.radar_detections: x, y = d*np.cos(a), d*np.sin(a) vx, vy = v*np.cos(a), v*np.sin(a) objects.append(DetectedObject('car' if d>15 else 'pedestrian', (x,y), (vx,vy), 0.82+0.12*np.random.random())) close = sensor.lidar_points[sensor.lidar_points[:,0] < 10] if len(close) > 80: objects.append(DetectedObject('obstacle', (float(np.mean(close[:,0])), float(np.mean(close[:,1]))), (0,0), 0.65)) return objects def plan(objects, ego, target): traj = []; x, y = ego for i in range(60): t = i * 0.1; dx, dy = target[0]-x, target[1]-y dist = np.hypot(dx, dy) if dist < 1.0: break off_x, off_y = 0, 0 for o in objects: od = np.hypot(x-o.position[0], y-o.position[1]) if od < 10: repel = 3.0 / max(od**2, 0.5) off_x += repel * (x-o.position[0])/max(od,0.1) off_y += repel * (y-o.position[1])/max(od,0.1) theta = np.arctan2(dy+off_y*0.5, dx+off_x*0.5) v = min(12.0, dist*0.4) x += v*np.cos(theta)*0.1; y += v*np.sin(theta)*0.1 traj.append(TrajPoint(x, y, theta, v, t)) return traj def ctrl(traj, state, wheelbase=2.8): ex, ey, eth, ev = state if not traj: return ControlCmd(0, 0, 1.0) min_d, idx = float('inf'), 0 for i, tp in enumerate(traj): d = (tp.x-ex)**2+(tp.y-ey)**2 if d < min_d: min_d, idx = d, i la = min(idx+5, len(traj)-1); tp = traj[la] alpha = np.arctan2(tp.y-ey, tp.x-ex) - eth L = np.hypot(tp.x-ex, tp.y-ey) steering = np.clip(np.arctan(2*wheelbase*np.sin(alpha)/max(L,0.1)), -0.6, 0.6) ve = tp.v - ev return ControlCmd(steering, np.clip(0.6*ve,0,1), np.clip(-0.6*ve,0,1)) # Run simulation ego = [0.0, 0.0, 0.0, 8.0]; target = (120.0, 0.0) positions = [(0.0, 0.0)]; speeds = [8.0]; t0 = time.time() for step in range(300): t = step * 0.1 sensor = simulate_sensors(t) objects = perceive(sensor) traj = plan(objects, (ego[0],ego[1]), target) cmd = ctrl(traj, tuple(ego)) ego[2] += (ego[3]/2.8)*np.tan(cmd.steering)*0.1 ego[3] = max(0, min(ego[3]+(cmd.throttle*5-cmd.brake*8)*0.1, 20.0)) ego[0] += ego[3]*np.cos(ego[2])*0.1; ego[1] += ego[3]*np.sin(ego[2])*0.1 positions.append((ego[0], ego[1])); speeds.append(ego[3]) if np.hypot(ego[0]-120, ego[1]) < 2.0: break elapsed = time.time()-t0; fdist = np.hypot(ego[0]-120, ego[1]) print(f"Steps={step+1} Time={elapsed:.3f}s Final=({ego[0]:.2f},{ego[1]:.2f}) Dist={fdist:.2f}m Speed={ego[3]:.2f}m/s")