Modeling and Trajectory Design for Mechanical Control...
Transcript of Modeling and Trajectory Design for Mechanical Control...
![Page 1: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/1.jpg)
CMU-20may04-p1
Modeling and Trajectory Design
for Mechanical Control Systems
Center for the Foundations of Robotics
Robotics Institute, Carnegie Mellon University
Francesco BulloCoordinated Science Lab
University of Illinois at Urbana-Champaign
1308 W. Main St, Urbana, IL 61801, USA
[email protected], http://motion.csl.uiuc.edu
Thanks to: Jorge Cortes, Andrew D. Lewis, Kevin Lynch, Sonia Martınez
![Page 2: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/2.jpg)
CMU-20may04-p2
Geometric Control of Mechanical Systems
Scientific Interests
(i) success in linear control theory is unlikely to be repeated for nonlinear systems.
In particular, nonlinear system design. no hope for general theory
mechanical systems as examples of control systems
(ii) nonlinear control and geometric mechanics
Framework based on affine connections
(i) reduction from 2n to n dimensional computations
(ii) controllability, kinematic models, planning, averaging not stabilization
![Page 3: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/3.jpg)
CMU-20may04-p3
Literature review
Modeling:
(i) R. Hermann. Differential Geometry and the Calculus of Variations, volume 49 of
Mathematics in Science and Engineering. Academic Press, New York, NY, 1968
(ii) A. M. Bloch and P. E. Crouch. Nonholonomic control systems on Riemannian
manifolds. SIAM JCO, 33(1):126–148, 1995
(iii) A. D. Lewis. Simple mechanical control systems with constraints. IEEE T. Automatic
Ctrl, 45(8):1420–1436, 2000
Reductions & Planning via Inverse Kinematics:
(i) H. Arai, K. Tanie, and N. Shiroma. Nonholonomic control of a three-DOF planar
underactuated manipulator. IEEE T. Robotics Automation, 14(5):681–695, 1998
(ii) K. M. Lynch, N. Shiroma, H. Arai, and K. Tanie. Collision-free trajectory planning
for a 3-DOF robot with a passive joint. Int. J. Robotic Research, 19(12):1171–1184,
2000
(iii) A. D. Lewis. When is a mechanical control system kinematic? In Proc CDC, pages
1162–1167, Phoenix, AZ, December 1999
![Page 4: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/4.jpg)
CMU-20may04-p4
Controllability:
(i) H. J. Sussmann. A general theorem on local controllability. SIAM JCO,
25(1):158–194, 1987
(ii) A. D. Lewis and R. M. Murray. Configuration controllability of simple mechanical
control systems. SIAM JCO, 35(3):766–790, 1997
Averaging:
(i) J. Baillieul. Stable average motions of mechanical systems subject to periodic forcing.
In M. J. Enos, editor, Dynamics and Control of Mechanical Systems: The Falling Cat
and Related Problems, volume 1, pages 1–23. Field Institute Communications, 1993
(ii) M. Levi. Geometry of Kapitsa’s potentials. Nonlinearity, 11(5):1365–8, 1998
Planning via approximate inversion:
(i) R. E. Bellman, J. Bentsman, and S. M. Meerkov. Vibrational control of nonlinear
systems: Vibrational stabilization. IEEE T. Automatic Ctrl, 31(8):710–716, 1986
(ii) W. Liu. An approximation algorithm for nonholonomic systems. SIAM JCO,
35(4):1328–1365, 1997
![Page 5: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/5.jpg)
CMU-20may04-p5
Francesco Bullo and Andrew D. Lewis
Geometric Control of
Mechanical Systems
Modeling, Analysis, and Design for
Simple Mechanical Control Systems
SPIN
– Monograph –
May 19, 2004
Springer
Berlin Heidelberg NewYork
Hong Kong London
Milan Paris Tokyo
![Page 6: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/6.jpg)
CMU-20may04-p6
Outline: from geometry to algorithms
(i) modeling
(ii) approach #1
(a) analysis: kinematic reductions and controllability
(b) design: inverse kinematics catalog
(iii) approach #2
(a) analysis: oscillatory controls and averaging
(b) design: approximate inversion
![Page 7: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/7.jpg)
CMU-20may04-p7
1 Models of Mechanical Control Systems
Ex #1: robotic manipulators with kinetic energy and forces at joints
systems with potential control forces
Ex #2: aerospace and underwater vehicles
invariant systems on Lie groups
Ex #3: systems subject to nonholonomic constraints
locomotion devices with drift, e.g., bicycle, snake-like robots
![Page 8: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/8.jpg)
CMU-20may04-p8
1.1 Basic geometric objects
• manifold Q ⊂ RN Rn,Tn, Sn, SO(3), SE(3)
• vector fields X = (X1, . . . , Xn) : Q 7→ TQ
• metric M is an inner product on TQ and its inverse M−1
matrix representations Mij and inverse Mlm
(i) a connection ∇ is a set of functions Γijk : Q → R, i, j, k ∈ 1, . . . , n(ii) the acceleration of a curve q : I → Q
(∇q q)i= qi + Γijkq
j qk
(iii) the covariant derivative ∇XY of two vector fields
(∇XY )i =∂Y i
∂qjXj + ΓijkX
jY k 〈X : Y 〉 = ∇XY + ∇YX
![Page 9: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/9.jpg)
CMU-20may04-p9
1.2 Constraints, distributions and kinematic modeling
x
y
φ
θ
=
ρ cosφ
ρ sinφ
0
1
v +
0
0
1
0
ω
(unicycle dynamics, simplest wheeled
robot dynamics)
xr
yr
θ
φ
=
cos θ
sin θ
1`tanφ
0
v +
0
0
0
1
ω
![Page 10: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/10.jpg)
CMU-20may04-p10
1.3 SMCS with Constraints: definition
A simple mechanical control system with constraints is
(i) an n-dimensional configuration manifold Q,
(ii) a metric M on Q describing the kinetic energy,
(iii) a function V on Q describing the potential energy,
(iv) a dissipative force Fdiss,
(v) a distribution D of feasible velocities describing the constraints
(vi) a set of m covector fields F = F 1, . . . , Fm defining the control forces
(Q,M, V, Fdiss,D ,F = F 1, . . . , Fm)
![Page 11: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/11.jpg)
CMU-20may04-p11
1.4 SMCS with Constraints: governing equations
Given (Q,M, V, Fdiss,D ,F ), there exists procedure:
∇q q = Y0(q) +R(q) +
m∑
a=1
Ya(q)ua (1)
or, in coordinates:
qk + Γkij(q)qiqj = Y0(q)
k +Rki (q)qi +
m∑
a=1
Y ka (q)ua
or, in different coordinates for the velocities,
q = viXi(q)
vk + Γkij(q)vivj = Y0(q)
k +Rki (q)qi +
m∑
a=1
Y ka (q)ua
![Page 12: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/12.jpg)
CMU-20may04-p12
1.5 Modeling construction (Lewis, IEEE TAC ’00)
From (Q,M, V, Fdiss,D ,F ) to
∇q q = Y0(q) +R(q) +
m∑
a=1
Ya(q)ua
(i) P : TQ → TQ is the M-orthogonal projection onto D
(ii) Y0(q) = −P (M−1(dV ))
(iii) R(q) = P (M−1(Fdiss(q)))
(iv) Ya = P (M−1(F a))
(v) M∇ is the Levi-Civita connection on (Q,M)
Γkij =1
2Mmk
(
∂Mmj
∂qi+∂Mmi
∂qj− ∂Mij
∂qm
)
(2)
(vi) ∇ is the constrained affine connection on (Q,M,D)
∇XY = M∇XY −(
M∇XP)
(Y ) (3)
![Page 13: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/13.jpg)
CMU-20may04-p13
1.6 Planar two links manipulator
(θ1, θ2) ∈ Q = T2
M =
I1 + (l21(m1 + 4m2))/4 (l1l2m2 cos[θ1 − θ2])/2
(l1l2m2 cos[θ1 − θ2])/2 I2 + (l22m2)/4
V (θ1, θ2) = m1gl1 sin θ1/2 + m2g(l1 sin θ1 + l2/2 sin θ2)
no Fdiss
no constraints
F 1 = dθ1, F2 = dθ2 − dθ1
Equations of motion:
θ1 + Γ111θ1θ1 + Γ1
12θ1θ2 + Γ122θ2θ2
θ2 + Γ211θ1θ1 + Γ2
12θ1θ2 + Γ222θ2θ2
= Y0 + u1Y1 + u2Y2
![Page 14: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/14.jpg)
CMU-20may04-p14
1.7 The snakeboard
(x, y, θ, ψ, φ) ∈ Q=SE(2) × T2
F 1 = dψ, F 2 = dφ
M =
m 0 0 0 0
0 m 0 0 0
0 0 `2m Jr 0
0 0 Jr Jr 0
0 0 0 0 Jw
X1 =
` cosφ cos θ
` cosφ sin θ
− sinφ
0
0
, X2 =
0
0
0
1
0
, X3 =
0
0
0
0
1
![Page 15: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/15.jpg)
CMU-20may04-p15
x
y
θ
ψ
φ
=
` cosφ cos θ
` cosφ sin θ
− sinφ
0
0
v1 +
Jr
m`cosφ sinφ cos θ
Jr
m`cosφ sinφ sin θ
− Jr
m`2(sinφ)2
1
0
v2+
0
0
0
0
1
v3
v1 +Jrm`2
(cosφ)v2v3 = 0
v2 −m`2 cosφ
m`2 + Jr(sinφ)2v1v3 −
Jr cosφ sinφ
m`2 + Jr(sinφ)2v2v3 =
m`2
m`2Jr + J2r (sinφ)2
uψ
v3 =1
Jwuφ .
q = viXi(q), vk + (XΓ)kij(q)vivj = Y0(q)
k +Rki (q)qi +
m∑
a=1
Y ka (q)ua
![Page 16: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/16.jpg)
CMU-20may04-p16
1.8 Underwater Vehicle in Ideal Fluid
3D rigid body with three forces:
(i) (R, p) ∈ SE(3), (Ω, V ) ∈ R6
(ii) KE = 12ΩT JΩ + 1
2VTMV ,
M = diagm1,m2,m3,J = diagJ1, J2, J3
(iii) f1 = e4, f2 = −he3 + e5, f3 = he2 + e6
Equations of Motion:
R
p
=
RΩ
RV
,
JΩ − JΩ × Ω + MV × V
MV − MV × Ω.
= u1f1 + u2f2 + u3f3
![Page 17: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/17.jpg)
CMU-20may04-p17
Outline: from geometry to algorithms
(i) modeling
(ii) approach #1
(a) analysis: kinematic reductions and controllability
(b) design: inverse kinematics catalog
(iii) approach #2
(a) analysis: oscillatory controls and averaging
(b) design: approximate inversion
![Page 18: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/18.jpg)
CMU-20may04-p18
2 Analysis of Kinematic Reductions
Goal: (low-complexity) kinematic representations for mechanical control systems
Assume: no potential energy, no dissipation: (Q,M, V = 0, Fdiss = 0,D ,F )
(i) dynamic model with accelerations as control inputs mechanical systems:
∇q q =
m∑
a=1
Ya(q)ua(t) Y = spanY1, . . . , Ym
(ii) kinematic model with velocities as control inputs
q =∑
b=1
Vb(q)wb(t) V = spanV1, . . . , V`
` is the rank of the reduction
![Page 19: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/19.jpg)
CMU-20may04-p19
2.1 When can a second order system follow the solution of a first order?
ex:Can follow any straight line and can turn
2 preferred velocity fields
(plus, configuration controllability)
Ok ? ? ?
![Page 20: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/20.jpg)
CMU-20may04-p20
2.2 Kinematic reductions (Bullo and Lynch, IEEE TRA ’01)
V = spanV1, . . . , V` is a kinematic reduction if any curve q : I → Q solving the
(controlled) kinematic model can be lifted to a solution to a solution of the
(controlled) dynamic model.
rank 1 reductions are called decoupling vector fields
Theorem The kinematic model induced by V1, . . . , V` is a kine-
matic reduction of (Q,M, V =0, Fdiss =0,D ,F)
if and only if
(i) V ⊂ Y
(ii) 〈V : V 〉 ⊂ Y
![Page 21: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/21.jpg)
CMU-20may04-p21
2.3 Examples of kinematic reductions (Bullo and Lewis, IEEE TRA ’03)
Two rank 1 kinematic reductions (decoupling vector fields)
no rank 2 kinematic reductions
![Page 22: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/22.jpg)
CMU-20may04-p22
2.4 Examples of maximally reducible systems
x
y
φ
θ
=
ρ cosφ
ρ sinφ
0
1
v +
0
0
1
0
ω
(unicycle dynamics, simplest wheeled
robot dynamics)
xr
yr
θ
φ
=
cos θ
sin θ
1`tanφ
0
v +
0
0
0
1
ω
![Page 23: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/23.jpg)
CMU-20may04-p23
2.5 When is a mechanical system kinematic? (Lewis, CDC ’99)
When are all dynamic trajectories executable by a single kinematic model?
A dynamic model is maximally reducible (MR) if all its controlled trajectory
(starting from rest) are controlled trajectory of a single kinematic reduction.
Theorem (Q,M, V =0, Fdiss =0,D ,F) is maximally reducible
if and only if
(i) the kinematic reduction is the input distribution Y
(ii) 〈Y : Y 〉 ⊂ Y
![Page 24: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/24.jpg)
CMU-20may04-p24
3 Controllability Analysis
Objective: controllability notions and tests for mechanical systems and reductions
Assume: no potential energy, no dissipation: (Q,M, V = 0, Fdiss = 0,D ,F )
Review: Controllable kinematic systems
q =∑
i=1
Xi(q)ui(t)
given two v.f.s X,Y , Lie bracket: [X,Y ]k =∂Y k
∂qiXi − ∂Xk
∂qiY i LARC
not accessible accessible controllable (STLC)
![Page 25: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/25.jpg)
CMU-20may04-p25
3.1 Controllability mechanisms
given control forces F 1, . . . , Fm
accessible accelerations Y1, . . . , YmYa = P (M−1F a)
⊃
⊃
accessible velocities SymY1, . . . , YmYi, 〈Yj : Yk〉, 〈〈Yj : Yk〉 : Yh〉, . . .
LieV1, . . . , V`: configurations
accessible via decoupling v.f.s
decoupling v.f.s V1, . . . , V`Vi, 〈Vi : Vi〉 ∈ Y1, . . . , Ym
access. configs LieSymY1, . . . , YmYi, 〈Yj : Yk〉, [Yj, Yk], [〈Yj : Yk〉, Yh], . . .
![Page 26: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/26.jpg)
CMU-20may04-p26
3.2 Controllability notions and tests (Lewis and Murray, SIAM JCO ’97)
V1, . . . , V` decoupling v.f.s
rank LieV1, . . . , V` = n
KC= locally kinematically controllable
(q0, 0)u−→ (qf, 0) can reach open set of
configurations by concatenating motions
along kinematic reductions
rank SymY = n,
“bad vs good”
STLC= small-time locally controllable
(q0, 0)u−→ (qf, vf) can reach open set
of configurations and velocities
rank LieSymY = n,
“bad vs good”
STLCC= small-time locally configura-
tion controllable
(q0, 0)u−→ (qf, vf) can reach open set
of configurations
![Page 27: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/27.jpg)
CMU-20may04-p27
3.3 Controllability inferences
STLC = small-time locally controllable
STLCC = small-time locally configuration controllable
KC = locally kinematically controllable
MR-KC = maximally reducible, locally kinematically controllable
STLC
STLCC
KC MR-KC
There exist counter-examples for each missing implication sign.
![Page 28: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/28.jpg)
CMU-20may04-p28
3.4 Cataloging kinematic reductions and controllability of example systems
System Picture Reducibility Controllability
planar 2R robot
single torque at either joint:
(1, 0), (0, 1)
n = 2,m = 1
(1, 0): no reductions
(0, 1): maximally reducible
accessible
not accessible or STLCC
roller racer
single torque at joint
n = 4,m = 1
no kinematic reductions accessible, not STLCC
planar body with single force
or torque
n = 3,m = 1
decoupling v.f. reducible, not accessible
planar body with single gen-
eralized force
n = 3,m = 1
no kinematic reductions accessible, not STLCC
planar body with two forces
n = 3,m = 2two decoupling v.f. KC, STLC
![Page 29: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/29.jpg)
CMU-20may04-p29
robotic leg
n = 3,m = 2
two decoupling v.f., maxi-
mally reducibleKC
planar 3R robot, two torques:
(0, 1, 1), (1, 0, 1), (1, 1, 0)
n = 3,m = 2
(1, 0, 1) and (1, 1, 0): two de-
coupling v.f.
(0, 1, 1): two decoupling v.f.
and maximally reducible
(1, 0, 1) and (1, 1, 0): KC
and STLC
(0, 1, 1): KC
rolling penny
n = 4,m = 2fully reducible KC
snakeboard
n = 5,m = 2two decoupling v.f. KC, STLCC
3D vehicle with 3 generalized
forces
n = 6,m = 3
three decoupling v.f. KC, STLC
![Page 30: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/30.jpg)
CMU-20may04-p30
Summary
• dynamic models (mechanics) vs kinematic models (trajectory analysis)
• general reductions (multiple, low rank) vs MR (one rank = m)
• STLCC (e.g., via STLC) vs kinematic controllability
Outline: from geometry to algorithms
(i) modeling
(ii) approach #1
(a) analysis: kinematic reductions and controllability
(b) design: inverse kinematics catalog
(iii) approach #2
(a) analysis: oscillatory controls and averaging
(b) design: approximate inversion
![Page 31: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/31.jpg)
CMU-20may04-p31
4 Trajectory Design via Inverse Kinematics
Objective: find u such that (qinitial, 0)u−→ (qtarget, 0)
Assume:
(i) (Q,M, V =0, Fdiss =0,D ,F ) is kinematically controllable
(ii) Q = G and decoupling v.f.s V1, . . . , V` are left-invariant
=⇒ matrix exponential exp: g → G gives closed-form flow
Objective: select a finite-length combination of k flows along V1, . . . , V` and
coasting times t1, . . . , tk such that
q−1initialqtarget = gdesired = exp(t1Vi1) · · · exp(tkVik).
No general methodology is available =⇒ catalog for relevant example systems
SO(3), SE(2), SE(3), etc
![Page 32: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/32.jpg)
CMU-20may04-p32
4.1 Inverse-kinematic planner on SO(3) (Martınez, Cortes, and Bullo, IROS ’03)
Any underactuated controllable system on SO(3) is equivalent to
V1 = ez = (0, 0, 1) V2 = (a, b, c) with a2 + b2 6= 0
Motion Algorithm: given R ∈ SO(3), flow along (ez, V2, ez) for coasting times
t1 = atan2 (w1R13 + w2R23,−w2R13 + w1R23) t2 = acos
(
R33 − c2
1 − c2
)
t3 = atan2 (v1R31 + v2R32, v2R31 − v1R32)
where z =
1 − cos t2
sin t2
,
w1
w2
=
ac b
cb −a
z,
v1
v2
=
ac −b
cb a
z
Local Identity Map = RIK−→ (t1, t2, t3)
FK−→ exp(t1ez) exp(t2V2) exp(t3ez)
![Page 33: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/33.jpg)
CMU-20may04-p33
4.2 Inverse-kinematic planner on SO(3): simulation
The system can rotate about (0, 0, 1) and (a, b, c) = (0, 1, 1)
Rotation from I3 onto target rotation exp(π/3, π/3, 0)
As time progresses, the body is translated along the inertial x-axis
![Page 34: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/34.jpg)
CMU-20may04-p34
4.3 Inverse-kinematic planner for Σ1-systems SE(2)
First class of underactuated controllable system on SE(2) is
Σ1 = (V1, V2)| V1 = (1, b1, c1), V2 = (0, b2, c2), b22 + c22 = 1
Motion Algorithm: given (θ, x, y), flow along (V1, V2, V1) for coasting times
(t1, t2, t3) = (atan2 (α, β) , ρ, θ − atan2 (α, β))
where ρ =√
α2 + β2 and
α
β
=
b2 c2
−c2 b2
x
y
−
−c1 b1
b1 c1
1 − cos θ
sin θ
Identity Map = (θ, x, y)IK−→ (t1, t2, t3)
FK−→ exp(t1V1) exp(t2V2) exp(t3V1)
![Page 35: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/35.jpg)
CMU-20may04-p35
4.4 Inverse-kinematic planner for Σ2-systems SE(2)
Second and last class of underactuated controllable system on SE(2):
Σ2 = (V1, V2)| V1 = (1, b1, c1), V2 = (1, b2, c2), b1 6= b2 or c1 6= c2
Motion Algorithm: given (θ, x, y), flow along (V1, V2, V1) for coasting times
t1 = atan2(
ρ,√
4 − ρ2)
+ atan2 (α, β) t2 = atan2(
2 − ρ2, ρ√
4 − ρ2)
t3 = θ − t1 − t2
where ρ=√
α2 + β2,
α
β
=
c1 − c2 b2 − b1
b1 − b2 c1 − c2
x
y
−
−c1 b1
b1 c1
1 − cos θ
sin θ
Local Identity Map = (θ, x, y)IK−→ (t1, t2, t3)
FK−→ exp(t1V1) exp(t2V2) exp(t3V1)
![Page 36: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/36.jpg)
CMU-20may04-p36
4.5 Inverse-kinematic planners on SE(2): simulation
Inverse-kinematics planners for sample systems in Σ1 and Σ2. The systems
parameters are (b1, c1) = (0, .5), (b2, c2) = (1, 0). The target location is (π/6, 1, 1).
![Page 37: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/37.jpg)
CMU-20may04-p37
4.6 Inverse-kinematic planners on SE(2): snakeboard simulation
snakeboard as Σ2-system
![Page 38: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/38.jpg)
CMU-20may04-p38
4.7 Inverse-kinematic planners on SE(2) × R: simulation
4 dof system in R3, no pitch no roll
kinematically controllable via body-fixed constant velocity fields:
V1= rise and rotate about inertial point; V2= translate forward and dive
The target location is (π/6, 10, 0, 1)
![Page 39: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/39.jpg)
CMU-20may04-p39
4.8 Inverse-kinematic planners on SE(3): simulation
kinematically controllable via
body-fixed constant velocity fields:
V1= translation along 1st axis
V2= rotation about 2nd axis
V3= rotation about 3rd axis
V3 : 0 → 1: rotation about 3rd axis
V2 : 1 → 2: rotation about 2nd axis
V1 : 2 → 3: translation along 1st axis
V3 : 3 → 4: rotation about 3rd axis
V2 : 4 → 5: rotation about 2nd axis
V3 : 5 → 6: rotation about 3rd axis
xg
zg
yg
x0
z0y0
0
1
2
3
4
5
6
![Page 40: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/40.jpg)
CMU-20may04-p40
Outline: from geometry to algorithms
(i) modeling and approach #1
• dynamic models (mechanics) vs kinematic models (trajectory analysis)
• general reductions (multiple, low rank) vs MR (one rank = m)
• STLCC (e.g., via STLC) vs kinematic controllability
• catalogs of systems and solutions
(ii) approach #2
(a) analysis: oscillatory controls and averaging
(b) design: approximate inversion
![Page 41: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/41.jpg)
CMU-20may04-p41
5 Averaging Analysis
Oscillations play key role in animal and robotic locomotion, oscillations generate
motion in Lie bracket directions useful for trajectory design
Objective: oscillatory controls in mechanical systems
∇q q = Y (q, t)
∫ T
0
Y (q, t)dt = 0
Assume: (Q,M, V, Fdiss,D ,F ). Let ε > 0
∇q q = Y0(q) +R(q) +
m∑
a=1
1
εua
(
t
ε, t
)
Ya(q),
where ua are T -periodic and zero-mean in their first argument.
![Page 42: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/42.jpg)
CMU-20may04-p42
5.1 Main Averaging Result (Martınez, Cortes, and Bullo, IEEE TAC ’03)
∇q q = Y0(q) +R(q) +
m∑
a=1
1
εua
(
t
ε, t
)
Ya(q),
∇q q = Y0(q) +R(q) −m
∑
a,b=1
Λab(t)〈Ya : Yb〉(q)
Λab(t) =1
2
(
U (a,b)(t) + U (b,a)(t) − U (a)(t)U (b)(t))
U(a)(τ, t) =
∫ t
0
ua(τ, s)ds, U(a,b)(τ, t) =
∫ t
0
ub(τ, s2)
∫ s2
0
ua(τ, s1)ds1ds2
approximation valid over certain time scale
![Page 43: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/43.jpg)
CMU-20may04-p43
5.2 Averaging analysis with control potential forces
Assume no constraints (D = TQ) and F = dϕ1, . . . , dϕm.
Then
Ya(q) = gradϕa(q), (gradϕa)i = M
ij ∂ϕa∂qj
Symmetric product restricts
〈gradϕa : gradϕb〉 ≡ grad 〈ϕa : ϕb〉
where Beltrami bracket (Crouch ’81):
〈ϕa : ϕa〉 = 〈〈dϕa , dϕb〉〉 = Mij ∂ϕa∂qi
∂ϕb∂qj
![Page 44: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/44.jpg)
CMU-20may04-p44
5.3 Averaged potential
M∇q q = − gradV (q) +R(q) +
m∑
a=1
ua(t) grad(ϕa)(q) .
M∇q q = − gradVaveraged(q) +R(q)
Vaveraged = V +
m∑
a,b=1
Λab〈ϕa : ϕb〉
![Page 45: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/45.jpg)
CMU-20may04-p45
5.4 Oscillations stabilization example: a 2-link manipulator
PSfrag replacements
π/2
0
20
40
60
time (sec)
θ1, θ2 (rad)
PSfrag replacements
π/2
0
20 40
60
time (sec)
θ 1,θ
2(r
ad)
u =1
εcos
(
t
ε
)
Two-link damped manipulator with oscillatory control at first joint. The averaging
analysis predicts the behavior. (the gray line is θ1, the black line is θ2).
![Page 46: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/46.jpg)
CMU-20may04-p46
6 Trajectory Design via Oscillatory Controls and
Approximate Inversion
Objective: steer configuration of (Q,M, V, Fdiss,D ,F ) along target trajectory
γtarget : [0, T ] → Q via oscillatory controls:
∇q q = Y0(q) +R(q) +
m∑
a=1
uaYa(q),
Low-order STLC assumption:
(i) spanYa, 〈Yb : Yc〉| a, b, c ∈ 1, . . . ,m is full rank
(ii) “bad vs good” condition: 〈Ya : Ya〉 ∈ Y = spanYa.
![Page 47: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/47.jpg)
CMU-20may04-p47
6.1 From the STLC assumption ...
(i) fictitious inputs zatarget, zabtarget : [0, T ] → R, a < b, with
∇γ′
targetγ′target = Y0(γtarget) +R(γ′target)
+
m∑
a=1
zatargetYa(γtarget(t)) +∑
a<b
zabtarget〈Ya : Yb〉(γtarget(t)),
(ii) for a, b ∈ 1, . . . ,m, bad/good coefficient functions αa,b : Q → R
〈Ya : Ya〉 =
m∑
b=1
αa,bYb .
Also, there are N = m(m− 1)/2 pairs of elements (a, b) in 1, . . . ,m, with
a < b. Let (a, b) 7→ ω(a, b) ∈ 1, . . . , N be a enumeration of these pairs, and
define ω-frequency sinusoidal function
ψω(a,b)(t) =√
2ω(a, b) cos(ω(a, b)t)
![Page 48: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/48.jpg)
CMU-20may04-p48
6.2 Trajectory tracking via Approximate Inversion
(Martınez, Cortes, and Bullo, IEEE TAC ’03)
Theorem Consider (Q,M, V, Fdiss,D ,F ). Let
ua = va(t, q) +1
εwa
(
t
ε, t
)
with
wa(τ, t) =
m∑
c=a+1
zactarget(t)ψω(a,c)(τ) −a−1∑
c=1
ψω(c,a)(τ)
va(t, q) = zatarget(t)+1
2
m∑
b=1
αa,b(q)
j − 1 +
m∑
c=j+1
(zbctarget(t))2
Then, t 7→ q(t) follows γtarget with an error of order ε over the time scale 1.
![Page 49: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/49.jpg)
CMU-20may04-p49
6.3 Oscillatory controls ex. #1: A second-order nonholonomic integrator
Consider
x1 = u1 , x2 = u2 , x3 = u1x2 + u2x1 ,
Controllability assumption ok. Design controls to track (xd1(t), xd2(t), x
d3(t)):
u1 = xd1 +1√2ε
(
xd3 − xd1xd2 − xd2x
d1
)
cos
(
t
ε
)
u2 = xd2 −√
2
εcos
(
t
ε
)
0 10 20 30 40 50
−1
0
1
0 10 20 30 40 50−1
0
1
0 10 20 30 40 50−1
0
1
PSfrag replacements
t
x1
x2
x3
![Page 50: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/50.jpg)
CMU-20may04-p50
7 Summary: from geometry to algorithms
Trajectory design via kinematic reductions
• dynamic models (mechanics) vs kinematic models (trajectory analysis)
• general reductions (multiple, low rank) vs MR (one rank = m)
• STLCC (e.g., via STLC) vs kinematic controllability
• catalogs of systems and solutions
Trajectory design via averaging
• high-amplitude high-frequency two time-scales averaging
• general tracking result based on STLC assumption
trajectory analysis: reduction, controllability, averaging
trajectory design: inverse kinematics and approximate inversion
![Page 51: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/51.jpg)
CMU-20may04-p51
Future research
(i) weaken strict assumptions for reductions approach
V = 0, kinematic controllability, group actions
(ii) render second approach more realistic
(iii) integrate with numerical and passivity methods for trajectory design
(iv) locomotion in fluid (fishes, flying insects, etc)
(v) computational geometry and coordination in multi-vehicle systems
![Page 52: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/52.jpg)
CMU-20may04-p52
Research work reflected in this talk: (http://motion.csl.uiuc.edu)
(i) F. Bullo and M. Zefran. On mechanical control systems with nonholonomic constraints and symmetries.
IFAC Syst. & Control L., 45(2):133–143, 2002
(ii) F. Bullo and K. M. Lynch. Kinematic controllability for decoupled trajectory planning in underactuated
mechanical systems. IEEE T. Robotics Automation, 17(4):402–412, 2001
(iii) F. Bullo, N. E. Leonard, and A. D. Lewis. Controllability and motion algorithms for underactuated
Lagrangian systems on Lie groups. IEEE T. Automatic Ctrl, 45(8):1437–1454, 2000
(iv) F. Bullo. Series expansions for the evolution of mechanical control systems. SIAM JCO, 40(1):166–190,
2001
(v) F. Bullo. Averaging and vibrational control of mechanical systems. SIAM JCO, 41(2):542–562, 2002
(vi) S. Martınez, J. Cortes, and F. Bullo. Analysis and design of oscillatory control systems. IEEE T.
Automatic Ctrl, 48(7):1164–1177, 2003
(vii) F. Bullo and A. D. Lewis. Kinematic controllability and motion planning for the snakeboard. IEEE T.
Robotics Automation, 19(3):494–498, 2003
(viii) F. Bullo and A. D. Lewis. Low-order controllability and kinematic reductions for affine connection control
systems. SIAM JCO, January 2004. To appear
(ix) S. Martınez, J. Cortes, and F. Bullo. A catalog of inverse-kinematics planners for underactuated systems
on matrix Lie groups. In Proc IROS, pages 625–630, Las Vegas, NV, October 2003
(x) F. Bullo. Trajectory design for mechanical systems: from geometry to algorithms. European Journal of
Control, December 2003. Submitted
![Page 53: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/53.jpg)
CMU-20may04-p53
7.1 Examples
![Page 54: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/54.jpg)
CMU-20may04-p54
7.2 Comparison with perturbation methods for mechanical control systems
forced response of Lagrangian system from rest
I) High magnitude high frequency
“oscillatory control &
vibrational stabilization”
H = H(q, p) +1
εϕ
(
q, p, u
(
t
ε
))
p(0) = p0
II) Small input from rest
“small-time local controllability”H = H(q, p) + εϕ(q, p, u(t))
p(0) = 0
III) Classical formulation
integrable Hamiltonian systemsH = H(q, p) + εϕ(q, p)
p(0) = p0
![Page 55: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/55.jpg)
CMU-20may04-p55
7.3 A planar vertical takeoff and landing (PVTOL) aircraft
x = cos θvx − sin θvz
z = sin θvx + cos θvz
θ = ω
vx − vzω = −g sin θ + (−k1/m)vx + (1/m)u2
vz + vxω = −g(cos θ − 1) + (−k2/m)vz + (1/m)u1
ω = (−k3/J)ω + (h/J)u2
Q = SE(2) : Configuration and velocity space via (x, z, θ, vx, vz, ω). x and z are
horizontal and vertical displacement, θ is roll angle. The angular velocity is ω and
the linear velocities in the body-fixed x (respectively z) axis are vx (respectively vz).
u1 is body vertical force minus gravity, u2 is force on the wingtips (with a net
horizontal component). ki-components are linear damping force, g is gravity
constant. The constant h is the distance from the center of mass to the wingtip,
m and J are mass and moment of inertia.
![Page 56: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/56.jpg)
CMU-20may04-p56
7.4 Oscillatory controls ex. #2: PVTOL model
Controllability assumption ok. Design
controls to track (xd(t), zd(t), θd(t)):
u1 =J
hθd +
k3
hθd −
√2
εcos
(
t
ε
)
u2 =h
J− f1 sin θd + f2 cos θd − J
√2
hε
(
f1 cos θd + f2 sin θd)
cos
(
t
ε
)
,
where we let c = Jhθd + k3
hθd and
f1 = mxd +(
k1 cos2 θd + k2 sin2 θd)
xd +sin(2θd)
2(k1 − k2)z
d +mg sin θd− c cos θd ,
f2 = mzd +sin(2θd)
2(k1 − k2)x
d +(
k1 sin2 θd + k2 cos2 θd)
zd +mg(1 − cos θd) − c sin θd .
![Page 57: Modeling and Trajectory Design for Mechanical Control Systemsmotion.me.ucsb.edu/talks/2004d-LHNMNC+CMU-may04.pdf · 2004-08-12 · CMU-20may04-p2 Geometric Control of Mechanical Systems](https://reader034.fdocuments.in/reader034/viewer/2022042111/5e8c92fc705e37501853d6da/html5/thumbnails/57.jpg)
CMU-20may04-p57
7.5 PVTOL Simulations: trajectories and error
0 5 10 15 20 25
−1
0
1
0 5 10 15 20 25
−1
0
1
z
0 5 10 15 20 25−0.5
0
0.5
PSfrag replacements
t
x
y
θ
Error
ε0 0.01 0.02 0.03 0.04 0.05 0.06 0.07 0.08 0.09 0.1
0
0.2
0.4
0.6
0.8
1
1.2
1.4
Error in x Error in z Error in θPSfrag replacements
t
x
y
θ
Err
or
ε
Trajectory design at ε = .01. Tracking errors at t = 10.