Controllability-Based Waypoint Refinement

• Controllability-based waypoint refinement is defined by evaluating intermediate targets based on their executability rather than pure geometry.
• It employs varied techniques such as hierarchical parameterization, reachability-triggered activation, and vehicle-specific reranking to ensure paths are safe and dynamically feasible.
• This approach effectively bridges the gap between high-level decision making and low-level control, balancing shortest-path fidelity with execution simplicity under real-world constraints.

Recent literature suggests that controllability-based waypoint refinement denotes a class of methods in which waypoints are not treated as purely geometric intermediate targets, but are selected, parameterized, filtered, activated, or smoothed according to whether a downstream controller, motion model, or low-level planner can execute them under platform constraints. The term is usually implicit rather than explicit. In continuous navigation, it appears as the choice of waypoint representations that reduce control burden and execution time; in conditional driving models, as training procedures that make waypoint following robust without imposing hard deadlines; in model predictive control, as online activation of waypoint constraints only when they become reachable; in driving and vehicular planning, as motion-model- or vehicle-specific mechanisms that reshape waypoint generation toward dynamically plausible trajectories; and in formal methods, as admissibility or reachability filters on waypoint segments and intermediate states (Krantz et al., 2021, Lioutas et al., 17 Jan 2025, Beck et al., 2024, Liu et al., 2022, Rodríguez-Vidal et al., 5 Feb 2026, Shafa et al., 3 Oct 2025, Bohrer et al., 2019).

1. Conceptual scope and defining distinctions

A useful synthesis is that waypoint refinement becomes “controllability-based” when waypoint quality is evaluated by executability rather than by geometry alone. In the cited literature, executability is operationalized in several different ways: local feasibility under a low-level navigator, controller-specific execution time, adherence under a learned waypoint follower, reachability within an MPC horizon, guaranteed reachable sets under uncertain dynamics, or formally verified admissibility conditions for waypoint segments. This suggests that the topic is broader than classical controllability theory, even though classical notions of reachability and feasible steering remain its most rigorous edge cases (Shafa et al., 3 Oct 2025, Bohrer et al., 2019).

A central distinction separates hard waypoint satisfaction from soft waypoint guidance. Control-ITRA, for example, explicitly does not cast waypoint control as exact route tracking with deadlines: agents are “free to take any actions necessary to reach the target point safely and realistically,” and waypoints that cannot be safely reached should be ignored (Lioutas et al., 17 Jan 2025). By contrast, waypoint MPC for manipulators activates a waypoint as a hard terminal constraint only after the current prediction indicates that it is reachable within the horizon (Beck et al., 2024). Reachable Predictive Control goes further by choosing the next target only from a guaranteed reachable set constructed from locally learned dynamics (Shafa et al., 3 Oct 2025). These positions occupy different points on a spectrum from guidance, to constrained activation, to certified reachability.

A second distinction concerns formal controllability versus practical control-aware surrogates. Several papers are highly relevant to waypoint refinement but explicitly do not use controllability Gramians, Kalman rank tests, viability kernels, or reachability-set optimization. “Multiple Waypoint Navigation in Unknown Indoor Environments” uses adaptive path resolution, yaw correction, and turn-specific path pruning to improve MPC path tracking, but does not rank waypoints by formal controllability criteria (Sood et al., 2022). “Navigating the Clutter” similarly uses motion infeasibility feedback, search certification, and RRT execution outcomes as practical proxies for whether waypoint-induced motion is executable, without deriving a control-theoretic controllability metric (Ji et al., 22 Apr 2026). A common misconception is therefore that controllability-based waypoint refinement must always mean classical system-theoretic controllability; the literature shows a broader usage centered on reachable, safe, stable, or low-cost execution.

2. Waypoints as the interface between planning and control

Waypoint refinement matters because waypoints frequently mediate between high-level decision making and low-level actuation. In VLN-CE, the action-space study in “Waypoint Models for Instruction-guided Navigation in Continuous Environments” makes this interface explicit. Directly predicting low-level actions such as “move forward $0.25$m” or “turn $15^\circ$” couples long-horizon instruction grounding to many short-horizon control decisions; episodes are about 55 low-level actions long, and such trajectories induce many starts, stops, and turns on a robot. The paper therefore replaces micro-actions by a predicted relative waypoint, executed by a lower-level navigator, and studies a family of action spaces ranging from fixed-step heading control to full continuous waypoint prediction with action

