一种基于VSLAM的室内高精度定位导航方法

摘要

本发明涉及一种基于VSLAM的室内高精度定位导航方法，其通过VSLAM与IMU数据融合建立高精度地图，通过梯度法和决策树相结合的方法捕捉和跟踪特征点，对角点描述子采用Kd树建立索引，当特征点跟踪丢失后可用于重新定位匹配，防止了误差累积，提高了精度。本发明适用于室内复杂环境下的路径导航，还可推广应用于如机器人室内自主导航、室内自动驾驶汽车导航，医院、商场、大厦室内定位导航等。

说明书

技术领域

本发明涉及一种基于VSLAM的室内高精度定位导航方法。

背景技术

VSLAM(视觉SLAM)是将地图创建和定位合并为一个估计问题来建立更加可信的环境地图。目前，VSLAM系统借助传感器数据，进行后端优化处理来完成定位和地图创建，其中，内部传感器(如里程计)，IMU不受外界环境影响，但有误差累积。外部传感器(如相机)等，依赖于外部环境特征的变化，容易受到相机曝光和内参影响，在相机快速移动时出现目标丢失情况。

发明内容

为有效防止累积误差和减少相机内参等传感器测量参数对定位精度的影响，本发明提出一种基于VSLAM的室内高精度定位导航方法与系统。

本发明所采用的技术方案为：

一种基于VSLAM的室内高精度定位导航方法，包括手机端和云服务器端，所述定位导航方法包括如下步骤：

步骤一、通过手机端相机拍摄室内待导航区域一圈，采集室内场景图像，提取每一帧室内场景图像的特征点；

基于梯度法，检测出室内场景图像中各物体边缘形状，并进一步地根据物体边缘形状提取出自然路标，通过决策树对自然路标寻找角点，作为特征点；

步骤二、根据相邻帧室内场景图像定量估算帧间相机的运动；

获取一帧室内场景图像中自然路标的角点信息，根据角点描述子采用Kd树建立索引，对于相邻帧室内场景图像进行角点匹配；当跟踪角点丢失后，可根据索引重新建立匹配；

步骤三、借助IMU参数积分获取位置、速度和角度信息，通过数据融合优化位姿估计；

步骤四、将相邻帧的运动轨迹串起来，构成手机端相机的运动轨迹；

步骤五、根据估算的每个时刻相机的位置和姿态，利用室内场景环境中角点和关键路标点增量式地构建地图；

步骤六、手机端拍摄室内场景图像并上传至云服务器端，室内场景图像经特征提取及特征匹配后，计算出当前的位置和姿态信息并反馈至手机端，完成定位导航。

进一步地，步骤二中角点匹配的具体步骤为：首先，计算出角点描述子16维上的数据方差，挑选出数据方差中最大的维度；然后，根据数据方差最大的维度，将角点描述子进行排序，找到位于中间的角点描述子中值作为分类的依据，迭代建立Kd树；接着，计算相邻两帧室内场景图像的匹配角点，以一个角点为圆心，两个角点描述子数据方差最大距离作为阈值半径，建立一个超球体；最后，查找Kd树中的最近邻居，得到相邻两帧室内场景图像中匹配的角点，利用匹配角点描述子计算两个角点之间汉明距离差，获得相机运动矢量。

进一步地，步骤三中，为避免相邻帧室内场景图像多次局部估计出现累积漂移，通过非线性优化对位姿估计进行优化；将优化的变量作为图的节点，误差项作为图的边，在给定初值后，进行迭代优化更新。

进一步地，步骤四中，在判断相机运动轨迹/定位的过程中，将手机端和云服务器端相结合实现分布式VSLAM，以网络拓扑容量参数作为阀值决定定位算法在云服务器端执行还是在手机端执行；具体地，手机端将拍摄的室内场景图像数据通过消息队列传递给云服务器端，从消息队列中取出观测和控制数据，通过Map任务并行完成路标点的更新，Reduce端根据更新的路标点实现定位和地图创建，最终将VSLAM结果发送给手机端。

进一步地，步骤五的地图构建中，采用栅格融合的方法，将地图分割为若干地图块，每个地图块上定义一组多项式函数f(x(t),v)作为融合规则，表示为：

f(x(t),v)＝min||x(t)-v||

