CN109633666B - 室内动态环境下基于激光雷达的定位方法及计算机存储介质 - Google Patents

Abstract

本发明提供一种室内动态环境下基于激光雷达的定位方法及计算机存储介质，所述方法包括以下步骤：S1、给定机器人初始位姿，并对机器人初始位姿进行采样生成多个初始粒子；S2、根据运动模型更新粒子状态；S3、采用激光雷达进行数据采样，获得点云数据；S4、根据点云数据更新粒子权重；S5、根据粒子权重对粒子进行重采样；S6、更新机器人位姿；S7、判断粒子集是否收敛，若未收敛，则跳转至步骤S2，若收敛，则进入步骤S8；S8、对机器人位姿进行二次重采样；S9、跳转至步骤S2。根据本发明实施例的定位方法，通过对基于粒子滤波的定位方法进行改进，使其适用于室内的动态环境。

Description

室内动态环境下基于激光雷达的定位方法及计算机存储介质

技术领域

本发明涉及激光定位领域，更具体地，涉及一种室内动态环境下基于激光雷达的定位方法及计算机存储介质。

背景技术

目前，基于粒子滤波的定位方法通常是通过运动模型对机器人位姿进行采样生成大量粒子，依据传感器的观测结果更新粒子的权重并进行重采样，最终粒子会收敛至正确位姿。但是传统的基于粒子滤波的定位方法假设机器人是在一个静态环境下运动，因此应用在商场、医院展会等人流较大的动态环境下易发生定位丢失现象。

发明内容

有鉴于此，本发明提供一种室内动态环境下基于激光雷达的定位方法及计算机存储介质，定位效果好，应用范围广。

为解决上述技术问题，一方面，本发明提供一种室内动态环境下基于激光雷达的定位方法，所述方法包括以下步骤：S1、给定机器人初始位姿，并对机器人初始位姿进行采样生成多个初始粒子；S2、根据运动模型更新粒子状态；S3、采用激光雷达进行数据采样，获得点云数据；S4、根据点云数据更新粒子权重；S5、根据粒子权重对粒子进行重采样；S6、更新机器人位姿；S7、判断粒子集是否收敛，若未收敛，则跳转至步骤S2，若收敛，则进入步骤S8；S8、对机器人位姿进行二次重采样；S9、跳转至步骤S2。

根据本发明的一些实施例，给定机器人初始位姿x_t，基于高斯分布对x_t进行采样生成N个初始粒子，记为：

其中，粒子权重初始化为

根据本发明的一些实施例，在步骤S2中，根据运动模型更新粒子状态，记为

其中，u_t为运动控制量，v_t为噪声。

根据本发明的一些实施例，步骤S3包括：S31、将所述激光雷达的数据按扫描角度分块，每20度为一块区域，最后不足以构成一块区域的激光雷达数据与前一组数据合并为一组；S32、提取每块区域的激光雷达数据中距离最大的5个数据作为点云数据。

根据本发明的一些实施例，在步骤S4中，根据步骤S3获得的点云数据，依据各个粒子的位姿，将点云数据映射到地图并计算点云与地图的匹配值，点云匹配评价函数为：

其中，σ＝0.1，d为点云在地图中与障碍点最近的距离，记点云数据为m个，在地图中第j个点云离障碍点最近距离记为d_j，则第i个粒子的点云匹配值为

取点云的匹配值为粒子的权重，则

归一化权重，则

根据本发明的一些实施例，在步骤S5中，根据粒子权重对粒子进行重采样，并重新设置权重为

根据本发明的一些实施例，在步骤S6中，更新机器人位姿，记为：

t＝t+1。

根据本发明的一些实施例，在步骤S8中，基于高斯分布对机器人位姿进行二次重采样以更新粒子集，即X_t～N(x_t,Σ₁)，其中，X_t为更新后的粒子集，x_t为步骤S6更新后的机器人位姿，Σ₁为采样协方差。

