CN112643664B - 定位误差消除方法、装置、机器人及存储介质 - Google Patents

定位误差消除方法、装置、机器人及存储介质 Download PDF

Info

Publication number
CN112643664B
CN112643664B CN201910957976.0A CN201910957976A CN112643664B CN 112643664 B CN112643664 B CN 112643664B CN 201910957976 A CN201910957976 A CN 201910957976A CN 112643664 B CN112643664 B CN 112643664B
Authority
CN
China
Prior art keywords
data
data point
matching
position information
robot
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910957976.0A
Other languages
English (en)
Other versions
CN112643664A (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 Ubtech Technology Co ltd
Original Assignee
Shenzhen Ubtech 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 Ubtech Technology Co ltd filed Critical Shenzhen Ubtech Technology Co ltd
Priority to CN201910957976.0A priority Critical patent/CN112643664B/zh
Priority to US16/726,196 priority patent/US11260528B2/en
Publication of CN112643664A publication Critical patent/CN112643664A/zh
Application granted granted Critical
Publication of CN112643664B publication Critical patent/CN112643664B/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
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position 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/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • 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/0272—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising means for registering the travel distance, e.g. revolutions of wheels
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J11/00—Manipulators not otherwise provided for
    • B25J11/003—Manipulators for entertainment
    • B25J11/0035—Dancing, executing a choreography
    • 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/1628—Programme controls characterised by the control loop
    • B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
    • 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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • 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/14—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 recording the course traversed by the object
    • 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/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser

Landscapes

  • Engineering & Computer Science (AREA)
  • Mechanical Engineering (AREA)
  • Robotics (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Evolutionary Computation (AREA)
  • Artificial Intelligence (AREA)
  • Fuzzy Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Optics & Photonics (AREA)
  • Electromagnetism (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

本申请适用于智能机器人技术领域，提供了定位误差消除方法、装置、机器人及存储介质，应用于机器人，其中方法通过机器人在执行运动指令前，获取第一位置信息作为参考数据，并在物体执行运动后且静止的情况下，采集第二位置信息，通过建立两个数据信息的空间坐标和变换模型，计算补偿值进行误差补偿，能够消除机器人运动后的积累的定位误差，实现机器人的自动位姿矫正，提高机器人的自适应能力。

Description

定位误差消除方法、装置、机器人及存储介质
技术领域
本申请属于智能机器人技术领域，尤其涉及定位误差消除方法、装置、机器人及存储介质。
背景技术
舞蹈是服务机器人一种提升娱乐性及表现力的方式。现有的轮式移动服务机器人的舞蹈动作通常包含底盘旋转及手臂摆动。底盘旋转时需要测定旋转角度，通常是基于安装在驱动电机上的编码器，通过机器人的运动学模型计算得到物理里程计，从而实时测得机器人的位姿。但在实际场景下，由于运动学模型的误差、轮子与地面的相对滑动、***的实时性、定位误差的累积等因素，在多次执行舞蹈旋转之后，机器人的朝向会明显偏离初始朝向，进而需要用户手动操作动机器人复位，而机器人无法自动实现位姿矫正，降低了机器人的自适应能力。
综上所述，目前的机器人存在无法自动实现位姿矫正而导致其自适应能力低的问题。
发明内容
本申请实施例提供了定位误差消除方法、装置、机器人及存储介质，可以解决目前的机器人存在无法自动实现位姿矫正而导致其自适应能力低的问题。
第一方面，本申请实施例提供了一种定位误差消除方法，应用于机器人，方法包括：
获取第一空间坐标系下的第一位置信息；
获取执行运动指令后的第二空间坐标系下的第二位置信息；
根据所述第一空间坐标系和所述第二空间坐标系，建立所述第一位置信息与所述第二位置信息的变换模型；
根据所述变换模型计算补偿值；
根据所述补偿值生成位置复位指令，并根据所述位置复位指令调整所述机器人的位置。
在一实施例中，所述机器人包括传感器；所述获取第一空间坐标系下的第一位置信息，包括：
获取第一初始数据；所述第一初始数据是指由所述传感器获取到的位置信息；
根据预设规则对所述第一初始数据进行筛选，得到第一动态数据信息和第一静态数据信息；
滤除所述第一动态数据信息，将所述第一静态数据信息作为所述第一位置信息。
在一实施例中，所述获取执行运动指令后的第二空间坐标系下的第二位置信息，包括：
获取第二初始数据；所述第二初始数据是指在所述机器人执行运动指令后，由所述传感器获取到的位置信息；
根据所述预设规则对所述第二初始数据进行筛选，得到第二动态数据信息和第二静态数据信息；
滤除所述第二动态数据信息，将所述第二静态数据信息作为所述第二位置信息。
在一实施例中，所述第一空间坐标系和所述第二空间坐标系均为二维空间坐标系，所述第一空间坐标系与所述第二空间坐标系的坐标原点相同；
所述根据所述第一空间坐标系和所述第二空间坐标系，建立所述第一位置信息与所述第二位置信息的变换模型，包括：
获取所述第一位置信息中的第一数据点集；所述第一数据点集包括各个第一数据点的第一位置向量；
获取所述第二位置信息中的第二数据点集；所述第二数据点集包括各个第二数据点的第二位置向量；
根据所述第一位置向量与所述第二位置向量，通过迭代最近点算法计算所述第一数据点集与第二数据点集的变换模型；所述变换模型包括平移向量和旋转矩阵。
在一实施例中，所述根据所述第一位置向量与所述第二位置向量，通过迭代最近点算法计算所述第一数据点集与第二数据点集的变换模型；所述变换模型包括平移向量和旋转矩阵，包括：
在所述第二数据点集中，对应查找与每个所述第一数据点各自相匹配的第二数据点，并根据匹配模型计算匹配误差值；所述匹配模型具体为：
Figure GDA0003697663950000031
其中，E(R，t)为第一数据点与第二数据点配准的误差值，所述Nx为所述第一数据点集中的点个数，所述Xi为第一数据点，所述Pj为第二数据点，所述R为所述第一数据点集与第二数据点集之间的旋转矩阵，所述t为所述第一数据点集与第二数据点集之间的平移向量；
根据匹配误差值配准的所述第一位置向量和所述第二位置向量，输出对应所述第一数据点与第二数据点中的变换模型；所述变换模型包括平移向量和旋转矩阵。
在一实施例中，所述获取所述第一位置信息中的第一数据点集；所述第一数据点集包括各个第一数据点的第一位置向量，包括：
获取里程计补偿值；
根据所述里程计补偿值初始化所述第一数据点集中的第一数据点，得到第一变换数据点；
获取所述第一变换数据点的位置向量作为第一位置向量。
在一实施例中，所述在所述第二数据点集中，对应查找与每个所述第一数据点各自相匹配的第二数据点，并根据匹配模型计算匹配误差值，包括：
将所述第一数据点集中所有第一变换数据点依次作为第一匹配数据点；其中，所述第一匹配数据点处于第二空间坐标系中；
在所述第二数据点集中，对应查找每个所述第一匹配数据点最邻近的第二匹配数据点；
根据所述第一匹配数据点的第一位置向量与第二匹配数据点的第二位置向量，计算所述第一数据点集与第二数据点集的变换旋转矩阵和变换平移向量；
根据所述变换旋转矩阵和变换平移向量，运用所述匹配模型计算匹配误差值；
判断所述匹配误差值的运算过程是否符合终止条件；
若所述运算过程符合终止条件，则输出所述匹配误差。
在一实施例中，所述根据所述补偿值生成位置复位指令，并根据所述位置复位指令调整所述机器人的位置，包括：
当获取到所述匹配误差值后，根据所述匹配误差值输出所述第一匹配数据点集与第二匹配数据点集之间的旋转矩阵R；
根据所述旋转矩阵计算第一误差角度；
根据所述第一误差角度对所述第二数据点集中的第二数据点进行补偿，得到补偿匹配数据点；
统计所述补偿匹配数据点与对应所述第一匹配数据点中，满足匹配优度的个数；
若所述满足匹配优度的个数大于第一阈值，则获取所述里程计补偿值对应的第二误差角度；
根据所述第一误差角度和第二误差角度，生成所述位置复位指令，并根据所述位置复位指令调整所述机器人的角度位置。
第二方面，本申请实施例提供了一种定位误差消除装置，应用于机器人，装置包括：
第一获取模块，用于获取第一空间坐标系下的第一位置信息；
第二获取用于，用于获取执行运动指令后的第二空间坐标系下的第二位置信息；
第一建立模块，用于根据所述第一空间坐标系和所述第二空间坐标系，建立所述第一位置信息与所述第二位置信息的变换模型；
第一计算模块，用于根据所述变换模型计算补偿值；
调整模块，用于根据所述补偿值生成位置复位指令，并根据所述位置复位指令调整所述机器人的位置。
第三方面，本申请实施例提供一种机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述第一方面中任一项所述的定位误差消除方法。
第四方面，本申请实施例提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上述第一方面中任一项所述的定位误差消除方法。
第五方面，本申请实施例提供了一种计算机程序产品，当计算机程序产品在机器人上运行时，使得机器人执行上述第一方面中任一项所述的定位误差消除方法。
可以理解的是，上述第二方面至第五方面的有益效果可以参见上述第一方面中的相关描述，在此不再赘述。
本申请实施例与现有技术相比存在的有益效果是：机器人在执行运动指令前，获取第一位置信息作为参考数据，并在物体执行运动后且静止的情况下，采集第二位置信息，通过建立两个数据信息的空间坐标和变换模型，计算补偿值进行误差补偿，能够消除机器人运动后的积累的定位误差，实现机器人的自动位姿矫正，提高机器人的自适应能力。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本申请实施例提供的定位误差消除方法的一种实现流程示意图；
图2是本申请实施例提供的定位误差消除装置的一种初始位置结构示意图；
图3是本申请实施例提供的定位误差消除方法的另一种实现流程示意图；
图4是本申请实施例提供的定位误差消除装置中传感器的一种结构示意图；
图5是本申请实施例提供的定位误差消除方法的又一种实现流程示意图；
图6是本申请实施例提供的定位误差消除方法中获取得第二数据点分布示意图；
图7是本申请实施例提供的定位误差消除方法中获取得第一匹配数据点分布示意图；
图8是本申请实施例提供的定位误差消除方法中旋转矩阵计算旋转角的示意图；
图9是本申请实施例提供的定位误差消除装置的一种偏移位置结构示意图；
图10是本申请实施例提供的定位误差消除装置的一种对偏移位置进行补偿的结构示意图；
图11是本申请实施例提供的定位误差消除装置中的一种结构示意图；
图12是本申请实施例提供的终端设备的结构示意图。
具体实施方式
以下描述中，为了说明而不是为了限定，提出了诸如特定***结构、技术之类的具体细节，以便透彻理解本申请实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本申请。在其它情况中，省略对众所周知的***、装置、电路以及方法的详细说明，以免不必要的细节妨碍本申请的描述。
应当理解，当在本申请说明书和所附权利要求书中使用时，术语“包括”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
还应当理解，在本申请说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
另外，在本申请说明书和所附权利要求书的描述中，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
在本申请说明书中描述的参考“一个实施例”或“一些实施例”等意味着在本申请的一个或多个实施例中包括结合该实施例描述的特定特征、结构或特点。由此，在本说明书中的不同之处出现的语句“在一个实施例中”、“在一些实施例中”、“在其他一些实施例中”、“在另外一些实施例中”等不是必然都参考相同的实施例，而是意味着“一个或多个但不是所有的实施例”，除非是以其他方式另外特别强调。术语“包括”、“包含”、“具有”及它们的变形都意味着“包括但不限于”，除非是以其他方式另外特别强调。
本申请实施例提供的定位误差消除方法可以应用于输送机器人、装运机器人和舞蹈机器人上，本申请实施例对机器人的具体类型不作任何限制。例如，定位误差消除方法应用于舞蹈机器人中，用于在舞蹈机器人在执行舞蹈旋转后，纠正当前位置会与初始位置产生的角度偏差，自动实现位姿矫正。
参照图1，本申请实施例提供一种定位误差消除方法，应用于机器人，方法包括：
S101、获取第一空间坐标系下的第一位置信息。
在应用中，上述获取的方式包括但不限于采取GPS进行定位、依靠声呐确定与参照物的距离进行定位或者依靠激光确定与参照物的距离进行定位，对此不作限定。上述第一位置信息为机器人的初始位置信息，该初始位置信息可以是机器人第一次执行动作(舞蹈)指令前获取的，用于与之后获取的位置信息作比对，判断是否产生角度误差。
在应用中，上述第一空间坐标系为根据第一位置信息建立的空间坐标，其中，第一空间坐标系可以为二维坐标系，也可以为三维坐标系，或者其他多维坐标系，第一位置信息对应的可以为用二维坐标(a，b)表示，也可以用三维坐标(a，b，c)表示，对此不作限定。具体的，第一空间坐标系可以为以二维空间坐标系，以机器人为原点的正前方方向为Y轴方向，以机器人为原点的水平方向的右侧为X轴方向建立坐标系。参照图2，图2中箭头所指方向可认为是机器人的正前方，即为Y轴方向，X轴方向图中未画出。
S102、获取执行运动指令后的第二空间坐标系下的第二位置信息。
在应用中，第二位置信息为机器人执行舞蹈指令之后进行获取的，其获取方式可以与获取第一位置信息的方式一致，且第二空间坐标系与第一空间坐标系为同维度坐标系。
在应用中，上述第二空间坐标系与第一空间坐标系一致，均为二维空间坐标系，以执行运动指令旋转后的机器人为原点，其正前方为Y轴方向，以机器人为原点的水平方向的右侧为X轴方向建立坐标系。如果产生角度误差，此时第一空间坐标系的Y轴和第二空间坐标系的Y轴之间会形成夹角，第一空间坐标系的X轴和第二空间坐标系的X轴之间也会形成夹角，夹角的角度就是该角度误差，如果没有产生角度误差，那第一空间坐标系的Y轴会和第二空间坐标系的Y轴重合，且第一空间坐标系的X轴会和第二空间坐标系的X轴重合。
S103、根据所述第一空间坐标系和所述第二空间坐标系，建立所述第一位置信息与所述第二位置信息的变换模型。
在应用中，上述第一空间坐标系与第二空间坐标系中的坐标原点可以一致，也可以是将第二空间坐标系的原点，在第一空间坐标系中用相应数据进行表示，对此不作限定。上述第一位置信息与第二位置信息具有变换关系，具体可以为，若第一空间坐标系与第二空间坐标系中的坐标原点一致，第一位置信息经过旋转变换，可以与第二位置信息重叠，即第一位置信息可以用第二空间坐标系进行表示。
S104、根据所述变换模型计算补偿值。
在应用中，上述变换模型用于当第一位置信息与第二位置信息经过旋转重叠时，计算第一位置信息变换为第二位置信息旋转的角度，或者记录移动的方向和计算移动的距离。其中，计算的旋转角度、记录移动的方向和计算移动的距离均为机器人在执行舞蹈指令后产生的补偿值。
S105、根据所述补偿值生成位置复位指令，并根据所述位置复位指令调整所述机器人的位置。
在应用中，上述当机器人计算出补偿值时，机器人会生成对应的位置复位指令，上述位置复位指令用于指示机器人自动调整位置，对机器人旋转之后(执行舞蹈动作之后)产生的角度误差进行补偿，其补偿角度值为计算出的误差角度值，进而自动实现机器人的位姿矫正。上述位置复位指令还包括在计算出产生的角度误差后，可保留该误差角度，在下次接收到舞蹈指令进行旋转时加入该误差角度进行补偿。
在其他应用中，若补偿值小于预设值，如预设值为2°，补偿值小于2°，则不进行误差补偿，即不产生位置复位指令，当下一次执行舞蹈指令进行旋转后，将再次获取的第二位置信息与第一位置信息进行比较。其中，本实施例中的第一位置信息为机器人第一次获取并确定的位置信息，后续旋转之后获取的所有第二位置信息均以第一次获取的第一位置信息为参照数据，即补偿值是以初始的第一位置信息为参照数据计算的，当补偿值大于或等于2°时，则生成位置复位指令调整所述机器人的位置，避免机器人旋转之后偏移角度过小，而机器人需次次调整位置的情况发生，缩短了机器人内部用于调整位置的装置的使用时间。
在本实施例中，机器人通过在执行运动指令前，获取第一位置信息作为参考数据，并在物体执行运动后且静止的情况下，采集第二位置信息，通过建立两个数据信息的空间坐标和变换模型，计算补偿值进行误差补偿，能够消除机器人运动后的积累的定位误差，实现机器人的自动位姿矫正，提高机器人的自适应能力。
参照图3，在一实施例中，所述机器人包括传感器；步骤S101包括：
S201、获取第一初始数据；所述第一初始数据是指由所述传感器获取到的位置信息。
在应用中，参照图4，上述传感器2为机器人3内部的结构，具体为激光雷达，设于机器人3的底盘中，图中1为驱动轮1，用于驱动机器人行走，获取第一位置信息可以为激光雷达旋转一周并发射激光，而后获取的激光数据，其中，激光数据用于标识机器人的第一位置信息。具体的，激光雷达为二维激光扫描测距雷达，激光头可360度旋转并同时发射激光，该激光信号在照射到周围的物体后产生的反光而后被探头采集，经过实时解算，可以得到被照射到的物体的距离值以及当前的夹角信息。产生的激光数据指激光投射到物体上的光斑与二维激光扫描测距雷达旋转中心的距离、以及光线与二维激光扫描测距雷达坐标系的夹角，根据距离与夹角确定机器人的位置信息。在旋转一周的过程中，二维激光扫描测距雷达会发射很多次激光，从而得到周围环境的几何信息，其中“一帧”激光数据就是指二维激光扫描测距雷达旋转一周得到的激光数据。
S202、根据预设规则对所述第一初始数据进行筛选，得到第一动态数据信息和第一静态数据信息。
在应用中，上述预设规则可以为机器人内部预先设置，也可以为用户自行设置。具体的，预设规则可以为只获取激光雷达在特定角度下获取的激光数据，例如，机器人划分角度为0°-360°，其中0°和360°为机器人事先设置的正前方，因此，机器人可以设定10°-350°获取的激光数据为第一静态数据信息，0°-10°和350°到360°为第一动态数据信息。
S203、滤除所述第一动态数据信息，将所述第一静态数据信息作为所述第一位置信息。
在应用中，采用激光数据是由于其能够反映周围环境的信息，周围环境可分为动态环境(人为走动产生的环境变化)和静态环境(不变动的环境)，一般来讲，机器人在执行舞蹈指令时，用户是处于机器人正前方进行观看的，因此可以去除0°-10°和350°到360°获取的激光数据，保留在静态环境获取的一帧激光数据。且在机器人连续的舞蹈旋转时，只采集最开始一次舞蹈旋转前的激光数据作为参考激光数据，后续的每次舞蹈旋转后采集的激光数据都会与之做匹配，以便做到无累积误差。
在本实施例中，通过获取静态环境中的第一初始数据来作为第一位置信息，以便减小第二位置信息在与第一位置信息匹配时产生的补偿值过大的问题。
参照图5，在一实施例中，步骤S102包括：
S301、获取第二初始数据；所述第二初始数据是指在所述机器人执行运动指令后，由所述传感器获取到的位置信息。
在应用中，上述第二初始数据的获取方式与获取第一初始数据的方式一致，均有激光雷达获取的激光数据，上述第二初始数据具体可以为机器人执行舞蹈指令后处于静止时，底盘中的激光雷达旋转一周获取的，即第一初始数据和第二初始数据均为机器人静止时获取，提高通过激光雷达获取机器人位置信息的准确性。
S302、根据所述预设规则对所述第二初始数据进行筛选，得到第二动态数据信息和第二静态数据信息。
在应用中，上述对第二初始数据进行筛选的预设规则与对第一初始数据进行筛选的预设规则一致，均是以机器人划分角度为0°-360°，其中0°和360°为机器人事先设置的正前方，其中此时机器人事先设置的正前方因机器人在旋转后发生角度偏移，假设为顺时针偏移5°的误差角度，其中该顺时针偏移5°的误差角度可以为由机器人内部的物理里程计计算初步得到的，与真实误差角度具有一定的偏差，也可以为用户自身设置的预估值，对此不做限定。因此，机器人此时根据预设规则实际设定因是15°-355°获取的激光数据为第一静态数据信息，机器人此时根据预设规则实际设定因是0°-15°和355°-360°为第一动态数据信息。
S303、滤除所述第二动态数据信息，将所述第二静态数据信息作为所述第二位置信息。
在应用中，为尽可能使前后两次获取的位置信息匹配成功，在机器人连续的舞蹈旋转后，通过对机器人正前方的预设角度范围的数据进行滤除，尽可能的消除机器人正前方用户走动而获取的激光数据，以达到减少动态数据的占比的目的。
在本实施例中，通过获取静态环境中的第一位置信息，和同时获取静态环境中的第二位置信息，通过增加两者位置信息中静态数据占比，提供第二位置信息与第一位置信息匹配成功的概率，减小第二位置信息在与第一位置信息匹配时产生的补偿值过大。
在一实施例中，所述第一空间坐标系和所述第二空间坐标系均为二维空间坐标系，所述第一空间坐标系与所述第二空间坐标系的坐标原点相同；步骤S103包括：
获取所述第一位置信息中的第一数据点集；所述第一数据点集包括各个第一数据点的第一位置向量。
在应用中，上述激光雷达获取的激光数据为二维数据，对此可建立二维空间坐标系，且二维空间坐标系的原点相同，均以机器人或者激光雷达为坐标原点构建二维空间坐标系。
在应用中，上述第一位置信息具体可以为第一数据点集中的各个第一数据点综合表示第一位置信息，如X＝{Xi|i＝1，2，…，N}，上述X为第一数据点集，Xi为第一数据点集中第i个第一数据点，其中该第一数据点的第一位置向量可以为以原点为起点的向量(a，b)，N为第一数据点集中共有N个第一数据点，对此不作限定。
获取所述第二位置信息中的第二数据点集；所述第二数据点集包括各个第二数据点的第二位置向量。
在应用中，上述第二位置信息具体可以为第二数据点集中的各个第二数据点综合表示第二位置信息，如P＝{Pj|j＝1，2，…，M}，上述P为第二数据点集，Pj为第二数据点集中第j个第二数据点，其中该第二数据点的第二位置向量可以为以原点为起点的向量(c，d)，M为第二数据点集中共有M个第二数据点，参照图6，图6为本实施例中获取的第二数据点。
根据所述第一位置向量与所述第二位置向量，通过迭代最近点算法计算所述第一数据点集与第二数据点集的变换模型；所述变换模型包括平移向量和旋转矩阵。
在应用中，上述第一数据点集与第二数据点集均包括多个第一位置向量与所述第二位置向量，具体可根据在第一数据点集中对应查找每个第一位置信息，与第二数据点集的第二位置信息的最近点，并依次进行迭代算法得到变换模型，或者在第二数据点集中对应查找每个第二位置信息，与第一数据点集的第一位置信息的最近点，对此不作限定。
在一实施例中，所述根据所述第一位置向量与所述第二位置向量，通过迭代最近点算法计算所述第一数据点集与第二数据点集的变换模型；所述变换模型包括平移向量和旋转矩阵，包括：
在所述第二数据点集中，对应查找与每个所述第一数据点各自相匹配的第二数据点，根据匹配模型计算匹配误差值；所述匹配模型具体为：
Figure GDA0003697663950000131
其中，E(R，t)为第一数据点与第二数据点配准的均方误差值，所述Nx为所述第一数据点集中的点个数，所述Xi为第一数据点，所述Pj为第二数据点，所述R为所述第一数据点集与第二数据点集之间的旋转矩阵，所述t为所述第一数据点集与第二数据点集之间的平移向量。
在应用中，上述第一数据点集和第二数据点集均对应有多个数据点，每个数据点均对应各自的第一位置向量或者第二位置向量，其中，可通过在第二空间坐标系中的第二数据点集中，对应查找与每个第一数据点各自相匹配的第二数据点，例如查找方式可包括查找每个第一数据点各自相邻近的第二数据点。
在应用中，第一数据点的Xi对应为第二数据点集中的某个第二数据点Pj。具体可为，E(R，t)为未知量，Nx为所述第一数据点集中的点个数为已知量，Pj为第j个第二数据点的第二位置向量为已知量，Xi为第i个第一数据点的第一位置向量为已知量，此时R为X与P之间的旋转矩阵，所述t为X与P之间的平移向量，均为已知量。因此，可根据上述匹配模型求出第一数据点与第二数据点的均方误差值E(R，t)。
根据匹配误差值配准的所述第一位置向量和所述第二位置向量，输出对应所述第一数据点集与第二数据点集中的变换模型；所述变换模型包括平移向量和旋转矩阵。
在应用中，上述步骤可对应查出每个第一数据点配准的第二数据点，因此，可输出此时用于计算的旋转矩阵R和平移向量t对应建立变换模型。具体的，单个第一数据点与第二数据点之间的点平移向量具体可以为t'(c-a，d-b)，其中，因第一数据点集X具有i个第一数据点，因此可在第二数据点集P中对应找出i个最邻近的第二数据点，之后根据i个第一数据点、i个第二数据点之间的位置向量，和根据线性关系得到两个数据点集的平移向量t以及旋转矩阵R。并且在二维空间中，旋转矩阵与旋转角的表达式是固定的，在二维空间中，旋转可以用一个单一的角θ定义。因此，可根据平移向量和旋转矩阵构建输出关于旋转角的变换模型。
在应用中，上述变换模型为对机器人旋转前后的激光数据进行匹配计算，利用算法对两个不同空间坐标系的数据点集X＝{Xi}、P＝{Pj}进行配准，找出两个数据点集之间的空间变换关系(包括平移向量t以及旋转矩阵R)。目的在于基于配准的第一数据点与第二数据点，精确求出两个数据点集之间的变换关系，且均以机器人为坐标原点构建空间坐标系更有利于算出第一位置信息与第二位置信息之间的变换关系。
在一实施例中，所述获取所述第一位置信息中的第一数据点集；所述第一数据点集包括各个第一数据点的第一位置向量，包括：
获取里程计补偿值。
在应用中，上述里程计补偿值为机器人内部设置的编码器根据机器人的运动学模型计算的补偿值。具体的，上述编码器为旋转编码器是由光栅盘(分度码盘)和光电检测装置(接收器)组成。光栅盘是在一定直径的圆板上等分地开通若干个长方形孔。由于光栅盘与机器人的电机同轴，电机旋转时，光栅盘与电机同速旋转，发光二极管垂直照射光栅盘，把光栅盘图像投射到由光敏元件构成的光电检测装置(接收器)上，光栅盘转动所产生的光变化经转换后以相应的脉冲信号的变化输出。可以通过上述方法测量电机转动的角度，从而获取驱动轮转过的角度，进而根据机器人的运动学模型初步计算机器人的偏移量，如根据角度与旋转矩阵的关系获取机器人的偏移量，将偏移量作为机器人的里程计补偿值。
根据所述里程计补偿值初始化所述第一数据点集中的第一数据点，得到第一变换数据点。
获取所述第一变换数据点的位置向量作为第一位置向量。
在应用中，上述机器人旋转之后发生偏移不仅包括通过运动学模型初步计算得到物理里程计误差，还包括物理里程计的误差累积和轮子与地面的相对滑动等，而其他误差可通过激光雷达获取激光数据计算得出。因此，为准确通过激光雷达获取激光数据计算补偿值，需对第一数据点进行初始化，如进行里程计误差补偿，滤除里程计对第一数据点的影响，降低第二数据点与第一数据点进行配准的误差值。具体可参照图7，图7为第一数据点进行初始化后得到的第一匹配数据点。
在本实施例中，先通过对第一数据点进行初始化，进行里程计误差补偿，可消除机器人运动后的积累的里程计误差，提高机器人自动位姿矫正的准确率。
在一实施例中，所述根据匹配模型，在所述第二数据点集中，对应查找与每个所述第一数据点各自相匹配的第二数据点，包括：
将所述第一数据点集中所有第一变换数据点依次作为第一匹配数据点，其中，所述第一变换数据点处于第二空间坐标系中。
在所述第二数据点集中，对应查找每个所述第一匹配数据点最邻近的第二匹配数据点。
在应用中，上述第一变换数据点为第一数据点经过物理里程计误差进行初始化补偿后得到，因此，可以初步认为第一数据点经过初始补偿与第二空间坐标系中的第二匹配数据点基本重合，即可认为第一变换数据点处于第二空间坐标系中。上述在第二数据点集中，对应查找每个第一匹配数据点最邻近的第二匹配数据点，其查找方式包括查找每个第一数据点各自相邻近的第二数据点，其中，最邻近的数据点可根据第一位置向量和第二位置向量计算两个数据点之间的距离，即先确定第一点集中的第一个第一变换数据点，而后计算与第二数据点集P中每个第二数据点之间的距离，其距离最小的即可判定为最邻近的第二匹配数据点，距离计算公式可位置向量进行计算，对此不作详细描述。
根据所述第一匹配数据点的第一位置向量与第二匹配数据点的第二位置向量，计算变换旋转矩阵和变换平移向量。
根据所述变换旋转矩阵和平移向量，运用所述匹配模型计算各个匹配误差值。
判断所述匹配误差值的运算过程是否符合终止条件。
若所述运算过程符合终止条件，则输出所述匹配误差值。
在应用中，上述变换旋转矩阵和变换平移向量为所有第一变换数据点的第一位置向量与所有第二匹配数据点的第二位置向量计算得到的，是为点集与点集之间的整体变换模型。具体的，可以离散点使用数据拟合的方法拟合出其符合第一数据点集中离散点的曲线方程K(z)，和第二数据点集中离散点的曲线方程Y(x)，曲线类型可为线性拟合，高斯曲线，平滑曲线等，根据需要选择对应曲线类型，给出拟合函数表达式，根据两个表达式计算第一数据点集与第二数据点集之间的变换旋转矩阵和变换平移向量；或者根据各个第一匹配数据点与第二匹配数据点之间的位置向量和线性关系，计算旋转矩阵和平移向量，对此不作限定。
在应用中，上述符合终止条件包括但不限于使用算法次数达到预设次数、均方误差值小于预设阈值等，具体的，运用上述变换模型得出的匹配误差值出现小于0.1的数值，即认为运算过程符合终止条件，对此不作限定。
在应用，为了精准第二数据点集与第一数据点集中各个数据点的匹配度，采取运算终止条件，计算最优匹配误差值，以达到机器人旋转后进行精确补偿的目的。
在另一实施例中，判断所述匹配误差值的运算过程是否符合终止条件之后，还包括：
若匹配误差值的运算过程不否符合终止条件，则根据所述变换旋转矩阵和平移向量，计算当前误差补偿值；
根据所述误差补偿值初始化所述第一变换数据点，得到新第一变换数据点；
在所述第二数据点集中，对应查找每个所述新第一变换数据点最邻近的新第二匹配数据点；
重复所述运算过程，直至所述运算过程符合终止条件，则输出当前匹配误差值作为最终的所述匹配误差值。
在应用中，上述运算过程为迭代算法运算过程。具体的，1)根据机器人内部的物理里程计获取一个初始旋转矩阵R'和初始平移向量t'，此初始旋转矩阵R'和初始平移向量t'可根据物理里程计直接获取，对此不做特别说明；2)对第一数据点集中的第一数据点进行初始化得到第一匹配数据点；此时，所有的第一匹配数据点都处于第二空间坐标系中，此时新第一数据点集以X'表示；3)在第二数据点集P中，找与新第一数据点集X'中的每一个第一匹配数据点，与之距离最近的点，形成最近点的点对关系，能找到M个点对(因为P中有M个点)；4)根据M个最近点的点对，和根据线性关系、位置向量，计算新第一数据点集X'和第二数据点集P的旋转矩阵R”和平移向量t”；5)之后根据匹配模型计算得到匹配误差值；6)若匹配误差值的运算过程不符合终止条件，则根据旋转矩阵R”和平移向量t”计算当前误差补偿值，并对第一数据点集X'中的第一匹配数据点进行补偿，再次得到新的第一数据点集X”(在上一次第一匹配数据点的基础上进行补偿得到的)，重复上述运算过程3)、4)、5)、6)直至匹配误差值的运算过程符合终止条件。
在一实施例中，步骤S105包括：
当获取到所述匹配误差值后，根据所述匹配误差值输出所述第一匹配数据点集与第二匹配数据点集之间的旋转矩阵R。
根据所述旋转矩阵计算第一误差角度。
在应用中，上述匹配误差值为第一数据点集经过多次迭代算法进行补偿后与第二数据点集计算出的，因此，可对应获取当前匹配误差值对应的平移向量t和旋转矩阵R。具体的，参照图8，平移向量和旋转矩阵的关系如下：设第一匹配数据点V坐标为(x，y)第二匹配数据点的坐标为(x'，y')，到原点的距离为r，原点到V点的向量与X轴的夹角为φ，设定旋转角为θ，则可得x＝rcosφ；y＝rsinφ；x'＝rcos(θ+φ)；y'＝rsin(θ+φ)；结合上述公式可得x'＝rcosθcosφ-rsinθsinφ；y'＝rsinθcosφ+rcosθsinφ；带入x和y表达式可得：x'＝xcosθ-ysinθ；y'＝xsinθ+ycosθ；即写成矩阵为：
Figure GDA0003697663950000181
根据所述第一误差角度对所述第二数据点集中的第二数据点进行补偿，得到补偿匹配数据点。
统计所述补偿匹配数据点与对应所述第一匹配数据点中，满足匹配优度的个数。
在应用中，上述补偿匹配数据点为第二数据点集中，第二数据点补偿旋转角θ之后得到的补偿匹配数据点，用于与第一匹配数据点进行配准。在应用中，上述补偿匹配数据点应与第一匹配数据点具有部分重叠的数据点，统计所有补偿旋转角后，补偿匹配数据点与第一匹配数据点满足匹配优度的个数。
在应用中，上述匹配优度具体可为，补偿匹配数据点与第一匹配数据点重叠，或者补偿匹配数据点与第一匹配数据点两者间的距离小于预设距离，即认为满足匹配优度，上述两者间的距离具体可为确定两者数据点的位置向量，根据位置向量计算距离值，对此不作限定。
若所述满足匹配优度的个数大于第一阈值，则获取所述里程计补偿值对应的第二误差角度。
根据所述第一误差角度和第二误差角度，生成所述位置复位指令，并根据所述位置复位指令调整所述机器人的角度位置。
在应用中，上述第一阈值具体可以为满足匹配优度的个数在所有补偿匹配数据点中的占比，若所有补偿匹配数据点为10个，满足匹配优度的个数大于或等于7个，则认为满足匹配优度的个数大于第一阈值。上述第二误差角度即为根据旋转矩阵R计算得出的旋转角θ，上述第一匹配数据点为第一数据点进行初始化得到第一变换数据点，即在进行补偿值时，需要叠加里程计补偿值对应的第二误差角度得到目标补偿角度
Figure GDA0003697663950000191
生成位置复位指令，调整所述机器人的角度位置。具体可参照图9和图10，图9为机器人在旋转过程中产生顺时针的误差角
Figure GDA0003697663950000192
因此，可控制机器人顺时针旋转
Figure GDA0003697663950000193
回到初始位置，具体可参照图10。
在本实施例中，通过机器人在执行运动指令前，获取第一位置信息作为参考数据，并在物体执行运动后且静止的情况下，采集第二位置信息，通过建立两个数据信息的空间坐标和变换模型，计算补偿值进行误差补偿，能够消除机器人运动后的积累的定位误差，实现机器人的自动位姿矫正，提高机器人的自适应能力。
参照图11，本实施例还提供一种定位误差消除装置100，应用于机器人，包括：
第一获取模块10，用于获取第一空间坐标系下的第一位置信息。
第二获取模块20，用于获取执行运动指令后的第二空间坐标系下的第二位置信息。
第一建立模块30，用于根据所述第一空间坐标系和所述第二空间坐标系，建立所述第一位置信息与所述第二位置信息的变换模型。
第一计算模块40，用于根据所述变换模型计算补偿值。
调整模块50，用于根据所述补偿值生成位置复位指令，并根据所述位置复位指令调整所述机器人的位置。
在应用中，上述定位误差消除装置可以为机器人内部的硬件结构，也可以为机器人本身，对此不作限定。
在一实施例中，所述机器人包括传感器；第一获取模块10还用于：
获取第一初始数据；所述第一初始数据是指由所述传感器获取到的位置信息；
根据预设规则对所述第一初始数据进行筛选，得到第一动态数据信息和第一静态数据信息；
滤除所述第一动态数据信息，将所述第一静态数据信息作为所述第一位置信息。
在一实施例中，第二获取模块20还用于：
获取第二初始数据；所述第二初始数据是指在所述机器人执行运动指令后，由所述传感器获取到的位置信息；
根据所述预设规则对所述第二初始数据进行筛选，得到第二动态数据信息和第二静态数据信息；
滤除所述第二动态数据信息，将所述第二静态数据信息作为所述第二位置信息。
在一实施例中，所述第一空间坐标系和所述第二空间坐标系均为二维空间坐标系，所述第一空间坐标系与所述第二空间坐标系的坐标原点相同；第一建立模块30还用于：
所述根据所述第一空间坐标系和所述第二空间坐标系，建立所述第一位置信息与所述第二位置信息的变换模型，包括：
获取所述第一位置信息中的第一数据点集；所述第一数据点集包括各个第一数据点的第一位置向量；
获取所述第二位置信息中的第二数据点集；所述第二数据点集包括各个第二数据点的第二位置向量；
根据所述第一位置向量与所述第二位置向量，通过迭代最近点算法计算所述第一数据点集与第二数据点集的变换模型；所述变换模型包括平移向量和旋转矩阵。
在一实施例中，第一建立模块30还用于：
在所述第二数据点集中，对应查找与每个所述第一数据点各自相匹配的第二数据点，并根据匹配模型计算匹配误差值；所述匹配模型具体为：
Figure GDA0003697663950000211
其中，E(R，t)为第一数据点与第二数据点配准的均方误差值，所述Nx为所述第一数据点集中的点个数，所述Xi为第一数据点，所述Pj为第二数据点，所述R为所述第一数据点集与第二数据点集之间的旋转矩阵，所述t为所述第一数据点集与第二数据点集之间的平移向量；
根据匹配误差值配准的所述第一位置向量和所述第二位置向量，输出对应所述第一数据点集与第二数据点集中的变换模型；所述变换模型包括平移向量和旋转矩阵。
在一实施例中，第一建立模块30还用于：
获取里程计补偿值；
根据所述里程计补偿值初始化所述第一数据点集中的第一数据点，得到第一变换数据点；
获取所述第一变换数据点的位置向量作为第一位置向量。
在一实施例中，第一建立模块30还用于：
将所述第一数据点集中所有第一变换数据点依次作为第一匹配数据点；其中，所述第一匹配数据点处于第二空间坐标系中；
在所述第二数据点集中，对应查找每个所述第一匹配数据点最邻近的的第二匹配数据点；
根据所述第一匹配数据点的第一位置向量与第二匹配数据点的第二位置向量，计算所述第一数据点集与第二数据点集的变换旋转矩阵和变换平移向量；
根据所述变换旋转矩阵和变换平移向量，运用所述匹配模型计算各个匹配误差值；
判断所述匹配误差值的运算过程是否符合终止条件；
若所述运算过程符合终止条件，则输出所述匹配误差值。
在一实施例中，调整模块50还用于：
当获取到所述匹配误差值后，根据所述匹配误差值输出所述第一匹配数据点集与第二匹配数据点集之间的旋转矩阵R；
根据所述旋转矩阵计算第一误差角度；
根据所述第一误差角度对所述第二数据点集中的第二数据点进行补偿，得到补偿匹配数据点；
统计所述补偿匹配数据点与对应所述第一匹配数据点中，满足匹配优度的个数；
若所述满足匹配优度的个数大于第一阈值，则获取所述里程计补偿值对应的第二误差角度；
根据所述第一误差角度和第二误差角度，生成所述位置复位指令，并根据所述位置复位指令调整所述机器人的角度位置。
本实施例中，通过机器人在执行运动指令前，获取第一位置信息作为参考数据，并在物体执行运动后且静止的情况下，采集第二位置信息，通过建立两个数据信息的空间坐标和变换模型，计算补偿值进行误差补偿，能够消除机器人运动后的积累的定位误差，实现机器人的自动位姿矫正，提高机器人的自适应能力。
本申请实施例提供一种机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时可实现上述各个方法实施例中的步骤。
本申请实施例提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时可实现上述各个方法实施例中的步骤。
本申请实施例提供了一种计算机程序产品，当计算机程序产品在机器人上运行时，使得机器人执行时可实现上述各个方法实施例中的步骤。
图12是本申请一实施例提供的机器人80的示意图。如图12所示，该实施例的机器人80包括：处理器803、存储器801以及存储在所述存储器801中并可在所述处理器803上运行的计算机程序802。所述处理器803执行所述计算机程序802时实现上述各个方法实施例中的步骤，例如图1所示的步骤S101至S105。或者，所述处理器803执行所述计算机程序802时实现上述各装置实施例中各模块/单元的功能。
示例性的，所述计算机程序802可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器801中，并由所述处理器803执行，以完成本申请。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序802在所述机器人80中的执行过程。例如，所述计算机程序802可以被分割成第一获取模块、第二获取模块、第一建立模块、第一计算模块和调整模块，各模块具体功能如下：
第一获取模块，用于获取第一空间坐标系下的第一位置信息；
第二获取用于，用于获取执行运动指令后的第二空间坐标系下的第二位置信息；
第一建立模块，用于根据所述第一空间坐标系和所述第二空间坐标系，建立所述第一位置信息与所述第二位置信息的变换模型；
第一计算模块，用于根据所述变换模型计算补偿值；
调整模块，用于根据所述补偿值生成位置复位指令，并根据所述位置复位指令调整所述机器人的位置。
所述机器人80可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述机器人可包括，但不仅限于，处理器803、存储器801。本领域技术人员可以理解，图12仅仅是机器人80的示例，并不构成对机器人80的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述机器人还可以包括输入输出设备、网络接入设备、总线等。
所称处理器803可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述存储器801可以是所述机器人80的内部存储单元，例如机器人80的硬盘或内存。所述存储器801也可以是所述机器人80的外部存储设备，例如所述机器人80上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。在一个实施例中，所述存储器801还可以既包括所述机器人80的内部存储单元也包括外部存储设备。所述存储器801用于存储所述计算机程序以及所述机器人所需的其他程序和数据。所述存储器801还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述***中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
在本申请所提供的实施例中，应该理解到，所揭露的装置/通信终端和方法，可以通过其它的方式实现。例如，以上所描述的装置/通信终端实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个***，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
以上所述实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。

