混合式A星代码解析_3 A星算法程序algorithm.cpp

4.A星算法主程序 algorithm.cpp

4.1 几个算法相关的重要的概念

(1)状态空间

• R3:三维状态空间,包括:($x,y,\theta$)
• R2:二维状态空间,包括:($x,y$)
• R4:四维状态空间,包括:($x,y,\theta ,r$), $r$代表前进方向,1为前,0为后.

(2)节点
A星搜索的每个状态都是一个节点,算法中用到了两种类型:

• 3D节点:程序中表示为Node3D类型,主要包含($x,y,\theta$)三种状态.
• 2D节点:程序中表示为Node2D类型,主要包括($x,y$)两种状态.

2D节点就是传统A星所使用的节点,主要是为了加入障碍物约束.
3D节点是基于RR曲线生成的,是为了加入动力学约束.

(3)表
在A星搜索的过程中,有两种类型的表open表和closed表,拓展过的点会加入到closed表中,未被拓展过的节点加入open表中.

4.2 (核心)Algorithm::hybridAStar

关于Algorithm::hybridAStar函数的输入和返回参数,在algorithm.h中,有详细的说明.

这里首先分析算法处理流程:

4.2.1 初始化变量

• 定义前任iPred和后继iSucc;
• 定义方向的数量:如果是考虑掉头的话,方向就是6个,不考虑掉头,方向就是3个.
• 迭代计数iterations:0

  // PREDECESSOR AND SUCCESSOR INDEX
  int iPred, iSucc;
  float newG;
  // Number of possible directions, 3 for forward driving and an additional 3 for reversing
  int dir = Constants::reverse ? 6 : 3;
  // Number of iterations the algorithm has run for stopping based on Constants::iterations
  int iterations = 0;

4.2.2 设置一个二项堆BH来存放数据

关于二项堆的概念,可看这个博客:参考博客
关于二项堆用于排序的程序案例,可以看这个博客:参考博客.
在此处,二相堆在源代码中设置成了变量0,我觉得不太好辨认,就改称了BH.

此处BH的用途是:存放节点,并且按照C值的大小进行自动排序.本算法中,是按照C值升序排列.便于获取BH中节点的C值的最小值.

// OPEN LIST AS BOOST IMPLEMENTATION
typedef boost::heap::binomial_heap<Node3D*,boost::heap::compare<CompareNodes>> priorityQueue;
priorityQueue BH;

BH中使用的比较结构是CompareNodes,具体的含义是根据节点的C值升序排列.

struct CompareNodes {
  bool operator()(const Node3D* lhs, const Node3D* rhs) const {
    return lhs->getC() > rhs->getC();
  }
  bool operator()(const Node2D* lhs, const Node2D* rhs) const {
    return lhs->getC() > rhs->getC();
  }
};

4.2.3 初始化h值,BH的初始值以及节点指针

• 更新启发式H的值,H代表到达goal的预估值.
• 把start节点加入open表.
• 把start节点的指针存在BH中.
• 把前任节点的索引值iPred设置为start节点的索引值.
• 设置nodes3D的对应位置的元素为start.
• 定义指向前任节点指针为nPred,后继节点指针为nSucc

  // update h value
  updateH(start, goal, nodes2D, dubinsLookup, width, height, configurationSpace, visualization);
  // mark start as open
  start.open();
  // push on priority queue aka open list
  BH.push(&start);
  iPred = start.setIdx(width, height);
  nodes3D[iPred] = start;

  // NODE POINTER
  Node3D* nPred;
  Node3D* nSucc; nSucc;

• 关于索引值的计算公式(来自setIdx()函数):
  $$f_{idx}=(int)72t/(2\pi )\ast w\ast h+(int)y\ast h+(int)x$$
  其中,h为地图高度,w为地图宽度,x,y,t为start的坐标.
  至于为什么取这个值,笔者也没有搞明白,留下一个疑问.
  nodes3D的容量设置为:
  $$f_{idxMax}=h\ast w\ast 72$$
  注意到当t趋近于$2\pi$时,$f_{idx}$的第一项就已经接近$f_{idxMax}$了.此处本想证明一下$f_{idxMax}$一定大于等于$f_{idx}$.但是最终也没有数学证明出来,只是人工采点做了个实验:

