[发明专利]一种封闭环境下轻量级视觉和惯性叠加行人定位和构图方法

说明书

本发明公开了一种封闭环境下轻量级视觉和惯性叠加行人定位和构图方法，通过蒙特卡洛法对N个粒子的t时刻行人位姿s_t进行预测采样使用VLAD算法进行当前图像I_i特征提取f_VLAD(I_i)并与已有图像特征f(I)进行匹配，得到匹配的粒子位姿对序列根据位姿对每个粒子构建栅格地图Θ^[m]并建立地图观测模型并结合匹配的粒子位姿对序列进行粒子权重更新，实现定位和构图误差的修正。本发明可以在低成本处理器中实现基于行人的视觉和惯性实时定位和构图，降低了视觉和惯性行人定位系统的成本，适合于工程应用。

技术领域

本发明属于视觉和惯性行人导航系统技术领域，具体涉及一种封闭环境下轻量级视觉和惯性叠加行人定位和构图方法。

背景技术

在地铁、地下管道、矿井等特殊复杂的场景下，特种作业人员通常处于封闭环境中。因导航卫星信号容易被干扰、遮蔽且信号不连续，卫星导航难以满足特种作业人员的定位需求。其次，环境未知且复杂，作业人员需要实时掌握当前环境地图信息，这对定位导航系统的环境感知能力提出了新的挑战。

基于惯性传感器的导航定位技术具有体积小、成本低、功耗低、抗干扰性强的特性，但由于传感器噪声等因素，其定位误差随时间累积。因此，在行人运动过程中如何消除惯性系统的累积误差，实现封闭环境下长时间稳定可靠的行人定位对于提高惯性行人定位系统的实用性具有重大意义。

即时定位与构图(SLAM)算法是一种在定位的同时构建环境地图，在利用构建的地图修正定位误差的方法，能够在封闭环境中有效消除定位系统的累积误差，可以弥补惯性导航系统误差随时间累积的缺点。然而目前以视觉、激光雷达等传感器件为核心的SLAM系统体积大、成本高，对信息处理系统的运算能力要求高，难以普及应用在行人定位中。因此构建轻量级视觉和惯性叠加行人SLAM框架，在保证视觉和惯性融合导航系统的高精度定位性能的同时，实现长续航、高可靠性、低成本的轻量化行人实时定位与构图系统具有突出的应用价值。

发明内容

发明目的：本发明提供了一种封闭环境下轻量级视觉和惯性叠加行人定位和构图方法，能够在地铁、地下管道、矿井等封闭空间中，满足基于行人的低成本、高精度、高可靠性的实时定位与导航的需求。

技术方案：本发明所述的一种封闭环境下轻量级视觉和惯性叠加行人定位和构图方法，包括以下步骤：

(1)使用N个粒子通过蒙特卡洛采样方法对t时刻行人位姿s_t进行预测采样

(2)基于VLAD算法对当前位姿s_t对应的图像I_i进行特征提取f_VLAD(I_i)并与图像特征序列A_I中的图像特征进行匹配，如果图像特征匹配成功，则对于每一个粒子根据位姿序列进行匹配点对筛选，得到匹配的粒子位姿对序列

(3)根据第m个粒子t时刻位姿更新该粒子的栅格地图Θ^[m]，根据栅格地图Θ^[m]中的栅格占用次数建立地图观测模型；

(4)根据粒子位姿对序列计算粒子图像匹配概率

(5)结合步骤(3)得到的地图观测模型进行粒子的权重更新，实现对定位和构图误差的修正。

进一步地，所述步骤(1)包括以下步骤：

(11)如果t时刻为初始时刻t₀，则对N个粒子进行初始位姿赋值：

式中为初始时刻行人x、y方向坐标，为初始时刻行人航向角，上标[m]代表第m个粒子，上标T代表矩阵的转置；