From 61a46c7dc37a36eb654ed6dd13de60001951878e Mon Sep 17 00:00:00 2001 From: lej627 <3097482133@qq.com> Date: Thu, 18 Dec 2025 18:01:40 +0800 Subject: [PATCH 1/8] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E8=AE=BA=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Unmanned_Aerial_Vehicle_Perception/README.md | 9 +++++++++ src/Unmanned_Aerial_Vehicle_Perception/main.py | 0 2 files changed, 9 insertions(+) create mode 100644 src/Unmanned_Aerial_Vehicle_Perception/README.md create mode 100644 src/Unmanned_Aerial_Vehicle_Perception/main.py diff --git a/src/Unmanned_Aerial_Vehicle_Perception/README.md b/src/Unmanned_Aerial_Vehicle_Perception/README.md new file mode 100644 index 0000000000..4c2cc530cd --- /dev/null +++ b/src/Unmanned_Aerial_Vehicle_Perception/README.md @@ -0,0 +1,9 @@ +无人车感知是自动驾驶系统的基础,相当于车辆的 “眼睛” 和 “耳朵”,核心是通过各类传感器采集环境数据,再借助算法处理信息,最终实现环境理解与自身定位,为后续决策和控制提供依据,以下是其关键组成部分的详细介绍: +核心感知硬件 +不同传感器特性互补,共同构建起多维度的感知体系,具体信息如下: +传感器类型 工作原理 核心优势 典型应用场景 +激光雷达 发射激光脉冲并接收反射信号,生成 3D 点云图 厘米级测距精度,可构建三维环境模型,360° 全方位扫描 高精地图绘制、复杂障碍物识别、精准定位 +车载摄像头 采集二维图像,通过图像算法做模式识别 还原接近人眼的环境细节,可识别交通标志、车道线等 交通信号灯识别、车道线检测、行人及车辆分类 +毫米波雷达 发射 10 - 300GHz 的电磁波,基于多普勒效应测距测速 抗雾、雨、尘能力强,全天候工作,测距远 自适应巡航、碰撞预警、盲区探测、自动紧急制动 +超声波雷达 发射 40kHz 超声波,通过时间差计算距离 近距离测距精度高(误差 1 - 3 厘米),成本低 倒车辅助、自动泊车等低速近距离障碍物探测 +GNSS/IMU 系统 GNSS 提供卫星定位,IMU 测量加速度和角速度 GNSS \ No newline at end of file diff --git a/src/Unmanned_Aerial_Vehicle_Perception/main.py b/src/Unmanned_Aerial_Vehicle_Perception/main.py new file mode 100644 index 0000000000..e69de29bb2 From 9d3ecab4530819dfe08acc182db7e0dc07df21e9 Mon Sep 17 00:00:00 2001 From: lej627 <3097482133@qq.com> Date: Thu, 18 Dec 2025 18:37:10 +0800 Subject: [PATCH 2/8] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E8=AE=BA=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 50 +++++++++++++++---- 1 file changed, 41 insertions(+), 9 deletions(-) diff --git a/src/Unmanned_Aerial_Vehicle_Perception/README.md b/src/Unmanned_Aerial_Vehicle_Perception/README.md index 4c2cc530cd..857385dd29 100644 --- a/src/Unmanned_Aerial_Vehicle_Perception/README.md +++ b/src/Unmanned_Aerial_Vehicle_Perception/README.md @@ -1,9 +1,41 @@ -无人车感知是自动驾驶系统的基础,相当于车辆的 “眼睛” 和 “耳朵”,核心是通过各类传感器采集环境数据,再借助算法处理信息,最终实现环境理解与自身定位,为后续决策和控制提供依据,以下是其关键组成部分的详细介绍: -核心感知硬件 -不同传感器特性互补,共同构建起多维度的感知体系,具体信息如下: -传感器类型 工作原理 核心优势 典型应用场景 -激光雷达 发射激光脉冲并接收反射信号,生成 3D 点云图 厘米级测距精度,可构建三维环境模型,360° 全方位扫描 高精地图绘制、复杂障碍物识别、精准定位 -车载摄像头 采集二维图像,通过图像算法做模式识别 还原接近人眼的环境细节,可识别交通标志、车道线等 交通信号灯识别、车道线检测、行人及车辆分类 -毫米波雷达 发射 10 - 300GHz 的电磁波,基于多普勒效应测距测速 抗雾、雨、尘能力强,全天候工作,测距远 自适应巡航、碰撞预警、盲区探测、自动紧急制动 -超声波雷达 发射 40kHz 超声波,通过时间差计算距离 近距离测距精度高(误差 1 - 3 厘米),成本低 倒车辅助、自动泊车等低速近距离障碍物探测 -GNSS/IMU 系统 GNSS 提供卫星定位,IMU 测量加速度和角速度 GNSS \ No newline at end of file +一、无人机感知的核心定义​ +无人机感知是指无人机通过搭载的各类传感器,获取自身状态、周围环境及目标对象的信息,并进行处理、分析与理解的过程,是无人机实现自主飞行、任务执行的 “眼睛” 和 “大脑”,核心目标是解决 “我在哪、周围有什么、要做什么” 三大问题。​ +二、核心感知技术分类​ +1. 环境感知技术​ +视觉感知:基于摄像头(单目 / 双目 / 多目)捕捉图像,通过计算机视觉算法(如目标检测、语义分割、SLAM)识别障碍物、地形、交通标识等,优势是成本低、信息丰富,适用于低空近距离场景。​ +激光雷达感知:发射激光束扫描环境,通过接收反射信号计算距离,生成高精度点云地图,具备抗强光、抗雾尘能力,测量精度达厘米级,是室外复杂环境下的核心感知方案。​ +毫米波雷达感知:利用毫米波频段电磁波探测目标,可穿透雨、雾、雪等恶劣天气,能同时获取目标距离、速度、角度信息,适用于高速移动目标检测和全天候飞行场景。​ +红外热成像感知:捕捉目标红外辐射,转化为热成像图,不受光照条件限制,可在黑夜、浓烟环境中识别活体、热源目标,常用于搜救、安防监控。​ +2. 自身状态感知技术​ +惯性导航(IMU):通过陀螺仪、加速度计实时测量无人机角速度和加速度,推算位置、速度和姿态,响应速度快,但存在累积误差,需与其他技术融合。​ +卫星导航(GNSS):包括 GPS、北斗、GLONASS 等,通过接收卫星信号定位,户外定位精度高(普通场景米级,RTK 技术可达厘米级),是无人机远程飞行的基础。​ +气压高度计:测量大气压力计算飞行高度,成本低、功耗小,用于辅助高度控制,弥补卫星导航在遮挡场景下的不足。​ +三、关键感知组件​ +​ +组件类型​ +典型设备​ +核心作用​ +图像传感器​ +高清摄像头、鱼眼相机​ +视觉图像采集、场景还原​ +距离传感器​ +激光雷达、超声波传感器​ +障碍物距离测量、地形测绘​ +定位传感器​ +GNSS 模块、IMU 惯性测量单元​ +位置定位、姿态控制、速度计算​ +数据处理单元​ +嵌入式芯片(FPGA/MCU)​ +传感器数据融合、算法实时运算​ +​ +四、主要应用场景​ +测绘勘探:通过激光雷达 + 视觉感知生成三维地形模型,应用于国土测绘、矿山勘探、道路规划。​ +安防监控:红外 + 视觉感知实现全天候目标跟踪、异常行为检测,用于城市安防、边境巡逻。​ +农业植保:视觉 + 毫米波雷达感知作物长势、识别病虫害,辅助精准施肥、农药喷洒。​ +物流配送:多传感器融合感知规避障碍物、定位配送点,实现末端无人配送。​ +应急救援:红外感知在地震、火灾现场探测被困人员,视觉感知传回现场画面,辅助救援决策。​ +五、技术发展趋势​ +多传感器融合:结合视觉、激光雷达、毫米波雷达优势,提升复杂环境下感知的可靠性和鲁棒性。​ +AI 赋能感知:通过深度学习算法优化目标识别、语义分割精度,实现端到端智能感知。​ +轻量化与低功耗:传感器和处理单元小型化、低功耗设计,适配微型无人机应用。​ +抗干扰能力强化:优化传感器抗电磁干扰、抗恶劣天气性能,拓展极端环境适用范围。​ From bb89af0113d15b0cda3c5a9719590d34b1e05276 Mon Sep 17 00:00:00 2001 From: lej627 <3097482133@qq.com> Date: Thu, 18 Dec 2025 18:45:07 +0800 Subject: [PATCH 3/8] =?UTF-8?q?=E6=8F=90=E4=BA=A4=E8=AE=BA=E9=A2=98?= =?UTF-8?q?=EF=BC=8C=E6=A0=BC=E5=BC=8F=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 58 +++++++++++-------- 1 file changed, 34 insertions(+), 24 deletions(-) diff --git a/src/Unmanned_Aerial_Vehicle_Perception/README.md b/src/Unmanned_Aerial_Vehicle_Perception/README.md index 857385dd29..dfb4a34275 100644 --- a/src/Unmanned_Aerial_Vehicle_Perception/README.md +++ b/src/Unmanned_Aerial_Vehicle_Perception/README.md @@ -1,34 +1,44 @@ -一、无人机感知的核心定义​ -无人机感知是指无人机通过搭载的各类传感器,获取自身状态、周围环境及目标对象的信息,并进行处理、分析与理解的过程,是无人机实现自主飞行、任务执行的 “眼睛” 和 “大脑”,核心目标是解决 “我在哪、周围有什么、要做什么” 三大问题。​ -二、核心感知技术分类​ -1. 环境感知技术​ +# **一、无人机感知的核心定义​** + +### 无人机感知是指无人机通过搭载的各类传感器,获取自身状态、周围环境及目标对象的信息,并进行处理、分析与理解的过程,是无人机实现自主飞行、任务执行的 “眼睛” 和 “大脑”,核心目标是解决 “我在哪、周围有什么、要做什么” 三大问题。​ + +## 二、核心感知技术分类​ + +#### 1. 环境感知技术​ + 视觉感知:基于摄像头(单目 / 双目 / 多目)捕捉图像,通过计算机视觉算法(如目标检测、语义分割、SLAM)识别障碍物、地形、交通标识等,优势是成本低、信息丰富,适用于低空近距离场景。​ 激光雷达感知:发射激光束扫描环境,通过接收反射信号计算距离,生成高精度点云地图,具备抗强光、抗雾尘能力,测量精度达厘米级,是室外复杂环境下的核心感知方案。​ 毫米波雷达感知:利用毫米波频段电磁波探测目标,可穿透雨、雾、雪等恶劣天气,能同时获取目标距离、速度、角度信息,适用于高速移动目标检测和全天候飞行场景。​ 红外热成像感知:捕捉目标红外辐射,转化为热成像图,不受光照条件限制,可在黑夜、浓烟环境中识别活体、热源目标,常用于搜救、安防监控。​ -2. 自身状态感知技术​ + +### 2. 自身状态感知技术​ + 惯性导航(IMU):通过陀螺仪、加速度计实时测量无人机角速度和加速度,推算位置、速度和姿态,响应速度快,但存在累积误差,需与其他技术融合。​ 卫星导航(GNSS):包括 GPS、北斗、GLONASS 等,通过接收卫星信号定位,户外定位精度高(普通场景米级,RTK 技术可达厘米级),是无人机远程飞行的基础。​ 气压高度计:测量大气压力计算飞行高度,成本低、功耗小,用于辅助高度控制,弥补卫星导航在遮挡场景下的不足。​ -三、关键感知组件​ -​ -组件类型​ -典型设备​ -核心作用​ -图像传感器​ -高清摄像头、鱼眼相机​ -视觉图像采集、场景还原​ -距离传感器​ -激光雷达、超声波传感器​ -障碍物距离测量、地形测绘​ -定位传感器​ -GNSS 模块、IMU 惯性测量单元​ -位置定位、姿态控制、速度计算​ -数据处理单元​ -嵌入式芯片(FPGA/MCU)​ -传感器数据融合、算法实时运算​ -​ -四、主要应用场景​ + +### 三、关键感知组件​ + +* ​ +* 组件类型​ +* 典型设备​ +* 核心作用​ +* 图像传感器​ +* 高清摄像头、鱼眼相机​ +* 视觉图像采集、场景还原​ +* 距离传感器​ +* 激光雷达、超声波传感器​ +* 障碍物距离测量、地形测绘​ +* 定位传感器​ +* GNSS 模块、IMU 惯性测量单元​ +* 位置定位、姿态控制、速度计算​ +* 数据处理单元​ +* 嵌入式芯片(FPGA/MCU)​ +* 传感器数据融合、算法实时运算​ +* ​ + +### 四、主要应用场景​ + 测绘勘探:通过激光雷达 + 视觉感知生成三维地形模型,应用于国土测绘、矿山勘探、道路规划。​ 安防监控:红外 + 视觉感知实现全天候目标跟踪、异常行为检测,用于城市安防、边境巡逻。​ 农业植保:视觉 + 毫米波雷达感知作物长势、识别病虫害,辅助精准施肥、农药喷洒。​ From a530fc4874d1314a11b6816e38fa1ff3ef2e6a48 Mon Sep 17 00:00:00 2001 From: lej627 <3097482133@qq.com> Date: Fri, 19 Dec 2025 16:12:13 +0800 Subject: [PATCH 4/8] =?UTF-8?q?=E5=B0=86readme=E5=86=85=E5=AE=B9=E4=BF=AE?= =?UTF-8?q?=E6=94=B9=E5=B9=B6=E5=AE=8C=E5=96=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 51 --- .../main.py | 0 src/Unmanned_Aerial_car_Perception/README.md | 276 ++++++++++++++ src/Unmanned_Aerial_car_Perception/img.png | Bin 0 -> 6029 bytes src/Unmanned_Aerial_car_Perception/img_1.png | Bin 0 -> 6029 bytes src/Unmanned_Aerial_car_Perception/main.py | 337 ++++++++++++++++++ 6 files changed, 613 insertions(+), 51 deletions(-) delete mode 100644 src/Unmanned_Aerial_Vehicle_Perception/README.md delete mode 100644 src/Unmanned_Aerial_Vehicle_Perception/main.py create mode 100644 src/Unmanned_Aerial_car_Perception/README.md create mode 100644 src/Unmanned_Aerial_car_Perception/img.png create mode 100644 src/Unmanned_Aerial_car_Perception/img_1.png create mode 100644 src/Unmanned_Aerial_car_Perception/main.py diff --git a/src/Unmanned_Aerial_Vehicle_Perception/README.md b/src/Unmanned_Aerial_Vehicle_Perception/README.md deleted file mode 100644 index dfb4a34275..0000000000 --- a/src/Unmanned_Aerial_Vehicle_Perception/README.md +++ /dev/null @@ -1,51 +0,0 @@ -# **一、无人机感知的核心定义​** - -### 无人机感知是指无人机通过搭载的各类传感器,获取自身状态、周围环境及目标对象的信息,并进行处理、分析与理解的过程,是无人机实现自主飞行、任务执行的 “眼睛” 和 “大脑”,核心目标是解决 “我在哪、周围有什么、要做什么” 三大问题。​ - -## 二、核心感知技术分类​ - -#### 1. 环境感知技术​ - -视觉感知:基于摄像头(单目 / 双目 / 多目)捕捉图像,通过计算机视觉算法(如目标检测、语义分割、SLAM)识别障碍物、地形、交通标识等,优势是成本低、信息丰富,适用于低空近距离场景。​ -激光雷达感知:发射激光束扫描环境,通过接收反射信号计算距离,生成高精度点云地图,具备抗强光、抗雾尘能力,测量精度达厘米级,是室外复杂环境下的核心感知方案。​ -毫米波雷达感知:利用毫米波频段电磁波探测目标,可穿透雨、雾、雪等恶劣天气,能同时获取目标距离、速度、角度信息,适用于高速移动目标检测和全天候飞行场景。​ -红外热成像感知:捕捉目标红外辐射,转化为热成像图,不受光照条件限制,可在黑夜、浓烟环境中识别活体、热源目标,常用于搜救、安防监控。​ - -### 2. 自身状态感知技术​ - -惯性导航(IMU):通过陀螺仪、加速度计实时测量无人机角速度和加速度,推算位置、速度和姿态,响应速度快,但存在累积误差,需与其他技术融合。​ -卫星导航(GNSS):包括 GPS、北斗、GLONASS 等,通过接收卫星信号定位,户外定位精度高(普通场景米级,RTK 技术可达厘米级),是无人机远程飞行的基础。​ -气压高度计:测量大气压力计算飞行高度,成本低、功耗小,用于辅助高度控制,弥补卫星导航在遮挡场景下的不足。​ - -### 三、关键感知组件​ - -* ​ -* 组件类型​ -* 典型设备​ -* 核心作用​ -* 图像传感器​ -* 高清摄像头、鱼眼相机​ -* 视觉图像采集、场景还原​ -* 距离传感器​ -* 激光雷达、超声波传感器​ -* 障碍物距离测量、地形测绘​ -* 定位传感器​ -* GNSS 模块、IMU 惯性测量单元​ -* 位置定位、姿态控制、速度计算​ -* 数据处理单元​ -* 嵌入式芯片(FPGA/MCU)​ -* 传感器数据融合、算法实时运算​ -* ​ - -### 四、主要应用场景​ - -测绘勘探:通过激光雷达 + 视觉感知生成三维地形模型,应用于国土测绘、矿山勘探、道路规划。​ -安防监控:红外 + 视觉感知实现全天候目标跟踪、异常行为检测,用于城市安防、边境巡逻。​ -农业植保:视觉 + 毫米波雷达感知作物长势、识别病虫害,辅助精准施肥、农药喷洒。​ -物流配送:多传感器融合感知规避障碍物、定位配送点,实现末端无人配送。​ -应急救援:红外感知在地震、火灾现场探测被困人员,视觉感知传回现场画面,辅助救援决策。​ -五、技术发展趋势​ -多传感器融合:结合视觉、激光雷达、毫米波雷达优势,提升复杂环境下感知的可靠性和鲁棒性。​ -AI 赋能感知:通过深度学习算法优化目标识别、语义分割精度,实现端到端智能感知。​ -轻量化与低功耗:传感器和处理单元小型化、低功耗设计,适配微型无人机应用。​ -抗干扰能力强化:优化传感器抗电磁干扰、抗恶劣天气性能,拓展极端环境适用范围。​ diff --git a/src/Unmanned_Aerial_Vehicle_Perception/main.py b/src/Unmanned_Aerial_Vehicle_Perception/main.py deleted file mode 100644 index e69de29bb2..0000000000 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 0000000000000000000000000000000000000000..dad2e0102345f671428a0635eab7a52179ddb334 GIT binary patch literal 6029 zcmeAS@N?(olHy`uVBq!ia0y~yU;$!wGY%%8$huIG9tH;S5KkA!kczlB2N@ZGf(;Y? zt$)V>(l81}Ltr!nMnhmU1V%$(Gz3ONU^E0qLtr!nMnhmU1V%$(Gz5lq2sGSob_R_Q zFgX5?uRc7iy*lc-(GVC7fzc2c4S~@RpjrqV@Nep6U_AIDvlL{cr>mdKI;Vst0IDq+ Ag#Z8m literal 0 HcmV?d00001 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 0000000000000000000000000000000000000000..dad2e0102345f671428a0635eab7a52179ddb334 GIT binary patch literal 6029 zcmeAS@N?(olHy`uVBq!ia0y~yU;$!wGY%%8$huIG9tH;S5KkA!kczlB2N@ZGf(;Y? zt$)V>(l81}Ltr!nMnhmU1V%$(Gz3ONU^E0qLtr!nMnhmU1V%$(Gz5lq2sGSob_R_Q zFgX5?uRc7iy*lc-(GVC7fzc2c4S~@RpjrqV@Nep6U_AIDvlL{cr>mdKI;Vst0IDq+ Ag#Z8m literal 0 HcmV?d00001 diff --git a/src/Unmanned_Aerial_car_Perception/main.py b/src/Unmanned_Aerial_car_Perception/main.py new file mode 100644 index 0000000000..1d9a1b019c --- /dev/null +++ b/src/Unmanned_Aerial_car_Perception/main.py @@ -0,0 +1,337 @@ +import cv2 +import numpy as np +import time +import math +import open3d as o3d # LiDAR点云处理 +from threading import Thread +import serial # 串口读取轮速/IMU(适配Arduino/车载MCU) + +# 适配YOLOv5(需提前下载YOLOv5权重文件) +from ultralytics import YOLO + + +class AutonomousCarPerception: + def __init__(self): + # 1. 初始化硬件接口 + self.lidar = None # LiDAR对象(示例用Open3D模拟,实际替换为Velodyne/Ouster SDK) + self.camera = cv2.VideoCapture(0) # 车载摄像头(0为默认摄像头,可替换为视频流/CSI摄像头) + self.serial_port = serial.Serial("/dev/ttyUSB0", 9600, timeout=0.1) # 串口读取IMU/轮速 + self.yolo_model = YOLO("yolov5s.pt") # 轻量化YOLOv5模型 + + # 2. 感知数据缓存 + self.perception_data = { + # 自身状态 + "speed": 0.0, # 车速 (m/s) + "heading": 0.0, # 航向角 (°,正北为0,顺时针) + "position": {"lat": 0.0, "lon": 0.0}, # GPS位置 + "imu": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0}, # IMU姿态 + # 视觉感知 + "lane": {"detected": False, "center_offset": 0.0}, # 车道线偏移 (m) + "visual_obstacle": [], # 视觉检测障碍物 [{"class": "car", "distance": 5.0, "x": 1.2}] + # LiDAR感知 + "lidar_obstacle": [], # LiDAR检测障碍物 [{"distance": 4.8, "width": 1.5, "angle": 5.0}] + # 融合结果 + "fused_obstacle": [], # 融合后障碍物 + "timestamp": time.time() + } + + # 3. 线程控制 + self.running = True + self.vision_thread = Thread(target=self._vision_loop) + self.lidar_thread = Thread(target=self._lidar_loop) + self.state_thread = Thread(target=self._state_loop) + self.fusion_thread = Thread(target=self._fusion_loop) + + # ------------------- 1. 自身状态感知 ------------------- + def _read_state_sensors(self): + """读取轮速计、IMU、GPS数据(串口/ROS话题)""" + try: + # 示例:从串口读取MCU发送的状态数据(格式:speed,heading,roll,pitch,yaw,lat,lon) + if self.serial_port.in_waiting > 0: + data = self.serial_port.readline().decode().strip().split(",") + if len(data) == 7: + self.perception_data["speed"] = float(data[0]) + self.perception_data["heading"] = float(data[1]) + self.perception_data["imu"]["roll"] = float(data[2]) + self.perception_data["imu"]["pitch"] = float(data[3]) + self.perception_data["imu"]["yaw"] = float(data[4]) + self.perception_data["position"]["lat"] = float(data[5]) + self.perception_data["position"]["lon"] = float(data[6]) + except Exception as e: + print(f"状态传感器读取失败: {e}") + + def _state_loop(self): + """状态感知循环(20Hz)""" + while self.running: + self._read_state_sensors() + self.perception_data["timestamp"] = time.time() + time.sleep(0.05) + + # ------------------- 2. 视觉感知 ------------------- + def _detect_lane(self, frame): + """车道线检测(霍夫变换)""" + # 预处理:灰度化 → 高斯模糊 → 边缘检测 → 感兴趣区域(ROI)裁剪 + height, width = frame.shape[:2] + gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + blur = cv2.GaussianBlur(gray, (5, 5), 0) + edges = cv2.Canny(blur, 50, 150) + + # 裁剪ROI(只检测画面下半部分的车道线) + roi_vertices = np.array([[(0, height), (width / 2, height / 2), (width, height)]], dtype=np.int32) + mask = np.zeros_like(edges) + cv2.fillPoly(mask, roi_vertices, 255) + roi_edges = cv2.bitwise_and(edges, mask) + + # 霍夫直线检测 + lines = cv2.HoughLinesP(roi_edges, 1, np.pi / 180, threshold=50, minLineLength=50, maxLineGap=100) + lane_detected = False + center_offset = 0.0 # 车道中心偏移(单位:米,正值偏右,负值偏左) + + if lines is not None: + lane_detected = True + # 分离左右车道线 + left_lines = [] + right_lines = [] + for line in lines: + x1, y1, x2, y2 = line[0] + slope = (y2 - y1) / (x2 - x1 + 1e-6) # 避免除零 + if slope < -0.5: # 左车道线(负斜率) + left_lines.append(line) + elif slope > 0.5: # 右车道线(正斜率) + right_lines.append(line) + + # 拟合左右车道线并计算中心偏移 + if left_lines and right_lines: + # 左车道线拟合 + left_x = [line[0][0] for line in left_lines] + [line[0][2] for line in left_lines] + left_y = [line[0][1] for line in left_lines] + [line[0][3] for line in left_lines] + left_fit = np.polyfit(left_y, left_x, 1) + left_x_base = int(left_fit[0] * height + left_fit[1]) + + # 右车道线拟合 + right_x = [line[0][0] for line in right_lines] + [line[0][2] for line in right_lines] + right_y = [line[0][1] for line in right_lines] + [line[0][3] for line in right_lines] + right_fit = np.polyfit(right_y, right_x, 1) + right_x_base = int(right_fit[0] * height + right_fit[1]) + + # 计算车道中心与画面中心的偏移(像素→米,需标定) + lane_center = (left_x_base + right_x_base) / 2 + frame_center = width / 2 + pixel2meter = 0.01 # 1像素=0.01米(需实际标定) + center_offset = (lane_center - frame_center) * pixel2meter + + # 绘制车道线 + cv2.line(frame, (left_x_base, height), (int(left_fit[0] * height / 2 + left_fit[1]), int(height / 2)), + (0, 255, 0), 2) + cv2.line(frame, (right_x_base, height), + (int(right_fit[0] * height / 2 + right_fit[1]), int(height / 2)), (0, 255, 0), 2) + cv2.line(frame, (int(lane_center), height), (int(frame_center), int(height / 2)), (0, 0, 255), 2) + + # 更新车道线数据 + self.perception_data["lane"]["detected"] = lane_detected + self.perception_data["lane"]["center_offset"] = center_offset + return frame + + def _detect_visual_obstacle(self, frame): + """YOLOv5检测视觉障碍物(车辆/行人/障碍物)""" + # 推理(过滤小目标,只保留car/person/bicycle) + results = self.yolo_model(frame, conf=0.5, classes=[0, 1, 2]) + obstacles = [] + + for result in results: + boxes = result.boxes + for box in boxes: + # 获取检测框信息 + x1, y1, x2, y2 = map(int, box.xyxy[0]) + cls = self.yolo_model.names[int(box.cls[0])] + conf = float(box.conf[0]) + + # 简易距离估算(基于检测框高度,需标定) + box_height = y2 - y1 + distance = 500 / box_height # 示例公式:高度越小,距离越远 + + # 绘制检测框 + cv2.rectangle(frame, (x1, y1), (x2, y2), (255, 0, 0), 2) + cv2.putText(frame, f"{cls}: {distance:.1f}m", (x1, y1 - 10), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 0, 0), 2) + + obstacles.append({ + "class": cls, + "distance": distance, + "x": (x1 + x2) / 2 - frame.shape[1] / 2, # 横向偏移(像素) + "confidence": conf + }) + + self.perception_data["visual_obstacle"] = obstacles + return frame + + def _vision_loop(self): + """视觉感知循环(10Hz)""" + while self.running: + ret, frame = self.camera.read() + if ret: + # 1. 车道线检测 + frame = self._detect_lane(frame) + # 2. 视觉障碍物检测 + frame = self._detect_visual_obstacle(frame) + # 显示画面 + cv2.putText(frame, f"Lane Offset: {self.perception_data['lane']['center_offset']:.2f}m", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 255, 0), 2) + cv2.imshow("Car Perception (Vision)", frame) + if cv2.waitKey(1) & 0xFF == ord('q'): + self.stop() + time.sleep(0.1) + + # ------------------- 3. LiDAR感知 ------------------- + def _process_lidar_pointcloud(self): + """LiDAR点云处理(聚类检测障碍物)""" + try: + # 示例:生成模拟点云(实际替换为LiDAR SDK读取) + points = np.random.rand(1000, 3) * 20 - 10 # [-10,10]米范围 + points = points[points[:, 2] > -1] # 过滤地面以下点 + + # Open3D点云处理 + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points) + # 下采样 + 去除离群点 + pcd = pcd.voxel_down_sample(voxel_size=0.2) + cl, ind = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0) + pcd = pcd.select_by_index(ind) + + # 聚类检测障碍物(DBSCAN) + with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Error): + labels = np.array(pcd.cluster_dbscan(eps=0.5, min_points=10)) + + obstacles = [] + max_label = labels.max() + if max_label >= 0: + for label in range(max_label + 1): + cluster_points = np.asarray(pcd.points)[labels == label] + # 计算障碍物中心、距离、尺寸 + center = np.mean(cluster_points, axis=0) + distance = np.linalg.norm(center) # 距车辆的距离 + angle = math.degrees(math.atan2(center[1], center[0])) # 方位角 + width = np.max(cluster_points[:, 0]) - np.min(cluster_points[:, 0]) + length = np.max(cluster_points[:, 1]) - np.min(cluster_points[:, 1]) + + obstacles.append({ + "distance": distance, + "angle": angle, + "width": width, + "length": length + }) + + self.perception_data["lidar_obstacle"] = obstacles + except Exception as e: + print(f"LiDAR处理失败: {e}") + + def _lidar_loop(self): + """LiDAR感知循环(5Hz)""" + while self.running: + self._process_lidar_pointcloud() + time.sleep(0.2) + + # ------------------- 4. 多传感器融合 ------------------- + def _fusion_obstacle(self): + """融合视觉+LiDAR障碍物数据(加权融合)""" + visual_obstacles = self.perception_data["visual_obstacle"] + lidar_obstacles = self.perception_data["lidar_obstacle"] + fused_obstacles = [] + + # 简易融合规则:LiDAR距离优先,视觉补充类别 + for lidar_obs in lidar_obstacles: + # 匹配视觉障碍物(方位角±5°内) + matched_visual = None + for visual_obs in visual_obstacles: + # 视觉方位角估算(基于横向偏移) + visual_angle = math.degrees(math.atan2(visual_obs["x"] * 0.01, visual_obs["distance"])) + if abs(visual_angle - lidar_obs["angle"]) < 5: + matched_visual = visual_obs + break + + fused_obstacles.append({ + "distance": lidar_obs["distance"], # LiDAR距离更精准 + "angle": lidar_obs["angle"], + "width": lidar_obs["width"], + "class": matched_visual["class"] if matched_visual else "unknown", + "confidence": matched_visual["confidence"] if matched_visual else 0.8 + }) + + self.perception_data["fused_obstacle"] = fused_obstacles + + def _fusion_loop(self): + """数据融合循环(5Hz)""" + while self.running: + self._fusion_obstacle() + time.sleep(0.2) + + # ------------------- 5. 安全判断与接口 ------------------- + def check_safety(self): + """安全状态判断""" + # 1. 碰撞风险:融合障碍物距离<3米 + for obs in self.perception_data["fused_obstacle"]: + if obs["distance"] < 3.0: + return False, f"前方{obs['distance']:.1f}米检测到{obs['class']},碰撞风险!" + # 2. 车道偏离风险:偏移>0.5米 + if abs(self.perception_data["lane"]["center_offset"]) > 0.5 and self.perception_data["lane"]["detected"]: + return False, f"车道偏移{self.perception_data['lane']['center_offset']:.2f}米,偏离风险!" + # 3. 车速风险:超速(>10m/s=36km/h) + if self.perception_data["speed"] > 10.0: + return False, f"车速{self.perception_data['speed']:.1f}m/s,超速!" + return True, "安全" + + def get_perception_result(self): + """获取最新感知结果""" + return self.perception_data + + def start(self): + """启动感知系统""" + print("启动无人车感知系统...") + self.vision_thread.start() + self.lidar_thread.start() + self.state_thread.start() + self.fusion_thread.start() + + def stop(self): + """停止感知系统""" + self.running = False + # 等待线程结束 + self.vision_thread.join() + self.lidar_thread.join() + self.state_thread.join() + self.fusion_thread.join() + # 释放资源 + self.camera.release() + self.serial_port.close() + cv2.destroyAllWindows() + print("无人车感知系统已停止") + + +# ------------------- 测试代码 ------------------- +if __name__ == "__main__": + # 初始化感知系统 + car_perception = AutonomousCarPerception() + + try: + # 启动感知 + car_perception.start() + + # 主循环打印感知结果 + while True: + data = car_perception.get_perception_result() + is_safe, msg = car_perception.check_safety() + + print(f"\n=== 无人车感知数据 [{time.ctime()}] ===") + print(f"车速: {data['speed']:.2f}m/s, 航向角: {data['heading']:.1f}°") + print( + f"车道线: {'检测到' if data['lane']['detected'] else '未检测到'}, 偏移: {data['lane']['center_offset']:.2f}m") + print(f"视觉障碍物数量: {len(data['visual_obstacle'])}") + print(f"LiDAR障碍物数量: {len(data['lidar_obstacle'])}") + print(f"融合障碍物数量: {len(data['fused_obstacle'])}") + print(f"安全状态: {'✅ 安全' if is_safe else '❌ 危险'} - {msg}") + + time.sleep(1) + + except KeyboardInterrupt: + car_perception.stop() + print("程序已手动终止") From 7b94e519291b0c24d1278cc3fb3f75410d51e961 Mon Sep 17 00:00:00 2001 From: lej627 <3097482133@qq.com> Date: Fri, 19 Dec 2025 18:10:44 +0800 Subject: [PATCH 5/8] =?UTF-8?q?=E8=BF=90=E8=A1=8C=E4=BB=A3=E7=A0=81?= =?UTF-8?q?=E7=94=9F=E6=88=90=E5=9B=BE=E5=83=8F=E5=BC=B9=E5=87=BA=E8=BD=A6?= =?UTF-8?q?=E9=81=93=E7=BA=BF=E6=A3=80=E6=B5=8B=E7=9A=84=E5=9B=BE=E5=83=8F?= =?UTF-8?q?=E7=AA=97=E5=8F=A3=E5=92=8C=20LiDAR=20=E7=82=B9=E4=BA=91?= =?UTF-8?q?=E5=8F=AF=E8=A7=86=E5=8C=96=E7=AA=97=E5=8F=A3=EF=BC=8C=E5=B9=B6?= =?UTF-8?q?=E5=9C=A8=E6=8E=A7=E5=88=B6=E5=8F=B0=E6=89=93=E5=8D=B0=E8=9E=8D?= =?UTF-8?q?=E5=90=88=E5=90=8E=E7=9A=84=E6=84=9F=E7=9F=A5=E7=BB=93=E6=9E=9C?= =?UTF-8?q?=EF=BC=88=E5=A6=82=E9=9A=9C=E7=A2=8D=E7=89=A9=E6=95=B0=E9=87=8F?= =?UTF-8?q?=E3=80=81=E8=B7=9D=E7=A6=BB=E3=80=81=E6=98=AF=E5=90=A6=E5=9C=A8?= =?UTF-8?q?=E8=BD=A6=E9=81=93=E5=86=85=E7=AD=89=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Unmanned_Aerial_car_Perception/main.py | 538 +++++++++------------ 1 file changed, 224 insertions(+), 314 deletions(-) diff --git a/src/Unmanned_Aerial_car_Perception/main.py b/src/Unmanned_Aerial_car_Perception/main.py index 1d9a1b019c..6a2a472d5a 100644 --- a/src/Unmanned_Aerial_car_Perception/main.py +++ b/src/Unmanned_Aerial_car_Perception/main.py @@ -1,337 +1,247 @@ import cv2 import numpy as np -import time -import math -import open3d as o3d # LiDAR点云处理 -from threading import Thread -import serial # 串口读取轮速/IMU(适配Arduino/车载MCU) +import random +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D -# 适配YOLOv5(需提前下载YOLOv5权重文件) -from ultralytics import YOLO +# 随机种子固定,保证结果可复现 +np.random.seed(42) +random.seed(42) -class AutonomousCarPerception: +class AutonomousVehiclePerception: def __init__(self): - # 1. 初始化硬件接口 - self.lidar = None # LiDAR对象(示例用Open3D模拟,实际替换为Velodyne/Ouster SDK) - self.camera = cv2.VideoCapture(0) # 车载摄像头(0为默认摄像头,可替换为视频流/CSI摄像头) - self.serial_port = serial.Serial("/dev/ttyUSB0", 9600, timeout=0.1) # 串口读取IMU/轮速 - self.yolo_model = YOLO("yolov5s.pt") # 轻量化YOLOv5模型 - - # 2. 感知数据缓存 - self.perception_data = { - # 自身状态 - "speed": 0.0, # 车速 (m/s) - "heading": 0.0, # 航向角 (°,正北为0,顺时针) - "position": {"lat": 0.0, "lon": 0.0}, # GPS位置 - "imu": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0}, # IMU姿态 - # 视觉感知 - "lane": {"detected": False, "center_offset": 0.0}, # 车道线偏移 (m) - "visual_obstacle": [], # 视觉检测障碍物 [{"class": "car", "distance": 5.0, "x": 1.2}] - # LiDAR感知 - "lidar_obstacle": [], # LiDAR检测障碍物 [{"distance": 4.8, "width": 1.5, "angle": 5.0}] - # 融合结果 - "fused_obstacle": [], # 融合后障碍物 - "timestamp": time.time() - } - - # 3. 线程控制 - self.running = True - self.vision_thread = Thread(target=self._vision_loop) - self.lidar_thread = Thread(target=self._lidar_loop) - self.state_thread = Thread(target=self._state_loop) - self.fusion_thread = Thread(target=self._fusion_loop) - - # ------------------- 1. 自身状态感知 ------------------- - def _read_state_sensors(self): - """读取轮速计、IMU、GPS数据(串口/ROS话题)""" - try: - # 示例:从串口读取MCU发送的状态数据(格式:speed,heading,roll,pitch,yaw,lat,lon) - if self.serial_port.in_waiting > 0: - data = self.serial_port.readline().decode().strip().split(",") - if len(data) == 7: - self.perception_data["speed"] = float(data[0]) - self.perception_data["heading"] = float(data[1]) - self.perception_data["imu"]["roll"] = float(data[2]) - self.perception_data["imu"]["pitch"] = float(data[3]) - self.perception_data["imu"]["yaw"] = float(data[4]) - self.perception_data["position"]["lat"] = float(data[5]) - self.perception_data["position"]["lon"] = float(data[6]) - except Exception as e: - print(f"状态传感器读取失败: {e}") - - def _state_loop(self): - """状态感知循环(20Hz)""" - while self.running: - self._read_state_sensors() - self.perception_data["timestamp"] = time.time() - time.sleep(0.05) - - # ------------------- 2. 视觉感知 ------------------- - def _detect_lane(self, frame): - """车道线检测(霍夫变换)""" - # 预处理:灰度化 → 高斯模糊 → 边缘检测 → 感兴趣区域(ROI)裁剪 - height, width = frame.shape[:2] - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + # 传感器参数初始化 + 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(只检测画面下半部分的车道线) - roi_vertices = np.array([[(0, height), (width / 2, height / 2), (width, height)]], dtype=np.int32) + # 感兴趣区域(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) - roi_edges = cv2.bitwise_and(edges, mask) + masked_edges = cv2.bitwise_and(edges, mask) # 霍夫直线检测 - lines = cv2.HoughLinesP(roi_edges, 1, np.pi / 180, threshold=50, minLineLength=50, maxLineGap=100) - lane_detected = False - center_offset = 0.0 # 车道中心偏移(单位:米,正值偏右,负值偏左) + 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: - lane_detected = True - # 分离左右车道线 - left_lines = [] - right_lines = [] for line in lines: x1, y1, x2, y2 = line[0] - slope = (y2 - y1) / (x2 - x1 + 1e-6) # 避免除零 - if slope < -0.5: # 左车道线(负斜率) - left_lines.append(line) - elif slope > 0.5: # 右车道线(正斜率) - right_lines.append(line) - - # 拟合左右车道线并计算中心偏移 - if left_lines and right_lines: - # 左车道线拟合 - left_x = [line[0][0] for line in left_lines] + [line[0][2] for line in left_lines] - left_y = [line[0][1] for line in left_lines] + [line[0][3] for line in left_lines] - left_fit = np.polyfit(left_y, left_x, 1) - left_x_base = int(left_fit[0] * height + left_fit[1]) - - # 右车道线拟合 - right_x = [line[0][0] for line in right_lines] + [line[0][2] for line in right_lines] - right_y = [line[0][1] for line in right_lines] + [line[0][3] for line in right_lines] - right_fit = np.polyfit(right_y, right_x, 1) - right_x_base = int(right_fit[0] * height + right_fit[1]) - - # 计算车道中心与画面中心的偏移(像素→米,需标定) - lane_center = (left_x_base + right_x_base) / 2 - frame_center = width / 2 - pixel2meter = 0.01 # 1像素=0.01米(需实际标定) - center_offset = (lane_center - frame_center) * pixel2meter - - # 绘制车道线 - cv2.line(frame, (left_x_base, height), (int(left_fit[0] * height / 2 + left_fit[1]), int(height / 2)), - (0, 255, 0), 2) - cv2.line(frame, (right_x_base, height), - (int(right_fit[0] * height / 2 + right_fit[1]), int(height / 2)), (0, 255, 0), 2) - cv2.line(frame, (int(lane_center), height), (int(frame_center), int(height / 2)), (0, 0, 255), 2) - - # 更新车道线数据 - self.perception_data["lane"]["detected"] = lane_detected - self.perception_data["lane"]["center_offset"] = center_offset - return frame - - def _detect_visual_obstacle(self, frame): - """YOLOv5检测视觉障碍物(车辆/行人/障碍物)""" - # 推理(过滤小目标,只保留car/person/bicycle) - results = self.yolo_model(frame, conf=0.5, classes=[0, 1, 2]) - obstacles = [] - - for result in results: - boxes = result.boxes - for box in boxes: - # 获取检测框信息 - x1, y1, x2, y2 = map(int, box.xyxy[0]) - cls = self.yolo_model.names[int(box.cls[0])] - conf = float(box.conf[0]) - - # 简易距离估算(基于检测框高度,需标定) - box_height = y2 - y1 - distance = 500 / box_height # 示例公式:高度越小,距离越远 - - # 绘制检测框 - cv2.rectangle(frame, (x1, y1), (x2, y2), (255, 0, 0), 2) - cv2.putText(frame, f"{cls}: {distance:.1f}m", (x1, y1 - 10), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 0, 0), 2) - - obstacles.append({ - "class": cls, - "distance": distance, - "x": (x1 + x2) / 2 - frame.shape[1] / 2, # 横向偏移(像素) - "confidence": conf - }) - - self.perception_data["visual_obstacle"] = obstacles - return frame - - def _vision_loop(self): - """视觉感知循环(10Hz)""" - while self.running: - ret, frame = self.camera.read() - if ret: - # 1. 车道线检测 - frame = self._detect_lane(frame) - # 2. 视觉障碍物检测 - frame = self._detect_visual_obstacle(frame) - # 显示画面 - cv2.putText(frame, f"Lane Offset: {self.perception_data['lane']['center_offset']:.2f}m", (10, 30), - cv2.FONT_HERSHEY_SIMPLEX, 0.8, (255, 255, 0), 2) - cv2.imshow("Car Perception (Vision)", frame) - if cv2.waitKey(1) & 0xFF == ord('q'): - self.stop() - time.sleep(0.1) - - # ------------------- 3. LiDAR感知 ------------------- - def _process_lidar_pointcloud(self): - """LiDAR点云处理(聚类检测障碍物)""" - try: - # 示例:生成模拟点云(实际替换为LiDAR SDK读取) - points = np.random.rand(1000, 3) * 20 - 10 # [-10,10]米范围 - points = points[points[:, 2] > -1] # 过滤地面以下点 - - # Open3D点云处理 - pcd = o3d.geometry.PointCloud() - pcd.points = o3d.utility.Vector3dVector(points) - # 下采样 + 去除离群点 - pcd = pcd.voxel_down_sample(voxel_size=0.2) - cl, ind = pcd.remove_statistical_outlier(nb_neighbors=20, std_ratio=2.0) - pcd = pcd.select_by_index(ind) - - # 聚类检测障碍物(DBSCAN) - with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Error): - labels = np.array(pcd.cluster_dbscan(eps=0.5, min_points=10)) - - obstacles = [] - max_label = labels.max() - if max_label >= 0: - for label in range(max_label + 1): - cluster_points = np.asarray(pcd.points)[labels == label] - # 计算障碍物中心、距离、尺寸 - center = np.mean(cluster_points, axis=0) - distance = np.linalg.norm(center) # 距车辆的距离 - angle = math.degrees(math.atan2(center[1], center[0])) # 方位角 - width = np.max(cluster_points[:, 0]) - np.min(cluster_points[:, 0]) - length = np.max(cluster_points[:, 1]) - np.min(cluster_points[:, 1]) - - obstacles.append({ - "distance": distance, - "angle": angle, - "width": width, - "length": length - }) - - self.perception_data["lidar_obstacle"] = obstacles - except Exception as e: - print(f"LiDAR处理失败: {e}") - - def _lidar_loop(self): - """LiDAR感知循环(5Hz)""" - while self.running: - self._process_lidar_pointcloud() - time.sleep(0.2) - - # ------------------- 4. 多传感器融合 ------------------- - def _fusion_obstacle(self): - """融合视觉+LiDAR障碍物数据(加权融合)""" - visual_obstacles = self.perception_data["visual_obstacle"] - lidar_obstacles = self.perception_data["lidar_obstacle"] - fused_obstacles = [] - - # 简易融合规则:LiDAR距离优先,视觉补充类别 - for lidar_obs in lidar_obstacles: - # 匹配视觉障碍物(方位角±5°内) - matched_visual = None - for visual_obs in visual_obstacles: - # 视觉方位角估算(基于横向偏移) - visual_angle = math.degrees(math.atan2(visual_obs["x"] * 0.01, visual_obs["distance"])) - if abs(visual_angle - lidar_obs["angle"]) < 5: - matched_visual = visual_obs - break - - fused_obstacles.append({ - "distance": lidar_obs["distance"], # LiDAR距离更精准 - "angle": lidar_obs["angle"], - "width": lidar_obs["width"], - "class": matched_visual["class"] if matched_visual else "unknown", - "confidence": matched_visual["confidence"] if matched_visual else 0.8 + 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 }) - self.perception_data["fused_obstacle"] = fused_obstacles - - def _fusion_loop(self): - """数据融合循环(5Hz)""" - while self.running: - self._fusion_obstacle() - time.sleep(0.2) - - # ------------------- 5. 安全判断与接口 ------------------- - def check_safety(self): - """安全状态判断""" - # 1. 碰撞风险:融合障碍物距离<3米 - for obs in self.perception_data["fused_obstacle"]: - if obs["distance"] < 3.0: - return False, f"前方{obs['distance']:.1f}米检测到{obs['class']},碰撞风险!" - # 2. 车道偏离风险:偏移>0.5米 - if abs(self.perception_data["lane"]["center_offset"]) > 0.5 and self.perception_data["lane"]["detected"]: - return False, f"车道偏移{self.perception_data['lane']['center_offset']:.2f}米,偏离风险!" - # 3. 车速风险:超速(>10m/s=36km/h) - if self.perception_data["speed"] > 10.0: - return False, f"车速{self.perception_data['speed']:.1f}m/s,超速!" - return True, "安全" - - def get_perception_result(self): - """获取最新感知结果""" - return self.perception_data - - def start(self): - """启动感知系统""" - print("启动无人车感知系统...") - self.vision_thread.start() - self.lidar_thread.start() - self.state_thread.start() - self.fusion_thread.start() - - def stop(self): - """停止感知系统""" - self.running = False - # 等待线程结束 - self.vision_thread.join() - self.lidar_thread.join() - self.state_thread.join() - self.fusion_thread.join() - # 释放资源 - self.camera.release() - self.serial_port.close() + 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() - print("无人车感知系统已停止") + + 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__": - # 初始化感知系统 - car_perception = AutonomousCarPerception() - - try: - # 启动感知 - car_perception.start() - - # 主循环打印感知结果 - while True: - data = car_perception.get_perception_result() - is_safe, msg = car_perception.check_safety() - - print(f"\n=== 无人车感知数据 [{time.ctime()}] ===") - print(f"车速: {data['speed']:.2f}m/s, 航向角: {data['heading']:.1f}°") - print( - f"车道线: {'检测到' if data['lane']['detected'] else '未检测到'}, 偏移: {data['lane']['center_offset']:.2f}m") - print(f"视觉障碍物数量: {len(data['visual_obstacle'])}") - print(f"LiDAR障碍物数量: {len(data['lidar_obstacle'])}") - print(f"融合障碍物数量: {len(data['fused_obstacle'])}") - print(f"安全状态: {'✅ 安全' if is_safe else '❌ 危险'} - {msg}") - - time.sleep(1) - - except KeyboardInterrupt: - car_perception.stop() - print("程序已手动终止") + perception_system = AutonomousVehiclePerception() + perception_system.run_perception() \ No newline at end of file From d935e02277cf28ebece72f3380a4eb59afa6752f Mon Sep 17 00:00:00 2001 From: lej627 <3097482133@qq.com> Date: Fri, 19 Dec 2025 19:18:37 +0800 Subject: [PATCH 6/8] =?UTF-8?q?=E5=9C=A8Carla=E4=B8=AD=E8=BF=90=E8=A1=8C?= =?UTF-8?q?=E4=BB=A3=E7=A0=81=EF=BC=8C=E8=BD=A6=E5=AD=90=E7=A7=BB=E5=8A=A8?= =?UTF-8?q?=E6=88=90=E5=8A=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Unmanned_Aerial_car_Perception/main.py | 261 ++------------------- 1 file changed, 24 insertions(+), 237 deletions(-) diff --git a/src/Unmanned_Aerial_car_Perception/main.py b/src/Unmanned_Aerial_car_Perception/main.py index 6a2a472d5a..ad858a2f8e 100644 --- a/src/Unmanned_Aerial_car_Perception/main.py +++ b/src/Unmanned_Aerial_car_Perception/main.py @@ -1,247 +1,34 @@ -import cv2 -import numpy as np -import random -import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D +import carla -# 随机种子固定,保证结果可复现 -np.random.seed(42) -random.seed(42) +def main(): + # 1. 连接Carla模拟器 + client = carla.Client("localhost", 2000) + client.set_timeout(10.0) -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最大探测距离(米) + try: + world = client.get_world() + print("✅ 成功连接Carla模拟器!") + print("📌 当前仿真地图:", world.get_map().name) - # 感知结果存储 - self.lane_lines = [] # 检测到的车道线 - self.obstacles = [] # 检测到的障碍物(位置+尺寸) - self.perception_fusion_result = {} # 融合结果 + # 2. 获取车辆蓝图 + vehicle_bp = world.get_blueprint_library().find("vehicle.tesla.model3") - def simulate_camera_data(self): - """模拟摄像头图像数据(生成带车道线的道路图像)""" - # 创建黑色背景 - img = np.zeros((self.camera_resolution[1], self.camera_resolution[0], 3), dtype=np.uint8) + # 3. 改用Carla内置的合法生成点(无碰撞) + spawn_points = world.get_map().get_spawn_points() # 获取所有合法生成点 + if spawn_points: + vehicle = world.spawn_actor(vehicle_bp, spawn_points[0]) # 用第一个合法点 + print("🚗 成功生成特斯拉车辆,ID:", vehicle.id) - # 绘制道路和车道线 - 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) + # 车辆简单前进 + vehicle.apply_control(carla.VehicleControl(throttle=0.5, steer=0.0)) + print("🚙 车辆已启动前进!") + else: + print("⚠️ 未找到合法的车辆生成点") - # 绘制两条车道线(带轻微弯曲) - 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 + except Exception as e: + print("❌ 调用失败:", e) - 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 + main() \ No newline at end of file From 1b3eff238114738a1eb8abbdbef4b3c132e54461 Mon Sep 17 00:00:00 2001 From: lej627 <3097482133@qq.com> Date: Fri, 19 Dec 2025 21:23:11 +0800 Subject: [PATCH 7/8] =?UTF-8?q?=E9=87=8D=E6=96=B0=E7=94=9F=E6=88=90?= =?UTF-8?q?=E4=BA=86readme=E6=96=87=E4=BB=B6=EF=BC=8C=E5=B9=B6=E4=B8=94?= =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86=E4=BB=A3=E7=A0=81=E8=B0=83=E7=94=A8?= =?UTF-8?q?Carla=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Unmanned_Aerial_car_Perception/README.md | 159 +++++++++---------- src/Unmanned_Aerial_car_Perception/img.png | Bin 6029 -> 0 bytes src/Unmanned_Aerial_car_Perception/img_1.png | Bin 6029 -> 0 bytes 3 files changed, 78 insertions(+), 81 deletions(-) delete mode 100644 src/Unmanned_Aerial_car_Perception/img.png delete mode 100644 src/Unmanned_Aerial_car_Perception/img_1.png diff --git a/src/Unmanned_Aerial_car_Perception/README.md b/src/Unmanned_Aerial_car_Perception/README.md index dd2969b6e0..8c38c8a280 100644 --- a/src/Unmanned_Aerial_car_Perception/README.md +++ b/src/Unmanned_Aerial_car_Perception/README.md @@ -9,94 +9,95 @@ ### 1.2 ??????? -#### 1.2.1 ???????? +### 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 ???????? +### 1.2.2 ???????? ?????????????????????????????????????????????[17]????????????????????????LiDAR????????[18]??????????????????????????????????SIFT?????FPFH??????????????[19]???????????????????????????????????[20]?????????????????? ??????????????????????????????????????EKF/UKF?????????????FusionNet[21]?PointPillars[22]?????EKF?????????????????????????????????????????????????????????????????????????????????????????[23]? -#### 1.2.3 ??????? +### 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.????????????????????????????????????????????????????? -##### **_1.???????+LiDAR+IMU+GPS???????????????????????????????????????????????16?LiDAR+????????????????2.????EKF?????????????????????/LiDAR??????????IMU?????GPS?????????????? +### 1.4 ?????? -##### 3.??Jetson Xavier??????????????????????YOLOv8n?????????TensorRT???????????????????30ms? - -##### 4.????????????????????????????????????????????????????? +?????5???1??????????????????????????????2?????????????????????LiDAR???????????????????????3???????????????????????????4????????????????????????????5?????????????? ## 2 ????????? ### 2.1 ?????? ?????????????????????????????????????????????1??? -?????1080P@30fps?????16????LiDAR?100Hz IMU?10Hz GPS/RTK?50Hz????????????????????????????????????? -* ???????????????????????????????EKF?????????????????????????? -* ?????????????????????????????????????????????????????**???????????????????????????? +??????? +* ??????1080P@30fps?????16????LiDAR?100Hz IMU?10Hz GPS/RTK?50Hz????????????????????????????????????? +* ????????????????????????????????EKF?????????????????????????? +* ?????????????????????????????????????????????????????????????????????????????????? ### 2.2 ???????? ???????????????????????????????+?????????????????????? -#### 2.2.1 ????? +### 2.2.1 ????? ???????????-????-??????????????????? -* 5.??????????????????????????????????5×5???????????????????????????????????????????????????????????????????Canny??????????????? -* 6.??????ROI?????????????????????????????ROI????????1/2???????????????????? -* 7.?????????????????HoughLinesP???ROI???????????????50px?????100px??????????????????????????????????????????????? -* 8.????????????MobileNetV2??????????????????????????????????????????????????????????????????? +* ??????????????????????????????????5×5???????????????????????????????????????????????????????????????????Canny??????????????? +* ??????ROI?????????????????????????????ROI????????1/2???????????????????? +* ?????????????????HoughLinesP???ROI???????????????50px?????100px??????????????????????????????????????????????? +* ????????????MobileNetV2??????????????????????????????????????????????????????????????????? +?????????????1???? +$$d = (x_c - x_f) \times k \tag{1}$$ +???$x_c$????????????$x_f$?????????????$k$???-??????????????????$k=0.008$m/????$d$??????????????????????????? -#### 2.2.2 ????? +### 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?????? +### 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.1 ????? -#### 2.3.2 ?????????? - -????????DBSCAN????????????????????????? +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 ???????? +### 2.4 ???????? ?????????????????????????????????EKF??????????? -#### 2.4.1 ???? +### 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???? +### 2.4.2 ??EKF???? ??EKF???IMU?GPS?????????/LiDAR??????????????????????????? -#### 2.4.2.1 ???? +### 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}$$ @@ -104,7 +105,7 @@ $$\mathbf{X}_{k} = \mathbf{F}_k \mathbf{X}_{k-1} + \mathbf{G}_k \mathbf{w}_{k-1} $$\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 ???? +### 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}$$ @@ -112,7 +113,7 @@ $$\mathbf{Z}_k = \mathbf{H}_k \mathbf{X}_k + \mathbf{v}_k \tag{6}$$ $$\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 ?????? +### 2.4.2.3 ?????? EKF??????????????????? ????????????????????????????????????????????? @@ -128,7 +129,7 @@ EKF??????????????????? $$\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 ??????? +### 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? ????????? @@ -139,29 +140,29 @@ EKF??????????????????? ## 3 ??????? -#### 3.1 ???????? +### 3.1 ???????? -#### 3.1.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 ???????? + +| ?? | ?? | ???? | +|------------|-----------------------|-----------------------------------| +| ?????? | 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??????????????????? +??????????????????????????KITTI[28]?BDD100K[29]????????????????????????????????10000???????????????????????????????????1000??????????????????? ### 3.2 ???? @@ -173,56 +174,51 @@ EKF??????????????????? ### 3.3 ??????? -#### 3.3.1 ??????? +### 3.3.1 ??????? ?????????????????YOLOv8n???LiDAR?DBSCAN???????????????????2??? -**?2 ?????????????** +?2 ????????????? -| ?? | ????%? | ????%? | mAP?%? | -|:---------------|:------:|:------:|:------:| -| ????YOLOv8n? | 88.5 | 86.2 | 87.3 | -| ?LiDAR?DBSCAN? | 90.3 | 89.1 | 89.7 | -| ?????? | 95.2 | 94.1 | 94.7 | +| ?? | ????%? | ????%? | 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.2 ??????? ???????????????3??? -**?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 ?????? +### 3.3.3 ?????? ????????????4??? -?4 ???????? -**?4 ????????** | ?? | ????MAE?m? | ?????MAE?°? | -|:--------------------|:-----------------|:-------------------| -| GPS+IMU???EKF?| 0.32 | 0.85 | -| ????EKF | 0.15 | 0.32 | - +|---------------------|------------------|--------------------| +| 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 ??????? +### 3.3.4 ??????? ????????????????5??? -?5 ??????? -**?5 ???????** | ?? | ?????ms? | -|:-------------------------|:--------------:| +|--------------------------|----------------| | ????????+???? | 12 | -| LiDAR??????+??? | 10 | +| LiDAR??????+??? | 10 | | ?????? | 5 | | ?????? | 27 | ??5?????????????????27ms???30ms???L2???????????????????50ms???????????????????TensorRT????????12ms?LiDAR????????????????????10ms???????????????5ms?????????????????????????????????????? -4 ????? + +## 4 ????? ### 4.1 ???? @@ -233,7 +229,7 @@ EKF??????????????????? ### 4.2 ???? -?????????????????????? +* ?????????????????????? * ????????????????????????????????????????????????????????? * ??????????????????????????????????????????????????????????+LiDAR+?????+?????????????????????????? * ??????????Transformer???????????????????LiDAR????????????????????????????? @@ -242,13 +238,13 @@ EKF??????????????????? ## 5 ?? ?????????????????????????????????????????????????LiDAR?IMU?GPS?????????????????? -* ??????????????????????????+?????????????????????????????LiDAR?????????RANSAC?????DBSCAN????????????????? -* ?????EKF?????????/LiDAR???????????IMU?????GPS??????????????? -* ????????????????????????????????????27ms????????? -* ???????????????????????????????????mAP?94.7%????????????0.1m??????????????????L2??????????? +??????????????????????????+?????????????????????????????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. @@ -273,4 +269,5 @@ EKF??????????????????? * [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 +* [24] ???. Jetson Xavier NX?????[EB/OL]. https://developer.nvidia.com/embedded/jetson-xavier-nx, 2022. +* [25] Han S, Mao H, Dally W J. Deep compression: Compressing deep neural networks with pruning, trained quantization and Huffman coding[C]//Proceedings of the International \ No newline at end of file diff --git a/src/Unmanned_Aerial_car_Perception/img.png b/src/Unmanned_Aerial_car_Perception/img.png deleted file mode 100644 index dad2e0102345f671428a0635eab7a52179ddb334..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6029 zcmeAS@N?(olHy`uVBq!ia0y~yU;$!wGY%%8$huIG9tH;S5KkA!kczlB2N@ZGf(;Y? zt$)V>(l81}Ltr!nMnhmU1V%$(Gz3ONU^E0qLtr!nMnhmU1V%$(Gz5lq2sGSob_R_Q zFgX5?uRc7iy*lc-(GVC7fzc2c4S~@RpjrqV@Nep6U_AIDvlL{cr>mdKI;Vst0IDq+ Ag#Z8m diff --git a/src/Unmanned_Aerial_car_Perception/img_1.png b/src/Unmanned_Aerial_car_Perception/img_1.png deleted file mode 100644 index dad2e0102345f671428a0635eab7a52179ddb334..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6029 zcmeAS@N?(olHy`uVBq!ia0y~yU;$!wGY%%8$huIG9tH;S5KkA!kczlB2N@ZGf(;Y? zt$)V>(l81}Ltr!nMnhmU1V%$(Gz3ONU^E0qLtr!nMnhmU1V%$(Gz5lq2sGSob_R_Q zFgX5?uRc7iy*lc-(GVC7fzc2c4S~@RpjrqV@Nep6U_AIDvlL{cr>mdKI;Vst0IDq+ Ag#Z8m From 2f046d6e19fd24ce718e834e296a5c2a4ec7228c Mon Sep 17 00:00:00 2001 From: lej627 <3097482133@qq.com> Date: Fri, 19 Dec 2025 21:38:17 +0800 Subject: [PATCH 8/8] =?UTF-8?q?=E9=87=8D=E6=96=B0=E7=94=9F=E6=88=90?= =?UTF-8?q?=E4=BA=86readme=E6=96=87=E4=BB=B6=EF=BC=8C=E5=B9=B6=E4=B8=94?= =?UTF-8?q?=E4=BF=AE=E6=94=B9=E4=BA=86=E4=BB=A3=E7=A0=81=E8=B0=83=E7=94=A8?= =?UTF-8?q?Carla=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Unmanned_Aerial_car_Perception/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Unmanned_Aerial_car_Perception/README.md b/src/Unmanned_Aerial_car_Perception/README.md index 8c38c8a280..552e5f1288 100644 --- a/src/Unmanned_Aerial_car_Perception/README.md +++ b/src/Unmanned_Aerial_car_Perception/README.md @@ -237,7 +237,7 @@ EKF??????????????????? ## 5 ?? -?????????????????????????????????????????????????LiDAR?IMU?GPS?????????????????? +????????????????????????????????????????????LiDAR?IMU?GPS?????????????????? ??????????????????????????+?????????????????????????????LiDAR?????????RANSAC?????DBSCAN????????????????? ?????EKF?????????/LiDAR???????????IMU?????GPS??????????????? ????????????????????????????????????27ms?????????