案例:如果map尺寸为h=50,w=80,那么:$$f_{idxMax}=288000$$
此时,选start点为(79.8,49.65,6.24),此时start点位于地图的右上角,并且朝向接近于$2\pi$,计算出来的$f_{idx}$值为:
$$f_{idx}=287999$$
实验似乎证明了索引值最大也不会超过$f_{idxMax}$.

• 关于start坐标和goal坐标的取值范围
  $$x:[0,w]y:[0,h]t:[0,2pi)$$

4.2.4 操作BH,寻找goal节点

只要BH不为空,就一直循环进行4.2.4的操作,直到BH为空:

(1)取出最小代价值BH.top()作为前任节点nPred,并且设置该节点的索引值为前任节点的索引值iPred.

    // pop node with lowest cost from priority queue
    nPred = BH.top();
    // set index
    iPred = nPred->setIdx(width, height);
    iterations++;

(2)判断前任节点nPred是否已经在closed表中了,如果是,则从BH表中弹出,然后直接进行下一个循环.

    // _____________________________
    // LAZY DELETION of rewired node
    // if there exists a pointer this node has already been expanded
    if (nodes3D[iPred].isClosed()) {
      // pop node from the open list and start with a fresh node
      BH.pop();
      continue;
    }

(3)如果取出来的点不在closed表中,那么就进行下面的流程:

(3.1)把当前的节点添加到closed表中,然后从BH中弹出.

      // add node to closed list
      nodes3D[iPred].close();
      // remove node from open list
      BH.pop();

(3.2)测试是否是目标节点.
如果是目标节点,并且叠代次数iterations大于Constants::iterations(可设置为0),则直接返回nPred作为目标节点,退出循环4.2.4.

      // _________
      // GOAL TEST
      if (*nPred == goal || iterations > Constants::iterations) {
        // DEBUG
        return nPred;
      }

(3.3)如果不是目标节点,那么进行以下流程:

(3.3.1)如果同时满足以下3个条件:(a)配置参数使能 (b)nPred在goal节点的 10m范围内 ©nPred向车辆的前方规划.那么进行以下流程:

• 根据杜宾斯发射函数dubinsShot生成后继节点nSucc.
• 如果nSucc不为空,并且等于goal,那么返回nSucc,跳出循环4.2.4

        // _______________________
        // SEARCH WITH DUBINS SHOT
        if (Constants::dubinsShot && nPred->isInRange(goal) && nPred->getPrim() < 3) {
          nSucc = dubinsShot(*nPred, goal, configurationSpace);

          if (nSucc != nullptr && *nSucc == goal) {
            //DEBUG
            // std::cout << "max diff " << max << std::endl;
            return nSucc;
          }
        }

(3.3.2)遍历6个方向的后继节点,此处考虑倒车规划,所以dir=6.

for (int i = 0; i < dir; i++) {
运行(3.3.2.1)-(3.3.2.2)
}

(3.3.2.1)首先生成后继节点createSuccessor(i)并更新索引值

          // create possible successor
          nSucc = nPred->createSuccessor(i);
          // set index of the successor
          iSucc = nSucc->setIdx(width, height);

(3.3.2.2)而后确保nSucc在栅格上isOnGrid,并且是可访问的isTraversable,并且节点不在closed表上或者nSucc和iPred的索引相同.
isTraversable主要是根据该节点占用的栅格是否存在障碍物来判断.

