多机器人路径规划CBS/ECBS等 libmultiRobotPlanning 代码阅读笔记

LoveBc0214

已于 2022-08-30 19:48:02 修改

阅读量4.1k

点赞数 6

分类专栏：学习记录论文笔记文章标签：算法

于 2022-06-20 22:25:01 首次发布

本文链接：https://blog.csdn.net/weixin_42983054/article/details/125367583

版权

学习记录同时被 2 个专栏收录

7 篇文章 1 订阅

订阅专栏

论文笔记

5 篇文章 0 订阅

订阅专栏

编译后会生成doc的doxygen的html文件，帮助对程序的理解。

CBS 的容易理解的一个博客:CBS多机器人规划–深一

cbs.hpp 中 search 函数结构为：
1、首先构建start节点，其中包含了空solution,空constrains，cost =0和id=0。
2、调用低层次搜索，产生初始解填补 start.solution.
3、构建最小堆open，并且将初始节点压入堆
4、弹出open堆中代价最小的节点P直至open为空
5、对P中的solution进行冲突检查
6、无冲突则返回solution，有冲突则创建相应的约束，并且构建新的节点NewNode, NewNode = P，但是需要增加新的约束。然后有一个减去代价的操作(未理解，可能是因为，上个节点不能用，所以需要回退到前一个，所以代价需要相应减少，在lowLevel.search中有计算每个解的代价，可以去看看)

发生冲突的路径，假设对A机器人和B机器人搜索的路径分别是 $P_A$ 和 $P_B$ ，对应的代价分别是 $C_A$ 和 $C_B$ ,发生冲突后，就需要添加约束重新搜索，比如是对A进行重新搜索，那确实是需要减去代价 $C_A$ 的

7、对NewNode进行低层次搜索，若成功搜索到路径，然后加入open堆中，跳至4。

高层次搜索：检测冲突并且构建约束树。

CBS serach 中调用低层次的搜索产生路径如下：为每个机器人搜索产生路径，失败的话直接返回false.第层次搜索是在有约束的基础上只搜索路径，不会检测是否有冲突。因此如果在构建好的约束上去搜索，没有路径。那直接失败。

    for (size_t i = 0; i < initialStates.size(); ++i) {
      // if (   i < solution.size()
      //     && solution[i].states.size() > 1) {
      //   start.solution[i] = solution[i];
      //   std::cout << "use existing solution for agent: " << i << std::endl;
      // } else {
      LowLevelEnvironment llenv(m_env, i, start.constraints[i]);
      LowLevelSearch_t lowLevel(llenv);
      bool success = lowLevel.search(initialStates[i], start.solution[i]);
      if (!success) {
        return false;
      }
      // }
      start.cost += start.solution[i].cost;
    }

对open列表循环检测。open 是约束节点队列
根据路径，利用Environment中成员函数getFirstConfilict检查冲突，无冲突返回true。

	  Conflict conflict;
      if (!m_env.getFirstConflict(P.solution, conflict)) {
        std::cout << "done; cost: " << P.cost << std::endl;
        solution = P.solution;
        return true;
      }

对于每个节点根据冲突增加约束，然后再进行搜索，直至while(!open.empty())不成立，或者是路径检查后无冲突。

m_env.createConstraintsFromConflict(conflict, constraints);
for(const auto& c : constrains)
{
....
}

example中的cbs等，对应multi_trajectory中的environment.h

整体结构并不是难以理解，但是现在疑问

1) 如何有约束的A *搜索路径; 2) Environment的设置，如何确定冲突；3）约束的建立

Answer 2） Environment的结构定义是在cbs.cpp文件中 Class Environment
环境的初始化函数包括了维度信息，比如4X4栅格，障碍物位置，目标位置，disappearAtGoal默认是false,指的是，到达目标点后就消失，怪不得不会检测目标点是否一样导致的冲突呢

Answer 3） 约束建立：
冲突的结构(源码还有一些运算符重载)：

struct Conflict {
  enum Type {
    Vertex,
    Edge,
  };

  int time;
  size_t agent1;
  size_t agent2;
  Type type;

  int x1;
  int y1;
  int x2;
  int y2;
  }

节点冲突结构(源码中同样有重载)：

struct VertexConstraint {
int time; int x.; int y;}

边冲突结构：

struct EdgeConstraint {
  EdgeConstraint(int time, int x1, int y1, int x2, int y2)
      : time(time), x1(x1), y1(y1), x2(x2), y2(y2) {}
  int time;
  int x1;
  int y1;
  int x2;
  int y2;
  }

