CN113256722B

CN113256722B - 一种位姿确定方法、装置及存储介质

Info

Publication number: CN113256722B
Application number: CN202110682444.8A
Authority: CN
Inventors: 石鹏; 卢维; 王政; 李铭; 胡鲲
Original assignee: Zhejiang Huaray Technology Co Ltd
Current assignee: Zhejiang Huaray Technology Co Ltd
Priority date: 2021-06-21
Filing date: 2021-06-21
Publication date: 2021-10-15
Anticipated expiration: 2041-06-21
Also published as: CN113256722A

Abstract

本申请提供了一种位姿确定方法，包括：电子设备确定图像对应的至少两条观测线段；基于各所述观测线段以及与各所述观测线段匹配的参考线段，确定第一角度候选集合；基于所述第一角度候选集合和所述电子设备的初始位姿，确定所述电子设备的第一位姿候选集合；基于所述第一位姿候选集合、各所述观测线段、以及与各所述观测线段匹配的参考线段，确定所述电子设备的位姿；本申请还提供一种位姿确定装置和存储介质，通过本申请提供的位姿确定方法、装置和存储介质，可以缩短位姿求解时长，提高位姿确定效率。

Description

一种位姿确定方法、装置及存储介质

技术领域

本申请涉及定位技术领域，尤其涉及一种位姿确定方法、装置及存储介质。

背景技术

机器人自主定位技术是机器人实现自主化、智能化的关键技术，相关技术中，通常使用激光雷达、视觉传感器、轮式里程计等方法实现机器人的自主定位，然而上述方案中大多采用点、线结合的方式实现机器人的位姿求解，耗时较长，因此，如何缩短位姿求解时长，提高位姿确定效率是需要解决的技术问题。

发明内容

本申请提供一种位姿确定方法、装置及存储介质，以至少解决现有技术中存在的以上技术问题。

本申请第一方面提供一种位姿确定方法，包括：

电子设备确定图像对应的至少两条观测线段；

基于各所述观测线段以及与各所述观测线段匹配的参考线段，确定第一角度候选集合；

基于所述第一角度候选集合和所述电子设备的初始位姿，确定所述电子设备的第一位姿候选集合；

基于所述第一位姿候选集合、各所述观测线段、以及与各所述观测线段匹配的参考线段，确定所述电子设备的位姿。

上述方案中，所述确定第一角度候选集合之前，所述方法还包括：

计算世界坐标系中，各所述观测线段与参考线段之间的距离，确定与各所述观测线段匹配的至少一条参考线段；

基于各所述观测线段与所匹配的至少一条参考线段的长度，和/或，各所述观测线段的法向量与所匹配的至少一条参考线段的法向量之间的角度，确定与各所述观测线段匹配的一条参考线段。

上述方案中，所述基于各所述观测线段与所匹配的至少一条参考线段的长度，和/或，各所述观测线段的法向量与所匹配的至少一条参考线段的法向量之间的角度，确定与各所述观测线段匹配的一条参考线段，包括：

计算各所述观测线段和与各所述观测线段所匹配的全部参考线段之间的距离；

以观测线段为维度，按照所述距离的大小对每个所述观测线段所匹配的全部参考线段排序；

确定所述全部参考线段中满足第一条件、且与所述观测线段的距离最小的参考线段为与所述观测线段匹配的参考线段；

所述第一条件包括：参考线段的长度大于或等于所述观测线段，和/或，所述参考线段的法向量与所述观测线段的法向量之间的角度小于第一角度阈值。

上述方案中，所述基于各所述观测线段以及与各所述观测线段匹配的参考线段，确定第一角度候选集合包括：

基于各所述观测线段在所述世界坐标系中与世界坐标系X轴之间的夹角、各所述观测线段匹配的参考线段在所述世界坐标系中与所述世界坐标系X轴之间的夹角，以及各所述观测线段基于所述电子设备的初始位姿在所述世界坐标系中的投影与所述世界坐标系X轴之间的夹角，确定角度残差代价函数；

基于所述角度残差代价函数确定角度优化目标函数；

求解所述角度优化目标函数，得到所述初始角度修正值。

上述方案中，所述基于各所述观测线段以及与各所述观测线段匹配的参考线段，确定第一角度候选集合，包括：

基于所述初始角度修正值、角度修正阈值范围和角度修正步长，确定所述第一角度候选集合。

上述方案中，所述基于所述初始角度修正值、角度修正阈值范围和角度修正步长，确定所述第一角度候选集合，包括：

基于所述电子设备的初始位姿中的初始角度、所述初始角度修正值范围和角度步长，确定所述第一角度候选集合中的每一个元素；

其中，所述第一角度候选集合中元素的数量基于角度候选范围和角度步长确定。

