PaperlayerView exact revision on arXiv

Source-linked AI summary

A New Approach to Time-Optimal Path Parameterization based on Reachability Analysis

Hung Pham, Quang-Cuong Pham

arXiv:1707.07239v2cs.RO

TL;DR

TOPP requires the fastest constraint-respecting traversal of a robot path, but existing Numerical Integration and Convex Optimization methods trade speed, implementation difficulty, and robustness. The paper introduces TOPP-RA, which recursively computes reachable and controllable sets with small Linear Programs; extensive evaluations report 100% success and faster computation than existing approaches, alongside additional uncertainty and planning benefits.

  • Problem

    TOPP needs fast, constraint-respecting traversal of prescribed robot paths, while existing Numerical Integration and Convex Optimization methods have complementary speed, robustness, and implementation trade-offs.

  • Method

    TOPP-RA recursively computes reachable and controllable sets at discretized path positions using small Linear Programs, then extracts the time-optimal parameterization.

  • Results

    TOPP-RA achieves a 100% success rate and is significantly faster than existing methods for problem instances with more than 22 constraints.

  • Takeaways & Limitations

    The reachability perspective also provides native redundant-actuation handling, Admissible Velocity Propagation, and natural robustness to parametric uncertainty.

Abstract

from arXiv · show

Time-Optimal Path Parameterization (TOPP) is a well-studied problem in robotics and has a wide range of applications. There are two main families of methods to address TOPP: Numerical Integration (NI) and Convex Optimization (CO). NI-based methods are fast but difficult to implement and suffer from robustness issues, while CO-based approaches are more robust but at the same time significantly slower. Here we propose a new approach to TOPP based on Reachability Analysis (RA). The key insight is to recursively compute reachable and controllable sets at discretized positions on the path by solving small Linear Programs (LPs). The resulting algorithm is faster than NI-based methods and as robust as CO-based ones (100% success rate), as confirmed by extensive numerical evaluations. Moreover, the proposed approach offers unique additional benefits: Admissible Velocity Propagation and robustness to parametric uncertainty can be derived from it in a simple and natural way.

I. INTRODUCTION

TOPP seeks the fastest constraint-respecting traversal of a prescribed robot path. TOPP-RA uses reachability analysis to combine robustness, speed, and additional capabilities within a broad constraint framework.

  • I. INTRODUCTION: TOPP finds the fastest way to traverse a robot configuration-space path while respecting system constraints.It supports industrial processes, mobile robotics, and kinodynamic motion planning.
  • I. INTRODUCTION: Existing TOPP methods are based mainly on Numerical Integration or Convex Optimization, trading implementation speed against robustness and computational cost.NI methods explicitly compute optimal controls but face switch-point and velocity-bound difficulties; CO methods are robust but solve large optimization programs.
  • Proposed new approach based on Reachability Analysis: TOPP-RA recursively computes reachable and controllable sets at discretized path positions using small Linear Programs, then extracts a parameterization from them.The method computes controllable states in a backward pass and obtains time-optimality through a greedy forward pass.
  • Proposed new approach based on Reachability Analysis: Compared with NI and CO methods, TOPP-RA is significantly faster for instances with more than 22 constraints and achieves a 100% success rate.The paper also identifies native redundant-actuation handling, Admissible Velocity Propagation, and natural robustness to parametric uncertainty as additional benefits.
  • A. Generalized constraints: The generalized formulation covers velocity and acceleration bounds, torque bounds, and contact-stability constraints through convex constraint sets.For the paper’s time-optimality developments, the constraint set is required to be polytopic; reachable and controllable-set developments remain valid for convex non-polytopic sets.

B. Projecting the constraints on the path

The paper projects robot dynamics and constraints onto a path parameter using position, velocity, and acceleration variables. It then discretizes the path into stages linked by a linear squared-velocity transition relation.

  • B. Projecting the constraints on the path: Differentiating the path configuration expresses robot motion using path position s, velocity ˙s, and acceleration ¨s.The derivative notation is with respect to the path parameter s.
  • B. Projecting the constraints on the path: Substituting the path derivatives transforms second-order system constraints into constraints on s, ˙s, and ¨s, with C(s) defined as C(q(s)).First-order constraints are similarly transformed, with C_v(s) defined as C_v(q(s)).
  • C. Path discretization: The path interval is divided into N segments and N + 1 grid points, with constant path acceleration u_i over each segment and squared velocity x_i at each grid point.This discretization introduces stage-wise controls and states for the parameterization.
  • C. Path discretization: Collocation enforces constraints only at finite grid points, producing an O(∆_i) constraint-satisfaction error along the continuous path.A first-order interpolation scheme achieves O(∆_i^2) error but uses more variables and inequalities.

III. REACHABILITY ANALYSIS OF THE PATH-PROJECTED DYNAMICS

