CN106840148A - 室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法 - Google Patents

Abstract

本发明公开了室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法，包括以下步骤：1)操作者进行环境探索，沿作业路径遍历整个作业现场环境，利用双目视觉里程计和GPS及IMU数据定位，同时创建作业环境概要地图；2)实时定位与路径引导的过程中，同时利用双目视觉里程计进行全局米制定位和利用闭环检测进行拓扑定位；3)判断出回路闭合时，利用场景特征计算出位姿偏差，对当前全局位姿进行纠偏，完成样本库更新；4)利用拓扑概要地图和实时定位结果，进行作业任务路径规划与作业路径引导提示，将信息推送给使用者。本发明针对室外环境下的设备巡检运维等一类任务的可穿戴式作业辅助系统，提供可靠、实时的作业路径引导及定位功能。

Description

室外作业环境下基于双目摄像机的可穿戴式定位与路径引导 方法

技术领域

本发明涉及室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法，属于一种可穿戴式计算技术。

背景技术

将可穿戴式系统用于设备巡检等应用领域是国内外一项最新技术，能够辅助对作业环境不熟悉的工人，按照任务规定和要求进行设备巡检作业，提高其工作效率，减轻其工作负担。而目前的可穿戴式设备及辅助技术大多集中在导盲等一类服务领域，也大多采用超声、红外等常规的传感器来简单地获取环境信息，仅提供简单避障功能，并不具备工业设备巡检及运维中最需要的点到点路径引导功能。对于视觉传感器的使用也仅仅限于简单的路标物体(如红绿灯等)图像识别，尚未有专利涉及可穿戴式视觉传感器在复杂的室外环境下进行自定位和路径引导问题。

在工业领域中，超宽带无线通信技术、近场电磁测距(NFER)系统等是目前较为成熟的定位技术。但是此类室内定位技术造价昂贵，在多金属、多遮挡区域内部署时均面临很多困难，即使增加成本密集布点，定位效果仍然差强人意，达不到预期指标。基于视觉的定位技术具有成本低廉、配置简单、不依赖人工标签、部署灵活方便的特点，已较为广泛地用于机器人导航领域，但是在工业可穿戴式设备领域的应用才刚刚起步。基于视觉的工业可穿戴式设备用于室外定位和路径引导是机器人领域的一项新技术。经专利检索查新，李成荣等人申请了中国发明专利第201110449925.0号，名称为“一种双目视觉里程计实现方法”，使用双目摄像机采集室内天花板图像对，推算摄像机的相对运动并更新运动里程信息。路丹晖等人申请了中国发明专利第201110417967.6号，名称为“基于机器视觉和惯导融合的移动机器人运动姿态估计方法，采用双目相机图像及三轴惯导数据融合，有效提高了视觉里程计的定位精度和鲁棒性。