上述方案中，所述基于所述第一角度候选集合和所述电子设备的初始位姿，确定所述电子设备的第一位姿候选集合，包括：

利用所述第一角度候选集合中的每一个元素替换所述初始位姿中的角度值，得到所述第一位姿候选集合。

上述方案中，所述基于所述第一位姿候选集合、各所述观测线段、以及与各所述观测线段匹配的参考线段，确定所述电子设备的位姿，包括：

基于所述各所述观测线段，以及与各所述观测线段匹配的参考线段，确定位姿修正向量；

基于所述位姿修正向量，修正所述第一位姿候选集合中的每个元素，得到目标位姿候选集合

从所述目标位姿候选集合中选择一个位姿为所述电子设备的位姿。

上述方案中，所述基于各所述观测线段，以及与各所述观测线段匹配的参考线段，确定位姿修正向量，包括：

基于各所述观测线段的中点位置、所述与各所述观测线段匹配的参考线段的中点位置和各所述参考线段的法向量，确定所述各观测线段与其所匹配的参考线段之间的距离向量；

基于所述距离向量确定位置残差代价函数；

求解所述位置残差代价函数，得到位姿修正向量。

上述方案中，所述从所述目标位姿候选集合中选择一个位姿为所述电子设备的位姿，包括：

利用所述位姿修正向量替换所述第一位姿候选集合中的位姿向量，得到目标位姿候选集合；

计算所述目标位姿候选集合中每一个元素对应的位置残差代价和角度残差代价；

基于所述位置残差代价、所述角度残差代价和第一权重系数，确定所述目标位姿候选集合中每一个元素的分数；

确定分数最大值对应的元素为所述电子设备的位姿。

本申请第二方面提供一种位姿确定装置，包括：确定单元，用于确定图像对应的至少两条观测线段；基于各所述观测线段以及与各所述观测线段匹配的参考线段，确定第一角度候选集合；基于所述第一角度候选集合和所述电子设备的初始位姿，确定所述电子设备的第一位姿候选集合；基于所述第一位姿候选集合、各所述观测线段、以及与各所述观测线段匹配的参考线段，确定所述电子设备的位姿。

本申请第三方面提供一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；存储器，用于存放计算机程序；处理器，用于执行存储器上所存放的程序时，实现所述位姿确定装置执行的位姿确定方法的步骤。

本申请第四方面提供一种计算机刻度存储介质，所述计算机可读存储介质内存储有计算机程序，所述计算机程序被处理器执行时，实现所述位姿确定装置执行的位姿确定方法的步骤。

通过本申请提供的位姿确定方法，电子设备确定图像对应的至少两条观测线段；基于各所述观测线段以及与各所述观测线段匹配的参考线段，确定第一角度候选集合；基于所述第一角度候选集合和所述电子设备的初始位姿，确定所述电子设备的第一位姿候选集合；基于所述第一位姿候选集合、各所述观测线段、以及与各所述观测线段匹配的参考线段，确定所述电子设备的位姿。将确定位姿的过程拆解成角度确定和位置确定两个部分，基于角度确定的结果进行位姿确定，缩短了位姿求解时长；同时，充分发挥了观测线段的线特征的作用，减少了激光点噪声的约束误差，提升了位姿确定的效率和精确度。

附图说明

图1示出了本申请实施例提供的位姿确定方法的一种可选流程示意图；

图2示出了本申请实施例提供的位姿确定方法的另一种可选流程示意图；

图3示出了示出了本申请实施例提供的确定第一角度候选集合的可选流程示意图；

图4示出了本申请实施例提供的参考线段与观测线段在世界坐标系中的示意图；

图5示出了本申请实施例提供的位姿确定装置的可选结构示意图；

图6示出了本申请实施例提供的电子设备的硬件组成结构示意图。

具体实施方式

为使本申请的目的、特征、优点能够更加的明显和易懂，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而非全部实施例。基于本申请中的实施例，本领域技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

自主定位技术是电子设备实现自主化、智能化的关键技术，用于电子设备自主定位的传感器通常有激光雷达、视觉传感器、全球定位系统（Global Positioning System，GPS）、惯性测量单元（Inertial Measurement Unit，IMU）、轮式里程计等。激光雷达具备测距精度高的特点，被广泛用于仓储、搬运、巡检、服务等移动机器人的自主定位技术中。室内环境中存在较多线特征，比单独的点具备更好的环境描述信息，为了优化定位效果，需要将这些线特征应用于激光定位方案中。

目前，线特征已经被应用于视觉和激光定位方案中，耗时主要体现在提取线特征和非线性优化迭代求解位姿的过程；由于提取线特征必不可少，需要优化后续求解过程的耗时。目前大多实时激光定位方案对位姿初值依赖较大，需要减小对初值的依赖，避免定位结果陷入局部最优。

