CN113112478B - 一种位姿的识别方法及终端设备 - Google Patents

Abstract

本发明适用于设备控制技术领域，提供了一种位姿的识别方法及终端设备，包括：获取终端设备待识别的位姿数据；所述位姿数据包括位置数据以及姿态数据；获取以所述位置数据为中心的区域点云地图；通过内置的激光雷达获取所述终端设备实际位姿下对应的激光点云数据；根据所述激光点云数据与所述区域点云地图，确定各个所述位姿数据对应的置信度得分；基于所述位姿数据的所述置信度得分，生成位姿识别结果。采用本发明可以确定当前已识别的位姿数据的可靠程度，能够大大提高终端设备在环境容易发生变化的场景中的鲁棒性。

Description

一种位姿的识别方法及终端设备

技术领域

本发明属于设备控制技术领域，尤其涉及一种位姿的识别方法及终端设备。

背景技术

随着智能化以及自动化的不断发展，智能机器人的应用领域也越来越广泛，例如可以应用在家居清扫领域、自动送货领域、领路导航领域等等，大大提高了用户生活的便捷性以及智能化程度。在智能机器人运行的过程中，往往依赖于导航技术以及地图自构建技术，而影响导航以及地图构建的准确性的重要因素之一，即为如何能够准确对智能机器人的位姿进行识别。

现有的位姿识别技术，一般是通过传感器获取位姿数据，并通过识别所在场景中关键标志物之间的相对位置，以识别智能机器人的位姿。然而，在家居等室内场景下，关键标志物的位置容易发生变化，或者在原本的行进路径中增加了新的障碍物，从而影响了智能机器人的位姿识别，降低了位姿识别的可靠性以及在环境容易发生变化的场景中的鲁棒性。

发明内容

有鉴于此，本发明实施例提供了一种位姿的识别方法及终端设备，以解决现有的位姿的识别技术，位姿识别的可靠性较低以及在环境容易发生变化的场景中的鲁棒性较低的问题。

本发明实施例的第一方面提供了一种位姿的识别方法，应用于终端设备，包括：

获取终端设备待识别的位姿数据；所述位姿数据包括位置数据以及姿态数据；

获取以所述位置数据为中心的区域点云地图；

通过内置的激光雷达获取所述终端设备实际位姿下对应的激光点云数据；

根据所述激光点云数据与所述区域点云地图，确定各个所述位姿数据对应的置信度得分；

基于所述位姿数据的所述置信度得分，生成位姿识别结果。

本发明实施例的第二方面提供了一种位姿的识别装置，包括：

位姿数据获取单元，用于获取终端设备待识别的位姿数据；所述位姿数据包括位置数据以及姿态数据；

区域点云地图获取单元，用于获取以所述位置数据为中心的区域点云地图；

激光点云数据采集单元，用于通过内置的激光雷达获取所述终端设备实际位姿下对应的激光点云数据；

置信度得分确定单元，用于根据所述激光点云数据与所述区域点云地图，确定各个所述位姿数据对应的置信度得分；

位姿识别结果生成单元，用于基于所述位姿数据的所述置信度得分，生成位姿识别结果。

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

本发明实施例的第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现第一方面的各个步骤。

实施本发明实施例提供的一种位姿的识别方法及终端设备具有以下有益效果：

本发明实施例在需要进行位姿识别时，获取终端设备待识别的位姿数据，继而确定位姿数据中的位置数据对应的区域点云地图；另一方面，终端设备在获取位姿数据时，还可以通过内置的激光雷达获取当前位姿对应的激光点云数据，通过将区域点云地图以及激光点云数据进行比对，可以确定当前位姿数据对应的置信度得分，并基于姿态数据的置信度得到对应的位姿识别结果，确定当前已识别的位姿数据的可靠程度，在可靠程度较高时，可以基于已识别的位姿数据执行对应的正常响应操作，例如控制终端设备在预设的轨迹上行驶；在可靠程度较低时，可以执行异常响应操作，例如重新识别终端设备的位姿数据或者对地图进行更新等，能够大大提高终端设备在环境容易发生变化的场景中的鲁棒性。

附图说明

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

图1是本发明第一实施例提供的一种位姿的识别方法的实现流程图；

图2是本发明一实施例提供的激光点云数据以及区域点云地图的示意图；

图3是本发明第二实施例提供的一种位姿的识别方法S104具体实现流程图；

图4是本发明第三实施例提供的一种位姿的识别方法S1041具体实现流程图；

图5是本发明第四实施例提供的一种位姿的识别方法S101和S105具体实现流程图；

图6是本发明第五实施例提供的一种位姿的识别方法S102具体实现流程图；

图7是本发明一实施例提供的一种位姿的识别装置的结构框图；

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

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明实施例在需要进行位姿识别时，获取终端设备待识别的位姿数据，继而确定位姿数据中的位置数据对应的区域点云地图；另一方面，终端设备在获取位姿数据时，还可以通过内置的激光雷达获取当前位姿对应的激光点云数据，通过将区域点云地图以及激光点云数据进行比对，可以确定当前位姿数据对应的置信度得分，并基于姿态数据的置信度得到对应的位姿识别结果，确定当前已识别的位姿数据的可靠程度，在可靠程度较高时，可以基于已识别的位姿数据执行对应的正常响应操作，例如控制终端设备在预设的轨迹上行驶；在可靠程度较低时，可以执行异常响应操作，例如重新识别终端设备的位姿数据或者对地图进行更新等，解决了位姿的识别技术，位姿识别的可靠性较低以及在环境容易发生变化的场景中的鲁棒性较低的问题。