Claims (10)

1.一种定位误差消除方法，其特征在于，应用于舞蹈机器人，所述舞蹈机器人包括传感器，方法包括：
获取第一空间坐标系下的第一位置信息；所述第一位置信息为所述舞蹈机器人第一次获取的位置信息；
获取执行运动指令后的第二空间坐标系下的第二位置信息；所述运动指令用于控制所述舞蹈机器人执行舞蹈旋转；
根据所述第一空间坐标系和所述第二空间坐标系，建立所述第一位置信息与所述第二位置信息的变换模型；
根据所述变换模型计算补偿值；
根据所述补偿值生成位置复位指令，并根据所述位置复位指令调整所述舞蹈机器人的位置；所述位置复位指令用于对所述舞蹈机器人执行舞蹈旋转后产生的角度误差进行补偿；
所述获取第一空间坐标系下的第一位置信息，包括：
获取第一初始数据；所述第一初始数据是指由所述传感器获取到的位置信息；
根据预设规则对所述第一初始数据进行筛选，得到第一动态数据信息和第一静态数据信息；所述预设规则包括将所述传感器在10°到350°获取的激光数据设定为所述第一静态数据信息，以及，在0°-10°和350°-360°获取的激光数据设定为所述第一动态数据信息；
滤除所述第一动态数据信息，将所述第一静态数据信息作为所述第一位置信息。
2.如权利要求1所述的定位误差消除方法，其特征在于，所述获取执行运动指令后的第二空间坐标系下的第二位置信息，包括：
获取第二初始数据；所述第二初始数据是指在所述舞蹈机器人执行运动指令后，由所述传感器获取到的位置信息；
根据所述预设规则对所述第二初始数据进行筛选，得到第二动态数据信息和第二静态数据信息；
滤除所述第二动态数据信息，将所述第二静态数据信息作为所述第二位置信息。
3.如权利要求1所述的定位误差消除方法，其特征在于，所述第一空间坐标系和所述第二空间坐标系均为二维空间坐标系，所述第一空间坐标系与所述第二空间坐标系的坐标原点相同；
所述根据所述第一空间坐标系和所述第二空间坐标系，建立所述第一位置信息与所述第二位置信息的变换模型，包括：
获取所述第一位置信息中的第一数据点集；所述第一数据点集包括各个第一数据点的第一位置向量；
获取所述第二位置信息中的第二数据点集；所述第二数据点集包括各个第二数据点的第二位置向量；
根据所述第一位置向量与所述第二位置向量，通过迭代最近点算法计算所述第一数据点集与第二数据点集的变换模型；所述变换模型包括平移向量和旋转矩阵。
4.如权利要求3所述的定位误差消除方法，其特征在于，所述根据所述第一位置向量与所述第二位置向量，通过迭代最近点算法计算所述第一数据点集与第二数据点集的变换模型；所述变换模型包括平移向量和旋转矩阵，包括：
在所述第二数据点集中，对应查找与每个所述第一数据点各自相匹配的第二数据点，并根据匹配模型计算匹配误差值；所述匹配模型具体为：
Figure FDA0003748544070000021
其中，E(R，t)为第一数据点与第二数据点配准的均方误差值，所述Nx为所述第一数据点集中的点个数，所述Xi为第一数据点，所述Pj为第二数据点，所述R为所述第一数据点集与第二数据点集之间的旋转矩阵，所述t为所述第一数据点集与第二数据点集之间的平移向量；
根据匹配误差值配准的所述第一位置向量和所述第二位置向量，输出对应所述第一数据点集与第二数据点集的变换模型；所述变换模型包括平移向量和旋转矩阵。
5.如权利要求4所述的定位误差消除方法，其特征在于，所述获取所述第一位置信息中的第一数据点集；所述第一数据点集包括各个第一数据点的第一位置向量，包括：
获取里程计补偿值；
根据所述里程计补偿值初始化所述第一数据点集中的第一数据点，得到第一变换数据点；
获取所述第一变换数据点的位置向量作为第一位置向量。
6.如权利要求5所述的定位误差消除方法，其特征在于，所述在所述第二数据点集中，对应查找与每个所述第一数据点各自相匹配的第二数据点，并根据匹配模型计算匹配误差值，包括：
将所述第一数据点集中所有第一变换数据点依次作为第一匹配数据点；其中，所述第一匹配数据点处于第二空间坐标系中；
在所述第二数据点集中，对应查找每个所述第一匹配数据点最邻近的第二匹配数据点；
根据所述第一匹配数据点的第一位置向量与第二匹配数据点的第二位置向量，计算所述第一数据点集与第二数据点集的变换旋转矩阵和变换平移向量；
根据所述变换旋转矩阵和变换平移向量，运用所述匹配模型计算匹配误差值；
判断所述匹配误差值的运算过程是否符合终止条件；
若所述运算过程符合终止条件，则输出所述匹配误差值。
7.如权利要求6所述的定位误差消除方法，其特征在于，所述根据所述补偿值生成位置复位指令，并根据所述位置复位指令调整所述舞蹈机器人的位置，包括：
当获取到所述匹配误差值后，根据所述匹配误差值输出所述第一数据点集与第二数据点集之间的旋转矩阵R；
根据所述旋转矩阵计算第一误差角度；
根据所述第一误差角度对所述第二数据点集中的第二数据点进行补偿，得到补偿匹配数据点；
统计所述补偿匹配数据点与对应所述第一匹配数据点中，满足匹配优度的个数；
若所述满足匹配优度的个数大于第一阈值，则获取所述里程计补偿值对应的第二误差角度；
根据所述第一误差角度和第二误差角度，生成所述位置复位指令，并根据所述位置复位指令调整所述舞蹈机器人的角度位置。
8.一种定位误差消除装置，其特征在于，应用于舞蹈机器人，所述舞蹈机器人包括传感器，包括：
第一获取模块，用于获取第一空间坐标系下的第一位置信息；所述第一位置信息为所述舞蹈机器人第一次获取的位置信息；
第二获取模块，用于获取执行运动指令后的第二空间坐标系下的第二位置信息；所述运动指令用于控制所述舞蹈机器人执行舞蹈旋转；
第一建立模块，用于根据所述第一空间坐标系和所述第二空间坐标系，建立所述第一位置信息与所述第二位置信息的变换模型；
第一计算模块，用于根据所述变换模型计算补偿值；
调整模块，用于根据所述补偿值生成位置复位指令，并根据所述位置复位指令调整所述舞蹈机器人的位置；所述位置复位指令用于对所述舞蹈机器人执行舞蹈旋转后产生的角度误差进行补偿；
所述第一获取模块还用于：
获取第一初始数据；所述第一初始数据是指由所述传感器获取到的位置信息；根据预设规则对所述第一初始数据进行筛选，得到第一动态数据信息和第一静态数据信息；所述预设规则包括将所述传感器在10°到350°获取的激光数据设定为所述第一静态数据信息，以及，在0°-10°和350°-360°获取的激光数据设定为所述第一动态数据信息；滤除所述第一动态数据信息，将所述第一静态数据信息作为所述第一位置信息。
9.一种舞蹈机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7任一项所述的方法。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于：所述计算机程序被处理器执行时实现如权利要求1至7任一项所述的方法。
CN201910957976.0A 2019-10-10 2019-10-10 定位误差消除方法、装置、机器人及存储介质 Active CN112643664B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201910957976.0A CN112643664B (zh) 2019-10-10 2019-10-10 定位误差消除方法、装置、机器人及存储介质
US16/726,196 US11260528B2 (en) 2019-10-10 2019-12-23 Localization correction method for robot, computer readable storage medium, and robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910957976.0A CN112643664B (zh) 2019-10-10 2019-10-10 定位误差消除方法、装置、机器人及存储介质

