CN115388902B - 室内定位方法和系统、ar室内定位导航方法和系统 - Google Patents
室内定位方法和系统、ar室内定位导航方法和系统 Download PDFInfo
- Publication number
- CN115388902B CN115388902B CN202211335098.7A CN202211335098A CN115388902B CN 115388902 B CN115388902 B CN 115388902B CN 202211335098 A CN202211335098 A CN 202211335098A CN 115388902 B CN115388902 B CN 115388902B
- Authority
- CN
- China
- Prior art keywords
- image
- monocular camera
- visual
- indoor
- map
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 80
- 230000000007 visual effect Effects 0.000 claims abstract description 142
- 238000004590 computer program Methods 0.000 claims abstract description 14
- 238000012937 correction Methods 0.000 claims description 48
- 238000004422 calculation algorithm Methods 0.000 claims description 25
- 230000004888 barrier function Effects 0.000 claims description 17
- 238000005259 measurement Methods 0.000 claims description 17
- 238000012360 testing method Methods 0.000 claims description 17
- 238000009432 framing Methods 0.000 claims description 13
- 230000009466 transformation Effects 0.000 claims description 12
- 238000013519 translation Methods 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000001514 detection method Methods 0.000 claims description 8
- 238000013135 deep learning Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 6
- 238000010276 construction Methods 0.000 claims description 5
- 230000011218 segmentation Effects 0.000 claims description 5
- 238000012216 screening Methods 0.000 claims description 4
- 230000001502 supplementing effect Effects 0.000 claims description 4
- 230000001131 transforming effect Effects 0.000 claims description 4
- 230000000873 masking effect Effects 0.000 claims description 3
- 238000005728 strengthening Methods 0.000 claims description 2
- 230000003014 reinforcing effect Effects 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 19
- 238000005457 optimization Methods 0.000 description 9
- 230000006870 function Effects 0.000 description 8
- 238000000605 extraction Methods 0.000 description 7
- 230000008569 process Effects 0.000 description 7
- 238000012545 processing Methods 0.000 description 6
- 238000009877 rendering Methods 0.000 description 5
- 230000008859 change Effects 0.000 description 4
- 230000010354 integration Effects 0.000 description 4
- 230000002708 enhancing effect Effects 0.000 description 3
- 239000000284 extract Substances 0.000 description 3
- 230000033001 locomotion Effects 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 238000003860 storage Methods 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000007781 pre-processing Methods 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 239000003086 colorant Substances 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000013136 deep learning model Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000002372 labelling Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 230000008719 thickening Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
Abstract
本发明涉及室内定位导航领域，公开一种室内定位方法和系统、AR室内定位导航方法和系统；室内定位方法包括使用单目相机采集图像并构建先验地图，使用视觉惯性里程计的实时位姿解算方法解算单目相机的相对位姿，根据视觉特征地图在视觉词袋模型的基础上结合几何信息和语义信息将单目相机的相对位姿变换为绝对位姿，结合室内路网地图、栅格地图和实景深度对单目相机的绝对位姿进行定位轨迹纠偏，室内定位系统包括实现各步骤的模块；AR室内定位导航方法在室内定位方法的基础上使用AR实时导航，系统包括移动终端和可在移动终端上运行AR室内定位导航方法的计算机程序。本发明可以提高图像匹配准确率、定位精度和纠偏能力。
Description
技术领域
本发明涉及室内定位导航技术领域，尤其是指一种室内定位方法和系统、AR室内定位导航方法和系统。
背景技术
随着智慧城市、物联网、移动互联等行业的迅速发展，人们对于高精度室内位置服务的需求日趋强烈。传统的室内定位方法大多基于电磁波、声波等，都属于有源定位，信号容易被干扰导致定位精度降低；计算机视觉定位作为无源定位具有更强的抗干扰能力，而其中单目视觉惯性里程系统又凭借其低廉的成本和较为精准的定位能力在众多视觉里程计中脱颖而出。
目前的单目视觉惯性里程计已有一定技术研究基础，其中视觉系统利用摄像头实时捕获图像，再根据图像变化来估计自身位姿，惯性测量单元则可以获取设备短时间内的位姿变化，通过两者的紧耦合以实现可靠的室内定位。但该定位方法存在两个主要问题：
一是图像特征跟踪严重依赖场景，在空间结构相似、纹理特征缺失、运动物体充斥的场景下，图像匹配易出错，定位效果差。针对这个问题，（专利公开号为：CN112381877A）不仅采集了与室内环境匹配的多张图像，还对每张环境采集图像中的三维物体进行了多类型的视觉定位特征提取，以提高特征匹配的鲁棒性；（专利公开号为：CN114708309A）充分利用建筑平面图的先验信息，设计了一套端到端建筑平面图到三维路标点集配准的深度学习模型，改变了传统的图像匹配模式，更加稳定；（专利公开号为：CN112329798A）对词袋模型中每个单词的场景表征能力进行计算，并提出了基于单词可信度修正直方图核函数的相似度度量方法，提高了图像相似度度量的准确度；（论文名称：基于动态物体特征点去除的视觉里程计算法，来源：《计算机测量与控制》）将获取图像中的动态物体特征点剔除，以提高匹配准确率，该方法更便于应用在人流量大或车流量大的环境中。
二是缺乏对定位轨迹的纠偏能力，完全依赖回环检测。针对这个问题，（专利公开号为：CN114739410A）在视觉定位的基础上结合使用了PDR（行人航迹推算算法）技术，构建了PDR步长估计模型，通过加权融合定位方法来提高定位精度；（专利公开号为：CN113358117A）采用了基于条件随机场的三维地图匹配算法来对视觉惯性里程计的输出进行修正。
综上所述，现有的单目视觉惯性里程计方法在一定条件下已具备室内场景的定位和导航能力，但仍然存在以下问题：在场景结构相似、纹理特征不足、运动物体繁多的情况下，图像匹配准确率会大幅降低，易产生运动跟踪失败以及无法重定位的问题；同时，在大范围、长时间、无回环的里程计运行过程中，由于缺乏有效的轨迹纠偏策略，定位误差会不断累积且无法消除。
发明内容
为此，本发明所要解决的技术问题在于克服现有技术中的不足，提供一种室内定位方法和系统、AR室内定位导航方法和系统，可以提高图像匹配准确率、定位精度和纠偏能力。
为解决上述技术问题，本发明提供了一种室内定位方法，包括：
步骤1：使用单目相机采集图像并构建视觉特征地图和室内路网地图，所述视觉特征地图包括视觉标签图像，所述视觉特征地图用于重定位单目相机，所述室内路网地图用于支撑导航路径的规划；
步骤2：使用视觉惯性里程计的实时位姿解算方法，根据单目相机采集的图像和惯性测量单元采集的惯性测量单元数据解算出每一时刻单目相机的相对位姿；
步骤3：根据所述视觉特征地图，在视觉词袋模型的基础上结合几何信息和语义信息对单目相机进行重定位，将单目相机的相对位姿变换为绝对位姿；
步骤4：结合所述室内路网地图、栅格地图和实景深度对单目相机的所述绝对位姿进行定位轨迹纠偏，得到纠偏后的定位轨迹。
作为优选的，根据所述视觉特征地图，在视觉词袋模型的基础上结合几何信息和语义信息对单目相机进行重定位，将单目相机的相对位姿变换为绝对位姿，具体包括：
步骤3.1：使用左右分幅策略补充图像的几何信息，使用深度学习语义分割模型强化图像的语义信息，结合所述结合几何信息和语义信息使用视觉词袋模型计算图像的相似度；
步骤3.2：对相似度得分超过预设阈值的两张图像分别进行特征点提取和暴力匹配，确定当前测试图像和匹配的视觉标签图像的2d-2d特征点的对应关系；
步骤3.3：根据当前测试图像和匹配的视觉标签图像的2d-2d特征点的对应关系以及视觉特征地图中视觉标签图像的2d特征点和3d地理坐标的对应关系，求解对应关系之间的变换旋转向量和平移向量；使用所述变换旋转向量和平移向量对当前匹配的视觉标签图像的单目相机位姿进行平移旋转变换得到当前单目相机在世界坐标系中的绝对位姿。
作为优选的，所述使用左右分幅策略补充图像的几何信息，使用深度学习语义分割模型强化图像的语义信息，结合所述结合几何信息和语义信息使用视觉词袋模型计算图像的相似度，具体包括：
步骤3.1.1：使用轻量级网络对所有视觉标签图像进行目标检测，找出待掩膜的噪声目标，剔除噪声目标的图像特征点得到掩膜后的图像特征点及其描述子；
步骤3.1.2：对图像进行左右两侧的对半分幅，将掩膜后的图像特征点和描述子分别存入左图、右图两个不同的数据库中，其中视觉特征地图中的视觉标签图像作为预存数据优先存储，实时获取的图像帧作为测试数据逐帧存储；
步骤3.1.3：分别计算掩膜后的所有视觉标签图像左图与当前测试图像左图、所有视觉标签图像右图与当前测试图像右图的相似度得分，再将两个得分相加作为最终的相似度得分。
作为优选的，结合所述室内路网地图、栅格地图和实景深度对单目相机的所述绝对位姿进行定位轨迹纠偏，具体包括：
步骤4.1：使用图像深度预测算法估计真实场景中单目相机与障碍物的实际距离，根据所述实际距离对定位轨迹进行纠偏；
步骤4.2：在使用实景深度纠偏后，判断室内场景的类型，若此时的室内场景狭窄交错则执行步骤4.3，若此时的室内场景开阔空旷则执行步骤4.4；
步骤4.3：根据所述室内路网地图中的路网节点和节点间的路网边对定位轨迹进行纠偏，
步骤4.4：构建栅格图像并计算当前轨迹点与可通行区域的几何关系进行纠偏。
作为优选的，所述使用图像深度预测算法估计真实场景中单目相机与障碍物的实际距离，根据所述实际距离对定位轨迹进行纠偏，具体包括：
步骤4.1.1：读取单目相机上传感器的重力值，计算所述重力值在X、Y、Z三个方向上的分量，根据所述三个方向上的分量计算单目相机与水平面的夹角；
若所述单目相机与水平面的夹角满足预设阈值条件，则执行步骤4.1.2，否则剔除单目相机采集的该图像；
步骤4.1.2：实时读取筛选后的单目相机采集的图像，输入轻量级的全卷积残差网络中进行场景深度预测，输出预测深度图像，将所述预测深度图像中的二维像素坐标转换为三维点云坐标；
步骤4.1.3：筛选所述三维点云坐标中距离相机较近的点云，将筛选后的点云网格化并根据障碍物在高程方向上垂直连续的几何特征识别出障碍物，计算单目相机和所述障碍物平面的垂线距离；
步骤4.1.4：根据单目相机的当前位姿，确定二维的所述预测深度图像中单目相机在当前视角下朝向的墙线或柱线，计算单目相机与最近障碍物线段的垂线距离；
将所述单目相机与最近障碍物线段的垂线距离和所述单目相机和所述障碍物平面的垂线距离对比，得到视觉惯性里程计累积的距离误差；
若所述单目相机与最近障碍物线段的垂线和所述单目相机和所述障碍物平面的垂线接近平行且所述距离误差在预设阈值范围内，则从当前轨迹坐标中剔除所述距离误差在x、y轴上的分量，得到纠偏后的轨迹坐标。
作为优选的，根据所述室内路网地图中的路网节点和节点间的路网边对定位轨迹进行纠偏，具体包括：
步骤4.3.1：从实景深度纠偏后的轨迹中按时序由近及远取出轨迹点，计算最近轨
迹点A和最远轨迹点B构成的向量
；
步骤4.3.2：根据向量
确定距离当前轨迹点最近的路网边及构成该路网边的节
点a和b；
步骤4.3.3：将当前轨迹点投影至所述距离当前轨迹点最近的路网边上。
作为优选的，所述构建栅格图像并计算当前轨迹点与可通行区域的几何关系进行纠偏，具体包括：
步骤4.4.1：获取图像的点云数据或者二维矢量地图生成栅格图像；
步骤4.4.2：标注所述栅格图像中的可通行栅格和不可通行栅格；
步骤4.4.3：计算实景深度纠偏后的轨迹点在栅格图像中的位置，若轨迹点落在所述栅格图像的可通行栅格内，则不进行纠偏；若轨迹点落在所述栅格图像的不可通行栅格内，则进行由近及远的邻域搜索直到找到最近的可通行栅格，将栅格像素坐标反算得到的地理坐标作为纠偏后的新轨迹点坐标。
本发明还提供了一种室内定位系统，包括先验地图构建模块、相对位姿定位模块、重定位模块和纠偏模块，
所述先验地图构建模块使用单目相机采集图像并构建视觉特征地图和室内路网地图，所述视觉特征地图包括视觉标签图像，所述视觉特征地图用于重定位单目相机，所述室内路网地图用于支撑导航路径的规划，将所述视觉特征地图传送给所述重定位模块，将所述室内路网地图传送给所述纠偏模块；
所述相对位姿定位模块使用视觉惯性里程计的实时位姿解算方法，根据单目相机采集的图像和惯性测量单元采集的惯性测量单元数据解算出每一时刻单目相机的相对位姿，将所述相对位姿传送给所述重定位模块；
所述重定位模块根据所述视觉特征地图，在视觉词袋模型的基础上结合几何信息和语义信息对单目相机进行重定位，将单目相机的相对位姿变换为绝对位姿，将所述绝对位姿传送给所述纠偏模块；
所述纠偏模块结合所述室内路网地图、栅格地图和实景深度对单目相机的所述绝对位姿进行定位轨迹纠偏，得到纠偏后的定位轨迹。
本发明还提供了一种AR室内定位导航方法，使用所述的室内定位方法得到纠偏后的定位轨迹，根据所述纠偏后的定位轨迹使用路径规划算法规划最优导航路线，使用AR导航实时显示所述最优导航路线。
本发明还提供了一种AR室内定位导航系统，包括移动终端和存储在移动终端上并可在移动终端上运行的计算机程序，所述移动终端执行所述计算机程序时实现所述AR室内定位导航方法的步骤。
本发明的上述技术方案相比现有技术具有以下优点：
本发明构建包括视觉特征地图和室内路网地图的先验地图，使用视觉词袋模型结合几何信息和语义信息对单目相机进行重定位，有效提高了图像匹配准确率，实现了定位轨迹的重定位；同时，结合室内路网地图、栅格地图和实景深度对单目相机的绝对位姿进行定位轨迹纠偏，有效提高纠偏能力，减小由视觉惯性里程计导致的累积误差，提高了定位轨迹的精度和实时性。
附图说明
为了使本发明的内容更容易被清楚的理解，下面根据本发明的具体实施例并结合附图，对本发明作进一步详细的说明，其中：
图1是本发明的流程图，
图2是本发明实施例中AR室内定位导航方法的流程示意图，
图3是本发明实施例中构建先验地图的流程示意图，
图4是本发明实施例中生成的场景稠密点云图，
图5是本发明实施例中视觉特征地图中的存储数据示意图，
图6是本发明实施例中办公室的室内路网地图，
图7是本发明实施例中视觉惯性里程计实时位姿解算方法的流程示意图，
图8是本发明实施例中视觉惯性里程计运行过程的示意图，
图9是本发明实施例中单目相机重定位算法流程示意图，
图10是本发明实施例中单目相机重定位结果示意图，
图11是本发明实施例中使用YOLOv3模型的语义提取结果示意图，
图12是本发明实施例中掩膜和分幅前后精度对比示意图，
图13是本发明实施例中步骤4的流程示意图，
图14是本发明实施例中FCRN图像深度预测模型的图像深度预测结果示意图，
图15是本发明实施例中由深度图像生成的三维点云示意图，
图16是本发明实施例中根据室内路网地图进行纠偏的结果示意图，
图17是本发明实施例中根据栅格进行纠偏的结果示意图，
图18是本发明实施例中在手机端实现的AR导航示意图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。
在本发明的描述中，需要理解的是，在本发明的描述中，“多”的含义是两个或两个以上，除非另有明确具体的限定。此外，术语“包括”意图在于覆盖不排他的包含，例如包含了一系列步骤或单元的过程、方法、系统、产品或设备，没有限定于已列出的步骤或单元而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其他步骤或单元。
参照图1-图2流程图所示，本发明公开了一种室内定位方法，包括：
步骤1：使用单目相机采集图像并构建先验地图，所述先验地图包括视觉特征地图和室内路网地图；所述视觉特征地图包括含有视觉标签图像特征的视觉标签图像，用于供单目相机重定位；所述室内路网地图用于支撑导航路径的规划；
构建先验地图的流程如图3所示，本实施例中选取一办公楼作为实验对象，步骤1具体包括：
步骤1.1：视觉特征地图是基于视觉标签构建特征地图，利用视觉SLAM（实时定位与建图）算法获取视觉标签的图像特征，生成如图4所示的场景稠密点云图（即办公楼稠密点云图），从而构建特征地图；其中视觉标签作为位置信息的载体，按类别可分为自然标签和人工标签。步骤1.1具体包括：
步骤1.1.1：自然视觉标签选取：根据室内场景特点选择特征显著且不可移动的自然标签，如门、灯、墙角、引导牌等，以便后续进行针对性扫描。本实施例中的自然视觉标签根据办公楼特点，选择特征显著且不可移动的自然标签，包括区域引导标志、不可移动的办公桌等。
步骤1.1.2：人工视觉标签设计与布设：空旷开阔、特征稀少的场景中自然标签缺失，需要人工设计易识别的标签并布设在特定位置。本实施例中的人工视觉标签根据办公楼特点，在办公楼的空旷、纹理缺失场景，如大厅、长走廊中布设二维码这类人工视觉标签。
结合自然和人工视觉标签可以提高图像匹配的准确率，减小图像误匹配导致的相机重定位误差。
步骤1.1.3：基于视觉的稠密建图：使用RGB-D深度单目相机和成熟的视觉SLAM方案扫描整个室内场景，在获取视觉标签图像特征和单目相机运动轨迹的同时构建场景稠密点云，记录拍摄到视觉标签的单目相机位姿、图像特征点、描述子以及特征点在稠密点云中的三维坐标。本实施例中使用奥比中光Astra Pro深度相机和ORB-SLAM2方案扫描整个办公楼场景，构建场景稠密点云，记录拍摄到视觉标签的相机位姿、图像特征点、描述子以及特征点在稠密点云中的三维坐标。
步骤1.1.4：视觉特征地图构建：将所有视觉标签图像的单目相机拍摄位姿、图像特征点坐标、特征描述子以及特征点三维地理坐标存入结构体中，通过序列化的方式编码保存为二进制文件，该文件即为视觉特征地图。本实施例中将如图5所示的每张视觉标签图像的相机拍摄位姿、图像特征点坐标、特征描述子以及特征点三维地理坐标存入同一个结构体中，以序列化的方式保存为二进制bin文件，亦即特征地图，使用时可通过反序列化的方式进行解码。
步骤1.2：室内路网地图构建：室内路网地图主要包括路网节点和节点间的边（连接关系），它完整的描述了路网的拓扑结构，首先基于稠密点云生成地面栅格图像，然后利用图像处理算法自动提取路网的节点和边生成室内路网地图，通过人工目视判别室内场景的类型。步骤1.2具体包括：
步骤1.2.1：基于点云的栅格图像生成：
首先根据步骤1.1.3获取的稠密点云提取地面点，针对室内点云地面高程基本一致的特点，通过设置高程阈值提取道路地面点，本实施例中将与高程最低点之间的距离小于0.3m的点视为地面点；
然后对提取的地面点进行栅格化，将所有高程设为同一值以将点云投影至平面，然后设置栅格分辨率为0.5m*0.5m，并生成对应的二值图像，存在点云的非空栅格的像素值设为255，没有点云的空栅格的像素值设为0；
最后填充道路区域内部的孔洞，并平滑道路区域的边缘，优化更新二值图像；
步骤1.2.2：基于栅格图像的路网自动提取：首先基于距离变换算法提取路网中轴线，然后通过角点检测算法提取路网中轴线的节点并记录节点坐标，最后利用区域生长算法恢复路网节点之间的连接关系。本实施例中基于opencv的distanceTransform距离变换算法提取路网中轴线，通过opencv的goodFeaturesToTrack角点提取函数提取路网节点。
步骤1.2.3：在构建室内路网地图的同时，通过人工目视判别室内场景的类型，根据室内场景的不同特点将室内场景划分为狭窄交错和开阔空旷两种类型，狭窄交错表示室内场景过道狭窄、路径分明。构建地图的同时确定场景的类型，以便后续执行步骤4.2~步骤4.4。
本实施例中基于办公楼稠密点云生成地面栅格图像，然后利用图像处理算法自动提取路网的节点和节点间的路网边，其中某一办公室的室内路网地图如图6所示（顺时针旋转90度展示）。图6中的数字表示节点id（节点id随机生成，唯一即可），图中的边表示节点之间的联通关系、即节点间的路网边，因此，有了室内路网地图就可以通过输入起始节点和终点规划出导航路线。
步骤2：基于传统单目视觉惯性里程计（visual-inertial odometry，VIO）的实时位姿解算方法。将单目相机（例如手机）采集的图像和惯性测量单元（InertialMeasurement Unit, IMU）采集的IMU数据对齐后传入VIO中解算出每一时刻单目相机的相对位姿，所述相对位姿包括航向、位姿和轨迹信息。如图7流程图和如图8视觉惯性里程计运行过程示意图所示，主要包括图像和惯性测量单元数据预处理、初始化、局部非线性优化、回环检测和全局优化四个部分，步骤2具体包括：
步骤2.1：图像和IMU数据预处理：使用手机实时读取相机图像和IMU数据，基于图像数据提取特征点，然后利用KLT（Kanade-Lucas-Tomasi ）金字塔进行光流跟踪；基于IMU数据进行预积分，得到当前时刻的位姿、速度、旋转信息，同时计算在后端优化中将要用到的相邻帧间的预积分增量以及预积分误差的协方差矩阵和雅可比矩阵；
步骤2.2：初始化：首先进行仅视觉的初始化，解算单目相机的相对位姿；然后与IMU预积分进行对齐求解初始化参数；
步骤2.3：局部非线性优化：基于滑动窗口的局部非线性优化，将边缘化误差、IMU测量误差和视觉重投影误差放在一个大的目标函数中进行优化，以此精化局部位姿，其中滑动窗口用于控制优化变量的个数；
步骤2.4：回环检测与全局优化：将当前帧与保存的图像关键帧进行匹配，若匹配成功，则表明发生了回环，即重复经过了同一个地方；同时基于回环约束对全局位姿进行一次优化调整，输出更为精确的6自由度位姿。
步骤3：根据所述视觉特征地图，在视觉词袋（(Bag of Visual Words, BoVW）模型的基础上结合几何信息和语义信息对单目相机进行重定位，将单目相机的相对位姿变换为绝对位姿；步骤2中的视觉惯性里程计只能输出相对位姿，无法确定单目相机在已知室内环境中的绝对位姿，因此需要基于步骤1.1获取的先验视觉特征地图，利用改进的图像匹配算法，确定单目相机坐标系和世界坐标系的转换关系，从而实现单目相机的重定位。步骤3具体包括：
步骤3.1：基于优化视觉词袋模型的图像相似度计算：视觉词袋中常用视觉单词来描述图像，根据单词出现的频率来计算图像间的相似度，但缺少几何和语义信息；因此使用左右分幅策略补充图像的几何信息，使用深度学习语义分割模型强化图像的语义信息，结合所述结合几何信息和语义信息使用视觉词袋模型计算图像的相似度，几何信息和语义信息两者结合可以提高图像相似度计算的准确度。步骤3.1具体包括：
步骤3.1.1：基于YOLOv3的图像掩膜：使用YOLOv3的轻量级网络对所有视觉标签图像进行目标检测，找出待掩膜的噪声目标，如移动的行人、可移动的座椅、盆栽等；并用矩形框标注，剔除噪声目标（即所有在矩形框内）的图像特征点，得到掩膜后的图像特征点及其描述子；
步骤3.1.2：图像左右分幅：常规视觉词袋匹配忽视了图像的几何信息，只关注整幅图像的相似度，因此对图像进行左右两侧的对半分幅，将掩膜后的图像特征点和描述子分别存入左图、右图两个不同的数据库中，其中视觉特征地图中的视觉标签图像作为预存数据优先存储，实时获取的图像帧作为测试数据逐帧存储在预存数据之后；
使用轻量级的图像深度学习目标检测算法剔除可移动物体的噪声影响，同时兼顾几何信息对图像进行左右分幅，分别与视觉标签匹配后再合并相似度得分，可以提高相似度计算的鲁棒性。
步骤3.1.3：相似度计算：利用开源的视觉词袋匹配模型（DBow2库）计算当前测试图像和所有视觉标签图像的相似度；具体操作是分别计算掩膜后的所有视觉标签图像左图与当前测试图像左图、所有视觉标签图像右图与测试图像右图的相似度得分，再将两个得分相加作为最终的相似度得分，若最高得分超过预设阈值0.05，则视为两张图像匹配成功。
本实施例中移动端YOLOv3模型可以在手机移动端显示，语义提取结果如图11所示，图11中的框注表示提取的结果。随机选取41张图像进行测试，对每一张图像选取了4张得分高的图像由高到低排序，匹配成功用加粗表示，其他表示的匹配失败，掩膜和分幅后精度的对比如图12所示，图12节选了16张图片匹配的结果，表格单元格中第一个数字表示相似度、数字后紧跟的是对应的匹配图片。从图12可以看出，匹配成功率从78.05%提升到了87.80%。
步骤3.2：图像特征点提取与暴力匹配：相似度计算后确定了两张图像的匹配关系，但缺少特征点间的对应关系；因此对相似度得分超过预设阈值（本实施例中阈值为0.05）的两张图像分别进行Orb特征点提取和暴力匹配，以汉明距离（两个二进制串之间不同位数的个数）描述两个特征之间的相似程度，若汉明距离小于最小距离的两倍，则表明特征点匹配成功；最终确定当前测试图像和匹配的视觉标签图像的2d-2d特征点的对应关系；
步骤3.3：基于3d-2d点的单目相机位姿估计：根据当前测试图像和匹配的视觉标签图像的2d-2d特征点的对应关系以及视觉特征地图中视觉标签图像的2d特征点和3d地理坐标的对应关系，利用PNP（N点透视位姿求解）算法中的opencv的solvePnP算法求解对应关系之间的变换旋转向量和平移向量；使用所述变换旋转向量和平移向量对当前匹配的视觉标签图像的单目相机位姿进行平移旋转变换得到当前单目相机在世界坐标系中的绝对位姿（包括位置和姿态）。
步骤3的流程图如图9所示，本实施例中将单目相机的相对位姿变换为绝对位姿的结果如图10所示。
步骤4：如图13所示，结合所述室内路网地图、栅格地图和实景深度对单目相机的所述绝对位姿进行定位轨迹纠偏，得到纠偏后的定位轨迹。结合矢量、栅格、实景深度的多维度定位轨迹纠偏算法，可以对单目相机绝对位姿进行校正以提高定位精度。步骤4具体包括：
步骤4.1：基于实景深度的轨迹纠偏：使用图像深度预测算法估计真实场景中单目相机与障碍物的实际距离，根据所述实际距离对定位轨迹进行纠偏。步骤4.1具体包括：
步骤4.1.1：基于重力分量的单目相机图像筛选：首先要确保单目相机始终与水平面保持接近垂直的角度，使单目相机视野内的不可通行障碍物是墙体或柱体，而非天花板和地面，具体操作为：读取单目相机上传感器的重力值，计算该值在X、Y、Z三个方向上的分量GravityX、GravityY、GravityZ，根据所述三个方向上的分量计算单目相机与水平面的夹角zTheta：
zTheta=atan2(gravityZ,sqrt(GravityX^2+GravityY^2))/M_PI*180.0 ；
其中，atan2( )表示函数atan2操作，M_PI为圆周率。
若所述单目相机与水平面的夹角满足预设阈值条件，本实施例中夹角满足大于60度且小于120度，则执行步骤4.1.2，否则剔除单目相机采集的该图像；
步骤4.1.2：基于FCRN（全卷积残差网络）的图像深度预测：实时读取筛选后的单目相机采集的图像，输入全卷积残差网络的轻量级神经网络中进行场景深度预测，输出预测深度图像，本实施例中得到的预测深度图像如图14所示，图14中颜色越深表示距离越近、越浅表示越远，通过预测深度可以有效得到每个像素的深度值。将所述预测深度图像中的二维像素坐标转换为三维点云坐标；
根据下式将预测深度图像的二维像素坐标转换为三维点云坐标：
其中，x、y、z表示转换后的三维点坐标，D为深度值，
、
为单目相机焦距，u、v是
预测深度图像的像素点坐标。本实施例中转换后的三维点云如图15所示。
步骤4.1.3：基于点云的障碍物识别：筛选所述三维点云坐标中距离相机较近、即精度高的点云，将筛选后的点云网格化并根据障碍物在高程方向上垂直连续的几何特征识别出障碍物，计算单目相机和所述障碍物平面的垂线距离；
障碍物主要以墙体和柱体为主，根据障碍物在高程方向上垂直连续的几何特征可以进行自动识别；由于单目相机的深度是估计出来的，存在一定误差，且距离越远精度越低，因此首先进行点云的滤波，将距离单目相机3米外的点剔除，只保留3米内的点；然后将滤波后点云沿x、y、z三个轴按0.3m*0.3m*0.3m的步长网格化，存在点云的网格标注为1，空网格标注为0；最后计算相同x、y内，z轴上连续且标签为1的网格数量，若数量大于预设阈值6，则判断该区域为障碍物、不可通行，通过PCL库的RANSAC（随机采样一致性算法）将该区域点云拟合为平面，计算单目相机和障碍物平面的垂线距离；
步骤4.1.4：距离误差计算与校正：首先根据单目相机的当前位姿，确定二维的所述预测深度图像中单目相机在该视角下朝向的墙线或柱线，计算单目相机与最近障碍物线段的垂线距离（作为带误差的测量值）；
将所述单目相机与最近障碍物线段的垂线距离和步骤4.1.3中计算得到的所述单目相机和所述障碍物平面的垂线距离（作为真值）对比，得到两者差值即为视觉惯性里程计累积的距离误差；
若所述单目相机与最近障碍物线段的垂线和所述单目相机和所述障碍物平面的垂线接近平行且所述距离误差在预设阈值范围内，本实施例中为两条垂线夹角小于30度且距离误差在3m以内，则表明障碍物识别无误，最后从当前轨迹坐标中剔除所述距离误差在x、y轴上的分量，得到图像深度纠偏后的轨迹坐标。
步骤4.2：在实景深度的轨迹纠偏结果基础上，进行进一步的轨迹校正。根据场景的不同划分为基于室内路网地图的轨迹纠偏和基于栅格地图的轨迹纠偏两种。相对应地，根据室内场景的不同特点将室内场景划分为狭窄交错和开阔空旷两种类型，若此时的室内场景为狭窄交错则执行步骤4.3，若此时的室内场景为开阔空旷则执行步骤4.4；
步骤4.3：基于室内路网地图的轨迹纠偏：在过道狭窄、路径分明的室内场景中，根据所述室内路网地图中的路网节点和节点间的路网边对定位轨迹进行有效纠偏；
步骤4.3.1：轨迹向量计算：从步骤4.1的实景深度纠偏后的轨迹中按时序由近及
远取出一定数量的轨迹点，计算最近轨迹点A和最远轨迹点B构成的向量
；
步骤4.3.2：轨迹与室内路网地图匹配：根据向量
确定距离当前轨迹点最近的
路网边及构成该路网边的节点a和b。步骤4.3.2具体包括：
步骤4.3.2.1：实时计算最近轨迹点A离节点a、b的欧式距离；
步骤4.3.2.2：若与节点a的距离由大变小且向量
与边向量
的夹角小于预
设阈值30度，则表明当前轨迹点落在线段ab上；
步骤4.3.2.3：若与节点a的距离由小变大，则表明当前的轨迹点序列中包含了拐
点，获取由节点a出发的所有边，计算轨迹向量
与所有边的夹角；若所有夹角中的最小
值满足预设阈值条件30度，则表明当前轨迹点A落在由节点a出发且与轨迹向量夹角最小的
那条边上，将此时的边和此时的节点a作为所述距离当前轨迹点最近的路网边及构成该路
网边的节点a；
步骤4.3.2.4：使用和步骤4.3.2.2、步骤4.3.2.3相同的方法根据向量
确定距
离当前轨迹点最近的路网边及构成该路网边的节点b。
步骤4.3.3：轨迹点坐标优化：基于当前轨迹点及距离当前轨迹点最近的路网边，将当前轨迹点投影至所述距离当前轨迹点最近的路网边上，实现对定位轨迹的纠偏，具体操作为：从轨迹点往路网边作垂线，垂足点即为经过室内路网地图优化后的新的轨迹点，这样可以保证纠偏后的轨迹点完全落在路网上，兼具美观性和实用性。
本实施例中根据室内路网地图进行纠偏的结果如图16所示，图16中的细曲线表示纠偏前的轨迹，带标号的为纠偏后的轨迹，可以看出纠偏后的轨迹更符合办公室内的道路特征。
步骤4.4：基于栅格地图的轨迹纠偏：在开阔空旷的室内场景中，轨迹的行进路线难以预测，此时通过室内路网地图进行纠偏会产生较大的误差，因此选择约束更为宽松的栅格地图纠偏，构建栅格图像并计算当前轨迹点与可通行区域的几何关系进行纠偏。步骤4.4具体包括：
步骤4.4.1：栅格图像生成：获取图像的点云数据或者二维矢量地图生成栅格图像；根据数据源的差异有两种不同方法，一种是基于点云生成栅格图像，方法如步骤1.2.1，另一种是基于二维矢量地图生成，首先截取矢量地图生成图像，然后获取图像四个角点的地理坐标，保证每个轨迹点地理坐标在栅格图像上都有对应的像素位置，根据式（1）即可将轨迹点的地理坐标转换为像素坐标；式中x1、y1表示图像左上角点地理坐标，x2、y2表示图像右下角点地理坐标，W、H分别表示图像的长和高，x、y表示轨迹点的地理坐标，w、h表示轨迹点的像素坐标；
步骤4.4.2：可通行区域标注：标注所述栅格图像中的可通行栅格和不可通行栅格；针对点云栅格图像，将非空栅格标注为可通行，空栅格标注为不可通行，因为在由地面点云构建的栅格图像中栅格内有点往往表示该区域存在地面点，亦即可通行；针对基于矢量地图生成的栅格图像，根据地面和其它地物色彩上的差异，利用opencv进行灰度图和二值图的转换，以灰度值200为二值化分界线，地面像素点设置为黑色，代表可通行，地物像素点赋予白色，表示不可通行；
步骤4.4.3：基于可通行性的轨迹纠偏：首先根据式（1）计算实景深度纠偏后的轨迹点在栅格图像中的位置，若轨迹点落在所述栅格图像的可通行栅格内，则不进行纠偏；若轨迹点落在所述栅格图像的不可通行栅格内，则进行由近及远的邻域搜索直到找到最近的可通行栅格，将栅格像素坐标反算得到的地理坐标作为纠偏后的新轨迹点坐标，实现对定位轨迹的纠偏，保证所有的轨迹点都在地图的可通行区域内。
本实施例中根据栅格进行纠偏的结果如图17所示，图17中的较大圆圈表示纠偏前的轨迹，较小圆圈表示纠偏后的轨迹（较小圆圈和较大圆圈有重叠），可以看出纠偏后的轨迹已经有效避开了障碍物，保证了所有的轨迹点都在地图的可通行区域内。
基于路网与轨迹匹配的位置校正、基于可通行性分析的栅格纠偏以及基于实景深度的轨迹纠偏，结合三个层面对定位轨迹进行纠偏。基于实景深度的轨迹纠偏一直进行的，且优先执行；接着根据场景的不同分别进行栅格纠偏或路网纠偏；路网纠偏在过道狭窄、路径分明区域执行，栅格纠偏在开阔区域执行；路网纠偏和栅格纠偏不会同时进行，所有的纠偏结束后输出的轨迹坐标为最终纠偏结果。
可以有效减少视觉惯性里程计在运动过程中产生的累积误差，提高触发率和准确率的稳定性。
本实施例还公开了一种AR室内定位导航方法，使用如前所述的室内定位方法中的步骤1~步骤4得到纠偏后的定位轨迹，根据所述纠偏后的定位轨迹使用Dijkstra算法规划最优导航路线，使用AR导航实时显示所述最优导航路线。AR导航作为一种导航模式，不仅可以将真实世界呈现在相机屏幕上，还能叠加导航箭头、点位信息等虚拟模型在现有图像上以达到增强现实世界的目的，从而更加直观、准确的指引行人行进路线，本实施例中在手机端实现的AR导航示意图如图18所示。具体包括：
步骤5.1：基于室内路网地图的路径规划：根据步骤1.2生成的室内路网地图，输入起始节点和终点，利用Dijkstra算法计算最优路径，输出最优路径经过的所有节点及其坐标；
步骤5.2：AR场景渲染，根据步骤3确定的相机坐标系和世界坐标系转换关系，将节点坐标转换为相机坐标系下的坐标，并对节点按0.8m步长进行加密，每一个加密点都放置一个导航箭头渲染素材，并根据路径方向确定箭头指向，最终将确定好坐标和角度的箭头模型由相机坐标系渲染至相机界面，以实现信息增强；
步骤5.3：纠偏后AR场景更新，基于步骤4获取的纠偏后定位轨迹的坐标，按照步骤5.1和5.2重新计算导航路径并进行场景渲染，保证纠偏能力以AR导航路径实时更新的形式体现出来。
本实施例中还公开了一种室内定位系统，包括先验地图构建模块、相对位姿定位模块、重定位模块和纠偏模块。所述先验地图构建模块使用单目相机采集图像并构建视觉特征地图和室内路网地图，所述视觉特征地图包括视觉标签图像，所述视觉特征地图用于重定位单目相机，所述室内路网地图用于支撑导航路径的规划，将所述视觉特征地图传送给所述重定位模块，将所述室内路网地图传送给所述纠偏模块。所述相对位姿定位模块使用视觉惯性里程计的实时位姿解算方法，根据单目相机采集的图像和惯性测量单元采集的惯性测量单元数据解算出每一时刻单目相机的相对位姿，将所述相对位姿传送给所述重定位模块。所述重定位模块根据所述视觉特征地图，在视觉词袋模型的基础上结合几何信息和语义信息对单目相机进行重定位，将单目相机的相对位姿变换为绝对位姿，将所述绝对位姿传送给所述纠偏模块。所述纠偏模块结合所述室内路网地图、栅格地图和实景深度对单目相机的所述绝对位姿进行定位轨迹纠偏，得到纠偏后的定位轨迹。
本实施例中的室内定位方法和AR室内定位导航方法均可在手机、平板等移动终端上通过计算机程序运行实现，移动终端执行计算机程序时实现室内定位方法或AR室内定位导航方法的步骤。
本发明从先验地图构建、相机重定位算法优化、定位轨迹纠偏、AR场景渲染层面解决了传统视觉惯性里程计系统图像匹配准确率低、定位精度差、纠偏策略弱的问题，达成了基于单目移动终端的在空间结构相似、纹理特征缺失、运动物体繁多的大范围复杂场景中长时间、高精度稳定运行的实时AR定位导航效果。具体为：
（1）构建包括视觉特征地图和室内路网地图的先验地图，视觉特征地图结合完备的自然视觉标签和人工视觉标签，可以解决图像误匹配的问题；使用视觉词袋模型结合几何信息和语义信息对单目相机进行重定位，有效提高了图像匹配准确率，实现了定位轨迹的重定位。
（2）结合室内路网地图、栅格地图和实景深度对单目相机的绝对位姿进行定位轨迹纠偏，有效提高纠偏能力，减小由视觉惯性里程计导致的累积误差，提高了定位轨迹的精度和实时性。
（3）保障了室内定位的精度，提升了场景理解水平，确保了室内定位导航的稳定性、连续性和准确性；并且，只需要一部单目移动终端即可实现实时定位，无需额外布设定位基站，兼顾自然视觉标签和人工视觉标签，适用于大部分室内场景；通过在实景图像上进行信息增强，有利于使用者理解场景信息并跟随导航指引，改善导航体验，实现了实时AR导航；泛用性高、成本低廉。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质（包括但不限于磁盘存储器、CD-ROM、光学存储器等）上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备（系统）、和计算机程序产品的流程图和／或方框图来描述的。应理解可由计算机程序指令实现流程图和／或方框图中的每一流程和／或方框、以及流程图和／或方框图中的流程和／或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的步骤。
显然，上述实施例仅仅是为清楚地说明所作的举例，并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引申出的显而易见的变化或变动仍处于本发明创造的保护范围之中。
Claims (10)
1.一种室内定位方法，其特征在于，包括：
步骤1：使用单目相机采集图像并构建视觉特征地图和室内路网地图，所述视觉特征地图包括视觉标签图像，所述视觉特征地图用于重定位单目相机，所述室内路网地图用于支撑导航路径的规划；
步骤2：使用视觉惯性里程计的实时位姿解算方法，根据单目相机采集的图像和惯性测量单元采集的惯性测量单元数据解算出每一时刻单目相机的相对位姿；
步骤3：根据所述视觉特征地图，在视觉词袋模型的基础上结合几何信息和语义信息对单目相机进行重定位，将单目相机的相对位姿变换为绝对位姿；
步骤4：结合所述室内路网地图、栅格地图和实景深度对单目相机的所述绝对位姿进行定位轨迹纠偏，得到纠偏后的定位轨迹。
2.根据权利要求1所述的室内定位方法，其特征在于：根据所述视觉特征地图，在视觉词袋模型的基础上结合几何信息和语义信息对单目相机进行重定位，将单目相机的相对位姿变换为绝对位姿，具体包括：
步骤3.1：使用左右分幅策略补充图像的几何信息，使用深度学习语义分割模型强化图像的语义信息，结合所述结合几何信息和语义信息使用视觉词袋模型计算图像的相似度；
步骤3.2：对相似度得分超过预设阈值的两张图像分别进行特征点提取和暴力匹配，确定当前测试图像和匹配的视觉标签图像的2d-2d特征点的对应关系；
步骤3.3：根据当前测试图像和匹配的视觉标签图像的2d-2d特征点的对应关系以及视觉特征地图中视觉标签图像的2d特征点和3d地理坐标的对应关系，求解对应关系之间的变换旋转向量和平移向量；使用所述变换旋转向量和平移向量对当前匹配的视觉标签图像的单目相机位姿进行平移旋转变换得到当前单目相机在世界坐标系中的绝对位姿。
3.根据权利要求2所述的室内定位方法，其特征在于：所述使用左右分幅策略补充图像的几何信息，使用深度学习语义分割模型强化图像的语义信息，结合所述结合几何信息和语义信息使用视觉词袋模型计算图像的相似度，具体包括：
步骤3.1.1：使用轻量级网络对所有视觉标签图像进行目标检测，找出待掩膜的噪声目标，剔除噪声目标的图像特征点得到掩膜后的图像特征点及其描述子；
步骤3.1.2：对图像进行左右两侧的对半分幅，将掩膜后的图像特征点和描述子分别存入左图、右图两个不同的数据库中，其中视觉特征地图中的视觉标签图像作为预存数据优先存储，实时获取的图像帧作为测试数据逐帧存储；
步骤3.1.3：分别计算掩膜后的所有视觉标签图像左图与当前测试图像左图、所有视觉标签图像右图与当前测试图像右图的相似度得分，再将两个得分相加作为最终的相似度得分。
4.根据权利要求1-3任一项所述的室内定位方法，其特征在于：结合所述室内路网地图、栅格地图和实景深度对单目相机的所述绝对位姿进行定位轨迹纠偏，具体包括：
步骤4.1：使用图像深度预测算法估计真实场景中单目相机与障碍物的实际距离，根据所述实际距离对定位轨迹进行纠偏；
步骤4.2：在使用实景深度纠偏后，判断室内场景的类型，若此时的室内场景狭窄交错则执行步骤4.3，若此时的室内场景开阔空旷则执行步骤4.4；
步骤4.3：根据所述室内路网地图中的路网节点和节点间的路网边对定位轨迹进行纠偏，
步骤4.4：构建栅格图像并计算当前轨迹点与可通行区域的几何关系进行纠偏。
5.根据权利要求4所述的室内定位方法，其特征在于：所述使用图像深度预测算法估计真实场景中单目相机与障碍物的实际距离，根据所述实际距离对定位轨迹进行纠偏，具体包括：
步骤4.1.1：读取单目相机上传感器的重力值，计算所述重力值在X、Y、Z三个方向上的分量，根据所述三个方向上的分量计算单目相机与水平面的夹角；
若所述单目相机与水平面的夹角满足预设阈值条件，则执行步骤4.1.2，否则剔除单目相机采集的该图像；
步骤4.1.2：实时读取筛选后的单目相机采集的图像，输入轻量级的全卷积残差网络中进行场景深度预测，输出预测深度图像，将所述预测深度图像中的二维像素坐标转换为三维点云坐标；
步骤4.1.3：筛选所述三维点云坐标中距离相机较近的点云，将筛选后的点云网格化并根据障碍物在高程方向上垂直连续的几何特征识别出障碍物，计算单目相机和所述障碍物平面的垂线距离；
步骤4.1.4：根据单目相机的当前位姿，确定二维的所述预测深度图像中单目相机在当前视角下朝向的墙线或柱线，计算单目相机与最近障碍物线段的垂线距离；
将所述单目相机与最近障碍物线段的垂线距离和所述单目相机和所述障碍物平面的垂线距离对比，得到视觉惯性里程计累积的距离误差；
若所述单目相机与最近障碍物线段的垂线和所述单目相机和所述障碍物平面的垂线接近平行且所述距离误差在预设阈值范围内，则从当前轨迹坐标中剔除所述距离误差在x、y轴上的分量，得到纠偏后的轨迹坐标。
6.根据权利要求4所述的室内定位方法，其特征在于：根据所述室内路网地图中的路网节点和节点间的路网边对定位轨迹进行纠偏，具体包括：
步骤4.3.1：从实景深度纠偏后的轨迹中按时序由近及远取出轨迹点，计算最近轨迹点
A和最远轨迹点B构成的向量
；
步骤4.3.2：根据向量
确定距离当前轨迹点最近的路网边及构成该路网边的节点a
和b；
步骤4.3.3：将当前轨迹点投影至所述距离当前轨迹点最近的路网边上。
7.根据权利要求4所述的室内定位方法，其特征在于：所述构建栅格图像并计算当前轨迹点与可通行区域的几何关系进行纠偏，具体包括：
步骤4.4.1：获取图像的点云数据或者二维矢量地图生成栅格图像；
步骤4.4.2：标注所述栅格图像中的可通行栅格和不可通行栅格；
步骤4.4.3：计算实景深度纠偏后的轨迹点在栅格图像中的位置，若轨迹点落在所述栅格图像的可通行栅格内，则不进行纠偏；若轨迹点落在所述栅格图像的不可通行栅格内，则进行由近及远的邻域搜索直到找到最近的可通行栅格，将栅格像素坐标反算得到的地理坐标作为纠偏后的新轨迹点坐标。
8.一种室内定位系统，其特征在于：包括先验地图构建模块、相对位姿定位模块、重定位模块和纠偏模块，
所述先验地图构建模块使用单目相机采集图像并构建视觉特征地图和室内路网地图，所述视觉特征地图包括视觉标签图像，所述视觉特征地图用于重定位单目相机，所述室内路网地图用于支撑导航路径的规划，将所述视觉特征地图传送给所述重定位模块，将所述室内路网地图传送给所述纠偏模块；
所述相对位姿定位模块使用视觉惯性里程计的实时位姿解算方法，根据单目相机采集的图像和惯性测量单元采集的惯性测量单元数据解算出每一时刻单目相机的相对位姿，将所述相对位姿传送给所述重定位模块；
所述重定位模块根据所述视觉特征地图，在视觉词袋模型的基础上结合几何信息和语义信息对单目相机进行重定位，将单目相机的相对位姿变换为绝对位姿，将所述绝对位姿传送给所述纠偏模块；
所述纠偏模块结合所述室内路网地图、栅格地图和实景深度对单目相机的所述绝对位姿进行定位轨迹纠偏，得到纠偏后的定位轨迹。
9.一种AR室内定位导航方法，其特征在于：使用如权利要求1~7任一项所述的室内定位方法得到纠偏后的定位轨迹，根据所述纠偏后的定位轨迹使用路径规划算法规划最优导航路线，使用AR导航实时显示所述最优导航路线。
10.一种AR室内定位导航系统，其特征在于：包括移动终端和存储在移动终端上并可在移动终端上运行的计算机程序，所述移动终端执行所述计算机程序时实现如权利要求9所述的AR室内定位导航方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211335098.7A CN115388902B (zh) | 2022-10-28 | 2022-10-28 | 室内定位方法和系统、ar室内定位导航方法和系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211335098.7A CN115388902B (zh) | 2022-10-28 | 2022-10-28 | 室内定位方法和系统、ar室内定位导航方法和系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115388902A CN115388902A (zh) | 2022-11-25 |
| CN115388902B true CN115388902B (zh) | 2023-03-24 |
Family
ID=84115090
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211335098.7A Active CN115388902B (zh) | 2022-10-28 | 2022-10-28 | 室内定位方法和系统、ar室内定位导航方法和系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115388902B (zh) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116156142A (zh) * | 2023-01-05 | 2023-05-23 | 杭州易现先进科技有限公司 | Ar视频数据的离线渲染方法及装置 |
| CN116313020B (zh) * | 2023-05-22 | 2023-08-18 | 合肥工业大学 | 一种用于医疗服务的智能化处理方法及系统 |
| CN116433756B (zh) * | 2023-06-15 | 2023-08-18 | 浪潮智慧科技有限公司 | 一种单目相机的地表物体空间分析方法、设备及介质 |
| CN117036966B (zh) * | 2023-10-08 | 2023-12-08 | 腾讯科技（深圳）有限公司 | 地图中点位特征的学习方法、装置、设备及存储介质 |
Family Cites Families (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108489482B (zh) * | 2018-02-13 | 2019-02-26 | 视辰信息科技(上海)有限公司 | 视觉惯性里程计的实现方法及系统 |
| DE112019001657T5 (de) * | 2018-03-29 | 2021-01-14 | Sony Corporation | Signalverarbeitungsvorrichtung und signalverarbeitungsverfahren, programm und mobiler körper |
| CN109084732B (zh) * | 2018-06-29 | 2021-01-12 | 北京旷视科技有限公司 | 定位与导航方法、装置及处理设备 |
| CN111197984A (zh) * | 2020-01-15 | 2020-05-26 | 重庆邮电大学 | 一种基于环境约束下的视觉-惯性运动估计方法 |
| CN112288816A (zh) * | 2020-11-16 | 2021-01-29 | Oppo广东移动通信有限公司 | 位姿优化方法、位姿优化装置、存储介质与电子设备 |
| CN112577493B (zh) * | 2021-03-01 | 2021-05-04 | 中国人民解放军国防科技大学 | 一种基于遥感地图辅助的无人机自主定位方法及系统 |
| CN113358117B (zh) * | 2021-03-09 | 2023-05-26 | 北京工业大学 | 一种利用地图的视觉惯性室内定位方法 |
| CN112734852B (zh) * | 2021-03-31 | 2021-06-29 | 浙江欣奕华智能科技有限公司 | 一种机器人建图方法、装置及计算设备 |
| CN113358125B (zh) * | 2021-04-30 | 2023-04-28 | 西安交通大学 | 一种基于环境目标检测和环境目标地图的导航方法及系统 |
| CN115200588B (zh) * | 2022-09-14 | 2023-01-06 | 煤炭科学研究总院有限公司 | 移动机器人的slam自主导航方法及其装置 |
-
2022
- 2022-10-28 CN CN202211335098.7A patent/CN115388902B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN115388902A (zh) | 2022-11-25 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111563442B (zh) | 基于激光雷达的点云和相机图像数据融合的slam方法及系统 | |
| CN115388902B (zh) | 室内定位方法和系统、ar室内定位导航方法和系统 | |
| CN110084272B (zh) | 一种聚类地图创建方法及基于聚类地图和位置描述子匹配的重定位方法 | |
| CN105096386B (zh) | 大范围复杂城市环境几何地图自动生成方法 | |
| Badino et al. | Visual topometric localization | |
| Zhao et al. | A vehicle-borne urban 3-D acquisition system using single-row laser range scanners | |
| CN110070615A (zh) | 一种基于多相机协同的全景视觉slam方法 | |
| Le et al. | Dense piecewise planar RGB-D SLAM for indoor environments | |
| Zho et al. | Reconstructing urban 3D model using vehicle-borne laser range scanners | |
| Yue et al. | Fast 3D modeling in complex environments using a single Kinect sensor | |
| WO2021021862A1 (en) | Mapping and localization system for autonomous vehicles | |
| CN112833892B (zh) | 一种基于轨迹对齐的语义建图方法 | |
| Demantké et al. | Streamed vertical rectangle detection in terrestrial laser scans for facade database production | |
| Fiala et al. | Robot navigation using panoramic tracking | |
| Zhu et al. | Fusing GNSS/INS/vision with a priori feature map for high-precision and continuous navigation | |
| Tao et al. | Automated processing of mobile mapping image sequences | |
| Zhang | LILO: A Novel Lidar–IMU SLAM System With Loop Optimization | |
| Wu et al. | A stepwise minimum spanning tree matching method for registering vehicle-borne and backpack LiDAR point clouds | |
| CN117053779A (zh) | 一种基于冗余关键帧去除的紧耦合激光slam方法及装置 | |
| CN114459483B (zh) | 基于机器人导航用地标导航地图构建与应用方法、系统 | |
| Thompson et al. | Geometric reasoning under uncertainty for map-based localization | |
| CN112818866A (zh) | 车辆定位的方法、装置及电子设备 | |
| Baligh Jahromi et al. | Layout slam with model based loop closure for 3d indoor corridor reconstruction | |
| US20210200237A1 (en) | Feature coverage analysis | |
| Wang et al. | A point-line feature based visual SLAM method in dynamic indoor scene |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CP01 | Change in the name or title of a patent holder |
Address after: 215000 No. 101, Suhong Middle Road, Suzhou Industrial Park, Jiangsu Province Patentee after: Yuance Information Technology Co.,Ltd. Address before: 215000 No. 101, Suhong Middle Road, Suzhou Industrial Park, Jiangsu Province Patentee before: SUZHOU INDUSTRIAL PARK SURVEYING MAPPING AND GEOINFORMATION Co.,Ltd. |
|
| CP01 | Change in the name or title of a patent holder |