23

ĐIỀU KHIỂN ỔN ĐỊNH CHO ROBOT HAI BÁNH TỰ CÂN BẰNG

STABILIZING CONTROL FOR
TWO-WHEELED SELF BALANCING ROBOT

Hồ Đắc Lộc1, Nguyễn Hùng1, Trương Ngọc Sơn2

Trường Đại Học Kỹ Thuật Công Nghệ Tp.HCM1
Trường Đại Học Sư Phạm Kỹ Thuật Tp.HCM2

TÓM TẮT
Bài báo trình bày thiết kế bộ điều khiển ổn định cho robot hai bánh tự cân bằng dùng kỹ
thuật điều khiển LQR và điều khiển trượt. Robot hai bánh tự cân bằng được mô hình như hệ
con lắc ngược di động. Robot được cấu tạo gồm thân robot gắn trên hai bánh xe đồng trục
truyền động bằng hai động cơ DC riêng rẽ. Mô hình động lực học phi tuyến cho hệ thống
được dẫn xuất. Bộ điều khiển trượt và bộ điều khiển LQR được thiết kế để giữ cân bằng cho
robot trong trạng thái cân bằng dựa trên mô hình được tuyến tính hóa ở điểm cân bằng. Kết
quả mô phỏng cho thấy bộ điều khiển được thiết kế có thể đáp ứng yêu cầu đặt ra của hệ
thống. Bộ điều khiển trượt cho thấy tính ổn định bền vững cao hơn bộ điều khiển LQR khi hệ
thống có nhiễu ngoài. Kết quả tiến hành thực nghiệm cũng chứng minh tính ổn định cân bằng
và hiệu quả của các bộ điều khiển đưa ra.
ABSTRACT
The paper proposes controllerdesign for two-wheeled self balancing robot based on LQR
and sliding mode control. Two-wheeled self balancing robot is modeled as an mobile inverted
pendulum system.It is composed of an inverted pendulum and a chassis with two coaxial
wheels.The sliding mode controllerand LQRare designed to stabilize the robot at upright
position under straight-line motion. The designed controller stabilizes the robotbased on the
linearized model. The simulation results are presented to show the effectiveness of the
proposed controllers.The sliding mode controller is more stablized than the LQR in case of
external disturbances. The experimentresults show that the LQR and sliding mode controller
provide the ability to keep the robot uprightfor a long time.
Từ khóa: Robot hai bánh tự cân bằng, điều khiển trượt, điều khiển LQR, con lắc ngược di
động.
1. GIỚI THIỆU đặt trên trọng tâm của hai bánh xe. Vấn đề
Robot hai bánh tự cân bằng đã cho thấy cần quan tâm là bài toán điều khiển cân
nhiều ứng dụng trong cuộc sống và trở bằng cho robot khi di chuyển trên các địa
thành đối tượng của các nhà nghiên cứu, hình khác nhau. Đã có nhiều giải thuật điều
các kỹ sư và những người đam mê robot. khiển đã được công bố và tiến hành thực
Robot hai bánh tự cân bằng được mô hình nghiệm như: điều khiển PID [2], điều khiển
như một hệ con lắc ngược di động[1]. bằng phương pháp Logic mờ [3], mạng
Robot được cấu tạo gồm phần thân robot và nơron nhân tạo [4]. Bài báo này áp dụng bộ
hai bánh xe đồng trục truyền động bằng hai điều khiển trượt và LQR để điều khiển ổn
động cơ DC riêng rẽ [1]. Để giữ thăng bằng định cho robot. Cả hai bộ điều khiển được
cho robot, một bộ xử lý đọc các tín hiệu thiết kế dựa trên mô hình hệ thống đã được
cảm biến, thông qua quá trình tính toán, tín tuyến tính hóa. Kết quả cho thấy bộ điều
hiệu điều khiển được gởi đến hai động cơ khiển trượt có ưu điểm là hoạt động tốt đối
để điều khiển robot di chuyển về sau hay với các nhiễu tác động vào hệ thống.
trước sao cho trọng lượng của robot luôn
 24