因此，针对相关技术中定位方法中存在的缺陷，本申请提供一种位姿确定方法，能够克服现有技术的部分或全部缺点。

图1示出了本申请实施例提供的位姿确定方法的一种可选流程示意图，将根据各个步骤进行说明。

步骤S101，电子设备确定图像对应的至少两条观测线段。

在一些可选实施例中，所述电子设备从图像中确定至少两条观测线段。其中，所述图像可以是电子设备实时扫描获取的图像，所述图像中包括激光点数据。所述至少两条观测线段不在同一条直线上。

具体实施时，所述电子设备从所述激光点数据中确定至少两条观测线段。其中，所述观测线段可以是基于所述激光点数据确定的至少两个线段特征。可选的，从激光点数据中确定线段特征可以根据相关技术中已有的方法，本申请不做具体限定。

在一些可选实施例中，所述电子设备可以是涉及室内定位或室外定位的设备，如扫地机器人、快递分拣机器人、巡逻设备、酒店服务机器人、银行服务机器人等，本申请不做具体限定。

步骤S102，基于各所述观测线段以及与各所述观测线段匹配的参考线段，确定第一角度候选集合。

在一些可选实施例中，所述参考线段可以是步骤S101之前，所述电子设备从点云地图或栅格地图中提取出来的。

在一些可选实施例中，所述电子设备确定第一角度候选集合之前，所述方法还可以包括：确定与各所述观测线段匹配的至少一条参考线段。

具体实施时，所述电子设备计算世界坐标系中，各所述观测线段与参考线段之间的距离，确定与各所述观测线段匹配的至少一条参考线段；基于各所述观测线段与所匹配的至少一条参考线段的长度，和/或，各所述观测线段的法向量与所匹配的至少一条参考线段的法向量之间的角度，确定与各所述观测线段匹配的一条参考线段。

可选的，所述电子设备可以计算各所述观测线段和与各所述观测线段所匹配的全部参考线段之间的距离；以观测线段为维度，按照所述距离的大小对每个所述观测线段所匹配的全部参考线段排序；确定所述全部参考线段中满足第一条件、且与所述观测线段的距离最小的参考线段为与所述观测线段匹配的参考线段；

其中，所述第一条件可以包括：参考线段的长度大于或等于所述观测线段，和/或，所述参考线段的法向量与所述观测线段的法向量之间的角度小于第一角度阈值。

例如，针对一条观测线段，分别计算所述观测线段与其对应的至少一条参考线段之间的距离，对计算得到的至少一个距离的值进行排序，确定所述全部参考线段中满足第一条件、且与所述观测线段的距离最小的参考线段为与所述观测线段匹配的参考线段。

在一些可选实施例中，所述电子设备基于各所述观测线段以及与各所述观测线段匹配的参考线段，确定第一角度候选集合。

具体实施时，所述电子设备基于各所述观测线段在所述世界坐标系中与世界坐标系X轴之间的夹角、各所述观测线段匹配的参考线段在所述世界坐标系中与所述世界坐标系X轴之间的夹角，以及各所述观测线段基于所述电子设备的初始位姿在所述世界坐标系中的投影与所述世界坐标系X轴之间的夹角，确定角度残差代价函数；基于所述角度残差代价函数确定角度优化目标函数；求解所述角度优化目标函数，得到所述初始角度修正值；所述电子设备基于所述初始角度修正值、角度修正阈值范围和角度修正步长，确定所述第一角度候选集合。

可选的，记第i对观测线段与参考线段所在的直线与世界坐标系X轴的夹角分别为

和

，

为所述观测线段根据初始位姿

投影于世界坐标系后确定的与X轴的夹角。

在一些可选实施例中，角度残差代价函数

可以通过式（1）确定：

（1）

其中，

为参考线段与观测线段对的数量（观测线段与其对应的参考线段为一对）。在此基础上，角度优化目标函可以是：

（2）

可选的，所述电子设备基于所述电子设备的初始位姿中的初始角度、初始角度修正值、所述初始角度修正值范围和角度步长，确定所述第一角度候选集合中的每一个元素。

具体的，基于所述角度修正值范围和角度步长，对所述初始角度和所述初始角度修正值之和进行加减；例如所述第一角度候选集合中第j个元素=初始角度+初始角度修正值+j*角度步长-0.5*初始角度修正范围；其中，所述初始角度修正范围是关于原点对称的区间。

步骤S103，基于所述第一角度候选集合和所述电子设备的初始位姿，确定所述电子设备的第一位姿候选集合。

在一些可选实施例中，所述电子设备基于所述第一角度候选集合和所述电子设备的初始位姿，确定所述电子设备的第一位姿候选集合。

