CN117490707A - 基于栅格化反光板及环境信息的自动建图及定位算法 - Google Patents

Abstract

本发明公开了一种基于栅格化反光板及环境信息的自动建图及定位算法，包含自动构建栅格化反光板地图以及反光板周边环境地图，根据所构建地图实现物流装备的定位，最终实现物流装备在所建地图空间内的定位。本发明通过将反光板及环境信息均进行栅格化，降低了完全使用反光板定位时对反光板布置的要求，扩充了完全使用环境信息进行建图定位时的适用场景范围。

Description

基于栅格化反光板及环境信息的自动建图及定位算法

技术领域

本发明属于物流设备建图定位技术领域，特别是一种基于栅格化反光板及环境信息的自动建图及定位算法。

背景技术

激光雷达与反光板的组合是一种常用的用于物流设备定位的方式，在有轨物流设备中，通常采用点激光与反光板的组合进行定位；针对如叉车、平板车等无轨设备，通常采用面扫描激光雷达与反光板或面扫描激光雷达与场景环境信息实现定位，目前，使用场景环境信息定位的物流装备，一般是通过栅格化的方式进行建图，通过栅格占位匹配的方式进行定位。而使用反光板定位的物流设备，一般是根据反光板在空间中的位置，形成拓扑地图并进行定位。在一些空旷场景或重复性很高或变动比较大的场景中，由于场景特征不够明显或场景经常变化，导致难以根据场景特征实现定位，因此基于场景环境信息的定位方式是存在局限的；对于基于反光板拓扑地图进行几何计算的定位方式，其主要问题是需要铺设的反光板多，且反光板布置时要考虑距离的差异化处理，施工设计难度比较大，且成本较高。目前，反光板与环境信息结合的定位方式其两种定位算法是独立的。专利CN109613547A提出使用反光板进行定位，辅助构建物流装备运行场景的占位栅格地图的方式，但是在使用反光板定位过程中，依然使用基于几何计算的方式，专利CN112344940A使用反光板以及里程计获取定位的初值，辅助环境信息实现定位，其在反光板定位时，主要使用基于几何计算的方式。在实际使用过程中，除了使用反光板协助建图之外，还可以采用主体导航逻辑使用环境信息，在一些特殊区域，如空旷场景或重复性很高或变动比较大的场景，通过加装反光板实现物流装备的定位，这种情况下，通常需要同时运行两种定位算法，增加了总体的计算量，且两种定位方式无法形成融合互补。综上，现有技术并未将环境信息与反光板信息进行真正的融合，不能满足一些特殊场景下的定位精度要求，甚至需要使用其他辅助定位手段。

发明内容

本发明的目的在于针对上述现有技术存在的问题，提供一种基于栅格化反光板及环境信息的自动建图及定位算法，通过将反光板及环境信息均进行栅格化，降低了完全使用反光板定位时对反光板布置的要求，扩充了完全使用环境信息进行建图定位时的适用场景范围。

实现本发明目的的技术解决方案为：一种基于栅格化反光板及环境信息的自动建图及定位算法，所述算法包括：

自动构建栅格化反光板地图以及反光板周边环境地图；

根据上述所构建地图实现物流装备的定位，最终实现物流装备在所建地图空间内的定位；

所述自动构建栅格化反光板地图以及反光板周边环境地图，包括初始环境信息扫描，使用里程计进行粗定位，扫描匹配实现精定位，建立新的场景地图，通过历史地图及当前扫描点对生成的场景地图进行修正。

进一步地，所述物流装备在所构建地图内的定位，包括初始位置定位、行进过程中的定位。

进一步地，所述初始环境信息扫描，具体包括：将当前激光雷达的位置作为平面坐标系原点，根据激光雷达扫描到的点云信息，使用反射强度筛选扫描到反光板上的点，采用体素滤波对反光板进行栅格化，并对反光板周围的环境信息进行栅格化，建立初始场景的栅格化地图。

进一步地，所述使用反射强度筛选扫描到反光板上的点的过程中，只采集反射率处于预设特定范围内的数据，反光板反射率范围与激光雷达距离反光板的距离的对应关系为：

