Bidirectional Polytope Trees to Optimal Formation Path Planning for Reconfigurable Modular Robots

Formation path planning for reconfigurable modular robotic systems involves multiple continuous collision-free configuration spaces, due to the variations of the formation arrangements. This paper proposes Bidirectional Polytope Tree* (BPT*) for formation path planning. Different from classical ones, each node in BPT* is defined as the intersection between two sampled polytopes in the obstacle-free workspace. The requirement of the inter-node connectivity makes the polytope sampling inefficient, this paper introduces Convex Polytope Generative Models (CPGMs) that take in as partial inputs the low-dimensional encoding of the nearby obstacles. By estimating the upper and lower bounds of the distances between connectable nodes, the candidates of node paths are found via branch-andbound. These candidates are converted into quadratic optimization problems with convex constraints, which are efficiently solved for the optimal formation path. The asymptotic optimality of BPT* is discussed, and the computational complexity is shown linear to the Lebesgue measure of the workspace space. Numerical simulations in cluttered environments have demonstrated that CPGMs and the two-tree structure in BPT* both improve the planning efficiency, and BPT* is at least 10 times more efficient than constrained optimization methods