具体实施时，所述电子设备利用所述第一角度候选集合中的每一个元素替换所述初始位姿中的角度值，得到所述第一位姿候选集合。

步骤S104，基于所述第一位姿候选集合、各所述观测线段、以及与各所述观测线段匹配的参考线段，确定所述电子设备的位姿。

在一些可选实施例中，所述电子设备基于所述各所述观测线段，以及与各所述观测线段匹配的参考线段，确定位姿修正向量；基于所述位姿修正向量，修正所述第一位姿候选集合中的每个元素，得到目标位姿候选集合；从所述目标位姿候选集合中选择一个位姿为所述电子设备的位姿。

具体实施时，所述电子设备基于各所述观测线段的中点位置、所述与各所述观测线段匹配的参考线段的中点位置和各所述参考线段的法向量，确定所述各观测线段与其所匹配的参考线段之间的距离向量；基于所述距离向量确定位置残差代价函数；求解所述位置残差代价函数，得到位姿修正向量。

可选的，第i对观测线段与参考线段中，所述观测线段的拟合线段的中点投影于世界坐标系后的坐标记为

，所述参考线段的中点投影于世界坐标系后的坐标记为

，所述参考线段的法向量为

，所述观测线段与所述参考线段之间的距离向量为：

（3）

其中，

表示向量的转置。

位置残差代价函数可以为：

（4）

其中，表示向量的模。

进一步，位置优化目标函数为：

（5）

其中，

为根据候选位姿

确定的所述观测线段的中点在世界坐标系中投影后的坐标。

具体实施时，所述电子设备还可以利用所述位姿修正向量替换所述第一位姿候选集合中的位姿向量，得到目标位姿候选集合；计算所述目标位姿候选集合中每一个元素对应的位置残差代价和角度残差代价；基于所述位置残差代价、所述角度残差代价和第一权重系数，确定所述目标位姿候选集合中每一个元素的分数；确定分数最大值对应的元素为所述电子设备的位姿。

可选的，目标位姿候选集合

中第j个候选位姿的分数可以通过下式计算：

（6）

其中，β为权重系数（[0,1]之间的任意数），

为第j个候选位姿的位置残差代价，可以通过式（4）确定；为

为第j个候选位姿的角度残差代价，可以通过式（1）确定。

如此，通过本申请实施例提供的位姿确定方法，将位姿确定过程分为角度求解和位置求解，首先确定第一角度候选集合，然后确定上述第一角度候选集合中全部元素对应的位置，进而确定最优的位姿；确定过程中均只涉及维度较低的解析过程，不涉及非线性优化方法迭代求解，缩短了位姿求解时长，提升了位姿确定效率。

图2示出了本申请实施例提供的位姿确定方法的另一种可选流程示意图，将根据各个步骤进行说明。

步骤S201，确定至少一条参考线段。

在一些可选实施例中，电子设备从点云地图或栅格地图中提取至少一个线段特征，所述线段特征为所述参考线段。可选的，所述电子设备可以通过Hough变化、LSD等方法从点云地图或栅格地图中提取至少一个线段特征；或者其他任何能够从点云地图或栅格地图中提取至少一个线段特征的方法，本申请实施例不做具体限定。所述至少两条观测线段不在同一条直线上。

在另一些可选实施例中，所述电子设备提取所述至少一条参考线段之后，将所述至少一条参考线段存储与kd-tree中。可选的，以各参考线段的中点作为元素构建kd-tree；或者其他任何可以构建kd-tree的方法，本申请实施例不做具体限定。

步骤S202，确定至少两条观测线段。

在一些可选实施例中，所述电子设备确定至少两条观测线段的具体流程可以与步骤S101相同，此处不再重复赘述。

在一些可选实施例中，所述电子设备还可以基于所述电子设备的初始位姿，将所述观测线段投影到世界坐标系中。可选的，所述电子设备可以基于所述观测线段的坐标、中点坐标和法向量，将所述观测线段投影到世界坐标系中。

其中，所述电子设备的初始位姿可以基于匀速模型、轮式里程计等常用的方式确定，本申请实施例不做具体限定。

步骤S203，确定与各所述观测线段匹配的至少一条参考线段。

在一些可选实施例中，所述电子设备确定与各所述观测线段匹配的至少一条参考线段。

具体实施时，所述电子设备计算各所述观测线段和与各所述观测线段所匹配的全部参考线段之间的距离；以观测线段为维度，按照所述距离的大小对每个所述观测线段所匹配的全部参考线段排序；确定所述全部参考线段中满足第一条件、且与所述观测线段的距离最小的参考线段为与所述观测线段匹配的参考线段；所述第一条件包括：参考线段的长度大于或等于所述观测线段，和/或，所述参考线段的法向量与所述观测线段的法向量之间的角度小于第一角度阈值。

