Nonholonomic Motion Planning: Steering Using Sinusoids

R. M. Murray and S. S. Sastry

Motion Planning without Constraints

• Obstacle positions are known and dynamic constrains on robot are not considered.

From Planning, geometry, and complexity of robot motion By Jacob T. Schwartz, John E. Hopcroft

Problem with Planning without Constraints

Paths may not be physically realizable

Mathematical Background

• Nonlinear Control System

mm uxguxgx )()( : 11

• Distribution

)(,),(1 xgxgspan m

Lie Bracket

• The Lie bracket has the properties

gx

ff

x

ggf

],[

• The Lie bracket is defined to be

],[],[ fggf 1.)

2.) 0]],[,[]],[,[]],[,[ gfhfhghgf (Jacobi identity)

Physical Interpretation of the Lie Bracket

Controllability

• Chow’s Theorem

Uxx 10 ,

mRTuandT ],0[:0

10 )()0(.. xTxandxxsatisfiests

• A system is controllable if for any

UxallforRIf nx

Uonlecontrollabissystemthethen

)( bracketingLieunderofclosuretheis

Classification of a Lie Algebra

• Construction of a Filtration

)(,),(11 xgxgspanGIf m

],[ 111 iii GGGG

1111 ,:],[],[ ii GhGghgspanGGWhere

Classification of a Lie Algebra

• Regular

Classification of a Lie Algebra

• Degree of Nonholonomy

Classification of a Lie Algebra• Maximally Nonholonomic

iip rankGrZr ,

0,, 01 rrrZ iiip

• Growth Vector

• Relative Growth Vector

Nonholonomic Systems• Example 1

Nonholonomic Systems• Example 2

Phillip Hall Basis

The Phillip Hall basis is a clever way of imposing the skew-symmetry of Jacobi identity

Phillip Hall Basis• Example 1

Phillip Hall Basis

• A Lie algebra being nilpotent is mentioned • A nilpotent Lie algebra means that all Lie

brackets higher than a certain order are zero• A lie algebra being nilpotent provides a

convenient way in which to determine when to terminate construction of the Lie algebra

• Nilpotentcy is not a necessary condition

Steering Controllable Systems Using Sinusoids: First-Order Systems

• Contract structures are first-order systems with growth vector

• Contact structures have a constraint which can be written

• Written in control system form

Steering Controllable Systems Using Sinusoids: First-Order Systems

More general version

Derive the Optimal Control: First-Order Systems

• To find the optimal control, define the Lagrangian

• Solve the Euler-Lagrange equations

Derive the Optimal Control: First-Order Systems

Example

Lagrangian:

Euler-Lagrange equations:

• Optimal control has the form

Derive the Optimal Control: First-Order Systems

• Which suggests that that the inputs are sinusoid at various frequencies

where is skew symmetric

Steering Controllable Systems Using Sinusoids: First-Order Systems Algorithm

yields

Hopping Robot (First Order)• Kinematic Equations

• Taylor series expansion at l=0

• Change of coordinates ll mm 1/

• Applying algorithm 1 a. Steer l and ψ to desired values by

b. Integrating over one period

Hopping Robot (First Order)

• Nonholonomic motion for a hopping robot

Hopping Robot (First Order)

Steering Controllable Systems Using Sinusoids: Second-Order Systems

Canonical form:

Front Wheel Drive Car (Second Order)• Kinematic Equations

• Change of coordinates

Front Wheel Drive Car (Second Order)• Sample trajectories for the car applying

algorithm 2

Maximal Growth System

• Want vectorfields for which the P. Hall basis is linearly independent

Maximal Growth Systems

Chained Systems

Possible ExtensionsCanonical form associated with maximal growth 2 input systems look

similar to a reconstruction equation

Possible Extensions

• Pull a Hatton…plot vector fields and use the body velocity integral as a height function

• The body velocity integral provides a decent approximation of the system’s macroscopic motion

Plot Vector Fields