当前位置：首页 > news >正文

第1章 SLAM技术导论

news 2025/11/8 8:19:03

第1章 SLAM技术导论

章节概述

本章将带你全面了解SLAM（Simultaneous Localization and Mapping）技术的基本概念、发展历程和在现代机器人领域的重要应用。通过本章学习，你将建立起SLAM技术的整体认知框架，为后续深入学习奠定坚实基础。

1.1 SLAM技术概述

内容讲解

SLAM，即同时定位与地图构建（Simultaneous Localization and Mapping），是机器人在未知环境中同时完成自身定位和环境地图构建的技术。这一技术解决了机器人导航中的"鸡生蛋，蛋生鸡"问题：要进行定位需要已知地图，而要构建地图又需要知道机器人的精确位置。

SLAM技术的核心挑战包括：

不确定性管理：传感器噪声、运动模型误差、数据关联不确定性
计算复杂度：实时性要求与算法复杂度的平衡
环境适应性：动态环境、光照变化、纹理缺乏等挑战
尺度问题：单目视觉的尺度不确定性

知识点

SLAM定义与核心问题
- 同时性：定位与建图的相互依赖
- 递归性：当前估计依赖于历史信息
- 不确定性：噪声与误差的传播
SLAM在机器人领域的重要性
- 自主导航的基础技术
- 人工智能的空间认知能力
- 工业4.0的关键支撑技术
SLAM技术分类
- 按传感器：视觉SLAM、激光SLAM、多传感器融合SLAM
- 按方法：滤波方法、图优化方法、深度学习方法
- 按环境：室内SLAM、室外SLAM、水下SLAM

代码示例

"""
SLAM基础概念演示：简单的2D机器人位置估计
这个例子展示了SLAM中位置不确定性的基本概念
"""
import numpy as np
import matplotlib.pyplot as plt
from scipy.stats import multivariate_normalclass SimpleSLAM2D:def __init__(self, initial_pos=[0, 0], initial_uncertainty=1.0):"""初始化简单的2D SLAM系统Args:initial_pos: 初始位置 [x, y]initial_uncertainty: 初始位置不确定性"""self.pos = np.array(initial_pos, dtype=float)self.cov = np.eye(2) * initial_uncertainty  # 位置协方差矩阵self.trajectory = [self.pos.copy()]self.landmarks = []  # 观测到的路标点def predict(self, motion, motion_noise=0.1):"""预测步骤：根据运动模型更新位置估计Args:motion: 运动向量 [dx, dy]motion_noise: 运动噪声标准差"""# 更新位置估计self.pos += motion# 增加运动不确定性Q = np.eye(2) * motion_noise**2  # 过程噪声协方差self.cov += Qself.trajectory.append(self.pos.copy())print(f"预测后位置: ({self.pos[0]:.2f}, {self.pos[1]:.2f})")print(f"不确定性: {np.sqrt(np.diag(self.cov))}")def update(self, observation, landmark_pos, obs_noise=0.05):"""更新步骤：根据观测信息修正位置估计Args:observation: 观测值 [距离, 角度]landmark_pos: 已知路标位置 [x, y]  obs_noise: 观测噪声标准差"""# 计算期望观测值diff = np.array(landmark_pos) - self.posexpected_distance = np.linalg.norm(diff)expected_angle = np.arctan2(diff[1], diff[0])expected_obs = np.array([expected_distance, expected_angle])# 观测残差residual = observation - expected_obs# 观测矩阵（雅可比矩阵）distance = expected_distanceH = np.array([[-diff[0]/distance, -diff[1]/distance],  # 距离对位置的偏导[diff[1]/distance**2, -diff[0]/distance**2]  # 角度对位置的偏导])# 观测噪声协方差R = np.eye(2) * obs_noise**2# 卡尔曼增益S = H @ self.cov @ H.T + R  # 创新协方差K = self.cov @ H.T @ np.linalg.inv(S)  # 卡尔曼增益# 更新状态估计self.pos += K @ residualself.cov = (np.eye(2) - K @ H) @ self.covprint(f"观测更新后位置: ({self.pos[0]:.2f}, {self.pos[1]:.2f})")print(f"不确定性减小到: {np.sqrt(np.diag(self.cov))}")def visualize(self):"""可视化SLAM过程"""plt.figure(figsize=(10, 8))# 绘制轨迹trajectory = np.array(self.trajectory)plt.plot(trajectory[:, 0], trajectory[:, 1], 'b-o', label='估计轨迹')# 绘制当前位置和不确定性椭圆from matplotlib.patches import Ellipseeigenvals, eigenvecs = np.linalg.eigh(self.cov)angle = np.degrees(np.arctan2(eigenvecs[1, 0], eigenvecs[0, 0]))width, height = 2 * np.sqrt(eigenvals)  # 2-sigma椭圆ellipse = Ellipse(self.pos, width, height, angle=angle, alpha=0.3, facecolor='red', label='位置不确定性')plt.gca().add_patch(ellipse)# 绘制路标if self.landmarks:landmarks = np.array(self.landmarks)plt.scatter(landmarks[:, 0], landmarks[:, 1], c='green', s=100, marker='^', label='路标')plt.xlabel('X坐标 (m)')plt.ylabel('Y坐标 (m)')plt.title('简单SLAM演示：位置估计与不确定性')plt.legend()plt.grid(True, alpha=0.3)plt.axis('equal')plt.show()# 演示SLAM基本概念
def demonstrate_slam_uncertainty():"""演示SLAM中的不确定性概念"""print("=== SLAM不确定性演示 ===")# 创建SLAM系统slam = SimpleSLAM2D(initial_pos=[0, 0], initial_uncertainty=0.5)# 添加一些已知路标landmarks = [[5, 3], [8, -2], [2, 6]]slam.landmarks = landmarksprint("\n1. 初始状态:")print(f"位置: {slam.pos}, 不确定性: {np.sqrt(np.diag(slam.cov))}")# 模拟机器人运动print("\n2. 机器人运动（无观测）:")slam.predict([2, 1], motion_noise=0.2)slam.predict([1.5, 0.5], motion_noise=0.2)# 观测路标进行修正print("\n3. 观测路标进行位置修正:")# 模拟观测第一个路标true_diff = np.array(landmarks[0]) - slam.postrue_distance = np.linalg.norm(true_diff)true_angle = np.arctan2(true_diff[1], true_diff[0])# 添加观测噪声obs_noise = 0.1observed_distance = true_distance + np.random.normal(0, obs_noise)observed_angle = true_angle + np.random.normal(0, obs_noise)slam.update([observed_distance, observed_angle], landmarks[0], obs_noise)# 可视化结果slam.visualize()if __name__ == "__main__":demonstrate_slam_uncertainty()

