28、基于采样的运动规划

基于采样的运动规划

1. 平衡双向搜索算法

在运动规划中，平衡双向搜索是一种有效的策略，它通过同时从起始点 ( q_I ) 和目标点 ( q_G ) 生长两棵树（RDT）来提高规划效率。以下是平衡双向搜索算法 RDT BALANCED BIDIRECTIONAL(qI, qG) 的代码：

RDT BALANCED BIDIRECTIONAL(qI, qG)
1 Ta.init(qI); Tb.init(qG);
2 for i = 1 to K do
3     qn ← nearest(Sa, α(i));
4     qs ← stopping - configuration(qn, α(i));
5     if qs ̸= qn then
6         Ta.add vertex(qs);
7         Ta.add edge(qn, qs);
8         q′n ← nearest(Sb, qs);
9         q′s ← stopping - configuration(q′n, qs);
10        if q′s ̸= q′n then
11            Tb.add vertex(q′s);
12            Tb.add edge(q′n, q′s);
13            if q′s = qs then return SOLUTION;
14        if |Tb| > |Ta| then SWAP(Ta, Tb);
15 return FAILURE

1.1 算法步骤