if (nSucc->isOnGrid(width, height) && configurationSpace.isTraversable(nSucc)) {

            // ensure successor is not on closed list or it has the same index as the predecessor
            if (!nodes3D[iSucc].isClosed() || iPred == iSucc) {
	    (3.3.2.2.1)
	    }else { delete nSucc; }	    
else { delete nSucc; }

(3.3.2.2.1)计算移动代价值

              // calculate new G value
              nSucc->updateG();
              newG = nSucc->getG();

如果nodes3D[iSucc]不在open表里面,或者iSucc和iPred的索引值相同,或者后继节点的移动代价小于之前的移动代价:

              // if successor not on open list or found a shorter way to the cell
              if (!nodes3D[iSucc].isOpen() || newG < nodes3D[iSucc].getG() || iPred == iSucc) {
{ 运行(3.3.2.2.1.1)}
	      else { delete nSucc; }

(3.3.2.2.1.1)使用新的后继节点更新预估值H.
如果后继和前任是同一个,并且后继的C值比前任的C值大,那么就意味着后继没有价值,所以删除后继delete nSucc,返回(3.2.2)判断下一个方向的后继.

如果后继和前任是同一个,并且后继的C值比前任的C值小,那么就意味着新的后继价值更高,所以设置后继的前任为前任的前任nSucc->setPred(nPred->getPred());

如果后继的前任等于该后继本身,那么搜索树中存在环了.输出错误语句looping.

最后,将后继加入open表中,并更新后继节点向量,将该节点添加到BH中,然后删除临时变量nSucc,返回(3.2.2)判断下一个方向的后继.

                // calculate H value
                updateH(*nSucc, goal, nodes2D, dubinsLookup, width, height, configurationSpace, visualization);

                // if the successor is in the same cell but the C value is larger
                if (iPred == iSucc && nSucc->getC() > nPred->getC() + Constants::tieBreaker) {
                  delete nSucc;
                  continue;
                }
                // if successor is in the same cell and the C value is lower, set predecessor to predecessor of predecessor
                else if (iPred == iSucc && nSucc->getC() <= nPred->getC() + Constants::tieBreaker) {
                  nSucc->setPred(nPred->getPred());
                }

                if (nSucc->getPred() == nSucc) {
                  std::cout << "looping";
                }

                // put successor on open list
                nSucc->open();
                nodes3D[iSucc] = *nSucc;
                BH.push(&nodes3D[iSucc]);
                delete nSucc;

附1:Dubinsshot()

(1)初始化路径和点

  // start
  double q0[] = { start.getX(), start.getY(), start.getT() };
  // goal
  double q1[] = { goal.getX(), goal.getY(), goal.getT() };
  // initialize the path
  DubinsPath path;
  // calculate the path
  dubins_init(q0, q1, Constants::r, &path);

获取Dubins曲线长度,并初始化Dubins节点向量

  int i = 0;
  float x = 0.f;
  float length = dubins_path_length(&path);

  Node3D* dubinsNodes = new Node3D [(int)(length / Constants::dubinsStepSize) + 1];

Dubins节点的长度设置为:(int)(length / Constants::dubinsStepSize) + 1

(2)遍历每一个Dubins节点
(2.1)障碍物检测函数isTraversable
其中关键函数是isTraversable函数中的configurationTest函数,它位于collisiondetection.cpp中,作用是检测某个目标点(x,y,t)是否有障碍物冲突,如果返回true,说明没有冲突.isTraversable也会返回true.

while (x <  length) {
    double q[3];
    dubins_path_sample(&path, x, q);
    dubinsNodes[i].setX(q[0]);
    dubinsNodes[i].setY(q[1]);
    dubinsNodes[i].setT(Helper::normalizeHeadingRad(q[2]));

    // collision check
    if (configurationSpace.isTraversable(&dubinsNodes[i])) {

      // set the predecessor to the previous step
      if (i > 0) {
        dubinsNodes[i].setPred(&dubinsNodes[i - 1]);
      } else {
        dubinsNodes[i].setPred(&start);
      }

      if (&dubinsNodes[i] == dubinsNodes[i].getPred()) {
        std::cout << "looping shot";
      }

      x += Constants::dubinsStepSize;
      i++;
    } else {
      //      std::cout << "Dubins shot collided, discarding the path" << "\n";
      // delete all nodes
      delete [] dubinsNodes;
      return nullptr;
    }
  }

(3)如果能顺利遍历完毕而没有return,说明生成的杜宾斯曲线上所有的节点都是无碰撞的,或者说都是可通过的,那么就返回最后一个节点

// std::cout << "Dubins shot connected, returning the path" << "\n";
return &dubinsNodes[i - 1];

附2:关于点的prim属性

node3d.cpp->Line66:

return new Node3D(xSucc, ySucc, tSucc, g, 0, this, i);

其中,i表示的是拓展的第i个后继节点.如果考虑不倒车,i的取值范围是(0,2),如果考虑倒车,那么i的取值范围是(0,5).