M CăL C
QUY T Đ NH GIAO Đ TÀI ....................................Error! Bookmark not defined.
Lụ L CH KHOA H C ................................................................................................ i
L I CAM ĐOAN ....................................................................................................... ii
C M T .................................................................................................................... iii
TịM T T .................................................................................................................. iv
ABSTRACT ................................................................................................................ v
M C L C ...................................................................................................................1
DANH SỄCH CỄC B NG ........................................................................................4
DANH SÁCH CÁC HÌNH .........................................................................................5
Ch

ng 1 .....................................................................................................................8

T NG QUAN .............................................................................................................8
1.1. T ng quan chung v lĩnh v c nghiên cứu ...........................................................8
1.2. Các k t qu nghiên cứu trong vƠ ngoƠi n

c ....................................................9

1.3.

M c tiêu của đ tƠi ..........................................................................................11

1.4.

Nhi m v vƠ gi i h n của đ tƠi .....................................................................11

1.4.1.

Nhi m v đ tƠi ...........................................................................................11

2.3.2. B n chất thống kê của l c Kalman. ...............................................................18
2.3.3. Gi i thu t l p trình b l c Kalman r i r c. ....................................................19
2.4. So sánh các b l c v i l c Kalman. ..................................................................21
Ch

ng 3 ...................................................................................................................24

THI T K MỌ PH NG ..........................................................................................24
XE HAI BỄNH T

CỂN B NG ............................................................................24

3.1. Môhìnhhóaxehaibánht cânb ng ........................................................................24
3.2. Thi t k b đi u khi n cho xe hai bánh cơn b ng .............................................31
3.2.1. B đi u khi n đặt c c .....................................................................................31
3.2.2. B đi u khi n PID ..........................................................................................35
3.2.3. B đi u khi n tr

t .........................................................................................36

3.3. So sánh k t qu mô ph ng của h thống ...........................................................39
3.4.

u, khuy t đi m của b đi u khi n thi t k cho mô hình h thống. ................43

Ch

ng 4 ...................................................................................................................45

THI CỌNG MỌ HỊNH XE HAI BỄNH T

4.4. Cách thu nh n vƠ xử lý tín hi u c m bi n .........................................................64
4.5. Mô hình xe hai bánh đƣ th c nghi m .................................................................65
Ch

ng 5 ...................................................................................................................66

K T QU TH C NGHI M ....................................................................................66
5.1. Xe gi thăng b ng không di chuy n .................................................................66
5.2. Xe gi thăng b ng khi di chuy n đ n v trí đặt tr

c .......................................67

5.3. Xe gi thăng b ng khi tác d ng ngo i l c ........................................................68
5.4. Xe gi thăng b ng di chuy n ti n ậ lùi .............................................................68
Ch

ng 6 ...................................................................................................................70

K T LU N VÀ H
6.1. Các k t qu đ t đ

NG PHỄT TRI N ................................................................70
c ..........................................................................................70

6.2. M t số h n ch ...................................................................................................70
6.3. H

ng phát tri n đ tƠi.......................................................................................71

TÀI LI U THAM KH O .........................................................................................72

Hình 3.2:S đ mô hình xe hai bánh trong mô ph ng Matlab ................................ 29
Hình 3.3:Đ th khi xe đứng, không có moment tác đ ng ....................................... 30
Hình 3.4: Đ th khi xe nghiêng góc nh , không có moment tác đ ng ................. 30
Hình 3.5: Đ th khi có moment tác đ ng vƠo th i đi m 1 giơy ............................ 31
Hình 3.6: nh h

ng của các c c lên h thống ..................................................... 33

Hình 3.7: S đ mô ph ng b đi u khi n PD cho h thống ................................... 35
Hình 3.8: K t qu mô ph ng b PID nhi u vòng ................................................... 36
Hình 3.9: S đ b đi u khi n Tr
Hình 3.10: Khối đi u khi n tr

t ........................................................................ 37

t trong h thống mô ph ng ................................... 38

Hình 3.11: K t qu mô ph ng b đi u khi n PD ậ tr

t ........................................ 39

Hình 3.12: Đ th xe gi thăng b ng góc nghiêng lƠ 5 đ

Trang 5

...................................... 39


Hình 3.13: Đ th xe gi thăng b ng góc nghiêng lƠ 10 đ


Hình 4.20: Hình nh mô hình xe 2 bánh th c nghi m ............................................. 65