流程图

总结

本节介绍了SLAM技术的基本定义和核心挑战。SLAM解决的是机器人同时定位与建图的问题，这是一个具有递归性质的不确定性估计问题。理解SLAM中的不确定性传播和管理是掌握后续高级算法的关键基础。

1.2 SLAM问题的数学建模

内容讲解

SLAM问题的数学建模是理解和设计SLAM算法的理论基础。从概率的角度，SLAM可以被建模为一个状态估计问题，其中我们需要在给定观测数据的情况下，估计机器人的轨迹和环境地图。

SLAM的核心数学表述：

状态向量：包含机器人位姿和地图特征的联合状态
运动模型：描述机器人状态随时间的演化
观测模型：描述传感器观测与状态的关系
概率框架：使用贝叶斯推理处理不确定性

知识点

状态空间表示
- 机器人状态：位置、方向、速度等
- 地图表示：路标点、占栅格、特征描述子
- 联合状态空间的高维性
概率图模型
- 隐马尔科夫模型（HMM）
- 因子图表示
- 贝叶斯网络
贝叶斯滤波框架
- 预测步骤（时间更新）
- 校正步骤（观测更新）
- 后验概率估计
SLAM的数学公式
- 联合后验概率：P(x₁:ₜ, m | z₁:ₜ, u₁:ₜ)
- 运动模型：P(xₜ | xₜ₋₁, uₜ)
- 观测模型：P(zₜ | xₜ, m)

代码示例