r＞r₁(d＜d₁)

r₂＞r＞r₃(d₁≤d＜d₂)

r₄＞r＞r₅(d＜d₁)

式中，r表示反射率，r₁,r₂,r₃,r₄,r₅分别代表不同距离情况下反射率的范围，d表示激光雷达距离反光板的距离，d₁,d₂,d₃,d₄,d₅分别表示划分的不同反射率所对应的距离。

进一步地，在所述采集反射率处于预设特定范围内的数据后，采集反射率不在所述预设特定范围内但是距离预设特定反射率的点最近的点的数据。

进一步地，所述采用体素滤波对反光板进行栅格化，并对反光板周围的环境信息进行栅格化，具体为将反光板及反光板周围环境，划分为以小栅格为划分标准的局部子地图以及以大栅格为划分标准的大地图，具体包括：

采用体素滤波，将激光扫描到的包含反光板信息的区域的数据转化为小栅格的占位信息，并将每一个反光板周围采用小栅格进行建图的区域保存为子地图；

接着对反光板周围区域采用大栅格进行建图，将激光扫描范围内的其他大栅格置为空。

进一步地，所述使用里程计进行粗定位，实现方式为：在物流装备移动过程中，综合轮系上的编码器和惯性导航单元的数据，得到在一定时间间隔内物流装备的位姿变化。

进一步地，所述扫描匹配实现精定位，具体包括：

以粗定位得到的当前时刻物流装备的位姿(x_o2,y_o2,θ_o2)为初始值，在(x_o2,y_o2,θ_o2)周围空间建立对于真实位姿的搜索空间，通过在搜索空间中的位姿进行匹配，得到更准确的定位数据；具体地：

设建立的搜索空间为物流设备装备偏转±Δθ_search，物流装备横坐标搜索范围±Δx_search，纵坐标搜索范围±Δy_search，即搜索范围为(x_search,y_search,θ_search)：

x_o2-Δx_search＜x_search＜x_o2+Δx_search

y_o2-Δy_search＜y_search＜y_o2+Δy_search

θ_o2-Δθ_search＜θ_search＜θ_o2+Δθ_search

设(x_{search_j},y_{search_j},θ_{search_j})为第j个搜索数据，将当前时刻的激光扫描数据带入(x_{search_j},y_{search_j},θ_{search_j})中，并与前一时刻建立的大栅格地图进行匹配，根据击中点个数进行评分，设其得分为score_{search_j}；

遍历搜索空间，对得到的所有评分进行比较，若某一个搜索数据的得分最高，则记录此数据，并记录其之后的几个次高数据，形成一组数据；

对该组数据采用小栅格进行匹配，再分别根据击中点个数进行评分，得到最高分的数据即为此时获得的最终精定位位姿(x₂,y₂,θ₂)。

进一步地，所述通过历史地图及当前扫描点对生成的场景地图进行修正，具体包括：

物流装备运行一段时间后，回到之前经过的某位置，使用当前位置以及该位置之前建立的栅格地图进行计算，得到此时预计击中的点；将预计击中的点的数据与实际击中的点进行对比，得到两组数据的偏差值，通过此偏差值对之前的数据进行修正，即完成全场景的建图。

进一步地，构建地图完成后，在物流装备后期运行过程中，使用当前采集数据与建图数据匹配的方式进行定位，且在与地图匹配评分时，通过增大反射率大于预设阈值的点的权重完成反光板与环境信息的融合定位。

本发明与现有技术相比，其显著优点为：

(1)本发明对于反光板的空间布置数量要求低，可减少反光板的需求量，节省了反光板成本。

(2)本发明对于反光板的空间布置复杂度要求低，减少了布置反光板过程中的人力成本。

(3)本发明有效解决了仅凭环境信息进行建图定位时，在空旷、场景变化大、相似场景多的情况下的建图定位问题。

下面结合附图对本发明作进一步详细描述。

附图说明

图1为基于栅格化反光板及环境信息的自动建图及定位算法的流程图。

图2为本发明以1cm进行栅格划分的反光板大栅格建图；

图3为本发明以10cm进行栅格划分的反光板小栅格建图。

具体实施方式

为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。