在本发明实施例中，流程的执行主体为终端设备，该终端设备包括但不限于：智能机器人，如扫地机器人、导航机器人等，该终端设备还可以为可移动设备，如智能模型汽车、无人机等；在一种可能的实现方式中，该流程的执行主体还可以为与终端设备建立有通信连接的另一电子设备，在该情况下，终端设备可以将位姿数据发送给电子设备，以便电子设备输出关于终端设备的位姿识别结果，基于此，上述电子设备可以计算机、智能手机以及平板电脑等能够执行位姿识别任务的设备，并将识别得到的位姿识别结果发送给终端设备，以便终端设备基于位姿识别结果执行相应的响应操作；电子设备还可以基于位姿识别结果确定对应的响应操作，并将该响应操作对应的控制指令发送给终端设备，以控制终端设备执行相应的动作。在后续的实施例中，流程的执行主体以终端设备为例进行说明。

图1示出了本发明第一实施例提供的位姿的识别方法的实现流程图，详述如下：

在S101中，获取终端设备待识别的位姿数据；所述位姿数据包括位置数据以及姿态数据。

在本实施例中，终端设备可以通过内置的数据采集模块，确定当前所在的位姿数据，其中，上述位姿数据处于一个待识别状态，由于内部的数据采集模块可能存在偏差，上述位姿数据可能是一个无效的或识别异常的位姿数据，需要对位姿数据的可信度进行识别，即通过S102～S105输出位置数据对应的位姿识别结果。举例性地，上述数据采集模块包括有：定位模块以及姿态识别识别模块，举例性地，上述定位模块可以是全球定位***GPS模块，若终端设备是基于其他原理确定设备位置的，例如基于无线非保真WIFI信号或者低功耗蓝牙信号确定设备位置，则上述定位模块还可以是WIFI通信模块，或者低功耗蓝牙模块等，通过搜索对应的无线设备的无线信号以及无线信号的信号强度，确定终端设备的位置数据；而上述位姿模块可以包含一个或两个以上的传感器，通过传感器反馈的数值，确定终端设备的姿态数据，其中，上述传感器包括但不限于：陀螺仪、加速度传感器以及角速度传感器等，若终端设备为一智能机器人，且该智能机器人具有多个可动关节，即机器人在改变自身姿态时具有多个自由度，则每个可动关节上可以配置有对应的传感器，终端设备可以基于各个可动关节对应的传感器反馈的感应值，确定该终端设备当前的姿态数据。

在一种可能的实现方式中，上述位置数据具体用于表示终端设备的绝对位置，例如，上述位置数据可以为终端设备所在位置的经纬度，可选地，上述位置数据还可以用于表示终端设备的相对位置，例如终端设备所在的场景下包含有多个标定物，终端设备在确定位姿数据时，采集的位置数据具体可以是与终端设备与各个标定物之间的距离值，在该情况下，上述标定物的个数可以是三个或以上。

在一种可能的实现方式中，上述位姿数据包括但限于：姿态角、各个面向的朝向信息等可以用于表示终端设备姿态的信息。当然，若终端设备包含有多个可活动关节，每个可活动关节具有多个自由度，则在确定终端设备的姿态数据时，可以确定可活动关节在各个自由度上对应的角度，继而根据多个可活动关节在各个自由度上的角度，确定终端设备的姿态数据。

在一种可能的实现方式中，终端设备可以配置有位姿识别的触发条件，终端设备若检测到当前满足任意一个预先配置的位姿识别的触发条件，则可以执行S101～S105的操作，以确定终端设备的位姿。示例性地，上述位姿识别的触发条件可以为一时间触发条件，终端设备可以配置有多个位姿识别的触发时刻，每个触发时刻之间的时间间隔可以是相同的(即具有预设的触发周期)，也可以是不相同的。上述位姿识别的触发时刻之间的时间间隔，可以根据所在场景的障碍物密度确定，若某一场景下，障碍物密度较大，即当前的环境较为复杂，此时需要频繁确认终端设备的位姿，以实现精准控制，因此对应的触发时刻的时间间隔较短；反之，若终端设备所在的场景下障碍物密度较小，即当前环境较为简单，此时无需频繁确认终端设备的位姿，因此对应的触发时刻的时间间隔可以较长。

在一种可能的实现方式中，上述位姿识别的触发条件还可以是事件触发条件，例如，终端设备基于预设的地图在行进的过程中，发生了碰撞，即表示可能终端设备的位姿异常，从而偏离的预设的轨道，才会与障碍物发生碰撞，此时，可以判定满足上述位姿识别的触发条件，确定终端设备的位姿。

在S102中，获取以所述位置数据为中心的区域点云地图。

在本实施例中，终端设备在确定了当前的位姿数据，需要确定该位姿数据的可靠度，即需要计算该位姿数据对应的置信度得分。因此，终端设备可以从位姿数据中提取出位置数据，并获取基于该位置数据为中心的区域点云地图，需要说明的是，上述区域点云地图是预先存储的，可以存储于终端设备的本地存储器内，也可以存储云端服务器处，当然，也可以存储于外部存储器内。

在一种可能的实现方式中，终端设备可以为每个区域点云地区关联有对应的预设位置，终端设备在获取得到当前所在的位置数据后，可以基于该位置数据进行寻址，从以存储的所有预设位置中选取与位置数据匹配的预设位置，并将匹配的预设位置的区域点云地图作为该位置数据对应的区域点云地图。