根据本发明的一些实施例，在步骤S8中，根据更新后的粒子集，计算位姿x_t的点云匹配值，记为s1，若s1>0.8，则采样协方差为：

否则采样协方差为：

设置粒子权重为

第二方面，本发明实施例提供一种计算机存储介质，包括一条或多条计算机指令，所述一条或多条计算机指令在执行时实现如上述实施例所述的方法。

本发明的上述技术方案至少具有如下有益效果之一：

根据本发明实施例的室内动态环境下基于激光雷达的定位方法及计算机存储介质，通过对激光雷达数据的提取，很大程度上滤除了室内环境的动态信息，使其应用在室内动态环境下不会发生定位丢失现象，并且增加二次重采样环节，增强了定位在室内动态环境下的鲁棒性，保证了定位效果。

附图说明

图1为本发明实施例的室内动态环境下基于激光雷达的定位方法的流程图；

图2为本发明实施例的室内动态环境下基于激光雷达的定位方法中的评价函数曲线图；

图3本发明实施例的电子设备的示意图。

附图标记：

室内动态环境下基于激光雷达的定位方法100；

电子设备300；

存储器310；操作系统311；应用程序312；

处理器320；网络接口330；输入设备340；硬盘350；显示设备360。

具体实施方式

下面将结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

下面首先结合附图具体描述根据本发明实施例的室内动态环境下基于激光雷达的定位方法100。

如图1所示，根据本发明实施例的室内动态环境下基于激光雷达的定位方法100包括以下步骤：

S1、给定机器人初始位姿，并对机器人初始位姿进行采样生成多个初始粒子。

S2、根据运动模型更新粒子状态。

S3、采用激光雷达进行数据采样，获得点云数据。

S4、根据点云数据更新粒子权重。

S5、根据粒子权重对粒子进行重采样。

S6、更新机器人位姿。

S7、判断粒子集是否收敛，若未收敛，则跳转至步骤S2，若收敛，则进入步骤S8。

S8、对步骤S6更新后的机器人位姿进行二次重采样。

S9、跳转至步骤S2。

由此，根据本发明实施例的室内动态环境下基于激光雷达的定位方法，通过对基于粒子滤波的定位方法进行改进，使其适用于室内的动态环境。通过对激光雷达数据的提取，很大程度上滤除了室内环境的动态信息，使其应用在室内动态环境下不会发生定位丢失现象，并且增加二次重采样环节，增强了定位在室内动态环境下的鲁棒性，保证了定位效果。

具体地，如图1所示，根据本发明的一个实施例，步骤S1为初始化过程，在步骤S1中，给定机器人初始位姿x_t，基于高斯分布对x_t进行采样生成N个初始粒子，记为：

其中，粒子权重初始化为

在步骤S2中，根据运动模型更新粒子状态，记为

其中，u_t为运动控制量，v_t为噪声。

可选地，在本发明的一些实施例中，步骤S3包括：

S31、将所述激光雷达的数据按扫描角度分块，每20度为一块区域，最后不足以构成一块区域的激光雷达数据与前一组数据合并为一组。

S32、提取每块区域的激光雷达数据中距离最大的5个数据作为点云数据。

进一步地，在步骤S4中，根据步骤S3获得的点云数据，依据各个粒子的位姿，将点云数据映射到地图并计算点云与地图的匹配值。

点云匹配评价函数为：

其中，σ＝0.1，d为点云在地图中与障碍点最近的距离，评价函数曲线如图2所示。

记点云数据为m个，在地图中第j个点云离障碍点最近距离记为d_j，则第i个粒子的点云匹配值为：

如果所有的d_j＝0，则有s＝1.0。

在本申请中，取点云的匹配值为粒子的权重，则

归一化权重，则

在步骤S5中，根据粒子权重对粒子进行重采样，并重新设置权重为

接着，在步骤S6中，更新机器人位姿，记为：

t＝t+1。

