CMU-TARE 探索算法官方社区问答汇总

参考引用

• TARE机器人自主导航系统社区-CSDN社区云
• TARE平台资源链接汇总
• CMU团队的terrainAnalysis地形分析代码笔记
• Local Planner 代码详解以及如何适用于现实移动机器人
• 论文翻译：Autonomous Exploration Development Environment and the Planning Algorithms
• Tare planner系列代码解析

首先感谢 CMU 团队的开源工作，本博客是为了方便在实车部署时遇到问题后查找相关思路用，相关问答全部来自 TARE机器人自主导航系统社区-CSDN社区云，问答汇总截止到 2024年3月25日，后面不定期更新

• 前置内容：CMU-自主探索导航系统（TARE & FAR Planner）学习-All in one

1. keypose 和 ：keypose_graph

• 问：keypose 指的是什么呢，cell 中 keypose_id 只有一个吗？

• 答：你好，keypose 指的是 SLAM 在产生关键帧的时候机器人所在的位置。当 loop closure 发生的时候，所有的 keypose 会被一起调整。这里 cell 里面存的 keypose 本意是想存一个能观测到这个 cell 的机器人的位置，但后来可能没有用这个数据
  

• 问：keypose_graph 的问题

• 答：keypose_graph 是用来搜索路径用的，相当于一个 roadmap。keypose 最开始是从 SLAM 端来的，如果 SLAM 算法提供闭环检测的话，keypose 就是关键帧对应的机器人位姿。在开源的版本里省去了对闭环的处理所以直接从机器人的轨迹上取点了。普通点是保留了机器人周围的可行驶视点。keypose_graph_cloud 内有两类，is_keypose 和 普通节点。keypose 是机器人行进轨迹上按一定频率留下来的点；其他普通节点是在 global planning里，以视点候选点为节点求出 cells 之间的路径后，在这些路径上留下来的点

2. cell 的 status 转换条件

• 问：cell 的 status 转换条件没看太懂，能否解释一下呢？
• 答：Cell status 的转换条件大概是如果能 sample 到至少一个 viewpoint，这个 cell 就是 exploring。如果 cell 里面有 viewpoint 且它们都看不到足够的 surface，这个 cell 就是会被加入 almost covered 的列队。等到它不在 local planning horizon 里面了就被标记成 covered。为了处理一些边界条件我在后面加了一些工程 tricks 来让 cell 的 status 保持稳定
  

3. viewpoint 的属性

• 问：viewpoint 的属性，collision_frame_count_、in_current_frame_line_of_sight_、in_line_of_sight_ 分别代表什么？
• 答：这几个变量是用来处理动态障碍物的。collision_frame_count_ 指一个 viewpoint 在连续多少帧内存在碰撞。in_current_frame_line_of_sight_ 指一个 viewpoint 是否能在当前帧被看（ray trace）到。in_line_of_sight_ 指一个 viewpoint 是否曾经被看（ray trace）到过。具体逻辑可以参考代码，但大概的思想是如果一个 viewpoint 在几帧之前被认为有 collision 但却能在当前帧被 ray trace 到，这个viewpoint就不被认为是有 collision

4. Boundary 指的是什么

• 问：Boundary 指的是什么，如下

  bool kUseCoverageBoundaryOnFrontier; 
  bool kUseCoverageBoundaryOnObjectSurface;
  

• 答：Boundary 指的是探索边界，如果用户想限制机器人探索的区域，可以通过设置这个 boundary 来实现。kUseCoverageBoundaryOnFrontier 指的是用边界来限制生成的frontier，也就是算法只考虑 boundary 以内的 frontier，以外的就不管了。kUseCoverageBoundaryOnObjectSurface 同理

5. ExtractVerticalSurface 方法的依据

• 问：ExtractVerticalSurface，请问该方法中的依据是什么呢，为啥可以提取出 surface 呢？
• 答：因为我们开源的代码是给地面机器人使用的，探索的过程中只考虑相对垂直的表面（vertical surface），比如墙上的点。ExtractVerticalSurface 这个函数是希望把墙上的点都提取出来，把地上和天花板上的点扔掉。主要的想法是看每个点邻域内其他点的分布。如果一个点的邻域内其他点都和它有一定的高度差，那这个点很有可能就是墙上的点（vertical surface）。具体实现上用了一个操作点云的小技巧。先把点云的高度（z）存在 PointXYZI 的 intensity 里面，然后把 z 设成 0，就得到了一个在xy平面上的扁平化的点云。然后对每一个点用 kdtree 搜索邻域，如果邻域内点的 intensity，也就是 z，存在一定高度差，那么它就是墙上的点。值得注意的是，如果这个高度差太大，那它可能是天花板或者地上的点。因为把点云在 xy 平面上压扁后天花板上的点和地上的点混合在了一起

6. diff_cloud_ 指的是什么呢

• diff_cloud 指的是一帧点云当中和之前几帧相比有变化的点

7. uncovered 和 covered 点

• 问：为甚么会有 uncovered 的点呢，点不都是来自于点云，只要是点云应该都是被扫描（covered）到才出现的呀，为什么还会有uncovered的呢？是指当前的 surface 中，未被 covered 的 surface 吗？
• 答：简单来说，但激光雷达扫描到环境中的点之后，我们并不认为这个点被covered了。只有当机器人离它足够近（理论上还需要在一个比较好的观测角度）才认为它被covered了
• 问：引入surface，而不用单独的 frontier 的目的，就是为了牵制 vehicle，不让它太贪心，一直朝着 frontier 走，对吗？？？
• 答：是的。我们这个探索算法的主要目的是建立一个完整的高质量的地图，所以我们希望机器人不仅仅只是“看到”了 surface，还要把它“看好”

8. 怎么保存建好的地图

• 问：TARE 的最终目的是针对环境自主探索并且建图，为了得到高质量的地图，还需要调整车辆的姿态，为了尽可能多地观察环境，但是我没有找到最终建好的地图保存在哪里，以及如何查看
• 答：TARE 只负责计算探索路径。最后的地图需要有另外一个线程来处理和保存。visualization_tools 会在 /explored_areas 话题下发出一个目前已经建立的点云地图。但这个地图是 downsample 过的。如果用户需要把所有 lidar 点都存下来，可以根据这个仿写一个处理的程序

9. 如何计算信息增益

• 问：没有看明白如何根据雷达模型来计算视点的信息增益的
• 答：一个视点的信息增益是它所能看到的 frontier 点和 object surface 点的和。每个视点有个单独分配的内存负责记录在某个方向上能看到多远。当有新的 frontier 或者 object surface 点的时候，我们分别判断它们能否被每个视点看到。如果能被看到则被算入增益。判断能否被看到的方法是比较它们到视点的距离是否超过内存当中记录的这个方向能看到的最远距离。具体可以参考https://github.com/caochao39/tare_planner/blob/1db22ff7eaf9431c5e898ad870f02449511d1fad/src/tare_planner/include/viewpoint_manager/viewpoint_manager.h

10. 局部路径的问题

• 问：获得视点集以后，通过 TSP 计算出各视点的顺序后，为什么不直接把这些视点直接当成 waypoint，直接交给 local planner 让机器人去就行了呢？而是去迭代各视点为 innerpoint 来平滑路径，得到路径了然后给出一个 waypoint 呢？因为在我认知里 local planer 很强大，给他一个 waypoint 他就能去，我跑了代码发现发现 waypoint 在路径外？
• 答：在复杂的环境中 local planner 确实可能会绕远路甚至无法到达视点，所以需要计算 local path 来连接每个视点。平滑路径用的 innerpoint 是为了给无人机探索用的，地面机器人的探索不需要这步平滑的操作。我们目前开源的代码中也不包括这部分。waypoint 在路径外是因为如果 waypoint 离机器人太近 local planner 的碰撞检测范围会缩小。为了保证机器人的安全我们把 waypoint 投射出去一定距离使 local planner 保持一个相对安全的碰撞检测范围。对于 local planner 来说 waypoint 离机器人的距离并不重要，相对于机器人的方向才重要。所以实际上 waypoint 还是在 local path 上，只是发给 local planner 的时候增加投射出去的操作
• 答2：通过这些视点计算出一条局部路径，让机器人走这条路径是最好的方案。但是如果直接拿各视点当 waypoint 的话，local planner 虽然会让机器人到达这里但是路径不能保证，说不定会绕远路。图中给的 waypoint 是能让 local planner 跟随路径的 waypoint。而且直接用视点当 waypoint 机器人的朝向不能保证

11. Cannot get candidate viewpoints, skipping this round

• 问：机器人不会自动探索，一动不动的，报警告 Cannot get candidate viewpoints, skipping this round
• 答：一般没有候选 viewpoint 很可能是因为环境不够开阔导致无法生成 collision-free 的 viewipoint。可以试一下 explore_matterport.launch。如果还不行，可以试下把 viewpoint 的 resolution（e.g. viewpoint_manager/resolution_x等）再调小

12. viewpoint_manager.cpp 几个问题

• 问：use_array_ind 指的是什么意思呢？每次采样的candidate viewpoint是当前帧视线范围内的，还是说只要看到过的呢？请问在 CheckViewPointLineOfSight() 中这段代码是什么意思呢，它是以什么原理取判断 viewpoint 是否在视线之内以及在当前视线之内呢

    for (int xi = 0; xi < 2; xi++)
      {
        for (int y = 0; y < vp_.kNumber.y(); y++)
        {
          for (int z = 0; z < vp_.kNumber.z(); z++)
          {
            int x = x_indices[xi];
            Eigen::Vector3i end_sub(x, y, z);
            int array_ind = grid_->GetArrayInd(end_sub);
            if (!checked[array_ind])
            {
              CheckViewPointLineOfSightHelper(robot_sub, end_sub, max_sub, min_sub);
              checked[array_ind] = true;
            }
          }
        }
      }

  for (int x = 0; x < vp_.kNumber.x(); x++)
  {
    for (int yi = 0; yi < 2; yi++)
    {
      for (int z = 0; z < vp_.kNumber.z(); z++)
      {
        int y = y_indices[yi];
        Eigen::Vector3i end_sub(x, y, z);
        int array_ind = grid_->GetArrayInd(end_sub);
        if (!checked[array_ind])
        {
          CheckViewPointLineOfSightHelper(robot_sub, end_sub, max_sub, min_sub);
          checked[array_ind] = true;
        }
      }
    }
  }

  for (int x = 0; x < vp_.kNumber.x(); x++)
  {
    for (int y = 0; y < vp_.kNumber.y(); y++)
    {
      for (int zi = 0; zi < 2; zi++)
      {
        int z = z_indices[zi];
        Eigen::Vector3i end_sub(x, y, z);
        int array_ind = grid_->GetArrayInd(end_sub);
        if (!checked[array_ind])
        {
          CheckViewPointLineOfSightHelper(robot_sub, end_sub, max_sub, min_sub);
          checked[array_ind] = true;
        }
      }
    }
  }

• 答：array_ind 是ind对应的点实际储存在vector的位置。为了实现滚动窗口的一个技巧。 判断一个点是否为候选，有三个条件，在当前视线+无碰撞+可连通。是否在视野，用了raycast的思想，连接机器人到最近碰撞点形成的线段上的点都是在视野中的。你贴的这段代码是采样raycast时需要用的终点，就是那些在窗口边界上的点

13. 怎么判断是否为交叉口

• 问：我理解的 tare 在进行局部规划时如果发现这个地方为交叉口（具有多个未探索的放向的时候）会选则其中一个进行继续探索，另外的方向则留待以后探索。想问一下您 tare 判断是否为交叉口时，是直接基于激光数据进行识别，还是先建立局部地图然后轨迹规划后发现有多个未探索方向后才将其作为交叉口？
• 答：tare 并不会去主动判断交叉口。每次规划的时候如果局部区域内还有未探索的地方，就会被留下来计成 unexplored 的 global subspace。而不是判断有几个未探索方向

14. 什么方法求解的 tsp 问题

• 问：关于tare，请问是通过什么方法求解的 tsp 问题
• 答：我们用的是 Google 的 Ortools：https://developers.google.com/optimization

15. 参数 H（local planning horizon）在哪里调整

