sdssd

1：q_rand ,q_near ,q_new ←∅；
2：for i=1 to k
3：按一定的概率设置q_rand←q_goal或在map中随机生成q_rand;
4：q_near←findQNear(q_rand ,vertices);//在q_rand 附近找到距离其最近的q_near.
5：q_new←findQNew(q_near ,q_rand ,deltaq);//生成沿 q_near和q_rand 方向上，距q_near 为deltaq的q_new；
6：对q_new到q_near做碰撞检测；
7：if 没有碰撞
8： Vertices←Vertices∪{q_new}；
9： Edges←Edges∪{q_new,q_near}；
10：if q_new=q_goal or q_new 和_near 将q_goal 包围
11：path←fillSolutionPath(edges,vertices);//将Edges连接起来，即为生成的路径。
12：endif
13：endif
14：endfor
15：pathSmooth←smooth(map,path,vertices,delta);//使用贪心算法提取缩短后的路径。
16：Return T；

qrand , qnear , qnew ←∅； for(i=0 to 途经点数目）{     qstart=qi;     qend=RRT(qstart,qend,vertices,delta);}RRT(qstart,qend,vertices,delta){ qrand ,qnear ,qnew ←∅；  for i=1 to k    //按一定的概率设置qrand←qgoal或在map中随机生成qrand;    qnear←findQNear(qrand ,vertices);//在qrand 附近找到距离其最近的qnear.    qnew←findQNew(qnear ,qrand ,deltaq);//生成沿 qnear和qrand 方向上，距                                                                            //qnear 为deltaq的qnew；     if qnew qnear没有碰撞        Vertices←Vertices∪{qnew}；        Edges←Edges∪{qnew,qnear}；        if qnew=qgoal or qnew 和near 将qgoal 包围           path←fillSolutionPath(edges,vertices);//将Edges连接起来，即为生成的路径。        endif     endif  endfor  pathSmooth←smooth(map,path,vertices,delta);//使用贪心算法提取缩短后的路径。  Return T； }