diff --git a/src/driverless_car_yb/main.light.py b/src/driverless_car_yb/main.light.py new file mode 100644 index 0000000000..ac6ec58194 --- /dev/null +++ b/src/driverless_car_yb/main.light.py @@ -0,0 +1,157 @@ +import cv2 +import numpy as np + + +class TrafficLightDetector: + def __init__(self): + # 定义红绿灯颜色范围(HSV 空间,抗光照干扰更强) + self.color_ranges = { + 'red': [ + (0, 120, 70), # 红色低阈值(低色相范围) + (10, 255, 255), # 红色高阈值(低色相范围) + (170, 120, 70), # 红色低阈值(高色相范围,因HSV色相环0和180相邻) + (180, 255, 255) # 红色高阈值(高色相范围) + ], + 'yellow': [ + (20, 120, 70), # 黄色低阈值 + (30, 255, 255) # 黄色高阈值 + ], + 'green': [ + (35, 120, 70), # 绿色低阈值 + (77, 255, 255) # 绿色高阈值 + ] + } + + def preprocess_image(self, frame): + """图像预处理:降噪 + 尺寸缩放""" + # 缩放图像(加快处理速度) + frame = cv2.resize(frame, (640, 480)) + # 高斯模糊降噪 + blur = cv2.GaussianBlur(frame, (5, 5), 0) + # 转换为 HSV 颜色空间(更适合颜色分割) + hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV) + return frame, hsv + + def get_color_mask(self, hsv, color): + """根据颜色获取掩码(筛选目标颜色区域)""" + if color == 'red': + # 红色需合并两个范围的掩码 + lower1 = np.array(self.color_ranges[color][0]) + upper1 = np.array(self.color_ranges[color][1]) + lower2 = np.array(self.color_ranges[color][2]) + upper2 = np.array(self.color_ranges[color][3]) + mask1 = cv2.inRange(hsv, lower1, upper1) + mask2 = cv2.inRange(hsv, lower2, upper2) + mask = cv2.bitwise_or(mask1, mask2) + else: + lower = np.array(self.color_ranges[color][0]) + upper = np.array(self.color_ranges[color][1]) + mask = cv2.inRange(hsv, lower, upper) + + # 形态学操作:去除小噪点 + 强化目标区域 + kernel = np.ones((5, 5), np.uint8) + mask = cv2.erode(mask, kernel, iterations=1) # 腐蚀 + mask = cv2.dilate(mask, kernel, iterations=2) # 膨胀 + return mask + + def detect_light(self, frame, hsv): + """检测红绿灯状态""" + max_area = 0 + detected_color = "none" + + # 遍历三种颜色,寻找最大的有效颜色区域(红绿灯灯芯) + for color in ['red', 'yellow', 'green']: + mask = self.get_color_mask(hsv, color) + # 查找轮廓 + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + + for cnt in contours: + # 计算轮廓面积(过滤小噪点) + area = cv2.contourArea(cnt) + if area > 100: # 最小面积阈值(可根据实际调整) + # 计算轮廓的外接圆(红绿灯近似圆形) + (x, y), radius = cv2.minEnclosingCircle(cnt) + center = (int(x), int(y)) + radius = int(radius) + + # 圆度筛选(确保是近似圆形的灯芯) + perimeter = cv2.arcLength(cnt, True) + if perimeter > 0: + circularity = 4 * np.pi * area / (perimeter ** 2) + if circularity > 0.5: # 圆度阈值(0.5~1.0,值越大越圆) + if area > max_area: + max_area = area + detected_color = color + # 在图像上绘制灯芯位置 + cv2.circle(frame, center, radius, self.get_color_bgr(color), 2) + cv2.putText(frame, color, (center[0] - 20, center[1] - 20), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, self.get_color_bgr(color), 2) + + # 在图像顶部显示检测结果 + cv2.putText(frame, f"Traffic Light: {detected_color.upper()}", (20, 40), + cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 255, 255), 2) + return frame, detected_color + + def get_color_bgr(self, color): + """将颜色名称转换为 BGR 格式(OpenCV 绘图用)""" + color_map = { + 'red': (0, 0, 255), + 'yellow': (0, 255, 255), + 'green': (0, 255, 0), + 'none': (255, 255, 255) + } + return color_map[color] + + def detect_image(self, image_path): + """识别单张图片中的红绿灯""" + frame = cv2.imread(image_path) + if frame is None: + print(f"错误:无法读取图片 {image_path}") + return + + frame, hsv = self.preprocess_image(frame) + result_frame, color = self.detect_light(frame, hsv) + + print(f"检测结果:{color.upper()}") + # 显示结果 + cv2.imshow("Traffic Light Detection (Image)", result_frame) + cv2.waitKey(0) + cv2.destroyAllWindows() + + def detect_video(self, camera_index=0): + """实时摄像头识别红绿灯""" + cap = cv2.VideoCapture(camera_index) + if not cap.isOpened(): + print("错误:无法打开摄像头") + return + + print("实时检测中,按 'q' 退出...") + while True: + ret, frame = cap.read() + if not ret: + print("错误:无法读取摄像头画面") + break + + frame, hsv = self.preprocess_image(frame) + result_frame, color = self.detect_light(frame, hsv) + + # 显示结果 + cv2.imshow("Traffic Light Detection (Video)", result_frame) + + # 按 'q' 退出 + if cv2.waitKey(1) & 0xFF == ord('q'): + break + + cap.release() + cv2.destroyAllWindows() + + +if __name__ == "__main__": + detector = TrafficLightDetector() + + # 选择识别模式(二选一) + # 1. 识别单张图片(替换为你的红绿灯图片路径) + # detector.detect_image("traffic_light.jpg") + + # 2. 实时摄像头识别(默认使用电脑内置摄像头) + detector.detect_video() \ No newline at end of file diff --git a/src/driverless_car_yb/main.move.py b/src/driverless_car_yb/main.move.py new file mode 100644 index 0000000000..9b3e941502 --- /dev/null +++ b/src/driverless_car_yb/main.move.py @@ -0,0 +1,286 @@ +import pygame +import numpy as np +import cv2 +import tensorflow as tf +from tensorflow.keras import layers, models +import os +from datetime import datetime + +# ========================= 1. 配置参数 ========================= +# 窗口设置 +WINDOW_WIDTH = 800 +WINDOW_HEIGHT = 600 +# 无人车参数 +CAR_SIZE = 40 +CAR_SPEED = 5 +# 环境参数 +OBSTACLE_NUM = 5 +OBSTACLE_SIZE = 30 +# 模型参数 +MODEL_PATH = "car_control_model.h5" +IMAGE_SIZE = (64, 64) # 输入模型的图像尺寸 +ACTIONS = ["stop", "forward", "backward", "left", "right"] # 动作空间 +ACTION_TO_IDX = {a: i for i, a in enumerate(ACTIONS)} +IDX_TO_ACTION = {i: a for i, a in enumerate(ACTIONS)} +# 数据记录(用于训练) +DATA_DIR = "training_data" +os.makedirs(DATA_DIR, exist_ok=True) + +# ========================= 2. 初始化 Pygame 环境 ========================= +pygame.init() +screen = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT)) +pygame.display.set_caption("深度学习无人车 - 方向键控制") +clock = pygame.time.Clock() + +# 颜色定义 +WHITE = (255, 255, 255) +BLUE = (0, 0, 255) # 无人车 +BLACK = (0, 0, 0) # 文字/障碍物 +GREEN = (0, 255, 0) # 路径 +RED = (255, 0, 0) # 提示 + +# 字体设置 +font = pygame.font.Font(None, 32) + +# ========================= 3. 无人车与环境类 ========================= +class Car: + def __init__(self): + # 初始位置(屏幕中心) + self.x = WINDOW_WIDTH // 2 - CAR_SIZE // 2 + self.y = WINDOW_HEIGHT // 2 - CAR_SIZE // 2 + self.angle = 0 # 朝向(0=向上,90=向右,180=向下,270=向左) + + def draw(self, surface): + # 绘制车身(旋转的矩形) + rotated_car = pygame.transform.rotate( + pygame.Surface((CAR_SIZE, CAR_SIZE), pygame.SRCALPHA), + -self.angle # pygame 旋转方向与常规相反 + ) + rotated_car.fill(BLUE) + # 绘制朝向箭头 + arrow_points = [ + (CAR_SIZE//2, 5), + (CAR_SIZE//2 - 8, 20), + (CAR_SIZE//2 + 8, 20) + ] + pygame.draw.polygon(rotated_car, BLACK, arrow_points) + # 贴图到屏幕(修正旋转后的坐标偏移) + surface.blit( + rotated_car, + (self.x - rotated_car.get_width()//2, self.y - rotated_car.get_height()//2) + ) + + def move(self, action): + # 根据动作更新位置和朝向 + dx, dy = 0, 0 + if action == "forward": + dx = CAR_SPEED * np.sin(np.radians(self.angle)) + dy = -CAR_SPEED * np.cos(np.radians(self.angle)) + elif action == "backward": + dx = -CAR_SPEED * np.sin(np.radians(self.angle)) + dy = CAR_SPEED * np.cos(np.radians(self.angle)) + elif action == "left": + self.angle = (self.angle - 5) % 360 # 左转5度 + elif action == "right": + self.angle = (self.angle + 5) % 360 # 右转5度 + + # 边界碰撞检测 + new_x = self.x + dx + new_y = self.y + dy + if 0 <= new_x <= WINDOW_WIDTH - CAR_SIZE and 0 <= new_y <= WINDOW_HEIGHT - CAR_SIZE: + self.x = new_x + self.y = new_y + +class Obstacle: + def __init__(self): + # 随机生成障碍物(不与初始车位置重叠) + while True: + self.x = np.random.randint(0, WINDOW_WIDTH - OBSTACLE_SIZE) + self.y = np.random.randint(0, WINDOW_HEIGHT - OBSTACLE_SIZE) + if not ( + abs(self.x - (WINDOW_WIDTH//2)) < CAR_SIZE + OBSTACLE_SIZE and + abs(self.y - (WINDOW_HEIGHT//2)) < CAR_SIZE + OBSTACLE_SIZE + ): + break + + def draw(self, surface): + pygame.draw.rect(surface, BLACK, (self.x, self.y, OBSTACLE_SIZE, OBSTACLE_SIZE)) + +# ========================= 4. 深度学习模型(CNN) ========================= +def build_model(): + """构建用于无人车控制的CNN模型(输入:环境图像,输出:动作概率)""" + model = models.Sequential([ + # 输入层:64x64x3(RGB图像) + layers.Conv2D(32, (3, 3), activation='relu', input_shape=(IMAGE_SIZE[0], IMAGE_SIZE[1], 3)), + layers.MaxPooling2D((2, 2)), + layers.Conv2D(64, (3, 3), activation='relu'), + layers.MaxPooling2D((2, 2)), + layers.Conv2D(128, (3, 3), activation='relu'), + layers.MaxPooling2D((2, 2)), + layers.Flatten(), + layers.Dense(128, activation='relu'), + layers.Dropout(0.5), # 防止过拟合 + layers.Dense(len(ACTIONS), activation='softmax') # 输出动作概率 + ]) + model.compile( + optimizer='adam', + loss='sparse_categorical_crossentropy', + metrics=['accuracy'] + ) + return model + +# 加载或创建模型 +if os.path.exists(MODEL_PATH): + model = tf.keras.models.load_model(MODEL_PATH) + print(f"已加载现有模型:{MODEL_PATH}") +else: + model = build_model() + print("已创建新模型") + +# ========================= 5. 工具函数 ========================= +def get_environment_image(screen): + """获取当前环境图像(用于模型输入)""" + # 将Pygame屏幕转换为OpenCV图像(RGB→BGR,尺寸缩放) + screen_surf = pygame.surfarray.array3d(screen) + screen_surf = cv2.cvtColor(screen_surf, cv2.COLOR_RGB2BGR) + screen_surf = cv2.resize(screen_surf, IMAGE_SIZE) + # 归一化(0-255→0-1) + return screen_surf / 255.0 + +def record_training_data(image, action): + """记录训练数据(环境图像+对应的键盘动作)""" + timestamp = datetime.now().strftime("%Y%m%d%H%M%S%f") + action_idx = ACTION_TO_IDX[action] + # 保存图像 + img_path = os.path.join(DATA_DIR, f"{timestamp}_{action_idx}.png") + cv2.imwrite(img_path, image * 255) # 还原为0-255保存 + # 保存动作标签(单独文件,方便后续读取) + with open(os.path.join(DATA_DIR, "labels.txt"), "a") as f: + f.write(f"{img_path},{action_idx}\n") + +def train_model_from_data(): + """从记录的训练数据中训练模型""" + # 读取标签文件 + labels_path = os.path.join(DATA_DIR, "labels.txt") + if not os.path.exists(labels_path): + print("无训练数据,跳过训练") + return + + # 加载数据和标签 + X, y = [], [] + with open(labels_path, "r") as f: + for line in f.readlines(): + img_path, action_idx = line.strip().split(",") + if os.path.exists(img_path): + # 读取图像并预处理 + img = cv2.imread(img_path) + img = cv2.resize(img, IMAGE_SIZE) / 255.0 + X.append(img) + y.append(int(action_idx)) + + if len(X) < 10: + print("训练数据不足(至少需要10条)") + return + + # 转换为numpy数组 + X = np.array(X) + y = np.array(y) + + # 训练模型 + print(f"开始训练,样本数:{len(X)}") + model.fit( + X, y, + epochs=10, + batch_size=32, + validation_split=0.2 # 20%数据用于验证 + ) + + # 保存模型 + model.save(MODEL_PATH) + print(f"模型已保存到:{MODEL_PATH}") + +# ========================= 6. 主控制逻辑 ========================= +def main(): + car = Car() + obstacles = [Obstacle() for _ in range(OBSTACLE_NUM)] + mode = "manual" # 初始模式:手动控制(manual)/自动控制(auto) + current_action = "stop" + + running = True + while running: + # 1. 填充背景 + screen.fill(WHITE) + + # 2. 事件处理 + for event in pygame.event.get(): + if event.type == pygame.QUIT: + running = False + # 切换模式(M键) + if event.type == pygame.KEYDOWN: + if event.key == pygame.K_m: + mode = "auto" if mode == "manual" else "manual" + print(f"模式切换为:{mode}") + # 训练模型(T键) + if event.key == pygame.K_t: + print("开始训练模型...") + train_model_from_data() + # 退出(ESC键) + if event.key == pygame.K_ESCAPE: + running = False + + # 3. 控制逻辑(手动/自动) + if mode == "manual": + # 手动控制:方向键映射动作 + keys = pygame.key.get_pressed() + if keys[pygame.K_UP]: + current_action = "forward" + elif keys[pygame.K_DOWN]: + current_action = "backward" + elif keys[pygame.K_LEFT]: + current_action = "left" + elif keys[pygame.K_RIGHT]: + current_action = "right" + else: + current_action = "stop" + + # 记录训练数据(手动模式下自动记录) + env_img = get_environment_image(screen) + record_training_data(env_img, current_action) + + else: + # 自动控制:模型预测动作 + env_img = get_environment_image(screen) + # 模型输入:扩展为批量维度((64,64,3)→(1,64,64,3)) + env_img_batch = np.expand_dims(env_img, axis=0) + action_probs = model.predict(env_img_batch, verbose=0)[0] + current_action = IDX_TO_ACTION[np.argmax(action_probs)] # 选择概率最大的动作 + + # 4. 无人车移动 + car.move(current_action) + + # 5. 绘制元素(障碍物→无人车→提示文字) + for obs in obstacles: + obs.draw(screen) + car.draw(screen) + + # 6. 显示提示信息 + hint_texts = [ + f"模式:{mode}(按M切换)", + f"当前动作:{current_action}", + "手动控制:↑↓←→ 移动", + "按T训练模型 | 按ESC退出" + ] + for i, text in enumerate(hint_texts): + surf = font.render(text, True, BLACK if mode == "manual" else GREEN) + screen.blit(surf, (20, 20 + i*35)) + + # 7. 更新屏幕 + pygame.display.flip() + clock.tick(30) # 30帧/秒 + + # 退出程序 + pygame.quit() + print("程序退出") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/src/driverless_car_yb/main.ring.py b/src/driverless_car_yb/main.ring.py new file mode 100644 index 0000000000..24edde1166 --- /dev/null +++ b/src/driverless_car_yb/main.ring.py @@ -0,0 +1,252 @@ +import torch +import torch.nn as nn +import numpy as np +import pandas as pd +import matplotlib.pyplot as plt +from sklearn.preprocessing import MinMaxScaler +from datetime import datetime +import time + +# -------------------------- 1. 配置参数 -------------------------- +DEVICE = torch.device("cuda" if torch.cuda.is_available() else "cpu") # 自动选择GPU/CPU +SEQ_LENGTH = 10 # 输入序列长度(用前10个时刻速度预测下1个) +PREDICT_STEP = 1 # 预测步长(预测未来1个时刻) +BATCH_SIZE = 32 +EPOCHS = 50 +LEARNING_RATE = 0.001 +SPEED_THRESHOLD_MAX = 60.0 # 最大允许速度(km/h) +SPEED_THRESHOLD_MIN = 5.0 # 最小允许速度(km/h) +ERROR_THRESHOLD = 5.0 # 预测误差阈值(超过则认为异常) + + +# -------------------------- 2. 数据生成与预处理 -------------------------- +def generate_speed_data(n_samples=1000, noise=0.5): + """生成无人车速度时序数据(含正常行驶+随机异常)""" + time_steps = np.arange(n_samples) + # 正常速度:模拟加速-匀速-减速过程 + normal_speed = np.zeros(n_samples) + normal_speed[:200] = np.linspace(0, 50, 200) # 加速 + normal_speed[200:700] = 50 + np.random.normal(0, noise, 500) # 匀速(含噪声) + normal_speed[700:] = np.linspace(50, 10, 300) # 减速 + + # 添加异常数据(超速/低速) + abnormal_indices = np.random.choice(n_samples, size=50, replace=False) # 50个异常点 + for idx in abnormal_indices: + if np.random.random() > 0.5: + normal_speed[idx] = np.random.uniform(65, 80) # 超速 + else: + normal_speed[idx] = np.random.uniform(0, 3) # 低速异常 + + # 封装为DataFrame + data = pd.DataFrame({ + "time": pd.date_range(start="2025-01-01", periods=n_samples, freq="1s"), + "speed": normal_speed + }) + return data + + +def create_sequences(data, seq_len, predict_step): + """将时序数据转换为输入序列(X)和标签(y)""" + X, y = [], [] + for i in range(len(data) - seq_len - predict_step + 1): + seq = data[i:i + seq_len] # 输入序列:前seq_len个数据 + label = data[i + seq_len:i + seq_len + predict_step] # 标签:后predict_step个数据 + X.append(seq) + y.append(label) + return np.array(X), np.array(y) + + +# 生成并预处理数据 +raw_data = generate_speed_data(n_samples=1000) +scaler = MinMaxScaler(feature_range=(0, 1)) # 归一化 +scaled_speed = scaler.fit_transform(raw_data[["speed"]].values) + +# 划分训练集(80%)和测试集(20%) +train_size = int(0.8 * len(scaled_speed)) +train_data = scaled_speed[:train_size] +test_data = scaled_speed[train_size:] + +# 创建序列数据 +X_train, y_train = create_sequences(train_data, SEQ_LENGTH, PREDICT_STEP) +X_test, y_test = create_sequences(test_data, SEQ_LENGTH, PREDICT_STEP) + +# 转换为PyTorch张量([batch_size, seq_len, input_dim]) +X_train = torch.tensor(X_train, dtype=torch.float32).to(DEVICE) +y_train = torch.tensor(y_train, dtype=torch.float32).to(DEVICE) +X_test = torch.tensor(X_test, dtype=torch.float32).to(DEVICE) +y_test = torch.tensor(y_test, dtype=torch.float32).to(DEVICE) + +# 创建DataLoader +from torch.utils.data import TensorDataset, DataLoader + +train_dataset = TensorDataset(X_train, y_train) +test_dataset = TensorDataset(X_test, y_test) +train_loader = DataLoader(train_dataset, batch_size=BATCH_SIZE, shuffle=True) +test_loader = DataLoader(test_dataset, batch_size=BATCH_SIZE, shuffle=False) + + +# -------------------------- 3. LSTM预测模型 -------------------------- +class SpeedPredictionLSTM(nn.Module): + def __init__(self, input_dim=1, hidden_dim=64, num_layers=2, output_dim=1): + super(SpeedPredictionLSTM, self).__init__() + self.hidden_dim = hidden_dim + self.num_layers = num_layers + # LSTM层 + self.lstm = nn.LSTM( + input_dim, hidden_dim, num_layers, + batch_first=True, dropout=0.2 # batch_first=True:输入格式[batch, seq, dim] + ) + # 全连接层(输出预测值) + self.fc = nn.Linear(hidden_dim, output_dim) + + def forward(self, x): + # x: [batch_size, seq_len, input_dim] + lstm_out, (hidden, cell) = self.lstm(x) # lstm_out: [batch, seq_len, hidden_dim] + # 取最后一个时间步的输出作为全连接层输入 + out = self.fc(lstm_out[:, -1, :]) # [batch_size, output_dim] + return out + + +# 初始化模型 +model = SpeedPredictionLSTM(input_dim=1, hidden_dim=64, num_layers=2, output_dim=1).to(DEVICE) +criterion = nn.MSELoss() # 均方误差损失 +optimizer = torch.optim.Adam(model.parameters(), lr=LEARNING_RATE) # Adam优化器 + + +# -------------------------- 4. 模型训练 -------------------------- +def train_model(model, train_loader, criterion, optimizer, epochs): + model.train() + train_losses = [] + for epoch in range(epochs): + epoch_loss = 0.0 + for batch_x, batch_y in train_loader: + optimizer.zero_grad() # 梯度清零 + outputs = model(batch_x) # 前向传播 + loss = criterion(outputs, batch_y.squeeze()) # 计算损失 + loss.backward() # 反向传播 + optimizer.step() # 更新参数 + epoch_loss += loss.item() * batch_x.size(0) + epoch_loss /= len(train_loader.dataset) + train_losses.append(epoch_loss) + if (epoch + 1) % 10 == 0: + print(f"Epoch [{epoch + 1}/{epochs}], Train Loss: {epoch_loss:.6f}") + return train_losses + + +# 开始训练 +print("开始训练模型...") +train_losses = train_model(model, train_loader, criterion, optimizer, EPOCHS) + +# 绘制训练损失曲线 +plt.figure(figsize=(10, 4)) +plt.plot(train_losses, label="Train Loss") +plt.xlabel("Epoch") +plt.ylabel("MSE Loss") +plt.title("Model Training Loss") +plt.legend() +plt.show() + + +# -------------------------- 5. 速度预测与异常检测 -------------------------- +def predict_and_detect(model, data, seq_len, scaler): + """预测速度并检测异常""" + model.eval() + predictions = [] + anomalies = [] # 存储异常信息:(时间索引, 实际速度, 预测速度, 异常类型) + + with torch.no_grad(): + for i in range(len(data) - seq_len): + # 构建输入序列 + seq = data[i:i + seq_len].reshape(1, seq_len, 1) # [1, seq_len, 1] + seq_tensor = torch.tensor(seq, dtype=torch.float32).to(DEVICE) + + # 预测 + pred_scaled = model(seq_tensor) + pred = scaler.inverse_transform(pred_scaled.cpu().numpy())[0][0] # 反归一化 + predictions.append(pred) + + # 获取实际速度(预测步长为1,实际速度是序列后一个时刻) + actual = scaler.inverse_transform(data[i + seq_len].reshape(1, 1))[0][0] + time_idx = i + seq_len # 实际速度对应的时间索引 + + # 计算预测误差 + error = abs(actual - pred) + + # 异常检测逻辑 + if actual > SPEED_THRESHOLD_MAX: + anomalies.append((time_idx, actual, pred, "超速")) + elif actual < SPEED_THRESHOLD_MIN: + anomalies.append((time_idx, actual, pred, "低速")) + elif error > ERROR_THRESHOLD: + anomalies.append((time_idx, actual, pred, "预测异常(误差过大)")) + + return predictions, anomalies + + +# 对测试集进行预测和异常检测 +test_predictions, test_anomalies = predict_and_detect( + model, test_data, SEQ_LENGTH, scaler +) + +# 处理测试集实际速度(对齐预测结果长度) +test_actual = scaler.inverse_transform(test_data[SEQ_LENGTH:SEQ_LENGTH + len(test_predictions)]).flatten() + + +# -------------------------- 6. 报警模块 -------------------------- +def trigger_alarm(anomaly_info): + """触发报警(控制台模拟+时间记录)""" + time_idx, actual_speed, pred_speed, anomaly_type = anomaly_info + current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S") + alarm_msg = ( + f"\n【{current_time}】⚠️ 速度异常报警!" + f"\n- 时间索引:{time_idx}" + f"\n- 实际速度:{actual_speed:.2f} km/h" + f"\n- 预测速度:{pred_speed:.2f} km/h" + f"\n- 异常类型:{anomaly_type}" + f"\n----------------------------------------" + ) + print(alarm_msg) + + # 可选:调用硬件报警(如蜂鸣器、LED灯),需根据硬件接口修改 + # import RPi.GPIO as GPIO + # GPIO.setmode(GPIO.BCM) + # GPIO.setup(18, GPIO.OUT) + # GPIO.output(18, GPIO.HIGH) # 蜂鸣器响 + # time.sleep(0.5) + # GPIO.output(18, GPIO.LOW) + +# 输出所有异常报警 +print("\n========== 异常检测结果 ==========") +if test_anomalies: + for anomaly in test_anomalies: + trigger_alarm(anomaly) +else: + print("✅ 未检测到速度异常!") + + +# -------------------------- 7. 结果可视化 -------------------------- +def plot_results(test_actual, test_predictions, test_anomalies, seq_len): + """绘制实际速度、预测速度和异常点""" + plt.figure(figsize=(15, 6)) + # 绘制实际速度和预测速度 + plt.plot(test_actual, label="实际速度", color="blue", linewidth=1.5) + plt.plot(test_predictions, label="预测速度", color="orange", linewidth=1.5, linestyle="--") + # 绘制速度阈值线 + plt.axhline(y=SPEED_THRESHOLD_MAX, color="red", linestyle=":", label=f"最大阈值 {SPEED_THRESHOLD_MAX} km/h") + plt.axhline(y=SPEED_THRESHOLD_MIN, color="green", linestyle=":", label=f"最小阈值 {SPEED_THRESHOLD_MIN} km/h") + # 标记异常点 + if test_anomalies: + anomaly_indices = [a[0] - seq_len for a in test_anomalies] # 对齐测试集索引 + anomaly_speeds = [a[1] for a in test_anomalies] + plt.scatter(anomaly_indices, anomaly_speeds, color="red", s=50, label="异常点", zorder=5) + # 图表配置 + plt.xlabel("时间步(1步=1秒)") + plt.ylabel("速度(km/h)") + plt.title("无人车速度预测与异常检测结果") + plt.legend() + plt.grid(True, alpha=0.3) + plt.show() + + +# 绘制结果 +plot_results(test_actual, test_predictions, test_anomalies, SEQ_LENGTH) \ No newline at end of file diff --git a/src/driverless_car_yb/main.stopcar.py b/src/driverless_car_yb/main.stopcar.py new file mode 100644 index 0000000000..d1f39b74b5 --- /dev/null +++ b/src/driverless_car_yb/main.stopcar.py @@ -0,0 +1,182 @@ +import numpy as np +import matplotlib.pyplot as plt +from matplotlib.animation import FuncAnimation +import matplotlib.patches as patches + +# -------------------------- 1. 系统参数配置 -------------------------- +# 车辆参数(简化 Ackermann 模型) +CAR_LENGTH = 4.5 # 车长(m) +CAR_WIDTH = 1.8 # 车宽(m) +WHEELBASE = 2.8 # 轴距(m) +MAX_STEER = np.radians(30) # 最大转向角(弧度) + +# 泊车场景参数 +PARKING_SPOT_LENGTH = 5.0 # 车位长度(m) +PARKING_SPOT_WIDTH = 2.5 # 车位宽度(m) +PARKING_POS = np.array([10, 5]) # 车位中心坐标(x,y) +INIT_CAR_POS = np.array([2, 5]) # 车辆初始位置(x,y) +INIT_CAR_YAW = 0.0 # 车辆初始航向角(0=水平向右) + +# 控制参数 +STEP_TIME = 0.1 # 仿真步长(s) +MAX_ITER = 200 # 最大仿真步数(防止无限循环) +TARGET_POS_TOL = 0.3 # 位置误差容忍度(m) +TARGET_YAW_TOL = np.radians(5) # 航向角误差容忍度(弧度) + + +# -------------------------- 2. 路径规划(几何泊车路径) -------------------------- +def plan_parking_path(init_pos, init_yaw, target_pos, target_yaw): + """ + 规划自动泊车路径(两段圆弧+直线的组合路径) + :return: 路径点列表 [(x1,y1,yaw1), (x2,y2,yaw2), ...] + """ + path = [] + current_pos = init_pos.copy() + current_yaw = init_yaw + path.append((current_pos[0], current_pos[1], current_yaw)) + + # 阶段1:第一段圆弧(向右转向,靠近车位) + steer_angle = MAX_STEER * 0.8 # 转向角(小于最大转向角) + curve_radius = WHEELBASE / np.tan(steer_angle) # 转弯半径 + step_num = 30 # 该阶段步数 + for _ in range(step_num): + # 车辆运动学更新 + current_yaw += (np.tan(steer_angle) / WHEELBASE) * STEP_TIME * 10 # 角速度=v*tan(δ)/L(v=10m/s简化) + current_pos[0] += 10 * np.cos(current_yaw) * STEP_TIME + current_pos[1] += 10 * np.sin(current_yaw) * STEP_TIME + path.append((current_pos[0], current_pos[1], current_yaw)) + + # 阶段2:直线行驶(调整横向位置) + steer_angle = 0.0 # 回正方向盘 + step_num = 20 + for _ in range(step_num): + current_pos[0] += 10 * np.cos(current_yaw) * STEP_TIME + current_pos[1] += 10 * np.sin(current_yaw) * STEP_TIME + path.append((current_pos[0], current_pos[1], current_yaw)) + + # 阶段3:第二段圆弧(向左转向,对准车位) + steer_angle = -MAX_STEER * 0.8 # 反向转向 + step_num = 35 + for _ in range(step_num): + current_yaw += (np.tan(steer_angle) / WHEELBASE) * STEP_TIME * 10 + current_pos[0] += 10 * np.cos(current_yaw) * STEP_TIME + current_pos[1] += 10 * np.sin(current_yaw) * STEP_TIME + path.append((current_pos[0], current_pos[1], current_yaw)) + + # 阶段4:直线入库(精准停车) + steer_angle = 0.0 + step_num = 15 + for _ in range(step_num): + current_pos[0] += 5 * np.cos(current_yaw) * STEP_TIME # 减速 + current_pos[1] += 5 * np.sin(current_yaw) * STEP_TIME + path.append((current_pos[0], current_pos[1], current_yaw)) + + return path + + +# -------------------------- 3. 车辆绘制函数 -------------------------- +def draw_car(ax, x, y, yaw, color='blue'): + """在指定坐标轴绘制车辆(矩形+航向箭头)""" + # 车辆四个顶点(基于中心坐标和航向角旋转) + car_corners = np.array([ + [CAR_LENGTH / 2, CAR_WIDTH / 2], + [CAR_LENGTH / 2, -CAR_WIDTH / 2], + [-CAR_LENGTH / 2, -CAR_WIDTH / 2], + [-CAR_LENGTH / 2, CAR_WIDTH / 2] + ]) + # 旋转矩阵 + rot_mat = np.array([ + [np.cos(yaw), -np.sin(yaw)], + [np.sin(yaw), np.cos(yaw)] + ]) + rotated_corners = (rot_mat @ car_corners.T).T + np.array([x, y]) + + # 绘制车身 + ax.fill(rotated_corners[:, 0], rotated_corners[:, 1], color=color, alpha=0.7) + # 绘制航向箭头(车头方向) + arrow_len = CAR_LENGTH / 2 + ax.arrow(x, y, arrow_len * np.cos(yaw), arrow_len * np.sin(yaw), + head_width=0.3, head_length=0.5, fc='red', ec='red') + + +# -------------------------- 4. 动画主函数 -------------------------- +def main(): + # 1. 规划泊车路径 + target_yaw = np.radians(0) # 车位航向角(与初始方向一致) + parking_path = plan_parking_path(INIT_CAR_POS, INIT_CAR_YAW, PARKING_POS, target_yaw) + + # 2. 创建画布和坐标轴 + fig, ax = plt.subplots(figsize=(12, 6)) + ax.set_xlim(-1, 15) + ax.set_ylim(2, 8) + ax.set_xlabel('X Position (m)') + ax.set_ylabel('Y Position (m)') + ax.set_title('Autonomous Parking Simulation (Self-Driving Car)') + ax.grid(True) + ax.axis('equal') + + # 3. 绘制车位(灰色矩形) + parking_corner = PARKING_POS - np.array([PARKING_SPOT_LENGTH / 2, PARKING_SPOT_WIDTH / 2]) + parking_rect = patches.Rectangle(parking_corner, PARKING_SPOT_LENGTH, PARKING_SPOT_WIDTH, + linewidth=2, edgecolor='green', facecolor='lightgreen', alpha=0.5) + ax.add_patch(parking_rect) + + # 4. 初始化车辆图形对象 + car_plot = ax.fill([], [], color='blue', alpha=0.7)[0] + arrow_plot = ax.arrow(0, 0, 0, 0, head_width=0.3, head_length=0.5, fc='red', ec='red') + path_line, = ax.plot([], [], 'orange', linewidth=2, alpha=0.6, label='Parking Path') # 路径轨迹线 + + # 5. 动画更新函数 + def update(frame): + if frame >= len(parking_path): + frame = len(parking_path) - 1 # 最后一帧保持静止 + + # 获取当前帧的车辆状态 + x, y, yaw = parking_path[frame] + + # 更新车辆绘制 + car_corners = np.array([ + [CAR_LENGTH / 2, CAR_WIDTH / 2], + [CAR_LENGTH / 2, -CAR_WIDTH / 2], + [-CAR_LENGTH / 2, -CAR_WIDTH / 2], + [-CAR_LENGTH / 2, CAR_WIDTH / 2] + ]) + rot_mat = np.array([[np.cos(yaw), -np.sin(yaw)], [np.sin(yaw), np.cos(yaw)]]) + rotated_corners = (rot_mat @ car_corners.T).T + np.array([x, y]) + car_plot.set_xy(rotated_corners) + + # 更新航向箭头 + arrow_len = CAR_LENGTH / 2 + arrow_plot.set_data(x=x, y=y, dx=arrow_len * np.cos(yaw), dy=arrow_len * np.sin(yaw)) + + # 更新路径轨迹(绘制到当前帧) + path_x = [p[0] for p in parking_path[:frame + 1]] + path_y = [p[1] for p in parking_path[:frame + 1]] + path_line.set_data(path_x, path_y) + + # 显示当前状态信息 + ax.set_title(f'Autonomous Parking Simulation (Frame: {frame}/{len(parking_path) - 1})') + + return car_plot, arrow_plot, path_line + + # 6. 创建并运行动画 + ani = FuncAnimation( + fig, update, + frames=len(parking_path), + interval=STEP_TIME * 1000, # 与仿真步长同步(毫秒) + blit=True, + repeat=False # 只播放一次 + ) + + # 显示图例 + ax.legend(loc='upper right') + + # 保存动画(可选,需要安装 ffmpeg,或改为保存为GIF) + # ani.save('autonomous_parking.gif', writer='pillow', fps=10) + + # 显示动画 + plt.show() + + +if __name__ == '__main__': + main() \ No newline at end of file