Humanoid navigation using a visual memory with obstacle avoidance ${}^{\text{a}}$${}^{\text{a }}$^("a "){ }^{\text {a }}
利用视觉记忆进行具有人形避障功能的导航 ${}^{\text{a}}$${}^{\text{a }}$^("a "){ }^{\text {a }}

Received 8 February 2018  2018 年 2 月 8 日收到
Received in revised form 11 August 2018
2018 年 8 月 11 日修订后收到
Accepted 24 August 2018
2018 年 8 月 24 日接受
Available online 7 September 2018
2018 年 9 月 7 日在线发布

Visual navigation  视觉导航
Humanoid robots  人形机器人
Visual memory  视觉记忆
Obstacle avoidance  障碍物避让

© 2018 Elsevier B.V. All rights reserved.
© 2018 Elsevier B.V. 保留所有权利。

One of the main objectives to be reached by a humanoid is to mimic human navigation strategies. An important feature of the human brain is the ability to visually memorize key scenes of previously visited places for facilitating a subsequent navigation in the same place [1]. The idea of using a sequence of key images for robot navigation was early introduced in [2], where the visual memory (VM) was called view-sequenced route representation. A VM is a topological map that represents an environment by a set of key images [3,4], which are typically organized as a directed graph where each node is a key image and the edges provide information
类人机器人的主要目标之一是模仿人类的导航策略。人脑的一个重要特征是能够视觉记忆先前访问地点的关键场景，从而促进在同一地点的后续导航[1]。利用关键图像序列进行机器人导航的思想最早在文献[2]中提出，其中视觉记忆（VM）被称为视图序列路径表示。视觉记忆是一种拓扑地图，通过一组关键图像来表示环境[3,4]，这些关键图像通常组织成有向图，每个节点代表一个关键图像，边则提供关键图像之间的关系信息。

of a relation between key images. Once this representation of the environment is known, the problems of robot localization, path planning and autonomous navigation can be solved.
一旦环境的这种表示被确定，机器人定位、路径规划和自主导航的问题便可得到解决。

Visual servo control schemes are built upon the sensor-based control approach, taking advantage of the rich information of images acquired by a vision system to command the motion of a robot. However, the scope of visual-servo controllers is limited to positioning tasks where the visual scene is partially observed from the initial and target locations [5-7]. This is due to the field-ofview constraint of typical vision systems [8]. This local behavior appears in any of the two classical approaches of visual servoing: the position-based scheme (PBVS), where the estimation of some 3D pose parameters is needed; and the image-based scheme (IBVS), where direct feedback of image features is used [9]. The extension suggested in $[2,10]$$[2,10]$[2,10][2,10] takes advantage of a sequence of target images (VM), which share visual information between each pair of consecutive images, and the image acquired at the initial location shares information with one of the target images. In this
视觉伺服控制方案建立在基于传感器的控制方法之上，利用视觉系统获取的丰富图像信息来指挥机器人的运动。然而，视觉伺服控制器的应用范围仅限于视觉场景在初始和目标位置部分可见的定位任务[5-7]。这是由于典型视觉系统的视场限制[8]。这种局部行为出现在视觉伺服的两种经典方法中：基于位置的方案（PBVS），需要估计某些三维位姿参数；以及基于图像的方案（IBVS），使用图像特征的直接反馈[9]。文献 $[2,10]$$[2,10]$[2,10][2,10] 中提出的扩展利用了一系列目标图像（视觉记忆，VM），这些图像之间共享视觉信息，且初始位置采集的图像与其中一幅目标图像共享信息。在这种
manner, the robot enlarges its workspace as the images associated to the initial and final locations do not need to share information. This strategy has been mainly exploited for navigation of wheeled mobile robots [2,4,11,12].
方式下，机器人扩大了其工作空间，因为与初始和最终位置相关联的图像不需要共享信息。这一策略主要被用于轮式移动机器人的导航[2,4,11,12]。

In this paper, we propose a humanoid navigation strategy based on a VM, which encloses robot localization, visual path planning and path following with obstacle avoidance. From a minimum number of detected point features, the proposed strategy computes continuous velocities that can be applied in indoor environments with natural scenes, like corridors, uncluttered or cluttered environments.
本文提出了一种基于视觉记忆（VM）的类人导航策略，该策略涵盖机器人定位、视觉路径规划以及带有障碍物避让的路径跟随。通过最少数量的检测点特征，所提策略计算出可应用于具有自然场景的室内环境（如走廊、无杂物或杂乱环境）的连续速度。

This paper is an incremental work with respect to our previous results in $[13,14]$$[13,14]$[13,14][13,14]. In the first reference, we have proposed a visual path following scheme for humanoid robots that is generic in the sense that it can be used for different types of visual servo controllers. In the second reference, we have extended the scheme by solving the initial humanoid localization problem in the VM to plan a visual path when the VM has several forks and/or cycles. As extensions to those works, in this paper we propose an enhanced pure vision-based localization algorithm that first finds a neighborhood of key images around the image currently observed by the robot, to later perform a local search for the key image that best fits the current image in terms of common visual information. In addition, we incorporate a versatile obstacle avoidance strategy that is aided by depth information from an RGB-D camera mounted on the robot’s head. The strategy deals with static obstacles that appear during the autonomous navigation, and that were not present during the teaching phase to generate the VM. Besides, the unexpected obstacles are localized in the VM, and they are taken into account for subsequent navigation tasks.
本文是在我们之前成果 $[13,14]$$[13,14]$[13,14][13,14] 基础上的增量工作。在第一个参考文献中，我们提出了一种适用于人形机器人视觉路径跟踪的方案，该方案具有通用性，可用于不同类型的视觉伺服控制器。在第二个参考文献中，我们通过解决视觉记忆（VM）中人形机器人初始定位问题，扩展了该方案，以便在 VM 存在多个分叉和/或环路时规划视觉路径。作为这些工作的扩展，本文提出了一种增强的纯视觉定位算法，该算法首先在机器人当前观察图像周围找到一组关键图像邻域，随后进行局部搜索，以确定在共有视觉信息方面最匹配当前图像的关键图像。此外，我们引入了一种多功能的障碍物规避策略，该策略借助安装在机器人头部的 RGB-D 相机提供的深度信息。该策略能够处理自主导航过程中出现的静态障碍物，这些障碍物在生成视觉记忆的教学阶段并不存在。 此外，意外障碍物在视觉记忆（VM）中被定位，并在后续导航任务中予以考虑。

The main contribution of this work is a novel vision-based navigation scheme for humanoids formulated in a unifying framework of consecutive and hierarchical tasks (visual servoing and obstacle avoidance tasks), for which, the transitions between tasks are performed using smooth functions to keep the balance of the humanoid platform. The generation of continuous velocities that are given to the walking pattern generator (WPG) is particularly important for visual control of humanoids [7]. Task transitions occur when a new target image is given, or when an obstacle is detected for stepping over or circumvent it while maintaining the visual target in sight during the navigation. The mobility of the humanoid robot is exploited to walk laterally when it is needed as well as to step over obstacles. Thus, no motion constraints are imposed to the robot. An extensive experimental evaluation using the NAO platform shows the good performance of the navigation scheme.
本工作的主要贡献是一种新颖的基于视觉的人形机器人导航方案，该方案在连续且分层的任务（视觉伺服和避障任务）统一框架下构建，任务之间的切换通过平滑函数实现，以保持人形平台的平衡。连续速度的生成并传递给步态生成器（WPG）对于人形机器人的视觉控制尤为重要[7]。当给出新的目标图像，或检测到障碍物需要跨越或绕行时，任务切换发生，同时在导航过程中保持视觉目标在视野内。人形机器人的机动性被充分利用，在需要时进行侧向行走以及跨越障碍物。因此，对机器人没有施加运动约束。基于 NAO 平台的广泛实验评估展示了该导航方案的良好性能。

Our proposal aims to use 2D information as much as possible, such that the only available information for navigation is a set of key images and the current view. The generation of the VM is done offline, i.e., the key images to form the VM are acquired and selected by means of a supervised teaching phase (humanguided). Depth information is just a complement to facilitate the obstacle avoidance task. We have left as future work the detection of obstacles relying only on monocular vision. The main advantage of using 2D information instead of 3D is the reduction of computational requirements, which are poor in commercial and small-sized humanoid platforms like NAO, since there is no need of generating and updating a complex spatial map. For instance, in the proposed localization method no extra information is needed, like odometry or sensor fusion.
我们的方案旨在尽可能多地使用二维信息，使得导航时唯一可用的信息是一组关键图像和当前视图。视觉记忆（VM）的生成是在离线状态下完成的，即通过监督教学阶段（人工引导）获取并选择形成 VM 的关键图像。深度信息仅作为辅助，以便于障碍物规避任务。我们将仅依靠单目视觉进行障碍物检测作为未来工作。使用二维信息而非三维信息的主要优势在于降低计算需求，而商业化和小型人形机器人平台如 NAO 的计算能力有限，因为无需生成和更新复杂的空间地图。例如，在所提出的定位方法中，不需要额外的信息，如里程计或传感器融合。

