CN112764053A - 一种融合定位方法、装置、设备和计算机可读存储介质 - Google Patents

一种融合定位方法、装置、设备和计算机可读存储介质 Download PDF

Info

Publication number
CN112764053A
CN112764053A CN202011586133.3A CN202011586133A CN112764053A CN 112764053 A CN112764053 A CN 112764053A CN 202011586133 A CN202011586133 A CN 202011586133A CN 112764053 A CN112764053 A CN 112764053A
Authority
CN
China
Prior art keywords
pose
robot
point cloud
road sign
cloud 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.)
Granted
Application number
CN202011586133.3A
Other languages
English (en)
Other versions
CN112764053B (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.)
Shenzhen Pudu Technology Co Ltd
Original Assignee
Shenzhen Pudu Technology Co Ltd
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 Shenzhen Pudu Technology Co Ltd filed Critical Shenzhen Pudu Technology Co Ltd
Priority to CN202011586133.3A priority Critical patent/CN112764053B/zh
Publication of CN112764053A publication Critical patent/CN112764053A/zh
Priority to US18/248,780 priority patent/US20230400585A1/en
Priority to JP2023521741A priority patent/JP2023545279A/ja
Priority to EP21913733.8A priority patent/EP4206736A1/en
Priority to PCT/CN2021/135015 priority patent/WO2022142992A1/zh
Application granted granted Critical
Publication of CN112764053B publication Critical patent/CN112764053B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • 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
    • 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
    • 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
    • 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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0234—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons
    • G05D1/0236—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons in combination with a laser
    • 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/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Optics & Photonics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本申请涉及机器人领域，提供了融合定位方法、装置、设备和计算机可读存储介质，以将路标定位与二维栅格地图定位结合，实现对机器人的精确定位。所该方法包括：通过机器人搭载的二维激光雷达对环境物进行采样，获取环境物的雷达点云相关数据；将环境物中路标Li在路标地图上的位置Pi与路标地图进行邻近匹配，获取路标地图上距离位置Pi最近的路标Lj的位置Pj；使用环境物的去畸变点云数据在二维栅格地图进行扫面匹配，获取机器人在二维栅格地图中的全局位姿作为机器人的全局位姿观测值Gpose；根据路标Li在路标地图上的位置Pi、路标Lj在路标地图上的位置Pj、机器人的估计位姿和全局位姿观测值Gpose对机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止。

Description

