Transcript Ppt (1.6Mb)

Computing stable equilibrium
stances of a legged robot in
frictional environments
Yizhar Or
Dept. of ME, Technion – Israel Institute of Technology
Ph.D. Advisor: Prof. Elon Rimon
g
x3
x1
x2
Outline
2D:
• Computation of frictional equilibrium stances
• Robustness w.r.t disturbance forces and torques
• Dynamics – contact modes and strong stability
3D:
• The support polygon principle for flat terrains
• Geometric Parametrization of equilibrium forces in 3D
• Exact Computation of frictional equilibrium stances
• Polyhedral approximation of equilibrium stances
Problem Statement
Setup: mechanism modeled as a variable c.o.m. body
Given a 2D (3D) multi-limbed mechanism standing on a terrain
with k frictional contacts, where should the center-of-mass be for:
• Static equilibrium?
• Robustness w.r.t disturbance forces?
• Dynamic stability?
g
Applications
• Quasistatic legged locomotion on rough terrain
(spider robots, snake robots, climbing, search-and-rescue robots)
• Graspless manipulation (part feeding, assemblies)
• Motion planning for Hybrid wheeled-legged robots
• Semi-dynamic locomotion
Application - Three-Legged Locomotion
2-3-2 gait pattern:
• Select 2-contact postures that share a common contact point
• 3-contact stage connecting two consequent 2-contact postures
• At the 3-contact stage: straight line motion of center-of-mass
Related Work – 2D
• Mason, Rimon, Burdick (1995):
Frictionless postures under gravity
• Mason (1991): Graphical methods for frictional equilibrium in 2D
• Erdmann (1998): Two-palm manipulation with friction in 2D
• Lotstedt (1982); Erdmann (1984); Mason & Wang (1988);
Rajan, Burridge and Schwartz (1987); Dupont (1992):
Contact modes and frictional dynamic ambiguity
• Trinkle & Pang (1998):
Strong stability, LCP formulation of frictional dynamics
• Greenfield, Choset and Rizzi (2005):
planning quasistatic climbing via bracing
Related Work – 3D
• McGhee and Frank, 1968:
The support polygon principle for legged locomotion
• Mason, Rimon and Burdick , 1997:
Computing stable equilibrium frictionless stances in 3D
• Han, Trinkle and Li, 2000:
Feasibility test of frictional postures in 3D as LMI problem
• Bretl and Lall, 2006:
Adaptive polyhedral approximation of 3D equilibrium stances
• Bretl, Latombe (2003): PRM-based motion planning
algorithm for climbing on vertical walls with discrete supports
Statics in 2D - LP Formulation
• Center of mass:
• External wrench:
• Contact forces:
x2
w=(fext,text) 2
f 2k
G f 
 f ext 
G  f   

• Equilibrium condition:
τ(
)
x


 τ
where t(x)= x Jfext+text
• Friction Cones Bounds:
Bf ≥ 0
Statics - LP Formulation (cont’d)
Theorem:
The feasible k-contact equilibrium region:
R(w) = {x: tmin  xJfext + text  tmax}
where
tmin = min{-Gtf}
tmax = min{-Gtf}
s.t.
Gff=-fext
s.t.
Gff=-fext
Bf ≥ 0
Bf ≥ 0
Infinite strip parallel to fext
Two Contacts Graphical Example
R(wo)
wo=(fg,0)
m = 0.3
g
x1
x2
Two Contacts Graphical Example (cont’d)
R(wo)
g
m = 2.0
x1
x2
2 Contacts - Graphical Characterization
++
S
+-
S
S++ = Strip (C1+,C2+)
S+ - = Strip (C1+,C2-)
S-- = Strip (C1-,C2-)
S -+ = Strip (C1-,C2+)
x2
P
Theorem:
R(wo)
x1
P = Strip (x1, x2)
R(wo) = [(S++  S-- )  P]
_
 [(S+-  S -+ )  P ]
