一种基于激光SLAM的室内AGV自适应定位方法

摘要

本发明涉及了一种基于激光SLAM的室内AGV自适应定位方法，涉及激光SLAM、里程计和IMU多种传感器融合的定位技术领域。室内AGV自适应定位系统包括上位机、下位机、电机驱动装置、2D激光雷达、里程计和惯性测量单元。利用激光雷达对AGV的工作环境构建二维栅格地图，根据工作环境中不同的结构特征，在基于特征点的ICP匹配算法和基于优化的匹配算法之间进行自适应的切换，从而提高AGV在复杂工作环境中的定位精度和速度，实现能够有效克服现有技术存在的AGV定位精度低、定位灵活性差和成本高的问题。

权利要求

1.一种基于激光SLAM的室内AGV自适应定位方法，其特征在于：包括以下步骤：S1、首先利用AGV搭载的激光雷达、惯性测量单元和里程计对AGV的工作环境构建二维栅格地图；

S2、在远程监控终端上发布位于二维栅格地图的目标点坐标信息；

S3、以AGV当前所在位置为起点，利用导航算法规划出一条到达目标点的最优路径；

S4、AGV按照规划好的路径运行，利用上位机实时获取AGV在工作环境中的位姿数据并在远程监控终端上显示AGV的运行状态；

S5、在AGV运行过程中，实时采集搭载的惯性测量单元和激光雷达的数据，将采集到的数据值经过处理后代入改进的Sigmoid函数中，根据计算得到的函数值来判断AGV当前时刻所处的工作环境从而采取相应的定位算法，若AGV进入弯道或者狭窄的货架区域时，则采用基于特征点的ICP匹配算法，若AGV进入宽阔的运行环境时，则采用基于优化的匹配算法；

步骤S5中改进的Sigmoid函数定义如下：公式(1)中，θ为弯道环境的角度值，λ为运行通道的宽窄度值，θ的值使用激光雷达扫描周围环境经过处理后得到，λ的值使用激光雷达的扫描数据直接得到，公式(1)中通过f(λ,θ)的值来确定AGV运行时采用的匹配算法；

当AGV启动运行时，同步采集惯性测量单元和激光雷达的数据，其中弯道环境角度值θ，通过激光雷达扫描周围环境经过处理后获得；通道的宽窄度值λ，首先对激光雷达的前几帧数据进行预处理，并将距离值变化较大的离群点滤除，最后求通道宽窄度的平均值λ，将θ和λ的值代入公式(1)中进行计算，根据计算得出的值来判断AGV当前时刻所处的环境状态；

S6、通过上位机实时计算AGV的位姿数据判断是否运行到目标点，当AGV运行到目标点时，上位机发出停止运行指令，下位机接收到指令控制驱动电机使AGV停止在目标点；否则继续执行步骤5，直至AGV到达目标点停止运行。

2.根据权利要求1所述的一种基于激光SLAM的室内AGV自适应定位方法，其特征在于，所述上位机为NVIDIA TX2高性能嵌入式开发板，装有Linux和ROS操作系统，下位机为STM32嵌入式开发板。

3.根据权利要求1所述的一种基于激光SLAM的室内AGV自适应定位方法，其特征在于，所述激光雷达与上位机通过USB线进行连接，将AGV周围的障碍物位置信息传递给上位机；

所述惯性测量单元(IMU)与上位机通过USB‑TTL线直接相连，采集AGV的角度和角速度信息；

所述里程计与下位机通过GPIO接口连接，采集AGV的行驶里程信息；

所述AGV电机驱动模块与下位机通过IO口进行连接，控制AGV按照规划好的路径进行运动；

所述上位机与下位机通过串口进行连接，下位机将里程计信息传递给上位机，上位机将运行指令通过串口发布到下位机中；

所述上位机与远程监控终端通过局域网进行连接，上位机将AGV的实时位姿传递给远程监控终端并接受远程监控终端发布的控制指令，远程监控终端用于发布目标点的位姿信息和监控AGV的运行状态。

说明书

一种基于激光SLAM的室内AGV自适应定位方法

技术领域

[0001] 本发明涉及机器人定位技术领域，具体涉及了2D激光雷达、里程计和惯性测量单元多传感器融合的自适应切换的激光SLAM定位方法。

背景技术

[0002] 随着社会的飞速发展，近年来网络购物的人越来越多，使得物流行业朝着更加智能化和高效化的方向发展。自动导引车(Automated Guided Vehicle，AGV)是一种在工作环
境中沿指定路径自动行进的车辆，通常用于工业生产或仓库货物的运输。它是目前物流行
业应用最为广泛的机器人之一。AGV在现代工业生产中发挥着越来越重要的作用，并得到了
广泛的应用。