需要说明的是，若本发明实施例中有涉及“第一”、“第二”等的描述，则该“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。另外，各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

在一个实施例中，结合图1，提供了一种基于栅格化反光板及环境信息的自动建图及定位算法，所述算法包括：

自动构建栅格化反光板地图以及反光板周边环境地图；

根据上述所构建地图实现物流装备的定位，最终实现物流装备在所建地图空间内的定位；

所述自动构建栅格化反光板地图以及反光板周边环境地图，包括初始环境信息扫描，使用里程计进行粗定位，扫描匹配实现精定位，建立新的场景地图，通过历史地图及当前扫描点对生成的场景地图进行修正。

这里，所述物流装备在所构建地图内的定位，包括初始位置定位、行进过程中的定位。

进一步地，在其中一个实施例中，所述初始环境信息扫描，具体包括：将当前激光雷达的位置作为平面坐标系原点，根据激光雷达扫描到的点云信息，使用反射强度筛选扫描到反光板上的点，采用体素滤波对反光板进行栅格化，并对反光板周围的环境信息进行栅格化，建立初始场景的栅格化地图。

进一步地，在其中一个实施例中，所述使用反射强度筛选扫描到反光板上的点的过程中，只采集反射率处于预设特定范围内的数据，反光板反射率范围与激光雷达距离反光板的距离的对应关系为：

r＞r₁(d＜d₁)

r₂＞r＞r₃(d₁≤d＜d₂)

r₄＞r＞r₅(d＜d₁)

式中，r表示反射率，r₁,r₂,r₃,r₄,r₅分别代表不同距离情况下反射率的范围，d表示激光雷达距离反光板的距离，d₁,d₂,d₃,d₄,d₅分别表示划分的不同反射率所对应的距离。

这里，在所述采集反射率处于预设特定范围内的数据后，采集反射率不在所述预设特定范围内但是距离预设特定反射率的点最近(例如周围1m)的点的数据。经过滤后得到满足条件的激光雷达扫描点的坐标：

(x_ref1,y_ref1,θ_ref1),(x_ref2,y_ref2,θ_ref2)......(x_refn,y_refn,θ_refn)

进一步地，在其中一个实施例中，所述采用体素滤波对反光板进行栅格化，并对反光板周围的环境信息进行栅格化，具体为将反光板及反光板周围环境，划分为以小栅格为划分标准的局部子地图以及以大栅格为划分标准的大地图，具体包括：

采用体素滤波，将激光扫描到的包含反光板信息的区域的数据转化为小栅格的占位信息(如图2所示)，并将每一个反光板周围采用小栅格进行建图的区域保存为子地图submap₁,submap₂.....submap_m；

接着对反光板周围区域采用大栅格进行建图(如图3所示)，将激光扫描范围内的其他大栅格置为空。

进一步地，在其中一个实施例中，所述使用里程计进行粗定位，实现方式为：在物流装备移动过程中，综合轮系上的编码器和惯性导航单元的数据，得到在一定时间间隔内物流装备的位姿变化。

设通过里程计得到物流设备的位姿变化为(Δx_{o1_2},Δy_{o1_2},Δθ_{o1_2})，粗定位得到当前时刻物流装备的位姿(x_o2,y_o2,θ_o2)为：

x_o2＝x₁+Δx_{o1_2}

y_o2＝y₁+Δy_{o1_2}

θ_o2＝θ₁+Δθ_{o1_2}

进一步地，在其中一个实施例中，所述扫描匹配实现精定位，具体包括：

以粗定位得到的当前时刻物流装备的位姿(x_o2,y_o2,θ_o2)为初始值，在(x_o2,y_o2,θ_o2)周围空间建立对于真实位姿的搜索空间，通过在搜索空间中的位姿进行匹配，得到更准确的定位数据；具体地：

设建立的搜索空间为物流设备装备偏转±Δθ_search，物流装备横坐标搜索范围±Δx_search，纵坐标搜索范围±Δy_search，即搜索范围为(x_search,y_search,θ_search)：

x_o2-Δx_search＜x_search＜x_o2+Δx_search

y_o2-Δy_search＜y_search＜y_o2+Δy_search