2. MÔ HÌNH ĐỘNG HỌC CỦA
ROBOT HAI BÁNH TỰ CÂN BẰNG
Hình 1 mô tả các lực tác dụng lên robot
hai bánh tự cân bằng. Mô hình robot hai
bánh tự cân bằng bao gồm mô hình con lắc
ngược được gắn trên khung với hai bánh xe
đồng trục. Ta xét mô hình robot đảm bảo
hai bánh xe luôn tiếp xúc với mặt đường và
lăn không trượt. Robot có thể xoay quanh
trục z với góc xoay  p và vận tốc góc tương
ứng p . Chuyển động tịnh tiến của thân
robot được mô tả bởi thông số vị trí xr và
vận tốc xr . Thân robot không di chuyển
theo hướng z. Thay vào đó robot có thể
xoay quanh trục y với góc xoay tương ứng
là  và vận tốc góc là  . Những thông số
mô hình của robot tự cân bằng được định
nghĩa trong Bảng 1.

Bảng 1. Các thông số của mô hình

Thông
Định nghĩa Đơn vị y RL
số  RL yp

p Góc nghiêng [ rad ] L

VL
HL xRL xRL 0
y RL y
FC
xp

f dRL FC fdP xabs

M RL g xRM
 Góc xoay [ rad ]

H TL
xRL L
xRL p
M pg


Moment quán tính của VTL

[ kgm 2 ]
y RR
J RL bánh xe y RR
HL
VL
L

 RR xRR
1 R
Mr Khối lượng bánh xe [ kg ] R
VR
HR xRR x RR 0
2
D

z
f dRR
Jp Moment quán tính của con 2 M RR g
HR
[ kgm ] HTR
xRR VR
lắc ngược quanh trục z 
VTR
Moment quán tính của con
J lắc ngược quanh trục y
2
[ kgm ] Hình 1. Lực tác động lên robot hai bánh tự cân
bằng
Mp Khối lượng con lắc ngược [ kg ]
2.1. Mô hình động lực học bánh xe
R Bán kính bánh xe [m ] Mô hình bánh xe được đặt trên mặt phẳng
Khoảng cách từ tâm bánh
nghiêng so với mặt phẳng ngang một góc  .
L xe đến trọng tâm con lắc [m ]
ngược Chọn phương chuyển động x trùng với mặt
Khoảng cách giữa 2 bánh phẳng nghiêng. Ta có phương trình lực tác
D xe
[m ]
dụng lên bánh xe như sau:
xp Vị trí của con lắc ngược xRL0Mr cosHL cos fdRL sinVL sinMr g HTL (1)
[m ]
trên trục x
g Phương trình cân bằng mô men:
Gia tốc trọng trường [ m /s2 ]
Lực tác động vào bánh xe RL0 J RL   L  HTL R (2)
 L , R trái và phải
[ Nm ]
Bởi vì chuyển động tịnh tiến xảy ra ở tâm
xRL 0 , xRR 0 Vị trí bánh xe trái và phải của bánh xe. Chuyển động quay của nó có thế
[m ]
được chuyển thành chuyển động tịnh tiến như
HL , HR sau:
Phản lực giữa bánh xe 
x (3)
[N ] x  R  
RL 0 x  R    RL 0
RL 0 RL 0 RL 0 RL 0
trái/phải và con lắc ngược R
VL ,VR Từ phương trình (2) và (3) ta có phản lực
 RL 0 , RR 0 Góc xoay của bánh xe trái [ rad ]
theo phương ngang là:
và phải L J RL   J
HTL    RL 0  L  RL2 
xRL 0 (4)
Nhiễu lực tác động lên R R R R
f dRL , f dRR trọng tâm bánh xe trái và [N ]
phải Thay phương trình (4) vào phương trình (1)
Nhiễu lực theo phương
f dP , FC ngang và phương thẳng
ta có phương trình động lực học của bánh xe
[N ] phải:
đứng tác động lên con lắc
ngược
 25

 J RL   2JR
Mr  2  
xRL 0   cos  H L  L  cos  f dRL (2 M r  M p  xr 0   M p L cos( p   )p
) 
 R  R R2
(5)
 sin  VL  M r g sin 
 M p L sin( p   )p 2  (2 M r  M p ) g sin  (14)
L R
Phương trình động lực học cho cả hai   ( f dP  f dRL  f dRR ) cos   FC sin 
R
bánh xe: Phương trình mô men của trọng tâm con
 2J     (6)
 2M r  
x   cos  ( H  H ) 
RL
2
R 
RL 0 L R
L

R
R
lắc ngược:

 cos  ( f dRL  f dRR )  sin  (VL  VR )  2M r g sin  p J p  (VR  VL ) L sin  p (15)
 ( H R  H L ) L cos  p  ( L   R )
