Source-linked AI summary
The Convex Feasible Set Algorithm for Real Time Optimization in Motion Planning
Changliu Liu, Chung-Yen Lin, Masayoshi Tomizuka
TL;DR
Real-time motion planning is difficult because obstacle constraints create highly non-convex optimization problems. The paper introduces CFS, which repeatedly solves convex subproblems over convex feasible sets, and proves feasibility and convergence while applying the method to mobile-robot planning.
Problem
Obstacle constraints make optimization-based motion planning highly non-convex and difficult to solve in real time.
Method
CFS constructs convex feasible sets inside a non-convex domain and iteratively solves convex subproblems over them.
Results
The paper proves feasibility and convergence properties for CFS, including convergence to strong or weak local optima under the analyzed cases.
Takeaways & Limitations
CFS provides a theoretically analyzed approach for convex-cost, non-constrained optimization problems and is applied to mobile-robot motion planning.
Takeaways & Limitations
The method's general applicability depends on a semi-convex constraint decomposition whose validity is left for future work, and nonlinear equality constraints remain difficult.
Abstract
from arXiv · showhide
With the development of robotics, there are growing needs for real time motion planning. However, due to obstacles in the environment, the planning problem is highly non-convex, which makes it difficult to achieve real time computation using existing non-convex optimization algorithms. This paper introduces the convex feasible set algorithm (CFS) which is a fast algorithm for non-convex optimization problems that have convex costs and non-convex constraints. The idea is to find a convex feasible set for the original problem and iteratively solve a sequence of subproblems using the convex constraints. The feasibility and the convergence of the proposed algorithm are proved in the paper. The application of this method on motion planning for mobile robots is discussed. The simulations demonstrate the effectiveness of the proposed algorithm.
1. Introduction.
The paper targets real-time optimization-based motion planning, where obstacle constraints make problems highly non-convex. It proposes CFS, which iteratively solves convex subproblems while providing theoretical guarantees for broader non-convex and non-differentiable settings.
- Motivation: Obstacle constraints make optimization-based motion planning difficult to solve in real time, despite convex objective functions often being available.The paper frames motion planning within model-predictive control, where constrained optimization is solved at each time step.
- Contribution: The convex feasible set algorithm transforms a non-convex problem into a sequence of convex subproblems by constructing convex feasible sets inside the original domain.Each subproblem is solved iteratively until convergence.
- Contribution: CFS differs from sequential quadratic programming by using the geometric structure of the original problem to obtain its convex subproblems.The paper states that this strategy makes computation faster than conventional SQP and other non-convex methods.
- Novelty: The convex feasible set can be interpreted as a convex corridor, while the paper extends corridor-based ideas to general non-convex and non-differentiable optimization with theoretical guarantees.Earlier convex-corridor methods are described as application-specific and lacking theoretical guarantees.
- Paper organization: The paper proceeds from a benchmark optimization problem through algorithm design and feasibility and convergence analysis to mobile-robot motion-planning applications.The application section focuses on motion planning for mobile robots.
2. The Optimization Problem.
The benchmark problem minimizes a smooth strictly convex cost over a geometrically structured but potentially non-convex feasible set. CFS constructs convex inner approximations of that set under semi-convex constraint representations, with explicit scope limitations for equality constraints and decomposition assumptions.
- The benchmark problem: The benchmark problem has a smooth strictly convex cost and a connected, closed feasible set whose non-convexity comes from constraints.The feasible set is assumed to contain an n-dimensional convex polytope around every point.
- Scope and assumptions: The framework excludes equality constraints under the main feasible-set assumption and requires a semi-convex decomposition satisfying Assumption 2.4, whose general validity is left for future work.Global linear equalities can be handled on a lower-dimensional manifold, whereas nonlinear equalities are described as more difficult.
- Algorithmic idea: CFS iteratively searches the non-convex feasible domain by computing a convex feasible set around the current reference point and optimizing within it.The resulting optimizer becomes the reference point for the next iteration.
- Analytical representation: The constraint set is represented as an intersection of sets Γi defined by continuous, piecewise smooth, semi-convex functions φi.This semi-convex decomposition supports the construction of convex inner feasible sets.
- Analytical representation: The representation permits overlapping component sets and non-unique decompositions, and signed distance functions may represent individual constraints in many cases.The number of components should be at least the number of disjoint components in the complement.
- Analytical representation: Sub-gradients are defined through directional derivatives so the method can handle constraint functions that are not differentiable everywhere.At smooth points, the sub-differential reduces to the ordinary gradient.
3. Solving the Optimization Problem.
CFS solves the constrained problem by repeatedly optimizing a convex cost over convex inner feasible sets. It constructs these sets from constraint geometry and selects sub-gradients to preserve feasible descent directions, with termination conditions linked to convergence.
- The Convex Feasible Set Algorithm: At each iteration, CFS computes a convex feasible set inside Γ and obtains the next reference point by solving a convex optimization problem.The optimization is repeated until a change-in-solution or cost-descent stopping condition is met.
- The Convex Feasible Set Algorithm: The algorithm terminates when either the solution change or the cost descent is sufficiently small, and the paper states that both conditions imply convergence.The process is summarized in Algorithm 3.1.
- Constructing feasible sets: For concave constraint functions, the entire constraint set Γi is used as the convex feasible set.This is the first construction case in the constraint decomposition.
- Constructing feasible sets: For convex or semi-convex constraints, CFS uses a reference-point sub-gradient and a supporting linear or quadratic inequality to construct an inner convex feasible set.The construction remains contained in Γi by the convexity or semi-convexity inequalities.
- Choosing sub-gradients: At non-smooth points, the method selects sub-gradients so the convex feasible set contains a feasible steepest-descent direction of the objective.The procedure considers feasible search directions and resolves ties deterministically.
- Theoretical analysis: The paper identifies feasibility and convergence of Algorithm 3.1 as the subject of the subsequent theoretical analysis.
4. Properties of the Convex Feasible Set Algorithm.
The CFS analysis establishes feasibility of the convex feasible sets and convergence of the generated sequence to a local optimum. Fixed points yield strong local optima, while strictly descending sequences converge to weak local optima.
- Main convergence result: Theorem 4.1 guarantees convergence to a feasible point that is a strong local optimum when the algorithm terminates, or at least a weak local optimum otherwise.The result assumes a nonempty initial convex feasible set.
- Feasibility: For every feasible reference point, the associated convex feasible set contains the reference point and has nonempty interior.This feasibility property enables the next convex subproblem to be solved within the feasible domain.
- Fixed points: A fixed point of Algorithm 3.1 is a strong local optimum of the original problem.The argument uses the convex subproblem's optimality conditions and the geometry of the active constraints.
- Descent: The objective has strong descent: equality between consecutive objective values implies that consecutive iterates are identical.This separates finite termination from the strictly descending case in the convergence proof.
- Strict descent: If the objective decreases strictly at every iteration, the generated sequence converges to a weak local optimum.The proof combines bounded objective sublevel sets, sequence convergence, and a limiting KKT condition.
- Regularity: When all constraint functions are smooth at the limit, the limiting convex feasible set equals the one at the limit point, yielding a strong local optimum.Without smoothness, the theorem guarantees only a weak local optimum in the non-terminating case.
5. Application on Motion Planning for Mobile Robots.
The mobile-robot application formulates trajectory planning around moving obstacles and transforms obstacle avoidance into convex feasible corridors. CFS is reported as feasible, locally optimal, scalable, and faster than ITP and SQP in the tested scenarios.
- 5.2. Transforming the Problem.: Obstacle avoidance is represented through safety indices, with separate constructions for convex, boundary, and non-convex obstacles after semi-convex decomposition.The resulting safety indices are continuous, piecewise smooth, and semi-convex.
- 5.2. Transforming the Problem.: The method constructs convex corridors around time-augmented obstacles by combining per-time-step feasible slices into a convex subset of trajectory space.A new trajectory is computed inside this corridor.
- 5.3. The Convex Feasible Set - Examples.: Under convex obstacles with disjoint closures, the convex feasible set has nonempty interior, and CFS converges to a local optimum from any initial reference.This feasibility result is the condition used to invoke the convergence theorem.
- 5.4. Performance and Comparison.: CFS always outperforms ITP and SQP in computation time because it uses fewer iterations and avoids additional line searches.The comparison reports lower time per iteration and fewer iterations to convergence for CFS.
- 5.4. Performance and Comparison.: CFS computation grows with the planning horizon; processing time dominates in Matlab, while optimization time dominates in C++.The two runtime components are safety-set processing and solving the convex optimization subproblem.
- 5.4. Performance and Comparison.: For horizon h = 100 in scenario 1, CFS becomes feasible at the first iteration, whereas ITP-C and SQP-C require three and five iterations respectively.The Matlab versions of ITP and SQP become feasible only after 100 iterations.
6. Conclusion.
The supplied conclusion figures show pre-convergence trajectories for scenario 2 and decompose per-iteration computation time for scenario 1.
- 6. Conclusion.: Scenario 2 is illustrated with trajectories before convergence for h = 50.
- 6. Conclusion.: Scenario 1 is illustrated with a decomposition of computation time per iteration using Algorithm 3.1.