The naive L2 metric typically used in motion planning algorithms tends to lead to long query times and overly complex solutions for some "awkward" queries (example: upper-left figure; the starting and goal orientations are "right" and "down" facing, respectively). This is a result of antagonistic behaviour between minimization of the linear and angular configuration components, which pull the planner in opposite directions in such cases.
A much more robust metric can be derived empirically, in the process of answering queries using any arbitrary other metric: the new metric is defined as the minimum observed path arc length in any solutions returned (or parts thereof). The figure at top shows a very coarse metric derived by observing a car. In the remaining figures on the left we see the new metric in action. The first image shows the RRT step of deciding which tree node is closest to target (right car); the red numbers are the L2 metric while the new metric is in green. Second image/animation shows a gradient descent along the new metric from that point. This example demonstrates how the planner now picks a starting point sufficiently back, and performs the necessary anticipatory rightward "swerve" before attempting to match position and orientation. |
