Pith. sign in

REVIEW 3 major objections 3 minor 1 cited by

Robustified Time-optimal Point-to-point Motion Planning and Control under Uncertainty

T0 review · 3 major / 3 minor · reviewed 2026-08-10 · deepseek-v4-flash

Challenge this review Re-run · record.json Download PDF Read on arXiv ↗ Review JSON

Pith's one-line read Time-optimal point-to-point motion under process noise can be planned and controlled in one robustified two-stage optimal-control problem, solved online by alternating Riccati recursion with a nominal time-optimal OCP.

desk verdict A credible two-stage robust time-optimal motion planning integration with a real gap: stage-2 safety margins are not backed by covariance propagation. read the letter →

arxiv 2501.14526 v1 pith:CRG6ZS4S submitted 2025-01-24 cs.RO cs.SYeess.SY

Shuhao Zhang , Jan Swevers This is my paper · ORCID
classification cs.ROcs.SYeess.SY
keywords time-optimalcontrolmotionplanningunderuncertaintytwo-stageoptimalRiccatirecursionmodelpredictivechanceconstraintsfeedbackmobilerobot
verification ladder T0 review T1 audit T2 compute T3 formal

The pith

A machine-rendered reading of the paper's core claim, the machinery that carries it, and where it could break.

The reading

This paper argues that time-optimal point-to-point motion under process noise can be planned and controlled in one robustified two-stage optimal-control problem, solved fast enough to be replanned online. Stage 1 uses a fixed time grid and optimizes the nominal trajectory together with feedback gains and state covariances, shrinking uncertainty before a variable-time stage 2 that minimizes the remaining motion time. Constraints in both stages are robustified with safety margins derived from those covariances, replacing heuristic margins with a chance-constraint rationale. A tailored algorithm alternates a Riccati recursion for the feedback gains with a nominal time-optimal OCP, and an asynchronous replanning loop keeps the plan feasible. In the paper's numerical example the replanned motion takes 5.22 s versus 5.2 s for single planning, with every replanning solve finishing within the 0.6 s stage-1 horizon.

What carries the argument

The load-bearing object is the two-stage OCP (6), whose first stage has a fixed time grid and optimizes nominal states and controls, feedback gains $K_1$, covariances $\Sigma_1$, and constraint variances $\beta_1$, while the second stage has a variable time grid and minimizes total time $T_2$. The robustification mechanism is the covariance recursion $\Sigma_{n+1} = \Phi(\bar{s}_n, \bar{u}_n, K_n, \Sigma_n)$ from linearization around the nominal trajectory, together with the constraint backoff $h + \sigma\sqrt{\beta} + \epsilon \le 0$ where $\beta$ is computed from the linearized constraint and the current covariance. The algorithm that carries the tractability claim is the alternation of a Riccati recursion for feedback gains and a nominal two-stage OCP with frozen safety margins and gradient corrections, iterating until the KKT conditions of the coupled problem are met.

What would settle it

Run the planned stage-2 motion on the real nonlinear system, or in Monte Carlo simulation, with the assumed process noise and count how often the obstacle constraint or control limits are violated; if the violation rate exceeds the chosen probability level $p = 1 - C(\sigma)$ during the variable-time stage, the frozen-margin premise is refuted. A targeted version would lengthen $T_2$ (for instance, by a more distant target) and check whether violation frequency grows with stage-2 duration.

Watch

Extended reading notes

Core claim

On its own terms, the paper establishes that the robustified two-stage OCP (6)—a fixed-grid stage 1 with optimized gains and covariances stitched to a variable-time stage 2 whose total time is minimized—is a tractable formulation for time-optimal motion planning and control under Gaussian process noise, and that a decoupled solver can handle it online. The solver alternates between a Riccati recursion that computes optimal feedback gains for the current nominal trajectory and a nominal two-stage OCP armed with frozen safety margins, iterating until the KKT conditions of the full problem are met. Embedded in an ASAP-MPC replanning loop, the approach keeps constraints robustified without a separate tracking layer. In the reported unicycle example, timely replanning reaches the target in 5.22 s while single planning takes 5.2 s, and each replanning solve completes within the 0.6 s stage-1 horizon.

Load-bearing premise

