1 引言（Introduction）

随着科学技术的快速发展，移动机器人从最初的工厂物流搬运，渐渐应用到家庭服务、农业、军事、医疗等各个领域，提升了生产效率并改变了人类的生活方式^[1-6]。同步定位和地图构建（SLAM）技术是自主移动机器人的核心技术之一^[7-9]，该技术可以根据运动过程中传感器的实时数据实现机器人自身的位姿估计，同时增量式地构建整个环境的地图^[10]，进而根据构建好的地图实现自主规划与导航。可见，构建机器人应用场景地图是使机器人具有自主运动能力的关键环节。但目前移动机器人还是需要技术人员手动控制机器人在陌生场景中的运动来实现地图构建。这种人工控制机器人建图的方式不仅对人员要求高且预先部署需要花费大量时间，效率低下，而且也会影响机器人在人类无法进入的场景中使用^[11-12]。因此，使机器人具备自主探索的能力，从而在陌生的环境中能够自动构建地图，是目前移动机器人需要解决的难题之一，这不仅要求机器人具备基本的SLAM能力，还要具备对当前接收到的环境信息进行分析和决策的能力，机器人需要自主地决策运动方向并规划移动路径^[13-15]。

在机器人自主探索领域的研究中，Yamauchi^[16]提出了一种基于前沿点的自主探索算法，该算法的核心思想是不断地从当前地图边界选取最佳目标点，在驱动机器人前往该目标点移动的同时，逐渐扩展地图边界，重复这个过程可构建出整个环境的地图。该算法最大的亮点在于机器人在探索过程中会更加有目的性地自主选取探索区域，相对随机式的探索而言，建图的效率得到了很大的提升。文[17-18]均是基于这一思路提出的自主探索方法，在基于前沿点的自主探索方法中，探测地图的边界是选取最佳目标点的前提；Keidar^[18]和Senarathne^[19]等改进了原有的边界提取模块，避免对整个地图数据进行处理，降低了算法的运算时间和内存消耗；刘瑞雪等^[20]提出了一种基于曲线拟合和目标探索点邻域规划的自主建图方法，同样也取得了一定的效果；李秀智等^[21]在前沿点探索理论的基础上，提出了一种基于几何规则的候选目标点生成方法，实现了目标点的快速提取。

快速扩展随机树（RRT）是一种用于解决点到点路径规划问题的算法，尽管所选取的路径并非是全局最优路径，但是RRT算法在执行时具有概率完备性。机器人在自主探索任务中，需要自主构建一个覆盖完整度较高的地图，而RRT算法的良好特性刚好可以保证较高的地图覆盖程度。文[22-25]均是通过动态建立的随机探索树来完成整个环境的探测。国内的高环宇等^[24]改善了探索树的回溯策略，减少了多次覆盖和陷入回路的情况。杨傥月等^[26]提出了一种改进的RRT算法，利用变生长率的全局RRT来提高探索的效率。杜明博等^[27]提出了一种连续曲率RRT算法，结合环境约束以及车辆自身的约束大大提高了规划的速度和质量。

在自主探索过程中，移动机器人的建图与定位还要依赖于视觉或激光传感器。基于粒子滤波的SLAM算法有Hector-SLAM、Gmapping等，这类算法在对机器人进行位姿估计时只需对当前帧的位姿进行优化，并不需要处理当前帧之前的位姿。在这种情况下，自主探索算法可以分成2个独立的模块，分别是未知环境路径规划模块和SLAM算法。Sun等^[28]采用了一种基于图优化的SLAM算法，由于图优化算法进行位姿估计时会对所有的历史位姿进行优化，因此将基于图优化的算法用于自主探索任务中时，地图建立的准确性会更高。文[29-30]同样也是把自主探索任务中的SLAM模块改为基于图优化的算法。然而基于图优化的SLAM算法在自主探索任务中计算量增长较快，将降低机器人自主探索的效率。

综上可知，目前机器人自主探索可以从以下2个方面进行改进：一个是在算法循环执行过程中，如何更加快速地提取前沿区域；另一个是在机器人移动探索过程中，如何减少其陷入回路的次数，进而提升整个自主探索算法的执行效率。因此本文提出了TMRRT自主探索算法，以RRT探索算法为基础，加入拓扑地图模块，拓扑地图模块会记录机器人过往探索的目标点，用于辅助筛选下一刻的最佳探索点，在一定程度上减少机器人陷入回路的次数。通过Mean Shift算法对前沿点进行聚类处理，在RRT模块中设置自适应的增长速度，加快初始阶段对前沿区域的检测速度，提高移动机器人自主探索的实时性。

