slide 1 of 21 balancing the segway - … slideshow-1.pdf · slide 3 of 21 segway body ü point...

of 21

Balancing the SegwayCharles [email protected]://nxtotherway.webs.com

of 21

Objective

To demonstrate that using available software to solve systems ofdifferential equations, it is not necessary to know elaborate control theory to solve the two wheel balancing problem.

2 Segway Slideshow.nb

of 21

Segway Body

ü Point mass, m

ü Tilt angle q from vertical reference

ü Wheel angle b from arm reference

Segway Slideshow.nb 3

of 21

Loop Model

MotorSegwayBodyS

q°

b°

s

b

fGyroOffset

Controller

ü Strategy for SolutionË Model the Motor and the Segway Body

Ë Design the Controller


of 21

Software Implementation of the Controller

while ( balancing ) { PHI[2] = ReadGyro( ) ; // read the gyro, tilt angle change per time, deg/sec

// This line implements the controller as a digital filter SIGMA[2] = -0.369389 PHI@0D + 0.389283 PHI@1D - 1.05127 SIGMA[0] + 2.05148 SIGMA[1] ; // shift the registers in preparation for the next cycle SIGMA[0] = SIGMA[1] ; SIGMA[1] = SIGMA[2] ; PHI[0] = PHI[1] ;

SetMotorSpeed( SIGMA[2] );

Wait( 12 milliseconds); }


f@nD

s@nD

Delay Delay Delay-0.369389

0.389283 2.05148

- 1.05127

S


of 21

5 Minute Frequency Domain Speed Date

1

m s2FHsL X(s)

1 D(s)

1s

U(s)

F(s) = m s2 XHsL

f(t) = m D2 x(t)

dHtL

uHtL

Time Domain Frequency Domain

Newton's Second Law


of 21

5 Minute Frequency Domain Speed Date (Continued)

1 - e-sT

sP(s)pHtL

1

m s2X(s)

gHt - TL GHsL e-s T

Note: In the discrete-time frequency domain, z ª es T is the independent variable.

gHn TL gn z-n

1 - e-sT

s

In[26]:= PlotB:InverseLaplaceTransformB1 - E-s 1

s

1

1 s2, s, tF,

InverseLaplaceTransformB1 - E-s 1

s, s, tF>, 8t, 0, 3<F

Out[26]=

0.5 1.0 1.5 2.0 2.5 3.0

0.5

1.0

1.5

2.0

2.5


of 21

Modeling of Segway Body (Lagrangian Mechanics)

y

x

Coordinates of point mass “m”...x(t) = r [q(t) + b(t)] + l sin(q(t))y(t) = l cos(q(t))

Kinetic energy, T...

T = 12m B „

„t xHtLE2+ A „

„t yHtLE2

Potential energy, V...V = m g y(t)


of 21

Modeling of Segway Body (Lagrangian Mechanics)

ü Lagrangian, LË L = T - V

Ë L =12m I@l q£HtL sinHqHtLLD2 +@l q£HtL cosHqHtLL+ r Hb£HtL+ q£HtLLD2M- m g l cosHqHtLL

ü Lagrangian equation for each coordinate (q, b).

Ë„„t

!L!q° - !L!q = tq

Ë„„t

!L!b° - !L!b = tb

Ë Torque on arm = - Torque on wheel

Ë„„t

!L!q° - !L!q = -J „

„t!L!b° - !L!b M

ü Result this equation describes the Segway body (relating q(t) to b(t))

l sinHqHtLL Ig+ 2 r q£HtL2M !q££HtL Il2 + 3 l r cosHqHtLL+ 2 r2M+ r b££HtL Hl cosHqHtLL+ 2 rL

ü Linearized Result

QHsLBHsL

=- rl+r s

2

s2 - g lHl+rL Hl+2 rL


of 21

Simulation of Body Model (no feedback control)

Out[28]=

t