• 答：在我们的开源代码里 local planning horizon 是由 viewpoint 的数量和相隔距离决定的。以 garage 的参数为例，H 里一共有 40x40x1 个 viewpoint，每个 viewpoint 相隔 1.2 米。所以整个 local planning horizon 的边长是 40x1.2=48 米。它的高由 kGridWorldCellHeight 决定，默认是 3 米

16. 判断覆盖的参数设置问题

• 问：判断 surface 的距离约束和角度约束的参数是在哪设置
  

• 答：参数 kSensorRange 对应的是 D。在开源的代码中我们省去了对角度的约束，因为在实验当中约束角度带来的效果并不明显

17. 空旷场景无法实现完整探索

• 问：当我的环境中有一些比较空旷的区域时，算法似乎就无法继续探索了。根据论文中的描述，tare 是根据判断是否覆盖 surface 来确定采样观测点，而全局子空间也是通过判断子空间内是否有未覆盖的 surface 来判断是否需要探索的。是否是因为环境较为空旷时难以生成采样观测点，而导致探索结束呢？还是说我可以通过调整某些参数来解决这种问题呢？
• 答：环境过于空旷确实有可能难以生成视点。可以试一下调整视点的分辨率（viewpoint_manager/resolution_x/y/z），数量（viewpoint_manager/number_x/y/z）和 kSensorRange。需要注意的是视点增多，kSensorRange 增大之后计算量也会增大，这个时候可以通过减少视点数量来平衡

18. 发布的 /waypoint 为 nan 的问题

• 问：在我运行的过程中，有时候会突然出现小车运行不了的问题，然后发现发布的/ waypoint 的 x，y 值为 nan，根据 PublishWaypoint()的代码，waypoint 应该只与 lookahead_point 和 robot_position_有关，我查看了 /state_estimate 的消息，是有位置消息发送的，所以 robot_position_ 应该不会出问题；也查看了 lookahead_point_cloud 也是一直有信息发出，但不知信息是否正常，您能帮忙看一下嘛？
• 答：我把 waypoint 的拓展置成 false 之后没再出现过这个问题了

19. terrain 中近距离障碍物无法识别的问题

• 问：我在实际测试时，发现一些时候视点（这里不是 waypoint，而是黄色的视点）会出现在不可通过的地方，应该是 terrain 的问题，于是我单独测试底层开发环境部分，发现确实会出现近距离（前方 1m 左右）的障碍物无法识别的问题（稍微远一点又能识别了）。我分析会不会是上一个视点确实是在可通行的区域内，但当小车来到这个视点时，前方的近距离障碍物扫描不到了，导致新的 terrain 有问题，所以视点才会出现在不可通过的地方。如果代码中特意设置了车周围一个固定的距离不作为扫描区域，那 terrain 的生成算法是不是也应该与障碍物之间有一个这样的距离约束呢？
• 答：这个问题应该是我弄错了，关于小车高度的参数没调好。我单独测试了 terrian 部分，对于静态低矮障碍物应该是会保留的，即使当前几帧扫不到，之前扫到了也会保留。一些近距离的障碍物确实会出现把近距离点云过滤的情况

20. or-tools 求解器报错的问题

• 问：在测试 tare 的时候出现了问题。就是 roslaunch vehicle_simulator system_garage.launch 之后。仿真环境正常启动，再然后在 tare 工作空间下 roslaunch tare_planner explore_garage.launch 之后，出现了 or-tools 求解器报错的问题，进程直接死了。导致探索没有办法进行
  

• 答：百度符号报错如何解决，发现 so 链接到了我之前的一个库，我的解决方法是在 zshrc 文件中去掉了 source 所有的 ros 工程，然后重新编译

21. tare 中的边界点如何确定

• 问：传统意义上的边界其实是基于建图基础上对处于已知区域和未知区域的区域称为边界，这样也是你论文里提到的传统基于边界的方法容易产生短视，所以你们在 tare 这篇论文里采用了递归和随机抽样的方法找到一个最小视点集，但有一个问题我没想明白，点云数据存储的是各种障碍物的位置，也就是只有 XYZ 坐标值，并没有体素的占用信息，所以我想问一下你们的边界点是怎么确定的？
• 答：我们的算法同时使用了点云代表的物体表面（occupied surface）和边界点（frontier）来鼓励探索

22. TARE 算法部署到小车抖动不能探索

• 问：我将TARE算法部署到实车，使用 ubuntu18.04 系统，16 线程雷达，开启 system_real_robot.launch 和 exploration.launch 之后开启小车，小车移动一段距离之后原地抖动，规划器显示探索完成，并且报错，不知道是什么状况，希望作者帮忙解答
• 答：但小车抖动的现象来看，有可能是底层 SLAM 有问题。可以试一下不跑 TARE 只跑 system_real_robot.launch 并用手柄控制移动看看有没有问题
• 问：后面我们试了，就用 system_real_robot.launch，小车探索的时候用的是 A-LOAM 算法，只携带了一个雷达，在地下车库中可以自己探索一段距离，但是总是返回原点，走廊中还没开始探索就返回原点，是不是环境特征的问题，或者需要改一些参数吗？
• 答：探索了一段距离就返回原点可能是参数问题。可以在走廊环境试一下 explore_matterport.launch，这里面的参数是针对小型室内环境

23. 关于 GetFrontier

• 问：src/tare_planner/src/rolling_occupancy_grid/rolling_occupancy_grid.cpp 文件中函数 void RollingOccupancyGrid::GetFrontier，If the unknown cell has neighboring free cells in xy but not z direction，为什么将这样的 cell 作为 frontier 呢？
• 答：你好，这主要是针对地面机器人，为了过滤掉天花板和地面上的 frontier 点
• 问：但是为什么是用 unkown 作为 frontier 而不是 occupied 呢？
• 答：你好，传统的 frontier 定义大部分都是用 unknown，含义是那个地方的信息未知，需要机器人去到并获取信息。比如在一个走廊入口，用 unknown 的话 frontier 会出现在走廊中间空的部分，机器人会被引导沿着走廊中间走。用 occupied 的话 frontier 会出现在走廊两侧的墙上，是机器人到不了的地方

24. 实车部署 TARE 局部探索区域过于小的问题

• 问：我在室外场景用 tare 跑真实实验遇到一些问题，我使用的是 VLP16，Ubuntu18.04，liosam，loam_interface 等文件都修改好了，运行的时候会原地转圈圈，或是向前探索一段距离又转圈圈后显示探索结束，但 rviz 显示的局部范围很少， Local Planning Horizon 里面只有 1-3 个 viewpoints，用的参数是 system_real_robot.launch 和 explore_matterport.launch，请问我该怎么修改呢
• 答：您好，explore_matterport.launch 适用于室内狭窄环境，室外的话可以试一下 explore_campus.launch。如果效果还是不好那可能是其他问题。可能需要您提供更多的信息我才能做出判断。一般来说，debug 的流程是先在 Rviz 里看 terrain_map 输出对不对，有没有因为 state estimation 不准而导致一些噪点被当成障碍物。其次在真机上验证的方法是在不跑 TARE 的情况下发送 way_point 给 local_planner 让机器人在环境中跟随。如果这些都没问题那就可能是 TARE 的问题

25. TARE 可以脱离 AEDE 环境使用吗？

• 答：理论上只要提供合适的位姿估计，点云输入和地形可行驶区域分析就能跑TARE。同时 TARE 输出的 waypoint 需要有一个能规避障碍物的 local planner 来跟随。但在实际操作上如果之前没有熟练的 ROS 开发经验或者对 TARE 不熟悉可能会有一定的复杂度。还是建议先在我们提供的环境下跑通 TARE，熟悉了整个流程之后再考虑脱离我们的环境或者移植到其他平台

26. TARE 中 frontiers 检测问题

• 问：请问一下，tare 中关于 frontiers 的更新，是在一个滚动网格上利用最新扫描点进行 raycast 得到的，那么当这个滚动网格回到了原来扫描过的区域，是否还会在这个区域检测出新的 frontiers？那么这些新检测出的 frontiers 是如何判断出 covered 和 uncovered? 我看这部分的代码的时候没有很明白，大佬能解释一下原理吗？
• 答：在滚动网格内我们用了一个 occupancy map 来进行 raycast 和检测 frontier。当网格滚动出一个区域后，这个区域的 occupancy map会被保存成点云。当网格滚动回来时，我们会用这些点云初始化网格的 occupany map，然后再在里面检测新的frontier。对于 frontier 来说，探索过的区域就不会再有 frontier 了，即是 cover 完了。对于物体表面（object surface）我们用了一个全局点云来保存 covered 过的 surface。点云里每个点的 intensity 用来表示这个点是否被 cover 过

27. 大尺度空旷或特征稀疏环境下 TARE 失效问题

• 问：我现在在有些大尺度空旷或特征稀疏环境运行 TARE 会出现规划了一段后停滞不前的情况，所以想问下如果外部定位失败，会导致地图不准确，从而导致 TARE 规划失败吗？如何确定是规划出问题还是定位有问题呢？
• 答：定位失败导致地图不准确大概率会导致TARE规划失败。可以通过查看 SLAM 输出的点云地图是否符合实际，是否有重影等来判断定位是否有问题

28. 局部规划的路径（Topic为：/path）变化频率比较快

• 问：启动 TARE，局部规划出来的路径（Topic为：/path）变化频率比较快，这也导致了车行走方式呈现走折线的状态。能不能通过更改参数减少路径规划的频率
• 答：/path 变化快的问题可以看看是否是 /way_point 频繁跳动导致的。如果是的话可以看看是否是 TARE 规划出来的路径在频繁变化。理论上来说 TARE 规划出来的路径在 1 赫兹左右，而且在机器人附近区域的路径应该不会频繁变动。如果有频繁变动可能是其他的问题而不是规划频率过快导致的

29. /terrain_map 在 Grid 网格以下的问题

• 问：实车部署后 Topic：/terrain_map 的点云在 rviz 上显示大部分位于 Grid 网格以下，但是在仿真里面却是皆在 Grid 以上，请问这是怎么回事，该如何解决？
• 答：真机部署的时候世界坐标系的原点和 state estimation 初始的原点是重合的，初始的时候 lidar 所在地位置是（0，0，0），所以地面就会在网格平面（z=0 的平面）以下。在虚拟环境中我们的时间坐标系原点设置在了地面上所以地平面会在网格平面上。这个差异应该不会对探索性能产生影响

30. /way_point 规划到障碍物外面

• 问：/way_point 规划到障碍物外面的情况导致车来回原地旋转，这种情况该怎么解决？
• 答：这个问题可以先检查一下 /terrain_map 和 state estimation 输出对不对。理论上来说 TARE 会用 /terrain_map 确定可行驶区域，不会把 /way_point 放在不能到的地方。但如果 /terrain_map 和 state estimation 输出有误，倒是障碍物没有被检测到则会出现这个问题。另外在小环境中可以尝试减小 kExtendWayPointDistance，这样 TARE 就不会把目标点投影过远至不可到达的地方

31. 小车 terrain_map 在地面没有障碍物的情况下仍然出现红点

• 问：小车的 terrain-map 在地面没有障碍物的情况下，仍然出现红点，如图所示，这因为什么呢，我已经调节了 vehicleHeight（平地面/轮胎底部到传感器中心的距离）为真实高度
  

• 答：地面没有障碍物仍然出现红点。你可以查看一下机器人运动过程中激光雷达的原始点云中是不是会出现一些噪点，从上面图看，机器人周围不可通过的点挺多的，这些点肯定都是真实扫描到的（假设你的定位是没有问题的）

• 问：当行人从小车周围经过后，留下的小红点多久会消失呢，如果要让这些点云能够快点消失，应该调节那些参数啊？

• 答：terrain_analysis 对于动态障碍物留下的点的消除效果并不出众，只能消除部分的点，并且是否消除这些点是与时间无关的，只和激光的视角有关，如果机器人静止的时候不能消除，那就需要让机器人运动一下，再查看是否消除

32. 能否将 far-planner 的动态障碍物提取和消除移植到 tare-planner