"""
SLAM数学建模演示：贝叶斯滤波框架
实现简单的扩展卡尔曼滤波（EKF）SLAM
"""
import numpy as np
import matplotlib.pyplot as plt
from scipy.linalg import block_diagclass EKF_SLAM:def __init__(self, initial_pose=[0, 0, 0], process_noise=0.1, obs_noise=0.05):"""EKF-SLAM实现Args:initial_pose: 初始位姿 [x, y, theta]process_noise: 过程噪声标准差obs_noise: 观测噪声标准差"""# 状态向量 [x, y, theta, landmark1_x, landmark1_y, ...]self.state = np.array(initial_pose, dtype=float)# 状态协方差矩阵self.P = np.eye(3) * 0.1  # 初始位姿不确定性# 噪声参数self.Q = np.eye(3) * process_noise**2  # 过程噪声协方差self.R = np.eye(2) * obs_noise**2      # 观测噪声协方差# 地图管理self.landmark_count = 0self.landmark_associations = {}  # 特征ID到状态索引的映射def motion_model(self, u, dt=1.0):"""运动模型：差分驱动机器人Args:u: 控制输入 [线速度, 角速度]dt: 时间步长Returns:预测的状态变化"""v, w = utheta = self.state[2]if abs(w) < 1e-6:  # 直线运动dx = v * dt * np.cos(theta)dy = v * dt * np.sin(theta)dtheta = 0else:  # 圆弧运动R = v / w  # 转弯半径dx = R * (np.sin(theta + w*dt) - np.sin(theta))dy = R * (-np.cos(theta + w*dt) + np.cos(theta))dtheta = w * dtreturn np.array([dx, dy, dtheta])def motion_jacobian(self, u, dt=1.0):"""运动模型的雅可比矩阵Args:u: 控制输入 [线速度, 角速度]dt: 时间步长Returns:状态转移雅可比矩阵"""v, w = utheta = self.state[2]if abs(w) < 1e-6:  # 直线运动G = np.array([[1, 0, -v*dt*np.sin(theta)],[0, 1,  v*dt*np.cos(theta)],[0, 0,  1]])else:  # 圆弧运动G = np.array([[1, 0, v/w * (np.cos(theta + w*dt) - np.cos(theta))],[0, 1, v/w * (np.sin(theta + w*dt) - np.sin(theta))],[0, 0, 1]])return Gdef observation_model(self, landmark_pos):"""观测模型：距离和角度观测Args:landmark_pos: 路标位置 [x, y]Returns:期望观测值 [距离, 角度]"""robot_pos = self.state[:2]robot_theta = self.state[2]diff = landmark_pos - robot_posdistance = np.linalg.norm(diff)angle = np.arctan2(diff[1], diff[0]) - robot_theta# 角度归一化到 [-π, π]angle = np.arctan2(np.sin(angle), np.cos(angle))return np.array([distance, angle])def observation_jacobian(self, landmark_pos):"""观测模型的雅可比矩阵Args:landmark_pos: 路标位置 [x, y]Returns:观测雅可比矩阵"""robot_pos = self.state[:2]robot_theta = self.state[2]diff = landmark_pos - robot_posdistance = np.linalg.norm(diff)dx, dy = diff# 对机器人状态的雅可比H_robot = np.array([[-dx/distance, -dy/distance, 0],[dy/distance**2, -dx/distance**2, -1]])# 对路标位置的雅可比H_landmark = np.array([[dx/distance, dy/distance],[-dy/distance**2, dx/distance**2]])return H_robot, H_landmarkdef predict(self, u, dt=1.0):"""EKF预测步骤Args:u: 控制输入 [线速度, 角速度]dt: 时间步长"""# 状态预测motion = self.motion_model(u, dt)self.state[:3] += motion# 协方差预测G = self.motion_jacobian(u, dt)# 扩展雅可比矩阵到完整状态空间n_states = len(self.state)G_full = np.eye(n_states)G_full[:3, :3] = G# 扩展过程噪声协方差Q_full = np.zeros((n_states, n_states))Q_full[:3, :3] = self.Q# 协方差更新self.P = G_full @ self.P @ G_full.T + Q_fulldef update(self, observation, landmark_id):"""EKF更新步骤Args:observation: 观测值 [距离, 角度]landmark_id: 路标ID"""# 检查是否为新路标if landmark_id not in self.landmark_associations:self.add_landmark(observation, landmark_id)return# 获取路标在状态向量中的索引landmark_idx = self.landmark_associations[landmark_id]landmark_pos = self.state[landmark_idx:landmark_idx+2]# 计算期望观测值expected_obs = self.observation_model(landmark_pos)# 观测残差residual = observation - expected_obsresidual[1] = np.arctan2(np.sin(residual[1]), np.cos(residual[1]))  # 角度归一化# 计算雅可比矩阵H_robot, H_landmark = self.observation_jacobian(landmark_pos)# 构造完整的观测雅可比矩阵n_states = len(self.state)H = np.zeros((2, n_states))H[:, :3] = H_robotH[:, landmark_idx:landmark_idx+2] = H_landmark# 卡尔曼增益S = H @ self.P @ H.T + self.R  # 创新协方差K = self.P @ H.T @ np.linalg.inv(S)  # 卡尔曼增益# 状态更新self.state += K @ residual# 协方差更新（Joseph形式，数值稳定）I = np.eye(n_states)self.P = (I - K @ H) @ self.P @ (I - K @ H).T + K @ self.R @ K.Tdef add_landmark(self, observation, landmark_id):"""添加新路标到地图Args:observation: 观测值 [距离, 角度]landmark_id: 路标ID"""distance, angle = observationrobot_pos = self.state[:2]robot_theta = self.state[2]# 计算路标的全局位置global_angle = robot_theta + anglelandmark_pos = robot_pos + distance * np.array([np.cos(global_angle), np.sin(global_angle)])# 扩展状态向量self.state = np.concatenate([self.state, landmark_pos])# 扩展协方差矩阵n_old = len(self.P)P_new = np.zeros((n_old + 2, n_old + 2))P_new[:n_old, :n_old] = self.P# 新路标的初始不确定性landmark_cov = np.eye(2) * 1.0  # 较大的初始不确定性P_new[n_old:, n_old:] = landmark_covself.P = P_new# 记录路标关联self.landmark_associations[landmark_id] = n_oldself.landmark_count += 1print(f"添加新路标 {landmark_id} 到位置 ({landmark_pos[0]:.2f}, {landmark_pos[1]:.2f})")# 演示EKF-SLAM算法
def demonstrate_ekf_slam():"""演示EKF-SLAM的数学建模"""print("=== EKF-SLAM数学建模演示 ===")# 创建EKF-SLAM系统slam = EKF_SLAM(initial_pose=[0, 0, 0])# 模拟机器人运动和观测trajectory = []# 真实路标位置true_landmarks = {1: [3, 2],2: [5, -1],3: [1, 4]}print("\n模拟SLAM过程:")for step in range(10):print(f"\n--- 时间步 {step+1} ---")# 机器人运动if step < 5:control = [1.0, 0.2]  # 向前运动并转弯else:control = [0.8, -0.1]  # 改变运动方向slam.predict(control)trajectory.append(slam.state[:3].copy())print(f"预测后位姿: ({slam.state[0]:.2f}, {slam.state[1]:.2f}, {slam.state[2]:.2f})")# 模拟观测（有概率观测到路标）for landmark_id, true_pos in true_landmarks.items():# 计算到路标的距离robot_pos = slam.state[:2]distance_to_landmark = np.linalg.norm(np.array(true_pos) - robot_pos)# 如果距离足够近，则观测到该路标if distance_to_landmark < 4.0 and np.random.random() < 0.7:# 生成带噪声的观测true_obs = slam.observation_model(true_pos)noisy_obs = true_obs + np.random.multivariate_normal([0, 0], slam.R)slam.update(noisy_obs, landmark_id)print(f"观测到路标 {landmark_id}: 距离={noisy_obs[0]:.2f}, 角度={noisy_obs[1]:.2f}")# 可视化结果visualize_ekf_slam(slam, trajectory, true_landmarks)def visualize_ekf_slam(slam, trajectory, true_landmarks):"""可视化EKF-SLAM结果"""plt.figure(figsize=(12, 8))# 绘制真实轨迹和估计轨迹trajectory = np.array(trajectory)plt.plot(trajectory[:, 0], trajectory[:, 1], 'b-o', label='估计轨迹', markersize=4)# 绘制真实路标true_pos = np.array(list(true_landmarks.values()))plt.scatter(true_pos[:, 0], true_pos[:, 1], c='red', s=100, marker='^', label='真实路标')# 绘制估计路标if slam.landmark_count > 0:estimated_landmarks = []for landmark_id, idx in slam.landmark_associations.items():landmark_pos = slam.state[idx:idx+2]estimated_landmarks.append(landmark_pos)# 绘制路标不确定性椭圆landmark_cov = slam.P[idx:idx+2, idx:idx+2]eigenvals, eigenvecs = np.linalg.eigh(landmark_cov)angle = np.degrees(np.arctan2(eigenvecs[1, 0], eigenvecs[0, 0]))width, height = 2 * np.sqrt(eigenvals)from matplotlib.patches import Ellipseellipse = Ellipse(landmark_pos, width, height, angle=angle,alpha=0.3, facecolor='blue')plt.gca().add_patch(ellipse)estimated_landmarks = np.array(estimated_landmarks)plt.scatter(estimated_landmarks[:, 0], estimated_landmarks[:, 1],c='blue', s=80, marker='s', label='估计路标')# 绘制当前机器人位置和不确定性robot_pos = slam.state[:2]robot_cov = slam.P[:2, :2]eigenvals, eigenvecs = np.linalg.eigh(robot_cov)angle = np.degrees(np.arctan2(eigenvecs[1, 0], eigenvecs[0, 0]))width, height = 2 * np.sqrt(eigenvals)from matplotlib.patches import Ellipseellipse = Ellipse(robot_pos, width, height, angle=angle,alpha=0.5, facecolor='green', label='机器人位置不确定性')plt.gca().add_patch(ellipse)plt.xlabel('X坐标 (m)')plt.ylabel('Y坐标 (m)')plt.title('EKF-SLAM演示：数学建模与实现')plt.legend()plt.grid(True, alpha=0.3)plt.axis('equal')plt.show()if __name__ == "__main__":demonstrate_ekf_slam()