θ_o2-Δθ_search＜θ_search＜θ_o2+Δθ_search

设(x_{search_j},y_{search_j},θ_{search_j})为第j个搜索数据，将当前时刻的激光扫描数据带入(x_{search_j},y_{search_j},θ_{search_j})中，并与前一时刻建立的大栅格地图进行匹配，根据击中点个数进行评分，设其得分为score_{search_j}；

遍历搜索空间，对得到的所有评分进行比较，若某一个搜索数据的得分最高，则记录此数据，并记录其之后的几个次高数据，形成一组数据(x_{search_high_1},y_{search_high_2}.......θ_{search_high_n})；

对该组数据采用小栅格进行匹配，再分别根据击中点个数进行评分，得到最高分的数据即为此时获得的最终精定位位姿(x₂,y₂,θ₂)。

进一步地，通过此时的定位数据以及此时的激光扫描数据，经过体素滤波再建立新扫描区域的栅格地图，以此类推，得到全场景的栅格地图。

进一步地，在其中一个实施例中，为实现对现场环境的修正优化，需进行历史数据修正。所述通过历史地图及当前扫描点对生成的场景地图进行修正，具体包括：

物流装备运行一段时间后，回到之前经过的某位置，使用当前位置以及该位置之前建立的栅格地图进行计算，得到此时预计击中的点；将预计击中的点的数据与实际击中的点进行对比，得到两组数据的偏差值，通过此偏差值对之前的数据进行修正，即完成全场景的建图。

进一步地，在其中一个实施例中，构建地图完成后，在物流装备后期运行过程中，使用当前采集数据与建图数据匹配的方式进行定位，且在与地图匹配评分时，通过增大反射率大于预设阈值的点的权重完成反光板与环境信息的融合定位。

在一个实施例中，提供了一种基于栅格化反光板及环境信息的自动建图及定位系统，所述系统包括：

第一模块，用于自动构建栅格化反光板地图以及反光板周边环境地图；

第二模块，用于根据上述所构建地图实现物流装备的定位，最终实现物流装备在所建地图空间内的定位；

所述自动构建栅格化反光板地图以及反光板周边环境地图，包括初始环境信息扫描，使用里程计进行粗定位，扫描匹配实现精定位，建立新的场景地图，通过历史地图及当前扫描点对生成的场景地图进行修正。

关于基于栅格化反光板及环境信息的自动建图及定位系统的具体限定可以参见上文中对于基于栅格化反光板及环境信息的自动建图及定位算法的限定，在此不再赘述。上述基于栅格化反光板及环境信息的自动建图及定位系统中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。

本发明对于反光板的空间布置数量要求低，对空间布置复杂度要求低，可减少反光板的需求量，节省了反光板成本，减少了布置反光板过程中的人力成本。

以上显示和描述了本发明的基本原理、主要特征及优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于栅格化反光板及环境信息的自动建图及定位算法，其特征在于，所述算法包括：

自动构建栅格化反光板地图以及反光板周边环境地图；

根据上述所构建地图实现物流装备的定位，最终实现物流装备在所建地图空间内的定位；

所述自动构建栅格化反光板地图以及反光板周边环境地图，包括初始环境信息扫描，使用里程计进行粗定位，扫描匹配实现精定位，建立新的场景地图，通过历史地图及当前扫描点对生成的场景地图进行修正。

2.根据权利要求1所述的基于栅格化反光板及环境信息的自动建图及定位算法，其特征在于，所述物流装备在所构建地图内的定位，包括初始位置定位、行进过程中的定位。

3.根据权利要求1所述的基于栅格化反光板及环境信息的自动建图及定位算法，其特征在于，所述初始环境信息扫描，具体包括：将当前激光雷达的位置作为平面坐标系原点，根据激光雷达扫描到的点云信息，使用反射强度筛选扫描到反光板上的点，采用体素滤波对反光板进行栅格化，并对反光板周围的环境信息进行栅格化，建立初始场景的栅格化地图。

4.根据权利要求3所述的基于栅格化反光板及环境信息的自动建图及定位算法，其特征在于，所述使用反射强度筛选扫描到反光板上的点的过程中，只采集反射率处于预设特定范围内的数据，反光板反射率范围与激光雷达距离反光板的距离的对应关系为：