• 答：tare-planner 本身是有部分动态障碍物消除能力的，但是可能没有 far-planner 效果好，如果你想移植的话，是完全可行，两者的信息来源都是一样的

33. 关于 tare 的可通行区域的识别问题

• 问：tare 的视频中，其中室内的一个视频，存在向下的楼梯，最终还是完整探索完毕了 ，tare 能够识别出向下的楼梯属于不可通行的区域么？我看源代码只有关于垂直面提取 用于区分是否可通行，没有关于地面是否可以通行，比如环境中地面存在坑洞或者向下的楼梯能处理么 ？
• 答：tare 本身不能识别向下的楼梯是否能通行。在我们的实验中，terrain_analysis 有个参数 noDataObstacle，设置为 true，这样 terrain_analysis 在做可行驶区域分析的时候会在没有激光数据的地方加上与机器人同高的虚拟障碍点，比如向下的楼梯当成障碍物，tare 会接受障碍物信息阻挡 viewpoint 生成

34. 关于 Terrain_analysis 平地出现障碍物及 TARE Planning 过早停止的问题

• 问：我们正在适配你们的 Autonomous Exploration Development Environment 和 TARE 算法到我们的实地 Jackal 机器人上，我们配备的是 ouster OS1-32 3D Lidar。现在遇到如下几个问题

• 1、我们的机器人雷达高度据地面为 0.335m，我们也实际修改了 terrain_analysis 中 vehicleHeight = 0.335, minRelZ = -0.335, and maxRelZ = 0.2. 其他参数与预设值一致。于是我们得到了如下 terrain_map
  

• 发现周围的墙壁变得非常矮，而重新将 vehicleHeight 改为较大数值如 10 时，周围墙壁又能按正常高度显示，如下图，请问这是什么原因呢？
  

• 2、我们得到的 terrain_map 在现实中地面上有许多的空白地区，尤其是机器人本身脚下及远处尤为严重，并不像 simulation 里所展示的 terrain_map 绿色区域填充了整个地面，并且实际前方并没有障碍物的区域也显示为了红色或黄色，请问有何潜在原因呢？结果如下图
  

• sensor_scan 同时刻返回点云图如下：
  

• 3、在现有情况下，TARE 仍旧可发出 waypoint 进行导航，但是在 waypoint 会逐渐移动到障碍物中，然后就显示探索停止，返回原点。目前使用的 planning 参数基于 matterport 设置。请问有哪有参数可以做调整呢？

• 答

  • 1、地形分析只会对必要的信息进行处理，vehicleHeight 影响的是我们考虑的地形的高度范围。当 vehicleHeight 很小的时候，墙壁比较高的部分实际上完全不影响车辆的行驶，但是 vehicleHeight 很大时，就需要考虑了
  • 2、你可以把 launch 文件里的 clearDyObs 设置成 false，不要检测动态障碍物，我们的动态障碍物检测考虑的分辨率比较粗糙，所以可能会把部分不是动态障碍物的部分当成障碍物消除掉，所以导致地面上出现一些空白。另外机器人下方的区域是因为传感器看不到任何信息，所以是空白，如果你操控机器人向前走一段距离，确保周围区域都观察到了，就可以把空白区域消除。另外，地面上出现的红色和黄色的点，你可以在 rviz 里选中红色的点，看看 intensity 是多少，然后看一下这些 intensity 与实际的是不是相符。intensity 的计算方式是该点的高度相对于周围 0.1m 范围内最低点的高度，你可以判断一下是不是正确。还有一种可能是，某些区域有反射，导致地面以下出现了噪点，你可以看一下地面下面是不是出现了不该出现的点
  • 3、视频里展示的信息太少了，我们难以做出判断，有可能是因为 terrain 影响的，也有可能是 tare planner 里的车辆高度设置或者相关的参数设置的不对。你可以先把 terrain_analysis 以及 local planner 都设置好，确保车辆在这个环境里能够正常的到达 waypoint，然后再设置 tare planner

35. local_coverage_planner 中关于（viewpoint）选取的问题

• 问：这里是在这个范围均匀生成随机数，我看论文说的是按照这些候选视点对的 covered_count 的大小来形成的一个概率，covered 越大被选中的概率越大，不应是均匀随机的嘞。上面是我的一些理解，有点疑惑，所以想来寻求下答案

  sample_range = std::min(parameters_.kGreedyViewPointSampleRange, sample_range);
  std::random_device rd;//获取随机数种子
  std::mt19937 gen(rd());
  std::uniform_int_distribution<int> gen_next_queue_idx(0, sample_range - 1);//生成0到sample_range之间的均匀离散分布函数
  int queue_idx = gen_next_queue_idx(gen);//在(0,sample_range - 1)生成随机数，然后赋予queue_idx
  int cur_ind = queue_copy[queue_idx].second;
  

• 答：排序之后，前几个就是覆盖值大的视点 ，然后在这前几个视点使用均匀随机， covered 越大被选中的概率越大，只有前几参与选择，当然是 covered 越大被选中的概率越大，你的意思是前几个概率也不应该是平均的，只能说代码实现和论文说法有出入，效果大差不差

36. 下楼梯检测问题

• 问：我们在室内运行 tare 算法时总会遇到楼梯检测不到直直的下楼梯去的问题，这种情况该怎么办呢，是激光雷达高度太高吗，还有就是在通过拐角门这种场景时机器人时常擦着门框过去偶尔甚至直接撞门上这种情况又该修改哪些参数呢
• 答：
  • LEGO-LOAM 会滤掉离得很近的障碍物点云，调大车宽不让它靠得那么近会有一定改善
  • 你好，关于下楼梯的问题，目前我们开源的地形分析并不能很好的应对，激光雷达高度太高，水平安装都会导致地面向下的地形看不到或者看不清，如果必须要应对这种情况，建议再添加一个传感器，比如摄像头，安装在能够看到较近地面的位置和角度，这样可以把负障碍物(楼梯，悬崖，深坑等)检测出来。对于擦着门框过去的问题，如果环境狭窄，规划会缩减碰撞检测的空间，提高通过的可能性，这时就有可能碰撞。这一点如果不是需要通过非常狭窄的环境，回复中的调大车身是一个比较好的选择。

37. use_momentum_ 是什么意思

• 问：请问一下这个函数中的 use_momentum_ 是什么意思呢？

  void SensorCoveragePlanner3D::CountDirectionChange()
  {
    Eigen::Vector3d current_moving_direction_ =
        Eigen::Vector3d(pd_.robot_position_.x, pd_.robot_position_.y, pd_.robot_position_.z) -
        Eigen::Vector3d(pd_.last_robot_position_.x, pd_.last_robot_position_.y, pd_.last_robot_position_.z);
   
    if (current_moving_direction_.norm() > 0.5)
    {
      if (pd_.moving_direction_.dot(current_moving_direction_) < 0)
      {
        direction_change_count_++;
        direction_no_change_count_ = 0;
        if (direction_change_count_ > pp_.kDirectionChangeCounterThr)
        {
          if (!use_momentum_)
          {
            momentum_activation_count_++;
          }
          use_momentum_ = true;
        }
      }
      else
      {
        direction_no_change_count_++;
        if (direction_no_change_count_ > pp_.kDirectionNoChangeCounterThr)
        {
          direction_change_count_ = 0;
          use_momentum_ = false;
        }
      }
      pd_.moving_direction_ = current_moving_direction_;
    }
    pd_.last_robot_position_ = pd_.robot_position_;
   
    std_msgs::Int32 momentum_activation_count_msg;
    momentum_activation_count_msg.data = momentum_activation_count_;
    momentum_activation_count_pub_.publish(momentum_activation_count_msg);
  }
  

• 答：这是控制方向改变的变量，在实际使用时，可能会遇到一些极端情况，有可能两条路径都是最短路径，机器人就会在两条路径来回切换，我们使用这个变量是希望机器人在想改变方向的时候有一些惯性，不会立马改变而是再继续保持一段时间，确认真的是需要改变方向时再去调整。这样可以有效避免最短路径不止一条的问题

38. 关于无法到达的区域如何重规划的问题

• 问：tare planner 所探索的区域，无人车无法到达，可以通过什么方式让探索算法重新规划路线，现在并未重规划
• 答：如果是不想让车到达的地方，可以发送一个 exploration boundary。如果是车无法行驶的 terrain，可以调成 terrain_analysis 和 tare_planner 里的 terrain threshold 参数让 viewpoint 不被 sample 在不能去的地方

39. 路径点跟当前位置之间存在障碍时小车进入徘徊状态的问题

• 问：现在 tare planner 可以规划路径，local planner 也可以避障，但如果给的路径点跟当前位置之间存在障碍，有时候小车会进入徘徊状态，您知道怎么解决吗？或者是什么原因导致的？
• 答：可以在 tare_planne 里把障碍物高度阈值设得跟 local_planner 里的一致

40. 全局路径的立方体子空间是怎么生成的

• 问：全局路径的立方体子空间最初怎么生成的，后面跟局部规划一样里面包含的信息也是会动态进行调整的，然后也是需要移动这个立方体子空间的吧？是如何进行的
• 答：tare planner 把整个空间都离散化成立方体子空间了。只是在探索的时候有些子空间没有被探索完所以被标记成 “exploring”，并没有一个生成和消失的过程

41. 在 Robocup 狭窄环境中无法探索的问题

• 问：现在想将 Tare planner 这套算法应用于 Robocup 比赛场地，这个场地较为狭窄且会出现一些缓斜坡。现在应用 Tare planner 这一套时遇到的问题就是机器人只能探索一个很小的范围就 Exploration completed, returning home 了。调试过程中打印 selected_viewpoint_indices_itr 值，发现只有 1432 一个点。调整过 kMinAddFrontierPointNum 和 kMinAddPointNumSmall 参数，效果不大。求大佬指点是什么问题，应该如何调整参数？
• 答：这种环境下可以试一下 explore_matterport.launch。参数上可以试着减小 kSurfaceCloudDwzLeafSize，viewpoint_manager/resolution_x(y)，kMinAddFrontierPointNum 和 kMinAddPointNumSmall。在 Rviz 里打开 viewpoint 的 visualization，把颜色设为 intensity，如果能看到比较多的 viewpoint 点有较大的 “intensity”（颜色偏紫色），说明有足够多的 viewpoint 能看到 surface point 或者 frontier point，这种情况下探索会更仔细和完整

42. 部署实物后启动后就结束与探索不完全问题

• 问：测试环境是楼里的走廊，配置文件用的是修改过的matterport.yam;，启动tare后经常直接就探索结束，或者探索不完全，拐角处的走廊探索不到
• 答：暂无

43. 上层规划器 TARE 和 autonomous 环境在避障时采用哪一个？

• 问：在实车部署实验的时候，我要同时启动 autonomous 环境的 system_real_world.launch 和 TARE 的explore_xxx.launch，然后小车就会开始自主探索。在探索过程中，有出现小车撞击到环境中的花坛的情况。我想修改的话，是去 autonomous 环境中的 local planner 和 terrain analysis 中修改，还是在 TARE 的代码里做修改？避障起作用的究竟是 autonomous 环境还是 TARE planner 呢，谢谢大家的回答和帮助！
• 答：直接对小车作用的是 autonomous 环境，高级规划算法 TARE 输出路径点引导 autonomous 环境的局部规划和跟随

44. 关于 autonomous 环境中 terrain_map 的问题

45. FAR Planner和 TARE Planner 算法关系

• 问：FAR Planner 和 TARE Planner 这两个算法都可以完成自主探索，那他们之间是什么关系呢？还是说需要配合起来使用的呢？如果要在未知地图的环境里进行路径规划导航，这两个算法更推荐使用哪个呢？请大佬解答
• 答
  • FAR Planner 和 TARE Planner 都是用于自主探索的算法，但它们的功能和应用场景有所不同。FAR Planner 是一种快速、可尝试的路线规划器，适用于已知和未知环境中的导航。而 TARE Planner 则是一种上层规划器，主要用于探索环境
  • 在实际应用中，这两种算法并不是孤立使用的，而是相辅相成。例如，CMU 团队开发的无人车仿真环境 autonomous_exploration_development_environment 可以结合 TARE Planner 和 FAR Planner 进行自主导航算法的测试和验证
  • 如果在未知地图的环境中进行路径规划导航，推荐使用 TARE Planner 进行环境探索，利用 FAR Planner 进行路径规划。首先，TARE Planner 可以帮助机器人感知和理解周围环境，然后基于这些信息，FAR Planner 可以为机器人规划出一条安全、高效的路径

