1

Lecture 26. Control of the Diver

In order for a diver to do what he or she doesthe diver applies effective torques at the joints

We want to find a recipe for doing this that will cause the simulated diver to execute the diver

This is a control problem

2

Tomorrow’s office hours will be

1:30 to 3:30

I have to leave at 3:30

3

We have a special case of nonlinear control hereand I want to explore that, beginning with some discussion

and then some simpler models

4

Suppose we want the solution of a second order system to follow some prescription

€

y t( ) → yd t( )

We can write the second order system

€

˙ ̇ y + 2ζλ ˙ y + λ2y = f t( )

The error

€

e = y t( ) − yd t( ) can be made to satisfy the homogeneous equation

€

˙ ̇ e + 2ζλ ˙ e + λ2e = 0

by the proper choice of f(t)

5

€

f t( ) = ˙ ̇ y d + 2ζλ ˙ y d + λ2yd

choose

€

˙ ̇ y + 2ζλ ˙ y + λ2y = f t( )

then

€

˙ ̇ y + 2ζλ ˙ y + λ2y = ˙ ̇ y d + 2ζλ ˙ y d + λ2yd

is the desired error equation

€

˙ ̇ e + 2ζλ ˙ e + λ2e = 0

6

This can be extended to a quasilinear system, which is what we typically have

€

˙ ̇ y = g ˙ y ,y, t( ) + f t( )

€

f t( ) = ˙ ̇ y d + 2ζλ ˙ y d + λ2yd − 2ζλ ˙ y − λ2y − g ˙ y , y, t( )

converts the nonlinear equation to the linear form that yields a decaying error

given

this choice for f(t)

This is called feedback linearization

7

What are we doing here?

€

˙ ̇ y = ˙ ̇ y d + 2ζλ ˙ y d + λ2yd − 2ζλ ˙ y − λ2y

We are asking that

€

˙ ̇ y become

and we choose the forcing function to make this happen

This is cool, but it’s only 1D — what happens in a coupled system?

8

For the class of problems we are considering, which includes divers and some robots,we can choose the generalized coordinates such that

each external force appears in only one reduced Hamilton equation

This is not necessary, but it cleans up the algebra and I’ll assume we’ve taken the trouble to do that, so my simplest coupled case is

€

a11˙ ̇ y 1 + a12˙ ̇ y 2 + g1 ˙ y 1,y1, ˙ y 2,y2, t( ) = f1 t( )

a21˙ ̇ y 1 + a22˙ ̇ y 2 + g2 ˙ y 1,y1, ˙ y 2,y2, t( ) = f2 t( )

9

€

˙ ̇ y 1 = ˙ ̇ y 1d + 2ζ 1λ1 ˙ y 1d + λ12y1d − 2ζ 1λ1 ˙ y 1 − λ1

2y1

€

˙ ̇ y 2 = ˙ ̇ y 2d + 2ζ 2λ 2 ˙ y 2d + λ 22 y2d − 2ζ 2λ 2 ˙ y 2 − λ 2

2 y2

substitute

into the system and solve for f1 and f2

When these are substituted back into the original equations we obtain

10

€

a11 ˙ ̇ y 1 + 2ζ 1λ1 ˙ y + λ12y1 − ˙ ̇ y 1d − 2ζ 1λ1 ˙ y 1d − λ1

2y1d( )

+ a12 ˙ ̇ y 2 + 2ζ 2λ 2 ˙ y + λ 22 y − ˙ ̇ y 2d − 2ζ 2λ 2 ˙ y 2d − λ 2

2 y2d( ) = 0

€

a21 ˙ ̇ y 1 + 2ζ 1λ1 ˙ y + λ12y1 − ˙ ̇ y 1d − 2ζ 1λ1 ˙ y 1d − λ1

2y1d( )

+ a22 ˙ ̇ y 2 + 2ζ 2λ 2 ˙ y + λ 22 y − ˙ ̇ y 2d − 2ζ 2λ 2 ˙ y 2d − λ 2

2 y2d( ) = 0

and if the system is not singular

€

a11a22 − a12a21 ≠ 0( )

then the identical parenthetical equations must equal zero: the errors vanish asymptotically

11

This looks like magic, but it isn’t

To make this work we have a necessary condition: as many forces as controlled variables

There’re more, but I can’t give them all to you.

It works for sufficiently simple systems with only revolute joints —divers and many robots (not the Stanford arm)

I’d like to look at the inverted double pendulum from this perspective before moving on to more complicated systems

12

Planar inverted double pendulum: base fixed to the ground

parameters:l1, l2m1, m2

I treat them as slendermembers

13

€

q =ψ1

ψ 2 −ψ1

⎧ ⎨ ⎩

⎫ ⎬ ⎭

We know how the double pendulum goes

Six holonomic constraints to restrict it to the x = 0 plane

Four connectivity constraints relating CMs to the ground

Apply all ten constraints and define a two dimensional q

14

We can form a Lagrangian

15

There are no additional constraints, so we can let qdot equal u

We can find p

and then the pieces of Hamilton’s equations

16

17

We have four differential equations and seven algebraic equations

(We can actually look at these in the Mathematica notebook)

The next issue is designing the control

€

u = ˙ q =˙ ψ 1

˙ ψ 2 − ˙ ψ 1

⎧ ⎨ ⎩

⎫ ⎬ ⎭⇒ ˙ u = ˙ ̇ q =

˙ ̇ ψ 1˙ ̇ ψ 2 − ˙ ̇ ψ 1

⎧ ⎨ ⎩

⎫ ⎬ ⎭

€

˙ ̇ ψ 1, ˙ ̇ ψ 2so by assigning we assign

€

˙ u 1, ˙ u 2

18

We can look at various desired angles, both steady and time-dependent

Here’s the algorithm for vertical stabilization

19

and the result

20

Here we ask both links to follow sinusoidal paths out of phase

21

and here’s the result of that

22

Here are the torques required to perform the tracking shown on the previous slide

23

How about a simple diving problem — a three link diver?

24

Three links —> 18 variables

Confine to the x constant plane —> nine variables

Four connectivity constraints —> five variables

Number the links from the bottom to the topand select the middle link as the reference link

25

There are two torques — one at each joint — as shown in the sketchand we want to choose our variables accordingly

€

q =

y2

z2

ψ 2

ψ1 −ψ 2

ψ 3 −ψ 2

⎧

⎨

⎪ ⎪ ⎪

⎩

⎪ ⎪ ⎪

⎫

⎬

⎪ ⎪ ⎪

⎭

⎪ ⎪ ⎪

26

We get the Lagrangian as usual

27

This is a fancy torque, suitable for 3D

but of course only two of the components actually do anything

The important thing is that the choice of q has isolated the two torques

28

Note that we have written this in terms of the desired qs, not the yi

The next slide shows the development of the Zs and their gradients

29

30

We need the symbolic versions to build the equations we intend to integrate

31

32

The two internal angles start at zero, go to a maximum of 2π/3, and back to zeroin the time interval tf

Midway through the ends of the two outboard links should touch

We can do this integration and see what the results are

We can start by looking at the closest approach, when the two links are to touch

33

34

35

36

37

38

It is now probably worth looking at the Mathematica notebooks