The rest of the paper is structured as follows: Section 2 presents a brief account of related work. Section 3 presents an overview of the whole navigation strategy. Section 4 details the structure of the graph that represents the VM. Section 5 describes the proposed localization algorithm and the path planning stage. Section 6 presents the proposed visual path following scheme with smooth transitions of visual tasks. Section 7 describes the obstacle avoidance strategies. Section 8 presents the experimental evaluation of the stages of the method and its integration. Finally, Section 9 gives some concluding remarks and ideas of future work.
本文余下部分结构如下：第 2 节简要介绍相关工作。第 3 节概述整体导航策略。第 4 节详细说明表示视觉记忆（VM）的图结构。第 5 节描述所提出的定位算法及路径规划阶段。第 6 节介绍带有视觉任务平滑过渡的视觉路径跟随方案。第 7 节阐述障碍物避让策略。第 8 节展示方法各阶段及其集成的实验评估。最后，第 9 节给出结论性意见及未来工作展望。

The use of VM has made possible to overcome the local behavior of visual servo control schemes. The navigation based on a VM has been mainly used for wheeled mobile robots. For instance, a local Euclidean reconstruction is used in [4] to avoid building a complete map. Geometric reconstruction is used in [11] to predict the feature positions, and it allows recovery of features tracking failures. An image-based scheme reported in [12] makes use of direct feedback from a visual geometric constraint. In [15], the integration of a laser range finder in the visual navigation helps to avoid unexpected obstacles. Different from other robots, humanoids are able to perform several tasks at the same time [16], which represents one of their main advantages.
视觉记忆（VM）的使用使得克服视觉伺服控制方案的局部行为成为可能。基于视觉记忆的导航主要应用于轮式移动机器人。例如，[4]中使用局部欧几里得重建以避免构建完整地图。[11]中采用几何重建来预测特征位置，并允许恢复特征跟踪失败。[12]中报道的基于图像的方案利用视觉几何约束的直接反馈。[15]中，激光测距仪与视觉导航的集成有助于避免意外障碍物。与其他机器人不同的是，人形机器人能够同时执行多项任务[16]，这也是其主要优势之一。

Vision-based control of humanoid robots is an interesting and challenging problem due to the inherent dynamic balance and the undesired sway motion introduced by bipedal locomotion [5, 6]. Solutions to the visual servoing problem for local positioning of humanoid robots have been reported in [5-7]. These control schemes exploit the reactive WPG proposed in [17], which provides automatic generation of foot placements from desired values of linear and angular velocities of the robot’s center of mass (CoM). In [5] and [7], a visual servoing scheme gives the velocity reference for the CoM as the main input to the WPG. Such schemes are considered decoupled approaches since the visual controller is independent of the WPG. In contrast, a coupled approach has been proposed in [6], where visual constraints are directly introduced in the WPG.
基于视觉的类人机器人控制是一个有趣且具有挑战性的问题，原因在于双足行走固有的动态平衡以及由此引起的不期望的摆动运动[5, 6]。关于类人机器人局部定位的视觉伺服问题的解决方案已在文献[5-7]中报道。这些控制方案利用了文献[17]中提出的反应式步态生成器（WPG），该生成器能够根据机器人质心（CoM）的线速度和角速度的期望值自动生成足部位置。在文献[5]和[7]中，视觉伺服方案为 CoM 提供速度参考，作为 WPG 的主要输入。这类方案被视为解耦方法，因为视觉控制器与 WPG 是独立的。相反，文献[6]提出了一种耦合方法，其中视觉约束直接引入到 WPG 中。

The extension of visual servo controllers to deal with humanoid navigation in indoor environments has important applications in service robotics for surveillance and assistance. In the literature, a camera onboard the robot has been used to achieve vision-based navigation, which requires a larger displacement than the local visual servoing task [10,18,19]. A landmark-based navigation approach that integrates motion planning through geometric primitives and visual servoing tasks is described in [20]. Although the motion planner returns obstacle-free paths, the locomotion model is not capable to step over obstacles. A trajectory tracking control in the Cartesian space, based on vision aided by odometry, is proposed in [18]. The estimation of the humanoid’s spatial location considers data fusion from different sensors, similarly than in [21].
将视觉伺服控制器扩展到处理室内环境中的类人导航，在服务机器人用于监控和辅助方面具有重要应用。文献中，机器人搭载的摄像头被用于实现基于视觉的导航，该导航任务所需的位移大于局部视觉伺服任务[10,18,19]。文献[20]描述了一种基于地标的导航方法，该方法通过几何基元和视觉伺服任务集成运动规划。尽管运动规划器返回无障碍路径，但运动模型无法跨越障碍物。文献[18]提出了一种基于视觉辅助里程计的笛卡尔空间轨迹跟踪控制。类人机器人的空间位置估计考虑了来自不同传感器的数据融合，类似于文献[21]。

Pure vision-based humanoid navigation strategies have also been proposed [10,19,22]. In particular, [22] suggests the use of landmarks like 2D bar codes while [19] proposes the use of vanishing points. Both schemes consider environments with corridors. The extension of the view-sequenced route representation for humanoid navigation is reported in [10]. The evaluation is also carried out in a corridor with no initial localization and the controller is based on template correlation to decide basic motion actions. Thus, our proposed scheme can be used in less restrictive indoor environments with natural scenes unlike the referred works [10, 19,22]. Moreover, our method considers the holonomic nature of humanoid robots, in contrast to previous works [18-20], which constraint the robot motion as a unicycle model.
纯视觉驱动的人形机器人导航策略也已被提出[10,19,22]。特别地，[22]建议使用类似二维条形码的地标，而[19]则提出使用消失点。这两种方案均考虑了带走廊的环境。[10]报道了视序路线表示法在人形机器人导航中的扩展。评估同样在无初始定位的走廊环境中进行，控制器基于模板相关性来决定基本运动动作。因此，我们提出的方案可用于比上述文献[10,19,22]更不受限制的自然场景室内环境。此外，我们的方法考虑了人形机器人全向运动的特性，而与之前将机器人运动限制为单轮模型的工作[18-20]不同。

An important issue for humanoid navigation is the ability of avoiding obstacles. In structured indoor scenarios, the scheme reported in [23] uses line-based scene analysis and feature tracking to step over or walk around obstacles. Vision aided by range sensors has also been used for humanoid navigation with obstacle avoidance in [24]. In that work, obstacles are identified by means of laser data and that information is then used to train visual classifiers that are later applied to the images during autonomous navigation. Some works focus on the generation of stable dynamic movements in scenarios with obstacles by stepping over them [25, 26]. These works were conceived for human-sized humanoid platforms, like the HRP-2 robot, and a priori knowledge of obstacles
类人机器人导航的一个重要问题是避障能力。在结构化的室内场景中，文献[23]中报道的方案利用基于线的场景分析和特征跟踪来跨越或绕行障碍物。文献[24]中也采用了结合测距传感器的视觉辅助方法实现类人机器人导航及避障。在该工作中，障碍物通过激光数据识别，随后利用这些信息训练视觉分类器，分类器在自主导航过程中应用于图像识别。一些研究则侧重于在有障碍物的场景中通过跨越障碍物生成稳定的动态运动[25, 26]。这些研究针对的是人体大小的类人机器人平台，如 HRP-2 机器人，并且依赖于对障碍物的先验知识。
dimensions. Recently, an extended version of the WPG of [17] has been introduced in [27], which considers the feet orientation, and it is applied for obstacle avoidance using the HRP-2 robot. However, the underlying nonlinear model predictive control must solve a nonlinear program with a time horizon, which is computationally expensive. Our navigation strategy proposes a simpler method for obstacle avoidance than the ones used in previous works, focusing in an adequate management of hierarchical tasks. Monocular vision aided by data from encoders has been suggested in [28]. The scheme uses optical flow to make a representation of mazelike environments with obstacles to identify the free space. In [29], the humanoid navigation in a cluttered environment has been addressed, considering an application where the robot transports a load. The navigation scheme relies on a 3D representation of the environment obtained using SLAM, from which, collision-free trajectories are computed.
维度。最近，[27]中引入了[17]的 WPG 的扩展版本，该版本考虑了足部方向，并应用于使用 HRP-2 机器人进行障碍物避让。然而，基础的非线性模型预测控制必须解决具有时间视野的非线性规划问题，计算代价较高。我们的导航策略提出了一种比以往工作中使用的方法更简单的障碍物避让方法，重点在于对层级任务的适当管理。[28]中建议使用单目视觉结合编码器数据。该方案利用光流来表示带有障碍物的迷宫环境，以识别自由空间。在[29]中，考虑机器人运输负载的应用，解决了杂乱环境中的人形机器人导航问题。导航方案依赖于通过 SLAM 获得的环境三维表示，从中计算无碰撞轨迹。

To the authors knowledge, the humanoid navigation based on VM has not been previously extended to deal with obstacle avoidance in indoor environments regardless the characteristics of the work space, i.e., uncluttered, cluttered or corridor-like indoor environments.
据作者所知，基于视觉记忆（VM）的类人导航此前尚未扩展到处理室内环境中的障碍物避让，无论工作空间的特性如何，即无杂乱、杂乱或走廊式的室内环境。