带有约束的A搜索，但是好像并不是A,代码中没有启发式的计算过程。而且另外一点，因为包含了约束，所以启发函数只用欧式距离可能并不能保证最优。
另外其最优解应该是用stl标准库的unordered_map找到的。

根据冲突建立约束，包括两类约束：
vertex constraint 和 edge constraint ，其中和冲突一样。

cbs.cpp中函数creatConstraintsFromConflict()为实际创建约束的函数，主要区别是增加了对agent的约束，两个结构体只是表示，在某个时间不能在顶点A 或者不能从顶点A到顶点B

  void createConstraintsFromConflict(
      const Conflict& conflict, std::map<size_t, Constraints>& constraints) {
    if (conflict.type == Conflict::Vertex) {
      Constraints c1;
      c1.vertexConstraints.emplace(
          VertexConstraint(conflict.time, conflict.x1, conflict.y1));
      constraints[conflict.agent1] = c1;
      constraints[conflict.agent2] = c1;
    } else if (conflict.type == Conflict::Edge) {
      Constraints c1;
      c1.edgeConstraints.emplace(EdgeConstraint(
          conflict.time, conflict.x1, conflict.y1, conflict.x2, conflict.y2));
      constraints[conflict.agent1] = c1;
      Constraints c2;
      c2.edgeConstraints.emplace(EdgeConstraint(
          conflict.time, conflict.x2, conflict.y2, conflict.x1, conflict.y1));
      constraints[conflict.agent2] = c2;
    }
  }

变量constraints是map对象，c1是冲突变量，即在某个时刻不能在某点，或在不能穿过某条边。当为顶点冲突时，只需要对产生冲突的1，2智能体分别添加约束，即constraints[agent1/2] = c1. 当为边冲突时，由于agent1是A->B, agent2是B->A，所以添加的约束不同。agent1是不能在t时刻，从(x1,y1)->(x2,y2)，agent2 是不能从(x2,y2)->(x1,y1)。注意：对车辆1建立约束，则边冲突（即交换位置产生的冲突，车辆1的位置在前
这里注意 constraints 是变量名，Constrains时结构体名

