CN115235479B - 自动导引车的定位方法、装置、可读存储介质及电子设备 - Google Patents

自动导引车的定位方法、装置、可读存储介质及电子设备 Download PDF

Info

Publication number
CN115235479B
CN115235479B CN202211162224.3A CN202211162224A CN115235479B CN 115235479 B CN115235479 B CN 115235479B CN 202211162224 A CN202211162224 A CN 202211162224A CN 115235479 B CN115235479 B CN 115235479B
Authority
CN
China
Prior art keywords
particle
weight
guided vehicle
automatic guided
positioning
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.)
Active
Application number
CN202211162224.3A
Other languages
English (en)
Other versions
CN115235479A (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.)
Jiangxi Intelligent Industry Technology Innovation Research Institute
Original Assignee
Jiangxi Intelligent Industry Technology Innovation Research Institute
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 Jiangxi Intelligent Industry Technology Innovation Research Institute filed Critical Jiangxi Intelligent Industry Technology Innovation Research Institute
Priority to CN202211162224.3A priority Critical patent/CN115235479B/zh
Publication of CN115235479A publication Critical patent/CN115235479A/zh
Application granted granted Critical
Publication of CN115235479B publication Critical patent/CN115235479B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G01C21/188—Compensation of inertial measurements, e.g. for temperature effects for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
    • 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
    • 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/004—Artificial life, i.e. computing arrangements simulating life
    • G06N3/006—Artificial life, i.e. computing arrangements simulating life based on simulated virtual individual or collective life forms, e.g. social simulations or particle swarm optimisation [PSO]

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Theoretical Computer Science (AREA)
  • Biophysics (AREA)
  • General Health & Medical Sciences (AREA)
  • Biomedical Technology (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Computational Linguistics (AREA)
  • Data Mining & Analysis (AREA)
  • Evolutionary Computation (AREA)
  • Artificial Intelligence (AREA)
  • Molecular Biology (AREA)
  • Computing Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Health & Medical Sciences (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

一种自动导引车的定位方法、装置、可读存储介质及电子设备，该定位方法包括：获取自动导引车按照规划路径移动至下一步时的定位信息，并与激光雷达采集的定位信息进行融合，得到观测位置的位置信息；计算粒子群在下一步位置时，各个粒子的位置与观测位置之间的距离；根据计算出的各个粒子的位置与观测位置之间的距离确定各个粒子的权重，并根据权重进行重采样；计算重采样后的粒子群的中心位置，得到自动导引车的估计位置信息；根据估计位置信息判断自动导引车是否到达所述目标点；当自动导引车到达目标点后，在自动导引车的当前位置周围新增预设数量个粒子。该方案有效提升AGV导航过程中的到点定位准确性及鲁棒性。

Description

自动导引车的定位方法、装置、可读存储介质及电子设备
技术领域
本发明涉及定位技术领域，特别是涉及一种自动导引车的定位方法、装置、可读存储介质及电子设备。
背景技术
AGV（Automated Guided Vehicle，自动导引车），是指装备有电磁或光学等自动导引装置，它能够沿规定的导引路径行驶，具有安全保护以及各种移载功能的运输车。
目前采用基于粒子滤波定位技术对自动导引车（AGV）进行定位，粒子滤波的核心思想在于使用较多的样本代表待估计状态的所有可能性结果。初始时刻在AGV运动范围内随机生成均匀分布的粒子，根据状态转移方程，将所有粒子依次代入，得到预测粒子，再对粒子进行重采样。然而，随着AGV运动不断进行迭代，采样的粒子会逐渐减少，影响最终定位精度。
发明内容
鉴于上述状况，有必要提供一种自动导引车的定位方法、装置、可读存储介质及电子设备，实现更高精度与鲁棒性的AGV定位方案。
一种自动导引车的定位方法，所述自动导引车上设置有激光雷达，所述定位方法包括：
获取远程终端分布的目标点的位置信息，并获取所述自动导引车按照规划路径移动至下一步时的定位信息，得到第一位置信息，以及获取所述激光雷达采集的定位信息，得到第二位置信息；
将所述第一位置信息和所述第二位置信息进行融合，得到观测位置的位置信息；
根据粒子群中的各个粒子的位置信息确定粒子群移动至下一步的位置，并计算所述粒子群在下一步位置时，各个粒子的位置与所述观测位置之间的距离；
根据计算出的各个粒子的位置与所述观测位置之间的距离确定各个粒子的权重，并根据各个粒子的权重对粒子群进行重采样；
计算重采样后的粒子群的中心位置，得到所述自动导引车的估计位置信息；
根据所述估计位置信息判断所述自动导引车是否到达所述目标点，若否，返回执行获取所述自动导引车按照规划路径移动至下一步时的定位信息的步骤；
当所述自动导引车到达所述目标点后，在所述自动导引车的当前位置周围新增预设数量个粒子。
进一步的，上述定位方法，其中，所述获取所述自动导引车按照规划路径移动至下一步时的定位信息的步骤包括：
通过轮式里程计、惯性导航单元IMU获取所述自动导引车按照规划路径移动至下一步时的定位信息。
进一步的，上述定位方法，其中，各个粒子的权重的计算公式为：
Figure 394730DEST_PATH_IMAGE001
，其中，
Figure 80927DEST_PATH_IMAGE002
为第i个粒子的权重，
Figure 881392DEST_PATH_IMAGE003
为第i 个粒子与观测位置之间的距离，σ为观测误差。
进一步的，上述定位方法，其中，所述根据各个粒子的权重对粒子群进行重采样的步骤包括：
随机生成权重阈值；
从粒子群中随机选取粒子i；
判断所述权重阈值是否大于粒子i的权重；
若所述权重阈值大于粒子i的权重，更新所述权重阈值，且赋值i=i+1，所述权重阈 值的更新所采用的公式为，
Figure 260421DEST_PATH_IMAGE004
，ωi为粒子i的权重，ω（th）为随机生成 权重阈值，
Figure 388914DEST_PATH_IMAGE005
为更新后的权重阈值；
判断i是否大于阈值m0；
当i不大于阈值m0时，返回执行判断所述权重阈值是否大于粒子i的权重的步骤；
当i是大于阈值m0时，赋值i=1，并返回执行判断所述权重阈值是否大于粒子i的权重的步骤；
若所述权重阈值不大于粒子i的权重，将粒子i加入新的粒子群，并判断所述新的粒子群的粒子数量是否达到预设值，若未达到所述阈值则返回执行随机生成权重阈值的初始值的步骤。
进一步的，上述定位方法，其中，所述自动导引车的当前位置周围新增的预设数量个粒子，其位置分布满足如下公式：
Figure 957430DEST_PATH_IMAGE006
，
其中，α1、α2和α3根据定位误差确定的常数，(x，y，θ)是表示粒子位置的三个参数。
本发明还公开了一种定位装置，应用于自动导引车，所述自动导引车上设置有激光雷达，所述定位装置包括：
获取模块，用于获取远程终端分布的目标点的位置信息，并获取所述自动导引车按照规划路径移动至下一步时的定位信息，得到第一位置信息，以及获取所述激光雷达采集的定位信息，得到第二位置信息；
信息融合模块，用于将所述第一位置信息和所述第二位置信息进行融合，得到观测位置的位置信息；
第一计算模块，用于根据粒子群中的各个粒子的位置信息确定粒子群移动至下一步的位置，并计算所述粒子群在下一步位置时，各个粒子的位置与所述观测位置之间的距离；
重采样模块，用于根据计算出的各个粒子的位置与所述观测位置之间的距离确定各个粒子的权重，并根据各个粒子的权重对粒子群进行重采样；
第二计算模块，用于计算重采样后的粒子群的中心位置，得到所述自动导引车的估计位置信息；
判断模块，用于根据所述估计位置信息判断所述自动导引车是否到达所述目标点，若否，获取模块返回执行获取所述自动导引车按照规划路径移动至下一步时的定位信息的步骤；
粒子增设模块，用于当所述自动导引车到达所述目标点后，在所述自动导引车的当前位置周围新增预设数量个粒子。
进一步的，上述定位装置，其中，所述重采样模块用于：
随机生成权重阈值；
从粒子群中随机选取粒子i；
判断所述权重阈值是否大于粒子i的权重；
若所述权重阈值大于粒子i的权重，更新所述权重阈值，且赋值i=i+1，所述权重阈 值的更新所采用的公式为，
Figure 18927DEST_PATH_IMAGE007
，ωi为粒子i的权重，ω（th）为随机生成 权重阈值，
Figure 772119DEST_PATH_IMAGE005
为更新后的权重阈值；
判断i是否大于阈值m0；
当i不大于阈值m0时，返回执行判断所述权重阈值是否大于粒子i的权重的步骤；
当i是大于阈值m0时，赋值i=1，并返回执行判断所述权重阈值是否大于粒子i的权重的步骤；
若所述权重阈值不大于粒子i的权重，将粒子i加入新的粒子群，并判断所述新的粒子群的粒子数量是否达到预设值，若未达到所述阈值则返回执行随机生成权重阈值的初始值的步骤。
进一步的，上述定位装置，其中，所述自动导引车的当前位置周围新增的预设数量个粒子，其位置分布满足如下公式：
Figure 450225DEST_PATH_IMAGE008
，
其中，α1、α2和α3根据定位误差确定的常数，(x，y，θ)是表示粒子位置的三个参数。
本发明还公开了一种电子设备，包括存储器和处理器，所述存储器存储有程序，所述程序被所述处理器执行时实现上述任一所述的定位方法。
本发明还公开了一种计算机可读存储介质，其上存储有程序，所述程序被处理器执行时实现上述任一所述的定位方法。
本发明中，AGV在导航行进过程中，无效粒子就会被算法淘汰掉，使得在抵达目标点前留存的粒子都为有效粒子，提高了定位精度。并且在每次导航结束后会增加粒子，即保障了定位粒子的数量，同时导航结束后增加的粒子控制在AGV当前位置的周围，当AGV中惯性导航单元IMU长期使用产生累积误差导致定位出现偏差时，周围新增粒子可以有效纠正该累积误差。同时在AGV周围增加粒子，有效避免因地图过大存在相似环境而定位错误的情况。该方案有效提升AGV导航过程中的到点定位准确性及鲁棒性。
附图说明
图1为本发明第一实施例中自动导引车的定位方法的流程图；
图2为本发明第一实施例中步骤S15的实现步骤流程图；
图3为本发明第二实施例中定位装置的结构框图；
图4为本发明实施例中电子设备的结构示意图。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
参照下面的描述和附图，将清楚本发明的实施例的这些和其他方面。在这些描述和附图中，具体公开了本发明的实施例中的一些特定实施方式，来表示实施本发明的实施例的原理的一些方式，但是应当理解，本发明的实施例的范围不受此限制。相反，本发明的实施例包括落入所附加权利要求书的精神和内涵范围内的所有变化、修改和等同物。
本发明提供一种用于激光导航方式提高AGV（自动导引车）到点定位精度及降低累积误差的定位方法。解决AGV长期使用过程中累积误差难以消除，导致定位精度持续降低问题。
请参阅图1，为本发明第一实施例中自动导引车的定位方法，该自动导引车上设置有激光雷达，除此之外还设置有轮式里程计和惯性导航单元IMU，本实施例中定位所需数据来源于AGV轮式里程计、惯性导航单元IMU、激光雷达。该定位方法包括包步骤S11~S18。
步骤S11，获取远程终端分布的目标点的位置信息。
步骤S12，获取所述自动导引车按照规划路径移动至下一步时的定位信息，得到第一位置信息，以及获取所述激光雷达采集的定位信息，得到第二位置信息。
本实施例应用于定位单元，该定位单元可以是设置在自动导引车中的单元结构，也可以是单独的设备。该定位装置与激光雷达，轮式里程计和惯性导航单元IMU电性连接。
远程终端主要为任务发布端，其可以为手机、个人电脑，服务器等设备。在远程终端上发布目标点，即AGV所到达的目的地。AGV接收到发布的任务后，按照规划路径开始运动。
AGV从初始位置移动至下一步，即移动一定步长后，定位单元通过轮式里程计、惯性导航单元IMU获取AGV的定位信息，得到第一位置信息（X1，Y1，θ1）。同时，获取激光雷达采集的定位信息，得到第二位置信息（X2，Y2，θ2）。
步骤S13，将所述第一位置信息和所述第二位置信息进行融合，得到观测位置的位置信息。
将第一位置信息和第二位置信息进行融合，可以更加精确地定位自动引导小车当前的位置。具体实施时，第一位置信息和第二位置信息可以通过取平均值来进行融合。即观测位置的信息为第一位置信息和第二位置信息的均值后的为准信息。
步骤S14，根据粒子群中的各个粒子的位置信息确定粒子群移动至下一步的位置，并计算所述粒子群在下一步位置时，各个粒子的位置与所述观测位置之间的距离。
步骤S15，根据计算出的各个粒子的位置与所述观测位置之间的距离确定各个粒子的权重，并根据各个粒子的权重对粒子群进行重采样。
AGV接收到任务的初始时刻，在AGV运动范围内随机生成均匀分布的粒子群，将把 粒子群中的全部粒子逐个代入到AGV运动方程中可以得到粒子群的下一步位置，即预测AGV 从初始位置移动至下一步时，粒子群的位置，即可确定预测粒子的位置。计算粒子群在该下 一步位置时，每个粒子的位置和观测位置之间的几何距离
Figure 806120DEST_PATH_IMAGE009
，即预测粒子的位置与 观测位置之间的几何距离，该几何距离可以为位置间的直线距离。预测粒子的位置与观测 位置越接近说明预测位置越靠近AGV的实际位置。根据计算出的各个预测粒子与观测位置 之间的距离确定各个粒子的权重，各个粒子的权重的计算公式为：
Figure 253282DEST_PATH_IMAGE010
，其中，
Figure 646217DEST_PATH_IMAGE011
为第i个粒子的权重，
Figure 811619DEST_PATH_IMAGE012
为第i个粒子与观测位置信息之间的距离，σ为观测误差，提前由传感器测试获 取。
进一步的，进行下续步骤是，需要归一化计算粒子权重：
Figure 954894DEST_PATH_IMAGE013
，
Figure 522141DEST_PATH_IMAGE014
为归一化后的粒子权重。
本实施例中，预测下一步粒子的位置，并进行评价（计算权重），越接近观测状态的粒子权重越大，然后重采样，根据粒子权重对粒子进行筛选，筛选过程中既要大量保留权重大的粒子又要有一部分权重小的粒子，对重采样后的粒子代入状态转移方程得到新的预测粒子，并随着AGV运动不断进行迭代。具体的，如图2所示，所述根据各个粒子的权重对粒子群进行重采样的步骤包括步骤S151~S158。
步骤S151，随机生成权重阈值。
需要说明的是，随机生成权重阈值ω(th)的公式为：
ω(th)=2*max(ω2)*rand，其中，rand为0~1的随机数，i为1~m0的随机整数，m0是采样前定位粒子的总数。
步骤S152，从粒子群中随机选取粒子i。即随机选取第i个粒子。
步骤S153，判断所述权重阈值是否大于粒子i的权重，若是执行步骤S154，否则执行步骤S157；
步骤S154，更新所述权重阈值，且赋值i=i+1。其中，所述权重阈值的更新所采用的 公式为，
Figure 85978DEST_PATH_IMAGE015
，ωi为粒子i的权重，ω（th）为随机生成权重阈值，
Figure 738676DEST_PATH_IMAGE016
为更新 后的权重阈值。
步骤S155，判断i是否大于阈值m0，若否，返回步骤S153。
步骤S156，赋值i=1，并返回步骤S153。
步骤S157，将所述粒子i加入新的粒子群。
步骤S158，判断所述新的粒子群的粒子数量是否达到预设值m，若否，返回执行步骤S151。
步骤S16，计算重采样后的粒子群的中心位置，得到所述自动导引车的估计位置信息。
步骤S17，根据所述估计位置信息判断所述自动导引车是否到达所述目标点，若是执行步骤S18，若否，返回执行步骤S12。
重采样后粒子中心位置即计算后为AGV的位置（X4，Y4，θ4），根据判断该位置（X4，Y4，θ4）判断AGV是否到达目标点，即位置（X4，Y4，θ4）与目标点的距离在误差内即可认为到达目标点。
当AGV未到达目标点时，则重复步骤S12至S15，直到达目标点，则停止导航。
步骤S18，在所述自动导引车的当前位置周围新增预设数量个粒子。
通过重采样可以提高每一步的导航定位准确性，但是随着运行时间的增加，采样的粒子会逐渐减少，影响下一次定位精度，因此，每次导航结束后会增加粒子。
导航结束后，在AGV的当前位置点周围更新n个粒子，新增的粒子的位置分布满足如下公式：
Figure 436373DEST_PATH_IMAGE017
；
其中
Figure 795811DEST_PATH_IMAGE018
根据定位误差选择，该值决定新增粒子分布范围，若传感器误 差较大，则该值适当增大，n由设备算力决定，定位粒子越多，定位效果越好。重复步骤S11~ S18，直到AGV停止工作。
AGV在进行路径导航时，在每个路径点到下一路径点之间均可以采用本实施例中的方法进行定位。本实施例中，AGV在导航行进过程中，无效粒子就会被算法淘汰掉，使得在抵达目标点前留存的粒子都为有效粒子，提高了定位精度。并且在每次导航结束后会增加粒子，即保障了定位粒子的数量，同时导航结束后增加的粒子控制在AGV当前位置的周围，当AGV中惯性导航单元IMU长期使用产生累积误差导致定位出现偏差时，周围新增粒子可以有效纠正该累积误差。同时在AGV周围增加粒子，有效避免因地图过大存在相似环境而定位错误的情况。该方案有效提升AGV导航过程中的到点定位准确性及鲁棒性。
请参阅图3，为本发明第二实施例中的定位装置，应用于自动导引车，所述自动导引车上设置有激光雷达，所述定位装置包括：
获取模块21，用于获取远程终端分布的目标点的位置信息，并获取所述自动导引车按照规划路径移动至下一步时的定位信息，得到第一位置信息，以及获取所述激光雷达采集的定位信息，得到第二位置信息；
信息融合模块22，用于将所述第一位置信息和所述第二位置信息进行融合，得到观测位置的位置信息；
第一计算模块23，用于根据粒子群中的各个粒子的位置信息确定粒子群移动至下一步的位置，并计算所述粒子群在下一步位置时，各个粒子的位置与所述观测位置之间的距离；
重采样模块24，用于根据计算出的各个粒子的位置与所述观测位置之间的距离确定各个粒子的权重，并根据各个粒子的权重对粒子群进行重采样；
第二计算模块25，用于计算重采样后的粒子群的中心位置，得到所述自动导引车的估计位置信息；
判断模块26，用于根据所述估计位置信息判断所述自动导引车是否到达所述目标点，若否，获取模块21返回执行获取所述自动导引车按照规划路径移动至下一步时的定位信息的步骤；
粒子增设模块27，用于当所述自动导引车到达所述目标点后，在所述自动导引车的当前位置周围新增预设数量个粒子。
进一步的，上述定位装置，其中，所述重采样模块用于：
随机生成权重阈值；
从粒子群中随机选取粒子i；
判断所述权重阈值是否大于粒子i的权重；
若所述权重阈值大于粒子i的权重，更新所述权重阈值，且赋值i=i+1，所述权重阈 值的更新所采用的公式为，
Figure 858445DEST_PATH_IMAGE019
，ωi为粒子i的权重，ω（th）为随机生成 权重阈值，
Figure 545909DEST_PATH_IMAGE020
为更新后的权重阈值；
判断i是否大于阈值m0；
当i不大于阈值m0时，返回执行判断所述权重阈值是否大于粒子i的权重的步骤；
当i是大于阈值m0时，赋值i=1，并返回执行判断所述权重阈值是否大于粒子i的权重的步骤；
若所述权重阈值不大于粒子i的权重，将粒子i加入新的粒子群，并判断所述新的粒子群的粒子数量是否达到预设值，若未达到所述阈值则返回执行随机生成权重阈值的初始值的步骤。
进一步的，上述定位装置，其中，所述自动导引车的当前位置周围新增的预设数量个粒子，其位置分布满足如下公式：
Figure 984981DEST_PATH_IMAGE008
，
其中，α1、α2和α3根据定位误差确定的常数，(x，y，θ)是表示粒子位置的三个参数。
本发明实施例所提供的定位装置，其实现原理及产生的技术效果和前述方法实施例相同，为简要描述，装置实施例部分未提及之处，可参考前述方法实施例中相应内容。
本发明另一方面还提出一种电子设备，请参阅图4，所示为本发明实施例当中的电子设备，包括处理器10、存储器20以及存储在存储器上并可在处理器上运行的计算机程序30，所述处理器10执行所述计算机程序30时实现如上述的定位方法。
其中，所述电子设备可以为但不限于个人电脑、控制器、自动导引车等设备。处理器10在一些实施例中可以是一中央处理器（Central Processing Unit, CPU）、控制器、微控制器、微处理器或其他数据处理芯片，用于运行存储器20中存储的程序代码或处理数据等。
其中，存储器20至少包括一种类型的可读存储介质，所述可读存储介质包括闪存、硬盘、多媒体卡、卡型存储器（例如，SD或DX存储器等）、磁性存储器、磁盘、光盘等。存储器20在一些实施例中可以是电子设备的内部存储单元，例如该电子设备的硬盘。存储器20在另一些实施例中也可以是电子设备的外部存储装置，例如电子设备上配备的插接式硬盘，智能存储卡（Smart Media Card, SMC），安全数字（Secure Digital, SD）卡，闪存卡（FlashCard）等。进一步地，存储器20还可以既包括电子设备的内部存储单元也包括外部存储装置。存储器20不仅可以用于存储安装于电子设备的应用软件及各类数据等，还可以用于暂时地存储已经输出或者将要输出的数据。
可选地，该电子设备还可以包括用户接口、网络接口、通信总线等，用户接口可以包括显示器（Display）、输入单元比如键盘（Keyboard），可选的用户接口还可以包括标准的有线接口、无线接口。可选地，在一些实施例中，显示器可以是LED显示器、液晶显示器、触控式液晶显示器以及OLED（Organic Light-Emitting Diode，有机发光二极管）触摸器等。其中，显示器也可以适当的称为显示屏或显示单元，用于显示在电子设备中处理的信息以及用于显示可视化的用户界面。网络接口可选的可以包括标准的有线接口、无线接口（如WI-FI接口），通常用于在该装置与其他电子装置之间建立通信连接。通信总线用于实现这些组件之间的连接通信。
需要指出的是，图4示出的结构并不构成对电子设备的限定，在其它实施例当中，该电子设备可以包括比图示更少或者更多的部件，或者组合某些部件，或者不同的部件布置。
本发明还提出一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上述的定位方法。
本领域技术人员可以理解，在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置（如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置中获取指令并执行指令的系统）使用，或结合这些指令执行系统、装置而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或结合这些指令执行系统、装置而使用的设备。
计算机可读介质的更具体的示例（非穷尽性列表）包括以下：具有一个或多个布线的电连接部（电子装置），便携式计算机盘盒（磁装置），随机存取存储器（RAM），只读存储器（ROM），可擦除可编辑只读存储器（EPROM或闪速存储器），光纤装置，以及便携式光盘只读存储器（CDROM）。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或它们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列（PGA），现场可编程门阵列（FPGA）等。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、 “示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种自动导引车的定位方法，所述自动导引车上设置有激光雷达，其特征在于，所述定位方法包括：
获取远程终端分布的目标点的位置信息，并获取所述自动导引车按照规划路径移动至下一步时的定位信息，得到第一位置信息，以及获取所述激光雷达采集的定位信息，得到第二位置信息；
将所述第一位置信息和所述第二位置信息进行融合，得到观测位置的位置信息；
根据粒子群中的各个粒子的位置信息确定粒子群移动至下一步的位置，并计算所述粒子群在下一步位置时，各个粒子的位置与所述观测位置之间的距离；
根据计算出的各个粒子的位置与所述观测位置之间的距离确定各个粒子的权重，并根据各个粒子的权重对粒子群进行重采样；
计算重采样后的粒子群的中心位置，得到所述自动导引车的估计位置信息；
根据所述估计位置信息判断所述自动导引车是否到达所述目标点，若否，返回执行获取所述自动导引车按照规划路径移动至下一步时的定位信息的步骤；
当所述自动导引车到达所述目标点后，在所述自动导引车的当前位置周围新增预设数量个粒子。
2.如权利要求1所述的定位方法，其特征在于，所述获取所述自动导引车按照规划路径移动至下一步时的定位信息的步骤包括：
通过轮式里程计、惯性导航单元IMU获取所述自动导引车按照规划路径移动至下一步时的定位信息。
3.如权利要求1所述的定位方法，其特征在于，各个粒子的权重的计算公式为：
Figure 131371DEST_PATH_IMAGE001
，其中，
Figure 594714DEST_PATH_IMAGE002
为第i个粒子的权重，
Figure 470266DEST_PATH_IMAGE003
为第i个粒 子与观测位置之间的距离，σ为观测误差。
4.如权利要求1所述的定位方法，其特征在于，所述根据各个粒子的权重对粒子群进行重采样的步骤包括：
随机生成权重阈值；
从粒子群中随机选取粒子i；
判断所述权重阈值是否大于粒子i的权重；
若所述权重阈值大于粒子i的权重，更新所述权重阈值，且赋值i=i+1，所述权重阈值的 更新所采用的公式为，
Figure 499402DEST_PATH_IMAGE004
，ωi为粒子i的权重，ω（th）为随机生成权重 阈值，
Figure 5469DEST_PATH_IMAGE005
为更新后的权重阈值；
判断i是否大于阈值m0；
当i不大于阈值m0时，返回执行判断所述权重阈值是否大于粒子i的权重的步骤；
当i是大于阈值m0时，赋值i=1，并返回执行判断所述权重阈值是否大于粒子i的权重的步骤；
若所述权重阈值不大于粒子i的权重，将粒子i加入新的粒子群，并判断所述新的粒子群的粒子数量是否达到预设值，若未达到所述阈值则返回执行随机生成权重阈值的初始值的步骤。
5.如权利要求1所述的定位方法，其特征在于，所述自动导引车的当前位置周围新增的预设数量个粒子，其位置分布满足如下公式：
Figure 2113DEST_PATH_IMAGE006
，
其中，α1、α2和α3根据定位误差确定的常数，(x，y，θ)是表示粒子位置的三个参数。
6.一种定位装置，应用于自动导引车，所述自动导引车上设置有激光雷达，其特征在于，所述定位装置包括：
获取模块，用于获取远程终端分布的目标点的位置信息，并获取所述自动导引车按照规划路径移动至下一步时的定位信息，得到第一位置信息，以及获取所述激光雷达采集的定位信息，得到第二位置信息；
信息融合模块，用于将所述第一位置信息和所述第二位置信息进行融合，得到观测位置的位置信息；
第一计算模块，用于根据粒子群中的各个粒子的位置信息确定粒子群移动至下一步的位置，并计算所述粒子群在下一步位置时，各个粒子的位置与所述观测位置之间的距离；
重采样模块，用于根据计算出的各个粒子的位置与所述观测位置之间的距离确定各个粒子的权重，并根据各个粒子的权重对粒子群进行重采样；
第二计算模块，用于计算重采样后的粒子群的中心位置，得到所述自动导引车的估计位置信息；
判断模块，用于根据所述估计位置信息判断所述自动导引车是否到达所述目标点，若否，获取模块返回执行获取所述自动导引车按照规划路径移动至下一步时的定位信息的步骤；
粒子增设模块，用于当所述自动导引车到达所述目标点后，在所述自动导引车的当前位置周围新增预设数量个粒子。
7.如权利要求6所述的定位装置，其特征在于，所述重采样模块用于：
随机生成权重阈值；
从粒子群中随机选取粒子i；
判断所述权重阈值是否大于粒子i的权重；
若所述权重阈值大于粒子i的权重，更新所述权重阈值，且赋值i=i+1，所述权重阈值的 更新所采用的公式为，
Figure 619039DEST_PATH_IMAGE007
，ωi为粒子i的权重，ω（th）为随机生成权重 阈值，
Figure 768261DEST_PATH_IMAGE008
为更新后的权重阈值；
判断i是否大于阈值m0；
当i不大于阈值m0时，返回执行判断所述权重阈值是否大于粒子i的权重的步骤；
当i大于阈值m0时，赋值i=1，并返回执行判断所述权重阈值是否大于粒子i的权重的步骤；
若所述权重阈值不大于粒子i的权重，将粒子i加入新的粒子群，并判断所述新的粒子群的粒子数量是否达到阈值，若未达到所述阈值则返回执行随机生成权重阈值的初始值的步骤。
8.如权利要求6所述的定位装置，其特征在于，所述自动导引车的当前位置周围新增的预设数量个粒子，其位置分布满足如下公式：
Figure 445230DEST_PATH_IMAGE009
，
其中，α1、α2和α3根据定位误差确定的常数，(x，y，θ)是表示粒子位置的三个参数。
9.一种电子设备，其特征在于，包括存储器和处理器，所述存储器存储有程序，所述程序被所述处理器执行时实现如权利要求1-5任一所述的定位方法。
10.一种计算机可读存储介质，其上存储有程序，其特征在于，所述程序被处理器执行时实现如权利要求1－5任一所述的定位方法。
CN202211162224.3A 2022-09-23 2022-09-23 自动导引车的定位方法、装置、可读存储介质及电子设备 Active CN115235479B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211162224.3A CN115235479B (zh) 2022-09-23 2022-09-23 自动导引车的定位方法、装置、可读存储介质及电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211162224.3A CN115235479B (zh) 2022-09-23 2022-09-23 自动导引车的定位方法、装置、可读存储介质及电子设备

Publications (2)

Publication Number Publication Date
CN115235479A CN115235479A (zh) 2022-10-25
CN115235479B true CN115235479B (zh) 2022-12-06

Family

ID=83667325

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211162224.3A Active CN115235479B (zh) 2022-09-23 2022-09-23 自动导引车的定位方法、装置、可读存储介质及电子设备

Country Status (1)

Country Link
CN (1) CN115235479B (zh)

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103902812A (zh) * 2014-03-05 2014-07-02 深圳大学 一种粒子滤波方法、装置及目标跟踪方法、装置
CN106248077A (zh) * 2016-07-06 2016-12-21 北京理工大学 一种基于粒子滤波的可见光组合定位系统和方法
CN107328406A (zh) * 2017-06-28 2017-11-07 中国矿业大学(北京) 一种基于多源传感器的矿井移动目标定位方法与系统
CN107908185A (zh) * 2017-10-14 2018-04-13 北醒（北京）光子科技有限公司 一种机器人自主全局重定位方法及机器人
CN109099922A (zh) * 2018-08-14 2018-12-28 桂林电子科技大学 一种基于物理场的室内定位方法
CN110617825A (zh) * 2019-09-29 2019-12-27 百度在线网络技术（北京）有限公司 一种车辆定位方法、装置、电子设备和介质
CN113934219A (zh) * 2021-12-16 2022-01-14 宏景科技股份有限公司 一种机器人自动避障方法、系统、设备及介质
CN114265084A (zh) * 2021-11-26 2022-04-01 淮阴工学院 一种基于粒子滤波的无人车动态避障方法
CN114974438A (zh) * 2022-05-18 2022-08-30 北京百度网讯科技有限公司 粒子运动模拟方法、装置、设备、存储介质和程序产品

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9020637B2 (en) * 2012-11-02 2015-04-28 Irobot Corporation Simultaneous localization and mapping for a mobile robot
EP3680618A1 (en) * 2019-01-10 2020-07-15 Technische Universität München Method and system for tracking a mobile device

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103902812A (zh) * 2014-03-05 2014-07-02 深圳大学 一种粒子滤波方法、装置及目标跟踪方法、装置
CN106248077A (zh) * 2016-07-06 2016-12-21 北京理工大学 一种基于粒子滤波的可见光组合定位系统和方法
CN107328406A (zh) * 2017-06-28 2017-11-07 中国矿业大学(北京) 一种基于多源传感器的矿井移动目标定位方法与系统
CN107908185A (zh) * 2017-10-14 2018-04-13 北醒（北京）光子科技有限公司 一种机器人自主全局重定位方法及机器人
CN109099922A (zh) * 2018-08-14 2018-12-28 桂林电子科技大学 一种基于物理场的室内定位方法
CN110617825A (zh) * 2019-09-29 2019-12-27 百度在线网络技术（北京）有限公司 一种车辆定位方法、装置、电子设备和介质
CN114265084A (zh) * 2021-11-26 2022-04-01 淮阴工学院 一种基于粒子滤波的无人车动态避障方法
CN113934219A (zh) * 2021-12-16 2022-01-14 宏景科技股份有限公司 一种机器人自动避障方法、系统、设备及介质
CN114974438A (zh) * 2022-05-18 2022-08-30 北京百度网讯科技有限公司 粒子运动模拟方法、装置、设备、存储介质和程序产品

Also Published As

Publication number Publication date
CN115235479A (zh) 2022-10-25

Similar Documents

Publication Publication Date Title
CN110687549B (zh) 障碍物检测方法和装置
CN108955713B (zh) 行驶轨迹的显示方法及装置
CN109141464A (zh) 导航变道提示方法和装置
CN105737826B (zh) 行人室内定位方法
WO2022056770A1 (zh) 一种路径规划方法和路径规划装置
CN110147105A (zh) 无人驾驶车辆的路径控制方法、设备、存储介质及装置
JP6842533B2 (ja) 需要予測装置
CN110542425B (zh) 导航路径选择方法、导航装置、计算机设备及可读介质
CN114202924B (zh) 冗余交通限制信息的识别方法及装置、电子设备和介质
CN109737971B (zh) 车载辅助导航定位系统、方法、设备及存储介质
CN113110505A (zh) 路径规划方法、装置、设备及存储介质
CN113071505A (zh) 驾驶行为习惯的确定、车辆行驶控制方法、装置及设备
CN112435469A (zh) 车辆预警控制方法、装置、计算机可读介质及电子设备
CN112674653B (zh) 障碍物位置标记方法、装置、计算机设备及存储介质
CN111076716B (zh) 用于车辆定位的方法、装置、设备和计算机可读存储介质
CN115339453B (zh) 车辆换道决策信息生成方法、装置、设备和计算机介质
CN115235479B (zh) 自动导引车的定位方法、装置、可读存储介质及电子设备
CN113119999A (zh) 自动驾驶特征的确定方法、装置、设备、介质及程序产品
CN116088538B (zh) 车辆轨迹信息生成方法、装置、设备和计算机可读介质
CN109934496B (zh) 区域间通行影响确定方法、装置、设备和介质
CN115917624A (zh) 中央装置、地图生成系统、地图生成方法
CN110389577B (zh) 一种确定驾驶风格的方法及装置
CN115953414A (zh) 基于语义分割的低矮障碍物检测方法和自动驾驶车辆
CN109827610A (zh) 用于校验传感器融合结果的方法和装置
CN112668371B (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