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

本发明属于计算机技术领域，尤其涉及一种机器人位姿估计方法、装置、终端及计算机存储介质。

背景技术：

机器人的位姿估计能力对于路径规划与运动控制是机器人实现自主导航的关键，对于提高机器人的自动化水平具有重要的意义。

目前，最常见的机器人位姿估计方法为使用惯性测量单元进行估计。由于惯性测量单元中的传感器测量得到的是机器人的角速度与位移加速度，需要经过积分才能得到机器人的位置和速度值，由于传感器存在噪声偏移量，因此，积分得到的位置和速度值会出现积分漂移，无法实现对机器人进行准确的位姿估计。

技术实现要素：

有鉴于此，本发明实施例提供了一种机器人位姿估计方法、装置、终端及计算机存储介质，能够惯性测量单元无法实现对机器人进行准确的位姿估计的技术问题。

本发明实施例的第一方面提供了一种机器人位姿估计方法，包括：

获取惯性测量单元imu测量得到的初始六自由度位姿数据；

利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据；

获取设置于稳定平台上的2d激光雷达传感器测量得到的三自由度位姿数据；

利用所述三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据。

本发明实施例的第二方面提供了一种机器人位姿估计装置，包括：

第一获取单元，用于获取惯性测量单元imu测量得到的初始六自由度位姿数据；

第一修正单元，用于利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据；

第二获取单元，用于获取设置于稳定平台上的2d激光雷达传感器测量得到的三自由度位姿数据；

第二修正单元，用于利用所述三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据。

本发明实施例的第三方面提供了一种终端，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述所述方法的步骤。

本发明实施例的第四方面提供了一种计算机存储介质，所述计算机存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述所述方法的步骤。

本发明实施例中，通过利用辅助传感器测量得到的位姿数据对惯性测量单元测量得到的初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据，接着，再利用设置于稳定平台上的2d激光雷达传感器测量得到的三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据；使得最终得到的六自由度位姿数据为经过两次修正得到的位姿数据，提高了机器人位姿数据的精度，实现了对机器人进行准确的位姿估计。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的一种机器人位姿估计方法的实现流程示意图；

图2是本发明实施例提供的一种机器人位姿估计方法步骤102的具体实现流程示意图；

图3是本发明实施例提供的对初始六自由度位姿数据进行第一次修正的示意图；

图4是本发明实施例提供的一种机器人位姿估计方法步骤103的具体实现流程示意图；

图5是本发明实施例提供的一种机器人位姿估计装置的示意图；

图6是本发明实施例提供的一种终端的示意图。

具体实施方式

以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本发明实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本发明。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本发明的描述。

为了说明本发明所述的技术方案，下面通过具体实施例来进行说明。

应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和“包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。

还应当理解，在此本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“所述”意在包括复数形式。

具体实现中，本发明实施例中描述的终端可以为机器人、服务器、电脑、手机等配置有机器人位姿估计装置的终端设备，为了描述的方便，本发明以机器人为例进行说明。

请参见图1，图1是本发明实施例提供的一种机器人位姿估计方法的实现流程示意图。本实施例中的机器人位姿估计方法的执行主体为机器人位姿估计装置，该装置可以由软件和/或硬件实现，集成于终端中，适用于需要进行机器人位姿估计的情形。如图1所示的机器人位姿估计方法，可以包括：步骤101至步骤104。

步骤101，获取惯性测量单元imu测量得到的初始六自由度位姿数据。

物体在空间具有六个自由度，即沿x、y、z三个直角坐标轴方向的移动自由度和绕这三个坐标轴的转动自由度。因此，要完全确定物体的位置，就必须清楚这六个自由度。

惯性测量单元(inertialmeasurementunit，imu)是测量物体三轴角速度以及加速度的装置。一般的，一个imu包含了三个单轴的加速度计和三个单轴的陀螺仪，加速度计检测物体在载体坐标系统独立三轴的加速度，而陀螺仪检测载体相对于导航坐标系的角速度，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。

因此，在进行机器人的位姿估计时，可以利用所述惯性测量单元imu测量得到机器人的初始六自由度位姿数据。

步骤102，利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据。

本发明实施例中，由于imu的传感器存在噪声偏移量，因此，积分得到的机器人的位置和速度值会出现积分漂移，无法实现对机器人进行准确的位姿估计，因此，需要利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据。

具体的，所述辅助传感器可以包括全球定位系统(globalpositioningsystem，gps)、气压计、磁力计、视觉测量仪等可以实现位姿数据测量的传感器，此处为举例说明，不表示为对本发明保护范围的限制，在本发明的其他实施方式中，所述辅助传感器还可以包括其他传感器。

可选的，如图2所示，上述步骤102中，利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据，可以包括：步骤201至步骤202。

步骤201，对所述初始六自由度位姿数据进行解耦，得到水平方向上的位姿数据、高度方向上的位姿数据，以及偏航角。

在对所述初始六自由度位姿数据进行解耦之后，可以得到水平方向上的位姿数据、高度方向上的位姿数据，以及偏航角。具体的，如图3所示，所述水平方向上的位姿数据包括：x轴方向的速度和y轴方向的速度，绕x轴运动的俯仰角和绕y轴运动的滚转角；所述高度方向上的位姿数据包括z轴方向的速度，所述偏航角为绕z轴运动的角度。

其中，具体的解耦方法可以采用通用的解耦技术实现。例如，可以利用欧几里得范数实现对所述初始六自由度位姿数据的解耦。

