CN107239076B - 基于虚拟扫描与测距匹配的agv激光slam方法 - Google Patents

Abstract

基于虚拟扫描与测距匹配的AGV激光SLAM方法，涉及移动机器人导航定位。栅格地图表示与创建方法；虚拟扫描与匹配定位方法；提高算法实时性的方法。采用轮廓外形遍历匹配的原理，在每一遍历位姿采用虚拟激光雷达对地图进行扫描，然后虚拟扫描的数据与当前激光雷达的数据直接进行比较，找出AGV机器人的最优位姿信息，再增量式构建地图。针对现有激光SLAM算法多数针对低精度传感器，其滤波、估计、优化的稳定性和定位精度无法绝对保证，难以达到工业AGV机器人应用要求的问题，可通过采用多GPU并行处理和改变虚拟测距的初始推进位置，解决采用反射板和三角定位原理进行导航存在的前期施工及标定问题，提高灵活性柔性、可靠和精度。

Description

基于虚拟扫描与测距匹配的AGV激光SLAM方法

技术领域

本发明涉及移动机器人导航定位，尤其是涉及到一种基于2D激光雷达的无轨导航AGV机器人的SLAM(同时定位与地图构建)方法。

背景技术

同时定位与地图构建(或并发建图与定位)(Simultaneous Localization AndMapping，SLAM)，是一种实现真正全自主移动机器人的关键技术。传统的AGV(AutomatedGuided Vehicle)导航主要有磁条导引、磁钉导引、色带或二维码导引等，虽然简单易行、路径跟踪可靠性好，但均属于固定路径引导方式、灵活性和柔性差。新的激光导航方式无需固定路线导引，在应用时具有更高的柔性，但目前激光导航方法大多数是采用反射板和三角定位原理进行的，依然存在前期施工及标定问题，其灵活性和柔性受到限制，基于SLAM的激光导航是AGV的重要发展趋势之一。

目前基于激光传感器的SLAM多数使用滤波器方法、概率方法、最小二乘法、以及图优化等，如常用的Gmapping、Hector SLAM、Karto SLAM等算法，拥有较小的误差和较低的计算量，在室内移动机器人中得到了一定应用，但滤波、估计、优化的稳定性以及定位精度无法绝对保证(这些方法多数针对的是低成本低精度的激光测距传感器)，在工业AGV机器人中的适用性还有待验证。使用轮廓外形匹配的方法相对于其他方法而言，物理意义更明确，符合人的思维方式，在保证AGV导航定位的稳定性和精度上具有很好的可行性。

参考文献：

[1]罗元,傅有力,程铁凤.基于改进Rao-Blackwellized粒子滤波器的同时定位与地图构建[J].控制理论与应用,2015,32(2):267-272.

[2]苏州艾吉威机器人有限公司.无反射板激光导航AGV在汽车零部件整厂物流调度中的应用[J].物流技术与应用,2016,21(10):175-176.

[3]吴文军,张岩,吴为民,等.一种运输自动导引车导航方法研究[J].物联网技术,2016,6(9):58-62.

[4]常皓,杨巍.基于全向移动模型的Gmapping算法[J].计量与测试技术,2016,43(10):1-4.

[5]葛艳茹,张国伟,沈宏双,等.基于激光测距仪全局匹配扫描的SLAM算法研究[J].计算机测量与控制,2016,24(12):198-199.

[6]汪威,吴耀华,陈云霞.自然导航在AGV中的应用[J].物流技术,2016,35(12):33-36.

[7]孙海,任翠平,卢军,等.激光测距在仓储搬运机器人运动中的应用[J].电子技术与软件工程,2017(1):103-104.

[8]林伟民,顿向明,林子洋,等.基于激光雷达的重型自动导引运载车定位研究[J].机械与电子,2017(3):64-68.

发明内容：

本发明的目的在于针对现有激光SLAM算法多数针对低精度传感器，其滤波、估计、优化的稳定性以及定位精度无法绝对保证，难以达到工业AGV机器人应用要求的问题，提供基于虚拟扫描与测距匹配的AGV激光SLAM方法。

本发明包括以下步骤：

1)栅格地图表示与创建方法；

