CN108225327B - 一种顶标地图的构建与定位方法 - Google Patents

Abstract

本发明揭示了一种顶标地图的构建与定位方法，包括以下步骤：步骤1、在地图区域设置初始路标，将初始路标的坐标系设置为地图坐标系；步骤2、位姿图的构建：通过图像进行顶标提取，并建立节点，针对所建立节点建立关联帧，顺序提取关联帧集合中的每一帧，通过计算建立位姿边，并通过遍历地图中的所有路径建立位姿图模型；步骤3、位姿图优化：根据位姿图模型计算得到每个关键帧的位姿；步骤4、顶标坐标计算：根据每个关键帧的位姿计算顶标中的点在地图坐标系中坐标。本发明要解决的是移动机器人在顶标传感器的引导下，能够建立一个精确的顶标地图，并能根据该地图准确的计算出相机位置的问题。

Description

一种顶标地图的构建与定位方法

技术领域

本发明涉及室内地图构建与定位技术，特别是涉及一种基于图优化的顶标地图构建与移动机器人定位技术。

背景技术

随着社会的发展和技术的进步，移动机器人越来越深的介入到人类的日常生活中，例如家庭中的清洁机器人、工厂中的搬运机器人以及餐馆中的送餐机器人等。移动机器人想要实现上述所有功能就必须要准确的知道自身所在的位置，即实时定位。而实现机器人实时定位的先决条件是建立地图，这是实现机器人导航和其它智能行为的关键。

移动机器人建图常用的传感器有摄像机和激光雷达等。其中激光雷达硬件成本较高，不利于移动机器人的大范围普及推广。而利用摄像机进行定位硬件成本低、定位精度高，在室内定位中，基于视觉的定位方法被广泛的应用。

基于视觉的室内定位首先要构建精准的室内地图，用于绝对坐标系下的摄像机姿态的计算以及机器人移动路径的规划。可以通过人工路标构建精确的几何地图，也可以利用环境地标来构建地图。

其中基于环境地标的方法虽然普适性较好，不需要人工布设额外的标志，但是计算复杂，实用性较差。人工路标往往在颜色、形状等视觉特征上具有明显的唯一性，通过计算机视觉方法能够很容易的将人工路标识别出来。

其中有一种在屋顶布设人工路标的方法，屋顶环境单一易于提取，且摄像机的视野不容易受到干扰，在室内定位中被广泛应用。

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

例如公开号为CN 101669144 A，专利名称为《用于移动式机器人的位置确定的路标和使用该路标的设备与方法》的公开文献，采用的是顶标建立室内地图和定位技术，该方案使用安装在相机上的两轴倾斜仪计算相机相对于重力方向的横摇角和俯仰角，再将两轴倾斜信息带入相机成像模型计算相机的姿态和位置。地图构建方法上，该方案直接以已知路标为基准计算未知路标点在地图中的坐标。

例如公开号为CN 102419178 B，专利名称为《基于红外路标的移动机器人定位系统和方法》的公开文献，采用的也是顶标构建地图和定位技术，该方案需要计算初始单应矩阵，并将后续相机的移动虚拟成路标的运动，从而反推出相机的位置。在建图方法上，该方案直接以已知路标为基准计算未知路标点在地图中的坐标。

例如公开号为CN 107180215 A，专利号为《基于库位和二维码的停车场自动建图与高精度定位方法》的公开文献，使用的路标进行定位，并使用图优化模型建立室内地图。在定位方法上该方案直接采用相机成像模型计算相机的位姿。

上述三个与本本发明解决相同技术问题的专利公开文献，同样存在一些问题，其系统(方法)在运行过程中仍会出现定位结果错误的缺陷，难以保证定位精度。

尤其《基于红外路标的移动机器人定位系统和方法》，该方案将路标张贴于屋顶，摄像机平行于屋顶拍摄。在定位方法上，该方案需要计算初始单应矩阵，并将后续相机的移动虚拟成路标的运动，从而反推出相机的位置。在地图构建上，该方案直接以已知路标为基准计算相机位姿，再利用相机位姿计算视野内的未知路标点的坐标。该技术方案存在以下问题：

