kinematska kontrola

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

    qq

    = f(u,

    qq

    ) =

    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