$$\mathcal{A}_t = (\hat{\theta}^{D}, \hat{\theta}^{offset}, \hat{r}).$$

This is a paradigmatic refinement setting because semantic choice and executable motion are deliberately separated (Krantz et al., 2021).

The same interface appears in multi-robot and indoor-navigation stacks. “Multiple Waypoint Navigation in Unknown Indoor Environments” decomposes the pipeline into a global next-best waypoint selector, a local path planner, and an adaptive MPC controller. Static obstacle avoidance is handled by planning, while control handles path following and fast maneuvers (Sood et al., 2022). “Navigating the Clutter” places waypoints between a task planner and a motion planner: $W_i = \{w_{i,1}, \ldots, w_{i,K}\},$ with each waypoint sequence later converted into executable trajectories by a classical motion planner, typically RRT (Ji et al., 22 Apr 2026). In both cases, refinement can be understood as modifying or screening these intermediate motion anchors so that the control layer receives references it can actually realize.

Driving models expose an analogous waypoint–action interface. Control-ITRA conditions a recurrent multi-agent simulator on sequential waypoints and target speeds, but keeps trajectory generation action-based and closed-loop (Lioutas et al., 17 Jan 2025). “Addressing the Waypoint-Action Gap in End-to-End Autonomous Driving via Vehicle Motion Models” makes the interface differentiable by defining a lifting operator

$$F_\sigma : \mathbb{R}^{C_f \times d_u} \times S \to \mathbb{R}^{C_f \times 2},$$

which maps a sequence of low-level actions and an initial state to ego-frame future waypoints (Rodríguez-Vidal et al., 5 Feb 2026). This reframes refinement: instead of predicting free-form waypoints and asking a controller to approximate them later, the method requires waypoints to emerge from a bounded vehicle model.

3. Refinement mechanisms

The literature contains several distinct refinement mechanisms rather than a single canonical algorithm.

Mechanism	Representative formulation	Representative source
Hierarchical waypoint parameterization	coarse heading plus offset and distance	(Krantz et al., 2021)
Spatial condition extraction	sample waypoints at random spatial intervals	(Lioutas et al., 17 Jan 2025)
Reachability-triggered activation	split MPC horizon at $N_s$ when waypoint becomes reachable	(Beck et al., 2024)
Vehicle-specific reranking	$$p_e(k_{1:T}\mid O,c)\propto p(k_{1:T}\mid c)\exp(V)$$	(Liu et al., 2022)
Reachability-certified replacement	choose $x_f^{\hat n+1}\in R_x(T,x_0)$	(Shafa et al., 3 Oct 2025)
Motion-model-constrained decoding	roll out actions through $F_\sigma$ to obtain waypoints	(Rodríguez-Vidal et al., 5 Feb 2026)

A first family of mechanisms performs refinement inside the waypoint representation itself. In VLN-CE, the model first selects one of 12 panorama headings plus STOP, then refines that direction with a continuous or discrete heading offset and an optional distance prediction. The full waypoint is represented in relative polar coordinates as

$(\hat r,\hat \theta^D + \hat \theta^{offset}),$

so refinement already occurs as a coarse-to-fine decomposition of directional choice. This structure is especially relevant because the paper finds the largest performance differences in the heading-offset space, with continuous offsets outperforming discrete or fixed offsets by $15^\circ$0–$15^\circ$1 success (Krantz et al., 2021).

A second family performs refinement through conditioning design rather than test-time optimization. Control-ITRA compares last-timestep conditioning against a proposed waypoint sampling scheme in which waypoints are extracted along a ground-truth trajectory at random spatial intervals: $15^\circ$2 This removes the hidden instruction “reach this exactly at 4 seconds” and replaces it by route-progress supervision. In the paper’s own framing, controllability is improved not by adding a separate planner but by better training-condition extraction and sequential condition advancement (Lioutas et al., 17 Jan 2025).