2.2. Mô hình động lực học con lắc ngược Thay thế phản lực theo phương ngang và
Phương trình lực tác dụng theo phương phương đứng trong công thức (15), phương
ngang thu được từ Hình 1. trình (15) có thể được viết lại:

x p M p  ( H R  H L )  f dP (7) ( J p  M p L2 )p  M p L cos( p   )
xr 0
(16)
M p gL sin  p  ( L  R )  FC L sin  p  fdP L cos p
Mối tương quan của gia tốc trọng lực
xp của con lắc với gia tốc theo phương Tuyến tính các phương trình trên với góc
lệch  p , tương đối nhỏ như sau:
ngang xr được biểu diễn như sau:
sin  p   p , cos p  1, sin    , cos  1, (17)
x p  p L cos  p  L sin  p .p 2  
 (8) xr
sin( p   )   p   , cos( p   )  1, (p ) 2  0
Mối tương quan của gia tốc theo phương Ta thu được mô hình tuyến tính của
ngang và gia tốc nghiêng của thân robot hai bánh tự cân bằng như sau:
robotđược biểu diễn như sau:  2JR  .. .. L R (18)
2Mr Mp  2 xr0 MpLp 2Mr Mp  ga

xr  cos   
xr 0 (9)  R  R
FC sina fdP  fdRL  fdRR
Phản lực theo phương ngang từ phương .. ..
trình (7) và (9) được viết lại: J p
2

 M p L  p   M p L x r 0  M p gL p (19)
( H L  H R )  
x p M p  f dP   L   R   FC L p  f dP L
(10)
 (p L cos p  L sin p .p2  
xr ) M p  f dP Phương trình mô men của con lắc ngược
 p L cos  p M p  L sin p .p2 M p quay quanh trục y là:
 cos  .
xr 0 .M p  f dP
J  ( H  H )
D (20)
 L R
2
Phương trình lực tác dụng theo phương
gia tốc yp  
6
( L   R )
(21)
( M p  9 M r ) RD

y p M p  VR  VL  M p g  FC (11) Sắp xếp các phương trình trên, tuyến
Mối tương quan giữa gia tốc theo tính hóa xung quanh điểm làm việc tĩnh
phương thẳng đứng của con lắc và gia tốc ( xr0  0, p  0,   0). Phương trình trạng thái
góc nghiêng của thân robot được thể hiện của hệ thống có thể được viết dưới dạng ma
như sau: trận sau:
y p  p L sin  p  L cos  p  p 2  sin   
 xr 0 (12)  xr 0   0 1 0 0 | 0   xr 0 
0
    0   xr 0 
Thay phương trình (12) vào phương  xr 0   0 0 A23 0 | 0

  p  0 0 0 1 | 0 0   p 
trình (11), phản lực theo phương thẳng      
  p   0 0 A43 0 | 0 0   p 
đứng là:           
VR  VL  M p  xr 0 sin   M p L sin  pp (13)      
   0 0 0 0 | 0 1   
 M p L  p 2 cos  p  M p g  FC     0
   0 0 0 | 0 0    
Thay thế phản lực theo phương ngang và  0 0   0 
B 0  d 
phương đứng trong phương trình (6) ta  2   2
được phương trình cho gia tốc xr 0 :  0

0   L   R   0 
  
  B4 0          d 4 
      L   R     
   
 0 0   0 
 0
 B6   0 
 
(22)
 26

Kết quả mô phỏng đáp ứng hệ thống cho

Trong đó: bộ điều khiển LQR được trình bày trên
gM p Y Hình 3 và Hình 4.
 4 Mp
A23  g  1  L


, A43  , B4  
 3 X  X X Ñaù p öù ng heä thoá ng ñieàu khieå n LQR
0.6

Vò trí[m],Vaän toá c daø i[m/s],Goù c[rad],Vaä n toác goù c[rad/s]

Vò trí
 4 LY 1 , 6 0.4 Vaän toá c daø i
B2     B6  Goù c
 3X M L  (9 M  M p ) RD 0.2 Vaän toá c goùc
 p  r
0
1 M p ( M p  12 M r ) L , Mp 1
X  Y  -0.2
3 M p  3M r ( M p  3M r ) R L
-0.4
4 g (2M r  M p ) 3 g (2 M r  M p )
d2   , d4   -0.6

12 M r  M p L(12 M r  M p ) -0.8

Mô hình động lực học cho robot hai -1

bánh tự cân bằng có thể được mô tả bởi -1.2