可选的，所述对所述初始六自由度位姿数据进行解耦之前，可以包括：将所述imu在固连系下测量得到的角速度ω＝(ωx,ωy,ωz)^t和加速度a＝(ax,ay,az)^t转换为导航系下的欧拉角位置向量p＝(px,py,pz)^t和速度向量v＝(vx,vy,vz)^t，接着对其进行积分得到所述初始六自由度位姿数据。

例如，通过机器人的刚体运动学方程式将固连系下测量得到的角速度ω＝(ωx,ωy,ωz)^t和加速度a＝(ax,ay,az)^t转换为导航系下的欧拉角位置向量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，求解：

其中，函数si(ξ)为扫描终点si＝(si,x,si,y)^t在世界坐标系下的值，其为三自由度位姿ξ的函数，具体形式为：

函数m(si(ξ))返回占据栅格地图在坐标si(ξ)处的取值(0或1)，在给定初始估计位姿ξ后，需要满足求取方程的位姿修正量δξ：

通过一阶泰勒展开可以得到：

通过高斯牛顿方程可以得到上式的求解为：

其中h阵为：

按照上式则可求取连续两帧扫描数据间位姿的变化量。

步骤104，利用所述三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据。

可选的，所述利用所述三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据，包括：

利用修正方程：对所述修正后的六自由度位姿

数据做第二次修正；

其中，为所述修正后的六自由度位姿数据，p为所述修正后的六自由度位姿数据对应的方差阵，ξ^*为所述三自由度位姿数据，r为所述三自由度位姿数据对应的方差阵，c为观测投影矩阵，用于将所述修正后的六自由度位姿数据投影到所述2d激光雷达传感器的扫描平面中，参数ω为修正权重。

在实际应用中，方差阵p、方差阵r和观测投影矩阵c可以根据传感器的参数计算得到。

可选的，在本发明的一些实施方式中，所述修正方程还可以表达为：

其中，k为滤波增益。

本发明实施例中，通过利用辅助传感器测量得到的位姿数据对惯性测量单元测量得到的初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据，接着，再利用设置于稳定平台上的2d激光雷达传感器测量得到的三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据；使得最终得到的六自由度位姿数据为经过两次修正得到的位姿数据，提高了机器人位姿数据的精度，实现了对机器人进行准确的位姿估计。

本发明实施例还提供一种机器人位姿估计装置，该装置包括用于执行前述的机器人位姿估计方法中的各步骤的模块，该装置可集成于上述终端。该装置中未详细描述之处请详见前述方法的描述。

参见图5，图5是本发明实施例提供的一种机器人位姿估计装置的示意框图。本实施例的机器人位姿估计装置5包括：第一获取单元51、第一修正单元52、第二获取单元53和第二修正单元54。

第一获取单元51，用于获取惯性测量单元imu测量得到的初始六自由度位姿数据；

第一修正单元52，用于利用辅助传感器测量得到的位姿数据对所述初始六自由度位姿数据进行第一次修正，得到修正后的六自由度位姿数据；

第二获取单元53，用于获取设置于稳定平台上的2d激光雷达传感器测量得到的三自由度位姿数据；

第二修正单元54，用于利用所述三自由度位姿数据对所述修正后的六自由度位姿数据做第二次修正，得到目标六自由度位姿数据。

可选的，所述第一修正单元，还具体用于：

对所述初始六自由度位姿数据进行解耦，得到水平方向上的位姿数据、高度方向上的位姿数据，以及偏航角；

利用辅助传感器测量得到的数据分别对所述水平方向上的位姿数据、高度方向上的位姿数据以及所述偏航角进行修正，得到修正后的六自由度位姿数据。

可选的，所述第二获取单元，还用于

将当前时刻以及上一时刻测量得到的两帧数据进行对齐，并估计两帧数据之间三自由度位姿数据的变化量；

利用所述变化量对上一时刻测量得到的三自由度位姿数据进行修正，得到当前时刻的2d激光雷达传感器测量得到的三自由度位姿数据。

可选的，所述第二修正单元，还具体用于：

利用修正方程：对所述修正后的六自由度位姿

数据做第二次修正；

其中，为所述修正后的六自由度位姿数据，p为所述修正后的六自由度位姿数据对应的方差阵，ξ^*为所述三自由度位姿数据，r为所述三自由度位姿数据对应的方差阵，c为观测投影矩阵，用于将所述修正后的六自由度位姿数据投影到所述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可以是中央处理单元(centralprocessingunit，cpu)，还可以是其他通用处理器、数字信号处理器(digitalsignalprocessor，dsp)、专用集成电路(applicationspecificintegratedcircuit，asic)、现成可编程门阵列(field-programmablegatearray，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

存储器61可以是终端6的内部存储单元，例如终端6的硬盘或内存。存储器61也可以是终端6的外部存储设备，例如终端6上配备的插接式硬盘，智能存储卡(smartmediacard，smc)，安全数字(securedigital，sd)卡，闪存卡(flashcard)等。进一步地，存储器61还可以既包括终端6的内部存储单元也包括外部存储设备。存储器61用于存储计算机程序以及终端设备所需的其他程序和数据。存储器61还可以用于暂时地存储已经输出或者将要输出的数据。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

在本发明所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。

作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，计算机程序包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读介质可以包括：能够携带计算机程序代码的任何实体或装置、记录介质、u盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(read-onlymemory，rom)、随机存取存储器(randomaccessmemory，ram)、电载波信号、电信信号以及软件分发介质等。需要说明的是，计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。