在一种可能的实现方式中，终端设备可以预先存储有当前场景对应的全局点云地图，在该情况下，终端设备可以从全局点云地图中截取以位置数据为中心的区域点云地图。同样地，该全局点云地图可以存储于终端设备的本地存储器，也可以存储于云端服务器上。

在一种可能的实现方式中，上述区域点云地图可以是终端设备在运行过程中构建得到的。举例性地，终端设备首次在当前场景下运行时，可以通过内置的激光雷达获取在行进过程中获取得到的激光数据，其中，上述行进过程具体是基于预设的探索路线确定的，终端设备会获取探索路线上各个方位对应的激光数据，从而基于采集得到的所有激光数据生成当前场景对应的全局点云地图，其中，上述全局点云地图包括有上述的区域点云地图。可选地，终端设备在行进的过程中，也可以对上述的区域点云地图进行更新，若终端设备在某次行进过程中获取得到的激光数据与之前对应位置获取的激光数据匹配度较低，此时，可以基于当前的激光数据对区域点云地图进行更新。

在一种可能的实现方式中，上述区域点云地图可以由终端设备外的其他设备构建得到的，其中，其他设备构建区域点云地图的方式与上述方式相同，具体不再赘述。在该情况下，其他设备可以将已经构建的区域点云地图发送给本实施例提供的终端设备，当然，其他设备也可以将已构建的地图的上传到第三方设备，例如云端服务器又或者上传到当前场景的管理设备中，终端设备可以通过上述的第三方设备下载与位置数据对应的区域点云地图。

在S103中，通过内置的激光雷达获取所述终端设备实际位姿下对应的激光点云数据。

在本实施例中，终端设备在获取位姿数据的同时，可以通过内置的激光雷达采集当前姿态下对应的激光点云数据。该激光点云数据用于表示：当前场景中各个场景对象与终端设备当前位姿之间相对位置关系。可选地，上述激光点云数据可以具体可以包括在各个空间维度上与终端设备间的距离值。

在本实施例中，由于激光雷达所采集的激光点云数据是基于终端设备实际位姿下采集得到的，即上述激光点云数据能够反应终端设备实际位姿，是可以可信度较高的数据。

在本实施例中，终端设备中配置有激光雷达，该激光雷达具有一定的采集角度以及采集深度，基于此，激光雷达具有一定的有效范围，获取该有效范围内各个像素点与终端设备之间的相对位置，从而构建得到上述的激光点云数据。

在S104中，根据所述激光点云数据与所述区域点云地图，确定各个所述位姿数据对应的置信度得分。

在本实施例中，终端设备在获取了基于待识别的位姿数据确定的区域点云地图以及基于实际位姿对应的激光点云数据之后，可以将两个点云数据进行匹配，从而基于两者之间的匹配度，确定该位姿数据对应的置信度得分。由于区域点云地图是基于待识别的位姿数据确定的，而激光点云数据是基于实际位姿下通过激光雷达采集得到的，若两者匹配度较高，则表示待识别的位姿数据与终端设备的实际位姿之间越接近，对应地，则该位姿数据对应的置信度得分越高；反之，若区域点云地图与激光点云数据之间的匹配度越低，则表示待识别的位姿数据与终端设备的实际位姿之间相差较大，对应地，则该位姿数据对应的置信度得分较低，基于上述结论，终端设备可以通过激光点云数据以及区域点云地图之间的差异程度，得到待识别的位姿数据的置信度得分。

在一种可能的实现方式中，终端设备可以配置有一预设的置信度转换函数，终端设备可以将激光点云数据以及区域点云地图导入到上述的置信度转换函数内，从而计算得到位姿数据对应的置信度得分。可选地，上述置信度转换函数具体包含以下几个模块：点云数据归一化模块，点云数据匹配模块以及置信度得分计算模块。其中，点云数据归一化模块具体是对激光点云数据以及区域点云地图进行标准化处理，例如将激光点云数据以及区域点云地图内各个参数值转换至统一的量纲，还可以将激光点云数据和/或区域点云地图进行矩阵转换，从而变换至一致的角度后，在进行后续的匹配度计算的过程；而点云数据匹配模块具体用于接收点云数据归一化模块输出的归一化后的激光点云数据以及区域点云地图，并计算两者间的相距距离，上述相距距离具体是基于归一化后的激光点云数据以及区域点云地图内各个对应点之间的距离值确定的，其中，上述相距距离可以为各个对应点的距离值的均值，也可以是基于各个对应的距离值构成的矩阵；上述置信度得分计算模块具体是基于传输得到相距距离，计算得到上述置信度得分。

在一种可能的实现方式中，上述置信度得分计算模块输入的是激光点云数据与区域点云地图内各个对应点之间的距离值构成的距离矩阵；在该情况下，上述置信度得分可以设置有对应的距离阈值，将距离矩阵中距离值超过上述距离阈值的对应点识别为异常点，基于异常点的个数，或者异常点的个数占总的点云数的百分比，计算对应的置信度得分。

在一种可能的实现方式中，上述区域点云地图具体可以是以上述位置数据为中心，一周(即360°)范围内的点云数据。上述激光点云数据具体可以为基于当前姿态下预设可视角度内对应的点云数据。示例性地，图2示出了本申请一实施例提供的区域点云地图与激光点云数据的示意图。参见图2所示，上述区域点云地图具体为区域1，而激光点云数据具体为区域2，可见，区域点云地图的区域可以是大于激光点云数据的区域。