Reachability Analysis reformulates the path-projected TOPP dynamics as a discrete-time linear system with linear constraints, enabling recursive computation of reachable and controllable velocity sets through small linear programs.

  • The path-projected dynamics form a discrete-time linear system with linear control-state inequality constraints.
  • The admissible control-state set projects onto a polygon in the (¨s, ˙s^2) plane, while admissible states and controls are intervals.Convexity of the projected set makes both state and control sets intervals.
  • An i-stage reachable set contains states attainable from an initial set through admissible controls over i stages.
  • Reach sets are intervals whose bounds can be computed by two linear programs over the admissible control-state polygon.The construction avoids explicitly computing the intersected polygon before solving for the bounds.
  • Reachable sets are recursively propagated forward, requiring 2i + 2 LPs to compute the i-stage set from an interval initial set.

IV. TOPP BY REACHABILITY ANALYSIS

TOPP-RA solves discretized TOPP instances with a backward controllability pass followed by a greedy forward control-selection pass. It returns an admissible solution whenever one exists and otherwise reports infeasibility.

  • TOPP-RA first computes controllable sets backward from the desired ending velocity and reports failure if feasibility conditions are not met.Failure occurs when a controllable set is empty or the starting squared velocity is outside the initial controllable set.
  • The forward pass greedily selects the highest admissible control whose next state lies in the next-stage controllable set.
  • A dual algorithm can instead propagate reachable sets forward and select the lowest control compatible with the previous reachable set during a backward pass.
  • TOPP-RA returns an admissible parameterization whenever one exists and reports Infeasible otherwise.
  • The returned sequence is an admissible parameterization that solves the discretized TOPP instance.

C. Asymptotic optimality of TOPP-RA

TOPP-RA has a linear-in-constraints complexity and achieves optimality under sufficiently fine discretization without zero-inertia points. With zero-inertia points, its cost approaches the minimum as the step size decreases, while experiments show strong speed and robustness advantages.

  • Zero-inertia points are the main difficulty in proving asymptotic optimality, although they do not affect the algorithm's robustness or correctness.
  • If the discretization step satisfies ∆ < ∆thr and the instance has no zero-inertia points, TOPP-RA returns an optimal parameterization.
  • With zero-inertia points, reducing the step size makes TOPP-RA's traversal cost arbitrarily close to the minimum, including cases with finitely many such points.Zero-inertia arcs may be more problematic, but the paper states they can be avoided during planning.
  • TOPP-RA solves 3N two-variable LPs, giving practical complexity O(mN), compared with O(m^2N) for Numerical Integration.The Convex Optimization approach is analyzed as O(KmN^3), where K is the number of SLP iterations.
  • For paths with n = 6 and m = 14, TOPP-RA and TOPP-NI produced virtually identical time-parameterizations and trajectories.
  • TOPP-RA becomes faster than TOPP-NI when m ≥ 22 and maintained a 100% success rate across tested instances.TOPP-NI struggled with instances having m ≥ 40 inequality constraints.

2) Effect of discretization grid size:

Grid size affects TOPP-RA and TOPP-NI's running time, success, constraint satisfaction, and solution quality. TOPP-RA is faster for sufficiently constrained instances, while TOPP-RA-intp generally improves quality at higher computational cost.

  • Running time depends linearly on grid size for both TOPP-RA and TOPP-NI.
  • 100% success rates were maintained by TOPP-RA and TOPP-RA-intp across tested grid sizes, while TOPP-NI failed twice.The failures occurred at N = 100 and N = 1000.
  • TOPP-RA and TOPP-NI had constraint errors of the same order for N < 500, while TOPP-RA was better for N ≥500.TOPP-RA-intp produced much higher-quality solutions, and interpolation discretization was better for solution quality.
  • TOPP-RA and TOPP-NI produced virtually identical parameterizations for a 6-dof path with m = 14 and N = 500.TOPP-NI exhibited small acceleration chattering attributed to integration, whereas TOPP-RA did not.
  • TOPP-RA's complexity is linear in m and becomes faster than TOPP-NI at m ≥22, whose complexity is quadratic in m.The comparison excludes setup and trajectory-extraction steps.
  • Both TOPP-RA variants were asymptotically optimal, with TOPP-RA's cost difference only 10^-2 sec at N = 100.The optimal cost was approximated using TOPP-RA-intp with N = 10000.
  • TOPP-RA-intp produced smoother, more accurate trajectories but required longer computation, whereas TOPP-RA was faster and showed jitters near dynamic singularities.The jitters can usually be removed with local cubic-spline smoothing.

VI. ADDITIONAL BENEFITS OF TOPP BY REACHABILITY ANALYSIS