A third family uses online reachability-aware activation. In waypoint MPC for manipulators, a waypoint initially appears only as a soft cost-to-go term. Once the current predicted trajectory shows that the waypoint can be reached within tolerance, the horizon is split at index $15^\circ$3, and the waypoint becomes a hard intermediate terminal set for the first sub-horizon (Beck et al., 2024). This is refinement in timing and enforcement rather than geometry: the waypoint location is fixed, but when it is imposed is continuously adjusted.

A fourth family reranks or rescales waypoints with controller-specific executability scores. “Vehicle Type Specific Waypoint Generation” starts from a generic probabilistic model $15^\circ$4, trains a vehicle-specific waypoint-following RL controller and value function $15^\circ$5, and then reweights sampled waypoint trajectories by exponentiated value. The refined posterior is written as

$15^\circ$6

with the practical approximation acting as importance reweighting by $15^\circ$7. This is not classical controllability, but it is a direct vehicle-conditioned followability filter (Liu et al., 2022).

A fifth family enforces refinement through explicit reachability or admissibility sets. Reachable Predictive Control first learns local dynamics online, constructs a guaranteed reachable set (GRS), and then chooses the next target only from that provably reachable set. The paper’s operational step is to “choose $15^\circ$8,” turning an arbitrary nominal path into a sequence of guaranteed reachable intermediate targets (Shafa et al., 3 Oct 2025). “A Formal Safety Net for Waypoint Following in Ground Robots” provides an even more conservative filter: waypoint segments are accepted only if they satisfy formal feasibility and invariant conditions such as

$15^\circ$9

which ensure that the robot can still reach the waypoint while obeying endpoint speed constraints under bounded delay and bounded actuation (Bohrer et al., 2019).

A sixth family treats refinement as geometric smoothing subject to control-oriented regularity requirements. “Directional Mollification for Controlled Smooth Path Generation” transforms a piecewise linear waypoint path into a $$\mathcal{A}_t = (\hat{\theta}^{D}, \hat{\theta}^{offset}, \hat{r}).$$0 path that exactly interpolates interior waypoints while retaining explicit curvature control. Its directional mollification operator

$$\mathcal{A}_t = (\hat{\theta}^{D}, \hat{\theta}^{offset}, \hat{r}).$$1

is not a dynamic feasibility model, but it is a control-enabling refinement primitive because path-following and trajectory-tracking controllers impose strict differentiability requirements, particularly for nonholonomic systems (González-Calvin et al., 23 Mar 2026).

4. Empirical trade-offs and evaluation criteria

The central empirical result across this literature is that waypoint refinement introduces a trade-off between shortest-path fidelity and execution simplicity. In VLN-CE, more expressive waypoint models produce simpler and faster-to-execute trajectories, while less expressive, lower-level stepping models can achieve better conventional navigation metrics by approximating shortest paths more finely. The paper reports that continuous-navigator waypoint models reduce average estimated execution time by 2.2× compared to low-level turn/forward models; the best WPN has EET lower than the best HPN by 144 seconds, nearly a 2× reduction; the best WPN attains $$\mathcal{A}_t = (\hat{\theta}^{D}, \hat{\theta}^{offset}, \hat{r}).$$2 of 6.9 cm/s versus 2.6 cm/s for the best HPN; and SCT rises from 11 to 23, despite lower SPL. At the same time, fixed-distance heading models generally outperform WPN counterparts in success by 2–3%, achieve higher SPL, and do so with approximately 4× more actions per trajectory (Krantz et al., 2021).

Control-ITRA shows an analogous trade-off between strict waypoint reach and realistic long-horizon control. On 4-second held-out prediction, last-timestep conditioning can appear superior, reaching waypoint reach rate 0.99 in both W and W/TS modes. But on 8-second rollouts, the same deadline-biased supervision causes unrealistic rushing: for W, collision rate rises to 0.29 with waypoint reach 0.96, whereas the proposed spatial waypoint sampling yields collision 0.03 with waypoint reach 0.77; for W/TS, last-timestep training gives collision 0.28 and waypoint reach 0.96, while the proposed scheme yields collision 0.02 and waypoint reach 0.84 (Lioutas et al., 17 Jan 2025). The resulting lesson is that refinement of waypoint supervision can improve controllability even when raw adherence decreases.