k-Contacts - Graphical Characterization
R(w) =
=conv{Rij (w) }
R(wo)
g
R13
R56
Algorithm:
δ max
O(k  log
)
δ min
x1
x2
x3
x4
x5
x6
External Wrench Neighborhood
• Wrench magnitude scales static response
• Parametrize wext=(fx,fy,text):
g
fx
p
 tan β
fy
τ ext
q
 dx
fy
•
b
c.o.m.
fext
dx
External wrench neighborhood:
N = {(p,q): -k≤p≤k , -n≤q≤n}
• Robust Equilibrium Region: R(N) = R(w)
wN
Robust Equilibrium Region – Example
R(N)
wN
= R(w)
fext
fgfext
n
Recipe:
If N = conv {wi}
R(N)
x1
Then R(N) =
R(wi) i
x2
Dynamic Contact Modes Theory
ma = fext + f1 + f2 + …+ fk
3 equations
3+2k unknowns
Ica = text + (x1-x)×f1 +… (xk-x) ×fk
Contact modes (F, R, U, W)
(or S)
• Contact modes add 2k equations
 a unique dynamic solution as a function of x
• Contact Mode’s inequalities  Feasibility Region of x
Example of Dynamic Ambiguity
Contact mode UF:
g
The Strong Stability Criterion
Strong Stability (Trinkle and Pang, 1998):
S (w) = RSS(w) - RFF (w)  RUF (w)  ... 
RWW(w)
• Eliminates ambiguity – only static solution is feasible
• Any roll/slide/break motion cannot evolve (at zero velocity)
• Yet, not formally related to classical dynamic stability
(bounded response to bounded position/velocity perturbations)
• Does not always imply bounds on c.o.m. height
 Must be augmented with robustness
Robust Stability - Definitions
Strong Stability:
S (w) = RSS(w) - RFF (w)  RUF (w)  ... 
RWW(w)
Robust Stability:
S(N) = S(w)
wN
Define:
Robust Equilibrium Region:
RSS(N) = RSS(w)
wN
Non-Static Modes’ N-Feasible Region:
RXY(N) = 
RXYwN
(w)
Robust Stability Region:
S(N) = RSS(N) - RFF(N)  RUF(N) ...  RWW(N)
Non-Static Modes N-Feasible Region
•
Definition:
RXY(N) = wN
 RXY(w)
• Express RXY as an intersection of halfspaces
in a four-dimensional space: Fi(x,y,p,q) ≥ 0
• RXY(N) is the projection of RXY onto xy plane
The Silhouette Theorem:
The Silhouette curves of the projection are critical
values of the projection function, on which the
generalized normal of RXY is parallel to xy plane.
N-Feasible Region of UF Mode
• Critical curves fi(x,y,p,q) are
linear in p,q and quadratic in x,y
• Critical curves generate
cell arrangement in xy planeN
• Line-Sweep Algorithm:
identifies the cells and generates sample points
• Checking cell membership: LP problem in p,q
Example - Robust Stability Region
S(N) = RSS(N) - RFF(N)  RUF(N) ...  RWW(N)
r=0.05
r=0.1
r=0.25

I 
 ρ  c 
m

N
S(N)
N
S(N)
N
N
S(N)
S(N)
Strong Stability and Dynamic stability
Force Closure  asymp. stability under keep-contact perturbations
Here: no force closure, passive contacts, arbitrary perturbations
Two contacts - neutral stability under keep-contact perturbations
Strong Stability  non-static mode decays until collision
• How to model collisions? treat sequence of collisions?
• Does strong stability really leads to dynamic stability?
• How to design stabilizing joints’ control laws
for a legged robot?
Frictional Equilibrium Stances in 3D
• Analyze 3D equilibrium stances of legged
mechanisms in frictional environments
• Support Polygon criterion does not
apply for non-flat terrains
• Exact formulation of equilibrium region
• Efficient conservative approximation by
projection of convex polytopes
g
Problem Statement
• Characterize feasible equilibrium postures of a multi-limbed
mechanism supported against frictional environment in 3D.
• Given k frictional contacts, find the feasible region R
of center-of-mass locations achieving frictional equilibrium.
• Assumption: point contacts, uniform friction coefficient m.
• Friction Cones in 3D:
Ci = {fi : (fi⋅ni)≥0 and (fi⋅si)2 + (fi⋅ti)2 ≤ m2(fi⋅ni)2}
• Feasible equilibrium region in 3D:

  x :


 f1 
 fk   f g 
 , f i  i 

    
  
 x1  f1 
 xk  f k   x  f g 