r＞r₁(d＜d₁)

r₂＞r＞r₃(d₁≤d＜d₂)

r₄＞r＞r₅(d＜d₁)

式中，r表示反射率，r₁,r₂,r₃,r₄,r₅分别代表不同距离情况下反射率的范围，d表示激光雷达距离反光板的距离，d₁,d₂,d₃,d₄,d₅分别表示划分的不同反射率所对应的距离。

5.根据权利要求4所述的基于栅格化反光板及环境信息的自动建图及定位算法，其特征在于，在所述采集反射率处于预设特定范围内的数据后，采集反射率不在所述预设特定范围内但是距离预设特定反射率的点最近的点的数据。

6.根据权利要求3所述的基于栅格化反光板及环境信息的自动建图及定位算法，其特征在于，所述采用体素滤波对反光板进行栅格化，并对反光板周围的环境信息进行栅格化，具体为将反光板及反光板周围环境，划分为以小栅格为划分标准的局部子地图以及以大栅格为划分标准的大地图，具体包括：

采用体素滤波，将激光扫描到的包含反光板信息的区域的数据转化为小栅格的占位信息，并将每一个反光板周围采用小栅格进行建图的区域保存为子地图；

接着对反光板周围区域采用大栅格进行建图，将激光扫描范围内的其他大栅格置为空。

7.根据权利要求1所述的基于栅格化反光板及环境信息的自动建图及定位算法，其特征在于，所述使用里程计进行粗定位，实现方式为：在物流装备移动过程中，综合轮系上的编码器和惯性导航单元的数据，得到在一定时间间隔内物流装备的位姿变化。

8.根据权利要求6所述的基于栅格化反光板及环境信息的自动建图及定位算法，其特征在于，所述扫描匹配实现精定位，具体包括：

以粗定位得到的当前时刻物流装备的位姿(x_o2,y_o2,θ_o2)为初始值，在(x_o2,y_o2,θ_o2)周围空间建立对于真实位姿的搜索空间，通过在搜索空间中的位姿进行匹配，得到更准确的定位数据；具体地：

设建立的搜索空间为物流设备装备偏转±Δθ_search，物流装备横坐标搜索范围±Δx_search，纵坐标搜索范围±Δy_search，即搜索范围为(x_search,y_search,θ_search)：

x_o2-Δx_search＜x_search＜x_o2+Δx_search

y_o2-Δy_search＜y_search＜y_o2+Δy_search

θ_o2-Δθ_search＜θ_search＜θ_o2+Δθ_search

设(x_{search_j},y_{search_j},θ_{search_j})为第j个搜索数据，将当前时刻的激光扫描数据带入(x_{search_j},y_{search_j},θ_{search_j})中，并与前一时刻建立的大栅格地图进行匹配，根据击中点个数进行评分，设其得分为score_{search_j}；

遍历搜索空间，对得到的所有评分进行比较，若某一个搜索数据的得分最高，则记录此数据，并记录其之后的几个次高数据，形成一组数据；

对该组数据采用小栅格进行匹配，再分别根据击中点个数进行评分，得到最高分的数据即为此时获得的最终精定位位姿(x₂,y₂,θ₂)。

9.根据权利要求8所述的基于栅格化反光板及环境信息的自动建图及定位算法，其特征在于，所述通过历史地图及当前扫描点对生成的场景地图进行修正，具体包括：

物流装备运行一段时间后，回到之前经过的某位置，使用当前位置以及该位置之前建立的栅格地图进行计算，得到此时预计击中的点；将预计击中的点的数据与实际击中的点进行对比，得到两组数据的偏差值，通过此偏差值对之前的数据进行修正，即完成全场景的建图。

10.根据权利要求9所述的基于栅格化反光板及环境信息的自动建图及定位算法，其特征在于，构建地图完成后，在物流装备后期运行过程中，使用当前采集数据与建图数据匹配的方式进行定位，且在与地图匹配评分时，通过增大反射率大于预设阈值的点的权重完成反光板与环境信息的融合定位。