The autonomous navigation framework presented in this work can be divided in four stages: 1) visual memory building, 2) robot localization and path planning, 3) path following and 4) obstacle localization and graph updating. Fig. 1 presents an outline of the complete navigation strategy. In the context of this work, a VM is a directed graph $\mathbf{G}=\{\mathbf{I},\mathcal{E}\}$$\mathbf{G}=\{\mathbf{I},\mathcal{E}\}$G={I,E}\mathbf{G}=\{\mathbf{I}, \mathcal{E}\}, where each node encodes an image of a set $\mathbf{I}=\{{\mathcal{I}}_1,{\mathcal{I}}_2,\dots ,{\mathcal{I}}_m\}$$\mathbf{I}=\left\{{\mathcal{I}}_1,{\mathcal{I}}_2,\dots ,{\mathcal{I}}_m\right\}$I={I_(1),I_(2),dots,I_(m)}\mathbf{I}=\left\{\mathcal{I}_{1}, \mathcal{I}_{2}, \ldots, \mathcal{I}_{m}\right\} of $m$$m$mm key images and $\mathcal{E}=\{{\mathcal{E}}_1,{\mathcal{E}}_2,\dots ,{\mathcal{E}}_p\}$$\mathcal{E}=\left\{{\mathcal{E}}_1,{\mathcal{E}}_2,\dots ,{\mathcal{E}}_p\right\}$E={E_(1),E_(2),dots,E_(p)}\mathcal{E}=\left\{\mathcal{E}_{1}, \mathcal{E}_{2}, \ldots, \mathcal{E}_{p}\right\} is the set of $p$$p$pp edges where each of them has an associated weight that links a pair of nodes. Recall that we assume that the VM generation is done offline by means of a supervised teaching phase (humanguided). The automatic selection of key images for building such VM represents a problem by itself, and it is not addressed in this work. However, we point out that some methods for keyframes selection have been proposed in the literature related to the field of visual SLAM [30] or video content analysis [31]. The aim of these methods is to ensure a minimum of common visual information between selected images. In this work, we assume that the VM is given as input of the navigation strategy, provided that the keyframes in the VM have enough overlapping of visual features.
本文提出的自主导航框架可分为四个阶段：1）视觉记忆构建，2）机器人定位与路径规划，3）路径跟踪，4）障碍物定位与图更新。图 1 展示了完整导航策略的概述。在本文的背景下，视觉记忆（VM）是一个有向图 $\mathbf{G}=\{\mathbf{I},\mathcal{E}\}$$\mathbf{G}=\{\mathbf{I},\mathcal{E}\}$G={I,E}\mathbf{G}=\{\mathbf{I}, \mathcal{E}\} ，其中每个节点编码一组 $\mathbf{I}=\{{\mathcal{I}}_1,{\mathcal{I}}_2,\dots ,{\mathcal{I}}_m\}$$\mathbf{I}=\left\{{\mathcal{I}}_1,{\mathcal{I}}_2,\dots ,{\mathcal{I}}_m\right\}$I={I_(1),I_(2),dots,I_(m)}\mathbf{I}=\left\{\mathcal{I}_{1}, \mathcal{I}_{2}, \ldots, \mathcal{I}_{m}\right\} 关键图像中的一幅 $m$$m$mm 图像， $\mathcal{E}=\{{\mathcal{E}}_1,{\mathcal{E}}_2,\dots ,{\mathcal{E}}_p\}$$\mathcal{E}=\left\{{\mathcal{E}}_1,{\mathcal{E}}_2,\dots ,{\mathcal{E}}_p\right\}$E={E_(1),E_(2),dots,E_(p)}\mathcal{E}=\left\{\mathcal{E}_{1}, \mathcal{E}_{2}, \ldots, \mathcal{E}_{p}\right\} 是 $p$$p$pp 条边的集合，每条边都具有一个关联权重，连接一对节点。需要指出的是，我们假设视觉记忆的生成是通过监督教学阶段（人类引导）离线完成的。自动选择关键图像以构建此类视觉记忆本身就是一个问题，本文未予以解决。然而，我们指出，文献中已有一些针对关键帧选择的方法，涉及视觉 SLAM 领域[30]或视频内容分析[31]。这些方法的目标是确保所选图像之间具有最小的公共视觉信息。 在本研究中，我们假设视觉记忆作为导航策略的输入，前提是视觉记忆中的关键帧具有足够的视觉特征重叠。

The robot localization, which is the second stage of the strategy, consists of finding the key image denoted by ${\mathcal{I}}_1^{\ast }$${\mathcal{I}}_1^{\ast }$I_(1)^(**)\mathcal{I}_{1}^{*} in the VM that best fits the current image $\mathcal{I}$$\mathcal{I}$I\mathcal{I} in terms of common visual information. Once $\mathcal{I}$$\mathcal{I}$I\mathcal{I} and ${\mathcal{I}}_1^{\ast }$${\mathcal{I}}_1^{\ast }$I_(1)^(**)\mathcal{I}_{1}^{*} are linked, then a key image that belongs to the VM is given as the target ${\mathcal{I}}_n^{\ast }$${\mathcal{I}}_n^{\ast }$I_(n)^(**)\mathcal{I}_{n}^{*}. The visual path planning is carried out to find the shortest visual path ${\mathbf{I}}^{\ast }$${\mathbf{I}}^{\ast }$I^(**)\mathbf{I}^{*} in $\mathbf{G}$$\mathbf{G}$G\mathbf{G} from ${\mathcal{I}}_1^{\ast }$${\mathcal{I}}_1^{\ast }$I_(1)^(**)\mathcal{I}_{1}^{*} to ${\mathcal{I}}_n^{\ast }$${\mathcal{I}}_n^{\ast }$I_(n)^(**)\mathcal{I}_{n}^{*} in terms of weights of the edges.
机器人定位作为策略的第二阶段，包含在视觉记忆（VM）中寻找与当前图像 $\mathcal{I}$$\mathcal{I}$I\mathcal{I} 在共同视觉信息方面最匹配的关键图像 ${\mathcal{I}}_1^{\ast }$${\mathcal{I}}_1^{\ast }$I_(1)^(**)\mathcal{I}_{1}^{*} 。一旦 $\mathcal{I}$$\mathcal{I}$I\mathcal{I} 和 ${\mathcal{I}}_1^{\ast }$${\mathcal{I}}_1^{\ast }$I_(1)^(**)\mathcal{I}_{1}^{*} 建立关联，则将属于 VM 的关键图像作为目标 ${\mathcal{I}}_n^{\ast }$${\mathcal{I}}_n^{\ast }$I_(n)^(**)\mathcal{I}_{n}^{*} 。视觉路径规划旨在根据边的权重，在 $\mathbf{G}$$\mathbf{G}$G\mathbf{G} 中从 ${\mathcal{I}}_1^{\ast }$${\mathcal{I}}_1^{\ast }$I_(1)^(**)\mathcal{I}_{1}^{*} 到 ${\mathcal{I}}_n^{\ast }$${\mathcal{I}}_n^{\ast }$I_(n)^(**)\mathcal{I}_{n}^{*} 寻找最短的视觉路径 ${\mathbf{I}}^{\ast }$${\mathbf{I}}^{\ast }$I^(**)\mathbf{I}^{*} 。

In the path following phase, the robot performs an autonomous navigation to follow the resulting visual path. A visual control scheme guides the humanoid along the sequence of key images until the robot achieves the desired location. The last step of the navigation strategy is the VM updating, which consists of modifying the weights attached to the edges of $\mathbf{G}$$\mathbf{G}$G\mathbf{G} whenever the robot encounters new static obstacles during the visual path following. In particular, if an obstacle is found while the robot is following the piece of visual path from ${\mathcal{I}}_i$${\mathcal{I}}_i$I_(i)\mathcal{I}_{i} to ${\mathcal{I}}_{i+1}$${\mathcal{I}}_{i+1}$I_(i+1)\mathcal{I}_{i+1}, the weight at the corresponding edge ${\mathcal{E}}_i$${\mathcal{E}}_i$E_(i)\mathcal{E}_{i} is increased. Thus, the number $N_o$$N_o$N_(o)N_{o} of detected obstacles and their location in the VM are memorized. This allows the robot to avoid costly paths with obstacles in a future navigation.
在路径跟随阶段，机器人执行自主导航以跟随生成的视觉路径。视觉控制方案引导人形机器人沿关键图像序列前进，直到机器人达到预期位置。导航策略的最后一步是视觉记忆（VM）的更新，即在机器人沿视觉路径行进时遇到新的静态障碍物时，修改附加在 $\mathbf{G}$$\mathbf{G}$G\mathbf{G} 边上的权重。具体来说，如果机器人在从 ${\mathcal{I}}_i$${\mathcal{I}}_i$I_(i)\mathcal{I}_{i} 到 ${\mathcal{I}}_{i+1}$${\mathcal{I}}_{i+1}$I_(i+1)\mathcal{I}_{i+1} 的视觉路径段上发现障碍物，则相应边 ${\mathcal{E}}_i$${\mathcal{E}}_i$E_(i)\mathcal{E}_{i} 的权重会增加。因此，检测到的障碍物数量 $N_o$$N_o$N_(o)N_{o} 及其在视觉记忆中的位置被记录下来。这使得机器人在未来导航中能够避免带有障碍物的高成本路径。

The main steps of the proposed humanoid navigation framework are presented in Algorithm 1, where each function is described in detail in the subsequent sections.
所提出的人形机器人导航框架的主要步骤在算法 1 中展示，每个函数将在后续章节中详细描述。

