CN111380514A - 机器人位姿估计方法、装置、终端及计算机存储介质 - Google Patents

机器人位姿估计方法、装置、终端及计算机存储介质 Download PDF

Info

Publication number
CN111380514A
CN111380514A CN201811636352.0A CN201811636352A CN111380514A CN 111380514 A CN111380514 A CN 111380514A CN 201811636352 A CN201811636352 A CN 201811636352A CN 111380514 A CN111380514 A CN 111380514A
Authority
CN
China
Prior art keywords
pose data
freedom
degree
pose
data
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201811636352.0A
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.)
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 CN201811636352.0A priority Critical patent/CN111380514A/zh
Priority to US16/556,233 priority patent/US11279045B2/en
Publication of CN111380514A publication Critical patent/CN111380514A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B25J13/088—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices with position, velocity or acceleration sensors
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B25J13/088—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices with position, velocity or acceleration sensors
    • B25J13/089—Determining the position of the robot with reference to its environment
    • 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
    • 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
    • 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
    • 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
    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G01S17/42—Simultaneous measurement of distance and other co-ordinates
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/497—Means for monitoring or calibrating

Abstract

本发明适用于计算机应用技术领域，提供了一种机器人位姿估计方法、装置、终端及计算机存储介质，方法包括：获取惯性测量单元IMU测量得到的初始六自由度位姿数据；利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据；获取设置于稳定平台上的2D激光雷达传感器测量得到的三自由度位姿数据；利用所述三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据；提高了机器人位姿数据的精度，实现了对机器人进行准确的位姿估计。

Description