1)在定位方法上，初始单应矩阵的计算需要3个以上的共面点，这些点在地图中的坐标必须是已知的，且这些点需要在图像中均匀分布。由于通常使用的单个顶标的尺寸较小所以无法直接拿来使用，就需要额外的布设已知点并和路标地图构建联系，该过程过于复杂且存在较多误差；此外，当标定好初始单应矩阵后，相机到屋顶的距离不能发生改变，而移动机器人在运动中不免会发生上下颠簸，某些应用场合中顶标的张贴也不可能一直保持在同一个平面上，这些原因都会导致定位结果的错误。

2)在地图构建方法上，直接以已知路标为基准计算未知路标点在地图中的坐标，会将已知点中的计算误差累积到待计算的路标点上，当路标的数目较多时，靠后的路标点的坐标误差会非常大，导致地图构建的失败。

发明内容

本发明所要解决的技术问题是实现一种基于图优化的顶标地图构建和移动机器人定位方法，能够大幅提高移动机器人室内地图构建和定位的精度。

为了实现上述目的，本发明采用的技术方案为：一种顶标地图的构建与定位方法，包括以下步骤：

步骤1、在地图区域设置初始路标，将初始路标的坐标系设置为地图坐标系；

步骤2、位姿图的构建：通过图像进行顶标提取，并建立节点，针对所建立节点建立关联帧，顺序提取关联帧集合中的每一帧，通过计算建立位姿边，并通过遍历地图中的所有路径建立位姿图模型；

步骤3、位姿图优化：根据位姿图模型计算得到每个关键帧的位姿；

步骤4、顶标坐标计算：根据每个关键帧的位姿计算顶标中的点在地图坐标系中坐标。

所述步骤2包括以下步骤：

1)顶标提取：利用图像处理算法提取图像中所有顶标，将该图像中的顶标集合作为当前帧；

2)建立节点：将当前帧与关键帧集合做对比，若当前帧的顶标集合与任一关键帧的顶标集合完全一样，则放弃该帧返回1)；否则将当前帧保存为关键帧，当前帧的位姿为一个新的节点，并进入步骤3)；

3)建立关联帧：若当前帧的顶标集合与关键帧集合中某一帧的顶标集合不完全一样，但存在相同的路标，则将该帧作为当前帧的关联帧，保存关键帧集合中当前帧的所有关联帧；

4)建立位姿边：顺序提取关联帧集合中的每一帧，计算当前帧与关联帧之间的位姿变换矩阵，已知当前帧中的路标L₁与关联帧中的路标L₂的ID相同，L₁、L₂中的像点坐标为

两个路标点在路标坐标系下的坐标相同为X^w，相机的内参矩阵M_C，令当前帧的旋转矩阵为单位矩阵I，平移向量为零向量0，根据后述根据位姿计算路标点坐标的方法，计算关键帧的路标点在该位姿情况下的坐标

根据

和

利用定位方法计算得到关联帧的位姿在到当前帧位姿的变换矩阵T，建立了位姿边；

5)重复1)-4)遍历地图中的所有路径，建立相机运动的位姿图模型。

所述定位方法包括以下步骤：

1)拍摄图像并利用图像处理方法提取图像中所有的路标点；

2)将提取到的路标与地图中的路标进行对比，得到路标点的图像坐标x_i和地图坐标X_w；

3)令

根据

和地图坐标X_w和相机的内参矩阵M_cam，利用仿射变换方程

计算出旋转矩阵

平移向量

再去除摄影深度因子的影响得到旋转矩阵R、平移向量t；

4)根据旋转矩阵R、平移向量t求得相机中心的位置为X＝-R^-1*t，相机的旋转角为angle＝atan(R_(1，0)/R_(0，0))。

所述步骤3包括以下步骤：

位姿图模型建成了一个带有n条边的图，位姿图模型的方程描述为：

其中，x_k为图模型中第k个节点位置信息；z_k为对图模型中第k个节点观测得到的位置信息；e_k为x_k与z_k之间的误差；Ω为信息矩阵，是协方差矩阵的逆；

z_k为已知项，则误差项：

对第k条边的e_k(x_k)进行一阶泰勒展开：

上述的J_k为e_k关于x_k的导数，矩阵形式为下雅克比阵，对第k条边的目标函数进一步展开有：