Algorithm 1: visualNavigation allows the autonomous navigation of
the robot.
    Input: Graph of key images \(\mathbf{G}=\{\mathbf{I}, \mathcal{E}\}\), target image \(\mathcal{I}_{n}^{*}\)
    Output: Autonomous visual navigation
    \(\mathcal{I}=\) captureCurrentImage
    \(\mathcal{I}_{1}^{*}=\operatorname{localizeRobot}(\mathcal{I}, \mathbf{I})\)
    \(\mathbf{I}^{*}=\operatorname{getShortestPath}\left(\mathcal{I}_{1}^{*}, \mathcal{I}_{n}^{*}, \mathbf{G}\right)\)
    (navigation, \(N_{o}, \mathcal{E}_{i}\) ) = pathFollower( \(\mathcal{I}, \mathbf{I}^{*}, \mathbf{G}\) )
    if \(N_{0}>0\) then
        \(\mathbf{G}=\operatorname{graphUpdating}\left(N_{o}, \mathcal{E}_{i}, \mathbf{G}\right)\)
    return

As aforementioned, a VM is a graph $\mathbf{G}=\{\mathbf{I},\mathcal{E}\}$$\mathbf{G}=\{\mathbf{I},\mathcal{E}\}$G={I,E}\mathbf{G}=\{\mathbf{I}, \mathcal{E}\} where the nodes $\mathbf{I}$$\mathbf{I}$I\mathbf{I} encode key images obtained during a teaching phase. It is worth noting that the graph is not always linear, the VM could have several branches and loops. In this section, we complement the description of the graph’s structure by defining the weights of the edges between nodes.
如前所述，视觉记忆（VM）是一个图 $\mathbf{G}=\{\mathbf{I},\mathcal{E}\}$$\mathbf{G}=\{\mathbf{I},\mathcal{E}\}$G={I,E}\mathbf{G}=\{\mathbf{I}, \mathcal{E}\} ，其中节点 $\mathbf{I}$$\mathbf{I}$I\mathbf{I} 编码了教学阶段获得的关键图像。值得注意的是，该图并非总是线性的，视觉记忆可能具有多个分支和环路。本节通过定义节点间边的权重，补充图结构的描述。

The weight attached to an edge represents the cost of traveling from the current node to one of the adjacent nodes, and it is denoted by ${\mathcal{E}}_i$${\mathcal{E}}_i$E_(i)\mathcal{E}_{i}. We define the weight of an edge in terms of the number of matched interest points and the amount of rotation between neighboring key images as follows:
边的权重表示从当前节点到相邻节点的移动代价，记为 ${\mathcal{E}}_i$${\mathcal{E}}_i$E_(i)\mathcal{E}_{i} 。我们根据匹配兴趣点的数量和相邻关键图像之间的旋转量定义边的权重，具体如下：
${\mathcal{E}}_i=\alpha (1-{\overline{s}}_i^{\ast })+\beta \overline{\theta u_i^{\ast }}$${\mathcal{E}}_i=\alpha \left(1-{\overline{s}}_i^{\ast }\right)+\beta \overline{\theta u_i^{\ast }}$E_(i)=alpha(1- bar(s)_(i)^(**))+beta bar(thetau_(i)^(**))\mathcal{E}_{i}=\alpha\left(1-\bar{s}_{i}^{*}\right)+\beta \overline{\theta u_{i}^{*}},
where ${\overline{s}}_i^{\ast }$${\overline{s}}_i^{\ast }$bar(s)_(i)^(**)\bar{s}_{i}^{*} and $\overline{\theta u_i^{\ast }}$$\overline{\theta u_i^{\ast }}$bar(thetau_(i)^(**))\overline{\theta u_{i}^{*}} are the normalized number of matches and the normalized rotation with respect to the vertical axis (perpendicular to the motion plane) between neighboring key images respectively, $\alpha$$\alpha$alpha\alpha and $\beta$$\beta$beta\beta are weights to favor one of the terms if desired. The cost related to the number of matches $(1-{\overline{s}}_i^{\ast })$$\left(1-{\overline{s}}_i^{\ast }\right)$(1- bar(s)_(i)^(**))\left(1-\bar{s}_{i}^{*}\right) means that, the more matches there are between nodes, the lower the cost will be. The cost related to the rotation means that, the more rotation there are between nodes the higher the cost will be.
其中 ${\overline{s}}_i^{\ast }$${\overline{s}}_i^{\ast }$bar(s)_(i)^(**)\bar{s}_{i}^{*} 和 $\overline{\theta u_i^{\ast }}$$\overline{\theta u_i^{\ast }}$bar(thetau_(i)^(**))\overline{\theta u_{i}^{*}} 分别是相邻关键图像之间匹配点数的归一化值和相对于垂直轴（垂直于运动平面）的归一化旋转量， $\alpha$$\alpha$alpha\alpha 和 $\beta$$\beta$beta\beta 是用于调整两项权重的系数。与匹配点数相关的代价 $(1-{\overline{s}}_i^{\ast })$$\left(1-{\overline{s}}_i^{\ast }\right)$(1- bar(s)_(i)^(**))\left(1-\bar{s}_{i}^{*}\right) 表示节点间匹配点越多，代价越低。与旋转相关的代价表示节点间旋转越大，代价越高。

In previous works [3,4], the weights of the edges are unitary, i.e., the cost of a visual path is given by counting the number of key images in the route, and an edge exists if a minimum number of point correspondences is achieved to relate neighboring nodes. In this work, the term related to rotation ${\overline{\theta u}}_i^{\ast }$${\overline{\theta u}}_i^{\ast }$bar(theta u)_(i)^(**)\overline{\theta u}_{i}^{*} is included to deal with environments where the richness of point features ${\overline{s}}_i^{\ast }$${\overline{s}}_i^{\ast }$bar(s)_(i)^(**)\bar{s}_{i}^{*} is low but sufficient, and the rotation can become more important to determine the cost of a route.
在先前的工作[3,4]中，边的权重是单位权重，即视觉路径的代价通过计算路径中关键图像的数量来确定，且当达到最小数量的点对应关系以关联相邻节点时，边才存在。在本工作中，加入了与旋转相关的项 ${\overline{\theta u}}_i^{\ast }$${\overline{\theta u}}_i^{\ast }$bar(theta u)_(i)^(**)\overline{\theta u}_{i}^{*} ，以应对点特征丰富度 ${\overline{s}}_i^{\ast }$${\overline{s}}_i^{\ast }$bar(s)_(i)^(**)\bar{s}_{i}^{*} 较低但足够的环境，其中旋转在确定路径代价时可能变得更为重要。

