Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a...

Modeling, Simulation

and Fabrication of a

Balancing RobotYe Ding1, Joshua Gafford1, Mie Kunio2

1Harvard University, 2MIT

2.151: Advanced Dynamics and Control

Dec. 18th, 2012

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

1

Presentation Outline

• Motivation

• Hardware Design

• Dynamic Modeling

• Equations of Motion (differential, state-space)

• Comments on controllability, observability

• Simulation Overview

• Kalman Filtering

• Noise Characterization

• C Code Implementation

• Tuning the system

• Functional Demonstration (throughout)

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

2

Motivation

• To apply principles learned in class to design, model, simulate

and fabricate a control system

• To get a deep understanding how the real system works

instead of simulation

• To have fun

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

3

HARDWARE DESIGN

Ye Ding

4

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

2/1

/20

13

Hardware Design-Concept

• To build a balance robot

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

5

Hardware Design-Motor

• System estimation

• Simple torque calculation

• Compare possible motor selections

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

6

m=0.5kg l=0.08m �� = 5°

�� = �� ∗ � ∗ ��=0.5x9.8x0.08xsin(5˚)=0.0342Nm

For each motor is around 0.017Nm

Then for torque requirement

�� = 17�� after gearbox

Speed Requirement

As long as it’s not too slow, then couple of hundreds rpm

Hardware Design-Motor

Compare Combination of Motor and Gearbox

Torque: 17mNm Speed: 400rpm and price

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

7

Motor

Name

Gearbox

Backlash

Output

Speed

(rpm)

Output

Torque(mNm)Encoder Total Price Motor Gearbox

RE 13 2 422. 15.47 No 122.7

RE 13 2 422 15.47 No 126.4

A-Max

161 496 16.06878 Yes 128.7

A-Max

191.4 450 17.4474 Yes 173.1

RE-Max

132 360 15.589 No 98.3

A-Max

161 496 16.06878 Yes 128.7

• Motor Controller

• ��

= 0.72�

• Two motors

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

8

• Sensor

• Encoder

• Measure angle

• Micro Processor

Qik Dual Serial Motor Controller

Triple Axis Accelerometer & Gyro

Arduino Mega1280

Hardware Design-Components

Maxon encoder MR, Type M

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

9

Hardware Design-CAD

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

10

Hardware Design-CAD

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

11

Hardware Design

Robot Weight 513g

Robot Inertia 13067.6gcm^2

Terminal Resistance 8.3Ω

Terminal Inductance 0.306mH

Torque Constant 5.57mNm/A

Speed Constant 1720rpm/V

Rotor Inertia 0.862gcm^2

Gearhead Ratio 9.1:1

Gearhead Efficiency 81%

Wheel Dia 31.5mm

Mass center: x 0.03mm

y -8.34mm

z 63.60mm

DYNAMIC MODELING

Mie Kunio

12

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

2/1

/20

13

Dynamic Modeling:Definition and Assumption

• Definition

• Inertia of body part: I

• Inertia of wheel part: Iw

θ

τ0

m

R

L

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

13

φ

Robot can be separated into 2 parts.

• Body part -- gray

• Wheel part --brownish gray

• Assumption

• There is no slipping between

• floor and wheel.mw

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

14

m1: mass of the head

m2: mass of the shaft

L1: height of the head (40 mm, fixed)

L2: height of the shaft (60 mm, fixed)21

1212

22 mm

mLLLL

+++=

222

2

21

1 12

1

2LmL

LmI +

+=

Dynamic Modeling:Inertia and the COM of the Body Part

• Assumption

• The head can be considered as

a point mass.

• The shaft can be considered as

a homogenous cylinder.

• Inertia and the COM

m = m1 + m2

m1

m2

L1

L2 L

Head

Shaft

• Derived via Lagrangian approach

• Generalized coordinates: φ and θ

• Generalized forces (torques) : µ and χ

µϕϕ

=

∂∂−

∂∂ LL

dt

d&

χθθ

=

∂∂−

∂∂ LL

dt

d&

+

−−

−−=

+++

χµ

θθϕθθ

θϕ

θθ

sin

0

00

sin0

cos