其中，所述第一角度阈值可以根据实际需求设置，本申请不作具体限定。

步骤S204，基于各所述观测线段以及与各所述观测线段匹配的参考线段，确定第一角度候选集合。

可选的，将第k时刻的电子设备的初始位姿记为

，其中，

为电子设备在世界坐标系下的位置坐标；

为电子设备在世界坐标系中的角度。初始位姿对应的修正数值为

，初始位姿的最优解为

。

步骤S204包括步骤S204a和步骤S204b，图3示出了本申请实施例提供的确定第一角度候选集合的可选流程示意图，将根据各个部分进行说明。

步骤S204a，确定初始角度修正值。

在一些可选实施例中，所述电子设备确定角度残差代价函数；基于所述角度残差代价函数确定角度优化目标函数；求解所述角度优化目标函数，得到所述初始角度修正值。

具体实施时，所述电子设备基于各所述观测线段在所述世界坐标系中与世界坐标系X轴之间的夹角、各所述观测线段匹配的参考线段在所述世界坐标系中与所述世界坐标系X轴之间的夹角，以及各所述观测线段基于所述电子设备的初始位姿在所述世界坐标系中的投影与所述世界坐标系X轴之间的夹角，确定角度残差代价函数。

和

，

为所述观测线段根据初始位姿

投影于世界坐标系后确定的与X轴的夹角。

图4示出了本申请实施例提供的参考线段与观测线段在世界坐标系中的示意图。

在一些可选实施例中，角度残差代价函数

可以通过式（7）确定：

（7）

其中，

（8）

求解所述角度优化目标函数，即式（8），确定所述初始角度修正值为：

（9）

在一些可选实施例中，可以通过二次函数求极限的方式求解所述角度优化目标函数，本申请实施例不做具体限定。

步骤S204b，确定第一角度候选集合。

在一些可选实施例中，所述电子设备基于所述初始角度修正值、角度修正阈值范围和角度修正步长，确定所述第一角度候选集合。基于所述电子设备的初始位姿中的初始角度、所述初始角度修正值范围和角度步长，确定所述第一角度候选集合中的每一个元素；其中，所述第一角度候选集合中元素的数量基于角度候选范围和角度步长确定。

可选的，记角度候选范围为

，角度步长为

，则第一角度候选集合为

。其中，所述第一角度候选集合中的每一个元素可以基于式（10）确定：

（10）

其中，

，

。

可选的，角度候选范围可以根据实际需要设置为关于原点的对称区间[-10°，10°]至[-50°，50°]之间。如[-18°，18°]等，本申请实施例不做具体限定。

步骤S205，基于所述第一角度候选集合和所述电子设备的初始位姿，确定所述电子设备的第一位姿候选集合。

在一些实施例中，所述电子设备利用所述第一角度候选集合中的每一个元素替换所述初始位姿中的角度值，得到所述第一位姿候选集合。

具体实施时，第一位姿候选集合为

，第j个候选位姿为

。

步骤S206，确定目标位姿候选集合。

在一些可选实施例中，所述装置将各所述观测线段基于所述第一位姿候选集合中的每一个元素投影于世界坐标系中。

在一些可选实施例中，所述电子设备基于所述各所述观测线段，以及与各所述观测线段匹配的参考线段，确定位姿修正向量；基于所述位姿修正向量，修正所述第一位姿候选集合中的每个元素，得到目标位姿候选集合。

具体实施时，所述电子设备基于各所述观测线段的中点位置、所述与各所述观测线段匹配的参考线段的中点位置和各所述参考线段的法向量，确定所述各观测线段与其所匹配的参考线段之间的距离向量；基于所述距离向量确定位置残差代价函数；求解所述位置残差代价函数，得到位姿修正向量；利用所述位姿修正向量替换所述第一位姿候选集合中的位姿向量，得到目标位姿候选集合。

，所述参考线段的中点投影于世界坐标系后的坐标记为

，所述参考线段的法向量为

，所述观测线段与所述参考线段之间的距离向量为：

（11）

其中，

表示向量的转置。

位置残差代价函数可以为：

（12）

其中，表示向量的模。

进一步，位置优化目标函数为：

（13）

其中，

为根据候选位姿

确定的所述观测线段的中点在世界坐标系中投影后的坐标。

求解所述位置优化目标函数，得到第j个候选解

的位姿修正量为：

（14）

在一些可选实施例中，可以通过二次函数求极限的方式求解所述位置优化目标函数，本申请实施例不做具体限定。

根据式（14）得到位姿修正量

，修正所述第一位姿候选集合

中的位姿，目标位姿候选集合

的第j个候选位姿为：

（15）

步骤S207，从所述目标位姿候选集合中选择一个位姿为所述电子设备的位姿。