[0003] AGV作为一种具有自主定位与导航功能的小车，是一个集环境感知、路径规划、智能决策与控制等功能于一体的复杂系统。在对自主移动机器人的各项相关技术研究中，定
位技术是自主导航中关键的技术之一，其定位精度直接影响AGV后续建图和导航任务的执
行。因此，如何保证AGV能够快速准确地进行定位是个技术难点，一直以来备受研究人员关
注。

[0004] AGV常用的定位方法为磁带定位、二维码定位、惯导定位，这些定位方法虽然定位精度较高、简单易行，但定位的灵活性差，当工作环境发生改变时，需要重新铺设定位设备，
增加额外的成本。

[0005] 现有的2D激光SLAM定位算法仍然存在一定的缺陷，无法提供一致、稳定的定位信息。但是，基于特征点的ICP和基于优化的匹配算法优缺点各不相同，可以相互补充。因此，
为了提高AGV的定位精度和鲁棒性，本发明提出了一种基于Sigmoid函数的自适应切换的定
位算法，在改进的ICP匹配算法和基于优化的匹配算法之间进行切换。将环境的角度值和运
行时的通道宽窄度值设为切换阈值条件，从而达到提高AGV在复杂工作环境中的定位精度。

发明内容

[0006] 针对上述问题，本发明目的在于提供一种基于激光SLAM的室内AGV自适应定位方法，即首先利用2D激光雷达建立工作环境的栅格地图，然后在已经建立好的地图上采用自
适应切换的方法在基于特征点的ICP和基于优化的匹配算法之间进行切换。

[0007] 为了实现上述目的，本发明采用下述技术方案：

[0008] 一种基于激光SLAM的室内AGV自适应定位方法，包括2D激光雷达、里程计、惯性测量单元、NVIDIA TX2嵌入式开发板、STM32单片机和远程监控终端；

[0009] 2D激光雷达与NVIDIA TX2上位机通过USB线进行连接，激光雷达对环境信息进行扫描，将扫描的每一帧数据传送给上位机进行处理；

[0010] 惯性测量单元(IMU)采用的是USB‑TTL转接线与上位机NVIDIA TX2进行连接，用于采集AGV的车体姿态信息；

[0011] AGV的里程计与STM32单片机通过GPIO接口连接，里程计采集AGV的运行里程和行驶速度信息，将采集到的信息传递给STM32下位机；

[0012] 上位机NVIDIA TX2和下位机STM32通过串口进行双向连接，AGV的电机驱动模块通过STM32单片机的IO口进行连接，上位机NVIDIA TX2使用局域网与远程监控终端进行通讯
连接；

[0013] 下位机STM32单片机将采集到的AGV里程计的速度和行驶里程信息通过串口传递给上位机，并接受上位机发出的AGV电机驱动信息；

[0014] 2D激光雷达将采集到的工作环境中的障碍物信息、惯性测量单元(IMU)将采集到的AGV车体姿态角度值、STM32单片机将采集到的AGV速度和行驶里程信息传递给NVIDIA 
TX2上位机中，通过采用激光SLAM算法，首先对AGV的工作环境进行建图，操控AGV进行移动
并建立二维栅格地图；

[0015] 远程监控终端用于监控AGV的运行状态和发布目标位置的坐标信息；

[0016] AGV为差速AGV，包括电机驱动电路、左轮驱动电机和右轮驱动电机、上位机发布运行指令给下位机，然后下位机发布驱动指令控制AGV左右轮电机的旋转，达到对AGV运行状
态的控制；

[0017] 上位机为NVIDIA的TX2高性能嵌入式开发板、装有Linux和ROS操作系统，该开发板拥有多核处理器且性能强大，可用于激光SLAM算法的运行和数据处理，作为算法移植的运
行平台；

[0018] 上位机、下位机、驱动电机、2D激光雷达、惯性测量单元和里程计利用AGV自身携带的电源经转换后进行供电；

[0019] 本发明中的AGV主要用于工业生产和物流仓库环境中，多数障碍物为静态障碍物，包含墙壁、生产设备、仓储货架、摆放的货物。

[0020] 本发明还提供了一种基于激光SLAM的室内AGV自适应定位算法，步骤如下：

[0021] 步骤1，在远程监控终端上，手动控制AGV在工作环境中进行移动，利用激光SLAM算法对所处环境构建二维栅格地图；

[0022] 步骤2，首先在远程监控终端上输入AGV目标点的位姿信息，利用导航算法中的路径规划算法规划出一条最优的运行路线；

[0023] 步骤3，本发明定义一个用于自适应切换匹配算法的改进Sigmoid函数，函数定义如下：

[0024]

