Upload
abaafa23423
View
227
Download
0
Embed Size (px)
Citation preview
7/30/2019 kinematska kontrola
1/55
Robotics
Dynamics & Non-holonomic Systems
1D point mass, damping & oscillation, PID,
general dynamic systems, Newton-Euler, joint
space control, reference trajectory following,
optimal operational space control, car system
equation, path-finding for non-holonomic
systems, control-based sampling, Dubins
curves
Marc Toussaint
FU Berlin
7/30/2019 kinematska kontrola
2/55
So far we always assumed we could arbitrarily set the next robot state qt+1 (or
an arbitrary velocity qt). What if we cant?
Examples:
An air plane flying: You cannot command it to hold still in the air, or to move
straight up.
A car: you cannot command it to move side-wards.
Your arm: you cannot command it to throw a ball with arbitrary speed (force
limits).
A torque controlledrobot: You cannot command it to instantly change velocity
(infinite acceleration/torque).
What all examples have in comment: One can set controls ut (air planes control stick, cars steering wheel, your
muscles activations, torque/voltage/current send to a robots motors)
But these controls only indirectly influence the dynamics of state,
xt+1 = f(xt, ut)
2/42
7/30/2019 kinematska kontrola
3/55
Dynamics
The dynamics of a system describes how the controls ut influence thechange-of-state of the system
xt+1 = f(xt, ut)
The notation xt refers to the dynamic stateof the system: e.g., joint
positions and velocitiesxt = (qt, qt).
f is an arbitrary function, often smooth
3/42
7/30/2019 kinematska kontrola
4/55
Dynamics
The dynamics of a system describes how the controls ut influence thechange-of-state of the systemxt+1 = f(xt, ut)
The notation xt refers to the dynamic stateof the system: e.g., joint
positions and velocitiesxt = (qt, qt).
f is an arbitrary function, often smooth
We define a nonholonomic system as one with differentialconstraints:
dim(ut) < dim(xt)
Not all degrees of freedom are directly controllable
3/42
7/30/2019 kinematska kontrola
5/55
Outline
We discuss three basic examples: 1D point mass
A general dynamic robot
A non-holonomic car model
We discuss how previous methods can be extended to thedynamic/non-holonomic case:
inverse kinematics control optimal operational space control trajectory optimization
trajectory optimization
RRTs RRTs with special tricks
4/42
7/30/2019 kinematska kontrola
6/55
The dynamics of a 1D point mass
Start with simplest possible example: 1D point mass(no gravity, no friction, just a single mass)
State x(t) = (q(t), q(t)) is described by: position q(t) R velocity q(t) R
The controls u(t) is the force we apply on the mass point
The system dynamics is:
q(t) = u(t)/m
5/42
7/30/2019 kinematska kontrola
7/55
1D point mass proportional feedback
Assume current position is q.The goal is to move it to the position q.
What can we do?
6/42
7/30/2019 kinematska kontrola
8/55
1D point mass proportional feedback
Assume current position is q.The goal is to move it to the position q.
What can we do?
IDEA 1Always pull the mass towards the goal q:
u = Kp (q q)
6/42
7/30/2019 kinematska kontrola
9/55
1D point mass proportional feedback
Whats the effect of IDEA 1?
m q = u = Kp (q
q)q = q(t) is a function of time, this is a second order differential equation
7/42
7/30/2019 kinematska kontrola
10/55
1D point mass proportional feedback
Whats the effect of IDEA 1?
m q = u = Kp (q
q)q = q(t) is a function of time, this is a second order differential equation
Solution: assume q(t) = a + bet(an non-imaginary alternative would be q(t) = a + b t cos(t))
7/42
7/30/2019 kinematska kontrola
11/55
1D point mass proportional feedback
Whats the effect of IDEA 1?
m q = u = Kp (q
q)q = q(t) is a function of time, this is a second order differential equation
Solution: assume q(t) = a + bet(an non-imaginary alternative would be q(t) = a + b t cos(t))
m b 2 et = Kp q Kp a Kp b et
(m b 2 + Kp b) et = Kp (q
a) (m b 2 + Kp b) = 0 (q a) = 0 = i
Kp/m
q(t) = q + b ei
Kp/m t
This is an oscillation around q with amplitude b = q(0)
q and
frequency
Kp/m! 7/42
7/30/2019 kinematska kontrola
12/55
1D point mass proportional feedback
m q = u = Kp (q q)
q(t) = q + b ei
Kp/m t
Oscillation around q with amplitude b = q(0)
q and frequency
Kp/m
-1
-0.5
0
0.5
1
0 2 4 6 8 10 12 14
cos(x)
8/42
7/30/2019 kinematska kontrola
13/55
1D point mass derivative feedback
That didnt solve the problem!
IDEA 2Pull less, when were heading the right direction already:
Damp the system:
u = Kp(q
q) + Kd(q
q)q is a desired goal velocity
For simplicity we set q = 0 in the following.
9/42
7/30/2019 kinematska kontrola
14/55
1D point mass derivative feedback
Whats the effect of IDEA 2?
mq = u = Kp(q q) + Kd(0 q)
Solution: again assume q(t) = a + bet
m b 2
et
= Kp q
Kp a Kp b et
Kd b et
(m b 2 + Kd b + Kp b) et = Kp (q
a) (m 2 + Kd + Kp) = 0 (q a) = 0
=
Kd K2d 4mKp
2mq(t) = q + b e t
The term Kd2m in is real exponential decay (damping)
10/42
7/30/2019 kinematska kontrola
15/55
1D point mass derivative feedback
q(t) = q + b e t , = KdK2
d4mKp2m
Effect of the second term K2d 4mKp/2m in :
K2
d < 4mKp has imaginary partoscillating with frequency
Kp/m K2d /4m2
q(t) = q + beKd/2m t ei
Kp/mK2d/4m2 t
K2d > 4mKp realstrongly damped
K2d = 4mKp second term zeroonly exponential decay
11/42
7/30/2019 kinematska kontrola
16/55
1D point mass derivative feedback
illustration from O. Brocks lecture
12/42
7/30/2019 kinematska kontrola
17/55
1D point mass derivative feedback
Alternative parameterization:
Instead of the gainsKp and Kd it is sometimes more intuitive to set the
wave length = 10 = 1Kp/m , Kp = m/2
damping ratio = Kd4mKp
= Kd2m , Kd = 2m/
> 1: over-damped
= 1: critically dampled
< 1: oscillatory-damped
q(t) = q + be/ t ei0
12 t
13/42
7/30/2019 kinematska kontrola
18/55
1D point mass integral feedback
IDEA 3Pull if the position error accumulated large in the past:
u = Kp(q
q) + Kd(q
q) + Ki
t
s=0
(q(s)
q(s)) ds
This is not a linear control law (not linear w.r.t. (q, q))Analytically solving the euqation is hard!
(no explicit discussion here)
14/42
7/30/2019 kinematska kontrola
19/55
1D point mass PID control
u = Kp(q q) + Kd(q q) + Ki
t
s=0
(q q(s)) ds
PID control
Proportional Control (Position Control)
f Kp(q q)
Derivative Control (Damping)
f Kd(q
q) (x
= 0 damping) Integral Control (Steady State Error)
f Kit
s=0(q(s) q(s)) ds
15/42
7/30/2019 kinematska kontrola
20/55
Controlling a 1D point mass lessons learnt
Proportional and derivative feedback (PD control) are like adding aspring and damper to the point mass.
PD control is a linear control law : (q, q) u = Kp(q
q) + Kd(q
q)(linear in the dynamic system state x = (q, q))
With such linear control laws we can design approach trajectories (bytuning the gains).
but no optimality principle behind such motions yet
16/42
7/30/2019 kinematska kontrola
21/55
Controlling a 1D point mass lessons learnt
The point mass is a basic example for a non-holonomic system: The state is x = (q, q) R2 The control u
R is the force
The dynamics are
= f(u,
) =
qu/m
17/42
G l b d i
7/30/2019 kinematska kontrola
22/55
General robot system dynamics
State x = (q, q) R2n joint positions q
R
n
joint velocities q Rn Controls u Rn are the torquesgenerated in each motor. The system dynamics are:
M(q) q+ C(q, q) q+ G(q) = u
M(q) Rnn is positive definite intertia matrix(can be inverted forward simulation of dynamics)
C(q, q)R
n are the centripetal and coriolis forces
G(q) Rn are the gravitational forcesu are the joint torques
More compact:M(q) q+ F(q, q) = u
18/42
7/30/2019 kinematska kontrola
23/55
Computing M and F
M(q) q+ F(q, q) = u
Recall:We have an algorithm to compute all positions and orientations given q:
TWi(q) = TWA TAA(q) TAB TBB(q) TBC TCC(q) TCi
19/42
7/30/2019 kinematska kontrola
24/55
Computing M and F
M(q) q+ F(q, q) = u
Recall:We have an algorithm to compute all positions and orientations given q:
TWi(q) = TWA TAA(q) TAB TBB(q) TBC TCC(q) TCi
This can be extended easily to compute also: linear velocities vi
angular velocities wi
linear accelerations vi
angular accelerations wifor any link i in the robot (via forward propagation; see geometry
notes for details).
19/42
N t E l ( hl )
7/30/2019 kinematska kontrola
25/55
Newton-Euler (roughly)
focussing at the ith link:
Newtons equation expresses the force acting at the center of mass
for an accelerated body:
fi = mvi
Eulers equation expresses the torque acting on a rigid body givenan angular velocity and angular acceleration:
ui = Iiw + w Iw
Idea of Newton-Euler algorithm Force and torque balance at every link
Recursion through all links
20/42
N t E l ( hl )
7/30/2019 kinematska kontrola
26/55
Newton-Euler (roughly)
focussing at the ith link:
Newtons equation expresses the force acting at the center of mass
for an accelerated body:
fi = mvi
Eulers equation expresses the torque acting on a rigid body givenan angular velocity and angular acceleration:
ui = Iiw + w Iw
Idea of Newton-Euler algorithm Force and torque balance at every link
Recursion through all links
Bottom line:There exist algorithms to efficiently compute M(q) and F(q, q) for any
dynamic state (q, q).
20/42
Controlling a general robot joint space approach
7/30/2019 kinematska kontrola
27/55
Controlling a general robot joint space approach
If we know the desired q for each joint, the eqn.M(q) q + F(q, q) = u gives the desired torques.
21/42
Controlling a general robot joint space approach
7/30/2019 kinematska kontrola
28/55
Controlling a general robot joint space approach
If we know the desired q for each joint, the eqn.M(q) q + F(q, q) = u gives the desired torques.
Where could we get the desired q from?Assume we have a nice smooth reference trajectory qref
0:T (generated
with some motion profile or alike), we can at each t read off the desired
acceleration as
qreft :=1
[(qt+1 qt)/ (qt qt-1)/] = (qt-1 + qt+1 2qt)/2
However, tiny errors in acceleration will accumulate greatly over time!
This is Instable!!
21/42
Controlling a general robot joint space approach
7/30/2019 kinematska kontrola
29/55
Controlling a general robot joint space approach
If we know the desired q for each joint, the eqn.M(q) q + F(q, q) = u gives the desired torques.
Where could we get the desired q from?Assume we have a nice smooth reference trajectory qref
0:T (generated
with some motion profile or alike), we can at each t read off the desired
acceleration as
qreft :=1
[(qt+1 qt)/ (qt qt-1)/] = (qt-1 + qt+1 2qt)/2
However, tiny errors in acceleration will accumulate greatly over time!
This is Instable!!
Choose a desired acceleration q
tthat implies a PD-like behavior
around the reference trajectory!
qt = qreft + Kp(q
reft qt) + Kd(qreft qt)
This is a standard and very convenient heuristic to track a reference trajectory
when the robot dynamics are known: All joints will exactly behave like a 1D
point particle around the reference trajectory! 21/42
7/30/2019 kinematska kontrola
30/55
Controlling a robot operational space approach
Recall the inverse kinematics problem: We know the desired step y (or velocity y) of the endeffector.
Which step q (or velocities q) should we make in the joints?
Equivalent dynamic problem: We know how the desired acceleration y of the endeffector.
What controls u should we apply?
22/42
7/30/2019 kinematska kontrola
31/55
Optimal operational space control
Given current state (qt, qt) and yt = (qt), yt = Jqtgiven desired y
compute a torque u such that
1) ||u||2 is small be lazy2) y is close to y
accelerate endeffector
We translate this to the objective function:
f(u) =
||u
||2H +
||JM-1(u
F) + Jq
y
||2C
note: y = ddty = d
dt(Jq) = Jq + Jq = JM-1(u F) + Jq
23/42
7/30/2019 kinematska kontrola
32/55
Optimal operational space control
Optimum: (analogous derivation to kinematic case)
u = T(y Jq+ T F)with T = JM-1 , T = (TCT + H)-1TC
(C T = H-1T(T H-1T)-1)
This generalizes the good oldq = Jy
to the dynamic case, taking account also of intrinsic forces (coriolis and
gravity) and J.
24/42
Controlling a robot operational space approach
7/30/2019 kinematska kontrola
33/55
g p p pp
Where could we get the desired y from?Reference trajectory yref
0:T in operational space!
PD-like behavior in each operational space:
yt = yreft + Kp(y
reft yt) + Kd(yreft yt)
25/42
Controlling a robot operational space approach
7/30/2019 kinematska kontrola
34/55
g p p pp
Where could we get the desired y from?Reference trajectory yref
0:T in operational space!
PD-like behavior in each operational space:
yt = yreft + Kp(y
reft yt) + Kd(yreft yt)
Operational space control: Let the system behave as if we could
directly apply 1D point mass behavior to the endeffector. 25/42
7/30/2019 kinematska kontrola
35/55
3rd example: A car
Note: The previous examples were dynamicsystems in the sense x = (q, q)
The car example is just kinematic (no mass/inertia/forces)
But the treatment is very much the same because a car is also anon-holonomicsystem!
(Just like a car cannot instantly move sidewards, a dynamic system
cannot instantly change its position q: the current change in position is
constrainedby the current velocity q.)
26/42
Car example
7/30/2019 kinematska kontrola
36/55
Car example
x = v cos
y = v sin
= (v/L) tan
|| <
State q =
x
y
Controls u =
v
Sytem equation
x
y
=
v cos
v sin
(v/L) tan
27/42
Car example
7/30/2019 kinematska kontrola
37/55
p
The car is a non-holonomicsystem: not all DoFs are controlled,
dim(u) < dim(q)
We have the differential constraintq:
x sin
y cos = 0
A car cannot move directly lateral!
28/42
Car example
7/30/2019 kinematska kontrola
38/55
p
The car is a non-holonomicsystem: not all DoFs are controlled,
dim(u) < dim(q)
We have the differential constraintq:
x sin
y cos = 0
A car cannot move directly lateral!
General definition of a differential constraint:
For any given state x, let Ux be the tangent space that is generated by controls:Ux = {x : x = f(x, u), u U}
(non-holonomic dim(Ux) < dim(x))
The elements of Ux are elements of Tx subject to additional differential
constraints.28/42
Path finding with a non-holonomic system
7/30/2019 kinematska kontrola
39/55
Could a car follow this trajectory?
This is generated with a PRM in the state space q = (x,y,) ignoring
the differntial constraint. 29/42
Path finding with a non-holonomic system
7/30/2019 kinematska kontrola
40/55
This is a solution we would like to have:
The path respects differential constraints.
Each step in the path corresponds to setting certain controls. 30/42
7/30/2019 kinematska kontrola
41/55
Path finding with a non-holonomic system
Control-based sampling: fulfils none of the nice exploration propertiesof RRTs but fulfils the differential constraints:
1) Select a q T from tree of current configurations
2) Pick control vector u at random
3) Integrate equation of motion over short duration (picked at random
or not)
4) If the motion is collision-free, add the endpoint to the tree
31/42
7/30/2019 kinematska kontrola
42/55
Control-based sampling for the car
1) Select a q
T
2) Pick v, , and 3) Integrate motion from q
4) Add result if collision-free
32/42
7/30/2019 kinematska kontrola
43/55
J. Barraquand and J.C. Latombe. Nonholonomic Multibody Robots:
Controllability and Motion Planning in the Presence of Obstacles. Algorithmica,
10:121-155, 1993.
car parking 33/42
7/30/2019 kinematska kontrola
44/55
car parking
34/42
7/30/2019 kinematska kontrola
45/55
parking with only left-steering
35/42
7/30/2019 kinematska kontrola
46/55
with a trailer
36/42
Better control-based exploration: RRTs revisited
7/30/2019 kinematska kontrola
47/55
RRTs with differential constraints:Input: qstart, number k of nodes, time interval
Output: tree T = (V, E)1: initialize V = {qstart}, E = 2: for i = 0 : k do
3: qtarget random sample from Q4: qnear
nearest neighbor of qtarget in V
5: use local planner to compute controls u that steer qnear towards
qtarget6: qnew qnear +
t=0
q(q, u)dt
7: if qnew Qfree then V V {qnew}, E E {(qnear, qnew)}8: end for
Crucial questions: How meassure near in nonholonomic systems?
How find controls u to steer towards target? 37/42
Metrics
7/30/2019 kinematska kontrola
48/55
Standard/Naive metrics:
Comparing two 2D rotations/orientations 1, 2 SO(2):
a) Euclidean metric between ei1 and ei2
b) d(1, 2) = min{|1 2|, 2 |1 2|}
Comparing two configurations (x , y , )1,2 in R2:
Eucledian metric on (x , y , ei)
Comparing two 3D rotations/orientations r1, r2 SO(3):
Represent both orientations as unit-length quaternions r1, r2 R4
:d(r1, d2) = min{|r1 r2|, |r1 + r2|}
where | | is the Euclidean metric.
(Recall that r1 and r1 represent exactly the same rotation.)
38/42
Metrics
7/30/2019 kinematska kontrola
49/55
Standard/Naive metrics:
Comparing two 2D rotations/orientations 1, 2 SO(2):
a) Euclidean metric between ei1 and ei2
b) d(1, 2) = min{|1 2|, 2 |1 2|}
Comparing two configurations (x , y , )1,2 in R2:
Eucledian metric on (x , y , ei)
Comparing two 3D rotations/orientations r1, r2 SO(3):
Represent both orientations as unit-length quaternions r1, r2 R4
:d(r1, d2) = min{|r1 r2|, |r1 + r2|}
where | | is the Euclidean metric.
(Recall that r1 and r1 represent exactly the same rotation.)
Ideal metric:Optimal cost-to-go between two states x1 and x2:
Use optimal trajectory cost as metric
This is as hard to compute as the original problem, of course!!
Approximate, e.g., by neglecting obstacles. 38/42
7/30/2019 kinematska kontrola
50/55
Dubins curves
Dubins car: constant velocity, and steer [, ]
Neglecting obstacles, there are only six types of trajectories thatconnect any configuration R2 S1:
{LRL,RLR,LSL,LSR,RSL,RSR}
annotating durations of each phase:{LR L, , RL R, LSdL, LSdR, RSdL, RSdR}
with
[0, 2),
(, 2), d
0
39/42
Dubins curves
7/30/2019 kinematska kontrola
51/55
Dubins curves
By testing all six types of trajectories for (q1, q2) we can define aDubins metric for the RRT and use the Dubins curves as controls!
40/42
Dubins curves
7/30/2019 kinematska kontrola
52/55
By testing all six types of trajectories for (q1, q2) we can define aDubins metric for the RRT and use the Dubins curves as controls!
Reeds-Shepp curves are an extension for cars which can drive back.(includes 46 types of trajectories, good metric for use in RRTs for cars)
40/42
Summary
7/30/2019 kinematska kontrola
53/55
We discuss 3 examples for dynamic/non-holonomic systems:
1D point mass
We learnt about PID and the corresponding damped/oscilatory trajectories General robot dynamics M(q) q+ C(q, q) q+ G(q) = u
We learnt how to follow a reference trajectory q0:T (joint space approach)
We learnt about optimal operational space control and following a task space
trajectory y0:T (operational space control)
A car We learnt about path finding under differential constraints
This generalizes also to path finding for dynamic robots (see LaValles
Planning Algorithms)
41/42
Summary
7/30/2019 kinematska kontrola
54/55
We discuss 3 examples for dynamic/non-holonomic systems:
1D point mass
We learnt about PID and the corresponding damped/oscilatory trajectories General robot dynamics M(q) q+ C(q, q) q+ G(q) = u
We learnt how to follow a reference trajectory q0:T (joint space approach)
We learnt about optimal operational space control and following a task space
trajectory y0:T (operational space control)
A car We learnt about path finding under differential constraints
This generalizes also to path finding for dynamic robots (see LaValles
Planning Algorithms)
We did not talk about trajectory optimization. But the general principle is
obvious from slide 23:
f(u0:T) =
t||ut||
2H + t(xt)
t(xt)
where xt depends on the controls u0:t-1, and t(xt) can be any task vector
containing positional (as in the kinematic case) or velocity-type task error.
41/42
Summary
7/30/2019 kinematska kontrola
55/55
State Controls Dynamics
general notation x u x = f(x, u)
Kinematic control x = q u = q q = u (can be set)
1D Point mass x =
q
q
u = force x =
q
u/m
General dynamic
system
x =
q
q
u = torque x =
q
M-1(u F)
Car q =
x
y
u =
v
q =
v cos
v sin (v/L) tan
Only kinematic control has dim(u) = dim(x), all others
dim(u) < dim(x)
42/42