一种融合定位方法、装置、设备和计算机可读存储介质
技术领域
本发明涉及机器人领域，特别涉及一种融合定位方法、装置、设备和计算机可读存储介质。
背景技术
移动机器人在各种场合的应用，精确定位是机器人能够顺利完成任务的关键。一般而言，机器人的定位是基于三维或二维激光雷达实现，其中，二维激光雷达的定位方法主要是使用激光雷达点云数据与预先构建的二维栅格地图进行匹配来实现机器人位姿的估计。然而，这种定位方式也遭遇一些问题，原因在于实际应用场景中存在一些长走廊、空旷等某个自由度超出二维激光雷达观测范围的情形。这种情形下，二维激光雷达无法完全约束机器人在二维空间位姿的三个自由度，从而导致机器人位姿估计错误。
针对机器人在长走廊、空旷环境下机器人定位时遇到的上述问题，一种方案是利用人工制作高反光的路标(Landmark)进行辅助定位，例如，使用反光板或反光柱进行辅助定位。然而，仅仅使用反光柱或反光板等高反光路标进行辅助定位，仍然无法适配对环境改造要求较高的环境。
发明内容
本申请提供一种融合定位方法、装置、设备和计算机可读存储介质，以将路标定位与二维栅格地图定位结合，实现对机器人的精确定位。
一方面，本申请提供了一种融合定位方法，包括：
通过机器人搭载的二维激光雷达对环境物进行采样，获取所述环境物的雷达点云相关数据，所述雷达点云相关数据包括所述路标Li在路标地图上的位置Pi和所述环境物的去畸变点云数据；
将所述路标Li在路标地图上的位置Pi与路标地图进行邻近匹配，获取所述路标地图上距离所述位置Pi最近的路标Lj的位置Pj；
使用所述环境物的去畸变点云数据在二维栅格地图进行扫面匹配，获取所述机器人在所述二维栅格地图中的全局位姿作为所述机器人的全局位姿观测值Gpose；
根据所述路标Li在路标地图上的位置Pi、所述路标Lj在所述路标地图上的位置Pj、所述机器人的估计位姿和所述全局位姿观测值Gpose，对所述机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止，所述误差E1为述路标Li与所述路标Lj在所述路标地图上的位置差值，所述误差E2为所述机器人的估计位姿与所述全局位姿观测值Gpose之间的差值。
另一方面，本申请提供了一种融合定位装置，包括：
第一获取模块，用于通过机器人搭载的二维激光雷达对环境物进行采样，获取所述环境物的雷达点云相关数据，所述雷达点云相关数据包括所述环境物中路标Li在路标地图上的位置Pi和所述环境物的去畸变点云数据；
第二获取模块，用于将所述路标Li在路标地图上的位置Pi与路标地图进行邻近匹配，获取所述路标地图上距离所述位置Pi最近的路标Lj的位置Pj；
第三获取模块，用于使用所述环境物的去畸变点云数据在二维栅格地图进行扫面匹配，获取所述机器人在所述二维栅格地图中的全局位姿作为所述机器人的全局位姿观测值Gpose；
迭代优化模块，用于根据所述路标Li在路标地图上的位置Pi、所述路标Lj在所述路标地图上的位置Pj、所述机器人的估计位姿和所述全局位姿观测值Gpose，对所述机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止，所述误差E1为述路标Li与所述路标Lj在所述路标地图上的位置差值，所述误差E2为所述机器人的估计位姿与所述全局位姿观测值Gpose之间的差值。
第三方面，本申请提供了一种设备，所述设备包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述融合定位方法的技术方案的步骤。
第四方面，本申请提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上述融合定位方法的技术方案的步骤。
从上述本申请提供的技术方案可知，本申请的技术方案一方面通过对环境物进行采样，匹配与环境物中路标Li的位置Pi最近的路标Lj，取得路标Li与路标Lj在路标地图上的位置差值E1；另一方面，使用环境物的去畸变点云数据在二维栅格地图进行扫面匹配，进而取得机器人的估计位姿与机器人全局位姿观测值Gpose之间的差值E2，最后对机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止，相对于现有技术仅仅采用路标辅助定位，本申请的技术方案融合了路标定位和二维栅格地图定位，从而使得对机器人的定位更加精确。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本申请实施例提供的融合定位方法的流程图；
图2是本申请实施例提供的融合定位装置的结构示意图；
图3是本申请实施例提供的设备的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
在本说明书中，诸如第一和第二这样的形容词仅可以用于将一个元素或动作与另一元素或动作进行区分，而不必要求或暗示任何实际的这种关系或顺序。在环境允许的情况下，参照元素或部件或步骤(等)不应解释为局限于仅元素、部件、或步骤中的一个，而可以是元素、部件、或步骤中的一个或多个等。
在本说明书中，为了便于描述，附图中所示的各个部分的尺寸并不是按照实际的比例关系绘制的。
本申请提出了一种融合定位方法，可应用于机器人，该机器人可以是在餐厅作业的机器人，例如，传菜机器人，也可以是在医疗场所，例如医院作业的送药机器人，还可以是在仓库等场所作业的搬运机器人，等等。如附图1所示，融合定位方法主要包括步骤S101至S104，详述如下：
步骤S101：通过机器人搭载的二维激光雷达对环境物进行采样，获取环境物的雷达点云相关数据，其中，雷达点云相关数据包括环境物中路标Li在路标地图上的位置Pi和环境物的去畸变点云数据。
在本申请实施例中，环境是指机器人工作的环境，环境物是指机器人工作的环境中的一切物体，包括在环境中设置的路标以及其他物体，例如，某种货物、一棵树、一堵墙或一张桌子，等等，而路标可以是在机器人工作环境中、二维激光雷达的可视范围内设置的反光柱、反光条等杆状物，其中，反光柱的直径可以为5厘米左右，反光条的宽度也可以为5厘米左右。为了发挥更准确的辅助定位作用，这些反光柱、反光条等路标至少需设置3个以上，并且尽量不要设置在同一直线上，间距也尽量保持大于1米，等等。
作为本申请一个实施例，通过机器人搭载的二维激光雷达对环境物进行采样，获取环境物的雷达点云相关数据可通过步骤S1011至步骤S1014实现，说明如下：
步骤S1011：通过二维激光雷达对环境物进行采样，得到环境物的原始雷达点云数据。
在本申请实施例中，机器人搭载的二维激光雷达的采样过程与现有技术一样，都是通过向周遭环境发射激光束，实时对当前环境进行扫描，采用飞行时间测距法计算得到机器人与环境中路标之间的距离，每一束激光击中环境物时，该激光束相对于机器人的角度以及激光源与被击中环境物之间的距离等信息，多个激光束击中环境物时的上述信息就构成环境物的原始雷达点云数据；二维激光雷达所采样的路标Li是环境中的任意一个路标。
步骤S1012：基于原始雷达点云数据中每个点的相对位姿，对环境物的原始雷达点云数据进行去畸变，得到环境物的去畸变点云数据。
由于二维激光雷达在对环境采样时，机器人在移动，因此，每次采样的点其在激光雷达坐标系下坐标的原点不一样。例如，二维激光雷达采样第二个点时，由于此时机器人产生了一个位移，二维激光雷达所在激光雷达坐标系的原点也产生了一个位移，同样地，第二个点至一个扫描周期结束时的最后一个点，每次采样的点，激光雷达坐标系的原点都不一样，这就使得雷达点云数据产生了畸变，这种畸变应当消除。具体而言，基于原始雷达点云数据中每个点的相对位姿，对环境物的原始雷达点云数据进行去畸变，得到环境物的去畸变点云数据可以是：确定一个扫描周期后，二维激光雷达所在激光雷达坐标系的新原点，将相对位姿△T与原始雷达点云数据中每个点在旧原点的激光雷达坐标系下的坐标P相乘，相乘后的结果△T*P作为原始雷达点云数据中每个点的坐标，其中，相对位姿△T为原始雷达点云数据中每个点相对于新原点得到的机器人位姿数据，其中，上述确定一个扫描周期后，二维激光雷达所在激光雷达坐标系的新原点，是指二维激光雷达一个扫描周期结束、二维激光雷达采样到一帧激光雷达点云数据最后一个点时，确定将此时二维激光雷达的中心点作为二维激光雷达所在激光雷达坐标系的新原点。
步骤S1013：对环境物中路标Li的去畸变点云数据进行聚类，得到路标Li的点云簇。
如前所述，环境物包括环境中设置的反光柱、反光条等路标，因此，经步骤S1012得到环境物的去畸变点云数据也包括环境物中路标Li的去畸变点云数据。至于如何提取环境物中路标Li的去畸变点云数据，一种方法是在对环境物的原始雷达点云数据进行去畸变，得到环境物的去畸变点云数据之后，将环境物的去畸变点云数据中各个点对应的激光反光强度与预设光强阈值相比，去除其中激光反光强度小于预设光强阈值的点，保留环境物的去畸变点云数据中激光反光强度高于预设光强阈值(即大于或等于预设光强阈值)的点云数据，将这些激光反光强度高于预设光强阈值的点云数据作为环境物中路标Li的去畸变点云数据。例如，若将二维激光雷达能够检测到的激光反光强度假设为0至1，由于反光柱、反光条等路标的激光反光强度一般在0.85以上，则可将预设光强阈值设置为0.85，然后，保留环境物的去畸变点云数据中激光反光强度高于0.85的点云数据，这些激光反光强度高于0.85的点云数据就是环境物中路标Li的去畸变点云数据。
考虑到具有噪声的基于密度的聚类(Density-Based Spatial Clustering ofApplications with Noise，DBSCAN)算法具有能够对任意形状的稠密数据集进行聚类，并且在聚类的同时发现异常点，聚类结果鲜有发生偏倚等优点，在本申请实施例中，可以采用DBSCAN算法对环境物中路标Li的去畸变点云数据进行聚类，得到路标Li的点云簇。
步骤S1014：求取路标Li的点云簇在机器人本体坐标系下的几何中心坐标，将路标Li的点云簇在机器人本体坐标系下的几何中心坐标作为路标Li在路标地图上的位置Pi。
在经过聚类，得到路标Li的点云簇之后，可以将路标Li的点云簇视为一个质点，可以用几何或者物理方法求取路标Li的点云簇在机器人本体坐标系下的几何中心坐标，将路标Li的点云簇在机器人本体坐标系下的几何中心坐标作为路标Li在路标地图上的位置Pi。由于本申请实施例中，反光柱直径和反光条宽度都比较小，通过简单地求取路标Li的点云簇几何中心，基本能够近似地反映反光柱、反光条等路标的几何中心。
步骤S102：将环境物中路标Li在路标地图上的位置Pi与路标地图进行邻近匹配，获取路标地图上距离位置Pi最近的路标Lj的位置Pj。
在本申请实施例中，路标地图是指预先制作的反映路标(Landmark)在世界坐标系中位置的地图，路标地图中的点即视为一个路标。如前所述，路标Li在路标地图上的位置Pi即路标Li的点云簇在机器人本体坐标系下的几何中心坐标。具体地，可以采用最近邻算法，例如kNN(k-NearestNeighbor，k最近邻)算法将路标Li在路标地图上的位置Pi与路标地图进行邻近匹配，获取路标地图上距离位置Pi最近的路标Lj的位置Pj。
步骤S103：使用环境物的去畸变点云数据在二维栅格地图进行扫面匹配，获取机器人在二维栅格地图中的全局位姿作为机器人的全局位姿观测值Gpose。
在本申请实施例中，二维栅格地图也称为二维栅格概率地图或二维占据栅格地图(Occupancy Grid Map)，这种地图是将平面划分为一个个栅格，每个栅格赋予一个占据率(Occupancy)。所谓占据率，是指二维占据栅格地图上，一个栅格被障碍物占据的状态(occupied状态)、无障碍物占据的状态(Free状态)或者介于这两种状态之间的概率，其中，被障碍物占据的状态使用1表示，无障碍物占据的状态使用0表示，介于两种状态之间则使用0～1之间的值来表示，显然，占据率表明了一个栅格被障碍物占据的可能性大小，当一个栅格的占据率越大，表明该栅格被障碍物占据的可能性越大，反之则反。
作为本申请一个实施例，使用环境物的去畸变点云数据在二维栅格地图进行扫面匹配，获取机器人在二维栅格地图中的全局位姿作为机器人的全局位姿观测值Gpose可以通过步骤S1031至步骤S1033实现，说明如下：
步骤S1031：由机器人当前时刻前一时刻的位姿确定位姿搜索空间中的若干候选位姿。
所谓机器人的位姿，是指机器人在世界坐标系中的状态，该状态使用位置和姿态表示，具体是以机器人在世界坐标系中的横坐标x、纵坐标y和相对于横轴的夹角θ，即(x，y，θ)表示了机器人在某个时刻的位姿。假设通过估计，得到的机器人当前时刻前一时刻的位姿为(x，y，θ)，那么，在该位姿上对其每个分量给予一个增量△x、△y和△θ，则在位姿搜索空间(△x，△y，△θ)中存在若干候选位姿(x1，y1，θ1)、(x2，y2，θ2)、…、(xi，yi，θi)、…、(xn，yn，θn)。
步骤S1032：利用若干候选位姿中的每一个候选位姿，将环境物的去畸变点云数据投影至二维栅格地图，计算每一个候选位姿在二维栅格地图上的匹配得分。
此处，将环境物的去畸变点云数据投影至二维栅格地图，是指按照某种变换关系，将环境物的去畸变点云数据变换至二维栅格地图上的某个占据位置或者栅格，当候选位姿不同时，环境物的去畸变点云数据会变换至二维栅格地图上不同的栅格。假设某个候选位姿(xi，yi，θi)，环境物的去畸变点云数据为(xj，yj，θj)，对应于候选位姿(xi，yi，θi)，该(xj，yj，θj)投影至二维栅格地图上的栅格位置用
Figure BDA0002866946870000081
表示，则：
Figure BDA0002866946870000082
其中，zk是二维栅格地图上位置为
Figure BDA0002866946870000083
的栅格的编号。
而如前所述，不同的栅格被赋予了不同占据率。对应于某个候选位姿(xi，yi，θi)，其在二维栅格地图上的匹配得分为环境物的去畸变点云数据中各个去畸变点云数据投影至二维栅格地图上不同栅格时，这些不同栅格的占据率之和，即，环境物的去畸变点云数据中任意一个去畸变点云数据(xj，yj，θj)投影至二维栅格地图上位置为
Figure BDA0002866946870000084
的栅格，其对应的占据率为Oj，对应于候选位姿(xi，yi，θi)，该环境物的去畸变点云数据投影至二维栅格地图时在二维栅格地图上的匹配得分
Figure BDA0002866946870000091
其中，m为该环境物的去畸变点云数据的去畸变点云数据个数。
步骤S1033：将若干候选位姿在二维栅格地图上的匹配得分最高的候选位姿确定为机器人在二维栅格地图中的全局位姿。
从步骤S1032分析可知，对于上述n个候选位姿，将得到n个不同的匹配得分Scorei，将n个候选位姿在二维栅格地图上的匹配得分最高的候选位姿确定为机器人在二维栅格地图中的全局位姿，即，选择Scorek＝max{Scorei}，将与Scorek对应的候选位姿(xk，yk，θk)确定为机器人在二维栅格地图中的全局位姿，该全局位姿作为机器人的全局位姿观测值Gpose。
步骤S104：根据路标Li在路标地图上的位置Pi、路标Lj在路标地图上的位置Pj、机器人的估计位姿和全局位姿观测值Gpose，对机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止，其中，误差E1为路标Li与路标Lj在路标地图上的位置差值，误差E2为机器人的估计位姿与全局位姿观测值Gpose之间的差值。
作为本申请一个实施例，根据路标Li在路标地图上的位置Pi、路标Lj在路标地图上的位置Pj、机器人的估计位姿和全局位姿观测值Gpose，对机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止可以是：以机器人的位姿作为状态构建非线性最小二乘化问题，以误差E1为非线性最小二乘化问题的误差约束条件，迭代优化机器人的位姿，直至误差E1和误差E2最小时为止，此处，误差E1为路标Li与路标Lj在路标地图上的位置差值，误差E2为机器人的估计位姿与全局位姿观测值Gpose之间的差值。
作为本申请另一实施例，根据路标Li在路标地图上的位置Pi、路标Lj在路标地图上的位置Pj、机器人的估计位姿和全局位姿观测值Gpose，对机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止还可以是：基于路标Li在路标地图上的位置Pi、路标Lj在路标地图上的位置Pj、机器人的估计位姿和全局位姿观测值Gpose，构建粒子滤波模型或扩展卡尔曼滤波模型，采用粒子滤波模型或扩展卡尔曼滤波模型，持续对机器人的估计位姿进行校正，直至误差E1和误差E2最小时为止，此处的误差E1和误差E2含义与前述实施例提及的误差E1和误差E2的含义相同。在本申请实施例中，扩展卡尔曼滤波(Extended Kalman Filter，EKF)是标准卡尔曼滤波在非线性情形下的一种扩展形式，它是一种高效率的递归滤波器即自回归滤波器，其基本思想是利用泰勒级数展开将非线性系统线性化，然后采用卡尔曼滤波框架对数据进行滤波，粒子滤波是通过寻找一组在状态空间中传播的随机样本来近似地表示概率密度函数，采用样本均值代替积分运算，进而获得系统状态的最小方差估计的过程，这些样本被形象地称为“粒子”。
上述基于路标Li在路标地图上的位置Pi、路标Lj在路标地图上的位置Pj、机器人的估计位姿和全局位姿观测值Gpose，构建粒子滤波模型或扩展卡尔曼滤波模型，采用粒子滤波模型，持续对机器人的估计位姿进行校正，直至误差E1和误差E2最小时为止可以是：基于机器人的估计位姿和预设时间段内的位姿变化量，获取当前粒子群，粒子群中的每一个粒子表示机器人的一个位姿，粒子群用于表示机器人位姿的概率分布；为当前粒子群中的每一个粒子分配权重，并根据分配的权重获得机器人的第一估计位姿，其中，权重用于表征每一个粒子为当前实际位姿的概率；基于第一估计位姿、路标Li在路标地图上的位置Pi、路标Lj在路标地图上的位置Pj和全局位姿观测值Gpose，采用预设扫描匹配模型进行扫描匹配，得到第二估计位姿；持续对第二估计位姿进行校正，直至误差E1和误差E2时得到的估计位姿作为机器人的最终位姿。
从上述附图1示例的融合定位方法可知，本申请的技术方案一方面通过环境物进行采样，匹配与环境物中路标Li的位置Pi最近的路标Lj，取得路标Li与路标Lj在路标地图上的位置差值E1；另一方面，使用环境物的去畸变点云数据在二维栅格地图进行扫面匹配，进而取得机器人的估计位姿与机器人全局位姿观测值Gpose之间的差值E2，最后对机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止，相对于现有技术仅仅采用路标辅助定位，本申请的技术方案融合了路标定位和二维栅格地图定位，从而使得对机器人的定位更加精确。
请参阅附图2，是本申请实施例提供的一种融合定位装置，可以包括第一获取模块201、第二获取模块202、第三获取模块203和迭代优化模块204，详述如下：
第一获取模块201，用于通过机器人搭载的二维激光雷达对环境物进行采样，获取环境物的雷达点云相关数据，其中，雷达点云相关数据包括环境物中路标Li在路标地图上的位置Pi和环境物的去畸变点云数据；
第二获取模块202，用于将环境物中路标Li在路标地图上的位置Pi与路标地图进行邻近匹配，获取路标地图上距离位置Pi最近的路标Lj的位置Pj；
第三获取模块203，用于使用环境物的去畸变点云数据在二维栅格地图进行扫面匹配，获取机器人在二维栅格地图中的全局位姿作为机器人的全局位姿观测值Gpose；
迭代优化模块204，用于根据路标Li在路标地图上的位置Pi、路标Lj在路标地图上的位置Pj、机器人的估计位姿和全局位姿观测值Gpose，对机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止，其中，误差E1为路标Li与路标Lj在路标地图上的位置差值，误差E2为机器人的估计位姿与全局位姿观测值Gpose之间的差值。
可选地，附图2示例的第一获取模块201可以包括采样单元、校正单元、聚类单元和中心求取单元，其中：
采样单元，用于通过二维激光雷达对环境物进行采样，得到环境物的原始雷达点云数据；
校正单元，用于基于原始雷达点云数据中每个点的相对位姿，对环境物的原始雷达点云数据进行去畸变，得到环境物的去畸变点云数据；
聚类单元，用于对环境物中路标Li的去畸变点云数据进行聚类，得到路标Li的点云簇；
中心求取单元，用于求取路标Li的点云簇在机器人本体坐标系下的几何中心坐标，将路标Li的点云簇在机器人本体坐标系下的几何中心坐标作为路标Li在路标地图上的位置Pi。
可选地，上述校正单元可包括新原点确定单元和相乘单元，其中：
新原点确定单元，用于确定一个扫描周期后，二维激光雷达所在激光雷达坐标系的新原点；
相乘单元，用于将相对位姿△T与原始雷达点云数据中每个点在旧原点的激光雷达坐标系下的坐标P相乘，相乘后的结果△T*P作为原始雷达点云数据中每个点的坐标，其中，相对位姿△T为原始雷达点云数据中每个点相对于新原点得到的机器人位姿数据。
可选地，附图2示例的装置还可以包括比较模块和筛选模块，其中：
比较模块，用于将环境物的去畸变点云数据中各个点对应的激光反光强度与预设光强阈值相比；
筛选模块，用于保留环境物的去畸变点云数据中激光反光强度高于预设光强阈值的点云数据，作为环境物中路标Li的去畸变点云数据。
可选地，附图2示例的第三获取模块203可以包括搜索单元、匹配单元和全局位姿确定单元，其中：
搜索单元，用于由机器人当前时刻前一时刻的位姿确定位姿搜索空间中的若干候选位姿；
匹配单元，用于利用若干候选位姿中的每一个候选位姿，将环境物的去畸变点云数据投影至二维栅格地图，计算每一个候选位姿在二维栅格地图上的匹配得分；
全局位姿确定单元，用于将若干候选位姿在二维栅格地图上的匹配得分最高的候选位姿确定为机器人在二维栅格地图中的全局位姿。
可选地，附图2示例的迭代优化模块204可以包括问题构建单元和第一校正单元，其中：
构建单元，用于以机器人的位姿作为状态构建非线性最小二乘化问题；
第一校正单元，用于以误差E1为非线性最小二乘化问题的误差约束条件，迭代优化机器人的位姿，直至误差E1和误差E2最小时为止。
可选地，附图2示例的迭代优化模块204可以包括模型构建单元和第二校正单元，其中:
模型构建单元，用于基于路标Li在路标地图上的位置Pi、路标Lj在路标地图上的位置Pj、机器人的估计位姿和全局位姿观测值Gpose，构建粒子滤波模型或扩展卡尔曼滤波模型；
第二校正单元，用于采用粒子滤波模型或扩展卡尔曼滤波模型，持续对机器人的估计位姿进行校正，直至误差E1和误差E2最小时为止。
从以上技术方案的描述中可知，本申请的技术方案一方面通过环境物进行采样，匹配与环境物中路标Li的位置Pi最近的路标Lj，取得路标Li与路标Lj在路标地图上的位置差值E1；另一方面，使用环境物的去畸变点云数据在二维栅格地图进行扫面匹配，进而取得机器人的估计位姿与机器人全局位姿观测值Gpose之间的差值E2，最后对机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止，相对于现有技术仅仅采用路标辅助定位，本申请的技术方案融合了路标定位和二维栅格地图定位，从而使得对机器人的定位更加精确。
请参阅图3，是本申请一实施例提供的设备的结构示意图。如图3所示，该实施例的设备3主要包括：处理器30、存储器31以及存储在存储器31中并可在处理器30上运行的计算机程序32，例如融合定位方法的程序。处理器30执行计算机程序32时实现上述融合定位方法实施例中的步骤，例如图1所示的步骤S101至S104。或者，处理器30执行计算机程序32时实现上述各装置实施例中各模块/单元的功能，例如图2所示第一获取模块201、第二获取模块202、第三获取模块203和迭代优化模块204的功能。
示例性地，融合定位方法的计算机程序32主要包括：通过机器人搭载的二维激光雷达对环境物进行采样，获取环境物的雷达点云相关数据，其中，雷达点云相关数据包括环境物中路标Li在路标地图上的位置Pi和环境物的去畸变点云数据；将环境物中路标Li在路标地图上的位置Pi与路标地图进行邻近匹配，获取路标地图上距离位置Pi最近的路标Lj的位置Pj；使用环境物的去畸变点云数据在二维栅格地图进行扫面匹配，获取机器人在二维栅格地图中的全局位姿作为机器人的全局位姿观测值Gpose；根据路标Li在路标地图上的位置Pi、路标Lj在路标地图上的位置Pj、机器人的估计位姿和全局位姿观测值Gpose，对机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止，其中，误差E1为路标Li与路标Lj在路标地图上的位置差值，误差E2为机器人的估计位姿与全局位姿观测值Gpose之间的差值。计算机程序32可以被分割成一个或多个模块/单元，一个或者多个模块/单元被存储在存储器31中，并由处理器30执行，以完成本申请。一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述计算机程序32在设备3中的执行过程。例如，计算机程序32可以被分割成第一获取模块201、第二获取模块202、第三获取模块203和迭代优化模块204(虚拟装置中的模块)的功能，各模块具体功能如下：第一获取模块201，用于通过机器人搭载的二维激光雷达对环境物进行采样，获取环境物的雷达点云相关数据，其中，雷达点云相关数据包括环境物中路标Li在路标地图上的位置Pi和环境物的去畸变点云数据；第二获取模块202，用于将环境物中路标Li在路标地图上的位置Pi与路标地图进行邻近匹配，获取路标地图上距离位置Pi最近的路标Lj的位置Pj；第三获取模块203，用于使用环境物的去畸变点云数据在二维栅格地图进行扫面匹配，获取机器人在二维栅格地图中的全局位姿作为机器人的全局位姿观测值Gpose；迭代优化模块204，用于根据路标Li在路标地图上的位置Pi、路标Lj在路标地图上的位置Pj、机器人的估计位姿和全局位姿观测值Gpose，对机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止，其中，误差E1为路标Li与路标Lj在路标地图上的位置差值，误差E2为机器人的估计位姿与全局位姿观测值Gpose之间的差值。
设备3可包括但不仅限于处理器30、存储器31。本领域技术人员可以理解，图3仅仅是设备3的示例，并不构成对设备3的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如计算设备还可以包括输入输出设备、网络接入设备、总线等。
所称处理器30可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
存储器31可以是设备3的内部存储单元，例如设备3的硬盘或内存。存储器31也可以是设备3的外部存储设备，例如设备3上配备的插接式硬盘，智能存储卡(Smart MediaCard，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，存储器31还可以既包括设备3的内部存储单元也包括外部存储设备。存储器31用于存储计算机程序以及设备所需的其他程序和数据。存储器31还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即，将装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述装置中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
在本申请所提供的实施例中，应该理解到，所揭露的装置/设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/设备实施例仅仅是示意性的，例如，模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个装置，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个非临时性计算机可读取存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，融合定位方法的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤，即，通过机器人搭载的二维激光雷达对环境物进行采样，获取环境物的雷达点云相关数据，其中，雷达点云相关数据包括环境物中路标Li在路标地图上的位置Pi和环境物的去畸变点云数据；将环境物中路标Li在路标地图上的位置Pi与路标地图进行邻近匹配，获取路标地图上距离位置Pi最近的路标Lj的位置Pj；使用环境物的去畸变点云数据在二维栅格地图进行扫面匹配，获取机器人在二维栅格地图中的全局位姿作为机器人的全局位姿观测值Gpose；根据路标Li在路标地图上的位置Pi、路标Lj在路标地图上的位置Pj、机器人的估计位姿和全局位姿观测值Gpose，对机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止，其中，误差E1为路标Li与路标Lj在路标地图上的位置差值，误差E2为机器人的估计位姿与全局位姿观测值Gpose之间的差值。其中，计算机程序包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。非临时性计算机可读介质可以包括：能够携带计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读内存(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，非临时性计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，非临时性计算机可读介质不包括电载波信号和电信信号。以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。以上所述的具体实施方式，对本申请的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本申请的具体实施方式而已，并不用于限定本申请的保护范围，凡在本申请的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种融合定位方法，其特征在于，所述方法包括：
通过机器人搭载的二维激光雷达对环境物进行采样，获取所述环境物的雷达点云相关数据，所述雷达点云相关数据包括所述环境物中路标Li在路标地图上的位置Pi和所述环境物的去畸变点云数据；
将所述路标Li在路标地图上的位置Pi与路标地图进行邻近匹配，获取所述路标地图上距离所述位置Pi最近的路标Lj的位置Pj；
使用所述环境物的去畸变点云数据在二维栅格地图进行扫面匹配，获取所述机器人在所述二维栅格地图中的全局位姿作为所述机器人的全局位姿观测值Gpose；
根据所述路标Li在路标地图上的位置Pi、所述路标Lj在所述路标地图上的位置Pj、所述机器人的估计位姿和所述全局位姿观测值Gpose，对所述机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止，所述误差E1为所述路标Li与所述路标Lj在所述路标地图上的位置差值，所述误差E2为所述机器人的估计位姿与所述全局位姿观测值Gpose之间的差值。
2.如权利要求1所述融合定位方法，其特征在于，所述通过机器人搭载的二维激光雷达对环境物进行采样，获取所述环境物的雷达点云相关数据，包括：
通过所述二维激光雷达对所述环境物进行采样，得到所述环境物的原始雷达点云数据；
基于所述原始雷达点云数据中每个点的相对位姿，对所述环境物的原始雷达点云数据进行去畸变，得到所述环境物的去畸变点云数据；
对所述环境物中路标Li的去畸变点云数据进行聚类，得到所述路标Li的点云簇；
求取所述点云簇在所述机器人本体坐标系下的几何中心坐标，将所述点云簇在所述机器人本体坐标系下的几何中心坐标作为所述路标Li在路标地图上的位置Pi。
3.如权利要求2所述融合定位方法，其特征在于，所述基于所述原始雷达点云数据中每个点的相对位姿，对所述环境物的原始雷达点云数据进行去畸变，得到所述环境物的去畸变点云数据，包括：
确定一个扫描周期后，所述二维激光雷达所在激光雷达坐标系的新原点；
将相对位姿△T与所述原始雷达点云数据中每个点在旧原点的激光雷达坐标系下的坐标P相乘，相乘后的结果△T*P作为所述原始雷达点云数据中每个点的坐标，所述相对位姿△T为所述原始雷达点云数据中每个点相对于所述新原点得到的机器人位姿数据。
4.如权利要求2所述融合定位方法，其特征在于，所述对所述环境物的原始雷达点云数据进行去畸变，得到所述环境物的去畸变点云数据之后，所述方法还包括：
将所述环境物的去畸变点云数据中各个点对应的激光反光强度与预设光强阈值相比；
保留所述环境物的去畸变点云数据中激光反光强度高于所述预设光强阈值的点云数据，作为所述环境物中路标Li的去畸变点云数据。
5.如权利要求1所述融合定位方法，其特征在于，所述使用所述环境物的去畸变点云数据在二维栅格地图进行扫面匹配，获取所述机器人在所述二维栅格地图中的全局位姿作为所述机器人的全局位姿观测值Gpose，包括：
由所述机器人当前时刻前一时刻的位姿确定位姿搜索空间中的若干候选位姿；
利用所述若干候选位姿中的每一个候选位姿，将所述环境物的去畸变点云数据投影至所述二维栅格地图，计算所述每一个候选位姿在所述二维栅格地图上的匹配得分；
将所述若干候选位姿在所述二维栅格地图上的匹配得分最高的候选位姿确定为所述机器人在所述二维栅格地图中的全局位姿。
6.如权利要求1所述融合定位方法，其特征在于，所述根据所述路标Li在路标地图上的位置Pi、所述路标Lj在所述路标地图上的位置Pj、所述机器人的估计位姿和所述全局位姿观测值Gpose，对所述机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止，包括：
以所述机器人的位姿作为状态构建非线性最小二乘化问题；
以所述误差E1为所述非线性最小二乘化问题的误差约束条件，迭代优化所述机器人的位姿，直至所述误差E1和所述误差E2最小时为止。
7.如权利要求1所述融合定位方法，其特征在于，所述根据所述路标Li在路标地图上的位置Pi、所述路标Lj在所述路标地图上的位置Pj、所述机器人的估计位姿和所述全局位姿观测值Gpose，对所述机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止，包括:
基于所述路标Li在路标地图上的位置Pi、所述路标Lj在所述路标地图上的位置Pj、所述机器人的估计位姿和所述全局位姿观测值Gpose，构建粒子滤波模型或扩展卡尔曼滤波模型；
采用所述粒子滤波模型或扩展卡尔曼滤波模型，持续对所述机器人的估计位姿进行校正，直至误差E1和误差E2最小时为止。
8.一种融合定位装置，其特征在于，所述装置包括：
第一获取模块，用于通过机器人搭载的二维激光雷达对环境物进行采样，获取所述环境物的雷达点云相关数据，所述雷达点云相关数据包括所述环境物中路标Li在路标地图上的位置Pi和所述环境物的去畸变点云数据；
第二获取模块，用于将所述路标Li在路标地图上的位置Pi与路标地图进行邻近匹配，获取所述路标地图上距离所述位置Pi最近的路标Lj的位置Pj；
第三获取模块，用于使用所述环境物的去畸变点云数据在二维栅格地图进行扫面匹配，获取所述机器人在所述二维栅格地图中的全局位姿作为所述机器人的全局位姿观测值Gpose；
迭代优化模块，用于根据所述路标Li在路标地图上的位置Pi、所述路标Lj在所述路标地图上的位置Pj、所述机器人的估计位姿和所述全局位姿观测值Gpose，对所述机器人的位姿进行迭代优化，直至误差E1和误差E2最小时为止，所述误差E1为所述路标Li与所述路标Lj在所述路标地图上的位置差值，所述误差E2为所述机器人的估计位姿与所述全局位姿观测值Gpose之间的差值。
9.一种设备，所述设备包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7任意一项所述方法的步骤。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7任意一项所述方法的步骤。
CN202011586133.3A 2020-12-29 2020-12-29 一种融合定位方法、装置、设备和计算机可读存储介质 Active CN112764053B (zh)

Priority Applications (5)

Application Number Priority Date Filing Date Title
CN202011586133.3A CN112764053B (zh) 2020-12-29 2020-12-29 一种融合定位方法、装置、设备和计算机可读存储介质
US18/248,780 US20230400585A1 (en) 2020-12-29 2021-12-02 Fusion positioning method and apparatus, device and computer-readable storage medium
JP2023521741A JP2023545279A (ja) 2020-12-29 2021-12-02 融合測位方法、装置、デバイス、及びコンピューター可読記憶媒体
EP21913733.8A EP4206736A1 (en) 2020-12-29 2021-12-02 Fusion positioning method and apparatus, device and computer-readable storage medium
PCT/CN2021/135015 WO2022142992A1 (zh) 2020-12-29 2021-12-02 融合定位方法、装置、设备和计算机可读存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011586133.3A CN112764053B (zh) 2020-12-29 2020-12-29 一种融合定位方法、装置、设备和计算机可读存储介质

Publications (2)

Publication Number Publication Date
CN112764053A true CN112764053A (zh) 2021-05-07
CN112764053B CN112764053B (zh) 2022-07-15

Family

ID=75696343

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011586133.3A Active CN112764053B (zh) 2020-12-29 2020-12-29 一种融合定位方法、装置、设备和计算机可读存储介质

Country Status (5)

Country Link
US (1) US20230400585A1 (zh)
EP (1) EP4206736A1 (zh)
JP (1) JP2023545279A (zh)
CN (1) CN112764053B (zh)
WO (1) WO2022142992A1 (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114355325A (zh) * 2021-12-17 2022-04-15 武汉大学 一种适用于毫米波雷达的室内自定位方法及系统
CN114643579A (zh) * 2022-03-29 2022-06-21 深圳优地科技有限公司 一种机器人定位方法、装置、机器人及存储介质
WO2022142992A1 (zh) * 2020-12-29 2022-07-07 深圳市普渡科技有限公司 融合定位方法、装置、设备和计算机可读存储介质
CN115183778A (zh) * 2022-07-01 2022-10-14 北京斯年智驾科技有限公司 一种基于码头石墩的建图方法、装置、设备以及介质
WO2024001083A1 (zh) * 2022-06-28 2024-01-04 广东利元亨智能装备股份有限公司 一种定位方法、装置、设备和存储介质
WO2024001649A1 (zh) * 2022-06-29 2024-01-04 深圳市海柔创新科技有限公司 机器人定位方法、装置和计算可读存储介质
CN117570998A (zh) * 2024-01-17 2024-02-20 山东大学 基于反光柱信息的机器人定位方法及系统

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115290098B (zh) * 2022-09-30 2022-12-23 成都朴为科技有限公司 一种基于变步长的机器人定位方法和系统
CN115307646B (zh) * 2022-10-08 2023-03-24 浙江光珀智能科技有限公司 一种多传感器融合的机器人定位方法、系统及装置
CN115686011A (zh) * 2022-10-31 2023-02-03 广州高新兴机器人有限公司 机器人沿栅栏沿边行走的算法、系统及电子设备
CN115421125B (zh) * 2022-11-07 2023-01-10 山东富锐光学科技有限公司 一种基于数据融合的雷达点云数据惯性校正方法
CN116164747B (zh) * 2022-12-15 2023-09-05 广东智能无人系统研究院(南沙) 一种水下机器人的定位与导航方法及系统

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180299273A1 (en) * 2017-04-17 2018-10-18 Baidu Online Network Technology (Beijing) Co., Ltd. Method and apparatus for positioning vehicle
CN108917759A (zh) * 2018-04-19 2018-11-30 电子科技大学 基于多层次地图匹配的移动机器人位姿纠正算法
CN109270544A (zh) * 2018-09-20 2019-01-25 同济大学 基于杆状物识别的移动机器人自定位系统
CN110221603A (zh) * 2019-05-13 2019-09-10 浙江大学 一种基于激光雷达多帧点云融合的远距离障碍物检测方法
CN110487286A (zh) * 2019-08-09 2019-11-22 上海电器科学研究所（集团）有限公司 一种基于点特征投影与激光点云融合的机器人位姿判断法
CN110927740A (zh) * 2019-12-06 2020-03-27 合肥科大智能机器人技术有限公司 一种移动机器人定位方法
CN111045017A (zh) * 2019-12-20 2020-04-21 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法
CN111812669A (zh) * 2020-07-16 2020-10-23 南京航空航天大学 绕机检查装置及其定位方法、存储介质

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106908775B (zh) * 2017-03-08 2019-10-18 同济大学 一种基于激光反射强度的无人车实时定位方法
US20200301015A1 (en) * 2019-03-21 2020-09-24 Foresight Ai Inc. Systems and methods for localization
CN111060092B (zh) * 2019-12-31 2021-07-30 芜湖哈特机器人产业技术研究院有限公司 一种反光柱的快速匹配方法
CN111781609B (zh) * 2020-04-10 2022-11-29 昆山同日智能技术有限公司 一种agv激光导航多边定位方法
CN112764053B (zh) * 2020-12-29 2022-07-15 深圳市普渡科技有限公司 一种融合定位方法、装置、设备和计算机可读存储介质

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180299273A1 (en) * 2017-04-17 2018-10-18 Baidu Online Network Technology (Beijing) Co., Ltd. Method and apparatus for positioning vehicle
CN108917759A (zh) * 2018-04-19 2018-11-30 电子科技大学 基于多层次地图匹配的移动机器人位姿纠正算法
CN109270544A (zh) * 2018-09-20 2019-01-25 同济大学 基于杆状物识别的移动机器人自定位系统
CN110221603A (zh) * 2019-05-13 2019-09-10 浙江大学 一种基于激光雷达多帧点云融合的远距离障碍物检测方法
CN110487286A (zh) * 2019-08-09 2019-11-22 上海电器科学研究所（集团）有限公司 一种基于点特征投影与激光点云融合的机器人位姿判断法
CN110927740A (zh) * 2019-12-06 2020-03-27 合肥科大智能机器人技术有限公司 一种移动机器人定位方法
CN111045017A (zh) * 2019-12-20 2020-04-21 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法
CN111812669A (zh) * 2020-07-16 2020-10-23 南京航空航天大学 绕机检查装置及其定位方法、存储介质

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022142992A1 (zh) * 2020-12-29 2022-07-07 深圳市普渡科技有限公司 融合定位方法、装置、设备和计算机可读存储介质
CN114355325A (zh) * 2021-12-17 2022-04-15 武汉大学 一种适用于毫米波雷达的室内自定位方法及系统
CN114355325B (zh) * 2021-12-17 2024-05-28 武汉大学 一种适用于毫米波雷达的室内自定位方法及系统
CN114643579A (zh) * 2022-03-29 2022-06-21 深圳优地科技有限公司 一种机器人定位方法、装置、机器人及存储介质
CN114643579B (zh) * 2022-03-29 2024-01-16 深圳优地科技有限公司 一种机器人定位方法、装置、机器人及存储介质
WO2024001083A1 (zh) * 2022-06-28 2024-01-04 广东利元亨智能装备股份有限公司 一种定位方法、装置、设备和存储介质
WO2024001649A1 (zh) * 2022-06-29 2024-01-04 深圳市海柔创新科技有限公司 机器人定位方法、装置和计算可读存储介质
CN115183778A (zh) * 2022-07-01 2022-10-14 北京斯年智驾科技有限公司 一种基于码头石墩的建图方法、装置、设备以及介质
CN117570998A (zh) * 2024-01-17 2024-02-20 山东大学 基于反光柱信息的机器人定位方法及系统
CN117570998B (zh) * 2024-01-17 2024-04-02 山东大学 基于反光柱信息的机器人定位方法及系统

Also Published As

Publication number Publication date
JP2023545279A (ja) 2023-10-27
CN112764053B (zh) 2022-07-15
EP4206736A1 (en) 2023-07-05
US20230400585A1 (en) 2023-12-14
WO2022142992A1 (zh) 2022-07-07

Similar Documents

Publication Publication Date Title
CN112764053B (zh) 一种融合定位方法、装置、设备和计算机可读存储介质
CN108732582B (zh) 车辆定位方法和装置
CN108871353B (zh) 路网地图生成方法、系统、设备及存储介质
EP3620823B1 (en) Method and device for detecting precision of internal parameter of laser radar
CN111060888B (zh) 一种融合icp和似然域模型的移动机器人重定位方法
US10627520B2 (en) Method and apparatus for constructing reflectance map
KR20220053513A (ko) 이미지 데이터 자동 라벨링 방법 및 장치
CN108734780B (zh) 用于生成地图的方法、装置和设备
CN111694903A (zh) 地图构建方法、装置、设备以及可读存储介质
CN109190573B (zh) 应用于无人驾驶车辆的地面检测方法、电子设备、车辆
WO2020102944A1 (zh) 点云处理方法、设备及存储介质
CN111552289B (zh) 检测方法及虚拟雷达装置、电子设备、存储介质
CN113989450A (zh) 图像处理方法、装置、电子设备和介质
CN111113422B (zh) 机器人定位方法、装置、计算机可读存储介质及机器人
CN114758086B (zh) 一种城市道路信息模型的构建方法及装置
CN114820749A (zh) 无人车井下定位方法、系统、设备及介质
CN112686951A (zh) 用于确定机器人位置的方法、装置、终端及存储介质
CN114882316A (zh) 目标检测模型训练方法、目标检测方法及装置
JP7351892B2 (ja) 障害物検出方法、電子機器、路側機器、及びクラウド制御プラットフォーム
CN115326051A (zh) 一种基于动态场景的定位方法、装置、机器人及介质
CN111611900A (zh) 一种目标点云识别方法、装置、电子设备和存储介质
CN109345567B (zh) 物体运动轨迹识别方法、装置、设备和存储介质
CN112381873A (zh) 一种数据标注方法及装置
CN110853098B (zh) 机器人定位方法、装置、设备及存储介质
CN109598199B (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