在一些可选实施例中，所述电子设备从所述目标位姿候选集合中选择一个位姿为所述电子设备的位姿。

具体实施时，所述电子设备计算所述目标位姿候选集合中每一个元素对应的位置残差代价和角度残差代价；基于所述位置残差代价、所述角度残差代价和第一权重系数，确定所述目标位姿候选集合中每一个元素的分数；确定分数最大值对应的元素为所述电子设备的位姿。

可选的，目标位姿候选集合

中第j个候选位姿的分数可以通过下式计算：

（16）

其中，β为权重系数（[0,1]之间的任意数），

为第j个候选位姿的位置残差代价，可以通过式（12）确定；为

为第j个候选位姿的角度残差代价，可以通过式（7）确定。

确定分数最大值对应的候选位姿为所述电子设备的位姿。

如此，通过本申请实施例提供的位姿确定方法，具备如下优势：

1、运算耗时少，运算量可控。

解耦位姿求解过程变成角度求解和位置求解两步；计算第一角度候选集合，然后该角度候选解情况下的最优位置解，两个过程的运算均为维度较低的解析解过程，不需要用非线性优化方法迭代求解；可根据设置角度候选解集合相关参数控制运算量（如角度修正阈值范围和角度修正步长）。

2、对匹配初值依赖较小，一定程度上避免了陷入局部最优解。

本申请实施例中，先构建第一角度候选解集合，然后针对第一角度候选集合中每一个角度计算最优位置解，得到候选位姿解集合; 通过构建局部区域的目标位姿候选集合，再遍历目标位姿候选集合中每一个候选解的评估分数筛选出最优位姿解，一定程度上避免了局部最优。

3、相对点与参考线段构建约束求解位姿的方法，本申请实施例求解的位姿误差更小，鲁棒性更高。

在角度约束中，为拟合的观测线段对应的直线与参考线段对应的直线的夹角，减少了激光点噪声造成的约束误差对位姿精度产生的影响；在位置约束中，线段距离约束为观测线段拟合直线段中点到参考线段的距离，不是直接使用观测点来构建约束，减少了激光点噪声造成的约束误差对位姿精度产生的影响；充分发挥了线段特征的作用。

图5示出了本申请实施例提供的位姿确定装置的可选结构示意图，将根据各个部分进行说明。

在一些实施例中，所述位姿确定装置500包括：确定单元501。

所述确定单元501，用于确定图像对应的至少两条观测线段；基于各所述观测线段以及与各所述观测线段匹配的参考线段，确定第一角度候选集合；基于所述第一角度候选集合和所述电子设备的初始位姿，确定所述电子设备的第一位姿候选集合；基于所述第一位姿候选集合、各所述观测线段、以及与各所述观测线段匹配的参考线段，确定所述电子设备的位姿。

在一些实施例中，所述位姿确定装置500还可以包括：处理单元502。

所述处理单元502，用于计算世界坐标系中，各所述观测线段与参考线段之间的距离，确定与各所述观测线段匹配的至少一条参考线段；基于各所述观测线段与所匹配的至少一条参考线段的长度，和/或，各所述观测线段的法向量与所匹配的至少一条参考线段的法向量之间的角度，确定与各所述观测线段匹配的一条参考线段。

所述处理单元502，具体用于计算各所述观测线段和与各所述观测线段所匹配的全部参考线段之间的距离；以观测线段为维度，按照所述距离的大小对每个所述观测线段所匹配的全部参考线段排序；确定所述全部参考线段中满足第一条件、且与所述观测线段的距离最小的参考线段为与所述观测线段匹配的参考线段；

所述确定单元501，具体用于基于各所述观测线段在所述世界坐标系中与世界坐标系X轴之间的夹角、各所述观测线段匹配的参考线段在所述世界坐标系中与所述世界坐标系X轴之间的夹角，以及各所述观测线段基于所述电子设备的初始位姿在所述世界坐标系中的投影与所述世界坐标系X轴之间的夹角，确定角度残差代价函数；基于所述角度残差代价函数确定角度优化目标函数；求解所述角度优化目标函数，得到所述初始角度修正值。

所述确定单元501，具体用于基于所述初始角度修正值、角度修正阈值范围和角度修正步长，确定所述第一角度候选集合。

所述确定单元501，具体用于基于所述电子设备的初始位姿中的初始角度、所述初始角度修正值范围和角度步长，确定所述第一角度候选集合中的每一个元素；其中，所述第一角度候选集合中元素的数量基于角度候选范围和角度步长确定。

所述确定单元501，具体用于利用所述第一角度候选集合中的每一个元素替换所述初始位姿中的角度值，得到所述第一位姿候选集合。