Thus, we estimate the relative rotation $\theta u_i^{\ast }$$\theta u_i^{\ast }$thetau_(i)^(**)\theta u_{i}^{*} between neighboring key images ${\mathcal{I}}_i$${\mathcal{I}}_i$I_(i)\mathcal{I}_{i} and ${\mathcal{I}}_{i+1}$${\mathcal{I}}_{i+1}$I_(i+1)\mathcal{I}_{i+1}, which have associated camera reference frames ${\mathcal{C}}_i$${\mathcal{C}}_i$C_(i)\mathcal{C}_{i} and ${\mathcal{C}}_{i+1}$${\mathcal{C}}_{i+1}$C_(i+1)\mathcal{C}_{i+1}, respectively. An option to recover the whole relative pose (with translation up to scale) between the frames ${\mathcal{C}}_i$${\mathcal{C}}_i$C_(i)\mathcal{C}_{i} and ${\mathcal{C}}_{i+1}$${\mathcal{C}}_{i+1}$C_(i+1)\mathcal{C}_{i+1} is by means of the decomposition of a geometric constraint, for instance, the homography matrix [32]. To estimate the homography matrix ${\mathbf{H}}_i^{\ast }$${\mathbf{H}}_i^{\ast }$H_(i)^(**)\mathbf{H}_{i}^{*}, only the key images ${\mathcal{I}}_i$${\mathcal{I}}_i$I_(i)\mathcal{I}_{i} and ${\mathcal{I}}_{i+1}$${\mathcal{I}}_{i+1}$I_(i+1)\mathcal{I}_{i+1} are needed. The relative transformation between ${\mathcal{C}}_i$${\mathcal{C}}_i$C_(i)\mathcal{C}_{i} and ${\mathcal{C}}_{i+1}$${\mathcal{C}}_{i+1}$C_(i+1)\mathcal{C}_{i+1} is encoded in the Euclidean homography as [33]:
因此，我们估计相邻关键图像 ${\mathcal{I}}_i$${\mathcal{I}}_i$I_(i)\mathcal{I}_{i} 和 ${\mathcal{I}}_{i+1}$${\mathcal{I}}_{i+1}$I_(i+1)\mathcal{I}_{i+1} 之间的相对旋转 $\theta u_i^{\ast }$$\theta u_i^{\ast }$thetau_(i)^(**)\theta u_{i}^{*} ，它们分别具有相关联的相机参考系 ${\mathcal{C}}_i$${\mathcal{C}}_i$C_(i)\mathcal{C}_{i} 和 ${\mathcal{C}}_{i+1}$${\mathcal{C}}_{i+1}$C_(i+1)\mathcal{C}_{i+1} 。恢复参考系 ${\mathcal{C}}_i$${\mathcal{C}}_i$C_(i)\mathcal{C}_{i} 和 ${\mathcal{C}}_{i+1}$${\mathcal{C}}_{i+1}$C_(i+1)\mathcal{C}_{i+1} 之间完整相对位姿（带尺度的平移）的一种方法是通过几何约束的分解，例如单应矩阵[32]。估计单应矩阵 ${\mathbf{H}}_i^{\ast }$${\mathbf{H}}_i^{\ast }$H_(i)^(**)\mathbf{H}_{i}^{*} 仅需关键图像 ${\mathcal{I}}_i$${\mathcal{I}}_i$I_(i)\mathcal{I}_{i} 和 ${\mathcal{I}}_{i+1}$${\mathcal{I}}_{i+1}$I_(i+1)\mathcal{I}_{i+1} 。 ${\mathcal{C}}_i$${\mathcal{C}}_i$C_(i)\mathcal{C}_{i} 和 ${\mathcal{C}}_{i+1}$${\mathcal{C}}_{i+1}$C_(i+1)\mathcal{C}_{i+1} 之间的相对变换编码在欧氏单应矩阵中，如[33]所示：
${\mathbf{H}}_i^{\ast }={\mathbf{R}}_i^{\ast }+\frac{{\mathbf{t}}_i^{\ast }}{d_i^{\ast }}{\mathbf{n}}_i^{\ast }$${\mathbf{H}}_i^{\ast }={\mathbf{R}}_i^{\ast }+\frac{{\mathbf{t}}_i^{\ast }}{d_i^{\ast }}{\mathbf{n}}_i^{\ast }$H_(i)^(**)=R_(i)^(**)+(t_(i)^(**))/(d_(i)^(**))n_(i)^(**)\mathbf{H}_{i}^{*}=\mathbf{R}_{i}^{*}+\frac{\mathbf{t}_{i}^{*}}{d_{i}^{*}} \mathbf{n}_{i}^{*},
where ${\mathbf{R}}_i^{\ast }$${\mathbf{R}}_i^{\ast }$R_(i)^(**)\mathbf{R}_{i}^{*} and ${\mathbf{t}}_i^{\ast }$${\mathbf{t}}_i^{\ast }$t_(i)^(**)\mathbf{t}_{i}^{*} are the rotation matrix and translation vector expressed in ${\mathcal{C}}_{i+1},d_i^{\ast }$${\mathcal{C}}_{i+1},d_i^{\ast }$C_(i+1),d_(i)^(**)\mathcal{C}_{i+1}, d_{i}^{*} is the distance from ${\mathcal{C}}_{i+1}$${\mathcal{C}}_{i+1}$C_(i+1)\mathcal{C}_{i+1} to a plane $\pi$$\pi$pi\pi, and ${\mathbf{n}}_i^{\ast }$${\mathbf{n}}_i^{\ast }$n_(i)^(**)\mathbf{n}_{i}^{*} is the unitary vector expressed in ${\mathcal{C}}_{i+1}$${\mathcal{C}}_{i+1}$C_(i+1)\mathcal{C}_{i+1} normal to $\pi$$\pi$pi\pi. According to (2), it is possible to decompose ${\mathbf{H}}_i^{\ast }$${\mathbf{H}}_i^{\ast }$H_(i)^(**)\mathbf{H}_{i}^{*} to obtain ${\mathbf{t}}_i^{\ast }$${\mathbf{t}}_i^{\ast }$t_(i)^(**)\mathbf{t}_{i}^{*} up to scale and ${\mathbf{R}}_i^{\ast }$${\mathbf{R}}_i^{\ast }$R_(i)^(**)\mathbf{R}_{i}^{*}. The rotation matrix ${\mathbf{R}}_i^{\ast }$${\mathbf{R}}_i^{\ast }$R_(i)^(**)\mathbf{R}_{i}^{*} can then be parametrized by the axis/angle $\theta {\mathbf{u}}_i^{\ast }$$\theta {\mathbf{u}}_i^{\ast }$thetau_(i)^(**)\theta \mathbf{u}_{i}^{*}. The second element of the vector $\theta {\mathbf{u}}_i^{\ast }$$\theta {\mathbf{u}}_i^{\ast }$thetau_(i)^(**)\theta \mathbf{u}_{i}^{*}, related to the rotation around the $y$$y$yy-axis of the camera (perpendicular to the motion plane), provides the required rotation between neighboring key images. In addition, such angular component is used to compute
其中 ${\mathbf{R}}_i^{\ast }$${\mathbf{R}}_i^{\ast }$R_(i)^(**)\mathbf{R}_{i}^{*} 和 ${\mathbf{t}}_i^{\ast }$${\mathbf{t}}_i^{\ast }$t_(i)^(**)\mathbf{t}_{i}^{*} 分别表示旋转矩阵和平移向量， ${\mathcal{C}}_{i+1},d_i^{\ast }$${\mathcal{C}}_{i+1},d_i^{\ast }$C_(i+1),d_(i)^(**)\mathcal{C}_{i+1}, d_{i}^{*} 表示从 ${\mathcal{C}}_{i+1}$${\mathcal{C}}_{i+1}$C_(i+1)\mathcal{C}_{i+1} 到平面 $\pi$$\pi$pi\pi 的距离， ${\mathbf{n}}_i^{\ast }$${\mathbf{n}}_i^{\ast }$n_(i)^(**)\mathbf{n}_{i}^{*} 是以 ${\mathcal{C}}_{i+1}$${\mathcal{C}}_{i+1}$C_(i+1)\mathcal{C}_{i+1} 表示的单位向量，垂直于 $\pi$$\pi$pi\pi 。根据公式(2)，可以分解 ${\mathbf{H}}_i^{\ast }$${\mathbf{H}}_i^{\ast }$H_(i)^(**)\mathbf{H}_{i}^{*} 以获得 ${\mathbf{t}}_i^{\ast }$${\mathbf{t}}_i^{\ast }$t_(i)^(**)\mathbf{t}_{i}^{*} （尺度不定）和 ${\mathbf{R}}_i^{\ast }$${\mathbf{R}}_i^{\ast }$R_(i)^(**)\mathbf{R}_{i}^{*} 。旋转矩阵 ${\mathbf{R}}_i^{\ast }$${\mathbf{R}}_i^{\ast }$R_(i)^(**)\mathbf{R}_{i}^{*} 随后可以通过轴/角 $\theta {\mathbf{u}}_i^{\ast }$$\theta {\mathbf{u}}_i^{\ast }$thetau_(i)^(**)\theta \mathbf{u}_{i}^{*} 参数化。向量 $\theta {\mathbf{u}}_i^{\ast }$$\theta {\mathbf{u}}_i^{\ast }$thetau_(i)^(**)\theta \mathbf{u}_{i}^{*} 的第二个元素与相机绕 $y$$y$yy 轴（垂直于运动平面）的旋转相关，提供了相邻关键图像之间所需的旋转。此外，该角度分量用于计算

Fig. 1. Outline of the navigation strategy. Once the VM is available, the robot must localize itself in the VM by finding the image that best fits the current image. Then the visual path to the target image is found by a path planner. The robot navigates autonomously by following the visual path while avoiding static obstacles detected with a range sensor, which were absent during the teaching phase. The notation $\{\cdot \}$$\{\cdot \}${*}\{\cdot\} stands for the reference frame of the robot $r$$r$rr, camera $c$$c$cc, obstacle $o$$o$oo and range sensor $s_0$$s_0$s_(0)s_{0}.
图 1. 导航策略概述。一旦视觉记忆（VM）可用，机器人必须通过找到与当前图像最匹配的图像来在 VM 中定位自身。然后，通过路径规划器找到到目标图像的视觉路径。机器人通过沿视觉路径自主导航，同时避开在教学阶段不存在的由测距传感器检测到的静态障碍物。符号 $\{\cdot \}$$\{\cdot \}${*}\{\cdot\} 代表机器人 $r$$r$rr 、相机 $c$$c$cc 、障碍物 $o$$o$oo 和测距传感器 $s_0$$s_0$s_(0)s_{0} 的参考坐标系。
the angular velocity, which corresponds to one of the inputs of the WPG. The other two inputs are longitudinal and lateral velocities as it will be explained in Section 6.
角速度，对应于 WPG 的一个输入。其他两个输入是纵向速度和横向速度，详见第 6 节说明。

In this work, we rely on the homography model to assign the weights of the edges in the graph, to localize the robot and to formulate the visual control scheme. The homography, as a geometric constraint between images, allows the process of points matching to be robust. This projective model is actually general, since it can be computed for three-dimensional non-planar scenes [32]. Besides, the homography model does not have the issue of being bad conditioned with short baseline, as other geometric constraints like the epipolar geometry [33]. In the scheme of [32], there is no need of verifying the existence of a dominant plane in the scene but a virtual plane is defined by selecting three non-collinear point features in the image. A practical suggestion is to select automatically the three points that maximize the area of the corresponding triangle in both images. Differently from the common homography matrix estimation, the scheme based on a virtual plane needs at least eight points (three reference points and five supplementary) to estimate the model.
在本研究中，我们依赖单应性模型为图中的边赋权，以实现机器人定位和视觉控制方案的制定。单应性作为图像间的几何约束，使得点匹配过程具有鲁棒性。该投影模型实际上是通用的，因为它可以用于三维非平面场景的计算[32]。此外，单应性模型不像极线几何等其他几何约束那样，在基线较短时存在条件不良的问题[33]。在文献[32]的方案中，无需验证场景中是否存在主导平面，而是通过选择图像中三个非共线的点特征定义一个虚拟平面。一个实用的建议是自动选择在两幅图像中对应三角形面积最大的三个点。与常见的单应性矩阵估计不同，基于虚拟平面的方案至少需要八个点（三个参考点和五个补充点）来估计模型。