Trang 6


Hình 5.1: Đ th góc, v trí xe thăng b ng ................................................................ 66
Hình 5.2:Đ th góc, v trí xe di chuy n đ n v trí đặt tr

c .................................... 67

Hình 5.3: Đ th góc, v trí khi tác đ ng ngo i l c ................................................. 68
Hình 5.4: Đ th góc, v trí xe di chuy n ti n ........................................................... 69
Hình 5.5: Đ th góc, v trí xe di chuy n lùi ............................................................. 69

Trang 7


Ch

ngă1

T NGăQUAN
1.1. T ngăquanăchungăv ălƿnhăv cănghiênăc u
Trongngànht đ nghóaậđi ukhi nt đ ngnóichungvàđi ukhi nh cnói
riêng,môhìnhconl cng
b iđặctínhđ ngkhông

clàm ttrongnh ngđốit

ngnghiêncứuđi nhìnhvàđặc

b ng…
Robothaibánht cânb ngđ
ng

cxemnh cầunốikinhnghi mgi amôhìnhcon

cv irobothaichânvàrobotgiốngng

tr c,dođókh cph cđ

cnh ngnh

i.Đâylàd ngrobotcóhaibánh

l c
đ ng

cđi mvốncócủacácrobothaihoặcbabánh

kinhđi n.Cácrobothaihoặcbabánhkinhđi n,theođócócấut og mbánhd nđ ng
vàbánht dođ đỡtr ngl

ngrobot.N utr ng

l

ngđ

nhi uvàobánhláithìrobots không nđ nhvàd b ngã,cònn uđặtvàonhi ubánh
đuôithìhaibánhchínhs mấtkh năngbám.Nhi uthi tk robotcóth dichuy ntốt


Hình 1.1.Xe 2 bánh t cơn b ng
Đ tƠi xe hai bánh t cơn b ng của tác gi TrƠ Ti Na vƠ Nguy n Tấn H u,
tr

ng ĐH L c H ng, tháng 11 năm 2013. Ph

ng pháp nghiên cứu của tác gi lƠ

kh o sát cấu trúc robot, c m bi n IMU, m ch đi u khi n đ ng c , thu t toán l c
nhi u nh b l c Kalman, b l c Complimentary vƠ thu t toán đi u khi n PID. H n
ch đ tƠi lƠ robot ch a di chuy n đ

c.

Hình 1.1. Mô hình xe 2 bánh

Trang 9


Robot JOE [10],Hình 1.2, Đơy lƠ s n phẩm của phòng thí nghi m Đi n tử
công nghi p của Vi n công ngh Federal, Lausanne, Th y Sĩ. Robot JOE cao 65cm,
nặng kho ng 12kg, tốc đ tối đa 1,5 m/s, có th di chuy n trên dốc nghiêng 30
đ .H thống đi u khi n g m hai b đi u khi n ắkhông gian tr ng tháiẰ (state space)
tách r i nhau, ki m soát đ ng c đ gi cơn b ng cho h thống.B đi u khi n trung
tơm vƠ xử lý tín hi u lƠ m t board xử lý tín hi u số (DSP) phát tri n b i chính nhóm
vƠ của vi n Federal, k t h p v i FPGA của XILINC.