所述确定单元501，具体用于基于所述各所述观测线段，以及与各所述观测线段匹配的参考线段，确定位姿修正向量；基于所述位姿修正向量，修正所述第一位姿候选集合中的每个元素，得到目标位姿候选集合；从所述目标位姿候选集合中选择一个位姿为所述电子设备的位姿。

所述确定单元501，具体用于基于各所述观测线段的中点位置、所述与各所述观测线段匹配的参考线段的中点位置和各所述参考线段的法向量，确定所述各观测线段与其所匹配的参考线段之间的距离向量；基于所述距离向量确定位置残差代价函数；求解所述位置残差代价函数，得到位姿修正向量。

所述确定单元501，具体用于利用所述位姿修正向量修正所述第一位姿候选集合中的位姿向量，得到目标位姿候选集合；计算所述目标位姿候选集合中每一个元素对应的位置残差代价和角度残差代价；基于所述位置残差代价、所述角度残差代价和第一权重系数，确定所述目标位姿候选集合中每一个元素的分数；确定分数最大值对应的元素为所述电子设备的位姿。

图6是本申请实施例提供的电子设备的硬件组成结构示意图，电子设备700包括：至少一个处理器701、存储器702和至少一个网络接口704。电子设备700中的各个组件通过总线系统705耦合在一起。可理解，总线系统705用于实现这些组件之间的连接通信。总线系统705除包括数据总线之外，还包括电源总线、控制总线和状态信号总线。但是为了清楚说明起见，在图6中将各种总线都标为总线系统705。

可以理解，存储器702可以是易失性存储器或非易失性存储器，也可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是ROM、可编程只读存储器（PROM，Programmable Read-Only Memory）、可擦除可编程只读存储器（EPROM，ErasableProgrammable Read-Only Memory）、电可擦除可编程只读存储器（EEPROM，ElectricallyErasable Programmable Read-Only Memory）、磁性随机存取存储器（FRAM，ferromagneticrandom access memory）、快闪存储器（Flash Memory）、磁表面存储器、光盘、或只读光盘（CD-ROM，Compact Disc Read-Only Memory）；磁表面存储器可以是磁盘存储器或磁带存储器。易失性存储器可以是随机存取存储器（RAM，Random Access Memory），其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的RAM可用，例如静态随机存取存储器（SRAM，Static Random Access Memory）、同步静态随机存取存储器（SSRAM，SynchronousStatic Random Access Memory）、动态随机存取存储器（DRAM，Dynamic Random AccessMemory）、同步动态随机存取存储器（SDRAM，Synchronous Dynamic Random AccessMemory）、双倍数据速率同步动态随机存取存储器（DDRSDRAM，Double Data RateSynchronous Dynamic Random Access Memory）、增强型同步动态随机存取存储器（ESDRAM，Enhanced Synchronous Dynamic Random Access Memory）、同步连接动态随机存取存储器（SLDRAM，SyncLink Dynamic Random Access Memory）、直接内存总线随机存取存储器（DRRAM，Direct Rambus Random Access Memory）。本申请实施例描述的存储器702旨在包括但不限于这些和任意其它适合类型的存储器。

本申请实施例中的存储器702用于存储各种类型的数据以支持电子设备700的操作。这些数据的示例包括：用于在电子设备700上操作的任何计算机程序，如应用程序722。实现本申请实施例方法的程序可以包含在应用程序722中。

所述本申请实施例揭示的方法可以应用于处理器701中，或者由处理器701实现。处理器701可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，所述方法的各步骤可以通过处理器701中的硬件的集成逻辑电路或者软件形式的指令完成。所述的处理器701可以是通用处理器、数字信号处理器（DSP，Digital Signal Processor），或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。处理器701可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者任何常规的处理器等。结合本申请实施例所公开的方法的步骤，可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于存储介质中，该存储介质位于存储器702，处理器701读取存储器702中的信息，结合其硬件完成前述方法的步骤。

在示例性实施例中，电子设备700可以被一个或多个应用专用集成电路（ASIC，Application Specific Integrated Circuit）、DSP、可编程逻辑器件（PLD，ProgrammableLogic Device）、复杂可编程逻辑器件（CPLD，Complex Programmable Logic Device）、FPGA、通用处理器、控制器、MCU、MPU、或其他电子元件实现，用于执行前述方法。

除了上述方法和设备以外，本申请的实施例还可以是计算机程序产品，其包括计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的方法中的步骤。

所述计算机程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本申请实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言，诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。

此外，本申请的实施例还可以是计算机可读存储介质，其上存储有计算机程序指令，所述计算机程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本申请各种实施例的方法中的步骤。

所述计算机可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子（非穷举的列表）包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器（RAM）、只读存储器（ROM）、可擦式可编程只读存储器（EPROM或闪存）、光纤、便携式紧凑盘只读存储器（CD-ROM）、光存储器件、磁存储器件、或者上述的任意合适的组合。

