基于SLAM 的物流机器人运动控制及路径规划算法研究

孙 涛， 王朝辉， 周佳杰

（广东工贸职业技术学院， 广东 广州 510510）

引言

物流机器人自美国于20世纪50年代被成功研发至今，历经地面控制器技术、无线式导引技术、计算机控制装配技术的发展嬗变，技术呈现标准化、系统化和专业化的趋势[1]。随着物流工业的转型升级，传统的人工搬运、物流管理模式被淘汰，以物流机器人为核心的自动化模式被广泛运用于工厂、生产企业，物流搬运、分拣、码垛等的智能化作业，已成为行业发展的大势所趋[2]。而运动控制、路径规划是物流机器人的核心功能之一，也是衡量物流机器人性能的重要指标，直接关系到其工作效率、自主避障性能[3]。现阶段遗传算法、神经网络算法、模糊控制算法等在物流机器人运动控制及路径规划中占据着主导地位，但该类技术的应用尚需完备的软硬件支持，且操作繁琐、技术复杂，在复杂多变的动态化环境中适用性不强。与之相比，SLAM操作简单、易于实现、定位精准、误差小的特点，受到了物流机器人领域研究者的关注，也对其运动控制及路径规划带来了新的解决方案[4]。

1 SLAM 算法的基本原理

SLAM技术主要描述的是，在未知环境的未知地点环境下，机器人在运动中通过环境特征观测，掌握自身的姿态、位置定位，在机器人特定位置的基础上建立周边环境的增量式地图，进而进行地图构建和定位的目的[5-6]。总体而言，SLAM用来更新位置主要是依托环境来进行的，它摒弃了自身的测距机制，通过环境的激光扫描、可靠的环境特征提取纠正位置、机器人状态，该技术的发展和路径规划、跟踪、定位技术一脉相承，在视觉增强、地图建模领域有着重大的研究价值[7]。伴随着SLAM算法的与时俱进，SLAM所运用的传感器、软件算法、硬件也在无时无刻的更新和进步，SLAM系统在30 多年的研发、应用与实践中，建立了成熟的技术理论。SLAM的运行步骤和方式为：在利用传感器模块读取数据后，借助视觉里程计进行两个时刻相对运动的估测分析，对于估测中存在的误差由后端予以处理。基于前端、后端检测到的运动轨迹信息，构建三维地图，之后参照不同时刻在同一场景的信息，采用空间约束的原理开展累积误差的消除。SLAM算法框架如图1 所示。

2 SLAM算法的特征

经典的SLAM算法由传感器信息、视觉里程计、后端、地图、检测五个环节组成。其中，传感器信息主要采集视频图像、激光扫描数据、点云信息等，它们是环境中发生的真实数据和原始资料[8]。视觉里程计用于直接配准算法设计、特征匹配设计，可估算不同时刻目标观测对象的相对位置。后端通常采用图优化等算法，消除视觉里程计运行中产生的累积误差。地图模块设计主要用来进行三维地图的构建，积累机器人运动规划的环境条件。检测环节，用来消除空间累计造成的误差。

3 基于SLAM算法的运动控制和路径规划解决方案

物流机器人SLAM 问题可归结为环境表述、感知信息匹配与数据关联、不确定信息处理、基于环境地图的机器人定位、机器人在感知范围内的覆盖及探索等[9-10]。为此，在SLAM视野下，其提供了多样化的机器人定位与路径规划解决方案，主要包含EKF-SLAM算法、EM-SLAM算法、FastSLAM算法三种，它们从不同的理论出发，解释了SLAM问题的处理对策。

3.1 EKF-SLAM算法

EKF-SLAM即扩展卡尔曼滤波算法，该算法的特征为：在非线性系统中，运用标准Kal-man 滤波衍生算法将之转化为线性系统，展开过程借助泰勒级数分析，将高于二阶的项数全部取出，再行滤波操作。算法在非高斯噪声模型中的适用性不强，在应用中还存在状态估计很难保持一致性、计算过程复杂的问题，也给EKF-SLAM带来了较大的优化空间，EKF-SLAM算法的基本公式为[11]：

在上述表达式中，Xk代表在k-1 时刻下对下一时刻的估测值，而Y（k）表示得到的观测向量。Fk被称为状态转移矩阵，W 是噪音，V 是速度，hk为测量矩阵。

3.2 EM-SLAM算法

EM-SLAM算法考虑到了环境特征、观测数据的性质，这些信息间的数据关联准确性高，通过构建地图问题向观测模型和运动模型概率约束环境下的最大相似度估测问题转化，利用EM模型加强三维地图的定位、创建性能，改善算法的收敛性[12]。EM-SLAM算法有着两个基本步骤，一是对机器人后验概率密度函数估计，利用先验环境特征地图和贝叶斯估计算法，开展正反两次后验，设估计的时刻为t，时间区间为（1，2，3…，t）中的某一个时刻，则最优估计的时刻t 算法公式为：

式中：P（s，slm[i]，d0，……dx）代表正向估测数据，代表在t 时刻前的条件下估测值，相对地，反向估测数据用P（s，lm[i]，dx+1，…，dxt）表示，体现了t 时刻后条件下的估测值，d 代表观测量、系统控制量的合集[13]。

利用机器人后验概率密度函数的最佳估测值，对最大似然估计进行测算，获得最新的最大似然地图，从而进行数据的优化分析。

