一种基于位姿图优化的顶标地图构建方法专利检索-机器人技术人工智能人工智能专利检索查询-专利查询网

一种基于位姿图优化的顶标地图构建方法

阅读：998发布：2024-02-16

专利汇可以提供一种基于位姿图优化的顶标地图构建方法专利检索，专利查询，专利分析的服务。并且本发明适用于机器人定位技术领域，提供了一种基于位姿图优化的顶标地图构建方法，该方法包括如下步骤：S1、基于拍摄图像构建路标坐标系的位姿图；S2、基于位姿图的优化来计算各路标坐标系在地图坐标系中的位姿；S3、基于各路标坐标系在地图坐标系中的位姿计算各路标点在地图坐标系中的坐标。本发明实施例提供的基于位姿图优化的顶标地图构建方法，将不同顶标中存在的直角坐标系当做是同一个直角坐标系的位姿变换，利用位姿图优化来计算路标点的坐标，相对于基于相机姿态构建的位姿图而言，能够大幅提高基于顶标的移动机器人室内地图构建精度，且计算简单，结果更为稳定。，下面是一种基于位姿图优化的顶标地图构建方法专利的具体信息内容。

权利要求

1.一种基于位姿图优化的顶标地图构建方法，其特征在于，所述方法包括如下步骤：
S1、基于拍摄图像构建路标坐标系的位姿图；
S2、基于位姿图的优化来计算各路标坐标系在地图坐标系中的位姿；
S3、基于各路标坐标系在地图坐标系中的位姿计算各路标点在地图坐标系中的坐标。
2.如权利要求1所述的基于位姿图优化的顶标地图构建方法，其特征在于，所述步骤S1包括如下步骤：
S11、提取拍摄图像中的所有路标；
S12、判断当前关键帧集合是否为空集，若判断结果为否，则执行步骤S13，若判断结果为是，则识别当前图像中的路标集合中是否存在初始路标，若存在，则将初始路标设置为初始帧，并保存至关键帧集合中，进入步骤S13，若不存在，则执行步骤S11，S13、识别图像中的已知路标和未知路标，遍历图像中的所有已知路标，判断当前已知路标与图像中的其它已知路标是否已经建立关联，若判断结果为否，则建立当前已知路标与图像中其它已知路标间的连接关系，遍历图像中的所有未知路标，建立当前未知路标与图像中所有已知路标的连接关系，并将当前未知路标保存为一个关键帧；所述已知路标是指已被纳入到位姿图中的路标；未知路标是指未纳入到位姿图中的路标；
S14、遍历地图中的所有路径，建立路标运动的位姿图。
3.如权利要求2所述的基于位姿图优化的顶标地图构建方法，其特征在于，路标点间连接关系的建立方法包括如下步骤：
基于路标n中路标点的图像坐标和在当前路标坐标系中的世界坐标，计算相机在路标n的路标坐标系下的姿态Rn、tn；
基于路标m中路标点的图像坐标和在当前路标坐标系中的世界坐标，计算相机在路标m的路标坐标系下的姿态Rm、tm；
从路标坐标系n到路标坐标系m的旋转矩阵为平移向量为
从路标坐标系n到路标坐标系m的位姿变换为(tnm(0),tnm(1),atan2(Rnm
(1,0),Rnm(0,0))。
4.如权利要求3所述的基于位姿图优化的顶标地图构建方法，其特征在于，相机在路标s坐标系下姿态的计算方法如下：
利用仿射变换方程计算出旋转矩阵及平移向量再去除摄影深度因子
得到旋转矩阵R、平移向量t，其中， xi为路标s中已知路标点的图像坐标xi，Xw为路标s中已知路标点的世界坐标，Mcam相机的内参矩阵。
5.如权利要求1所述的基于位姿图优化的顶标地图构建方法，其特征在于，基于公式Xw＝R*X′w+t来计算各路标点在地图坐标系中的坐标，其中，R为当前路标坐标系的旋转矩阵，t为当前路标坐标系的平移向量，X′w为路标点在路标坐标系中的坐标。

说明书全文

一种基于位姿图优化的顶标地图构建方法

技术领域

[0001] 本发明属于机器人定位技术领域，提供了一种基于位姿图优化的定标地图构建方法。

背景技术

[0002] 随着社会的发展和技术的进步，移动机器人越来越深的介入到人类的日常生活中，例如家庭中的清洁机器人、工厂中的搬运机器人以及餐馆中的送餐机器人等。移动机器人想要实现上述所有功能就必须要准确的知道自身所在的位置，而机器人能够实时定位的先决条件是建立地图，这是机器人导航和其它智能行为的关键。移动机器人建图和定位常用的传感器有摄像机和激光雷达等。其中激光雷达硬件成本较高，不利于移动机器人的大范围普及推广。而利用摄像机进行定位硬件成本低、定位精度高，在室内定位中，基于视觉的定位方法被广泛的应用。基于视觉的室内定位首先要构建精准的室内地图，用于绝对坐标系下摄像机姿态的计算以及机器人移动路径的规划。可以通过人工路标构建精确的几何地图，也可以利用环境地标来构建地图。其中基于环境地标的方法虽然普适性较好，不需要人工布设额外的标志，但是计算复杂，实用性较差。人工路标往往在颜色、形状等视觉特征上具有明显的唯一性，通过计算机视觉方法能够很容易的将人工路标识别出来。其中有一种在屋顶布设人工路标的方法，屋顶环境单一易于提取，且摄像机的视野不容易受到干扰，在室内定位中被广泛应用。

[0003] 但是现有的顶标地图构建方法是根据已知路标依次计算未知路标在地图中的位置，计算过程中的误差会累积向后传播，在顶标数目较多时地图构建结果不准确，导致定位失败。

发明内容

[0004] 本发明实施例提供一种基于位姿图优化的定标地图构建方法，旨在解决现有顶标地图构建方法在基于已知路标依次计算未知路标在地图中的位置时，计算过程中的误差会累积向后传播，在顶标数目较多时会导致地图构建结果不准确的问题。

[0005] 本发明是这样实现的，一种基于位姿图优化的定标地图构建方法，该方法包括如下步骤：

[0006] S1、基于拍摄图像构建路标坐标系的位姿图；

[0007] S2、基于位姿图的优化来计算各路标坐标系在地图坐标系中的位姿；

[0008] S3、基于各路标坐标系在地图坐标系中的位姿计算各路标点在地图坐标系中的坐标。

[0009] 进一步的，所述步骤S1包括如下步骤：

[0010] S11、提取拍摄图像中的所有路标；

[0011] S12、判断当前关键帧集合是否为空集，若判断结果为否，则执行步骤S13，若判断结果为是，则识别当前图像中的路标集合中是否存在初始路标，若存在，则将初始路标设置为初始帧，并保存至关键帧集合中，进入步骤S13，若不存在，则执行步骤S11，[0012] S13、识别图像中的已知路标和未知路标，遍历图像中的所有已知路标，判断当前已知路标与图像中的其它已知路标是否已经建立关联，若判断结果为否，则建立当前已知路标与图像中其它已知路标间的连接关系，遍历图像中的所有未知路标，建立当前未知路标与图像中所有已知路标的连接关系，并将当前未知路标保存为一个关键帧；所述已知路标是指已被纳入到位姿图中的路标；未知路标是指未纳入到位姿图中的路标；

[0013] S14、遍历地图中的所有路径，建立路标运动的位姿图。

[0014] 进一步的，路标点间连接关系的建立方法包括如下步骤：

[0015] 基于路标n中路标点的图像坐标和在当前路标坐标系中的世界坐标，计算相机在路标n的路标坐标系下的姿态Rn、tn；

[0016] 基于路标m中路标点的图像坐标和在当前路标坐标系中的世界坐标，计算相机在路标m的路标坐标系下的姿态Rm、tm；

[0017] 从路标坐标系n到路标坐标系m的旋转矩阵为平移向量为从路标坐标系n到路标坐标系m的位姿变换为(tnm(0),tnm(1),atan2(Rnm
(1,0),Rnm(0,0))。

[0018] 进一步的，相机在路标s坐标系下姿态的计算方法如下：

[0019] 利用仿射变换方程计算出旋转矩阵及平移向量再去除摄影深度因子得到旋转矩阵R、平移向量t，其中， xi为路标s中已知路标点的图像坐标xi，Xw为路标s中已知路标点的世界坐标，Mcam相机的内参矩阵。

[0020] 进一步的，基于公式Xw＝R*X′w+t来计算各路标点在地图坐标系中的坐标，其中，R为当前路标坐标系的旋转矩阵，t为当前路标坐标系的平移向量，X′w为路标点在路标坐标系中的坐标。

[0021] 本发明实施例提供的基于位姿图优化的顶标地图构建方法，将不同顶标中存在的直角坐标系当做是同一个直角坐标系的位姿变换，利用位姿图优化来计算路标点的坐标，相对于基于相机姿态构建的位姿图而言，能够大幅提高基于顶标的移动机器人室内地图构建精度，且计算简单，结果更为稳定。附图说明

[0022] 图1为本发明实施例提供的基于位姿图优化的定标地图构建方法的流程图。

具体实施方式

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

[0024] 图1为本发明实施例提供的基于位姿图优化的定标地图构建方法的流程图，该方法包括如下步骤：

[0025] S1、基于拍摄图像构建路标坐标系的位姿图；

[0026] 在本发明实施例中，路标设于机器人运动区域的屋顶，摄像机平行于屋顶设置，用于拍摄屋顶的路标；

[0027] 在本发明步骤S1具体包括如下步骤：

[0028] S11、利用图像提取算法来提取拍摄图像中的所有路标；

[0029] S12、判断当前关键帧集合是否为空集，若判断结果为否，则执行步骤S13，若判断结果为是，则识别当前图像中的路标集合中是否存在初始路标，若存在，则将初始路标设置为初始帧，并保存至关键帧集合中，进入步骤S13，若不存在，则执行步骤S11，[0030] S13、识别图像中的已知路标和未知路标，遍历图像中的所有已知路标，判断当前已知路标与图像中的其它已知路标是否已经建立关联，若判断结果为否，则建立当前已知路标与图像中其它已知路标间的连接关系，遍历图像中的所有未知路标，建立当前未知路标与图像中所有已知路标的连接关系，并将当前未知路标保存为一个关键帧，已知路标是指已被纳入到位姿图中的路标，未知路标是指未纳入到位姿图中的路标。

[0031] 在本发明实施例中，路标点间连接关系的建立方法具体如下：

[0032] 基于路标n中路标点的图像坐标和世界坐标，计算相机在路标n的路标坐标系下的姿态Rn、tn；

[0033] 基于路标m中路标点的图像坐标和世界坐标，计算相机在路标m的路标坐标系下的姿态Rm、tm；

[0034] 从路标坐标系n到路标坐标系m的旋转矩阵为平移向量为从路标坐标系n到路标坐标系m的位姿变换为(tnm(0),tnm(1),atan2(Rnm
(1,0),Rnm(0,0))。

[0035] 在本发明实施例中，相机在路标s坐标系下姿态的计算方法如下：

[0036] 利用仿射变换方程计算出旋转矩阵及平移向量再去除摄影深度因子得到旋转矩阵R、平移向量t，其中， xi为路标s中已知路标点的图像坐标xi，Xw为路标s中已知路标点的世界坐标，Mcam相机的内参矩阵。

[0037] S14、遍历地图中的所有路径，建立路标运动的位姿图。

[0038] 在本发明实施例中，地图是针对地图坐标系而言的，地图坐标系中纳入所有路标，一般选用某个路标坐标系作为地图坐标系。

[0039] S2、基于位姿图的优化来计算各路标坐标系在地图坐标系中的位姿；

[0040] 在本发明实施例中，将位姿模型图用方程描述，方程表示如下：

[0041]

[0042] 其中，xk为第k个节点位置信息，k个节点即为第k个路标坐标系的原点；zk为第k个节点观测得到的位置信息；ek为xk与zk之间的误差；Ω为信息矩阵，是协方差矩阵的逆；

[0043] 误差项F(x)表示如下：

[0044]

[0045] 对第k条边的ek(xk)进行一阶泰勒展开：

[0046]

[0047] 上述的Jk为ek关于xk的导数，矩阵形式为下雅克比阵，对第k条边的目标函数进一步展开有：

[0048] Fk(xk+Δx)＝ek(xk+Δx)TΩkek(xk+Δx)

[0049] ≈(ek+JkΔx)TΩk(ek+JkΔx)

[0050] ≈Ck+2bkΔx+ΔxTHkΔx

[0051] 其中Ck为常数项，2bk为一次项系数，Hk为二次项系数，则目标函数的Fk改变的值为ΔFk＝2bkΔx+ΔxTHkΔx

[0052] 令

[0053]

[0054] 于是该问题转变成一个线性方程的求解：HkΔx＝-bk；

[0055] 解得全局最优的x*＝x+Δx，并最为初始值代入F(x)进行迭代计算，最后计算得到每个关键帧的位姿，即各路标坐标系在地图坐标系中的位姿。

[0056] S3、基于各路标坐标系在地图坐标系中的位姿计算各路标点在地图坐标系中的坐标。

[0057] 在本发明实施例中，基于公式Xw＝R*X′w+t来计算各路标点在地图坐标系中的坐标，其中，R为当前路标坐标系的旋转矩阵，t为当前路标坐标系的平移向量，X′w为路标点在路标坐标系中的坐标。

[0058] 本发明实施例提供的基于位姿图优化的顶标地图构建方法，将不同顶标中存在的直角坐标系当做是同一个直角坐标系的位姿变换，利用位姿图优化来计算路标点的坐标，相对于基于相机姿态构建的位姿图而言，能够大幅提高基于顶标的移动机器人室内地图构建精度，且计算简单，结果更为稳定。

[0059] 以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

标题	发布/更新时间	阅读量
学习型机器人动作数据采集方法	2021-07-05	1
一种先桩法海上风电导管架基础吊装水下辅助定位装置及安装方法	2020-06-29	2
一种履带式机器人性能试验台	2021-12-03	1
心理咨询智能机器人	2020-12-16	0
激光导航器及外科手术机器人	2020-05-13	2
投递装置、分拣场地及分拣系统	2020-08-10	0
一种自动化上下料设备	2022-05-20	2
红外缓冲装配座及智能扫地机器人	2023-11-14	0
自动化物流分拣设备	2021-02-20	2
一种水下清洁机器人充电装置	2022-06-30	2

一种基于位姿图优化的顶标地图构建方法

一种基于位姿图优化的顶标地图构建方法

技术领域

背景技术

发明内容

具体实施方式

该功能需要专业版企业版VIP权限，您可以：