2 TMRRT自主探索算法（TMRRT autono-mous exploration algorithm）

本文提出的自主探索算法的执行流程如图 1所示。整个算法总体可分为3个模块，分别是基于改进RRT算法的前沿点探测模块、前沿点筛选模块以及执行器模块。前沿点探测模块利用改进的RRT路径规划算法探测当前地图的已知区域与未知区域的边界，并提取边界上周围所有可能有一定的未知区域点的前沿目标点。前沿点筛选模块接收探测模块探测到的所有前沿点，并对前沿点进行聚类处理，通过设定效用函数筛选出最佳前沿点。执行器模块将筛选出的最佳前沿点发送给执行器进行后续的路径规划，并利用SLAM技术同步扩展更新地图。同时，执行器会将最佳前沿点添加到拓扑地图中，从而更新拓扑地图。

2.1 拓扑地图表示法

在拓扑地图的表示模型中，整个环境地图根据一定的分辨率进行栅格化处理，每个栅格有占用、空闲和未知3种状态。用$ - $1、0和1来表示每个栅格的3种状态，在地图建立的过程中，对于已经建立地图的区域，所有的栅格只会有2种状态。若某个栅格对应的物理空间中是有障碍物的，则该栅格的状态为占用，在地图上的值为1；若该物理空间没有障碍物存在，则该栅格的状态为空闲，其对应的值为0。其他未显示的区域为未知栅格，其对应的值为$ - $1。与未知栅格相邻的所有空闲栅格（即图 2中标注有f的栅格）都属于当前地图的边界，也被称为前沿点。

RRT算法具有概率完备性，因而可以根据是否生成新的前沿点来判断地图是否还有未探索的区域。若没有新的前沿点生成，且聚类后的前沿点皆被探索到，则认为已经完成对未知区域的探索。

2.2 TMRRT前沿点探测模块

在前沿点探测模块中，分别采用全局RRT和局部RRT算法探测地图上的边界。初始阶段，RRT的状态为$ V=\{X_{\text{init}}\} $，$ E=\varnothing $，其中$ X_{\text{init}} $表示机器人的出发位置，$ E $表示节点间的拓扑距离的集合，初始为空，$ V $表示整个生成树的所有节点，令$ G(V, E) $表示整个RRT树状结构。全局RRT算法以机器人出发点为根节点，通过SampleTree函数不断地在地图中进行采点，得到$ X_{\text{sample}} $，随后从当前的生成树中找到与采样点最近的树节点$ X_{\text{nearest}} $，然后在$ X_{\text{sample}} $和$ X_{\text{nearest}} $之间根据RRT的生长率参数$ \eta $，通过Steer函数生成一个新节点$ X_{\text{new}} $。将参数$ \eta $由一般的经验值设置为可变的值来加快RRT的生长速率，其具体的值由$ \eta_{\text{gain}} $函数确定，$ \eta_{\text{gain}} $的求解原理是通过求解$ X_{\text{init}} $和$ X_{\text{nearest}} $之间的距离，在满足一定距离条件下设置$ \eta $与该距离的值成反比。最后通过一个Check函数来判断$ X_{\text{new}} $周围的情况，当检测到$ X_{\text{new}} $周围没有未探索的栅格点时，则将$ X_{\text{new}} $加入到RRT $ G $中；反之将$ X_{\text{new}} $视作一个前沿点，并将其发送到前沿点筛选模块中。整个过程执行的伪代码如算法1所示。

算法1  自适应生长速率的全局RRT
1:  V = Xinit; E = Ø
2:  while True do
3:    Xsample ← SampleFree()
4:    Xnearest ← Nearest (G(V; E); Xsample)
5:    η ← ηgain(Xnearest; Xinit)
6:    Xnew ← Steer (Xnearest; Xsample; η)
7:    if Check (Xnew) = −1 then
8:      发送Xnew到筛选模块
9:    else if Check (Xnew) = 0 then
10:      V ← V ∪ {Xnew}, E ← E ∪ {(Xnearest; Xnew)}
11:    end if
12:  end while

局部RRT探测器的执行过程和全局RRT探测器的执行流程基本一致。考虑到探索后期，全局RRT的步长会越来越短，导致无法及时有效地探测到部分角落处的点，因此需要用局部RRT来弥补这一缺陷，且局部RRT的生长速率较全局RRT更低。为了提高执行速度，利用局部RRT每探测到一个前沿点，便清空局部RRT的所有节点，并重新建立新的RRT。

