CN116442226A - 位姿正确性判断方法、装置、机器人和存储介质 - Google Patents

位姿正确性判断方法、装置、机器人和存储介质 Download PDF

Info

Publication number
CN116442226A
CN116442226A CN202310431056.1A CN202310431056A CN116442226A CN 116442226 A CN116442226 A CN 116442226A CN 202310431056 A CN202310431056 A CN 202310431056A CN 116442226 A CN116442226 A CN 116442226A
Authority
CN
China
Prior art keywords
pose
covariance
determining
key
score
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
CN202310431056.1A
Other languages
English (en)
Other versions
CN116442226B (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 CN202310431056.1A priority Critical patent/CN116442226B/zh
Priority claimed from CN202310431056.1A external-priority patent/CN116442226B/zh
Publication of CN116442226A publication Critical patent/CN116442226A/zh
Application granted granted Critical
Publication of CN116442226B publication Critical patent/CN116442226B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • 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/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • 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/1612—Programme controls characterised by the hand, wrist, grip control
    • 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/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Mechanical Engineering (AREA)
  • Robotics (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • Image Analysis (AREA)

Abstract

本申请涉及一种位姿正确性判断方法、装置、机器人和存储介质。所述方法包括：通过相关扫描匹配计算得到各位姿和各所述位姿对应的分数；在各所述位姿中，确定对应的所述分数大于分数阈值的第一位姿，以及在所述相关扫描匹配对应的关键分辨率下，确定在所述分数中的关键分数对应的关键位姿；所述位姿包括关键位姿；确定所述第一位姿对应的第一协方差；当所述第一协方差不大于协方差阈值时，则在所述关键分辨率下，确定所述关键位姿的附近位姿，并确定所述附近位姿对应的第二协方差；当所述第二协方差不大于所述协方差阈值时，则确定所述关键位姿匹配正确。采用本方法能够对机器人的位姿正确性进行精确判断。

Description

位姿正确性判断方法、装置、机器人和存储介质
技术领域
本申请涉及机器人技术领域，特别是涉及一种位姿正确性判断方法、装置、机器人和存储介质。
背景技术
随着科学技术的不断发展，机器人逐渐走入人们的生活，目前，在社会上随处可见机器人的身影，随着机器人应用场景的不断增加，对机器人定位技术的要求也越来越高。
相关扫描匹配是一种快速求取当前激光帧在已有地图上的匹配位姿的方法，通过构造多层地图和分支限界快速求取出地图中与当前激光帧匹配得分最高的位姿。由于场景变化、动态物体的干扰，容易出现当前激光帧有很多干扰信息的情况，这些干扰信息同样会参与匹配过程，因此可能造成有干扰信息的激光帧在错误的位姿上匹配得分是最高的，从而导致存在对机器人的位姿判断错误的问题。
发明内容
基于此，有必要针对上述技术问题，提供一种能够提高对机器人位姿判断正确性的位姿正确性判断方法、装置、机器人、计算机可读存储介质和计算机程序产品。
第一方面，本申请提供了一种位姿正确性判断方法。所述方法包括：
通过相关扫描匹配计算得到各位姿和各所述位姿对应的分数；
在各所述位姿中，确定对应的所述分数大于分数阈值的第一位姿，以及在所述相关扫描匹配对应的关键分辨率下，确定在所述分数中的关键分数对应的关键位姿；所述位姿包括关键位姿；
确定所述第一位姿对应的第一协方差；
当所述第一协方差不大于协方差阈值时，则在所述关键分辨率下，确定所述关键位姿的附近位姿，并确定所述附近位姿对应的第二协方差；
当所述第二协方差不大于所述协方差阈值时，则确定所述关键位姿匹配正确。
在其中一个实施例中，所述确定所述第一位姿对应的第一协方差包括：
确定所述第一位姿与所述关键位姿之间的第一差值；
确定第一差值对应的第一转置差值；所述第一转置差值为所述第一差值的转置；
基于所述第一位姿的分数、所述第一差值和所述第一转置差值得到第一协方差。
在其中一个实施例中，所述基于所述第一位姿的分数、所述第一差值和所述第一转置差值得到第一协方差包括：
确定所述第一位姿的分数、所述第一差值和所述第一转置差值之间的第一积值；
对所述第一积值进行求和，得到第一总积值；
确定所述第一位姿的分数进行求和，得到第一总分数；
基于所述第一总积值和所述第一总分数得到第一协方差。
在其中一个实施例中，所述确定所述附近位姿对应的第二协方差包括：
确定所述附近位姿与所述关键位姿之间的第二差值；
确定第二差值对应的第二转置差值；所述第二转置差值为所述第二差值的转置；
基于所述附近位姿的分数、所述第二差值和所述第二转置差值得到第二协方差。
在其中一个实施例中，所述基于所述附近位姿的分数、所述第二差值和所述第二转置差值得到第二协方差包括：
确定所述附近位姿的分数、所述第二差值和所述第二转置差值之间的第二积值；
对所述第二积值进行求和，得到第二总积值；
确定所述附近位姿的分数进行求和，得到第二总分数；
基于所述第二总积值和所述第二总分数得到第二协方差。
在其中一个实施例中，所述方法还包括：
当所述第一协方差大于所述协方差阈值，或所述第二协方差大于所述协方差阈值时，则确定所述关键位姿匹配错误。
在其中一个实施例中，所述在所述关键分辨率下，确定所述关键位姿的附近位姿包括：
获取预设关键半径；
依据所述预设关键半径确定以所述关键位姿为中心的关键范围；
在所述关键分辨率下，确定所述关键范围中的位姿，并将所述关键范围中的位姿作为附近位姿。
第二方面，本申请还提供了一种位姿正确性判断装置。所述装置包括：
计算模块，用于通过相关扫描匹配计算得到各位姿和各所述位姿对应的分数；
第一确定模块，用于在各所述位姿中，确定对应的所述分数大于分数阈值的第一位姿，以及在所述相关扫描匹配对应的关键分辨率下，确定在所述分数中的关键分数对应的关键位姿；所述位姿包括关键位姿；
第二确定模块，用于确定所述第一位姿对应的第一协方差；
第三确定模块，用于当所述第一协方差不大于协方差阈值时，则在所述关键分辨率下，确定所述关键位姿的附近位姿，并确定所述附近位姿对应的第二协方差；
第四确定模块，用于当所述第二协方差不大于所述协方差阈值时，则确定所述关键位姿匹配正确。
在其中一个实施例中，所述第二确定模块还用于确定所述第一位姿与所述关键位姿之间的第一差值；确定第一差值对应的第一转置差值；所述第一转置差值为所述第一差值的转置；基于所述第一位姿的分数、所述第一差值和所述第一转置差值得到第一协方差。
在其中一个实施例中，所述第二确定模块还用于确定所述第一位姿的分数、所述第一差值和所述第一转置差值之间的第一积值；对所述第一积值进行求和，得到第一总积值；确定所述第一位姿的分数进行求和，得到第一总分数；基于所述第一总积值和所述第一总分数得到第一协方差。
在其中一个实施例中，所述第三确定模块还用于确定所述第二位姿与所述关键位姿之间的第二差值；确定第二差值对应的第二转置差值；所述第二转置差值为所述第二差值的转置；基于所述附近位姿的分数、所述第二差值和所述第二转置差值得到第二协方差。
在其中一个实施例中，所述第三确定模块还用于确定所述附近位姿的分数、所述第二差值和所述第二转置差值之间的第二积值；对所述第二积值进行求和，得到第二总积值；确定所述附近位姿的分数进行求和，得到第二总分数；基于所述第二总积值和所述第二总分数得到第二协方差。
在其中一个实施例中，所述第四确定模块还用于当所述第一协方差大于所述协方差阈值，或所述第二协方差大于所述协方差阈值时，则确定所述关键位姿匹配错误。
在其中一个实施例中，所述第三确定模块还用于获取预设关键半径；依据所述预设关键半径确定以所述关键位姿为中心的关键范围；在所述关键分辨率下，确定所述关键范围中的位姿，并将所述关键范围中的位姿作为附近位姿。
第三方面，本申请还提供了一种机器人。所述机器人包括存储器和处理器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序时实现上述方法的步骤。
第四方面，本申请还提供了一种计算机可读存储介质。所述计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述方法的步骤。
第五方面，本申请还提供了一种计算机程序产品。所述计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现上述方法的步骤。
上述位姿正确性判断方法、装置、机器人和存储介质，通过相关扫描匹配计算得到各位姿和各位姿对应的分数；在各位姿中，确定对应的分数大于分数阈值的第一位姿，以及在相关扫描匹配对应的关键分辨率下，确定在分数中的关键分数对应的关键位姿；位姿包括关键位姿；确定第一位姿对应的第一协方差；从而实现了对相关扫描匹配中分数较高的第一位姿进行判断，是否第一位姿的第一协方差不大于协方差阈值，当第一协方差不大于协方差阈值时，即当第一位姿符合高斯分布时，则在关键分辨率下，确定关键位姿的附近位姿；并确定附近位姿对应的第二协方差；从而进一步的对关键位姿的附近区域的位姿的协方差进行判断，当第二协方差不大于协方差阈值时，则确定关键位姿匹配正确，实现了对机器人的位姿正确性的精确判断，从而有效的提高了对机器人的精准定位。
附图说明
图1为一个实施例中位姿正确性判断方法的应用环境图；
图2为一个实施例中位姿正确性判断方法的流程示意图；
图3为一个实施例中确定附近位姿步骤的流程示意图；
图4为另一个实施例中位姿正确性判断方法的流程示意图；
图5为一个实施例中位姿正确性判断装置的结构框图；
图6为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
本申请实施例提供的位姿正确性判断方法，可以应用于如图1所示的应用环境中。其中，终端102通过网络与服务器104进行通信。数据存储***可以存储服务器104需要处理的数据。数据存储***可以集成在服务器104上，也可以放在云上或其他网络服务器上。本申请可由终端102或服务器104执行，本实施例以终端102为机器人执行为例进行说明。
机器人通过相关扫描匹配计算得到各位姿和各位姿对应的分数；机器人在各位姿中，确定对应的分数大于分数阈值的第一位姿，以及在相关扫描匹配对应的关键分辨率下，确定在分数中的关键分数对应的关键位姿；位姿包括关键位姿；机器人确定第一位姿对应的第一协方差；机器人当第一协方差不大于协方差阈值时，则在关键分辨率下，确定关键位姿的附近位姿，并确定附近位姿对应的第二协方差；机器人当第二协方差不大于协方差阈值时，则确定关键位姿匹配正确。
其中，终端102可以但不限于是各种智能化移动设备。智能化移动设备可以是各种机器人，例如：配送机器人、清洁机器人、导览机器人、迎宾机器人和购物机器人等。服务器104可以用独立的服务器或者是多个服务器组成的服务器集群来实现。
在一个实施例中，如图2所示，提供了一种位姿正确性判断方法，以该方法应用于图1中的机器人为例进行说明，包括以下步骤：
S202，通过相关扫描匹配计算得到各位姿和各位姿对应的分数。
其中，相关扫描匹配可以指相关性扫描匹配，相关性扫描匹配可用于求取激光帧在地图上的匹配位姿。位姿可用于描述机器人的方位，位姿包括位置和朝向，位置可用坐标来表示，例如，位置可以是(x，y)和(x，y，z)等等。朝向可用角度来表示，例如，朝向可以是θ。分数可用于表示位姿与激光帧的匹配度，即分数越高，则匹配度越高。
具体地，机器人可以通过相关性扫描匹配先确定匹配的各位姿，再依次计算各位姿对应的分数。
S204，在各位姿中，确定对应的分数大于分数阈值的第一位姿，以及在相关扫描匹配对应的关键分辨率下，确定在分数中的关键分数对应的关键位姿；位姿包括关键位姿。
其中，分数阈值可用于对位姿进行筛选。第一位姿可以指通过分数阈值筛选得到的位姿。关键分辨率可以指对位姿正确性判断过程中的位姿对应的分辨率，例如，关键分辨率可以是相关性扫描匹配的最高分辨率。关键位姿可以指在关键分辨率下的最匹配的位姿。关键分数可以指关键位姿对应的分数。例如，关键分数可以是最高的分数。
具体地，机器人可以获取分数阈值，在不同分辨率下的所有位姿中，将位姿的分数大于分数阈值的位姿进行筛选，得到第一位姿，确定相关扫描匹配对应的关键分辨率，并在相关扫描匹配对应的关键分辨率下，确定对应的所有位姿，从该关键分辨率对应的所有位姿的分数中，选出最高分数，将该最高分数作为关键分数，该最高分数对应的位姿作为关键位姿。
S206，确定第一位姿对应的第一协方差。
其中，第一协方差可以指第一位姿与关键位姿之间的协方差。
具体地，可以确定第一位姿与关键位姿之间的第一差值；确定第一差值对应的第一转置差值；第一转置差值为第一差值的转置；基于第一位姿的分数、第一差值和第一转置差值得到第一协方差。
其中，第一差值可以指第一位姿与关键位姿之间的差值。第一转置差值可以为第一差值的转置。
在一个实施例中，基于第一位姿的分数、第一差值和第一转置差值得到第一协方差包括机器人可以确定第一位姿的分数、第一差值和第一转置差值之间的第一积值；对第一积值进行求和，得到第一总积值；确定第一位姿的分数进行求和，得到第一总分数；基于第一总积值和第一总分数得到第一协方差。
其中，第一积值可以指第一位姿的分数、第一差值和第一转置差值之间的积。第一总积值可以指各第一位姿的第一积值之和。第一总分数可以指第一位姿的分数之和。
例如，第一位姿的数量有n个，当前的第一位姿为i，第一积值的计算公式如下：第一积值i＝第一位姿的分数i*第一差值i*第一转置差值i；第一总积值的计算公式为：第一总积值＝第一积值1+第一积值2…+第一积值n；第一总分数的计算分数为：第一总分数＝第一位姿的分数1+第一位姿的分数2+…+第一位姿的分数n。
在一个实施例中，基于第一总积值和第一总分数得到第一协方差包括机器人可将第一总积值与第一总分数的倒数之积作为第一协方差。
例如，第一协方差的计算公式为：第一协方差＝第一总积值/第一总分数。
S208，当第一协方差不大于协方差阈值时，则在关键分辨率下，确定关键位姿的附近位姿，并确定附近位姿对应的第二协方差。
其中，协方差阈值可用于对第一协方差的大小进行判断。附近位姿可以指关键位姿附近一定范围内的位姿。第二协方差可以指附近位姿与关键位姿之间的协方差。
在一个实施例中，确定附近位姿对应的第二协方差包括机器人确定附近位姿与关键位姿之间的第二差值；确定第二差值对应的第二转置差值；第二转置差值为第二差值的转置；基于附近位姿的分数、第二差值和第二转置差值得到第二协方差。
其中，第二差值可以指附近位姿与关键位姿之间的差值。第二转置差值可以为第二差值的转置。
在一个实施例中，基于附近位姿的分数、第二差值和第二转置差值得到第二协方差包括机器人可以确定附近位姿的分数、第二差值和第二转置差值之间的第二积值；对第二积值进行求和，得到第二总积值；确定附近位姿的分数进行求和，得到第二总分数；基于第二总积值和第二总分数得到第二协方差。
其中，第二积值可以指附近位姿的分数、第二差值和第二转置差值之间的积。第二总积值可以指各附近位姿的第二积值之和。第二总分数可以指附近位姿的分数之和。
例如，附近位姿的数量有m个，当前的附近位姿为j，第二积值的计算公式如下：第二积值j＝附近位姿的分数j*第二差值j*第二转置差值j；第二总积值的计算公式为：第二总积值＝第二积值1+第二积值2…+第二积值m；第二总分数的计算分数为：第二总分数＝附近位姿的分数1+附近位姿的分数2+…+附近位姿的分数m。
在一个实施例中，基于第二总积值和第二总分数得到第二协方差包括机器人可将第二总积值与第二总分数的倒数之积作为第二协方差。
例如，第二协方差的计算公式为：第二协方差＝第二总积值/第二总分数。
S210，当第二协方差不大于协方差阈值时，则确定关键位姿匹配正确。
在一个实施例中，当第一协方差大于协方差阈值，或第二协方差大于协方差阈值时，则确定关键位姿匹配错误。
上述位姿正确性判断方法中，通过相关扫描匹配计算得到各位姿和各位姿对应的分数；在各位姿中，确定对应的分数大于分数阈值的第一位姿，以及在相关扫描匹配对应的关键分辨率下，确定在分数中的关键分数对应的关键位姿；位姿包括关键位姿；确定第一位姿对应的第一协方差；从而实现了对相关扫描匹配中分数较高的第一位姿进行判断，是否第一位姿的第一协方差不大于协方差阈值，当第一协方差不大于协方差阈值时，即当第一位姿符合高斯分布时，则在关键分辨率下，确定关键位姿的附近位姿；并确定附近位姿对应的第二协方差；从而进一步的对关键位姿的附近区域的位姿的协方差进行判断，当第二协方差不大于协方差阈值时，则确定关键位姿匹配正确，实现了对机器人的位姿正确性的精确判断，从而有效的提高了对机器人的精准定位。
在一个实施例中，如图3所示，确定附近位姿步骤包括：
S302，获取预设关键半径。
其中，预设关键半径可以指预先设置的，用于确定关键范围的半径。关键范围可以指以关键位姿的位置为中心，形成的范围。具体地，机器人可以响应于确定附近位姿指令，获取内部存储的预设关键半径。
S304，依据预设关键半径确定以关键位姿为中心的关键范围。
具体地，机器人可以先确定关键位姿的坐标，将该关键位姿的坐标和预设关键半径输入作图函数，得到关键范围。
其中，作图函数可用于确定关键范围。
S306，在关键分辨率下，确定关键范围中的位姿，并将关键范围中的位姿作为附近位姿。
具体地，机器人可以在关键分辨率下，确定关键范围中，通过相关扫描匹配得到的所有位姿，并将该关键范围中的所有位姿作为附近位姿。
本实施例中，通过获取预设关键半径，依据预设关键半径确定以关键位姿为中心的关键范围，在关键分辨率下，确定关键范围中的位姿，并将关键范围中的位姿作为附近位姿。能够达到精确的确定关键位姿的附近位姿。
作为一个示例，本实施例如下：
图4为另一个实施例中位姿正确性判断方法的流程示意图；如图所示，从上图可以看出，由于相关扫描匹配对地图做了金字塔结构的分层，匹配计算的时候从低分辨率开始，只对分数更高的匹配块继续计算更高分辨率下的匹配得分，以此类推直到计算到最高分辨率下的最高得分(关键分数)。
但不能保证计算过程对最高分辨率下的关键位姿附近的所有的位姿都进行了计算，因此，我们首先判断计算过程中分数较高(大于分数阈值)的候选解是否符合高斯分布，即计算第一协方差是否足够小，如果是，才进一步计算最高分辨率下的关键位姿的附近区域的附近位姿，然后再次验证第二协方误差足够小才是正确匹配。两次计算第一协方差和第二协方差只要有一次出现协方差太大的情况，则说明匹配结果是奇异解，即关键位姿匹配不正确，设置匹配得分为0，本次相关扫描匹配失败。其中，奇异解是指无法由通解中求出但仍能符合微分方程式的解，奇异解只会在非线性方程式中才会出现，在本案中，奇异解是指存在多个不同的位姿都能匹配，即匹配的位姿不唯一的情况。
其中，对于根据一系列匹配位姿计算协方差(第一协方差或第二协方差)的方法，可以描述为如下过程：
Step1.定义矩阵cov，得分统计norm；
Step2.存储所有score>score_thresh的pose和score组成数组score_poses；
Step3.对score_poses中的每个元素执行如下操作：
diff_pose(第一差值或第二差值)＝pose(第一位姿或附近位姿)–best_pose(关键位姿)；
cov+＝score(第一位姿的分数或附近位姿的分数)*diff_pose*Tdiff_pose；
norm+＝score；
其中，Tdiff_pose为diff_pose的转置；
Step4.归一化操作cov＝cov/norm。
经过上述步骤，即可简单计算出关键位姿为中心的附近区域匹配的第一协方差或第二协方差，如果是正确解，则在所有符合位姿的分数>分数阈值的位姿应该在关键位姿附近为高斯分布，否则关键位姿为奇异解，即关键位姿匹配不正确，应当弃用。
应该理解的是，虽然如上的各实施例所涉及的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，如上的各实施例所涉及的流程图中的至少一部分步骤可以包括多个步骤或者多个阶段，这些步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤中的步骤或者阶段的至少一部分轮流或者交替地执行。
基于同样的发明构思，本申请实施例还提供了一种用于实现上述所涉及的位姿正确性判断方法的位姿正确性判断装置。该装置所提供的解决问题的实现方案与上述方法中所记载的实现方案相似，故下面所提供的一个或多个位姿正确性判断装置实施例中的具体限定可以参见上文中对于位姿正确性判断方法的限定，在此不再赘述。
在一个实施例中，如图5所示，提供了一种位姿正确性判断装置，包括：计算模块502、第一确定模块504、第二确定模块506、第三确定模块508和第四确定模块510，其中：
计算模块502，用于通过相关扫描匹配计算得到各位姿和各位姿对应的分数；
第一确定模块504，用于在各位姿中，确定对应的分数大于分数阈值的第一位姿，以及在相关扫描匹配对应的关键分辨率下，确定在分数中的关键分数对应的关键位姿；位姿包括关键位姿；
第二确定模块506，用于确定第一位姿对应的第一协方差；
第三确定模块508，用于当第一协方差不大于协方差阈值时，则在关键分辨率下，确定关键位姿的附近位姿，并确定附近位姿对应的第二协方差；
第四确定模块510，用于当第二协方差不大于协方差阈值时，则确定关键位姿匹配正确。
在一个实施例中，第二确定模块506还用于确定第一位姿与关键位姿之间的第一差值；确定第一差值对应的第一转置差值；第一转置差值为第一差值的转置；基于第一位姿的分数、第一差值和第一转置差值得到第一协方差。
在一个实施例中，第二确定模块506还用于确定第一位姿的分数、第一差值和第一转置差值之间的第一积值；对第一积值进行求和，得到第一总积值；确定第一位姿的分数进行求和，得到第一总分数；基于第一总积值和第一总分数得到第一协方差。
在一个实施例中，第三确定模块508还用于；确定附近位姿与关键位姿之间的第二差值；确定第二差值对应的第二转置差值；第二转置差值为第二差值的转置；基于附近位姿的分数、第二差值和第二转置差值得到第二协方差。
在一个实施例中，第三确定模块508还用于确定附近位姿的分数、第二差值和第二转置差值之间的第二积值；对第二积值进行求和，得到第二总积值；确定附近位姿的分数进行求和，得到第二总分数；基于第二总积值和第二总分数得到第二协方差。
在一个实施例中，第四确定模块510还用于当第一协方差大于协方差阈值，或第二协方差大于协方差阈值时，则确定关键位姿匹配错误。
在一个实施例中，第三确定模块508还用于获取预设关键半径；依据预设关键半径确定以关键位姿为中心的关键范围；在关键分辨率下，确定关键范围中的位姿，并将关键范围中的位姿作为附近位姿。
上述实施例，通过相关扫描匹配计算得到各位姿和各位姿对应的分数；在各位姿中，确定对应的分数大于分数阈值的第一位姿，以及在相关扫描匹配对应的关键分辨率下，确定在分数中的关键分数对应的关键位姿；位姿包括关键位姿；确定第一位姿对应的第一协方差；从而实现了对相关扫描匹配中分数较高的第一位姿进行判断，是否第一位姿的第一协方差不大于协方差阈值，当第一协方差不大于协方差阈值时，即当第一位姿符合高斯分布时，则在关键分辨率下，确定关键位姿的附近位姿；并确定附近位姿对应的第二协方差；从而进一步的对关键位姿的附近区域的位姿的协方差进行判断，当第二协方差不大于协方差阈值时，则确定关键位姿匹配正确，实现了对机器人的位姿正确性的精确判断，从而有效的提高了对机器人的精准定位。
上述位姿正确性判断装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是机器人，以该计算机设备为机器人为例进行说明，其内部结构图可以如图6所示。该计算机设备包括处理器、存储器、输入/输出接口、通信接口、显示单元和输入装置。其中，处理器、存储器和输入/输出接口通过***总线连接，通信接口、显示单元和输入装置通过输入/输出接口连接到***总线。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作***和计算机程序。该内存储器为非易失性存储介质中的操作***和计算机程序的运行提供环境。该计算机设备的输入/输出接口用于处理器与外部设备之间交换信息。该计算机设备的通信接口用于与外部的终端进行有线或无线方式的通信，无线方式可通过WIFI、移动蜂窝网络、NFC(近场通信)或其他技术实现。该计算机程序被处理器执行时以实现一种位姿正确性判断方法。该计算机设备的显示单元用于形成视觉可见的画面，可以是显示屏、投影装置或虚拟现实成像装置，显示屏可以是液晶显示屏或电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解，图6中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，提供了一种机器人，包括存储器和处理器，存储器中存储有计算机程序，该处理器用于执行计算机程序时实现上述各实施例。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述各实施例。
在一个实施例中，提供了一种计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现上述各实施例。
需要说明的是，本申请所涉及的用户信息(包括但不限于用户设备信息、用户个人信息等)和数据(包括但不限于用于分析的数据、存储的数据、展示的数据等)，均为经用户授权或者经过各方充分授权的信息和数据，且相关数据的收集、使用和处理需要遵守相关国家和地区的相关法律法规和标准。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、数据库或其它介质的任何引用，均可包括非易失性和易失性存储器中的至少一种。非易失性存储器可包括只读存储器(Read-OnlyMemory，ROM)、磁带、软盘、闪存、光存储器、高密度嵌入式非易失性存储器、阻变存储器(ReRAM)、磁变存储器(Magnetoresistive Random Access Memory，MRAM)、铁电存储器(Ferroelectric Random Access Memory，FRAM)、相变存储器(Phase Change Memory，PCM)、石墨烯存储器等。易失性存储器可包括随机存取存储器(Random Access Memory，RAM)或外部高速缓冲存储器等。作为说明而非局限，RAM可以是多种形式，比如静态随机存取存储器(Static Random Access Memory，SRAM)或动态随机存取存储器(Dynamic RandomAccess Memory，DRAM)等。本申请所提供的各实施例中所涉及的数据库可包括关系型数据库和非关系型数据库中至少一种。非关系型数据库可包括基于区块链的分布式数据库等，不限于此。本申请所提供的各实施例中所涉及的处理器可为通用处理器、中央处理器、图形处理器、数字信号处理器、可编程逻辑器、基于量子计算的数据处理逻辑器等，不限于此。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本申请专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请的保护范围应以所附权利要求为准。

Claims (10)

1.一种位姿正确性判断方法，其特征在于，应用于机器人，所述方法包括：
通过相关扫描匹配计算得到各位姿和各所述位姿对应的分数；
在各所述位姿中，确定对应的所述分数大于分数阈值的第一位姿，以及在所述相关扫描匹配对应的关键分辨率下，确定在所述分数中的关键分数对应的关键位姿；所述位姿包括关键位姿；
确定所述第一位姿对应的第一协方差；
当所述第一协方差不大于协方差阈值时，则在所述关键分辨率下，确定所述关键位姿的附近位姿，并确定所述附近位姿对应的第二协方差；
当所述第二协方差不大于所述协方差阈值时，则确定所述关键位姿匹配正确。
2.根据权利要求1所述的方法，其特征在于，所述确定所述第一位姿对应的第一协方差包括：
确定所述第一位姿与所述关键位姿之间的第一差值；
确定第一差值对应的第一转置差值；所述第一转置差值为所述第一差值的转置；
基于所述第一位姿的分数、所述第一差值和所述第一转置差值得到第一协方差。
3.根据权利要求2所述的方法，其特征在于，所述基于所述第一位姿的分数、所述第一差值和所述第一转置差值得到第一协方差包括：
确定所述第一位姿的分数、所述第一差值和所述第一转置差值之间的第一积值；
对所述第一积值进行求和，得到第一总积值；
确定所述第一位姿的分数进行求和，得到第一总分数；
基于所述第一总积值和所述第一总分数得到第一协方差。
4.根据权利要求2所述的方法，其特征在于，所述确定所述附近位姿对应的第二协方差包括：
确定所述附近位姿与所述关键位姿之间的第二差值；
确定第二差值对应的第二转置差值；所述第二转置差值为所述第二差值的转置；
基于所述附近位姿的分数、所述第二差值和所述第二转置差值得到第二协方差。
5.根据权利要求4所述的方法，其特征在于，所述基于所述附近位姿的分数、所述第二差值和所述第二转置差值得到第二协方差包括：
确定所述附近位姿的分数、所述第二差值和所述第二转置差值之间的第二积值；
对所述第二积值进行求和，得到第二总积值；
确定所述附近位姿的分数进行求和，得到第二总分数；
基于所述第二总积值和所述第二总分数得到第二协方差。
6.根据权利要求1所述的方法，其特征在于，所述方法还包括：
当所述第一协方差大于所述协方差阈值，或所述第二协方差大于所述协方差阈值时，则确定所述关键位姿匹配错误。
7.根据权利要求1所述的方法，其特征在于，所述在所述关键分辨率下，确定所述关键位姿的附近位姿包括：
获取预设关键半径；
依据所述预设关键半径确定以所述关键位姿为中心的关键范围；
在所述关键分辨率下，确定所述关键范围中的位姿，并将所述关键范围中的位姿作为附近位姿。
8.一种位姿正确性判断装置，其特征在于，所述装置包括：
计算模块，用于通过相关扫描匹配计算得到各位姿和各所述位姿对应的分数；
第一确定模块，用于在各所述位姿中，确定对应的所述分数大于分数阈值的第一位姿，以及在所述相关扫描匹配对应的关键分辨率下，确定在所述分数中的关键分数对应的关键位姿；所述位姿包括关键位姿；
第二确定模块，用于确定所述第一位姿对应的第一协方差；
第三确定模块，用于当所述第一协方差不大于协方差阈值时，则在所述关键分辨率下，确定所述关键位姿的附近位姿，并确定所述附近位姿对应的第二协方差；
第四确定模块，用于当所述第二协方差不大于所述协方差阈值时，则确定所述关键位姿匹配正确。
9.一种机器人，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器用于执行所述计算机程序时实现权利要求1至7中任一项所述的方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。
CN202310431056.1A 2023-04-13 位姿正确性判断方法、装置、机器人和存储介质 Active CN116442226B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310431056.1A CN116442226B (zh) 2023-04-13 位姿正确性判断方法、装置、机器人和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310431056.1A CN116442226B (zh) 2023-04-13 位姿正确性判断方法、装置、机器人和存储介质

Publications (2)

Publication Number Publication Date
CN116442226A true CN116442226A (zh) 2023-07-18
CN116442226B CN116442226B (zh) 2024-07-26

Family

ID=

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110412596A (zh) * 2019-07-10 2019-11-05 上海电机学院 一种基于图像信息和激光点云的机器人定位方法
US20200206945A1 (en) * 2018-12-29 2020-07-02 Ubtech Robotics Corp Ltd Robot pose estimation method and apparatus and robot using the same
CN111708047A (zh) * 2020-06-16 2020-09-25 浙江大华技术股份有限公司 机器人定位评估方法、机器人及计算机存储介质
US20200320738A1 (en) * 2019-04-08 2020-10-08 Naver Labs Corporation Method and system for updating map for pose estimation based on images
CN112461230A (zh) * 2020-12-07 2021-03-09 深圳市优必选科技股份有限公司 机器人重定位方法、装置、机器人和可读存储介质

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20200206945A1 (en) * 2018-12-29 2020-07-02 Ubtech Robotics Corp Ltd Robot pose estimation method and apparatus and robot using the same
US20200320738A1 (en) * 2019-04-08 2020-10-08 Naver Labs Corporation Method and system for updating map for pose estimation based on images
CN110412596A (zh) * 2019-07-10 2019-11-05 上海电机学院 一种基于图像信息和激光点云的机器人定位方法
CN111708047A (zh) * 2020-06-16 2020-09-25 浙江大华技术股份有限公司 机器人定位评估方法、机器人及计算机存储介质
CN112461230A (zh) * 2020-12-07 2021-03-09 深圳市优必选科技股份有限公司 机器人重定位方法、装置、机器人和可读存储介质

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
贾晓雪， 赵冬青， 张乐添: "基于自适应惯导辅助特征匹配的视觉SLAM 算法", 光学精密工程, vol. 31, no. 5, 31 March 2023 (2023-03-31) *

Similar Documents

Publication Publication Date Title
CN110096929A (zh) 基于神经网络的目标检测
CN110648363A (zh) 相机姿态确定方法、装置、存储介质及电子设备
CN113223078B (zh) 标志点的匹配方法、装置、计算机设备和存储介质
CN110490839A (zh) 一种检测高速公路中损坏区域的方法、装置及计算机设备
CN110163095B (zh) 回环检测方法、回环检测装置及终端设备
CN113963072A (zh) 双目摄像头标定方法、装置、计算机设备和存储介质
CN116596935B (zh) 形变检测方法、装置、计算机设备及计算机可读存储介质
CN111027551B (zh) 图像处理方法、设备和介质
CN112749576A (zh) 图像识别方法和装置、计算设备以及计算机存储介质
CN116442226B (zh) 位姿正确性判断方法、装置、机器人和存储介质
CN116310899A (zh) 基于YOLOv5改进的目标检测方法及装置、训练方法
CN116442226A (zh) 位姿正确性判断方法、装置、机器人和存储介质
CN114663449A (zh) 道路病害面积计算方法、装置、计算机设备和存储介质
CN115601283A (zh) 图像增强方法、装置、计算机设备及计算机可读存储介质
CN112528500B (zh) 一种场景图构造模型的评估方法及评估设备
CN114549857A (zh) 图像信息识别方法、装置、计算机设备和存储介质
CN117576645B (zh) 基于bev视角的车位检测方法、装置和计算机设备
CN116295031B (zh) 弧垂的测量方法、装置、计算机设备和存储介质
CN115100286B (zh) 无人机采集视点确定方法、装置、计算机设备和存储介质
CN115577728B (zh) 一维码定位方法、装置、计算机设备及存储介质
CN114445458B (zh) 目标跟踪方法、装置、电子设备及存储介质
CN115880249B (zh) 基于图像的对象分割方法、装置、设备和介质
CN117372343A (zh) 芯片空洞的检测方法、装置、设备、存储介质和程序产品
CN117635554A (zh) 线路缺陷检测方法、装置、计算机设备及存储介质
CN117745814A (zh) Pcb板中的单元定位方法、装置、计算机设备及存储介质

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