在S105中，基于所述位姿数据的所述置信度得分，生成位姿识别结果。

在本实施例中，终端设备在计算得到置信度得分后，可以生成该待识别的位姿数据对应的位姿识别结果。其中，上述位姿识别结果包括但不限于：位姿识别无异常、位姿偏差、位姿识别异常等，通过输出位姿识别结果，不仅能够实现终端设备的位姿识别，也能够给该位姿识别的结果给出可靠性评价，以便终端设备可以基于上述的位姿识别结果进行对应的操作。

在一种可能的实现方式中，终端设备若识别到上述位姿识别结果为位姿识别无异常，则表示待识别的位姿数据与终端设备的实际位姿相同，可以基于该位姿数据执行对应的操作。例如，终端设备按照预设的轨迹行进，则可以确定当前并未偏离上述轨迹，可以基于保持原本的行进策略运行。

在一种可能的实现方式中，终端设备若识别到上述位姿识别结果为位姿偏差，则表示待识别的位姿数据与终端设备的实际位姿存在一定偏差，但是差异并不太大，可以调整终端设备的位姿。

在一种可能的实现方式中，终端设备若识别到上述位姿识别结果为位姿识别异常，则表示待识别的位姿数据与终端设备的实际位姿偏差较大，实际位姿并非预期的位姿，此时可以控制终端设备移动到预设的位姿上。

需要说明的是，终端设备基于位姿识别结果执行对应的响应操作，可以根据终端设备当前执行的任务类型确定，在此不再一一举例。

以上可以看出，本发明实施例提供的一种位姿的识别方法在需要进行位姿识别时，获取终端设备待识别的位姿数据，继而确定位姿数据中的位置数据对应的区域点云地图；另一方面，终端设备在获取位姿数据时，还可以通过内置的激光雷达获取当前位姿对应的激光点云数据，通过将区域点云地图以及激光点云数据进行比对，可以确定当前位姿数据对应的置信度得分，并基于姿态数据的置信度得到对应的位姿识别结果，确定当前已识别的位姿数据的可靠程度，在可靠程度较高时，可以基于已识别的位姿数据执行对应的正常响应操作，例如控制终端设备在预设的轨迹上行驶；在可靠程度较低时，可以执行异常响应操作，例如重新识别终端设备的位姿数据或者对地图进行更新等，能够大大提高终端设备在环境容易发生变化的场景中的鲁棒性。

图3示出了本发明第二实施例提供的一种位姿的识别方法S104的具体实现流程图。参见图3，相对于图1所述实施例，本实施例提供的一种位姿的识别方法中S104包括：S1041～S1042，具体详述如下：

进一步地，所述根据所述激光点云数据与所述区域点云地图，确定各个所述位姿数据对应的置信度得分，包括：

在S1041中，将所述激光点云数据以及所述区域点云地图导入到预设的点云匹配度算法，计算所述激光点云数据与所述区域点云地图之间的匹配度。

在本实施例中，终端设备可以配置有点云匹配算法，该点云匹配算法具体是用于计算任意两个点云数据之间的匹配程度。若两个点云数据之间的匹配度越高，则表示拍摄该点云数据时对应的场景相似度越高，对于同一场景而言，即可以认为两者处于相同的位姿；反之，若两个点云数据之间的匹配度越低，则表示拍摄该点云数据时对应的场景相似度较低，对于同一场景而言，即可以认为两者处于不同的位姿。基于此，终端设备为了判断待识别的位姿数据与终端设备的实际位姿是否相同，可以将激光点云数据以及区域点云地图导入到上述的匹配度算法内，计算得到两者之间的匹配度。

可选地，上述点云匹配算法具体可以采用迭代最近点算法或者正态分布变换算法。终端设备可以分别计算上述两种算法在当前场景下对应的准确率，选取准确率较高的算法作为上述的点云匹配算法。

在S1042中，将所述匹配度导入预设的评价函数，得到所述姿态数据对应的置信度得分；所述评价函数具体为：

其中，score(p)为所述匹配度；F[score(p)]为所述评价函数；bel(x)为所述置信度得分。

在本实施例中，终端设备在计算得到上述匹配度后，可以导入到预设的评价函数，将上述匹配度转换为对应的置信度得分。具体地，上述匹配度越高，则表示识别得到的位姿数据与终端设备的实际位姿越相近，则对应的置信度得分也越高。需要说明的是，上述评价函数并非一线性函数，即匹配度与置信度之间是一个非线性关系，由于终端设备所在的环境可能是一个易变环境，例如在家庭环境下，家居容易移位或者增减，因此，若当前位姿下获取得到的激光点云数据会于预设的区域点云地图之间在部分区域会存在较大的偏差，从而导致整体的匹配度数值较低，即便位姿数据与实际位姿一致，匹配度也较低。基于此，终端设备在匹配度较高的区间内，对应的置信度得分趋于相同，从而能够保证在环境变化较大的场景下，能够增加容错率；而在匹配度较低的区间，会增大相互之间的差异性，即对应的置信度得分会相差较大，从而能够在保证易变场景下的容错率的同时，也能够提高位姿异常识别的准确性。

示例性地，若上述计算得到的匹配度为60％，即0.6，则通过上述评价函数进行转换后，可以计算得到上述置信度得分为100％。