46. TARE 底层的 local planner 参数不太理解

• 问

  • 1.dirtoVehicle参数这个如何理解？
  • 2.pathscale，minpathscale，pathscalestep，defPathSacle 的作用是什么。最后没有找到路径，即 selectedGroupID＜0 时的处理(pathscale 做 step 运算)什么含义？
  • 3.minObsAngCW 和 minObsAngCCW 的作用是什么？
  • 4.在计算 rotDir，joyDir，angOffset 的坐标系，xy 轴方向是什么样的，能否画图解释下
  • 5.代码是将 360° 划分为 36 个扇区么，rotdir 为什么也要从 0-36 进行 for 循环呢

• 答

  • 1。dirToVehicle 这个参数是为一些类似汽车的机器人平台设置的，这些平台不能原地转向而只能一边向前或者向后运动一边转向。当这个参数设置成 false 的时候，就是应用在大多数可以差速驱动的机器人平台上，只考虑目标方向附近的轨迹，机器人可以任意无碰撞的转向需要的方向。而当这个参数设置成 true 的时候，就是应用在上述类似汽车的平台上，这时候只考虑机器人朝向 dirThr 之内的轨迹，因为机器人只能向前或者向后运动的时候转向，dirThr 之外的轨迹机器人是没有能力实现的。一般 dirToVehicle 是 true 的时候，dirThr 会设置的比较小，比如只考虑机器人朝前或者朝后 10° 的范围内的轨迹。如果用的是差速驱动的机器人，那就可以保持现在的 v 默认设置
  • 2.pathscale 是用来控制碰撞检测范围的，pathscale 越小，碰撞检测范围就越小，pathscale 是当前值，minpathscale 是最小需要考虑的碰撞检测尺度，pathscalestep 就是在当前的碰撞检测范围内找不到无碰撞路径的时候缩短 pathscale 的步长，defPathScale 是最大的需要考虑的碰撞检测的范围尺度，这个值是初始化时就已经设置好的，是定值。如果最后没找到路径，就会一步一步的缩短 pathscale，也就是缩短碰撞检测的范围，直到找到路径或者缩短到 minpathscale。另外提醒一下，做碰撞检测的时候，如果某条路上任意一点有碰撞，那这一整条路都会认为是有碰撞的
  • 3.minObsAngCW 和 minObsAngCCW 这两个参数是当车不是圆形的时候做碰撞检测使用的中间参数，相当于是从机器人中心到机器人最长的边缘的长度，由于是中间变量，launch文件不能调节，可以保持默认的状态
  • 4.计算这三个角度的时候的坐标系都是机器人的 vehicle 坐标系，也就是机器人的旋转中心坐标系，x 轴是向机器人前方，y 轴向机器人的左方
  • 5.确实是将 360° 划分成了 36 扇区，每 10° 一个扇区，我们不是考虑的连续的过程，而是按照扇区来筛选轨迹的，所以 rotdir 从 0-36 循环

47. 规划中是否考虑了崎岖地形

• 问：请问全局路径或者局部路径规划中，有没有考虑崎岖地形的地形评估，比如地形是一个有坡度的，或在有坑洼小凸起的区域，需要局部地形评估
• 答
  • 就目前我看源码，是需要自己根据实际场景设置的。terrain 分析文件中会把所有点云分片然后计算出这个点云距离地面最低点的高度，其中有 considerDrop 参数是把负方向取绝对值，不过可能需要前面 useSorting 并调整 quantileZ 才能把坑距离值放进去，常规平缓的坡的话这个地面分割应该是 ok 的。局部路径规划中有 obstacleHeightThre，看底盘越障能力和 z 方向定位精度修改吧
  • 在这个系统里的地形评估就是指该位置的点相对于地面的高度，比如是坡的话，如果坡不是很陡，我们仍然会认为这个坡是地面，因为坡上的点相对于地面的高度都很小。但是如果是坑洼小凸起，那就取决于这个凸起有多高，如果超过了我们设置的障碍物高度阈值，仍然会被当作是障碍物。无论是全局还是局部规划，都会考虑到地形因素

48. 关于室内扫描的小门或者狭窄区域扫不到的问题

• 答：一般出现这种探索不完全的原因可能有两个。一个是房间入口太窄了，这个可以通过调小 kViewPointCollisionMargin 来减少碰撞检测的距离。需要注意的是这个参数需要和 viewpoint_manager/resolution_[x,y,z] 相互配合，确保 kViewPointCollisionMargin 的两倍要大于 viewpoint 的 resolution ，要不然计算出来的路径可能会穿墙。第二个原因可能是房间本身比较小，里面的 frontier 数量太少，这个可以通过调 小kFrontierClusterMinSize 来降低 frontier clustering 的阈值

49. 如何将 GBP、MBP、NBVP 等算法用于 AEDE 仿真环境下

• 问：请问如何将 GBP、MBP、NBVP 等自主探索算法用于 autonomous exploration development enviroment 这个仿真环境下，本人想测试比较这些算法的性能，但是没有实体可以测试，所以想利用该仿真环境，看博主您 DSVP 这篇论文比较过这些算法的性能，所以想请教一下该如何将这些算法配置到这个仿真环境下，若可以解答，则万分感谢！！
• 答：他们原来的算法是用在无人机上的，所以我们做了一些改动让这些算法可以用在无人车上。同时也也把算法的输出改成了一个一个的 waypoint，这样就可以应用在我们的平台上。由于他们都是开源的，我们不能把我们自己的改动开源给大家。如果你也想和他们对比，这就需要你自己去更加深入的了解他们的算法和代码以及我们这个平台，然后自己去做改动
• 问：能否提供一个思路，我说下我的思路，比如GBP这个算法，在这个算法中 subscribe autonomous exploration development enviroment 发布的雷达信息，里程计信息，然后在 GBP 进行路径规划，然后发布 waypoint，autonomous exploration development enviroment 收到 waypoint 就进行自主探索，这样可行吗？
• 答：这样是可以的。不过还得考虑一下采样的时候 z 的值，因为原来是用在无人机上的，所以 viewpoint 可以飘在空中，但是用在无人车上就不行了

50. 关于机器人的激光雷达问题

• 问：请问自主导航系统有对激光雷达的 3d 点云数据进行建模（或者其他处理）吗？还是直接将点云看作障碍物就可以实现导航与避障？
• 答：只是把数据 register 到了全局坐标系下，然后把局部的点云叠加起来先做地形分析再用地形图做避障和导航

51. 请问怎样使用这套仿真系统验证自己的规划算法呢？

• 问：我想使用仿真系统的中的不同场景去验证自己的规划算法，规划算法需要点云生成的障碍物栅格地图，这个地图之前一直是订阅lidar的point cloud数据生成的，请问，现在想要实现，读取雷达数据，生成2D 障碍物地图，发送给规划模块，规划模块输出控制量给无人车进而控制无人车运动，这一套流程，具体该怎么做呢？
• 答：其实问题最后思路说的挺清楚了，雷达数据生成 2d 地图，用 2d 地图做避障和轨迹规划。你可以参考一下我们的 local planner 用地形分析图的做法，改一下里面的程序，让它能够使用你们的 2d 地图取代原来的地形分析图，这个难度应该会小很多。具体的代码实现还得你去理解一下我们的代码才可以做到

52. local planner 左右摇摆比较多

• 问：挂载了自己的全局路径规划器，底层的 local planner 会频繁变换行使方向，小车可以完成自主导航但是左右回摆比较多，请问有什么改进点或者参数调整思路？目前 local planner 除了适配底盘的参数，其余基本都是默认参数
• 答
  • 你好，首先得确认一下全局路径规划输出的目标点是否有跳动，如果有跳动也会导致local planner 变换方向。如果没有跳动的话，就需要在 rviz 里同时看一下 terrain_map 和 free_paths 的情况，看看是不是把地面识别成了障碍物，如果定位有 z 轴方向的跳动就很容易引发这种问题。你改的这个参数应对上面说的第二种情况确实有一定的效果，但是还是得找到根本的原因，再看有没有可能解决
  • 你好，这个可能和里程计有关系，如果里程计震荡的话也可能出现这个问题，还有就是pathFollower 的参数设置，转弯的 PID 参数
  • 尝试了增加 obstacleHeightThre 以降低地面点云中障碍物的误识别，但是会导致低矮的障碍物识别不出来，目前保持 0.15
  • 修改下 local_planner launch 中的参数，让黄色的局部路径不要测这么远试试

53. local_planner.launch 里的参数问题

• 问

  • 1、useInclToStop 是使用线性速度停止的意思吗？对应的两个关联参数是 slowDwnDisThre 和 inclRateThre，指的是停止前的减速距离和线性函数斜率吗？
  • 2、pubSkipNum的作用是什么呢，是减少消息发布数吗？是否跟 pointPerPathThre 参数有关？
  • 3、checkRotObstacle 和 checkObstacle 的区别是什么呢？
  • 4、adjacentRange 是指的避障距离吗？
  • 5、在autonomous_exploration_development_environment包中有两个与地形分析有关的cpp，terrainAnalysis.cpp和terrainAnalysisExt.cpp，这两个文件的区别是什么呢？关于（Fast Likelihood-based Collision Avoidance with Extension to Human-guided Navigation）文中体素概率分布的公式是不是在其中体现了？
  • 6、我想问一下关于Explored Volume这一项实验数据是通过什么方法画出的曲线呢？包括跟其他方法的比对？

• 答

  • 1.这几个参数的意义确实是和你说的一致
  • 2.pubSkipNum是用来控制cmd_vel发布的频率的，这个和pointPerPathThre并没有关系。使用这个参数只是想在不影响整个循环频率的前提下控制cmd_vel的频率，你可以根据自己目标节点需要的频率去调节
  • 3.checkObstacle是控制是否做碰撞检测的，这个值为false就是完全不做碰撞检测，设为true就是smart joystick模式。checkRotObstacle是控制是否检测在机器人半径范围内的点云导致的碰撞，比如机器人本体携带的一些物体
  • 4.adjacentRange是机器人做碰撞检测的最大范围(并不是最终的范围)，在这个范围内的点都会首先被加入到碰撞检测的点云内，但是这并不是最终碰撞检测的范围，最终检测的范围是和速度相关的，小于这个值的
  • 5.terrainAnalysis.cpp是做小范围的地形分析，另一个是做大范围的地形分析，前者用于避障，比较精细，后者用于上层规划，会粗糙一些。体素概率分布在path文件夹中的.m文件里有所体现，在cpp文件直接用了离线生成的结果
  • 6.Explored Volume是统计机器人建出的点云地图中点的数量然后乘以每个点代表的体积得到的，每个点代表的体积实际上是点云的分辨率，这个可以使用pcl降采样得到不同密度的点云

54. Falco 是在具有先验地图的情况下才使用全局最大化到达概率吗

• 问：Falco 是在具有先验地图的情况下才使用全局最大化到达概率吗（文中公式6）？在不具备先验地图的情况下是不是也是以探测好的局部范围作为“全局地图”采用这个公式？都是在.m文件中离线生成的吗？
• 答：你好，falco 是没有先验地图的，如果有先验地图的话，论文中对比的其他几种方式也几乎可以实现一样的效果。在实际的应用中，都是用的局部地图，范围并不大。这些轨迹都是离线生成的，这样能保证在线计算的时候非常的快

55. 室外非平坦地面环境仿真平台

• 问：求助：有没有开源的小车在室外崎岖环境运动的仿真平台？
• 答：你好，目前我们的仿真平台暂时不支持车在崎岖路面上的行驶的动态模拟（车轮和路面的接触之类的）。但如果只关注路面的可行驶区域分析，用户可以自己在我们的平台上替换成自己的环境来实现想要的效果

56. AEDE 开发环境应用到现实小车的几个问题？

