|
/* RRT:快速扩展随机树算法 始点作为跟节点，通过随机采样增加叶子节点的方式，生成一个随机扩展数， 当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中 找到一条由从初始点到目标点的路径。 */ FunctionRRTPlan：BOOL （env：environment，T：RRTTree，qgoal：node） Var qtarget，qnearest，qnew：node While（searchtime/spaceremaining）do qtarget=ChooseTarget（qgoal） qnearest=Nearest（T，qtarget） If（Distance（qnearest，qgoal）<DistanceThreshold）then Returntrue qnew=Extend（qnearest，qtarget） If（qnew≠NULL）then T.AddNode（qnew） Returnfalse Function ChooseTarget（qgoal：node）：node Varp：real p=Random（0，1.0） if0<p<GoalProbthen Returngoal ElseifGoalProb<p<1.0then ReturnRandomNode（）； /* RRT connect:连接型RRT 该算法一开始同时从初始状态点和目标状态点生长两棵树随机树， 每一次迭代过程中，其中一棵树进行扩展，尝试连接另一棵树的 最近节点来扩展新节点。 然后，两棵树交换次序重复上一迭代过程。 这种双向的RRT技术具有良好的搜索特性，相比原始快速扩展随机树算法 在搜索速度、搜索效率有了显著提高。 */ |
|
|
