CN117367419A - 机器人定位方法、装置和计算可读存储介质 - Google Patents

机器人定位方法、装置和计算可读存储介质 Download PDF

Info

Publication number
CN117367419A
CN117367419A CN202210746832.2A CN202210746832A CN117367419A CN 117367419 A CN117367419 A CN 117367419A CN 202210746832 A CN202210746832 A CN 202210746832A CN 117367419 A CN117367419 A CN 117367419A
Authority
CN
China
Prior art keywords
robot
pose
data
laser
point cloud
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
Application number
CN202210746832.2A
Other languages
English (en)
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.)
Hai Robotics Co Ltd
Original Assignee
Hai Robotics 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 Hai Robotics Co Ltd filed Critical Hai Robotics Co Ltd
Priority to CN202210746832.2A priority Critical patent/CN117367419A/zh
Priority to PCT/CN2023/097297 priority patent/WO2024001649A1/zh
Priority to TW112122106A priority patent/TW202411613A/zh
Publication of CN117367419A publication Critical patent/CN117367419A/zh
Pending legal-status Critical Current

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/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
    • 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/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
    • 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/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
    • 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
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system

Landscapes

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

Abstract

本申请涉及一种机器人定位方法、装置和计算可读存储介质。该方法包括：通过机器人搭载的激光传感器获取激光点云数据；通过机器人搭载的运动类传感器获取机器人的位姿数据；根据导航二维码和运动类传感器获取的机器人的位姿数据并基于扩展卡尔曼滤波器，计算得到机器人的先验位姿；基于机器人的先验位姿，将激光点云数据与机器人地图匹配，得到机器人的第一位姿；将机器人的第一位姿与扩展卡尔曼滤波器当前输出的第二位姿融合，得到机器人最终的位姿。本申请提供的方案可以提升机器人定位的准确性。

Description