[0025] 公式(1)中，θ为弯道环境的角度值，λ为运行通道的宽窄度值，θ的值使用激光雷达扫描周围环境经过处理后得到，λ的值使用激光雷达的扫描数据可直接得到；

[0026] 当公式(1)中 因此可通过f(λ,θ)的值来确定AGV运行时采用的匹配算法；

[0027] 当AGV启动运行时，同步采集惯性测量单元和激光雷达的数据，其中弯道环境角度值θ，通过激光雷达扫描周围环境经过处理后获得；通道的宽窄度值λ，首先对激光雷达的前
几帧数据进行预处理，并将距离值变化较大的离群点滤除，最后求通道宽窄度的平均值λ，
将θ和λ的值代入公式(1)中进行计算，根据计算得出的值来判断AGV当前时刻所处的环境状
态；

[0028] 步骤4，当计算得到的f(λ,θ)值大于0时，说明此时AGV处于宽阔的环境中，采用基于优化的匹配算法，该方法适用于大场景的环境，如AGV工业生产中的上下料区、物流仓库
中的货物中转区；

[0029] 基于优化的算法思路为给定一个目标函数，将激光雷达的帧间匹配问题转化为求解目标函数极值的问题，目标函数的定义如下：

[0030]

[0031] 公式(2)中，T＝(Tx,Ty,Tθ)为机器人的在(x,y,z)坐标系下的位姿，Si(T)表示把激光雷达某一帧第i个数据用位姿T进行坐标转换，M(x)表示转换后的坐标x在地图中的占用
概率；

[0032] 公式(2)中，

[0033] M(Si(T))为非线性函数，对公式(2)进行一阶泰勒展开，得公式(3)

[0034]

[0035] 对于线性系统，对公式(3)中的ΔT求导数，并令其等于0，得公式(4)

[0036]

[0037] 求解公式(4)可得ΔT，如公式(5)所示：

[0038]

[0039] 公式(5)中， 通过对地图进行双线性插值进行求解。令T＝T+ΔT，不断进行迭代，直到满足精度要求或者到达最大迭代次数求解出机器人的位姿(x,y,θ)。

[0040] 通过以上算法，得到AGV在宽阔环境运行下的位姿，为导航提供高精度的定位数据；

[0041] 步骤5，当AGV进入弯道环境时，会因为轮子打滑、激光雷达扫描数据点覆盖的问题，导致激光SLAM的前端匹配算法出现匹配错误，从而影响AGV的定位精度；

[0042] 当计算得到的f(λ,θ)值小于0时，说明此时AGV处于弯道或者货架的环境，因此采用基于特征点的ICP匹配算法，通过提取环境结构中的特征点来提高匹配的精度和速度，从
而提高AGV的定位精度；

[0043] 使用激光雷达扫描环境结构，通过对数据进行预处理来提取特征点，如公式(6)和公式(7)所示：

[0044]

[0045]

[0046] 公式(6)中 为所选取的特征点同一帧数据中，特征点左侧范围内点集的计算值，公式(7)中 为所选取的特征点同一帧数据中，特征点右侧范围内点集的计算
值；

[0047] 将公式(6)和公式(7)代入公式(8)，如下式所示：

[0048]

[0049]

[0050]

[0051] 通过公式(8)、公式(9)和公式(10)，将可能为特征点的点两侧范围内的点进行直线拟合，并计算两条拟合直线的夹角，若计算得到的f(k)值越大，说明两条拟合的直线夹角
越接近90度，则该点为所需要的特征点，若f(k)的值越小，则越偏离90度，通过此方法将不
是特征点的点进行剔除；

[0052] 通过步骤5的计算，来提取环境结构中满足要求的特征点；

[0053] 步骤6，通过步骤5提取出的环境结构中的特征点，因为采用的是只提取环境中的特征点进行匹配，相比于原算法采用的逐点匹配法则可以提高算法的匹配速度和准确度。
首先建立AGV的运动学模型，使用里程计通过运动学模型估计AGV在环境中的位姿，然后将
激光雷达特征匹配和IMU观测得到的信息进行融合计算出AGV在实际环境中的位姿，利用计
算出的实际位姿信息来更新估计的AGV位姿，从而得到AGV的精确位姿(x,y,θ)。采用基于特
征点的ICP匹配算法可以提高算法的匹配速度和匹配精度，从而减少AGV在弯道环境运行时
的定位误差；

[0054] 步骤7，当AGV从宽阔的区域运行到货架区域时，由于货架结构的特殊性，其可以使用的特征点多且易提取出来，因此当AGV运行到货架这类狭窄的区域时采用基于特征点的
ICP匹配算法也可以提高定位精度；