Reachability analysis gives TOPP-RA additional capabilities beyond time-optimal parameterization, including force recovery, Admissible Velocity Propagation, and robust handling of parametric uncertainty. These benefits arise from the same reachable and controllable-set framework and LP-based computations.

  • Admissible Velocity Propagation computes reachable velocity intervals, requiring 2N LPs for the forward and backward set computations.The corresponding backward computation yields controllable sets for desired final velocity intervals.
  • Parametric uncertainty can be incorporated by replacing nominal constraints with sets describing possible parameter values at each path position.Examples include uncertain inertia matrices, foot positions, and friction coefficients.
  • Robust controllable sets identify states from which a desired endpoint can be reached using controls admissible for all modeled uncertainties.The robust backward pass computes these sets, and the forward pass selects the greatest robust admissible control.
  • Robust one-step computations may involve hundreds of uncertain inequalities, handled as robust linear programs or conic quadratic programs depending on the uncertainty set.Polyhedral uncertainty can be converted to ordinary LPs with suitable slack variables.
  • The paper concludes that TOPP-RA combines robustness and competitive computation with no projection requirement, velocity propagation, and uncertainty handling.

APPENDIX

The appendix relates TOPP-RA to TOPP-NI and establishes when its greedy forward pass is optimal. The proof depends on monotonicity of maximal transition functions, which is guaranteed below a threshold discretization step under a no-zero-inertia assumption.

  • A. Relation between TOPP-RA and TOPP-NI: TOPP-RA and TOPP-NI compute the same velocity profiles in different orders.The appendix rearranges TOPP-NI's switch-point and limiting-curve construction to compare it with TOPP-RA's two passes.
  • A. Relation between TOPP-RA and TOPP-NI: TOPP-RA avoids explicitly computing acceleration-switch points because controllable sets identify them implicitly.This removes a major implementation difficulty of TOPP-NI.
  • A. Relation between TOPP-RA and TOPP-NI: TOPP-RA requires post-processing to remove jitters from its returned profiles.
  • B. Proof of optimality (no zero-inertia point): The appendix defines minimal and maximal controls by intersecting a horizontal state level with the feasible control-state polygon.
  • B. Proof of optimality (no zero-inertia point): If every maximal transition function is non-decreasing, TOPP-RA's greedy strategy produces the optimal parameterization.The proof shows the resulting state is no lower in velocity than that of any admissible parameterization at every stage.
  • B. Proof of optimality (no zero-inertia point): The maximal transition function is not always non-decreasing, so the greedy optimality argument requires the stated monotonicity condition.
  • B. Proof of optimality (no zero-inertia point): Without zero-inertia points, a threshold step size ∆thr exists such that every ∆ < ∆thr makes TOPP-RA optimal.The threshold ensures maximal transition functions are non-decreasing throughout the controllable sets.

C. Proof of asymptotic optimality (with zero-inertia point)

With a zero-inertia point, the maximal transition can lose monotonicity over a finite perturbation interval, but the resulting sub-optimality gap decreases to zero as the discretization step shrinks.

  • Perturbation interval: Zero-inertia points prevent bounding the acceleration coefficient away from zero and create a finite interval where the maximal transition is non-monotonic.The analysis assumes the zero-inertia point lies on the grid and identifies the exceptional stages around it.
  • Asymptotic optimality: The sub-optimality gap caused by the perturbation interval converges to zero as ∆ approaches zero.The proof compares the TOPP-RA and true optimal profiles through three cost terms and combines the resulting estimates.
  • Cost-to-go analysis: The greedy transition is optimal outside the exceptional stages and within the strip follows the cost-to-go properties established by backward induction.The proof characterizes TOPP-RA and optimal cost-to-go functions over intervals bounded by the controllable sets and perturbation profiles.
  • Perturbation strip: The perturbation strip is bounded by upper profile (κi) and lower profile (µi) boundaries constructed around the (λi) profile.The profiles are defined recursively, with (µi) chosen as the highest profile remaining below (λi).
  • Perturbation strip: Consecutive κi and µi values differ by at most 2C∆, so the strip width is controlled linearly by the discretization step.The bound uses an upper bound C on admissible controls and the fact that µp = λp for some index p.

1) First-order interpolation scheme:

The first-order interpolation scheme enforces constraints at the beginning and end of each discretized interval, using the state-control pair at the start and its propagated endpoint.

  • First-order interpolation scheme: At stage i, the scheme requires (ui, xi) to satisfy constraints at si and (ui, xi + 2∆iui) to satisfy them at si+1.The endpoint condition is applied for i = 0, . . . , N−1; at i = N, only the top half of the equations is used.

2) Error analysis:

The error analysis compares collocation and first-order interpolation discretizations, showing that interpolation achieves a higher-order constraint satisfaction error while requiring a larger discrete program.

  • Error analysis: The collocation discretization scheme has greatest constraint satisfaction error of order O(∆).The estimate follows from interpolation-error analysis under bounded derivatives of the admissible-set functions.
  • Error analysis: First-order interpolation improves the error order from O(∆) to O(∆2) but introduces more variables and inequality constraints than collocation.The comparison concerns continuous constraint satisfaction between the discretization points.
  • Error analysis: The first-order interpolation scheme has greatest constraint satisfaction error of order O(∆2).This follows under bounded derivatives of the functions defining admissible control-state pairs.
Loading 1707.07239v2…