The paper assumes that the uncertainty at the end of stage 1 remains a valid measure of the uncertainty during the entire variable-time stage 2: the stage-2 safety margins freeze the stage-1 terminal covariance $\Sigma_{1,N_1}$ and feedback gain $K_{1,N_1-1}$, so if process noise accumulates more as the second stage runs longer, the constraints could be under-protected.

Editorial extensions

If this is right

  • Time-optimal motion planning and feedback control are solved in one OCP, so the planned trajectory, feedback gains, and covariance are consistent instead of being designed separately.
  • Safety margins in the constraints come from propagated state covariances, giving a formal chance-constraint interpretation rather than heuristic tuning, and the margins shrink near activated obstacle constraints.
  • The tailored alternating solver makes the robustified OCP solvable in real time: each replanning in the example finishes within the 0.6 s stage-1 horizon.
  • Closing the loop via ASAP-MPC keeps feasibility under replanning delays; the final approach uses an exponentially weighted OCP to reach exactly the target.
  • The cost of robustness in total motion time is small in the example: 5.22 s with timely replanning versus 5.2 s for single planning.

Reading between the lines

Editorial extensions of the paper, not claims the author makes directly.

  • The frozen stage-2 covariance is the main extrapolation risk: if a longer variable-time stage 2 lets uncertainty grow substantially, the fixed safety margins could under-cover; one way to test this is to scale the process-noise covariance or lengthen the second-stage duration.
  • The same alternation of Riccati recursion and nominal OCP could be applied to other robustification schemes, such as tube-based or zero-order robust NMPC, potentially carrying the online-computability property to those settings.
  • The reported computation times depend on a specific laptop and solver settings, so the architecture's real-time claim should be re-verified on embedded hardware, a step the paper itself lists as future work.
Share X Bluesky LinkedIn Reddit HN

Signed reviews

No signed human review yet.

Editorial analysis

A structured set of objections, weighed in public.

Desk editor's note, referee report, and a circularity audit.

Referee Report

3 major / 3 minor

Summary. The paper proposes a robustified two-stage optimal control problem (OCP) for time-optimal point-to-point motion planning and control under Gaussian process noise. Stage 1 uses a fixed time grid and optimizes the nominal trajectory together with feedback gains and state covariances; stage 2 uses a variable time grid to minimize the remaining motion time. The stage-1 covariance and gain information is used to robustify stage and terminal constraints in both stages. The authors embed this OCP in an asynchronous MPC scheme with timely replanning and present an alternating solver that iterates between a Riccati recursion for the feedback gains and a nominal time-optimal OCP with frozen safety margins. A numerical example with a unicycle robot moving around an ellipsoidal obstacle reports a 5.22 s motion time under timely replanning versus 5.2 s for single planning, with each replanning solved within the 0.6 s stage-1 horizon.

Significance. If the safety argument were fully valid, the paper would offer a practically attractive framework for combining time optimality with constraint robustification at a computational cost suitable for online replanning. The decoupling into a Riccati subproblem and a nominal OCP is elegant, and the numerical example demonstrates plausible computation times. However, the stage-2 robustification as formulated does not propagate uncertainty through the variable-time stage, so the advertised chance-constraint guarantees are not established. Since this issue directly affects the central robustness claim, the paper in its current form cannot be accepted without substantial revision.