2.3 前沿点筛选模块

算法在执行过程中，前沿点探测模块的探测速度会比较快，因此筛选模块会收到大量的前沿点，并且一部分前沿点会聚集分布在地图中。故本文采用MeanShift聚类算法在筛选最佳前沿点之前对所有前沿点的2维坐标进行均值聚类，从而降低前沿点的数据规模，最后得到若干个聚类中心坐标。

设共有$ n $个2维前沿点$ x_{1}, x_{2}, \cdots, x_{n} $，每个前沿点的MeanShift向量形式为

$ \begin{align} {\mathit{\boldsymbol{M}}}_{{{h}}} (x)=\frac{1}{k}\sum _{\mathit{\boldsymbol{x}}_{i} \in {S}_{h}} (\mathit{\boldsymbol{x}}_{i} -\mathit{\boldsymbol{x}}) \end{align} $

(1)

其中，$ {S}_{h} $是指除去$ x $点（聚类中心）外其他前沿点坐标的集合，即：

$ \begin{align} {S}_{{{h}}} (\mathit{\boldsymbol{x}})=\{\mathit{\boldsymbol{y}}\big|(\mathit{\boldsymbol{y}}-\mathit{\boldsymbol{x}})(\mathit{\boldsymbol{y}}-\mathit{\boldsymbol{x}})^{\rm T}\leqslant h^{2}\} \end{align} $

(2)

式(1) 为算法迭代一次的过程，可见每次迭代就是把样本点$ x $往某个邻域的中心移动，并把邻域$ {S}_{h} $内的其他点标记为同一类，移动结束之后开始新的一轮迭代。然而这种简易的迭代存在一个问题，即该方法在邻域$ {S}_{h} $内将所有点对样本点$ x $的贡献认为是一样的。MeanShift算法利用一个核函数来衡量每个偏移点对中心点偏移向量的贡献。核函数的定义如下：假设存在一个$ d $维的欧氏空间$ X $，$ x $是空间$ X $中的一个点，其中$ {\mathit{\boldsymbol{x}}} $的模为$ \|\mathit{\boldsymbol{x}}\|^{2}=\mathit{\boldsymbol{x}}{{\mathit{\boldsymbol{x}}}}^{\rm T} $，如果函数$ K $存在一个剖面函数$ k $，即：

$ \begin{align} K(\mathit{\boldsymbol{x}})=k(\|\mathit{\boldsymbol{x}}\|^{2}) \end{align} $

(3)

则称$ K(\mathit{\boldsymbol{x}}) $为核函数，其中$ k $是非负的、非增的、分段连续的，这里取高斯函数作为核函数，即：

$ \begin{align} N({\mathit{\boldsymbol{x}}})=\frac{1}{h\sqrt{2\pi }}{\rm e}^{-\frac{{\|\mathit{\boldsymbol{x}}\|}^{2}}{2 h^{2}}} \end{align} $

(4)

其中，$ h $为带宽参数。

为了将样本点$ x $的$ {S}_{h} $邻域内的其他点设定不同的权值，将核函数添加到MeanShift向量的公式中，得到改进后的MeanShift向量公式：

$ \begin{align} {\mathit{\boldsymbol{M}}}_{\mathit{\boldsymbol{x}}} (\mathit{\boldsymbol{x}})=\frac{ \sum _{i=1}^{n} G \left(\frac{\mathit{\boldsymbol{x}}_{i} -\mathit{\boldsymbol{x}}}{h_{i}}\right)(\mathit{\boldsymbol{x}}_{i} -\mathit{\boldsymbol{x}})} { \sum _{i=1}^{n} G \left(\frac{\mathit{\boldsymbol{x}}_{i} -\mathit{\boldsymbol{x}}}{h_{i}}\right)} \end{align} $

(5)

其中，$ G\big(\dfrac{\mathit{\boldsymbol{x}}_{i} -\mathit{\boldsymbol{x}}}{h_{i}}\big) $为核函数。

由上式可知，邻域$ {S}_{h} $内除样本点$ x $之外的点在$ x $的MeanShift向量中重要性是不同的，因此可以考虑引入一个权值系数用于将MeanShift向量还原为样本点，得到扩展后的向量形式，如式(6) 所示，其中$ w(\mathit{\boldsymbol{x}}_{i}) $为每个点的权重。