• 问
  • 1、车的速度如何控制？
  • 2、导航时，在去目标点的过程中，会出现原地旋转的情况，该怎么改参数？
  • 3、建图时，重影严重，需等待一会才能正常，这如何修改
• 答：
  • 你好，在部署到实际机器人时，车速应该还是与 cmd_vel 输出的值成正比的，所以首先得确保你的机器人速度是由这个指令控制的，并且是正比关系。关于车导航过程中原地打转的现象有可能和你第三个重影是相同的问题造成的，这个你可以在社区看一下以前的问题贴，有几个人遇到过类似的问题，应该是因为定位算法输出的机器人位姿有跳动造成局部的地面飘到了空中被当成了障碍物，可以参考以前的帖子里建议的方法去修改参数
  • 关于定位有误差这个问题，因为我们只提供和 slam 算法的接口，我们只能建议你尽可能想办法消除定位的误差或者至少让误差不影响实际的行走

57. 机器人怎么确定他到达了 viewpoint 位置？

• 问：请问一下autonomous explore中，机器人是如何知道他已经到达了目标位置了，除了pathfollow中，还有其他的地方会发布cmd_vel的消息吗？
• 答：你好，只有path follower会发布cmd_vel消息，在机器人前往目标点的过程中，local planner会不断的对比机器人当前位置和目标点位置的差距，根据这个差值去控制机器人的直线速度和转向速度，最终不断逼近目标点

58. legoloam 和 tare 的 tf 怎么统一呢？

• 问：legoloam和tare的tf怎么统一呢？movebase 框架是map odom baselink lidar 这个应该怎么统一？legoloam 在没有 IMU 情况下漂移严重的，以及 tare 接收地图导航 重影严重，在仿真环境是姿态估计是 200HZ，而loam 算法基本都是 10HZ，会是这个原因导致的吗？
• 答：你好，tare当中没有用到tf。tare只接受一个当前机器人的位姿估计（state estimation）和一个在全局坐标系下的当前帧点云(registered scan)。在movebase框架下，可以把map当作全局坐标系。在odom和lidar差别不大的情况下可以直接用odom。否则建议使用lidar的实时姿态作为state estimation输入。在有imu的情况下，真实环境中loam应该也有200hz的输出。如果位姿估计漂移严重确实会在tare中产生重影，这个目前没有很好的解决办法

59. terrain map轨迹残留问题

• 问：您好，最近在测试中发现当人走过时会在 terrain map 中留下“毛毛虫”轨迹，被 local planner 识别为障碍物进而无法通过。正常情况下随着 Lidar 刷新点云，毛毛虫轨迹可以在一段时间后被清除掉，但是在某些情况下这些红色的毛毛虫轨迹长时间无法被清除，机器人被障碍物卡死，该问题属于属于偶发现象，请教各位老师有没有经验处理这个情况？
• 答：同学你好，根据你的描述，猜测这些红色的轨迹可能是由环境中的动态障碍物所引起。在local planner中，可以通过调节参数 “pointPerPathThre”来改善对于小型动态障碍物的处理，对于大型动态障碍物的处理，例如，行人，车辆等，可参考FAR Planner中的处理方法来提取点云中的动态障碍物 https://github.com/MichaelFYang/far_planner/blob/melodic-noetic/src/far_planner/include/far_planner/scan_handler.h#L36

我的毛毛虫现象解决了，原来很简单，在Rviz里的 registered_scan 对应的参数decay time设置成了10，改成0就行了。不明白参数设置成10是有什么特殊目的，这是话题延时

60. 底层如何处理一个带有位姿朝向的 waypoint 呢

• 答：在我们的代码里当使用摇杆控制的时候，如果把摇杆往两侧推，车就会转向。一样的，如果希望底层能够处理带方向的waypoint，比较简单的方式是写一个类似摇杆控制器的node专门来把处理waypoint的转向，先让机器人使用现有代码到达目标点，然后再发布转向指令让机器人转到目标方向

61. 在 RVIZ 中给 waypoint，小车往反方向运动

• 答：你好，请问你的传感器也就是 lidar 的前方和机器人地盘前方是一致的吗，如果安装反了就会导致机器人向相反方向运动

62. 对于 path_generator 来说 dis 参数直接影响到生成路径的尺度

• 问：
  • 对于path_generator来说dis参数直接影响到生成路径的尺度（或者说分辨率），是不是分辨率越高导航效果越好呢？我们在实验中发现dis设定越大导航的精度就越差，而按照原始dis=1的查找表进行导航效果就很好。下面三图的参数分别为：1-27-0.65（好），2-27-0.65（一般），5-27-0.65（差，近乎不转弯，且到达目标点附近时直接越过目标点继续前行）
    

• 答：path generator里的参数dis是和环境相关的，这个参数影响的是做碰撞检测的范围，我们设置的默认参数适用于大多数的环境，如果你的环境非常开阔或者车速非常的高，可以设置一些大的参数值，不然效果确实会有影响

• 问

  • 我们用的底盘是husky A200四轮式底盘（是可以原地转向的），Path使用的文件也是默认文件，文件默认是给了7条主路径，然后每条主路径又根据朝向不同各有分支，但是会出现一种情况，就是在选择路径的时候，仅能选择第一段路径的中间三个朝向（左一 中 右一），在选择左二（三）、右二（三）等朝向的时候，机器人会出现“卡死”的情况，也就是不能转弯也不能前进，cmd_vel话题停止输出，这是怎么一回事呢？已经确认没有开启dirToVehicle
  • 在实际使用的过程中，发现仅有第一段轨迹分支对机器人行走有直接有影响，感觉后续的两段并没有对导航起到实际作用？这样的话是否只生成第一段路径即可呢？

• 答

  • 你好，如果你发现车完全停住不动，说明local planner已经找不到路了，这时候cmd_vel应该会始终发布0，cmd_vel不会停止发消息，所以你需要再确认一下这个消息的内容以及机器人当时的我环境。
  • local planner实际上是考虑所有的路径的，但是在发布时只发布第一段。local planner是用path group的，只用第一段就可以决定用的是哪一组，所以只发布第一段就可以了

63. local planner 代码问题请教

• 问
  • 各位大神，请问这行代码（446-449）在计算每条路径终点的方向角时，为什么乘以 2 呢？

  if (pathID >= 0 && pathID < pathNum && groupID >= 0 && groupID < groupNum) {
    pathList[pathID] = groupID;
    endDirPathList[pathID] = 2.0 * atan2(endY, endX) * 180 / PI;
  }
  

• 答
  • 你好，这里的2是一个经验数字，类似于一种驾驶的直觉，如果一条路径是向左或者向右弯曲的，后续的路径还会继续向相同的方向弯曲，所以我们把角度设置的比实际的大。当使用waypoint来引导local planner的时候，就会像把车开出去更远那样考虑车辆的行驶方向，而不只是考虑当前的状态。在实际测试中，我们已经把这个参数调到最好的效果了，不过你仍然可以自己去调节，尝试不同的效果
  • 个人理解可以把它看做为一个角度值的权重，目的就是让在计算代价值的时候轨迹终点方向角离目标点的越小的轨迹越容易被选择。但是也不能设置为太大，太大容易造成基本选择车的方向前进，特别是到终点的时候会不断的调整。也不能设置太小，太小容易在直道上轨迹很容易弯曲

64. AEDE 如何导入自己学校的地图

• 问
  • 我有一个问题想请教一下，请问下如果我想在学校构建一个自己的地图导入这个仿真平台，用自己学校的环境进行仿真。我需要如何实现？我能否用纯激光雷达点云地图（加了绝对位置约束）还是必须用图像生成3D模型？
• 答
  • 好，如果你们不需要用地图渲染rgb图像的话，我们通常的做法是把纯激光雷达点云地图导入CloudCompare进行downsample和计算每个点的normal。然后将其保存成文件（比如ply格式），再用meshlab打开并重建成mesh。最后用meshlab把它存成dae格式的文件就能被加载进gazebo了
  • 如果你们对图像渲染有要求的话则可以用图像生成3D模型。我们的campus环境就是那么做的。我们先用google earth对其从不同的角度采集图片。最后直接用这些图片进行3D reconstruction
• 问
  • 请问用谷歌地球采集图片之后，3D重建再保存为dae这个过程使用的什么算法或者软件呢？
• 答
  • 我们用的是 meshroom：https://github.com/alicevision/meshroom

65. Exploration completed,return home

• 问
  • 机器附近0.5m左右有障碍物的情况下启动探索建图，会出现Exploration completed,return home提示， 机器一直在原地不动
• 答
  • 可以试下用针对小环境的launch file，比如explore_matterport.launch。如果还不行的话可以调小机器人的碰撞半径（https://github.com/caochao39/tare_planner/blob/d9c539d289721ba429c10e692c44cf20e0452c31/src/tare_planner/config/matterport.yaml#L81%EF%BC%89%EF%BC%8C%E7%9B%B8%E5%BA%94%E7%9A%84%E7%82%B9%E4%BA%91%E5%88%86%E8%BE%A8%E7%8E%87%E4%B9%9F%E9%9C%80%E8%A6%81%E7%9B%B8%E5%BA%94%E6%94%B9%E5%8F%98%EF%BC%88https://github.com/caochao39/tare_planner/blob/d9c539d289721ba429c10e692c44cf20e0452c31/src/tare_planner/config/matterport.yaml#L48）

66. 关于 path Follower 中的控制律

• 问
  • 请问一下 pathFollower.cpp 中的控制律设计有相关的参考文献吗？或者从哪里找代码中对应控制律的具体表达式？
• 答
  • 你好，我们并没有用很复杂的控制律，我们用的是一个简单的反馈控制，距离目标点越远或者角度相差越大，输出的速度指令也会越大，不过会设置一个上限值。在控制过程中我们会考虑直线行走和转向之间的优先级，比如在需要大的转向的情况下，直线速度会先降为0再转向，但是很小的转向可能会随着直线行走的时候就同时执行。另外还有一些特殊的减速处理来应对比较大的roll和pitch速度。理论上并不复杂，你可以对照代码去得到我们的控制方式，用在大多数的差速驱动的机器人上应该都是合理的

67. vehicleX 和 vehicleXRec 这两个变量是干嘛的

• 答
  • 你好，vehicleX是当前的机器人位置，vehicleRec是接收到第一帧数据时的位置，因为后面需要使用noDataInited来辅助判断一些负障碍物，也就是没有数据的地方，需要确保是因为传感器视野限制导致没有看到数据而不是当前没有接受到数据导致的没有数据，所以需要设置vehicleRec。在每次清理地形图之后也需要重置这个参数

68. Local Planner 是否可以屏蔽后向轨迹

• 问：请问 Local Planner 是否可以屏蔽后向轨迹，目的是约束机器不后退。是否可以通过调整参数实现呢，感谢！
• 答：只要把 launch 文件里的 twoWayDrive 设置成 false，这样机器人就只有向前的轨迹，只能向前走

69. 关于 local planner 小车悬空

• 答：如果开始加载进来的时候车就飘在空中，那有可能是激光雷达没有看到真实地面的信息，导致高度判断有误差。我们提供的vehicle_simulator会结合terrain_map的信息中的地面判断车当前的高度，所以一定程度会收到周围地图的影响。针对这种情况有两种解决方式，第一是你可以用gazebo里的ground_truth插件(具体名称可能是odometry，你可以搜一下)来提供车的位置，不过这样做可能需要对vehicle_simulator做较多改动，第二种方式就是只用ground_truth提供的高度信息替换掉我们原来的高度信息，这样其他的都不需要改动。如果你只是想做一些简单的测试，也可以不做任何改动把车在环境绕一圈，高度应该是可以自动的调整回来

70. TerrainAnalysis 动态点云去除（clearDyObs）的设计思想是怎样的？

• 答：你好，我们会先检测当前的terrain_map里每一块区域中的点是否高度超过了我们设定的最小动态障碍物高度的阈值并且超过阈值的点在当前机器人的视野范围内，如果满足上述条件我们先设定该区域包含动态障碍物。然后使用最新一帧的点云，检测这些点云是不是也超过了我们设置的最小动态障碍物高度的阈值，如果仍然超过，则说明该点云所在的区域包含的是静态障碍物，则取消该区域的动态障碍物设定。如果其余被设定为包含动态障碍物的区域在最新一帧的点云中没有点仍然满足动态障碍物的设定，那么我们就认为该区域的障碍物已经消失，则该区域的点云是动态的，直接消除整片区域的点云。希望我的回答能够帮助到你