机器人位姿估计方法、装置、终端及计算机存储介质
技术领域
本发明属于计算机技术领域，尤其涉及一种机器人位姿估计方法、装置、终端及计算机存储介质。
背景技术
机器人的位姿估计能力对于路径规划与运动控制是机器人实现自主导航的关键，对于提高机器人的自动化水平具有重要的意义。
目前，最常见的机器人位姿估计方法为使用惯性测量单元进行估计。由于惯性测量单元中的传感器测量得到的是机器人的角速度与位移加速度，需要经过积分才能得到机器人的位置和速度值，由于传感器存在噪声偏移量，因此，积分得到的位置和速度值会出现积分漂移，无法实现对机器人进行准确的位姿估计。
发明内容
有鉴于此，本发明实施例提供了一种机器人位姿估计方法、装置、终端及计算机存储介质，能够惯性测量单元无法实现对机器人进行准确的位姿估计的技术问题。
本发明实施例的第一方面提供了一种机器人位姿估计方法，包括：
获取惯性测量单元IMU测量得到的初始六自由度位姿数据；
利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据；
获取设置于稳定平台上的2D激光雷达传感器测量得到的三自由度位姿数据；
利用所述三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据。
本发明实施例的第二方面提供了一种机器人位姿估计装置，包括：
第一获取单元，用于获取惯性测量单元IMU测量得到的初始六自由度位姿数据；
第一修正单元，用于利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据；
第二获取单元，用于获取设置于稳定平台上的2D激光雷达传感器测量得到的三自由度位姿数据；
第二修正单元，用于利用所述三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据。
本发明实施例的第三方面提供了一种终端，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述所述方法的步骤。
本发明实施例的第四方面提供了一种计算机存储介质，所述计算机存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述所述方法的步骤。
本发明实施例中，通过利用辅助传感器测量得到的位姿数据对惯性测量单元测量得到的初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据，接着，再利用设置于稳定平台上的2D激光雷达传感器测量得到的三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据；使得最终得到的六自由度位姿数据为经过两次修正得到的位姿数据，提高了机器人位姿数据的精度，实现了对机器人进行准确的位姿估计。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种机器人位姿估计方法的实现流程示意图；
图2是本发明实施例提供的一种机器人位姿估计方法步骤102的具体实现流程示意图；
图3是本发明实施例提供的对初始六自由度位姿数据进行第一次修正的示意图；
图4是本发明实施例提供的一种机器人位姿估计方法步骤103的具体实现流程示意图；
图5是本发明实施例提供的一种机器人位姿估计装置的示意图；
图6是本发明实施例提供的一种终端的示意图。
具体实施方式
以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本发明实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本发明。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本发明的描述。
为了说明本发明所述的技术方案，下面通过具体实施例来进行说明。
应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和“包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
还应当理解，在此本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“所述”意在包括复数形式。
具体实现中，本发明实施例中描述的终端可以为机器人、服务器、电脑、手机等配置有机器人位姿估计装置的终端设备，为了描述的方便，本发明以机器人为例进行说明。
请参见图1，图1是本发明实施例提供的一种机器人位姿估计方法的实现流程示意图。本实施例中的机器人位姿估计方法的执行主体为机器人位姿估计装置，该装置可以由软件和/或硬件实现，集成于终端中，适用于需要进行机器人位姿估计的情形。如图1所示的机器人位姿估计方法，可以包括：步骤101至步骤104。
步骤101，获取惯性测量单元IMU测量得到的初始六自由度位姿数据。
物体在空间具有六个自由度，即沿x、y、z三个直角坐标轴方向的移动自由度和绕这三个坐标轴的转动自由度。因此，要完全确定物体的位置，就必须清楚这六个自由度。
惯性测量单元(Inertial measurement unit，IMU)是测量物体三轴角速度以及加速度的装置。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计检测物体在载体坐标系统独立三轴的加速度，而陀螺仪检测载体相对于导航坐标系的角速度，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。
因此，在进行机器人的位姿估计时，可以利用所述惯性测量单元IMU测量得到机器人的初始六自由度位姿数据。
步骤102，利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据。
本发明实施例中，由于IMU的传感器存在噪声偏移量，因此，积分得到的机器人的位置和速度值会出现积分漂移，无法实现对机器人进行准确的位姿估计，因此，需要利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据。
具体的，所述辅助传感器可以包括全球定位系统(Global Positioning System，GPS)、气压计、磁力计、视觉测量仪等可以实现位姿数据测量的传感器，此处为举例说明，不表示为对本发明保护范围的限制，在本发明的其他实施方式中，所述辅助传感器还可以包括其他传感器。
可选的，如图2所示，上述步骤102中，利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据，可以包括：步骤201至步骤202。
步骤201，对所述初始六自由度位姿数据进行解耦，得到水平方向上的位姿数据、高度方向上的位姿数据，以及偏航角。
在对所述初始六自由度位姿数据进行解耦之后，可以得到水平方向上的位姿数据、高度方向上的位姿数据，以及偏航角。具体的，如图3所示，所述水平方向上的位姿数据包括：X轴方向的速度和Y轴方向的速度，绕X轴运动的俯仰角和绕Y轴运动的滚转角；所述高度方向上的位姿数据包括Z轴方向的速度，所述偏航角为绕Z轴运动的角度。
其中，具体的解耦方法可以采用通用的解耦技术实现。例如，可以利用欧几里得范数实现对所述初始六自由度位姿数据的解耦。
可选的，所述对所述初始六自由度位姿数据进行解耦之前，可以包括：将所述IMU在固连系下测量得到的角速度ω＝(ωx,ωy,ωz)T和加速度a＝(ax,ay,az)T转换为导航系下的欧拉角
Figure BDA0001930140300000051
位置向量p＝(px,py,pz)T和速度向量v＝(vx,vy,vz)T，接着对其进行积分得到所述初始六自由度位姿数据。
例如，通过机器人的刚体运动学方程式
Figure BDA0001930140300000052
将固连系下测量得到的角速度ω＝(ωx,ωy,ωz)T和加速度a＝(ax,ay,az)T转换为导航系下的欧拉角
Figure BDA0001930140300000061
位置向量p＝(px,py,pz)T和速度向量v＝(vx,vy,vz)T，其中，RΩ为将任意向量由固连系转至导航系的方向余弦矩阵，EΩ为将固连系下角速度转至欧拉角导数的转换矩阵，g为重力常值向量。
步骤202，利用辅助传感器测量得到的数据分别对所述水平方向上的位姿数据、高度方向上的位姿数据以及所述偏航角进行修正，得到修正后的六自由度位姿数据。
在对所述初始六自由度位姿数据进行解耦，得到水平方向上的位姿数据、高度方向上的位姿数据，以及偏航角之后，即可利用辅助传感器测量得到的数据分别对所述水平方向上的位姿数据、高度方向上的位姿数据以及所述偏航角进行修正，得到修正后的六自由度位姿数据。
例如，如图3所示，利用所述GPS和视觉测量仪测量得到的水平方向上的位姿数据对所述初始六自由度位姿数据解耦得到的水平方向上的位姿数据进行第一次修正；利用GPS测量得到的高度方向的速度值和气压计测量得到的高度值对所述初始六自由度位姿数据解耦得到的高度方向上的位姿数据进行第一次修正；利用磁力计测量得到的偏航角对所述初始六自由度位姿数据解耦得到的偏航角进行第一次修正，最终得到修正后的六自由度位姿数据。
步骤103，获取设置于稳定平台上的2D激光雷达传感器测量得到的三自由度位姿数据。
在得到所述修正后的六自由度位姿数据之后，即可通过获取设置于稳定平台上的2D激光雷达传感器测量得到的三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，以得到精度较高的目标六自由度位姿数据。
其中，所述稳定平台是指不随机器人抖动而抖动的平台。
由于3D激光雷达传感器价格比较贵，且扫描周期较长，因此，本发明采用2D激光雷达传感器测量得到的三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，以节省机器人位姿估计的成本和效率。
在使用2D激光雷达传感器作为扫描平面的位置和角度的传感器时，是通过向某个方向发射激光扫描线，并接收经障碍物反射回来的激光，得到激光从发射到接收的时间差，并根据该时间差计算出该方向上距离最近的障碍物的距离数据，从而实现机器人自身的定位，即，得到机器人在平面上的位置(X轴和Y轴上的坐标)和航向角。对于一些不平整的路面，机器人可能会出现较大的幅度的俯仰和滚转，因此，为了保证2D激光雷达传感器能够尽可能地进行Z平面上的扫描，需要将其与机器人在不平整的地面上行走时发生的俯仰和滚转进行隔离，以保证稳定平台保持稳定。
其具体的隔离方法可以包括：根据所述修正后的六自由度位姿数据计算所述稳定平台的控制量，根据所述控制量控制执行机构驱动所述稳定平台反向运动，以使所述稳定平台保持稳定。
例如，通过获取修正后的六自由度位姿数据中绕X轴运动的俯仰角和绕Y轴运动的滚转角，以及角速度计算所述稳定平台的控制量，从而根据所述控制量控制执行机构驱动所述稳定平台反向运动，以使所述稳定平台保持稳定，进而得到2D激光雷达传感器测量得到的较准确的三自由度位姿数据。
可选的，如图4所示，在本发明的一些实施方式中，所述获取设置于稳定平台上的2D激光雷达传感器测量得到的三自由度位姿数据，可以包括：步骤401至步骤402。
步骤401，将当前时刻以及上一时刻测量得到的两帧数据进行对齐，并估计两帧数据之间三自由度位姿数据的变化量。
步骤402，利用所述变化量对上一时刻测量得到的三自由度位姿数据进行修正，得到当前时刻的2D激光雷达传感器测量得到的三自由度位姿数据。
本发明实施例中，所述估计两帧数据之间三自由度位姿数据的变化量包括：利用高斯牛顿迭代法估计所述两帧数据之间三自由度位姿数据的变化量，其具体过程包括：
获取当前时刻的刚体位姿ξ＝[px Tpy Tψ]T，求解：
Figure BDA0001930140300000071
其中，函数Si(ξ)为扫描终点si＝(si,x,si,y)T在世界坐标系下的值，其为三自由度位姿ξ的函数，具体形式为：
Figure BDA0001930140300000081
函数M(Si(ξ))返回占据栅格地图在坐标Si(ξ)处的取值(0或1)，在给定初始估计位姿ξ后，需要满足求取方程的位姿修正量Δξ：
Figure BDA0001930140300000082
通过一阶泰勒展开可以得到：
Figure BDA0001930140300000083
通过高斯牛顿方程可以得到上式的求解为：
Figure BDA0001930140300000084
其中H阵为：
Figure BDA0001930140300000085
按照上式则可求取连续两帧扫描数据间位姿的变化量。
步骤104，利用所述三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据。
可选的，所述利用所述三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据，包括：
利用修正方程：
Figure BDA0001930140300000086
对所述修正后的六自由度位姿
数据做第二次修正；
其中，
Figure BDA0001930140300000087
为所述修正后的六自由度位姿数据，P为所述修正后的六自由度位姿数据对应的方差阵，ξ*为所述三自由度位姿数据，R为所述三自由度位姿数据对应的方差阵，C为观测投影矩阵，用于将所述修正后的六自由度位姿数据
Figure BDA0001930140300000091
投影到所述2D激光雷达传感器的扫描平面中，参数ω为修正权重。
在实际应用中，方差阵P、方差阵R和观测投影矩阵C可以根据传感器的参数计算得到。
可选的，在本发明的一些实施方式中，所述修正方程还可以表达为：
Figure BDA0001930140300000092
其中，K为滤波增益。
本发明实施例中，通过利用辅助传感器测量得到的位姿数据对惯性测量单元测量得到的初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据，接着，再利用设置于稳定平台上的2D激光雷达传感器测量得到的三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据；使得最终得到的六自由度位姿数据为经过两次修正得到的位姿数据，提高了机器人位姿数据的精度，实现了对机器人进行准确的位姿估计。
本发明实施例还提供一种机器人位姿估计装置，该装置包括用于执行前述的机器人位姿估计方法中的各步骤的模块，该装置可集成于上述终端。该装置中未详细描述之处请详见前述方法的描述。
参见图5，图5是本发明实施例提供的一种机器人位姿估计装置的示意框图。本实施例的机器人位姿估计装置5包括：第一获取单元51、第一修正单元52、第二获取单元53和第二修正单元54。
第一获取单元51，用于获取惯性测量单元IMU测量得到的初始六自由度位姿数据；
第一修正单元52，用于利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据；
第二获取单元53，用于获取设置于稳定平台上的2D激光雷达传感器测量得到的三自由度位姿数据；
第二修正单元54，用于利用所述三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据。
可选的，所述第一修正单元，还具体用于：
对所述初始六自由度位姿数据进行解耦，得到水平方向上的位姿数据、高度方向上的位姿数据，以及偏航角；
利用辅助传感器测量得到的数据分别对所述水平方向上的位姿数据、高度方向上的位姿数据以及所述偏航角进行修正，得到修正后的六自由度位姿数据。
可选的，所述第二获取单元，还用于
将当前时刻以及上一时刻测量得到的两帧数据进行对齐，并估计两帧数据之间三自由度位姿数据的变化量；
利用所述变化量对上一时刻测量得到的三自由度位姿数据进行修正，得到当前时刻的2D激光雷达传感器测量得到的三自由度位姿数据。
可选的，所述第二修正单元，还具体用于：
利用修正方程：
Figure BDA0001930140300000101
对所述修正后的六自由度位姿
数据做第二次修正；
其中，
Figure BDA0001930140300000102
为所述修正后的六自由度位姿数据，P为所述修正后的六自由度位姿数据对应的方差阵，ξ*为所述三自由度位姿数据，R为所述三自由度位姿数据对应的方差阵，C为观测投影矩阵，用于将所述修正后的六自由度位姿数据
Figure BDA0001930140300000103
投影到所述2D激光雷达传感器的扫描平面中，参数ω为修正权重。
可选的，所述机器人位姿估计装置还包括平台控制单元，用于根据所述修正后的六自由度位姿数据计算所述稳定平台的控制量，根据所述控制量控制执行机构驱动所述稳定平台反向运动，以使所述稳定平台保持稳定。
图6是本发明实施例提供的终端的示意图。如图6所示，该实施例的终端6包括：处理器60、存储器61以及存储在存储器61中并可在处理器60上运行的计算机程序62，例如，机器人位姿估计的程序。处理器60执行计算机程序62时实现上述机器人位姿估计方法实施例中的步骤，例如图1所示的步骤101至步骤104。或者，处理器60执行计算机程序62时实现上述各装置实施例中各模块/单元的功能，例如图5所示单元51至54的功能。
示例性的，计算机程序62可以被分割成一个或多个模块/单元，一个或者多个模块/单元被存储在存储器61中，并由处理器60执行，以完成本发明。一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述计算机程序62在终端6中的执行过程。例如，计算机程序62可以被分割成第一获取单元、第一修正单元、第二获取单元以及第二修正单元(虚拟装置中的单元)，各模块具体功能如下：
第一获取单元，用于获取惯性测量单元IMU测量得到的初始六自由度位姿数据；
第一修正单元，用于利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据；
第二获取单元，用于获取设置于稳定平台上的2D激光雷达传感器测量得到的三自由度位姿数据；
第二修正单元，用于利用所述三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据。
终端6可包括，但不仅限于，处理器60、存储器61。本领域技术人员可以理解，图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 MediaCard，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，存储器61还可以既包括终端6的内部存储单元也包括外部存储设备。存储器61用于存储计算机程序以及终端设备所需的其他程序和数据。存储器61还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
在本发明所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，计算机程序包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读介质可以包括：能够携带计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、电载波信号、电信信号以及软件分发介质等。需要说明的是，计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。