Basic Properties of R
•
R is a convex and connected set.
•
~
R is a vertical prism with horizontal cross-section .
• The dimension of R is generically min{k,3}.

~
Focus on computing the boundary of  for 3-contact stances.
Assumption: upward pointing contacts: fi e > 0 for all fi 
Ci ,
where e is the upward direction
Motivational Example
m = 0.5
R
z
g
y
x
The Support Polygon Principle:
x must lie in the vertical prism spanned by the contacts:
Motivational Example (cont’d)
top view
X3
m = 0.2
z
g
x3
y
~
 ???
y
X1
x2
X2
x1
x
x
Support Polygon Principle is unsafe!!!
Parametrizing Equilibrium Forces
• Horizontal and vertical components:
~
   ET fi , fi z  e  fi , ~
   E T xi , xiz  e  xi , ~  E T 
1 0
0 
where E  0 1 , e  0


 
0 0
1
• fi must intersect a common vertical line lr

~
  must intersect at a common point r in horizontal plane
~
   σ λi ( r)( r  ~
xi )
where λi ( r)  ( r  ~
xi 1 )  J ( ~
xi  2  ~
xi 1 )
0  1
σR ,J  

1
0


Permissible Polygonal Region of r
• Projected frictional constraints:
~ ~
f i  i
• r must lie in the polygonal region P = P+ P- ,where
Graphical Example of P
top view
~
3
y
~
1
P+
~
2
Px
Complete Graphical Parametrization
• Action line of fi intersects the common vertical line lr at pi
• Define zi – height of pi about xi
zi = e∙(pi – xi)
p2
• Parametrize contact forcespby (r,z)  2 3, where
z=(z1,z2,z3):
3
p1
where
Permissible Region in (r,z) space
P Q=Q Q Q ,
• The permissible region: (r,z) 
1
2
3
where
and
Q = Q1  Q2  Q3
Qi
where
Ci
• for fi lying on the boundary of Ci , zi=zi*(r)
P
~
Computing the Boundary of 
• Torque balance implies a map
from (r,z) to :

~
Horizontal cross section  is the image of Q under
• Formulate the restriction of
• Compute critical curves of
to all possible manifolds of Q
on each manifold of Q
~
• Candidate boundary curves of  are -image of critical curves
~
Graphical Example of 
type-2 boundary
fiC
, f Cj,
r=r* i j
y
type-3 boundary
fi Ci, i=1..3
~

x
type-1 boundary
fi,fj≠0 ; fk=0
Conservative Polyhedral Approximation
• Replacing exact friction cones with inscribed pyramids.
• Reduces to projection of a convex polytope onto a plane
• Approximate outer bound by taking circumscribing pyramids
• Graphical example – with 6-sided pyramids
Polyhedral Approximation of R Example
top view
x3
~
 ''
y
~
'
x2
x1
x
Future Research
• Physical geometric intuition of boundary curves, effect of m
Already done
• Relation to line geometry and parallel robots’ singularities
• Generalization to multiple contact points
In progress
• Robustness with respect to disturbance forces and torques
• Elimination of non-static contact modes
(Complementarity formulation, Pang and Trinkle, 2000)
• Application to legged locomotion on rough terrain in 3D
Computing stable equilibrium
stances of a legged robot in
frictional environments
Yizhar Or
Dept. of ME, Technion – Israel Institute of Technology
Ph.D. Advisor: Prof. Elon Rimon
[email protected]
robots.technion.ac.il/yizhar
g
x3
x1
x2
Thank You
‫תודה רבה‬