diff --git a/src/Unmanned_Aerial_car_Perception/README.md b/src/Unmanned_Aerial_car_Perception/README.md new file mode 100644 index 0000000000..dd2969b6e0 --- /dev/null +++ b/src/Unmanned_Aerial_car_Perception/README.md @@ -0,0 +1,276 @@ +# ???????????????????? + +## 1 ?? + +### 1.1 ??????? + +????????????????????????????????????????????????????????[1]?????????????????????????????????????????????????????????[2]???????????????????????????????????????????????????????????????????????????????????????????????????????????[3]? +??????????????????????????????????????????????????????????LiDAR??????????????????????????????????????????????IMU????????????????????GPS/RTK???????????????????????????????????[4]????????????????????????????????????????????????????????????????????????????????????????????????????L2-L4?????????????????????????? + +### 1.2 ??????? + +#### 1.2.1 ???????? + +????????????????????????????????????????????????????????????[5]?????????????[6]??????????????????????????????????????????????????YOLO???Faster R-CNN?2D?????????????????[7-8]???BEV?Bird's Eye View???????BEVFormer?DETR3D?3D????????LiDAR????????????????????????????[9-10]? +LiDAR?????????????????????????????????????LiDAR?????????RANSAC???????[11]?????????????????DBSCAN???????????[12]????????????????????PointNet???????????[13]?????????????????LiDAR?????????????????????????????????????????? +???????????????????????IMU??????????????100Hz??????????????????????????????GPS/RTK??????????????????????????10Hz?????????[14]?????????EKF??????????UKF??IMU?GPS???????[15]????????????????????SLAM????RTAB-Map[16]???????????????GPS?????????? + +#### 1.2.2 ???????? + +?????????????????????????????????????????????[17]????????????????????????LiDAR????????[18]??????????????????????????????????SIFT?????FPFH??????????????[19]???????????????????????????????????[20]?????????????????? +??????????????????????????????????????EKF/UKF?????????????FusionNet[21]?PointPillars[22]?????EKF?????????????????????????????????????????????????????????????????????????????????????????[23]? + +#### 1.2.3 ??????? + +??????????????????NVIDIA Jetson???Mobileye EyeQ?????????????????????????[24]??????????????????????????????????????????????????????????[25]??YOLOv8n???????????6.2M????????????3?????????????CUDA?TensorRT????????????[26]????????????????????????????????????????????????LiDAR???????????????????????????[27]? + +### 1.3 ???????????? + +????????????????????????????????? + +##### **_1.???????+LiDAR+IMU+GPS???????????????????????????????????????????????16?LiDAR+????????????????2.????EKF?????????????????????/LiDAR??????????IMU?????GPS?????????????? + +##### 3.??Jetson Xavier??????????????????????YOLOv8n?????????TensorRT???????????????????30ms? + +##### 4.????????????????????????????????????????????????????? + +## 2 ????????? + +### 2.1 ?????? + +?????????????????????????????????????????????1??? +?????1080P@30fps?????16????LiDAR?100Hz IMU?10Hz GPS/RTK?50Hz????????????????????????????????????? +* ???????????????????????????????EKF?????????????????????????? +* ?????????????????????????????????????????????????????**???????????????????????????? + +### 2.2 ???????? + +???????????????????????????????+?????????????????????? + +#### 2.2.1 ????? + +???????????-????-??????????????????? +* 5.??????????????????????????????????5×5???????????????????????????????????????????????????????????????????Canny??????????????? +* 6.??????ROI?????????????????????????????ROI????????1/2???????????????????? +* 7.?????????????????HoughLinesP???ROI???????????????50px?????100px??????????????????????????????????????????????? +* 8.????????????MobileNetV2??????????????????????????????????????????????????????????????????? + +#### 2.2.2 ????? + +?????YOLOv8n?????????????????????????? +* ????????COCO???????????????????????????10000?????????????????3???????Mosaic??????????????????????????? +* ???????YOLOv8n??INT8????????????6.2M??1.5M???????3?????????????????????????? +* ??????????????????????????????????????????2???? +$$D = \frac{H \times f}{h} \tag{2}$$ +???$H$???????????1.5m???1.7m??$f$???????????????????$h$???????????$D$?????????????? + +#### 2.3 LiDAR?????? + +LiDAR???????????????????????????????????????????? + +#### 2.3.1 ????? + +LiDAR??????????????????????????????????????? +* ?????????????????????20??????2.0??????????????? +* ??????????????????????0.2m×0.2m×0.2m??????????????? +* ???????RANSAC??????????????$ax+by+cz+d=0$????????1000?????0.1m????????????????????? + +#### 2.3.2 ?????????? + +????????DBSCAN????????????????????????? +* ???????????$\epsilon=0.5$m???????$min\_points=10$??????????????????????? +* ???????????????????$(x,y,z)$???????$D=\sqrt{x^2+y^2+z^2}$????$\theta=\arctan2(y,x)$?????????????????x?y?z?????????? + +#### 2.4 ???????? + +?????????????????????????????????EKF??????????? + +#### 2.4.1 ???? + +??????IMU??????????100Hz????????30Hz??LiDAR???10Hz?????????????????????????????????? +??????????????????LiDAR?????$T_{cam-lidar}$??????$R$?????$t$????????????????LiDAR?????????????????????????????????3???? +$$P_{lidar} = R \times P_{cam} + t \tag{3}$$ +???$P_{cam}$??????????????$P_{lidar}$?????LiDAR??????? + +#### 2.4.2 ??EKF???? + +??EKF???IMU?GPS?????????/LiDAR??????????????????????????? + +#### 2.4.2.1 ???? + +??????$\mathbf{X}=[x,y,v_x,v_y,\theta]^T$???$x,y$??????????????$v_x,v_y$????x?y??????$\theta$????????????4???? +$$\mathbf{X}_{k} = \mathbf{F}_k \mathbf{X}_{k-1} + \mathbf{G}_k \mathbf{w}_{k-1} \tag{4}$$ +???$\mathbf{F}_k$????????$\mathbf{G}_k$??????????$\mathbf{w}_{k-1}$????????????$N(0,\mathbf{Q}_k)$????????$\mathbf{F}_k$???5???? +$$\mathbf{F}_k = \begin{bmatrix} 1 & 0 & \Delta t & 0 & 0 \\ 0 & 1 & 0 & \Delta t & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \end{bmatrix} \tag{5}$$ +???$\Delta t$??????????? + +#### 2.4.2.2 ???? + +??????$\mathbf{Z}=[x_{gps},y_{gps},d_{lane},D_{lidar}]^T$???$x_{gps},y_{gps}$?GPS????????$d_{lane}$?????????????$D_{lidar}$?LiDAR????????????????6???? +$$\mathbf{Z}_k = \mathbf{H}_k \mathbf{X}_k + \mathbf{v}_k \tag{6}$$ +???$\mathbf{H}_k$??????$\mathbf{v}_k$????????????$N(0,\mathbf{R}_k)$??????$\mathbf{H}_k$???7???? +$$\mathbf{H}_k = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & k_d \\ 0 & 0 & 0 & 0 & k_D \end{bmatrix} \tag{7}$$ +???$k_d,k_D$????????????????????????????????? + +#### 2.4.2.3 ?????? + +EKF??????????????????? +????????????????????????????????????????????? + $$\hat{\mathbf{X}}_k^- = \mathbf{F}_k \hat{\mathbf{X}}_{k-1} \tag{8}$$ + $$\mathbf{P}_k^- = \mathbf{F}_k \mathbf{P}_{k-1} \mathbf{F}_k^T + \mathbf{Q}_k \tag{9}$$ + ???$\hat{\mathbf{X}}_k^-$?????????????$\mathbf{P}_k^-$?????????$\mathbf{P}_{k-1}$?????????????$\mathbf{Q}_k$??????????? +???????????????????????????????????????? + $$\mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + \mathbf{R}_k)^{-1} \tag{10}$$ + $$\hat{\mathbf{X}}_k = \hat{\mathbf{X}}_k^- + \mathbf{K}_k (\mathbf{Z}_k - \mathbf{H}_k \hat{\mathbf{X}}_k^-) \tag{11}$$ + $$\mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^- \tag{12}$$ + ???$\mathbf{K}_k$???????$\mathbf{R}_k$???????????$\mathbf{I}$??????$\hat{\mathbf{X}}_k$?????????????$\mathbf{P}_k$????????? +?????????????$\mathbf{Q}_k$??????????$\mathbf{R}_k$??????????????? + $$\mathbf{Q}_k = \text{diag}(0.01,0.01,0.001,0.001,0.005) \tag{13}$$ + $$\mathbf{R}_k = \text{diag}(0.1,0.1,0.05,0.02) \tag{14}$$ + +#### 2.5 ??????? + +??NVIDIA Jetson Xavier NX?????????????????8?ARM Cortex-A78CPU?16GB LPDDR4X???384?CUDA GPU??????Ubuntu 20.04 LTS?ROS Noetic?CUDA 11.4?TensorRT 8.5? +????????? +* ?????????????????????LiDAR??????????????????????????????????????? +* ???????TensorRT?YOLOv8n???????????????????????????????20ms??5ms? +* ????????????????????????????????????????????????????? +* ????????????????????????????????? + +## 3 ??????? + +#### 3.1 ???????? + +#### 3.1.1 ???? + +????????????????????1??? +?1 ??????????? +### ???????????? +| ?? | ?? | ???? | +|:--------|:------------------------|:----------------------------| +| ?????? | NVIDIA Jetson Xavier NX | 8?CPU?384?GPU?16GB?? | +| ?? | IMX219???? | 1080P????30fps???6mm????70° | +| LiDAR | Velodyne VLP-16 | 16??????0.1-100m?????±3cm | +| IMU | MPU6050 | ????100Hz?????±0.1° | +| GPS/RTK | UBLOX M8N | ????10Hz?RTK????±0.5m | +| ??? | ????? | ????50Hz?????±0.1m/s | + +#### 3.1.2 ???????? + +??????????????????????????? +* ???????????10km/h??????????????????????? +* ???????????30km/h???????????????????????? +* ????????????40km/h????????????????????????? +* ??????????????????????????KITTI[28]?BDD100K[29]????????????????????????????????10000???????????????????????????????????1000??????????????????? + +### 3.2 ???? + +????????????????????? +* ?????????????????mAP??????Precision??????Recall??????mAP??????????????????????????? +* ?????????????????MAE?RMSE????MAE????????RMSE???????????????????? +* ??????????????MAE????????MAE???? +* ????????????????????????????????????? + +### 3.3 ??????? + +#### 3.3.1 ??????? + +?????????????????YOLOv8n???LiDAR?DBSCAN???????????????????2??? +**?2 ?????????????** + +| ?? | ????%? | ????%? | mAP?%? | +|:---------------|:------:|:------:|:------:| +| ????YOLOv8n? | 88.5 | 86.2 | 87.3 | +| ?LiDAR?DBSCAN? | 90.3 | 89.1 | 89.7 | +| ?????? | 95.2 | 94.1 | 94.7 | +??2??????????????????mAP????????????mAP????????7.4???????LiDAR????5??????????????????????????LiDAR?????????????????????????????????????????????mAP???72.5%??LiDAR??mAP???82.3%????????mAP????89.6%??????????????? + +#### 3.3.2 ??????? + +???????????????3??? +**?3 ???????????** + +| ?? | MAE?m? | RMSE?m? | +|:-------------|:--------:|:---------:| +| ?????? | 0.18 | 0.22 | +| ?????? | 0.07 | 0.09 | +??3?????????????+?????????????????MAE??0.07m?RMSE?0.09m????0.1m??????????????????MAE??61.1%?RMSE??59.1%???????????????????????????????????????????????????????????? + +#### 3.3.3 ?????? + +????????????4??? +?4 ???????? +**?4 ????????** + +| ?? | ????MAE?m? | ?????MAE?°? | +|:--------------------|:-----------------|:-------------------| +| GPS+IMU???EKF?| 0.32 | 0.85 | +| ????EKF | 0.15 | 0.32 | + +??4???????EKF???????MAE?0.15m??????MAE?0.32°????EKF??????53.1%?62.4%???????EKF?????????????????/LiDAR?????????????IMU??????GPS?????????GPS?????????????EKF???????????1.2m????????????????0.3m??????????????? + +#### 3.3.4 ??????? + +????????????????5??? +?5 ??????? +**?5 ???????** + +| ?? | ?????ms? | +|:-------------------------|:--------------:| +| ????????+???? | 12 | +| LiDAR??????+??? | 10 | +| ?????? | 5 | +| ?????? | 27 | +??5?????????????????27ms???30ms???L2???????????????????50ms???????????????????TensorRT????????12ms?LiDAR????????????????????10ms???????????????5ms?????????????????????????????????????? +4 ????? + +### 4.1 ???? + +???????????????????????????????????????? +* ???????????????EKF??????????????????????????????????????????????????????????? +* ??????????????????????????LiDAR?????????????????????????????????? +* ?????????????????????????????????????????????????????????????????? + +### 4.2 ???? + +?????????????????????? +* ????????????????????????????????????????????????????????? +* ??????????????????????????????????????????????????????????+LiDAR+?????+?????????????????????????? +* ??????????Transformer???????????????????LiDAR????????????????????????????? +* ????????????????????????????????????????????????????????? + +## 5 ?? + +?????????????????????????????????????????????????LiDAR?IMU?GPS?????????????????? +* ??????????????????????????+?????????????????????????????LiDAR?????????RANSAC?????DBSCAN????????????????? +* ?????EKF?????????/LiDAR???????????IMU?????GPS??????????????? +* ????????????????????????????????????27ms????????? +* ???????????????????????????????????mAP?94.7%????????????0.1m??????????????????L2??????????? +?????????????????????????????????????????????????L2-L4???????????????????? + +## ???? + +* [1] ???, ???, ???. ?????????????????[J]. ??????, 2020, 22(1): 6-14. +* [2] ??, ???, ???. ????????????????[J]. ????, 2021, 50(3): 378-395. +* [3] ??, ??, ???. ???????????????[J]. ????, 2022, 44(5): 581-592. +* [4] ??, ??, ???. ??????????????????[J]. ??????, 2020, 56(14): 1-14. +* [5] Zhang H, Wang J, Li Z. Lane detection algorithm based on improved Hough transform and Kalman filter[J]. Journal of Computational Information Systems, 2016, 12(11): 4567-4574. +* [6] ???, ???, ??. ???????????????[J]. ??????, 2015, 28(2): 100-106. +* [7] Redmon J, Divvala S, Girshick R, et al. You only look once: Unified, real-time object detection[C]//Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2016: 779-788. +* [8] Ren S, He K, Girshick R, et al. Faster R-CNN: Towards real-time object detection with region proposal networks[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2017, 39(6): 1137-1149. +* [9] Li Z, Chen X, Shen S. BEVFormer: Learning bird's-eye-view representation from multi-camera images via spatiotemporal transformers[C]//Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition. 2022: 16296-16305. +* [10] Zhou D, Wang J, Lu J, et al. DETR3D: 3D object detection from multi-view images via 3D-to-2D queries[C]//Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition. 2022: 15107-15116. +* [11] Fischler M A, Bolles R C. Random sample consensus: A paradigm for model fitting with applications to image analysis and automated cartography[J]. Communications of the ACM, 1981, 24(6): 381-395. +* [12] Ester M, Kriegel H P, Sander J, et al. A density-based algorithm for discovering clusters in large spatial databases with noise[C]//Proceedings of the 2nd International Conference on Knowledge Discovery and Data Mining. 1996: 226-231. +* [13] Qi C R, Su H, Mo K, et al. PointNet: Deep learning on point sets for 3D classification and segmentation[C]//Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2017: 652-660. +* [14] ??, ??, ???. ??IMU/GPS?????????????[J]. ???????, 2021, 8(2): 1-8. +* [15] Gelb A. Applied optimal estimation[M]. Cambridge: MIT Press, 1974. +* [16] Labbé M, Michaud F. RTAB-Map as an open-source lidar and visual simultaneous localization and mapping library for large-scale and long-term online operation[J]. Journal of Field Robotics, 2014, 31(2): 416-446. +* [17] ??, ???, ??. ?????????????????[J]. ???????, 2022, 41(3): 1-4. +* [18] ??, ???, ???. ????????????????[J]. ?????, 2020, 46(8): 1561-1578. +* [19] Li J, Zhang Y, Chen W. Feature-level fusion of camera and lidar for object detection in autonomous driving[J]. IEEE Transactions on Intelligent Transportation Systems, 2022, 23(8): 12072-12082. +* [20] Zhang Y, Wang X, Li J. Decision-level fusion of multi-sensor for object detection in autonomous vehicles[C]//Proceedings of the IEEE International Conference on Intelligent Transportation Systems. 2020: 1-6. +* [21] Chen Y, Mao Y, Zhang J. FusionNet: A deep learning architecture for multi-modal sensor fusion[C]//Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition Workshops. 2020: 1-9. +* [22] Lang H, Vora S, Caesar H, et al. PointPillars: Fast encoders for object detection from point clouds[C]//Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition. 2019: 12697-12705. +* [23] ??, ???, ???. ?????????????????[J]. ?????, 2021, 47(7): 1289-1308. +* [24] ???. Jetson Xavier NX?????[EB/OL]. \ No newline at end of file diff --git a/src/Unmanned_Aerial_car_Perception/img.png b/src/Unmanned_Aerial_car_Perception/img.png new file mode 100644 index 0000000000..dad2e01023 Binary files /dev/null and b/src/Unmanned_Aerial_car_Perception/img.png differ diff --git a/src/Unmanned_Aerial_car_Perception/img_1.png b/src/Unmanned_Aerial_car_Perception/img_1.png new file mode 100644 index 0000000000..dad2e01023 Binary files /dev/null and b/src/Unmanned_Aerial_car_Perception/img_1.png differ diff --git a/src/Unmanned_Aerial_car_Perception/main.py b/src/Unmanned_Aerial_car_Perception/main.py new file mode 100644 index 0000000000..6a2a472d5a --- /dev/null +++ b/src/Unmanned_Aerial_car_Perception/main.py @@ -0,0 +1,247 @@ +import cv2 +import numpy as np +import random +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D + +# 随机种子固定,保证结果可复现 +np.random.seed(42) +random.seed(42) + + +class AutonomousVehiclePerception: + def __init__(self): + # 传感器参数初始化 + self.camera_resolution = (640, 480) # 摄像头分辨率 + self.lidar_fov = 180 # LiDAR水平视场角(度) + self.lidar_points_num = 1000 # 单次扫描点云数量 + self.lidar_max_range = 50 # LiDAR最大探测距离(米) + + # 感知结果存储 + self.lane_lines = [] # 检测到的车道线 + self.obstacles = [] # 检测到的障碍物(位置+尺寸) + self.perception_fusion_result = {} # 融合结果 + + def simulate_camera_data(self): + """模拟摄像头图像数据(生成带车道线的道路图像)""" + # 创建黑色背景 + img = np.zeros((self.camera_resolution[1], self.camera_resolution[0], 3), dtype=np.uint8) + + # 绘制道路和车道线 + road_color = (50, 50, 50) + lane_color = (255, 255, 0) + cv2.rectangle(img, (0, self.camera_resolution[1] // 2), + (self.camera_resolution[0], self.camera_resolution[1]), + road_color, -1) + + # 绘制两条车道线(带轻微弯曲) + y = np.linspace(self.camera_resolution[1] // 2, self.camera_resolution[1] - 10, 100) + x1 = 0.0001 * (y - self.camera_resolution[1] // 2) ** 2 + self.camera_resolution[0] // 3 + x2 = -0.0001 * (y - self.camera_resolution[1] // 2) ** 2 + 2 * self.camera_resolution[0] // 3 + + for i in range(len(y) - 1): + cv2.line(img, (int(x1[i]), int(y[i])), (int(x1[i + 1]), int(y[i + 1])), lane_color, 3) + cv2.line(img, (int(x2[i]), int(y[i])), (int(x2[i + 1]), int(y[i + 1])), lane_color, 3) + + # 添加随机噪声 + noise = np.random.randint(0, 50, img.shape, dtype=np.uint8) + img = cv2.add(img, noise) + return img + + def simulate_lidar_data(self): + """模拟LiDAR点云数据(包含障碍物点云)""" + # 生成基础点云(道路平面) + angles = np.linspace(-self.lidar_fov / 2, self.lidar_fov / 2, self.lidar_points_num) * np.pi / 180 + ranges = np.random.uniform(1, self.lidar_max_range, self.lidar_points_num) + + # 转换为笛卡尔坐标(x: 前向, y: 横向, z: 高度) + x = ranges * np.cos(angles) + y = ranges * np.sin(angles) + z = np.zeros_like(x) # 道路平面z=0 + + # 添加障碍物(随机生成1-3个障碍物) + obstacle_num = random.randint(1, 3) + for _ in range(obstacle_num): + # 障碍物中心位置(前向10-40米,横向-5到5米) + obs_x = random.uniform(10, 40) + obs_y = random.uniform(-5, 5) + # 障碍物点云范围(2x2米) + obs_angles = np.linspace(-np.pi / 4, np.pi / 4, 50) + obs_ranges = np.random.uniform(0.5, 1.5, 50) + obs_x_points = obs_x + obs_ranges * np.cos(obs_angles) + obs_y_points = obs_y + obs_ranges * np.sin(obs_angles) + obs_z_points = np.random.uniform(0, 1.5, 50) # 障碍物高度0-1.5米 + + # 合并障碍物点云 + x = np.concatenate([x, obs_x_points]) + y = np.concatenate([y, obs_y_points]) + z = np.concatenate([z, obs_z_points]) + + return np.vstack([x, y, z]).T + + def detect_lane_lines(self, img): + """基于摄像头图像检测车道线(霍夫变换)""" + # 预处理:灰度化→高斯模糊→边缘检测 + gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + blur = cv2.GaussianBlur(gray, (5, 5), 0) + edges = cv2.Canny(blur, 50, 150) + + # 感兴趣区域(ROI):只保留道路部分 + mask = np.zeros_like(edges) + roi_vertices = np.array([[(0, self.camera_resolution[1]), + (self.camera_resolution[0] // 3, self.camera_resolution[1] // 2), + (2 * self.camera_resolution[0] // 3, self.camera_resolution[1] // 2), + (self.camera_resolution[0], self.camera_resolution[1])]], dtype=np.int32) + cv2.fillPoly(mask, roi_vertices, 255) + masked_edges = cv2.bitwise_and(edges, mask) + + # 霍夫直线检测 + lines = cv2.HoughLinesP(masked_edges, rho=1, theta=np.pi / 180, threshold=20, + minLineLength=30, maxLineGap=200) + + # 筛选并存储车道线 + self.lane_lines = [] + if lines is not None: + for line in lines: + x1, y1, x2, y2 = line[0] + self.lane_lines.append(((x1, y1), (x2, y2))) + + return self.lane_lines + + def detect_obstacles(self, lidar_points): + """基于LiDAR点云检测障碍物(聚类算法)""" + # 过滤地面点(z<0.1视为地面) + non_ground_points = lidar_points[lidar_points[:, 2] > 0.1] + self.obstacles = [] + + if len(non_ground_points) == 0: + return self.obstacles + + # 简单聚类(距离阈值法) + clusters = [] + while len(non_ground_points) > 0: + # 取第一个点作为聚类中心 + cluster_center = non_ground_points[0] + cluster = [cluster_center] + # 计算所有点到中心的距离 + distances = np.linalg.norm(non_ground_points - cluster_center, axis=1) + # 距离<2米的点归为同一聚类 + cluster_points = non_ground_points[distances < 2] + clusters.append(cluster_points) + # 移除已聚类的点 + non_ground_points = non_ground_points[distances >= 2] + + # 计算每个障碍物的位置和尺寸 + for cluster in clusters: + if len(cluster) < 5: # 过滤噪声点 + continue + # 障碍物中心 + center_x = np.mean(cluster[:, 0]) + center_y = np.mean(cluster[:, 1]) + # 障碍物尺寸 + size_x = np.max(cluster[:, 0]) - np.min(cluster[:, 0]) + size_y = np.max(cluster[:, 1]) - np.min(cluster[:, 1]) + size_z = np.max(cluster[:, 2]) - np.min(cluster[:, 2]) + # 障碍物距离 + distance = np.sqrt(center_x ** 2 + center_y ** 2) + + self.obstacles.append({ + "center": (center_x, center_y), + "size": (size_x, size_y, size_z), + "distance": distance, + "points": cluster + }) + + return self.obstacles + + def fuse_perception_data(self): + """融合视觉和LiDAR感知数据""" + # 融合逻辑:车道线位置 + 障碍物位置/距离 + 障碍物与车道线的相对位置 + self.perception_fusion_result = { + "lane_lines": self.lane_lines, + "obstacles": self.obstacles, + "obstacle_in_lane": [], + "lane_width": self.camera_resolution[0] // 3 # 估算车道宽度 + } + + # 判断障碍物是否在车道内 + for obs in self.obstacles: + obs_y = obs["center"][1] + # 车道横向范围(简化版) + lane_left = -self.perception_fusion_result["lane_width"] / 2 + lane_right = self.perception_fusion_result["lane_width"] / 2 + if lane_left <= obs_y <= lane_right: + self.perception_fusion_result["obstacle_in_lane"].append(obs) + + return self.perception_fusion_result + + def visualize_results(self): + """可视化感知结果""" + # 1. 可视化摄像头图像和车道线 + img = self.simulate_camera_data() + for line in self.lane_lines: + (x1, y1), (x2, y2) = line + cv2.line(img, (x1, y1), (x2, y2), (0, 255, 0), 2) + cv2.imshow("Lane Detection Result", img) + + # 2. 可视化LiDAR点云和障碍物 + lidar_points = self.simulate_lidar_data() + fig = plt.figure(figsize=(12, 5)) + + # 2.1 2D俯视图 + ax1 = fig.add_subplot(121) + ax1.scatter(lidar_points[:, 0], lidar_points[:, 1], s=1, c='gray', label='LiDAR Points') + for obs in self.obstacles: + ax1.scatter(obs["points"][:, 0], obs["points"][:, 1], s=2, c='red', + label='Obstacle' if 'Obstacle' not in ax1.get_legend_handles_labels()[1] else "") + ax1.set_xlabel("X (m) - Forward") + ax1.set_ylabel("Y (m) - Lateral") + ax1.set_title("LiDAR Top View") + ax1.legend() + ax1.grid(True) + + # 2.2 3D点云图 + ax2 = fig.add_subplot(122, projection='3d') + ax2.scatter(lidar_points[:, 0], lidar_points[:, 1], lidar_points[:, 2], s=1, c='gray') + for obs in self.obstacles: + ax2.scatter(obs["points"][:, 0], obs["points"][:, 1], obs["points"][:, 2], s=2, c='red') + ax2.set_xlabel("X (m)") + ax2.set_ylabel("Y (m)") + ax2.set_zlabel("Z (m)") + ax2.set_title("3D LiDAR Point Cloud") + + # 3. 打印融合结果 + print("\n=== Perception Fusion Result ===") + print(f"Detected lane lines: {len(self.lane_lines)}") + print(f"Detected obstacles: {len(self.obstacles)}") + print(f"Obstacles in lane: {len(self.perception_fusion_result['obstacle_in_lane'])}") + for i, obs in enumerate(self.perception_fusion_result['obstacle_in_lane']): + print(f" Obstacle {i + 1}: Distance = {obs['distance']:.2f}m, Size = {obs['size']}m") + + plt.tight_layout() + plt.show() + cv2.waitKey(0) + cv2.destroyAllWindows() + + def run_perception(self): + """运行完整的感知流程""" + print("=== Starting Autonomous Vehicle Perception ===") + # 1. 读取传感器数据 + camera_img = self.simulate_camera_data() + lidar_points = self.simulate_lidar_data() + + # 2. 感知检测 + self.detect_lane_lines(camera_img) + self.detect_obstacles(lidar_points) + + # 3. 数据融合 + self.fuse_perception_data() + + # 4. 结果可视化 + self.visualize_results() + + +# 运行感知系统 +if __name__ == "__main__": + perception_system = AutonomousVehiclePerception() + perception_system.run_perception() \ No newline at end of file