以上结合具体实施例描述了本申请的基本原理，但是，需要指出的是，在本申请中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本申请的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本申请为必须采用上述具体的细节来实现。

本申请中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“如但不限于”，且可与其互换使用。

还需要指出的是，在本申请的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本申请的等效方案。

提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本申请。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本申请的范围。因此，本申请不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。

为了例示和描述的目的已经给出了以上描述。此外，此描述不意图将本申请的实施例限制到在此公开的形式。尽管以上已经讨论了多个示例方面和实施例，但是本领域技术人员将认识到其某些变型、修改、改变、添加和子组合。

Claims

1.一种位姿确定方法，其特征在于，所述方法包括：

电子设备确定图像对应的至少两条观测线段；

基于所述第一位姿候选集合、各所述观测线段、以及与各所述观测线段匹配的参考线段，确定所述电子设备的位姿；

所述基于各所述观测线段以及与各所述观测线段匹配的参考线段，确定第一角度候选集合，包括：

基于各所述观测线段在世界坐标系中与世界坐标系X轴之间的夹角、各所述观测线段匹配的参考线段在所述世界坐标系中与所述世界坐标系X轴之间的夹角，以及各所述观测线段基于所述电子设备的初始位姿在所述世界坐标系中的投影与所述世界坐标系X轴之间的夹角，确定角度残差代价函数；

基于所述角度残差代价函数确定角度优化目标函数；求解所述角度优化目标函数，得到初始角度修正值；基于所述初始角度修正值、角度修正阈值范围和角度修正步长，确定所述第一角度候选集合。

2.根据权利要求1所述的方法，其特征在于，所述基于所述初始角度修正值、角度修正阈值范围和角度修正步长，确定所述第一角度候选集合，包括：

基于所述电子设备的初始位姿中的初始角度、所述初始角度修正值、所述初始角度修正值范围和角度步长，确定所述第一角度候选集合中的每一个元素；

3.根据权利要求1所述的方法，其特征在于，所述基于所述第一角度候选集合和所述电子设备的初始位姿，确定所述电子设备的第一位姿候选集合，包括：

4.根据权利要求1所述的方法，其特征在于，所述基于所述第一位姿候选集合、各所述观测线段、以及与各所述观测线段匹配的参考线段，确定所述电子设备的位姿，包括：

基于所述位姿修正向量，修正所述第一位姿候选集合中的每个元素，得到目标位姿候选集合；

5.根据权利要求4所述的方法，其特征在于，所述基于各所述观测线段，以及与各所述观测线段匹配的参考线段，确定位姿修正向量，包括：

基于各所述观测线段的中点位置、所述与各所述观测线段匹配的参考线段的中点位置和各所述参考线段的法向量，确定各所述观测线段与其所匹配的参考线段之间的距离向量；

基于所述距离向量确定位置残差代价函数；

求解所述位置残差代价函数，得到位姿修正向量。

6.根据权利要求4所述的方法，其特征在于，所述从所述目标位姿候选集合中选择一个位姿为所述电子设备的位姿，包括：

利用所述位姿修正向量修正所述第一位姿候选集合中的位姿向量，得到目标位姿候选集合；

确定分数最大值对应的元素为所述电子设备的位姿。

7.一种位姿确定装置，其特征在于，所述装置包括：

确定单元，用于确定图像对应的至少两条观测线段；基于各所述观测线段以及与各所述观测线段匹配的参考线段，确定第一角度候选集合；基于所述第一角度候选集合和电子设备的初始位姿，确定所述电子设备的第一位姿候选集合；基于所述第一位姿候选集合、各所述观测线段、以及与各所述观测线段匹配的参考线段，确定所述电子设备的位姿；

所述确定单元，具体用于基于各所述观测线段在世界坐标系中与世界坐标系X轴之间的夹角、各所述观测线段匹配的参考线段在所述世界坐标系中与所述世界坐标系X轴之间的夹角，以及各所述观测线段基于所述电子设备的初始位姿在所述世界坐标系中的投影与所述世界坐标系X轴之间的夹角，确定角度残差代价函数；基于所述角度残差代价函数确定角度优化目标函数；求解所述角度优化目标函数，得到初始角度修正值；基于所述初始角度修正值、角度修正阈值范围和角度修正步长，确定所述第一角度候选集合。

8.一种电子设备，其特征在于，包括处理器、通信接口、存储器和通信总线，其中，处理器，通信接口，存储器通过通信总线完成相互间的通信；存储器，用于存放计算机程序；处理器，用于执行存储器上所存放的程序时，实现权利要求1-6任一项所述的方法步骤。

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