In end-to-end driving, the benefits of control-aware refinement appear most clearly in reactive and closed-loop settings. “Addressing the Waypoint-Action Gap” reports that unconstrained action-to-waypoint MLP mappings can remain competitive in non-reactive open-loop evaluation, but vehicle-model-constrained rollouts dominate when scene evolution depends on ego behavior. On NAVSIM navhard, reported EPDMS rises from 24.2 for MILE with FMLP to 28.2 with FKBM and 29.5 with FCCPP. On Bench2Drive, MILE improves from 51.7 ± 1.2 DS to 67.7 ± 1.2 with FKBM, while CIL++ improves from 43.6 ± 1.5 to 62.8 ± 0.8 with FKBM and 66.9 ± 1.6 with FCCPP; route completion for CIL++ rises from 52.0 ± 1.0 to 83.9 ± 0.1 or 83.8 ± 0.0 depending on the vehicle model (Rodríguez-Vidal et al., 5 Feb 2026). The paper interprets these gains as evidence that motion-model-constrained waypoint outputs remain more stable and drivable when feedback matters.

Vehicle-conditioned reranking shows the same phenomenon from a different angle. In “Vehicle Type Specific Waypoint Generation,” generic waypoint proposals are already highly executable for agile vehicles, but value-based posterior refinement is most beneficial for constrained platforms. For the truck at $$\mathcal{A}_t = (\hat{\theta}^{D}, \hat{\theta}^{offset}, \hat{r}).$$3, mean percentage of waypoints hit improves from $$\mathcal{A}_t = (\hat{\theta}^{D}, \hat{\theta}^{offset}, \hat{r}).$$4 in the prior to $$\mathcal{A}_t = (\hat{\theta}^{D}, \hat{\theta}^{offset}, \hat{r}).$$5 in the posterior; for the fire truck at $$\mathcal{A}_t = (\hat{\theta}^{D}, \hat{\theta}^{offset}, \hat{r}).$$6, it improves from $$\mathcal{A}_t = (\hat{\theta}^{D}, \hat{\theta}^{offset}, \hat{r}).$$7 to $$\mathcal{A}_t = (\hat{\theta}^{D}, \hat{\theta}^{offset}, \hat{r}).$$8 (Liu et al., 2022). This suggests that refinement is especially valuable when control authority is limited or vehicle dynamics are restrictive.

5. Formal safety, reachability, and system-theoretic interpretations

The most rigorous end of the spectrum appears in methods that make waypoint refinement depend on verified admissibility or provable reachability. The formally verified safety net for Dubins-type robots models waypoint following as a hybrid program and proves both safety and liveness. Its invariant

$$\mathcal{A}_t = (\hat{\theta}^{D}, \hat{\theta}^{offset}, \hat{r}).$$9

functions as a conservative inner approximation of the state set from which the waypoint remains reachable while respecting terminal speed bounds. A waypoint segment is not accepted merely because it is geometrically near; it must lie inside a verified controllable tube determined by curvature, tolerance, delay, and acceleration limits (Bohrer et al., 2019).

Reachable Predictive Control generalizes the same principle to unknown nonlinear dynamics. Starting from

$W_i = \{w_{i,1}, \ldots, w_{i,K}\},$0

the algorithm perturbs the system to learn $W_i = \{w_{i,1}, \ldots, w_{i,K}\},$1 and $W_i = \{w_{i,1}, \ldots, w_{i,K}\},$2, uses local Lipschitz growth bounds to construct a conservative proxy system, computes a guaranteed reachable set, and then selects the next intermediate target only from states analytically proven reachable for all dynamics compatible with the local information (Shafa et al., 3 Oct 2025). In this sense, waypoint refinement becomes a receding-horizon projection of the nominal path onto a locally certified reachable set.

Waypoint MPC for manipulators occupies a middle position between soft guidance and hard certification. It does not compute explicit reachable sets or controllability matrices, but it does refuse to harden a waypoint until the current predicted trajectory reaches it within tolerance under the model and constraints. Its horizon-splitting rule around index $W_i = \{w_{i,1}, \ldots, w_{i,K}\},$3, together with shrinking horizons once waypoint and goal become reachable, operationalizes a practical principle: do not enforce an intermediate target before the current system state and control authority support it (Beck et al., 2024).