机器人定位方法、装置和计算可读存储介质
技术领域
本申请涉及人工智能领域，尤其涉及机器人定位方法、装置和计算可读存储介质。
背景技术
随着人工智能(Artificial Intelligence，AI)技术的发展，一些赋予了AI的机器人广泛应用于餐饮、医疗和仓储等领域。在应用这些AI机器人时，比较关键之处是能够对机器人进行精确定位，因为只有对机器人进行精确定位，才能够使得机器人顺利抵达目的地，完成既定任务。相关技术中，机器人定位方法是基于二维码的定位方法，具体是在机器人工作场地装贴若干用于导航的二维码，这些二维码包含了机器人在此处的方位信息，因此，只要机器人能够观察到导航二维码，通过解码其中包含的信息即可对自身定位。
显然，上述基于二维码的定位方法以机器人能够观察到导航二维码为前提。然而，在实际应用场景中，机器人的工作场地的导航二维码间距较大或者基于各种原因不能装贴导航二维码，例如，导航二维码的间距超过1米，一旦机器人长时间或运行长距离均观察不到导航二维码，就会导致定位不准确或者累计误差持续增大，最终使得机器人走偏、甚至无法继续执行任务。
发明内容
为解决或部分解决相关技术中存在的问题，本申请提供一种机器人定位方法、装置和计算可读存储介质，可以提升机器人定位的准确性。
本申请第一方面提供一种机器人定位方法，包括：
通过机器人搭载的激光传感器获取激光点云数据；
通过所述机器人搭载的运动类传感器获取所述机器人的位姿数据；
根据导航二维码和所述位姿数据并基于扩展卡尔曼滤波器，计算得到所述机器人的先验位姿；
基于所述机器人的先验位姿，将所述激光点云数据与机器人地图匹配，得到所述机器人的第一位姿；
将所述机器人的第一位姿与所述扩展卡尔曼滤波器当前输出的第二位姿融合，得到所述机器人最终的位姿。
本申请第二方面提供一种机器人定位装置，包括：
第一获取模块，用于通过机器人搭载的激光传感器获取激光点云数据；
第二获取模块，用于通过所述机器人搭载的运动类传感器获取所述机器人的位姿数据；
计算模块，用于根据导航二维码和所述位姿数据并基于扩展卡尔曼滤波器，计算得到所述机器人的先验位姿；
匹配模块，用于基于所述机器人的先验位姿，将所述激光点云数据与机器人地图匹配，得到所述机器人的第一位姿；
融合模块，用于将所述机器人的第一位姿与所述扩展卡尔曼滤波器当前输出的第二位姿融合，得到所述机器人最终的位姿。
本申请第三方面提供一种电子设备，包括：
处理器；以及
存储器，其上存储有可执行代码，当所述可执行代码被所述处理器执行时，使所述处理器执行如上所述的方法。
本申请第四方面提供一种计算机可读存储介质，其上存储有可执行代码，当所述可执行代码被电子设备的处理器执行时，使所述处理器执行如上所述的方法。
本申请提供的技术方案可知，本申请的技术方案根据导航二维码和运动类传感器获取的位姿数据并基于扩展卡尔曼滤波器，计算得到机器人的先验位姿后，基于机器人的先验位姿，将激光点云数据与机器人地图匹配，得到机器人的第一位姿，最后将机器人的第一位姿与扩展卡尔曼滤波器当前输出的第二位姿融合，得到机器人最终的位姿。与现有技术完全依赖于导航二维码得到机器人的位姿，本申请的技术方案基于导航二维码，融合多种传感器获取的位姿数据，从而即使在导航二维码设置较为稀疏时，仍然能够获取到机器人的位姿数据，对机器人进行较为精确的定位。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本申请。
附图说明
通过结合附图对本申请示例性实施方式进行更详细地描述，本申请的上述以及其它目的、特征和优势将变得更加明显，其中，在本申请示例性实施方式中，相同的参考标号通常代表相同部件。
图1是本申请实施例提供的机器人定位方法的流程示意图；
图2是本申请实施例提供的多传感器的数据在时间戳上进行对齐示意图；
图3是本申请实施例提供的机器人定位装置的结构示意图；
图4是本申请实施例提供的电子设备的结构示意图。
具体实施方式
下面将参照附图更详细地描述本申请的实施方式。虽然附图中显示了本申请的实施方式，然而应该理解，可以以各种形式实现本申请而不应被这里阐述的实施方式所限制。相反，提供这些实施方式是为了使本申请更加透彻和完整，并且能够将本申请的范围完整地传达给本领域的技术人员。
在本申请使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。
应当理解，尽管在本申请可能采用术语“第一”、“第二”、“第三”等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本申请范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本申请的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
在应用AI机器人时，比较关键之处是能够对机器人进行精确定位，因为只有对机器人进行精确定位，才能够使得机器人顺利抵达目的地，完成既定任务。相关技术中，机器人定位方法是基于二维码的定位方法，具体是在机器人工作场地装贴若干用于导航的二维码，这些二维码包含了机器人在此处的方位信息，因此，只要机器人能够观察到导航二维码，通过解码其中包含的信息即可对自身定位。显然，上述基于二维码的定位方法以机器人能够观察到导航二维码为前提。然而，在实际应用场景中，机器人的工作场地的导航二维码间距较大或者基于各种原因不能装贴导航二维码，例如，导航二维码的间距超过1米，一旦机器人长时间或运行长距离均观察不到导航二维码，就会导致定位不准确或者累计误差持续增大，最终使得机器人走偏、甚至无法继续执行任务。
针对上述问题，本申请实施例提供一种机器人定位方法，可以提升机器人定位的准确性。
以下结合附图详细描述本申请实施例的技术方案。
参见图1，是本申请实施例示出的机器人定位方法的流程示意图，主要包括步骤S101至步骤S105，说明如下：
步骤S101：通过机器人搭载的激光传感器获取激光点云数据。
激光点云数据为激光传感器通过扫描，在激光束击中目标后，从目标表面返回的点携带的信息，因从目标表面返回的点一般较多(其数量级通常以万或十万为单位)而类似一团云状，因此称为激光点云，激光点云携带的三维坐标、目标的纹理、反射强度和回波次数等信息就是激光点云数据。在本申请实施例中，机器人搭载的激光传感器可以是激光雷达或毫米波雷达等2D激光传感器。
为了更大范围地识别机器人周围的环境并避开障碍物，在本申请一个实施例中，机器人搭载的激光传感器可以是部署在机器人的各至少一个2D激光传感器(可部署在机器人前后、左右或斜对角位置，对具体部署位置本申请不限制)。由于在安装这至少两个2D激光传感器时可能存在安装位置误差，或者，在机器人运行了一段时间后，原本已配准的至少两个2D激光传感器的安装位置出现了误差。考虑到单个2D激光传感器的扫描范围是270°，当安装的各至少一个2D激光传感器进行数据拼接时，必然会有数据重叠处，而至少两个2D激光传感器存在上述误差时，则至少两个2D激光传感器获取的数据在重叠处不能完全匹配。因此，在本申请一个实施例中，当机器人搭载的激光传感器是部署在机器人的各至少一个2D激光传感器时，在通过机器人搭载的激光传感器获取激光点云数据之前，离线标定各至少一个2D激光传感器，以获取各至少一个2D激光传感器的安装位置误差。在获取各至少一个2D激光传感器的安装位置误差后，当需要对安装的各至少一个2D激光传感器进行数据拼接时，可以将这个安装位置误差作为补偿数据，使得各至少一个2D激光传感器获取的数据在其扫描重叠处能够完全匹配。
上述实施例中，离线标定上述至少两个2D激光传感器可以是使得机器人在环境良好(譬如，晴天、光照良好以及导航二维码安排得较为稠密以确保机器人在绝大多数时间都能观察到导航二维码)的特定测试场地中运动，将仅仅依靠至少两个2D激光传感器而不涉及其他传感器进行地图匹配获得的机器人位姿与导航二维码获得的位姿进行坐标转换，通过得到的转换矩阵可以计算出至少两个2D激光传感器的实际安装位置，从而得到至少两个2D激光传感器的安装位置误差。从上述实施例可知，虽然通过离线方式能够标定至少两个2D激光传感器以获取至少两个2D激光传感器的安装位置误差，然而，离线标定对测试场地的要求较为苛刻。在本申请另一实施例中，可以在通过机器人搭载的激光传感器获取激光点云数据之前，在线标定至少两个2D激光传感器，以获取至少两个2D激光传感器的安装位置误差。所谓在线标定，是指可以在机器人运行在任意场地时，依靠其上部署的各种传感器，一边运行一边对至少两个2D激光传感器进行标定。相对于离线标定，在线标定的优势在于对场地没有过多限制，因而可以随时进行，此外，如前所述，在机器人运行了一段时间后，原本已配准的至少两个2D激光传感器的安装位置可能出现误差，因此，在线标定可以实时解决机器人长久运行导致的2D激光传感器安装位置误差的问题。
作为本申请一个实施例，在线标定各至少一个2D激光传感器，以获取各至少一个2D激光传感器的安装位置误差可以是：通过对视觉设备获取的图像数据进行特征点匹配，获取特征点对应的重投影误差；对各至少一个2D激光传感器中任意一个激光传感器获取的激光点云数据中两帧点云进行位姿修正和配准，计算两帧点云之间的相对位姿；通过运动类传感器获取的位姿数据，计算两帧点云之间的位姿偏差；在设定时长内，根据特征点对应的重投影误差、两帧点云之间的相对位姿以及计算两帧点云之间的位姿偏差进行迭代优化求解，获取各至少一个2D激光传感器的实际安装位置。在获取各至少一个2D激光传感器的实际安装位置后，将各至少一个2D激光传感器的实际安装位置相减即得到各至少一个2D激光传感器的安装位置误差。上述实施例中，视觉设备可以单目相机、双目相机或者深度相机等视觉类传感器，而运动类传感器可以是轮式里程计或惯性测量单元(InertialMeasurement Unit，IMU)等。
步骤S102：通过机器人搭载的运动类传感器获取机器人的位姿数据。
在本申请实施例中，机器人搭载的运动类传感器可以是前述提及的轮式里程计或惯性测量单元IMU等传感器，运动类传感器获取机器人的位姿数据包括机器人的三维坐标、加速度、速度和朝向等信息。
需要说明的是，传感器本身从数据获取到输出需要一定的时间dt，因此，真实的传感器数据采集时间应该是T-dt，此处，T为传感器供应商给出的这一帧输出的实际UTC时间，而由于不同的传感器即使硬件同步以后也存在采样频率不一致的差异，因此，在多种传感器融合定位中必然存在传感器获取的数据在时间戳上不同步的问题。鉴于上述事实，在本申请实施例中，在通过机器人搭载的激光传感器获取激光点云数据以及通过机器人搭载的运动类传感器获取机器人的位姿数据之后，将激光点云数据和机器人的位姿数据在时间上进行对齐，具体地，考虑到线性插值算法具有简单易行、计算量小等优点，上述将激光点云数据和机器人的位姿数据在时间上进行对齐可以是：通过线性插值算法，将激光点云数据和机器人的位姿数据的时间戳对齐。以上述实施例的运动类传感器包含惯性测量单元IMU和轮式里程计为例，如图2所示，假设IMU在ti时刻获取的机器人的位姿数据为Dti，理想状态下是轮式里程计也能够在ti时刻获取该机器人的位姿数据，然而由于采样频率不一致等原因，轮式里程计只能在t'li时刻获取到机器人的位姿数据D'li，这就是传感器之间的数据在时间戳上没有对齐；对于激光传感器采集的激光点云数据，也同样存在与IMU和轮式里程计采集的数据在时间戳上不对齐的情形，即由于采样频率不一致等原因，IMU在ti时刻获取的机器人位姿数据为Dti时，激光传感器只能在t'xi时刻获取到激光点云数据D'xi。对于上述情形，需要采用数据对齐的方案。
在本申请一个实施例中，通过线性插值算法，将激光点云数据和机器人的位姿数据的时间戳对齐可以是：使用与当前帧激光点云数据前后相邻时间戳IMU获取的机器人位姿数据对机器人位姿数据进行插值，以使插值后所得机器人位姿数据与激光传感器采集的当前帧激光点云数据对齐；以及使用与当前帧激光点云数据前后相邻时间戳轮式里程计获取的机器人的位姿数据对机器人的位姿数据进行插值，以使插值后所得机器人的位姿数据与激光传感器采集的当前帧激光点云数据对齐。仍然以图2为例，使用与当前帧激光点云数据前后相邻时间戳IMU获取的机器人的位姿数据，即，使用ti-1时刻机器人的位姿数据D'ti-1与ti时刻机器人位姿数据Dti对机器人的位姿数据进行插值，得到t'xi时刻的机器人的位姿插值数据从图2可以看出，经过上述插值操作，机器人的位姿插值数据/>与激光传感器采集的当前帧激光点云数据D'xi已经对齐。同样地，使用与当前帧激光点云数据前后相邻时间戳轮式里程计获取的机器人的位姿数据，即，使用t'li时刻机器人的位姿数据D'li与t'li+1时刻机器人的位姿数据D'li+1对机器人的位姿数据进行插值，得到t'xi时刻的机器人的位姿插值数据/>从图2可以看出，经过上述插值操作，机器人的位姿插值数据/>与激光传感器采集的当前帧激光点云数据D'xi已经对齐。
步骤S103：根据导航二维码和运动学数据并基于扩展卡尔曼滤波器，计算得到机器人的先验位姿。
在本申请实施例中，导航二维码中包含机器人移动到装贴该导航二维码之处时该机器人的位置信息和由机器人搭载的IMU得到的机器人的位姿信息，这些位姿信息的置信度较高，因此可以作为扩展卡尔曼滤波器计算机器人的位姿信息的观测值，而机器人搭载的轮式里程计得到的数据可以作为扩展卡尔曼滤波器计算机器人位姿信息的先验值。作为一种利用线性系统状态方程，卡尔曼滤波(Kalman filtering)可通过系统输入输出观测数据对系统状态进行最优估计的算法。换言之，卡尔曼滤波过程可以对系统输入观测数据进行处理，得到误差最小的真实信号估计值的过程。而扩展卡尔曼滤波器(Extended KalmanFilter，EKF)可以对非线性函数的泰勒(Taylor)展开式进行一阶线性化截断，忽略其余高阶项，从而将非线性问题转化为线性问题，因此，在本申请实施例中，在得到导航二维码携带的机器人的位置信息和运动类传感器获取的机器人的位姿数据(其中，导航二维码包含机器人移动到装贴该导航二维码之处时该机器人的位置信息和由机器人搭载的IMU得到的机器人的位姿信息作为扩展卡尔曼滤波器计算机器人的位姿信息的观测值，而机器人搭载的轮式里程计得到的数据可以作为扩展卡尔曼滤波器计算机器人位姿信息的先验值)之后，可以根据导航二维码和运动学数据并基于扩展卡尔曼滤波器，计算得到机器人的先验位姿。具体地，计算机器人的先验证位姿可以是：获取机器人在k-1时刻(即k时刻之前一时刻)的目标位姿信息；根据运动类传感器获取的机器人位姿数据(包括由机器人搭载的IMU得到的机器人的位姿信息和机器人搭载的轮式里程计得到的机器人的位姿信息)和机器人在k-1时刻的目标位姿信息，确定机器人在k时刻的位姿信息估计值；获取机器人在k时刻的协方差矩阵估计值；根据机器人在k时刻的协方差矩阵估计值确定卡尔曼增益；根据机器人移动到装贴导航二维码之处时该机器人的位置信息、机器人在k时刻的位姿信息估计值以及卡尔曼增益确定机器人在k时刻的目标位姿信息作为机器人的先验位姿。
步骤S104：基于机器人的先验位姿，将激光点云数据与机器人地图匹配，得到机器人的第一位姿。
在机器人移动的环境中，除了存在墙面、货架等不可移动的物体之外，还存在移动的人、机器人或其他物体等移动目标。这些移动目标对机器人的定位是一种干扰或者在进行数据匹配时是一种噪声。因此，在上述实施例中，在基于机器人的先验位姿，将激光点云数据与机器人地图匹配之前，通过数据关联算法剔除激光点云数据中对应于移动目标的激光点云数据。至于数据关联算法，可以是联合相容分支定界数据关联(JointCompatibility Branch and Bound，JCBB)算法、独立兼容最临近数据关联(IndividualCompatibility Nearest Neighbor，ICNN)算法或者两者的改进等。例如，考虑到JCBB算法和ICNN算法在数据关联过程中所存在的关联精度和计算效率之间的矛盾，本申请实施例可以设计一种基于关联准则的混合自适应数据关联方案，即，在数据关联基本准则的约束下，评判ICCN算法下数据关联结果是否正确，若判断数据关联结果错误，则采用JCBB算法重新进行数据关联，以此提高关联精度和关联效率。
在上述实施例中，机器人地图可以为二维栅格地图。二维栅格地图也称为二维栅格概率地图或二维占据栅格地图(Occupancy Grid Map)，这种地图是将平面划分为一个个栅格，每个栅格赋予一个占据率(Occupancy)。所谓占据率，是指二维占据栅格地图上，一个栅格被障碍物占据的状态(occupied状态)、无障碍物占据的状态(Free状态)或者介于这两种状态之间的概率，其中，被障碍物占据的状态使用1表示，无障碍物占据的状态使用0表示，介于两种状态之间则使用0～1之间的值来表示，显然，占据率表明了一个栅格被障碍物占据的可能性大小，当一个栅格的占据率越大，表明该栅格被障碍物占据的可能性越大，反之则反。考虑到扩展卡尔曼滤波器的计算周期较短，而激光点云数据与机器人地图匹配的计算周期较长，因此，可以基于机器人的先验位姿，将激光点云数据与机器人地图匹配，得到机器人的第一位姿。相应于机器人地图为二维栅格地图，上述实施例的基于机器人的先验位姿，将激光点云数据与机器人地图匹配，得到机器人的第一位姿，具体可以是：由机器人的先验位姿确定位姿搜索空间中的若干候选位姿；利用若干候选位姿中的每一个候选位姿，将激光点云数据投影至二维栅格地图，计算若干候选位姿中的每一个候选位姿在二维栅格地图上的匹配得分；将若干候选位姿在二维栅格地图上的匹配得分最高的候选位姿确定为机器人的第一位姿。上述由机器人的先验位姿确定位姿搜索空间中的若干候选位姿具体是以在机器人的先验位姿的附近，由Ceres求解器搜索机器人的若干候选位姿，而在计算若干候选位姿中的每一个候选位姿在二维栅格地图上的匹配得分时，可以机器人的位姿作为状态构建非线性最小二乘化问题，以误差E1为非线性最小二乘化问题的误差约束条件，迭代优化机器人的位姿，直至误差E1最小时为止，此处，误差E1为机器人的估计位姿与全局位姿观测值之间的差值。当误差E1最小时，若干候选位姿中的某个候选位姿在二维栅格地图上的匹配得分最高，将该某个候选位姿确定为机器人的第一位姿。
步骤S105：将机器人的第一位姿与扩展卡尔曼滤波器当前输出的第二位姿融合，得到机器人最终的位姿。
一方面，由于轮式里程计等运动类传感器存在累计误差，需要通过其他传感器来修正这个误差值，激光点云数据与机器人地图匹配也存在二维栅格地图的误差，需要通过优化匹配和准确的先验值来消除二维栅格地图累计误差；另一方面，激光点云数据与机器人地图匹配得到的机器人的第一位姿、扩展卡尔曼滤波器当前输出的第二位姿以及根据导航二维码和运动类传感器获取的位姿数据并基于扩展卡尔曼滤波器计算得到的机器人的先验位姿，三者之间的数据不相同。对于通过不同方法得到的机器人状态，需要通过数据融合得到一个更加可信的准确的值。因此，在本申请实施例中，可以将机器人的第一位姿与扩展卡尔曼滤波器当前输出的第二位姿融合，得到机器人最终的位姿。具体地，可以计算机器人的第一位姿与机器人的先验位姿之差，得到位姿偏差；求取位姿偏差与扩展卡尔曼滤波器当前输出的第二位姿之和，得到机器人最终的位姿。此处，扩展卡尔曼滤波器当前输出的第二位姿是以导航二维码和运动类传感器获取的机器人的位姿数据为扩展卡尔曼滤波器的输入，计算得到的机器人的位姿数据。
从上述图1示例的机器人定位方法可知，本申请的技术方案根据导航二维码和运动类传感器获取的运动学数据并基于扩展卡尔曼滤波器，计算得到机器人的先验位姿后，基于机器人的先验位姿，将激光点云数据与机器人地图匹配，得到机器人的第一位姿，最后将机器人的第一位姿与扩展卡尔曼滤波器当前输出的第二位姿融合，得到机器人最终的位姿。与现有技术完全依赖于导航二维码得到机器人的位姿，本申请的技术方案基于导航二维码，融合多种传感器获取的位姿数据，从而即使在导航二维码设置较为稀疏时，仍然能够获取到机器人的位姿数据，对机器人进行较为精确的定位。
参见图3，是本申请实施例示出的机器人定位装置的结构示意图。为了便于说明，仅示出了与本申请实施例相关的部分。图3示例的机器人定位装置主要包括第一获取模块301、第二获取模块302、计算模块303、匹配模块304和融合模块305，其中：
第一获取模块301，用于通过机器人搭载的激光传感器获取激光点云数据；
第二获取模块302，用于通过机器人搭载的运动类传感器获取机器人的位姿数据；
计算模块303，用于根据导航二维码和运动类传感器获取的机器人的位姿数据并基于扩展卡尔曼滤波器，计算得到机器人的先验位姿；
匹配模块304，用于基于机器人的先验位姿，将激光点云数据与机器人地图匹配，得到机器人的第一位姿；
融合模块305，用于将机器人的第一位姿与扩展卡尔曼滤波器当前输出的第二位姿融合，得到机器人最终的位姿。
关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不再做详细阐述说明。
从上述图3示例的机器人定位装置可知，本申请的技术方案根据导航二维码和运动类传感器获取的运动学数据并基于扩展卡尔曼滤波器，计算得到机器人的先验位姿后，基于机器人的先验位姿，将激光点云数据与机器人地图匹配，得到机器人的第一位姿，最后将机器人的第一位姿与扩展卡尔曼滤波器当前输出的第二位姿融合，得到机器人最终的位姿。与现有技术完全依赖于导航二维码得到机器人的位姿，本申请的技术方案基于导航二维码，融合多种传感器获取的位姿数据，从而即使在导航二维码设置较为稀疏时，仍然能够获取到机器人的位姿数据，对机器人进行较为精确的定位。
图4是本申请实施例示出的电子设备的结构示意图。
参见图4，电子设备400包括存储器410和处理器420。
处理器420可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
存储器410可以包括各种类型的存储单元，例如系统内存、只读存储器(ROM)和永久存储装置。其中，ROM可以存储处理器420或者计算机的其他模块需要的静态数据或者指令。永久存储装置可以是可读写的存储装置。永久存储装置可以是即使计算机断电后也不会失去存储的指令和数据的非易失性存储设备。在一些实施方式中，永久性存储装置采用大容量存储装置(例如磁或光盘、闪存)作为永久存储装置。另外一些实施方式中，永久性存储装置可以是可移除的存储设备(例如软盘、光驱)。系统内存可以是可读写存储设备或者易失性可读写存储设备，例如动态随机访问内存。系统内存可以存储一些或者所有处理器在运行时需要的指令和数据。此外，存储器410可以包括任意计算机可读存储媒介的组合，包括各种类型的半导体存储芯片(例如DRAM，SRAM，SDRAM，闪存，可编程只读存储器)，磁盘和/或光盘也可以采用。在一些实施方式中，存储器410可以包括可读和/或写的可移除的存储设备，例如激光唱片(CD)、只读数字多功能光盘(例如DVD-ROM，双层DVD-ROM)、只读蓝光光盘、超密度光盘、闪存卡(例如SD卡、min SD卡、Micro-SD卡等)、磁性软盘等。计算机可读存储媒介不包含载波和通过无线或有线传输的瞬间电子信号。
存储器410上存储有可执行代码，当可执行代码被处理器420处理时，可以使处理器420执行上文述及的方法中的部分或全部。
此外，根据本申请的方法还可以实现为一种计算机程序或计算机程序产品，该计算机程序或计算机程序产品包括用于执行本申请的上述方法中部分或全部步骤的计算机程序代码指令。
或者，本申请还可以实施为一种计算机可读存储介质(或非暂时性机器可读存储介质或机器可读存储介质)，其上存储有可执行代码(或计算机程序或计算机指令代码)，当可执行代码(或计算机程序或计算机指令代码)被电子设备(或服务器等)的处理器执行时，使处理器执行根据本申请的上述方法的各个步骤的部分或全部。
以上已经描述了本申请的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术的改进，或者使本技术领域的其他普通技术人员能理解本文披露的各实施例。