总结

本节从数学角度建模了SLAM问题，介绍了概率图模型和贝叶斯滤波框架。EKF-SLAM作为经典的解决方案，展示了如何在状态估计框架下处理同时定位与建图问题。理解这些数学基础对于掌握现代SLAM算法至关重要。

1.3 SLAM系统架构设计

内容讲解

SLAM系统的架构设计是将理论算法转化为实际可部署系统的关键环节。一个完整的SLAM系统通常包含多个功能模块，需要考虑模块间的接口设计、数据流管理、实时性要求以及系统的可扩展性和可维护性。

现代SLAM系统通常采用分层架构：

感知层：传感器数据采集与预处理
算法层：核心SLAM算法实现
决策层：路径规划与导航控制
应用层：用户接口与任务管理

知识点

SLAM系统基本组成
- 前端（Front-end）：数据关联与短期状态估计
- 后端（Back-end）：长期优化与一致性维护
- 回环检测：全局一致性保证
- 地图管理：地图存储与查询
前端与后端的划分
- 前端职责：特征提取、数据关联、帧间位姿估计
- 后端职责：全局优化、回环闭合、地图维护
- 接口设计：关键帧、观测约束、图结构
数据流与控制流
- 传感器数据流：原始数据→预处理→特征提取
- 估计数据流：观测→状态更新→结果输出
- 控制流：任务调度、异常处理、资源管理
性能评估指标
- 精度指标：绝对轨迹误差（ATE）、相对位姿误差（RPE）
- 实时性指标：处理延迟、帧率
- 鲁棒性指标：失败率、恢复能力

