一种封闭环境下轻量级视觉和惯性叠加行人定位和构图方法技术

本发明专利技术公开了一种封闭环境下轻量级视觉和惯性叠加行人定位和构图方法，通过蒙特卡洛法对N个粒子的t时刻行人位姿s

【技术实现步骤摘要】
一种封闭环境下轻量级视觉和惯性叠加行人定位和构图方法


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

技术介绍

[0002]在地铁、地下管道、矿井等特殊复杂的场景下，特种作业人员通常处于封闭环境中。因导航卫星信号容易被干扰、遮蔽且信号不连续，卫星导航难以满足特种作业人员的定位需求。其次，环境未知且复杂，作业人员需要实时掌握当前环境地图信息，这对定位导航系统的环境感知能力提出了新的挑战。
[0003]基于惯性传感器的导航定位技术具有体积小、成本低、功耗低、抗干扰性强的特性，但由于传感器噪声等因素，其定位误差随时间累积。因此，在行人运动过程中如何消除惯性系统的累积误差，实现封闭环境下长时间稳定可靠的行人定位对于提高惯性行人定位系统的实用性具有重大意义。
[0004]即时定位与构图(SLAM)算法是一种在定位的同时构建环境地图，在利用构建的地图修正定位误差的方法，能够在封闭环境中有效消除定位系统的累积误差，可以弥补惯性导航系统误差随时间累积的缺点。然而目前以视觉、激光雷达等传感器件为核心的SLAM系统体积大、成本高，对信息处理系统的运算能力要求高，难以普及应用在行人定位中。因此构建轻量级视觉和惯性叠加行人SLAM框架，在保证视觉和惯性融合导航系统的高精度定位性能的同时，实现长续航、高可靠性、低成本的轻量化行人实时定位与构图系统具有突出的应用价值。

技术实现思路

[0005]专利技术目的：本专利技术提供了一种封闭环境下轻量级视觉和惯性叠加行人定位和构图方法，能够在地铁、地下管道、矿井等封闭空间中，满足基于行人的低成本、高精度、高可靠性的实时定位与导航的需求。
[0006]技术方案：本专利技术所述的一种封闭环境下轻量级视觉和惯性叠加行人定位和构图方法，包括以下步骤：
[0007](1)使用N个粒子通过蒙特卡洛采样方法对t时刻行人位姿s
t
进行预测采样
[0008](2)基于VLAD算法对当前位姿s
t
对应的图像I
i
进行特征提取f
VLAD
(I
i
)并与图像特征序列A
I
中的图像特征进行匹配，如果图像特征匹配成功，则对于每一个粒子根据位姿序列进行匹配点对筛选，得到匹配的粒子位姿对序列
[0009](3)根据第m个粒子t时刻位姿更新该粒子的栅格地图Θ
[m]，根据栅格地图Θ
[m]中的栅格占用次数建立地图观测模型；
[0010](4)根据粒子位姿对序列计算粒子图像匹配概率
[0011](5)结合步骤(3)得到的地图观测模型进行粒子的权重更新，实现对定位和构图误
差的修正。
[0012]进一步地，所述步骤(1)包括以下步骤：
[0013](11)如果t时刻为初始时刻t0，则对N个粒子进行初始位姿赋值：
[0014][0015]式中为初始时刻行人x、y方向坐标，为初始时刻行人航向角，上标[m]代表第m个粒子，上标T代表矩阵的转置；
[0016](12)如果t时刻不是初始时刻t0，则对t时刻的行人位姿分布s
t
进行蒙特卡洛采样；对t时刻步长
‑
航向变化控制向量u
t
进行蒙特卡洛采样
[0017]u
t
＝[d
t Δψ
t
]T
[0018][0019]式中，d为步长，Δψ为航向角变化，下标t为时间，式中，d为步长，Δψ为航向角变化，下标t为时间，σ
d
为步长测量噪声标准差，σ
Δψ
为航向变化测量噪声标准差，N表示正态分布；
[0020](13)建立行人运动基于步长
‑
航向变化的位姿推算模型，通过t
‑
1时刻位姿和步骤(12)中得到的控制向量采样对t时刻行人位姿s
t
进行预测采样
[0021]s
t
＝[x
t y
t ψ
t
]T
[0022][0023][0024]式中，x为方向坐标，y为方向坐标，ψ为航向角，下标t代表时间，为t时刻第m个粒子的行人位姿预测采样，Δs
[m]为第m个粒子的位姿变化向量。
[0025]进一步地，所述步骤(2)包括以下步骤：
[0026](21)提取图像I
i
的M个D维ORB特征q
1:M
；
[0027](22)通过K
‑
means算法对图像库中的图像进行聚类，得到K个聚类中心c
1:k
；
[0028](23)将图像I
i
的M个D维ORB特征q
1:M
转换为全局特征f
VLAD
(I
i
)并加入至图像特征序列A
I
；
[0029][0030][0031]其中，q
i
表示第i个ORB特征，c
k
表示第k个聚类中心，q
i
与c
k
都是D维向量；表示当式||q
i
‑
c
k
||最小时k的值，f
VLAD
(I
i
)代表图像I
i
的VLAD全局特征：
[0032]A
I
＝[A
I f
VLAD
(I
i
)][0033]其中，A
I
表示图像特征序列；
[0034](24)将图像I
i
的全局特征与图像特征序列中的前i
‑
1个图像特征A
I
(1:i
‑
1)计算欧式距离d(I
i
,I
1:i
‑1)并取其中的最小值d
min
，距离越小即代表图像相似度越高；
[0035]d
min
＝min{d(I
i
,I
j
)},j＝1,2,
…
,i
‑1[0036]d(I
i
,I
j
)＝||f
VLAD
(I
i
)
‑
f
VLAD
(I
j
)||
[0037]其中，f
VLAD
(I
i
)为图像I
i
经步骤(2.2)得到的全局特征；
[0038](25)如果最小欧式距离d
min
小于阈值Tur
pic
，且对于单个粒子，图像I
i
与图像I
j
对应的位置距离小于阈值Tur
...

