CN112506190A - 一种机器人定位方法、机器人定位装置及机器人 - Google Patents
一种机器人定位方法、机器人定位装置及机器人 Download PDFInfo
- Publication number
- CN112506190A CN112506190A CN202011308068.8A CN202011308068A CN112506190A CN 112506190 A CN112506190 A CN 112506190A CN 202011308068 A CN202011308068 A CN 202011308068A CN 112506190 A CN112506190 A CN 112506190A
- Authority
- CN
- China
- Prior art keywords
- robot
- coordinate transformation
- expression
- transformation matrix
- odometer
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 230000009466 transformation Effects 0.000 claims abstract description 113
- 239000011159 matrix material Substances 0.000 claims abstract description 98
- 230000004927 fusion Effects 0.000 claims abstract description 39
- 230000008569 process Effects 0.000 claims abstract description 28
- 238000006073 displacement reaction Methods 0.000 claims description 27
- 238000004364 calculation method Methods 0.000 claims description 17
- 238000004590 computer program Methods 0.000 claims description 16
- 238000010586 diagram Methods 0.000 description 6
- 230000009897 systematic effect Effects 0.000 description 6
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 238000012545 processing Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Abstract
本申请公开了一种机器人定位方法、机器人定位装置、机器人及计算机可读存储介质。其中，该机器人定位方法包括：确定机器人当前时刻的第一表达式，第一表达式用于表示基于机器人的里程计的输出及机器人的惯性传感器的输出进行融合所得的融合后的位姿数据；确定机器人当前时刻的第二表达式，第二表达式用于表示基于里程计的输出所得的融合前的位姿数据；根据第一表达式及第二表达式，计算得到第一坐标变换矩阵，第一坐标变换矩阵用于表示当前时刻下，融合后的位姿数据相对于融合前的位姿数据的坐标变换；在机器人的运动过程中，基于第一坐标变换矩阵及里程计的输出计算得到定位结果。通过本申请方案，可一定程度上提升机器人定位的精度。
Description
技术领域
本申请属于机器人技术领域，尤其涉及一种机器人定位方法、机器人定位装置、机器人及计算机可读存储介质。
背景技术
对于部分机器人来说，在机器人移动的过程中，会基于机器人所内置的里程计的输出对机器人的位置进行迭代计算，以实现对机器人的实时定位。然而，由于机器人存在着系统误差(例如机器人结构不对称或轮子大小不同等)和非系统误差(例如机器人移动过程中打滑等)，使得里程计的精度往往不够理想，这导致对机器人的定位也存在较大的误差。
发明内容
本申请提供了一种机器人定位方法、机器人定位装置、机器人及计算机可读存储介质，可以一定程度上提升机器人定位的精度。
第一方面，本申请提供了一种机器人定位方法，包括：
确定机器人的第一表达式，上述第一表达式用于表示当前时刻基于上述机器人的里程计的输出及上述机器人的惯性传感器的输出进行融合所得的融合后的位姿数据；
确定上述机器人的第二表达式，上述第二表达式用于表示当前时刻基于上述里程计的输出所得的融合前的位姿数据；
根据上述第一表达式及上述第二表达式，计算得到第一坐标变换矩阵，上述第一坐标变换矩阵用于表示当前时刻下，上述融合后的位姿数据相对于上述融合前的位姿数据的坐标变换；
在上述机器人的运动过程中，基于上述第一坐标变换矩阵及上述里程计的输出计算得到定位结果。
第二方面，本申请提供了一种机器人定位装置，包括：
第一确定单元，用于确定机器人的第一表达式，上述第一表达式用于表示当前时刻基于上述机器人的里程计的输出及上述机器人的惯性传感器的输出进行融合所得的融合后的位姿数据；
第二确定单元，用于确定上述机器人的第二表达式，上述第二表达式用于表示当前时刻基于上述里程计的输出所得的融合前的位姿数据；
计算单元，用于根据上述第一表达式及上述第二表达式，计算得到第一坐标变换矩阵，上述第一坐标变换矩阵用于表示当前时刻下，上述融合后的位姿数据相对于上述融合前的位姿数据的坐标变换；
定位单元，用于在上述机器人的运动过程中，基于上述第一坐标变换矩阵及上述里程计的输出计算得到定位结果。
第三方面，本申请提供了一种机器人，上述机器人包括存储器、处理器以及存储在上述存储器中并可在上述处理器上运行的计算机程序，上述处理器执行上述计算机程序时实现如上述第一方面的方法的步骤。
第四方面，本申请提供了一种计算机可读存储介质，上述计算机可读存储介质存储有计算机程序，上述计算机程序被处理器执行时实现如上述第一方面的方法的步骤。
第五方面，本申请提供了一种计算机程序产品，上述计算机程序产品包括计算机程序，上述计算机程序被一个或多个处理器执行时实现如上述第一方面的方法的步骤。
本申请与现有技术相比存在的有益效果是：首先确定机器人的第一表达式，上述第一表达式用于表示当前时刻基于上述机器人的里程计的输出及上述机器人的惯性传感器的输出进行融合所得的融合后的位姿数据，同时还会去确定上述机器人的第二表达式，上述第二表达式用于表示当前时刻基于上述里程计的输出所得的融合前的位姿数据，接着即可根据上述第一表达式及上述第二表达式，计算得到第一坐标变换矩阵，上述第一坐标变换矩阵用于表示当前时刻下，上述融合后的位姿数据相对于上述融合前的位姿数据的坐标变换，最后可在上述机器人的运动过程中，基于上述第一坐标变换矩阵及上述里程计的输出计算得到定位结果。上述过程中，不再直接将里程计的输出作为机器人的定位结果，而是会先基于里程计的输出及惯性传感器的输出得到机器人高精度的融合后的位姿数据，同时基于里程计的输出得到机器人低精度的融合前的位姿数据，以此计算得到用于补偿里程计的输出的第一坐标变换矩阵，并在机器人的运动过程中，通过该第一坐标变换矩阵对里程计的输出进行补偿计算，以得到高精度的定位结果。
可以理解的是，上述第二方面至第五方面的有益效果可以参见上述第一方面中的相关描述，在此不再赘述。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本申请实施例提供的机器人在移动过程中的位姿示意图；
图2是本申请实施例提供的机器人定位方法的实现流程示意图；
图3是本申请实施例提供的坐标变换树的示意图；
图4是本申请实施例提供的实验效果比对示意图；
图5是本申请实施例提供的机器人定位装置的结构框图；
图6是本申请实施例提供的机器人的结构示意图。
具体实施方式
以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本申请实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本申请。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本申请的描述。
下面对本申请实施例提供的一种机器人定位方法进行描述。为了便于理解，先对基于机器人的里程计的定位作出解释及说明：
机器人往往使用多种传感器对其所处的环境进行感知。其中，机器人的轮子上通常安装有码盘，可用于记录机器人轮子旋转的角度；除此之外，机器人的本体上还安装有惯性传感器(Inertial Measurement Unit，IMU)，可用于测量机器人姿态。示例性地，上述机器人可以双轮差速底盘为载体，且在该双轮差速底盘上的左右驱动轮处各安装了一个码盘用于计算一段时间内对应驱动轮的旋转角度。基于该旋转角度及驱动轮的半径，即可计算出驱动轮的位移。具体地，双轮差速底盘的里程推算公式如下式1：
其中，在Δt这一时间段内，Δθi为双轮差速底盘旋转的角度，也即机器人旋转的角度；Δdi为双轮差速底盘移动的距离，也即机器人移动的距离；ΔdL,i为左驱动轮移动的距离；ΔdR,i为右驱动轮移动的距离；B是左右驱动轮之间的距离，对于一已知的机器人来说通常为定值，由该机器人的实际结构及尺寸而决定。通过上式，可在当前时刻(也即i时刻)到前一时刻(也即i-1时刻)这一时间段(也即Δt)内，基于左右驱动轮的移动距离计算Δdi和Δθi，也即计算机器人移动的距离及旋转的角度。可以认为，本申请所指的前一时刻为前次对上述机器人进行定位的时刻。
请参阅图1，图1给出了机器人在不同时刻下的位姿的示意。在该图1中，机器人从原点开始运动。可记当前时刻(也即i时刻)机器人在该坐标系中的位姿为(Xi,Yi,θi)。基于上述式1，即可得到机器人里程计的迭代公式如下式2：
其中，上式2中，X及Y为坐标数据，与机器人的位移相关；θ为方向角数据，与机器人的旋转角度相关；上述坐标数据及方向角数据构成了机器人的位姿数据。具体地，Xi-1，Yi-1，θi-1为前一时刻(也即i-1时刻)的位姿数据；Xi，Yi，θi为当前时刻(也即i时刻)的位姿数据。上述公式的推导过程为：机器人运动的距离为Δdi，前一时刻的旋转角度为θi-1；为了方便计算，可认为在Δt这一时间段内，机器人的旋转的角度未发生变化；则在投影到坐标系中后，即可得到机器人在坐标系的X方向上移动的距离为Δdicosθi-1，Y方向上移动的距离为Δdisinθi-1；将移动的距离与前一时刻相应的坐标数据进行累加后，即可得到式2。
需要注意的是，本申请实施例所提出的坐标系，指的是以机器人的起始位置(也即机器人开机时的位置)为原点，以机器人在起始位置时机器人的前方为X轴正方向，以机器人在起始位置时机器人的左方为Y轴正方向，竖直向上方向为Z轴正方向所构建的右手坐标系。本实施例所说的方向角数据θ，指的是机器人沿着Z轴旋转后，与X轴正方向所呈现的角度。
前文说明了基于机器人的里程计的输出而进行定位的过程。考虑到机器人存在系统误差及非系统误差，基于里程计的输出进行定位的精度不高。基于此，本申请实施例提出了一种使用里程计和惯性传感器为数据源进行融合的方式进行机器人定位，可输出更准确的机器人在坐标系下的位姿。基于此，本申请实施例将基于里程计所输出的数据所得的位姿数据记为融合前的位姿数据，将基于里程计的输出及惯性传感器的输出进行融合所得的位姿数据记为融合后的位姿数据。在本实施例中，融合前的位姿数据均以不带上角标的字母进行表示，融合后的位姿数据均以带上角标’的字母进行表示。请参阅图2，本申请实施例中的机器人定位方法包括：
步骤101，确定机器人的第一表达式，上述第一表达式用于表示当前时刻基于上述机器人的里程计的输出及上述机器人的惯性传感器的输出进行融合所得的融合后的位姿数据；
在本申请实施例中，第一表达式用于表示当前时刻融合后的位置数据，也即当前时刻基于上述机器人的里程计的输出及上述机器人的惯性传感器的输出进行融合所得的位姿数据。一般情况下，机器人在0时刻时，其位姿的初始值全部为0；也即，机器人的初始时刻的位姿数据为：(X′0,Y′0,θ′0)＝(0,0,0)。需要注意的是，在初始时刻下，融合前的位姿数据与融合后的位姿数据相同。
具体地，上述步骤101包括：
A1、基于上述里程计，计算预设时间段内上述机器人的位移，其中，上述预设时间段为前一时刻至当前时刻；
考虑到机器人从里程计处获得的信息仅为Xi，Yi，θi，无法直接获得机器人的位移；因而，需要首先基于里程计的输出，计算出预设时间段内机器人的位移，其中，上述预设时间段为前一时刻至当前时刻。具体地，可基于上述里程计在当前时刻的输出及前一时刻的输出，计算得到上述预设时间段内上述机器人的移动距离，其计算公式如下式3：
上式3计算出的是机器人的移动距离，而本申请实施例实际所需要的是机器人的位移。考虑到移动距离为标量，式3的计算结果必然为正数；而位移为矢量，机器人实际移动过程中可能存在负值。基于此，可以根据里程计中的公式(Yi-Yi-1)＝Δdisinθi-1判断机器人的移动方向，也即位移的符号。又考虑到此处仅需计算符号，不关心数值结果，而除法运算存在分母为0的情况，会导致计算过程变复杂，因而此处选择进行乘法运算，具体为：计算(Yi-Yi-1)sinθi-1的值；若该值为负，则式3中的Δdi＝-Δdi，也即位移为负；反之则不做操作。也即，对于机器人的位移来说，最终由机器人的移动距离及移动方向来确定。
A2、基于上述位移及上述机器人在前一时刻的融合后的位姿数据进行迭代，确定上述机器人当前时刻的融合后的坐标数据；
根据计算出的Δdi和前一时刻的融合后的位姿数据的X′i-1，Y′i-1，θ′i-1，即可基于下式4计算出当前时刻的融合后的坐标数据：
A3、基于上述惯性传感器，获得当前时刻融合后的方向角数据；
使用惯性传感器融合后的方向角数据替换传统里程计迭代出的方向角数据，更新公式为下式5：
θ′i＝θyaw (式5)
其中，惯性传感器融合后的方向角数据的获取方式如下：
惯性传感器包括陀螺仪和加速度计。通过陀螺仪和加速度计使用卡尔曼滤波或互补滤波融合可得到四元数，再转换成欧拉角。下式6即为四元数转欧拉角的计算公式：
上述公式为通用公式，此处不作过多赘述。也即，当前时刻的融合后的方向角数据可根据上式6实时由四元数转换而得。
A4、基于上述当前时刻融合后的方向角数据，以及当前时刻融合后的坐标数据，确定上述机器人当前时刻的第一表达式。
基于上式4、5及6，可得到机器人当前时刻的第一表达式为：
步骤102，确定上述机器人当前时刻的第二表达式，上述第二表达式用于表示当前时刻基于上述里程计的输出所得的融合前的位姿数据；
在本申请实施例中，该第二表达式即为前文所介绍的式2，此处不作赘述。
步骤103，根据上述第一表达式及上述第二表达式，计算得到第一坐标变换矩阵，上述第一坐标变换矩阵用于表示当前时刻下，上述融合后的位姿数据相对于上述融合前的位姿数据的坐标变换；
请参阅图3，图3示出了机器人的坐标变换树。基于该图3，Origin代表初始时刻的机器人的位置，也即坐标系中的坐标原点(0，0)；基于机器人里程计的输出(Xi,Yi,θi)，可获得机器人当前时刻(也即i时刻)到初始时刻的位置坐标变换Todom_origin，记作第三坐标变换矩阵；基于融合后的里程计(X′i,Y′i,θ′i)，可获得当前时刻(也即i时刻)到初始时刻的位置坐标变换Tfusion_origin，记作第二坐标变换矩阵。基于下式7，可计算出用于修正里程计误差(也即对里程计的输出进行补偿)的位置坐标变换Tfusion_odom，也即第一坐标变换矩阵：
具体地，可根据第一表达式，获得第二坐标变换矩阵，如下式8所示：
可根据第二表达式，获得第三坐标变换矩阵，如下式9所示：
则基于式7，将第二坐标变换矩阵与第三坐标变换矩阵的逆矩阵进行乘法运算，即可得到第一坐标变换矩阵，如下式10所示：
步骤104，在上述机器人的运动过程中，基于上述第一坐标变换矩阵及上述里程计的输出计算得到定位结果。
在本申请实施例中，第一坐标变换矩阵可用于对里程计的输出进行补偿，可一定程度上消除里程计的输出的误差。具体地，可在机器人运动的过程中，可将第一坐标变换矩阵与里程计的输出进行乘法运算，所得结果即为融合了里程计的输出及惯性传感器的输出的高精度的定位结果。也即，通过该第一坐标变换矩阵，即可实时的将低精度的里程计的输出转换为融合后的高精度的定位结果。
请参阅图4，图4示出了本申请实施例的实验效果比对示意图，具体为基于本申请实施例提出的机器人定位方法所得的定位结果与现有技术中基于里程计的输出所得的定位结果之间的比对。由图4可知，机器人移动过程为：机器人从原点出发，经过一个环形区域然后回到原点。最终回到原点时，机器人的坐标理论值为(0,0)。由于里程计系统误差和非系统误差的存在，如图4所示，融合后的定位结果(也即基于本申请实施例提出的机器人定位方法所得的定位结果)的最终误差为(-0.42,1.40)，非融合的定位结果(也即现有技术中基于里程计的输出所得的定位结果)的最终误差为(2.33,1.56)。
由上可见，本申请实施例中，不再直接将里程计的输出作为机器人的定位结果，而是会先基于里程计的输出及惯性传感器的输出得到机器人高精度的融合后的位姿数据，同时基于里程计的输出得到机器人低精度的融合前的位姿数据，以此计算得到用于补偿里程计的输出的第一坐标变换矩阵，并在机器人的运动过程中，通过该第一坐标变换矩阵对里程计的输出进行补偿计算，以得到高精度的定位结果。并且，上述过程所涉及的计算量小，还具备有易于应用及实现简单的优点。
应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。
对应于上文所提供的机器人定位方法，本申请实施例还提供了一种机器人定位装置，上述机器人定位装置可集成于机器人中，参见图5，本申请实施例中的机器人定位装置500包括：
第一确定单元501，用于确定机器人当前时刻的第一表达式，上述第一表达式用于表示基于上述机器人的里程计的输出及上述机器人的惯性传感器的输出进行融合所得的融合后的位姿数据；
第二确定单元502，用于确定上述机器人当前时刻的第二表达式，上述第二表达式用于表示基于上述里程计的输出所得的融合前的位姿数据；
计算单元503，用于根据上述第一表达式及上述第二表达式，计算得到第一坐标变换矩阵，上述第一坐标变换矩阵用于表示当前时刻下，上述融合后的位姿数据相对于上述融合前的位姿数据的坐标变换；
定位单元504，用于在上述机器人的运动过程中，基于上述第一坐标变换矩阵及上述里程计的输出计算得到定位结果。
可选地，上述第一确定单元501，包括：
位移计算子单元，用于基于上述里程计，计算预设时间段内上述机器人的位移，其中，上述预设时间段为前一时刻至当前时刻，上述前一时刻为前次对上述机器人进行定位的时刻；
坐标数据获取子单元，用于基于上述位移及上述机器人在前一时刻的融合后的位姿数据进行迭代，确定上述机器人当前时刻的融合后的坐标数据；
方向角数据获取子单元，用于基于上述惯性传感器，获得当前时刻融合后的方向角数据；
第一表达式确定子单元，用于基于上述当前时刻融合后的方向角数据，以及当前时刻融合后的坐标数据，确定上述机器人的第一表达式。
可选地，上述位移计算子单元，具体用于基于上述里程计在当前时刻的输出及前一时刻的输出，计算得到上述预设时间段内上述机器人的移动距离及移动方向；根据上述移动距离及移动方向，确定上述位移。
可选地，上述计算单元503，包括：
第二坐标变换矩阵获取子单元，用于根据上述第一表达式，获得第二坐标变换矩阵，上述第二坐标变换矩阵用于表示当前时刻上述融合后的位姿数据相对于初始时刻的位姿数据的坐标变换；
第三坐标变换矩阵获取子单元，用于根据上述第二表达式，获得第三坐标变换矩阵，上述第三坐标变换矩阵用于表示当前时刻上述融合前的位姿数据相对于初始时刻的位姿数据的坐标变换；
第一坐标变换矩阵计算子单元，用于根据上述第二坐标变换矩阵及上述第三坐标变换矩阵计算得到上述第一坐标变换矩阵。
可选地，上述第一坐标变换矩阵计算子单元，具体用于将上述第二坐标变换矩阵与上述第三坐标变换矩阵的逆矩阵进行乘法运算，得到上述第一坐标变换矩阵。
可选地，上述定位单元504，具体用于将上述第一坐标变换矩阵与上述里程计的输出进行乘法运算，得到定位结果。
由上可见，本申请实施例中，不再直接将里程计的输出作为机器人的定位结果，而是会先基于里程计的输出及惯性传感器的输出得到机器人高精度的融合后的位姿数据，同时基于里程计的输出得到机器人低精度的融合前的位姿数据，以此计算得到用于补偿里程计的输出的第一坐标变换矩阵，并在机器人的运动过程中，通过该第一坐标变换矩阵对里程计的输出进行补偿计算，以得到高精度的定位结果。并且，上述过程所涉及的计算量小，还具备有易于应用及实现简单的优点。
对应于上文所提供的机器人定位方法，本申请实施例还提供了一种机器人。参见图6，本申请实施例中的机器人6包括：存储器601，一个或多个处理器602(图6中仅示出一个)及存储在存储器601上并可在处理器上运行的计算机程序。其中：存储器601用于存储软件程序以及模块，处理器602通过运行存储在存储器601的软件程序以及单元，从而执行各种功能应用以及数据处理，以获取上述预设事件对应的资源。具体地，处理器602通过运行存储在存储器601的上述计算机程序时实现以下步骤：
确定机器人的第一表达式，上述第一表达式用于表示当前时刻基于上述机器人的里程计的输出及上述机器人的惯性传感器的输出进行融合所得的融合后的位姿数据；
确定上述机器人的第二表达式，上述第二表达式用于表示当前时刻基于上述里程计的输出所得的融合前的位姿数据；
根据上述第一表达式及上述第二表达式，计算得到第一坐标变换矩阵，上述第一坐标变换矩阵用于表示当前时刻下，上述融合后的位姿数据相对于上述融合前的位姿数据的坐标变换；
在上述机器人的运动过程中，基于上述第一坐标变换矩阵及上述里程计的输出计算得到定位结果。
假设上述为第一种可能的实施方式，则在第一种可能的实施方式作为基础而提供的第二种可能的实施方式中，上述位姿数据包括方向角数据及坐标数据，上述确定机器人的第一表达式，包括：
基于上述里程计，计算预设时间段内上述机器人的位移，其中，上述预设时间段为前一时刻至当前时刻，上述前一时刻为前次对上述机器人进行定位的时刻；
基于上述位移及上述机器人在前一时刻的融合后的位姿数据进行迭代，确定上述机器人当前时刻的融合后的坐标数据；
基于上述惯性传感器，获得当前时刻融合后的方向角数据；
基于上述当前时刻融合后的方向角数据，以及当前时刻融合后的坐标数据，确定上述机器人的第一表达式。
在上述第二种可能的实施方式作为基础而提供的第三种可能的实施方式中，上述基于上述里程计，计算预设时间段内上述机器人的位移，包括：
基于上述里程计在当前时刻的输出及前一时刻的输出，计算得到上述预设时间段内上述机器人的移动距离及移动方向；
根据上述移动距离及移动方向，确定上述位移。
在上述第一种可能的实施方式作为基础而提供的第四种可能的实施方式中，上述根据上述第一表达式及上述第二表达式，计算得到第一坐标变换矩阵，包括：
根据上述第一表达式，获得第二坐标变换矩阵，上述第二坐标变换矩阵用于表示当前时刻上述融合后的位姿数据相对于初始时刻的位姿数据的坐标变换；
根据上述第二表达式，获得第三坐标变换矩阵，上述第三坐标变换矩阵用于表示当前时刻上述融合前的位姿数据相对于初始时刻的位姿数据的坐标变换；
根据上述第二坐标变换矩阵及上述第三坐标变换矩阵计算得到上述第一坐标变换矩阵。
在上述第四种可能的实施方式作为基础而提供的第五种可能的实施方式中，上述根据上述第二坐标变换矩阵及上述第三坐标变换矩阵计算得到上述第一坐标变换矩阵，包括：
将上述第二坐标变换矩阵与上述第三坐标变换矩阵的逆矩阵进行乘法运算，得到上述第一坐标变换矩阵。
在上述第一种可能的实施方式作为基础而提供的第六种可能的实施方式中，上述基于上述第一坐标变换矩阵及上述里程计的输出计算得到定位结果，包括：
将上述第一坐标变换矩阵与上述里程计的输出进行乘法运算，得到定位结果。
应当理解，在本申请实施例中，所称处理器602可以是中央处理单元(CentralProcessing Unit，CPU)，该处理器还可以是其他通用处理器、数字信号处理器(DigitalSignal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
存储器601可以包括只读存储器和随机存取存储器，并向处理器602提供指令和数据。存储器601的一部分或全部还可以包括非易失性随机存取存储器。例如，存储器601还可以存储设备类型的信息。
由上可见，本申请实施例中，不再直接将里程计的输出作为机器人的定位结果，而是会先基于里程计的输出及惯性传感器的输出得到机器人高精度的融合后的位姿数据，同时基于里程计的输出得到机器人低精度的融合前的位姿数据，以此计算得到用于补偿里程计的输出的第一坐标变换矩阵，并在机器人的运动过程中，通过该第一坐标变换矩阵对里程计的输出进行补偿计算，以得到高精度的定位结果。并且，上述过程所涉及的计算量小，还具备有易于应用及实现简单的优点。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将上述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者外部设备软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
在本申请所提供的实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的系统实施例仅仅是示意性的，例如，上述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
上述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
上述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关联的硬件来完成，上述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，上述计算机程序包括计算机程序代码，上述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。上述计算机可读存储介质可以包括：能够携带上述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机可读存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，上述计算机可读存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读存储介质不包括是电载波信号和电信信号。
以上实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。
Claims (10)
1.一种机器人定位方法，其特征在于，包括：
确定机器人的第一表达式，所述第一表达式用于表示当前时刻基于所述机器人的里程计的输出及所述机器人的惯性传感器的输出进行融合所得的融合后的位姿数据；
确定所述机器人的第二表达式，所述第二表达式用于表示当前时刻基于所述里程计的输出所得的融合前的位姿数据；
根据所述第一表达式及所述第二表达式，计算得到第一坐标变换矩阵，所述第一坐标变换矩阵用于表示当前时刻下，所述融合后的位姿数据相对于所述融合前的位姿数据的坐标变换；
在所述机器人的运动过程中，基于所述第一坐标变换矩阵及所述里程计的输出计算得到定位结果。
2.如权利要求1所述的机器人定位方法，其特征在于，所述位姿数据包括方向角数据及坐标数据，所述确定机器人的第一表达式，包括：
基于所述里程计，计算预设时间段内所述机器人的位移，其中，所述预设时间段为前一时刻至当前时刻，所述前一时刻为前次对所述机器人进行定位的时刻；
基于所述位移及所述机器人在前一时刻的融合后的位姿数据进行迭代，确定所述机器人当前时刻的融合后的坐标数据；
基于所述惯性传感器，获得当前时刻融合后的方向角数据；
基于所述当前时刻融合后的方向角数据，以及当前时刻融合后的坐标数据，确定所述机器人的第一表达式。
3.如权利要求2所述的机器人定位方法，其特征在于，所述基于所述里程计，计算预设时间段内所述机器人的位移，包括：
基于所述里程计在当前时刻的输出及前一时刻的输出，计算得到所述预设时间段内所述机器人的移动距离及移动方向；
根据所述移动距离及移动方向，确定所述位移。
4.如权利要求1所述的机器人定位方法，其特征在于，所述根据所述第一表达式及所述第二表达式，计算得到第一坐标变换矩阵，包括：
根据所述第一表达式，获得第二坐标变换矩阵，所述第二坐标变换矩阵用于表示当前时刻所述融合后的位姿数据相对于初始时刻的位姿数据的坐标变换；
根据所述第二表达式，获得第三坐标变换矩阵，所述第三坐标变换矩阵用于表示当前时刻所述融合前的位姿数据相对于初始时刻的位姿数据的坐标变换；
根据所述第二坐标变换矩阵及所述第三坐标变换矩阵计算得到所述第一坐标变换矩阵。
5.如权利要求4所述的机器人定位方法，其特征在于，所述根据所述第二坐标变换矩阵及所述第三坐标变换矩阵计算得到所述第一坐标变换矩阵，包括：
将所述第二坐标变换矩阵与所述第三坐标变换矩阵的逆矩阵进行乘法运算，得到所述第一坐标变换矩阵。
6.如权利要求1所述的机器人定位方法，其特征在于，所述基于所述第一坐标变换矩阵及所述里程计的输出计算得到定位结果，包括：
将所述第一坐标变换矩阵与所述里程计的输出进行乘法运算，得到定位结果。
7.一种机器人定位装置，其特征在于，包括：
第一确定单元，用于确定机器人的第一表达式，所述第一表达式用于表示当前时刻基于所述机器人的里程计的输出及所述机器人的惯性传感器的输出进行融合所得的融合后的位姿数据；
第二确定单元，用于确定所述机器人的第二表达式，所述第二表达式用于表示当前时刻基于所述里程计的输出所得的融合前的位姿数据；
计算单元，用于根据所述第一表达式及所述第二表达式，计算得到第一坐标变换矩阵，所述第一坐标变换矩阵用于表示当前时刻下，所述融合后的位姿数据相对于所述融合前的位姿数据的坐标变换；
定位单元，用于在所述机器人的运动过程中，基于所述第一坐标变换矩阵及所述里程计的输出计算得到定位结果。
8.如权利要求7所述的机器人定位装置，其特征在于，所述第一确定单元包括：
位移计算子单元，用于基于所述里程计，计算预设时间段内所述机器人的位移，其中，所述预设时间段为前一时刻至当前时刻，所述前一时刻为前次对所述机器人进行定位的时刻；
坐标数据获取子单元，用于基于所述位移及所述机器人在前一时刻的融合后的位姿数据进行迭代，确定所述机器人当前时刻的融合后的坐标数据；
方向角数据获取子单元，用于基于所述惯性传感器，获得当前时刻融合后的方向角数据；
第一表达式确定子单元，用于基于所述当前时刻融合后的方向角数据，以及当前时刻融合后的坐标数据，确定所述机器人的第一表达式。
9.一种机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至6任一项所述的方法。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至6任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011308068.8A CN112506190A (zh) | 2020-11-19 | 2020-11-19 | 一种机器人定位方法、机器人定位装置及机器人 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011308068.8A CN112506190A (zh) | 2020-11-19 | 2020-11-19 | 一种机器人定位方法、机器人定位装置及机器人 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112506190A true CN112506190A (zh) | 2021-03-16 |
Family
ID=74958980
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011308068.8A Pending CN112506190A (zh) | 2020-11-19 | 2020-11-19 | 一种机器人定位方法、机器人定位装置及机器人 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112506190A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113124872A (zh) * | 2021-03-30 | 2021-07-16 | 深圳市优必选科技股份有限公司 | 一种机器人定位导航方法、装置、终端设备及机器人 |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017008454A1 (zh) * | 2015-07-10 | 2017-01-19 | 中国科学院深圳先进技术研究院 | 一种机器人的定位方法 |
| CN108680159A (zh) * | 2018-04-03 | 2018-10-19 | 中科微至智能制造科技江苏有限公司 | 一种基于数据融合的机器人定位方法 |
| CN110579738A (zh) * | 2018-06-11 | 2019-12-17 | 深圳市优必选科技有限公司 | 运动目标方向角获取方法及终端设备 |
| CN111113422A (zh) * | 2019-12-30 | 2020-05-08 | 深圳市优必选科技股份有限公司 | 机器人定位方法、装置、计算机可读存储介质及机器人 |
| CN111121768A (zh) * | 2019-12-23 | 2020-05-08 | 深圳市优必选科技股份有限公司 | 一种机器人位姿估计方法、装置、可读存储介质及机器人 |
| CN111238496A (zh) * | 2020-01-14 | 2020-06-05 | 深圳市锐曼智能装备有限公司 | 机器人位姿确认方法、装置、计算机设备及存储介质 |
| CN111319041A (zh) * | 2020-01-17 | 2020-06-23 | 深圳市优必选科技股份有限公司 | 一种机器人位姿确定方法、装置、可读存储介质及机器人 |
| US20200206945A1 (en) * | 2018-12-29 | 2020-07-02 | Ubtech Robotics Corp Ltd | Robot pose estimation method and apparatus and robot using the same |
| US20200318973A1 (en) * | 2019-04-02 | 2020-10-08 | GM Global Technology Operations LLC | Method and apparatus of parallel tracking and localization via multi-mode slam fusion process |
| CN111949929A (zh) * | 2020-08-12 | 2020-11-17 | 智能移动机器人(中山)研究院 | 一种多传感器融合的四足机器人运动里程计设计方法 |
-
2020
- 2020-11-19 CN CN202011308068.8A patent/CN112506190A/zh active Pending
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017008454A1 (zh) * | 2015-07-10 | 2017-01-19 | 中国科学院深圳先进技术研究院 | 一种机器人的定位方法 |
| CN108680159A (zh) * | 2018-04-03 | 2018-10-19 | 中科微至智能制造科技江苏有限公司 | 一种基于数据融合的机器人定位方法 |
| CN110579738A (zh) * | 2018-06-11 | 2019-12-17 | 深圳市优必选科技有限公司 | 运动目标方向角获取方法及终端设备 |
| US20200206945A1 (en) * | 2018-12-29 | 2020-07-02 | Ubtech Robotics Corp Ltd | Robot pose estimation method and apparatus and robot using the same |
| US20200318973A1 (en) * | 2019-04-02 | 2020-10-08 | GM Global Technology Operations LLC | Method and apparatus of parallel tracking and localization via multi-mode slam fusion process |
| CN111121768A (zh) * | 2019-12-23 | 2020-05-08 | 深圳市优必选科技股份有限公司 | 一种机器人位姿估计方法、装置、可读存储介质及机器人 |
| CN111113422A (zh) * | 2019-12-30 | 2020-05-08 | 深圳市优必选科技股份有限公司 | 机器人定位方法、装置、计算机可读存储介质及机器人 |
| CN111238496A (zh) * | 2020-01-14 | 2020-06-05 | 深圳市锐曼智能装备有限公司 | 机器人位姿确认方法、装置、计算机设备及存储介质 |
| CN111319041A (zh) * | 2020-01-17 | 2020-06-23 | 深圳市优必选科技股份有限公司 | 一种机器人位姿确定方法、装置、可读存储介质及机器人 |
| CN111949929A (zh) * | 2020-08-12 | 2020-11-17 | 智能移动机器人(中山)研究院 | 一种多传感器融合的四足机器人运动里程计设计方法 |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113124872A (zh) * | 2021-03-30 | 2021-07-16 | 深圳市优必选科技股份有限公司 | 一种机器人定位导航方法、装置、终端设备及机器人 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP7299261B2 (ja) | 車両のデッドレコニング方法、装置、デバイス、記憶媒体、及びプログラム | |
| CN111415387A (zh) | 相机位姿确定方法、装置、电子设备及存储介质 | |
| US20230271656A1 (en) | Robot state estimation method, computer-readable storage medium, and legged robot | |
| CN110887493A (zh) | 基于局部地图匹配的轨迹推算方法、介质、终端和装置 | |
| CN114179825A (zh) | 多传感器融合获取量测值置信度方法及自动驾驶车辆 | |
| CN112506190A (zh) | 一种机器人定位方法、机器人定位装置及机器人 | |
| WO2022135070A1 (zh) | 惯性导航方法及设备 | |
| US20230392938A1 (en) | Navigation information processing method, electronic device, and storage medium | |
| CN109866217B (zh) | 机器人里程定位方法、装置、终端设备及计算机存储介质 | |
| CN113494910A (zh) | 一种基于uwb定位的车辆定位方法、装置及存储介质 | |
| CN112157651B (zh) | 位姿确定方法、装置及机器人 | |
| CN115560744A (zh) | 机器人以及基于多传感器的三维建图方法、存储介质 | |
| CN112455541B (zh) | 一种车轮转角确定方法、装置及设备 | |
| CN114187359A (zh) | 基于位姿增量约束的激光雷达固定位姿标定方法及系统 | |
| CN114323007A (zh) | 一种载体运动状态估计方法及装置 | |
| CN113763481A (zh) | 一种移动场景中多相机视觉三维地图构建与自标定方法 | |
| CN113483762A (zh) | 一种位姿优化方法及设备 | |
| CN111964676A (zh) | 航姿解算方法、装置、电子设备及计算机可读存储介质 | |
| CN113030504B (zh) | 车辆测速方法、装置、车载计算机设备和存储介质 | |
| CN115839726B (zh) | 磁传感器和角速度传感器联合标定的方法、系统及介质 | |
| CN113932835B (zh) | 自动驾驶车辆定位杆臂的标定方法、装置及电子设备 | |
| CN115451958B (zh) | 基于相对旋转角度的相机绝对姿态优化方法 | |
| CN110857860B (zh) | 一种定位转换方法及其系统 | |
| CN117773928A (zh) | 移动设备标定方法、装置、电子设备以及存储介质 | |
| CN114674342A (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 |