71. terrainAnalysis.cpp 中一些疑问

• 问

  • 1、参数clearDyObs的含义？看源码里，这是把激光雷达坐标系以下0.5m作为判断依据，大于则把planarVoxelDyObs计数加1；后续如何用，不太明白？
  • 2、如何理解planarVoxelDyObs参数？为何做两次clearDyObs判断处理？为何第一次planarVoxelDyObs[planarVoxelWidth * indX + indY]++，第二次planarVoxelDyObs[planarVoxelWidth * indX + indY] =0；如何理解？
  • 3、判断条件 angle4 > minDyObsVFOV && angle4 < maxDyObsVFOV的作用是什么，激光雷达的点云不是一直在激光角度范围内吗？
  • 4、对于上一帧激光雷达检测到的低矮静态障碍物，下一帧激光雷达检测看不到了，会不会被保留？如何实现的？从源码中没有理解这块；

• 答

  • 1.clearDyObs是控制是否消除动态障碍物的。我们会检测terrain_map里的所有点是不是超过了我们设定的高度阈值，这里我们的高度不是根据地面高度判断的，而是相对于车的高度来判断的，如果低于车高(传感器高度)0.5m我们就认为是障碍物。结合你得第三个问题同时还得判断上述被检测的点是不是在当前的激光传感器视野范围内。因为这里检测的是所有terrain_map里的点，这些点是之前很多帧点云的叠加，所以并不是所有的点都在视野范围内，我们只检测在当前视野范围内的动态障碍物。如果上述条件都满足，我们这里先认为该点是动态障碍物，所以把planarVoxelDyObs加1实际上就是统计该区域内有多少点满足动态障碍物的设定。
  • 2.planarVoxelDyObs就像我上面说的存储的是每一块区域包含的动态障碍物点的个数，第一次的clearDyObs是在统计当前terrain_map中有多少点满足障碍物的设定，第二次clearDyObs就是使用最新一帧的点云做二次检测，检测当前点云的这些点是不是仍然满足动态障碍物的设定，如果仍然满足，则说明该点所在的区域一直有障碍物，则之前的动态障碍物的检测不正确，这些障碍物不是动态障碍物，所以设定planarVoxelDyObs为0，代表该区域没有动态障碍物的点。如果某一个区域(planarVoxelDyObs>minDyObsPointNum)在当前点云中没有点满足动态障碍物的设定，则说明该区域之前存在的障碍物在当前视野中消失了，说明这些是动态障碍物，所以需要保持之前设定的planarVoxelDyObs值。在后续会使用这个值判断是不是需要保留该区域的点。
  • 3.这一点参考第一点回答。
  • 4.对于低矮的静态障碍物会一直保留。参考我上面的回答，如果下一帧看不到，则不会把该区域或者该障碍物考虑为动态障碍物，所有的动态障碍物检测只考虑视野范围内的区域或点

72. TerrainAnalysis 关于负障碍物问题

• 问：您好，我想请问一下在 terrainAnalysis.cpp 中这一段代码的作用

  for (int i = 0; i < planarVoxelNum; i++) {
      if (planarVoxelEdge[i] > noDataBlockSkipNum) {
          int indX = int(i / planarVoxelWidth);
          int indY = i % planarVoxelWidth;
  
          point.x = planarVoxelSize * (indX - planarVoxelHalfWidth) + vehicleX;
          point.y = planarVoxelSize * (indY - planarVoxelHalfWidth) + vehicleY;
          point.z = vehicleZ;
          point.intensity = vehicleHeight;
          point.x -= planarVoxelSize / 4.0;
          point.y -= planarVoxelSize / 4.0;
  
          terrainCloudElev->push_back(point);
          point.x += planarVoxelSize / 2.0;
          terrainCloudElev->push_back(point);
  
          point.y += planarVoxelSize / 2.0;
          terrainCloudElev->push_back(point);
  
          point.x -= planarVoxelSize / 2.0;
          terrainCloudElev->push_back(point);
      }
  }
  

• 答：你好，这里是为一些负障碍物(坑，陡坡或者悬崖)加的虚拟的点。当满足 planarVoxelEdge[i] > noDataBlockSkipNum的时候，表示那一块是负障碍物与正常地面的交界处，是不能行走的，但是激光雷达受视野限制在那里无法感知到任何点，所以我们会手动的在那里生成一些比较高的虚拟点作为障碍物，防止机器人继续前往

73. 关于 viewpoint_manager 的问题

• 问
  • 前辈们好，想请问为什么有两个 GetViewPointCoveredPointNum 函数，我明白函数是用来取出该 viewpoint 能够 cover 的点的个数，但是我不懂第二个函数，也就是下面这个函数的意义是什么

  int ViewPointManager::GetViewPointCoveredPointNum(const std::vector<bool>&point_list, int viewpoint_index, bool use_array_ind) {
      int covered_point_num = 0;
      for (const auto& point_ind : GetViewPointCoveredPointList(viewpoint_index, use_array_ind)) {
          MY_ASSERT(misc_utils_ns::InRange<bool>(point_list, point_ind));
          if (!point_list[point_ind]) {
              covered_point_num++;
          }
      }
      return covered_point_num;
  }
  

• 答
  • 你好，第一个是取某个viewpoint能cover的点的数目。第二个是取所有viewpoint能cover的点的数目。命名有些重复不好意思

74. 关于 terrainAnalysis 的问题，地形评估后点云漂移

• 问
  • 请问为什么我用的地形评估代码，随着我车子移动，评估后的点云会不断叠加，最后是漂移的效果
• 答
  • 你好，我们的地形分析输入的点云是在/map下的，所以如果你想直接用的话，需要先把你的点云转换到map下，或者你可以改一下我们的代码，接收你自己的点云
• 答：好的 我改了算法，不存储多帧处理后的点云就行了

74. LocalPlanner 没有可通行路径的问题

• 问

  • 目标点在前面，但是前半部分没有路径可走，我多视角看了，都没有点阻挡呀。好奇怪。偶尔会出现这样的问题
    

• 答

  • 你好，请问遇到这种情况之后你使用遥控器控制机器人向前，机器人会执行吗？

• 问

  • 情况是这样：时不时会遇到给目标点机器人不动的情况，但是用手柄遥感控制机器人它又能正常运动

• 答

  • 你好，这个看上去像是autonomy模式被干扰了，所以重启local planner之后相当于重新设置成了autonomy模式。你可以看一下自动模式下joy这个话题里是不是时不时会有消息发布，如果有的话就会自动触发手动模式，机器人就不会动了。还有一个方式，机器人不动的时候，你用遥控器触发autonomoy(官网主页上的遥控器使用教程里有)，看看车会不会走，如果能走，那么就是上面说的问题

75. navigation_boundary导航边界怎么修改呢？

• 问
  • 1.navigation_boundary导航边界怎么修改呢？是修改boundary_garage.ply文件中的参数吗？感觉修改参数要测量好仿真环境的大小再逐步调整，有没有更直接的方法通过软件或在gazebo中框选区域进行设置呢
• 答
  • 你好，你可以在cloudcompare打开我们的garage的点云文件，然后在里面选择选择边界点构成你需要的区域，然后把选中的点保存成ply文件就可以了。这个你可以参考我们教程里配置matterport的部分，里面有一些教程。或者也可以去网上搜索cloudcompare的使用教程

76. 关于 pathfollow 中的 dirDiffThre 的具体含义？

• 问：关于pathfollowing中的dirDiffThre 参数我有点疑问，我所理解的这个参数设置的目的是为了当角度比较大的时候，让小车原地转的，不知道我理解的对不对？而它默认值是0.1，是不是和lookAheadDis这个参数也有关系？我使用阿克曼底盘的车是不是可以直接通过增大这个值来实现呀？

  if (fabs(dirDiff) < dirDiffThre && dis > stopDisThre) {
          if (vehicleSpeed < joySpeed3) vehicleSpeed += maxAccel / 100.0;
          else if (vehicleSpeed > joySpeed3) vehicleSpeed -= maxAccel / 100.0;
        } else {
          if (vehicleSpeed > 0) vehicleSpeed -= maxAccel / 100.0;
          else if (vehicleSpeed < 0) vehicleSpeed += maxAccel / 100.0;
        }
  

• 答
  • 你好，这个值确实是在角度相差大于它的时候优先转向，而小于它的时候才可以有直线行走的速度。这个参数和 lookAheadDis 是没有关系的，只是控制什么时候该转向的，如果是 akerman 的底盘，可以试着将这个值调大，让车能够同时直行和转向

77. 关于 loam_interface 接口中 flipStateEstimation 和 flipRegisteredScan

• 问
  • 在看源码时，如果这两个参数设置为false时，这个接口就是接受slam提供的里程计和激光雷达话题信息，并以新话题/state_estimation和/registered_scan发布出去，但是当设置true时，我看源码里是把接收到的数据做了一个x,y,z坐标的转换，请问为什么要这样做呢? 还想麻烦作者大大解答一下
• 答
  • 因为不同的slam算法发布的odometry以及registered cloud坐标系是不一样的，比如loam发布的点云坐标系是map_rot，与我们使用loam_interface里发布出来的registered_cloud的坐标系map是差了一个旋转变换的，这时候就需要flipRegiateredCloud设置为true，但是也有别的slam算法发布出来的点云和loaminterface发布的点云坐标系一致，那就不需要转换，参数就是false

78. 报错 [ERROR] Spawn service failed. Exiting

• 答
  • 可以忽略这个报错，对实际的运行并没有影响

79. 室内导航遇到楼梯或斜坡应该怎么处理？

• 问
  • 想请教下，如果需要建图和导航的室内环境不只包含平面，也包含楼梯和斜坡，或者是进行跨楼层的建图导航，那么是应该将室内区域划分成若干子平面然后通过楼梯、斜坡进行连接吗，或者有现有的建图方法能兼容楼梯和斜坡吗
• 答
  • 你好，我们这套导航算法是可以应对斜坡的，但是楼梯更多的是当作障碍物处理，是无法应对的。现有的算法能处理楼梯的非常少，尤其是对于轮式或者履带式的无人车来说。如果你想让车能够自主的攀爬楼梯，可能需要用一些楼梯识别的算法首先找出楼梯再判断是否可以让车攀爬

80. terrainAnalysis.cpp 地面点分割代码疑问（滚动代码）

• 问

  • 请问各位老师，这个滚动操作在地面点分割中有什么用啊？不是很明白
    

• 答

  • 你好，这里的滚动更新是为了节省计算量。我们将机器人周围地图栅格化，并且每个栅格都存储该栅格内包含的点云，当机器人移动超过一个栅格的距离后，就会剔除超过地形分析范围的栅格内的点云，并加入新的点云
  • 举个简单的例子，机器人当前时刻周围 10 * 10m 一共有100个栅格，也就是说分辨率为11m，每个栅格的索引也是固定的，当机器人右移超过1m之后，最左边的栅格内的点云实际已经超过了1010的范围，应该删除，并且上一时刻的栅格索引实际已经不适用于当前时刻，所有的栅格内的点云索引都应该向左移动一格，而最右边的栅格内则加入新的点云。通过这种滚动更新，每次只需要更新栅格内点云的索引，剔除和加入新的点云也会非常的快

81. 关于在实际机器人中没有地图的问题

• 问
  • 您好，我想问问我在机器人中跑autonomous_exploration_development_environment没有地图，waypoint也无法使用，是要自己连接slam算法吗？
• 答
  • 当然，loam_interface这个功能包提供了slam的接口
• 问
  • 这个我看了，但是我需要连接自己的建图算法吗，我记得仿真的时候有内置的vloam
• 答
  • 你可以看看system_real_robot.launch，和别的launch文件中启动的程序区别，实际跑时你都需要接入自己传感器的数据，仿真时所用到的位姿估计和定位是作者他们写好了便于仿真跑的。也就是vehicle_siulator这个功能包
  • 你也需要在你的实际机器人上部署slam算法，然后从loam_interface这个接口接入，同时小车运行时slam算法也要保持开启状态
