CN106597435A - 实现基于粒子滤波slam算法的方法及系统 - Google Patents
实现基于粒子滤波slam算法的方法及系统 Download PDFInfo
- Publication number
- CN106597435A CN106597435A CN201611128122.4A CN201611128122A CN106597435A CN 106597435 A CN106597435 A CN 106597435A CN 201611128122 A CN201611128122 A CN 201611128122A CN 106597435 A CN106597435 A CN 106597435A
- Authority
- CN
- China
- Prior art keywords
- conditioned
- data set
- predecessor
- original
- data
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/88—Radar or analogous systems specially adapted for specific applications
- G01S13/89—Radar or analogous systems specially adapted for specific applications for mapping or imaging
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Traffic Control Systems (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明提供了实现基于粒子滤波的SLAM算法的方法及系统，涉及数字图象处理技术领域，包括：获取激光雷达数据和里程计数据，根据原始地图的多个原始位姿信息，对里程计数据进行运动估计，同时得到多个第一位姿信息，根据原始地图，对多个第一位姿信息同时进行匹配优化，得到多个第二位姿信息，并根据多个第二位姿信息与激光雷达数据计算得到第一权重值，根据第一权重值对原始地图进行粒子滤波，得到多个更新粒子数据组，根据多个更新粒子数据组，对原始地图进行更新，生成更新地图。本发明可以降低成本和提高数据处理效率。
Description
技术领域
本发明涉及数字图象处理技术领域，尤其是涉及实现基于粒子滤波SLAM算法的方法及系统。
背景技术
基于粒子滤波的SLAM(Simultaneous Localization And Mapping，即时定位与地图构建)是一种强调实时性与准确性的算法，其在匹配优化等方面运算量很大，现行的基于软件的SLAM方案对硬件配置的要求很高。现行的SLAM方案一般只能顺序执行。例如，对于一个多粒子的SLAM算法来说，软件的处理方式就要依据粒子数量进行同等数量的循环处理。而一次的处理就需要经过运动位姿的匹配优化、将点插入地图，权重的计算等一系列处理，而循环多次其处理时间将大大增加。如果使用高性能的CPU当然可以解决这个问题，但是硬件成本也一样提高了。对于一个强调实时性的多传感器系统来说，处理时间十分重要。
发明内容
有鉴于此，本发明的目的在于提供实现基于粒子滤波SLAM算法的方法及系统，以降低成本和提高数据处理效率。
第一方面，本发明实施例提供了一种实现基于粒子滤波SLAM算法的方法，包括：
获取激光雷达数据和里程计数据；
根据原始地图的多个原始位姿信息，对所述里程计数据进行运动估计，同时得到多个第一位姿信息；
根据所述原始地图，对所述多个第一位姿信息同时进行匹配优化，得到多个第二位姿信息，并根据所述多个第二位姿信息与所述激光雷达数据计算得到第一权重值；
根据所述第一权重值对所述原始地图进行粒子滤波，得到多个更新粒子数据组；
根据所述多个更新粒子数据组，对所述原始地图进行更新，生成更新地图。
结合第一方面，本发明实施例提供了第一方面的第一种可能的实施方式，其中，所述原始地图包括多个原始粒子数据组，所述根据所述第一权重值对所述原始地图进行粒子滤波，得到多个更新粒子数据组包括：
根据所述第一权重值设置第一阈值和第二阈值；
判断每个所述原始粒子数据组是否满足预设条件，其中，预设条件包括第一预设条件、第二预设条件或第三预设条件；
如果每个所述原始粒子数据组满足所述第一预设条件，则将满足所述第一预设条件的原始粒子数据组进行淘汰，其中，所述第一预设条件为每个所述原始粒子数据组对应的第二权重值小于所述第一阈值；
如果每个所述原始粒子数据组满足所述第二预设条件，则将满足所述第二预设条件的原始粒子数据组按照预设的次数进行复制，其中，所述第二预设条件为每个所述原始粒子数据组对应的第二权重值大于所述第二阈值；
如果每个所述原始粒子数据组满足所述第三预设条件，则将满足所述第三预设条件的原始粒子数据组进行保留，其中，所述第三预设条件为每个所述原始粒子数据组对应的第二权重值位于所述第一阈值与第二阈值之间；
将复制的所述原始粒子数据组和保留的所述原始粒子数据组构成所述多个更新粒子数据组。
结合第一方面的第一种可能的实施方式，本发明实施例提供了第一方面的第二种可能的实施方式，其中，所述多个原始粒子数据组包括所述多个原始位姿信息和原始激光雷达数据。
结合第一方面的第一种可能的实施方式，本发明实施例提供了第一方面的第三种可能的实施方式，其中，所述预设次数是根据淘汰的原始粒子数据组的个数进行设置。
结合第一方面，本发明实施例提供了第一方面的第四种可能的实施方式，其中，所述方法还包括：
如果不存在所述原始地图，则直接根据所述激光雷达数据和所述里程计数据生成地图。
第二方面，本发明实施例还提供一种实现基于粒子滤波SLAM算法的系统，包括：数据接收模块、运动估计模块、匹配优化模块、粒子滤波模块和更新地图模块；
所述数据接收模块，用于获取激光雷达数据和里程计数据，并将所述里程计数据发送给所述运动估计模块，将所述激光雷达数据发送给所述匹配优化模块；
所述运动估计模块，用于根据原始地图的多个原始位姿信息，对所述里程计数据进行运动估计，同时得到多个第一位姿信息；
所述匹配优化模块，用于根据所述原始地图，对所述多个第一位姿信息同时进行匹配优化，得到多个第二位姿信息，并根据所述多个第二位姿信息与所述激光雷达数据计算得到第一权重值；
所述粒子滤波模块，用于根据所述第一权重值对所述原始地图进行粒子滤波，得到多个更新粒子数据组；
所述更新地图模块，用于根据所述多个更新粒子数据组，对所述原始地图进行更新，生成更新地图。
结合第二方面，本发明实施例提供了第二方面的第一种可能的实施方式，其中，所述原始地图包括多个原始粒子数据组，所述粒子滤波模块包括：
设置单元，用于根据所述第一权重值设置第一阈值和第二阈值；
判断单元，用于判断每个所述原始粒子数据组是否满足预设条件，其中，预设条件包括第一预设条件、第二预设条件或第三预设条件；
处理单元，用于在每个所述原始粒子数据组满足所述第一预设条件的情况下，则将满足所述第一预设条件的原始粒子数据组进行淘汰，其中，所述第一预设条件为每个所述原始粒子数据组对应的第二权重值小于所述第一阈值，在每个所述原始粒子数据组满足所述第二预设条件的情况下，将满足所述第二预设条件的原始粒子数据组按照预设的次数进行复制，其中，所述第二预设条件为每个所述原始粒子数据组对应的第二权重值大于所述第二阈值，在每个所述原始粒子数据组满足所述第三预设条件的情况下，将满足所述第三预设条件的原始粒子数据组进行保留，其中，所述第三预设条件为每个所述原始粒子数据组对应的第二权重值位于所述第一阈值与第二阈值之间；
组合单元，用于将复制的所述原始粒子数据组和保留的所述原始粒子数据组构成所述多个更新粒子数据组。
结合第二方面的第一种可能的实施方式，本发明实施例提供了第二方面的第二种可能的实施方式，其中，所述多个原始粒子数据组包括所述多个原始位姿信息和原始激光雷达数据。
结合第二方面的第一种可能的实施方式，本发明实施例提供了第二方面的第三种可能的实施方式，其中，所述预设次数是根据淘汰的原始粒子数据组的个数进行设置。
结合第二方面，本发明实施例提供了第二方面的第四种可能的实施方式，其中，所述更新地图模块，还用于在不存在所述原始地图的情况下，直接根据所述激光雷达数据和所述里程计数据生成地图。
本发明提供的实现基于粒子滤波SLAM算法的方法及系统，通过获取激光雷达数据和里程计数据，根据原始地图的多个原始位姿信息，对里程计数据进行运动估计，同时得到多个第一位姿信息，根据原始地图，对多个第一位姿信息同时进行匹配优化，得到多个第二位姿信息，并根据多个第二位姿信息与激光雷达数据计算得到第一权重值，根据第一权重值对原始地图进行粒子滤波，得到多个更新粒子数据组，根据多个更新粒子数据组，对原始地图进行更新，生成更新地图。本发明可以降低成本和提高数据处理效率。
本发明的其他特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。
为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
附图说明
为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例一提供的实现基于粒子滤波SLAM算法的方法流程图；
图2为本发明实施例一提供的实现基于粒子滤波SLAM算法的方法中步骤S140的流程图；
图3为本发明实施例二提供的实现基于粒子滤波SLAM算法的系统示意图；
图4为本发明实施例二提供的粒子滤波模块的结构示意图。
图标：
100-数据接收模块；200-运动估计模块；300-匹配优化模块；400-粒子滤波模块；500-更新地图模块；410-设置单元；420-判断单元；430-处理单元；440-组合单元。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
目前基于粒子滤波的SLAM是一种强调实时性与准确性的算法，其在匹配优化等方面运算量很大，现行的基于软件的SLAM方案对硬件配置的要求很高。现行的SLAM方案一般使用C++实现，软件实现的一个特点就是只能顺序执行，处理时间较长。使用高性能的CPU却提高了硬件成本。对于一个强调实时性的多传感器系统来说，处理时间十分重要。同时，粒子滤波的特点使得对于每个粒子的运算使用并行处理的方式，可以极大的提高算法处理能力，对于软件实现的方案来说，很难做到。FPGA是一种可编程的芯片，它具有并行处理的特点，正好契合SLAM的实时性的需求。
基于此，本发明实施例提供的实现基于粒子滤波SLAM算法的方法及系统，可以降低成本和提高数据处理效率。
为便于对本实施例进行理解，首先对本发明实施例所公开的实现基于粒子滤波SLAM算法的方法进行详细介绍。
实施例一：
图1为本发明实施例一提供的实现基于粒子滤波SLAM算法的方法流程图。
参照图1，实现基于粒子滤波SLAM算法的方法包括：
步骤S110，获取激光雷达数据和里程计数据；
具体地，通过数据接收模块来获取激光雷达数据和里程计数据。由于激光雷达数据和里程计数据有各自的协议，所以需要对激光雷达数据和里程计数据进行数据格式和数据进制的处理。
步骤S120，根据原始地图的多个原始位姿信息，对里程计数据进行运动估计，同时得到多个第一位姿信息；
具体地，里程计数据就是一个位姿信息，原始地图中包含有多个原始位姿信息。假设原始位姿信息为100个，那么相应的，运动估计模块根据原始地图的100个原始位姿信息，对里程计数据进行运动估计，同时得到100个第一位姿信息，由于本发明采用了FPGA，它具有并行处理的特点，契合SLAM的实时性的需求，所以该过程是通过并行处理方式进行的。
步骤S130，根据原始地图，对多个第一位姿信息同时进行匹配优化，得到多个第二位姿信息，并根据多个第二位姿信息与激光雷达数据计算得到第一权重值；
具体的，根据上述假设的数值为100，本发明后面继续以数值100为例进行描述说明。原始地图是根据原始激光雷达数据和多个原始位姿信息生成的，多个原始位姿信息即为100个原始位姿信息。匹配优化模块接收激光雷达数据和100个第一位姿信息，使用FPGA通过并行处理的方式，根据原始地图，对100个第一位姿信息同时进行匹配优化，即修正，得到100个第二位姿信息；根据激光雷达数据和100个第二位姿信息计算得到第一权重值。
步骤S140，根据第一权重值对原始地图进行粒子滤波，得到多个更新粒子数据组；
具体的，粒子滤波模块，也是以并行处理的方式，根据第一权重值对原始地图进行粒子滤波，即滤去低权重值粒子、保留中权重值粒子、复制高权重值粒子，得到多个更新粒子数据组，可以为100个更新粒子数据组。
步骤S150，根据多个更新粒子数据组，对原始地图进行更新，生成更新地图。
具体地，更新地图模块，也是以并行处理的方式，根据多个更新粒子数据组，可以为100个更新粒子数据组，对原始地图进行更新，即将100个更新粒子数据组写入原始地图完成更新，生成更新地图。
另外，如果不存在原始地图，则直接根据激光雷达数据和里程计数据生成地图。
根据本发明实施例，原始地图包括多个原始粒子数据组，如图2所示，实施例一实现基于粒子滤波SLAM算法的方法中，步骤S140可采用如下步骤实现，包括：
步骤S210，根据第一权重值设置第一阈值和第二阈值；
步骤S220，判断每个原始粒子数据组是否满足预设条件，其中，预设条件包括第一预设条件、第二预设条件或第三预设条件，如果满足第一预设条件，则执行步骤S231，如果满足第二预设条件，则执行步骤S232，如果满足第三预设条件，则执行步骤S233；
步骤S231，将满足第一预设条件的原始粒子数据组进行淘汰，其中，第一预设条件为每个原始粒子数据组对应的第二权重值小于第一阈值；
步骤S232，将满足第二预设条件的原始粒子数据组按照预设的次数进行复制，其中，第二预设条件为每个原始粒子数据组对应的第二权重值大于第二阈值；
步骤S233，将满足第三预设条件的原始粒子数据组进行保留，其中，第三预设条件为每个原始粒子数据组对应的第二权重值位于第一阈值与第二阈值之间；
步骤S240，将复制的原始粒子数据组和保留的原始粒子数据组构成多个更新粒子数据组。
具体地，原始粒子数据组包括原始激光雷达数据和多个原始位姿信息，根据原始激光雷达数据和多个原始位姿信息计算得到原始粒子数据组的第二权重值。为了保证原始地图中粒子数量的不变，复制原始粒子数据组时的预设次数与淘汰的原始粒子数据组的数量相同。由于更新粒子数据组是原始粒子数据组经过淘汰、保留和复制之后得到的，所以更新粒子数据组包括原始激光雷达数据和原始位姿信息。
本发明提供的实现基于粒子滤波SLAM算法的方法，通过获取激光雷达数据和里程计数据，根据原始地图的多个原始位姿信息，对里程计数据进行运动估计，同时得到多个第一位姿信息，根据原始地图，对多个第一位姿信息同时进行匹配优化，得到多个第二位姿信息，并根据多个第二位姿信息与激光雷达数据计算得到第一权重值，根据第一权重值对原始地图进行粒子滤波，得到多个更新粒子数据组，根据多个更新粒子数据组，对原始地图进行更新，生成更新地图。本发明可以降低成本和提高数据处理效率。
实施例二：
图3为本发明实施例二提供的实现基于粒子滤波SLAM算法的系统示意图。
参照图3，实现基于粒子滤波SLAM算法的系统包括：数据接收模块100、运动估计模块200、匹配优化模块300、粒子滤波模块400和更新地图模块500；
具体地，数据接收模块100、运动估计模块200、匹配优化模块300、粒子滤波模块400和更新地图模块500依次连接。
数据接收模块100，用于获取激光雷达数据和里程计数据，并将里程计数据发送给运动估计模块200，将激光雷达数据发送给匹配优化模块300；
运动估计模块200，用于根据原始地图的多个原始位姿信息，对里程计数据进行运动估计，同时得到多个第一位姿信息；
具体的，将多个第一位姿信息发送给匹配优化模块300；
匹配优化模块300，用于根据原始地图，对多个第一位姿信息同时进行匹配优化，得到多个第二位姿信息，并根据多个第二位姿信息与激光雷达数据计算得到第一权重值；
具体的，将第一权重值发送给粒子滤波模块400；
粒子滤波模块400，用于根据第一权重值对原始地图进行粒子滤波，得到多个更新粒子数据组；
具体的，将多个更新粒子数据组发送给更新地图模块500；
更新地图模块500，用于根据多个更新粒子数据组，对原始地图进行更新，生成更新地图。
根据本发明实施例，原始地图包括多个原始粒子数据组，如图4所示，粒子滤波模块400包括设置单元410、判断单元420、处理单元430和组合单元440；
设置单元410，用于根据第一权重值设置第一阈值和第二阈值；
判断单元420，用于判断每个原始粒子数据组是否满足预设条件，其中，预设条件包括第一预设条件、第二预设条件或第三预设条件；
处理单元430，用于在每个原始粒子数据组满足第一预设条件的情况下，则将满足第一预设条件的原始粒子数据组进行淘汰，其中，第一预设条件为每个原始粒子数据组对应的第二权重值小于第一阈值，在每个原始粒子数据组满足第二预设条件的情况下，将满足第二预设条件的原始粒子数据组按照预设的次数进行复制，其中，第二预设条件为每个原始粒子数据组对应的第二权重值大于第二阈值，在每个原始粒子数据组满足第三预设条件的情况下，将满足第三预设条件的原始粒子数据组进行保留，其中，第三预设条件为每个原始粒子数据组对应的第二权重值位于第一阈值与第二阈值之间；
组合单元440，用于将复制的原始粒子数据组和保留的原始粒子数据组构成多个更新粒子数据组。
根据本发明实施例，多个原始粒子数据组包括多个原始位姿信息和原始激光雷达数据。
根据本发明实施例，预设次数是根据淘汰的原始粒子数据组的个数进行设置。
根据本发明实施例，更新地图模块500，还用于在不存在原始地图的情况下，直接根据激光雷达数据和里程计数据生成地图。
本发明提供的实现基于粒子滤波SLAM算法的系统，包括数据接收模块、运动估计模块、匹配优化模块、粒子滤波模块和更新地图模块，通过获取激光雷达数据和里程计数据，根据原始地图的多个原始位姿信息，对里程计数据进行运动估计，同时得到多个第一位姿信息，根据原始地图，对多个第一位姿信息同时进行匹配优化，得到多个第二位姿信息，并根据多个第二位姿信息与激光雷达数据计算得到第一权重值，根据第一权重值对原始地图进行粒子滤波，得到多个更新粒子数据组，根据多个更新粒子数据组，对原始地图进行更新，生成更新地图。本发明可以降低成本和提高数据处理效率。
本发明实施例所提供的实现基于粒子滤波SLAM算法的方法及系统的计算机程序产品，包括存储了程序代码的计算机可读存储介质，所述程序代码包括的指令可用于执行前面方法实施例中所述的方法，具体实现可参见方法实施例，在此不再赘述。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统和装置的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
另外，在本发明实施例的描述中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。
最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。
Claims (10)
1.一种实现基于粒子滤波SLAM算法的方法，其特征在于，包括：
获取激光雷达数据和里程计数据；
根据原始地图的多个原始位姿信息，对所述里程计数据进行运动估计，同时得到多个第一位姿信息；
根据所述原始地图，对所述多个第一位姿信息同时进行匹配优化，得到多个第二位姿信息，并根据所述多个第二位姿信息与所述激光雷达数据计算得到第一权重值；
根据所述第一权重值对所述原始地图进行粒子滤波，得到多个更新粒子数据组；
根据所述多个更新粒子数据组，对所述原始地图进行更新，生成更新地图。
2.根据权利要求1所述的实现基于粒子滤波SLAM算法的方法，其特征在于，所述原始地图包括多个原始粒子数据组，所述根据所述第一权重值对所述原始地图进行粒子滤波，得到多个更新粒子数据组包括：
根据所述第一权重值设置第一阈值和第二阈值；
判断每个所述原始粒子数据组是否满足预设条件，其中，预设条件包括第一预设条件、第二预设条件或第三预设条件；
如果每个所述原始粒子数据组满足所述第一预设条件，则将满足所述第一预设条件的原始粒子数据组进行淘汰，其中，所述第一预设条件为每个所述原始粒子数据组对应的第二权重值小于所述第一阈值；
如果每个所述原始粒子数据组满足所述第二预设条件，则将满足所述第二预设条件的原始粒子数据组按照预设的次数进行复制，其中，所述第二预设条件为每个所述原始粒子数据组对应的第二权重值大于所述第二阈值；
如果每个所述原始粒子数据组满足所述第三预设条件，则将满足所述第三预设条件的原始粒子数据组进行保留，其中，所述第三预设条件为每个所述原始粒子数据组对应的第二权重值位于所述第一阈值与第二阈值之间；
将复制的所述原始粒子数据组和保留的所述原始粒子数据组构成所述多个更新粒子数据组。
3.根据权利要求2所述的实现基于粒子滤波SLAM算法的方法，其特征在于，所述多个原始粒子数据组包括所述多个原始位姿信息和原始激光雷达数据。
4.根据权利要求2所述的实现基于粒子滤波SLAM算法的方法，其特征在于，所述预设次数是根据淘汰的原始粒子数据组的个数进行设置。
5.根据权利要求1所述的实现基于粒子滤波SLAM算法的方法，其特征在于，所述方法还包括：
如果不存在所述原始地图，则直接根据所述激光雷达数据和所述里程计数据生成地图。
6.一种实现基于粒子滤波SLAM算法的系统，其特征在于，包括：数据接收模块、运动估计模块、匹配优化模块、粒子滤波模块和更新地图模块；
所述数据接收模块，用于获取激光雷达数据和里程计数据，并将所述里程计数据发送给所述运动估计模块，将所述激光雷达数据发送给所述匹配优化模块；
所述运动估计模块，用于根据原始地图的多个原始位姿信息，对所述里程计数据进行运动估计，同时得到多个第一位姿信息；
所述匹配优化模块，用于根据所述原始地图，对所述多个第一位姿信息同时进行匹配优化，得到多个第二位姿信息，并根据所述多个第二位姿信息与所述激光雷达数据计算得到第一权重值；
所述粒子滤波模块，用于根据所述第一权重值对所述原始地图进行粒子滤波，得到多个更新粒子数据组；
所述更新地图模块，用于根据所述多个更新粒子数据组，对所述原始地图进行更新，生成更新地图。
7.根据权利要求6所述的实现基于粒子滤波SLAM算法的系统，其特征在于，所述原始地图包括多个原始粒子数据组，所述粒子滤波模块包括：
设置单元，用于根据所述第一权重值设置第一阈值和第二阈值；
判断单元，用于判断每个所述原始粒子数据组是否满足预设条件，其中，预设条件包括第一预设条件、第二预设条件或第三预设条件；
处理单元，用于在每个所述原始粒子数据组满足所述第一预设条件的情况下，则将满足所述第一预设条件的原始粒子数据组进行淘汰，其中，所述第一预设条件为每个所述原始粒子数据组对应的第二权重值小于所述第一阈值，在每个所述原始粒子数据组满足所述第二预设条件的情况下，将满足所述第二预设条件的原始粒子数据组按照预设的次数进行复制，其中，所述第二预设条件为每个所述原始粒子数据组对应的第二权重值大于所述第二阈值，在每个所述原始粒子数据组满足所述第三预设条件的情况下，将满足所述第三预设条件的原始粒子数据组进行保留，其中，所述第三预设条件为每个所述原始粒子数据组对应的第二权重值位于所述第一阈值与第二阈值之间；
组合单元，用于将复制的所述原始粒子数据组和保留的所述原始粒子数据组构成所述多个更新粒子数据组。
8.根据权利要求7所述的实现基于粒子滤波SLAM算法的系统，其特征在于，所述多个原始粒子数据组包括所述多个原始位姿信息和原始激光雷达数据。
9.根据权利要求7所述的实现基于粒子滤波SLAM算法的系统，其特征在于，所述预设次数是根据淘汰的原始粒子数据组的个数进行设置。
10.根据权利要求6所述的实现基于粒子滤波SLAM算法的系统，其特征在于，所述更新地图模块，还用于在不存在所述原始地图的情况下，直接根据所述激光雷达数据和所述里程计数据生成地图。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611128122.4A CN106597435A (zh) | 2016-12-08 | 2016-12-08 | 实现基于粒子滤波slam算法的方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611128122.4A CN106597435A (zh) | 2016-12-08 | 2016-12-08 | 实现基于粒子滤波slam算法的方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN106597435A true CN106597435A (zh) | 2017-04-26 |
Family
ID=58598392
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201611128122.4A Pending CN106597435A (zh) | 2016-12-08 | 2016-12-08 | 实现基于粒子滤波slam算法的方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106597435A (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107807651A (zh) * | 2017-11-29 | 2018-03-16 | 北京康力优蓝机器人科技有限公司 | 一种移动机器人的自充电控制系统和方法 |
| CN108648214A (zh) * | 2018-06-14 | 2018-10-12 | 广东工业大学 | 一种基于宽度学习滤波的视觉slam算法及系统 |
| CN112965076A (zh) * | 2021-01-28 | 2021-06-15 | 上海思岚科技有限公司 | 一种用于机器人的多雷达定位系统及方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101183266A (zh) * | 2006-11-16 | 2008-05-21 | 三星电子株式会社 | 使用粒子滤波器估算移动机器人姿态的方法、设备和介质 |
| CN103644903A (zh) * | 2013-09-17 | 2014-03-19 | 北京工业大学 | 基于分布式边缘无味粒子滤波的同步定位与地图构建方法 |
| CN103856185A (zh) * | 2014-02-17 | 2014-06-11 | 上海大学 | 一种基于fpga的粒子滤波权值处理及重采样方法 |
| CN104236551A (zh) * | 2014-09-28 | 2014-12-24 | 北京信息科技大学 | 一种蛇形机器人基于激光测距仪的地图创建方法 |
| CN105865449A (zh) * | 2016-04-01 | 2016-08-17 | 深圳杉川科技有限公司 | 基于激光和视觉的移动机器人的混合定位方法 |
-
2016
- 2016-12-08 CN CN201611128122.4A patent/CN106597435A/zh active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101183266A (zh) * | 2006-11-16 | 2008-05-21 | 三星电子株式会社 | 使用粒子滤波器估算移动机器人姿态的方法、设备和介质 |
| CN103644903A (zh) * | 2013-09-17 | 2014-03-19 | 北京工业大学 | 基于分布式边缘无味粒子滤波的同步定位与地图构建方法 |
| CN103856185A (zh) * | 2014-02-17 | 2014-06-11 | 上海大学 | 一种基于fpga的粒子滤波权值处理及重采样方法 |
| CN104236551A (zh) * | 2014-09-28 | 2014-12-24 | 北京信息科技大学 | 一种蛇形机器人基于激光测距仪的地图创建方法 |
| CN105865449A (zh) * | 2016-04-01 | 2016-08-17 | 深圳杉川科技有限公司 | 基于激光和视觉的移动机器人的混合定位方法 |
Non-Patent Citations (1)
| Title |
|---|
| 朱福利: ""基于SLAM的移动机器人室内环境感知和地图构建研究"", 《中国优秀硕士学位论文全文数据库信息科技辑》 * |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107807651A (zh) * | 2017-11-29 | 2018-03-16 | 北京康力优蓝机器人科技有限公司 | 一种移动机器人的自充电控制系统和方法 |
| CN108648214A (zh) * | 2018-06-14 | 2018-10-12 | 广东工业大学 | 一种基于宽度学习滤波的视觉slam算法及系统 |
| CN108648214B (zh) * | 2018-06-14 | 2022-04-29 | 广东工业大学 | 一种基于宽度学习滤波的视觉slam算法及系统 |
| CN112965076A (zh) * | 2021-01-28 | 2021-06-15 | 上海思岚科技有限公司 | 一种用于机器人的多雷达定位系统及方法 |
| CN112965076B (zh) * | 2021-01-28 | 2024-05-24 | 上海思岚科技有限公司 | 一种用于机器人的多雷达定位系统及方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110942154B (zh) | 基于联邦学习的数据处理方法、装置、设备及存储介质 | |
| CN104779680B (zh) | 电动汽车预约充电控制方法及装置 | |
| CN108090208A (zh) | 融合数据处理方法及装置 | |
| EP2750087A1 (en) | Methods and systems for determining estimated package delivery/pick-up times | |
| CN109144696A (zh) | 一种任务调度方法、装置、电子设备及存储介质 | |
| CN106597435A (zh) | 实现基于粒子滤波slam算法的方法及系统 | |
| CN111723947A (zh) | 一种联邦学习模型的训练方法及装置 | |
| CN107451692B (zh) | 一种基于人工蜂群算法的航空备件配置优化方法 | |
| CN106803197A (zh) | 拼单方法和设备 | |
| CN107545315A (zh) | 订单处理方法及装置 | |
| CN104077309A (zh) | 一种对时序序列进行降维处理的方法和装置 | |
| CN110378942A (zh) | 基于双目相机的障碍物标识方法、系统、设备和存储介质 | |
| CN107491674A (zh) | 基于特征信息进行用户验证的方法及装置 | |
| CN109976901A (zh) | 一种资源调度方法、装置、服务器及可读存储介质 | |
| CN109978619A (zh) | 机票定价策略筛选的方法、系统、设备以及介质 | |
| CN103164529A (zh) | 一种基于Voronoi图的反k最近邻查询方法 | |
| CN105550772A (zh) | 一种在线历史数据潮流趋势分析方法 | |
| CN110929885A (zh) | 一种面向智慧校园的分布式机器学习模型参数聚合方法 | |
| CN104506636B (zh) | 一种数据同步方法及装置 | |
| CN109102303A (zh) | 风险检测方法和相关装置 | |
| CN112860996A (zh) | 一种兴趣点处理方法、装置、电子设备和介质 | |
| CN107679635A (zh) | 基于优先级的客户跟进方法、系统及可读存储介质 | |
| CN111860554A (zh) | 风险监控方法、装置、存储介质及电子设备 | |
| CN104765776A (zh) | 一种数据样本的聚类方法和装置 | |
| CN111309774A (zh) | 数据处理方法、装置、电子设备及存储介质 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20170426 |
|
| RJ01 | Rejection of invention patent application after publication |