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

权利要求书

1.一种封闭环境下轻量级视觉和惯性叠加行人定位和构图方法，其特征在于，包括以下步骤：

(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)得到的地图观测模型进行粒子的权重更新，实现对定位和构图误差的修正。

2.根据权利要求1所述的封闭环境下轻量级视觉和惯性叠加行人定位和构图方法，其特征在于，所述步骤(1)包括以下步骤：

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

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

(12)如果t时刻不是初始时刻t₀，则对t时刻的行人位姿分布s_t进行蒙特卡洛采样；对t时刻步长-航向变化控制向量u_t进行蒙特卡洛采样

u_t＝[d_t △ψ_t]^T

式中，d为步长，△ψ为航向角变化，下标t为时间，σ_d为步长测量噪声标准差，σ_Δψ为航向变化测量噪声标准差，N表示正态分布；

(13)建立行人运动基于步长-航向变化的位姿推算模型，通过t-1时刻位姿和步骤(12)中得到的控制向量采样对t时刻行人位姿s_t进行预测采样

s_t＝[x_t y_t ψ_t]^T

式中，x为方向坐标，y为方向坐标，ψ为航向角，下标t代表时间，为t时刻第m个粒子的行人位姿预测采样，Δs^[m]为第m个粒子的位姿变化向量。