major comments (3)
  1. [Sec. II-C, Eqs. (6j)-(6l)] The stage-2 constraint variances are computed from stage-1 terminal data only: β2,n = H(¯s2,n, ¯u2,n, K1,N1−1, Σ1,N1−1) in (6j) and β2,N2 = Htf(¯s2,N2, Σ1,N1) in (6l). No covariance recursion is performed along the variable-time stage-2 grid, even though each stage-2 step covers roughly 1.5 s in the numerical example (N2 = 3, T2 ≈ 4.6 s). Over such an interval, process noise with the reported covariance can accumulate substantially, and Σ1,N1 does not by itself provide information about the covariance at later times. Moreover, using Σ1,N1−1 for the intermediate stage-2 constraints is not the covariance at the stitch point after the last stage-1 control. Consequently, the constraints in (6i) and (6k) are not calibrated to the actual closed-loop distribution, and the claimed σ-level chance constraints are not guaranteed. The authors should either propagate covariance through stage 2 (including the time-scaling map) or provide a rigorous argument, such as a dominant-tube bound, showing that the frozen stage-1 covariance is conservative for the entire stage-2 tail.
  2. [Sec. III, Algorithm 2] The alternating tailored solver is presented without a convergence proof. The stopping criterion "KKT conditions (10) meet threshold" assumes that the iterates approach a KKT point, but no theorem is given to show that the alternation between subproblem (14) and subproblem (16) converges, nor that a fixed point of the iteration satisfies the KKT conditions of the original problem (9). The numerical example shows convergence for one instance and one tolerance, which is not sufficient to support the paper's claim of a tailored algorithm for reliable real-time OCP execution. A local convergence analysis, or at least an explicit statement that convergence is heuristic and must be verified online, is needed.
  3. [Sec. II-C, Eq. (6a) and Sec. IV] The objective (6a) minimizes T2 together with weighted covariance traces, so the resulting "time-optimal" trajectory is only optimal for a particular choice of the regularization weights Rregu and Rregu_tf. These weights are free parameters, and they are chosen differently for single planning (Rregu = diag(80, 80, 80, 500, 500), Rregu_tf = 1000 I3) and for timely replanning (Rregu = I5, Rregu_tf = 50 I3) in the numerical section. Therefore the reported comparison of 5.2 s versus 5.22 s does not isolate the effect of the replanning strategy; it also reflects different objective tunings. The authors should either justify a canonical weight selection or present a Pareto-style discussion of the trade-off between motion time and state/control uncertainty.
minor comments (3)
  1. [Sec. II-C, Eq. (6h)] Equation (6h) reads ¯s2,n+1 = fT(¯s2,n, ¯s2,n, T2/N2), where the second argument appears to be a typo; based on the surrounding text and the discretized dynamics, it should depend on the control ¯u2,n, e.g., fT(¯s2,n, ¯u2,n, T2/N2).
  2. [Sec. III, Eq. (10)] The KKT stationarity condition for β is printed as µiσ 2√βi + ϵ − ηi = 0, which appears to have missing parentheses and a division; it should presumably be µi σ/(2 sqrt(βi + ϵ)) − ηi = 0.
  3. [Fig. 1 caption] The caption states that blue ellipses and red ellipses denote closed-loop and open-loop uncertainty, but the figure and text do not explain how the ellipse sizes relate to the covariances or to the probability level σ; adding a short explanation would improve interpretability.

Circularity Check

0 steps flagged · score 0.0 of 10

No significant circularity: the derivation is self-contained and the numerical results are demonstrations, not fitted predictions.

full rationale

The derivation chain is self-contained. The robustified two-stage OCP (6) is stated directly: stage-1 covariance recursion (6d) and constraint variances (6f) are defined by (3)-(5); stage-2 robustification (6j),(6l) is an explicit, if approximate, choice to freeze the stage-1 terminal gain and covariance, not a fitted value renamed as a prediction. The tailored solver iterates between the Riccati recursion (15) and the nominal OCP (16) using KKT conditions derived in (10)-(13); no step equates a target quantity to an input by construction. Citations [8], [17], and [18] are prior published components (two-stage OCP, ASAP-MPC) that are restated or used as building blocks, and [15], [19] are external; none is invoked as an unverified uniqueness theorem. The numerical example is a demonstration, not a test of a derived constant; hand-tuned regularization weights affect the reported times but do not make the reported 5.22 s a fitted prediction. The skeptical concern about stage-2 covariance freezing is a robustness/correctness limitation (the claimed safety margins may be underestimated), not a circularity: the OCP still minimizes T2 and the robustification terms are computed from the stated formulas rather than from the quantity being predicted.

Assumptions & free parameters 5 free parameters · 6 assumptions · 0 invented entities

No new physical entities, forces, or conserved quantities are introduced. The formulation uses standard ellipsoidal uncertainty tubes, feedback gains, time scaling, and robust constraint approximations from the cited literature. The main load-bearing axioms are the linearization accuracy and the stage-2 frozen-covariance assumption.