其中，t表示时刻，x(t)＝E(a

基于多项式函数融合规则，对不同地图块进行栅格融合，融合后的局部地图，作为粒子滤波的输入，经过与已建全局地图的扫描匹配，融入GPS和GIS空间数据库信息，同时进行定位与地图创建；融合的过程实际上是根据更新的粒子集进行粒子重采样更新局部地图，是新地图顺序增加替换旧地图部分，从而完成地图的构建。

进一步地，步骤六中，在定位导航/路径搜索过程中，采用启发式搜索，构造与最优路径有关的可视边，完成定位导航功能。

本发明的有益效果在于：

本发明通过VSLAM与IMU数据融合建立高精度地图，通过梯度法和决策树相结合的方法捕捉和跟踪特征点，对角点描述子采用Kd树建立索引，当特征点跟踪丢失后可用于重新定位匹配，防止了误差累积，提高了精度。本发明适用于室内复杂环境下的路径导航，还可推广应用于如机器人室内自主导航、室内自动驾驶汽车导航，医院、商场、大厦室内定位导航等。

附图说明

图1为本发明的基于VSLAM的室内高精度定位导航方法的流程框图。

具体实施方式

下面结合附图和具体的实施例对本发明的基于VSLAM的室内高精度定位导航方法作进一步地详细说明。

如图1所示，一种基于VSLAM的室内高精度定位导航方法，包括手机端和云服务器端，所述定位导航方法包括如下步骤：

步骤一、通过手机端相机拍摄室内待导航区域一圈，采集室内场景图像，提取每一帧室内场景图像的特征点(用于构建导航地图，也可使用其他专业视频、图像设备代替手机端相机采集图像)。

基于梯度法，检测出室内场景图像中各物体边缘形状(检测出图像中明暗变化剧烈/梯度变化较大的像素点)，并进一步地根据物体边缘形状提取出自然路标(如墙棱边、墙平面等)，通过决策树对自然路标寻找角点，作为特征点。采用决策树可对拍摄的每帧图像在16个方向上进行训练，得到一棵决策树，从而在判定某一像素点是否为角点时，不再需要对所有方向进行检测，而只需要按照决策树指定的方向进行2-3次判定即可确定该点是否为角点。

步骤二、根据相邻帧室内场景图像定量估算帧间相机的运动。

获取一帧室内场景图像中自然路标的角点信息，根据角点描述子(即角点方向矢量)采用Kd树(K-dimensional Tree)建立索引，对于相邻帧室内场景图像进行角点匹配。当跟踪角点丢失后，可根据索引重新建立匹配。

角点匹配的具体步骤为：首先，计算出角点描述子16维上的数据方差，挑选出数据方差中最大的维度。然后，根据数据方差最大的维度，将角点描述子进行排序，找到位于中间的角点描述子中值作为分类的依据，迭代建立Kd树。接着，计算相邻两帧室内场景图像的匹配角点，以一个角点为圆心，两个角点描述子数据方差最大距离作为阈值半径，建立一个超球体。最后，查找Kd树中的最近邻居，得到相邻两帧室内场景图像中匹配的角点，利用匹配角点描述子计算两个角点之间汉明距离差，获得相机运动矢量。

步骤三、借助IMU参数积分获取位置、速度和角度信息，通过数据融合优化位姿估计。

由于IMU的累积误差性，为避免相邻帧室内场景图像多次局部估计出现累积漂移，通过非线性优化(图优化)对位姿估计进行优化。将优化的变量作为图的节点，误差项作为图的边，在给定初值后，进行迭代优化更新。由于图优化的稀疏性，可以在保证精度的同时，降低计算量。

步骤四、将相邻帧的运动轨迹串起来，构成手机端相机的运动轨迹(解决了定位的问题)。

在判断相机运动轨迹/定位的过程中，将手机端和云服务器端相结合实现分布式VSLAM，将VSLAM过程在云服务器端进行并行实时处理，弥补手机端机载能力不足，负担过重的短板，提高手机端SLAM处理效率。分布式VSLAM根据手机性能和当前网络情况，以网络拓扑容量参数作为阀值决定定位算法在云服务器端执行还是在手机端执行，优化不同性能手机在不同联网条件下的定位。具体地，手机端将拍摄的室内场景图像数据通过消息队列传递给云服务器端，从消息队列中取出观测和控制数据，通过Map任务并行完成路标点的更新，Reduce端根据更新的路标点实现定位和地图创建，最终将VSLAM结果发送给手机端。

步骤五、根据估算的每个时刻相机的位置和姿态，并利用室内场景环境中角点和关键路标点增量式地构建地图。

由于各类相机的硬件差异和内参的不同(导致图像存在差异问题)，对应地图空间同一栅格属性判断存在差异。地图构建中，采用栅格融合的方法，将地图分割为若干地图块，每个地图块上定义一组多项式函数f(x(t),v)作为融合规则，表示为：

f(x(t),v)＝min||x(t)-v||

其中，t表示时刻，x(t)＝E(a

这是一个用不断细化的网格来解析模型，并对不同网格下得到的结果进行比较的过程。通过对图片中一个或多个点上的建图进行分析，可以实现结果比较。基于多项式函数融合规则，对不同地图块进行栅格融合，融合后的局部地图，作为粒子滤波的输入，经过与已建全局地图的扫描匹配，融入GPS和GIS空间数据库信息，同时进行定位与地图创建。融合的过程实际上是根据更新的粒子集进行粒子重采样更新局部地图，是新地图顺序增加替换旧地图部分，从而完成地图的构建。

步骤六、手机端拍摄室内场景图像并上传至云服务器端，室内场景图像经特征提取及特征匹配后，计算出当前的位置和姿态信息并反馈至手机端，完成定位导航。

在定位导航/路径搜索过程中，采用启发式搜索，构造与最优路径有关的可视边，完成定位导航功能。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术方法范围内，可轻易想到的替换或变换方法，都应该涵盖在本发明的保护范围之内。