Sampling Heuristics for Optimal Motion Planning in High Dimensions论文总结

该论文有两点创新
1、LOCAL_BIAS
对局部路径优化，随机选取一个节点q，其子节点为q1，父节点为q2，该算法是找到q1->q
->q2的最短路径，当q在q1到q2连线的中点时cost最小。

算法中的rmin=0.1，rmax=2，也可根据实际需要自行调整

2、Node Rejection
节点拒绝，对于不可能比当前cost小的节点拒绝，即q_start->q->q_goal的cost没有当前的好，这样可以进一步减小就算量。

注意：LOCAL_BIAS和Node Rejection均是在RRT*找到solution后对路径进行优化

上图中的p为在未找到Path时偏向goal采样的概率，β为在找到Path后对局部进行优化的概率，p和β可自行设置