Claims (11)

1.一种机器人定位方法，其特征在于，所述方法包括：
通过机器人搭载的激光传感器获取激光点云数据；
通过所述机器人搭载的运动类传感器获取所述机器人的位姿数据；
根据导航二维码和所述位姿数据并基于扩展卡尔曼滤波器，计算得到所述机器人的先验位姿；
基于所述机器人的先验位姿，将所述激光点云数据与机器人地图匹配，得到所述机器人的第一位姿；
将所述机器人的第一位姿与所述扩展卡尔曼滤波器当前输出的第二位姿融合，得到所述机器人最终的位姿。
2.根据权利要求1所述的机器人定位方法，其特征在于，所述激光传感器包括部署在所述机器人的各至少一个2D激光传感器，所述方法还包括：
在通过机器人搭载的激光传感器获取激光点云数据之前，离线标定或在线标定所述各至少一个2D激光传感器，以获取所述各至少一个2D激光传感器的安装位置误差。
3.根据权利要求2所述的机器人定位方法，其特征在于，所述在线标定所述各至少一个2D激光传感器，以获取所述各至少一个2D激光传感器的安装位置误差，包括：
通过对视觉设备获取的图像数据进行所述特征点匹配，获取所述特征点对应的重投影误差；
对所述2D激光传感器中任意一个激光传感器获取的激光点云数据中两帧点云进行位姿修正和配准，计算所述两帧点云之间的相对位姿；
通过所述运动类传感器获取的位姿数据，计算两帧点云之间的位姿偏差；
在设定时长内，根据所述重投影误差、所述相对位姿以及所述位姿偏差进行迭代优化求解，获取所述2D激光传感器的实际安装位置以得到所述2D激光传感器的安装位置误差。
4.根据权利要求1所述的机器人定位方法，其特征在于，所述方法还包括：
在通过机器人搭载的激光传感器获取激光点云数据以及通过所述机器人搭载的运动类传感器获取所述机器人的位姿数据之后，将所述激光点云数据和所述机器人的位姿数据在时间上进行对齐。
5.根据权利要求4所述的机器人定位方法，其特征在于，所述将所述激光点云数据和所述机器人的位姿数据在时间上进行对齐，包括：
通过线性插值算法，将所述激光点云数据和所述机器人的位姿数据的时间戳对齐。
6.根据权利要求1所述的机器人定位方法，其特征在于，所述方法还包括：
在基于所述机器人的先验位姿，将所述激光点云数据与机器人地图匹配之前，通过数据关联算法剔除所述激光点云数据中对应于移动目标的激光点云数据。
7.根据权利要求1所述的机器人定位方法，其特征在于，所述机器人地图为二维栅格地图，所述基于所述机器人的先验位姿，将所述激光点云数据与机器人地图匹配，得到所述机器人的第一位姿，包括：
由所述机器人的先验位姿确定位姿搜索空间中的若干候选位姿；
利用所述若干候选位姿中的每一个候选位姿，将所述激光点云数据投影至所述二维栅格地图，计算所述每一个候选位姿在所述二维栅格地图上的匹配得分；
将所述若干候选位姿在所述二维栅格地图上的匹配得分最高的候选位姿确定为所述机器人的第一位姿。
8.根据权利要求1所述的机器人定位方法，其特征在于，所述将所述机器人的第一位姿与所述扩展卡尔曼滤波器当前输出的第二位姿融合，得到所述机器人最终的位姿，包括：
计算所述机器人的第一位姿与所述机器人的先验位姿之差，得到位姿偏差；
求取所述位姿偏差与所述扩展卡尔曼滤波器当前输出的第二位姿之和，得到所述机器人最终的位姿。
9.一种机器人定位装置，其特征在于，所述装置包括：
第一获取模块，用于通过机器人搭载的激光传感器获取激光点云数据；
第二获取模块，用于通过所述机器人搭载的运动类传感器获取所述机器人的位姿数据；
计算模块，用于根据导航二维码和所述位姿数据并基于扩展卡尔曼滤波器，计算得到所述机器人的先验位姿；
匹配模块，用于基于所述机器人的先验位姿，将所述激光点云数据与机器人地图匹配，得到所述机器人的第一位姿；
融合模块，用于将所述机器人的第一位姿与所述扩展卡尔曼滤波器当前输出的第二位姿融合，得到所述机器人最终的位姿。
10.一种电子设备，其特征在于，包括：
处理器；以及
存储器，其上存储有可执行代码，当所述可执行代码被所述处理器执行时，使所述处理器执行如权利要求1至8中任意一项所述的方法。
11.一种计算机可读存储介质，其上存储有可执行代码，当所述可执行代码被电子设备的处理器执行时，使所述处理器执行如权利要求1至8中任意一项所述的方法。
CN202210746832.2A 2022-06-29 2022-06-29 机器人定位方法、装置和计算可读存储介质 Pending CN117367419A (zh)