*Answer 1) *
A_star.cpp中，Node节点结构为：

  struct Node {
    Node(const State& state, Cost fScore, Cost gScore)
        : state(state), fScore(fScore), gScore(gScore) {}

    bool operator<(const Node& other) const {
      // Sort order
      // 1. lowest fScore
      // 2. highest gScore

      // Our heap is a maximum heap, so we invert the comperator function here
      if (fScore != other.fScore) {
        return fScore > other.fScore;
      } else {
        return gScore < other.gScore;
      }
    }

    friend std::ostream& operator<<(std::ostream& os, const Node& node) {
      os << "state: " << node.state << " fScore: " << node.fScore
         << " gScore: " << node.gScore;
      return os;
    }

    State state;

    Cost fScore;
    Cost gScore;

初始化中，第一个是状态，第二个是F=G+H中的F，第三个是G
State是在cbs.cpp中定义的结构体，包括当前时间和位置，time, x,y

邻居的定义，启发式信息是什么，每个状态和action ，如何满足约束

A* 搜索
普通A* 流程

    把起点加入 open list 。

    重复如下过程：

a. 遍历 open list ，查找 F 值最小的节点，把它作为当前要处理的节点。

b. 把这个节点移到 close list 。

c. 对当前方格的 8 个相邻方格的每一个方格？

◆ 如果它是不可抵达的或者它在 close list 中，忽略它。否则，做如下操作。

◆ 如果它不在 open list 中，把它加入 open list ，并且把当前方格设置为它的父亲，记录该方格的 F ， G 和 H 值。

◆ 如果它已经在 open list 中，检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好，用 G 值作参考。更小的 G 值表示这是更好的路径。如果是这样，把它的父亲设置为当前方格，并重新计算它的 G 和 F 值。如果你的 open list 是按 F 值排序的话，改变后你可能需要重新排序。

d. 停止，当你

◆ 把终点加入到了 open list 中，此时路径已经找到了，或者

◆ 查找终点失败，并且 open list 是空的，此时没有路径。

3.保存路径。从终点开始，每个方格沿着父节点移动直至起点，这就是你的路径。

在带有约束的搜索中，主要是
首先创建open_set，并且把初始状态，以及对应的F和G值作为一个节点push进openSet中。
初始的G即为0，所以初始F即为H，这里的启发式函数是曼哈顿距离。
然后放入stateToHeap中，stateToHeap 和openSet是一样的，根据源码猜测，openSet是用来排序，然后找到最小代价的，stateToHeap是用来根据句柄查找的，检测扩展的节点信息是否在openSet中。（map的查询复杂度O(1)）

查询openSet中F值最小的，由于采用最小堆，所以top的就是最小的
检查检查终点是否在open_list中，如果在,就对之前的solution进行clear，然后重新push_back，返回路径。如下：

if (m_env.isSolution(current.state)) {
        solution.states.clear();
        solution.actions.clear();
        auto iter = cameFrom.find(current.state);
        while (iter != cameFrom.end()) {
          solution.states.push_back(
              std::make_pair<>(iter->first, std::get<3>(iter->second))); //-- 状态和G值
          solution.actions.push_back(std::make_pair<>(
              std::get<1>(iter->second), std::get<2>(iter->second)));//-- Action和F值
          iter = cameFrom.find(std::get<0>(iter->second)); //--find状态(应该是父节点)
        }
        solution.states.push_back(std::make_pair<>(startState, initialCost));
        std::reverse(solution.states.begin(), solution.states.end());
        std::reverse(solution.actions.begin(), solution.actions.end());
        solution.cost = current.gScore;
        solution.fmin = current.fScore;

        return true;
      }

如果不是终点，那就把当前节点从open_list中除去pop，然后在close_list中加入

     openSet.pop();
      stateToHeap.erase(current.state); //--在stateToHeap中删除当前节点的状态
      closedSet.insert(current.state);

然后对当前节点扩展周围四连通区域，包括前进后退左右和wait，这个代码在
cbs.cpp文件中Environment class中。在增加状态时，会检查两类约束，stateValid()函数和transitionValid(s,n)

bool stateValid(const State& s) {
   assert(m_constraints);
   const auto& con = m_constraints->vertexConstraints;
   return s.x >= 0 && s.x < m_dimx && s.y >= 0 && s.y < m_dimy &&
          m_obstacles.find(Location(s.x, s.y)) == m_obstacles.end() &&
          con.find(VertexConstraint(s.time, s.x, s.y)) == con.end();
 }

 bool transitionValid(const State& s1, const State& s2) {
   assert(m_constraints);
   const auto& con = m_constraints->edgeConstraints;
   return con.find(EdgeConstraint(s1.time, s1.x, s1.y, s2.x, s2.y)) ==
          con.end();
 }

对满足约束的周围节点，检测是否已经进行过扩展（是否在close_list中）,是否在open_list中，若不在open_list中，计算代价，然后填充到open_list和stateToHeap中
若在open_list中，则检查之前G值和当前G值，若之前>现在，进行更新

openSet.increase(handle); //--修改G和F值后，在open_set中重新排序，数据结构必须配置为可变的

然后放到cameFrom中，这个为了找到父节点，然后寻路的。

细节：有z方向，代表的是车头的朝向，其实就是改成了基元类型，禄五年中也有体现

安全走廊建立

就是对线段在x,y方向进行扩展，扩展成为正方形的
首先对每个线段进行分割，分成5个，具体原因论文中有体现：如果是整个线段，那如果线段的一部分扩展后有障碍，则整个都废了，但是分成小段，就会解决这个问题
主要函数是corridor.hpp中的uodateObsBox()
首先在corridor类构造函数中对N:轨迹点的数量，plan: 轨迹点的信息，位置，速度信息。当两个轨迹点之间距离小于0.1的时候，速度为0,距离在0.1-1.1时，为0.7的最大速度，当大于1.1时，为最大速度。

函数isObstacleInBox()，是遍历box中的点，以octomap的分辨率进行遍历，逐个进行遍历，查看该点到障碍物的距离，是否和膨胀后的智能体距离一样，从而看是否碰撞。

函数isBoxInBoundary()，看点box是否在地图中

函数expand_box()，对当前box，在xyz的6个方向上，以分辨率逐个进行扩展，直至碰到边缘（isBoxInBoundary())或者障碍(isObstacleInBox())后停止。

函数updateObsBox()，主要函数，在prioritized_traj_planner.cpp和couple_traj_planner.cpp中调用生成安全走廊。思路就是对相邻的两个点，选择最小的xyz作为box的下限，最大的xyz的box的上限。然后利用expand_box()函数对Box进行扩展。