根据本发明的一个实施例，在步骤S8中，基于高斯分布对机器人位姿进行二次重采样以更新粒子集，即X_t～N(x_t,Σ₁)，其中，X_t为更新后的粒子集，x_t为步骤S6更新后的机器人位姿，Σ₁为采样协方差。

根据更新后的粒子集，计算位姿x_t的点云匹配值，记为s1，若s1>0.8，则采样协方差为：

否则采样协方差为：

设置粒子权重为

总而言之，根据本发明实施例的室内动态环境下基于激光雷达的定位方法，通过对基于粒子滤波的定位方法进行改进，使其适用于室内的动态环境。通过对激光雷达数据的提取，很大程度上滤除了室内环境的动态信息，使其应用在室内动态环境下不会发生定位丢失现象，并且增加二次重采样环节，增强了定位在室内动态环境下的鲁棒性，保证了定位效果。

此外，本发明还提供一种计算机存储介质，所述计算机存储介质包括一条或多条计算机指令，所述一条或多条计算机指令在执行时实现上述任一所述的室内动态环境下基于激光雷达的定位方法100。

也就是说，所述计算机存储介质存储有计算机程序，所述计算机程序被处理器运行时，使得所述处理器执行上述任一所述的室内动态环境下基于激光雷达的定位方法100。

如图3所示，本发明实施例提供了一种电子设备300，包括存储器310和处理器320，所述存储器310用于存储一条或多条计算机指令，所述处理器320用于调用并执行所述一条或多条计算机指令，从而实现上述任一所述的方法。

也就是说，电子设备300包括：处理器320和存储器310，在所述存储器310中存储有计算机程序指令，其中，在所述计算机程序指令被所述处理器运行时，使得所述处理器320执行上述任一所述的方法100。

进一步地，如图3所示，电子设备300还包括网络接口330、输入设备340、硬盘350、和显示设备360。

上述各个接口和设备之间可以通过总线架构互连。总线架构可以是可以包括任意数量的互联的总线和桥。具体由处理器320代表的一个或者多个中央处理器(CPU)，以及由存储器310代表的一个或者多个存储器的各种电路连接在一起。总线架构还可以将诸如外围设备、稳压器和功率管理电路等之类的各种其它电路连接在一起。可以理解，总线架构用于实现这些组件之间的连接通信。总线架构除包括数据总线之外，还包括电源总线、控制总线和状态信号总线，这些都是本领域所公知的，因此本文不再对其进行详细描述。

所述网络接口330，可以连接至网络(如因特网、局域网等)，从网络中获取相关数据，并可以保存在硬盘350中。

所述输入设备340，可以接收操作人员输入的各种指令，并发送给处理器320以供执行。所述输入设备340可以包括键盘或者点击设备(例如，鼠标，轨迹球(trackball)、触感板或者触摸屏等。

所述显示设备360，可以将处理器320执行指令获得的结果进行显示。

所述存储器310，用于存储操作系统运行所必须的程序和数据，以及处理器320计算过程中的中间结果等数据。

可以理解，本发明实施例中的存储器310可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(ROM)、可编程只读存储器(PROM)、可擦除可编程只读存储器(EPROM)、电可擦除可编程只读存储器(EEPROM)或闪存。易失性存储器可以是随机存取存储器(RAM)，其用作外部高速缓存。本文描述的装置和方法的存储器310旨在包括但不限于这些和任意其它适合类型的存储器。

在一些实施方式中，存储器310存储了如下的元素，可执行模块或者数据结构，或者他们的子集，或者他们的扩展集：操作系统311和应用程序312。

其中，操作系统311，包含各种系统程序，例如框架层、核心库层、驱动层等，用于实现各种基础业务以及处理基于硬件的任务。应用程序312，包含各种应用程序，例如浏览器(Browser)等，用于实现各种应用业务。实现本发明实施例方法的程序可以包含在应用程序312中。

本发明上述实施例揭示的方法可以应用于处理器320中，或者由处理器320实现。处理器320可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器320中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器320可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件，可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器310，处理器320读取存储器310中的信息，结合其硬件完成上述方法的步骤。

可以理解的是，本文描述的这些实施例可以用硬件、软件、固件、中间件、微码或其组合来实现。对于硬件实现，处理单元可以实现在一个或多个专用集成电路(ASIC)、数字信号处理器DSP)、数字信号处理设备(DSPD)、可编程逻辑设备(PLD)、现场可编程门阵列(FPGA)、通用处理器、控制器、微控制器、微处理器、用于执行本申请所述功能的其它电子单元或其组合中。