【技术保护点】

【技术特征摘要】
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时刻为初始时刻t0，则对N个粒子进行初始位姿赋值：式中为初始时刻行人x、y方向坐标，为初始时刻行人航向角，上标[m]代表第m个粒子，上标T代表矩阵的转置；(12)如果t时刻不是初始时刻t0，则对t时刻的行人位姿分布s
t
进行蒙特卡洛采样；对t时刻步长
‑
航向变化控制向量u
t
进行蒙特卡洛采样u
t
＝[d
t
ꢀ△
ψ
t
]
T
式中，d为步长，
△
ψ为航向角变化，下标t为时间，ψ为航向角变化，下标t为时间，σ
d
为步长测量噪声标准差，σ
Δψ
为航向变化测量噪声标准差，N表示正态分布；(13)建立行人运动基于步长
‑
航向变化的位姿推算模型，通过t
‑
1时刻位姿和步骤(12)中得到的控制向量采样对t时刻行人位姿s
t
进行预测采样s
t
＝[x
t y
t ψ
t
]
TT
式中，x为方向坐标，y为方向坐标，ψ为航向角，下标t代表时间，为t时刻第m个粒子的行人位姿预测采样，Δs
[m]
为第m个粒子的位姿变化向量。3.根据权利要求1所述的封闭环境下轻量级视觉和惯性叠加行人定位和构图方法，其特征在于，所述步骤(2)包括以下步骤：(21)提取图像I
i
的M个D维ORB特征q
1：M
；
(22)通过K
‑
means算法对图像库中的图像进行聚类，得到K个聚类中心c
1：k
；(23)将图像I
i
的M个D维ORB特征q
1：M
转换为全局特征f
VLAD
(I
i
)并加入至图像特征序列A
I
；；其中，q
i
表示第i个ORB特征，c
k
表示第k个聚类中心，q
i
与c
k
都是D维向量；表示当式||q
i
‑
c
k
||最小时k的值，f
VLAD
(I
i
)代表图像I
i
的VLAD全局特征：A
I
＝[A
I f
VLAD
(I<...

【专利技术属性】
技术研发人员：崔岩，熊智，丁一鸣，彭政，王婕，李晓东，孙银收，刘晨，张玲，张苗，陈芷心，李婉玲，曹志国，王征淳，
申请(专利权)人：南京航空航天大学，
类型：发明
国别省市：