Although the homography model is a good option for extracting the required information directly from images in all stages of the proposed approach, the navigation scheme could handle other geometric constraints such as the fundamental matrix. However, the epipolar geometry is ill-conditioned with short baseline and planar scenes, but a model selection between the homography and fundamental matrices could be formulated as in [34].
尽管单应性模型是从图像中直接提取所需信息的良好选择，适用于所提方法的所有阶段，但导航方案也可以处理其他几何约束，如基础矩阵。然而，极线几何在基线较短和平面场景下条件较差，但可以如文献[34]中所述，将单应性矩阵和基础矩阵之间的模型选择进行公式化。

Once the VM is available with the structure described in the previous section, and before starting the autonomous navigation, the robot must localize itself by comparing its current view with the set of memorized key images. Next, given a target image, a path planning stage is needed to find the sequence of key images connecting the image resulting from the localization and the target image. In both stages, we take advantage of the homography estimation for planar and non-planar scenes [32] based on the formulation of a virtual plane.
一旦视觉记忆（VM）具备了上一节所述的结构，并且在开始自主导航之前，机器人必须通过将当前视图与记忆中的关键图像集合进行比较来实现自我定位。接下来，给定目标图像，需要路径规划阶段以找到连接定位结果图像与目标图像的关键图像序列。在这两个阶段中，我们利用基于虚拟平面公式的单应性估计方法，适用于平面和非平面场景[32]。

Algorithm 2: localizeRobot finds the most similar key image \(\mathcal{I}_{1}^{*}\) in
the graph with respect to the current image \(\mathcal{I}\).
    Input: Graph of key images \(\mathbf{G}=\{\mathbf{I}, \mathcal{E}\}\), current image \(\mathcal{I}\), target
            image \(\mathcal{I}_{n}^{*}\)
    Output: Most similar key image \(\mathcal{I}_{1}^{*}\)
    \(\widehat{\mathbf{I}}=\) findNodesNeighborhood \((\mathcal{I}, \mathbf{G}) / / \operatorname{sIZE}(\widehat{\mathbf{I}})=\widehat{m}\)
    for \(j \leftarrow 1\) to \(\widehat{m}\) do
        matches \(=\boldsymbol{\operatorname { m a t c h }}(\mathcal{I}, \widehat{\mathbf{I}}[j])\)
        if matches \(>\mu\) then
            \(\mathbf{H}_{j}=\) computeHomography( matches )
            \(\left(\mathbf{R}_{j}, \mathbf{t}_{j}\right)=\) decomposeHomography \(\left(\mathbf{H}_{j}\right)\)
            \(\mathbf{t}_{ \pm j}=\) computeDirection \(\left(\mathbf{t}_{j}\right)\)
            \(\left\|\mathbf{t}_{j}\right\|=\) computeDistance \(\left(\mathbf{t}_{j}\right)\)
            \(\mathbf{D}[j]=\operatorname{saveImageData}\left(\mathbf{t}_{ \pm j},\left\|\mathbf{t}_{j}\right\|\right)\)
    \(\mathrm{I}_{+}=\operatorname{selectForwardImages}(\mathrm{D})\)
    \(\mathrm{I}_{-}=\operatorname{selectBackwardImages}(\mathrm{D})\)
    if \(\operatorname{size}\left(\mathbf{I}_{+}\right)>0\) then
        \(\mathcal{I}_{1}^{*}=\operatorname{selectMostSimilar}\left(\mathrm{I}_{+}\right)\)
    else
        \(\mathcal{I}_{1}^{*}=\) selectMostSimilar \(\left(\mathbf{I}_{-}\right)\)
    return \(\mathcal{I}_{1}^{*}\)
    Function selectMostSimilar( \(\mathbf{I}_{\mu}\) )
        \(\left(\mathcal{I}_{ \pm 1}^{*}, \mathcal{I}_{ \pm 2}^{*}\right)=\operatorname{selectTwoCandidates}\left(\mathbf{I}_{\mu}\right)\)
        if \(\left(\mathcal{I}_{ \pm 1}^{*}, \mathcal{I}_{ \pm 2}^{*}\right)\) belong to same branch then
            matches \(=\boldsymbol{\operatorname { m a t c h }}\left(\mathcal{I}_{ \pm 1}^{*}, \mathcal{I}_{ \pm 2}^{*}, \mathcal{I}\right)\)
            \(\left\{\mathbf{H}_{1}, \mathbf{H}_{2}\right\}=\) parametersFixedPlane( matches )
            \(\left\{\left\|\mathbf{t}_{1}\right\|,\left\|\mathbf{t}_{2}\right\|\right\}=\) computeDistance \(\left(\left\{\mathbf{H}_{1}, \mathbf{H}_{2}\right\}\right)\)
            \(\mathcal{I}_{1}^{*}=\operatorname{selectTheClosest}\left(\left\{\left\|\mathbf{t}_{1}\right\|,\left\|\mathbf{t}_{2}\right\|\right\}\right)\)
        else
            \(\left\{\mathbf{I}_{1}, \mathbf{I}_{2}\right\}=\operatorname{getShortestPath}\left(\mathcal{I}_{ \pm 1}^{*}, \mathcal{I}_{ \pm 2}^{*}, \mathcal{I}^{*}, \mathbf{G}\right)\)
            \(\left\{\mathbf{l}_{1}, \mathbf{l}_{2}\right\}=\) computePathLength \(\left(\left\{\mathbf{I}_{1}, \mathbf{I}_{2}\right\}\right)\)
            \(\mathcal{I}_{1}^{*}=\operatorname{getImageWithShortestPath}\left(\left\{\mathbf{l}_{1}, \mathbf{l}_{2}\right\}\right)\)
        return \(\mathcal{I}_{1}^{*}\)

We propose Algorithm 2 to perform the required appearancebased robot localization. To avoid an exhaustive comparison between the current image $\mathcal{I}$$\mathcal{I}$I\mathcal{I} and each key image in the VM, we
我们提出算法 2 以执行所需的基于外观的机器人定位。为了避免对当前图像 $\mathcal{I}$$\mathcal{I}$I\mathcal{I} 与视觉记忆中每个关键图像进行穷尽比较，我们...
propose a strategy to find a reduced neighborhood of key images surrounding $\mathcal{I}$$\mathcal{I}$I\mathcal{I}. This process is performed in line 1 , function findNodesNeighborhood. The underlying algorithmic strategy uses the divide-and-conquer paradigm [35]. First, the current image is compared with each of the “Intersection” nodes, i.e. the nodes where the branches of the graph intersect. Then, each branch of the graph is divided in two to identify the node at the middle of the branch which is compared with $\mathcal{I}$$\mathcal{I}$I\mathcal{I}, and in the next iteration each half of the branch is divided again. Thus, the worst case happens when the algorithm iterates until the current image has been eventually compared to all existing nodes in the graph. The search stops when at least one of the comparisons get a number of matches greater than $\mu$$\mu$mu\mu. This minimum number of matches ( $\mu =8$$\mu =8$mu=8\mu=8 ) guarantees the computation of the homography [32]. Once the search terminates, a neighborhood of $\hat{m}$$\widehat{m}$widehat(m)\widehat{m} nodes surrounding this node is selected, and they are collected in a vector $\hat{\mathbf{I}}$$\widehat{\mathbf{I}}$widehat(I)\widehat{\mathbf{I}} of selected key images. The next step consists of finding the most similar key image to $\mathcal{I}$$\mathcal{I}$I\mathcal{I} among the reduced set $\hat{\mathbf{I}}$$\widehat{\mathbf{I}}$widehat(I)\widehat{\mathbf{I}}. For this, match finds the matched features between the current image and all key images in $\hat{\mathbf{I}}$$\widehat{\mathbf{I}}$widehat(I)\widehat{\mathbf{I}}.
提出了一种策略，以找到围绕 $\mathcal{I}$$\mathcal{I}$I\mathcal{I} 的关键图像的缩减邻域。该过程在第 1 行的函数 findNodesNeighborhood 中执行。其基本算法策略采用分治范式[35]。首先，将当前图像与每个“交叉点”节点进行比较，即图中分支相交的节点。然后，将图的每个分支分成两部分，以识别分支中间的节点，并将其与 $\mathcal{I}$$\mathcal{I}$I\mathcal{I} 进行比较，在下一次迭代中，每个分支的一半再次被分割。因此，最坏情况是算法迭代直到当前图像最终与图中所有现有节点进行比较。搜索在至少有一次比较的匹配数大于 $\mu$$\mu$mu\mu 时停止。该最小匹配数（ $\mu =8$$\mu =8$mu=8\mu=8 ）保证了单应矩阵的计算[32]。搜索终止后，选择围绕该节点的 $\hat{m}$$\widehat{m}$widehat(m)\widehat{m} 个邻域节点，并将它们收集到一个选定关键图像的向量 $\hat{\mathbf{I}}$$\widehat{\mathbf{I}}$widehat(I)\widehat{\mathbf{I}} 中。下一步是从缩减集合 $\hat{\mathbf{I}}$$\widehat{\mathbf{I}}$widehat(I)\widehat{\mathbf{I}} 中找到与 $\mathcal{I}$$\mathcal{I}$I\mathcal{I} 最相似的关键图像。 为此，match 在当前图像与集合 $\hat{\mathbf{I}}$$\widehat{\mathbf{I}}$widehat(I)\widehat{\mathbf{I}} 中的所有关键图像之间找到匹配特征。