$ \begin{align} {\mathit{\boldsymbol{M}}}_{\mathit{\boldsymbol{x}}} (\mathit{\boldsymbol{x}})=\frac{ \sum _{i=1}^{n} G \left(\frac{\mathit{\boldsymbol{x}}_{i} -\mathit{\boldsymbol{x}}}{h_{i}}\right)w(\mathit{\boldsymbol{x}}_{i})(\mathit{\boldsymbol{x}}_{i} -\mathit{\boldsymbol{x}})} { \sum _{i=1}^{n} G \left(\frac{\mathit{\boldsymbol{x}}_{i} -\mathit{\boldsymbol{x}}}{h_{i}}\right)w(\mathit{\boldsymbol{x}}_{i})} \end{align} $

(6)

最终将探测器模块中采集到的所有前沿点的坐标$ (x, y) $保存至一个数组中，并通过MeanShift聚类算法进行聚类。最终得到多个类的中心，并保存为2维坐标，这样可以极大地减少计算量。

2.4 执行器模块

拓扑地图相对于普通的栅格地图或者其他形式的地图，地图数据量更少。因为拓扑地图仅仅提取了环境中一些关键信息，以节点或者带权边的形式进行存储。文中提出的拓扑地图通过带权的无向图数据结构的形式进行存储，其中拓扑节点由每轮循环选出的最佳前沿点构成，节点之间的权值代表 2个节点之间的拓扑距离。在探索过程中不断动态更新拓扑地图，同时在前沿点的评估函数中将机器人与当下前沿点的拓扑距离作为一个损失项，从而引导机器人选取距离其当前位置最近的一个前沿点，这在一定程度上能够提高探索效率。

本文建立的拓扑地图表示为$ G(V, E) $，其中$ V $由机器人在探索过程中收到的一系列最佳探索点$ X_{f}^{*} $构成。假设当前拓扑地图中有$ N $个节点，每个节点$ V_{i} $的定义如下：

$ \begin{align} V_{i} =\langle i, p_{i}, \mathit{\boldsymbol{c}}_{i}, E_{i}\rangle \end{align} $

(7)

其中，$ i $是节点的索引，$ p_{i} $是$ V_{i} $节点的父节点在拓扑地图中的索引，$ \mathit{\boldsymbol{c}}_{i} $是第$ i $个节点对应的2维物理坐标。$ E_{i} =\{e_{1}, e_{2}, \cdots, e_{j} \} $是与节点$ V_{i} $相连的所有带权的边，由于一个节点可能存在多条带权值的边，因此集合$ E_{i} $中每条带权边可以表示为

$ \begin{align} e=\langle k, d\rangle \end{align} $

(8)

其中，$ k $是指该带权边指向的终点节点序号，$ d $是指从拓扑地图中第$ i $个节点至第$ k $个节点之间的欧氏距离。

在执行器模块中，得到具有最大探索增益$ R_{\max} $的最佳探索点$ X_{f}^{*} $，随后驱动机器人移动到这个点上，在移动过程中栅格地图也在同步更新，同时添加拓扑地图模块$ G_\text{TOP} (V, E) $，对拓扑地图进行动态更新，这样才能保证机器人高效地完成环境的遍历，构建整个环境的地图。

对于每个新得到的拓扑节点$ X_{f}^{*} $，首先找到拓扑地图中离$ X_{f}^{*} $最近的拓扑节点$ p_{f} $，并计算$ X_{f}^{*} $和$ p_{f} $的欧氏距离$ d $。由于拓扑图记录了历史最佳探索点的全局位置，因此当$ d $过小时，拓扑地图并不会将$ X_{f}^{*} $加入到拓扑结构中，这是为了避免过多的拓扑节点出现聚集现象。而对于满足条件的候选拓扑节点$ X_{f}^{*} $，利用广度优先算法（BFS），遍历计算$ X_{f}^{*} $与$ {{V}}=\{{ X}_{1}, { X}_{2}, \cdots, { X}_{n} \} $所有连通节点的拓扑距离，最终可得到更新之后的拓扑地图$ G_\text{TOP} ({V}, E) $，其伪代码如算法2所示。

算法2  动态更新拓扑地图
1:  V = {Xinit}; E = Ø
2:  while True do
3:    Xf* ← filter
4:    if Xf* is NULL then
5:      break
6:    else
7:      d = Nearest(GTOP(V; E); Xf*)
8:      if d > N0 then
9:        V ← V ∪ {Xf*}
10:        E ← BFS{GTOP(V; E); Xf*}
11:      end if
12:    end if
13:  end while