然而，将视觉用于可穿戴式设备室外定位和路径引导存在的技术难点在于：(1室外环境下基于可穿戴式视觉的可靠、全局、米制(metric)定位；(2)如何刻画室外作业环境信息，以简要抽象的方式描述能够反映作业任务的工作点特征、环境结构特征、工作路径特征等必要信息即可；(3)在这种概要描述的作业环境地图中，如何通过场景识别(闭环检测)来进行拓扑定位，判断当前工人所处的场景点位置，即通常是需要检修的设备所处工作点位置；(4)如何根据工人的作业任务需求，对不熟悉实际现场环境的工人给出合理、友好的路径引导提示。

视觉里程计是室外环境下最常用的定位技术，考虑到室外作业环境需求，不低于GPS的定位精度才能确保准确获取自身位置以及后续的路径引导。但是基于单目摄像机来实现视觉里程计存在较大困难，很难在长距离下实现准确的定位；此外，相比移动机器人在平面或者颠簸路面行驶，人的运动一般具有更大的随机性和不平稳性，单目传感器难以胜任。只有双目和全景摄像机的视觉里程计(见“A.C.Murillo,Wearable OmnidirectionalVision System for Personal Localization and Guidance,CVPR 2012”)能够确保较长距离下的准确定位。视觉里程计仅实现了位姿推算而并非解决全局定位问题，一般需要结合GPS及IMU数据才能实现全局定位。另外，已在室内定位和导航中取得成功应用的深度视觉传感器(Young Hoon Lee,Gérard Medioni,RGB-D camera based wearable navigationsystem for the visually impaired,Computer Vision and Image Understanding,2016)由于在室外强光环境下存在成像失效性，故也不适合用于室外定位应用。

由于在工业设备巡检及运维中，工人的现场任务大多是点到点的导航(见“QianliXu,A Wearable Virtual Guide for Context-Aware Cognitive IndoorNavigation.MobileHCI2014”)，即到达目的地一般是待检修设备的工作点，因此路径引导必须依赖环境地图描述。室外作业环境的描述没有必要完全照搬用于机器人的环境栅格地图、三维点云地图等形式。可以采用简化方式，以简要抽象的方式描述能够反映作业任务的工作点特征、环境结构特征、工作路径特征等必要信息即可。

在这种概要描述的作业环境地图中，场景识别技术(见“Labbe M,MichaudF.Appearance-based loop closure detection for online large-scale and long-term operation.IEEE Transactions on Robotics,2013.”)可以用来进行拓扑定位，判断当前工人所处的场景点位置，即通常是需要检修的设备所处工作点位置。拓扑定位的结果可用于进行工作点到工作点之间的路径规划，得到最短或其它意义上最优的作业轨迹并向工人推荐；还可以通过与原地图中的节点比对来纠正视觉里程计实时定位结果，克服视觉里程计只能开环地进行位姿推算、容易累计定位误差的缺点。

在路径引导方面，面向人的可穿戴式辅助系统与面向机器人的导航控制系统有着明显的差异。人的自主性与环境认知能力相对于机器人更强且人也无法做到严格的按照规划出的路径行走，因此面向于个人的定位与路径引导不需要实时提供严格的线速度与角速度，而是需要更多的环境信息，例如当前行走的线路、位置与应沿着的行走方向等，来指导使用者。

总之，可穿戴式技术已在服务辅助等领域初步用于定位和路径引导，但是针对工业设备巡检及运维领域的应用尚少见报道，存在诸多技术难点亟待解决。

发明内容

发明目的：解决室外作业环境的建图问题以及室外作业环境下定位与全局路径引导问题，对于不熟悉作业环境的使用者有很好的指导作用。

技术方案：本发明提出了一种室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法，包括以下步骤：

步骤1，操作者穿戴包含双目视觉传感器及随身处理计算机的可穿戴式辅助系统进行环境探索，沿着室外作业路径遍历整个作业现场环境，利用双目视觉里程计和GPS及IMU数据进行定位，同时创建包含场景点、作业路径在内的作业环境概要地图；

步骤2，实时定位与路径引导的过程中，同时利用双目视觉里程计进行全局米制定位和利用闭环检测进行拓扑定位；

步骤3，判断出回路闭合即重新访问了某个已创建过的场景点时，利用场景特征由粗到细配准算法计算出当前与关键定位点的位姿偏差，利用该偏差对当前全局位姿进行纠偏，并完成场景样本库的自动更新；

步骤4，利用拓扑概要地图和实时定位结果，进行作业任务路径规划与引导作业路径引导提示，将工人完成巡检运维任务所需提示的位置、作业路径等信息推送给使用者。

进一步的，步骤1中操作者佩戴着可穿戴式辅助系统进行环境探索，沿着作业路径遍历整个作业现场环境并建立作业环境概要地图，在探索初始时利用GPS数据和IMU偏航角作为全局位姿初值，用双目视觉里程计算法进行增量式位姿推算，并通过坐标转换计算出全局位姿，同时随着探索过程沿途创建拓扑结构地图，包括场景点及场景点之间的连接边，软件内部自动创建节点数据结构并保存场景图像模板、位置、语义标记、相邻场景点之间的有向路径等信息，最终完成环境概要地图及场景样本库的创建。

进一步的，步骤1中所述的环境概要地图刻画了含有感兴趣设备的场景点位置、场景图像特征、场景点之间的路径等核心信息。

进一步的，步骤2中拓扑定位利用当前实时获取的环境图像与已预先创建的环境概要地图中保存的场景点中的图像进行闭环检测，闭环检测采用贝叶斯滤波器估计闭环概率。若与地图中的某一场景点闭环，则通过初始位姿偏差补偿计算得出当前的全局位置。

进一步的，步骤3在在判断出回路闭合即重新访问了某个已创建过的场景点时，利用场景特征由粗到细配准算法计算出当前与关键定位点的位姿偏差，然后利用该偏差对当前全局位姿进行纠偏，同时纠正之前走过的路径点上的位姿，得到正确的全局路径，里程计后续的轨迹计算均叠加在此初始全局位姿上，则后续轨迹也均是咋全局坐标系下，对于无法检测到回路闭合(Loop closure)的关键定位点，将当前场景加入场景样本库，从而完成场景样本库的自动更新。

进一步的，步骤3中由粗到细的配准算法包括粗配准和精配准两步，粗配准步骤针对SURF特征匹配对采用RANSAC算法来剔除不准确的匹配点对，利用匹配点对映射到三维空间的点云作为初始数据，用来计算两幅场景的初始变换关系；细配准步骤采用广义的迭代最近点算法来进行两张图像间的精确配准。

进一步的，步骤4中考虑到室外作业环境中工作人员的行走路径通常受道路约束而非全空间可达，在拓扑概要地图的拓扑图(Graph)结构中进行作业路径规划，规划出达到作业点最短的路径，并在检测到闭环时进行动态重规划，系统将当前位置、目标位置、推荐行走的线路、当前应沿着行走方向箭头等信息推送给用户作为引导提示，并在其完全偏离预定路径或朝着错误方向行走时给出警告。

有益效果：本发明提供的室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法，相比现有技术，具有以下有益效果：

1.考虑到室外巡检作业环境下工人的行走路径存在一定约束，即通常巡检工人的行走路径大多沿着特定的道路，而并非全空间都可达，因此针对解决可穿戴式辅助系统室外作业环境下全局路径引导问题，提出一种混合式概要地图，同时包含米制路径与关键定位点信息，既保留了拓扑地图精简信息的特征，又能够支持沿作业路径的全局路径引导。

2.为了克服视觉例程仅能进行位姿推算不能进行全局定位的缺陷，提出两点措施。首先，在初始定位中采用GPS数据叠加视觉里程计输出实现全局定位，其次在实时定位的初始化阶段，采用闭环检测和图像特征配准，来补偿实际初始位姿的偏差，从而达到准确、全局定位的结果。

3.充分考虑了个人化路径引导与作业环境的特点，通过基于概要地图的拓扑路径规划得到作业点之间的最短连接边，又通过调用保存在概要地图中的作业路径来向使用者推送显示路径，从而完成对人的路径引导，确保了实际效果直观友好。

附图说明

图1为作业环境概要地图创建流程；

图2为初始全局位姿计算示意图；

图3为某室外测试环境的概要地图，其中示意了巡检作业路径和若干关键定位点；

图4为图3对应的视觉定位结果轨迹；

图5为可穿戴式视觉定位及路径引导方法流程；

图6为闭环检测原理示意；

图7为基于闭环检测的位姿偏差计算和纠正；

图8给出了测试环境下的路径引导结果，其中，(a)为某时刻三维视角下的路径引导界面，(b)为当前时刻双目摄像机中左眼相机采集的画面，(c)为对应场景。

具体实施方式

下面结合附图和具体实施例，进一步阐明本发明。

本发明所采用的可穿戴式定位与路径引导技术，是先由工程师佩戴可穿戴式辅助系统(Wearable Assisted System,WAS)进行环境探索并创建作业环境的概要地图；之后工人在现场佩戴该系统进行辅助作业时，系统根据已有的作业环境概要地图为使用者提供实时定位和路径引导功能。

本发明实施所采用的可穿戴式辅助系统原型样机，前台设备的硬件组成包括支撑传感器安装的轻质头盔一个、可USB供电的Zed Camera双目摄像机一个、便携式笔记本PC一台、GPS定位器或者带有GPS的智能手机一部、惯性测量单元(IMU)一个，后台设备为服务器PC一台，前台设备与后台服务器之间需要通过网络通信互联，后台服务器上的各个任务也进行任务间通信和协调。

本发明所提出的方法，具体包括以下步骤：

步骤1，操作者穿戴WAS系统进行环境探索，沿着作业路径遍历整个作业现场环境并建立作业环境概要地图。在探索初始时利用GPS数据和IMU偏航角作为全局位姿初值，用双目视觉里程计算法进行增量式位姿推算，并通过坐标转换计算出全局位姿；同时随着探索过程沿途创建拓扑结构地图，包括场景点及场景点之间的连接边。软件内部自动创建节点数据结构并保存场景图像模板、位置、语义标记、相邻场景点之间的有向路径等信息，最终完成环境概要地图及场景样本库的创建。此步骤的流程如图1所示。

步骤1包括两个子步骤，具体实施方法如下。

步骤111：采用GPS数据和IMU偏航角作为全局位姿初值，用双目视觉里程计算法进行增量式位姿推算，并通过坐标转换计算出全局位姿。

双目视觉里程计算法是根据帧间特征点的匹配和跟踪，得到摄像机在前后两个时刻对应的特征点对的三维坐标，从中恢复出这两个相邻时刻摄像机的运动参数估计值。设机器人在运动前后两个时刻通过SURF特征点匹配和跟踪得到特征点在摄像机坐标系下的三维坐标分别为P_t-1,j和P_t,j，R和T分别为摄像机相对运动的旋转矩阵和平移矩阵，则有如下关系：

P_t,j＝RP_t-1,j+T (1)

为了计算最优的R和T矩阵，构造对所有点对的运动估计误差累计并令其最小：

其中上式中的n为匹配点对数目。为了求解上述带约束的非线性优化问题，并且考虑到特征匹配与跟踪中存在的外点(outliers)对求解的影响，采用奇异值分解(SVD)并结合RANSAC算法来克服外点的影响，最终得到R和T分别为摄像机在两帧之间的相对运动旋转矩阵和平移矩阵。

根据以上计算，双目视觉里程计增量式地输出相邻帧之间的摄像机相对运动，设R_t和T_t分别为t时刻相对t-1时刻的旋转矩阵和平移矩阵。设定全局坐标系为平面坐标系(X轴为正东方向，Y轴为正北方向)，令平面坐标系原点作为全局坐标系的原点，选取大地的平面坐标系作为全局坐标系，一是统一此里程计的坐标与实际地图坐标一致，可以在将来的应用中与第三方地图相衔接；二是使里程计得出的轨迹有实际的方向参考价值，可以给使用者指示实际行走的方向。将此时的GPS的读数记为GPS数据没有偏航角信息；由于考虑作业环境下二维平面运动的一般情况，仅需偏航角即可得出初始方向，偏航角由惯性测量单元(IMU)测算得到，记为θ₀。通过经纬度坐标以及偏航角θ₀，将当前摄像机位置在全局坐标系下计算得出摄像机在地平面X、Y方向的位置(x₀,y₀)。

初始时刻全局坐标系(O_G)与视觉里程计坐标系(O_p)位置关系如图2所示，即视觉里程计坐标系与全局坐标系存在固定的变换关系：平移矩阵可由(x₀,y₀)计算得出，旋转矩阵可由θ₀计算得出。设视觉里程计坐标系变换到全局坐标系的变换关系为T_g，则：

其中设初始全局位姿为T_g0，初始视觉里程计相对位姿为T₀，可得初始全局位姿为

T_g0＝T_gT₀ (4)

简写成三维坐标的形式为

P_g＝R_gP₀+t_g (5)

其中P₀为视觉里程计初始相对位姿，即为(0,0,0)^T，P_g是变换到全局坐标系下的结果。同理，第n帧求解出的视觉里程计全局位姿T_gn为：

T_gn＝R_gT_n+t_g (6)

步骤112：在操作者穿戴WAS系统进行环境探索的过程中，随着探索过程同时沿途创建拓扑结构地图，包括场景点及场景点之间的连接边。软件内部自动创建节点数据结构并保存场景图像模板、位置、语义标记、相邻场景点之间的有向路径等信息，最终完成环境概要地图及场景样本库的创建。

环境概要地图包括场景点及其之间的连接边。场景点又分为两大类：一般关键场景点与感兴趣场景点，一般关键场景点仅用于闭环检测，也可通过对地图离线编辑将关键场景点转化为感兴趣场景点，感兴趣场景点除了用于闭环检测，还含有环境语义信息，对应到实际作业环境中，即为需要前往作业的目标位置，可用于作业路径规划时作为候选目标点。

本发明在具体实施中采用如下的关键点创建策略。对于关键场景点采用固定时间间隔法创建，即系统运行时每隔2s创建一个关键场景；而对于感兴趣场景点，由于考虑在室外作业环境下一般为待检修设备位置，故在本发明实施中采用的是手动创建方式。

一个场景点对应的数据结构保存场景图像模板、位置、语义标记。一个场景点可表示为L＝{id,mi,n,x,y,im}，其中id、mi、n、x、y、im分别表示场景编号、场景在地图中的编号、场景名称(使地图含有语义信息)、场景位置与场景图像模板。

相邻场景点之间的连接边即保存了节点之间的拓扑连接逻辑关系，又关联存储了工人建图时在这两个场景点之间行走的有向米制(metric)路径片段。由于考虑室外作业环境下工人一般沿作业路径行走而非全空间可达，因此这种米制路径片段可用于在路径引导时供调用显示。设t时刻创建了一个关键场景点，此时在拓扑地图中加入一个节点，记为location_k，并建立与上一个关键场景地点location_k-1之间的有向连接边location_k-1→location_k；对于该节点同时关联保存其数据结构L，此时的场景图像I_k加入场景图像库，以及上一个场景点到当前场景点之间的摄像机运动轨迹traj_k＝{p_t′,...,p_t}，其中t′和t分别为上一个场景点和当前场景点创建的时刻。

设遍历完整个环境后共得到K个节点，对应的场景图像特征点集合为{D₁,...,D_K}，对应的场景点为{p₁,...,p_K}，保存的路径段序列为{traj₁,...,traj_K}，就是当前更新后的环境概要地图。图3给出了一个室外作业环境的概要地图实例，对应的行径轨迹在图4中给出。该例中总行走长度约为300m，图中粗线为GPS轨迹，细线为视觉里程计输出轨迹，圆点为实际起点，星型为实际终点。

步骤2、3、4对应的可穿戴式视觉定位及路径引导方法流程如图5所示。

步骤2，实时定位与路径引导的过程中，同时利用双目视觉里程计进行全局定位和利用闭环检测进行拓扑定位。拓扑定位利用当前实时获取的环境图像与已预先创建的环境概要地图中保存的场景点中的图像进行闭环检测，若与地图中的某一场景点闭环，则通过初始位姿偏差补偿计算得出当前的全局位置。

拓扑定位过程如下：使用者在作业环境中的任意位置开始，系统中的双目视觉里程计模块与拓扑定位模块同时实时运行，采用贝叶斯滤波器估计闭环概率，即通过估计当前场景点和概要地图中的场景点形成闭环的概率记录闭环假设。在使用者开始走动后一旦检测出闭环事件，则拓扑定位进行初始位姿偏差补偿计算，得出初始全局位置，后续的里程计实时得出的位置即为全局位置。图6给出了闭环检测原理示意。

步骤3，判断出回路闭合即重新访问了某个已创建过的场景点时，利用场景特征由粗到细配准算法计算出当前与关键定位点的位姿偏差。然后利用该偏差对当前全局位姿进行纠偏，同时纠正之前走过的路径点上的位姿，得到正确的全局路径，里程计后续的轨迹计算均叠加在此初始全局位姿上，则后续轨迹也均是咋全局坐标系下。对于无法检测到回路闭合(Loop closure)的关键定位点，将当前场景加入场景样本库，从而完成场景样本库的自动更新。

步骤3包括以下两个具体步骤。

步骤311：首先是对于判断结果为匹配的情况，利用场景特征由粗到细配准算法，计算出当前场景与模板场景对应位姿之间的位姿变换关系。

假设I₁是当前场景图像，I₂是与之匹配的场景点图像模板，当使用者初始化定位时，拓扑定位检测到闭环，但是实际获取的场景图像与模板库中保存的场景模板图像一定具有偏差。所述的场景特征由粗到细配准算法，就是利用图像特征匹配及配准，恢复I₁和I₂之间的旋转关系矩阵R和平移关系矩阵T。

图7为基于闭环检测的位姿偏差计算和纠正原理示意。设X_W为全局坐标系，X_K为摄像机坐标系，环境中的空间景物在不同扫描视点下成像，世界坐标系下环境中的物体由绿色表示，每帧图像上红色点是二维成像点。以上的配准过程最终实现的是，从两幅连续图像数据的重叠区域中找出若干对相应特征，根据对应特征来求解旋转矩阵R和平移向量T。

由粗到细的配准算法包括粗配准和精配准两步。

粗配准步骤针对SURF特征匹配对采用RANSAC算法来剔除不准确的匹配点对，排除可能存在的误匹配点的影响，利用匹配点对映射到三维空间的点云作为初始数据，用来计算两幅场景的初始变换关系，使得两组点集数据的公共区域能够大致重合，并消除了外点对为下一步精确匹配的影响。

在执行RANSAC时，抽样次数K是一个十分关键的参数，关系到配准速度以及配准精度。如果K值过大，那么迭代次数太多，就会使配准速度相对较慢；相反，若K值过小，则会导致很难抽到一个好的样本。设源数据集S中数据点数量为N，内点数量所占比例为w。从S中随机地抽取一个大小为h的样本，从前文的分析可知，h的值应该取3，则使K次抽样样本，均为坏样本的概率足够小，即保证K次抽样，获得一个好样本的概率非常大，设p为K次抽样样本至少含有一个好样本的概率，则：

1-p＝(1-w^h)^K (8)

则抽样次数K为：

上式给出了为要获得好样本的概率p条件下，即获得正确估计的概率p的条件下，要给出的抽样次数的K的取值。

对特征匹配对进行选择计算时会产生很多对候选的[R′,T′]，对应每一对[R′,T′]会有一些特征内点，保留最多的特征内点对及与最多内点数在一定阈值范围内对应的[R′,T′]，在候选的多对[R′,T′]中，对每一对旋转平移矩阵都计算相对应的匹配度，这样每一对[R′,T′]就会存在一个匹配度，选择当匹配度最大时所对应的[R′,T′]作为此两场景数据帧间的最佳位姿偏差即[R,T]。

由于从不同角度获得的场景图像，彼此之间仅仅是部分重叠，重叠区域占总体图像特征的比率一般较低，RANSAC算法获得的结果不一定是全局最优，这就要求两个数据集的初始相对位置，不能与真实的相对位置相差太大，因此，ICP算法常常被用来对初始配准的结果作进一步的精细配准。细配准步骤采用广义的迭代最近点(G-ICP)算法来进行两张图像间的精确配准，它与ICP算法的主要区别是在计算残留误差时引入了概率模型。由于G-ICP算法仅仅对上一步已经剔除了外点的剩余内点进行处理，因此配准的准确性和计算效率大大提高。

步骤312：对当前位姿和已走过的路径上的位姿进行偏差补偿。利用上一步求得的位姿偏差即旋转矩阵和平移向量[R,T]对当前全局位姿进行纠偏，同时纠正之前走过的路径点上的位姿，得到正确的全局路径。

假设在上一步已经检测到当前地点p_t与某个已训练过的关键定位点location_m形成闭环，即重新访问，节点location_m存储了获取该关键定位点时的全局位姿p_m，上一步骤计算得到的位姿偏差[R,T]描述了p_t与p_m之间的位姿变换关系。因此，对当前地点p_t进行如下计算得出全局位姿p_gt：

p_gt＝Rp_m+T (10)

并对之前的所有轨迹进行，如上一时刻t-1时的原位姿为p_t-1，设t-1时刻到t时刻的旋转矩阵与平移矩阵分别为R_t-1与T_t-1，则拓扑定位前有如下关系：

p_t＝R_t-1p_t-1+T_t-1 (11)

则p_t-1的全局位姿p_g(t-1)为

将式(11)带入，得

最后对于无法检测到回路闭合(Loop closure)的关键定位点，将当前场景加入场景样本库，从而完成场景样本库的自动更新。

步骤4，利用拓扑概要地图和实时定位结果，进行作业任务路径规划与引导作业路径引导提示。考虑到室外作业环境中工作人员的行走路径通常受道路约束而非全空间可达，因此在拓扑概要地图的拓扑图结构中进行作业路径规划，并将当前位置、目标位置、推荐行走的线路、当前应沿着行走方向箭头等信息推送给用户，实现对使用者引导提示，并在使用者完全偏离规划路径或朝着错误方向行走时，向使用者提出警告。

使用者在系统完成拓扑定位得出初始全局位后，视觉里程计的实时位姿计算均是在拓扑概要地图的坐标系下,即人在实际环境中的位置与里程计计算的结果是一致的。因此可利用拓扑概要地图，进行作业任务路径规划与引导。使用者通过可穿戴系统的人机交互设备设定导航目标点即某个感兴趣场景点，概要地图中可能存在多条从当前位置到目标位置的路径。系统根据概要地图中保存的场景点拓扑图规划出一条最短的路径，此路径即为一个场景点序列L{l₁,l₂,...,l_n}，将此场景点序列通过场景点的地图编号mi映射到路径地图中路径点集，即可规划出从当前位置到目标位置的路径P{p₁,p₂,...,p_n}，P为概要地图路径点集的一个子集，此路径通过叠加在全局地图中，显示在界面上，方便与使用者交互。

由于规划出的路径为创建环境概要地图时的路径的子集，使用者在行走过程中，并不需要严格沿着规划出的路径行走，只需沿此方向行走，由于有作业道路的约束，作业人员的行走路径与规划出的路径不会偏差太大；在引导过程中，若拓扑定位检测到闭环，则路径重规划，因此可以满足作业环境中个人路径引导的要求。

当系统根据当前的里程计得出的位置与导航目标点的距离小于设定的阈值d_m，此处d_m＝0.5m，或者拓扑定位的闭环检测模块检测到当前场景与导航目标点的场景模板闭环，则认为到达目标点，结束本次路径引导，提示使用者，已经到达目标位置。

图8给出了测试环境下的路径引导结果。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (7)

1.一种室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法，其特征在于，包括以下步骤：

步骤1，操作者穿戴包含双目视觉传感器及随身处理计算机的可穿戴式辅助系统进行环境探索，沿着室外作业路径遍历整个作业现场环境，利用双目视觉里程计和GPS及IMU数据进行定位，同时创建包含场景点、作业路径在内的作业环境概要地图；

步骤2，实时定位与路径引导的过程中，同时利用双目视觉里程计进行全局米制定位和利用闭环检测进行拓扑定位；

步骤3，判断出回路闭合即重新访问了某个已创建过的场景点时，利用场景特征由粗到细配准算法计算出当前与关键定位点的位姿偏差，利用该偏差对当前全局位姿进行纠偏，并完成场景样本库的自动更新；

步骤4，利用拓扑概要地图和实时定位结果，进行作业任务路径规划与引导作业路径引导提示，将工人完成巡检运维任务所需提示的位置、作业路径等信息推送给使用者。

2.根据权利要求1所述的室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法，其特征在于：步骤1操作者佩戴着可穿戴式辅助系统进行环境探索，沿着作业路径遍历整个作业现场环境并建立作业环境概要地图，在探索初始时利用GPS数据和IMU偏航角作为全局位姿初值，用双目视觉里程计算法进行增量式位姿推算，并通过坐标转换计算出全局位姿，同时随着探索过程沿途创建拓扑结构地图，包括场景点及场景点之间的连接边，软件内部自动创建节点数据结构并保存场景图像模板、位置、语义标记、相邻场景点之间的有向路径等信息，最终完成环境概要地图及场景样本库的创建。

3.根据权利要求1或2所述的室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法，其特征在于：步骤1中所述的环境概要地图刻画了含有感兴趣设备的场景点位置、场景图像特征、场景点之间的路径等核心信息。

4.根据权利要求1所述的室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法，其特征在于：步骤2中拓扑定位利用当前实时获取的环境图像与已预先创建的环境概要地图中保存的场景点中的图像进行闭环检测，若与地图中的某一场景点闭环，则通过初始位姿偏差补偿计算得出当前的全局位置。

5.根据权利要求1所述的室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法，其特征在于：步骤3在判断出回路闭合即重新访问了某个已创建过的场景点时，利用场景特征由粗到细配准算法计算出当前与关键定位点的位姿偏差，然后利用该偏差对当前全局位姿进行纠偏，同时纠正之前走过的路径点上的位姿，得到正确的全局路径，里程计后续的轨迹计算均叠加在此初始全局位姿上，则后续轨迹也均是咋全局坐标系下，对于无法检测到回路闭合的关键定位点，将当前场景加入场景样本库，从而完成场景样本库的自动更新。

6.根据权利要求1或5所述的室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法，其特征在于：步骤3中由粗到细的配准算法包括粗配准和精配准两步，粗配准步骤针对SURF特征匹配对采用RANSAC算法来剔除不准确的匹配点对，利用匹配点对映射到三维空间的点云作为初始数据，用来计算两幅场景的初始变换关系；细配准步骤采用广义的迭代最近点算法来进行两张图像间的精确配准。

7.根据权利要求1所述的室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法，其特征在于：步骤4中考虑到室外作业环境中工作人员的行走路径通常受道路约束而非全空间可达，在拓扑概要地图的拓扑图结构中进行作业路径规划，规划出达到作业点最短的路径，并在检测到闭环时进行动态重规划，系统将当前位置、目标位置、推荐行走的线路、当前应沿着行走方向箭头等信息推送给用户作为引导提示，并在其完全偏离预定路径或朝着错误方向行走时给出警告。