RRT和RRT*路径规划算法 - dyy5091821/Algorithm GitHub Wiki

RRT算法

算法简介

  通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划。
  缺点：概率完备但不最优。

RRT伪代码

function RRT(qinit, K dq)                         // 根节点qinit  
    T.init(qinit);                                // 初始化起始点  
    for k = 1 to K  
        qrand = Sample();  // 随机生成一个点  
        qnearest = Nearest(T, qrand);             // 在T的节点中找一个距离qrand最近的节点--qnearest  
        if Distance(qnearest, qgoal) < Threshold  // 如果最近点距离目标点很近(距离小于阈值)，认为成功  
            return true;    
        qnew = Extend(qnearest, qrand, dq);       // 连接qnearest和qrand，在连线上选取一点qnew(qnew距离qnearest的距离为dq)  
        if qnew != NULL                           // 如果qnew不为空，将qnew添加到树中(T)  
            T.AddNode(qnew);  
    return false;  

function Sample()  
    p = Random(0, 1.0);  
    if 0 < p < Prob  
        return qgoal;  
    else if Prob < p < 1.0  
        return RandomNode();

归纳RRT算法步骤

  1. 初始化随机树，初始化的随机树只包含一个节点即根节点--qinit
  2. 随机产生一个采样点--qrand
  3. 遍历树T，从中找到一个距离采样点qrand最近的点--qnearest
  4. 通过扩展函数Extend，从qnearest向qrand扩展一段距离dq，得到一个新的节点--qnew
  5. 检查，如果qnew与障碍物有碰撞，函数Extend返回NULL，则放弃这次生长；否则将新节点qnew加入到随机树T中
  6. 判断，如果最近点与目标点距离非常近(距离小于设定阈值)，随机树到达了目标点
    **
       为了加快随机树到达目标点的速度，简单的改进方法是：在随机树每次的生长过程中，根据随机概率来决定qrand是目标点还是随机点。
       在函数Sample中设定参数Prob，每次得到一个介于0和1.0的随机值，当0 < p < Prob时，随机树朝目标点生长；当Prob < p < 1.0时，随机树朝一个随机方向生长。

RRT*算法

RRT*算法图解

算法图解
改进RRT算法--路径规划

算法简介

  1. RRT*算法能快速的找出初始路径，之后随着采样点的增加，不断地进行优化直到找到目标点或者达到设定的最大循环次数。
  2. RRT*算法是渐进优化的，也就是随着迭代次数的增加，得出的路径是越来越优化的，而且永远不可能在有限的时间中得出最优的路径。因此换句话说，要想得出相对满意的优化路径，是需要一定的运算时间的。
  3. 所以RRT*算法的收敛时间是一个比较突出的研究问题。
  4. 但不可否认的是，RRT*算法计算出的路径的代价相比RRT来说减小了不少。

算法对比

相比较于RRT算法，RRT*算法有两个方面的改进：

  1. 为xnew 重新选择父节点；
  2. 重新布线;

RRT*算法伪代码

第一部分与RRT算法相同

InitializeTree();                         // 初始化随机树T  
for k = 1 to K  
    xrand = RANDOM_STATE();               // 选取随机采样点xrand    
    xnear = NEAREST_NEIGHBOR(xrand, T);   // 遍历随机树T，寻找采样点xrand的最近点xnear  
    u = SELECT_INPUT(xrand, xnear);       // 选择输入量u  
    xnew = NEW_STATE(xnear, u, dt);       // 生成新的节点xnew  
    if obstaclefree(xnew)                 // 判断是否碰撞  
        return xnew;

第二部分为xnew重新选择父节点

xnear_neighbor = findnear_neighbor(T, xnew, r);  // 遍历随机树T，寻找以xnew为圆心r为半径内的所有点为xnew的临近点集xnear_neighbor  
Chooseparent(xnew, xnear_neighbor, T);           // 在临近点集xnear_neighbor中选择xnew的父节点  
for each xnear_neighbor                          // 对于临近点集xnear_neighbor中的每一个点
    expense = calculate(dist(xnew, xnear_neighbor) + cost(xnear_neighbor, xinit));  // 计算费用  
    xnear_neighbor_mincost = min(expense);       // 选出费用最小的点xnear_neighbor_mincost  
    xnew_parent = xnear_neighbor_mincost;        // 费用最小的点xnear_neighbor_mincost就是xnew的父节点xnew_parent  
return xnew_parent;

第三部分重新布线

rewire(T, xnew, xnear_neighbor);  // 为随机树T重新布线  
for each xnear_neighbor  
    expense1 = calculate(dist(xnear_neighbor, xnew) + cost(xnew, xinit));  
    expense2 = calculate(cost(xnear_neighbor, xinit));  
    if (expense1 < expense2)
        xnew_mincost = xnew;  
        xnear_neighbor_newparent = xnew_mincost;  
return xnear_neighbor_newparent;
第一部分 第二部分 第三部分
属于RRT算法部分，找到一个随机点xnew。 为xnew重新寻找父节点，为的是将xnew用最小成本连接到树T上。以xnew为圆心，选取半径r画圆,得到圆内所有点xnear_neighbor。计算圆内所有点的花费成本expense,选择花费expense成本最小的那本xnear_neighbor与xnew相连。 检查通过xnew到达xnear_neighbor其他点的成本是否比原来更小，是的话连接新线(连接xnew到xnear_neighbor的线，同时删掉xnear_neighbor与其原来父节点之间的连线。)

算法改进的意义

  1. 重新选择父节点： 使新生成的节点路径代价尽可能的小；
  2. 重新布线：使得生成新节点后的随机树减少冗余通路，减少路径代价；
⚠️ **GitHub.com Fallback** ⚠️