Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a...
Transcript of Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a...
![Page 1: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/1.jpg)
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
![Page 2: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/2.jpg)
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
![Page 3: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/3.jpg)
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
![Page 4: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/4.jpg)
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
![Page 5: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/5.jpg)
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
![Page 6: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/6.jpg)
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
![Page 7: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/7.jpg)
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
![Page 8: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/8.jpg)
• 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
![Page 9: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/9.jpg)
2/1
/20
13
2.1
51
: A
dva
nce
d D
yna
mic
s a
nd
Co
ntr
ol
9
Hardware Design-CAD
![Page 10: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/10.jpg)
2/1
/20
13
2.1
51
: A
dva
nce
d D
yna
mic
s a
nd
Co
ntr
ol
10
Hardware Design-CAD
![Page 11: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/11.jpg)
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
![Page 12: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/12.jpg)
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
![Page 13: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/13.jpg)
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
![Page 14: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/14.jpg)
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
![Page 15: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/15.jpg)
• 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
![Page 16: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/16.jpg)
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,
![Page 17: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/17.jpg)
• 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.
![Page 18: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/18.jpg)
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
![Page 19: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/19.jpg)
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.
![Page 20: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/20.jpg)
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
![Page 21: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/21.jpg)
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)
![Page 22: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/22.jpg)
• 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.
![Page 23: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/23.jpg)
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])
![Page 24: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/24.jpg)
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:
![Page 25: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/25.jpg)
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
]
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:
When the position of the COM goes down,
the maximum horizontal displacement decreases
the initial moving speed of the robot also decreases
![Page 26: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/26.jpg)
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
]
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:
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.
![Page 27: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/27.jpg)
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
![Page 28: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/28.jpg)
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
![Page 29: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/29.jpg)
Noise Characterization
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
![Page 30: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/30.jpg)
Noise Characterization
• 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
![Page 31: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/31.jpg)
Noise Characterization
• 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
![Page 32: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/32.jpg)
Kalman Filtering Algorithm
2/1
/20
13
2.1
51
: A
dva
nce
d D
yna
mic
s a
nd
Co
ntr
ol
32
State Estimation ���|��� = ������|��� + ����
Forecast Error Covariance ��|��� = �����|����� + ��
Innovation �� = � −�� �|���
Innovation Covariance �� = ���|����� + ��
Compute Kalman Gains �� = ��|�������
��
Update a posteriori state ���|� = ���|��� +����
Update Covariance ��|� = (� −���)��|���
1Terejanu, Gabriel. “Discrete Kalman Filter Tutorial.” Department of Computer Science and
Engineering, University at Buffalo.
![Page 33: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/33.jpg)
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
![Page 34: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/34.jpg)
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
![Page 35: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/35.jpg)
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
![Page 36: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/36.jpg)
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
![Page 37: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/37.jpg)
Revised Model
2/1
/20
13
2.1
51
: A
dva
nce
d D
yna
mic
s a
nd
Co
ntr
ol
37
![Page 38: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/38.jpg)
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
![Page 39: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/39.jpg)
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));
}
![Page 40: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/40.jpg)
System Tuning
2/1
/20
13
2.1
51
: A
dva
nce
d D
yna
mic
s a
nd
Co
ntr
ol
40
![Page 41: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/41.jpg)
FUNCTIONAL DEMONSTRATION 41
2.1
51
: A
dva
nce
d D
yna
mic
s a
nd
Co
ntr
ol
2/1
/20
13
![Page 42: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/42.jpg)
Functional Demonstration
2/1
/20
13
2.1
51
: A
dva
nce
d D
yna
mic
s a
nd
Co
ntr
ol
42
![Page 43: Modeling, Simulation 2/1/2013 and Fabrication of a ... · Modeling, Simulation and Fabrication of a Balancing Robot Ye Ding 1, Joshua Gafford 1, Mie Kunio 2 1Harvard University, 2MIT](https://reader030.fdocuments.us/reader030/viewer/2022041002/5ea377d0497222449c716279/html5/thumbnails/43.jpg)
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