机器人探索中的地图如图 3所示，其中红色为生成的拓扑地图，深黑色为环境中的障碍物。

3 同步定位与地图构建（Simultaneous localization and mapping）

在2维环境下的自主探索任务中，通常用占据栅格地图作为地图的表现形式，地图也是机器人实现后续路径规划和导航的基础。在栅格地图中通常以固定分辨率将实际环境划分成若干个正方形栅格，从而得到环境的栅格地图。复杂的室内环境通常存在诸多分层障碍物，如桌子、椅子等。受单线激光雷达安装高度的影响，机器人难以识别高于激光安装高度的障碍物，进而导致机器人与障碍物相撞，故本文在开源的SLAM算法Gmapping上融合了激光与3维视觉传感器，将分层障碍物构建在栅格地图中，避免机器人与分层障碍物相撞。

经过深度相机和激光联合标定之后，需要将深度图像进行转换。具体来说，首先需要提取每个像素列上具有最小深度值的像素点$ p_{\min} ^{i} $，将每列的像素坐标$ (u^{i}, v^{i}) $进行转换，得到对应的伪激光坐标$ (r^{i}, \theta^{i}) $，最终形成一系列的虚拟激光点。转换模块的具体步骤如下：

1) 设定高度值。对深度图像中每列所有的像素点，只选用其中的一部分数据。通过设定高度阈值$ h $，最终保证所有的像素点均处于图 4(b)中深度图像中2条虚线范围之内，并假定每个像素坐标的取值区间为$ [b, b+h] $。

2) 伪激光数据转换。假设经过高度筛选之后，每列的像素个数为$ N $个，共有$ M $列。把每列$ N $个像素点对应空间点的3维坐标转换成相机坐标系下的极坐标$ (r, \theta) $，并选取每列具有最小$ r $值的像素点，最终可得到含有$ M $个测距值的激光数据$ r_{\min}^{1}, $ $ r_{\min}^{2}, $ $ \cdots, $ $ r_{\min}^{M} $。

如图 5所示，以第$ i $列的第$ i $个像素点$ P' $为例，其为空间中的任意一点，为了求其极坐标$ (r_{j}^{i}, \theta_{j}^{i} ) $，首先根据RGB图像和深度图像可直接得到点在相机坐标系下的坐标$ (x_{\rm c}, y_{\rm c}, z_{\rm c}) $。将该点投影到极坐标平面$ NO_{\rm c} X_{\rm c} $得到极坐标：

$ \begin{align} r_{j}^{i} & =\sqrt{x_{\rm c}^{2} +z_{\rm c}^{2}} \end{align} $

(9)

$ \begin{align} \theta_{j}^{i} & =\arctan \dfrac{z_{\rm c} }{x_{\rm c}} \end{align} $

(10)

经上述步骤之后，由一张$ N\times M $像素的RGB-D图像最终得到了在相机坐标系下的测距数据$ D_{\rm c} =\{d_{1}, d_{2}, \cdots, d_{M}\} $，其中$ d_{i} =(r_{\min}^{i}, \theta^{i}) $。令$ [\alpha_{\min}, \alpha_{\max} ] $为深度相机的视角区间，视角区间的大小值一般由图像的分辨率决定。同时激光传感器获取到的测距数据为$ D_{\rm L} =\{d_{1}, d_{2}, \cdots, d_{Q}\} $，视角区间为$ [\beta_{\min}, \beta_{\max}] $，一般来说相机的覆盖范围小于激光雷达覆盖的角度范围，即$ [\alpha_{\min}, \alpha_{\max}]\subset [\beta_{\min}, \beta_{\max} ] $。融合两部分测距数据的算法伪代码如算法3所示，其中$ D_{\rm F} $代表融合后的传感器数据，初始化为空集。

算法3  激光测距数据融合相机测距数据
输入: Dc; DL
输出: DF
1:  DF = Ø
2:  for (r; θ) in DL do
3:    if θ > βmin and θ < βmax then
4:      j = findNearestIdxFromAngle(βmin; βmax; θ)
5:      (l; γ) = getDistanceFromIndex(Dc; j)
6:      if l < r then
7:        Append(DF; l; γ)
8:      else
9:        Append(DF; r; θ)
10:      end if
11:    else
12:      Append(DF; r; θ)
13:    end if
14:  end for

算法3中，findNearestIdxFromAngle函数输入角度值$ \theta $，输出$ D_{\rm c} $中与角度$ \theta $最接近的$ d_{i} $；getDistanceFromIndex函数则根据输入的索引项$ j $返回$ D_{\rm L} $中的$ d_{j} $；Append函数则直接在$ D_{\rm F} $中添加一个极坐标值。