在步骤1)中，所述栅格地图表示与创建方法的具体方法可为：采用栅格表示地图M，每个栅格为正方形，其长宽为Δ(Δ≤定位精度指标)，对于某个栅格m_i为障碍时，定义其标志位s_i＝1(为非障碍时s_i＝-1、不确定时s_i＝0)，其列行编号为(x_i,y_i)，栅格地图M表示为：

M＝{m₁(x₁,y₁,s₁),m₂(x₂,y₂,s₂),...,m_i(x_i,y_i,s_i),...,m_N(x_N,y_N,s_N)} (1)

使用2D激光雷达(Lidar)进行扫描测距(最大测距范围为d_max)，得到一帧测距数据为{d₁,d₂,...,d_i,...,d_n}，对于其中某一方向的测距d_i，其相对于激光雷达的角度为θ_i，定义其标志位为a_i(若d_i<d_max，则令a_i＝1；否则a_i＝-1)；激光雷达总的一帧测距数据L表示为：

L＝{l₁(d₁,θ₁,a₁),l₂(d₂,θ₂,a₂),...,l_i(d_i,θ_i,a_i),...,l_n(d_n,θ_n,a_n)} (2)

AGV机器人采用2D激光雷达进行导航，假设激光雷达的位姿即为机器人的位姿，x_r、y_r表示位置坐标，θ_r表示方向角，在已知激光雷达位姿情况下，则可以把当前的测距数据L映射到栅格地图M当中，其中M为增量式构建地图，对于L中的l_i，其对应的栅格m_i为：

若a_i＝1，取栅格标志位s_i(x_i,y_i)＝1；以Δ为单位长度(或更小)把线段d_i等分，采用同样的方法计算各等分点对应的栅格并令其标志位为-1(即线段d_i穿过的栅格为非障碍点，标志位为-1)。

2)虚拟扫描与匹配定位方法；

在步骤2)中，所述虚拟扫描与匹配定位方法的具体方法可为：AGV机器人每运行一步后，通过激光雷达获得当前的测距数据L，再结合之前建立的栅格地图M，进行当前位姿的估算(即定位)：

(1)虚拟激光雷达(vLidar)扫描：

采用局部遍历的方式，先设置一定的位置和角度遍历范围Ω，在所有的遍历位姿(假设为可能的激光雷达位姿)，模拟激光雷达，逐一对地图M进行虚拟扫描；在某一遍历位姿(vx_i,vy_i,vθ_i)，获得虚拟扫描数据L_i如下：

L_i＝{vl_i1(vd_i1,vθ_i1,va_i1),...,vl_ij(vd_ij,vθ_ij,va_ij),...,vl_in(vd_in,vθ_in,va_in)} (4)

在虚拟扫描的vθ_i1方向获得测距vd_i1(若vd_i1<d_max，则令va_i1＝1；否则va_i1＝-1)；在vθ_i2方向获得测距vd_i2(若vd_i2<d_max，则令va_i2＝1；否则va_i2＝-1)；...；在vθ_in方向获得测距vd_in(若vd_in<d_max，则令va_in＝1；否则va_in＝-1)；

以方向vθ_ij为例，采用逐步推进的方式获得测距vd_ij，其过程如下：从起始点(vx_i,vy_i)开始，沿相对于vθ_i角度为vθ_ij的方向，以长度Δ(或更小)为增量向前推进，每推进一次计算一次到达点对应的栅格(计算方法同式(3))，直到所到达点栅格的标志位为1则停止推进(若推进距离>dmax则同样停止推进)，此时起始点(vx_i,vy_i)到该栅格的推进距离即为vd_ij(若vd_ij<d_max，则令va_ij＝1；否则va_ij＝-1)；

遍历方式：逐一遍历Ω中的每个栅格点，遍历到每个栅格点时再逐一遍历每个角度值。例如，若Ω的位置范围为10×10＝100个栅格，角度范围为5°/0.1°＝50个角度值，则总的遍历次数为100×50＝5000＝K，遍历完后获得K个虚拟扫描数据VSs(Ω)＝{L₁,L₂,...,L_i,...,L_K}；

(2)基于测距的轮廓匹配：

采用基于测距的轮廓匹配方式进行定位，将VSs(Ω)中所有的虚拟扫描数据与当前的真实测距数据L进行比较，找出与L的测距数据最接近的虚拟扫描数据L*，即：