• 问
  • 我有个疑惑，实车跑的时候，SLAM算法也在建图么，还是只是定位呢？
• 答
  • 也在建图的

82. 在实机上运行，小车避障时会左右摆动，请问这是为什么呢？

• 答：你好，请问你说的避障时左右摆的现像在开阔区域不避障时会发生吗，能不能录一个左右摆的视频发上来，只要录实际跑的时候rviz里显示的信息就可以，包括地形分析、当前机器人坐标以及free_paths和path。通常来说左右摆原因可能是因为定位有抖动或者机器人本身的控制及运动能力有问题，不过还是需要具体的视频才能判断是什么原因

83. 在产生路径时，为什么代码中第二次使用三次插值时要乘以一个缩放参数scale？

• 问：如图所示，我看代码中scale取的是0.65，这个值的大小会对产生的路径组有什么影响呢？
  

• 答

  • 你好，这个值是控制路径组的弯曲角度的。正常来说，当机器人越靠近障碍物，避障时的转弯幅度就越大，这就需要距离机器人越近的路径组弯曲角度越大，这样机器人靠近障碍物时能够及时的避障。而距离机器人较远的路径组，避障的距离足够长，转弯幅度就可以小一些，所以乘一个小于1的系数调节后面两次样条曲线的弯曲角度

84. 有关 pathscale 这个参数的含义

• 问
  • 我的理解是在点云满足 dis<pathRange / pathScale时，就处理点云信息，这里pathScale默认是1.25，相当于是对pathRange进行了缩放，那就是说把满足距离条件的点云又做了一次筛选，处理距离更近的点云信息
• 答
  • 你好，pathscale有两个作用，第一个就像你说的，是对需要考虑的点云进一步筛选，第二个作用是在缩减pathrange之前，缩减pathscale来调整碰撞检测的范围，只有当pathscale缩减到最小仍然找不到路径的时候，才会直接缩减pathrange，步长更大一些，或者说更大胆一些

85. angDiff，dirToVehicle 两个参数的含义

• 问：在这个判断条件内，定义了三种情况，当满足其中一个时，就不处理这次路径，请问这里angDiff，和dirToVehicle两个参数的含义是什么呢，这三种情况分别对应的又是什么呢？
  

• 答

  • 1.dirToVehicle这个参数是为一些类似汽车的机器人平台设置的，这些平台不能原地转向而只能一边向前或者向后运动一边转向。当这个参数设置成false的时候，就是应用在大多数可以差速驱动的机器人平台上，只考虑目标方向附近的轨迹，机器人可以任意无碰撞的转向需要的方向。而当这个参数设置成true的时候，就是应用在上述类似汽车的平台上，这时候只考虑机器人朝向dirThr之内的轨迹，因为机器人只能向前或者向后运动的时候转向，dirThr之外的轨迹机器人是没有能力实现的。一般dirToVehicle是true的时候，dirThr会设置的比较小，比如只考虑机器人朝前或者朝后10°的范围内的轨迹。如果用的是差速驱动的机器人，那就可以保持现在的默认设置。
  • 2.angDiff是目标方向与待检测的轨迹方向的差值。
  • 三种情况可以从上面第一点回答中找到对应情况，第一种是应用在差速机器人上，只考虑目标方向附近的轨迹，第二种及第三种是应用在阿克曼转向的机器人上，分别对应向前及向后运动的情况，只考虑向前及向后小范围内的轨迹

86. 从 local_planner 传入 path_follower 的路径为什么是 /path 而不是 /free_path 呢？

• 问

  • 当从36*7条路径中选出了得分最高的一条发布出来后，传递给path_follower,这个path我的理解只是第一次样条插值采样到的路径点呀？为什么不传入/free_path呢，是我理解错了吗？小车在具体执行跟随的路径应该是/path吧？
    

• 答

  • freepath是所有无障碍阻挡的路径，path是在这里面选出的最高的分路径。pathfollower跟踪path就行了

• 问

  • 请问小车在运行时，选出一条path后，是先运动到这个path的末端再重新选下一条path吗，就是说这个path多久更新一次呢

• 答

  • 每个循环都要更新一次。 你可以在rviz里吧freepath关了，只留path 那根很短的绿色小线就是选出来的path

87. 有关 local_planner 对路径进行评分的公式如何来的？

• 问

  • dirDiff是该条路径与目标点之间的角度差值，rotDirW代表了该条路径的方向与当前车辆朝向的角度差，这两个参数我没理解错吧？他们分别为啥要开四次根号，为啥要4次方呢？
    

• 答

  • 你好，参数没有理解错，这个是一个经验公式，在实际的应用中，这种非线性的关系效果更好

88. Forest 环境下 time duration 计时失真问题

• 问
  • 在forest环境中，运行vehicle_system.launch system_forest.launch仿真环境，使用tare探索时，会出现横坐标和真实时间失真的问题：例如，真实完成forest的探索为40min左右，但time duration统计只有大约800s。同时在time duration中数据横坐标过去10s，真实秒表过去大约30s。请问各位作者是由于环境卡顿导致的吗？
• 答
  • 你好，是因为森林这个环境在运行时比较卡，real-time factor很低，比如你运行的时候大概只有0.3左右，也就是实际的时间几乎是仿真里面时间的三倍。我们统计的时间都是按照仿真里的时间的。如果电脑显卡配置高一些的话，会流畅非常多，这两个时间也会很接近

89. 关于可视化部分的体积计算问题

• 问：在可视化工具中，有一个地方不是很明白，就是探索体积随时间的变化曲线，这个探索体积是如何求得呢？给定扫描得到的点云怎么计算体积大小？
  

• 答

  • 你好，因为点云是有分辨率的，在PCL对点云降采样时，会设置voxel size，这就相当于设置了分辨率，在一个voxel里只会有一个点。所以在固定的分辨率之下，比如0.5 0.5 0.5m，用总的点数乘以每个voxel对应的体积，就是总体积了。这一部分程序在visualization_tools里有。当然，这种计算方式实际上是相当的粗糙的，与实际的探索空间的体积并不一定一致，但一定是成正比的，实际的体积越大，我们的算法计算的体积也一定越大

90. 关于论文 AEDE 中的 planning horizon

• 问
  • 在 V. BEST PRACTICES 一节中提到 The local planner uses a planning horizon at the distance between the vehicle and waypoint，这里的 planning horizon 指小车和路径点间的距离？
  • This ensures that the vehicle can stop at waypoints relatively close to obstacles - collision check does not consider obstacles further than the waypoint. 为什么 planning horizon可以确保小车停在靠近障碍的路径点？这与碰撞检测有什么联系？
• 答
  • 你好，这里的planning horizon确实是小车与waypoint之间的距离。当waypoint很远的时候，planning horizon没有影响，local planner按照最大的范围做碰撞检测。但是当waypoint距离机器人较近并且其后面很接近障碍物时，如果仍然正常的进行碰撞检测，就会认为前往waypoint的方向被障碍物挡住了，local planner就会阻止机器人前往，这样就无法到达waypoint了，所以此时用planning horizon控制碰撞检测的范围，只检测waypoint和机器人之间的障碍物，后面的障碍物不考虑，机器人可以顺利到达机器人位置

91. 车辆方向和目标点方向夹角很大时，速度会置 0 发布速度

• 答：你好，当目标点角度与车辆前进方向差别很大时，优先转向，所以会停下转到两者差距足够小时直线速度就会增加，再继续的前进，所以弯道很急时，会先停下转向，然后再直行，并不会导致车停下不再移动。indoor环境的时候，环境会稍微的开阔一些，所以目标点可能会处于斜前方，而不是正右方或左方，这时如果判定可以，就会边走边转向。如果你发现matterport中车转向过程中停下不动了，并且free_paths都消失了，那可能是某些地方过于狭窄，被判定为无法通过，与弯道是否角度过大是没有关系的

92. 同样曲线函数加默认参数生成的路径束，在ground里就会出现不走直线的情况，而在aerial里就可以正常进行

• 答：ground不走直线，根据视频分析，并不是pathfollower跟踪的问题，而是在local planner中筛选路径的时候并没有筛选出唯一的路径，最终跟踪的路径仍然有多条，这种情况下具体跟踪的效果会和你想要的不一样。你可以具体看一下你生成新的库之后是不是有些路径代码中的筛选条件无法区分

93. 在论文中是如何对比不同规划算法的路径长度和执行（或者计算）时间的呢?

• 答：你好，dsvp以及tare中对比路径长度以及时间的方式都在visulization_tools这个包中。路程长度是用的定位输出的里程计的结果，而规划时间则是根据规划开始到结束的时间间隔，可以使用ros time去抓取，也可以使用c++中的time去抓取，在实际的实验中，两种时间几乎是一致的

94. 用 local_planner 上不去坡？

• 答
  • 目前算法的地形分析会把陡坡识别成障碍，如果是个大缓坡就不存在这个问题了，应该需要在地形分析的cpp中进行改进
  • 坡度过大是会被检测为障碍物，根据地形分析的原理，我们是将每个栅格内最低点作为地面点的，其余点相对于最低点的高度作为通过代价，坡度大的时候，单个栅格内最上面的点会比最下面的点高很多，超过可通过阈值就会被认定为障碍物。暂时只能通过调节参数来解决，但是有可能导致正常地面可通过障碍物阈值过大，这一点需要你自己来权衡了

95. 关于 local planner 中调节速度的问题

• 问
  • 我把ground的导航程序移植到实际机器人上以后，发现机器人旋转时的角速度很大，是需要修改图1的三个参数来调整转弯的角速度吗？还是需要在底层的驱动里来限制最大的角速度？
  • 图2中的adjacent Range是指的导航时实际计算避障的范围区域吗？minrelZ和maxrelZ是指的什么呢？是传感器平面的上下范围吗？
    

• 答
  • 你好，图一中的三个参数前两个是调整角加速度，第三个是最大角速度，角度单位都是度（°）。adjacent_range是最大速度对应的避障范围，实际的避障范围是和实际速度成线性关系的，速度越慢，避障范围越小，同时会有一个最小避障范围。minRelZ以及maxRelZ限制了有效点的Z值范围，可以查看代码，Z值的最大最小范围由这两个值决定，但是并不等于这两个值，是有一个比例关系的，距离机器人越远，限定范围就越大，这其中考虑了坡度

96. terrain_analysis.launch 和 terrain_analysis_ext.launch

• 问：各位大佬，请教个问题，我想用terrain_analysis包来实现velodyne16对地面的分析，不过我的机器人载体是四足机器人，机器人在运动过程中幅度会有些大，导致点云是晃动的，请问这样的情况下可以使用terrain_analysis吗。在整个autonomous的大工程里，我发现像system_indoor这些仿真的文件里有运行了两个terrain_analysis，这两个包的功能性不重复吗？
• 答：你好，即使是四足机器人也是可以使用我们的terrain_analysis，点云晃动没关系，只要保证点云在世界坐标系下不晃动就可以了，也就是位姿估计不晃动。这两个terrain_analysis范围以及分辨率上有区别，第一个是10m10m范围的，分辨率为0.1m，主要用于局部的路径规划，第二个则是40m40m范围的，分辨率为0.2m。主要用于探索全局路径规划之类的上层规划算法分辨可通行与不可通行的区域。如果你只是需要局部避障，可以不用运行第二个。至于没有看到红色绿色区域，可能是rviz设置的问题，rviz里显示点云的时候可以按照intensity显示颜色，这个你可以查一些rviz设置相关的知识

97. 在室外柏油路面上打转会出现小车周围的点云都变红的情况

• 问

  • 如上图所示，我们在室外柏油路面上打转会出现小车周围的点云都变红的情况，即地形代价太大；柏油路上直行走也很容易出现一些点代价变大的情况，但比转圈要好很多；我们猜测是地形算法的阈值需要调整，太灵敏了。请问我们需要在哪些配置文件上更改哪些参数；或者源码哪个部分？
    