在本申请实施例中，通过计算激光点云数据以及区域点云地图之间的匹配度，并将两者的匹配度转换为对应的置信度得分，能够根据两个点云数据之间的匹配程度的高低，从而确定待识别的位姿数据与实际位置的差异程度，并基于差异程度确定上述位姿数据的置信度得分，能够提高置信度得分的准确性。

图4示出了本发明第三实施例提供的一种位姿的识别方法S1041的具体实现流程图。参见图4，相对于图3所述实施例，本实施例提供的一种位姿的识别方法S1041包括S401～S403，具体详述如下：

进一步地，所述基于所述三维模型，确定多组具有相邻关系的模型组，并建立各个所述模型组对应的接触力函数，包括：

在S401中，在所述区域点云地图中查找所述激光点云数据中任意N个第一特征点对应的第二特征点，并基于所述N个所述第一特征点以及N个所述第二特征点生成点云转换矩阵；所述N为大于等于3的正整数。

在本实施例中，终端设备在需要计算激光点云数据与区域点云地图之间的匹配度之前，可以对激光点云数据进行转换，由于激光点云数据在采集时的姿态与区域点云地图采集时对应的姿态可能存在一定的偏差，为了提高识别的准确性，可以通过点云转换矩阵来消除因采集姿态上的差异而引起的误差。

在一种可能的实现方式中，由于区域点云地图可以是基于位置数据下一周范围内的点云数据，而激光点云数据可以是预设的可视角度内的点云数据，即区域点云地图的范围会大于激光点云数据的范围，因此，需要确定用于对激光点云数据的点云转换矩阵。

在本实施例中，终端设备需要确定点云转换矩阵时，需要在两个点云数据中获取至少三个特征点，根据上述至少三个特征点，能够确定在三维空间维度上的转换矩阵。基于此，终端设备可以从激光点云数据中确定任意至少3个第一特征点，并在区域点云地图中分别确定各个第一特征点对应的第二特征点，并基于上述确定的第一特征点以及第二特征点对应位置坐标，即可以生成点云转换矩阵。

在一种可能的实现方式中，当前场景中配置有标志物，并将标志物在点云数据中的点作为上述的特征点。基于此，终端设备可以在区域点云地图中确定标志物对应的点，作为上述的第一特征点，以及在激光点云数据中确定标志物对应的点，作为上述的第二特征点。

在S402中，基于所述点云转换矩阵，生成所述激光点云数据对应的激光转换矩阵。

在本实施例中，终端设备获取得到点云转换矩阵后，可以基于激光点云数据进行转换，从而生成激光转换矩阵。在完成转换后，终端设备能够快速确定激光点云数据中各个点在区域点云数据中对应的点。

在S403中，计算所述激光转换矩阵中所述第一特征点与所述区域点云地图中所述第一特征点对应的第二特征点之间的偏差距离。

在本实施例中，终端设备由于确定了各个第一特征点在激光点云数据中的位置，因此，在进行矩阵转换后，终端设备也可以在激光转换矩阵中定位出各个第一特征点所在的位置，并且各个第一特征点与第二特征点之间的对应关系也保持不变，而第一特征点与对应的第二特征点之间的偏差距离越小，则表示两个点云数据之间越相似；反之，第一特征点与对应第二特征点之间的偏差距离越大，则表示两个点云数据之间差异较大，因此可以通过各个特征点之间的偏差距离以计算两者之间的匹配度。

在一种可能的实现方式中，终端设备还可以基于第一特征点以及第二特征点之间的对应关系，建立激光转换矩阵中各个点与区域点云地图中对应的点，从而构建各个点之间的对应关系，终端设备各个点之间的偏差距离，并基于所有偏差距离确定激光点云数据以及区域点云地图之间的匹配度。

在S404中，基于所有所述偏差距离得到所述匹配度。

在本实施例中，终端设备可以根据各个对应特征点之间的偏差距离，导入到预设的匹配度计算函数，从而能够计算得到上述的匹配度。其中，上述偏差距离越大，对应的匹配度越低；反之，若偏差距离越小，则对应的匹配度越高。

在一种可能的实现，终端设备可以计算偏差距离的均值，并基于偏差距离均值的倒数确定上述的匹配度。

在本申请实施例中，通过识别两个点云数据中具有关联关系的第一特征点以及第二特征点，并基于特征点生成点云转换矩阵，对激光点云数据进行转换，得到激光转换矩阵，从而能够计算关联的特征点之间的偏差距离，从而根据偏差程度计算出两者之间的匹配度，提高了匹配度的准确性。

图5示出了本发明第四实施例提供的一种位姿的识别方法S101和S104的具体实现流程图。参见图5，相对于图1-4任一项所述实施例，本实施例提供的一种位姿的识别方法S101包括：S1011，对应地，S105包括S1051～S1504，具体详述如下：

进一步地，所述获取终端设备待识别的位姿数据，包括：

在S1011中，在所述终端设备移动的过程中，以预设的采集周期获取所述位姿数据。

在本实施例中，终端设备在移动的过程中，需要实时识别自身的位姿，并基于自身的位姿以调整移动方向以及移动速度等，因此，终端设备可以配置有采集周期，并周期性地获取终端设备的位姿数据。

对应地，所述基于所述位姿数据的所述置信度得分，生成位姿识别结果，包括：

在S1051中，若任一所述采集周期对应的所述位姿识别结果为位姿偏差，则基于所述激光点云数据以及所述区域点云地图，调整所述终端设备的位姿，或更新所述采集周期的所述位姿数据。

