From c9d3f89a6ab5f150a270b99c59df922a4d157fbc Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Thu, 18 Dec 2025 18:20:44 +0800 Subject: [PATCH 01/20] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E4=BC=A0=E6=84=9F?= =?UTF-8?q?=E5=99=A8=E6=95=B0=E6=8D=AE=E9=87=87=E9=9B=86=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sensor_interface.py | 184 ++++++++++++++++++ 1 file changed, 184 insertions(+) create mode 100644 src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py new file mode 100644 index 0000000000..b569f67d4a --- /dev/null +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py @@ -0,0 +1,184 @@ +# sensor_interface.py +import numpy as np +import time +import threading +from queue import Queue +import json +from dataclasses import dataclass +from typing import Optional, Tuple + + +@dataclass +class PointCloud: + points: np.ndarray # (N, 3) or (N, 4) for intensity + timestamp: float + frame_id: str = "laser" + + +@dataclass +class IMUData: + accel: np.ndarray # (3,) m/s² + gyro: np.ndarray # (3,) rad/s + timestamp: float + + +@dataclass +class GNSSData: + lat: float + lon: float + alt: float + cov: np.ndarray # (3, 3) covariance + timestamp: float + + +class SensorInterface: + """多传感器数据采集与同步接口""" + + def __init__(self, config_path: str = "config/sensors.json"): + self.load_config(config_path) + self.pointcloud_queue = Queue(maxsize=10) + self.imu_queue = Queue(maxsize=50) + self.gnss_queue = Queue(maxsize=5) + self.odom_queue = Queue(maxsize=10) + self.running = False + self.sync_tolerance = 0.01 # 10ms同步容差 + + def load_config(self, config_path: str): + """加载传感器配置""" + default_config = { + "lidar": { + "type": "velodyne", + "port": 2368, + "max_range": 100.0, + "min_range": 0.5 + }, + "imu": { + "type": "xsens", + "port": "/dev/ttyUSB0", + "baudrate": 115200 + }, + "gnss": { + "type": "ublox", + "port": "/dev/ttyACM0", + "baudrate": 9600, + "use_rtk": True + } + } + self.config = default_config + + def start_capture(self): + """启动所有传感器""" + self.running = True + self.lidar_thread = threading.Thread(target=self._lidar_capture) + self.imu_thread = threading.Thread(target=self._imu_capture) + self.gnss_thread = threading.Thread(target=self._gnss_capture) + + self.lidar_thread.start() + self.imu_thread.start() + self.gnss_thread.start() + + print("所有传感器已启动") + + def stop_capture(self): + """停止传感器采集""" + self.running = False + if hasattr(self, 'lidar_thread'): + self.lidar_thread.join() + if hasattr(self, 'imu_thread'): + self.imu_thread.join() + if hasattr(self, 'gnss_thread'): + self.gnss_thread.join() + + def _lidar_capture(self): + """模拟激光雷达数据采集""" + from sensor_simulators import LidarSimulator + + sim = LidarSimulator() + while self.running: + points = sim.generate_pointcloud() + pc = PointCloud( + points=points, + timestamp=time.time() + ) + if not self.pointcloud_queue.full(): + self.pointcloud_queue.put(pc) + time.sleep(0.1) # 10Hz + + def _imu_capture(self): + """模拟IMU数据采集""" + from sensor_simulators import IMUSimulator + + sim = IMUSimulator() + while self.running: + imu_data = IMUData( + accel=sim.get_acceleration(), + gyro=sim.get_gyro(), + timestamp=time.time() + ) + if not self.imu_queue.full(): + self.imu_queue.put(imu_data) + time.sleep(0.01) # 100Hz + + def _gnss_capture(self): + """模拟GNSS数据采集""" + from sensor_simulators import GNSSTrueSimulator + + sim = GNSSTrueSimulator() + while self.running: + gnss_data = GNSSData( + lat=sim.get_latitude(), + lon=sim.get_longitude(), + alt=sim.get_altitude(), + cov=np.eye(3) * 0.1, # 模拟协方差 + timestamp=time.time() + ) + if not self.gnss_queue.full(): + self.gnss_queue.put(gnss_data) + time.sleep(0.2) # 5Hz + + def get_synchronized_data(self, timeout=1.0) -> Tuple[Optional[PointCloud], + Optional[IMUData], + Optional[GNSSData]]: + """获取时间同步的传感器数据""" + try: + # 获取最新的点云 + pc_data = None + if not self.pointcloud_queue.empty(): + pc_data = self.pointcloud_queue.get(timeout=timeout) + + # 获取同步的IMU数据 + imu_data = None + target_time = pc_data.timestamp if pc_data else time.time() + + # 从队列中寻找时间最接近的IMU数据 + closest_imu = None + min_diff = float('inf') + + temp_imu_list = [] + while not self.imu_queue.empty(): + imu = self.imu_queue.get(timeout=0.01) + temp_imu_list.append(imu) + + for imu in temp_imu_list: + time_diff = abs(imu.timestamp - target_time) + if time_diff < min_diff and time_diff < self.sync_tolerance: + min_diff = time_diff + closest_imu = imu + + # 放回未使用的数据 + for imu in temp_imu_list: + if imu is not closest_imu and not self.imu_queue.full(): + self.imu_queue.put(imu) + + imu_data = closest_imu + + # 获取同步的GNSS数据 + gnss_data = None + if not self.gnss_queue.empty(): + gnss_data = self.gnss_queue.get(timeout=0.1) + + return pc_data, imu_data, gnss_data + + except Exception as e: + print(f"数据同步失败: {e}") + return None, None, None \ No newline at end of file From 0d5b42a9cb1e926b8e5fa2212416706a5c8d18a2 Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Thu, 18 Dec 2025 20:07:24 +0800 Subject: [PATCH 02/20] =?UTF-8?q?=E6=A8=A1=E5=9D=97=E5=8C=96PID=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../object_detection.py | 106 ++++++++++++------ 1 file changed, 70 insertions(+), 36 deletions(-) diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py index 97cba38106..919f3f96c9 100644 --- a/src/Object_Tracking_Planning/object_detection.py +++ b/src/Object_Tracking_Planning/object_detection.py @@ -1178,7 +1178,7 @@ def _pid_control(self, target_speed, current_speed): return np.clip((self._k_p * error) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) -class PIDLateralController(): +class PIDLateralController: """ PIDLateralController implements lateral control using a PID. """ @@ -1187,64 +1187,98 @@ def __init__(self, vehicle, K_P=1.0, K_D=0.0, K_I=0.0, dt=0.03): """ Constructor method. - :param vehicle: actor to apply to local planner logic onto - :param K_P: Proportional term - :param K_D: Differential term - :param K_I: Integral term - :param dt: time differential in seconds + Args: + vehicle: Actor to apply local planner logic onto + K_P: Proportional term coefficient (default: 1.0) + K_D: Differential term coefficient (default: 0.0) + K_I: Integral term coefficient (default: 0.0) + dt: Time differential in seconds (default: 0.03) """ self._vehicle = vehicle self._k_p = K_P self._k_d = K_D self._k_i = K_I self._dt = dt - self._e_buffer = deque(maxlen=10) + self._error_buffer = deque(maxlen=10) def run_step(self, waypoint): """ - Execute one step of lateral control to steer - the vehicle towards a certain waypoin. + Execute one step of lateral control to steer the vehicle towards a waypoint. - :param waypoint: target waypoint - :return: steering control in the range [-1, 1] where: - -1 maximum steering to left - +1 maximum steering to right + Args: + waypoint: Target waypoint + + Returns: + float: Steering control in range [-1, 1] where: + -1 = maximum steering to left + +1 = maximum steering to right """ return self._pid_control(waypoint, self._vehicle.get_transform()) def _pid_control(self, waypoint, vehicle_transform): """ - Estimate the steering angle of the vehicle based on the PID equations + Estimate steering angle based on PID equations. - :param waypoint: target waypoint - :param vehicle_transform: current transform of the vehicle - :return: steering control in the range [-1, 1] + Args: + waypoint: Target waypoint + vehicle_transform: Current transform of the vehicle + + Returns: + float: Steering control in range [-1, 1] """ + # Calculate vehicle direction vector v_begin = vehicle_transform.location - v_end = v_begin + carla.Location(x=math.cos(math.radians(vehicle_transform.rotation.yaw)), - y=math.sin(math.radians(vehicle_transform.rotation.yaw))) + yaw_rad = math.radians(vehicle_transform.rotation.yaw) + v_end = v_begin + carla.Location( + x=math.cos(yaw_rad), + y=math.sin(yaw_rad) + ) + # Calculate vectors v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0]) - w_vec = np.array([waypoint.transform.location.x - - v_begin.x, waypoint.transform.location.y - - v_begin.y, 0.0]) - _dot = math.acos(np.clip(np.dot(w_vec, v_vec) / - (np.linalg.norm(w_vec) * np.linalg.norm(v_vec)), -1.0, 1.0)) - - _cross = np.cross(v_vec, w_vec) + w_vec = np.array([ + waypoint.transform.location.x - v_begin.x, + waypoint.transform.location.y - v_begin.y, + 0.0 + ]) + + # Calculate angle difference + v_norm = np.linalg.norm(v_vec) + w_norm = np.linalg.norm(w_vec) + dot_product = np.dot(w_vec, v_vec) / (v_norm * w_norm) + dot_product_clipped = np.clip(dot_product, -1.0, 1.0) + angle_diff = math.acos(dot_product_clipped) + + # Determine sign based on cross product + cross_product = np.cross(v_vec, w_vec) + if cross_product[2] < 0: + angle_diff *= -1.0 + + # Update error buffer and calculate PID terms + self._error_buffer.append(angle_diff) + + # Calculate derivative and integral terms + derivative_error = self._calculate_derivative_error() + integral_error = self._calculate_integral_error() + + # PID calculation with clamping + pid_output = ( + self._k_p * angle_diff + + self._k_d * derivative_error + + self._k_i * integral_error + ) - if _cross[2] < 0: - _dot *= -1.0 + return np.clip(pid_output, -1.0, 1.0) - self._e_buffer.append(_dot) - if len(self._e_buffer) >= 2: - _de = (self._e_buffer[-1] - self._e_buffer[-2]) / self._dt - _ie = sum(self._e_buffer) * self._dt - else: - _de = 0.0 - _ie = 0.0 + def _calculate_derivative_error(self): + """Calculate derivative error term.""" + if len(self._error_buffer) >= 2: + return (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt + return 0.0 - return np.clip((self._k_p * _dot) + (self._k_d * _de) + (self._k_i * _ie), -1.0, 1.0) + def _calculate_integral_error(self): + """Calculate integral error term.""" + return sum(self._error_buffer) * self._dt # ============================================================================== From 737c4bdf0d281f4a3f383d205052db61377de670 Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Thu, 18 Dec 2025 20:18:52 +0800 Subject: [PATCH 03/20] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E4=BC=A0=E6=84=9F?= =?UTF-8?q?=E5=99=A8=E6=95=B0=E6=8D=AE=E9=87=87=E9=9B=86=E6=A8=A1=E5=9E=8B?= =?UTF-8?q?=E5=8F=8A=E8=BF=90=E8=A1=8C=E6=95=88=E6=9E=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sensor_interface.py | 161 +++++++++++++++++- 1 file changed, 154 insertions(+), 7 deletions(-) diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py index b569f67d4a..16a6d6c236 100644 --- a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/sensor_interface.py @@ -6,8 +6,55 @@ import json from dataclasses import dataclass from typing import Optional, Tuple +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import random +# ====================== 传感器模拟器部分(原缺失的依赖) ====================== +class LidarSimulator: + def generate_pointcloud(self): + """模拟生成(1000, 3)的点云数据(x,y,z)""" + n_points = 1000 + x = np.random.uniform(-50, 50, n_points) + y = np.random.uniform(-50, 50, n_points) + z = np.random.uniform(-5, 5, n_points) + return np.column_stack((x, y, z)) + + +class IMUSimulator: + def get_acceleration(self): + """模拟加速度(含重力+微小噪声)""" + return np.array([0, 0, 9.81 + random.uniform(-0.1, 0.1)]) + + def get_gyro(self): + """模拟陀螺仪数据(微小噪声)""" + return np.array([random.uniform(-0.01, 0.01) for _ in range(3)]) + + +class GNSSTrueSimulator: + def __init__(self): + self.lat = 39.9042 # 基准纬度(如北京天安门) + self.lon = 116.4074 # 基准经度 + self.alt = 50.0 # 基准海拔 + + def get_latitude(self): + """模拟纬度缓慢变化""" + self.lat += random.uniform(-1e-6, 1e-6) + return self.lat + + def get_longitude(self): + """模拟经度缓慢变化""" + self.lon += random.uniform(-1e-6, 1e-6) + return self.lon + + def get_altitude(self): + """模拟海拔缓慢变化""" + self.alt += random.uniform(-0.01, 0.01) + return self.alt + + +# ====================== 原代码核心部分(无任何修改) ====================== @dataclass class PointCloud: points: np.ndarray # (N, 3) or (N, 4) for intensity @@ -91,8 +138,7 @@ def stop_capture(self): def _lidar_capture(self): """模拟激光雷达数据采集""" - from sensor_simulators import LidarSimulator - + # 直接使用上方定义的模拟器(不再需要外部导入) sim = LidarSimulator() while self.running: points = sim.generate_pointcloud() @@ -106,8 +152,7 @@ def _lidar_capture(self): def _imu_capture(self): """模拟IMU数据采集""" - from sensor_simulators import IMUSimulator - + # 直接使用上方定义的模拟器 sim = IMUSimulator() while self.running: imu_data = IMUData( @@ -121,8 +166,7 @@ def _imu_capture(self): def _gnss_capture(self): """模拟GNSS数据采集""" - from sensor_simulators import GNSSTrueSimulator - + # 直接使用上方定义的模拟器 sim = GNSSTrueSimulator() while self.running: gnss_data = GNSSData( @@ -181,4 +225,107 @@ def get_synchronized_data(self, timeout=1.0) -> Tuple[Optional[PointCloud], except Exception as e: print(f"数据同步失败: {e}") - return None, None, None \ No newline at end of file + return None, None, None + + +# ====================== 可视化与运行逻辑(外部扩展整合进来) ====================== +def visualize_synchronized_data(pc, imu, gnss, fig, axes): + """可视化同步数据""" + ax_pc, ax_imu_accel, ax_imu_gyro, ax_gnss = axes + # 清空画布 + ax_pc.clear() + ax_imu_accel.clear() + ax_imu_gyro.clear() + ax_gnss.clear() + + # 1. 绘制点云 + if pc is not None: + points = pc.points + ax_pc.scatter(points[:, 0], points[:, 1], points[:, 2], s=1, c='b') + ax_pc.set_xlabel('X (m)') + ax_pc.set_ylabel('Y (m)') + ax_pc.set_zlabel('Z (m)') + ax_pc.set_title(f'Point Cloud (time: {pc.timestamp:.2f})') + else: + ax_pc.set_title('Point Cloud: No Data') + + # 2. 绘制IMU数据 + if imu is not None: + # 加速度 + ax_imu_accel.bar(['X', 'Y', 'Z'], imu.accel, color=['r', 'g', 'b']) + ax_imu_accel.set_ylim(9.7, 9.9) + ax_imu_accel.set_title(f'IMU Accel (time: {imu.timestamp:.2f})') + ax_imu_accel.set_ylabel('Accel (m/s²)') + # 陀螺仪 + ax_imu_gyro.bar(['X', 'Y', 'Z'], imu.gyro, color=['r', 'g', 'b']) + ax_imu_gyro.set_ylim(-0.01, 0.01) + ax_imu_gyro.set_title('IMU Gyro (rad/s)') + ax_imu_gyro.set_ylabel('Gyro (rad/s)') + else: + ax_imu_accel.set_title('IMU Accel: No Data') + ax_imu_gyro.set_title('IMU Gyro: No Data') + + # 3. 绘制GNSS数据 + if gnss is not None: + ax_gnss.text(0.1, 0.8, f'Lat: {gnss.lat:.6f}', transform=ax_gnss.transAxes) + ax_gnss.text(0.1, 0.6, f'Lon: {gnss.lon:.6f}', transform=ax_gnss.transAxes) + ax_gnss.text(0.1, 0.4, f'Alt: {gnss.alt:.2f} m', transform=ax_gnss.transAxes) + ax_gnss.text(0.1, 0.2, f'Time: {gnss.timestamp:.2f}', transform=ax_gnss.transAxes) + ax_gnss.set_xlim(0, 1) + ax_gnss.set_ylim(0, 1) + ax_gnss.axis('off') + else: + ax_gnss.set_title('GNSS: No Data') + ax_gnss.axis('off') + + # 实时更新 + plt.draw() + plt.pause(0.001) + + +def main(): + """主运行函数""" + # 1. 实例化传感器接口 + sensor_if = SensorInterface() + + # 2. 初始化可视化画布 + fig = plt.figure(figsize=(12, 8)) + ax_pc = fig.add_subplot(221, projection='3d') + ax_imu_accel = fig.add_subplot(222) + ax_imu_gyro = fig.add_subplot(223) + ax_gnss = fig.add_subplot(224) + axes = (ax_pc, ax_imu_accel, ax_imu_gyro, ax_gnss) + plt.ion() # 交互模式 + + try: + # 3. 启动传感器采集 + sensor_if.start_capture() + + # 4. 循环获取同步数据并展示 + while True: + pc_data, imu_data, gnss_data = sensor_if.get_synchronized_data() + + # 文本输出 + print("\n=== 同步数据摘要 ===") + print(f"点云: {'有数据' if pc_data else '无数据'} (数量: {pc_data.points.shape[0] if pc_data else 0})") + print(f"IMU: {'有数据' if imu_data else '无数据'} (加速度: {imu_data.accel if imu_data else 'None'})") + print( + f"GNSS: {'有数据' if gnss_data else '无数据'} (经纬度: {f'{gnss_data.lat:.6f}, {gnss_data.lon:.6f}' if gnss_data else 'None'})") + + # 可视化输出 + visualize_synchronized_data(pc_data, imu_data, gnss_data, fig, axes) + + time.sleep(0.1) # 与点云频率同步 + + except KeyboardInterrupt: + # 停止采集 + print("\n停止传感器采集...") + sensor_if.stop_capture() + plt.ioff() + plt.show() + print("采集已停止") + + +# 程序入口 +if __name__ == "__main__": + main() \ No newline at end of file From c15f438c79c0d990a53cb5ec9381a52a1092965b Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Fri, 19 Dec 2025 00:39:19 +0800 Subject: [PATCH 04/20] =?UTF-8?q?=E4=BF=AE=E6=94=B9readme=E6=96=87?= =?UTF-8?q?=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md index c7c8ba177a..202373667b 100644 --- a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md @@ -3,6 +3,7 @@ # 2.可靠的全局定位 # 3.无人工干预的自主路径规划与导航 + # 构建目录结构 ```text @@ -21,4 +22,11 @@ outdoor_ws/src/robot_description/ ├── config/ │ ├── controllers.yaml # ros2_control配置 │ └── robot_control.yaml # 控制参数 -└── meshes/ # 可选:3D模型文件 \ No newline at end of file +└── meshes/ # 可选:3D模型文件 +``` +## 核心功能 +1. **多传感器数据采集、同步与统一接口管理** + - 封装激光雷达、IMU、GNSS和轮式编码器的数据采集 + - 实现多传感器时间同步(10ms容差) + - 提供线程安全的数据队列 + - 包含传感器配置管理和模拟器(用于测试) \ No newline at end of file From 10fb8195e9f80cf4972a623e970f81e860d87d4c Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Fri, 19 Dec 2025 12:21:25 +0800 Subject: [PATCH 05/20] =?UTF-8?q?=E6=B7=BB=E5=8A=A0LiDAR-IMU=E7=B4=A7?= =?UTF-8?q?=E8=80=A6=E5=90=88=E9=87=8C=E7=A8=8B=E8=AE=A1=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../lio_odometry.py | 665 ++++++++++++++++++ 1 file changed, 665 insertions(+) create mode 100644 src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/lio_odometry.py diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/lio_odometry.py b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/lio_odometry.py new file mode 100644 index 0000000000..d920f30885 --- /dev/null +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/lio_odometry.py @@ -0,0 +1,665 @@ +# lio_odometry.py +import numpy as np +from scipy.spatial.transform import Rotation as R +from scipy.spatial import KDTree +from collections import deque +import time +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import warnings + +warnings.filterwarnings('ignore') + + +class LIOdometry: + """LiDAR-IMU紧耦合里程计""" + + def __init__(self, config=None): + self.config = config or self._default_config() + + # 状态变量 + self.position = np.zeros(3) # 位置 (x, y, z) + self.velocity = np.zeros(3) # 速度 + self.orientation = np.eye(3) # 旋转矩阵 + self.gravity = np.array([0, 0, -9.81]) # 重力向量 + + # 偏差 + self.gyro_bias = np.zeros(3) + self.accel_bias = np.zeros(3) + + # 缓冲区 + self.imu_buffer = deque(maxlen=100) + self.last_lidar_time = None + + # 协方差矩阵 + self.covariance = np.eye(15) * 0.1 + + # 预积分 + self.delta_p = np.zeros(3) # 位置增量 + self.delta_v = np.zeros(3) # 速度增量 + self.delta_r = np.eye(3) # 旋转增量 + + # === 新增:可视化相关变量 === + self.trajectory = [] # 轨迹历史 + self.features_history = [] # 特征点历史 + self.max_history = 1000 # 最大历史记录数 + # ========================== + + def _default_config(self): + return { + "imu_noise": { + "gyro": 0.01, # rad/s + "accel": 0.1, # m/s² + "gyro_bias": 1e-5, + "accel_bias": 1e-4 + }, + "lidar_noise": { + "rotation": 0.001, # rad + "translation": 0.01 # m + }, + "mapping": { + "voxel_size": 0.2, + "max_range": 50.0, + "min_range": 1.0 + } + } + + def preintegrate_imu(self, imu_data_list): + """IMU预积分""" + if not imu_data_list: + return + + # 重置预积分量 + self.delta_p = np.zeros(3) + self.delta_v = np.zeros(3) + self.delta_r = np.eye(3) + + last_time = imu_data_list[0].timestamp + + for i in range(1, len(imu_data_list)): + current_imu = imu_data_list[i] + dt = current_imu.timestamp - last_time + + if dt <= 0: + continue + + # 去除偏差后的测量值 + accel = current_imu.accel - self.accel_bias + gyro = current_imu.gyro - self.gyro_bias + + # 中值积分 + accel_0 = imu_data_list[i - 1].accel - self.accel_bias + gyro_0 = imu_data_list[i - 1].gyro - self.gyro_bias + + accel_hat = 0.5 * (accel_0 + accel) + gyro_hat = 0.5 * (gyro_0 + gyro) + + # 旋转更新 + delta_theta = gyro_hat * dt + delta_rotation = R.from_rotvec(delta_theta).as_matrix() + self.delta_r = self.delta_r @ delta_rotation + + # 速度更新 + accel_world = self.delta_r @ accel_hat + self.delta_v += (accel_world + self.gravity) * dt + + # 位置更新 + self.delta_p += self.delta_v * dt + 0.5 * (accel_world + self.gravity) * dt * dt + + last_time = current_imu.timestamp + + def extract_features(self, point_cloud): + """从点云中提取几何特征""" + if point_cloud.shape[0] < 100: + return [], [] + + # 地面分割 + ground_points, non_ground = self._ground_segmentation(point_cloud) + + # 提取平面特征(来自地面点) + plane_features = self._extract_plane_features(ground_points) + + # 提取边缘特征(来自非地面点) + edge_features = self._extract_edge_features(non_ground) + + return plane_features, edge_features + + def _ground_segmentation(self, points): + """简单的地面分割""" + if points.shape[0] == 0: + return np.zeros((0, 3)), np.zeros((0, 3)) + + # 基于高度的简单分割 + z_values = points[:, 2] + ground_mask = z_values < 0.2 # 假设地面高度阈值 + ground_points = points[ground_mask] + non_ground = points[~ground_mask] + + return ground_points, non_ground + + def _extract_plane_features(self, points, num_features=20): + """提取平面特征点""" + if points.shape[0] < num_features: + return points + + # 使用曲率选择最平坦的点 + curvatures = self._compute_curvature(points) + flat_indices = np.argsort(curvatures)[:num_features] + + return points[flat_indices] + + def _extract_edge_features(self, points, num_features=20): + """提取边缘特征点""" + if points.shape[0] < num_features: + return points + + # 使用曲率选择最尖锐的点 + curvatures = self._compute_curvature(points) + edge_indices = np.argsort(curvatures)[-num_features:] + + return points[edge_indices] + + def _compute_curvature(self, points, k=5): + """计算点云曲率""" + from sklearn.neighbors import NearestNeighbors + + if points.shape[0] < k: + return np.zeros(points.shape[0]) + + nbrs = NearestNeighbors(n_neighbors=k + 1, algorithm='kd_tree').fit(points) + distances, indices = nbrs.kneighbors(points) + + curvatures = [] + for i in range(points.shape[0]): + neighbor_points = points[indices[i][1:]] # 排除自身 + centered = neighbor_points - neighbor_points.mean(axis=0) + cov = centered.T @ centered + eigenvalues = np.linalg.eigvalsh(cov) + curvature = eigenvalues[0] / (eigenvalues.sum() + 1e-6) + curvatures.append(curvature) + + return np.array(curvatures) + + def scan_to_map_matching(self, current_features, map_features): + """当前扫描与地图匹配""" + if len(current_features) == 0 or len(map_features) == 0: + return np.eye(4) + + # 使用ICP进行配准 + transformation = self._icp_registration(current_features, map_features) + + return transformation + + def _icp_registration(self, source, target, max_iterations=20): + """迭代最近点算法""" + if len(source) < 3 or len(target) < 3: + return np.eye(4) + + # 构建KD树加速最近邻搜索 + tree = KDTree(target) + + # 初始变换矩阵 + transformation = np.eye(4) + + for iteration in range(max_iterations): + # 找到最近邻对应点 + distances, indices = tree.query(source) + + # 过滤掉距离太远的点对 + valid_mask = distances < 1.0 # 1米阈值 + if np.sum(valid_mask) < 3: + break + + source_valid = source[valid_mask] + target_valid = target[indices[valid_mask]] + + # 计算最优刚体变换 + R_mat, t_vec = self._compute_rigid_transform(source_valid, target_valid) + + # 更新变换 + delta_transform = np.eye(4) + delta_transform[:3, :3] = R_mat + delta_transform[:3, 3] = t_vec + + transformation = delta_transform @ transformation + + # 更新源点云 + source = (R_mat @ source.T + t_vec.reshape(3, 1)).T + + # 检查收敛 + if np.linalg.norm(t_vec) < 0.001 and np.linalg.norm(R_mat - np.eye(3)) < 0.001: + break + + return transformation + + def _compute_rigid_transform(self, A, B): + """计算点集A到点集B的最优刚体变换""" + centroid_A = np.mean(A, axis=0) + centroid_B = np.mean(B, axis=0) + + AA = A - centroid_A + BB = B - centroid_B + + H = AA.T @ BB + U, S, Vt = np.linalg.svd(H) + + R = Vt.T @ U.T + + # 处理反射情况 + if np.linalg.det(R) < 0: + Vt[-1, :] *= -1 + R = Vt.T @ U.T + + t = centroid_B - R @ centroid_A + + return R, t + + def process_frame(self, point_cloud, imu_data_list, gnss_data=None): + """处理一帧数据""" + # IMU预积分 + self.preintegrate_imu(imu_data_list) + + # 提取特征 + plane_features, edge_features = self.extract_features(point_cloud) + all_features = np.vstack([plane_features, edge_features]) if len(plane_features) > 0 else edge_features + + # 运动估计(这里简化为使用IMU预积分) + # 在实际系统中,这里会进行扫描匹配 + delta_pose = np.eye(4) + delta_pose[:3, :3] = self.delta_r + delta_pose[:3, 3] = self.delta_p + + # 更新状态 + self.orientation = self.orientation @ self.delta_r + self.position += self.orientation @ self.delta_p + self.velocity += self.orientation @ self.delta_v + + # 如果有GNSS数据,可以在这里融合 + if gnss_data: + self._fuse_gnss(gnss_data) + + # 构建当前位姿 + pose = np.eye(4) + pose[:3, :3] = self.orientation + pose[:3, 3] = self.position + + # === 新增:记录轨迹和特征用于可视化 === + self._record_for_visualization(pose, all_features) + # ================================= + + return pose, all_features + + def _fuse_gnss(self, gnss_data): + """融合GNSS数据(简化版本)""" + # 将经纬度转换为局部坐标(需要知道原点) + # 这里简化为直接信任GNSS(在实际系统中应使用卡尔曼滤波) + local_pos = self._llh_to_local(gnss_data.lat, gnss_data.lon, gnss_data.alt) + + # 简单的加权融合 + gnss_weight = 0.1 # 信任度 + self.position = (1 - gnss_weight) * self.position + gnss_weight * local_pos + + def _llh_to_local(self, lat, lon, alt): + """经纬度转局部坐标(简化版)""" + # 实际实现中需要知道原点的经纬度 + return np.array([lon * 111320.0, lat * 110540.0, alt]) # 近似转换 + + # ============================================= + # === 以下是新增的可视化方法,原功能完全不变 === + # ============================================= + + def _record_for_visualization(self, pose, features): + """记录数据用于可视化(内部方法)""" + # 记录轨迹 + self.trajectory.append(pose[:3, 3].copy()) + + # 记录特征点 + if isinstance(features, np.ndarray) and features.shape[0] > 0: + self.features_history.append(features.copy()) + else: + self.features_history.append(np.zeros((0, 3))) + + # 限制历史数据长度 + if len(self.trajectory) > self.max_history: + self.trajectory = self.trajectory[-self.max_history:] + if len(self.features_history) > self.max_history: + self.features_history = self.features_history[-self.max_history:] + + def show_trajectory(self): + """显示轨迹(2D和3D)""" + if not self.trajectory: + print("No trajectory data to visualize") + return + + trajectory_array = np.array(self.trajectory) + + fig = plt.figure(figsize=(15, 5)) + + # 1. 3D轨迹 + ax1 = fig.add_subplot(131, projection='3d') + ax1.plot(trajectory_array[:, 0], trajectory_array[:, 1], trajectory_array[:, 2], + 'b-', linewidth=2, label='Trajectory') + if len(trajectory_array) > 1: + ax1.scatter(trajectory_array[0, 0], trajectory_array[0, 1], trajectory_array[0, 2], + c='g', s=100, marker='o', label='Start') + ax1.scatter(trajectory_array[-1, 0], trajectory_array[-1, 1], trajectory_array[-1, 2], + c='r', s=100, marker='s', label='End') + ax1.set_xlabel('X (m)') + ax1.set_ylabel('Y (m)') + ax1.set_zlabel('Z (m)') + ax1.set_title('3D Trajectory') + ax1.legend() + ax1.grid(True) + + # 2. 2D轨迹(XY平面) + ax2 = fig.add_subplot(132) + ax2.plot(trajectory_array[:, 0], trajectory_array[:, 1], 'b-', linewidth=2) + if len(trajectory_array) > 1: + ax2.scatter(trajectory_array[0, 0], trajectory_array[0, 1], c='g', s=100) + ax2.scatter(trajectory_array[-1, 0], trajectory_array[-1, 1], c='r', s=100) + ax2.set_xlabel('X (m)') + ax2.set_ylabel('Y (m)') + ax2.set_title('2D Trajectory (XY Plane)') + ax2.axis('equal') + ax2.grid(True) + + # 3. 高程变化 + ax3 = fig.add_subplot(133) + if len(trajectory_array) > 1: + distances = np.cumsum(np.sqrt(np.sum(np.diff(trajectory_array[:, :2], axis=0) ** 2, axis=1))) + distances = np.insert(distances, 0, 0) + ax3.plot(distances, trajectory_array[:, 2], 'b-', linewidth=2) + ax3.set_xlabel('Distance (m)') + ax3.set_ylabel('Z (m)') + ax3.set_title('Elevation Profile') + ax3.grid(True) + + plt.tight_layout() + plt.show() + return fig + + def show_features(self, point_cloud=None): + """显示特征点和点云""" + if not self.features_history: + print("No feature data to visualize") + return + + recent_features = self.features_history[-1] + + fig = plt.figure(figsize=(15, 5)) + + # 1. 3D特征点 + ax1 = fig.add_subplot(131, projection='3d') + if recent_features.shape[0] > 0: + # 转换特征点到世界坐标系 + if hasattr(self, 'orientation') and hasattr(self, 'position'): + features_homogeneous = np.hstack([recent_features, np.ones((len(recent_features), 1))]) + current_pose = np.eye(4) + current_pose[:3, :3] = self.orientation + current_pose[:3, 3] = self.position + world_features = (current_pose @ features_homogeneous.T).T[:, :3] + + ax1.scatter(world_features[:, 0], world_features[:, 1], world_features[:, 2], + c='red', s=20, label='Features') + + ax1.scatter(self.position[0], self.position[1], self.position[2], + c='blue', s=100, marker='s', label='Current Position') + ax1.set_xlabel('X (m)') + ax1.set_ylabel('Y (m)') + ax1.set_zlabel('Z (m)') + ax1.set_title('Feature Points in World Frame') + ax1.legend() + ax1.grid(True) + + # 2. 特征点统计 + ax2 = fig.add_subplot(132) + feature_counts = [len(f) for f in self.features_history[-20:] if f.shape[0] > 0] + if feature_counts: + ax2.plot(range(len(feature_counts)), feature_counts, 'b-o', linewidth=2, markersize=6) + ax2.set_xlabel('Recent Frames') + ax2.set_ylabel('Number of Features') + ax2.set_title('Feature Count History') + ax2.grid(True) + + # 3. 点云(如果提供) + ax3 = fig.add_subplot(133, projection='3d') + if point_cloud is not None and point_cloud.shape[0] > 0: + # 随机采样显示 + if point_cloud.shape[0] > 1000: + indices = np.random.choice(point_cloud.shape[0], 1000, replace=False) + point_cloud = point_cloud[indices] + + ax3.scatter(point_cloud[:, 0], point_cloud[:, 1], point_cloud[:, 2], + c='gray', s=1, alpha=0.3, label='Point Cloud') + + if recent_features.shape[0] > 0: + ax3.scatter(recent_features[:, 0], recent_features[:, 1], recent_features[:, 2], + c='red', s=20, label='Features') + + ax3.set_xlabel('X') + ax3.set_ylabel('Y') + ax3.set_zlabel('Z') + ax3.set_title('Latest Point Cloud & Features') + ax3.legend() + + plt.tight_layout() + plt.show() + return fig + + def show_comprehensive_analysis(self): + """显示综合分析图表""" + if not self.trajectory: + print("No data for analysis") + return + + trajectory_array = np.array(self.trajectory) + + fig = plt.figure(figsize=(16, 12)) + + # 1. 3D轨迹 + ax1 = fig.add_subplot(331, projection='3d') + ax1.plot(trajectory_array[:, 0], trajectory_array[:, 1], trajectory_array[:, 2], + 'b-', linewidth=2) + ax1.set_xlabel('X (m)') + ax1.set_ylabel('Y (m)') + ax1.set_zlabel('Z (m)') + ax1.set_title('3D Trajectory') + ax1.grid(True) + + # 2. 2D轨迹 + ax2 = fig.add_subplot(332) + ax2.plot(trajectory_array[:, 0], trajectory_array[:, 1], 'b-', linewidth=2) + ax2.set_xlabel('X (m)') + ax2.set_ylabel('Y (m)') + ax2.set_title('2D Trajectory') + ax2.axis('equal') + ax2.grid(True) + + # 3. 位置分量 + ax3 = fig.add_subplot(333) + frames = range(len(trajectory_array)) + ax3.plot(frames, trajectory_array[:, 0], 'r-', label='X', linewidth=1) + ax3.plot(frames, trajectory_array[:, 1], 'g-', label='Y', linewidth=1) + ax3.plot(frames, trajectory_array[:, 2], 'b-', label='Z', linewidth=1) + ax3.set_xlabel('Frame') + ax3.set_ylabel('Position (m)') + ax3.set_title('Position Components') + ax3.legend() + ax3.grid(True) + + # 4. 速度信息 + ax4 = fig.add_subplot(334) + try: + velocity_norm = np.linalg.norm(self.velocity) + ax4.bar(['Speed'], [velocity_norm], color='skyblue') + ax4.set_ylabel('m/s') + ax4.set_title(f'Current Speed: {velocity_norm:.2f} m/s') + ax4.grid(True, alpha=0.3) + except: + pass + + # 5. 特征点统计 + ax5 = fig.add_subplot(335) + if self.features_history: + feature_counts = [len(f) for f in self.features_history[-50:] if f.shape[0] > 0] + if feature_counts: + ax5.plot(range(len(feature_counts)), feature_counts, 'purple', linewidth=2) + ax5.set_xlabel('Recent Frames') + ax5.set_ylabel('Feature Count') + ax5.set_title('Feature Extraction History') + ax5.grid(True) + + # 6. 轨迹长度 + ax6 = fig.add_subplot(336) + if len(trajectory_array) > 1: + total_distance = np.sum(np.sqrt(np.sum(np.diff(trajectory_array, axis=0) ** 2, axis=1))) + ax6.text(0.5, 0.5, f'Total Distance:\n{total_distance:.2f} m', + ha='center', va='center', fontsize=14, transform=ax6.transAxes) + ax6.set_title('Trajectory Statistics') + ax6.axis('off') + + # 7. 最新特征点 + ax7 = fig.add_subplot(337, projection='3d') + if self.features_history and self.features_history[-1].shape[0] > 0: + recent_features = self.features_history[-1] + ax7.scatter(recent_features[:, 0], recent_features[:, 1], recent_features[:, 2], + c='red', s=10, alpha=0.6) + ax7.set_xlabel('X') + ax7.set_ylabel('Y') + ax7.set_zlabel('Z') + ax7.set_title('Latest Features (Local)') + + # 8. 状态信息 + ax8 = fig.add_subplot(338) + info_text = f"Frames: {len(self.trajectory)}\n" + info_text += f"Position: ({self.position[0]:.2f}, {self.position[1]:.2f}, {self.position[2]:.2f})\n" + info_text += f"Velocity: {np.linalg.norm(self.velocity):.2f} m/s" + ax8.text(0.1, 0.5, info_text, ha='left', va='center', fontsize=12, + transform=ax8.transAxes, family='monospace') + ax8.set_title('Current State') + ax8.axis('off') + + # 9. 空图表(预留) + ax9 = fig.add_subplot(339) + ax9.text(0.5, 0.5, 'LIO Odometry\nVisualization', + ha='center', va='center', fontsize=16, transform=ax9.transAxes) + ax9.axis('off') + + plt.tight_layout() + plt.show() + return fig + + def export_trajectory(self, filename="trajectory.txt", format="kitti"): + """导出轨迹到文件""" + if not self.trajectory: + print("No trajectory to export") + return + + import os + os.makedirs(os.path.dirname(filename) if os.path.dirname(filename) else '.', exist_ok=True) + + with open(filename, 'w') as f: + if format.lower() == "kitti": + # KITTI格式 + for pos in self.trajectory: + # 使用当前方向或单位矩阵 + rot_matrix = self.orientation if hasattr(self, 'orientation') else np.eye(3) + line = ' '.join([f'{val:.6f}' for val in rot_matrix.flatten()]) + line += f' {pos[0]:.6f} {pos[1]:.6f} {pos[2]:.6f}' + f.write(line + '\n') + print(f"Trajectory exported in KITTI format to {filename}") + + elif format.lower() == "tum": + # TUM格式 + f.write("# timestamp tx ty tz qx qy qz qw\n") + for i, pos in enumerate(self.trajectory): + timestamp = i * 0.1 # 假设时间间隔 + rot = self.orientation if hasattr(self, 'orientation') else np.eye(3) + rotation = R.from_matrix(rot) + quat = rotation.as_quat() # [x, y, z, w] + line = f"{timestamp:.6f} {pos[0]:.6f} {pos[1]:.6f} {pos[2]:.6f} " + line += f"{quat[0]:.6f} {quat[1]:.6f} {quat[2]:.6f} {quat[3]:.6f}" + f.write(line + '\n') + print(f"Trajectory exported in TUM format to {filename}") + else: + print(f"Unknown format: {format}") + + def clear_visualization_data(self): + """清除可视化数据""" + self.trajectory.clear() + self.features_history.clear() + + +# ============================================= +# === 以下是为了测试而添加的模拟代码 === +# ============================================= + +class MockIMUData: + """模拟IMU数据类(仅用于测试)""" + + def __init__(self, timestamp, accel, gyro): + self.timestamp = timestamp + self.accel = accel + self.gyro = gyro + + +class MockGNSSData: + """模拟GNSS数据类(仅用于测试)""" + + def __init__(self, lat, lon, alt): + self.lat = lat + self.lon = lon + self.alt = alt + + +def test_demo(): + """测试演示函数(可以直接在PyCharm中运行)""" + print("Starting LIO Odometry Visualization Demo...") + + # 创建LIOdometry实例 + odom = LIOdometry() + + # 模拟运行10帧 + for i in range(10): + # 生成模拟点云 + n_points = 500 + np.random.randint(0, 500) + point_cloud = np.random.randn(n_points, 3) * 3.0 + point_cloud[:, 2] = np.abs(point_cloud[:, 2]) * 0.5 + + # 生成模拟IMU数据(轻微运动) + imu_data = [ + MockIMUData(0.0, np.array([0, 0, -9.81]), np.array([0, 0, 0])), + MockIMUData(0.01, np.array([0.1 * (i / 10), 0, -9.81]), np.array([0, 0, 0.05 * (i / 10)])) + ] + + # 处理帧(原有功能) + pose, features = odom.process_frame(point_cloud, imu_data) + + print(f"Frame {i + 1}: Position = [{pose[0, 3]:.2f}, {pose[1, 3]:.2f}, {pose[2, 3]:.2f}], " + f"Features = {features.shape[0]}") + + print("\nGenerating visualizations...") + + # 显示轨迹 + odom.show_trajectory() + + # 显示特征点 + # 注意:这里需要最新的点云,我们重新生成一个 + test_point_cloud = np.random.randn(300, 3) * 2.0 + odom.show_features(test_point_cloud) + + # 显示综合分析 + odom.show_comprehensive_analysis() + + # 导出轨迹 + odom.export_trajectory("test_trajectory_kitti.txt", "kitti") + odom.export_trajectory("test_trajectory_tum.txt", "tum") + + print("\nDemo completed!") + print("Close the matplotlib windows to exit.") + + +# 主程序入口 +if __name__ == "__main__": + # 自动运行测试演示 + test_demo() \ No newline at end of file From 837902a843be986dcc615ac4ee2a23b05222a015 Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Fri, 19 Dec 2025 14:57:41 +0800 Subject: [PATCH 06/20] =?UTF-8?q?=E7=AE=80=E6=B4=81=E5=8C=96=E4=B8=8E?= =?UTF-8?q?=E7=8E=AF=E5=A2=83=E7=AE=A1=E7=90=86=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../object_detection.py | 172 ++++++++++++------ 1 file changed, 113 insertions(+), 59 deletions(-) diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py index 919f3f96c9..a05617ff4c 100644 --- a/src/Object_Tracking_Planning/object_detection.py +++ b/src/Object_Tracking_Planning/object_detection.py @@ -105,113 +105,167 @@ def get_actor_display_name(actor, truncate=250): # ============================================================================== class World(object): - """ Class representing the surrounding environment """ + """Class representing the surrounding environment""" + + # 常量提取 + DEFAULT_VEHICLE_MODEL = 'model3' + DEFAULT_VEHICLE_COLOR = '236,102,17' def __init__(self, carla_world, hud, args): """Constructor method""" self.world = carla_world - try: - self.map = self.world.get_map() - except RuntimeError as error: - print('RuntimeError: {}'.format(error)) - print(' The server could not send the OpenDRIVE (.xodr) file:') - print(' Make sure it exists, has the same name of your town, and is correct.') - sys.exit(1) self.hud = hud + self.args = args + + self._initialize_map() + self._weather_presets = find_weather_presets() + self._weather_index = 0 + + # 初始化所有传感器和玩家为None self.player = None self.collision_sensor = None self.lane_invasion_sensor = None self.gnss_sensor = None self.camera_manager = None - self._weather_presets = find_weather_presets() - self._weather_index = 0 - self._actor_filter = args.filter - self._gamma = args.gamma - self.restart(args) - self.world.on_tick(hud.on_world_tick) + self.recording_enabled = False self.recording_start = 0 - def restart(self, args): + self.restart() + self.world.on_tick(hud.on_world_tick) + + def _initialize_map(self): + """初始化地图,处理可能的错误""" + try: + self.map = self.world.get_map() + except RuntimeError as error: + print(f'RuntimeError: {error}') + print('The server could not send the OpenDRIVE (.xodr) file:') + print('Make sure it exists, has the same name of your town, and is correct.') + sys.exit(1) + + def restart(self): """Restart the world""" - # Keep same camera config if the camera manager exists. - cam_index = self.camera_manager.index if self.camera_manager is not None else 0 - cam_pos_id = self.camera_manager.transform_index if self.camera_manager is not None else 0 - # Set the seed if requested by user - if args.seed is not None: - random.seed(args.seed) - - # KK Get a vehicle blueprint - # blueprint = self.world.get_blueprint_library().filter('vehicle.lincoln.mkz2017')[0] # KK - blueprint = self.world.get_blueprint_library().filter('model3')[0] + # 保存相机状态 + cam_index = self.camera_manager.index if self.camera_manager else 0 + cam_pos_id = self.camera_manager.transform_index if self.camera_manager else 0 + + # 设置随机种子 + if self.args.seed is not None: + random.seed(self.args.seed) + + # 创建并配置车辆 + blueprint = self._create_vehicle_blueprint() + + # 生成玩家车辆 + self._spawn_player(blueprint) + + # 设置传感器 + self._setup_sensors(cam_index, cam_pos_id) + + def _create_vehicle_blueprint(self): + """创建并配置车辆蓝图""" + blueprint_lib = self.world.get_blueprint_library() + blueprint = blueprint_lib.filter(self.DEFAULT_VEHICLE_MODEL)[0] blueprint.set_attribute('role_name', 'hero') + if blueprint.has_attribute('color'): - color = '236,102,17' # KK Packt logo color - blueprint.set_attribute('color', color) - # Spawn the player. - print("Spawning the player") + blueprint.set_attribute('color', self.DEFAULT_VEHICLE_COLOR) + + return blueprint + + def _spawn_player(self, blueprint): + """生成玩家车辆""" + # 如果玩家已存在,在当前位置重新生成 if self.player is not None: - spawn_point = self.player.get_transform() - spawn_point.location.z += 2.0 - spawn_point.rotation.roll = 0.0 - spawn_point.rotation.pitch = 0.0 - self.destroy() + spawn_point = self._get_respawn_point() + self.destroy() # 销毁现有actor self.player = self.world.try_spawn_actor(blueprint, spawn_point) - while self.player is None: - if not self.map.get_spawn_points(): - print('There are no spawn points available in your map/town.') - print('Please add some Vehicle Spawn Point to your UE4 scene.') - sys.exit(1) - spawn_points = self.map.get_spawn_points() - spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform() - # spawn_point = spawn_points[0] if spawn_points else carla.Transform() # KK + # 首次生成或重新生成失败时 + if self.player is None: + spawn_point = self._get_random_spawn_point() self.player = self.world.try_spawn_actor(blueprint, spawn_point) - # Set up the sensors. + + def _get_respawn_point(self): + """获取重新生成点(当前位置上方)""" + spawn_point = self.player.get_transform() + spawn_point.location.z += 2.0 + spawn_point.rotation.roll = 0.0 + spawn_point.rotation.pitch = 0.0 + return spawn_point + + def _get_random_spawn_point(self): + """获取随机生成点""" + spawn_points = self.map.get_spawn_points() + + if not spawn_points: + print('There are no spawn points available in your map/town.') + print('Please add some Vehicle Spawn Point to your UE4 scene.') + sys.exit(1) + + return random.choice(spawn_points) + + def _setup_sensors(self, cam_index, cam_pos_id): + """设置所有传感器""" self.collision_sensor = CollisionSensor(self.player, self.hud) self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud) self.gnss_sensor = GnssSensor(self.player) - self.camera_manager = CameraManager(self.player, self.hud, self._gamma) + + self.camera_manager = CameraManager(self.player, self.hud, self.args.gamma) self.camera_manager.transform_index = cam_pos_id self.camera_manager.set_sensor(cam_index, notify=False) + + # 通知玩家类型 actor_type = get_actor_display_name(self.player) self.hud.notification(actor_type) def next_weather(self, reverse=False): - """Get next weather setting""" + """切换到下一个天气预设""" self._weather_index += -1 if reverse else 1 self._weather_index %= len(self._weather_presets) + preset = self._weather_presets[self._weather_index] - self.hud.notification('Weather: %s' % preset[1]) - self.player.get_world().set_weather(preset[0]) + self.hud.notification(f'Weather: {preset[1]}') + self.world.set_weather(preset[0]) def tick(self, clock): - """Method for every tick""" + """每一帧的更新方法""" self.hud.tick(self, clock) def render(self, display): - """Render world""" + """渲染世界""" self.camera_manager.render(display) self.hud.render(display) def destroy_sensors(self): - """Destroy sensors""" - self.camera_manager.sensor.destroy() - self.camera_manager.sensor = None - self.camera_manager.index = None + """销毁所有传感器""" + if self.camera_manager and self.camera_manager.sensor: + self.camera_manager.sensor.destroy() + self.camera_manager.sensor = None + self.camera_manager.index = None def destroy(self): - """Destroys all actors""" + """销毁所有actor""" actors = [ - self.camera_manager.sensor, - self.collision_sensor.sensor, - self.lane_invasion_sensor.sensor, - self.gnss_sensor.sensor, - self.player] + self.camera_manager.sensor if self.camera_manager else None, + self.collision_sensor.sensor if self.collision_sensor else None, + self.lane_invasion_sensor.sensor if self.lane_invasion_sensor else None, + self.gnss_sensor.sensor if self.gnss_sensor else None, + self.player + ] + for actor in actors: if actor is not None: actor.destroy() + # 重置引用 + self.player = None + self.collision_sensor = None + self.lane_invasion_sensor = None + self.gnss_sensor = None + self.camera_manager = None + # ============================================================================== # -- KeyboardControl ----------------------------------------------------------- From a92adb9cc41362edd4888f86fe0fc7b03cc506a1 Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Fri, 19 Dec 2025 15:19:08 +0800 Subject: [PATCH 07/20] =?UTF-8?q?=E6=9B=B4=E6=96=B0readme=E6=96=87?= =?UTF-8?q?=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md index 202373667b..aa571532a4 100644 --- a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md @@ -29,4 +29,10 @@ outdoor_ws/src/robot_description/ - 封装激光雷达、IMU、GNSS和轮式编码器的数据采集 - 实现多传感器时间同步(10ms容差) - 提供线程安全的数据队列 - - 包含传感器配置管理和模拟器(用于测试) \ No newline at end of file + - 包含传感器配置管理和模拟器(用于测试) +2. **实时估计机器人运动位姿** + - IMU预积分补偿激光雷达运动畸变 + - 从点云提取几何特征(地面/平面/边缘点) + - 扫描匹配(ICP)估计相对运动 + - 融合IMU、激光雷达和GNSS数据 + - 输出高频位姿估计(前端里程计) From 3b3f9b8e1d3180be32e65a8f5438a6529e25c98e Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Fri, 19 Dec 2025 15:52:37 +0800 Subject: [PATCH 08/20] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=9B=A0=E5=AD=90?= =?UTF-8?q?=E5=9B=BE=E4=BC=98=E5=8C=96=E5=90=8E=E7=AB=AF=E6=A8=A1=E5=9D=97?= =?UTF-8?q?=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../factor_graph_backend.py | 1194 +++++++++++++++++ 1 file changed, 1194 insertions(+) create mode 100644 src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/factor_graph_backend.py diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/factor_graph_backend.py b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/factor_graph_backend.py new file mode 100644 index 0000000000..bc3ca19d57 --- /dev/null +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/factor_graph_backend.py @@ -0,0 +1,1194 @@ +# factor_graph_backend.py +import numpy as np +from dataclasses import dataclass +from typing import List, Dict, Tuple, Optional, Any +import time +from collections import deque +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import matplotlib.animation as animation +from matplotlib.patches import FancyArrowPatch +from mpl_toolkits.mplot3d import proj3d +# 在 factor_graph_backend.py 文件开头,import语句后面添加 + +import matplotlib.pyplot as plt + +# 设置中文字体 +try: + # Windows系统字体路径 + plt.rcParams['font.sans-serif'] = ['SimHei', 'Microsoft YaHei', 'SimSun'] + # Mac系统字体路径 + # plt.rcParams['font.sans-serif'] = ['Arial Unicode MS', 'STHeiti', 'Heiti SC'] + plt.rcParams['axes.unicode_minus'] = False # 正确显示负号 +except: + print("警告:无法加载中文字体,将使用默认字体") + pass + +# 设置matplotlib默认参数 +plt.rcParams['figure.figsize'] = [10, 8] +plt.rcParams['figure.dpi'] = 100 +plt.rcParams['savefig.dpi'] = 150 +plt.rcParams['font.size'] = 10 +plt.rcParams['legend.fontsize'] = 9 +plt.rcParams['axes.titlesize'] = 12 +plt.rcParams['axes.labelsize'] = 10 + + +@dataclass +class Factor: + """因子基类""" + factor_type: str + nodes: List[int] # 关联的节点ID + measurement: np.ndarray + information: np.ndarray # 信息矩阵 + + +@dataclass +class Node: + """位姿节点""" + node_id: int + timestamp: float + pose: np.ndarray # 4x4变换矩阵 + fixed: bool = False + + +class FactorGraphVisualizer: + """因子图可视化类""" + + def __init__(self, backend: 'FactorGraphBackend'): + self.backend = backend + self.fig = None + self.ax = None + + def plot_trajectory_2d(self, show_factor_graph: bool = True, + show_submaps: bool = True, + show_loop_closures: bool = True, + save_path: Optional[str] = None): + """绘制2D轨迹图""" + if len(self.backend.nodes) == 0: + print("没有节点可显示") + return + + # 创建图形 + self.fig, (self.ax1, self.ax2) = plt.subplots(1, 2, figsize=(15, 7)) + + # 绘制轨迹 + self._plot_trajectory_2d_on_axis(self.ax1, "优化前轨迹", show_factor_graph, show_submaps, show_loop_closures) + + # 如果有优化结果,绘制优化后的轨迹 + if hasattr(self.backend, 'optimized_poses') and self.backend.optimized_poses: + self._plot_optimized_trajectory_2d(self.ax2) + else: + # 否则绘制节点和因子图 + self._plot_trajectory_2d_on_axis(self.ax2, "节点和因子图", True, False, False) + + plt.tight_layout() + + if save_path: + plt.savefig(save_path, dpi=150, bbox_inches='tight') + print(f"轨迹图已保存到 {save_path}") + + plt.show() + + def _plot_trajectory_2d_on_axis(self, ax, title: str, + show_factor_graph: bool, + show_submaps: bool, + show_loop_closures: bool): + """在指定轴上绘制轨迹""" + # 提取轨迹点 + x_coords = [] + y_coords = [] + timestamps = [] + + for node_id in sorted(self.backend.nodes.keys()): + node = self.backend.nodes[node_id] + pos = node.pose[:3, 3] + x_coords.append(pos[0]) + y_coords.append(pos[1]) + timestamps.append(node.timestamp) + + # 绘制轨迹线 + ax.plot(x_coords, y_coords, 'b-', alpha=0.5, linewidth=1, label='轨迹') + + # 绘制节点点 + ax.scatter(x_coords, y_coords, c=timestamps, cmap='viridis', + s=20, alpha=0.7, label='节点') + + # 绘制固定节点(如果有) + fixed_x = [] + fixed_y = [] + for node_id, node in self.backend.nodes.items(): + if node.fixed: + pos = node.pose[:3, 3] + fixed_x.append(pos[0]) + fixed_y.append(pos[1]) + + if fixed_x: + ax.scatter(fixed_x, fixed_y, c='red', s=100, + marker='*', label='固定节点', zorder=5) + + # 绘制因子图连接 + if show_factor_graph: + self._plot_factor_connections_2d(ax) + + # 绘制子图 + if show_submaps: + self._plot_submaps_2d(ax) + + # 绘制闭环 + if show_loop_closures: + self._plot_loop_closures_2d(ax) + + ax.set_xlabel('X (米)') + ax.set_ylabel('Y (米)') + ax.set_title(title) + ax.grid(True, alpha=0.3) + ax.legend() + ax.axis('equal') + + def _plot_factor_connections_2d(self, ax): + """绘制因子连接""" + # 定义颜色映射 + factor_colors = { + 'odometry': 'green', + 'gnss': 'blue', + 'loop_closure': 'red' + } + + # 绘制因子连接 + for factor in self.backend.factors: + if len(factor.nodes) >= 2: + color = factor_colors.get(factor.factor_type, 'gray') + linewidth = 1.0 + alpha = 0.3 + + # 不同类型因子使用不同样式 + if factor.factor_type == 'loop_closure': + linewidth = 1.5 + alpha = 0.8 + + # 获取节点位置 + positions = [] + for node_id in factor.nodes: + if node_id in self.backend.nodes: + pos = self.backend.nodes[node_id].pose[:3, 3] + positions.append(pos[:2]) # 只取XY + + if len(positions) >= 2: + # 绘制连接线 + for i in range(len(positions) - 1): + ax.plot([positions[i][0], positions[i + 1][0]], + [positions[i][1], positions[i + 1][1]], + color=color, linewidth=linewidth, + alpha=alpha, linestyle='--') + + def _plot_submaps_2d(self, ax): + """绘制子图区域""" + for i, submap in enumerate(self.backend.submaps): + if submap["nodes"]: + # 提取子图节点位置 + positions = [] + for node_id in submap["nodes"]: + if node_id in self.backend.nodes: + pos = self.backend.nodes[node_id].pose[:3, 3] + positions.append(pos[:2]) + + if positions: + positions = np.array(positions) + # 计算边界框 + x_min, y_min = positions.min(axis=0) + x_max, y_max = positions.max(axis=0) + + # 绘制矩形框 + rect = plt.Rectangle((x_min, y_min), + x_max - x_min, + y_max - y_min, + fill=False, + edgecolor='orange', + linewidth=1, + alpha=0.5, + linestyle=':') + ax.add_patch(rect) + + # 添加子图编号 + ax.text(x_min, y_min, f'Submap {i}', + fontsize=8, color='orange', + bbox=dict(boxstyle="round,pad=0.3", + facecolor="white", + alpha=0.7)) + + def _plot_loop_closures_2d(self, ax): + """绘制闭环连接""" + # 从因子中提取闭环 + for factor in self.backend.factors: + if factor.factor_type == 'loop_closure' and len(factor.nodes) >= 2: + node_i, node_j = factor.nodes[:2] + if node_i in self.backend.nodes and node_j in self.backend.nodes: + pos_i = self.backend.nodes[node_i].pose[:3, 3][:2] + pos_j = self.backend.nodes[node_j].pose[:3, 3][:2] + + # 绘制闭环连接线 + ax.plot([pos_i[0], pos_j[0]], + [pos_i[1], pos_j[1]], + color='red', linewidth=2, + alpha=0.8, linestyle='-', + label='闭环' if not ax.get_legend() else '') + + def _plot_optimized_trajectory_2d(self, ax): + """绘制优化后的轨迹""" + # 提取优化前后的轨迹 + orig_x, orig_y = [], [] + opt_x, opt_y = [], [] + + for node_id in sorted(self.backend.nodes.keys()): + # 原始位置 + orig_pos = self.backend.nodes[node_id].pose[:3, 3] + orig_x.append(orig_pos[0]) + orig_y.append(orig_pos[1]) + + # 优化后位置 + if node_id in self.backend.optimized_poses: + opt_pos = self.backend.optimized_poses[node_id][:3, 3] + opt_x.append(opt_pos[0]) + opt_y.append(opt_pos[1]) + + # 绘制轨迹 + ax.plot(orig_x, orig_y, 'b-', alpha=0.3, linewidth=1, label='优化前') + ax.plot(opt_x, opt_y, 'r-', alpha=0.8, linewidth=2, label='优化后') + + # 绘制节点 + ax.scatter(orig_x, orig_y, c='blue', s=20, alpha=0.3, label='原节点') + ax.scatter(opt_x, opt_y, c='red', s=30, alpha=0.6, label='优化节点') + + # 绘制修正箭头 + for i in range(len(orig_x)): + if i < len(opt_x): + ax.arrow(orig_x[i], orig_y[i], + opt_x[i] - orig_x[i], + opt_y[i] - orig_y[i], + head_width=0.1, head_length=0.2, + fc='green', ec='green', alpha=0.3) + + ax.set_xlabel('X (米)') + ax.set_ylabel('Y (米)') + ax.set_title('优化前后轨迹对比') + ax.grid(True, alpha=0.3) + ax.legend() + ax.axis('equal') + + def plot_3d_trajectory(self, show_poses: bool = False, + save_path: Optional[str] = None): + """绘制3D轨迹图""" + if len(self.backend.nodes) == 0: + print("没有节点可显示") + return + + self.fig = plt.figure(figsize=(12, 10)) + self.ax = self.fig.add_subplot(111, projection='3d') + + # 提取轨迹点 + x_coords = [] + y_coords = [] + z_coords = [] + timestamps = [] + + for node_id in sorted(self.backend.nodes.keys()): + node = self.backend.nodes[node_id] + pos = node.pose[:3, 3] + x_coords.append(pos[0]) + y_coords.append(pos[1]) + z_coords.append(pos[2]) + timestamps.append(node.timestamp) + + # 绘制轨迹线 + self.ax.plot(x_coords, y_coords, z_coords, 'b-', alpha=0.5, linewidth=1) + + # 绘制节点点 + scatter = self.ax.scatter(x_coords, y_coords, z_coords, + c=timestamps, cmap='viridis', + s=20, alpha=0.7) + + # 绘制位姿方向(可选) + if show_poses: + self._plot_3d_poses() + + # 绘制因子连接 + self._plot_3d_factor_connections() + + # 添加颜色条 + cbar = self.fig.colorbar(scatter, ax=self.ax, shrink=0.5, aspect=5) + cbar.set_label('时间戳') + + self.ax.set_xlabel('X (米)') + self.ax.set_ylabel('Y (米)') + self.ax.set_zlabel('Z (米)') + self.ax.set_title('3D轨迹图') + + # 设置视角 + self.ax.view_init(elev=30, azim=45) + + plt.tight_layout() + + if save_path: + plt.savefig(save_path, dpi=150, bbox_inches='tight') + print(f"3D轨迹图已保存到 {save_path}") + + plt.show() + + def _plot_3d_poses(self): + """在3D图中绘制位姿方向""" + for node_id in sorted(self.backend.nodes.keys()): + if node_id % 5 == 0: # 每5个节点绘制一个方向 + node = self.backend.nodes[node_id] + pose = node.pose + + # 位置 + pos = pose[:3, 3] + + # X方向(红色) + x_dir = pose[:3, 0] + self.ax.quiver(pos[0], pos[1], pos[2], + x_dir[0] * 0.5, x_dir[1] * 0.5, x_dir[2] * 0.5, + color='r', linewidth=1, alpha=0.6) + + # Y方向(绿色) + y_dir = pose[:3, 1] + self.ax.quiver(pos[0], pos[1], pos[2], + y_dir[0] * 0.5, y_dir[1] * 0.5, y_dir[2] * 0.5, + color='g', linewidth=1, alpha=0.6) + + # Z方向(蓝色) + z_dir = pose[:3, 2] + self.ax.quiver(pos[0], pos[1], pos[2], + z_dir[0] * 0.5, z_dir[1] * 0.5, z_dir[2] * 0.5, + color='b', linewidth=1, alpha=0.6) + + def _plot_3d_factor_connections(self): + """在3D图中绘制因子连接""" + factor_colors = { + 'odometry': 'green', + 'gnss': 'blue', + 'loop_closure': 'red' + } + + for factor in self.backend.factors: + if len(factor.nodes) >= 2: + color = factor_colors.get(factor.factor_type, 'gray') + alpha = 0.3 + linewidth = 1 + + if factor.factor_type == 'loop_closure': + alpha = 0.8 + linewidth = 2 + + # 获取节点位置 + positions = [] + for node_id in factor.nodes: + if node_id in self.backend.nodes: + pos = self.backend.nodes[node_id].pose[:3, 3] + positions.append(pos) + + if len(positions) >= 2: + for i in range(len(positions) - 1): + x = [positions[i][0], positions[i + 1][0]] + y = [positions[i][1], positions[i + 1][1]] + z = [positions[i][2], positions[i + 1][2]] + + self.ax.plot(x, y, z, color=color, + linewidth=linewidth, alpha=alpha, + linestyle='--') + + def plot_error_evolution(self, save_path: Optional[str] = None): + """绘制误差演化图""" + if not hasattr(self.backend, 'optimization_history'): + print("没有优化历史数据") + return + + history = self.backend.optimization_history + + self.fig, axes = plt.subplots(2, 2, figsize=(12, 10)) + + # 总误差 + if 'total_errors' in history: + ax = axes[0, 0] + ax.plot(history['total_errors'], 'b-', linewidth=2) + ax.set_xlabel('迭代次数') + ax.set_ylabel('总误差') + ax.set_title('总误差收敛曲线') + ax.grid(True, alpha=0.3) + + # 位置误差 + if 'position_errors' in history: + ax = axes[0, 1] + ax.plot(history['position_errors'], 'g-', linewidth=2) + ax.set_xlabel('迭代次数') + ax.set_ylabel('位置误差 (米)') + ax.set_title('位置误差收敛曲线') + ax.grid(True, alpha=0.3) + + # 旋转误差 + if 'rotation_errors' in history: + ax = axes[1, 0] + ax.plot(history['rotation_errors'], 'r-', linewidth=2) + ax.set_xlabel('迭代次数') + ax.set_ylabel('旋转误差 (弧度)') + ax.set_title('旋转误差收敛曲线') + ax.grid(True, alpha=0.3) + + # 梯度范数 + if 'gradient_norms' in history: + ax = axes[1, 1] + ax.plot(history['gradient_norms'], 'm-', linewidth=2) + ax.set_xlabel('迭代次数') + ax.set_ylabel('梯度范数') + ax.set_title('梯度范数变化') + ax.grid(True, alpha=0.3) + ax.set_yscale('log') + + plt.tight_layout() + + if save_path: + plt.savefig(save_path, dpi=150, bbox_inches='tight') + print(f"误差演化图已保存到 {save_path}") + + plt.show() + + def animate_trajectory(self, interval: float = 100, + save_path: Optional[str] = None): + """创建轨迹动画""" + if len(self.backend.nodes) < 2: + print("节点数量不足,无法创建动画") + return + + self.fig, self.ax = plt.subplots(figsize=(10, 8)) + + # 提取所有位置 + x_coords = [] + y_coords = [] + node_ids = sorted(self.backend.nodes.keys()) + + for node_id in node_ids: + pos = self.backend.nodes[node_id].pose[:3, 3] + x_coords.append(pos[0]) + y_coords.append(pos[1]) + + # 初始化动画元素 + line, = self.ax.plot([], [], 'b-', linewidth=2, alpha=0.8) + point, = self.ax.plot([], [], 'ro', markersize=8, alpha=0.8) + timestamp_text = self.ax.text(0.05, 0.95, '', transform=self.ax.transAxes) + node_text = self.ax.text(0.05, 0.90, '', transform=self.ax.transAxes) + + # 设置图形范围 + margin = 5.0 + self.ax.set_xlim(min(x_coords) - margin, max(x_coords) + margin) + self.ax.set_ylim(min(y_coords) - margin, max(y_coords) + margin) + self.ax.set_xlabel('X (米)') + self.ax.set_ylabel('Y (米)') + self.ax.set_title('轨迹生成动画') + self.ax.grid(True, alpha=0.3) + self.ax.axis('equal') + + def init(): + """初始化动画""" + line.set_data([], []) + point.set_data([], []) + timestamp_text.set_text('') + node_text.set_text('') + return line, point, timestamp_text, node_text + + def animate(i): + """动画更新函数""" + # 更新轨迹线 + line.set_data(x_coords[:i + 1], y_coords[:i + 1]) + + # 更新当前点 + if i < len(x_coords): + point.set_data([x_coords[i]], [y_coords[i]]) + + # 更新文本 + if i < len(node_ids): + node = self.backend.nodes[node_ids[i]] + timestamp_text.set_text(f'时间戳: {node.timestamp:.2f}s') + node_text.set_text(f'节点ID: {node.node_id}') + + return line, point, timestamp_text, node_text + + # 创建动画 + anim = animation.FuncAnimation(self.fig, animate, init_func=init, + frames=len(node_ids), + interval=interval, blit=True) + + plt.tight_layout() + + # 保存动画 + if save_path: + anim.save(save_path, writer='ffmpeg', fps=30) + print(f"轨迹动画已保存到 {save_path}") + + plt.show() + return anim + + +class FactorGraphBackend: + """因子图优化后端(带可视化功能)""" + + def __init__(self, config=None): + self.config = config or self._default_config() + + # 图结构 + self.nodes = {} # node_id -> Node + self.factors = [] # List[Factor] + + # 子图管理 + self.submaps = [] + self.current_submap = None + + # 优化参数 + self.optimization_interval = 10 # 每10帧优化一次 + self.frame_count = 0 + + # GNSS融合 + self.gnss_reference = None # 局部坐标原点 + + # 优化历史 + self.optimization_history = { + 'total_errors': [], + 'position_errors': [], + 'rotation_errors': [], + 'gradient_norms': [] + } + + # 可视化器 + self.visualizer = FactorGraphVisualizer(self) + + # 优化结果缓存 + self.optimized_poses = {} + + def _default_config(self): + return { + "optimization": { + "max_iterations": 50, + "convergence_threshold": 1e-6, + "use_sparse": True + }, + "loop_closure": { + "search_radius": 10.0, # 米 + "min_chain_length": 10, + "score_threshold": 0.5 + }, + "submap": { + "size": 100, # 每100帧一个子图 + "overlap": 20 # 子图间重叠帧数 + }, + "visualization": { + "show_trajectory": True, + "show_factors": True, + "show_submaps": True, + "show_loop_closures": True + } + } + + def add_node(self, pose: np.ndarray, timestamp: float, + node_type: str = "odometry", fixed: bool = False) -> int: + """添加位姿节点""" + node_id = len(self.nodes) + node = Node( + node_id=node_id, + timestamp=timestamp, + pose=pose.copy(), + fixed=fixed + ) + self.nodes[node_id] = node + + # 添加到当前子图 + if self.current_submap is None: + self.current_submap = { + "start_id": node_id, + "end_id": node_id, + "nodes": [node_id], + "features": [] + } + else: + self.current_submap["nodes"].append(node_id) + self.current_submap["end_id"] = node_id + + # 检查子图是否完成 + if len(self.current_submap["nodes"]) >= self.config["submap"]["size"]: + self.submaps.append(self.current_submap) + # 创建新子图,保持重叠 + overlap_nodes = self.current_submap["nodes"][-self.config["submap"]["overlap"]:] + self.current_submap = { + "start_id": overlap_nodes[0], + "end_id": node_id, + "nodes": overlap_nodes.copy(), + "features": [] + } + + return node_id + + def add_odometry_factor(self, node_i: int, node_j: int, + relative_pose: np.ndarray, covariance: np.ndarray): + """添加里程计因子""" + info_matrix = np.linalg.inv(covariance) + + factor = Factor( + factor_type="odometry", + nodes=[node_i, node_j], + measurement=relative_pose, + information=info_matrix + ) + self.factors.append(factor) + + def add_gnss_factor(self, node_id: int, gnss_position: np.ndarray, + covariance: np.ndarray): + """添加GNSS因子""" + if self.gnss_reference is None: + self.gnss_reference = gnss_position.copy() + + # 转换为局部坐标 + local_pos = gnss_position - self.gnss_reference + + # 创建位置测量(只有位置,没有姿态) + measurement = np.eye(4) + measurement[:3, 3] = local_pos + + info_matrix = np.linalg.inv(covariance) + + factor = Factor( + factor_type="gnss", + nodes=[node_id], + measurement=measurement, + information=info_matrix + ) + self.factors.append(factor) + + def add_loop_closure_factor(self, node_i: int, node_j: int, + relative_pose: np.ndarray, covariance: np.ndarray): + """添加闭环因子""" + info_matrix = np.linalg.inv(covariance) + + factor = Factor( + factor_type="loop_closure", + nodes=[node_i, node_j], + measurement=relative_pose, + information=info_matrix + ) + self.factors.append(factor) + + def detect_loop_closure(self, current_node_id: int, features: np.ndarray) -> List[Tuple[int, np.ndarray]]: + """检测闭环""" + if len(self.submaps) < 2: + return [] + + loops = [] + current_node = self.nodes[current_node_id] + current_pos = current_node.pose[:3, 3] + + # 只在历史子图中搜索 + for submap in self.submaps[:-1]: # 排除当前子图 + # 距离检查 + submap_center = self._get_submap_center(submap) + distance = np.linalg.norm(current_pos - submap_center) + + if distance < self.config["loop_closure"]["search_radius"]: + # 特征匹配 + for node_id in submap["nodes"]: + if node_id == current_node_id: + continue + + # 时间间隔检查(避免匹配太近的帧) + time_diff = abs(current_node.timestamp - self.nodes[node_id].timestamp) + if time_diff < 5.0: # 5秒内不闭环 + continue + + # 特征匹配(简化版本) + match_score = self._feature_matching_score(features, submap["features"]) + + if match_score > self.config["loop_closure"]["score_threshold"]: + # 计算相对位姿 + relative_pose = self._compute_relative_pose( + current_node.pose, + self.nodes[node_id].pose + ) + loops.append((node_id, relative_pose)) + + return loops + + def _feature_matching_score(self, features1, features2): + """计算特征匹配分数(简化版)""" + if len(features1) == 0 or len(features2) == 0: + return 0.0 + + # 在实际系统中,这里应该使用更复杂的特征描述子匹配 + # 这里返回一个随机分数作为示例 + return np.random.uniform(0, 1) + + def _compute_relative_pose(self, pose_i, pose_j): + """计算相对位姿""" + return np.linalg.inv(pose_j) @ pose_i + + def _get_submap_center(self, submap): + """获取子图中心位置""" + positions = [] + for node_id in submap["nodes"]: + if node_id in self.nodes: + positions.append(self.nodes[node_id].pose[:3, 3]) + + if positions: + return np.mean(positions, axis=0) + else: + return np.zeros(3) + + def plot_statistics(self): + """绘制统计信息""" + if len(self.nodes) == 0: + print("没有数据可统计") + return + + fig, axes = plt.subplots(2, 3, figsize=(15, 10)) + + # 1. 节点数量统计 + ax = axes[0, 0] + factor_types = {} + for factor in self.factors: + factor_types[factor.factor_type] = factor_types.get(factor.factor_type, 0) + 1 + + if factor_types: + colors = plt.cm.Set3(np.linspace(0, 1, len(factor_types))) + ax.pie(factor_types.values(), labels=factor_types.keys(), + autopct='%1.1f%%', colors=colors, startangle=90) + ax.set_title('因子类型分布') + else: + ax.text(0.5, 0.5, '无因子数据', ha='center', va='center') + ax.set_title('因子类型分布') + + # 2. 轨迹长度统计 + ax = axes[0, 1] + distances = [] + prev_pos = None + for node_id in sorted(self.nodes.keys()): + pos = self.nodes[node_id].pose[:3, 3] + if prev_pos is not None: + distances.append(np.linalg.norm(pos - prev_pos)) + prev_pos = pos + + if distances: + ax.hist(distances, bins=20, alpha=0.7, color='skyblue', edgecolor='black') + ax.set_xlabel('帧间距离 (米)') + ax.set_ylabel('频次') + ax.set_title('帧间距离分布') + else: + ax.text(0.5, 0.5, '无距离数据', ha='center', va='center') + ax.set_title('帧间距离分布') + + # 3. 时间间隔统计 + ax = axes[0, 2] + time_intervals = [] + timestamps = sorted([node.timestamp for node in self.nodes.values()]) + + if len(timestamps) > 1: + for i in range(1, len(timestamps)): + time_intervals.append(timestamps[i] - timestamps[i - 1]) + + ax.plot(time_intervals, 'g-', alpha=0.7) + ax.fill_between(range(len(time_intervals)), 0, time_intervals, + alpha=0.3, color='green') + ax.set_xlabel('帧序号') + ax.set_ylabel('时间间隔 (秒)') + ax.set_title('时间间隔变化') + ax.grid(True, alpha=0.3) + else: + ax.text(0.5, 0.5, '无时间间隔数据', ha='center', va='center') + ax.set_title('时间间隔变化') + + # 4. 节点分布统计 + ax = axes[1, 0] + if len(self.nodes) > 0: + node_ids = list(self.nodes.keys()) + node_types = ['固定' if node.fixed else '非固定' for node in self.nodes.values()] + + fixed_count = sum(1 for node in self.nodes.values() if node.fixed) + non_fixed_count = len(self.nodes) - fixed_count + + ax.bar(['固定节点', '非固定节点'], [fixed_count, non_fixed_count], + color=['red', 'blue'], alpha=0.7) + ax.set_ylabel('数量') + ax.set_title('节点类型分布') + ax.grid(True, alpha=0.3, axis='y') + else: + ax.text(0.5, 0.5, '无节点数据', ha='center', va='center') + ax.set_title('节点类型分布') + + # 5. 子图统计 + ax = axes[1, 1] + if self.submaps: + submap_sizes = [len(submap['nodes']) for submap in self.submaps] + submap_ids = list(range(len(self.submaps))) + + ax.bar(submap_ids, submap_sizes, color='orange', alpha=0.7) + ax.set_xlabel('子图编号') + ax.set_ylabel('节点数量') + ax.set_title('子图大小分布') + ax.grid(True, alpha=0.3, axis='y') + else: + ax.text(0.5, 0.5, '无子图数据', ha='center', va='center') + ax.set_title('子图大小分布') + + # 6. 优化历史统计(如果有) + ax = axes[1, 2] + if hasattr(self, 'optimization_history') and self.optimization_history.get('total_errors'): + iterations = list(range(len(self.optimization_history['total_errors']))) + ax.plot(iterations, self.optimization_history['total_errors'], + 'b-', linewidth=2, label='总误差') + + if self.optimization_history.get('position_errors'): + ax.plot(iterations, self.optimization_history['position_errors'], + 'g-', linewidth=2, label='位置误差', alpha=0.7) + + if self.optimization_history.get('rotation_errors'): + ax.plot(iterations, self.optimization_history['rotation_errors'], + 'r-', linewidth=2, label='旋转误差', alpha=0.7) + + ax.set_xlabel('迭代次数') + ax.set_ylabel('误差') + ax.set_title('优化收敛曲线') + ax.legend() + ax.grid(True, alpha=0.3) + ax.set_yscale('log') + else: + ax.text(0.5, 0.5, '无优化历史数据', ha='center', va='center') + ax.set_title('优化收敛曲线') + + plt.suptitle('因子图统计信息', fontsize=16, fontweight='bold') + plt.tight_layout() + + # 保存图片 + plt.savefig('factor_graph_statistics.png', dpi=150, bbox_inches='tight') + print("统计图已保存为 'factor_graph_statistics.png'") + + plt.show() + def optimize(self): + """执行图优化""" + print(f"开始优化,节点数: {len(self.nodes)},因子数: {len(self.factors)}") + + # 清空历史记录 + self.optimization_history = { + 'total_errors': [], + 'position_errors': [], + 'rotation_errors': [], + 'gradient_norms': [] + } + + # 简化的优化实现 + # 在实际系统中,应该使用g2o、GTSAM或Ceres等优化库 + + # 这里实现一个简单的梯度下降优化 + for iteration in range(self.config["optimization"]["max_iterations"]): + total_error = 0.0 + position_error = 0.0 + rotation_error = 0.0 + + # 计算每个节点的梯度 + gradients = {node_id: np.zeros(6) for node_id in self.nodes} + + for factor in self.factors: + error, jacobians = self._compute_factor_error(factor) + total_error += np.sum(error ** 2) + + # 计算位置和旋转误差 + if factor.factor_type == 'odometry': + position_error += np.linalg.norm(error[3:]) + rotation_error += np.linalg.norm(error[:3]) + + # 更新梯度 + for i, node_id in enumerate(factor.nodes): + if node_id in gradients: + gradients[node_id] += jacobians[i] @ error + + # 记录历史 + self.optimization_history['total_errors'].append(total_error) + self.optimization_history['position_errors'].append(position_error) + self.optimization_history['rotation_errors'].append(rotation_error) + + # 计算平均梯度范数 + grad_norm = 0.0 + for grad in gradients.values(): + grad_norm += np.linalg.norm(grad) + grad_norm /= len(gradients) + self.optimization_history['gradient_norms'].append(grad_norm) + + # 更新节点位姿 + for node_id, gradient in gradients.items(): + if not self.nodes[node_id].fixed: + # 将梯度转换为位姿更新 + delta_pose = self._gradient_to_pose(gradient, 0.01) + self.nodes[node_id].pose = delta_pose @ self.nodes[node_id].pose + + # 检查收敛 + if total_error < self.config["optimization"]["convergence_threshold"]: + print(f"优化在第{iteration}次迭代收敛") + break + + print(f"优化完成,最终误差: {total_error}") + + # 保存优化结果 + self.optimized_poses = {} + for node_id, node in self.nodes.items(): + self.optimized_poses[node_id] = node.pose.copy() + + # 可视化优化结果 + if self.config["visualization"]["show_trajectory"]: + self.visualize() + + return self.optimized_poses + + def _compute_factor_error(self, factor): + """计算因子误差(简化实现)""" + # 这里应该根据因子类型计算不同的误差 + # 简化实现返回零误差和雅可比 + error = np.zeros(6) + jacobians = [np.eye(6) for _ in factor.nodes] + + return error, jacobians + + def _gradient_to_pose(self, gradient, step_size): + """将梯度转换为位姿更新""" + # 将6维梯度转换为SE(3)变换 + delta_pose = np.eye(4) + + # 旋转部分(前3维) + rotation_vec = gradient[:3] * step_size + if np.linalg.norm(rotation_vec) > 0: + from scipy.spatial.transform import Rotation + delta_rotation = Rotation.from_rotvec(rotation_vec).as_matrix() + delta_pose[:3, :3] = delta_rotation + + # 平移部分(后3维) + delta_pose[:3, 3] = gradient[3:] * step_size + + return delta_pose + + def get_optimized_trajectory(self): + """获取优化后的轨迹""" + trajectory = [] + for node_id in sorted(self.nodes.keys()): + node = self.nodes[node_id] + trajectory.append({ + "timestamp": node.timestamp, + "pose": node.pose, + "position": node.pose[:3, 3] + }) + return trajectory + + def save_map(self, filename): + """保存地图""" + import pickle + + map_data = { + "nodes": self.nodes, + "submaps": self.submaps, + "config": self.config, + "gnss_reference": self.gnss_reference, + "optimized_poses": self.optimized_poses, + "optimization_history": self.optimization_history + } + + with open(filename, 'wb') as f: + pickle.dump(map_data, f) + + print(f"地图已保存到 {filename}") + + def load_map(self, filename): + """加载地图""" + import pickle + + with open(filename, 'rb') as f: + map_data = pickle.load(f) + + self.nodes = map_data["nodes"] + self.submaps = map_data["submaps"] + self.config = map_data["config"] + self.gnss_reference = map_data.get("gnss_reference") + self.optimized_poses = map_data.get("optimized_poses", {}) + self.optimization_history = map_data.get("optimization_history", {}) + + print(f"已加载地图,包含 {len(self.nodes)} 个节点") + + # ==================== 可视化方法 ==================== + + def visualize(self, show_2d: bool = True, show_3d: bool = False, + show_errors: bool = True, show_animation: bool = False, + save_prefix: Optional[str] = None): + """综合可视化功能""" + + if show_2d: + save_path = f"{save_prefix}_2d.png" if save_prefix else None + self.visualizer.plot_trajectory_2d( + show_factor_graph=self.config["visualization"]["show_factors"], + show_submaps=self.config["visualization"]["show_submaps"], + show_loop_closures=self.config["visualization"]["show_loop_closures"], + save_path=save_path + ) + + if show_3d: + save_path = f"{save_prefix}_3d.png" if save_prefix else None + self.visualizer.plot_3d_trajectory(save_path=save_path) + + if show_errors and hasattr(self, 'optimization_history'): + save_path = f"{save_prefix}_errors.png" if save_prefix else None + self.visualizer.plot_error_evolution(save_path=save_path) + + if show_animation: + save_path = f"{save_prefix}_animation.mp4" if save_prefix else None + self.visualizer.animate_trajectory(save_path=save_path) + + def plot_statistics(self): + """绘制统计信息""" + if len(self.nodes) == 0: + print("没有数据可统计") + return + + fig, axes = plt.subplots(2, 3, figsize=(15, 10)) + + # 1. 节点数量统计 + ax = axes[0, 0] + factor_types = {} + for factor in self.factors: + factor_types[factor.factor_type] = factor_types.get(factor.factor_type, 0) + 1 + + colors = plt.cm.Set3(np.linspace(0, 1, len(factor_types))) + ax.pie(factor_types.values(), labels=factor_types.keys(), + autopct='%1.1f%%', colors=colors, startangle=90) + ax.set_title('因子类型分布') + + # 2. 轨迹长度统计 + ax = axes[0, 1] + distances = [] + prev_pos = None + for node_id in sorted(self.nodes.keys()): + pos = self.nodes[node_id].pose[:3, 3] + if prev_pos is not None: + distances.append(np.linalg.norm(pos - prev_pos)) + prev_pos = pos + + if distances: + ax.hist(distances, bins=20, alpha=0.7, color='skyblue', edgecolor='black') + ax.set_xlabel('帧间距离 (米)') + ax.set_ylabel('频次') + ax.set_title('帧间距离分布') + + +# 在 factor_graph_backend.py 文件末尾添加以下代码 + +def main(): + """示例演示函数""" + print("因子图后端可视化演示") + + # 创建因子图后端 + backend = FactorGraphBackend() + + # 添加一些示例节点和因子 + import random + + # 生成一个简单的圆形轨迹 + num_nodes = 50 + radius = 10.0 + center_x, center_y = 0, 0 + + for i in range(num_nodes): + # 生成圆形轨迹上的位姿 + angle = 2 * np.pi * i / num_nodes + + # 位置 + x = center_x + radius * np.cos(angle) + y = center_y + radius * np.sin(angle) + z = 0.0 + + # 姿态(朝向切线方向) + from scipy.spatial.transform import Rotation + + # 切线方向 + dx = -np.sin(angle) + dy = np.cos(angle) + + # 创建旋转矩阵 + rotation = Rotation.from_euler('z', angle).as_matrix() + + # 创建位姿矩阵 + pose = np.eye(4) + pose[:3, :3] = rotation + pose[:3, 3] = [x, y, z] + + # 添加节点 + node_id = backend.add_node(pose, timestamp=i) + + # 添加里程计因子 + if i > 0: + # 计算相对位姿 + prev_pose = backend.nodes[i - 1].pose + relative_pose = np.linalg.inv(prev_pose) @ pose + + # 添加里程计因子 + covariance = np.eye(6) * 0.1 # 协方差矩阵 + backend.add_odometry_factor(i - 1, i, relative_pose, covariance) + + # 添加一些GNSS因子 + for i in range(0, num_nodes, 5): + gnss_pos = backend.nodes[i].pose[:3, 3] + np.random.randn(3) * 0.5 + covariance = np.eye(3) * 1.0 + backend.add_gnss_factor(i, gnss_pos, covariance) + + # 添加一个闭环因子(模拟回环检测) + if num_nodes > 20: + # 连接起点和终点附近的一个点 + loop_i = 0 + loop_j = num_nodes - 10 + + pose_i = backend.nodes[loop_i].pose + pose_j = backend.nodes[loop_j].pose + relative_pose = np.linalg.inv(pose_j) @ pose_i + + covariance = np.eye(6) * 0.05 + backend.add_loop_closure_factor(loop_i, loop_j, relative_pose, covariance) + print(f"添加闭环因子: 节点 {loop_i} <-> 节点 {loop_j}") + + # 创建一个子图(模拟子图分割) + backend.submaps.append({ + "start_id": 0, + "end_id": 19, + "nodes": list(range(20)), + "features": [] + }) + + backend.submaps.append({ + "start_id": 20, + "end_id": 39, + "nodes": list(range(20, 40)), + "features": [] + }) + + # 执行优化 + print("执行优化...") + optimized_poses = backend.optimize() + print(f"优化完成,优化了 {len(optimized_poses)} 个位姿") + + # 显示所有可视化 + print("显示可视化...") + + # 1. 2D轨迹图 + backend.visualize(show_2d=True, show_3d=False, show_errors=True, + save_prefix="factor_graph_demo") + + # 2. 3D轨迹图 + backend.visualize(show_2d=False, show_3d=True, show_errors=False, + save_prefix="factor_graph_demo") + + # 3. 统计信息 + backend.plot_statistics() + + print("演示完成!生成的图片已保存为 'factor_graph_demo_*.png'") + + +# 在文件末尾添加 +if __name__ == "__main__": + main() \ No newline at end of file From 7dfbfeaf46b9bd20bcbff3b1efe5ae8bbc1941ed Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Fri, 19 Dec 2025 17:59:32 +0800 Subject: [PATCH 09/20] =?UTF-8?q?PID=E6=8E=A7=E5=88=B6=E5=99=A8=E5=86=8D?= =?UTF-8?q?=E5=BA=A6=E6=8F=90=E5=8D=87=E6=B8=85=E6=99=B0=E5=BA=A6=E4=BC=98?= =?UTF-8?q?=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../object_detection.py | 116 ++++++++++++------ 1 file changed, 77 insertions(+), 39 deletions(-) diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py index a05617ff4c..27cc7819dc 100644 --- a/src/Object_Tracking_Planning/object_detection.py +++ b/src/Object_Tracking_Planning/object_detection.py @@ -1267,7 +1267,8 @@ def run_step(self, waypoint): -1 = maximum steering to left +1 = maximum steering to right """ - return self._pid_control(waypoint, self._vehicle.get_transform()) + vehicle_transform = self._vehicle.get_transform() + return self._pid_control(waypoint, vehicle_transform) def _pid_control(self, waypoint, vehicle_transform): """ @@ -1280,58 +1281,95 @@ def _pid_control(self, waypoint, vehicle_transform): Returns: float: Steering control in range [-1, 1] """ - # Calculate vehicle direction vector - v_begin = vehicle_transform.location - yaw_rad = math.radians(vehicle_transform.rotation.yaw) - v_end = v_begin + carla.Location( - x=math.cos(yaw_rad), - y=math.sin(yaw_rad) + # 计算当前角度误差 + angle_diff = self._calculate_angle_error(waypoint, vehicle_transform) + + # 更新误差缓冲区 + self._error_buffer.append(angle_diff) + + # 计算PID输出 + pid_output = self._calculate_pid_value(angle_diff) + + # 限制输出范围 + return np.clip(pid_output, -1.0, 1.0) + + def _calculate_angle_error(self, waypoint, vehicle_transform): + """计算车辆当前方向与目标点方向之间的角度误差""" + # 获取车辆位置和方向 + vehicle_pos = vehicle_transform.location + vehicle_yaw = math.radians(vehicle_transform.rotation.yaw) + + # 计算车辆方向向量 + vehicle_direction = carla.Location( + x=math.cos(vehicle_yaw), + y=math.sin(vehicle_yaw) ) + vehicle_vec_end = vehicle_pos + vehicle_direction + + # 计算目标方向向量 + target_pos = waypoint.transform.location + + # 创建向量 + vehicle_vec = np.array([ + vehicle_vec_end.x - vehicle_pos.x, + vehicle_vec_end.y - vehicle_pos.y, + 0.0 + ]) - # Calculate vectors - v_vec = np.array([v_end.x - v_begin.x, v_end.y - v_begin.y, 0.0]) - w_vec = np.array([ - waypoint.transform.location.x - v_begin.x, - waypoint.transform.location.y - v_begin.y, + target_vec = np.array([ + target_pos.x - vehicle_pos.x, + target_pos.y - vehicle_pos.y, 0.0 ]) - # Calculate angle difference - v_norm = np.linalg.norm(v_vec) - w_norm = np.linalg.norm(w_vec) - dot_product = np.dot(w_vec, v_vec) / (v_norm * w_norm) + # 计算角度 + return self._calculate_signed_angle(vehicle_vec, target_vec) + + def _calculate_signed_angle(self, vec1, vec2): + """计算两个向量之间的有符号角度""" + # 归一化向量 + vec1_norm = np.linalg.norm(vec1) + vec2_norm = np.linalg.norm(vec2) + + # 避免除以零 + if vec1_norm < 1e-6 or vec2_norm < 1e-6: + return 0.0 + + # 计算点积和夹角 + dot_product = np.dot(vec1, vec2) / (vec1_norm * vec2_norm) dot_product_clipped = np.clip(dot_product, -1.0, 1.0) - angle_diff = math.acos(dot_product_clipped) + angle = math.acos(dot_product_clipped) - # Determine sign based on cross product - cross_product = np.cross(v_vec, w_vec) + # 根据叉积确定方向(正负) + cross_product = np.cross(vec1, vec2) if cross_product[2] < 0: - angle_diff *= -1.0 + angle = -angle - # Update error buffer and calculate PID terms - self._error_buffer.append(angle_diff) + return angle - # Calculate derivative and integral terms - derivative_error = self._calculate_derivative_error() - integral_error = self._calculate_integral_error() + def _calculate_pid_value(self, current_error): + """计算PID控制器的输出值""" + # 比例项 + proportional = self._k_p * current_error - # PID calculation with clamping - pid_output = ( - self._k_p * angle_diff + - self._k_d * derivative_error + - self._k_i * integral_error - ) + # 微分项 + derivative = self._k_d * self._get_derivative_error() - return np.clip(pid_output, -1.0, 1.0) + # 积分项 + integral = self._k_i * self._get_integral_error() - def _calculate_derivative_error(self): - """Calculate derivative error term.""" - if len(self._error_buffer) >= 2: - return (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt - return 0.0 + return proportional + derivative + integral + + def _get_derivative_error(self): + """获取微分误差项""" + if len(self._error_buffer) < 2: + return 0.0 + return (self._error_buffer[-1] - self._error_buffer[-2]) / self._dt - def _calculate_integral_error(self): - """Calculate integral error term.""" + def _get_integral_error(self): + """获取积分误差项""" + if not self._error_buffer: + return 0.0 return sum(self._error_buffer) * self._dt From 3ac7328179354c4017e28e21e3bd2cb652257bd9 Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Fri, 19 Dec 2025 18:22:01 +0800 Subject: [PATCH 10/20] =?UTF-8?q?=E6=9B=B4=E6=96=B0README=E6=96=87?= =?UTF-8?q?=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md index aa571532a4..b9cc179c85 100644 --- a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md @@ -36,3 +36,9 @@ outdoor_ws/src/robot_description/ - 扫描匹配(ICP)估计相对运动 - 融合IMU、激光雷达和GNSS数据 - 输出高频位姿估计(前端里程计) +3. **全局地图优化与闭环检测** + - 构建因子图优化问题(节点=位姿,因子=约束) + - 融合多种约束:里程计因子、GNSS因子、闭环因子 + - 检测回环并消除累积误差 + - 子图管理优化大范围建图 + - 执行全局优化(类似g2o/GTSAM的功能) \ No newline at end of file From ae3ff1e4c3bb66c3604b9560f2d1ad54994578e0 Mon Sep 17 00:00:00 2001 From: 0023csw <3485741830@qq.com> Date: Fri, 19 Dec 2025 19:00:41 +0800 Subject: [PATCH 11/20] =?UTF-8?q?FadingText=E9=80=8F=E6=98=8E=E5=BA=A6?= =?UTF-8?q?=E8=AE=A1=E7=AE=97=E4=BC=98=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../object_detection.py | 54 +++++++++++++++---- 1 file changed, 45 insertions(+), 9 deletions(-) diff --git a/src/Object_Tracking_Planning/object_detection.py b/src/Object_Tracking_Planning/object_detection.py index ce1e890a4b..3526889a37 100644 --- a/src/Object_Tracking_Planning/object_detection.py +++ b/src/Object_Tracking_Planning/object_detection.py @@ -463,7 +463,13 @@ def render(self, display): class FadingText(object): - """ Class for fading text """ + """Class for fading text""" + + # 常量定义 + ALPHA_MULTIPLIER = 500.0 # 透明度计算系数 + TEXT_OFFSET = (10, 11) # 文本偏移量 + DEFAULT_COLOR = (255, 255, 255) # 默认文本颜色 + DEFAULT_SECONDS = 2.0 # 默认显示时间 def __init__(self, font, dim, pos): """Constructor method""" @@ -471,25 +477,56 @@ def __init__(self, font, dim, pos): self.dim = dim self.pos = pos self.seconds_left = 0 + self._init_surface() + + def _init_surface(self): + """初始化Surface""" self.surface = pygame.Surface(self.dim) + self._clear_surface() + + def _clear_surface(self): + """清空Surface""" + self.surface.fill((0, 0, 0, 0)) - def set_text(self, text, color=(255, 255, 255), seconds=2.0): + def set_text(self, text, color=None, seconds=None): """Set fading text""" + # 使用默认值 + if color is None: + color = self.DEFAULT_COLOR + if seconds is None: + seconds = self.DEFAULT_SECONDS + + # 渲染文本 text_texture = self.font.render(text, True, color) + + # 准备Surface self.surface = pygame.Surface(self.dim) self.seconds_left = seconds - self.surface.fill((0, 0, 0, 0)) - self.surface.blit(text_texture, (10, 11)) + self._clear_surface() + + # 绘制文本 + self.surface.blit(text_texture, self.TEXT_OFFSET) def tick(self, _, clock): """Fading text method for every tick""" - delta_seconds = 1e-3 * clock.get_time() + # 计算经过的时间 + delta_seconds = clock.get_time() / 1000.0 # 转换为秒 + + # 更新时间 self.seconds_left = max(0.0, self.seconds_left - delta_seconds) - self.surface.set_alpha(500.0 * self.seconds_left) + + # 更新透明度 + self._update_alpha() + + def _update_alpha(self): + """更新Surface的透明度""" + alpha_value = self.seconds_left * self.ALPHA_MULTIPLIER + self.surface.set_alpha(alpha_value) def render(self, display): """Render fading text method""" - display.blit(self.surface, self.pos) + if self.seconds_left > 0: + display.blit(self.surface, self.pos) # ============================================================================== @@ -1267,8 +1304,7 @@ def run_step(self, waypoint): -1 = maximum steering to left +1 = maximum steering to right """ - vehicle_transform = self._vehicle.get_transform() - return self._pid_control(waypoint, vehicle_transform) + return self._pid_control(waypoint, self._vehicle.get_transform()) def _pid_control(self, waypoint, vehicle_transform): """ From 0cd6a4c853d69e3311f3c904313300c493be5dbb Mon Sep 17 00:00:00 2001 From: abc123335 <1607504213@qq.com> Date: Fri, 19 Dec 2025 19:23:02 +0800 Subject: [PATCH 12/20] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E6=96=B0=E7=9A=84?= =?UTF-8?q?=E6=A3=80=E6=B5=8B=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- yolo_det.py | 60 +++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 60 insertions(+) create mode 100644 yolo_det.py diff --git a/yolo_det.py b/yolo_det.py new file mode 100644 index 0000000000..d37aa118aa --- /dev/null +++ b/yolo_det.py @@ -0,0 +1,60 @@ +from ultralytics import YOLO +import cv2 +import numpy as np + + +class ObjectDetector: + def __init__(self, model_name='yolov8n.pt'): + """ + 初始化 YOLO 模型 + :param model_name: 模型名称,'yolov8n.pt' 是最快最小的模型,适合CPU + """ + print(f"正在加载 YOLO 模型: {model_name} (首次运行会自动下载)...") + self.model = YOLO(model_name) + + # 定义我们关心的类别ID (COCO数据集) + # 2: car, 3: motorcycle, 5: bus, 7: truck + self.target_classes = [2, 3, 5, 7] + + def detect(self, frame): + """ + 输入一帧图像,返回画好框的图像 + """ + # 运行推理,stream=True 节省内存,verbose=False 关闭打印日志 + results = self.model(frame, stream=True, verbose=False) + + # 在原图的副本上绘图 + annotated_frame = frame.copy() + + for result in results: + boxes = result.boxes + for box in boxes: + # 获取类别ID + cls_id = int(box.cls[0]) + + # 只处理车辆相关的类别 + if cls_id in self.target_classes: + # 获取坐标 (x1, y1, x2, y2) + x1, y1, x2, y2 = box.xyxy[0] + x1, y1, x2, y2 = int(x1), int(y1), int(x2), int(y2) + + # 获取置信度 + conf = float(box.conf[0]) + if conf < 0.3: continue # 过滤掉置信度低的 + + # 获取类别名称 + cls_name = self.model.names[cls_id] + + # 绘制矩形框 (红色) + cv2.rectangle(annotated_frame, (x1, y1), (x2, y2), (0, 0, 255), 2) + + # 绘制标签背景 + label = f"{cls_name} {conf:.2f}" + (w, h), _ = cv2.getTextSize(label, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1) + cv2.rectangle(annotated_frame, (x1, y1 - 20), (x1 + w, y1), (0, 0, 255), -1) + + # 绘制文字 + cv2.putText(annotated_frame, label, (x1, y1 - 5), + cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) + + return annotated_frame \ No newline at end of file From 33026f68028b7c452618afba9977cb195d637b08 Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Sat, 20 Dec 2025 04:35:08 +0800 Subject: [PATCH 13/20] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=85=A8=E5=B1=80?= =?UTF-8?q?=E5=AE=9A=E4=BD=8D=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../global_localization.py | 635 ++++++++++++++++++ 1 file changed, 635 insertions(+) create mode 100644 src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py new file mode 100644 index 0000000000..aa92d0a511 --- /dev/null +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py @@ -0,0 +1,635 @@ +# global_localization.py +import numpy as np +from scipy.spatial import KDTree +from scipy.spatial.transform import Rotation as R +import time +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +import matplotlib.animation as animation +from matplotlib.widgets import Slider, Button + + +class GlobalLocalization: + """全局定位模块""" + + def __init__(self, map_data=None, visualize=True): + self.map_data = map_data + self.map_points = None + self.map_tree = None + self.map_features = None + self.initialized = False + + # 定位状态 + self.current_pose = np.eye(4) + self.confidence = 0.0 + self.localization_mode = "local" # "local" or "global" + + # 扫描上下文描述子 + self.scan_contexts = {} + + # 可视化相关 + self.visualize = visualize + self.fig = None + self.ax = None + self.history_poses = [] + self.history_confidences = [] + self.scan_history = [] + + # 颜色设置 + self.colors = { + 'map': (0.3, 0.3, 0.3, 0.3), # 灰色半透明 + 'current_scan': (1.0, 0.0, 0.0, 0.5), # 红色 + 'matched_scan': (0.0, 1.0, 0.0, 0.5), # 绿色 + 'trajectory': (0.0, 0.0, 1.0, 1.0), # 蓝色 + 'candidates': (1.0, 1.0, 0.0, 0.3), # 黄色 + } + + def initialize_visualization(self): + """初始化可视化窗口""" + if not self.visualize: + return + + plt.ion() # 开启交互模式 + + self.fig = plt.figure(figsize=(15, 10)) + + # 创建3D点云视图 + self.ax1 = self.fig.add_subplot(231, projection='3d') + self.ax1.set_title('3D Point Cloud View') + self.ax1.set_xlabel('X (m)') + self.ax1.set_ylabel('Y (m)') + self.ax1.set_zlabel('Z (m)') + + # 创建2D俯视图 + self.ax2 = self.fig.add_subplot(232) + self.ax2.set_title('2D Top View') + self.ax2.set_xlabel('X (m)') + self.ax2.set_ylabel('Y (m)') + self.ax2.set_aspect('equal') + + # 创建轨迹视图 + self.ax3 = self.fig.add_subplot(233) + self.ax3.set_title('Trajectory and Confidence') + self.ax3.set_xlabel('Step') + self.ax3.set_ylabel('Confidence') + + # 创建扫描上下文视图 + self.ax4 = self.fig.add_subplot(234) + self.ax4.set_title('Scan Context Descriptor') + self.ax4.set_xlabel('Sector') + self.ax4.set_ylabel('Ring') + + # 创建位姿误差视图 + self.ax5 = self.fig.add_subplot(235) + self.ax5.set_title('Pose Error') + self.ax5.set_xlabel('Step') + self.ax5.set_ylabel('Error (m/rad)') + + # 创建ICP匹配视图 + self.ax6 = self.fig.add_subplot(236) + self.ax6.set_title('ICP Matching') + self.ax6.set_xlabel('X (m)') + self.ax6.set_ylabel('Y (m)') + self.ax6.set_aspect('equal') + + plt.tight_layout() + plt.show(block=False) + + def load_map(self, map_points, map_features=None): + """加载全局地图""" + self.map_points = map_points + self.map_features = map_features + + if map_points is not None and len(map_points) > 0: + self.map_tree = KDTree(map_points[:, :3]) # 只使用xyz + self.initialized = True + print(f"地图已加载,包含 {len(map_points)} 个点") + + # 可视化地图 + if self.visualize: + if self.fig is None: + self.initialize_visualization() + self._visualize_map() + else: + print("警告:地图点云为空") + + def _visualize_map(self): + """可视化地图点云""" + if not self.visualize or self.map_points is None: + return + + # 3D视图 + self.ax1.clear() + if len(self.map_points) > 10000: # 太多点的话采样显示 + indices = np.random.choice(len(self.map_points), 10000, replace=False) + display_points = self.map_points[indices] + else: + display_points = self.map_points + + self.ax1.scatter(display_points[:, 0], display_points[:, 1], display_points[:, 2], + c=[self.colors['map']], s=1, marker='.') + + # 2D俯视图 + self.ax2.clear() + self.ax2.scatter(display_points[:, 0], display_points[:, 1], + c=[self.colors['map'][:3]], s=1, marker='.', alpha=0.3) + + self.ax1.set_title(f'Map Points: {len(self.map_points)}') + self.fig.canvas.draw_idle() + + def _visualize_localization(self, scan_points, pose, confidence, + matched_points=None, candidates=None): + """可视化定位结果""" + if not self.visualize: + return + + # 变换当前扫描点 + transformed_scan = self._transform_points(scan_points, pose) + + # 更新历史记录 + self.history_poses.append(pose) + self.history_confidences.append(confidence) + self.scan_history.append(transformed_scan) + + # 限制历史记录长度 + max_history = 50 + if len(self.history_poses) > max_history: + self.history_poses = self.history_poses[-max_history:] + self.history_confidences = self.history_confidences[-max_history:] + self.scan_history = self.scan_history[-max_history:] + + # 1. 3D点云视图 + self.ax1.clear() + if len(self.map_points) > 10000: + indices = np.random.choice(len(self.map_points), 10000, replace=False) + display_map = self.map_points[indices] + else: + display_map = self.map_points + + self.ax1.scatter(display_map[:, 0], display_map[:, 1], display_map[:, 2], + c=[self.colors['map']], s=1, marker='.', alpha=0.3, label='Map') + + # 当前扫描 + self.ax1.scatter(transformed_scan[:, 0], transformed_scan[:, 1], transformed_scan[:, 2], + c='r', s=5, marker='o', alpha=0.5, label='Current Scan') + + # 轨迹 + if len(self.history_poses) > 1: + trajectory = np.array([p[:3, 3] for p in self.history_poses]) + self.ax1.plot(trajectory[:, 0], trajectory[:, 1], trajectory[:, 2], + c=self.colors['trajectory'], linewidth=2, label='Trajectory') + + self.ax1.set_title(f'3D View - Confidence: {confidence:.3f}') + self.ax1.set_xlabel('X (m)') + self.ax1.set_ylabel('Y (m)') + self.ax1.set_zlabel('Z (m)') + self.ax1.legend() + + # 2. 2D俯视图 + self.ax2.clear() + self.ax2.scatter(display_map[:, 0], display_map[:, 1], + c=[self.colors['map'][:3]], s=1, marker='.', alpha=0.3) + + self.ax2.scatter(transformed_scan[:, 0], transformed_scan[:, 1], + c='r', s=10, marker='o', alpha=0.6, label='Current Scan') + + if matched_points is not None: + self.ax2.scatter(matched_points[:, 0], matched_points[:, 1], + c='g', s=20, marker='x', alpha=0.8, label='Matched Points') + + # 轨迹 + if len(self.history_poses) > 1: + trajectory = np.array([p[:3, 3] for p in self.history_poses]) + self.ax2.plot(trajectory[:, 0], trajectory[:, 1], + c=self.colors['trajectory'], linewidth=2, label='Trajectory') + + # 显示方向箭头 + dx, dy = 2 * np.cos(pose[2, 0]), 2 * np.sin(pose[2, 0]) + self.ax2.arrow(pose[0, 3], pose[1, 3], dx, dy, + head_width=0.5, head_length=0.5, fc='blue', ec='blue') + + # 候选位姿 + if candidates is not None: + for i, candidate in enumerate(candidates[:5]): # 只显示前5个 + self.ax2.scatter(candidate[0, 3], candidate[1, 3], + c='y', s=50, marker='*', alpha=0.5) + + self.ax2.set_title(f'2D Top View - Pose: [{pose[0, 3]:.1f}, {pose[1, 3]:.1f}]') + self.ax2.set_xlabel('X (m)') + self.ax2.set_ylabel('Y (m)') + self.ax2.legend() + self.ax2.grid(True, alpha=0.3) + + # 3. 轨迹和置信度 + self.ax3.clear() + if self.history_confidences: + steps = range(len(self.history_confidences)) + self.ax3.plot(steps, self.history_confidences, 'b-', linewidth=2, label='Confidence') + self.ax3.fill_between(steps, 0, self.history_confidences, alpha=0.3) + self.ax3.set_ylim(0, 1.1) + self.ax3.set_xlabel('Step') + self.ax3.set_ylabel('Confidence') + self.ax3.set_title('Localization Confidence History') + self.ax3.grid(True, alpha=0.3) + self.ax3.legend() + + # 4. 扫描上下文 + self.ax4.clear() + current_context = self.create_scan_context(scan_points) + im = self.ax4.imshow(current_context, aspect='auto', cmap='viridis') + self.ax4.set_title('Current Scan Context') + self.ax4.set_xlabel('Sector') + self.ax4.set_ylabel('Ring') + plt.colorbar(im, ax=self.ax4) + + # 5. 位姿误差(如果有真值的话) + self.ax5.clear() + if len(self.history_poses) > 1: + positions = np.array([p[:3, 3] for p in self.history_poses]) + position_changes = np.diff(positions, axis=0) + position_norms = np.linalg.norm(position_changes, axis=1) + + steps = range(len(position_norms)) + self.ax5.plot(steps, position_norms, 'r-', label='Position Change (m)') + + if len(steps) > 0: + self.ax5.axhline(y=np.mean(position_norms), color='r', linestyle='--', + alpha=0.5, label=f'Avg: {np.mean(position_norms):.3f}m') + + self.ax5.set_xlabel('Step') + self.ax5.set_ylabel('Error / Change') + self.ax5.set_title('Pose Changes') + self.ax5.grid(True, alpha=0.3) + self.ax5.legend() + + # 6. ICP匹配视图 + self.ax6.clear() + if matched_points is not None and len(matched_points) > 0: + # 显示匹配点对 + for i in range(min(20, len(transformed_scan))): # 只显示20个匹配对 + scan_pt = transformed_scan[i, :2] + map_pt = matched_points[i, :2] + self.ax6.plot([scan_pt[0], map_pt[0]], [scan_pt[1], map_pt[1]], + 'g-', alpha=0.3, linewidth=0.5) + + self.ax6.scatter(matched_points[:, 0], matched_points[:, 1], + c='g', s=20, marker='x', alpha=0.8, label='Map Points') + + self.ax6.scatter(transformed_scan[:, 0], transformed_scan[:, 1], + c='r', s=10, marker='o', alpha=0.6, label='Scan Points') + + self.ax6.set_title(f'ICP Matching - Inliers: {len(matched_points) if matched_points is not None else 0}') + self.ax6.set_xlabel('X (m)') + self.ax6.set_ylabel('Y (m)') + self.ax6.legend() + self.ax6.grid(True, alpha=0.3) + + plt.tight_layout() + self.fig.canvas.draw_idle() + plt.pause(0.01) # 短暂暂停以更新显示 + + def localize_with_gnss(self, gnss_position, initial_guess=None): + """使用GNSS进行初始全局定位""" + if not self.initialized or self.map_points is None: + return np.eye(4), 0.0 + + # 将GNSS坐标转换为局部坐标 + # 这里需要知道地图的GNSS参考点 + local_position = gnss_position # 简化:假设已经是局部坐标 + + # 在地图中寻找最近的点 + distances, indices = self.map_tree.query(local_position.reshape(1, -1), k=10) + + # 计算平均位置作为初始位姿 + if len(indices) > 0: + nearest_points = self.map_points[indices[0], :3] + estimated_position = np.mean(nearest_points, axis=0) + + # 构建位姿(假设水平) + pose = np.eye(4) + pose[:3, 3] = estimated_position + + confidence = 1.0 / (1.0 + np.mean(distances)) + + # 可视化 + if self.visualize: + self._visualize_localization( + scan_points=np.array([[local_position[0], local_position[1], 0]]), + pose=pose, + confidence=confidence, + matched_points=nearest_points + ) + + return pose, confidence + + return np.eye(4), 0.0 + + def localize_with_scan_matching(self, scan_points, initial_guess=None, visualize=True): + """使用扫描匹配进行定位""" + if not self.initialized or self.map_points is None: + return np.eye(4), 0.0 + + if initial_guess is None: + initial_guess = np.eye(4) + + # 使用ICP进行精配准 + refined_pose, matched_points = self._icp_localization(scan_points, initial_guess) + + # 计算匹配分数 + confidence = self._compute_match_confidence(scan_points, refined_pose) + + # 可视化 + if self.visualize and visualize: + self._visualize_localization( + scan_points=scan_points, + pose=refined_pose, + confidence=confidence, + matched_points=matched_points + ) + + return refined_pose, confidence + + def _icp_localization(self, scan_points, initial_guess, max_iterations=30): + """ICP定位""" + pose = initial_guess.copy() + all_matched_points = None + + for iteration in range(max_iterations): + # 变换扫描点 + transformed_points = self._transform_points(scan_points, pose) + + # 寻找最近邻 + distances, indices = self.map_tree.query(transformed_points[:, :3]) + + # 过滤掉距离太远的点 + valid_mask = distances < 2.0 # 2米阈值 + if np.sum(valid_mask) < 10: # 至少需要10个有效点 + break + + valid_scan = scan_points[valid_mask] + valid_map = self.map_points[indices[valid_mask], :3] + + # 保存匹配点用于可视化 + if iteration == max_iterations - 1: # 最后一次迭代 + all_matched_points = valid_map + + # 计算最优变换 + R_mat, t_vec = self._compute_rigid_transform(valid_scan[:, :3], valid_map) + + # 更新位姿 + delta_pose = np.eye(4) + delta_pose[:3, :3] = R_mat + delta_pose[:3, 3] = t_vec + + pose = delta_pose @ pose + + # 检查收敛 + if np.linalg.norm(t_vec) < 0.001 and np.linalg.norm(R_mat - np.eye(3)) < 0.001: + break + + return pose, all_matched_points + + def _transform_points(self, points, pose): + """变换点云""" + transformed = (pose[:3, :3] @ points[:, :3].T + pose[:3, 3:4]).T + + if points.shape[1] > 3: + transformed = np.hstack([transformed, points[:, 3:]]) + + return transformed + + def _compute_rigid_transform(self, A, B): + """计算刚体变换""" + centroid_A = np.mean(A, axis=0) + centroid_B = np.mean(B, axis=0) + + AA = A - centroid_A + BB = B - centroid_B + + H = AA.T @ BB + U, S, Vt = np.linalg.svd(H) + + R_mat = Vt.T @ U.T + + # 处理反射情况 + if np.linalg.det(R_mat) < 0: + Vt[-1, :] *= -1 + R_mat = Vt.T @ U.T + + t_vec = centroid_B - R_mat @ centroid_A + + return R_mat, t_vec + + def _compute_match_confidence(self, scan_points, pose): + """计算匹配置信度""" + transformed_points = self._transform_points(scan_points, pose) + + distances, _ = self.map_tree.query(transformed_points[:, :3], k=1) + + # 计算内点比例 + inlier_ratio = np.mean(distances < 0.5) # 0.5米内点阈值 + + # 综合置信度 + confidence = min(1.0, inlier_ratio * 2.0) + + return confidence + + def create_scan_context(self, scan_points): + """创建扫描上下文描述子""" + num_sectors = 60 + num_rings = 20 + max_range = 50.0 + + descriptor = np.zeros((num_rings, num_sectors)) + + for point in scan_points: + x, y, z = point[:3] + + # 转换为极坐标 + radius = np.sqrt(x ** 2 + y ** 2) + angle = np.arctan2(y, x) + + if radius > max_range: + continue + + # 计算环和扇区索引 + ring_idx = min(int(radius / max_range * num_rings), num_rings - 1) + sector_idx = int((angle + np.pi) / (2 * np.pi) * num_sectors) % num_sectors + + # 存储最大高度 + height = z + descriptor[ring_idx, sector_idx] = max(descriptor[ring_idx, sector_idx], height) + + return descriptor + + def scan_context_match(self, current_descriptor): + """扫描上下文匹配""" + if not self.scan_contexts: + return None, 0.0 + + best_match = None + best_score = 0.0 + + for node_id, descriptor in self.scan_contexts.items(): + # 计算描述子相似度 + score = self._descriptor_similarity(current_descriptor, descriptor) + + if score > best_score: + best_score = score + best_match = node_id + + return best_match, best_score + + def _descriptor_similarity(self, desc1, desc2): + """计算描述子相似度""" + # 列向量的余弦相似度 + similarity = 0.0 + + for i in range(desc1.shape[1]): + col1 = desc1[:, i] + col2 = desc2[:, i] + + norm1 = np.linalg.norm(col1) + norm2 = np.linalg.norm(col2) + + if norm1 > 0 and norm2 > 0: + similarity += np.dot(col1, col2) / (norm1 * norm2) + + return similarity / desc1.shape[1] + + def update_scan_context(self, node_id, descriptor): + """更新扫描上下文数据库""" + self.scan_contexts[node_id] = descriptor + + def global_relocalization(self, scan_points, gnss_hint=None): + """全局重定位""" + if not self.initialized: + return np.eye(4), 0.0 + + # 方法1:使用扫描上下文 + current_descriptor = self.create_scan_context(scan_points) + matched_node, context_score = self.scan_context_match(current_descriptor) + + if context_score > 0.7: # 高置信度匹配 + print(f"扫描上下文匹配成功,分数: {context_score}") + # 可以返回匹配节点的位姿 + return np.eye(4), context_score + + # 方法2:使用GNSS提示 + if gnss_hint is not None: + pose, confidence = self.localize_with_gnss(gnss_hint) + if confidence > 0.5: + print(f"GNSS提示定位成功,置信度: {confidence}") + return pose, confidence + + # 方法3:全局ICP(计算量大,作为最后手段) + print("尝试全局ICP重定位...") + + # 在地图中采样一些候选位姿 + candidates = self._generate_candidates(scan_points) + + best_pose = np.eye(4) + best_score = 0.0 + best_matched_points = None + + for candidate in candidates: + pose, matched_points = self._icp_localization(scan_points, candidate, max_iterations=10) + score = self._compute_match_confidence(scan_points, pose) + + if score > best_score: + best_score = score + best_pose = pose + best_matched_points = matched_points + + # 可视化候选位姿和最佳匹配 + if self.visualize: + self._visualize_localization( + scan_points=scan_points, + pose=best_pose, + confidence=best_score, + matched_points=best_matched_points, + candidates=candidates + ) + + if best_score > 0.3: + print(f"全局ICP重定位成功,分数: {best_score}") + return best_pose, best_score + + print("重定位失败") + return np.eye(4), 0.0 + + def _generate_candidates(self, scan_points, num_candidates=20): + """生成候选位姿""" + candidates = [] + + # 在地图边界内随机采样 + if self.map_points is not None and len(self.map_points) > 0: + map_min = np.min(self.map_points[:, :3], axis=0) + map_max = np.max(self.map_points[:, :3], axis=0) + + for _ in range(num_candidates): + position = np.random.uniform(map_min, map_max) + yaw = np.random.uniform(-np.pi, np.pi) + + pose = np.eye(4) + pose[:3, 3] = position + pose[:3, :3] = R.from_euler('z', yaw).as_matrix() + + candidates.append(pose) + + return candidates + + def save_visualization(self, filename="localization_result.png"): + """保存可视化结果""" + if self.fig is not None: + self.fig.savefig(filename, dpi=300, bbox_inches='tight') + print(f"可视化结果已保存到 {filename}") + + def close_visualization(self): + """关闭可视化窗口""" + if self.fig is not None: + plt.close(self.fig) + self.fig = None + + +# 使用示例 +def main(): + # 创建模拟地图数据 + map_points = np.random.randn(10000, 3) * 50 # 10000个随机点 + + # 创建定位器并启用可视化 + locator = GlobalLocalization(visualize=True) + locator.load_map(map_points) + + # 模拟扫描数据 + for i in range(50): + # 创建模拟扫描(部分地图点加上噪声) + scan_indices = np.random.choice(len(map_points), 100) + scan_points = map_points[scan_indices] + np.random.randn(100, 3) * 0.1 + + # 模拟运动 + motion = np.eye(4) + motion[:3, 3] = np.array([0.5, 0.2, 0]) # 平移 + if i > 0: + locator.current_pose = motion @ locator.current_pose + + # 进行定位 + pose, confidence = locator.localize_with_scan_matching( + scan_points, + locator.current_pose + ) + + print(f"Step {i}: Confidence = {confidence:.3f}") + + time.sleep(0.1) # 模拟实时处理 + + # 保存结果 + locator.save_visualization() + + # 保持窗口打开 + print("按任意键关闭窗口...") + plt.show(block=True) + + +if __name__ == "__main__": + main() \ No newline at end of file From c45abac20f7c69e6a5ebf11d32fab78bbd11073f Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Sat, 20 Dec 2025 20:02:11 +0800 Subject: [PATCH 14/20] =?UTF-8?q?=E5=88=A0=E9=99=A4=E5=A4=9A=E4=BD=99?= =?UTF-8?q?=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- yolo_det.py | 60 ----------------------------------------------------- 1 file changed, 60 deletions(-) delete mode 100644 yolo_det.py diff --git a/yolo_det.py b/yolo_det.py deleted file mode 100644 index d37aa118aa..0000000000 --- a/yolo_det.py +++ /dev/null @@ -1,60 +0,0 @@ -from ultralytics import YOLO -import cv2 -import numpy as np - - -class ObjectDetector: - def __init__(self, model_name='yolov8n.pt'): - """ - 初始化 YOLO 模型 - :param model_name: 模型名称,'yolov8n.pt' 是最快最小的模型,适合CPU - """ - print(f"正在加载 YOLO 模型: {model_name} (首次运行会自动下载)...") - self.model = YOLO(model_name) - - # 定义我们关心的类别ID (COCO数据集) - # 2: car, 3: motorcycle, 5: bus, 7: truck - self.target_classes = [2, 3, 5, 7] - - def detect(self, frame): - """ - 输入一帧图像,返回画好框的图像 - """ - # 运行推理,stream=True 节省内存,verbose=False 关闭打印日志 - results = self.model(frame, stream=True, verbose=False) - - # 在原图的副本上绘图 - annotated_frame = frame.copy() - - for result in results: - boxes = result.boxes - for box in boxes: - # 获取类别ID - cls_id = int(box.cls[0]) - - # 只处理车辆相关的类别 - if cls_id in self.target_classes: - # 获取坐标 (x1, y1, x2, y2) - x1, y1, x2, y2 = box.xyxy[0] - x1, y1, x2, y2 = int(x1), int(y1), int(x2), int(y2) - - # 获取置信度 - conf = float(box.conf[0]) - if conf < 0.3: continue # 过滤掉置信度低的 - - # 获取类别名称 - cls_name = self.model.names[cls_id] - - # 绘制矩形框 (红色) - cv2.rectangle(annotated_frame, (x1, y1), (x2, y2), (0, 0, 255), 2) - - # 绘制标签背景 - label = f"{cls_name} {conf:.2f}" - (w, h), _ = cv2.getTextSize(label, cv2.FONT_HERSHEY_SIMPLEX, 0.5, 1) - cv2.rectangle(annotated_frame, (x1, y1 - 20), (x1 + w, y1), (0, 0, 255), -1) - - # 绘制文字 - cv2.putText(annotated_frame, label, (x1, y1 - 5), - cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1) - - return annotated_frame \ No newline at end of file From 9e4fbad65e71e6b89a899ae0bf007bf4774ab3db Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Sun, 21 Dec 2025 14:43:51 +0800 Subject: [PATCH 15/20] =?UTF-8?q?=E6=9B=B4=E6=96=B0readme=E6=96=87?= =?UTF-8?q?=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md index b9cc179c85..32c04dfab0 100644 --- a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md @@ -41,4 +41,10 @@ outdoor_ws/src/robot_description/ - 融合多种约束:里程计因子、GNSS因子、闭环因子 - 检测回环并消除累积误差 - 子图管理优化大范围建图 - - 执行全局优化(类似g2o/GTSAM的功能) \ No newline at end of file + - 执行全局优化(类似g2o/GTSAM的功能) +4. **重定位与全局位姿估计** + - 从先验地图中定位当前扫描 + - 支持多种定位方式:GNSS提示、扫描匹配、扫描上下文 + - 处理"绑架问题"(全局重定位) + - 提供定位置信度评估 + - 构建和维护全局特征描述子数据库 From cd0fb3195f6592c77ff8111b33e0db4b791c0bd3 Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Sun, 21 Dec 2025 19:34:54 +0800 Subject: [PATCH 16/20] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=85=A8=E5=B1=80?= =?UTF-8?q?=E5=AF=BC=E8=88=AA=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../autonomous_navigation.py | 1135 +++++++++++++++++ 1 file changed, 1135 insertions(+) create mode 100644 src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/autonomous_navigation.py diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/autonomous_navigation.py b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/autonomous_navigation.py new file mode 100644 index 0000000000..9364153a38 --- /dev/null +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/autonomous_navigation.py @@ -0,0 +1,1135 @@ +# autonomous_navigation.py +import numpy as np +from queue import PriorityQueue +from dataclasses import dataclass +from typing import List, Tuple, Optional +import heapq +import matplotlib.pyplot as plt +import matplotlib.patches as patches +import matplotlib.gridspec as gridspec +from matplotlib.patches import Circle, Rectangle, Polygon +import matplotlib +import time +import threading +from scipy.spatial.transform import Rotation +import random +import os + + +# ==================== 设置中文字体 ==================== +def setup_chinese_font(): + """设置中文字体支持""" + # 尝试不同的方法设置中文字体 + try: + # 方法1: 使用系统自带的中文字体 + font_names = ['SimHei', 'Microsoft YaHei', 'Arial Unicode MS', 'DejaVu Sans'] + + # 获取系统中文字体 + system_fonts = matplotlib.font_manager.findSystemFonts(fontpaths=None, fontext='ttf') + chinese_fonts = [f for f in system_fonts if any(name in f for name in ['SimHei', 'YaHei', 'msyh', 'arial'])] + + if chinese_fonts: + # 添加字体路径 + for font_path in chinese_fonts[:3]: # 尝试前3个 + matplotlib.font_manager.fontManager.addfont(font_path) + font_name = matplotlib.font_manager.FontProperties(fname=font_path).get_name() + font_names.append(font_name) + + # 尝试设置字体 + for font_name in font_names: + try: + plt.rcParams['font.sans-serif'] = [font_name] + plt.rcParams['axes.unicode_minus'] = False + print(f"已设置字体: {font_name}") + return True + except: + continue + + # 如果都不行,使用默认字体显示英文 + print("警告: 未找到中文字体,将使用英文字体") + plt.rcParams['font.sans-serif'] = ['DejaVu Sans'] + plt.rcParams['axes.unicode_minus'] = False + return False + + except Exception as e: + print(f"字体设置错误: {e}") + # 使用英文字体 + plt.rcParams['font.sans-serif'] = ['DejaVu Sans'] + plt.rcParams['axes.unicode_minus'] = False + return False + + +# 在程序开始前设置字体 +setup_chinese_font() +@dataclass +class GridCell: + x: int + y: int + cost: float = float('inf') + parent: Optional[Tuple[int, int]] = None + visited: bool = False + + +class AutonomousNavigation: + """自主导航模块""" + + def __init__(self, config=None): + self.config = config or self._default_config() + + # 地图相关 + self.occupancy_grid = None + self.costmap = None + self.resolution = 0.05 # 米/像素,提高分辨率 + + # 路径规划 + self.global_path = None + self.local_path = None + + # 控制参数 + self.current_speed = 0.0 + self.current_angular = 0.0 + self.target_speed = 0.0 + self.target_angular = 0.0 + self.max_acceleration = 0.5 # m/s² + + # 机器人状态 + self.robot_pose = np.eye(4) + self.goal_pose = np.eye(4) + self.goal_pose[0, 3] = 5.0 # 默认目标在X方向5米处 + self.goal_pose[1, 3] = 5.0 # Y方向5米处 + + # 避障 + self.obstacle_buffer = [] + self.safety_distance = 0.5 # 米 + self.obstacles = [] # 障碍物列表 + + # 导航状态 + self.navigation_active = False + self.goal_reached = False + self.path_found = False + self.collision_warning = False + + # 可视化相关 + self.fig = None + self.ax_map = None + self.ax_path = None + self.ax_info = None + self.trajectory_history = [] + + # 初始化障碍物环境 + self._initialize_environment() + + # 导航线程 + self.navigation_thread = None + + def _default_config(self): + return { + "planning": { + "inflation_radius": 0.3, + "path_smoothing": True, + "replan_interval": 1.0, # 重规划间隔缩短 + "goal_tolerance": 0.2, # 目标容忍距离 + }, + "control": { + "max_linear_speed": 0.5, # 降低最大速度便于观察 + "max_angular_speed": 1.0, + "lookahead_distance": 0.8, + "pid_kp": 1.0, # PID参数 + "pid_ki": 0.1, + "pid_kd": 0.05, + }, + "obstacle": { + "max_obstacle_height": 0.5, + "min_obstacle_height": 0.05, + "inflation_layers": 3 + } + } + + def _initialize_environment(self): + """初始化模拟环境""" + # 创建一些随机障碍物 + self.obstacles = [ + # 矩形障碍物 (x, y, width, height) + (1.5, 1.5, 0.8, 0.3), + (2.5, 3.0, 0.5, 1.0), + (4.0, 2.0, 0.3, 0.8), + (3.0, 4.5, 1.0, 0.4), + (0.5, 3.5, 0.6, 0.6), + ] + + # 设置随机目标点 + self.goal_pose = np.eye(4) + self.goal_pose[0, 3] = random.uniform(3, 6) + self.goal_pose[1, 3] = random.uniform(3, 6) + + print(f"目标点位置: ({self.goal_pose[0, 3]:.2f}, {self.goal_pose[1, 3]:.2f})") + + def start_navigation(self, start_pose=None, goal_pose=None): + """开始自主导航""" + if start_pose is not None: + self.robot_pose = start_pose.copy() + + if goal_pose is not None: + self.goal_pose = goal_pose.copy() + + self.navigation_active = True + self.goal_reached = False + self.path_found = False + self.collision_warning = False + + print(f"开始导航: 起点({self.robot_pose[0, 3]:.2f}, {self.robot_pose[1, 3]:.2f}) -> " + f"目标({self.goal_pose[0, 3]:.2f}, {self.goal_pose[1, 3]:.2f})") + + # 启动导航线程 + self.navigation_thread = threading.Thread(target=self._navigation_loop) + self.navigation_thread.daemon = True + self.navigation_thread.start() + + return True + + def stop_navigation(self): + """停止导航""" + self.navigation_active = False + self.target_speed = 0.0 + self.target_angular = 0.0 + print("导航已停止") + + def _navigation_loop(self): + """导航主循环""" + last_replan_time = time.time() + replan_interval = self.config["planning"]["replan_interval"] + + while self.navigation_active and not self.goal_reached: + try: + # 1. 更新感知 + self._update_perception() + + # 2. 检查是否到达目标 + if self._check_goal_reached(): + self.goal_reached = True + print("成功到达目标点!") + self.target_speed = 0.0 + self.target_angular = 0.0 + break + + # 3. 碰撞检测 + if self._check_collision(): + self.collision_warning = True + print("警告:检测到碰撞风险!") + # 执行紧急避障 + self._emergency_avoidance() + continue + else: + self.collision_warning = False + + # 4. 路径规划(按需重规划) + current_time = time.time() + if (not self.path_found or + (current_time - last_replan_time > replan_interval) or + self.global_path is None): + + print("执行路径规划...") + if self.global_path_planning(self.robot_pose, self.goal_pose): + self.path_found = True + last_replan_time = current_time + else: + print("路径规划失败,尝试绕行...") + self._recover_from_planning_failure() + continue + + # 5. 局部路径规划 + if self.path_found and self.global_path is not None: + result = self.local_path_planning( + self.global_path, self.robot_pose, self.obstacles + ) + + if result is not None: + local_path, control_cmd = result + self.target_speed, self.target_angular = control_cmd + + # 应用PID控制 + self._apply_pid_control() + else: + print("局部路径规划失败") + self.target_speed = 0.0 + self.target_angular = 0.0 + + # 6. 更新机器人位置(模拟移动) + self._update_robot_position() + + # 7. 更新轨迹历史 + robot_pos = self.robot_pose[:3, 3] + self.trajectory_history.append((robot_pos[0], robot_pos[1])) + if len(self.trajectory_history) > 200: + self.trajectory_history.pop(0) + + # 控制循环频率 + time.sleep(0.05) # 20Hz + + except Exception as e: + print(f"导航循环错误: {e}") + import traceback + traceback.print_exc() + time.sleep(0.1) + + self.navigation_active = False + print("导航循环结束") + + def _update_perception(self): + """更新感知信息(模拟传感器数据)""" + # 生成模拟点云数据 + point_cloud = self._generate_simulated_pointcloud() + + # 更新占据栅格 + self.update_occupancy_grid(point_cloud, self.robot_pose) + + return point_cloud + + def _generate_simulated_pointcloud(self): + """生成模拟点云数据""" + point_cloud = [] + + # 添加障碍物点 + for obs in self.obstacles: + x, y, w, h = obs + # 在障碍物表面采样点 + num_points = 20 + for _ in range(num_points): + px = x + random.uniform(-w / 2, w / 2) + py = y + random.uniform(-h / 2, h / 2) + # 转换为机器人坐标系 + robot_pos = self.robot_pose[:3, 3] + dx = px - robot_pos[0] + dy = py - robot_pos[1] + distance = np.sqrt(dx ** 2 + dy ** 2) + + # 只添加在传感器范围内的点 + if distance < 3.0: # 3米范围内 + point_cloud.append([dx, dy, 0.1]) + + # 添加一些随机噪声点 + for _ in range(10): + angle = random.uniform(0, 2 * np.pi) + distance = random.uniform(0.5, 3.0) + x = distance * np.cos(angle) + y = distance * np.sin(angle) + point_cloud.append([x, y, 0.1]) + + return point_cloud + + def _check_goal_reached(self): + """检查是否到达目标""" + robot_pos = self.robot_pose[:3, 3] + goal_pos = self.goal_pose[:3, 3] + + distance = np.linalg.norm(robot_pos[:2] - goal_pos[:2]) + tolerance = self.config["planning"]["goal_tolerance"] + + return distance < tolerance + + def _check_collision(self): + """碰撞检测""" + if self.occupancy_grid is None: + return False + + robot_grid = self._pose_to_grid(self.robot_pose) + grid_size = self.occupancy_grid.shape[0] + + # 检查机器人周围的栅格 + check_radius = int(self.safety_distance / self.resolution) + + for dx in range(-check_radius, check_radius + 1): + for dy in range(-check_radius, check_radius + 1): + nx, ny = robot_grid[0] + dx, robot_grid[1] + dy + distance = np.sqrt(dx ** 2 + dy ** 2) * self.resolution + + if distance <= self.safety_distance: + if 0 <= nx < grid_size and 0 <= ny < grid_size: + if self.occupancy_grid[nx, ny] > 0.3: + return True + + return False + + def _emergency_avoidance(self): + """紧急避障""" + print("执行紧急避障...") + + # 后退并转向 + self.target_speed = -0.1 # 后退 + self.target_angular = random.choice([-0.5, 0.5]) # 随机转向 + + # 应用控制 + self._apply_pid_control() + self._update_robot_position() + + # 短暂避障后停止 + time.sleep(0.5) + self.target_speed = 0.0 + self.target_angular = 0.0 + + # 标记需要重新规划 + self.path_found = False + + def _recover_from_planning_failure(self): + """从规划失败中恢复""" + # 尝试随机移动以摆脱困境 + self.target_speed = 0.1 + self.target_angular = random.uniform(-1.0, 1.0) + + self._apply_pid_control() + self._update_robot_position() + + time.sleep(0.3) + self.target_speed = 0.0 + self.target_angular = 0.0 + + def _apply_pid_control(self): + """应用PID控制更新实际速度""" + # 简化的速度控制 + acceleration = self.max_acceleration + dt = 0.05 + + # 线性速度控制 + speed_error = self.target_speed - self.current_speed + self.current_speed += np.clip(speed_error, -acceleration * dt, acceleration * dt) + + # 角速度控制 + angular_error = self.target_angular - self.current_angular + self.current_angular += np.clip(angular_error, -2.0 * dt, 2.0 * dt) + + def _update_robot_position(self): + """更新机器人位置(模拟运动)""" + # 获取当前位姿 + pos = self.robot_pose[:3, 3] + rot = self.robot_pose[:3, :3] + + # 获取偏航角 + r = Rotation.from_matrix(rot) + yaw = r.as_euler('zyx')[0] + + # 更新位置 + dt = 0.05 + dx = self.current_speed * np.cos(yaw) * dt + dy = self.current_speed * np.sin(yaw) * dt + dtheta = self.current_angular * dt + + # 更新位置 + self.robot_pose[0, 3] += dx + self.robot_pose[1, 3] += dy + + # 更新朝向 + new_yaw = yaw + dtheta + new_rot = Rotation.from_euler('z', new_yaw).as_matrix() + self.robot_pose[:3, :3] = new_rot + + # 确保位置在合理范围内 + self.robot_pose[0, 3] = np.clip(self.robot_pose[0, 3], -2.0, 8.0) + self.robot_pose[1, 3] = np.clip(self.robot_pose[1, 3], -2.0, 8.0) + + # ============== 原有的路径规划方法 ============== + + def update_occupancy_grid(self, point_cloud, robot_pose): + """从点云更新占据栅格地图""" + if point_cloud is None or len(point_cloud) == 0: + return + + # 初始化栅格地图 + grid_size = 200 # 200x200网格,提高分辨率 + if self.occupancy_grid is None: + self.occupancy_grid = np.zeros((grid_size, grid_size), dtype=np.float32) + self.costmap = np.ones((grid_size, grid_size), dtype=np.float32) + + # 清空地图 + self.occupancy_grid.fill(0.0) + self.costmap.fill(1.0) + + # 机器人位置(地图中心) + robot_x, robot_y = grid_size // 2, grid_size // 2 + + # 将点云转换到地图坐标系 + for point in point_cloud: + x, y, z = point[:3] + + # 过滤障碍物高度 + if self.config["obstacle"]["min_obstacle_height"] < z < self.config["obstacle"]["max_obstacle_height"]: + # 转换为栅格坐标 + grid_x = int(robot_x + x / self.resolution) + grid_y = int(robot_y + y / self.resolution) + + # 检查边界 + if 0 <= grid_x < grid_size and 0 <= grid_y < grid_size: + self.occupancy_grid[grid_x, grid_y] = 1.0 + + # 添加静态障碍物 + self._add_static_obstacles_to_grid() + + # 生成代价地图 + self._generate_costmap() + + def _add_static_obstacles_to_grid(self): + """将静态障碍物添加到栅格地图""" + if self.occupancy_grid is None: + return + + grid_size = self.occupancy_grid.shape[0] + robot_x, robot_y = grid_size // 2, grid_size // 2 + robot_pos = self.robot_pose[:3, 3] + + for obs in self.obstacles: + x, y, w, h = obs + + # 转换为相对于机器人的坐标 + dx = x - robot_pos[0] + dy = y - robot_pos[1] + + # 转换为栅格坐标 + grid_x = int(robot_x + dx / self.resolution) + grid_y = int(robot_y + dy / self.resolution) + + # 计算障碍物在栅格中的大小 + grid_w = int(w / self.resolution) + grid_h = int(h / self.resolution) + + # 填充障碍物区域 + for i in range(-grid_w // 2, grid_w // 2 + 1): + for j in range(-grid_h // 2, grid_h // 2 + 1): + nx, ny = grid_x + i, grid_y + j + if 0 <= nx < grid_size and 0 <= ny < grid_size: + self.occupancy_grid[nx, ny] = 1.0 + + def _generate_costmap(self): + """生成代价地图(包含障碍物膨胀)""" + if self.occupancy_grid is None: + return + + grid_size = self.occupancy_grid.shape[0] + inflation_radius = int(self.config["planning"]["inflation_radius"] / self.resolution) + + # 创建膨胀后的障碍物地图 + inflated_grid = self.occupancy_grid.copy() + + for i in range(grid_size): + for j in range(grid_size): + if self.occupancy_grid[i, j] > 0.5: # 障碍物 + # 膨胀操作 + for di in range(-inflation_radius, inflation_radius + 1): + for dj in range(-inflation_radius, inflation_radius + 1): + distance = np.sqrt(di ** 2 + dj ** 2) + if distance <= inflation_radius: + ni, nj = i + di, j + dj + if 0 <= ni < grid_size and 0 <= nj < grid_size: + # 距离越近,代价越高 + cost = 1.0 - (distance / inflation_radius) * 0.8 + inflated_grid[ni, nj] = max(inflated_grid[ni, nj], cost) + + self.costmap = 1.0 - inflated_grid # 转换为通行代价(0为高代价,1为低代价) + + def global_path_planning(self, start_pose, goal_pose): + """全局路径规划(A*算法)""" + # 转换为栅格坐标 + start_grid = self._pose_to_grid(start_pose) + goal_grid = self._pose_to_grid(goal_pose) + + # 检查目标是否可达 + if not self._is_cell_free(goal_grid[0], goal_grid[1]): + print("目标点被障碍物占据") + return None + + # A*算法 + open_set = PriorityQueue() + closed_set = set() + + # 初始化起始节点 + start_node = GridCell(start_grid[0], start_grid[1], 0, None) + start_f = self._heuristic(start_grid, goal_grid) + open_set.put((start_f, start_node)) + + while not open_set.empty(): + current_f, current_node = open_set.get() + + # 到达目标 + if (current_node.x, current_node.y) == goal_grid: + self.global_path = self._reconstruct_path(current_node) + return self.global_path + + closed_set.add((current_node.x, current_node.y)) + + # 扩展邻居节点 + for dx, dy in [(1, 0), (-1, 0), (0, 1), (0, -1), + (1, 1), (-1, 1), (1, -1), (-1, -1)]: + nx, ny = current_node.x + dx, current_node.y + dy + + # 检查边界和障碍物 + if not self._is_cell_free(nx, ny): + continue + + # 计算移动代价 + move_cost = 1.0 if dx == 0 or dy == 0 else 1.414 # 对角线代价 + cell_cost = self.costmap[nx, ny] + total_cost = current_node.cost + move_cost * (2.0 - cell_cost) # 代价高的区域惩罚 + + # 检查是否已在closed_set + if (nx, ny) in closed_set: + continue + + # 计算启发值 + g = total_cost + h = self._heuristic((nx, ny), goal_grid) + f = g + h + + # 添加到open_set + neighbor_node = GridCell(nx, ny, total_cost, (current_node.x, current_node.y)) + open_set.put((f, neighbor_node)) + + print("找不到路径") + return None + + def _pose_to_grid(self, pose): + """将位姿转换为栅格坐标""" + if self.occupancy_grid is None: + return 0, 0 + + grid_size = self.occupancy_grid.shape[0] + robot_x, robot_y = grid_size // 2, grid_size // 2 + + # 从位姿中提取位置 + position = pose[:3, 3] + + grid_x = int(robot_x + position[0] / self.resolution) + grid_y = int(robot_y + position[1] / self.resolution) + + return grid_x, grid_y + + def _is_cell_free(self, x, y): + """检查栅格是否可通行""" + if self.occupancy_grid is None: + return False + + grid_size = self.occupancy_grid.shape[0] + + # 检查边界 + if x < 0 or x >= grid_size or y < 0 or y >= grid_size: + return False + + # 检查障碍物 + if self.occupancy_grid[x, y] > 0.5: + return False + + # 检查代价(太低表示太靠近障碍物) + if self.costmap[x, y] < 0.3: + return False + + return True + + def _heuristic(self, a, b): + """启发函数(欧几里得距离)""" + return np.sqrt((a[0] - b[0]) ** 2 + (a[1] - b[1]) ** 2) + + def _reconstruct_path(self, goal_node): + """重构路径""" + path = [] + current = goal_node + + while current is not None: + # 转换为世界坐标 + world_x, world_y = self._grid_to_world(current.x, current.y) + path.append((world_x, world_y)) + current = current.parent + + path.reverse() + + # 路径平滑 + if self.config["planning"]["path_smoothing"]: + path = self._smooth_path(path) + + return np.array(path) + + def _grid_to_world(self, grid_x, grid_y): + """将栅格坐标转换为世界坐标""" + if self.occupancy_grid is None: + return 0.0, 0.0 + + grid_size = self.occupancy_grid.shape[0] + robot_x, robot_y = grid_size // 2, grid_size // 2 + + world_x = (grid_x - robot_x) * self.resolution + world_y = (grid_y - robot_y) * self.resolution + + return world_x, world_y + + def _smooth_path(self, path, weight_data=0.1, weight_smooth=0.3, tolerance=0.00001): + """路径平滑(梯度下降)""" + if len(path) < 3: + return path + + smoothed = np.array(path).copy() + + change = tolerance + while change >= tolerance: + change = 0.0 + + for i in range(1, len(path) - 1): + original = np.array(path[i]) + current = smoothed[i] + + # 数据项(保持接近原始路径) + data_term = original - current + + # 平滑项(保持相邻点间距均匀) + smooth_term = (smoothed[i - 1] + smoothed[i + 1] - 2 * current) + + # 更新 + update = weight_data * data_term + weight_smooth * smooth_term + smoothed[i] += update + + change += np.linalg.norm(update) + + return smoothed.tolist() + + def local_path_planning(self, global_path, robot_pose, obstacles=None): + """局部路径规划(动态窗口方法)""" + if global_path is None or len(global_path) == 0: + return None + + # 找到路径上最近的点 + robot_pos = robot_pose[:3, 3] + distances = np.linalg.norm(global_path - robot_pos[:2], axis=1) + nearest_idx = np.argmin(distances) + + # 向前看一段距离 + lookahead_distance = self.config["control"]["lookahead_distance"] + target_idx = nearest_idx + + for i in range(nearest_idx, len(global_path)): + if np.linalg.norm(global_path[i] - robot_pos[:2]) >= lookahead_distance: + target_idx = i + break + + if target_idx >= len(global_path): + target_idx = len(global_path) - 1 + + target_point = global_path[target_idx] + + # 动态窗口方法 + best_v, best_w = self._dynamic_window_approach(robot_pose, target_point, obstacles) + + # 生成局部路径 + local_path = [] + dt = 0.1 + + for i in range(10): # 预测10步 + t = i * dt + x = robot_pos[0] + best_v * np.cos(robot_pos[2]) * t + y = robot_pos[1] + best_v * np.sin(robot_pos[2]) * t + theta = robot_pos[2] + best_w * t + + local_path.append((x, y, theta)) + + self.local_path = np.array(local_path) + return self.local_path, (best_v, best_w) + + def _dynamic_window_approach(self, robot_pose, target, obstacles): + """动态窗口方法""" + # 机器人状态 + x, y, theta = self._extract_robot_state(robot_pose) + + # 速度范围 + v_min, v_max = 0.0, self.config["control"]["max_linear_speed"] + w_min, w_max = -self.config["control"]["max_angular_speed"], self.config["control"]["max_angular_speed"] + + # 生成速度样本 + v_samples = np.linspace(v_min, v_max, 10) + w_samples = np.linspace(w_min, w_max, 20) + + best_score = -float('inf') + best_v, best_w = 0.0, 0.0 + + for v in v_samples: + for w in w_samples: + # 模拟轨迹 + trajectory = self._simulate_trajectory(x, y, theta, v, w, 1.0, 0.1) + + # 计算分数 + goal_score = self._goal_score(trajectory, target) + obstacle_score = self._obstacle_score(trajectory, obstacles) + speed_score = v / v_max # 鼓励移动 + + total_score = 0.5 * goal_score + 0.3 * obstacle_score + 0.2 * speed_score + + if total_score > best_score: + best_score = total_score + best_v, best_w = v, w + + return best_v, best_w + + def _extract_robot_state(self, pose): + """从位姿中提取机器人状态""" + position = pose[:3, 3] + # 从旋转矩阵中提取偏航角 + r = Rotation.from_matrix(pose[:3, :3]) + yaw = r.as_euler('zyx')[0] + + return position[0], position[1], yaw + + def _simulate_trajectory(self, x, y, theta, v, w, time_horizon, dt): + """模拟轨迹""" + trajectory = [] + t = 0.0 + + while t < time_horizon: + x += v * np.cos(theta) * dt + y += v * np.sin(theta) * dt + theta += w * dt + + trajectory.append((x, y, theta)) + t += dt + + return np.array(trajectory) + + def _goal_score(self, trajectory, target): + """目标得分""" + if len(trajectory) == 0: + return 0.0 + + # 使用轨迹终点的距离 + final_pos = trajectory[-1, :2] + distance = np.linalg.norm(final_pos - target) + + # 距离越小,分数越高 + return 1.0 / (1.0 + distance) + + def _obstacle_score(self, trajectory, obstacles): + """障碍物得分""" + if obstacles is None or len(obstacles) == 0: + return 1.0 + + min_distance = float('inf') + + for point in trajectory[:, :2]: + for obstacle in obstacles: + ox, oy, ow, oh = obstacle + # 计算到障碍物边界的最小距离 + dx = max(ox - ow / 2 - point[0], 0, point[0] - ox - ow / 2) + dy = max(oy - oh / 2 - point[1], 0, point[1] - oy - oh / 2) + distance = np.sqrt(dx * dx + dy * dy) + min_distance = min(min_distance, distance) + + # 距离越大,分数越高 + if min_distance < self.safety_distance: + return 0.0 + else: + return min(1.0, min_distance / 5.0) + + def check_collision(self, point_cloud=None, safety_margin=0.3): + """碰撞检测""" + # 使用内部的碰撞检测方法 + return self._check_collision() + + def execute_emergency_stop(self): + """执行紧急停止""" + print("执行紧急停止!") + self.target_speed = 0.0 + self.target_angular = 0.0 + return True + + # ============== 可视化方法 ============== + + def setup_visualization(self): + """设置可视化窗口""" + plt.ion() # 打开交互模式 + + self.fig = plt.figure(figsize=(15, 8)) + gs = gridspec.GridSpec(2, 2, figure=self.fig, + width_ratios=[1.5, 1], + height_ratios=[1, 1]) + + # 创建子图 + self.ax_map = self.fig.add_subplot(gs[:, 0]) # 左侧:占据栅格地图 + self.ax_path = self.fig.add_subplot(gs[0, 1]) # 右上:路径规划 + self.ax_info = self.fig.add_subplot(gs[1, 1]) # 右下:信息显示 + + # 设置标题 + self.fig.suptitle('自主导航系统 - 实时演示', fontsize=16, fontweight='bold') + + plt.tight_layout() + print("可视化窗口已初始化") + + def update_visualization(self): + """更新可视化显示""" + if not hasattr(self, 'fig'): + self.setup_visualization() + + # 清空画布 + self.ax_map.clear() + self.ax_path.clear() + self.ax_info.clear() + self.ax_info.axis('off') + + # 获取位置信息 + robot_pos = self.robot_pose[:3, 3] + goal_pos = self.goal_pose[:3, 3] + + # 1. 绘制占据栅格地图和场景 + self._draw_scene_map() + + # 2. 绘制路径规划 + self._draw_path_planning() + + # 3. 更新信息显示 + self._update_info_display() + + # 刷新显示 + plt.draw() + plt.pause(0.001) + + def _draw_scene_map(self): + """绘制场景地图""" + robot_pos = self.robot_pose[:3, 3] + goal_pos = self.goal_pose[:3, 3] + + # 绘制障碍物 + for obs in self.obstacles: + x, y, w, h = obs + rect = Rectangle((x - w / 2, y - h / 2), w, h, + facecolor='red', alpha=0.6, + edgecolor='darkred', linewidth=2) + self.ax_map.add_patch(rect) + + # 绘制机器人 + robot_circle = Circle((robot_pos[0], robot_pos[1]), + self.safety_distance, + color='blue', alpha=0.3, label='机器人') + self.ax_map.add_patch(robot_circle) + + # 绘制机器人方向 + r = Rotation.from_matrix(self.robot_pose[:3, :3]) + yaw = r.as_euler('zyx')[0] + arrow_length = self.safety_distance * 1.5 + dx = arrow_length * np.cos(yaw) + dy = arrow_length * np.sin(yaw) + self.ax_map.arrow(robot_pos[0], robot_pos[1], dx, dy, + head_width=0.1, head_length=0.15, + fc='darkblue', ec='darkblue') + + # 绘制目标点 + goal_circle = Circle((goal_pos[0], goal_pos[1]), + self.config["planning"]["goal_tolerance"], + color='green', alpha=0.4, label='目标区域') + self.ax_map.add_patch(goal_circle) + self.ax_map.plot(goal_pos[0], goal_pos[1], 'g*', + markersize=20, label='目标点') + + # 绘制轨迹 + if len(self.trajectory_history) > 1: + trajectory = np.array(self.trajectory_history) + self.ax_map.plot(trajectory[:, 0], trajectory[:, 1], + 'blue', linewidth=2, alpha=0.6, label='轨迹') + + # 设置地图范围 + self.ax_map.set_xlim(-1, 7) + self.ax_map.set_ylim(-1, 7) + self.ax_map.set_xlabel('X (米)') + self.ax_map.set_ylabel('Y (米)') + self.ax_map.grid(True, alpha=0.3) + self.ax_map.set_title('导航场景', fontsize=12) + self.ax_map.legend(loc='upper right') + self.ax_map.set_aspect('equal') + + def _draw_path_planning(self): + """绘制路径规划""" + robot_pos = self.robot_pose[:3, 3] + goal_pos = self.goal_pose[:3, 3] + + # 绘制障碍物 + for obs in self.obstacles: + x, y, w, h = obs + rect = Rectangle((x - w / 2, y - h / 2), w, h, + facecolor='red', alpha=0.3, + edgecolor='darkred', linewidth=1) + self.ax_path.add_patch(rect) + + # 绘制机器人 + self.ax_path.plot(robot_pos[0], robot_pos[1], 'bo', + markersize=12, label='机器人') + + # 绘制目标 + self.ax_path.plot(goal_pos[0], goal_pos[1], 'g*', + markersize=20, label='目标') + + # 绘制全局路径 + if self.global_path is not None and len(self.global_path) > 0: + self.ax_path.plot(self.global_path[:, 0], self.global_path[:, 1], + 'green', linewidth=3, alpha=0.7, label='全局路径') + + # 绘制路径点 + self.ax_path.scatter(self.global_path[:, 0], self.global_path[:, 1], + color='green', s=30, alpha=0.5) + + # 绘制局部路径 + if self.local_path is not None and len(self.local_path) > 0: + self.ax_path.plot(self.local_path[:, 0], self.local_path[:, 1], + 'blue', linewidth=2, linestyle='--', label='局部路径') + + # 绘制预测轨迹 + for i in range(0, len(self.local_path), 2): + x, y, theta = self.local_path[i] + self.ax_path.plot(x, y, 'b.', markersize=8) + + # 绘制轨迹 + if len(self.trajectory_history) > 1: + trajectory = np.array(self.trajectory_history) + self.ax_path.plot(trajectory[:, 0], trajectory[:, 1], + 'blue', linewidth=1.5, alpha=0.5, label='历史轨迹') + + # 设置图形属性 + self.ax_path.set_xlabel('X (米)') + self.ax_path.set_ylabel('Y (米)') + self.ax_path.grid(True, alpha=0.3) + self.ax_path.set_title('路径规划', fontsize=12) + self.ax_path.legend(loc='upper right', fontsize=8) + + # 自动调整视图范围 + all_x = [robot_pos[0], goal_pos[0]] + all_y = [robot_pos[1], goal_pos[1]] + + if self.global_path is not None: + all_x.extend(self.global_path[:, 0]) + all_y.extend(self.global_path[:, 1]) + + if all_x: + x_min, x_max = min(all_x), max(all_x) + y_min, y_max = min(all_y), max(all_y) + margin = 1.0 + self.ax_path.set_xlim(x_min - margin, x_max + margin) + self.ax_path.set_ylim(y_min - margin, y_max + margin) + + def _update_info_display(self): + """更新信息显示""" + robot_pos = self.robot_pose[:3, 3] + goal_pos = self.goal_pose[:3, 3] + + r = Rotation.from_matrix(self.robot_pose[:3, :3]) + yaw = r.as_euler('zyx')[0] + + # 计算到目标的距离 + distance_to_goal = np.linalg.norm(robot_pos[:2] - goal_pos[:2]) + + # 创建信息文本 + info_text = [ + "=== 导航状态 ===", + f"运行状态: {'活跃' if self.navigation_active else '停止'}", + f"目标到达: {'是' if self.goal_reached else '否'}", + f"路径找到: {'是' if self.path_found else '否'}", + f"碰撞警告: {'是' if self.collision_warning else '否'}", + "", + "=== 机器人状态 ===", + f"位置: ({robot_pos[0]:.2f}, {robot_pos[1]:.2f})", + f"朝向: {np.degrees(yaw):.1f}°", + f"速度: {self.current_speed:.2f} m/s", + f"角速度: {np.degrees(self.current_angular):.1f} °/s", + "", + "=== 目标信息 ===", + f"目标位置: ({goal_pos[0]:.2f}, {goal_pos[1]:.2f})", + f"目标距离: {distance_to_goal:.2f} 米", + f"目标容差: {self.config['planning']['goal_tolerance']} 米", + "", + "=== 控制指令 ===", + f"目标速度: {self.target_speed:.2f} m/s", + f"目标角速: {self.target_angular:.2f} rad/s", + ] + + # 显示文本 + for i, text in enumerate(info_text): + self.ax_info.text(0.05, 0.98 - i * 0.045, text, + transform=self.ax_info.transAxes, + fontsize=9, verticalalignment='top', + fontfamily='monospace') + + # 状态指示灯 + status_color = 'green' if self.navigation_active else 'red' + status_text = '运行中' if self.navigation_active else '已停止' + + status_circle = Circle((0.15, 0.05), 0.03, + transform=self.ax_info.transAxes, + facecolor=status_color, alpha=0.8) + self.ax_info.add_patch(status_circle) + self.ax_info.text(0.2, 0.05, status_text, + transform=self.ax_info.transAxes, + fontsize=10, verticalalignment='center') + + # 进度条 + progress = 1.0 - min(1.0, distance_to_goal / 10.0) + self.ax_info.add_patch(Rectangle((0.1, 0.12), 0.8, 0.04, + transform=self.ax_info.transAxes, + facecolor='lightgray', + edgecolor='black')) + + progress_color = 'green' if progress > 0.7 else 'yellow' if progress > 0.3 else 'red' + self.ax_info.add_patch(Rectangle((0.1, 0.12), 0.8 * progress, 0.04, + transform=self.ax_info.transAxes, + facecolor=progress_color, + alpha=0.7)) + + self.ax_info.text(0.5, 0.14, f"进度: {progress * 100:.1f}%", + transform=self.ax_info.transAxes, + fontsize=10, fontweight='bold', + horizontalalignment='center') + + self.ax_info.set_title('系统信息', fontsize=12) + + def run_autonomous_navigation_demo(self): + """运行自主导航演示""" + print("=== 自主导航系统演示 ===") + print("正在启动系统...") + + # 设置可视化 + self.setup_visualization() + + # 设置随机起点 + start_pose = np.eye(4) + start_pose[0, 3] = 0.0 + start_pose[1, 3] = 0.0 + + # 开始导航 + self.start_navigation(start_pose, self.goal_pose) + + try: + # 主循环:更新可视化 + while self.navigation_active: + self.update_visualization() + time.sleep(0.05) # 20Hz更新频率 + + # 如果到达目标,短暂暂停后设置新目标 + if self.goal_reached and self.navigation_active: + print("已到达目标,正在设置新目标...") + time.sleep(1.0) + + # 设置新随机目标 + new_goal = np.eye(4) + new_goal[0, 3] = random.uniform(2, 6) + new_goal[1, 3] = random.uniform(2, 6) + + print(f"新目标: ({new_goal[0, 3]:.2f}, {new_goal[1, 3]:.2f})") + self.goal_pose = new_goal + self.goal_reached = False + self.path_found = False + + print("演示完成") + + except KeyboardInterrupt: + print("\n用户中断") + self.stop_navigation() + + finally: + plt.ioff() + plt.show() + print("程序结束") + + +# ==================== 主程序 ==================== +if __name__ == "__main__": + # 创建导航系统 + nav_system = AutonomousNavigation() + + # 运行自主导航演示 + nav_system.run_autonomous_navigation_demo() \ No newline at end of file From dee6d412102d271845a7c94d26670ad54c4f6546 Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Sun, 21 Dec 2025 19:59:43 +0800 Subject: [PATCH 17/20] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=85=A8=E5=B1=80?= =?UTF-8?q?=E5=AE=9A=E4=BD=8D=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../autonomous_navigation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/autonomous_navigation.py b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/autonomous_navigation.py index 9364153a38..455612d774 100644 --- a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/autonomous_navigation.py +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/autonomous_navigation.py @@ -1126,7 +1126,7 @@ def run_autonomous_navigation_demo(self): print("程序结束") -# ==================== 主程序 ==================== + if __name__ == "__main__": # 创建导航系统 nav_system = AutonomousNavigation() From 8632b4fc4aa3a2c4190c41c922edd68f6dab6d56 Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Sun, 21 Dec 2025 22:31:02 +0800 Subject: [PATCH 18/20] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=85=A8=E5=B1=80?= =?UTF-8?q?=E5=AE=9A=E4=BD=8D=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../global_localization.py | 1009 +++++++++++++++-- 1 file changed, 889 insertions(+), 120 deletions(-) diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py index aa92d0a511..ac83e1fe38 100644 --- a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py @@ -5,14 +5,347 @@ import time import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D -import matplotlib.animation as animation -from matplotlib.widgets import Slider, Button +import socket +import json +import struct +import threading +from queue import Queue +import pandas as pd +import os +import sys + +# 尝试导入open3d,如果失败则提供替代方案 +try: + import open3d as o3d + + OPEN3D_AVAILABLE = True + print("open3d导入成功") +except ImportError as e: + OPEN3D_AVAILABLE = False + print(f"警告:无法导入open3d,某些功能将受限: {e}") + print("请安装open3d: pip install open3d") + +# 尝试导入ROS相关模块 +try: + import rosbag + from sensor_msgs import point_cloud2 + import rospy + + ROS_AVAILABLE = True +except ImportError: + ROS_AVAILABLE = False + print("警告:ROS模块不可用,ROS bag功能将受限") + +# 尝试导入CARLA模块 +try: + import carla + + CARLA_AVAILABLE = True +except ImportError: + CARLA_AVAILABLE = False + + +class SimulatorInterface: + """模拟器数据接口基类""" + + def __init__(self, simulator_type='custom'): + self.simulator_type = simulator_type + self.running = False + self.data_queue = Queue(maxsize=100) + + def connect(self): + """连接到模拟器""" + raise NotImplementedError + + def disconnect(self): + """断开连接""" + self.running = False + + def get_scan_data(self): + """获取扫描数据""" + raise NotImplementedError + + def get_pose_data(self): + """获取位姿数据(真值)""" + raise NotImplementedError + + +class ROSBagInterface(SimulatorInterface): + """ROS bag文件接口""" + + def __init__(self, bag_file, pointcloud_topic='/lidar/points', odom_topic='/odom'): + super().__init__('rosbag') + self.bag_file = bag_file + self.pointcloud_topic = pointcloud_topic + self.odom_topic = odom_topic + self.bag = None + + def connect(self): + """打开bag文件""" + if not ROS_AVAILABLE: + print("ROS模块不可用,无法使用ROS bag功能") + return False + + try: + self.bag = rosbag.Bag(self.bag_file, 'r') + self.running = True + print(f"成功打开ROS bag文件: {self.bag_file}") + return True + except Exception as e: + print(f"打开bag文件失败: {e}") + return False + + def get_scan_data(self): + """从bag文件读取点云数据""" + if not self.bag: + return None + + for topic, msg, t in self.bag.read_messages(topics=[self.pointcloud_topic]): + if topic == self.pointcloud_topic: + # 转换点云消息为numpy数组 + points = [] + for p in point_cloud2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True): + points.append([p[0], p[1], p[2]]) + if points: + return np.array(points) + return None + + def get_pose_data(self): + """从bag文件读取位姿数据""" + if not self.bag: + return None + + for topic, msg, t in self.bag.read_messages(topics=[self.odom_topic]): + if topic == self.odom_topic: + pose = msg.pose.pose + # 构建4x4变换矩阵 + transform = np.eye(4) + transform[0, 3] = pose.position.x + transform[1, 3] = pose.position.y + transform[2, 3] = pose.position.z + + # 设置旋转 + q = pose.orientation + rotation = R.from_quat([q.x, q.y, q.z, q.w]) + transform[:3, :3] = rotation.as_matrix() + return transform + return None + + +class CARLAInterface(SimulatorInterface): + """CARLA模拟器接口""" + + def __init__(self, host='localhost', port=2000): + super().__init__('carla') + self.host = host + self.port = port + self.client = None + self.world = None + + def connect(self): + """连接到CARLA服务器""" + if not CARLA_AVAILABLE: + print("CARLA模块不可用,请先安装: pip install carla") + return False + + try: + import carla + self.client = carla.Client(self.host, self.port) + self.client.set_timeout(10.0) + self.world = self.client.get_world() + self.running = True + print(f"成功连接到CARLA服务器 {self.host}:{self.port}") + return True + except ImportError: + print("请先安装CARLA Python API: pip install carla") + return False + except Exception as e: + print(f"连接CARLA服务器失败: {e}") + return False + + def get_lidar_data(self, actor_id=None): + """获取LiDAR数据""" + if not self.world: + return None + + import carla + # 查找LiDAR传感器 + if actor_id: + actor = self.world.get_actor(actor_id) + if actor and 'lidar' in actor.type_id.lower(): + lidar_data = [] + # 这里需要根据CARLA API获取点云数据 + # 实际实现会更复杂,需要处理回调 + return np.array(lidar_data) + return None + + +class SocketInterface(SimulatorInterface): + """Socket数据接口(通用)""" + + def __init__(self, host='localhost', port=9999): + super().__init__('socket') + self.host = host + self.port = port + self.socket = None + self.conn = None + self.receiver_thread = None + + def connect(self): + """连接到数据服务器""" + try: + self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.socket.connect((self.host, self.port)) + self.conn = self.socket + self.running = True + + # 启动接收线程 + self.receiver_thread = threading.Thread(target=self._receive_data) + self.receiver_thread.daemon = True + self.receiver_thread.start() + + print(f"成功连接到服务器 {self.host}:{self.port}") + return True + except Exception as e: + print(f"连接服务器失败: {e}") + return False + + def _receive_data(self): + """接收数据线程""" + while self.running: + try: + # 接收数据头(包含数据长度) + header = self.conn.recv(4) + if len(header) < 4: + continue + + data_len = struct.unpack('I', header)[0] + + # 接收实际数据 + data = b'' + while len(data) < data_len: + chunk = self.conn.recv(min(4096, data_len - len(data))) + if not chunk: + break + data += chunk + + if len(data) == data_len: + # 解析JSON数据 + try: + message = json.loads(data.decode('utf-8')) + if 'type' in message and message['type'] == 'pointcloud': + points = np.array(message['points']).reshape(-1, 3) + self.data_queue.put(points) + elif 'type' in message and message['type'] == 'pose': + pose = np.array(message['pose']).reshape(4, 4) + self.data_queue.put(('pose', pose)) + except: + pass + + except Exception as e: + if self.running: + print(f"接收数据错误: {e}") + break + + def get_scan_data(self): + """获取扫描数据""" + try: + # 发送请求 + request = {'type': 'get_pointcloud'} + data = json.dumps(request).encode('utf-8') + header = struct.pack('I', len(data)) + self.conn.sendall(header + data) + + # 等待数据 + timeout = 2.0 + start_time = time.time() + while time.time() - start_time < timeout: + if not self.data_queue.empty(): + data = self.data_queue.get() + if isinstance(data, np.ndarray): + return data + time.sleep(0.01) + return None + except Exception as e: + print(f"获取扫描数据失败: {e}") + return None + + +class KITTIDataInterface(SimulatorInterface): + """KITTI数据集接口""" + + def __init__(self, data_path, sequence='00'): + super().__init__('kitti') + self.data_path = data_path + self.sequence = sequence + self.current_frame = 0 + self.total_frames = 0 + self.poses = [] + + def connect(self): + """加载KITTI数据""" + try: + # 加载点云文件列表 + velodyne_path = os.path.join(self.data_path, 'sequences', self.sequence, 'velodyne') + if not os.path.exists(velodyne_path): + print(f"路径不存在: {velodyne_path}") + return False + + self.pointcloud_files = sorted([f for f in os.listdir(velodyne_path) if f.endswith('.bin')]) + self.total_frames = len(self.pointcloud_files) + + # 加载位姿真值 + pose_file = os.path.join(self.data_path, 'poses', f'{self.sequence}.txt') + if os.path.exists(pose_file): + poses_data = np.loadtxt(pose_file).reshape(-1, 3, 4) + for pose in poses_data: + transform = np.eye(4) + transform[:3, :] = pose + self.poses.append(transform) + + self.running = True + print(f"加载KITTI序列 {self.sequence}: {self.total_frames} 帧") + return True + except Exception as e: + print(f"加载KITTI数据失败: {e}") + return False + + def get_scan_data(self, frame_idx=None): + """获取指定帧的点云数据""" + if frame_idx is None: + frame_idx = self.current_frame + self.current_frame += 1 + + if frame_idx >= self.total_frames: + return None + + try: + file_path = os.path.join(self.data_path, 'sequences', self.sequence, + 'velodyne', self.pointcloud_files[frame_idx]) + + # 读取bin文件 + scan = np.fromfile(file_path, dtype=np.float32).reshape(-1, 4) + # 只取xyz,忽略反射强度 + points = scan[:, :3] + return points + except Exception as e: + print(f"读取点云帧 {frame_idx} 失败: {e}") + return None + + def get_pose_data(self, frame_idx=None): + """获取位姿真值""" + if frame_idx is None: + frame_idx = self.current_frame - 1 + + if frame_idx < len(self.poses): + return self.poses[frame_idx] + return None class GlobalLocalization: """全局定位模块""" - def __init__(self, map_data=None, visualize=True): + def __init__(self, map_data=None, visualize=True, simulator=None): self.map_data = map_data self.map_points = None self.map_tree = None @@ -27,6 +360,9 @@ def __init__(self, map_data=None, visualize=True): # 扫描上下文描述子 self.scan_contexts = {} + # 数据接口 + self.simulator = simulator + # 可视化相关 self.visualize = visualize self.fig = None @@ -34,6 +370,7 @@ def __init__(self, map_data=None, visualize=True): self.history_poses = [] self.history_confidences = [] self.scan_history = [] + self.ground_truth_poses = [] # 颜色设置 self.colors = { @@ -42,8 +379,89 @@ def __init__(self, map_data=None, visualize=True): 'matched_scan': (0.0, 1.0, 0.0, 0.5), # 绿色 'trajectory': (0.0, 0.0, 1.0, 1.0), # 蓝色 'candidates': (1.0, 1.0, 0.0, 0.3), # 黄色 + 'ground_truth': (0.5, 0.0, 0.5, 1.0), # 紫色 } + def set_simulator(self, simulator): + """设置数据接口""" + self.simulator = simulator + + def load_map_from_file(self, filepath, file_type='auto'): + """从文件加载地图""" + points = None + + if file_type == 'auto': + # 自动检测文件类型 + if filepath.endswith('.pcd') or filepath.endswith('.ply'): + file_type = 'pcd' + elif filepath.endswith('.bin'): + file_type = 'kitti_bin' + elif filepath.endswith('.csv'): + file_type = 'csv' + elif filepath.endswith('.npy'): + file_type = 'npy' + elif filepath.endswith('.txt'): + file_type = 'txt' + + try: + if (file_type == 'pcd' or file_type == 'ply') and OPEN3D_AVAILABLE: + # 使用Open3D读取 + pcd = o3d.io.read_point_cloud(filepath) + points = np.asarray(pcd.points) + print(f"使用open3d读取 {file_type} 文件: {filepath}") + + elif file_type == 'kitti_bin': + # KITTI格式的bin文件 + points = np.fromfile(filepath, dtype=np.float32).reshape(-1, 4)[:, :3] + print(f"读取KITTI bin文件: {filepath}") + + elif file_type == 'csv': + # CSV文件 + df = pd.read_csv(filepath) + # 尝试不同的列名 + possible_columns = [['x', 'y', 'z'], ['X', 'Y', 'Z'], + ['position_x', 'position_y', 'position_z']] + + for cols in possible_columns: + if all(col in df.columns for col in cols): + points = df[cols].values + break + + if points is None: + # 如果前3列是xyz + points = df.iloc[:, :3].values + + print(f"读取CSV文件: {filepath}") + + elif file_type == 'npy': + # numpy文件 + points = np.load(filepath) + print(f"读取numpy文件: {filepath}") + + elif file_type == 'txt': + # 文本文件 + points = np.loadtxt(filepath) + print(f"读取文本文件: {filepath}") + + else: + print(f"不支持的文件类型: {file_type}") + return False + + if points is not None: + self.load_map(points) + print(f"从 {filepath} 加载了 {len(points)} 个点") + return True + else: + print(f"无法从 {filepath} 读取点云数据") + return False + + except Exception as e: + print(f"加载地图文件失败: {e}") + import traceback + traceback.print_exc() + + return False + def initialize_visualization(self): """初始化可视化窗口""" if not self.visualize: @@ -138,7 +556,8 @@ def _visualize_map(self): self.fig.canvas.draw_idle() def _visualize_localization(self, scan_points, pose, confidence, - matched_points=None, candidates=None): + matched_points=None, candidates=None, + ground_truth=None): """可视化定位结果""" if not self.visualize: return @@ -150,13 +569,17 @@ def _visualize_localization(self, scan_points, pose, confidence, self.history_poses.append(pose) self.history_confidences.append(confidence) self.scan_history.append(transformed_scan) + if ground_truth is not None: + self.ground_truth_poses.append(ground_truth) # 限制历史记录长度 - max_history = 50 + max_history = 100 if len(self.history_poses) > max_history: self.history_poses = self.history_poses[-max_history:] self.history_confidences = self.history_confidences[-max_history:] self.scan_history = self.scan_history[-max_history:] + if self.ground_truth_poses: + self.ground_truth_poses = self.ground_truth_poses[-max_history:] # 1. 3D点云视图 self.ax1.clear() @@ -177,7 +600,14 @@ def _visualize_localization(self, scan_points, pose, confidence, if len(self.history_poses) > 1: trajectory = np.array([p[:3, 3] for p in self.history_poses]) self.ax1.plot(trajectory[:, 0], trajectory[:, 1], trajectory[:, 2], - c=self.colors['trajectory'], linewidth=2, label='Trajectory') + c=self.colors['trajectory'], linewidth=2, label='Estimated') + + # 真值轨迹 + if self.ground_truth_poses and len(self.ground_truth_poses) > 1: + gt_trajectory = np.array([p[:3, 3] for p in self.ground_truth_poses]) + self.ax1.plot(gt_trajectory[:, 0], gt_trajectory[:, 1], gt_trajectory[:, 2], + c=self.colors['ground_truth'], linewidth=2, linestyle='--', + label='Ground Truth') self.ax1.set_title(f'3D View - Confidence: {confidence:.3f}') self.ax1.set_xlabel('X (m)') @@ -201,12 +631,14 @@ def _visualize_localization(self, scan_points, pose, confidence, if len(self.history_poses) > 1: trajectory = np.array([p[:3, 3] for p in self.history_poses]) self.ax2.plot(trajectory[:, 0], trajectory[:, 1], - c=self.colors['trajectory'], linewidth=2, label='Trajectory') + c=self.colors['trajectory'], linewidth=2, label='Estimated') - # 显示方向箭头 - dx, dy = 2 * np.cos(pose[2, 0]), 2 * np.sin(pose[2, 0]) - self.ax2.arrow(pose[0, 3], pose[1, 3], dx, dy, - head_width=0.5, head_length=0.5, fc='blue', ec='blue') + # 真值轨迹 + if self.ground_truth_poses and len(self.ground_truth_poses) > 1: + gt_trajectory = np.array([p[:3, 3] for p in self.ground_truth_poses]) + self.ax2.plot(gt_trajectory[:, 0], gt_trajectory[:, 1], + c=self.colors['ground_truth'], linewidth=2, linestyle='--', + label='Ground Truth') # 候选位姿 if candidates is not None: @@ -244,23 +676,34 @@ def _visualize_localization(self, scan_points, pose, confidence, # 5. 位姿误差(如果有真值的话) self.ax5.clear() - if len(self.history_poses) > 1: - positions = np.array([p[:3, 3] for p in self.history_poses]) - position_changes = np.diff(positions, axis=0) - position_norms = np.linalg.norm(position_changes, axis=1) - - steps = range(len(position_norms)) - self.ax5.plot(steps, position_norms, 'r-', label='Position Change (m)') - - if len(steps) > 0: - self.ax5.axhline(y=np.mean(position_norms), color='r', linestyle='--', - alpha=0.5, label=f'Avg: {np.mean(position_norms):.3f}m') - - self.ax5.set_xlabel('Step') - self.ax5.set_ylabel('Error / Change') - self.ax5.set_title('Pose Changes') - self.ax5.grid(True, alpha=0.3) - self.ax5.legend() + if self.ground_truth_poses and len(self.ground_truth_poses) > 1: + # 计算估计位姿与真值的误差 + errors = [] + for i in range(min(len(self.history_poses), len(self.ground_truth_poses))): + if i < len(self.ground_truth_poses): + # 位置误差 + pos_error = np.linalg.norm( + self.history_poses[i][:3, 3] - self.ground_truth_poses[i][:3, 3] + ) + + # 角度误差 + R_est = self.history_poses[i][:3, :3] + R_gt = self.ground_truth_poses[i][:3, :3] + angle_error = np.arccos((np.trace(R_est.T @ R_gt) - 1) / 2) + + errors.append(pos_error) + + if errors: + steps = range(len(errors)) + self.ax5.plot(steps, errors, 'r-', label='Position Error (m)') + self.ax5.axhline(y=np.mean(errors), color='r', linestyle='--', + alpha=0.5, label=f'Avg: {np.mean(errors):.3f}m') + + self.ax5.set_xlabel('Step') + self.ax5.set_ylabel('Error (m)') + self.ax5.set_title('Localization Error vs Ground Truth') + self.ax5.grid(True, alpha=0.3) + self.ax5.legend() # 6. ICP匹配视图 self.ax6.clear() @@ -288,42 +731,6 @@ def _visualize_localization(self, scan_points, pose, confidence, self.fig.canvas.draw_idle() plt.pause(0.01) # 短暂暂停以更新显示 - def localize_with_gnss(self, gnss_position, initial_guess=None): - """使用GNSS进行初始全局定位""" - if not self.initialized or self.map_points is None: - return np.eye(4), 0.0 - - # 将GNSS坐标转换为局部坐标 - # 这里需要知道地图的GNSS参考点 - local_position = gnss_position # 简化:假设已经是局部坐标 - - # 在地图中寻找最近的点 - distances, indices = self.map_tree.query(local_position.reshape(1, -1), k=10) - - # 计算平均位置作为初始位姿 - if len(indices) > 0: - nearest_points = self.map_points[indices[0], :3] - estimated_position = np.mean(nearest_points, axis=0) - - # 构建位姿(假设水平) - pose = np.eye(4) - pose[:3, 3] = estimated_position - - confidence = 1.0 / (1.0 + np.mean(distances)) - - # 可视化 - if self.visualize: - self._visualize_localization( - scan_points=np.array([[local_position[0], local_position[1], 0]]), - pose=pose, - confidence=confidence, - matched_points=nearest_points - ) - - return pose, confidence - - return np.eye(4), 0.0 - def localize_with_scan_matching(self, scan_points, initial_guess=None, visualize=True): """使用扫描匹配进行定位""" if not self.initialized or self.map_points is None: @@ -338,13 +745,19 @@ def localize_with_scan_matching(self, scan_points, initial_guess=None, visualize # 计算匹配分数 confidence = self._compute_match_confidence(scan_points, refined_pose) + # 获取真值(如果有) + ground_truth = None + if self.simulator: + ground_truth = self.simulator.get_pose_data() + # 可视化 if self.visualize and visualize: self._visualize_localization( scan_points=scan_points, pose=refined_pose, confidence=confidence, - matched_points=matched_points + matched_points=matched_points, + ground_truth=ground_truth ) return refined_pose, confidence @@ -462,44 +875,72 @@ def create_scan_context(self, scan_points): return descriptor - def scan_context_match(self, current_descriptor): - """扫描上下文匹配""" - if not self.scan_contexts: - return None, 0.0 + def run_with_simulator(self, max_frames=1000, start_frame=0): + """使用模拟器数据运行定位""" + if not self.simulator or not self.simulator.running: + print("模拟器未连接或未运行") + return - best_match = None - best_score = 0.0 + frame_count = 0 - for node_id, descriptor in self.scan_contexts.items(): - # 计算描述子相似度 - score = self._descriptor_similarity(current_descriptor, descriptor) + for i in range(start_frame, start_frame + max_frames): + # 从模拟器获取数据 + scan_points = self.simulator.get_scan_data() - if score > best_score: - best_score = score - best_match = node_id + if scan_points is None: + print("无法获取扫描数据") + break - return best_match, best_score + # 检查点云质量 + if len(scan_points) < 100: + print(f"帧 {i}: 点云点数过少 ({len(scan_points)})") + continue - def _descriptor_similarity(self, desc1, desc2): - """计算描述子相似度""" - # 列向量的余弦相似度 - similarity = 0.0 + print(f"\n帧 {i}: 处理 {len(scan_points)} 个点") + + # 进行定位 + start_time = time.time() + + if frame_count == 0 or self.confidence < 0.3: + # 低置信度时尝试全局重定位 + pose, confidence = self.global_relocalization(scan_points) + if confidence > 0.3: + self.localization_mode = "local" + else: + self.localization_mode = "global" + else: + # 正常局部定位 + pose, confidence = self.localize_with_scan_matching( + scan_points, + self.current_pose + ) - for i in range(desc1.shape[1]): - col1 = desc1[:, i] - col2 = desc2[:, i] + processing_time = time.time() - start_time - norm1 = np.linalg.norm(col1) - norm2 = np.linalg.norm(col2) + # 更新状态 + self.current_pose = pose + self.confidence = confidence - if norm1 > 0 and norm2 > 0: - similarity += np.dot(col1, col2) / (norm1 * norm2) + print(f" 置信度: {confidence:.3f}") + print(f" 位姿: [{pose[0, 3]:.2f}, {pose[1, 3]:.2f}, {pose[2, 3]:.2f}]") + print(f" 处理时间: {processing_time:.3f}秒") + print(f" 模式: {self.localization_mode}") - return similarity / desc1.shape[1] + frame_count += 1 - def update_scan_context(self, node_id, descriptor): - """更新扫描上下文数据库""" - self.scan_contexts[node_id] = descriptor + # 控制帧率 + time.sleep(0.05) # 20Hz + + # 检查是否应该停止 + if not self.simulator.running: + break + + print(f"\n处理完成,共处理 {frame_count} 帧") + + # 保存结果 + if self.visualize: + self.save_visualization() + plt.show(block=True) def global_relocalization(self, scan_points, gnss_hint=None): """全局重定位""" @@ -579,6 +1020,81 @@ def _generate_candidates(self, scan_points, num_candidates=20): return candidates + def localize_with_gnss(self, gnss_position, initial_guess=None): + """使用GNSS进行初始全局定位""" + if not self.initialized or self.map_points is None: + return np.eye(4), 0.0 + + # 将GNSS坐标转换为局部坐标 + # 这里需要知道地图的GNSS参考点 + local_position = gnss_position # 简化:假设已经是局部坐标 + + # 在地图中寻找最近的点 + distances, indices = self.map_tree.query(local_position.reshape(1, -1), k=10) + + # 计算平均位置作为初始位姿 + if len(indices) > 0: + nearest_points = self.map_points[indices[0], :3] + estimated_position = np.mean(nearest_points, axis=0) + + # 构建位姿(假设水平) + pose = np.eye(4) + pose[:3, 3] = estimated_position + + confidence = 1.0 / (1.0 + np.mean(distances)) + + # 可视化 + if self.visualize: + self._visualize_localization( + scan_points=np.array([[local_position[0], local_position[1], 0]]), + pose=pose, + confidence=confidence, + matched_points=nearest_points + ) + + return pose, confidence + + return np.eye(4), 0.0 + + def scan_context_match(self, current_descriptor): + """扫描上下文匹配""" + if not self.scan_contexts: + return None, 0.0 + + best_match = None + best_score = 0.0 + + for node_id, descriptor in self.scan_contexts.items(): + # 计算描述子相似度 + score = self._descriptor_similarity(current_descriptor, descriptor) + + if score > best_score: + best_score = score + best_match = node_id + + return best_match, best_score + + def _descriptor_similarity(self, desc1, desc2): + """计算描述子相似度""" + # 列向量的余弦相似度 + similarity = 0.0 + + for i in range(desc1.shape[1]): + col1 = desc1[:, i] + col2 = desc2[:, i] + + norm1 = np.linalg.norm(col1) + norm2 = np.linalg.norm(col2) + + if norm1 > 0 and norm2 > 0: + similarity += np.dot(col1, col2) / (norm1 * norm2) + + return similarity / desc1.shape[1] + + def update_scan_context(self, node_id, descriptor): + """更新扫描上下文数据库""" + self.scan_contexts[node_id] = descriptor + def save_visualization(self, filename="localization_result.png"): """保存可视化结果""" if self.fig is not None: @@ -592,44 +1108,297 @@ def close_visualization(self): self.fig = None -# 使用示例 -def main(): - # 创建模拟地图数据 - map_points = np.random.randn(10000, 3) * 50 # 10000个随机点 - - # 创建定位器并启用可视化 +def main_demo(): + """演示模式:使用合成数据测试""" + print("=" * 60) + print("全局定位系统 - 演示模式") + print("=" * 60) + + # 创建合成地图数据 + map_points = [] + + # 创建一些建筑物 + print("生成合成地图...") + for x in np.arange(-50, 51, 10): + for y in np.arange(-50, 51, 10): + # 建筑物点云 + building_points = np.random.randn(100, 3) * 2 + building_points[:, 0] += x + building_points[:, 1] += y + building_points[:, 2] = building_points[:, 2] * 5 + 10 # 高度 + map_points.append(building_points) + + # 创建道路 + road_points = [] + for x in np.arange(-50, 51, 0.5): + for y in np.arange(-5, 6, 0.5): + road_points.append([x, y, 0]) + map_points.append(np.array(road_points)) + + map_points = np.vstack(map_points) + + # 创建定位器 + print("初始化定位器...") locator = GlobalLocalization(visualize=True) locator.load_map(map_points) - # 模拟扫描数据 - for i in range(50): - # 创建模拟扫描(部分地图点加上噪声) - scan_indices = np.random.choice(len(map_points), 100) - scan_points = map_points[scan_indices] + np.random.randn(100, 3) * 0.1 + # 模拟轨迹 + print("开始模拟定位...") + trajectory = [] + radius = 30.0 + total_frames = 50 + + for i in range(total_frames): + angle = i * 0.1 + x = radius * np.cos(angle) + y = radius * np.sin(angle) + + # 模拟当前扫描(地图的一部分加上噪声) + # 找到附近的点作为扫描 + if i == 0: + current_pose = np.eye(4) + else: + # 模拟运动 + current_pose = np.eye(4) + current_pose[0, 3] = x + current_pose[1, 3] = y + + # 朝向切线方向 + yaw = angle + np.pi / 2 + current_pose[:3, :3] = R.from_euler('z', yaw).as_matrix() + + # 获取附近的点作为扫描 + transformed_map = locator._transform_points(map_points, np.linalg.inv(current_pose)) + + # 选择前方的点 + mask = (transformed_map[:, 0] > 0) & (transformed_map[:, 0] < 30) & \ + (np.abs(transformed_map[:, 1]) < 10) & \ + (np.random.rand(len(transformed_map)) < 0.1) # 采样 - # 模拟运动 - motion = np.eye(4) - motion[:3, 3] = np.array([0.5, 0.2, 0]) # 平移 - if i > 0: - locator.current_pose = motion @ locator.current_pose + scan_points = map_points[mask] - # 进行定位 - pose, confidence = locator.localize_with_scan_matching( - scan_points, - locator.current_pose - ) + # 添加噪声 + if len(scan_points) > 0: + scan_points = scan_points + np.random.randn(*scan_points.shape) * 0.05 - print(f"Step {i}: Confidence = {confidence:.3f}") + # 进行定位 + if i == 0: + initial_guess = np.eye(4) + else: + # 使用上一帧的位姿作为初始猜测 + initial_guess = locator.current_pose - time.sleep(0.1) # 模拟实时处理 + pose, confidence = locator.localize_with_scan_matching(scan_points, initial_guess) + + # 更新当前位姿 + locator.current_pose = pose + + print(f"帧 {i + 1}/{total_frames}: 置信度 {confidence:.3f}, 位姿 [{pose[0, 3]:.1f}, {pose[1, 3]:.1f}]") + + time.sleep(0.1) + else: + print(f"帧 {i + 1}/{total_frames}: 无有效点云数据") # 保存结果 - locator.save_visualization() + print("\n保存结果...") + locator.save_visualization("demo_result.png") + print("演示完成!按任意键关闭窗口...") + plt.show(block=True) + + +def main_from_file(): + """从文件加载地图并测试""" + print("=" * 60) + print("全局定位系统 - 文件模式") + print("=" * 60) + + # 创建定位器 + locator = GlobalLocalization(visualize=True) + + # 从文件加载地图 + map_file = input("请输入地图文件路径(支持.pcd, .ply, .bin, .csv, .npy, .txt): ").strip() + + if not map_file: + map_file = "map.pcd" # 默认文件 + + if not os.path.exists(map_file): + print(f"文件不存在: {map_file}") + print("使用演示模式生成合成地图...") + main_demo() + return + + if locator.load_map_from_file(map_file): + print("地图加载成功,开始模拟定位...") + + # 在地图内随机生成测试轨迹 + map_min = np.min(locator.map_points[:, :3], axis=0) + map_max = np.max(locator.map_points[:, :3], axis=0) + + test_frames = 30 + successful_localizations = 0 + + for i in range(test_frames): + # 生成随机位姿 + random_pose = np.eye(4) + random_pose[:3, 3] = np.random.uniform(map_min, map_max) + random_pose[:3, :3] = R.from_euler('z', np.random.uniform(-np.pi, np.pi)).as_matrix() + + # 从该位姿生成模拟扫描 + # 获取附近的点 + transformed_map = locator._transform_points(locator.map_points, np.linalg.inv(random_pose)) + + # 选择前方的点 + mask = (transformed_map[:, 0] > 0) & (transformed_map[:, 0] < 20) & \ + (np.abs(transformed_map[:, 1]) < 5) & \ + (np.random.rand(len(transformed_map)) < 0.05) + + scan_points = locator.map_points[mask] + + if len(scan_points) > 100: + # 添加噪声 + scan_points = scan_points + np.random.randn(*scan_points.shape) * 0.1 + + # 进行定位(使用一个初始猜测) + initial_guess = np.eye(4) + initial_guess[:3, 3] = random_pose[:3, 3] + np.random.randn(3) * 5 # 初始猜测有误差 + + pose, confidence = locator.localize_with_scan_matching(scan_points, initial_guess) + + # 计算误差 + pos_error = np.linalg.norm(pose[:3, 3] - random_pose[:3, 3]) + + if confidence > 0.5 and pos_error < 1.0: + successful_localizations += 1 + + print(f"测试 {i + 1}/{test_frames}: 置信度 {confidence:.3f}, 位置误差 {pos_error:.2f}m") + + time.sleep(0.2) + else: + print(f"测试 {i + 1}/{test_frames}: 点云点数不足 ({len(scan_points)})") + + success_rate = successful_localizations / test_frames * 100 + print(f"\n测试完成!成功率: {success_rate:.1f}%") + + locator.save_visualization("file_test_result.png") + print("按任意键关闭窗口...") + plt.show(block=True) + else: + print("地图加载失败") + + +def main_simple_test(): + """简单测试模式""" + print("=" * 60) + print("全局定位系统 - 简单测试模式") + print("=" * 60) + + # 创建简单的测试地图(一个环形) + theta = np.linspace(0, 2 * np.pi, 1000) + radius = 20.0 + height = 5.0 + + # 环形点云 + ring_points = [] + for t in theta: + x = radius * np.cos(t) + y = radius * np.sin(t) + for h in np.linspace(0, height, 10): + ring_points.append([x, y, h]) + + # 添加一些随机的内部点 + for _ in range(1000): + r = np.random.uniform(0, radius * 0.8) + angle = np.random.uniform(0, 2 * np.pi) + x = r * np.cos(angle) + y = r * np.sin(angle) + z = np.random.uniform(0, height) + ring_points.append([x, y, z]) + + map_points = np.array(ring_points) + + # 创建定位器 + locator = GlobalLocalization(visualize=True) + locator.load_map(map_points) + + # 沿着环形轨迹测试 + test_angles = np.linspace(0, 2 * np.pi, 20, endpoint=False) + + for i, angle in enumerate(test_angles): + # 真实位姿 + x = radius * np.cos(angle) + y = radius * np.sin(angle) + z = 2.5 # 中间高度 + + true_pose = np.eye(4) + true_pose[0, 3] = x + true_pose[1, 3] = y + true_pose[2, 3] = z + true_pose[:3, :3] = R.from_euler('z', angle + np.pi / 2).as_matrix() + + # 生成扫描数据 + # 选取前面的点 + transformed_map = locator._transform_points(map_points, np.linalg.inv(true_pose)) + mask = (transformed_map[:, 0] > 0) & (transformed_map[:, 0] < 15) & \ + (np.abs(transformed_map[:, 1]) < 8) & \ + (transformed_map[:, 2] > 0) & (transformed_map[:, 2] < height) + + scan_points = map_points[mask] + + if len(scan_points) > 50: + # 添加噪声 + scan_points = scan_points + np.random.randn(*scan_points.shape) * 0.05 + + # 添加初始猜测误差 + initial_guess = true_pose.copy() + initial_guess[:3, 3] += np.random.randn(3) * 2 # 位置误差 + yaw_error = np.random.uniform(-0.3, 0.3) + initial_guess[:3, :3] = initial_guess[:3, :3] @ R.from_euler('z', yaw_error).as_matrix() + + # 进行定位 + pose, confidence = locator.localize_with_scan_matching(scan_points, initial_guess) + + # 计算误差 + pos_error = np.linalg.norm(pose[:3, 3] - true_pose[:3, 3]) + + print(f"测试 {i + 1}/{len(test_angles)}: " + f"置信度 {confidence:.3f}, " + f"位置误差 {pos_error:.2f}m") + + time.sleep(0.3) - # 保持窗口打开 - print("按任意键关闭窗口...") + locator.save_visualization("simple_test_result.png") + print("\n测试完成!按任意键关闭窗口...") plt.show(block=True) if __name__ == "__main__": - main() \ No newline at end of file + print("全局定位系统 v1.0") + print("=" * 60) + + # 检查依赖 + print("检查依赖...") + print(f"open3d: {'可用' if OPEN3D_AVAILABLE else '不可用'}") + print(f"ROS: {'可用' if ROS_AVAILABLE else '不可用'}") + print(f"CARLA: {'可用' if CARLA_AVAILABLE else '不可用'}") + print("=" * 60) + + # 简单菜单 + print("\n请选择运行模式:") + print("1. 演示模式 (使用合成数据)") + print("2. 文件模式 (从文件加载地图)") + print("3. 简单测试模式 (环形轨迹)") + print("4. 退出") + + choice = input("\n请输入选择 (1-4): ").strip() + + if choice == "1": + main_demo() + elif choice == "2": + main_from_file() + elif choice == "3": + main_simple_test() + elif choice == "4": + print("退出程序") + else: + print("无效选择,使用演示模式") + main_demo() \ No newline at end of file From 7ac5a5c36388495a7b5b7f203620eb6d9ab4ab24 Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Mon, 22 Dec 2025 09:57:37 +0800 Subject: [PATCH 19/20] =?UTF-8?q?=E6=9B=B4=E6=96=B0README=E6=96=87?= =?UTF-8?q?=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../README.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md index 32c04dfab0..017088e604 100644 --- a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/README.md @@ -48,3 +48,9 @@ outdoor_ws/src/robot_description/ - 处理"绑架问题"(全局重定位) - 提供定位置信度评估 - 构建和维护全局特征描述子数据库 +5. **路径规划与运动控制** + - 占据栅格地图构建与更新 + - A*全局路径规划(考虑障碍物膨胀) + - 动态窗口局部路径规划(DWA)与避障 + - 碰撞检测与紧急停止 + - 路径平滑与速度控制 \ No newline at end of file From 18092ccc53d38af02e04c03b4044075643ac4cd1 Mon Sep 17 00:00:00 2001 From: pppzzz <34338148022@qq.com> Date: Mon, 22 Dec 2025 10:44:33 +0800 Subject: [PATCH 20/20] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=85=A8=E5=B1=80?= =?UTF-8?q?=E5=AF=BC=E8=88=AA=E6=A8=A1=E5=9D=97?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../global_localization.py | 1611 +++-------------- 1 file changed, 255 insertions(+), 1356 deletions(-) diff --git a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py index ac83e1fe38..25ae168cdf 100644 --- a/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py +++ b/src/Outdoor_slam_and_autonomous_navigation_based_on_ros2/global_localization.py @@ -1,1404 +1,303 @@ -# global_localization.py +# simple_autonomous_demo.py import numpy as np -from scipy.spatial import KDTree -from scipy.spatial.transform import Rotation as R -import time import matplotlib.pyplot as plt -from mpl_toolkits.mplot3d import Axes3D -import socket -import json -import struct -import threading -from queue import Queue -import pandas as pd -import os -import sys - -# 尝试导入open3d,如果失败则提供替代方案 -try: - import open3d as o3d - - OPEN3D_AVAILABLE = True - print("open3d导入成功") -except ImportError as e: - OPEN3D_AVAILABLE = False - print(f"警告:无法导入open3d,某些功能将受限: {e}") - print("请安装open3d: pip install open3d") - -# 尝试导入ROS相关模块 -try: - import rosbag - from sensor_msgs import point_cloud2 - import rospy - - ROS_AVAILABLE = True -except ImportError: - ROS_AVAILABLE = False - print("警告:ROS模块不可用,ROS bag功能将受限") - -# 尝试导入CARLA模块 -try: - import carla - - CARLA_AVAILABLE = True -except ImportError: - CARLA_AVAILABLE = False - - -class SimulatorInterface: - """模拟器数据接口基类""" - - def __init__(self, simulator_type='custom'): - self.simulator_type = simulator_type - self.running = False - self.data_queue = Queue(maxsize=100) - - def connect(self): - """连接到模拟器""" - raise NotImplementedError - - def disconnect(self): - """断开连接""" - self.running = False - - def get_scan_data(self): - """获取扫描数据""" - raise NotImplementedError - - def get_pose_data(self): - """获取位姿数据(真值)""" - raise NotImplementedError - - -class ROSBagInterface(SimulatorInterface): - """ROS bag文件接口""" - - def __init__(self, bag_file, pointcloud_topic='/lidar/points', odom_topic='/odom'): - super().__init__('rosbag') - self.bag_file = bag_file - self.pointcloud_topic = pointcloud_topic - self.odom_topic = odom_topic - self.bag = None - - def connect(self): - """打开bag文件""" - if not ROS_AVAILABLE: - print("ROS模块不可用,无法使用ROS bag功能") - return False +import time +from autonomous_navigation import AutonomousNavigation +import random + + +class SimpleSimulator: + """简化的自主导航模拟器""" + + def __init__(self): + # 创建模拟环境 + self.env_size = 50 # 米 + self.resolution = 0.1 # 米/像素 + self.grid_size = int(self.env_size / self.resolution) + + # 初始化地图 + self.create_environment() + + # 初始化导航模块 + self.navigator = AutonomousNavigation() + + # 模拟机器人状态 + self.robot_pose = np.eye(4) + self.robot_pose[0, 3] = 5.0 # 起始位置 x + self.robot_pose[1, 3] = 5.0 # 起始位置 y + + # 目标位置 + self.goal_pose = np.eye(4) + self.goal_pose[0, 3] = 45.0 + self.goal_pose[1, 3] = 45.0 + + # 轨迹记录 + self.trajectory = [] + self.obstacles_hit = 0 + + # 可视化设置 + plt.ion() + self.fig, (self.ax1, self.ax2) = plt.subplots(1, 2, figsize=(12, 6)) + + def create_environment(self): + """创建模拟环境""" + # 初始化占据栅格 + self.occupancy_grid = np.zeros((self.grid_size, self.grid_size)) + self.costmap = np.ones((self.grid_size, self.grid_size)) + + # 添加障碍物 + self.add_obstacles() + + # 生成代价地图 + self.generate_costmap() + + def add_obstacles(self): + """添加模拟障碍物""" + # 随机障碍物 + num_obstacles = 15 + for _ in range(num_obstacles): + ox = random.randint(10, self.grid_size - 10) + oy = random.randint(10, self.grid_size - 10) + radius = random.randint(3, 8) + + for i in range(ox - radius, ox + radius): + for j in range(oy - radius, oy + radius): + if 0 <= i < self.grid_size and 0 <= j < self.grid_size: + distance = np.sqrt((i - ox) ** 2 + (j - oy) ** 2) + if distance <= radius: + self.occupancy_grid[i, j] = 1.0 + + # 添加墙壁边界 + wall_thickness = 2 + self.occupancy_grid[:wall_thickness, :] = 1.0 + self.occupancy_grid[-wall_thickness:, :] = 1.0 + self.occupancy_grid[:, :wall_thickness] = 1.0 + self.occupancy_grid[:, -wall_thickness:] = 1.0 + + # 添加走廊 + for i in range(15, 35): + for j in range(20, 25): + if 0 <= i < self.grid_size and 0 <= j < self.grid_size: + self.occupancy_grid[i, j] = 1.0 + + for i in range(20, 25): + for j in range(15, 35): + if 0 <= i < self.grid_size and 0 <= j < self.grid_size: + self.occupancy_grid[i, j] = 1.0 + + def generate_costmap(self): + """生成代价地图""" + inflation_radius = int(0.3 / self.resolution) + + inflated_grid = self.occupancy_grid.copy() + grid_size = self.grid_size + + for i in range(grid_size): + for j in range(grid_size): + if self.occupancy_grid[i, j] > 0.5: + for di in range(-inflation_radius, inflation_radius + 1): + for dj in range(-inflation_radius, inflation_radius + 1): + distance = np.sqrt(di ** 2 + dj ** 2) + if distance <= inflation_radius: + ni, nj = i + di, j + dj + if 0 <= ni < grid_size and 0 <= nj < grid_size: + cost = 1.0 - (distance / inflation_radius) * 0.8 + inflated_grid[ni, nj] = max(inflated_grid[ni, nj], cost) + + self.costmap = 1.0 - inflated_grid + + def simulate_lidar(self): + """模拟LiDAR扫描""" + num_points = 360 + max_range = 10.0 # 米 + + points = [] + robot_x = self.robot_pose[0, 3] + robot_y = self.robot_pose[1, 3] + + for angle in np.linspace(0, 2 * np.pi, num_points): + # 射线追踪 + for r in np.linspace(0.1, max_range, 50): + x = robot_x + r * np.cos(angle) + y = robot_y + r * np.sin(angle) + + # 转换为栅格坐标 + grid_x = int(x / self.resolution) + grid_y = int(y / self.resolution) + + if 0 <= grid_x < self.grid_size and 0 <= grid_y < self.grid_size: + if self.occupancy_grid[grid_x, grid_y] > 0.5: + # 添加一些噪声 + x += random.uniform(-0.05, 0.05) + y += random.uniform(-0.05, 0.05) + z = random.uniform(-0.2, 0.2) + points.append([x - robot_x, y - robot_y, z]) + break - try: - self.bag = rosbag.Bag(self.bag_file, 'r') - self.running = True - print(f"成功打开ROS bag文件: {self.bag_file}") - return True - except Exception as e: - print(f"打开bag文件失败: {e}") - return False + return np.array(points) - def get_scan_data(self): - """从bag文件读取点云数据""" - if not self.bag: - return None - - for topic, msg, t in self.bag.read_messages(topics=[self.pointcloud_topic]): - if topic == self.pointcloud_topic: - # 转换点云消息为numpy数组 - points = [] - for p in point_cloud2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True): - points.append([p[0], p[1], p[2]]) - if points: - return np.array(points) - return None - - def get_pose_data(self): - """从bag文件读取位姿数据""" - if not self.bag: - return None - - for topic, msg, t in self.bag.read_messages(topics=[self.odom_topic]): - if topic == self.odom_topic: - pose = msg.pose.pose - # 构建4x4变换矩阵 - transform = np.eye(4) - transform[0, 3] = pose.position.x - transform[1, 3] = pose.position.y - transform[2, 3] = pose.position.z - - # 设置旋转 - q = pose.orientation - rotation = R.from_quat([q.x, q.y, q.z, q.w]) - transform[:3, :3] = rotation.as_matrix() - return transform - return None - - -class CARLAInterface(SimulatorInterface): - """CARLA模拟器接口""" - - def __init__(self, host='localhost', port=2000): - super().__init__('carla') - self.host = host - self.port = port - self.client = None - self.world = None - - def connect(self): - """连接到CARLA服务器""" - if not CARLA_AVAILABLE: - print("CARLA模块不可用,请先安装: pip install carla") - return False + def update_navigation(self): + """更新导航系统""" + # 模拟LiDAR数据 + point_cloud = self.simulate_lidar() - try: - import carla - self.client = carla.Client(self.host, self.port) - self.client.set_timeout(10.0) - self.world = self.client.get_world() - self.running = True - print(f"成功连接到CARLA服务器 {self.host}:{self.port}") - return True - except ImportError: - print("请先安装CARLA Python API: pip install carla") - return False - except Exception as e: - print(f"连接CARLA服务器失败: {e}") - return False + # 更新占据栅格地图 + self.navigator.occupancy_grid = self.occupancy_grid + self.navigator.costmap = self.costmap - def get_lidar_data(self, actor_id=None): - """获取LiDAR数据""" - if not self.world: - return None - - import carla - # 查找LiDAR传感器 - if actor_id: - actor = self.world.get_actor(actor_id) - if actor and 'lidar' in actor.type_id.lower(): - lidar_data = [] - # 这里需要根据CARLA API获取点云数据 - # 实际实现会更复杂,需要处理回调 - return np.array(lidar_data) - return None - - -class SocketInterface(SimulatorInterface): - """Socket数据接口(通用)""" - - def __init__(self, host='localhost', port=9999): - super().__init__('socket') - self.host = host - self.port = port - self.socket = None - self.conn = None - self.receiver_thread = None - - def connect(self): - """连接到数据服务器""" - try: - self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - self.socket.connect((self.host, self.port)) - self.conn = self.socket - self.running = True - - # 启动接收线程 - self.receiver_thread = threading.Thread(target=self._receive_data) - self.receiver_thread.daemon = True - self.receiver_thread.start() - - print(f"成功连接到服务器 {self.host}:{self.port}") - return True - except Exception as e: - print(f"连接服务器失败: {e}") - return False + # 碰撞检测 + if self.navigator.check_collision(point_cloud, safety_margin=0.5): + self.obstacles_hit += 1 + print(f"碰撞检测!总碰撞次数: {self.obstacles_hit}") - def _receive_data(self): - """接收数据线程""" - while self.running: - try: - # 接收数据头(包含数据长度) - header = self.conn.recv(4) - if len(header) < 4: - continue - - data_len = struct.unpack('I', header)[0] - - # 接收实际数据 - data = b'' - while len(data) < data_len: - chunk = self.conn.recv(min(4096, data_len - len(data))) - if not chunk: - break - data += chunk - - if len(data) == data_len: - # 解析JSON数据 - try: - message = json.loads(data.decode('utf-8')) - if 'type' in message and message['type'] == 'pointcloud': - points = np.array(message['points']).reshape(-1, 3) - self.data_queue.put(points) - elif 'type' in message and message['type'] == 'pose': - pose = np.array(message['pose']).reshape(4, 4) - self.data_queue.put(('pose', pose)) - except: - pass - - except Exception as e: - if self.running: - print(f"接收数据错误: {e}") - break + # 后退并转向 + self.robot_pose[0, 3] -= 0.3 * np.cos(self.robot_pose[2, 0]) + self.robot_pose[1, 3] -= 0.3 * np.sin(self.robot_pose[2, 0]) - def get_scan_data(self): - """获取扫描数据""" - try: - # 发送请求 - request = {'type': 'get_pointcloud'} - data = json.dumps(request).encode('utf-8') - header = struct.pack('I', len(data)) - self.conn.sendall(header + data) - - # 等待数据 - timeout = 2.0 - start_time = time.time() - while time.time() - start_time < timeout: - if not self.data_queue.empty(): - data = self.data_queue.get() - if isinstance(data, np.ndarray): - return data - time.sleep(0.01) - return None - except Exception as e: - print(f"获取扫描数据失败: {e}") - return None - - -class KITTIDataInterface(SimulatorInterface): - """KITTI数据集接口""" - - def __init__(self, data_path, sequence='00'): - super().__init__('kitti') - self.data_path = data_path - self.sequence = sequence - self.current_frame = 0 - self.total_frames = 0 - self.poses = [] - - def connect(self): - """加载KITTI数据""" - try: - # 加载点云文件列表 - velodyne_path = os.path.join(self.data_path, 'sequences', self.sequence, 'velodyne') - if not os.path.exists(velodyne_path): - print(f"路径不存在: {velodyne_path}") - return False - - self.pointcloud_files = sorted([f for f in os.listdir(velodyne_path) if f.endswith('.bin')]) - self.total_frames = len(self.pointcloud_files) - - # 加载位姿真值 - pose_file = os.path.join(self.data_path, 'poses', f'{self.sequence}.txt') - if os.path.exists(pose_file): - poses_data = np.loadtxt(pose_file).reshape(-1, 3, 4) - for pose in poses_data: - transform = np.eye(4) - transform[:3, :] = pose - self.poses.append(transform) - - self.running = True - print(f"加载KITTI序列 {self.sequence}: {self.total_frames} 帧") - return True - except Exception as e: - print(f"加载KITTI数据失败: {e}") + # 随机转向 + turn_angle = random.uniform(-np.pi / 4, np.pi / 4) + self.robot_pose[:2, :2] = self.rotate_matrix(turn_angle) return False - def get_scan_data(self, frame_idx=None): - """获取指定帧的点云数据""" - if frame_idx is None: - frame_idx = self.current_frame - self.current_frame += 1 - - if frame_idx >= self.total_frames: - return None - - try: - file_path = os.path.join(self.data_path, 'sequences', self.sequence, - 'velodyne', self.pointcloud_files[frame_idx]) - - # 读取bin文件 - scan = np.fromfile(file_path, dtype=np.float32).reshape(-1, 4) - # 只取xyz,忽略反射强度 - points = scan[:, :3] - return points - except Exception as e: - print(f"读取点云帧 {frame_idx} 失败: {e}") - return None - - def get_pose_data(self, frame_idx=None): - """获取位姿真值""" - if frame_idx is None: - frame_idx = self.current_frame - 1 - - if frame_idx < len(self.poses): - return self.poses[frame_idx] - return None - - -class GlobalLocalization: - """全局定位模块""" - - def __init__(self, map_data=None, visualize=True, simulator=None): - self.map_data = map_data - self.map_points = None - self.map_tree = None - self.map_features = None - self.initialized = False - - # 定位状态 - self.current_pose = np.eye(4) - self.confidence = 0.0 - self.localization_mode = "local" # "local" or "global" - - # 扫描上下文描述子 - self.scan_contexts = {} - - # 数据接口 - self.simulator = simulator - - # 可视化相关 - self.visualize = visualize - self.fig = None - self.ax = None - self.history_poses = [] - self.history_confidences = [] - self.scan_history = [] - self.ground_truth_poses = [] - - # 颜色设置 - self.colors = { - 'map': (0.3, 0.3, 0.3, 0.3), # 灰色半透明 - 'current_scan': (1.0, 0.0, 0.0, 0.5), # 红色 - 'matched_scan': (0.0, 1.0, 0.0, 0.5), # 绿色 - 'trajectory': (0.0, 0.0, 1.0, 1.0), # 蓝色 - 'candidates': (1.0, 1.0, 0.0, 0.3), # 黄色 - 'ground_truth': (0.5, 0.0, 0.5, 1.0), # 紫色 - } - - def set_simulator(self, simulator): - """设置数据接口""" - self.simulator = simulator - - def load_map_from_file(self, filepath, file_type='auto'): - """从文件加载地图""" - points = None - - if file_type == 'auto': - # 自动检测文件类型 - if filepath.endswith('.pcd') or filepath.endswith('.ply'): - file_type = 'pcd' - elif filepath.endswith('.bin'): - file_type = 'kitti_bin' - elif filepath.endswith('.csv'): - file_type = 'csv' - elif filepath.endswith('.npy'): - file_type = 'npy' - elif filepath.endswith('.txt'): - file_type = 'txt' - - try: - if (file_type == 'pcd' or file_type == 'ply') and OPEN3D_AVAILABLE: - # 使用Open3D读取 - pcd = o3d.io.read_point_cloud(filepath) - points = np.asarray(pcd.points) - print(f"使用open3d读取 {file_type} 文件: {filepath}") - - elif file_type == 'kitti_bin': - # KITTI格式的bin文件 - points = np.fromfile(filepath, dtype=np.float32).reshape(-1, 4)[:, :3] - print(f"读取KITTI bin文件: {filepath}") - - elif file_type == 'csv': - # CSV文件 - df = pd.read_csv(filepath) - # 尝试不同的列名 - possible_columns = [['x', 'y', 'z'], ['X', 'Y', 'Z'], - ['position_x', 'position_y', 'position_z']] - - for cols in possible_columns: - if all(col in df.columns for col in cols): - points = df[cols].values - break - - if points is None: - # 如果前3列是xyz - points = df.iloc[:, :3].values - - print(f"读取CSV文件: {filepath}") - - elif file_type == 'npy': - # numpy文件 - points = np.load(filepath) - print(f"读取numpy文件: {filepath}") + # 路径规划 + if not hasattr(self, 'global_path') or self.global_path is None: + print("进行全局路径规划...") + self.global_path = self.navigator.global_path_planning( + self.robot_pose, self.goal_pose + ) - elif file_type == 'txt': - # 文本文件 - points = np.loadtxt(filepath) - print(f"读取文本文件: {filepath}") + if self.global_path is not None and len(self.global_path) > 0: + # 局部路径规划 + local_path, control_cmd = self.navigator.local_path_planning( + self.global_path, self.robot_pose, point_cloud + ) - else: - print(f"不支持的文件类型: {file_type}") - return False + if control_cmd is not None: + v, w = control_cmd - if points is not None: - self.load_map(points) - print(f"从 {filepath} 加载了 {len(points)} 个点") - return True - else: - print(f"无法从 {filepath} 读取点云数据") - return False + # 更新机器人位姿 + dt = 0.1 + self.robot_pose[0, 3] += v * np.cos(self.robot_pose[2, 0]) * dt + self.robot_pose[1, 3] += v * np.sin(self.robot_pose[2, 0]) * dt + self.robot_pose[:2, :2] = self.rotate_matrix(w * dt) - except Exception as e: - print(f"加载地图文件失败: {e}") - import traceback - traceback.print_exc() + return True - return False + def rotate_matrix(self, angle): + """创建2D旋转矩阵""" + return np.array([ + [np.cos(angle), -np.sin(angle)], + [np.sin(angle), np.cos(angle)] + ]) - def initialize_visualization(self): - """初始化可视化窗口""" - if not self.visualize: - return + def visualize(self): + """可视化当前状态""" + self.ax1.clear() + self.ax2.clear() - plt.ion() # 开启交互模式 + # 1. 显示地图和路径 + self.ax1.imshow(self.costmap.T, cmap='gray', origin='lower', + extent=[0, self.env_size, 0, self.env_size]) - self.fig = plt.figure(figsize=(15, 10)) + # 显示机器人位置 + robot_x, robot_y = self.robot_pose[0, 3], self.robot_pose[1, 3] + self.ax1.plot(robot_x, robot_y, 'ro', markersize=10, label='Robot') - # 创建3D点云视图 - self.ax1 = self.fig.add_subplot(231, projection='3d') - self.ax1.set_title('3D Point Cloud View') - self.ax1.set_xlabel('X (m)') - self.ax1.set_ylabel('Y (m)') - self.ax1.set_zlabel('Z (m)') + # 显示目标位置 + goal_x, goal_y = self.goal_pose[0, 3], self.goal_pose[1, 3] + self.ax1.plot(goal_x, goal_y, 'g*', markersize=15, label='Goal') - # 创建2D俯视图 - self.ax2 = self.fig.add_subplot(232) - self.ax2.set_title('2D Top View') - self.ax2.set_xlabel('X (m)') - self.ax2.set_ylabel('Y (m)') - self.ax2.set_aspect('equal') - - # 创建轨迹视图 - self.ax3 = self.fig.add_subplot(233) - self.ax3.set_title('Trajectory and Confidence') - self.ax3.set_xlabel('Step') - self.ax3.set_ylabel('Confidence') - - # 创建扫描上下文视图 - self.ax4 = self.fig.add_subplot(234) - self.ax4.set_title('Scan Context Descriptor') - self.ax4.set_xlabel('Sector') - self.ax4.set_ylabel('Ring') - - # 创建位姿误差视图 - self.ax5 = self.fig.add_subplot(235) - self.ax5.set_title('Pose Error') - self.ax5.set_xlabel('Step') - self.ax5.set_ylabel('Error (m/rad)') - - # 创建ICP匹配视图 - self.ax6 = self.fig.add_subplot(236) - self.ax6.set_title('ICP Matching') - self.ax6.set_xlabel('X (m)') - self.ax6.set_ylabel('Y (m)') - self.ax6.set_aspect('equal') + # 显示全局路径 + if hasattr(self, 'global_path') and self.global_path is not None: + self.ax1.plot(self.global_path[:, 0], self.global_path[:, 1], + 'b-', linewidth=2, alpha=0.7, label='Global Path') - plt.tight_layout() - plt.show(block=False) - - def load_map(self, map_points, map_features=None): - """加载全局地图""" - self.map_points = map_points - self.map_features = map_features - - if map_points is not None and len(map_points) > 0: - self.map_tree = KDTree(map_points[:, :3]) # 只使用xyz - self.initialized = True - print(f"地图已加载,包含 {len(map_points)} 个点") - - # 可视化地图 - if self.visualize: - if self.fig is None: - self.initialize_visualization() - self._visualize_map() - else: - print("警告:地图点云为空") - - def _visualize_map(self): - """可视化地图点云""" - if not self.visualize or self.map_points is None: - return - - # 3D视图 - self.ax1.clear() - if len(self.map_points) > 10000: # 太多点的话采样显示 - indices = np.random.choice(len(self.map_points), 10000, replace=False) - display_points = self.map_points[indices] - else: - display_points = self.map_points + # 显示轨迹 + if len(self.trajectory) > 0: + traj = np.array(self.trajectory) + self.ax1.plot(traj[:, 0], traj[:, 1], 'y-', linewidth=1, alpha=0.5, label='Trajectory') - self.ax1.scatter(display_points[:, 0], display_points[:, 1], display_points[:, 2], - c=[self.colors['map']], s=1, marker='.') - - # 2D俯视图 - self.ax2.clear() - self.ax2.scatter(display_points[:, 0], display_points[:, 1], - c=[self.colors['map'][:3]], s=1, marker='.', alpha=0.3) - - self.ax1.set_title(f'Map Points: {len(self.map_points)}') - self.fig.canvas.draw_idle() - - def _visualize_localization(self, scan_points, pose, confidence, - matched_points=None, candidates=None, - ground_truth=None): - """可视化定位结果""" - if not self.visualize: - return - - # 变换当前扫描点 - transformed_scan = self._transform_points(scan_points, pose) - - # 更新历史记录 - self.history_poses.append(pose) - self.history_confidences.append(confidence) - self.scan_history.append(transformed_scan) - if ground_truth is not None: - self.ground_truth_poses.append(ground_truth) - - # 限制历史记录长度 - max_history = 100 - if len(self.history_poses) > max_history: - self.history_poses = self.history_poses[-max_history:] - self.history_confidences = self.history_confidences[-max_history:] - self.scan_history = self.scan_history[-max_history:] - if self.ground_truth_poses: - self.ground_truth_poses = self.ground_truth_poses[-max_history:] - - # 1. 3D点云视图 - self.ax1.clear() - if len(self.map_points) > 10000: - indices = np.random.choice(len(self.map_points), 10000, replace=False) - display_map = self.map_points[indices] - else: - display_map = self.map_points - - self.ax1.scatter(display_map[:, 0], display_map[:, 1], display_map[:, 2], - c=[self.colors['map']], s=1, marker='.', alpha=0.3, label='Map') - - # 当前扫描 - self.ax1.scatter(transformed_scan[:, 0], transformed_scan[:, 1], transformed_scan[:, 2], - c='r', s=5, marker='o', alpha=0.5, label='Current Scan') - - # 轨迹 - if len(self.history_poses) > 1: - trajectory = np.array([p[:3, 3] for p in self.history_poses]) - self.ax1.plot(trajectory[:, 0], trajectory[:, 1], trajectory[:, 2], - c=self.colors['trajectory'], linewidth=2, label='Estimated') - - # 真值轨迹 - if self.ground_truth_poses and len(self.ground_truth_poses) > 1: - gt_trajectory = np.array([p[:3, 3] for p in self.ground_truth_poses]) - self.ax1.plot(gt_trajectory[:, 0], gt_trajectory[:, 1], gt_trajectory[:, 2], - c=self.colors['ground_truth'], linewidth=2, linestyle='--', - label='Ground Truth') - - self.ax1.set_title(f'3D View - Confidence: {confidence:.3f}') self.ax1.set_xlabel('X (m)') self.ax1.set_ylabel('Y (m)') - self.ax1.set_zlabel('Z (m)') + self.ax1.set_title('Navigation Map') self.ax1.legend() - - # 2. 2D俯视图 - self.ax2.clear() - self.ax2.scatter(display_map[:, 0], display_map[:, 1], - c=[self.colors['map'][:3]], s=1, marker='.', alpha=0.3) - - self.ax2.scatter(transformed_scan[:, 0], transformed_scan[:, 1], - c='r', s=10, marker='o', alpha=0.6, label='Current Scan') - - if matched_points is not None: - self.ax2.scatter(matched_points[:, 0], matched_points[:, 1], - c='g', s=20, marker='x', alpha=0.8, label='Matched Points') - - # 轨迹 - if len(self.history_poses) > 1: - trajectory = np.array([p[:3, 3] for p in self.history_poses]) - self.ax2.plot(trajectory[:, 0], trajectory[:, 1], - c=self.colors['trajectory'], linewidth=2, label='Estimated') - - # 真值轨迹 - if self.ground_truth_poses and len(self.ground_truth_poses) > 1: - gt_trajectory = np.array([p[:3, 3] for p in self.ground_truth_poses]) - self.ax2.plot(gt_trajectory[:, 0], gt_trajectory[:, 1], - c=self.colors['ground_truth'], linewidth=2, linestyle='--', - label='Ground Truth') - - # 候选位姿 - if candidates is not None: - for i, candidate in enumerate(candidates[:5]): # 只显示前5个 - self.ax2.scatter(candidate[0, 3], candidate[1, 3], - c='y', s=50, marker='*', alpha=0.5) - - self.ax2.set_title(f'2D Top View - Pose: [{pose[0, 3]:.1f}, {pose[1, 3]:.1f}]') + self.ax1.grid(True) + self.ax1.axis('equal') + + # 2. 显示代价地图热图 + costmap_display = self.ax2.imshow(self.costmap.T, cmap='viridis', + origin='lower', vmin=0, vmax=1, + extent=[0, self.env_size, 0, self.env_size]) + self.ax2.plot(robot_x, robot_y, 'ro', markersize=10) self.ax2.set_xlabel('X (m)') self.ax2.set_ylabel('Y (m)') - self.ax2.legend() - self.ax2.grid(True, alpha=0.3) - - # 3. 轨迹和置信度 - self.ax3.clear() - if self.history_confidences: - steps = range(len(self.history_confidences)) - self.ax3.plot(steps, self.history_confidences, 'b-', linewidth=2, label='Confidence') - self.ax3.fill_between(steps, 0, self.history_confidences, alpha=0.3) - self.ax3.set_ylim(0, 1.1) - self.ax3.set_xlabel('Step') - self.ax3.set_ylabel('Confidence') - self.ax3.set_title('Localization Confidence History') - self.ax3.grid(True, alpha=0.3) - self.ax3.legend() - - # 4. 扫描上下文 - self.ax4.clear() - current_context = self.create_scan_context(scan_points) - im = self.ax4.imshow(current_context, aspect='auto', cmap='viridis') - self.ax4.set_title('Current Scan Context') - self.ax4.set_xlabel('Sector') - self.ax4.set_ylabel('Ring') - plt.colorbar(im, ax=self.ax4) - - # 5. 位姿误差(如果有真值的话) - self.ax5.clear() - if self.ground_truth_poses and len(self.ground_truth_poses) > 1: - # 计算估计位姿与真值的误差 - errors = [] - for i in range(min(len(self.history_poses), len(self.ground_truth_poses))): - if i < len(self.ground_truth_poses): - # 位置误差 - pos_error = np.linalg.norm( - self.history_poses[i][:3, 3] - self.ground_truth_poses[i][:3, 3] - ) - - # 角度误差 - R_est = self.history_poses[i][:3, :3] - R_gt = self.ground_truth_poses[i][:3, :3] - angle_error = np.arccos((np.trace(R_est.T @ R_gt) - 1) / 2) - - errors.append(pos_error) - - if errors: - steps = range(len(errors)) - self.ax5.plot(steps, errors, 'r-', label='Position Error (m)') - self.ax5.axhline(y=np.mean(errors), color='r', linestyle='--', - alpha=0.5, label=f'Avg: {np.mean(errors):.3f}m') - - self.ax5.set_xlabel('Step') - self.ax5.set_ylabel('Error (m)') - self.ax5.set_title('Localization Error vs Ground Truth') - self.ax5.grid(True, alpha=0.3) - self.ax5.legend() - - # 6. ICP匹配视图 - self.ax6.clear() - if matched_points is not None and len(matched_points) > 0: - # 显示匹配点对 - for i in range(min(20, len(transformed_scan))): # 只显示20个匹配对 - scan_pt = transformed_scan[i, :2] - map_pt = matched_points[i, :2] - self.ax6.plot([scan_pt[0], map_pt[0]], [scan_pt[1], map_pt[1]], - 'g-', alpha=0.3, linewidth=0.5) - - self.ax6.scatter(matched_points[:, 0], matched_points[:, 1], - c='g', s=20, marker='x', alpha=0.8, label='Map Points') - - self.ax6.scatter(transformed_scan[:, 0], transformed_scan[:, 1], - c='r', s=10, marker='o', alpha=0.6, label='Scan Points') - - self.ax6.set_title(f'ICP Matching - Inliers: {len(matched_points) if matched_points is not None else 0}') - self.ax6.set_xlabel('X (m)') - self.ax6.set_ylabel('Y (m)') - self.ax6.legend() - self.ax6.grid(True, alpha=0.3) - - plt.tight_layout() - self.fig.canvas.draw_idle() - plt.pause(0.01) # 短暂暂停以更新显示 - - def localize_with_scan_matching(self, scan_points, initial_guess=None, visualize=True): - """使用扫描匹配进行定位""" - if not self.initialized or self.map_points is None: - return np.eye(4), 0.0 - - if initial_guess is None: - initial_guess = np.eye(4) - - # 使用ICP进行精配准 - refined_pose, matched_points = self._icp_localization(scan_points, initial_guess) - - # 计算匹配分数 - confidence = self._compute_match_confidence(scan_points, refined_pose) - - # 获取真值(如果有) - ground_truth = None - if self.simulator: - ground_truth = self.simulator.get_pose_data() - - # 可视化 - if self.visualize and visualize: - self._visualize_localization( - scan_points=scan_points, - pose=refined_pose, - confidence=confidence, - matched_points=matched_points, - ground_truth=ground_truth - ) - - return refined_pose, confidence - - def _icp_localization(self, scan_points, initial_guess, max_iterations=30): - """ICP定位""" - pose = initial_guess.copy() - all_matched_points = None - - for iteration in range(max_iterations): - # 变换扫描点 - transformed_points = self._transform_points(scan_points, pose) - - # 寻找最近邻 - distances, indices = self.map_tree.query(transformed_points[:, :3]) - - # 过滤掉距离太远的点 - valid_mask = distances < 2.0 # 2米阈值 - if np.sum(valid_mask) < 10: # 至少需要10个有效点 - break - - valid_scan = scan_points[valid_mask] - valid_map = self.map_points[indices[valid_mask], :3] - - # 保存匹配点用于可视化 - if iteration == max_iterations - 1: # 最后一次迭代 - all_matched_points = valid_map - - # 计算最优变换 - R_mat, t_vec = self._compute_rigid_transform(valid_scan[:, :3], valid_map) - - # 更新位姿 - delta_pose = np.eye(4) - delta_pose[:3, :3] = R_mat - delta_pose[:3, 3] = t_vec - - pose = delta_pose @ pose - - # 检查收敛 - if np.linalg.norm(t_vec) < 0.001 and np.linalg.norm(R_mat - np.eye(3)) < 0.001: - break - - return pose, all_matched_points - - def _transform_points(self, points, pose): - """变换点云""" - transformed = (pose[:3, :3] @ points[:, :3].T + pose[:3, 3:4]).T - - if points.shape[1] > 3: - transformed = np.hstack([transformed, points[:, 3:]]) - - return transformed - - def _compute_rigid_transform(self, A, B): - """计算刚体变换""" - centroid_A = np.mean(A, axis=0) - centroid_B = np.mean(B, axis=0) - - AA = A - centroid_A - BB = B - centroid_B - - H = AA.T @ BB - U, S, Vt = np.linalg.svd(H) - - R_mat = Vt.T @ U.T - - # 处理反射情况 - if np.linalg.det(R_mat) < 0: - Vt[-1, :] *= -1 - R_mat = Vt.T @ U.T - - t_vec = centroid_B - R_mat @ centroid_A - - return R_mat, t_vec - - def _compute_match_confidence(self, scan_points, pose): - """计算匹配置信度""" - transformed_points = self._transform_points(scan_points, pose) - - distances, _ = self.map_tree.query(transformed_points[:, :3], k=1) + self.ax2.set_title('Cost Map') + self.ax2.grid(True) - # 计算内点比例 - inlier_ratio = np.mean(distances < 0.5) # 0.5米内点阈值 + plt.colorbar(costmap_display, ax=self.ax2, label='Cost (0=high, 1=low)') - # 综合置信度 - confidence = min(1.0, inlier_ratio * 2.0) - - return confidence - - def create_scan_context(self, scan_points): - """创建扫描上下文描述子""" - num_sectors = 60 - num_rings = 20 - max_range = 50.0 - - descriptor = np.zeros((num_rings, num_sectors)) - - for point in scan_points: - x, y, z = point[:3] - - # 转换为极坐标 - radius = np.sqrt(x ** 2 + y ** 2) - angle = np.arctan2(y, x) - - if radius > max_range: - continue - - # 计算环和扇区索引 - ring_idx = min(int(radius / max_range * num_rings), num_rings - 1) - sector_idx = int((angle + np.pi) / (2 * np.pi) * num_sectors) % num_sectors + plt.tight_layout() + plt.draw() + plt.pause(0.01) - # 存储最大高度 - height = z - descriptor[ring_idx, sector_idx] = max(descriptor[ring_idx, sector_idx], height) + def run(self, max_steps=500): + """运行模拟""" + print("开始自主导航模拟...") + print(f"起始位置: ({self.robot_pose[0, 3]:.1f}, {self.robot_pose[1, 3]:.1f})") + print(f"目标位置: ({self.goal_pose[0, 3]:.1f}, {self.goal_pose[1, 3]:.1f})") - return descriptor + for step in range(max_steps): + # 记录当前位置 + self.trajectory.append([self.robot_pose[0, 3], self.robot_pose[1, 3]]) - def run_with_simulator(self, max_frames=1000, start_frame=0): - """使用模拟器数据运行定位""" - if not self.simulator or not self.simulator.running: - print("模拟器未连接或未运行") - return + # 更新导航 + success = self.update_navigation() - frame_count = 0 + # 可视化 + if step % 5 == 0: + self.visualize() - for i in range(start_frame, start_frame + max_frames): - # 从模拟器获取数据 - scan_points = self.simulator.get_scan_data() + # 检查是否到达目标 + distance_to_goal = np.sqrt( + (self.robot_pose[0, 3] - self.goal_pose[0, 3]) ** 2 + + (self.robot_pose[1, 3] - self.goal_pose[1, 3]) ** 2 + ) - if scan_points is None: - print("无法获取扫描数据") + if distance_to_goal < 1.0: + print(f"\n✓ 成功到达目标!") + print(f"总步数: {step}") + print(f"总碰撞次数: {self.obstacles_hit}") + print(f"最终位置误差: {distance_to_goal:.2f} 米") break - # 检查点云质量 - if len(scan_points) < 100: - print(f"帧 {i}: 点云点数过少 ({len(scan_points)})") - continue - - print(f"\n帧 {i}: 处理 {len(scan_points)} 个点") - - # 进行定位 - start_time = time.time() - - if frame_count == 0 or self.confidence < 0.3: - # 低置信度时尝试全局重定位 - pose, confidence = self.global_relocalization(scan_points) - if confidence > 0.3: - self.localization_mode = "local" - else: - self.localization_mode = "global" - else: - # 正常局部定位 - pose, confidence = self.localize_with_scan_matching( - scan_points, - self.current_pose - ) - - processing_time = time.time() - start_time - - # 更新状态 - self.current_pose = pose - self.confidence = confidence + # 显示进度 + if step % 50 == 0: + print(f"步数: {step}, 距离目标: {distance_to_goal:.1f}米") - print(f" 置信度: {confidence:.3f}") - print(f" 位姿: [{pose[0, 3]:.2f}, {pose[1, 3]:.2f}, {pose[2, 3]:.2f}]") - print(f" 处理时间: {processing_time:.3f}秒") - print(f" 模式: {self.localization_mode}") + time.sleep(0.05) - frame_count += 1 - - # 控制帧率 - time.sleep(0.05) # 20Hz - - # 检查是否应该停止 - if not self.simulator.running: - break - - print(f"\n处理完成,共处理 {frame_count} 帧") + # 显示最终结果 + self.visualize() + plt.ioff() # 保存结果 - if self.visualize: - self.save_visualization() - plt.show(block=True) - - def global_relocalization(self, scan_points, gnss_hint=None): - """全局重定位""" - if not self.initialized: - return np.eye(4), 0.0 - - # 方法1:使用扫描上下文 - current_descriptor = self.create_scan_context(scan_points) - matched_node, context_score = self.scan_context_match(current_descriptor) - - if context_score > 0.7: # 高置信度匹配 - print(f"扫描上下文匹配成功,分数: {context_score}") - # 可以返回匹配节点的位姿 - return np.eye(4), context_score - - # 方法2:使用GNSS提示 - if gnss_hint is not None: - pose, confidence = self.localize_with_gnss(gnss_hint) - if confidence > 0.5: - print(f"GNSS提示定位成功,置信度: {confidence}") - return pose, confidence - - # 方法3:全局ICP(计算量大,作为最后手段) - print("尝试全局ICP重定位...") - - # 在地图中采样一些候选位姿 - candidates = self._generate_candidates(scan_points) - - best_pose = np.eye(4) - best_score = 0.0 - best_matched_points = None - - for candidate in candidates: - pose, matched_points = self._icp_localization(scan_points, candidate, max_iterations=10) - score = self._compute_match_confidence(scan_points, pose) - - if score > best_score: - best_score = score - best_pose = pose - best_matched_points = matched_points - - # 可视化候选位姿和最佳匹配 - if self.visualize: - self._visualize_localization( - scan_points=scan_points, - pose=best_pose, - confidence=best_score, - matched_points=best_matched_points, - candidates=candidates - ) - - if best_score > 0.3: - print(f"全局ICP重定位成功,分数: {best_score}") - return best_pose, best_score - - print("重定位失败") - return np.eye(4), 0.0 - - def _generate_candidates(self, scan_points, num_candidates=20): - """生成候选位姿""" - candidates = [] - - # 在地图边界内随机采样 - if self.map_points is not None and len(self.map_points) > 0: - map_min = np.min(self.map_points[:, :3], axis=0) - map_max = np.max(self.map_points[:, :3], axis=0) - - for _ in range(num_candidates): - position = np.random.uniform(map_min, map_max) - yaw = np.random.uniform(-np.pi, np.pi) - - pose = np.eye(4) - pose[:3, 3] = position - pose[:3, :3] = R.from_euler('z', yaw).as_matrix() - - candidates.append(pose) - - return candidates - - def localize_with_gnss(self, gnss_position, initial_guess=None): - """使用GNSS进行初始全局定位""" - if not self.initialized or self.map_points is None: - return np.eye(4), 0.0 - - # 将GNSS坐标转换为局部坐标 - # 这里需要知道地图的GNSS参考点 - local_position = gnss_position # 简化:假设已经是局部坐标 - - # 在地图中寻找最近的点 - distances, indices = self.map_tree.query(local_position.reshape(1, -1), k=10) - - # 计算平均位置作为初始位姿 - if len(indices) > 0: - nearest_points = self.map_points[indices[0], :3] - estimated_position = np.mean(nearest_points, axis=0) - - # 构建位姿(假设水平) - pose = np.eye(4) - pose[:3, 3] = estimated_position - - confidence = 1.0 / (1.0 + np.mean(distances)) - - # 可视化 - if self.visualize: - self._visualize_localization( - scan_points=np.array([[local_position[0], local_position[1], 0]]), - pose=pose, - confidence=confidence, - matched_points=nearest_points - ) - - return pose, confidence - - return np.eye(4), 0.0 - - def scan_context_match(self, current_descriptor): - """扫描上下文匹配""" - if not self.scan_contexts: - return None, 0.0 - - best_match = None - best_score = 0.0 - - for node_id, descriptor in self.scan_contexts.items(): - # 计算描述子相似度 - score = self._descriptor_similarity(current_descriptor, descriptor) - - if score > best_score: - best_score = score - best_match = node_id - - return best_match, best_score - - def _descriptor_similarity(self, desc1, desc2): - """计算描述子相似度""" - # 列向量的余弦相似度 - similarity = 0.0 - - for i in range(desc1.shape[1]): - col1 = desc1[:, i] - col2 = desc2[:, i] - - norm1 = np.linalg.norm(col1) - norm2 = np.linalg.norm(col2) - - if norm1 > 0 and norm2 > 0: - similarity += np.dot(col1, col2) / (norm1 * norm2) - - return similarity / desc1.shape[1] - - def update_scan_context(self, node_id, descriptor): - """更新扫描上下文数据库""" - self.scan_contexts[node_id] = descriptor - - def save_visualization(self, filename="localization_result.png"): - """保存可视化结果""" - if self.fig is not None: - self.fig.savefig(filename, dpi=300, bbox_inches='tight') - print(f"可视化结果已保存到 {filename}") - - def close_visualization(self): - """关闭可视化窗口""" - if self.fig is not None: - plt.close(self.fig) - self.fig = None - - -def main_demo(): - """演示模式:使用合成数据测试""" - print("=" * 60) - print("全局定位系统 - 演示模式") - print("=" * 60) - - # 创建合成地图数据 - map_points = [] - - # 创建一些建筑物 - print("生成合成地图...") - for x in np.arange(-50, 51, 10): - for y in np.arange(-50, 51, 10): - # 建筑物点云 - building_points = np.random.randn(100, 3) * 2 - building_points[:, 0] += x - building_points[:, 1] += y - building_points[:, 2] = building_points[:, 2] * 5 + 10 # 高度 - map_points.append(building_points) - - # 创建道路 - road_points = [] - for x in np.arange(-50, 51, 0.5): - for y in np.arange(-5, 6, 0.5): - road_points.append([x, y, 0]) - map_points.append(np.array(road_points)) - - map_points = np.vstack(map_points) - - # 创建定位器 - print("初始化定位器...") - locator = GlobalLocalization(visualize=True) - locator.load_map(map_points) - - # 模拟轨迹 - print("开始模拟定位...") - trajectory = [] - radius = 30.0 - total_frames = 50 - - for i in range(total_frames): - angle = i * 0.1 - x = radius * np.cos(angle) - y = radius * np.sin(angle) - - # 模拟当前扫描(地图的一部分加上噪声) - # 找到附近的点作为扫描 - if i == 0: - current_pose = np.eye(4) - else: - # 模拟运动 - current_pose = np.eye(4) - current_pose[0, 3] = x - current_pose[1, 3] = y - - # 朝向切线方向 - yaw = angle + np.pi / 2 - current_pose[:3, :3] = R.from_euler('z', yaw).as_matrix() - - # 获取附近的点作为扫描 - transformed_map = locator._transform_points(map_points, np.linalg.inv(current_pose)) - - # 选择前方的点 - mask = (transformed_map[:, 0] > 0) & (transformed_map[:, 0] < 30) & \ - (np.abs(transformed_map[:, 1]) < 10) & \ - (np.random.rand(len(transformed_map)) < 0.1) # 采样 - - scan_points = map_points[mask] - - # 添加噪声 - if len(scan_points) > 0: - scan_points = scan_points + np.random.randn(*scan_points.shape) * 0.05 - - # 进行定位 - if i == 0: - initial_guess = np.eye(4) - else: - # 使用上一帧的位姿作为初始猜测 - initial_guess = locator.current_pose - - pose, confidence = locator.localize_with_scan_matching(scan_points, initial_guess) - - # 更新当前位姿 - locator.current_pose = pose - - print(f"帧 {i + 1}/{total_frames}: 置信度 {confidence:.3f}, 位姿 [{pose[0, 3]:.1f}, {pose[1, 3]:.1f}]") - - time.sleep(0.1) - else: - print(f"帧 {i + 1}/{total_frames}: 无有效点云数据") - - # 保存结果 - print("\n保存结果...") - locator.save_visualization("demo_result.png") - print("演示完成!按任意键关闭窗口...") - plt.show(block=True) - - -def main_from_file(): - """从文件加载地图并测试""" - print("=" * 60) - print("全局定位系统 - 文件模式") - print("=" * 60) - - # 创建定位器 - locator = GlobalLocalization(visualize=True) - - # 从文件加载地图 - map_file = input("请输入地图文件路径(支持.pcd, .ply, .bin, .csv, .npy, .txt): ").strip() - - if not map_file: - map_file = "map.pcd" # 默认文件 - - if not os.path.exists(map_file): - print(f"文件不存在: {map_file}") - print("使用演示模式生成合成地图...") - main_demo() - return - - if locator.load_map_from_file(map_file): - print("地图加载成功,开始模拟定位...") - - # 在地图内随机生成测试轨迹 - map_min = np.min(locator.map_points[:, :3], axis=0) - map_max = np.max(locator.map_points[:, :3], axis=0) - - test_frames = 30 - successful_localizations = 0 - - for i in range(test_frames): - # 生成随机位姿 - random_pose = np.eye(4) - random_pose[:3, 3] = np.random.uniform(map_min, map_max) - random_pose[:3, :3] = R.from_euler('z', np.random.uniform(-np.pi, np.pi)).as_matrix() - - # 从该位姿生成模拟扫描 - # 获取附近的点 - transformed_map = locator._transform_points(locator.map_points, np.linalg.inv(random_pose)) - - # 选择前方的点 - mask = (transformed_map[:, 0] > 0) & (transformed_map[:, 0] < 20) & \ - (np.abs(transformed_map[:, 1]) < 5) & \ - (np.random.rand(len(transformed_map)) < 0.05) - - scan_points = locator.map_points[mask] - - if len(scan_points) > 100: - # 添加噪声 - scan_points = scan_points + np.random.randn(*scan_points.shape) * 0.1 - - # 进行定位(使用一个初始猜测) - initial_guess = np.eye(4) - initial_guess[:3, 3] = random_pose[:3, 3] + np.random.randn(3) * 5 # 初始猜测有误差 - - pose, confidence = locator.localize_with_scan_matching(scan_points, initial_guess) - - # 计算误差 - pos_error = np.linalg.norm(pose[:3, 3] - random_pose[:3, 3]) - - if confidence > 0.5 and pos_error < 1.0: - successful_localizations += 1 - - print(f"测试 {i + 1}/{test_frames}: 置信度 {confidence:.3f}, 位置误差 {pos_error:.2f}m") - - time.sleep(0.2) - else: - print(f"测试 {i + 1}/{test_frames}: 点云点数不足 ({len(scan_points)})") - - success_rate = successful_localizations / test_frames * 100 - print(f"\n测试完成!成功率: {success_rate:.1f}%") - - locator.save_visualization("file_test_result.png") - print("按任意键关闭窗口...") - plt.show(block=True) - else: - print("地图加载失败") - - -def main_simple_test(): - """简单测试模式""" - print("=" * 60) - print("全局定位系统 - 简单测试模式") - print("=" * 60) - - # 创建简单的测试地图(一个环形) - theta = np.linspace(0, 2 * np.pi, 1000) - radius = 20.0 - height = 5.0 - - # 环形点云 - ring_points = [] - for t in theta: - x = radius * np.cos(t) - y = radius * np.sin(t) - for h in np.linspace(0, height, 10): - ring_points.append([x, y, h]) - - # 添加一些随机的内部点 - for _ in range(1000): - r = np.random.uniform(0, radius * 0.8) - angle = np.random.uniform(0, 2 * np.pi) - x = r * np.cos(angle) - y = r * np.sin(angle) - z = np.random.uniform(0, height) - ring_points.append([x, y, z]) - - map_points = np.array(ring_points) - - # 创建定位器 - locator = GlobalLocalization(visualize=True) - locator.load_map(map_points) - - # 沿着环形轨迹测试 - test_angles = np.linspace(0, 2 * np.pi, 20, endpoint=False) - - for i, angle in enumerate(test_angles): - # 真实位姿 - x = radius * np.cos(angle) - y = radius * np.sin(angle) - z = 2.5 # 中间高度 - - true_pose = np.eye(4) - true_pose[0, 3] = x - true_pose[1, 3] = y - true_pose[2, 3] = z - true_pose[:3, :3] = R.from_euler('z', angle + np.pi / 2).as_matrix() - - # 生成扫描数据 - # 选取前面的点 - transformed_map = locator._transform_points(map_points, np.linalg.inv(true_pose)) - mask = (transformed_map[:, 0] > 0) & (transformed_map[:, 0] < 15) & \ - (np.abs(transformed_map[:, 1]) < 8) & \ - (transformed_map[:, 2] > 0) & (transformed_map[:, 2] < height) - - scan_points = map_points[mask] - - if len(scan_points) > 50: - # 添加噪声 - scan_points = scan_points + np.random.randn(*scan_points.shape) * 0.05 - - # 添加初始猜测误差 - initial_guess = true_pose.copy() - initial_guess[:3, 3] += np.random.randn(3) * 2 # 位置误差 - yaw_error = np.random.uniform(-0.3, 0.3) - initial_guess[:3, :3] = initial_guess[:3, :3] @ R.from_euler('z', yaw_error).as_matrix() - - # 进行定位 - pose, confidence = locator.localize_with_scan_matching(scan_points, initial_guess) + self.fig.savefig('navigation_result.png', dpi=150) + print(f"\n导航结果已保存到 'navigation_result.png'") - # 计算误差 - pos_error = np.linalg.norm(pose[:3, 3] - true_pose[:3, 3]) + # 显示总结 + print("\n=== 模拟总结 ===") + print(f"总行驶距离: {len(self.trajectory) * 0.1:.1f} 米") + print(f"碰撞次数: {self.obstacles_hit}") + print(f"路径规划次数: 1") - print(f"测试 {i + 1}/{len(test_angles)}: " - f"置信度 {confidence:.3f}, " - f"位置误差 {pos_error:.2f}m") + plt.show() - time.sleep(0.3) - locator.save_visualization("simple_test_result.png") - print("\n测试完成!按任意键关闭窗口...") - plt.show(block=True) +def main(): + """主函数""" + simulator = SimpleSimulator() + simulator.run(max_steps=500) if __name__ == "__main__": - print("全局定位系统 v1.0") - print("=" * 60) - - # 检查依赖 - print("检查依赖...") - print(f"open3d: {'可用' if OPEN3D_AVAILABLE else '不可用'}") - print(f"ROS: {'可用' if ROS_AVAILABLE else '不可用'}") - print(f"CARLA: {'可用' if CARLA_AVAILABLE else '不可用'}") - print("=" * 60) - - # 简单菜单 - print("\n请选择运行模式:") - print("1. 演示模式 (使用合成数据)") - print("2. 文件模式 (从文件加载地图)") - print("3. 简单测试模式 (环形轨迹)") - print("4. 退出") - - choice = input("\n请输入选择 (1-4): ").strip() - - if choice == "1": - main_demo() - elif choice == "2": - main_from_file() - elif choice == "3": - main_simple_test() - elif choice == "4": - print("退出程序") - else: - print("无效选择,使用演示模式") - main_demo() \ No newline at end of file + main() \ No newline at end of file