cos)(2

2

mgL

mRL

mLImRL

mRLRmmI ww

&

&&

&&

&&

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

15

m: mass of body part

mw: mass of wheel

I: inertia of body part

Iw: inertia of wheel

L: distance between the COM

and the center of the wheel

R: radius of wheel

Dynamic Modeling:Derivation of equations

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

16

−−+

−

−=

θϕ

βββββ

τχµ γ

&

&

mm

mm01

1

• Generalized torques can be written by

• Input torque: τ0

• Rolling damping ratio: βγ

• Friction damping ratio: βm

Dynamic Modeling:Expression of the generalized torques

τ0

µ

χ

τfloor

τhinge

( )( )θϕβτττχ

ϕβθϕβττττµ γ

&&

&&&

−+−=+−=

−−−=−−=

mhinge

mfloorhinge

00

00

Therefore,

• Control objective

• Stand vertically

=> θ=0, dφ/dt =0, dθ/dt=0 ∴ cosθ ≈1, sinθ ≈ θ

0

2

2

1

10

)(

τθθϕ

βββββ

θϕ

γ

−+

−−

−−+

−=

+++

mgL

mLImRL

mRLRmmI

mm

mm

ww

&

&

&&

&&

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

17

Dynamic Modeling:LinearizationWe need to linearize the system to solve the equations easily.

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

18

02

2

1

10)( τθθϕ

βββββ

θϕ γ

−+

−−

−−+

−=

+++

mgLmLImRL

mRLRmmI

mm

mmww

&

&

&&

&&

E F G H

0

0

0

0

0

1000

0100

τ

θϕθϕ

θϕθϕ

−−−−−−

+

−−−−−−−−−=

&

&

&

&dt

d

This image cannot currently be displayed.

-E-1G -E-1F -E-1H

m = 0.513 kg

mw= 7.2×10-3 kg

Iw = 7.79×10-7 kg m2

R = 1.60×10-2 m

βγ

= 0.01 Nm/(rad/s)

βm = 0.01 Nm/(rad/s)

Dynamic Modeling:State-space representation

Controllability and Observability

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

19

• Controllability

[ ]BABAABBCont 32=

Rank (Cont) = 4 => Completely controllable

• Observability

=

3

2

CA

CA

CA

C

O Rank (O) = 4 => Completely observable

Is this robot easy to be controlled?

When does this robot become difficult to be controlled?

No matter how much we change the masses of the head (m1) and

the shaft (m2), this system is always completely controllable and

completely observable.

MATLAB program to calculate the

controllability and observability

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

20

%Parameters

m=513.3*10^(-3); %body part mass [kg]

m2=0*10^(-3); %shaft part mass [kg], CAN CHANGABLE!!!!

m1=m-m2; %head part mass [kg], change by mass of shaft

mw=7.2*10^(-3); %wheel(*2) mass [kg]

L1=40*10^(-3); %height of head [m]

L2=60*10^(-3); %height of shaft [m]

I=m1*(L1/2+L2)^2+m2*L2*L2/12; %inertia of body part [kg*m^2]

Iw=389.6*10^(-9)*2; %inertia of wheel [kg*m^2]

Br=0.01; %rolling damping ratio [N*m/(rad/s)]

Bm=0.01; %bearing damping ratio [N*m/(rad/s)]

L=L2/2+(L1+L2)*m1/(2*m) %position of COM [m]

R=16*10^(-3); %radius of wheel [m]

g=9.8; %gravity [m/s^2]

%Weighting matrices

E=[Iw+(mw+m)*R*R m*R*L; m*R*L m*L*L+I]; %for d^2/dt^2 (phi and theta)

F=[Br+Bm -Bm; -Bm Bm]; %for d/dt (phi and theta)

G=[0; -m*g*L]; %for theta

H=[1; -1]; %for input torque

MATLAB program to calculate the

controllability and observability

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

21

%state-space representation of the system

%state variable: phi, theta, d(phi)/dt, d(theta)/dt

A=[0 0 1 0; 0 0 0 1; [0; 0] -inv(E)*G -inv(E)*F] %system matrix

B=[0; 0; inv(E)*H] %input matrix

C=[R 0 0 0; 0 1 0 0] %output matrix

D=0

sys1=ss(A,B,C,D)

G1=tf(sys1) %transfer function of sys1

G1zp=zpk(sys1) %Gain/pole/zero representation of sys1

%controllability and observability check for sys1

Cont=[B A*B A*A*B A*A*A*B];

rank(Cont)

Obs=[C; C*A; C*A*A; C*A*A*A];

rank(Obs)

• Weight matrices

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

22

Designing the controller

1

1000

0100

0010

0001

=

=

R

Q

To understand when it is difficult to control the system,

we design the optimum controller by using the LQR method

and check the free response of the closed-loop system.

Set the weighting matrices Q and R such that

each component of state variables and input

should be considered equally.

MATLAB program to design

the controller

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

23

%controller design

xweight=eye(4);

uweight=1;

K=-lqr(A,B,xweight,uweight)

sys1_lqr=ss(A+B*K,B,C,D);

initial(sys1_lqr, [0; 0.5; 0; 0])

24

2.1

51

: A

dva

nce

d S

yste

m D

yna

mic

s a

nd

Co

ntr

ol

2/1

/20

13

Ho

rizo

nta

l d

isp

lace

me

nt x

[m]

Ro

tati

on

al

an

gle

θ[r

ad

]

Free response of the closed-loop system( )Cxy

BrxBKAx

=++=&

=

0

0

17.0

0

)0(xstarting from the position which

the robot inclines at 10 degrees:

25

2.1

51

: A

dva

nce

d S

yste

m D

yna

mic

s a

nd

Co

ntr

ol

2/1

/20

13

Ho

rizo

nta

l d

isp

lace

me

nt x

[m]

Ro

tati

on

al

an

gle

θ[r

ad

]


BrxBKAx

=++=&

=

0

0

17.0

0



When the position of the COM goes down,

the maximum horizontal displacement decreases

the initial moving speed of the robot also decreases

When the position of the COM goes down,

the maximum horizontal displacement decreases

the initial moving speed of the robot also decreases

26

2.1

51

: A

dva

nce

d S

yste

m D

yna

mic

s a

nd

Co

ntr

ol

2/1

/20

13

Ho

rizo

nta

l d

isp

lace

me

nt x

[m]

Ro

tati

on

al

an

gle

θ[r

ad

]


BrxBKAx

=++=&

=

0

0

17.0

0



Because of the limitation of the robot performance,

Mr. Struggles becomes difficult to be controlled

when the position of the COM is located at the higher position.

KALMAN FILTERING

Joshua Gafford

27

2.1

51

: A

dva

nce

d S

yste

m D

yna

mic

s a

nd

Co

ntr

ol

2/1

/20

13

Noise Characterization

• Static Tests

• Characterize static

noise contamination

to obtain covariance

of white noise

• Stepping Through

Known Angles

• Characterize angular

error to obtain error

covariance

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

28


Sensor Noise Immunity Response Time Drift Rate

Accelerometer Low Slow No

Gyroscope Okay Fast Yes

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

29


• Objective: derive noise covariance for Kalman filter

implementation

• Accelerometer Error: X ~ N(µ=0.540, σ2 =0.342)

• Gyro Error: Y ~ N(µ=0.230, σ2 =0.035)

• Assumption: Cov(X,Y)=0

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

30


• Noise Covariance:

• Gyro measures rate of a rigid body, thus gyro measurement noise

and bias enter filter as process noise1

• Bias covariance difficult to determine empirically, so it was

determined iteratively to minimize RMSD of estimator

• Accelerometer enters filter as measurement noise

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

31

=

=

3

2

2

deg0007.00

0deg035.0

)(0

0)(

sYVar

YVarb

Q

deg342.0)( === XVarRR angle

1Sabatini, “Kalman-Filter-Based Orientation Determination Using Inertial/Magnetic Sensors:

Observability Analysis and Performance Evaluation” Sensors, 2011

Control Loop

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

33

Full State

Feedback

Controller

Dynamic

System

Kalman Filter

xest

u y

Dynamic Simulation

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

34

• Initial angle of 30 degrees

Simulation Results

• Kalman filter smoothes response, improves convergence and

overall performance!

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

35

Struggles Data

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

36

• Accelerometer vulnerability

to motor vibration is a huge

unmodeled effect

• Gyro drift is clear

Revised Model

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

37

C CODE IMPLEMENTATION & TUNING

Ye Ding

38

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

2/1

/20

13

C Code Implementation

Kalman Filter Loop:

float kalmanCalculate(float newAngle, float newRate,int looptime) {

dt = float(looptime)/1000;

x_angle += dt * (newRate - x_bias);

P_00 += - dt * (P_10 + P_01) + Q_angle * dt;

P_01 += - dt * P_11;

P_10 += - dt * P_11;

P_11 += + Q_gyro * dt;

y = newAngle - x_angle;

S = P_00 + R_angle;

K_0 = P_00 / S;

K_1 = P_10 / S;

x_angle += K_0 * y;

x_bias += K_1 * y;

P_00 -= K_0 * P_00;

P_01 -= K_0 * P_01;

P_10 -= K_1 * P_00;

P_11 -= K_1 * P_01;

return x_angle;

}

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

39

Full State Feedback Controller Loop:

int fsfbController(float kt, float kw, float kf,

float kv)

{

float pkt,pkw,pkf,pkv;

Angle=actAngle;

pkt=kt*Angle;

pkw=kw*(Angle-lastAngle);

pkf=kf*enc_Pos;

pkv=kv*(enc_Pos-lastenc_Pos);

lastAngle=Angle;

lastenc_Pos=enc_Pos;

return -((int)(pkt+pkw+pkf+pkv));

}

System Tuning

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

40

FUNCTIONAL DEMONSTRATION 41

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

2/1

/20

13

Functional Demonstration

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

42

Conclusions

• We have modeled, simulated, and built a functioning

balancing robot

• Inclusion of Kalman Filtering significantly improved response

and steady-state convergence

• Individual Contributions

• Ye: Mr. Struggles hardware and software

• Josh: Simulink model implementation, sensor noise

characterization, Kalman filtering

• Mie: Dynamic model development, controllability, observability,

LQR controller design

2/1

/20

13

2.1

51

: A

dva

nce

d D

yna

mic

s a

nd

Co

ntr

ol

43

Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a...

Documents

Transcript of Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a...