The resulting matches allow the computation of the homography matrix and its decomposition in lines 5-6. Consequently, the estimation of the rotation and translation up to scale is obtained. Then, we classify the key images in two groups according to the resultant direction of the estimated translation vector $\mathbf{t}$$\mathbf{t}$t\mathbf{t} that is expressed in the current camera reference frame, which can be forward ${\mathbf{I}}_{+}$${\mathbf{I}}_{+}$I_(+)\mathbf{I}_{+}or backward ${\mathbf{I}}_{-}$${\mathbf{I}}_{-}$I_(-)\mathbf{I}_{-}with respect to the current image location. From the third component of $\mathbf{t}$$\mathbf{t}$t\mathbf{t} denoted by $t_z$$t_z$t_(z)t_{z}, which is pointing towards the motion direction (see $\{c\}$$\{c\}${c}\{c\} reference frame in Fig. 1), we assign a value in line 7 as $t_{\pm }=+1$$t_{\pm }=+1$t_(+-)=+1t_{ \pm}=+1 if $t_z>0$$t_z>0$t_(z) > 0t_{z}>0 or $t_{\pm }=-1$$t_{\pm }=-1$t_(+-)=-1t_{ \pm}=-1 if $t_z<0$$t_z<0$t_(z) < 0t_{z}<0.
由此得到的匹配允许在第 5-6 行计算单应矩阵及其分解。因此，获得了旋转和平移（至尺度）的估计。然后，我们根据估计平移向量 $\mathbf{t}$$\mathbf{t}$t\mathbf{t} 的结果方向将关键图像分为两组，该向量以当前相机参考系表示，相对于当前图像位置可为前进方向 ${\mathbf{I}}_{+}$${\mathbf{I}}_{+}$I_(+)\mathbf{I}_{+} 或后退方向 ${\mathbf{I}}_{-}$${\mathbf{I}}_{-}$I_(-)\mathbf{I}_{-} 。根据 $\mathbf{t}$$\mathbf{t}$t\mathbf{t} 的第三分量 $t_z$$t_z$t_(z)t_{z} ，该分量指向运动方向（参见图 1 中的 $\{c\}$$\{c\}${c}\{c\} 参考系），我们在第 7 行赋值为 $t_{\pm }=+1$$t_{\pm }=+1$t_(+-)=+1t_{ \pm}=+1 ，如果满足 $t_z>0$$t_z>0$t_(z) > 0t_{z}>0 ，否则赋值为 $t_{\pm }=-1$$t_{\pm }=-1$t_(+-)=-1t_{ \pm}=-1 ，如果满足 $t_z<0$$t_z<0$t_(z) < 0t_{z}<0 。

Although the vector $\mathbf{t}$$\mathbf{t}$t\mathbf{t} is scaled, its norm $\Vert \mathbf{t}\Vert$$\Vert \mathbf{t}\Vert$||t||\|\mathbf{t}\| in line 8 gives a notion of distance between key images if the homography computation considers that the distance $d_i^{\ast }$$d_i^{\ast }$d_(i)^(**)d_{i}^{*} to the plane is constant (see Eq. (2)). The direction and distance parameters are saved in a vector $\mathbf{D}$$\mathbf{D}$D\mathbf{D} in line 9, from which the forward ${\mathbf{I}}_{+}$${\mathbf{I}}_{+}$I_(+)\mathbf{I}_{+}and backward ${\mathbf{I}}_{-}$${\mathbf{I}}_{-}$I_(-)\mathbf{I}_{-} groups of images are obtained (lines 10-11). Using the set ${\mathbf{I}}_{+}$${\mathbf{I}}_{+}$I_(+)\mathbf{I}_{+}, the function selectMostSimilar finds (if exists) the most similar key image to the current one in terms of matched points. If there are no forward images, the algorithm selects the closest image among the backward set ${\mathbf{I}}_{-}$${\mathbf{I}}_{-}$I_(-)\mathbf{I}_{-}. Clearly, we prefer to localize the robot even if the most similar key image is behind the current location.
尽管向量 $\mathbf{t}$$\mathbf{t}$t\mathbf{t} 被缩放，其在第 8 行的范数 $\Vert \mathbf{t}\Vert$$\Vert \mathbf{t}\Vert$||t||\|\mathbf{t}\| 提供了关键图像之间距离的概念，前提是单应性计算假设到平面的距离 $d_i^{\ast }$$d_i^{\ast }$d_(i)^(**)d_{i}^{*} 是恒定的（见公式(2)）。方向和距离参数保存在第 9 行的向量 $\mathbf{D}$$\mathbf{D}$D\mathbf{D} 中，从中获得前向 ${\mathbf{I}}_{+}$${\mathbf{I}}_{+}$I_(+)\mathbf{I}_{+} 和后向 ${\mathbf{I}}_{-}$${\mathbf{I}}_{-}$I_(-)\mathbf{I}_{-} 图像组（第 10-11 行）。利用集合 ${\mathbf{I}}_{+}$${\mathbf{I}}_{+}$I_(+)\mathbf{I}_{+} ，函数 selectMostSimilar 找到（如果存在）与当前图像在匹配点方面最相似的关键图像。如果没有前向图像，算法则从后向集合 ${\mathbf{I}}_{-}$${\mathbf{I}}_{-}$I_(-)\mathbf{I}_{-} 中选择最近的图像。显然，即使最相似的关键图像位于当前位置之后，我们也更倾向于对机器人进行定位。

The function selectMostSimilar (lines 17-28) selects two candidate images ( ${\mathcal{I}}_{\pm 1}^{\ast },{\mathcal{I}}_{\pm 2}^{\ast }$${\mathcal{I}}_{\pm 1}^{\ast },{\mathcal{I}}_{\pm 2}^{\ast }$I_(+-1)^(**),I_(+-2)^(**)\mathcal{I}_{ \pm 1}^{*}, \mathcal{I}_{ \pm 2}^{*} ), i.e., the two images with the highest number of point matches. Then, the algorithm verifies if both candidates belong to the same branch of the graph $\mathbf{G}$$\mathbf{G}$G\mathbf{G}. If it is the case, the algorithm selects the most similar key image using the relative distance $\Vert \mathbf{t}\Vert$$\Vert \mathbf{t}\Vert$||t||\|\mathbf{t}\| from a new homography decomposition. This process computes again $\mathbf{H}$$\mathbf{H}$H\mathbf{H} and $\mathbf{t}$$\mathbf{t}$t\mathbf{t} for the candidate images with respect to the current image, but now the virtual plane used for the homography estimation is fixed (lines 21-22). Thus, the estimated distances are consistent. This process is illustrated in Fig. 2. It consists in matching point features between the three images $\mathcal{I}$$\mathcal{I}$I\mathcal{I}, ${\mathcal{I}}_{\pm 1}^{\ast }$${\mathcal{I}}_{\pm 1}^{\ast }$I_(+-1)^(**)\mathcal{I}_{ \pm 1}^{*} and ${\mathcal{I}}_{\pm 2}^{\ast }$${\mathcal{I}}_{\pm 2}^{\ast }$I_(+-2)^(**)\mathcal{I}_{ \pm 2}^{*}. From the matched features, we select the three points which maximize the surface of the corresponding triangle in the three images [32]. Once the new distances $\{\Vert {\mathbf{t}}_1\Vert ,\Vert {\mathbf{t}}_2\Vert \}$$\left\{\left({\mathbf{t}}_1\right),\left({\mathbf{t}}_2\right)\right\}${||t_(1)||,||t_(2)||}\left\{\left\|\mathbf{t}_{1}\right\|,\left\|\mathbf{t}_{2}\right\|\right\} are obtained, the closest candidate image ${\mathcal{I}}_1^{\ast }$${\mathcal{I}}_1^{\ast }$I_(1)^(**)\mathcal{I}_{1}^{*} to the current one is selected (line 23), i.e., the image with smaller $\Vert \mathbf{t}\Vert$$\Vert \mathbf{t}\Vert$||t||\|\mathbf{t}\|.
函数 selectMostSimilar（第 17-28 行）选择两个候选图像（ ${\mathcal{I}}_{\pm 1}^{\ast },{\mathcal{I}}_{\pm 2}^{\ast }$${\mathcal{I}}_{\pm 1}^{\ast },{\mathcal{I}}_{\pm 2}^{\ast }$I_(+-1)^(**),I_(+-2)^(**)\mathcal{I}_{ \pm 1}^{*}, \mathcal{I}_{ \pm 2}^{*} ），即具有最多点匹配数的两幅图像。然后，算法验证这两个候选图像是否属于图的同一分支 $\mathbf{G}$$\mathbf{G}$G\mathbf{G} 。如果是，则算法使用新的单应性分解中的相对距离 $\Vert \mathbf{t}\Vert$$\Vert \mathbf{t}\Vert$||t||\|\mathbf{t}\| 选择最相似的关键图像。该过程重新计算候选图像相对于当前图像的 $\mathbf{H}$$\mathbf{H}$H\mathbf{H} 和 $\mathbf{t}$$\mathbf{t}$t\mathbf{t} ，但此时用于单应性估计的虚拟平面是固定的（第 21-22 行）。因此，估计的距离是一致的。该过程如图 2 所示。它包括在三幅图像 $\mathcal{I}$$\mathcal{I}$I\mathcal{I} 、 ${\mathcal{I}}_{\pm 1}^{\ast }$${\mathcal{I}}_{\pm 1}^{\ast }$I_(+-1)^(**)\mathcal{I}_{ \pm 1}^{*} 和 ${\mathcal{I}}_{\pm 2}^{\ast }$${\mathcal{I}}_{\pm 2}^{\ast }$I_(+-2)^(**)\mathcal{I}_{ \pm 2}^{*} 之间匹配点特征。从匹配的特征中，我们选择在三幅图像中对应三角形面积最大的三个点[32]。一旦获得新的距离 $\{\Vert {\mathbf{t}}_1\Vert ,\Vert {\mathbf{t}}_2\Vert \}$$\left\{\left({\mathbf{t}}_1\right),\left({\mathbf{t}}_2\right)\right\}${||t_(1)||,||t_(2)||}\left\{\left\|\mathbf{t}_{1}\right\|,\left\|\mathbf{t}_{2}\right\|\right\} ，则选择与当前图像最接近的候选图像 ${\mathcal{I}}_1^{\ast }$${\mathcal{I}}_1^{\ast }$I_(1)^(**)\mathcal{I}_{1}^{*} （第 23 行），即具有较小 $\Vert \mathbf{t}\Vert$$\Vert \mathbf{t}\Vert$||t||\|\mathbf{t}\| 的图像。