free parameters (5)
  • Regularization weights Rregu and Rregu_tf = Timely replanning: Rregu=I5, Rregu_tf=50 I3. Single planning: Rregu=diag(80,80,80,500,500), Rregu_tf=1000 I3.
    Positive-definite weights in objective (6a) and (7) balance time optimality against state and control covariance. The single-planning weights are described as 'appropriately chosen to ensure feasibility', so they are hand-tuned.
  • Stage-1 horizon N1 and stage-2 horizon N2 = N1=30, N2=3, ts=0.02 s
    Horizon lengths chosen by the user; N1*ts=0.6 s is deliberately set so that replanning completes within the stage-1 horizon, as stated in Sec. IV.
  • Chance-constraint factor sigma = 3
    Sigma in constraint robustification (4) sets the probability level through 1-C(sigma)=p. It is chosen by the user, not derived from the problem data.
  • Smoothing epsilon = 1e-8
    Small offset added under square roots in (6e), (6i), and (6k) to ensure differentiability at all feasible points. It is a numerical tuning constant.
  • Exponential weighting gamma = 1.015
    Used in the final OCP (8) and chosen to balance time optimality against numerical conditioning, per Sec. IV.
assumptions (6)
  • domain assumption Process noise w_n is zero-mean Gaussian with known covariance Sigma_w.
    Introduced in eq. (1) and used in the covariance recursion (3). The entire ellipsoidal uncertainty propagation and sigma-based chance constraint handling depend on this assumption.
  • domain assumption First-order linearization about the nominal trajectory is accurate enough for covariance propagation (3) and constraint variance computation (5).
    Uncertainty tubes and safety margins inherit approximation error from dropping higher-order terms. No error bound or nonlinearity measure is provided.
  • domain assumption A Gaussian CDF factor sigma with variance beta computed by linearization yields valid chance constraints.
    Constraint (4) treats h_i as approximately Gaussian. The true distribution of a nonlinear constraint under nonlinear dynamics is unknown, so the probability guarantee is approximate.
  • ad hoc to paper Stage-2 constraints can be robustified with the stage-1 terminal covariance and gain without propagating uncertainty through stage 2.
    Constraints (6j) and (6l) set Beta_2 from K_1,N1-1 and Sigma_1,N1. Uncertainty growth over the variable-time stage-2 tail is not modeled, which is the weakest premise for the safety claim.
  • ad hoc to paper The alternating Algorithm 2 converges to a KKT point of OCP (6).
    No convergence theorem is given. The loop in Algorithm 2 terminates only when KKT residuals fall below a user-set threshold, so convergence is empirical.
  • domain assumption The time-scaled discretization fT with T2/N2 faithfully represents the continuous-time dynamics on the variable grid.
    Eq. (6h) uses a scaled time variable so the total time T2 enters the discrete map. Accuracy depends on the integration scheme and the stage-2 horizon N2.

how reviews work

0 comments
Cite this review

Pith. "Pith review of Robustified Time-optimal Point-to-point Motion Planning and Control under Uncertainty." pith.science (2026). https://pith.science/paper/CRG6ZS4S