代码示例

"""
SLAM系统架构设计演示
实现一个模块化的SLAM系统框架
"""
import threading
import queue
import time
import numpy as np
from abc import ABC, abstractmethod
from dataclasses import dataclass
from typing import List, Dict, Optional, Tuple
import json@dataclass
class SensorData:"""传感器数据的统一格式"""timestamp: floatsensor_type: str  # 'camera', 'lidar', 'imu'data: np.ndarraymetadata: Dict = None@dataclass
class Observation:"""观测数据结构"""timestamp: floatfeatures: np.ndarraydescriptors: np.ndarraypose_estimate: np.ndarrayconfidence: float@dataclass
class KeyFrame:"""关键帧数据结构"""id: inttimestamp: floatpose: np.ndarrayobservations: List[Observation]connectivity: List[int]  # 与其他关键帧的连接class SLAMModule(ABC):"""SLAM模块的抽象基类"""def __init__(self, name: str):self.name = nameself.is_running = Falseself.input_queue = queue.Queue()self.output_queue = queue.Queue()self.thread = Noneself.performance_stats = {'processed_frames': 0,'processing_time': [],'memory_usage': []}@abstractmethoddef process(self, data):"""处理输入数据的抽象方法"""passdef start(self):"""启动模块"""self.is_running = Trueself.thread = threading.Thread(target=self._run)self.thread.start()print(f"{self.name} 模块已启动")def stop(self):"""停止模块"""self.is_running = Falseif self.thread:self.thread.join()print(f"{self.name} 模块已停止")def _run(self):"""模块运行主循环"""while self.is_running:try:# 获取输入数据（带超时）data = self.input_queue.get(timeout=0.1)# 记录处理开始时间start_time = time.time()# 处理数据result = self.process(data)# 记录性能统计processing_time = time.time() - start_timeself.performance_stats['processed_frames'] += 1self.performance_stats['processing_time'].append(processing_time)# 输出结果if result is not None:self.output_queue.put(result)except queue.Empty:continueexcept Exception as e:print(f"{self.name} 模块处理错误: {e}")def send_data(self, data):"""向模块发送数据"""self.input_queue.put(data)def get_result(self, timeout=None):"""获取处理结果"""try:return self.output_queue.get(timeout=timeout)except queue.Empty:return Nonedef get_performance_stats(self):"""获取性能统计"""stats = self.performance_stats.copy()if stats['processing_time']:stats['avg_processing_time'] = np.mean(stats['processing_time'])stats['max_processing_time'] = np.max(stats['processing_time'])return statsclass SensorModule(SLAMModule):"""传感器数据采集模块"""def __init__(self, sensor_type: str):super().__init__(f"Sensor_{sensor_type}")self.sensor_type = sensor_typeself.frame_id = 0def process(self, data):"""模拟传感器数据采集"""# 模拟数据预处理time.sleep(0.01)  # 模拟处理时间self.frame_id += 1sensor_data = SensorData(timestamp=time.time(),sensor_type=self.sensor_type,data=np.random.randn(100, 3),  # 模拟传感器数据metadata={'frame_id': self.frame_id})return sensor_dataclass FrontEnd(SLAMModule):"""SLAM前端模块"""def __init__(self):super().__init__("Frontend")self.last_pose = np.eye(4)self.keyframes = []self.feature_tracker = FeatureTracker()def process(self, sensor_data: SensorData):"""前端处理：特征提取和跟踪"""# 特征提取features, descriptors = self.feature_tracker.extract_features(sensor_data.data)# 位姿估计（简化版）if len(self.keyframes) > 0:# 使用特征匹配估计相对位姿relative_pose = self.estimate_relative_pose(features, descriptors)current_pose = self.last_pose @ relative_poseelse:current_pose = np.eye(4)# 判断是否为关键帧if self.is_keyframe(current_pose):keyframe = KeyFrame(id=len(self.keyframes),timestamp=sensor_data.timestamp,pose=current_pose,observations=[Observation(timestamp=sensor_data.timestamp,features=features,descriptors=descriptors,pose_estimate=current_pose,confidence=0.8)],connectivity=[])self.keyframes.append(keyframe)self.last_pose = current_posereturn keyframereturn Nonedef estimate_relative_pose(self, features, descriptors):"""估计相对位姿（简化实现）"""# 这里是简化的实现，实际中需要特征匹配和几何验证# 模拟小的随机运动translation = np.random.normal(0, 0.1, 3)rotation_angle = np.random.normal(0, 0.05)T = np.eye(4)T[:3, 3] = translation# 简化的旋转矩阵（绕Z轴）T[:2, :2] = [[np.cos(rotation_angle), -np.sin(rotation_angle)],[np.sin(rotation_angle), np.cos(rotation_angle)]]return Tdef is_keyframe(self, current_pose):"""判断是否为关键帧"""if len(self.keyframes) == 0:return True# 简单的关键帧选择策略：基于距离last_pose = self.keyframes[-1].posetranslation_dist = np.linalg.norm(current_pose[:3, 3] - last_pose[:3, 3])return translation_dist > 0.5  # 距离阈值class BackEnd(SLAMModule):"""SLAM后端模块"""def __init__(self):super().__init__("Backend")self.pose_graph = {}self.optimization_window = 10def process(self, keyframe: KeyFrame):"""后端处理：图优化"""self.pose_graph[keyframe.id] = keyframe# 执行局部优化if len(self.pose_graph) >= self.optimization_window:self.local_optimization()# 检测回环loop_closure = self.detect_loop_closure(keyframe)if loop_closure:self.global_optimization()return keyframedef local_optimization(self):"""局部优化（简化实现）"""print("执行局部图优化...")# 这里应该实现实际的图优化算法time.sleep(0.05)  # 模拟优化时间def detect_loop_closure(self, keyframe: KeyFrame):"""回环检测（简化实现）"""# 简化的回环检测：检查与历史关键帧的距离for kf_id, kf in self.pose_graph.items():if kf_id < keyframe.id - 5:  # 避免检测相邻帧distance = np.linalg.norm(keyframe.pose[:3, 3] - kf.pose[:3, 3])if distance < 1.0:  # 距离阈值print(f"检测到回环：关键帧 {keyframe.id} 与关键帧 {kf_id}")return Truereturn Falsedef global_optimization(self):"""全局优化（简化实现）"""print("执行全局图优化...")time.sleep(0.1)  # 模拟优化时间class FeatureTracker:"""特征跟踪器"""def extract_features(self, data):"""提取特征点和描述子"""# 模拟特征提取num_features = np.random.randint(50, 200)features = np.random.rand(num_features, 2) * 640  # 图像坐标descriptors = np.random.randn(num_features, 128)  # 特征描述子return features, descriptorsclass SLAMSystem:"""完整的SLAM系统"""def __init__(self):# 创建各个模块self.sensor_module = SensorModule("camera")self.frontend = FrontEnd()self.backend = BackEnd()# 数据连接self.connections = [(self.sensor_module, self.frontend),(self.frontend, self.backend)]# 系统状态self.is_running = Falseself.system_thread = Nonedef start(self):"""启动SLAM系统"""print("启动SLAM系统...")# 启动各个模块for module in [self.sensor_module, self.frontend, self.backend]:module.start()# 启动数据传输线程self.is_running = Trueself.system_thread = threading.Thread(target=self._data_flow)self.system_thread.start()print("SLAM系统已启动")def stop(self):"""停止SLAM系统"""print("停止SLAM系统...")self.is_running = Falseif self.system_thread:self.system_thread.join()# 停止各个模块for module in [self.sensor_module, self.frontend, self.backend]:module.stop()print("SLAM系统已停止")def _data_flow(self):"""管理模块间的数据流"""while self.is_running:# 传输数据for source, target in self.connections:result = source.get_result(timeout=0.01)if result is not None:target.send_data(result)time.sleep(0.001)  # 避免CPU占用过高def simulate_operation(self, duration=10):"""模拟SLAM系统运行"""print(f"模拟SLAM系统运行 {duration} 秒...")start_time = time.time()frame_count = 0while time.time() - start_time < duration:# 模拟传感器数据输入self.sensor_module.send_data("sensor_trigger")frame_count += 1time.sleep(0.1)  # 10Hz 采样率print(f"模拟完成，处理了 {frame_count} 帧数据")def get_system_stats(self):"""获取系统统计信息"""stats = {}for module_name, module in [("sensor", self.sensor_module),("frontend", self.frontend),("backend", self.backend)]:stats[module_name] = module.get_performance_stats()return stats# 演示SLAM系统架构
def demonstrate_slam_architecture():"""演示SLAM系统架构设计"""print("=== SLAM系统架构演示 ===")# 创建SLAM系统slam_system = SLAMSystem()try:# 启动系统slam_system.start()# 模拟运行slam_system.simulate_operation(duration=5)# 获取性能统计stats = slam_system.get_system_stats()print("\n=== 系统性能统计 ===")for module_name, module_stats in stats.items():print(f"\n{module_name.upper()} 模块:")print(f"  处理帧数: {module_stats['processed_frames']}")if 'avg_processing_time' in module_stats:print(f"  平均处理时间: {module_stats['avg_processing_time']:.4f}s")print(f"  最大处理时间: {module_stats['max_processing_time']:.4f}s")finally:# 停止系统slam_system.stop()if __name__ == "__main__":demonstrate_slam_architecture()