从RRT到RRT*，再到Informed RRT*，路径规划算法怎么写

做个正直的人

RRT中文名字是“快速搜索随机树”（Rapidly-exploring Random Tree），是一种比较常用的基于采样的静态路径规划算法。

我们可以这么理解：小明住在北京，小红住在南京，有一天小红给小明发了一条短信，短信上说只要小明从北京来南京见小红，小红就做小明的女朋友。单身青年小明一看给激动坏了，当即收拾东西要来南京。

1、RRT算法

1.1 假设

为了说明RRT算法，我们作出如下假设：

• 小明不知道从北京直达南京的路线；

• 在每一个城市，只能够获得到达相邻城市的路线，而不能到达不相邻的城市；

• 小明看不懂地图，只有一个列出了中国所有城市的城市清单。

1.2 RRT算法步骤与实现

小明的舍友甲为小明提出了一种找到从北京到达南京的算法——RRT算法。
小明只能拿着这个清单，重复如下过程：

    （1）随机选择一个城市，
    （2）看看这个城市是不是可达，
    （3）离南京（目标位置）多远，如果到达目标位置就终止算法
    （4）在已经选择过得城市中哪一个离它最近，就把那个城市设置为他的父节点。

不断的重复这个过程，这一个随机树就会不断的生长，直到到达目标位置。从下图可以看出经过许许多多次随机选择之后，目标城市就会变成这个随机树的叶节点，也就是说，小明就能找到一条从北京到南京的可行路径（仅仅是可行而已，很大概率上不会是最优）。

比如上面这幅图就是小明使用RRT算法找到的一条从北京到南京的路线。

1.3 伪代码

我们可以把RRT算法用伪代码的形式写出来，我认为伪代码用英语描述能够更加准确精炼的表达算法的步骤和精髓，所以我用英语来写伪代码：

//RRT算法伪代码——作者黄唐，2021.4.9于南京//initialization
Obtain map, 0 stands for free space and 1 obstacle.
Set start and goal in this map .
Set step_size which means how long we will search from the closest pos to the 
random pos obtained from sampling.(Why we need to set a step_size? Because the sample may be far away from our tree and we don't want to produce a very long edge.)
Set max iteration that means how many times we crop our tree.
Set current iteration as 0.
Set a flag which flags whether we have reached the goal.
Set goal_radius, if the distance from new_point to the goal is less than. 
goal_radius we think we have reached the goal.
If we don't set this goal_radius we may never reach the goal because the 
probability that we sample the goal is 0.
//make plan
We need a vector to memorize the position the we have detected
Push the start into the vector and set its parent as -1 and its index 0 which 
means it is the root of our tree while(we haven't reached the goal and the max iteration){
    Sample in the map and get a random pos as random_point
    Get the closest pos as closest_point in the vector(tree) whose distance to 
    random_point is min among all members in the vector
    Move from the closest_point to the random_point and produce a new point as 
    new_point in this edge whose distance to closest_point is step_size 
    if (the edge from closest_point to new_point is safe which means there is no 
        collision with any obstacle){
        Push the new_point into the vector and set its parent as closest_point
        current iteration ++
        if (the distance from new_point to goal < goal_radius){
            if (the edge from new_point to goal is safe){
                flag =true
                Set new_point as parent of the goal
            }
        }
        //RRT*算法的rewrite和relink写在这里哟
     }
     else{
        Abandon this sample and continue
     }
} 
return the index of new_point as the parent of the goal


//build plan 
if (the index of parent of the goal>=0){
    push the goal at the front of the path 
    current_parent =the index of parent of the goal
     while(the index of current_parent>0){
        Push the current_parent at the front of the path
        current_parent= index of parent of current_parent
     }
    Push start at the front of the path