CN108871341A - 一种全局优化的并发定位与建图方法 - Google Patents

一种全局优化的并发定位与建图方法 Download PDF

Info

Publication number
CN108871341A
CN108871341A CN201810734733.6A CN201810734733A CN108871341A CN 108871341 A CN108871341 A CN 108871341A CN 201810734733 A CN201810734733 A CN 201810734733A CN 108871341 A CN108871341 A CN 108871341A
Authority
CN
China
Prior art keywords
particle
current time
road sign
weight
moment
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.)
Granted
Application number
CN201810734733.6A
Other languages
English (en)
Other versions
CN108871341B (zh
Inventor
罗光春
段贵多
刘贵松
卢国明
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Neijiang Next Generation Internet Data Processing Technology Research Institute
University of Electronic Science and Technology of China
Original Assignee
Neijiang Next Generation Internet Data Processing Technology Research Institute
University of Electronic Science and Technology of China
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Neijiang Next Generation Internet Data Processing Technology Research Institute, University of Electronic Science and Technology of China filed Critical Neijiang Next Generation Internet Data Processing Technology Research Institute
Priority to CN201810734733.6A priority Critical patent/CN108871341B/zh
Publication of CN108871341A publication Critical patent/CN108871341A/zh
Application granted granted Critical
Publication of CN108871341B publication Critical patent/CN108871341B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/4808—Evaluating distance, position or velocity data
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0272—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising means for registering the travel distance, e.g. revolutions of wheels

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种全局优化的并发定位与建图方法；首先根据机器人的初始坐标，在初始时刻随机生成n个粒子，得到初始时刻的粒子集；采用粒子滤波的SLAM算法，利用上一时刻粒子集中各粒子的粒子信息更新当前时刻各粒子的粒子信息；采用全局优化和重采样对当前时刻各粒子进行更新；利用更新后的当前时刻粒子的粒子信息获得当前时刻机器人运动路径和路标点；重复以上过程，直至机器人完成所有时刻的运动，采用本方法，能够提高机器人的定位精度。

Description

一种全局优化的并发定位与建图方法
技术领域
本发明涉及机器人导航领域，具体涉及一种全局优化的并发定位与建图方法。
背景技术
随着计算机和人工智能等学科的发展，移动机器人相关技术得到了突飞猛进的发展，是目前科学技术最前沿的一个领域。移动机器人应用场景非常广阔，主要包括卫生清洁，医疗辅助，安保消防等领域。而为了完成这些任务，就需要移动机器人具有感知周围环境信息和自身状态的能力。在已知环境下，机器人可以通过对路标点的感知，纠正自身的位姿信息。然而在周围环境信息未知时，移动机器人需要在没有先验信息的情况下，在自身运动的过程中一边建立周围环境的模型即建图，一边估计自身的运动状态即定位。在这个过程中机器人的建图和定位的精度是相互影响的，这就引出了机器人领域的热门研究同时定位与建图问题(SimultaneousLocalization And Mapping，SLAM)。
SLAM研究的是机器人在陌生环境下通过本体携带的传感器构建地图，同时确定自己所在地图的位置的问题。SLAM系统主要有三个组成部分，并且是一个迭代的过程。首先通过当前机器人的位姿信息和运动方程估计下一时刻机器人的位姿信息。然后通过相机或者激光雷达等传感器获取路标信息，并且把路标信息和机器人的位姿信息相关联。等到下一时刻获取到了新的路标信息和机器人位姿信息之后，对于第一部分的信息进行更新。
通过贝叶斯滤波器解决SLAM问题的方法主要有两种。一种是基于扩展卡尔曼滤波(Extended Kalman Filter，EKF)的SLAM算法。一种是基于粒子滤波(Particle Filter，PF)的SLAM算法。EKF的方法解决了卡尔曼滤波(Kalman Filter，KF)仅限于处理线性系统的问题，将之扩展到了非线性系统问题的应用当中。EKF需要保证噪声服从高斯分布，而粒子滤波(Particle Filter，PF)是非线性非高斯的，适用性更好。粒子滤波的方法是在机器人的初始位置周围按照某种先验分布随机产生一定数目的粒子，然后根据状态转移方程更新粒子的位姿信息。通过观测模型获取的路标点评估当前粒子的优劣，计算权重根据权值进行重采样，然后进行下一时刻的估计。但是为了提高粒子滤波的效果不得不增加粒子的数目，从而造成计算量的增大。
发明内容
本发明的目的在于：提供一种全局优化的并发定位与建图方法，解决了目前采用粒子滤波的SLAM算法实现机器人的定位与建图时定位精度低的技术问题。
本发明采用的技术方案如下：
一种全局优化的并发定位与建图方法，包括以下步骤：
步骤1：根据机器人的初始坐标，在初始时刻随机生成n个粒子，得到初始时刻的粒子集；
步骤2：采用粒子滤波的SLAM算法，利用上一时刻粒子集中各粒子的粒子信息更新当前时刻各粒子的粒子信息；
步骤3：采用全局优化和重采样对当前时刻各粒子进行更新；
步骤4：利用更新后的当前时刻粒子的粒子信息获得当前时刻机器人运动路径和路标点；
步骤5：重复步骤2-4，直至机器人完成所有时刻的运动。
进一步的，所述步骤2具体为：
步骤21：采用机器人运动模型，利用上一时刻各粒子的粒子信息中的位姿数据得到当前时刻各粒子的位姿数据，并记录当前时刻粒子观测到的路标点；
步骤22：采用提议分布对当前时刻的位姿数据和路标点位置进行更新，利用更新后的位姿数据和路标点位置计算各粒子的权重；
步骤23：对所述权重进行归一化处理，得到当前时刻各粒子的权重。
进一步的，所述步骤3具体步骤为：
步骤31：对当前时刻的各粒子进行克隆和变异操作，更新粒子集中的粒子；
步骤32：以更新后粒子集中的各个粒子为引领者随机产生N个跟随者，计算跟随者和引领者的可能性，利用可能性筛选所述引领者和跟随者，得到再次更新后的粒子集；
步骤33：利用再次更新后粒子集中粒子的权重计算有效粒子的数目Neff，当有效粒子数目Neff大于设定的阈值Φ，则不进行重采样，否则进行重采样，得到当前时刻最终的粒子集。
进一步的，所述步骤31具体为：
步骤311：利用步骤2得到的粒子信息中的权重计算当前时刻各粒子的亲和力faffinity，公式为：
其中i表示粒子的序号，t表示时刻；
步骤312：利用所述亲和力faffinity计算当前时刻粒子需要克隆的数目Ncl，计算公式如下：
其中，M表示预先设定的每一时刻粒子的总数，round表示取整函数；
步骤313：将新获得的粒子添加至所述粒子集中，更新当前时刻的粒子集，利用所述亲和力faffinity对粒子进行变异操作，变异采用的公式为：
其中表示第i个粒子的粒子信息中的位姿数据，random表示一个服从(0，1)分布的随机数；
步骤314：利用变异后粒子间的位姿差计算两两粒子间的排斥力freject，计算公式为：
其中，表示t时刻第i个粒子的位置，表示t时刻第j个粒子的位置；
步骤315：当粒子总数大于M时，将排斥力小于设定阈值的两个粒子中的一个抛弃，更新所述粒子集合Ω，其中权重更新公式为：
Zt表示t时刻粒子观测到的路标点位置，p表示权重更新中采用的权重值。
进一步的，所述步骤32具体为：
步骤321：对步骤21更新后粒子集中的每个粒子，以各个粒子为引领者随机产生对应的N个跟随者，分别计算引领者与跟随者的适应度ffitness，计算适应度采用的函数为：
其中，Rt表示t时刻观测过程的协方差矩阵，表示当前时刻粒子观测到的路标位置，表示上一时刻粒子观测到的路标位置；
步骤322：利用所述适应度ffitness计算各个粒子的可能性pi，采用的公式为：
步骤323：判断引领者和跟随者的可能性pi，当跟随者可能性大于引领者可能性，即利用可能性最大的跟随者替换引领者，否则不进行替换，得到再次更新后粒子集。
进一步的，所述步骤33具体为：
步骤331：利用更新后粒子集中粒子的权重计算有效粒子的数目Neff，计算公式为：
步骤332：当有效粒子数目Neff大于设定的阈值Φ，则不进行重采样，得到当前时刻最终的粒子集，否则进行重采样后跳转至步骤31。
进一步的，所述重采样为：复制权值大的粒子，去掉权值小的粒子进行，保持粒子集中粒子数目稳定。
进一步的，所述步骤4具体为：
步骤41：利用当前时刻最终粒子集中各粒子的粒子信息中的路标点位置，对不同时刻相同粒子观测到的同一路标点采用扩展卡尔曼滤波进行更新，得到当前时刻的路标点位置；
步骤42：利用当前时刻最终粒子集中各个粒子粒子信息中的位姿数据和权重计算机器人运动路径中各点的位置X，计算公式为：
步骤43：利用当前时刻最终的路标点位置和路径中各点的位置，得到当前时刻机器人运动路径和路标点。
综上所述，由于采用了上述技术方案，本发明的有益效果是：
1.本发明能够极大的提升机器人的定位精度。
2.采用本发明公开的全局优化方法，粒子复制的数目随着亲和力的增加而减少，也就是对权值高的粒子进行多复制，权值小的粒子减少复制，通过这种方法可以有效的抑制亲和力高的抗体而促进亲和力低的抗体，加快收敛速度；通过变异操作使权值较大的粒子的位姿变化较小，而权值较小的粒子变化较大，从而使权值较小的粒子去寻找更好的位姿，增加粒子的多样性。
附图说明
本发明将通过例子并参照附图的方式说明，其中：
图1是本发明的整体流程图；
图2是本发明中全局优化的流程图；
图3是具体实施例2设定的地图和路标点；
图4是具体实施例2中采用EKFLAM算法运行的结果图；
图5是具体实施例2中采用FastSLAM算法运行的结果图；
图6是具体实施例2中采用本发明运行的结果图。
具体实施方式
本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任何方式组合。
下面结合图1、图2对本发明作详细说明。
一种全局优化的并发定位与建图方法，包括以下步骤：
步骤1：根据机器人的初始坐标，在初始时刻随机生成n个粒子，得到初始时刻的粒子集；
步骤2：采用粒子滤波的SLAM算法，利用上一时刻粒子集中各粒子的粒子信息更新当前时刻各粒子的粒子信息；
步骤3：采用全局优化和重采样对当前时刻各粒子进行更新；
步骤4：利用更新后的当前时刻粒子的粒子信息获得当前时刻机器人运动路径和路标点；
步骤5：重复步骤2-4，直至机器人完成所有时刻的运动。
进一步的，所述步骤2具体为：
步骤21：采用机器人运动模型，利用上一时刻各粒子的粒子信息中的位姿数据得到当前时刻各粒子的位姿数据，并记录当前时刻粒子观测到的路标点；
步骤22：采用提议分布对当前时刻的位姿数据和路标点位置进行更新，利用更新后的位姿数据和路标点位置计算各粒子的权重；
步骤23：对所述权重进行归一化处理，得到当前时刻各粒子的权重。
进一步的，所述步骤3具体步骤为：
步骤31：对当前时刻的各粒子进行克隆和变异操作，更新粒子集中的粒子；
步骤32：以更新后粒子集中的各个粒子为引领者随机产生N个跟随者，计算跟随者和引领者的可能性，利用可能性筛选所述引领者和跟随者，得到再次更新后的粒子集；
步骤33：利用再次更新后粒子集中粒子的权重计算有效粒子的数目Neff，当有效粒子数目Neff大于设定的阈值Φ，则不进行重采样，否则进行重采样，得到当前时刻最终的粒子集。
进一步的，所述步骤31具体为：
步骤311：利用步骤2得到的粒子信息中的权重计算当前时刻各粒子的亲和力faffinity，公式为：
其中i表示粒子的序号，t表示时刻；
步骤312：利用所述亲和力faffinity计算当前时刻粒子需要克隆的数目Ncl，计算公式如下：
其中，M表示预先设定的每一时刻粒子的总数，round表示取整函数；
步骤313：将新获得的粒子添加至所述粒子集中，更新当前时刻的粒子集，利用所述亲和力faffinity对粒子进行变异操作，变异采用的公式为：
其中表示第i个粒子的粒子信息中的位姿数据，random表示一个服从(0，1)分布的随机数；
步骤314：利用变异后粒子间的位姿差计算两两粒子间的排斥力freject，计算公式为：
其中，表示t时刻第i个粒子的位置，表示t时刻第j个粒子的位置：
步骤315：当粒子总数大于M时，将排斥力小于设定阈值的两个粒子中的一个抛弃，更新所述粒子集合Ω，其中权重更新公式为：
Zt表示t时刻粒子观测到的路标点位置，p表示权重更新中采用的权重值。
进一步的，所述步骤32具体为：
步骤321：对步骤21更新后粒子集中的每个粒子，以各个粒子为引领者随机产生对应的N个跟随者，分别计算引领者与跟随者的适应度ffitness，计算适应度采用的函数为：
其中，Rt表示t时刻观测过程的协方差矩阵，表示当前时刻粒子观测到的路标位置，表示上一时刻粒子观测到的路标位置；
步骤322：利用所述适应度ffitness计算各个粒子的可能性pi，采用的公式为：
步骤323：判断引领者和跟随者的可能性pi，当跟随者可能性大于引领者可能性，即利用可能性最大的跟随者替换引领者，否则不进行替换，得到再次更新后粒子集。
进一步的，所述步骤33具体为：
步骤331：利用更新后粒子集中粒子的权重计算有效粒子的数目Neff，计算公式为：
步骤332：当有效粒子数目Neff大于设定的阈值Φ，则不进行重采样，得到当前时刻最终的粒子集，否则进行重采样后跳转至步骤231。
进一步的，所述重采样为：复制权值大的粒子，去掉权值小的粒子进行，保持粒子集中粒子数目稳定。
进一步的，所述步骤4具体为：
步骤41：利用当前时刻最终粒子集中各粒子的粒子信息中的路标点位置，对不同时刻相同粒子观测到的同一路标点采用扩展卡尔曼滤波进行更新，得到当前时刻的路标点位置；
步骤42：利用当前时刻最终粒子集中各个粒子粒子信息中的位姿数据和权重计算机器人运动路径中各点的位置X，计算公式为：
步骤43：利用当前时刻最终的路标点位置和路径中各点的位置，得到当前时刻机器人运动路径和路标点。
具体实施例1
一种全局优化的并发定位与建图方法，包括以下步骤：
步骤1：根据机器人的初始坐标，在初始时刻随机生成n个粒子，得到初始时刻的粒子集；
步骤2：采用粒子滤波的SLAM算法，利用上一时刻粒子集中各粒子的粒子信息更新当前时刻各粒子的粒子信息；
步骤21：采用机器人运动模型，利用上一时刻各粒子的粒子信息中的位姿数据得到当前时刻各粒子的位姿数据，并记录当前时刻粒子观测到的路标点；机器人的惯性传感器获取运动数据，所述运动信息中包括加速度数据和角度数据，所述加速度数据和角度数据即为位姿数据；机器人的视觉传感器观测所述路标点；
对于当前时刻观测到的路标，需要和数据库中已经观测过的路标进行关联，得到新的路标的关联表；
步骤22：采用提议分布对当前时刻的位姿数据和路标点位置进行更新，利用更新后的位姿数据和路标点位置计算各粒子的权重；
步骤23：对所述权重进行归一化处理，得到当前时刻各粒子的权重。
步骤3：由于多次迭代会造成粒子退化现象，造成很少一部分粒子权值较大，而大部分粒子权值较小，造成后续的计算很大一部分用在权值较小的粒子上面，而这些粒子又对最终机器人的位姿估计作用很小，造成资源的浪费。如果通过现有的重采样算法进行重采样，复制权值大的粒子，去除权值小的粒子，增加有效粒子数目，又会造成最后所有的粒子可能是由很少的一部分粒子重采样得来的，降低了粒子的多样性，造成粒子耗尽的问题；因此本发明采用全局优化和重采样对当前时刻各粒子进行更新；
步骤31：把粒子集中的粒子作为初始抗体种群存入记忆单元，将机器人的真实位姿作为抗原，粒子集合中的每一个粒子作为抗体。由于每一个粒子的权值可以表明当前粒子位姿估计的准确程度，所以认为每个粒子的亲和力和它自身的权值相关，如果权值大那么亲和力就小，权值小亲和力就大，通过亲和力可以控制粒子的克隆数目以及粒子的变异程度；因此对当前时刻的各粒子进行克隆和变异操作，更新粒子集中的粒子；
步骤311：利用步骤2得到的粒子信息中的权重计算当前时刻各粒子的亲和力faffinity，公式为：
其中i表示粒子的序号，t表示时刻；
步骤312：利用所述亲和力faffinity计算当前时刻粒子需要克隆的数目Ncl，计算公式如下：
其中，M表示预先设定的每一时刻粒子的总数，round表示取整函数；
通过cos函数使得粒子复制的数目随着亲和力的增加而减少，也就是对权值高的粒子进行多复制，权值小的粒子减少复制，通过这种方法可以有效的抑制亲和力高的抗体而促进亲和力低的抗体，加快收敛速度。
步骤313：将新获得的粒子添加至所述粒子集中，更新当前时刻的粒子集，利用所述亲和力faffinity对粒子进行变异操作，通过变异操作使权值较大的粒子的位姿变化较小，而权值较小的粒子变化较大，从而使权值较小的粒子去寻找更好的位姿，增加粒子的多样性，变异采用的公式为：
其中表示第i个粒子的粒子信息中的位姿数据，random表示一个服从(0，1)分布的随机数。
步骤314：利用变异后粒子间的位姿差计算两两粒子间的排斥力freject，计算公式为：
其中，表示t时刻第i个粒子的位置，表示t时刻第j个粒子的位置；
步骤315：当粒子总数大于M时，将排斥力小于设定阈值的两个粒子中的一个抛弃，更新所述粒子集合Ω，其中权重更新公式为：
Zt表示t时刻粒子观测到的路标点位置，p表示权重更新中采用的权重值。
步骤32：通过步骤31保留了权值较低的粒子的同时增加了粒子集的多样性，缓解了粒子退化问题，以更新后粒子集中的各个粒子为引领者随机产生N个跟随者，计算跟随者和引领者的可能性，利用可能性筛选所述引领者和跟随者，得到再次更新后的粒子集；
步骤321：以各个粒子为引领者在其领域内随机产生对应的N个跟随者(替代粒子)，分别计算引领者与跟随者的适应度ffitness，计算适应度采用的函数为：
其中，Rt表示t时刻观测过程的协方差矩阵，表示当前时刻粒子观测到的路标位置，表示上一时刻粒子观测到的路标位置；
步骤322：利用所述适应度ffitness计算各个粒子的可能性pi，采用的公式为：
步骤323：判断引领者和跟随者的可能性pi，当跟随者可能性大于引领者可能性，即利用可能性最大的跟随者替换引领者，否则不进行替换，得到再次更新后粒子集。
步骤33：利用再次更新后粒子集中粒子的权重计算有效粒子的数目Neff，当有效粒子数目Neff大于设定的阈值Φ，则不进行重采样，否则进行重采样，得到当前时刻最终的粒子集。
步骤331：利用更新后粒子集中粒子的权重计算有效粒子的数目Neff，计算公式为：
步骤332：当有效粒子数目Neff大于设定的阈值Φ，则不进行重采样，得到当前时刻最终的粒子集，否则进行重采样后跳转至步骤31。
所述重采样为：复制权值大的粒子，去掉权值小的粒子进行，保持粒子集中粒子数目稳定。
步骤4：机器人在运动过程中，粒子集中每一个粒子对于观测的路标点都有自己的估计，所以每个粒子都有自己的地图，利用更新后的当前时刻粒子的粒子信息获得当前时刻机器人运动路径和路标点；
步骤41：利用当前时刻最终粒子集中各粒子的粒子信息中的路标点位置，对不同时刻相同粒子观测到的同一路标点采用扩展卡尔曼滤波进行更新，得到当前时刻的路标点位置；
步骤42：利用当前时刻最终粒子集中各个粒子粒子信息中的位姿数据和权重计算机器人运动路径中各点的位置X，计算公式为：
步骤43：利用当前时刻最终的路标点位置和路径中各点的位置，得到当前时刻机器人运动路径和路标点。
步骤5：完成当前时刻后，机器人继续运动进行下一时刻的运动，重复步骤2-4，直至机器人完成所有时刻的运动，生成地图信息和机器人的定位信息。
具体实施例2
本实施例基于具体实施例1，对本发明进行验证。
构建一个200m*200m的环形地图，如图3所示，其中路径点有21个，同时在路径点周围标定65个路标点用于模仿双目相机观察到的路标信息，在实验开始之前设定机器人从(0，0)开始运动，运动的速度是5m/s，运动的误差是0.5m/s，运动最大转向角是40度，最大转向角变化率25度/s，误差是4度。观测的距离误差是0.2m，角度误差是1度。
采用EKFSLAM算法进行处理，得到的位置估计均方根误差为2.1325，角度估计均方根误差为0.2379；
采用FastSLAM算法进行处理，得到的位置估计均方根误差为1.3826，角度估计均方根误差为0.1910；
而采用本发明进行处理，得到的位置估计均方根误差为1.0187，角度估计均方根误差为0.1473；
从以上数据可看出，采用本发明能提高机器人运动的定位精度。

Claims (8)

1.一种全局优化的并发定位与建图方法，其特征在于：包括以下步骤：
步骤1：根据机器人的初始坐标，在初始时刻随机生成n个粒子，得到初始时刻的粒子集；
步骤2：采用粒子滤波的SLAM算法，利用上一时刻粒子集中各粒子的粒子信息更新当前时刻各粒子的粒子信息；
步骤3：采用全局优化和重采样对当前时刻各粒子进行更新；
步骤4：利用更新后的当前时刻粒子的粒子信息获得当前时刻机器人运动路径和路标点；
步骤5：重复步骤2-4，直至机器人完成所有时刻的运动，输出地图和机器人的定位信息。
2.根据权利要求1所述的一种全局优化的并发定位与建图方法，其特征在于：所述步骤2具体为：
步骤21：采用机器人运动模型，利用上一时刻各粒子的粒子信息中的位姿数据得到当前时刻各粒子的位姿数据，并记录当前时刻粒子观测到的路标点；
步骤22：采用提议分布对当前时刻的位姿数据和路标点位置进行更新，利用更新后的位姿数据和路标点位置计算各粒子的权重；
步骤23：对所述权重进行归一化处理，得到当前时刻各粒子的权重。
3.根据权利要求1所述的一种全局优化的并发定位与建图方法，其特征在于：所述步骤3具体步骤为：
步骤31：对当前时刻的各粒子进行克隆和变异操作，更新粒子集中的粒子；
步骤32：以更新后粒子集中的各个粒子为引领者随机产生N个跟随者，计算跟随者和引领者的可能性，利用可能性筛选所述引领者和跟随者，得到再次更新后的粒子集；
步骤33：利用再次更新后粒子集中粒子的权重计算有效粒子的数目Neff，当有效粒子数目Neff大于设定的阈值Φ，则不进行重采样，否则进行重采样，得到当前时刻最终的粒子集。
4.根据权利要求3所述的一种全局优化的并发定位与建图方法，其特征在于：所述步骤31具体为：
步骤311：利用步骤2得到的粒子信息中的权重计算当前时刻各粒子的亲和力faffinity，公式为：
其中i表示粒子的序号，t表示时刻；
步骤312：利用所述亲和力fafiinity计算当前时刻粒子需要克隆的数目Ncl，计算公式如下：
其中，M表示预先设定的每一时刻粒子的总数，round表示取整函数；
步骤313：将新获得的粒子添加至所述粒子集中，更新当前时刻的粒子集，利用所述亲和力faffinity粒子进行变异操作，变异采用的公式为：
其中表示第i个粒子的粒子信息中的位姿数据，random表示一个服从(0，1)分布的随机数；
步骤314：利用变异后粒子间的位姿差计算两两粒子间的排斥力freject，计算公式为：
其中，表示t时刻第i个粒子的位置，表示t时刻第j个粒子的位置；
步骤315：当粒子总数大于M时，将排斥力小于设定阈值的两个粒子中的一个抛弃，更新所述粒子集合Ω，其中权重更新公式为：
Zt表示t时刻粒子观测到的路标点位置，p表示权重更新中采用的权重值。
5.根据权利要求3所述的一种全局优化的并发定位与建图方法，其特征在于：所述步骤32具体为：
步骤321：对步骤21更新后粒子集中的每个粒子，以各个粒子为引领者随机产生对应的N个跟随者，分别计算引领者与跟随者的适应度fftness，计算适应度采用的函数为：
其中，Rt表示t时刻观测过程的协方差矩阵，表示当前时刻粒子观测到的路标位置，表示上一时刻粒子观测到的路标位置；
步骤322：利用所述适应度ffitness计算各个粒子的可能性pi，采用的公式为：
步骤323：判断引领者和跟随者的可能性pi，当跟随者可能性大于引领者可能性，即利用可能性最大的跟随者替换引领者，否则不进行替换，得到再次更新后粒子集。
6.根据权利要求3所述的一种全局优化的并发定位与建图方法，其特征在于：所述步骤33具体为：
步骤331：利用更新后粒子集中粒子的权重计算有效粒子的数目Neff，计算公式为：
步骤332：当有效粒子数目Neff大于设定的阈值Φ，则不进行重采样，得到当前时刻最终的粒子集，否则进行重采样后跳转至步骤31。
7.根据权利要求6所述的一种全局优化的并发定位与建图方法，其特征在于：所述重采样为：复制权值大的粒子，去掉权值小的粒子进行，保持粒子集中粒子数目稳定。
8.根据权利要求1所述的一种全局优化的并发定位与建图方法，其特征在于：所述步骤4具体为：
步骤41：利用当前时刻最终粒子集中各粒子的粒子信息中的路标点位置，对不同时刻相同粒子观测到的同一路标点采用扩展卡尔曼滤波进行更新，得到当前时刻的路标点位置；
步骤42：利用当前时刻最终粒子集中各个粒子粒子信息中的位姿数据和权重计算机器人运动路径中各点的位置X，计算公式为：
步骤43：利用当前时刻最终的路标点位置和路径中各点的位置，得到当前时刻机器人运动路径和路标点。
CN201810734733.6A 2018-07-05 2018-07-05 一种全局优化的并发定位与建图方法 Active CN108871341B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810734733.6A CN108871341B (zh) 2018-07-05 2018-07-05 一种全局优化的并发定位与建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810734733.6A CN108871341B (zh) 2018-07-05 2018-07-05 一种全局优化的并发定位与建图方法

Publications (2)

Publication Number Publication Date
CN108871341A true CN108871341A (zh) 2018-11-23
CN108871341B CN108871341B (zh) 2021-12-24

Family

ID=64299728

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810734733.6A Active CN108871341B (zh) 2018-07-05 2018-07-05 一种全局优化的并发定位与建图方法

Country Status (1)

Country Link
CN (1) CN108871341B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109798896A (zh) * 2019-01-21 2019-05-24 东南大学 一种室内机器人定位与建图方法及装置
CN109917332A (zh) * 2019-02-01 2019-06-21 广东工业大学 一种基于改进粒子滤波的室内机器人定位方法
CN110954113A (zh) * 2019-05-30 2020-04-03 北京初速度科技有限公司 一种车辆位姿的修正方法和装置
CN111076724A (zh) * 2019-12-06 2020-04-28 苏州艾吉威机器人有限公司 三维激光定位方法及系统
CN111324116A (zh) * 2020-02-14 2020-06-23 南京航空航天大学 基于粒子滤波的机器人定位方法
CN112698345A (zh) * 2020-12-04 2021-04-23 江苏科技大学 一种激光雷达的机器人同时定位与建图优化方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100816269B1 (ko) * 2006-09-22 2008-03-25 학교법인 포항공과대학교 언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도작성 방법
CN103631264A (zh) * 2013-12-04 2014-03-12 苏州大学张家港工业技术研究院 一种同时定位与地图创建方法及装置
CN104597900A (zh) * 2014-12-02 2015-05-06 华东交通大学 一种基于类电磁机制优化的FastSLAM方法
CN105333879A (zh) * 2015-12-14 2016-02-17 重庆邮电大学 同步定位与地图构建方法
CN106289257A (zh) * 2016-07-27 2017-01-04 无锡知谷网络科技有限公司 室内定位方法及定位系统
CN107103355A (zh) * 2017-04-21 2017-08-29 北京工业大学 一种基于改进人工鱼群算法的智能车slam数据关联方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100816269B1 (ko) * 2006-09-22 2008-03-25 학교법인 포항공과대학교 언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도작성 방법
CN103631264A (zh) * 2013-12-04 2014-03-12 苏州大学张家港工业技术研究院 一种同时定位与地图创建方法及装置
CN104597900A (zh) * 2014-12-02 2015-05-06 华东交通大学 一种基于类电磁机制优化的FastSLAM方法
CN105333879A (zh) * 2015-12-14 2016-02-17 重庆邮电大学 同步定位与地图构建方法
CN106289257A (zh) * 2016-07-27 2017-01-04 无锡知谷网络科技有限公司 室内定位方法及定位系统
CN107103355A (zh) * 2017-04-21 2017-08-29 北京工业大学 一种基于改进人工鱼群算法的智能车slam数据关联方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
KWON O S 等: "An improved FastSLAM algorithm using fitness sharing technique", 《JOURNAL OF KOREAN INSTITUTE OF INTELLIGENT SYSTEMS》 *
刘洞波: "移动机器人粒子滤波定位与地图创建方法研究", 《中国博士学位论文全文数据库信息科技辑》 *
程泽等: "基于免疫克隆粒子群算法的无人机航迹规划", 《弹箭与制导学报》 *

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109798896A (zh) * 2019-01-21 2019-05-24 东南大学 一种室内机器人定位与建图方法及装置
CN109917332A (zh) * 2019-02-01 2019-06-21 广东工业大学 一种基于改进粒子滤波的室内机器人定位方法
CN109917332B (zh) * 2019-02-01 2022-12-16 广东工业大学 一种基于改进粒子滤波的室内机器人定位方法
CN110954113A (zh) * 2019-05-30 2020-04-03 北京初速度科技有限公司 一种车辆位姿的修正方法和装置
WO2020237996A1 (zh) * 2019-05-30 2020-12-03 魔门塔(苏州)科技有限公司 一种车辆位姿的修正方法和装置
CN110954113B (zh) * 2019-05-30 2021-10-15 北京初速度科技有限公司 一种车辆位姿的修正方法和装置
CN111076724A (zh) * 2019-12-06 2020-04-28 苏州艾吉威机器人有限公司 三维激光定位方法及系统
CN111076724B (zh) * 2019-12-06 2023-12-22 苏州艾吉威机器人有限公司 三维激光定位方法及系统
CN111324116A (zh) * 2020-02-14 2020-06-23 南京航空航天大学 基于粒子滤波的机器人定位方法
CN111324116B (zh) * 2020-02-14 2021-09-21 南京航空航天大学 基于粒子滤波的机器人定位方法
CN112698345A (zh) * 2020-12-04 2021-04-23 江苏科技大学 一种激光雷达的机器人同时定位与建图优化方法
CN112698345B (zh) * 2020-12-04 2024-01-30 江苏科技大学 一种激光雷达的机器人同时定位与建图优化方法

Also Published As

Publication number Publication date
CN108871341B (zh) 2021-12-24

Similar Documents

Publication Publication Date Title
CN108871341A (zh) 一种全局优化的并发定位与建图方法
CN112859859B (zh) 一种基于三维障碍物体素对象映射的动态栅格地图更新方法
US11878433B2 (en) Method for detecting grasping position of robot in grasping object
Wang et al. Application of Dijkstra algorithm in robot path-planning
Kehoe et al. A survey of research on cloud robotics and automation
CN108986801A (zh) 一种人机交互方法、装置及人机交互终端
CN109711262B (zh) 一种基于深度卷积神经网络的智能挖掘机行人检测方法
Cao et al. Rapid detection of blind roads and crosswalks by using a lightweight semantic segmentation network
CN106874914A (zh) 一种基于深度卷积神经网络的工业机械臂视觉控制方法
CN110363816A (zh) 一种基于深度学习的移动机器人环境语义建图方法
CN106352877A (zh) 一种移动装置及其定位方法
CN106406518A (zh) 手势控制装置及手势识别方法
CN113221726A (zh) 一种基于视觉与惯性信息融合的手部姿态估计方法及系统
Kumar et al. A unified grid-based wandering pattern detection algorithm
CN106251721A (zh) 虚拟运动教学方法及系统
CN113239803A (zh) 一种基于行人运动状态识别的航迹推算定位方法
CN113378656A (zh) 一种基于自适应图卷积神经网络的动作识别方法和装置
CN114387462A (zh) 一种基于双目相机的动态环境感知方法
CN112446462B (zh) 目标神经网络模型的生成方法和装置
Kashyap et al. Multi-objective trajectory planning of humanoid robot using hybrid controller for multi-target problem in complex terrain
Zhou et al. Motion recognition by using a stacked autoencoder-based deep learning algorithm with smart phones
CN107203271A (zh) 基于多传感器融合技术的双手识别方法
Zheng et al. An object recognition grasping approach using proximal policy optimization with yolov5
CN109242882A (zh) 视觉跟踪方法、装置、介质及设备
CN108051001A (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
GR01 Patent grant
GR01 Patent grant