Publications (2)

Publication Number Publication Date
CN112643664A CN112643664A (zh) 2021-04-13
CN112643664B true CN112643664B (zh) 2022-09-23

Family

ID=75342482

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910957976.0A Active CN112643664B (zh) 2019-10-10 2019-10-10 定位误差消除方法、装置、机器人及存储介质

Country Status (2)

Country Link
US (1) US11260528B2 (zh)
CN (1) CN112643664B (zh)

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109011651B (zh) * 2018-07-25 2020-06-09 上海葡萄纬度科技有限公司 一种交互式玩具
US11592570B2 (en) * 2020-02-25 2023-02-28 Baidu Usa Llc Automated labeling system for autonomous driving vehicle lidar data
CN113176781B (zh) * 2021-05-17 2023-06-23 北京京东乾石科技有限公司 基于物流机器人的补偿方法和装置
CN113334387B (zh) * 2021-06-30 2024-06-04 北京博清科技有限公司 焊接机器人的控制方法、装置、存储介质及焊接机器人
CN113551692B (zh) * 2021-07-19 2024-04-02 杭州迅蚁网络科技有限公司 无人机磁力计和相机安装角度校准方法、装置
CN113650018B (zh) * 2021-09-01 2022-12-16 珠海格力智能装备有限公司 轴式机器人轨迹规划方法、装置与计算机可读存储介质
CN113984073B (zh) * 2021-09-29 2024-05-28 杭州电子科技大学 一种基于方位的移动机器人协同校正算法
CN114081634B (zh) * 2022-01-21 2022-05-03 极限人工智能有限公司 角度补偿方法、装置、电子设备及存储介质
CN114469354B (zh) * 2022-04-13 2022-06-24 深圳市爱博医疗机器人有限公司 一种介入手术机器人从端初始化方法和可读存储介质
CN114537705B (zh) * 2022-04-25 2022-08-12 成都飞机工业（集团）有限责任公司 飞机扩口导管带误差装配方法、装置、存储介质及设备
CN115256465A (zh) * 2022-07-29 2022-11-01 江苏航鼎智能装备有限公司 一种消除工业机器人齿轮传动中反向间隙的方法
CN115460544A (zh) * 2022-09-21 2022-12-09 北京锐星远畅科技有限公司 一种寻找蓝牙设备的方法和装置