F_k(x_k+Δx)＝e_k(x_k+Δx)^TΩ_ke_k(x_k+Δx)

≈(e_k+J_kΔx)^TΩ_k(e_k+J_kΔx)

≈C_k+2b_kΔx+Δx^TH_kΔx

其中C_k为常数项，2b_k为一次项系数，H_k为二次项系数。则目标函数的F_k改变的值为ΔF_k＝2b_kΔx+Δx^TH_kΔx；

令

根据H_kΔx＝-b_k方程的求解：

解得全局最优的x^*＝x+Δx，并作为初始值代入F(x)进行迭代计算，最后计算得到每个关键帧的位姿。

所述步骤4包括以下步骤：

根据每个关键帧的位姿计算顶标中的点在地图坐标系中坐标，已知关键帧的旋转矩阵rMat与平移向量tMat，相机的内参矩阵M_C，路标点在关键帧上的图像坐标x_i,路标点在路标坐标系中的坐标x_w，计算过程如下：

根据x_i和x_w计算通过前述定位方法计算图像在当前路标坐标系中的位姿rMatU，tMatU；

路标坐标系到地图坐标系的转换矩阵为R＝rMatU^-1*rMat，t＝rMatU^-1*(tMat-rMatU)；

路标点在地图坐标系中的坐标计算x_map＝R*x_w+t。

地图区域内需要定位的装置为机器人，所述标记点为贴在机器人需要运动的区域屋顶的顶标贴。

本发明能够通过移动机器人在顶标传感器的引导下，建立一个精确的顶标地图，并能根据该地图准确的计算出相机位置，能够在不需要测角仪和提前标定相机姿态的情况下可实时、准确的计算出相机的位置，通过顶标构建位姿图方法简单、结果准确，数据量小，对位姿图进行图优化之后得到的顶标地图精度高，稳定性好。

附图说明

下面对本发明说明书中每幅附图表达的内容作简要说明：

图1为位姿图模型示意图。

具体实施方式

本发明实施时需要将顶标贴于运动装置(下面以机器人为例说明)需要运动的区域的屋顶，相机平行于屋顶进行拍摄。由于实施时需要相机平行于屋顶拍摄，但是在实际应用过程中，相机无法始终保持与屋顶的绝对平行，这样计算结果中会存在一定的误差。可以考虑安装倾角仪对定位结果作一定的角度补偿，或者可以对定位结果进行滤波处理，如卡尔曼滤波等，能够改善由于相机与屋顶不平行而带来的定位结果的偏差。

基于图优化的顶标地图构建与定位技术主要分为地图构建与机器人定位两个部分，首先定义以下几个概念：

路标点：路标中的用于定位的点；

路标坐标系：单个路标内的路标点构成的坐标系；

地图坐标系：全局地图使用的坐标系统，一般选用某个路标坐标系作为地图坐标系；

已知顶标：顶标中所有的点在地图坐标系中的位置都已知的顶标；

未知顶标：顶标中所有的点在地图坐标系中的位置都未知的顶标；

机器人构建地图和定位之前需要对搭载的摄像头进行标定，得到内参矩阵M_C。

机器人定位就是根据图像中的已知顶标计算相机中心在地图坐标系中的位置以及相机姿态的过程，机器人定位的过程如下：

拍摄图像并利用图像处理方法提取图像中所有的路标点；

将提取到的路标与地图中的路标进行对比，得到路标点的图像坐标x_i和地图坐标X_w；

令

根据

和地图坐标X_w和相机的内参矩阵M_cam，利用仿射变换方程

计算出旋转矩阵

平移向量

再去除摄影深度因子的影响得到旋转矩阵R、平移向量t；

根据旋转矩阵R、平移向量t求得相机中心的位置为X＝-R^-1*t，相机的旋转角为angle＝atan(R_(1,0)/R_(0,0))。

构建顶标地图就是将室内所有的顶标纳入同一个坐标系统中的过程。顶标地图的构建过程分为位姿图的构建、位姿图优化和顶标坐标计算三个过程，建地图之前需要设置初始路标，将初始路标的坐标系设置为地图坐标系。