-10 -5 0 5 10 15 20-15

-10

-5

0

5

10

15t ! 0.00


of 21

Modeling of NXT Motorü Assume single pole response

Ë b£HtL+ tM b££HtL ! aMsHtL Hthis equation describes the motorL

Out[29]=

aM 8.5

tM 0.06

20 40 60 80 100

-200

-100

100

200

Unloaded motor output, b° (deg/sec), 10 ms interval

Blue: Data Red: Model Black: Motor input, s (30 unit step)


of 21

Modeling of NXT Motor (continued)

Out[30]=

aM 4.78

tM 0.0256

100 200 300 400 500 600

-40

-20

20

40

60

80

100

Motor output, b (deg), measurement under actual balancing conditionsBlue: DataRed: ModelGreen: Input, s


of 21

Controller Designü “NXTWay”

S

f(s)

s(s)

k1

k2 ês

k3 s

k4

b(s)

Ë s(s) = k1 f(s) + k2s f(s) + k3 s bHsL + k4 b(s)


of 21

Controller Designü “AnotherWay”

S

f(s)

s(s)1s

k2

s

k3

s s(s)

k1

k4

s

Ë s s(s) = k1 f(s) + k2s f(s) + k3 s(s) + k4s s(s)


of 21

Controller Design (Continued)Ë Simulation of differential equations describing the system...

l Sin@q@tDD Ig + 2 r q£@tD2M =

r H2 r + l Cos@q@tDDL b££@tD + Il2 + 2 r2 + 3 l r Cos@q@tDDM q££@tD;

b£ HtL + tM b££HtL ! aM s HtL;

f HtL = q£@tD + offset ;

s££@tD = k1 f£@tD + k2 f@tD + k3 s£@tD + k4 s@tD;

Out[31]=

k1 37

k2 194

k3 4.8

k4 2

2 4 6 8 10

-50

50

100

150

Continuous time simulation, .5deg/sec gyro offset, -1.5 deg/sec q° offset

Red: Tilt angle, q (x 100)Blue: Controller output, s


of 21

Continuous Time Simulation

t

-5 0 5 10 15-15

-10

-5

0

5

10

15t ! 0.00


of 21

Parameter Sensitivity Simulation

Out[33]=

offset 0

aM 5

tM 0.033

2 4 6 8 10

-0.5

-0.4

-0.3

-0.2

-0.1

0.1

Red: Tilt angle, q (x 100)Blue: Controller output, s


of 21

Digital Controller ImplementationË Differential Equation

s ''[t] = 194 f[t] + 37 f '[t] + 2 s[t] + 4.8 s '[t]

Ë Frequency Domain GainsHsLfHsL = 194 + 37 s

-2 -4.8 s+ s2

Ë Use software to convert to discrete time modelË Zero Order Hold method is good (also called step invariant transform)

Ë Mathematica function “ToDiscreteTimeModel”, Matlab function “c2d”

sHzLfHzL =

-0.369389+0.389283 z1.05127- 2.05148 z+ z2

Ë Solve for latest output terms(z) z2 = -0.369389 fHzL + 0.389283 z fHzL - 1.05127 s(z) + 2.05148 z s(z)

Ë Convert to time domainThis equation is implemented in the software

s[n+2] = - 0.369389 f[n] + 0.389283 f[n+1] - 1.05127 s[n] + 2.05148 s[n+1]


of 21

NXTWay Balancing

0.5 1.0 1.5 2.0

-100

-50

50

100

s, Controller Output

q°,Controller Input


of 21

Conclusionsü A controller using positive feedback is a feasible solution for balancing a two wheeled robot.ü Changing motor direction causes significant nonlinearity and noise.ü Sampling time had to be increased from the design value to achieve balance due to probable

inaccuracy from approximations.


slide 1 of 21 balancing the segway - … slideshow-1.pdf · slide 3 of 21 segway body ü point...

Documents