By contrast, several influential waypoint papers are explicitly not formal controllability methods. The indoor multiple-waypoint framework uses adaptive MPC-compatible path shaping—adaptive resolution, shorter-turn logic, yaw fixing, and local waypoint filtering after upcoming turns—to improve tracking, but it does not optimize waypoint positions using controllability, reachability, or energy criteria (Sood et al., 2022). Directional mollification is likewise not a reachability method, but it addresses a different necessary condition for controlled execution: a piecewise linear path with corner singularities is not a suitable reference for many controllers. Its curvature expression

$W_i = \{w_{i,1}, \ldots, w_{i,K}\},$4

makes curvature-bounded exact waypoint interpolation analytically accessible, which is relevant for nonholonomic path following even though it is not a controllability test (González-Calvin et al., 23 Mar 2026).

6. Limitations, misconceptions, and open directions

A recurrent limitation is that many systems relevant to controllability-based waypoint refinement still delegate feasibility to the waypoint predictor instead of checking it explicitly. In VLN-CE, the continuous navigator simply turns and moves straight to the predicted waypoint and “does no obstacle avoidance,” so local collision freedom is only implicit in the predictor (Krantz et al., 2021). Control-ITRA deliberately treats waypoints as soft guidance and allows unsafe or unreachable ones to be ignored rather than repaired (Lioutas et al., 17 Jan 2025). Waypoint MPC for manipulators uses soft collision penalties rather than hard collision constraints, and Reachable Predictive Control remains conservative and local because its guarantees depend on short-horizon proxy reachable sets and known Lipschitz bounds (Beck et al., 2024, Shafa et al., 3 Oct 2025). A plausible implication is that many current systems would benefit from an explicit waypoint-refinement layer between high-level prediction and low-level execution.

Another common misconception is that exact waypoint adherence is always desirable. The literature shows the opposite: last-timestep training in driving can maximize short-horizon reach while producing unsafe rushing; lower-level step controllers in VLN can improve SPL while causing approximately 4× more actions; and classical mollification can preserve convex-hull containment at the cost of losing exact waypoint interpolation (Lioutas et al., 17 Jan 2025, Krantz et al., 2021, González-Calvin et al., 23 Mar 2026). Controllability-based refinement therefore concerns which waypoint properties should be preserved and which should be relaxed. In some settings the correct behavior is to hit the waypoint exactly; in others, to reach it only when safe; in others, to replace it by a nearby executable surrogate.

The literature also leaves several open design variables. One is where refinement should occur: in the representation itself, as in hierarchical heading–offset–distance models; in data construction, as in spatial waypoint sampling; in a vehicle-specific posterior over candidate trajectories; in a model-based decoder; or in a runtime-certified reachable-set selector (Krantz et al., 2021, Lioutas et al., 17 Jan 2025, Liu et al., 2022, Rodríguez-Vidal et al., 5 Feb 2026, Shafa et al., 3 Oct 2025). Another is what metric should drive refinement: success and SPL, estimated execution time, SCT, collision rate, waypoint reach, target-speed reach, route completion, curvature bounds, or formal safety invariants (Krantz et al., 2021, Lioutas et al., 17 Jan 2025, Bohrer et al., 2019, Rodríguez-Vidal et al., 5 Feb 2026). The papers do not converge on a single criterion; instead, they show that controllability-aware refinement is inherently multi-objective.

The clearest overall pattern is that waypoint refinement becomes more valuable as the gap widens between high-level intent and low-level realizability. In navigation this gap is created by continuous motion and robot-specific execution time; in driving by nonholonomic vehicle dynamics and aggressiveness modulation; in manipulation by short MPC horizons and dynamically changing intermediate goals; in multi-robot planning by motion infeasibility and collision coupling across planning levels; and in formal methods by the need to preserve invariants under delay and bounded actuation (Krantz et al., 2021, Lioutas et al., 17 Jan 2025, Beck et al., 2024, Ji et al., 22 Apr 2026, Bohrer et al., 2019). The concept of controllability-based waypoint refinement is therefore best understood not as a single algorithm, but as an organizing principle: intermediate motion anchors should be chosen, transformed, or validated according to what the actual closed-loop system can safely and efficiently realize.