位姿图的构建过程如下：

1)顶标提取：利用图像处理算法提取图像中所有顶标，将该图像中的顶标集合作为当前帧；

2)建立节点：将当前帧与关键帧集合做对比，若当前帧的顶标集合与任一关键帧的顶标集合完全一样，则放弃该帧返回1)；否则将当前帧保存为关键帧，当前帧的位姿为一个新的节点，并进入步骤3)；

3)建立关联帧：若当前帧的顶标集合与关键帧集合中某一帧的顶标集合不完全一样，但存在相同的路标，则将该帧作为当前帧的关联帧，保存关键帧集合中当前帧的所有关联帧。

4)建立位姿边：顺序提取关联帧集合中的每一帧，计算当前帧与关联帧之间的位姿变换矩阵，已知当前帧中的路标L₁与关联帧中的路标L₂的ID相同，L₁、L₂中的像点坐标为

两个路标点在路标坐标系下的坐标相同为X^w，相机的内参矩阵M_C。令当前帧的旋转矩阵为单位矩阵I，平移向量为零向量0，根据后述根据位姿计算路标点坐标的方法，计算路标点在该位姿情况下的坐标

根据

和

利用前述定位方法计算得到关联帧的位姿在到当前帧位姿的变换矩阵T，即建立了位姿边。

5)遍历地图中的所有路径，就可以建立相机运动的位姿图模型。

位姿图的优化：

在上一步中，我们把机器人的运动构建成了一个带有n条边的图，所建图模型可以用以下方程描述：

其中，x_k为图模型中第k个节点位置信息；z_k为对图模型中第k个节点观测得到的位置信息；e_k为x_k与z_k之间的误差；Ω为信息矩阵，是协方差矩阵的逆。因为z_k为已知项，则误差项可写成如下形式：

对第k条边的e_k(x_k)进行一阶泰勒展开：

上述的J_k为e_k关于x_k的导数，矩阵形式为下雅克比阵。对第k条边的目标函数进一步展开有：

F_k(x_k+Δx)＝e_k(x_k+Δx)^TΩ_ke_k(x_k+Δx)

≈(e_k+J_kΔx)^TΩ_k(e_k+J_kΔx)

≈C_k+2b_kΔx+Δx^TH_kΔx

其中C_k为常数项，2b_k为一次项系数，H_k为二次项系数。则目标函数的F_k改变的值为ΔF_k＝2b_kΔx+Δx^TH_kΔx

令

于是该问题转变成一个线性方程的求解：

H_kΔx＝-b_k

解得全局最优的x^*＝x+Δx，并作为初始值代入F(x)进行迭代计算，最后计算得到每个关键帧的位姿。

顶标坐标计算是完成图优化后得到关键帧的位姿，根据关键帧位姿计算顶标中的点在地图坐标系中坐标。已知关键帧的旋转矩阵rMat与平移向量tMat，相机的内参矩阵M_C，路标点在关键帧上的图像坐标x_i,路标点在路标坐标系中的坐标x_w，计算过程如下：

1)根据x_i和x_w计算通过前述定位方法计算图像在当前路标坐标系中的位姿rMatU，tMatU；

2)路标坐标系到地图坐标系的转换矩阵为R＝rMatU^-1*rMat，t＝rMatU^-1*(tMat-rMatU)；

3)路标点在地图坐标系中的坐标计算x_map＝R*x_w+t。

Claims (4)

1.一种顶标地图的构建与定位方法，其特征在于，包括以下步骤：

步骤1、在地图区域设置初始路标，将初始路标的坐标系设置为地图坐标系；

步骤2、位姿图的构建：通过图像进行顶标提取，并建立节点，针对所建立节点建立关联帧，顺序提取关联帧集合中的每一帧，通过计算建立位姿边，并通过遍历地图中的所有路径建立位姿图模型；

步骤3、位姿图优化：根据位姿图模型计算得到每个关键帧的位姿；

步骤4、顶标坐标计算：根据每个关键帧的位姿计算顶标中的点在地图坐标系中坐标；

所述步骤2包括以下步骤：

1)顶标提取：利用图像处理算法提取图像中所有顶标，将该图像中的顶标集合作为当前帧；

