TPS-A*: Path Planning for Any-Shape Vehicles with Certified Collision Checking in Trajectory Parameter Space
Organizations: Department of Engineering, CIESOL, CeiA3. University of Almería, 04120, Almería, Spain. · Teladoc Health, Santa Barbara, CA 93117, USA. · Department of Informatics, CIESOL, CeiA3. University of Almería, 04120, Almería, Spain.
Abstract
Global planners for ground vehicles usually approximate the vehicle by a disk, trading safety against conservativeness, while exact-footprint checking pays for every candidate motion, pose, and footprint edge. We present TPS-A*, a lattice planner whose edges are kinematically feasible trajectories from parameterized trajectory generators, built on certified collision checking in trajectory parameter space. A grid precomputed per footprint returns, in one pass over the local obstacles, a certified free distance along every candidate trajectory, so the per-node collision cost is independent of footprint complexity and trajectory count. We prove that this grid never overestimates the free distance of the continuously swept, possibly non-convex footprint, and state the planner's guarantees: collision-free paths and bounded suboptimality relative to its lattice abstraction. Goals with or without heading are reached exactly by Reeds-Shepp shots certified as trajectories of the same generators. An exact independent test finds no collision in its paths. On 300 cluttered worlds, with the true footprint and the same goal pose for all planners, it solves every query with a collision-free goal pose in a median of three milliseconds on an AMD Ryzen 7 2700X, on par with hybrid A*, 45% of whose paths overlap obstacles, and nearly sixty times faster than a state-lattice planner, the only other collision-free one; with position goals, eighty-nine times faster than hybrid A*. It matches hybrid A* in automated parking and on narrow doorways, docks a non-convex fork footprint that no convex model admits, and drove a real robot across a university campus among people.
Figures & tables
| Planner | Collision model | Cost per node | Guarantees |
|---|---|---|---|
| State lattice (SBPL) | swept cells per primitive | complete on a fixed graph, -suboptimal | |
| Hybrid-A* (Nav2 Smac) | outline rasterized per pose | representative pruning | |
| RRT*/BIT* + FCL (OMPL) | exact polygon, broadphase | probabilistic, anytime | |
| Swept-voxel roadmap [ 20 ] | voxels per roadmap edge | , all edges | those of the fixed roadmap |
| TP-Space RRT [ 7 ] | TP-Space grid, per trajectory | per trajectory | probabilistic |
| TPS-A* (ours) | certified TP-Space grid, batched | , indep. of , | certified; -suboptimal w.r.t. abstraction |
| Variant | Success | Time [ms] | Expansions | |
|---|---|---|---|---|
| median | max | median | ||
| Reference | 100% | 1.9 | 691 | 12 |
| 100% | 1.9 | 754 | 12 | |
| 99.7% | 1.9 | 673 | 12 | |
| Forward PTG only | 99.7% | 1.6 | 1491 | 27 |
| No analytic expansion | 100% | 2.3 | 692 | 25 |
| Planner | Success | Verified | Time [ms] | Len. | Clr. | |
| median | mean | [m] | [cm] | |||
| Shared goal pose | ||||||
| TPS-A* (ours) | 92.3% | 92.3% | 2.9 | 24.1 | 7.34 | 5.9 |
| TPS-A*, no shot | 92.3% | 92.3% | 68.2 | 142 | 8.42 | 7.6 |
| TPS-A*, costmap | 92.3% | 92.3% | 90.6 | 606 | 7.48 | 9.7 |
| Nav2 Hybrid-A* (Jazzy) | 100% | 54.7% | 5.4 | 10.3 | 7.11 | 1.4 |
| Planner | Solved | Verif. | Time | Len. | Cusps | Goal err. |
|---|---|---|---|---|---|---|
| s | [s] | [m] | [m / ∘ ] | |||
| TPS-A* (ours) | 19/19 | 0.76 | 19.2 | 3 | 0.11 / 4.7 | |
| Nav2 Hybrid-A* | 10/19 | 1.61 | 18.8 | 4 | 0.12 / 2.5 | |
| Nav2, no smoother | 19/19 | 1.76 | 18.9 | 4 | 0.12 / 2.5 |
| Run | Dist. | Duration | Paths | Upd. | First | Clear. | Goal error |
|---|---|---|---|---|---|---|---|
| [m] | [s] | [s] | [m] | [cm] / [ ∘ ] | |||
| 1 | 21.9 | 45.7 | 1 | 10 | 0.4 | 1.02 | 6.7 / 2.0 |
| 2 | 22.7 | 45.9 | 1 | 16 | 0.7 | 0.45 | 6.3 / 10.4 |
| 3 | 33.7 | 68.1 | 1 | 26 | 0.4 | 0.28 | 12.2 / 10.6 |
| 4 | 15.3 | 30.4 | 1 | 7 | 0.2 | 1.39 | 7.6 / 5.1 |
| 5 | 38.6 | 87.0 | 1 | 15 | 0.2 | 0.38 | 12.5 / 6.6 |
Motion Planning
Motion planning finds safe, feasible paths and trajectories for robots and vehicles amid geometric, dynamic, and task constraints.
See all 440 papers