3.3 FastLAM算法

FastLAM 算法是EKF-SLAM 算法、粒子滤波算法结合的产物，它在运算过程中用定位、图形构建两者来考虑，即采用粒子滤波算法解决定位问题，采用EKF 算法处理图形构建问题，该算法的复杂度为0，而FastSLAM算法中的环境特征个数、采用的粒子个数分别用M、N 表示[14]。

4 基于SLAM 的物流机器人运动控制及路径规划算法实现

以SLAM算法为核心，基于现有的SLAM技术原理、解决方案，文章在汲取已有理论的基础上，将物流机器人运动控制及路径规划过程划分为环境描述、传感数据、定位分析和模型建模四个部分，以此实现物流机器人的精准定位、运动轨迹监测和智能化的行走路径分析[15]。

4.1 环境地图构建

采用由Moravec 等提出的栅格地图法，构建物流机器人工作周边的环境地图，即合理界定机器人的运动空间，把空间结合操作需求设计为不同的单元格，单元格为固定大小，之后进行栅格概率的初始化。经初始化的各个单元格，它们之间占用的概率相同，假设将环境划分为m×n 等分，则基本的表达式为[16]：

式中：P（0≤P≤1）代表各个栅格拥有的概率值；1 表示栅格被完全占用；0 表示栅格完全空闲；0.5 代表各栅格占用的概率。

4.2 传感数据获取

当机器人由一个点转移到目标点位时，可采用传感器对周边的信息进行感知，当识别到障碍物时，可采用贝叶斯法则更新栅格的概率值。在机器人不断行进过程中，可实时更新单元栅格概率值，及时获取最新的环境感知动态，直到物流机器人工作完成。利用激光传感器，可根据栅格的概率值对栅格性质进行区别，正确区分出现障碍物的栅格、未出现障碍物的栅格，完善栅格地图的框架。传感数据获取的原理为：激光传感器在工作时能够向对应的位置发射光束，这些光束在遭遇到障碍物时会产生反射的作用，可获取到自激光发射到收到的时间差，通过运算即可获得传感器到相应方向最近障碍物的距离。以贝叶斯公式为例，若对测量值进行更新，在x 发生条件下s 的状态为：

采用激光传感器地图更新如图2 所示。

4.3 定位分析

采用基于ICP 的点云匹配算法，实现物流机器人的定位问题，基本思路为：在点集P 中选择初始点集Pi0，求点集Pi0在目标点集Q 中最近的点集，采用欧氏距离阀值法，剔除噪声点得到点集Qi0，点集Pi0和Qi0构成对应点集。对点集Pi0和Qi0应用方向向量阀值法，选定运算初值点集Pi1和Qi1。采用SVD 法获得点集Pi1和Qi1间的旋转矩阵R 和平移矢量Tα，重复上述步骤，直到满足条件。在匹配完成后，进行地图融合与更新。考虑到点云匹配算法的可行性，文章着重采用Trimmed ICP 算法设计，即围绕n 个对应的点数，依据误差度量函数值的大小排序，将距离最大的n 个对应的点去除，为实现ICP，以精度性能、收敛性能为依托进行η（0＜η＜1）的选择，该数值的选择若较小则会导致匹配结果和正常值偏离，若选取的数值偏大，则会导致收敛性能降低。

4.4 模型建模

为辅助物流机器人运动控制及路径规划，基于SLAM算法，采用扫描匹配法运动模型、坐标系统模型结合的方式，对物流机器人的运动轨迹进行控制。扫描匹配法运动模型主要将引入扫描匹配原理，校正周期性的误差。为提高物流机器人定位的精度，采用组合导航、局部扫描匹配相对定位的思路，对两个扫描数据帧连续采集，将采集的数据匹配，了解两者之间在相对位姿上的联系，对机器人目前的位姿增量式的更新，操作步骤为：假设当前时刻激光扫描的点位为D，激光对上一个时刻的扫描记为点位M。采用扫描匹配方法，测算把D 匹配到M 的最优变换策略，记为（R，T），R 代表旋转变换矩阵，T 代表平移矢量。采用（R，T）对机器人的位姿变化情况计算，依据计算结果对物流机器人的位姿状态进行更新。然后，将扫描点D 记为参考的扫描点M，转入前一步骤行迭代计算分析。在测算中，对于给定的k 时刻位姿、位姿变化及k+1 时刻的位姿开展分析，借助该模型可掌握物流机器人的运动情况，提升物流机器人的运动精度。

而物流机器人坐标系统模型的建构方式有着多种选择，如DIN70000 坐标系统、笛卡尔坐标系统、极坐标系统中。而笛卡尔坐标系统相对于其他坐标系统，有着得天独厚的优势，则坐标系统建构中主要将描述传感器位置s=（xs，ys）、环境特征位置mi=（xi，yi）等。由两种物流机器人运动系统结合，可准确地描述机器人周边的不确定信息、提取到有效的环境特征，增强运动控制及路径规划算法的实施效果。

5 结论

物流机器人运动控制及路径规划是长期困扰研究学界及工业应用层面的难题之一，如何进行物流机器人的定位、环境特征提取、地图表示，是关系到机器人运行成效的关键。采用SLAM构建的物流机器人运动控制及路径规划算法，操作中通过栅格地图构建，可以更好地指导机器人定位，不致于使机器人在高强度的作业中迷失位置和方向，为产业发展提供强有力的支持。