在本实施例中，终端设备若检测到任一采集周期对应的位姿识别结果为位姿偏差，即表示待识别的位姿数据与终端设备的实际位姿之间差异较小，终端设备可以根据激光点云数据与区域点云地图之间的差异程度，在该情况下，对于终端设备有两种调整方式，其中一种是调整终端设备的实际位姿，以使实际位姿与待识别的位姿数据一致；另一种则是更新位姿数据，以使更新后的位姿数据与实际位姿一致。

因此，对于上述的第一种方式，终端设备确定调整角度以及调整距离，并基于上述的调整角度以及调整距离对终端设备当前的姿态进行调整，将终端设备的实际位姿，调整至与待识别的位姿数据一致；

对于上述的第二种方式，终端设备可以从全局点云地图中确定与上述激光点云数据对应的中心位置，并基于上述确定的中心位置对上述的位姿数据进行更新。

在本申请实施例中，终端设备在移动过程中可以预设的采集周期周期性获取位姿数据，以周期识别终端设备的位姿，并在出现位姿发生偏差时，可以实时对终端设备的位姿或位姿数据进行调整，以提高移动过程的准确性。

可选地，所述基于所述位姿数据的所述置信度得分，生成位姿识别结果，还可以包括：

在S1052中，若连续M个所述采集周期对应的所述位姿识别结果为位姿识别异常，则生成位置丢失指令；所述M大于或等于预设的异常响应阈值。

在本实施例中，终端设备可以配置有对应的异常响应阈值，若连续多个采集周期对应的位姿识别结果为位姿识别异常，则表示终端设备连续多次待识别的位姿数据与终端设备的实际位姿偏差较大，即终端设备并非基于预设的移动轨迹进行移动，基于此，终端设备可以判定当前的处于位置丢失的情况，为了对上述异常情况进行响应，终端设备可以生成位置丢失指令，以便修复异常情况。

在S1053中，响应于所述位置丢失指令，控制所述终端设备移动至预设的复位位置。

在本实施例中，终端设备当前场景中可以配置有至少一个复位位置，该复位位置可以配置有引导信号，终端设备可以基于引导信号移动至上述定位点，从而能够重新找回终端设备的位置。上述引导信号可以为无线WIFI信号，也可以是蓝牙信号等，终端设备可以根据信号强度与位置之间的具有强相关的特性，移动至上述的复位位置。

在本申请实施例中，检测到终端设备的位姿数据出现连续的为位姿识别异常时，识别终端设备处于位置丢失状态，并移动至复位位置，能够自动修复位置丢失的异常情况，提高了异常情况的鲁棒性。

可选地，所述基于所述位姿数据的所述置信度得分，生成位姿识别结果，还可以包括：

S1054，若连续Q个所述采集周期对应的所述位姿识别结果为位姿识别异常，则基于采集周期获取的所述激光点云数据更新所述区域点云地图；所述Q小于预设的异常响应阈值。

在本实施例中，终端设备若检测到连续的Q个位姿数据的位姿识别结果为位姿识别异常，则但识别异常的个数Q并未大于异常响应阈值，此时，在Q个采集周期后，终端设备又重新能够准确识别自身的位姿，即表示终端设备并未丢失当前的位置，造成上述异常情况可能是由于环境变化较大而导致的，例如家居移位或增减等，此时，终端设备可以基于周期采集得到的激光点云数据对对应的区域点云地图进行更新，从而能够使得区域点云地图与障碍物变化后的场景相匹配。

在本申请实施例中，在位姿识别结果的异常个数较少的情况下，可以基于激光点云数据对区域点云地图进行更新，实现了对区域点云地图的实时更新的目的，提高了终端设备对于可变环境的适应性。

图6示出了本发明第五实施例提供的一种位姿的识别方法的具体实现流程图。参见图6，相对于图1-4任一项所述实施例，本实施例提供的一种位姿的识别方法中S102包括：S1021～S1023，具体详述如下：

进一步地，所述姿态数据包括所述终端设备的姿态角；所述获取以所述位置数据为中心的区域点云地图，包括：

在S1021中，从预设的全局点云地图中，确定所述位置数据对应的中心点。

在本实施例中，终端设备预先存储有全局点云地图，该全局点云地图中每个点可以关联有预设位置，终端设备可以将本次识别的位置数据与全局点云地图中各个点关联的预设位置进行比对，将与位置数据相匹配的预设位置对应的点，作为上述的中心点。

在S1022中，以所述中心点为圆心、所述姿态角为起始角度、预设的角分辨率为半径，确定有效识别区域。

在本实施例中，终端设备可以上述中心点为圆心，姿态数据内识别得到的姿态角为起始角度，预设的角分辨率为有效识别半径，获取以上述中心点为圆心对应的一周范围内的区域，将上述区域识别为有效识别区域。

在S1023中，在所述全局点云地图中截取所述有效识别区域对应的点云地图作为所述区域点云地图。

在本实施例中，终端设备可以从全局点云地图中提取上述有效识别区域对应的点云数据，并将提取得到的点云数据作为上述区域点云地图。

应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。

图7示出了本发明一实施例提供的一种位姿的识别装置的结构框图，该终端设备包括的各单元用于执行图1对应的实施例中的各步骤。具体请参阅图1与图1所对应的实施例中的相关描述。为了便于说明，仅示出了与本实施例相关的部分。

参见图7，所述位姿的识别装置包括：

位姿数据获取单元71，用于获取终端设备待识别的位姿数据；所述位姿数据包括位置数据以及姿态角；

区域点云地图获取单元72，用于获取以所述位置数据为中心的区域点云地图；