@misc{pith2026250114526,
  author       = {Pith},
  title        = {Pith review of: Robustified Time-optimal Point-to-point Motion Planning and Control under Uncertainty},
  year         = {2026},
  howpublished = {\url{https://pith.science/paper/CRG6ZS4S}},
  note         = {Machine review of arXiv:2501.14526}
}
read the original abstract

This paper proposes a novel approach to formulate time-optimal point-to-point motion planning and control under uncertainty. The approach defines a robustified two-stage Optimal Control Problem (OCP), in which stage 1, with a fixed time grid, is seamlessly stitched with stage 2, which features a variable time grid. Stage 1 optimizes not only the nominal trajectory, but also feedback gains and corresponding state covariances, which robustify constraints in both stages. The outcome is a minimized uncertainty in stage 1 and a minimized total motion time for stage 2, both contributing to the time optimality and safety of the total motion. A timely replanning strategy is employed to handle changes in constraints and maintain feasibility, while a tailored iterative algorithm is proposed for efficient, real-time OCP execution.

Figures

Figures reproduced from arXiv: 2501.14526 by the authors.

Figure 1
Figure 1. Point-to-point nominal trajectory and the corresponding closed-loop [PITH_FULL_IMAGE:figures/full_fig_p006_1.png] view at source ↗
Figure 3
Figure 3. Safety margins on the collision avoidance constraint. [PITH_FULL_IMAGE:figures/full_fig_p006_3.png] view at source ↗
Figure 4
Figure 4. Computation time and iteration number of each replanning. [PITH_FULL_IMAGE:figures/full_fig_p007_4.png] view at source ↗

Discussion (0). Continue with ORCID to comment.

Forward citations

Cited by 1 Pith paper

Reviewed papers in the Pith corpus that reference this work. Sorted by Pith novelty score. Full citation record

  1. Disturbance-aware minimum-time planning strategies for motorsport vehicles with probabilistic safety certificates

    cs.RO 2025-06 conditional novelty 4.0 of 10

    Minimum-lap-time planners that tighten track and tire-limit constraints using propagated covariance, with a closed-loop LQR variant that is less conservative than an open-loop horizon-based variant.

Reference graph

Works this paper leans on

23 extracted references · 18 canonical work pages · cited by 1 Pith paper

  1. [1]

    Model predictive contouring control,

    D. Lam, C. Manzie, and M. Good, “Model predictive contouring control,” in 49th IEEE Conference on Decision and Control (CDC) , 2010, pp. 6137–6142

    2010

  2. [2]

    Learning- based model predictive control for autonomous racing,

    J. Kabzan, L. Hewing, A. Liniger, and M. N. Zeilinger, “Learning- based model predictive control for autonomous racing,” IEEE Robotics and Automation Letters , vol. 4, no. 4, pp. 3363–3370, 2019

    2019

  3. [3]

    Mpcc++: Model predictive contouring control for time-optimal flight with safety constraints,

    M. Krinner, A. Romero, L. Bauersfeld, M. Zeilinger, A. Carron, and D. Scaramuzza, “Mpcc++: Model predictive contouring control for time-optimal flight with safety constraints,” 2024. [Online]. Available: https://arxiv.org/abs/2403.17551

    arXiv 2024

  4. [4]

    Unmanned aerial vehicles (uavs): A survey on civil applications and key research challenges,

    H. Shakhatreh, A. H. Sawalmeh, A. Al-Fuqaha, Z. Dou, E. Almaita, I. Khalil, N. S. Othman, A. Khreishah, and M. Guizani, “Unmanned aerial vehicles (uavs): A survey on civil applications and key research challenges,” IEEE Access, vol. 7, pp. 48 572–48 634, 2019

    work page 2019

  5. [5]

    Time-optimal motions of robots in assembly tasks,

    H. Geering, L. Guzzella, S. Hepner, and C. Onder, “Time-optimal motions of robots in assembly tasks,” IEEE Transactions on Automatic Control, vol. 31, no. 6, pp. 512–518, 1986

    work page 1986

  6. [6]

    A stabilizing nonlinear model predictive control scheme for time-optimal point-to-point motions,

    R. Verschueren, H. J. Ferreau, A. Zanarini, M. Mercang ¨oz, and M. Diehl, “A stabilizing nonlinear model predictive control scheme for time-optimal point-to-point motions,” in 2017 IEEE 56th Annual Conference on Decision and Control (CDC) , 2017, pp. 2525–2530

    work page 2017

  7. [7]

    Timed-elastic-bands for time-optimal point-to-point nonlinear model predictive control,

    C. R ¨osmann, F. Hoffmann, and T. Bertram, “Timed-elastic-bands for time-optimal point-to-point nonlinear model predictive control,” in 2015 European Control Conference (ECC) , 2015, pp. 3352–3357

    work page 2015

  8. [8]

    Time-optimal point-to-point motion plan- ning: A two-stage approach,

    S. Zhang and J. Swevers, “Time-optimal point-to-point motion plan- ning: A two-stage approach,” IFAC-PapersOnLine, vol. 58, no. 18, pp. 139–145, 2024

    work page 2024

Show all 23 references
  1. [9]

    J. B. Rawlings, M. M. Diehl, and D. Q. Mayne, Model predictive control: theory, computation and design . Nob Hill Madison, 2017

    2017

  2. [10]

    Robustness and stability optimization of power generating kite systems in a periodic pumping mode,

    B. Houska and M. Diehl, “Robustness and stability optimization of power generating kite systems in a periodic pumping mode,” in 2010 IEEE International Conference on Control Applications , 2010, pp. 2172–2177

    2010

  3. [11]

    A positive definiteness preserving discretiza- tion method for lyapunov differential equations,

    J. Gillis and M. Diehl, “A positive definiteness preserving discretiza- tion method for lyapunov differential equations,” in 52nd IEEE Con- ference on Decision and Control , 2013, pp. 7759–7764

    2013

  4. [12]

    Cautious model predictive control using gaussian process regression,

    L. Hewing, J. Kabzan, and M. N. Zeilinger, “Cautious model predictive control using gaussian process regression,” IEEE Transactions on Control Systems Technology, vol. 28, no. 6, pp. 2736–2743, 2020

    2020

  5. [13]

    Inexact adjoint- based sqp algorithm for real-time stochastic nonlinear mpc,

    X. Feng, S. D. Cairano, and R. Quirynen, “Inexact adjoint- based sqp algorithm for real-time stochastic nonlinear mpc,” IFAC- PapersOnLine, vol. 53, no. 2, pp. 6529–6535, 2020

    2020

  6. [14]

    Zero-order robust nonlinear model predictive control with ellipsoidal uncertainty sets,

    A. Zanelli, J. Frey, F. Messerer, and M. Diehl, “Zero-order robust nonlinear model predictive control with ellipsoidal uncertainty sets,” IFAC-PapersOnLine, vol. 54, no. 6, pp. 50–57, 2021

    2021

  7. [15]

    An efficient algorithm for tube-based robust nonlinear optimal control with optimal linear feedback,

    F. Messerer and M. Diehl, “An efficient algorithm for tube-based robust nonlinear optimal control with optimal linear feedback,” in2021 60th IEEE Conference on Decision and Control (CDC) , 2021, pp. 6714–6721

    2021

  8. [16]

    Collision-free motion planning for mobile robots by zero-order robust optimization-based mpc,

    Y . Gao, F. Messerer, J. Frey, N. v. Duijkeren, and M. Diehl, “Collision-free motion planning for mobile robots by zero-order robust optimization-based mpc,” in2023 European Control Conference (ECC), 2023, pp. 1–6

    2023

  9. [17]

    Robustified time-optimal collision-free motion planning for autonomous mobile robots under disturbance conditions,

    S. Zhang, M. Bos, B. Vandewal, W. Decr ´e, J. Gillis, and J. Sw- evers, “Robustified time-optimal collision-free motion planning for autonomous mobile robots under disturbance conditions,” in 2024 IEEE International Conference on Robotics and Automation (ICRA) , 2024, pp. 14 2...

    2024

  10. [18]

    Asap-mpc: An asynchronous update scheme for online motion planning with nonlinear model predictive control,

    D. Dirckx, M. Bos, B. Vandewal, L. Vanroye, W. Decr ´e, and J. Swevers, “Asap-mpc: An asynchronous update scheme for online motion planning with nonlinear model predictive control,” 2024. [Online]. Available: https://arxiv.org/abs/2402.06263

    2024 arXiv

  11. [19]

    Practical methods for approximate robust periodic optimal control of nonlinear mechanical systems,

    J. Gillis, “Practical methods for approximate robust periodic optimal control of nonlinear mechanical systems,” PhD thesis, Katholieke Universiteit Leuven, 2015

    2015

  12. [20]

    On the implementation of an interior- point filter line-search algorithm for large-scale nonlinear program- ming,

    A. W ¨achter and L. T. Biegler, “On the implementation of an interior- point filter line-search algorithm for large-scale nonlinear program- ming,” Mathematical Programming, vol. 106, p. 25–57, 2006

    2006

  13. [21]

    Effortless modeling of optimal control problems with rockit

    J. Gillis, B. Vandewal, G. Pipeleers, and J. Swevers, “Effortless modeling of optimal control problems with rockit.” in The 39nd Benelux Meeting on Systems and Control , 2020

    2020

  14. [22]

    A collection of fortran codes for large scale scientific computation,

    HSL (Harwell Subroutine Library), “A collection of fortran codes for large scale scientific computation,” https://www.hsl.rl.ac.uk/, 2023

    2023

  15. [23]

    CasADi – A software framework for nonlinear optimization and optimal control,

    J. A. E. Andersson, J. Gillis, G. Horn, J. B. Rawlings, and M. Diehl, “CasADi – A software framework for nonlinear optimization and optimal control,” Mathematical Programming Computation , vol. 11, no. 1, pp. 1–36, 2019

    2019

Pith tools

Reviewed August 10, 2026 · model on record in the stance chip above.