2)建立节点：将当前帧与关键帧集合做对比，若当前帧的顶标集合与任一关键帧的顶标集合完全一样，则放弃该帧返回1)；否则将当前帧保存为关键帧，当前帧的位姿为一个新的节点，并进入步骤3)；

3)建立关联帧：若当前帧的顶标集合与关键帧集合中某一帧的顶标集合不完全一样，但存在相同的路标，则将该帧作为当前帧的关联帧，保存关键帧集合中当前帧的所有关联帧；

4)建立位姿边：顺序提取关联帧集合中的每一帧，计算当前帧与关联帧之间的位姿变换矩阵，已知当前帧中的路标L₁与关联帧中的路标L₂的ID相同，L₁、L₂中的像点坐标为

两个路标点在路标坐标系下的坐标相同为X^w，相机的内参矩阵M_C，令当前帧的旋转矩阵为单位矩阵I，平移向量为零向量0，根据位姿计算路标点坐标的方法，计算关键帧的路标点在该位姿情况下的坐标X₁ ^w，根据

和X₁ ^w，利用定位方法计算得到关联帧的位姿在到当前帧位姿的变换矩阵T，建立了位姿边；

5)重复1)-4)遍历地图中的所有路径，建立相机运动的位姿图模型；

所述定位方法包括以下步骤：

1)拍摄图像并利用图像处理方法提取图像中所有的路标点；

2)将提取到的路标与地图中的路标进行对比，得到路标点的图像坐标x_i和地图坐标X_w；

3)令

根据

和地图坐标X_w和相机的内参矩阵M_C，利用仿射变换方程

计算出旋转矩阵

平移向量

再去除摄影深度因子的影响得到旋转矩阵R、平移向量t；

4)根据旋转矩阵R、平移向量t求得相机中心的位置为X＝-R^-1*t，相机的旋转角为angle＝atan(R_(1,0)/R_(0,0))。

2.根据权利要求1所述的顶标地图的构建与定位方法，其特征在于：所述步骤3包括以下步骤：

位姿图模型建成了一个带有n条边的图，位姿图模型的方程描述为：

其中，x_k为图模型中第k个节点位置信息；z_k为对图模型中第k个节点观测得到的位置信息；e_k为x_k与z_k之间的误差；Ω为信息矩阵，是协方差矩阵的逆；

z_k为已知项，则误差项：

对第k条边的e_k(x_k)进行一阶泰勒展开：

上述的J_k为e_k关于x_k的导数，矩阵形式为下雅克比阵，对第k条边的目标函数进一步展开有：

F_k(x_k+Δx)＝e_k(x_k+Δx)^TΩ_ke_k(x_k+Δx)

≈(e_k+J_kΔx)^TΩ_k(e_k+J_kΔx)

≈C_k+2b_kΔx+Δx^TH_kΔx

其中C_k为常数项，2b_k为一次项系数，H_k为二次项系数，则目标函数的F_k改变的值为ΔF_k＝2b_kΔx+Δx^TH_kΔx；

令

根据H_kΔx＝-b_k方程的求解：

解得全局最优的x^*＝x+Δx，并作为初始值代入F(x)进行迭代计算，最后计算得到每个关键帧的位姿。

3.根据权利要求2所述的顶标地图的构建与定位方法，其特征在于：所述步骤4包括以下步骤：

根据每个关键帧的位姿计算顶标中的点在地图坐标系中坐标，已知关键帧的旋转矩阵rMat与平移向量tMat，相机的内参矩阵M_C，路标点在关键帧上的图像坐标x_i，路标点在路标坐标系中的坐标x_w，计算过程如下：

1)根据x_i和x_w计算通过前述定位方法计算图像在当前路标坐标系中的位姿rMatU，tMatU；

2)路标坐标系到地图坐标系的转换矩阵为R＝rMatU^-1*rMat，t＝rMatU^-1*(tMat-rMatU)；

3)路标点在地图坐标系中的坐标计算x_map＝R*x_w+t。

4.根据权利要求1或3所述的顶标地图的构建与定位方法，其特征在于：地图区域内需要定位的装置为机器人，所述路标点为贴在机器人需要运动的区域屋顶的顶标贴。

Images