激光点云数据采集单元73，用于通过内置的激光雷达获取所述终端设备实际位姿下对应的激光点云数据；

置信度得分确定单元74，用于根据所述激光点云数据与所述区域点云地图，确定各个所述位姿数据对应的置信度得分；

位姿识别结果生成单元75，用于基于所述位姿数据的所述置信度得分，生成位姿识别结果。

可选地，所述置信度得分确定单元74包括：

匹配度计算单元，用于将所述激光点云数据以及所述区域点云地图导入到预设的点云匹配度算法，计算所述激光点云数据与所述区域点云地图之间的匹配度；

评价函数导入单元，用于将所述匹配度导入预设的评价函数，得到所述姿态数据对应的置信度得分；所述评价函数具体为：

其中，score(p)为所述匹配度；F[score(p)]为所述评价函数；bel(x)为所述置信度得分。

可选地，所述匹配度计算单元包括：

特征点确定单元，用于在所述区域点云地图中查找所述激光点云数据中任意N个第一特征点对应的第二特征点，并基于所述N个所述第一特征点以及N个所述第二特征点生成点云转换矩阵；所述N为大于等于3的正整数；

激光转换矩阵生成单元，用于基于所述点云转换矩阵，生成所述激光点云数据对应的激光转换矩阵；

偏差距离计算单元，用于计算所述激光转换矩阵中所述第一特征点与所述区域点云地图中所述第一特征点对应的第二特征点之间的偏差距离；

偏差距离转换单元，用于基于所有所述偏差距离得到所述匹配度。

可选地，所述位姿数据获取单元，具体用于在所述终端设备移动的过程中，以预设的采集周期获取所述位姿数据；

对应地，则所述位姿识别结果生成单元75，包括：

位姿偏差响应单元，用于若任一所述采集周期对应的所述位姿识别结果为位姿偏差，则基于所述激光点云数据以及所述区域点云地图，调整所述终端设备的位姿，或更新所述采集周期的所述位姿数据。

可选地，所述位姿识别结果生成单元75，还包括：

位置丢失指令响应单元，用于若连续M个所述采集周期对应的所述位姿识别结果为位姿识别异常，则生成位置丢失指令；所述M大于或等于预设的异常响应阈值；

位置丢失修复单元，用于响应于所述位置丢失指令，控制所述终端设备移动至预设的复位位置。

可选地，所述位姿识别结果生成单元75，还包括：

地图更新单元，用于若连续Q个所述采集周期对应的所述位姿识别结果为位姿识别异常，则基于采集周期获取的所述激光点云数据更新所述区域点云地图；所述Q小于预设的异常响应阈值。

可选地，所述姿态数据包括所述终端设备的姿态角；所述区域点云地图获取单元72包括：

中心点确定单元，用于从预设的全局点云地图中，确定所述位置数据对应的中心点；

有效识别区域确定单元，用于以所述中心点为圆心、所述姿态角为起始角度、预设的角分辨率为半径，确定有效识别区域；

区域点云地图生成单元，用于在所述全局点云地图中截取所述有效识别区域对应的点云地图作为所述区域点云地图。

因此，本发明实施例提供的位姿的识别装置同样可以在需要进行位姿识别时，获取终端设备待识别的位姿数据，继而确定位姿数据中的位置数据对应的区域点云地图；另一方面，终端设备在获取位姿数据时，还可以通过内置的激光雷达获取当前位姿对应的激光点云数据，通过将区域点云地图以及激光点云数据进行比对，可以确定当前位姿数据对应的置信度得分，并基于姿态数据的置信度得到对应的位姿识别结果，确定当前已识别的位姿数据的可靠程度，在可靠程度较高时，可以基于已识别的位姿数据执行对应的正常响应操作，例如控制终端设备在预设的轨迹上行驶；在可靠程度较低时，可以执行异常响应操作，例如重新识别终端设备的位姿数据或者对地图进行更新等，能够大大提高终端设备在环境容易发生变化的场景中的鲁棒性。

图8是本发明另一实施例提供的一种终端设备的示意图。如图8所示，该实施例的终端设备8包括：处理器80、存储器81以及存储在所述存储器81中并可在所述处理器80上运行的计算机程序82，例如位姿的识别程序。所述处理器80执行所述计算机程序82时实现上述各个位姿的识别方法实施例中的步骤，例如图1所示的S101至S105。或者，所述处理器80执行所述计算机程序82时实现上述各装置实施例中各单元的功能，例如图7所示模块71至75功能。

示例性的，所述计算机程序82可以被分割成一个或多个单元，所述一个或者多个单元被存储在所述存储器81中，并由所述处理器80执行，以完成本发明。所述一个或多个单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序82在所述终端设备8中的执行过程。例如，所述计算机程序82可以被分割成位姿数据获取单元、区域点云地图获取单元、激光点云数据采集单元、置信度得分确定单元以及位姿识别结果生成单元，各单元具体功能如上所述。

所述终端设备可包括，但不仅限于，处理器80、存储器81。本领域技术人员可以理解，图8仅仅是终端设备8的示例，并不构成对终端设备8的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述终端设备还可以包括输入输出设备、网络接入设备、总线等。

所称处理器80可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述存储器81可以是所述终端设备8的内部存储单元，例如终端设备8的硬盘或内存。所述存储器81也可以是所述终端设备8的外部存储设备，例如所述终端设备8上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器81还可以既包括所述终端设备8的内部存储单元也包括外部存储设备。所述存储器81用于存储所述计算机程序以及所述终端设备所需的其他程序和数据。所述存储器81还可以用于暂时地存储已经输出或者将要输出的数据。

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

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