其中d_min≥0，根据具体应用环境来设定；L*所在的遍历位姿(vx*,vy*,vθ*)即为激光雷达当前位姿的最优估计值，即：

x_r＝vx*,y_r＝vy*,θ_r＝vθ* (6)

3)提高算法实时性的方法。

在步骤3)中，所述提高算法实时性的方法的具体方法可为：基于虚拟扫描与测距匹配的SLAM方法可先用于离线建图，然后用于AGV机器人工作时的即时定位，其中，虚拟激光雷达扫描的计算量最大，每次遍历时都需要模拟激光雷达在其n个扫描方向上进行推进测距，采取以下措施：

(1)采用多GPU并行处理方式，每个GPU运行一个扫描方向上模拟测距的推进测距算法；

(2)对于第i个扫描方向，改变虚拟测距方向vθ_i上的初始推进位置，从小于真实测距离d_i的某个位置开始(即不从遍历位置开始)，推进时遇到障碍栅格或到距离大于d_i的某个位置则停止推进。

所述虚拟扫描方向vθ_i对应的GPU推进测距算法如下：

算法输入：遍历位姿(vx,vy,vθ)，地图M，激光测距L

算法输出：虚拟扫描方向上的测距vd_i，标志位va_i

本发明采用轮廓外形遍历匹配的原理，在每一遍历位姿采用虚拟激光雷达对地图进行扫描，然后虚拟扫描的数据与当前激光雷达的数据直接进行比较，找出AGV机器人的最优位姿信息，再增量式构建地图。本发明基于虚拟扫描与测距匹配的AGV激光SLAM方法，涉及一种应用于AGV机器人导航的同时定位与地图构建方法。主要包括一个栅格地图表示与创建步骤；一个虚拟扫描与匹配定位步骤；一种提高虚拟扫描实时性的措施和算法实现。针对现有激光SLAM算法多数针对低精度传感器，其滤波、估计、优化的稳定性以及定位精度无法绝对保证，难以达到工业AGV机器人应用要求的问题，提供了一种适合高精度激光雷达的SLAM方法。该方法采用轮廓外形遍历匹配的原理，在每一遍历位姿采用虚拟激光雷达对地图进行扫描，然后虚拟测距数据与当前激光雷达的测距数据直接进行比较，找出AGV机器人的最优位姿信息，再增量式构建地图；其中可通过采用多GPU并行处理和改变虚拟测距的初始推进位置，提高该SLAM方法的实时性。因此，解决了采用反射板和三角定位原理进行导航存在的前期施工及标定问题，为提高AGV与其他移动机器人的灵活性和柔性，提供了一种简单可靠和有精度保证的可行SLAM方法。

附图说明

图1为栅格地图表示与创建方法示意图。

图2为虚拟激光雷达(vLidar)扫描方法示意图。

图3为基于虚拟扫描与测距匹配的SLAM方法组成框图。

具体实施方式

下面结合附图和实施例对本发明的具体实施方式进行说明。

实施例1：本发明基于虚拟扫描与测距匹配的SLAM方法用于离线建图，具体实施的操作过程如下。

Step 1：采用遥控或其他人工操作方式，控制AGV机器人在工作环境中行走一遍，激光雷达采集并保存所有时刻(t₀至t_end)的测距信息{d₁,d₂,...,d_n}|t₀、{d₁,d₂,...,d_n}|t₁、...、{d₁,d₂,...,d_n}|t_end。

Step 2：采用合适的数据格式和文件，定义如式(1)所示的栅格地图M，并进行初始化。

Step 3：从t₀开始时刻到t_end结束时刻，利用测距信息构建地图，步骤如下。

Step 3.1：对于初始时刻t₀，激光雷达的初始位姿已知，根据式(2)把{d₁,d₂,...,d_n}|t₀转换为L_t0，然后根据式(3)将L_t0映射到栅格地图M当中；

Step 3.2：对于时刻t₁，①激光雷达的当前位姿未知，先根据式(2)把{d₁,d₂,...,d_n}|t₁转换为L_t1，在遍历范围Ω中逐一模拟激光雷达进行扫描，得到虚拟扫描数据VSs(Ω)＝{L₁,L₂,...,L_i,...,L_K}(其L_i的格式如式(4)所示)，然后与L_t1进行比较，根据式(5)找出与L_t1最接近的虚拟扫描数据L*；②根据L*和式(6)获得激光雷达t₁时刻的位姿，进而根据式(3)将L_t1映射到栅格地图M当中；