Priority Applications (3)

Application Number Priority Date Filing Date Title
CN202210746832.2A CN117367419A (zh) 2022-06-29 2022-06-29 机器人定位方法、装置和计算可读存储介质
PCT/CN2023/097297 WO2024001649A1 (zh) 2022-06-29 2023-05-31 机器人定位方法、装置和计算可读存储介质
TW112122106A TW202411613A (zh) 2022-06-29 2023-06-14 機器人定位方法、裝置和計算可讀存儲介質

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210746832.2A CN117367419A (zh) 2022-06-29 2022-06-29 机器人定位方法、装置和计算可读存储介质

Publications (1)

Publication Number Publication Date
CN117367419A true CN117367419A (zh) 2024-01-09

Family

ID=89382782

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210746832.2A Pending CN117367419A (zh) 2022-06-29 2022-06-29 机器人定位方法、装置和计算可读存储介质

Country Status (3)

Country Link
CN (1) CN117367419A (zh)
TW (1) TW202411613A (zh)
WO (1) WO2024001649A1 (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117824667A (zh) * 2024-03-06 2024-04-05 成都睿芯行科技有限公司 一种基于二维码和激光的融合定位方法及介质

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111854769B (zh) * 2019-04-25 2022-07-22 北京嘀嘀无限科技发展有限公司 位姿数据处理方法、装置、地图生成设备和存储介质

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113804184A (zh) * 2020-06-15 2021-12-17 上海知步邦智能科技有限公司 基于多传感器的地面机器人定位方法
CN112097768B (zh) * 2020-11-17 2021-03-02 深圳市优必选科技股份有限公司 机器人位姿的确定方法、装置、机器人及存储介质
CN112764053B (zh) * 2020-12-29 2022-07-15 深圳市普渡科技有限公司 一种融合定位方法、装置、设备和计算机可读存储介质
CN112598757B (zh) * 2021-03-03 2021-06-01 之江实验室 一种多传感器时间空间标定方法及装置
CN112964291B (zh) * 2021-04-02 2023-07-14 清华大学 一种传感器标定的方法、装置、计算机存储介质及终端
CN113091736B (zh) * 2021-04-02 2023-04-07 京东科技信息技术有限公司 机器人定位方法、装置、机器人及存储介质
CN113538699A (zh) * 2021-06-21 2021-10-22 广西综合交通大数据研究院 基于三维点云的定位方法、装置、设备及存储介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117824667A (zh) * 2024-03-06 2024-04-05 成都睿芯行科技有限公司 一种基于二维码和激光的融合定位方法及介质
CN117824667B (zh) * 2024-03-06 2024-05-10 成都睿芯行科技有限公司 一种基于二维码和激光的融合定位方法及介质

Also Published As

Publication number Publication date
TW202411613A (zh) 2024-03-16
WO2024001649A1 (zh) 2024-01-04

Similar Documents

Publication Publication Date Title
CN112268559B (zh) 复杂环境下融合slam技术的移动测量方法
CN111551186B (zh) 一种车辆实时定位方法、系统及车辆
EP2990828B1 (en) Point cloud position data processing device, point cloud position data processing system, point cloud position data processing method, and program therefor
WO2022127532A1 (zh) 一种激光雷达与imu的外参标定方法、装置及设备
CN110873883B (zh) 融合激光雷达和imu的定位方法、介质、终端和装置
US8855911B2 (en) Systems and methods for navigation using cross correlation on evidence grids
CN117367419A (zh) 机器人定位方法、装置和计算可读存储介质
EP2133662A2 (en) Methods and system of navigation using terrain features
WO2012033807A1 (en) Automatic correction of past position errors for location and inventory tracking
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN113721248B (zh) 一种基于多源异构传感器的融合定位方法及系统
WO2019204800A1 (en) Method and system for generating high definition map
US11983890B2 (en) Method and apparatus with motion information estimation
US20220398825A1 (en) Method for generating 3d reference points in a map of a scene
CN112781586A (zh) 一种位姿数据的确定方法、装置、电子设备及车辆
CN112946681B (zh) 融合组合导航信息的激光雷达定位方法
CN115200572B (zh) 三维点云地图构建方法、装置、电子设备及存储介质
CN110637209B (zh) 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质
CN114442133A (zh) 无人机定位方法、装置、设备及存储介质
CN116399324A (zh) 建图方法、装置及控制器、无人驾驶车辆
CN115930959A (zh) 视觉初始化方法、装置及飞行汽车
CN112965076B (zh) 一种用于机器人的多雷达定位系统及方法
CN117824667A (zh) 一种基于二维码和激光的融合定位方法及介质
CN116465393A (zh) 基于面阵激光传感器的同步定位和建图方法及装置
CN116972834A (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