Claims (8)

1.一种位姿的识别方法，应用于终端设备，其特征在于，包括：

获取终端设备待识别的位姿数据；所述位姿数据包括位置数据以及姿态数据；

获取以所述位置数据为中心的区域点云地图；

通过内置的激光雷达获取所述终端设备实际位姿下对应的激光点云数据；

根据所述激光点云数据与所述区域点云地图，确定各个所述位姿数据对应的置信度得分；

基于所述位姿数据的所述置信度得分，生成位姿识别结果；

所述根据所述激光点云数据与所述区域点云地图，确定各个所述位姿数据对应的置信度得分，包括：

将所述激光点云数据以及所述区域点云地图导入到预设的点云匹配度算法，计算所述激光点云数据与所述区域点云地图之间的匹配度；

将所述匹配度导入预设的评价函数，得到所述姿态数据对应的置信度得分；所述评价函数具体为：

其中，score(p)为所述匹配度；F[score(p)]为所述评价函数；bel(x)为所述置信度得分；

所述将所述激光点云数据以及所述区域点云地图导入到预设的点云匹配度算法，计算所述激光点云数据与所述区域点云地图之间的匹配度，包括：

在所述区域点云地图中查找所述激光点云数据中任意N个第一特征点对应的第二特征点，并基于所述N个所述第一特征点以及N个所述第二特征点生成点云转换矩阵；所述N为大于等于3的正整数；

基于所述点云转换矩阵，生成所述激光点云数据对应的激光转换矩阵；

计算所述激光转换矩阵中所述第一特征点与所述区域点云地图中所述第一特征点对应的第二特征点之间的偏差距离；

基于所有所述偏差距离得到所述匹配度。

2.根据权利要求1所述的识别方法，其特征在于，所述获取终端设备待识别的位姿数据，包括：

在所述终端设备移动的过程中，以预设的采集周期获取所述位姿数据；

对应地，则所述基于所述位姿数据的所述置信度得分，生成位姿识别结果，包括：

若任一所述采集周期对应的所述位姿识别结果为位姿偏差，则基于所述激光点云数据以及所述区域点云地图，调整所述终端设备的位姿，或更新所述采集周期的所述位姿数据。

3.根据权利要求2所述的识别方法，其特征在于，所述基于所述位姿数据的所述置信度得分，生成位姿识别结果，还包括：

若连续M个所述采集周期对应的所述位姿识别结果为位姿识别异常，则生成位置丢失指令；所述M大于或等于预设的异常响应阈值；

响应于所述位置丢失指令，控制所述终端设备移动至预设的复位位置。

4.根据权利要求2所述的识别方法，其特征在于，所述基于所述位姿数据的所述置信度得分，生成位姿识别结果，还包括：

若连续Q个所述采集周期对应的所述位姿识别结果为位姿识别异常，则基于采集周期获取的所述激光点云数据更新所述区域点云地图；所述Q小于预设的异常响应阈值。

5.根据权利要求1-4任一项所述的识别方法，其特征在于，所述姿态数据包括所述终端设备的姿态角；所述获取以所述位置数据为中心的区域点云地图，包括：

从预设的全局点云地图中，确定所述位置数据对应的中心点；

以所述中心点为圆心、所述姿态数据为起始角度、预设的角分辨率为半径，确定有效识别区域；

在所述全局点云地图中截取所述有效识别区域对应的点云地图作为所述区域点云地图。

6.一种位姿的识别装置，其特征在于，包括：

位姿数据获取单元，用于获取终端设备待识别的位姿数据；所述位姿数据包括位置数据以及姿态数据；

区域点云地图获取单元，用于获取以所述位置数据为中心的区域点云地图；

激光点云数据采集单元，用于通过内置的激光雷达获取所述终端设备实际位姿下对应的激光点云数据；

置信度得分确定单元，用于根据所述激光点云数据与所述区域点云地图，确定各个所述位姿数据对应的置信度得分；

位姿识别结果生成单元，用于基于所述位姿数据的所述置信度得分，生成位姿识别结果；

所述置信度得分确定单元包括：

匹配度计算单元，用于将所述激光点云数据以及所述区域点云地图导入到预设的点云匹配度算法，计算所述激光点云数据与所述区域点云地图之间的匹配度；

评价函数导入单元，用于将所述匹配度导入预设的评价函数，得到所述姿态数据对应的置信度得分；所述评价函数具体为：

其中，score(p)为所述匹配度；F[score(p)]为所述评价函数；bel(x)为所述置信度得分；

所述匹配度计算单元包括：

特征点确定单元，用于在所述区域点云地图中查找所述激光点云数据中任意N个第一特征点对应的第二特征点，并基于所述N个所述第一特征点以及N个所述第二特征点生成点云转换矩阵；所述N为大于等于3的正整数；

激光转换矩阵生成单元，用于基于所述点云转换矩阵，生成所述激光点云数据对应的激光转换矩阵；

偏差距离计算单元，用于计算所述激光转换矩阵中所述第一特征点与所述区域点云地图中所述第一特征点对应的第二特征点之间的偏差距离；

偏差距离转换单元，用于基于所有所述偏差距离得到所述匹配度。

7.一种终端设备，其特征在于，所述终端设备包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时如权利要求1至5任一项所述方法的步骤。

8.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至5任一项所述方法的步骤。