4 实验（Experiment） 4.1 仿真实验

为了验证所提算法的有效性，在机器人操作系统（ROS）环境中利用仿真器Gazebo搭建室内仿真环境1和2，整个环境的覆盖面积约为20 m$ \times $20 m，如图 6所示。仿真实验所用的机器人的最大移动线速度设置为0.3 m/s，机器人搭载的单线激光雷达传感器的最远探测距离设置为10 m，激光雷达水平探测范围设置为$ - $90$ ^{\circ} $~90$ ^{\circ} $。

实验分别对比了RRT自主探索算法^[9]和FA自主探索算法。在实验条件相同的条件下对RRT自主探索算法、FA自主探索算法与TMRRT自主探索算法进行10次对比实验。实验过程中以环境的中心点为起点，3种自主探索算法在室内环境1中所行驶的路线如图 7所示，图 8为3种自主探索算法在环境2中所行驶的路线。从图 7、8中可以看出TMRRT自主探索算法的路径长度较FA与RRT算法明显减小，机器人不会多次经过已到达的区域，进而重复路径的情况有所改善，轨迹曲线变得更加平滑。这表明拓扑结构的引入可以降低自主探索对计算机内存的依赖。

为了进一步验证本文算法的有效性，从机器人探索时间与路径长度2个方面评估算法的优劣。在2种地图下运行算法10次所得到的运行距离与运行时间如图 9所示。图 10分别是3种自主探索算法在2个仿真环境中测试运行距离与运行时间的平均值。实验数据表明：在仿真环境1中，TMRRT算法相对RRT算法，其平均用时减小了7.5%，平均移动距离减小了20.8%；相对FA算法，其在运行时长和移动距离上分别减小了15.7% 和38.6%。在仿真环境2中，TMRRT算法相对RRT算法，其探索时长减小了11.8%，平均移动距离减小了19.8%；相对FA算法，其平均运行时间和移动距离分别减小了22.7% 和34.3%。

综上所述，本文所提出的TMRRT自主探索算法在RRT自主探索算法的基础上引入拓扑地图结构可以避免机器人在探索过程中反复到达已探索的路径，使机器人在运行时间与运行路径总长度上均有所改善，提高了机器人自主探索的效率。

4.2 实际场景实验

在实物实验中所用机器人如图 11所示，搭载的传感器包括思岚RPLidar-A2的单线激光雷达、RealSense D435i深度相机以及MPU9250惯性传感单元。底盘的控制主机型号为ThinkPad-T480，CPU型号为Intel Core i5-8250U 8核1.6 GHz，内存为16 GB。搭载的传感器中单线激光雷达的视角范围为$ - $180$ ^{\circ} $~180$ ^{\circ} $，探测的距离为6 m，深度相机的视角范围约为58$ ^{\circ} $，检测距离的范围设置为3 m。将相机和单线激光雷达均安装在机器人的中央。机器人的最高运行速度设定为0.25 m/s。为了模拟真实场景，场地中均摆放有不同高度的障碍物。

图 12分别表示了实验环境与机器人所构建的环境地图，记录的数据如表 1所示，其中机器人进行自主探索所消耗的时间为$ T $，小车走过的路径长度为$ L $。

从图 12可知本文提出的自主探索算法在实际环境中同样能够通过机器人构建完整的环境地图，且能够将不同高度的障碍物建立到地图中。从表 1最终测试的数据来看，在面积约40 m$ ^{2} $的场地1中探索时长为46.5 s，可以快速完成对复杂室内环境的探索。

5 结论（Conclusion）

提出了一种基于拓扑地图的高效自主探索算法TMRRT，将具有最大增益的前沿点作为拓扑结构的顶点，2个前沿点之间的边为二者之间的距离，进而形成拓扑地图，在机器人运动过程中保持更新，用于机器人回溯。且RRT的生成速率可以随着点之间的距离自动改变，以便更好地探测到环境中的细微角落。相较于RRT与FA自主探索算法可以避免机器人反复到达已知区域，进而提高探索效率，且轨迹更加平滑。并且在单线激光雷达的基础上融入了相机的信息，使其可以将分层障碍物建立到地图中。

本文算法的实验环境为静态环境，未来将迁移到动态环境下测试算法的鲁棒性，并且建立高精度地图，供机器人进行搬运、擦拭等室内环境的作业。