Claims (10)

1.一种机器人位姿估计方法，其特征在于，包括：
获取惯性测量单元IMU测量得到的初始六自由度位姿数据；
利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据；
获取设置于稳定平台上的2D激光雷达传感器测量得到的三自由度位姿数据；
利用所述三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据。
2.如权利要求1所述的机器人位姿估计方法，其特征在于，所述利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据，包括：
对所述初始六自由度位姿数据进行解耦，得到水平方向上的位姿数据、高度方向上的位姿数据，以及偏航角；
利用辅助传感器测量得到的数据分别对所述水平方向上的位姿数据、高度方向上的位姿数据以及所述偏航角进行修正，得到修正后的六自由度位姿数据。
3.如权利要求1所述的机器人位姿估计方法，其特征在于，所述获取设置于稳定平台上的2D激光雷达传感器测量得到的三自由度位姿数据，包括：
将当前时刻以及上一时刻测量得到的两帧数据进行对齐，并估计两帧数据之间三自由度位姿数据的变化量；
利用所述变化量对上一时刻测量得到的三自由度位姿数据进行修正，得到当前时刻的2D激光雷达传感器测量得到的三自由度位姿数据。
4.如权利要求1-3任一项所述的机器人位姿估计方法，其特征在于，所述利用所述三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据，包括：
利用修正方程：
Figure FDA0001930140290000021
对所述修正后的六自由度位姿数据做第二次修正；
其中，
Figure FDA0001930140290000022
为所述修正后的六自由度位姿数据，P为所述修正后的六自由度位姿数据对应的方差阵，ξ*为所述三自由度位姿数据，R为所述三自由度位姿数据对应的方差阵，C为观测投影矩阵，用于将所述修正后的六自由度位姿数据
Figure FDA0001930140290000023
投影到所述2D激光雷达传感器的扫描平面中，参数ω为修正权重。
5.如权利要求1所述的机器人位姿估计方法，其特征在于，所述机器人位姿估计方法，还包括：
根据所述修正后的六自由度位姿数据计算所述稳定平台的控制量，根据所述控制量控制执行机构驱动所述稳定平台反向运动，以使所述稳定平台保持稳定。
6.一种机器人位姿估计装置，其特征在于，包括：
第一获取单元，用于获取惯性测量单元IMU测量得到的初始六自由度位姿数据；
第一修正单元，用于利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据；
第二获取单元，用于获取设置于稳定平台上的2D激光雷达传感器测量得到的三自由度位姿数据；
第二修正单元，用于利用所述三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据。
7.如权利要求6所述的机器人位姿估计装置，其特征在于，所述第一修正单元，具体用于：
对所述初始六自由度位姿数据进行解耦，得到水平方向上的位姿数据、高度方向上的位姿数据，以及偏航角；
利用辅助传感器测量得到的数据分别对所述水平方向上的位姿数据、高度方向上的位姿数据以及所述偏航角进行修正，得到修正后的六自由度位姿数据。
8.如权利要求6所述的机器人位姿估计装置，其特征在于，所述第二获取单元，还用于
将当前时刻以及上一时刻测量得到的两帧数据进行对齐，并估计两帧数据之间三自由度位姿数据的变化量；
利用所述变化量对上一时刻测量得到的三自由度位姿数据进行修正，得到当前时刻的2D激光雷达传感器测量得到的三自由度位姿数据。
9.一种终端，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至5任一项所述方法的步骤。
10.一种计算机存储介质，所述计算机存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至5任一项所述方法的步骤。
CN201811636352.0A 2018-12-29 2018-12-29 机器人位姿估计方法、装置、终端及计算机存储介质 Pending CN111380514A (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201811636352.0A CN111380514A (zh) 2018-12-29 2018-12-29 机器人位姿估计方法、装置、终端及计算机存储介质
US16/556,233 US11279045B2 (en) 2018-12-29 2019-08-30 Robot pose estimation method and apparatus and robot using the same

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811636352.0A CN111380514A (zh) 2018-12-29 2018-12-29 机器人位姿估计方法、装置、终端及计算机存储介质

Publications (1)

Publication Number Publication Date
CN111380514A true CN111380514A (zh) 2020-07-07

Family

ID=71121644

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811636352.0A Pending CN111380514A (zh) 2018-12-29 2018-12-29 机器人位姿估计方法、装置、终端及计算机存储介质

Country Status (2)

Country Link
US (1) US11279045B2 (zh)
CN (1) CN111380514A (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113253748A (zh) * 2021-06-08 2021-08-13 深圳市优必选科技股份有限公司 并联构型仿人机器人的状态估计方法、装置、设备及介质
CN113375658A (zh) * 2021-06-15 2021-09-10 电子科技大学中山学院 移动机器人故障下同时fdd和slam的方法及系统
CN113405565A (zh) * 2021-05-27 2021-09-17 深圳市优必选科技股份有限公司 一种物体状态的检测方法、装置及电子设备
CN114750153A (zh) * 2022-04-13 2022-07-15 上海电气集团股份有限公司 机器人机械臂的运动控制系统、协作机器人及存储介质
CN117400334A (zh) * 2023-11-13 2024-01-16 山东大学 一种玻璃纤维复合材料汽车板簧切割机器人姿态调整方法

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111929699B (zh) * 2020-07-21 2023-05-09 北京建筑大学 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统
CN112506190A (zh) * 2020-11-19 2021-03-16 深圳市优必选科技股份有限公司 一种机器人定位方法、机器人定位装置及机器人
CN114589690A (zh) * 2020-12-07 2022-06-07 山东新松工业软件研究院股份有限公司 机器人关节位姿监控方法及装置
CN114851184B (zh) * 2021-01-20 2023-05-09 广东技术师范大学 一种面向工业机器人的强化学习奖励值计算方法
CN113305830B (zh) * 2021-04-28 2022-08-16 吉林大学 一种基于人体姿态控制的仿人机器人动作系统及控制方法
CN113580137B (zh) * 2021-08-12 2023-09-22 天津大学 基于视觉测量的移动机器人基座-工件相对位姿确定方法
CN115683170B (zh) * 2023-01-04 2023-03-14 成都西物信安智能系统有限公司 基于雷达点云数据融合误差的校准方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103868521A (zh) * 2014-02-20 2014-06-18 天津大学 基于激光雷达的四旋翼无人机自主定位及控制方法
CN105628026A (zh) * 2016-03-04 2016-06-01 深圳大学 一种移动物体的定位定姿方法和系统
CN106123890A (zh) * 2016-06-14 2016-11-16 中国科学院合肥物质科学研究院 一种多传感器数据融合的机器人定位方法
CN107246868A (zh) * 2017-07-26 2017-10-13 上海舵敏智能科技有限公司 一种协同导航定位系统及导航定位方法
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法
CN108732603A (zh) * 2017-04-17 2018-11-02 百度在线网络技术（北京）有限公司 用于定位车辆的方法和装置
CN109099912A (zh) * 2017-08-11 2018-12-28 黄润芳 室外精确定位导航方法、装置、电子设备及存储介质

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10481265B2 (en) * 2011-12-21 2019-11-19 Robotic paradigm Systems LLC Apparatus, systems and methods for point cloud generation and constantly tracking position
US9476730B2 (en) * 2014-03-18 2016-10-25 Sri International Real-time system for multi-modal 3D geospatial mapping, object recognition, scene annotation and analytics
US11739542B2 (en) * 2016-09-30 2023-08-29 Eth Singapore Sec Ltd System for placing objects on a surface and method thereof
US10371530B2 (en) * 2017-01-04 2019-08-06 Qualcomm Incorporated Systems and methods for using a global positioning system velocity in visual-inertial odometry

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103868521A (zh) * 2014-02-20 2014-06-18 天津大学 基于激光雷达的四旋翼无人机自主定位及控制方法
CN105628026A (zh) * 2016-03-04 2016-06-01 深圳大学 一种移动物体的定位定姿方法和系统
CN106123890A (zh) * 2016-06-14 2016-11-16 中国科学院合肥物质科学研究院 一种多传感器数据融合的机器人定位方法
CN108732603A (zh) * 2017-04-17 2018-11-02 百度在线网络技术（北京）有限公司 用于定位车辆的方法和装置
CN107246868A (zh) * 2017-07-26 2017-10-13 上海舵敏智能科技有限公司 一种协同导航定位系统及导航定位方法
CN109099912A (zh) * 2017-08-11 2018-12-28 黄润芳 室外精确定位导航方法、装置、电子设备及存储介质
CN107991683A (zh) * 2017-11-08 2018-05-04 华中科技大学 一种基于激光雷达的机器人自主定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
康坦: "双减振块磁流变减振镗杆的结构设计与仿真分析", 《中国优秀博硕士学位论文全文数据库（硕士）•工程科技Ⅰ辑》 *
蔡东松: "双自由度减振刀杆的研究", 《中国优秀博硕士学位论文全文数据库（硕士）•工程科技Ⅰ辑》 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113405565A (zh) * 2021-05-27 2021-09-17 深圳市优必选科技股份有限公司 一种物体状态的检测方法、装置及电子设备
CN113253748A (zh) * 2021-06-08 2021-08-13 深圳市优必选科技股份有限公司 并联构型仿人机器人的状态估计方法、装置、设备及介质
CN113253748B (zh) * 2021-06-08 2022-02-22 深圳市优必选科技股份有限公司 并联构型仿人机器人的状态估计方法、装置、设备及介质
CN113375658A (zh) * 2021-06-15 2021-09-10 电子科技大学中山学院 移动机器人故障下同时fdd和slam的方法及系统
CN113375658B (zh) * 2021-06-15 2023-05-09 电子科技大学中山学院 移动机器人故障下同时fdd和slam的方法及系统
CN114750153A (zh) * 2022-04-13 2022-07-15 上海电气集团股份有限公司 机器人机械臂的运动控制系统、协作机器人及存储介质
CN114750153B (zh) * 2022-04-13 2024-03-19 上海电气集团股份有限公司 机器人机械臂的运动控制系统、协作机器人及存储介质
CN117400334A (zh) * 2023-11-13 2024-01-16 山东大学 一种玻璃纤维复合材料汽车板簧切割机器人姿态调整方法

Also Published As

Publication number Publication date
US20200206945A1 (en) 2020-07-02
US11279045B2 (en) 2022-03-22

Similar Documents

Publication Publication Date Title
CN111380514A (zh) 机器人位姿估计方法、装置、终端及计算机存储介质
CN111811506B (zh) 视觉/惯性里程计组合导航方法、电子设备及存储介质
CN111156998B (zh) 一种基于rgb-d相机与imu信息融合的移动机器人定位方法
US20210183100A1 (en) Data processing method and apparatus
CN110325822B (zh) 云台位姿修正方法和装置
CN113551665B (zh) 一种用于运动载体的高动态运动状态感知系统及感知方法
CN112113582A (zh) 时间同步处理方法、电子设备及存储介质
WO2019191288A1 (en) Direct sparse visual-inertial odometry using dynamic marginalization
Kelly et al. Fast relative pose calibration for visual and inertial sensors
CN104848861A (zh) 一种基于图像消失点识别技术的移动设备姿态测量方法
CN109141411B (zh) 定位方法、定位装置、移动机器人及存储介质
CN111238469A (zh) 一种基于惯性/数据链的无人机编队相对导航方法
Ding et al. Adding optical flow into the GPS/INS integration for UAV navigation
CN110873563A (zh) 一种云台姿态估计方法及装置
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN111025330B (zh) 一种基于深度图的目标倾斜角检测方法及设备
CN110991085B (zh) 一种机器人图像仿真数据的构建方法、介质、终端和装置
CN112154480B (zh) 可移动平台的定位方法、装置、可移动平台及存储介质
CN116952229A (zh) 无人机定位方法、装置、系统和存储介质
CN114543786B (zh) 一种基于视觉惯性里程计的爬壁机器人定位方法
CN115930948A (zh) 一种果园机器人融合定位方法
CN112556681A (zh) 一种基于视觉的果园机械导航定位方法
MacArthur et al. Unmanned ground vehicle state estimation using an unmanned air vehicle
Tao et al. A fast and accurate camera-IMU calibration method for localization system
Yang et al. Inertial-aided vision-based localization and mapping in a riverine environment with reflection measurements

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
RJ01 Rejection of invention patent application after publication

Application publication date: 20200707

RJ01 Rejection of invention patent application after publication