Dynamic Path Planning
A preplanned path assumes a known start and a mostly known field. Dynamic planning starts from the current vision-corrected pose, finds a collision-free route to a target, and can replace that route when the field changes.
- Vision measurements accepted into drivetrain odometry.
- A basic and profiled drive-to-point command that finish reliably.
- The PathPlanner field model and navigation grid from Workshop 3.
The planner chooses geometry around obstacles. The follower turns that geometry into motion. The pose estimator tells both where the robot really is.
Replanning is a decision, not a reflex. A small pose correction should not throw away a good route; a blocked corridor or large deviation should.
Estimate, plan, follow, validate, and replan only when the current route is no longer safe or useful.
The five parts
- Current pose
- Swerve odometry supplies smooth motion; vision corrects drift. Reject stale or implausible measurements before planning.
- Obstacles
- The navigation grid describes fixed blocked space. Runtime detections add temporary obstacle bounds.
- Route
- The planner searches from the current translation to the goal and refines the path while keeping clear of blocked cells.
- Motion
- A profiled follower tracks the route within velocity and acceleration limits. Final heading can be handled separately from travel direction.
- Replan decision
- Watch route clearance, pose error, and target validity. Replace the route only when a defined condition crosses its threshold.
Write the replan policy
Choose measurable triggers rather than "replan whenever it looks wrong."
| Signal | Replan when | Do not replan for |
|---|---|---|
| Route clearance | A trusted obstacle intersects the remaining corridor. | An obstacle behind the robot. |
| Cross-track error | The robot remains outside a tolerance for a defined time. | One noisy pose sample. |
| Goal | The requested target moves or becomes invalid. | A new goal equal to the current goal within tolerance. |
Add hysteresis and a short delay
Use a stricter threshold to trigger replanning than to remain on the new route, and wait briefly before another replan. Without those two guards, noise can make the planner alternate between equally good routes every loop.
Travel, not alignment
Global planning is good at finding a clear route across the field. It is not the best tool for the last few centimeters at a scoring station. Plan to a staging pose with clearance, then hand control to the profiled drive-to-point command for the final approach. That keeps obstacle avoidance and precision alignment independently testable.
Test failures before testing speed
- Static route: start and goal with no runtime obstacles.
- Blocked corridor: add one obstacle and confirm the route clears the inflated boundary.
- Obstacle appears: introduce it after motion begins and verify one controlled replan.
- Obstacle disappears: verify the current safe route is not abandoned merely because a shorter one becomes available.
- Vision loss: confirm the system slows, stops, or continues under the written confidence policy.
- No route: verify the command finishes or fails safely instead of driving through blocked space.