I. 摘要
同步定位与绘图（SLAM）对自主导航任务至关重要，极大地推动了机器人技术的发展[1]。激光雷达-SLAM 系统因其提供高分辨率地图的能力而日益受到关注[2-4]。 激光雷达和摄像头具有互补性，整合激光雷达 SLAM 和视觉 SLAM 的优势是一个值得研究的课题。Shao 等人[5]提出的立体视觉-惯性-激光雷达（VIL）SLAM 在这些劣化情况下表现更好。基于视觉的方法在纹理丰富的环境中表现良好，但对光线变化和初始化非常敏感[6]。同时，多个传感器的数据融合和一致性可能会给闭环检测带来挑战。为了满足工业应用的需求并确保在仓库中运行的自动导引车（AGV）的安全，必须在恶劣环境中持续提供精确的定位值。路径规划对于机器人至关重要，良好的路径规划可以使AGV选择最优路径，避免拥堵和重复，适应不同环境和任务需求，提高了AGV的适应性和通用性。同时路径规划算法可以优化AGV的能源消耗，延长电池续航时间，降低能源成本，从而提高运输效率和安全性。
II. 实验方法
我们的框架由两个子系统组成：视觉系统和激光雷达-惯性系统（图 1）。视觉系统旨在检测标记特征，提供精确定位。它与激光雷达-惯性系统协作，以提高整个系统在不同环境下的鲁棒性和可靠性。在特征有限的情况下，定位估计基于 Aruco Marker，定位数据与通过节点姿态内插的相对姿态之间的差值被用作残差项，并与后端的激光雷达残差共同优化。
图 1. 激光雷达-视觉-惯性系统架构。
在优化过程中，我们使用检测到的第一幅图像的相机坐标系作为参考原点。然后，我们识别所选图像中的所有标记点，利用预定义的标记点尺寸和检测到的标记点像素计算世界坐标系中的初始姿态，在完成新添加摄像机的定位后，我们需要遍历所有检测到的标记。这些检测到的标记可分为共同观测到的标记和未共同观测到的标记。对于未共同观测到的标记，我们可以使用 PnP 算法和标记大小将其更新到地图中。对于所有观测到的标记，我们需要在因子图中添加观测约束。具体算法如 Alg.1 所示。
在后端优化中涉及视觉定位信息和激光雷达里程测量。标记可作为网格图的约束条件，减少因累积误差导致的闭环失败。
我们利用激光雷达里程计来确定世界坐标系中两个节点之间的相对位置变换。这两个节点的相对位置变换（相机姿态变换）又可从标记处理结果中获得。数据不同步问题可以通过插值法来解决。
图 2. (a)系统接收来自单目摄像头、激光雷达和 IMU 的输入 (b)节点之间的相对姿态变换。
利用视觉里程计定位与通过激光雷达节点姿态插值得到的相对姿态之间的差值作为残差项，求解这两个相对姿态变换之间差值的最小二乘法问题。将视觉标记定位数据融合到激光雷达系统中，共同优化视觉残差和激光雷达残差，以形成一致的环境表示，并在后端与其他残差同时进行优化。
Cartographer只能生成部分地图，因为没有足够的特征进行匹配。本文方法可明显改善地图漂移问题，实现全面地图构建。
图3 Cartographer 建图效果
图 4本文方法 建图效果
路径跟踪算法在AGV底盘控制中起着至关重要的作用。路径跟踪是指底盘在行驶过程中，控制无人车沿着预先设定好的路径行驶。路径跟踪模块负责根据机器人当前的位姿、速度及路径点信息计算出下一时刻机器人的控制参数（速度、角速度及刹车控制），使机器人尽可能的沿着规划的路径平稳运动。常见的路径跟踪算法包括LQR（Linear Quadratic Regulator）、PID（Proportional-Integral-Derivative）、Pure Pursuit等。路径跟踪功能主要依靠横向运动控制算法实现，然而使用单一算法实现横向控制可能会面对参数设置敏感、性能不稳定、超调或者震荡现象，导致底盘控制系统的控制精度和安全性降低。因此需要针对使用场景选择合适的路径规划及路径跟踪算法，综合多种控制系统设计，提高整底盘控制系统的精度和安全性，满足机器人自动行驶过程中的整体性能要求。
路径跟踪是实现自主移动和任务执行的重要环节，底盘的横向控制是路径跟踪的关键环节，其目标是确保AGV能够准确、稳定地沿着规划路径行驶，尽可能减少与参考路径间的空间误差。本文针对无人仓库的通道特点和机动性使用需求，基于四驱四转移动底盘进行开发。常见差速结构底盘在实际运行中由于路面摩擦力的问题，可能会出现位置漂移等情况；在驶入死胡同通道时，也会面临调头方式复杂的问题，影响作业效率。四驱四转移动底盘每个驱动轮都采用多连杆独立悬挂，能够实现原地转向和小转弯半径转向，拥有多种运动模式和较高的灵活性。
横向控制是机器人运动中重要的控制方式，是AGV或机器人垂直于其运动方向的控制，通常涉及控制AGV的转向以及控制在道路上的位置。横向运动控制算法的综合设计能够提高底盘的整体控制性能和安全性。仓库巡检机器人的底盘横向控制模块在直线行驶时采用Pure Pursuit控制算法，在转向行驶时使用PID控制算法结合原地转向功能。
图5 自行车模型
Pure Pursuit（纯追踪）是一种基于几何的路径追踪算法，基于几何的控制方法不需要考虑底盘的运动学模型，控制时使用的参数少，能够较好的运用到实践使用中。Pure Pursuit算法的基本思想是把转向结构的机器人抽象成自行车模型，构建前轮转角和后轴曲率的约束关系，然后将车的后轴视为切点, 将纵向的车身作为切线，控制AGV保持后轴的中心经过轨迹上一系列的点，使其朝着离它当前位置最近的路径点前进。
PID控制算法(比例积分微分控制算法)，是控制理论中应用最为广泛的控制算法。PID分为三个组成部分，比例调节、积分调节和微分调节。其具体实现方式如公式所示，其中是控制量(角速度)，是横向追踪误差，横向跟踪误差(cross track error)为前轴中心点到最近路径点的距离。
仓库定点巡检功能是一套完整的机器人控制算法，也是无人仓库巡检机器人的主要任务，其由感知模块、控制模块、通讯模块、执行模块四部分组成。
图6 系统流程
III. 实验结果
Glassroom 是由玻璃隔断的房间内录制的一系列Rosbag组成的数据集，实验室场景的范围约为21米×13米。本实验将传感器放置在四足机器人的后平台上收集数据，该平台距离地面0.85米。当激光雷达数据作为主要外部输入时，玻璃物体无法正确对齐。这是由于入射光穿过玻璃表面或在玻璃表面反射，导致玻璃上的距离测量不准确，从而影响定位和地图构建性能。本实验在Glassroom中使用四足机器人测试所提出的系统，四足机器人运动中会产生抖动与颠簸，运动会导致雷达传感器的高度、姿态等信息发生突变，可以在模拟巡检过程中出现振动的干扰情况，测试定位系统的抗震能力及鲁棒性。以上问题增加SLAM算法的复杂性，对定位和地图构建的准确性提出了挑战。
图7 玻璃房数据集.
利用放置在玻璃附近的ArUco标记信息进行定位。同步后的视觉定位结果将在扫描和地图绘制过程中传入后端。系统完成制图并处理所有传感器数据后将终止当前轨迹并对当前优化图中的子地图、扫描位姿以及子地图坐标中的扫描匹配位姿进行最终优化。最后，使用 Protobuf将优化后的地图信息序列化并保存到文件中。
图8 将ArUco标记定位结果作为路标可视化，将EVO输出轨迹与真值对比
具体实验流程如下步骤所示：
系统初始化：启动仓库巡检机器人，设置需要巡检的目标货架或SKU(库存管理单位)，选择地图、设备速度等参数。启动雷达、IMU和相机传感器。选择机器人的初始位置，并将其放置在该位置上在出发点等待任务下达。
下达巡检任务：通过本机远程连接巡检机器人上的工控机，设置巡检需要经过的目标点任务列表，将一系列目标输入规划路径程序，启动控制运动的主线程。
生成路径：基于仓库巡检机器人导航自动化控制技术和基于改进的DWA算法的仓库巡检机器人避障技术进行处理生成路径，并转换为控制指令启动自主导航和自主避障。
底盘自动化控制：基于上述方法获取控制信息，将其输入到控制硬件环境中，自动执行规划好的路径，动态调整行驶线速度、角速度参数，以实现自动巡检以及避障。
将目标巡检点与实际到达位置进行对比，选取到达目标点后离目标点距离最小的点作为准到达点，并选取准到达点前后各10个点作为近似到达点，求取空间距离，偏差情况如表所示。平均偏差计算公式如下：
实验结果表明，在实际巡检过程中受到光照条件、IMU累计误差、地面起伏、底盘电机控制误差以及多种机械误差等影响因素下，连续执行巡检作业时长三小时，行进距离约2000米，行进中到达目标点的平均偏差偏差在0.1米以内，且距离准确到达点偏差小于30%的数据点占比超过80%。这表明巡检过程能够准确地到达目标点，并且具备良好的稳定性。
表1 实际准到达点与目标点偏差
IV. 结论与总结
为了满足机器人在不同场景中的需求，我们提出了 一种激光雷达-视觉-惯性 SLAM 系统，通过平滑和映射在复杂环境中进行实时状态估计和映射，旨在克服大规模环境中因累积误差导致的闭环和匹配失败等问题。本文介绍了为实现全自动仓库巡检机器人的自主导航任务所需的关键技术。除了通过SLAM进行定位外，还需要进行路径规划和路径跟踪，以确保机器人沿着计划的路径安全行进。路径规划分为两个阶段：全局路径规划和局部路径规划，本文节应用A*算法完成了全局路径规划阶段。路径跟踪算法负责控制底盘沿着规划好的路径点移动，本文节采取不同的横向运动控制算法（PID和Pure Pursuit）综合设计实现路径跟踪，能够提高机器人的整体控制性能和安全性。测试结果表明本文方法与现有方法相比具有优越性及更好的稳定性，能够适应不同类型仓库环境，实现高精度自主巡检。部分成果已在仓库中得到常态化应用，未来将进一步服务于智慧仓库中设备的自动化与智能化建设。
参考文献
[1] Yao, C.; Shi, W.; Liu, C.; Chen, H.; Chen, Q. Overview of mobile robot navigation technology.Sci. Sin. Inf. 2023,53, 2303–2324.
[2] Fernandes D, Afonso T, Girão P, et al. Real-Time 3D Object Detection and SLAM Fusion in a Low-Cost LiDAR Test Vehicle 360 Setup[J]. Sensors, 2021, 21(24): 8381. 361
[3] Zheng, L.; Fu, Z. BALM: Bundle Adjustment for Lidar Mapping. In Proceedings of the International Conference on Robotics and 362 Automation, Xian, China, 30 May–5 June 2021; Volume 6, pp. 3184–3191. 363
[4] Ilmir, Z.I.; Ilya, M.A. Comparison of ROS-based Visual SLAM methods in homogeneous indoor environment. In Proceedings of 364the IEEE Workshop on Positioning, Navigation and Communications (WPNC),
[5] Mapping. IEEE Robot. Autom. Lett. 2021, 6, 7469–7476.
[6] Weizhao Shao; Srinivasan Vijayarangan; Cong Li; George Kantor; "Stereo Visual Inertial LiDAR Simultaneous Localization And 409
[7] Mapping", ARXIV-CS.RO, 2019. (IF: 3)
