CN107808395A - 一种基于slam的室内定位方法 - Google Patents

Abstract

本发明提供了一种基于SLAM的室内定位方法，包括如下步骤：加载SLAM地图得到关键帧集合KFS与3D点云，下采样关键帧集合KFS得到sKFS；提取输入图像的特征点，并对图像进行下采样得到缩略图；求出sKFS中每一个关键帧与输入图像之间的变化矩阵H；对齐每个sKFS与输入图像，算出最小误差e_i，若最小误差e_i小于阈值T，则认为输入图像与KFS中第i个关键帧Ki匹配；根据Ki的位姿E_i将3D点云映射到Ki中；根据变化矩阵H将Ki中的点映射到输入图像中，并在输入图像中查找与之匹配的特征点；根据匹配上的特征点，利用对极几何原理计算出输入图像的位姿E。本发明的室内定位方法在SLAM对室内已经建图的基础上，对设备进行定位，通过匹配特征点，优化相机位姿，得到更准确的结果。

Description

一种基于SLAM的室内定位方法

技术领域

本发明涉及一种室内定位方法，尤其是一种基于SLAM的室内定位方法。

背景技术

SLAM(simultaneous localization and mapping)分为两大功能：定位与建图。其中建图主要作用是对周边环境的理解，建立周边环境与空间的对应关系；定位主要是根据建好的图，判断设备在地图中的位姿，从而得到在环境中的信息。

常见的室内定位技术主要采用无线通讯、基站定位、惯导定位、蓝牙、wifi等多种技术集成一套定位体系。然而该类方法对环境的依赖比较大，比如：气温，距离等蓝牙影响较大，故在不同环境下都需要特定的方法重新矫正，而且往往需要部署多个设备；此外，该类方法只能得到远近信息，无法知道设备在空间中的姿态。

发明内容

本发明的目的在于：提供一种基于SLAM的室内定位方法，能够在室内实现较高精度的定位。

为了实现上述发明目的，本发明提供了一种基于SLAM的室内定位方法，包括如下步骤：

步骤1，加载SLAM地图，得到关键帧集合KFS与3D点云，KFS中包含每一个关键帧的位姿E_i，下采样关键帧集合KFS得到sKFS；

步骤2，利用FAST算法提取输入图像的角点作为特征点，并对输入图像进行下采样得到缩略图；

步骤3，用ESM-tracking算法求出sKFS中每一个关键帧与输入图像之间的变化矩阵H，H中包含了二维图像之间的旋转和平移变换信息；

步骤4，对齐每个sKFS与输入图像，算出最小误差e_i，若最小误差e_i小于阈值T，则认为输入图像与KFS中第i个关键帧Ki匹配，最小误差e_i的计算公式为：

e_i＝I(P_sKFS(i))-I(H·P_query)

式中，P_sKFS(i)表示sKFS中的第i帧，P_query为输入图像的缩略图，I表示像素值；

步骤5，根据Ki的位姿E_i，将3D点云映射到Ki中，映射公式为：

式中，P_i表示3D点云中的点，E_i为相机姿态，E_i·P_i表示将3D点投影到相机坐标系，CamProj(E·P_i)表示将相机坐标系中的点投影到Ki中；

步骤6，根据变化矩阵H将Ki中的点映射到输入图像中，并利用SSD算法在输入图像中查找与之匹配的特征点，若匹配上的特征点数量小于N，则重定位失败，否则重定位成功；

步骤7，根据匹配上的特征点，利用对极几何原理计算出输入图像的位姿E，E中包括R和T，R表示当前相机的旋转信息，T表示平移矩阵。

进一步地，对极几何原理计算出输入图像的R和T的具体公式为：

P_l＝R(P_r-T)

(R_TP_r)^T·T×P_l＝0

式中，P_l和P_r为两幅图像中匹配上的特征点，R^T为R的转置，×为叉乘。

本发明的有益效果在于：本发明在SLAM对室内已经建图的基础上，对设备进行定位，SLAM得到的地图主要包括3D点云和关键帧信息，一般根据相机获取的图像与关键帧对齐，从而得到相机的姿态，为了增加算法的实时性及鲁棒性，本发明先对图像与关键帧进行下采样，在低分辨率上实现对齐，估算出粗略位姿；然后再将3D点云映射到当前关键帧，匹配特征点，优化相机位姿，得到更准确的结果。

附图说明

图1为本发明的方法流程图。

具体实施方式

如图1所示，本发明公开的一种基于SLAM的室内定位方法，包括如下步骤：

步骤1，加载SLAM地图，得到关键帧集合KFS与3D点云，KFS中包含每一个关键帧的位姿E_i，下采样关键帧集合KFS得到sKFS，本实施例中下采样到40*30；

