Center of mass: c
Mass of body system:
Mass of wheel:
Wheel radial: r
Distance between center mass of thee body and wheel
axle: L
Motor gear ratio: n
Pitch angle: b
Distance between two wheels: d
Angle velocity of left wheel: lw
Angle velocity of right wheel:
(b)
Fig. 2. (a) Free body diagram of the oveerall system, (b) Free body
diagram of the wheel.
w
rw
z
z
z
z
z
z
z
(a)
V2 z = sin
V3 x =
)
(6)
r lw rw
L sin b sin
d
(7)
(8)
V3 y = 0
V3 z = L sin b cos
= lw b
r lw +lw
2
r lw rw
2
(9)
HE ROBOT
III. KINEMATICS MODEL OF TH
The motion of the overall system can be ddescribed as three
parts: inclination speed V1 caused by the opperator, rotation
speed V2 along Y axis acted by operator andd physical speed
V3 caused by wheel rotation. Each part can bbe decoupled
into three directional axis x, y, z as shown inn Fig. 2.
V1x = Lb cos b cos
(1)
V1 y = Lb sin b
(2)
(3)
os
V = V + V + V = L b cos co
1x
2x
3x
x
b
r lw +rw
r lw rw
s
+
+
L sin sin
b
2
2
V y = Lb sin b
n
V = V + V + V = L cos sin
z
1z
2z
3z
b
b
r lw +rw
r lw rw
+ sin
+ L sin cos
b
2
2
(10)
(11)
(12)
r lw +rw
cos
V2 x =
2
V2 y = 0
(4)
(5)
457
) (
)(
(13)
G=
(14)
=A +Bu
b
b
(15)
The total kinetic energy k of overall system can be
divided into three parts: the wheel kinetic energy kw , the
0
0
A=
A41
A
51
A61
the motor k m .
(16)
2 2
m mv L sin b + Jvy 1
1
1
k = mwr2 + J z + r2 w +
+ Jmn2mwr2
16
2
2 2
D2
(23)
0 0
0 0
0 0
0 0
0 0
0 0
0
0
0
1
0
,B =
,
A46
B41 B42
B
A46
B52
51
B61 B62
A66
1 0 0
0 1 0
0 0
A44 A45
A54 A55
A64 A65
u = l .
r
(17)
(21)
where
(20)
k = km + kv + k w
k U G
d k
+
+
= ( l + r )
(
)
dt b b b b
2
2
1
km = J m n 2 lw b + rw b
2
) (
d k k U G
+ = l
( ) +
dt
d k k U G
+
= r
( ) +
dt
1
2 + 2 + 2 + b + 2b +
2 2
1
2 1
2
+ 2
2 2
2
(19)
U = mv gL cos b
(18)
2 2
m mv L sin b + Jvy
1
+ mwr2 r2 w +
2
4
D2
458
B. LQR Control
The linear-quadratic regulator (LQR) is to run a dynamic
system under optimal solution. We assume that the robot
system has the state equation
de ( t )
.
dt
( )
(24)
cr
0.5
0.5
de ( t )
, t = kT .
dt
(k )actual (k )) +
(
p desire
k (desire (k )actual (k )desire (k 1)+actual (k 1))
d
2
(33)
(26)
(35)
Then we set
(36)
The (35) can be changed to
(27)
(25)
(31)
In this case, the overall system can be divided into selfbalancing subsystem and yaw control subsystem. The
control theory used in two different subsystems here is the
PD control method.
u = k p e ( t ) + kd
Here we assume
0 . In order to find the optimum
control vector ( ) under two chosen constants Q and R, the
function
)
(
(32)
must be minimum. Where Q is positive semi-definite
0) and R to be positive definite (
0). In
(
this robot system, we use output as feedback signal.
(29)
b
cl
input(t )
( )
(37)
(28)
459
U'
(c)
Fig. 5. (a) PID simulation diagram for self-balancing control,
(b) Result of tilt angle, (c) Result of tilt angle rate.
x 2 = A2 x2 + B2u 2
which is =
43 0.17 1.075
Y1 b =
b
], where
A =
2
0.4
, c1 = 1 0 0
0 1 0
( 1 + 2 )
= 0.12,
r2
2
2
2( mwr + J z + 2
J vy + J m n )
d2
r
= 0.07.
B =
2
r2
d (2 mwr 2 + J z + 2
J vy + J m n 2 )
d2
(39)
which is in detail as
1
0
b 0
0
= 25 1.85 19 x + 0.3 +
b
1
[ l r]
s
(40)
(a)
(a)
(b)
Fig. 6. (a) PD simulation diagram for yaw control, (b)
Simulation result of yaw angle rate.
(b)
460
VII. CONCLUSSION
In this paper, based on mechanicaal structure and dynamic
models, the modeling and analysiss of self-balancing robot
are discussed by using Lagrangee equation and energy
conservation principle, PID and LQ
QR control algorithms are
tried. It is concluded that LQR has a better performance than
PID in self-balancing control. In our
o future work, we will
use LQR method to control a physiccal two-wheeled robot to
further verify the controllers perform
mance.
References
[1]
200 0 0
[2]
[3]
[4]
[5]
[6]
[7]
[8]
[9]
[10]
[11]
[12]
(a)
[13]
[14]
[15]
(b)
Fig. 7. (a) LQR simulation diagram, ((b) LQR control
simulation.
[16]
[17]