If the candidate images do not belong to the same branch of the graph, the algorithm selects the most similar key image aided by a path planner that finds the shortest path from the two candidates ( ${\mathcal{I}}_{\pm 1}^{\ast },{\mathcal{I}}_{\pm 2}^{\ast }$${\mathcal{I}}_{\pm 1}^{\ast },{\mathcal{I}}_{\pm 2}^{\ast }$I_(+-1)^(**),I_(+-2)^(**)\mathcal{I}_{ \pm 1}^{*}, \mathcal{I}_{ \pm 2}^{*} ) to the target image ${\mathcal{I}}_n^{\ast }$${\mathcal{I}}_n^{\ast }$I_(n)^(**)\mathcal{I}_{n}^{*} (lines 25-27). This consideration is needed to discard a localization that might derive in a long path in the navigation stage. The paths associated to the candidate images are denoted ${\mathbf{I}}_1,{\mathbf{I}}_2$${\mathbf{I}}_1,{\mathbf{I}}_2$I_(1),I_(2)\mathbf{I}_{1}, \mathbf{I}_{2} and their corresponding lengths are ${\mathbf{l}}_1,{\mathbf{l}}_2$${\mathbf{l}}_1,{\mathbf{l}}_2$l_(1),l_(2)\mathbf{l}_{1}, \mathbf{l}_{2}. The solution ${\mathcal{I}}_1^{\ast }$${\mathcal{I}}_1^{\ast }$I_(1)^(**)\mathcal{I}_{1}^{*} of the localization is the candidate image associated
如果候选图像不属于图的同一分支，算法将借助路径规划器选择最相似的关键图像，该路径规划器寻找从两个候选图像（ ${\mathcal{I}}_{\pm 1}^{\ast },{\mathcal{I}}_{\pm 2}^{\ast }$${\mathcal{I}}_{\pm 1}^{\ast },{\mathcal{I}}_{\pm 2}^{\ast }$I_(+-1)^(**),I_(+-2)^(**)\mathcal{I}_{ \pm 1}^{*}, \mathcal{I}_{ \pm 2}^{*} ）到目标图像 ${\mathcal{I}}_n^{\ast }$${\mathcal{I}}_n^{\ast }$I_(n)^(**)\mathcal{I}_{n}^{*} 的最短路径（第 25-27 行）。此考虑是为了排除可能导致导航阶段路径过长的定位。与候选图像相关联的路径记为 ${\mathbf{I}}_1,{\mathbf{I}}_2$${\mathbf{I}}_1,{\mathbf{I}}_2$I_(1),I_(2)\mathbf{I}_{1}, \mathbf{I}_{2} ，其对应的长度为 ${\mathbf{l}}_1,{\mathbf{l}}_2$${\mathbf{l}}_1,{\mathbf{l}}_2$l_(1),l_(2)\mathbf{l}_{1}, \mathbf{l}_{2} 。定位的解 ${\mathcal{I}}_1^{\ast }$${\mathcal{I}}_1^{\ast }$I_(1)^(**)\mathcal{I}_{1}^{*} 是与路径最短的候选图像相关联的图像，
to the path with the minimum length, which is obtained by a function getShortestPath. The length of a path is the sum of the values of its edges as defined in (1).
该路径最短由函数 getShortestPath 获得。路径的长度是其边值的总和，如公式（1）所定义。

Once the robot is localized, i.e., ${\mathcal{I}}_1^{\ast }$${\mathcal{I}}_1^{\ast }$I_(1)^(**)\mathcal{I}_{1}^{*} and ${\mathcal{I}}_n^{\ast }$${\mathcal{I}}_n^{\ast }$I_(n)^(**)\mathcal{I}_{n}^{*} are known, the next stage is the visual path planning, which consists of finding the set of key images ${\mathbf{I}}^{\ast }=\{{\mathcal{I}}_1^{\ast },{\mathcal{I}}_2^{\ast },\dots ,{\mathcal{I}}_{n-1}^{\ast },{\mathcal{I}}_n^{\ast }\}$${\mathbf{I}}^{\ast }=\left\{{\mathcal{I}}_1^{\ast },{\mathcal{I}}_2^{\ast },\dots ,{\mathcal{I}}_{n-1}^{\ast },{\mathcal{I}}_n^{\ast }\right\}$I^(**)={I_(1)^(**),I_(2)^(**),dots,I_(n-1)^(**),I_(n)^(**)}\mathbf{I}^{*}=\left\{\mathcal{I}_{1}^{*}, \mathcal{I}_{2}^{*}, \ldots, \mathcal{I}_{n-1}^{*}, \mathcal{I}_{n}^{*}\right\} that links the starting and the desired key images of the VM. Thus, ${\mathbf{I}}^{\ast }$${\mathbf{I}}^{\ast }$I^(**)\mathbf{I}^{*} is the path of minimum length in $\mathbf{G}$$\mathbf{G}$G\mathbf{G} that connects the nodes ${\mathcal{I}}_1^{\ast }$${\mathcal{I}}_1^{\ast }$I_(1)^(**)\mathcal{I}_{1}^{*} and ${\mathcal{I}}_n^{\ast }$${\mathcal{I}}_n^{\ast }$I_(n)^(**)\mathcal{I}_{n}^{*} and it is obtained by a function getShortestPath.
一旦机器人定位完成，即已知 ${\mathcal{I}}_1^{\ast }$${\mathcal{I}}_1^{\ast }$I_(1)^(**)\mathcal{I}_{1}^{*} 和 ${\mathcal{I}}_n^{\ast }$${\mathcal{I}}_n^{\ast }$I_(n)^(**)\mathcal{I}_{n}^{*} ，下一阶段是视觉路径规划，其内容是寻找连接起始关键图像和目标关键图像的关键图像集合 ${\mathbf{I}}^{\ast }=\{{\mathcal{I}}_1^{\ast },{\mathcal{I}}_2^{\ast },\dots ,{\mathcal{I}}_{n-1}^{\ast },{\mathcal{I}}_n^{\ast }\}$${\mathbf{I}}^{\ast }=\left\{{\mathcal{I}}_1^{\ast },{\mathcal{I}}_2^{\ast },\dots ,{\mathcal{I}}_{n-1}^{\ast },{\mathcal{I}}_n^{\ast }\right\}$I^(**)={I_(1)^(**),I_(2)^(**),dots,I_(n-1)^(**),I_(n)^(**)}\mathbf{I}^{*}=\left\{\mathcal{I}_{1}^{*}, \mathcal{I}_{2}^{*}, \ldots, \mathcal{I}_{n-1}^{*}, \mathcal{I}_{n}^{*}\right\} 。因此， ${\mathbf{I}}^{\ast }$${\mathbf{I}}^{\ast }$I^(**)\mathbf{I}^{*} 是在 $\mathbf{G}$$\mathbf{G}$G\mathbf{G} 中连接节点 ${\mathcal{I}}_1^{\ast }$${\mathcal{I}}_1^{\ast }$I_(1)^(**)\mathcal{I}_{1}^{*} 和 ${\mathcal{I}}_n^{\ast }$${\mathcal{I}}_n^{\ast }$I_(n)^(**)\mathcal{I}_{n}^{*} 的最短路径，由函数 getShortestPath 获得。