[0055] 利用AGV搭载的激光雷达采集两侧的障碍物信息，当AGV进入狭窄的货架区时f(λ,θ)小于0，从而将匹配算法切换为基于特征点的ICP匹配算法，采用步骤(5)和步骤(6)的方
式通过提取货架的特征点进行匹配，从而得到精度更高的AGV位姿数据；

[0056] 与现有AGV采用的定位方式相比，本发明采用的基于激光SLAM的自适应定位方法定位灵活性高、适应性好、成本低，可以满足大部分应用环境的需求，并且采用基于ROS的开
发系统，降低了开发的难度、提高了算法调试的效率。

附图说明

[0057] 图1为本发明中基于激光SLAM的AGV自适应定位方法硬件结构图。

[0058] 图2为本发明中基于激光SLAM的AGV自适应定位算法流程图。

具体实施方式

[0059] 以下结合附图并通过具体实施方式来进一步说明本发明的技术方案。

[0060] 图1为本发明中基于激光SLAM的AGV自适应定位方法硬件结构图，由图1所示。

[0061] 在本发明中，上位机选用的是NVIDIA TX2高性能嵌入式开发板，装有Ubuntu 16.04和ROS Kinetic操作系统。下位机为STM32嵌入式开发板，远程监控终端为笔记本电
脑，同样装有Ubuntu和ROS操作系统，用于显示AGV的运行状态和发布目标点坐标指令。

[0062] 采用的RPLidar A2激光雷达与上位机通过USB线进行连接，激光雷达采集有效范围内障碍物的距离和角度信息，并将扫描的信息传送给上位机进行处理。

[0063] 采用的惯性测量单元(IMU)采用USB‑TTL线与上位机进行连接，用于采集AGV运行时车体的旋转角度信息，将采集到的信息直接传递给上位机，可以提高算法的计算效率，避
免与下位相连再传递给上位机带来的时间延迟和传送时带来的数据误差。

[0064] 采用的里程计与下位机通过GPIO接口单向连接，用于采集AGV的线速度、角速度和运行里程信息，并将采集到的信息经过处理后通过下位机传递到上位机。

[0065] 本定位方法上位机与下位机通过串口线进行双向连接，下位机通过IO口与AGV的电机驱动模块进行连接，远程监控终端与上位机通过局域网进行连接。

[0066] 远程监控终端发布AGV要到达的目标点位置信息，通过局域网上位机接收到指令，使用激光雷达采集环境中的障碍物位置信息，同时采集IMU和里程计的信息，将这些数据传
送到上位机中，利用激光SLAM算法，实时计算出AGV的位姿数据，并将其传送给导航算法部
分，上位机将规划好的路径信息传递给下位机，下位机通过IO口发布电机驱动指令，控制
AGV朝目标点进行运动。

[0067] 图2为本发明中基于激光SLAM的室内AGV自适应定位算法流程图。由图2可知，本发明提供了一种基于激光SLAM的AGV自适应定位算法，步骤如下：

[0068] 步骤1，首先通过远程监控终端，操控AGV构建工作环境的二维栅格地图。

[0069] 步骤2，远程监控终端向上位机传送AGV要到达的目标点位置信息。

[0070] 步骤3，上位机接收远程监控终端发布的目标点位置信息，将AGV当前的位置作为起点，在二维栅格地图上利用路径规划算法规划出一条到达目标点的最优路径。

[0071] 步骤4，在AGV朝目标点进行运动时，导航算法需要AGV在环境中运行的实时位姿数据，只有高精度的定位数据，才能保证AGV准确无误地到达目标点，因此本发明采用自适应
的定位算法来提高AGV的定位精度，该定位算法过程如下：

[0072] 步骤4.1，当AGV接收到远程监控终端发出的运行指令后，上位机采集AGV搭载的传感器检测数据，将经过处理的θ和λ值代入公式(1)中进行计算，若计算后的f(λ,θ)值小于0，
说明此时AGV处于弯道或者狭窄的运行环境中，上位机将匹配算法切换为基于特征点的ICP
算法，可以充分利用环境结构特征进行匹配，也能避免AGV因处于弯道环境车轮打滑导致的
匹配误差，从而提高AGV的定位精度和速度；

[0073] 步骤4.2，若计算后的f(λ,θ)值大于0，说明此时AGV运行在宽阔的环境中，上位机则将匹配算法切换为基于优化的算法，从而保证AGV在大场景环境下的定位精度；

[0074] 步骤5，判断AGV是否到达目标位置点，如果到达目标位置点，上位机向下位机发出停止运行指令，下位机则向AGV的电机驱动模块发出停止运行指令，并且等待下一次运行指
令的发出；若未达到目标点，则返回步骤4，AGV继续运动直至到达目标点停止运行等待下一
次指令。