CN112444820A - 一种机器人位姿确定方法、装置、可读存储介质及机器人 - Google Patents

一种机器人位姿确定方法、装置、可读存储介质及机器人 Download PDF

Info

Publication number
CN112444820A
CN112444820A CN201910800816.5A CN201910800816A CN112444820A CN 112444820 A CN112444820 A CN 112444820A CN 201910800816 A CN201910800816 A CN 201910800816A CN 112444820 A CN112444820 A CN 112444820A
Authority
CN
China
Prior art keywords
pose
pointer
laser
robot
map
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
CN201910800816.5A
Other languages
English (en)
Other versions
CN112444820B (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.)
Ubtech Robotics Corp
Original Assignee
Ubtech Robotics Corp
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 Ubtech Robotics Corp filed Critical Ubtech Robotics Corp
Priority to CN201910800816.5A priority Critical patent/CN112444820B/zh
Priority to US16/709,931 priority patent/US11498227B2/en
Publication of CN112444820A publication Critical patent/CN112444820A/zh
Application granted granted Critical
Publication of CN112444820B publication Critical patent/CN112444820B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/02—Sensing devices
    • B25J19/021—Optical sensing devices
    • B25J19/022—Optical sensing devices using lasers
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B25J9/1692—Calibration of manipulator
    • 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/87—Combinations of systems using electromagnetic waves other than radio waves
    • G01S17/875—Combinations of systems using electromagnetic waves other than radio waves for determining attitude
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/0095—Means or methods for testing manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B25J9/1684—Tracking a line or surface by means of sensors
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/048—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators using a predictor
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/39—Robotics, robotics to robotics hand
    • G05B2219/39033—Laser tracking of end effector, measure orientation of rotatable mirror

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Optics & Photonics (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明属于计算机技术领域，尤其涉及一种机器人位姿确定方法、装置、计算机可读存储介质及机器人。所述方法获取由安装在机器人上的激光雷达当前所采集的激光帧；根据所述激光帧计算所述机器人在由第一指针所指向的地图中的当前位姿，并获取已插入由所述第一指针所指向的地图的激光帧数；若小于第一阈值，则将所述激光帧插入由所述第一指针所指向的地图；若大于等于第一阈值且小于第二阈值，则分别将所述激光帧插入由所述第一指针所指向的地图以及由预设的第二指针所指向的地图；若等于第二阈值，则将所述第一指针指向由所述第二指针所指向的地图，将所述第二指针指向新建的空地图，并将所述激光帧插入由所述第一指针所指向的地图。

Description

一种机器人位姿确定方法、装置、可读存储介质及机器人
技术领域
本发明属于计算机技术领域，尤其涉及一种机器人位姿确定方法、装置、计算机可读存储介质及机器人。
背景技术
激光雷达里程计作为激光雷达即时定位与地图构建(SimultaneousLocalization And Mapping，SLAM)系统的前端，提高其精度有助于提高建图精度和质量。激光雷达里程计采用激光雷达数据帧之间进行匹配，计算帧间位移，然后通过对位移与前面激光帧对应的设备位姿进行位姿积分，得到当前激光帧的位姿。这个过程中，每帧之间的匹配都存在误差，这些误差会逐渐累积下去，造成激光雷达里程计的定位逐渐漂移失准。
为解决这一问题，现有技术中，有通过固定窗口建立子地图和通过滑动窗口建立子地图的技术方案，其中，常规的固定窗口建立子地图用于激光匹配的方式是：每个窗口的激光帧数量固定，地图插入的激光帧到了固定帧数后，销毁地图重新建立新的地图，这种方式在子地图中插入的激光帧数量较少时，匹配的精度较低。滑动窗口建立子地图用于激光匹配的方式是：存储最近的固定帧数激光帧和每一帧对应的位姿来建立地图。每次需要插入新激光帧时，删除存储的最旧的一帧激光帧，维持固定帧数的数量。然后重新建立地图进行匹配。这种方式在每次插入新的激光帧时都需要重建地图并插入固定帧数数据，运算量较大。
发明内容
有鉴于此，本发明实施例提供了一种机器人位姿确定方法、装置、计算机可读存储介质及机器人，以解决现有的机器人位姿确定方法精度较低、运算量较大的问题。
本发明实施例的第一方面提供了一种机器人位姿确定方法，可以包括：
获取由安装在机器人上的激光雷达当前所采集的激光帧；
根据所述激光帧计算所述机器人在由预设的第一指针所指向的地图中的当前位姿，并获取已插入由所述第一指针所指向的地图的激光帧数；
若所述激光帧数小于预设的第一阈值，则将所述激光帧插入由所述第一指针所指向的地图；
若所述激光帧数大于等于所述第一阈值且小于预设的第二阈值，则分别将所述激光帧插入由所述第一指针所指向的地图以及由预设的第二指针所指向的地图；
若所述激光帧数等于所述第二阈值，则将所述第一指针指向由所述第二指针所指向的地图，将所述第二指针指向新建的空地图，并将所述激光帧插入由所述第一指针所指向的地图。
进一步地，所述根据所述激光帧计算所述机器人在由预设的第一指针所指向的地图中的当前位姿包括：
若所述激光帧为第一帧，则将所述当前位姿设置为预设的初始位姿；
若所述激光帧不是第一帧，则获取里程计数据，并根据所述里程计数据计算所述机器人的预测位姿；
使用预设的匹配算法在由所述预测位姿所确定的区域内进行匹配计算，得到所述当前位姿。
进一步地，所述根据所述里程计数据计算所述机器人的预测位姿包括：
根据下式计算所述机器人的预测位姿：
pose_odom_predict＝p_odom+v*(t_lidar-t_odom)
其中，t_odom为所述里程计数据的采集时刻，t_lidar为所述激光帧的采集时刻，p_odom为所述里程计数据中的位姿，v为所述里程计数据中的速度，pose_odom_predict为所述预测位姿。
进一步地，在根据所述激光帧计算所述机器人在由预设的第一指针所指向的地图中的当前位姿之后，还包括：
判断所述当前位姿是否满足预设的位姿判定条件，所述位姿判定条件为：所述当前位姿相对于在先位姿的位移大于预设的位移阈值，或所述当前位姿相对于所述在先位姿的旋转角度大于预设的角度阈值，或所述当前位姿相对于所述在先位姿的时间差大于预设的时间阈值，所述在先位姿为根据上一帧插入由所述第一指针所指向的地图的激光帧计算得到的位姿；
若所述当前位姿不满足所述位姿判定条件，则不再执行所述获取已插入由所述第一指针所指向的地图的帧数的步骤及其后续步骤。
进一步地，在将所述第一指针指向由所述第二指针所指向的地图之前，还包括：
释放由所述第一指针所指向的地图占用的存储空间。
本发明实施例的第二方面提供了一种机器人位姿确定装置，可以包括：
激光帧获取模块，用于获取由安装在机器人上的激光雷达当前所采集的激光帧；
位姿计算模块，用于根据所述激光帧计算所述机器人在由预设的第一指针所指向的地图中的当前位姿；
帧数获取模块，用于获取已插入由所述第一指针所指向的地图的激光帧数；
第一处理模块，用于若所述激光帧数小于预设的第一阈值，则将所述激光帧插入由所述第一指针所指向的地图；
第二处理模块，用于若所述激光帧数大于等于所述第一阈值且小于预设的第二阈值，则分别将所述激光帧插入由所述第一指针所指向的地图以及由预设的第二指针所指向的地图；
第三处理模块，用于若所述激光帧数等于所述第二阈值，则将所述第一指针指向由所述第二指针所指向的地图，将所述第二指针指向新建的空地图，并将所述激光帧插入由所述第一指针所指向的地图。
进一步地，所述位姿计算模块可以包括：
初始位姿设置单元，用于若所述激光帧为第一帧，则将所述当前位姿设置为预设的初始位姿；
预测位姿计算单元，用于若所述激光帧不是第一帧，则获取里程计数据，并根据所述里程计数据计算所述机器人的预测位姿；
当前位姿计算单元，用于使用预设的匹配算法在由所述预测位姿所确定的区域内进行匹配计算，得到所述当前位姿。
进一步地，所述预测位姿计算单元具体用于根据下式计算所述机器人的预测位姿：
pose_odom_predict＝p_odom+v*(t_lidar-t_odom)
其中，t_odom为所述里程计数据的采集时刻，t_lidar为所述激光帧的采集时刻，p_odom为所述里程计数据中的位姿，v为所述里程计数据中的速度，pose_odom_predict为所述预测位姿。
进一步地，所述机器人位姿确定装置还可以包括：
判断模块，用于判断所述当前位姿是否满足预设的位姿判定条件，所述位姿判定条件为：所述当前位姿相对于在先位姿的位移大于预设的位移阈值，或所述当前位姿相对于所述在先位姿的旋转角度大于预设的角度阈值，或所述当前位姿相对于所述在先位姿的时间差大于预设的时间阈值，所述在先位姿为根据上一帧插入由所述第一指针所指向的地图的激光帧计算得到的位姿。
进一步地，所述机器人位姿确定装置还可以包括：
存储空间释放单元，用于释放由所述第一指针所指向的地图占用的存储空间。
本发明实施例的第三方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可读指令，所述计算机可读指令被处理器执行时实现上述任一种机器人位姿确定方法的步骤。
本发明实施例的第四方面提供了一种机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机可读指令，所述处理器执行所述计算机可读指令时实现上述任一种机器人位姿确定方法的步骤。
本发明与现有技术相比存在的有益效果是：本发明首先获取由安装在机器人上的激光雷达当前所采集的激光帧，然后根据所述激光帧计算所述机器人在由预设的第一指针所指向的地图中的当前位姿，并获取已插入由所述第一指针所指向的地图的激光帧数，若所述激光帧数小于预设的第一阈值，则将所述激光帧插入由所述第一指针所指向的地图；若所述激光帧数大于等于所述第一阈值且小于预设的第二阈值，则分别将所述激光帧插入由所述第一指针所指向的地图以及由预设的第二指针所指向的地图；若所述激光帧数等于所述第二阈值，则将所述第一指针指向由所述第二指针所指向的地图，将所述第二指针指向新建的空地图，并将所述激光帧插入由所述第一指针所指向的地图。相较于常规的通过固定窗口建立子地图用于激光匹配的方式，本发明中交错使用相差一定帧数的两个地图，并在激光帧数量较多的地图(即所述第一指针所指向的地图)中进行位姿计算，避免了出现固定窗口场景下可能出现的激光帧数量较少的问题，提高了计算精度，而相较于通过滑动窗口建立子地图用于激光匹配的方式，本发明中只有在插入的激光帧数达到足够数量(即所述第二阈值)时，才会重建地图，极大减少了运算量，在精度和运算量之间取得了较好的平衡。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。
图1为本发明实施例中一种机器人位姿确定方法的一个实施例流程图；
图2为计算机器人在地图中的当前位姿的示意流程图；
图3为由第一指针所指向的地图的示意图；
图4为第一指针所指向的地图和第二指针所指向的地图交替切换使用的示意图；
图5为本发明实施例中一种机器人位姿确定装置的一个实施例结构图；
图6为本发明实施例中一种机器人的示意框图。
具体实施方式
为使得本发明的发明目的、特征、优点能够更加的明显和易懂，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，下面所描述的实施例仅仅是本发明一部分实施例，而非全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
请参阅图1，本发明实施例中一种机器人位姿确定方法的一个实施例可以包括：
步骤S101、获取由安装在机器人上的激光雷达当前所采集的激光帧。
优选地，在获取所述激光帧的同时，还可以读取所述激光帧的采集时刻，此处将其记为t_lidar。在获取所述激光帧之后，还可以进一步对其进行过滤，去掉其中太远和太近的点，以避免对最终的位姿结果造成干扰，提高位姿计算精度。
步骤S102、根据所述激光帧计算所述机器人在由预设的第一指针所指向的地图中的当前位姿。
具体地，若所述激光帧为第一帧，则可新建一个空地图，将所述第一指针指向该地图，并将所述当前位姿设置为预设的初始位姿，该初始位姿即为所述第一指针所指向的地图的中心，将其记为pose:{x,y,theta}＝{0,0,0}。
若所述激光帧不是第一帧，则可以通过如图2所示的过程计算所述当前位姿：
步骤S1021、获取里程计数据。
所述里程计数据可以为轮式里程计数据或惯性测量单元(Inertial MeasurementUnit，IMU)数据或二者融合后的底盘里程计的数据，该数据以轮式里程计开机时的位姿为原点。此处将所述里程计数据中的位姿记为p_odom，将所述里程计数据中的速度记为v，将所述里程计数据的采集时刻记为t_odom。
步骤S1022、根据所述里程计数据计算所述机器人的预测位姿。
具体地，可以根据下式计算所述机器人的预测位姿：
pose_odom_predict＝p_odom+v*(t_lidar-t_odom)
其中，pose_odom_predict即为所述预测位姿。
步骤S1023、使用预设的匹配算法在由所述预测位姿所确定的区域内进行匹配计算，得到所述当前位姿。
此处将所述第一指针记为submap，由submap所指向的地图如图3所示，其中，黑色是障碍物，白色是空白区域，灰色是未知区域，在该地图中，可以确定一块以所述预测位姿为中心的区域，例如，该区域可以为以所述预测位姿为圆心的圆形区域，其半径可以根据实际情况预先进行设置，当然，该区域也可以设置为矩形或者其它形状，本实施例对此不作具体限定。在所述区域内可以采用PCL匹配或模板搜索匹配算法，跟地图上已有的信息匹配，得到最佳匹配位姿，并将该最佳匹配位姿作为所述当前位姿，此处将其记为matched_pose，并将其对应的匹配分值记为score。
步骤S103、获取已插入由所述第一指针所指向的地图的激光帧数。
对于新建的空地图而言，所述激光帧数被初始化为0，每当有激光帧被插入到该地图中，则可以通过预设的计数器对所述激光帧数进行统计，若满足条件1，即所述激光帧数小于预设的第一阈值，则执行步骤S104，若满足条件2，即所述激光帧数大于等于所述第一阈值且小于预设的第二阈值，则执行步骤S105，若满足条件3，即所述激光帧数等于所述第二阈值，则执行步骤S106。
所述第一阈值和所述第二阈值的具体取值可以根据实际情况进行设置，优选地，可以将所述第二阈值设置为所述第一阈值的2倍，将所述第一阈值记为n/2，将所述第二阈值记为n，n为大于2的偶数。
步骤S104、将所述激光帧插入由所述第一指针所指向的地图。
步骤S105、分别将所述激光帧插入由所述第一指针所指向的地图以及由预设的第二指针所指向的地图。
此处将所述第二指针记为submap_backup，由submap_backup所指向的地图的形式与图3所示的由submap所指向的地图类似，具体可参见前述说明，此处不再赘述。需要注意的是，所述第二指针所指向的地图在初始建立时为空地图。
步骤S106、将所述第一指针指向由所述第二指针所指向的地图，将所述第二指针指向新建的空地图，并将所述激光帧插入由所述第一指针所指向的地图。
优选地，在将所述第一指针指向由所述第二指针所指向的地图之前，可以先销毁由所述第一指针所指向的地图，从而释放由所述第一指针所指向的地图占用的存储空间。
如图4所示，由所述第一指针所指向的地图为进行位姿计算的地图，此处将其记为MAP1，由所述第二指针所指向的地图为备用地图，此处将其记为MAP2，MAP2是在MAP1中插入的激光帧数达到n/2帧后才建立并开始插入激光帧，因此MAP1中插入的激光帧数比MAP2多n/2帧，当在MAP1中插入的激光帧数达到n帧后，则销毁原有的MAP1，将原有的MAP2作为新的MAP1，并新建一个空地图作为新的MAP2，并不断重复以上过程。通过这样的方式，交错使用相差一定帧数的两个地图，并在激光帧数量较多的地图中进行位姿计算，避免了出现固定窗口场景下可能出现的激光帧数量较少的问题，提高了计算精度，而相较于通过滑动窗口建立子地图用于激光匹配的方式，本发明中只有在插入的激光帧数达到足够数量时，才会重建地图，极大减少了运算量，在精度和运算量之间取得了较好的平衡。
进一步地，在步骤S102之后、步骤S103之前，还可以判断所述当前位姿是否满足预设的位姿判定条件。
所述位姿判定条件为：所述当前位姿相对于在先位姿的位移大于预设的位移阈值，或所述当前位姿相对于所述在先位姿的旋转角度大于预设的角度阈值，或所述当前位姿相对于所述在先位姿的时间差大于预设的时间阈值。
所述在先位姿为根据上一帧插入由所述第一指针所指向的地图的激光帧计算得到的位姿。所述位移阈值、角度阈值、时间阈值的具体取值可以根据实际情况进行设置，本实施例对此不作具体限定。
若所述当前位姿满足所述位姿判定条件，则执行步骤S103及其后续步骤，若所述当前位姿不满足所述位姿判定条件，则不再执行步骤S103及其后续步骤，继续获取所述激光雷达所采集的下一帧数据并进行位姿的更新计算。通过这样的方式，可以有效减少将激光帧插入地图中的频次，从而减少地图重建及切换的频次，达到进一步减少运算量的目的。
综上所述，本发明实施例首先获取由安装在机器人上的激光雷达当前所采集的激光帧，然后根据所述激光帧计算所述机器人在由预设的第一指针所指向的地图中的当前位姿，并获取已插入由所述第一指针所指向的地图的激光帧数，若所述激光帧数小于预设的第一阈值，则将所述激光帧插入由所述第一指针所指向的地图；若所述激光帧数大于等于所述第一阈值且小于预设的第二阈值，则分别将所述激光帧插入由所述第一指针所指向的地图以及由预设的第二指针所指向的地图；若所述激光帧数等于所述第二阈值，则将所述第一指针指向由所述第二指针所指向的地图，将所述第二指针指向新建的空地图，并将所述激光帧插入由所述第一指针所指向的地图。相较于常规的通过固定窗口建立子地图用于激光匹配的方式，本发明中交错使用相差一定帧数的两个地图，并在激光帧数量较多的地图(即所述第一指针所指向的地图)中进行位姿计算，避免了出现固定窗口场景下可能出现的激光帧数量较少的问题，提高了计算精度，而相较于通过滑动窗口建立子地图用于激光匹配的方式，本发明中只有在插入的激光帧数达到足够数量(即所述第二阈值)时，才会重建地图，极大减少了运算量，在精度和运算量之间取得了较好的平衡。
应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
对应于上文实施例所述的一种机器人位姿确定方法，图5示出了本发明实施例提供的一种机器人位姿确定装置的一个实施例结构图。
本实施例中，一种机器人位姿确定装置可以包括：
激光帧获取模块501，用于获取由安装在机器人上的激光雷达当前所采集的激光帧；
位姿计算模块502，用于根据所述激光帧计算所述机器人在由预设的第一指针所指向的地图中的当前位姿；
帧数获取模块503，用于获取已插入由所述第一指针所指向的地图的激光帧数；
第一处理模块504，用于若所述激光帧数小于预设的第一阈值，则将所述激光帧插入由所述第一指针所指向的地图；
第二处理模块505，用于若所述激光帧数大于等于所述第一阈值且小于预设的第二阈值，则分别将所述激光帧插入由所述第一指针所指向的地图以及由预设的第二指针所指向的地图；
第三处理模块506，用于若所述激光帧数等于所述第二阈值，则将所述第一指针指向由所述第二指针所指向的地图，将所述第二指针指向新建的空地图，并将所述激光帧插入由所述第一指针所指向的地图。
进一步地，所述位姿计算模块可以包括：
初始位姿设置单元，用于若所述激光帧为第一帧，则将所述当前位姿设置为预设的初始位姿；
预测位姿计算单元，用于若所述激光帧不是第一帧，则获取里程计数据，并根据所述里程计数据计算所述机器人的预测位姿；
当前位姿计算单元，用于使用预设的匹配算法在由所述预测位姿所确定的区域内进行匹配计算，得到所述当前位姿。
进一步地，所述预测位姿计算单元具体用于根据下式计算所述机器人的预测位姿：
pose_odom_predict＝p_odom+v*(t_lidar-t_odom)
其中，t_odom为所述里程计数据的采集时刻，t_lidar为所述激光帧的采集时刻，p_odom为所述里程计数据中的位姿，v为所述里程计数据中的速度，pose_odom_predict为所述预测位姿。
进一步地，所述机器人位姿确定装置还可以包括：
判断模块，用于判断所述当前位姿是否满足预设的位姿判定条件，所述位姿判定条件为：所述当前位姿相对于在先位姿的位移大于预设的位移阈值，或所述当前位姿相对于所述在先位姿的旋转角度大于预设的角度阈值，或所述当前位姿相对于所述在先位姿的时间差大于预设的时间阈值，所述在先位姿为根据上一帧插入由所述第一指针所指向的地图的激光帧计算得到的位姿。
进一步地，所述机器人位姿确定装置还可以包括：
存储空间释放单元，用于释放由所述第一指针所指向的地图占用的存储空间。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的装置，模块和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
图6示出了本发明实施例提供的一种机器人的示意框图，为了便于说明，仅示出了与本发明实施例相关的部分。
如图6所示，该实施例的机器人6包括：处理器60、存储器61以及存储在所述存储器61中并可在所述处理器60上运行的计算机程序62。所述处理器60执行所述计算机程序62时实现上述各个机器人位姿确定方法实施例中的步骤，例如图1所示的步骤S101至步骤S106。或者，所述处理器60执行所述计算机程序62时实现上述各装置实施例中各模块/单元的功能，例如图5所示模块501至模块506的功能。
示例性的，所述计算机程序62可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器61中，并由所述处理器60执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序62在所述机器人6中的执行过程。
本领域技术人员可以理解，图6仅仅是机器人6的示例，并不构成对机器人6的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述机器人6还可以包括输入输出设备、网络接入设备、总线等。
所述处理器60可以是中央处理单元(Central Processing Unit，CPU)，还可以是其它通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述存储器61可以是所述机器人6的内部存储单元，例如机器人6的硬盘或内存。所述存储器61也可以是所述机器人6的外部存储设备，例如所述机器人6上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器61还可以既包括所述机器人6的内部存储单元也包括外部存储设备。所述存储器61用于存储所述计算机程序以及所述机器人6所需的其它程序和数据。所述存储器61还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
在本发明所提供的实施例中，应该理解到，所揭露的装置/机器人和方法，可以通过其它的方式实现。例如，以上所描述的装置/机器人实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
以上所述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。

Claims (10)

1.一种机器人位姿确定方法，其特征在于，包括：
获取由安装在机器人上的激光雷达当前所采集的激光帧；
根据所述激光帧计算所述机器人在由预设的第一指针所指向的地图中的当前位姿，并获取已插入由所述第一指针所指向的地图的激光帧数；
若所述激光帧数小于预设的第一阈值，则将所述激光帧插入由所述第一指针所指向的地图；
若所述激光帧数大于等于所述第一阈值且小于预设的第二阈值，则分别将所述激光帧插入由所述第一指针所指向的地图以及由预设的第二指针所指向的地图；
若所述激光帧数等于所述第二阈值，则将所述第一指针指向由所述第二指针所指向的地图，将所述第二指针指向新建的空地图，并将所述激光帧插入由所述第一指针所指向的地图。
2.根据权利要求1所述的机器人位姿确定方法，其特征在于，所述根据所述激光帧计算所述机器人在由预设的第一指针所指向的地图中的当前位姿包括：
若所述激光帧为第一帧，则将所述当前位姿设置为预设的初始位姿；
若所述激光帧不是第一帧，则获取里程计数据，并根据所述里程计数据计算所述机器人的预测位姿；
使用预设的匹配算法在由所述预测位姿所确定的区域内进行匹配计算，得到所述当前位姿。
3.根据权利要求2所述的机器人位姿确定方法，其特征在于，所述根据所述里程计数据计算所述机器人的预测位姿包括：
根据下式计算所述机器人的预测位姿：
pose_odom_predict＝p_odom+v*(t_lidar-t_odom)
其中，t_odom为所述里程计数据的采集时刻，t_lidar为所述激光帧的采集时刻，p_odom为所述里程计数据中的位姿，v为所述里程计数据中的速度，pose_odom_predict为所述预测位姿。
4.根据权利要求1所述的机器人位姿确定方法，其特征在于，在根据所述激光帧计算所述机器人在由预设的第一指针所指向的地图中的当前位姿之后，还包括：
判断所述当前位姿是否满足预设的位姿判定条件，所述位姿判定条件为：所述当前位姿相对于在先位姿的位移大于预设的位移阈值，或所述当前位姿相对于所述在先位姿的旋转角度大于预设的角度阈值，或所述当前位姿相对于所述在先位姿的时间差大于预设的时间阈值，所述在先位姿为根据上一帧插入由所述第一指针所指向的地图的激光帧计算得到的位姿；
若所述当前位姿不满足所述位姿判定条件，则不再执行所述获取已插入由所述第一指针所指向的地图的帧数的步骤及其后续步骤。
5.根据权利要求1至4中任一项所述的机器人位姿确定方法，其特征在于，在将所述第一指针指向由所述第二指针所指向的地图之前，还包括：
释放由所述第一指针所指向的地图占用的存储空间。
6.一种机器人位姿确定装置，其特征在于，包括：
激光帧获取模块，用于获取由安装在机器人上的激光雷达当前所采集的激光帧；
位姿计算模块，用于根据所述激光帧计算所述机器人在由预设的第一指针所指向的地图中的当前位姿；
帧数获取模块，用于获取已插入由所述第一指针所指向的地图的激光帧数；
第一处理模块，用于若所述激光帧数小于预设的第一阈值，则将所述激光帧插入由所述第一指针所指向的地图；
第二处理模块，用于若所述激光帧数大于等于所述第一阈值且小于预设的第二阈值，则分别将所述激光帧插入由所述第一指针所指向的地图以及由预设的第二指针所指向的地图；
第三处理模块，用于若所述激光帧数等于所述第二阈值，则将所述第一指针指向由所述第二指针所指向的地图，将所述第二指针指向新建的空地图，并将所述激光帧插入由所述第一指针所指向的地图。
7.根据权利要求6所述的机器人位姿确定装置，其特征在于，所述位姿计算模块包括：
初始位姿设置单元，用于若所述激光帧为第一帧，则将所述当前位姿设置为预设的初始位姿；
预测位姿计算单元，用于若所述激光帧不是第一帧，则获取里程计数据，并根据所述里程计数据计算所述机器人的预测位姿；
当前位姿计算单元，用于使用预设的匹配算法在由所述预测位姿所确定的区域内进行匹配计算，得到所述当前位姿。
8.根据权利要求7所述的机器人位姿确定装置，其特征在于，所述预测位姿计算单元具体用于根据下式计算所述机器人的预测位姿：
pose_odom_predict＝p_odom+v*(t_lidar-t_odom)
其中，t_odom为所述里程计数据的采集时刻，t_lidar为所述激光帧的采集时刻，p_odom为所述里程计数据中的位姿，v为所述里程计数据中的速度，pose_odom_predict为所述预测位姿。
9.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可读指令，其特征在于，所述计算机可读指令被处理器执行时实现如权利要求1至5中任一项所述的机器人位姿确定方法的步骤。
10.一种机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机可读指令，其特征在于，所述处理器执行所述计算机可读指令时实现如权利要求1至5中任一项所述的机器人位姿确定方法的步骤。
CN201910800816.5A 2019-08-28 2019-08-28 一种机器人位姿确定方法、装置、可读存储介质及机器人 Active CN112444820B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201910800816.5A CN112444820B (zh) 2019-08-28 2019-08-28 一种机器人位姿确定方法、装置、可读存储介质及机器人
US16/709,931 US11498227B2 (en) 2019-08-28 2019-12-11 Robot pose determination method and apparatus and robot using the same

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910800816.5A CN112444820B (zh) 2019-08-28 2019-08-28 一种机器人位姿确定方法、装置、可读存储介质及机器人

Publications (2)

Publication Number Publication Date
CN112444820A true CN112444820A (zh) 2021-03-05
CN112444820B CN112444820B (zh) 2023-06-09

Family

ID=74679477

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910800816.5A Active CN112444820B (zh) 2019-08-28 2019-08-28 一种机器人位姿确定方法、装置、可读存储介质及机器人

Country Status (2)

Country Link
US (1) US11498227B2 (zh)
CN (1) CN112444820B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113325435A (zh) * 2021-05-25 2021-08-31 武汉大学 定位方法、装置、设备及可读存储介质

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114012783B (zh) * 2021-10-12 2023-04-07 深圳优地科技有限公司 机器人故障检测方法、机器人及存储介质

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150103193A1 (en) * 2013-10-10 2015-04-16 Nvidia Corporation Method and apparatus for long term image exposure with image stabilization on a mobile device
US20150373567A1 (en) * 2014-06-24 2015-12-24 Ruckus Wireless, Inc. Performance Mapping
CN106272423A (zh) * 2016-08-31 2017-01-04 哈尔滨工业大学深圳研究生院 一种针对大尺度环境的多机器人协同制图与定位的方法
US20180315201A1 (en) * 2017-04-28 2018-11-01 Entit Software Llc Stitching maps generated using simultaneous localization and mapping
US20180314324A1 (en) * 2017-04-27 2018-11-01 Imam Abdulrahman Bin Faisal University Systems and methodologies for real time eye tracking for electronic device interaction
US20180341022A1 (en) * 2017-05-24 2018-11-29 Beijing Green Valley Technology Co., Ltd. Lidar-based mapping method, device and system
US20190003836A1 (en) * 2016-03-11 2019-01-03 Kaarta,Inc. Laser scanner with real-time, online ego-motion estimation
CN109141437A (zh) * 2018-09-30 2019-01-04 中国科学院合肥物质科学研究院 一种机器人全局重定位方法
JP2019133658A (ja) * 2018-01-31 2019-08-08 株式会社リコー 測位方法、測位装置及び読取り可能な記憶媒体

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6922632B2 (en) * 2002-08-09 2005-07-26 Intersense, Inc. Tracking, auto-calibration, and map-building system
US11567201B2 (en) * 2016-03-11 2023-01-31 Kaarta, Inc. Laser scanner with real-time, online ego-motion estimation
WO2019079211A1 (en) * 2017-10-19 2019-04-25 DeepMap Inc. LIDAR-CAMERA CALIBRATION TO GENERATE HIGH DEFINITION MAPS

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150103193A1 (en) * 2013-10-10 2015-04-16 Nvidia Corporation Method and apparatus for long term image exposure with image stabilization on a mobile device
US20150373567A1 (en) * 2014-06-24 2015-12-24 Ruckus Wireless, Inc. Performance Mapping
US20190003836A1 (en) * 2016-03-11 2019-01-03 Kaarta,Inc. Laser scanner with real-time, online ego-motion estimation
CN106272423A (zh) * 2016-08-31 2017-01-04 哈尔滨工业大学深圳研究生院 一种针对大尺度环境的多机器人协同制图与定位的方法
US20180314324A1 (en) * 2017-04-27 2018-11-01 Imam Abdulrahman Bin Faisal University Systems and methodologies for real time eye tracking for electronic device interaction
US20180315201A1 (en) * 2017-04-28 2018-11-01 Entit Software Llc Stitching maps generated using simultaneous localization and mapping
US20180341022A1 (en) * 2017-05-24 2018-11-29 Beijing Green Valley Technology Co., Ltd. Lidar-based mapping method, device and system
JP2019133658A (ja) * 2018-01-31 2019-08-08 株式会社リコー 測位方法、測位装置及び読取り可能な記憶媒体
CN109141437A (zh) * 2018-09-30 2019-01-04 中国科学院合肥物质科学研究院 一种机器人全局重定位方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
宋佳儒;胡士强;杨永胜;: "基于滑动窗口滤波器的无误差累积位姿估计", 计算机仿真 *
尹磊: "低成本激光和视觉相结合的同步定位与建图研究", 集成技术 *
廖自威: "激光雷达/微惯性室内自主建图与导航技术研究", 中国优秀硕士学位论文全文数据库 信息科技辑 *
薛锋: "基于激光雷达的移动机器人环境探测与定位导航研究", 中国优秀硕士学位论文全文数据库 信息科技辑 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113325435A (zh) * 2021-05-25 2021-08-31 武汉大学 定位方法、装置、设备及可读存储介质
CN113325435B (zh) * 2021-05-25 2024-01-09 武汉大学 定位方法、装置、设备及可读存储介质

Also Published As

Publication number Publication date
US20210060802A1 (en) 2021-03-04
CN112444820B (zh) 2023-06-09
US11498227B2 (en) 2022-11-15

Similar Documents

Publication Publication Date Title
CN108496059B (zh) 一种惯量传感器的校正方法、装置、终端设备和存储介质
CN108513676B (zh) 一种路况识别方法、装置及设备
WO2021017471A1 (zh) 一种基于图像处理的点云滤波方法、装置和存储介质
CN111220169B (zh) 一种轨迹纠偏方法、装置、终端设备及存储介质
CN112348765A (zh) 数据增强方法、装置、计算机可读存储介质及终端设备
CN111161169B (zh) 基于霍夫变换的绝对相位噪声去除方法、装置和存储介质
CN112444820A (zh) 一种机器人位姿确定方法、装置、可读存储介质及机器人
CN108829996B (zh) 获得车辆定位信息的方法及装置
CN113706592A (zh) 一种定位信息的修正方法、装置、电子设备和存储介质
CN114612616A (zh) 建图方法、装置、电子设备和存储介质
CN113050660B (zh) 误差补偿方法、装置、计算机设备及存储介质
CN112215887A (zh) 一种位姿确定方法、装置、存储介质及移动机器人
CN112198878B (zh) 一种即时地图构建方法、装置、机器人及存储介质
CN111127590B (zh) 一种二阶贝塞尔曲线绘制方法及装置
CN109344877B (zh) 一种样本数据处理方法、样本数据处理装置及电子设备
CN116481560A (zh) 车辆行驶里程的计算方法、装置、终端及存储介质
CN110887490A (zh) 一种激光定位导航的关键帧选取方法、介质、终端和装置
CN112669196B (zh) 一种硬件加速引擎中因子图优化数据的方法和设备
CN115865607A (zh) 一种分布式训练的计算节点管理方法及相关装置
CN113190429B (zh) 服务器性能预测方法、装置及终端设备
CN111368860A (zh) 重定位方法及终端设备
CN115239520A (zh) 模型构件的误差消除方法、装置、电子设备及存储介质
CN111360796B (zh) 一种机器人及其定位方法和装置
CN112686904A (zh) 车道划分方法、装置、电子设备和存储介质
CN111310597B (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