步骤2，利用FAST算法提取输入图像的角点作为特征点，并对输入图像进行下采样得到缩略图，本实施例中大小为40*30；

步骤3，用ESM-tracking算法求出sKFS中每一个关键帧与输入图像之间的变化矩阵H，H中包含了二维图像之间的旋转和平移变换信息；

步骤4，对齐每个sKFS与输入图像，算出最小误差e_i，若最小误差e_i小于阈值T，则认为输入图像与KFS中第i个关键帧Ki匹配，本实施例中T＝100000，最小误差e_i的计算公式为：

e_i＝I(P_sKFS(i))-I(H·P_query)

式中，P_sKFS(i)表示sKFS中的第i帧，P_query为输入图像的缩略图，I表示像素值；

步骤5，根据Ki的位姿E_i，将3D点云映射到Ki中，映射公式为：

式中，P_i表示3D点云中的点，E_i为相机姿态，E_i·P_i表示将3D点投影到相机坐标系，CamProj(E·P_i)表示将相机坐标系中的点投影到Ki中；

步骤6，根据变化矩阵H将Ki中的点映射到输入图像中，并利用SSD算法在输入图像中查找与之匹配的特征点，若匹配上的特征点数量小于N，则重定位失败，否则重定位成功，本实施例中N＝50；

步骤7，根据匹配上的特征点，利用对极几何原理计算出输入图像的位姿E，E中包括R和T，R表示当前相机的旋转信息，T表示平移矩阵，其中，对极几何原理计算出输入图像的R和T的具体公式为：

P_l＝R(P_r-T)

(R^TP_r)^T·T×P_l＝0

式中，P_l和P_r为两幅图像中匹配上的特征点，R^T为R的转置，×为叉乘。

基于SLAM的定位系统，可以得到设备在空间中6自由度的信息，比传统的室内定位技术用途更加广泛。因此本发明在SLAM对室内已经建图的基础上，对设备进行定位，SLAM得到的地图主要包括3D点云和关键帧信息，一般根据相机获取的图像与关键帧对齐，从而得到相机的姿态，为了增加算法的实时性及鲁棒性，本发明先对图像与关键帧进行下采样，在低分辨率上实现对齐，估算出粗略位姿；然后再将3D点云映射到当前关键帧，匹配特征点，优化相机位姿，得到更准确的结果。

Claims (2)

1.一种基于SLAM的室内定位方法，其特征在于，包括如下步骤：

步骤1，加载SLAM地图，得到关键帧集合KFS与3D点云，KFS中包含每一个关键帧的位姿E_i，下采样关键帧集合KFS得到sKFS；

步骤2，利用FAST算法提取输入图像的角点作为特征点，并对输入图像进行下采样得到缩略图；

步骤3，用ESM-tracking算法求出sKFS中每一个关键帧与输入图像之间的变化矩阵H，H中包含了二维图像之间的旋转和平移变换信息；

步骤4，对齐每个sKFS与输入图像，算出最小误差e_i，若最小误差e_i小于阈值T，则认为输入图像与KFS中第i个关键帧Ki匹配，最小误差e_i的计算公式为：

e_i＝I(P_sKFS(i))-I(H·P_query)

式中，P_sKFS(i)表示sKFS中的第i帧，P_query为输入图像的缩略图，I表示像素值；

步骤5，根据Ki的位姿E_i，将3D点云映射到Ki中，映射公式为：

<mrow> <mfenced open = "(" close = ")"> <mtable> <mtr> <mtd> <msub> <mi>u</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>v</mi> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mi>C</mi> <mi>a</mi> <mi>m</mi> <mi>Pr</mi> <mi>o</mi> <mi>j</mi> <mrow> <mo>(</mo> <msub> <mi>E</mi> <mi>i</mi> </msub> <mo>&amp;CenterDot;</mo> <msub> <mi>P</mi> <mi>i</mi> </msub> <mo>)</mo> </mrow> </mrow>

式中，P_i表示3D点云中的点，E_i为相机姿态，E_i·P_i表示将3D点投影到相机坐标系，CamProj(E·P_i)表示将相机坐标系中的点投影到Ki中；

步骤6，根据变化矩阵H将Ki中的点映射到输入图像中，并利用SSD算法在输入图像中查找与之匹配的特征点，若匹配上的特征点数量小于N，则重定位失败，否则重定位成功；

步骤7，根据匹配上的特征点，利用对极几何原理计算出输入图像的位姿E，E中包括R和T，R表示当前相机的旋转信息，T表示平移矩阵。

2.根据权利要求1所述的基于SLAM的室内定位方法，其特征在于，对极几何原理计算出输入图像的R和T的具体公式为：

P_l＝R(P_r-T)

(R^TP_r)^T·T×P_l＝0

式中，P_l和P_r为两幅图像中匹配上的特征点，R^T为R的转置，×为叉乘。