Hình 1.2. Robot JOE
Segway PT [11](vi t t t của Segway Personal Transporter - xe cá nhân

Thi t k mô ph ng b đi u khi n d a vƠo ph

ng pháp đi u khi n tr

t

cho xe hai bánh t cơn b ng trên Matlab Simulink.
-

Thi công vƠ ch t o mô hình xe hai bánh t cơn b ng.

1.4. Nhi măv ăvƠăgi iăh năcủaăđ ătƠi
1.4.1. Nhi măv ăđ ătƠi
Mô hình hóa h thống đ ng của xe hai bánh t cơn b ng nh đ
bƠy

tƠi li u tham kh o[1]
-

Sử d ng các ph

ng pháp đi u khi n tr

mô hình xe hai bánh t cơn b ng, h con l c ng
1.4.2.

c trình

t đ thi t k b đi u khi n cho



ng trình toán cho h thống vƠ ti n hƠnh mô ph ng

trên phần m m Malab Simulink.
-

Xơy d ng mô hình th c nghi m bao g m:
 Thi t k phần c khí, đ nh v trí 2 đ ng c , ngu n, board trung tơm vƠ
board c m bi n.
 Thi t k m ch đi u khi n trên Orcad 9.2.
 Xơy d ng gi i thu t vƠ vi t ch

ng trình đi u khi n.

Trang 11


Ch

ngă2

C ăS ăLụăTHUY T
2.1.ăNguyênălýăho tăđ ngăcủaăxeăhaiăbánhăcơnăb ng.
Xehaibánht cânb ngho tđ ngk th pgi amôhìnhconl cng
xeđ

cv ih

haibánh



ngh pxenghiêngv phía

saucũngph iđi ukhi nt

ngt nh v y,nghĩalàđi ukhi nxech y

v

h

ng

đang

nghiêng,đ robot thăngb ngtr l i.
Hình nh d

i đơy lƠ các tr

ng h p di chuy n của xe 2 bánh t cơn b ng.

Trang 12


OG

OG

Forward

ng: Xét h thống tr
= ( , , ,

Đặt

1

= ,

2

= ,

3

−1

t bi u di n b i ph

ng trình vi phơn
(2.1)

)

= ,…,

=

Trang 13


t: Đ nh nghĩa tín hi u sai l ch

e=y−r
vƠ mặt tr t S nh sau:

(2.4)

S = e n−1 + an−2 e n−2 + ⋯ + a1 e + a0 e

Trong đó, a0 , a1 , … , an−3 , an−2 lƠ các h số đ

đặc tr ng của ph

(2.5)
c ch n tr

c sau cho đa thức

ng trình vi phơn sau Hurwitz có tất c các nghi m v i phần th c

âm
−1

+

Khi đó, n u

−2

−2

+

0

(2.6)

=0

→ 0 thì → ∞

= 0 thì sai l ch

Thay (2.4) vƠ (2.2) vƠo (2.5) ta đ
=

1

c

0 1

+ ⋯+

+

1

(2.7)

0

−

+

−1
0

= −� �

+⋯

(2.8)

( )

(2.9)

2

−

Trang 14


trong đó � lƠ h ng số d

ng ch n tr

Khi đó lu t đi u khi n u đ
1


+� 0 =0

+ H thống con

1

có b c t

2

− �( 1 )

(2.13)

c ch n th a các đi u ki n sau

=

t ađ .
+

=

1 ( 1 , �( 1 ))

có đi m cơn b ng n đ nh ti m c n t i gốc

ng đối b ng 1

- Lu t đi u khi n:

trong đó, � lƠ h ng số d
=−

1

2

( )

ng ch n tr
�

−

1

1

1

2.3.ăLýăthuy tăv ăl căKalman

(2.15)

c, khi đó lu t đi u khi n u đ

+� �

c xác đ nh b i


ng tr ng thái

ng trình sai phơn tuy n tính.
(2.17)

−1

(2.18)

Bi n ng u nhiên

bi u di n nhi u đo vƠ nhi u quá trình. Trong thu t

và

toán l c Kalman, đặc tính thống kê của hai bi n nƠy ph i đ

c bi t tr

c. Chúng ta

gi sử các bi n nƠy đ c l p có ph tr ng vƠ phơn bố Gauss.
~

0,

Trong th c t , ma tr n hi p ph
ph

(2.19)

2.3.1.ăB năchấtătoánăh căcủaăb ăl căkalman.
Chúng ta đ nh nghĩa

∈

lƠ tr ng thái ti n

Trang 16

cl

ng

th i đi m thứ k,


lƠ tr ng thái h u

∈

c l

ng t i th i đi m thứ k vƠ cho ra giá tr đo

Chúng ta có th đ nh nghĩa các l i ti n
−

−

=


c xác đ nh trong ph

(2.24)

Giá tr zk − Hxk− trong công thức (2.39) đ
−

và giá tr tiên

ng trình sau.

xk = xk− + K(zk − Hxk− )

tr tiên đoán

ng

ngă�� nh lƠ m t t h p tuy n tính của tr ng

ngă � �− vƠ s khác nhau gi a giá tr đo th că t

că l
đ

ng:

(2.23)

trình tính toán tr ng thái h uă


−

cl

.

vƠ giá tr th c t

đo đ

c g i là giá tr sai khác gi a giá

c. Giá tr nƠy b ng 0 nghĩa lƠ hai giá tr

hoƠn toƠn đ ng nhất v i nhau.
Ma tr n K(mxn) trong ph

ng trình (2.24) g i lƠ h số khu ch đ i Kalman

nh m m c đích tối thi u hoá hi p ph
khu ch đ i Kalman có th đ

c xác đ nh b i ph

K k = Pk− HT (HPk− HT + R)−1 =
Quan sát ph

l i đo l


cl

ng

ti n t i

thì
(2.27)

ng sai l i đo l

trong khi giá tr tiên đoán

ng sai

(2.26)

ng h p khác, khi hi p ph

Khi hi p ph

ng trong (2.38). Đ

ng trình sau:

T
P−
kH

ng R ti n t i 0 thì h số khu ch đ i K đ


Công thức (2.39) th hi n b n chất thống kê của ti n
trên tất c các giá tr đo tr

c

ng xk− qui đ nh

cl

.

(2.28)

=
E xk − x k xk − x k

Tr ng thái h u

cl

T

(2.29)

= Pk

ng trong ph

ng trình (2.24) ph n ánh giá tr trung bình


ng tr ng thái quá trình t i m t vƠi th i đi m

vƠ sau đó quan sát ph n h i trong d ng của nhi u đo.
Gi i thu t b l c Kalman r i r c s bao g m 2 chu trình chính: chuătrìnhăc pă
nh tăth iăgianăvƠăchuătrìnhăc pănh tăgiáătr ăđo. Chu trình c p nh t th i gian d a
vƠo giá tr tr ng thái hi n t i đ d đoán tr

c giá tr

cl

Chu trình c p nh t giá tr đo d a vƠo k t qu th t đo đ
đi u ch nh giá tr

cl

ng đƣ đ

c d báo tr

c

ng của th i đi m k ti p.
c t i ngay th i đi m đó đ

chu trình c p nh t th i gian. Giá

tr của chu trình c p nh t giá tr đo lƠ giá tr ngõ ra của b l c Kalman.



ng quan

Kh i đ ng giá tr
c
l ng cho
và
Hình 2.3: Thu t toán b l c Kalman r i r c
2.3.3.ăăGi iăthu tăl pătrìnhăb ăl căKalmanăr iăr c.
Khai báo bi n:
Pinit =
:t

1
0

0
1

ng tr ng cho giáă tr ă nhi u covariance. Trong tr

ma tr n 1x1, là giá tr đo đ

c mong đ i. Giá tr của

ng h p này, nó là

ph thu c vƠo đ nh y [V/g]

của c m bi n gia tốc góc, nên ta có th ch n R = 0.08 [V].

c th c hi n trong kho ng th i gian dt trên giá tr c s của c m

bi n v n tốc góc. Nó có chức năng c p nh t giá tr góc hi n th i vƠ v n tốc
l

ng.

Trang 19

c


Giá tr gyro_m đ

c chia thƠnh đúng đ n v th t, nh ng không cần b

gyro_bias đ nghiêng. B l c quan sát giá tr góc nghiêng.
Vector giá tr đ
Suy ra

c khai báo nh sau:

=

−

=

_ �


_ �
_ �

_ �

=

ng trình của hƠm state_update() đ

l c C.

−1
0

0
0

c trình bƠy trong phần ph

- Hàm Kalman_update()
HƠm nƠy đ

c g i đ th c thi khi đƣ có sẵn giá tr của c m bi n gia tốc góc

(bi n angel_m). Giá tr của angle_m ph i đ

c chuẩn mức 0.

HƠm nƠy không cần ph i th c hi n
trong lu n văn nƠy, hƠm nƠy đ

l

ng v i giá tr mong

c b i c m bi n gia tốc vƠ giá tr góc

c

ng.
+ Th c hi n lần l

t 3 phép toán của chu trình c p nh t giá tr đo trong hình

2.3.
Chi ti t ch

ng trình của hƠm Kalman_update() đ

Trang 20

c trình bƠy trong phần


ph l c C.
Hình 2.8 trình bƠy s so sánh giá tr góc nghiêng thơn xe (trong mô hình xe
th c nghi m) tr

c vƠ sau khi đ

c l c b i b l c Kalman.

2.5

3

Hình 2.4: Góc nghiêng thân xe khi có vƠ không có l c Kalman
Nhận xét: Trong hình 2.4, ta thấy tín hi u góc nghiêng thơn xe tr

c khi đ a

vƠo b l c Kalman có đ dao đ ng rất l n vƠ nhi u tín hi u nhi u. Còn tín hi u góc
nghiêng sau khi đƣ đ

c l c b i b l c Kalman có đ dao đ ng nh vƠ gần nh

không có nhi u.
2.4.ăăSoăsánhăcácăb ăl căv iăl căKalman.
Đối v i b l c thông thấp, thông cao hoặc thông d i (l c th đ ng) xấp x
Butterworth, Bassel vƠ Chebychev hay elliptic, th

ng đ

c sử d ng cho m t tín

hi u vƠo vƠ m t tín hi u ra, v i tần số lƠm vi c xác đ nh. NgoƠi d i tần nƠy, tín hi u
s b l ch pha, hoặc đ l i không còn lƠ h ng số mƠ b tối thi u hóa. Do v y trong
tr

ng h p nƠy, ta dùng hai c m bi n đ đo m t giá tr lƠ góc (cũng nh v n tốc góc),

nên vi c ch dùng m t b l c th đ ng t ra không phù h p.


cc p

nh ng môi tr

ng

rung đ ng hay có nhi t đ khác nhau. NgoƠi ra, cũng ph i k đ n vi c chuẩn tr c b
l c nƠy khá khó khăn n u không có thi t b quan sát.
Tóm l i, các b l c thông th

ng lƠ m t kỹ thu t dùng phần cứng (các m ch

đi n tử R,L,C) hoặc phần m m (l c FIR, l c IIR, của s Hamming… trong xử lý tín
hi u số) lƠ nh m gi l i các tín hi u trong m t kho ng thông d i tần số nƠo đó vƠ lo i
b tín hi u
đ i tr

các d i tần số còn l i. Đối v i vi c xơy d ng b l c b ng phần cứng ra

c khi dùng các b l c phần m m, nh ng vi c hi u ch nh đặc tính, thay đ i

các tham số của b l c phức t p h n rất nhi u so v i sử d ng gi i thu t xử lý tín hi u
số.
Trong các b l c nƠy, n u t n t i các tín hi u nhi u trong d i thông tần thì k t
qu tín hi u tr nên kém đi rất nhi u đ có th xử lý vƠ đi u khi n h thống m t cách
n đ nh. Đi u nƠy cƠng t ra rất th c t đối v i các b l c phần cứng, vốn rất d b
nhi u b i các tín hi u đi n trong lúc ho t đ ng do s kém chính xác của các linh ki n
vƠ s bất th


v i nh ng tín hi u ngõ vƠo không đáng tin c y. Mặc dù ph i tốn khá nhi u th i gian
xử lý l nh, nh ng v i tốc đ hi n t i của các vi đi u khi n lƠm vi c tính toán
l

c

ng tối u của b l c nƠy tr nên đ n gi n vƠ đáng tin c y rất nhi u. Nh có c ch

Trang 22


t c p nh t các giá tr c s (bias) t i m i th i đi m tính toán cũng nh xác đ nh sai
l ch của k t qu đo tr

c v i k t qu đo sau nên giá tr đo luôn đ

c n đ nh, chính

xác, gần nh không b sai số v đ l i vƠ đ l ch pha của các tín hi u. H n th , do
đ

c xơy d ng b i hƠm tr ng thái, vì v y b l c Kalman có th k t h p không ch hai

tín hi u từ hai c m bi n, mƠ có th k t h p đ
khác nhau của cùng m t giá tr đ i l

c nhi u c m bi n đo

nh ng d i tần



ng trình tr ng thái mô t h thống xe hai bánh t cơn

b ng[1].

Hình 3.1: Bi u di n l c vƠ moment trong mô hình

Trang 24


B ngă3.1:Ký hi u vƠ giá tr các thông số của xe hai bánh cơn b ng
Kýăhi u
=

=

Khối l

ng bánh xe trái vƠ ph i

Giáătr ă[đ năv ]
0.45[kg]

Khối l ng qui đ i t i tr ng tơm của
thân xe

6.5[kg]

,


[rad]

Góc quay của xe

[rad]

D

Kho ng cách gi a hai bánh xe

,

L c ma sát gi a bánh xe trái, bánh xe
ph i v i mặt đ ng

[Nm]

,

Ph n l c của mặt đất t ng tác lên
hai bánh xe trái, bánh xe ph i

[Nm]

,

L c t ng tác gi a thơn xe vƠ hai
bánh xe trái, bánh xe ph i

[Nm]