对于软件实现，可通过执行本文所述功能的模块(例如过程、函数等)来实现本文所述的技术。软件代码可存储在存储器中并通过处理器执行。存储器可以在处理器中或在处理器外部实现。

具体地，处理器320还用于读取所述计算机程序，执行上述任一所述的方法。

在本申请所提供的几个实施例中，应该理解到，所揭露方法和装置，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理包括，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。

上述以软件功能单元的形式实现的集成的单元，可以存储在一个计算机可读取存储介质中。上述软件功能单元存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述收发方法的部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，简称ROM)、随机存取存储器(Random Access Memory，简称RAM)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以作出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种室内动态环境下基于激光雷达的定位方法，其特征在于，所述方法包括以下步骤：

S1、给定机器人初始位姿，并对机器人初始位姿进行采样生成多个初始粒子；

S2、根据运动模型更新粒子状态；

S3、采用激光雷达进行数据采样，获得点云数据；

S4、根据点云数据更新粒子权重；

S5、根据粒子权重对粒子进行重采样；

S6、更新机器人位姿；

S7、判断粒子集是否收敛，若未收敛，则跳转至步骤S2，若收敛，则进入步骤S8；

S8、对步骤S6更新后的机器人位姿进行二次重采样；

S9、跳转至步骤S2。

2.根据权利要求1所述的方法，其特征在于，在步骤S1中，给定机器人初始位姿x_t，基于高斯分布对x_t进行采样生成N个初始粒子，记为：

其中，粒子权重初始化为

3.根据权利要求2所述的方法，其特征在于，在步骤S2中，根据运动模型更新粒子状态，记为

其中，u_t为运动控制量，v_t为噪声。

4.根据权利要求3所述的方法，其特征在于，步骤S3包括：

S31、将所述激光雷达的数据按扫描角度分块，每20度为一块区域，最后不足以构成一块区域的激光雷达数据与前一组数据合并为一组；

S32、提取每块区域的激光雷达数据中距离最大的5个数据作为点云数据。

5.根据权利要求4所述的方法，其特征在于，在步骤S4中，根据步骤S3获得的点云数据，依据各个粒子的位姿，将点云数据映射到地图并计算点云与地图的匹配值，

点云匹配评价函数为：

其中，σ＝0.1，d为点云在地图中与障碍点最近的距离，

记点云数据为m个，在地图中第j个点云离障碍点最近距离记为d_j，则第i个粒子的点云匹配值为

取点云的匹配值为粒子的权重，则

归一化权重，则

6.根据权利要求5所述的方法，其特征在于，在步骤S5中，根据粒子权重对粒子进行重采样，并重新设置权重为

7.根据权利要求6所述的方法，其特征在于，在步骤S6中，更新机器人位姿，记为：

t＝t+1。

8.根据权利要求7所述的方法，其特征在于，在步骤S8中，基于高斯分布对机器人位姿进行二次重采样以更新粒子集，即X_t～N(x_t,Σ₁)，其中，X_t为更新后的粒子集，x_t为步骤S6更新后的机器人位姿，Σ₁为采样协方差。

9.根据权利要求8所述的方法，其特征在于，在步骤S8中，根据更新后的粒子集，计算位姿x_t的点云匹配值，记为s1，若s1>0.8，则采样协方差为：

否则采样协方差为：

设置粒子权重为

10.一种计算机存储介质，其特征在于，包括一条或多条计算机指令，所述一条或多条计算机指令在执行时实现如权利要求1-9中任一项所述的方法。

Images