0 1 2 3 4 5
Thôø i gian [s]
6 7 8 9 10

phương trình tổng quát sau:

Hình 3.Đáp ứng của bộ điều khiển LQR không
 )  Ax  Bu
x  Ax  Bu  Dd  Ax  B(u  D (23)
d 1 có nhiễu
với
 D
BD Ñaù p öù ng heä thoá ng ñieà u khieå n LQR vôù i nhieã u
d d 0.6

Vò trí[m],Vaä n toá c daø i [m/s],Goù c[rad],Vaän toá c goù c[rad/s]

Vò trí

  (BT B)1 BT D  1  B2 d 2  B4 d 4  (24) 0.4 Vaä n toá c daø i

Dd d   Goù c
B22  B42  0  0.2 Vaä n toá c goù c

-0.2
3. THIẾT KẾ BỘ ĐIỀU KHIỂN -0.4

3.1. Thiết kế bộ điều khiển LQR -0.6

Bộ điều khiển LQR được áp dụng cho hệ -0.8

thống tuyến tính được cho bởi phương -1

trình: -1.2
0 1 2 3 4 5 6 7 8 9 10
. Thôø i gian [s]

x  Ax(t ) Bu (t ) (25) Hình 4.Đáp ứng của bộ điều khiển LQR có
x(0)  x0 nhiễu
Mục tiêu là thiết kế các bộ điều khiển 3.2. Thiết kế bộ điều khiển trượt
nhằm cực tiểu chỉ số năng lượng: Mặt trượt s được chọn sao cho:

1 lim sT s  0
J
20 
x ' (t )Qx(t )  u ' (t ) Ru (t ) dt  (26) t 
(29)
Nghĩa là s được chọn thỏa mãn:
Luật điều khiển cho LQR được chọn như
ds
sau: s    sgn(s ) (30)
_ dt
u   R 1 B ' P x (27) với   0 .
_ _
Trong đó ma trận P  P'  0 được xác Lấy vi phân phương trình (30) theo thời
gian và kết hợp với (23) ta được:
định là nghiệm của phương trình đại số ds dx
Ricatti: s  S  S( Ax  Bu  Dd ) (31)
dt dt
0  PA  A' P  PBR 1 B ' P  Q (28) Từ phương trình (30) và (31) ta được:
Kết quả xác định được ma trận hồi tiếp s  SAx  SBu  SDd   sgn(s) (32)
_
trạng thái K  R 1 B ' P của hệ thống điều Luật điều khiển trượt được thiết kế như
khiển. Sơ đồ khối của bộ điều khiển LQR sau:
u   (SB ) 1 (SAx  SDd   sgn(s )) (33)
được mô tả trên Hình 2.
Kết quả mô phỏng đáp ứng hệ thống cho
bộ điều khiển trượt được trình bày trên
Hình 5 và Hình 6.

Hình 2. Sơ đồ khối bộ điều khiển LQR

 27

0.6
Ñaù p öù ng heä thoán g ñieà u khieå n tröôï t
thực nghiệm của robot được trình bày trên
Vò trí[m],Vaä n toác daø i[m/s],Goùc[rad],Vaän toá c goùc[rad/s] Vò trí
0.4 Vaä n toác daø i Hình 7.
Goù c
0.2 Vaä n toác goùc

-0.2

-0.4

-0.6

-0.8

-1

-1.2
0 1 2 3 4 5 6 7 8 9 10
Thôø i gian [s]

Hình 5. Đáp ứng của bộ điều khiển trượt không Hình 7. Mô hình thực nghiệm
có nhiễu Các thông số của mô hình:
Ñaùp öù n g heä thoán g ñieàu khieån tröôït vôù i nhieãu
Mp=2.45 (Kg), Mr=0.25 (Kg), L=0.1
0.6
Vò trí (m), R=0.042 (m), D=0.26 (m).
Vò trí[m],Vaän toá c daø i[m/s],Goùc[rad],Vaän toá c goù c[rad/s]

0.4 Vaän toá c daø i

0.2
Goù c
Vaän toá c goùc
Sơ đồ khối hệ thống điều khiển được
0 trình bày trên Hình 8.
-0.2

-0.4

-0.6

-0.8

-1

-1.2
0 1 2 3 4 5 6 7 8 9 10
Thôø i gian [s]

Hình 6. Đáp ứng của bộ điều khiển trượt có

nhiễu
Kết quả mô phỏng cho thấy cả hai bộ Hình 8.Sơ đồ khối hệ thống điều khiển
điều khiển đều đưa các biến trạng thái về Chương trình điều khiển đượcxây dựng
giá trị 0 sau khoảng thời gian 5s. Đối với bộ trên công nghệ vi điều khiển ATMEGA32.
điều khiển LQR hệ thống không ổn định Vi điều khiển đọc các giá trị cảm biến sau
khi bị tác động của nhiễu ngoài. Đối với hệ đó thực hiện quá trình tính toán theo giải
thống sử dụng bộ điều khiển trượt các thuật định trước, sau đó gửi tín hiệu điều
biếntrạng thái dao động quanh điểm cân khiển đến hai động cơ có gắn encoder.
bằng. Tính ổn định bền vững của bộ điều Kết quả thực thi chương trình:
khiển trượt cao hơn bộ điều khiển LQR - Robot đạt trạng thái cân bằngổn
ngay cả khi hệ thống có nhiễu ngoài hoặc địnhkhi sử dụng phương pháp điều khiển
sự không chắc của thông số mô hình. LQR và điều khiển trượt.
- Dao động xung quanh điểm cân bằng
4. THI CÔNG MÔ HÌNH THỰC khoảng 5 độ.
NGHIỆM
Robot thực nghiệm được thiết kế gồm 2 5.KẾT LUẬN
phần: phần cơ khí và mạch điều khiển. Bài báo đã đưa ra hai bộ điều khiển LQR
Phần cơ khí robot bao gồm 2 phần: thân và điều khiển trượt cho robot hai bánh tự
robot và 2 bánh xe đồng trục. Cảm biến cân bằng. Các kết quả mô phỏng và thực
encoder được lắp có trục song song với trục nghiệm cho thấy sự hiệu quả của chúng,
động cơ và được truyền động từ động cơ để đặc biệt bộ điều khiển trượt ổn định bền
đo vị trí dịch chuyển và tính vận tốc dài. Để vững hơn trong trường hợp hệ thống bị tác
xác định góc nghiêng bằng cách dùng một động của nhiễu ngoài. Kết quả cho thấy khả
encode với một thanh kim loại gắn lên trục, năng áp dụng các bộ điều khiển này cho hệ
đầu còn lại của thanh kim loại tì xuống mặt thống ứng dụng trong thực tế.
tiếp xúc. Trong giới hạn góc nghiêng nhỏ
có thể xem góc lệch của encode này đồng
dạng với với nghiêng của robot. Mô hình

TÀI LIỆU THAM KHẢO
[1]Minh Tao Kang, Hak Kyeong Kim, and
Sang Bong Kim, Control System
Design for a Mobile Inverted
Pendulum via Sliding Mode Techni-
que.
[2]Christanto Dwicahyo Putro, Endra
Pitowano, Effect of Dynamics Loading
on a PID controlled Two-Wheeled
Vehicle of Wheelchair-Based Inverted
Pendulum.
[3] Eric Minnaert, Brian Hemmelman, Dan
Dolan, Inverted Pendulum Design With
Hardware Fuzzy Logic Controller.
[4] Martin Seyr, Stefan Jakubek, Gregor
Novark, Neural Network Predictive
Trajectory Tracking of an Autonomous
Two-Wheeled Mobile Robot.
[5] Nawawi S.W, Ahmad M.N, Osman
J.H.S, Husain A.R and Abdollah M.F,
Controller Design for Two-wheels
Inverted Pendulum Mobile Robot
Using PISMC.
28
[6] Nak Soon Choi, Seo Kwang Kim, Gun
You Lee, Hak Kyeong Kim and Sang
Bong Kim, Sliding mode control for
rejecting disturbance of mobile invert-
ed pendulum.
[7] Wilfrid Perruquetti, Jean Pierre Barbot,
Sliding mode control for engineering,
2002.
[8] Muhammad Usman Asad, Umar Frooq,
Athar Hanlf, Mahmood ul Hasan,
Student Member IEEE, Design And
Implementation of Real Time Control-
ler for Self Balancing Robot.
[9] Seul Jung, Member IEEE, Sung Su
Kim. Control Experiment of a Wheel-
Driven Mobile Inverted Pendulum
Using Nueral Network, march 2008.
[10] Jian Huang, Zhi-Hong Guan,
Takayyuki Matsuno, Toshio Fukuda,
Kosuke Sekiyama, Sliding Mode
Velocity Control of Mobile – Wheeled
Inverted Pendulum System.