Step 3.3：对于时刻t₂，定位和建图过程与Step 3.2类似；

...(依此一直往下进行，直到时刻t_end，则建图结束)

实施例2：在离线建图后，本发明用于有地图的AGV机器人实时定位，具体实施的操作过程如下。

Step 1：在当前时刻，激光雷达获得测距信息{d₁,d₂,...,d_n}，根据式(2)把{d₁,d₂,...,d_n}转换为L。

Step 2：在遍历范围Ω中的每一位姿，模拟激光雷达对地图M进行扫描(采用多GPU并行处理和推进测距算法，每个GPU对应一个激光扫描方向)，得到虚拟扫描数据VSs(Ω)＝{L₁,L₂,...,L_i,...,L_K}。

Step 3：将数据VSs(Ω)＝{L₁,L₂,...,L_i,...,L_K}与L进行比较，根据式(5)找出与L最接近的虚拟扫描数据L*；

Step 4：根据L*和式(6)获得激光雷达当前时刻的位姿估计值(即定位)。

实施例3：本发明基于虚拟扫描与测距匹配的SLAM方法直接用于实时并发建图与定位，具体实施的操作过程如下。

Step 1：采用合适的数据格式和文件，定义如式(1)所示的栅格地图M，并进行初始化。

Step 2：对于初始时刻t₀，激光雷达的初始位姿已知，根据式(2)把测距{d₁,d₂,...,d_n}|t₀转换为L_t0，然后根据式(3)将L_t0映射到栅格地图M当中；

Step 3：在下一个当前时刻，激光雷达获得测距信息{d₁,d₂,...,d_n}，根据式(2)把{d₁,d₂,...,d_n}转换为L。

Step 4：在遍历范围Ω中的每一位姿，模拟激光雷达对地图M进行扫描(采用多GPU并行处理和推进测距算法，每个GPU对应一个激光扫描方向)，得到所有虚拟扫描数据VSs(Ω)＝{L₁,L₂,...,L_i,...,L_K}。

Step 5：将数据VSs(Ω)＝{L₁,L₂,...,L_i,...,L_K}与L进行比较，根据式(5)找出与L最接近的虚拟扫描数据L*；

Step 6：根据L*和式(6)获得激光雷达当前时刻的位姿估计值(即定位)；

Step 7：激光雷达的位姿估计值已知，根据式(3)将L映射到栅格地图M当中；返回到Step3。

以上实施例中，栅格地图表示与创建方法的如图1所示，虚拟激光雷达扫描方法的原理如图2所示，步骤之间的逻辑关系如图3所示。

Claims (3)

1.基于虚拟扫描与测距匹配的AGV激光SLAM方法，其特征在于包括以下步骤：

1)栅格地图表示与创建方法：采用栅格表示栅格地图M，每个栅格为正方形，其长宽为Δ≤定位精度指标，对于某个栅格m_i为障碍时，定义其标志位s_i＝1，为非障碍时s_i＝-1、不确定时s_i＝0，其列行编号为(x_i,y_i)，栅格地图M表示为：

M＝{m₁(x₁,y₁,s₁),m₂(x₂,y₂,s₂),...,m_i(x_i,y_i,s_i),...,m_N(x_N,y_N,s_N)}

2)虚拟扫描与匹配定位方法：AGV机器人每运行一步后，通过激光雷达获得当前的测距数据L，再结合之前建立的栅格地图M，进行当前位姿的估算，即定位；

虚拟激光雷达扫描如下：

采用局部遍历的方式，先设置一定的位置和角度遍历范围Ω，在所有的遍历位姿，假设为可能的激光雷达位姿，模拟激光雷达，逐一对栅格地图M进行虚拟扫描；在某一遍历位姿(vx_i,vy_i,vθ_i)，获得虚拟扫描数据L_i如下：

L_i＝{vl_i1(vd_i1,vθ_i1,va_i1),...,vl_ij(vd_ij,vθ_ij,va_ij),...,vl_in(vd_in,vθ_in,va_in)}