Family Cites Families (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7756615B2 (en) * 2005-07-26 2010-07-13 Macdonald, Dettwiler & Associates Inc. Traffic management system for a passageway environment
CN102519481B (zh) * 2011-12-29 2013-09-04 中国科学院自动化研究所 一种双目视觉里程计实现方法
US9043069B1 (en) * 2012-11-07 2015-05-26 Google Inc. Methods and systems for scan matching approaches for vehicle heading estimation
SG2013042890A (en) * 2013-06-03 2015-01-29 Ctrlworks Pte Ltd Method and apparatus for offboard navigation of a robotic device
CN104715469A (zh) * 2013-12-13 2015-06-17 联想(北京)有限公司 一种数据处理方法及电子设备
CN104657981B (zh) * 2015-01-07 2017-05-24 大连理工大学 一种移动机器人运动中三维激光测距数据动态补偿方法
CN106123890A (zh) * 2016-06-14 2016-11-16 中国科学院合肥物质科学研究院 一种多传感器数据融合的机器人定位方法
CN106296693B (zh) * 2016-08-12 2019-01-08 浙江工业大学 基于3d点云fpfh特征实时三维空间定位方法
CN109211236B (zh) * 2017-06-30 2022-03-04 沈阳新松机器人自动化股份有限公司 导航定位方法、装置及机器人
US10983199B2 (en) * 2017-08-11 2021-04-20 Zoox, Inc. Vehicle sensor calibration and localization
CN108460779B (zh) * 2018-02-12 2021-09-24 浙江大学 一种动态环境下的移动机器人图像视觉定位方法
JP2020057307A (ja) * 2018-10-04 2020-04-09 日本電産株式会社 自己位置推定のための地図データを加工する装置および方法、ならびに移動体およびその制御システム
TWI683197B (zh) * 2019-03-19 2020-01-21 東元電機股份有限公司 移動平台圖資校正系統
DE112020004139T5 (de) * 2019-08-31 2022-05-12 Nvidia Corporation Erstellung von karten und lokalisierung für anwendungen im bereich des autonomen fahrens

Also Published As

Publication number Publication date
CN112643664A (zh) 2021-04-13
US11260528B2 (en) 2022-03-01
US20210107147A1 (en) 2021-04-15

Similar Documents

Publication Publication Date Title
CN112643664B (zh) 定位误差消除方法、装置、机器人及存储介质
US11042723B2 (en) Systems and methods for depth map sampling
RU2713611C2 (ru) Способ моделирования трехмерного пространства
CN113748357B (zh) 激光雷达的姿态校正方法、装置和***
US11072072B2 (en) Robot recharge docking method and robot with the same
WO2021016854A1 (zh) 一种标定方法、设备、可移动平台及存储介质
WO2014168848A1 (en) Multi-sensor camera recalibration
JP2022515225A (ja) センサキャリブレーション方法及び装置、記憶媒体、キャリブレーションシステム並びにプログラム製品
CN110873883A (zh) 融合激光雷达和imu的定位方法、介质、终端和装置
US11112780B2 (en) Collaborative determination of a load footprint of a robotic vehicle
CN112330752A (zh) 多相机联合标定方法、装置、终端设备及可读存储介质
CN111699410A (zh) 点云的处理方法、设备和计算机可读存储介质
CN113111513B (zh) 传感器配置方案确定方法、装置、计算机设备及存储介质
JP2023505987A (ja) 人間関節を用いた無人航空機上のカメラの校正
CN114820749A (zh) 无人车井下定位方法、***、设备及介质
CN110930444B (zh) 一种基于双边优化的点云匹配方法、介质、终端和装置
CN112967347B (zh) 位姿标定方法、装置、机器人及计算机可读存储介质
JPH07146121A (ja) 視覚に基く三次元位置および姿勢の認識方法ならびに視覚に基く三次元位置および姿勢の認識装置
US11915449B2 (en) Method and apparatus for estimating user pose using three-dimensional virtual space model
CN114359338A (zh) 位姿估计方法、装置、终端设备及计算机可读存储介质
CN113776517A (zh) 地图生成方法、装置、***、存储介质及电子设备
CN116659487B (zh) 位姿调整方法、位姿调整装置、电子设备及可读存储介质
CN115019167B (zh) 基于移动终端的融合定位方法、***、设备及存储介质
JP2024031992A (ja) 移動体、及びプログラム
CN114693802A (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