自主导航流程图

控制指令生成模块

接收控制指令，驱动各关节电机或角速度、线速度仪执行具体的移动或操作动作，如左转、右转、左平移、右平移、减速、停车等

又称为视觉里程计，主要做相邻图像间相机的运动，以及局部地图的样子，为了解决定位的问题1 图像获取与预处理：通过摄像头获取连续的视频帧，对图像进行畸变校正、灰度化等预处理2 特征提取与匹配：从图像中提取关键特征点（如角点、边缘等），并在连续帧或多视角间进行特征匹配，以获取相对运动信息。3 运动估计（姿态估计）：利用匹配的特征点对，估计摄像头的相对位姿变化，常用方法包括PnP算法、EPnP等。

其核心是统一处理相机、IMU、激光雷达等异构传感器的原始数据，同步时序并结合多源信息完成误差矫正与初步优化，解决数据异构、时序不同步及单源误差大的问题，为 SLAM 和视觉理解的后续的融合感知与位姿估计提供精准输入。1 时刻统一：先通过传感器时间戳校准消除各设备数据采集的延迟差，确保同一时刻的感知数据对应同一环境状态；2 清洗滤波：再对各传感器数据做针对性清洗，包括相机图像去畸变、IMU 数据去零偏噪声、激光点云降采样滤波；3 格式转换：最后将预处理后的异构数据（如图像矩阵、IMU 六轴量、点云坐标）转换为 SLAM 系统兼容的统一格式，以此支撑后续的特征关联与融合计算。

VSLAM(Visual Simultaneous Localization and Mapping)实时定位和地图构建

判断机器人是否达到了之前到过的位置，如果检测到了回环，则信息提供给后端。场景识别：通过图像描述子（如Bag of Words、深度学习特征等）识别当前帧是否与之前的某一帧对应同一场景。全局优化：一旦检测到闭环，通过全局图优化（如位姿图优化）修正累计的定位误差，提升系统的鲁棒性和精度。

根据估计的轨迹，建立与任务要求对应的地图，更前言的是，将语义分割结果融入 SLAM，构建带语义标签的地图代表工作：SemanticFusion、MaskFusion、Kimera-Semantics

路径规划模块

根据路径规划结果和机器人的当前位置，生成相应的运动控制指令

多传感器融合

前端Visual Odometry

后端(非线性)优化 Back-End

提取视觉传感器的信息

图像预处理：对原始图像进行降噪、增强等操作。特征提取：从图像中提取有用的特征点。语义分割：识别并分类图像中的不同对象，如行人、车辆、道路标志等

视觉理解模块

AGV执行单元

建图

回环检测Loop Closure Detection

基于当前的位置信息和目标位置，计算出到达目标的最佳路径常用算法：A*、Dijkstra、RRT* / PRM*、Jump Point Search (JPS)

后端接收不同时刻视觉里程计的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。1 地图构建：根据前端提供的位姿信息与特征点，构建环境的三维地图2 图优化：将位姿和地图点作为图的节点，通过优化算法（如最小二乘）最小化重投影误差，提高整体系统的精度和一致性。3 关键帧选择与管理：为了提高计算效率，后端会选择关键帧进行优化，管理地图点的添加、删除