Source-linked AI summary
A Homotopy Method for Motion Planning
Shenyu Liu, Mohamed Ali Belabbas
TL;DR
Motion planning must find trajectories satisfying system and obstacle constraints while connecting specified states. MotionSketch encodes those constraints in an inner product and deforms a sketch into an admissible trajectory; the authors report good practical results across several constraint types.
Problem
Method
Results
The authors report good practical results on examples with holonomic, non-holonomic, and obstacle constraints.
Source: §5 · p. 16Takeaways & Limitations
The initial sketch need not satisfy holonomic or non-holonomic constraints, so it is often easy to obtain; in convex spaces a straight line most often meets the constraints.
Source: §5 · p. 16The constraint construction assumes its constraint matrix has constant rank almost everywhere in configuration space.
Source: §3.1.1 · p. 4
Abstract
from arXiv · showhide
We propose a novel method for motion planning and illustrate its implementation on several canonical examples. The core novel idea underlying the method is to define a metric for which a path of minimal length is an admissible path, that is path that respects the various constraints imposed by the environment and the physics of the system on its dynamics. To be more precise, our method takes as input a control system with holonomic and non-holonomic constraints, an initial and final point in configuration space, a description of obstacles to avoid, and an initial trajectory for the system, called a sketch. This initial trajectory does not need to meet the constraints, except for the obstacle avoidance constraints. The constraints are then encoded in an inner product, which is used to deform (via a homotopy) the initial sketch into an admissible trajectory from which controls realizing the transfer can be obtained. We illustrate the method on various examples, including vehicle motion with obstacles and a two-link manipulator problem.
1 Introduction
Robotic motion planning must produce trajectories that satisfy system dynamics and obstacle-avoidance constraints. MotionSketch deforms an initial curve into an admissible one, while this paper restricts its presentation to systems affine in the control.
1 Introduction
- MotionSketch deforms a curve between initial and desired states into an admissible trajectory satisfying dynamics and obstacle constraints.The initial curve need not satisfy the constraints; the paper focuses on systems affine in the control.Source: §1 · p. 1
- Many existing approaches focus on non-holonomic dynamics, while the proposed method is built on a different set of ideas.The introduction also surveys sampling-, graph-, and optimization-based approaches and refers readers to work on hybrid systems and self-driving vehicles.Source: §1 · pp. 1–2
- Sinusoidal driving signals use a system approximation involving the Lie bracket of the control vector fields.The cited background relates this idea to work by Brockett, Murray et al., and Laferriere and Sussman.Source: §1 · pp. 1–2
2 Background and problem set-up
The paper defines motion planning as finding a constraint-respecting curve between specified states and controls that realize the transfer. It uses a Riemannian inner product to measure curve length and weight directions differently.
2 Background and problem set-up
- The setup models systems on a configuration space with controlled dynamics, and distinguishes a geometric path from the times at which its states are visited.A fixed-endpoint homotopy is a differentiable deformation between curves sharing endpoints.Source: §2 · p. 2
- A Riemannian inner product is an x-dependent positive-definite symmetric matrix that defines curve length on the configuration space.The length is approximated by subdividing the unit interval and evaluating the metric along the curve.Source: §2 · pp. 2–3
- MotionSketch seeks a curve between initial and final states that respects holonomic, non-holonomic, and obstacle constraints, together with controls driving the system between them.The presentation normalizes the time interval to one for simplicity.Source: §2 · p. 3
- Factoring the metric into an orthogonal basis and positive diagonal weights lets the method make selected infinitesimal directions contribute more to curve length.The basis vectors give coordinates for each curve increment, whose components are weighted by the diagonal entries.Source: §2 · p. 3
3 The method MotionSketch
MotionSketch encodes motion-planning constraints in a Riemannian inner product, then uses geometric heat flow to deform an initial sketch into a trajectory from which controls can be extracted.
3 The method MotionSketch
- The method encodes obstacles, holonomic, non-holonomic, and dynamical constraints in a Riemannian inner product.This inner product is designed so that minimizing path length favors constraint-respecting motion.Source: §3 · p. 3
- Starting from a sketch between the desired states, MotionSketch numerically solves geometric heat flow and extracts controls from its solution.The sketch must avoid obstacles but need not satisfy holonomic, non-holonomic, or dynamical constraints.Source: §3 · pp. 3–4
3.1 Step 1: Encoding the constraints in a Riemannian inner product
MotionSketch encodes constraints by making undesirable directions or states costly under a Riemannian metric. The section describes this construction for system constraints, obstacles, multiple vehicles, and two illustrative systems.
3.1 Step 1: Encoding the constraints in a Riemannian inner product
- The section’s constraint-encoding step uses system directions and obstacle functions to shape the metric, with a barrier example also described for planar vehicle pairs.The multi-vehicle formulation uses a safety radius to specify when two vehicles are too close.Source: §3.1 · p. 4, §3.1.2 · pp. 5–6
3.1.1 Holonomic and non-holonomic constraints
- Constraint encoding makes motions along directions that violate holonomic or non-holonomic constraints costly in the inner product.The construction assumes the constraint matrix has constant rank almost everywhere; when constraints are dependent, a full-rank matrix with the same column span is used.Source: §3.1.1 · pp. 4–5
3.1.2 Obstacle constraints
- Obstacle barriers increase the metric near obstacle boundaries, making paths that pass nearby longer than paths that steer clear.The distance treated as “well-clear” depends on the barrier function and how quickly it decays near the boundary.Source: §3.1.2 · pp. 5–6, Figure 2 · p. 6
3.1.3 simultaneous multi-vehicle path planning
- For simultaneous multi-vehicle planning, a pairwise collision barrier raises the metric when vehicles are too close, treating collision avoidance similarly to static-obstacle avoidance.The passage contrasts this with planning vehicles individually, which makes the other vehicles moving obstacles.Source: §3.1.3 · p. 7
3.1.4 The inner product with three type of constraints
- The method combines the constraint metric and obstacle barrier in a single inner product, with derivatives computed by the chain rule.The resulting metric is used to compute the Christoffel symbols from its values and derivatives.Source: §3.1.4 · p. 7
3.1.5 Examples
- The two-link manipulator is represented in a four-dimensional configuration space with two holonomic constraints, while the unicycle model has a non-slippage constraint preventing sideways motion.The examples construct the constraint directions and corresponding metric for each system.Source: §3.1.5 · pp. 7–8
3.2 Step 2: Initial sketch and solving the Geometric Heat Flow equation
The initial sketch must connect the endpoints and avoid obstacles, but need not satisfy holonomic or non-holonomic constraints. Geometric heat flow deforms it toward a minimal-length curve under the chosen inner product.
3.2 Step 2: Initial sketch and solving the Geometric Heat Flow equation
- The initial curve joins the required endpoints and avoids obstacles, but may violate holonomic and non-holonomic constraints.It serves as the user-defined initial condition for the flow.Source: §3.2 · p. 9
- As the flow parameter increases, the curve uses less of the constrained directions.The method takes the flow solution at a finite simulation time as its output curve.Source: §3.2 · p. 9
- The geometric heat flow extends curve-shortening to curves in higher dimensions and general Riemannian inner products, with solutions converging to minimal-length curves.In the Euclidean plane, the corresponding curve-shortening flow tends toward the straight line between the endpoints.Source: §3.2 · p. 9
3.3 Step 3: Extracting the controls
The method extracts controls from the flow-produced curve using a pseudo-inverse of the free-direction matrix. If the curve is not exactly admissible, the extracted controls drive the system along a nearby path.
3.3 Step 3: Extracting the controls
- The control is computed using the pseudo-inverse of the free-direction matrix.The passage identifies this pseudo-inverse as f.Source: §3.3 · pp. 9–10
- When the solution curve is admissible, the recovered control should make the system follow that curve exactly.Admissibility is expressed as the curve derivative being representable through the free-direction matrix and a control.Source: §3.3 · p. 10
- The minimal square-error projection onto the free-direction column space yields a trajectory close to the solution curve, even when it has small constrained-direction components.Source: §3.3 · p. 10
3.4 On the implementation
The implementation builds the constraint-dependent inner product and solves the geometric heat flow numerically in MATLAB using symbolic expressions and pdepe.
3.4 On the implementation
- The implementation derives the free-direction matrix from the constraints and symbolically obtains the inner product and its state derivatives.The symbolic expressions are evaluated at state values and stored in arrays.Source: §3.4 · p. 10
- At each PDE iteration, Christoffel symbols are computed from the inner product and its derivatives and stored in an array.The solver uses pdepe with the boundary conditions and customized initial condition.Source: §3.4 · p. 10
- For pdepe, the implementation sets c=ones(4,1), m=0, f=DxDt, and s(i)=DxDt’*Chris(i,:,:)*DxDt.Source: §3.4 · p. 10
3.5 Theoretical guarantee
Under mild assumptions, the method constructs controls whose state trajectory satisfies the system’s holonomic and non-holonomic constraints. The theorem assumes a globally Lipschitz system field and a unit-norm constrained-direction matrix.
3.5 Theoretical guarantee
- Under mild assumptions, the constructed controls yield a state trajectory satisfying both holonomic and non-holonomic constraints.Source: §3.5 · p. 10
- The theorem assumes F is globally Lipschitz with constant L and that the constrained-direction matrix has unit norm everywhere.Source: §3.5 · p. 10
- The theorem considers the flow curve at parameter s, controls derived from it, and the resulting system trajectory initialized at the initial state.For any positive error tolerance and natural number k, it specifies a threshold T(ϵ, k) after which its stated bound applies.Source: §3.5 · p. 10
- Figure 4 depicts vertical and circular motions of the two-link arm, with tip trajectories in red and joint angles in panels (b) and (d).Source: Figure 4 · p. 11
4 Case study
The case studies apply the method to a two-link arm, a unicycle, cars, and multiple vehicles. They show trajectories adapting to motion constraints, obstacles, and street width.
Articulated arm
- For the two-link arm, adding a circular tip-path constraint demonstrates using the method to solve inverse kinematics numerically.This replaces the vertical-motion constraint with a circle constraint.Source: §4 · p. 11
Unicycle
- For a unicycle constrained by two point obstacles, the sketch deforms into an almost admissible path that first backs up, then overshoots an obstacle before parking.The initial sinusoidal sketch violates the non-slip constraint; at s = 4, the resulting trajectory is nearly followable.Source: §4 · pp. 11–12, Figure 5 · p. 13
Car
- A car’s infeasible 180°-turn sketch deforms into the practical three-point turn when the car lacks omniwheels and d ≠ 0.The passage describes the three-point turn as the most efficient practical 180° turn when there are no spatial obstacles.Source: §4 · p. 12, Figure 6 · p. 14
- For a street-corner turn, the same initial sketch yields a one-shot turn in a relatively wide street but extra maneuvering in a narrow one.Street width alone changes the outcome; the method determines the maneuver without further specification.Source: §4 · p. 12, Figure 7 · p. 14
Multi-vehicle path planning
- In a two-unicycle position swap, the algorithm deforms the initial circular sketches into V-shaped paths.The initial paths are infeasible because the unicycles’ orientations are not tangent to them.Source: §4 · p. 12, Figure 8 · p. 15
5 Summary and discussion
MotionSketch encodes constraints in a Riemannian inner product and uses an initial sketch to produce admissible trajectories. The discussion extends the approach to multi-vehicle collision avoidance and explains why its geometric heat-flow computation can scale to higher-dimensional problems.
5 Summary and discussion
- The method is illustrated on holonomic, non-holonomic, and obstacle constraints, with the authors reporting good practical results.Source: §5 · p. 16
- MotionSketch encodes constraints in a Riemannian inner product and requires an initial endpoint-joining sketch that need not satisfy holonomic or non-holonomic constraints.The sketch is often easy to obtain; in a convex space, a straight line between states most often meets the constraints.Source: §5 · p. 16
- For multiple cars, MotionSketch models their joint state with a block-diagonal metric and collisions as an obstacle around the subspace where their positions coincide.The authors state that this construction generalizes to more than two vehicles.Source: §5 · p. 16
- The geometric heat-flow PDE has a fixed two-dimensional solution domain, so interpolation-point growth is linear with image dimension rather than exponential with domain dimension.The authors also describe the PDE solver's complexity as polynomial rather than exponential and note that parallel algorithms exist.Source: §5 · p. 16
- On a common laptop, non-optimized MATLAB runs took seconds to minutes depending on problem complexity, and the authors saw room for improvement.Source: §5 · p. 16