在虚拟扫描的vθ_i1方向获得测距vd_i1，若vd_i1<d_max，则令va_i1＝1；否则va_i1＝-1；在vθ_i2方向获得测距vd_i2，若vd_i2<d_max，则令va_i2＝1；否则va_i2＝-1；...；在vθ_in方向获得测距vd_in，若vd_in<d_max，则令va_in＝1；否则va_in＝-1；

以方向vθ_ij时，采用逐步推进的方式获得测距vd_ij，其过程如下：从起始点(vx_i,vy_i)开始，沿相对于vθ_i角度为vθ_ij的方向，以长度Δ，或更小，为增量向前推进，每推进一次计算一次到达点对应的栅格，直到所到达点栅格的标志位为1则停止推进，若推进距离>dmax，则同样停止推进，此时起始点(vx_i,vy_i)到该栅格的推进距离即为vd_ij，若vd_ij<d_max，则令va_ij＝1；否则va_ij＝-1；

遍历方式时，逐一遍历Ω中的每个栅格点，遍历到每个栅格点时再逐一遍历每个角度值，若Ω的位置范围为10×10＝100个栅格，角度范围为5°/0.1°＝50个角度值，则总的遍历次数为100×50＝5000＝K，遍历完后获得K个虚拟扫描数据VSs(Ω)＝{L₁,L₂,...,L_i,...,L_K}；

基于测距的轮廓匹配为：

采用基于测距的轮廓匹配方式进行定位，将VSs(Ω)中所有的虚拟扫描数据与当前的真实测距数据L进行比较，找出与L的测距数据最接近的虚拟扫描数据L*，即：

其中d_min≥0，根据具体应用环境来设定；L*所在的遍历位姿(vx*,vy*,vθ*)即为激光雷达当前位姿的最优估计值，即：

x_r＝vx*,y_r＝vy*,θ_r＝vθ*；

3)提高算法实时性的方法：基于虚拟扫描与测距匹配的SLAM方法先用于离线建图，然后用于AGV机器人工作时的即时定位，其中，虚拟激光雷达扫描的计算量最大，每次遍历时都需要模拟激光雷达在其n个扫描方向上进行推进测距，采取以下措施：

(1)采用多GPU并行处理方式，每个GPU运行一个扫描方向上模拟测距的推进测距算法；

(2)对于第i个扫描方向，改变虚拟测距方向vθ_i上的初始推进位置，从小于真实测距离d_i的某个位置开始，即不从遍历位置开始，推进时遇到障碍栅格或到距离大于d_i的某个位置则停止推进；

所述虚拟扫描方向vθ_i对应的GPU推进测距算法如下：

算法输入：遍历位姿(vx,vy,vθ)，栅格地图M，测距数据L

算法输出：虚拟扫描方向上的测距vd_i，标志位va_i

2.如权利要求1所述基于虚拟扫描与测距匹配的AGV激光SLAM方法，其特征在于在步骤1)中，使用2D激光雷达进行扫描测距，最大测距范围为d_max，得到一帧测距数据为{d₁,d₂,...,d_i,...,d_n}，对于其中某一方向的测距d_i，其相对于激光雷达的角度为θ_i，定义其标志位为a_i，若d_i<d_max，则令a_i＝1；否则a_i＝-1；激光雷达总的一帧测距数据L表示为：

L＝{l₁(d₁,θ₁,a₁),l₂(d₂,θ₂,a₂),...,l_i(d_i,θ_i,a_i),...,l_n(d_n,θ_n,a_n)}。

3.如权利要求1所述基于虚拟扫描与测距匹配的AGV激光SLAM方法，其特征在于在步骤1)中，AGV机器人采用2D激光雷达进行导航，假设激光雷达的位姿即为机器人的位姿，x_r、y_r表示位置坐标，θ_r表示方向角，在已知激光雷达位姿情况下，则把当前的测距数据L映射到栅格地图M当中，对于L中的l_i，其对应的栅格m_i为：

xl_i＝x_r+di×cos(θ_r+θ_i)向上取整得

yl_i＝y_r+di×sin(θ_r+θ_i)向上取整得

若a_i＝1，取栅格标志位s_i(x_i,y_i)＝1；以Δ为单位长度或更小把线段d_i等分，或计算各等分点对应的栅格并令其标志位为-1，即线段d_i穿过的栅格为非障碍点。

Images