• 答

  • 你好，请问柏油路面是平整的吗，如果直行出现的不可通行点少，我建议你可以重点查看一下里程计的输出在转向时是不是不稳定，有可能是定位不准造成的，阈值的调整更多的还是影响高度在临界条件的点，平整路面上的点如果没有反光之类的影响不会被识别为不可通过的点
  • 问题已找到，主要调整vehicleHeight = 1.5（车辆的高度）、minRelZ （点云处理的最小高度）、maxRelZ（点云处理的最大高度）这三个参数；经过调整，车辆高度根据实际设为0.75，minRelZ 设为-0.7，在原地打转的情况下好了很多，没有出现很多红色点；但是由于这个框架中的地形处理是直接对点进行阈值处理，就会有很灵敏的反应，在这种柏油路面很容易有红色不可通行出现，即噪点
  • 可能是差速车在室外路面原地旋转的时候，轮胎出现拖拽就会导致车身存在高频小幅抖动。现在限制了车辆旋转速度，减少了抖动基本解决了这个问题

98. 部署真实机器人，向前探索 5 米后显示探索结束

• 问：最近在将tare部署到真实机器人上跑室外实验，运行后会出现这种情况，向前行驶几米后出现探索结束 ，返回到起点，有时还会原地转圈，这是跟tare里面的参数有关系吗？需要根据自己的场景调正参数大小吗？都涉及到了哪些参数呢？
• 答：您好，就您描述的信息来看我也无法准确判读是什么问题。一般来说，可以先检查一下state_estimation输出和terrain_map输出对不对，有没有因为state estimation的不准而导致噪点被当成障碍物。其次可以试一下发way_point给local_planner让机器人在环境中跟随，看机器人能不能避障以及顺利到达目标点。如果这些都没问题，可以试下不同的tare的launch file。如果是室外可以试一下explore_campus.launch. 如果是室内可以试一下explore_matterport.launch

99. 使用 fastlio 部署 system_real_robot.launch

• 问：使用fastlio部署system_real_robot.launch loam接口按照要求已经调整好，发现tf树没连接上，不知道怎么修改，还有fastlio的frame坐标在地图上混乱的 应该怎么调整?
• 答：你好，我们的系统实际需要的就是两个话题，一个是包含sensor在map下的odometry(integrated_to_init)，另一个就是当前帧点云在map下的投影(velodyne_cloud_registered)，只要提供这两个信息，loam_interface会自己发布一套tf以及点云出来。需要注意的就是在fastlio里不能发布重复的map到sensor的关系，最简单的做法就是将fastlio里的map和sensor(如果有的话)都换成别的名字，防止重复发布有冲突的tf

100. way_point 发布一个轨迹，用来寻迹加避障呢？

• 问：您好，您这套算法尝试过way_point 发布一个轨迹，用来寻迹加避障呢？我在仿真环境下尝试过，感觉应该可行，现在由于实验设备有问题，暂时做不了实验，想问下理论上的可行性
• 答：是肯定可以的，我们的仿真环境里waypoint_example里就有这个功能，实际上实现是没有问题的

101. terrain_analysis 矮障碍物检测的问题

• 问：您好，我使用了16线激光雷达，使用local_planner时无法绕开这些矮障碍物，请问 在terrain_analysis中能否准确识别出这种5cm左右的障碍物，需要改参数吗？
  

• 答：你好，可以尝试将local planner里的obstacleHeightThre改成0.05，如果定位准确的话，应该是可以识别的

102. 自己的仿真环境跑 local_planner，规划的速度一会大一会小

• 问

  • 我在使用自己的模型进行gazebo仿真的时候，他规划出来的速度和角速度一直波动，直线运动的时候速度也是一会大一会小，下面是截图； 如果是跑自带的仿真环境 system_garage 就不会出现上面的情况;不知道这是什么原因?
    

• 答

  • 你好，你可以查看一下在你自己的环境里启动之后传感器距离地面的高度是不是符合预期，机器人的初始位置包括高度根据环境不一样有可能会有区别。然后你可以使用smart joystick模式，试试机器人能不能稳定行驶，再disable collision check之后看看机器人的行驶状态

103. 激光雷达安装高度不同，是否对整套系统有影响？

• 答
  • 在 terrain_analysis.launch 中设置了 vehicleHeight，你可以将这个参数修改为激光雷达对应高度，实际上安装高度真正影响的是terrain_analysis

104. 单独启动局部规划器，给予近处目标点，小车行走来回摇摆前进可能原因是什么？小车到达目标点附近，前后反复移动多次才停下，该如何解决？

• 答
  • 小车来回摇摆以及在目标点附近前后反复，原因可能一致，有两个可能的原因，第一是小车的控制频率太低或者控制写的不好，local planner发出的控制指令不能被及时响应，第二是定位跳动较大，干扰local planner的规划。第二点可以通过rviz判断定位是否足够精确稳定，第二点可以使用local planner的手动模式，试试小车的控制有没有延迟

105. 请问能否将看到的点云一直保存呢？如果可以应该如何更改呢？

• 问
  • 在实车部署cmu的过程中，因为安装的原因，雷达的有着较大的视野盲区，大概是以车体为圆心2m的范围内，都是盲区，在车体距离障碍物较远的时候，可以看到障碍物，但是走近障碍物的时候，原先看到的障碍物随着时间消失了，而此时雷达也看不到如此近的障碍物，导致避障效果不佳，想问问大家如何提高点云的存在时间
• 答
  • 你好，我们是会根据距离调整看到的物体，不会根据时间，也就是说机器人看到过的五米范围内的点都会一直保留，如果盲区是两米的话，那在靠近过程中点应该是不会消失的
• 问
  • 你好，请问能否将看到的点云一直保存呢？如果可以应该如何更改呢？
• 答
  • 你可以设置 noDecayDis 和 clearingDis 这两个参数，第一个是设置地形一直保留的范围，第二个设置在到达保留时间之前会保留的范围。你可以将这两个参数都设置的大一些，就能保留大一些范围的地形。但是不建议把所有看到过的都保留，地形分析并不是单纯的点云叠加，内部还会对点云进行处理，如果范围太大，计算量也会很大，所以范围不宜太大

106. 请问导航过程中生成的点云地图可以全部保留吗？

• 问
  • 你好，请问导航过程中生成的点云地图可以全部保留吗？就是导航结束后最后有一个整体环境的三维地图，目前是经过一段时间后原先生成的点云会消失，尝试着保存点云数据没有成功，希望能得到解答，谢谢！
• 答
  • 你好，visualization_tools 会发布 explored_areas，这个话题里会保留所有已经看到过的点，至于你说的经过一段时间点云会消失应该是指 terrain_map 吧，这个是不会一直保留的，因为这部分地形分析计算量很大，所以如果保留的范围过大会耗费大量计算量

107. 如何设置让机器人在户外固定区域内完成探索，或者设置探索的时间，在规定时间内探索完毕回到原点

• 答：你好，程序里都有boundary.ply配置文件，这个文件可以设置边界点，限制探索范围。如果是想限制时间，就需要你单独在程序里加一部分检测探索时间的功能

108. 雷达向下的FOV角度比较小，不能很好地扫到地面，会影响导航吗？

• 问
  • 雷达向下的FOV角度比较小，不能很好地扫到地面，会影响导航吗？机器人周围的基本都是黑色或者红色，好像会影响导航，这个应该怎么解决？？？
• 答
  • 你好，FOV看不到近处的地面，但是在作地形分析时会保留远处一定范围的点，所以机器人行驶一段距离之后就能保证周围全部被观察到。只有在初始阶段或者刚刚转过直角弯这种场景，地面如果有低矮障碍物可能会看不到。这个除了改变安装角度或者更换传感器，目前没有更好的解决方式

109. 请问关于实际机器人的坐标转换怎么解决

• 问
  • Autonomous Exploration Development Environment仿真中的点云的是在 map 坐标系下的，地形分析和规划都是在 map 系下计算的，而实际机器人的点云数据是在雷达坐标系下的，这个点云的 frame_id 转换好像会出问题。请问关于实际机器人的坐标转换怎么解决？？？
• 答
  • 在机器人领域，常用到的坐标系有map坐标系、雷达坐标系、车体坐标系等等。而点云数据的坐标系也会随着传感器的不同而不同。因此，在进行坐标转换时，需要注意当前使用的点云数据的坐标系，以及目标坐标系
  • 接下来，介绍一种解决机器人坐标转换的方法：通过tf库进行坐标转换。tf库是ROS中常用的一个坐标转换库，可以将机器人在不同坐标系下的位置和姿态进行转换。在代码中，我们需要先获取实际机器人的点云数据的坐标系和map坐标系之间的转换关系，即相对位姿。然后，利用tf库提供的函数进行坐标转换即可
  • 在进行坐标转换时，我们可以先在rviz中可视化一下点云数据和机器人模型，以确保坐标系的正确性

110. 发布的 cmd_vel 角速度变化太快

• 问
  • 我在运行roslaunch vehicle_simulator system_real_robot.launch后，在rviz中小车的前方设置一个waypoint后，发布的cmd_vel如下，能看出来angular的z值跳动的非常大，导致我的无人车一直在频繁的左右摆动这向前移动，老师们有什么好的解决办法吗
  • 另外rviz中octomapOccupied会在空旷的区域出现下面红色的点，而且在小车的初始位置附近（绿色箭头指向的地方）每次都会出现很多红圈，下面两个图的区别是有没有显示octomapOccupied，想问问各位老师为什么会这样
    

• 答

  • 角速度变化太快的问题解决了，通过将local_planner.launch中yawRateGain和stopYawRateGain改为3
  • 这样更改之后转弯的幅度也变小了，waypoint有时会出现在小车的后方，我的小车因为一些原因只能前进不能后退，这样小幅度的转弯要等很久才能到达后面的waypoint，所以我想问问，当周围都是空旷区域的时候，有没有办法限制waypoint出现在前方

• 答

  • 你好，角度变化太大实际上不会导致车左右摇摆，左右摇摆应该是规划输出有问题。你该的参数值确实是角加速度，会让加速缓一些。如果需要waypoint一直在前方，就要你自己去加一些判定逻辑，判断一下waypoint和车的相对位置
  • 你说车周围会有occupied，那这些occupied的区域高度是怎么样的呢，地面也会被认为是occupied，如果高度很低就是地面。但是如果是在空中，那就要看一些terrain_map上是不是出现了一些空中的噪点，有可能是这些噪点导致车左右摇摆

111. Local Planner 与机器人实际部署问题，机器人不断旋转（FAST-LIO2定位）

• 问答超链接

112. 如果我想在地图中已知坐标中添加障碍物，是通过 /added_obstacles 这个话题吗？

• 问
  • 如果我想在地图中已知坐标中添加障碍物，是通过 /added_obstacles 这个话题吗？这个话题如何发布？这个话题的发布有没有demo?谢谢！
• 答
  • 你好，是这个话题。这个话题我们是通过单独的节点发送的，并没有包含在我们的开源代码中。具体的实现是在指定的坐标处添加点云，点云形状可以自己定义，排列生成就可以，后续也是以点云的形式被发出并被其他节点接收

113. 实车测试，发现空旷地带，产生的噪点过多，导致无法规划

• 问：最近在测试 far_planner + fast-lio 实车试验，目前采用的是matterport.config参数,发现在直线上表现良好，遇到转弯或是远距离行驶，就会出现较多噪点，无法获取路线，视频是手动遥控无人小车进行测试录屏，望大佬分析一下 是什么原因造成的，能从那些方面入手 调整参数或者代码
  

• 答

  • 你好，在测试过程中你可以着重关注一下terrain_map以及terrain_map_ext的结果，看看你觉得不应该是障碍物的地方是不是地形点的intensity(可通过难度)也比较高，如果是默认显示的话地面都应该是一个颜色，颜色会随着可通过难度变化。我看视频发现你的第一幅图中的地形会有抖动，所以可能是定位的抖动，导致地形分析有误差，进而影响far planner

• 答：感谢回复，是地面不平整导致车抖动厉害，目前通过设置点云处理高度解决

114. 将Autonomous Exploration Development Environment部署到阿克曼转向及全向轮平台

• 阿克曼转向及全向轮平台部署