XÂY DỰNG mô HÌNH và mô PHỎNG hệ XE HAI BÁNH tự cân BẰNG DÙNG kỹ THUẬT điều KHIỂN TRƯỢT

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

1.4.2.

Gi i h n đ tƠi .............................................................................................11
ng pháp nghiên cứu ................................................................................11

1.5.

Ph

Ch

ng 2 ...................................................................................................................12

C S Lụ THUY T ...............................................................................................12
2.1. Nguyên lý ho t đ ng của xe hai bánh cơn b ng. ..............................................12
2.2. Ph

ng pháp đi u khi n tr

t ...........................................................................13

2.2.1. Đi u khi n bám...............................................................................................13
2.2.2. n đ nh hóa .....................................................................................................15
2.3. Lý thuy t v l c Kalman ...................................................................................16

Trang 1


2.3.1. B n chất toán h c của b l c kalman. ...........................................................16

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

CỂN B NG .....................................45

4.1.Thi t k mô hình c khí ......................................................................................45
4.1.1. S đ cấu trúc c khí của robot ......................................................................45
4.1.2. Đ ng c DC-Servo dùng đ truy n đ ng cho robot ......................................46
4.2. S đ khối vƠ chức năng các m ch phần cứng liên quan..................................49
4.2.1. S đ khối c m bi n đo góc nghiêng .............................................................49
4.2.2. Gi i thi u IC MC33932 vƠ chức năng m ch cầu H (ph l c B)....................52
4.2.3. S đ vƠ chức năng IC đi u khi n trung tơm (ph l c B)...............................53
4.2.4. Chức năng m ch giao ti p v i máy vi tính....................................................55
4.2.5. Chức năng m ch đi u khi n nút nhấn ............................................................56
4.2.6. Chức năng m ch hi n th LCD.......................................................................57
4.2.7. Mô-đun thu phát tín hi u qua sóng radio .......................................................58
4.3. Gi i thu t ch

ng trình cho mô hình xe hai bánh .............................................59

Trang 2


4.3.1. Gi i thu t ch

ng trình ...................................................................................59

4.3.2. Chi ti t các hƠm trong ch

ng trình con .........................................................60

4.3.3. Khai báo vƠ qu n lý ng t .................................................................................62

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

Trang 3


DANHăSỄCHăCỄCăB NG
B NGă

TRANG

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 .......................26
B ngă3.2. Giá tr thông số của các b đi u khi n ......................................................45
B ngă4.1. Giá tr đ nh y của c m bi n MMA7260..................................................50

Trang 4


DANH SÁCH CÁC HÌNH
HÌNH

TRANG

Hình 1.1: Xe 2 bánh t cơn b ng ............................................................................. 9
Hình 1.2: Mô hình xe 2 bánh t cơn b ng ............................................................... 9
Hình 1.3: Mô hình Robot JOE .................................................................................. 10
Hình 1.4: Xe Segway .............................................................................................. 10
Hình 2.1: Nguyên lý ho t đ ng của xe 2 bánh t cơn b ng ..................................... 12
Hình 2.2: Cách di chuy n của xe 2 bánh t cơn b ng .............................................. 13
Hình 2.3: Thu t toán b l c Kalman r i r c ............................................................. 19
Hình 2.4: Góc nghiêng thơn xe khi có vƠ không có l c Kalman ............................. 21
Hình 3.1: Bi u di n l c vƠ moment trong mô hình ................................................. 24

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 đ

.................................... 40

Hình 3.14: Đ th so sánh xe gi thăng b ng góc nghiêng l n h n 10 đ .............. 41
Hình 3.15: Đ th so sánh xe di chuy n v i v trí đặt 0.5m .................................... 42
Hình 3.16: Đ th so sánh xe di chuy n v i v trí đặt 1m ........................................ 42
Hình 4.1: Mô hình phần cứng của xe 2 bánh t cơn b ng th c nghi m ................. 46
Hình 4.2: Đ ng c DC dùng đ thi t k xe 2 bánh ............................................... 46
Hình 4.3: Nguyên lý m ch đi u khi n xung ............................................................ 47
Hình 4.4: Đi n áp ra sau khi đi u ch . ................................................................... 48
Hình 4.5: Encoder quang lo i t

ng đối v i 2 xung tín hi u ................................... 48

Hình 4.6: Xung ra của Encoder ................................................................................ 49
Hình 4.7: S đ khối xử lý modun c m bi n góc ..................................................... 49
Hình 4.8: S đ chơn của c m bi n MMA7260 ....................................................... 50
Hình 4.9 :Cấu t o của c m bi n gia tốc MMA7260 ................................................ 50
Hình 4.10: S đ chơn của c m bi n v n tốc góc LISY300AL .............................. 51
Hình 4.11: Board c m bi n góc ................................................................................ 51
Hình 4.12: S đ t ng quát m ch cầu đi u khi n đ ng c ....................................... 52
Hình 4.13: Board m ch cầu H .................................................................................. 53
Hình 4.14: S đ chơn IC vi đi u khi n MC9S12XDT256 ..................................... 53
Hình 4.15: Board đi u khi n trung tơm .................................................................... 55
Hình 4.16: Mô đun thu ậ phát b ng tìn hi u RF 4 kênh........................................... 58
Hình 4.17: L u đ đi u khi n xe 2 bánh .................................................................. 60
Hình 4.18: L u đ th c hi n ng t trong ch

ng trình ............................................. 63

Hình 4.19: S đ xử lý tín hi u c m bi n trong mô hình ......................................... 64

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

nđ nhcủamôhìnhnênvi cđi ukhi nđ

trênth ct đặt ranh m t thửthách. Công ngh robot đƣ đ t đ

cđối

t

thù
ngnày

c nhi u thƠnh t u l n

vƠ xuất hi n trong nhi u lĩnh v c nh : robot dơn d ng giúp vi c gia đình, robot th c
hi n vi c gi i trí - qu ng cáo, robot công nghi p, robot t hƠnh thám hi m trong lòng
đất, robot thăm dò các hƠnh tinh trong khoa h c vũ tr .
K tqu nghiêncứumôhìnhconl cng
conl cng

cc b n,víd nh môhìnhxe-conl c,

cquay…cóth ứngd ngvàk thừasangcácmôhìnht

ngt khácnh ng

cótínhứngd ngth cti nh n,chẳngh nnh môhìnhtênlửa,môhìnhxehaibánht

cân

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

trênđ ahìnhphẳngnh ngkhôngth dichuy nlênxuốngtrênđ ahìnhl ilõmhoặcmặt
phẳngnghiêng.Khidichuy nlênđ i,tr ngl
năngbámvàtr

ngrobotd nvàođuôixelàmmấtkh

tngã.

Trang 8

cđặt


1.2. Cácăk tăqu ănghiênăc uătrongăvƠăngoƠiăn

c

Tai Vi t Nam đƣ có nhi u tác gi th c hi n đ tƠi nƠy nh lu n văn th c sĩ ắ
Đi u khi n phi tuy n xe hai bánh t cơn b ngẰ , Hình 1.1của tác gi Nguy n Gia
Minh Th o, D

ng HoƠi Nghĩa, Nguy n H u Phúc tr

ng ĐHTP.HCM,10/2010,[1].

Đ tƠi nƠy đƣ th c hi n thƠnh công vi c mô ph ng vƠ th c nghi m robot hai bánh t
cơn b ng v i b đi u khi n Backstepping có đáp ứng nhanh, kh năng gi thăng b ng
tốt vƠ kh năng n đ nh tr

c tác đ ng của nhi u lo n.

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

Segway) hình 1.3 do Dean Kamen phát minh, s n xuất b i công ty segway Inc,
bang New Hampshire, Hoa Kỳ. đặc đi m n i b t của Segway lƠ c ch t cơn b ng
nh h thống máy tính, đ ng c vƠ con quay h i chuy n đặt bên trong xe, nó giúp
cho xe dù ch có m t tr c chuy n đ ng v i hai bánh nh ng luôn
b ng, ng

i sử d ng ch vi c ng v đ ng tr

c hoặc đ ng sau đ đi u khi n xe đi

ti n hoặc đi lùi.

Hình 1.3. Xe segway

Trang 10

tr ng thái cơn


1.3. M cătiêu củaăđ ătƠi
-

Tìm hi u t ng quan vƠ ứng d ng của Robot hai bánh t cơn b ng.

-

Xơy d ng mô hình toán cho Robot.

-

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

c,nh đ

c trình bƠy [2],[3]

Gi iăh năđ ătƠi
Trong đ tƠi tác gi ch nghiên cứu v Robot hai bánh t cơn b ng, xơy d ng

mô hình toán, thi t k b đi u khi n b ng ph

ng pháp PID vƠ đi u khi n tr

tđ

đi u khi n Robot. Mô ph ng h thống xe 2 bánh t cơn b ng trên phần m m Matlab
vƠ thi t k mô hình th c nghi m.
1.5. Ph
-

ngăphápănghiênăc u
Xơy d ng mô hình lý thuy t bao g m:
 Ti p c n mô hình t

ng đ

ng, mô hình con l c ng

c đ n mô hình

Robot hai bánh t cơn b ng.
 Xơy d ng các ph

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

c đi ukhi nđ cl pnhau.

`

`

Cơnăb ng

`

B ănghiêng

Cáchăđi uăkhi n

Hình 2.1Nguyênlýho tđ ngcủaxehaibánht cânb ng
- Khicân b ng,gócnghiêng củathânxe v iph
Tr

Khixe

nghiêngv phíatr

ngh pnày,cầnph iđi ukhi nxech yv phíatr

ngtr ngl cb ng0.
c,n ukhôngđi ukhi nthìxes b ngã.
csaochogóc

nghiêngb ng0,đ xethăngb ngtr l i.
Tr

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

Xe cơn b ng

Đ xe di chuy n v tr c thơn
xe ph i nghiêng v tr c

OG

OG
Forward

Forward
Đ xe xuống dốc thơn xe
ph i nghiêng v sau đ gi m
v n tốc vƠ gi thăng b ng

Đ xe lên dốc thơn xe ph i
nghiêng v tr c m t góc l n
h n góc nghiêng của dốc

Hình 2.2: Cách di chuy n của xe 2 bánh cơn b ng
2.2.ăPh

ngăphápăđi uăkhi nătr

t

2.2.1.ăĐi uăkhi năbám
Đối t

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

−1

(2.2)


khi đó, ta đ

c bi u di n tr ng thái
1
2

=
=
..

2
3

(2.3)

.
−1 =
=
+ ( )

y = x1
Yêuă cầu: Xác đ nh tín hi u đi u khi n u sao cho tín hi u ra y bám theo tín
hi u đặt r
- Mặt tr

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

+ ⋯+

+

−2

−1

−

Ph
mặt tr

+ ⋯+

−1

+

ng trình

1 2

−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

= 0 xác đ nh m t mặt cong trong không gian n chi u g i lƠ

t. Vấn đ lƠ xác đ nh lu t đi u khi n u đ đ a các quỹ đ o pha của h thống

v mặt tr

t vƠ duy trì trên mặt tr

t m t cách b n v ng đối v i các bi n đ ng của

f(x) và g(x).
Lấy đ o hƠm (2.7) vƠ áp d ng vƠo (2.2) ta có
=

+

+
+

1

−2

3

−

Có th ch n u sao cho

−

+

−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

u=−

g(x)

c.

c xác đ nh b i:

f x + an−2 xn − r n−1 + ⋯
+a1 x3 − r + a0 x2 − r + αsign(S)

- Tính b n v ng của mặt tr

(2.10)

t

Trong đi u ki n có sai số mô hình, lu t đi u khi n (2.9) luôn đ a đ
đ o pha của h thống v mặt tr
N u

> 0 thì

= 0 n u đi u ki n sau đơy đ

t

c th a mƣn:

<0

N u S < 0 thì S > 0

N u

S = 0 thì S = 0
- Ph

(2.11)

ng pháp ch n mặt tr

+ không ph thu c t
+ Ph

c quỹ

t: HƠm S trong (2.5) th a m n đi u ki n sau:

ng minh vƠo u nh ng S ph thu c t

ng minh vƠo u.

ng trình vi phơn (2.6) Hurwitz đ nghi m e → 0 khi t → ∞

2.2.2.ă năđ nhăhóa
- Đối t

ng đi u khi n:

Xét h thống

1
2

=

2

=
1,

1( 1, 2)

+ ( 1,

2

2)

(2.12)

Yêu cầu: đ a vec t tr ng thái x v 0
- Mặt tr

t: Đ nh nghĩa

Trong đó, �( 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:

=

2

−

�

1

1

1, 2

= f2 x1 , x2 + g x1 , x2 u −

∂φ

f
∂x 1 1

x1 , x2

Trang 15

(2.14)


Ta có th ch n u sao cho S = −αsign(S)

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

(2.16)

( )

Năm 1960 R.E Kalman đƣ xuất b n m t bƠi báo v i tiêu đ ắA New Approach
to Linear Filtering and Predication ProblemsẰ[8] . Nghiên cứu của Kalman đƣ kh c
ph c h n ch của b l c Wiener-Hopf trong vi c gi i quy t bài toán thống kê t
nhiên. K từ đó, danh từ b l c Kalman đƣ ra đ i. B l c nƠy
∈

của quá trình th i gian r i r c theo ph
=

−1

+

V i vi c đo
=

+

∈

−1

+

cl

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)

à ( )~ (0, )

ng sai nhi uă quáă trình Q vƠ ma tr n hi p

ng sai nhi uăđo R ph i thay đ i theo từng th i đi m, tuy nhiên chúng ta có th

gi sử lƠ h ng số.
Ma tr n A(nxn) trong ph
thái từ th i đi m tr

ng trình sai phơn (2.17) lƠ ma tr n chuy n tr ng

c (k-1) sang th i đi m hi n t i (k). Chú ý r ng, trong th c t A

có th thay đ i theo từng th i đi m. Nh ng chúng ta cũng có th gi sử nó lƠ h ng số.
Ma tr n B (nx1) lƠ ma tr n đi u khi n có lối vƠo ∈

ph

ng trình (2.18) lƠ ma tr n đo l

thay đ i theo từng th i đi m,

. Ma tr n H(mxn) trong

ng (ma tr n quan sát). Trong th c t H có th

đơy chúng ta gi sử lƠ h ng số.

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
−

−

=

−

=

−

(

−

(

−

)

−

)

ng sai l i ti n
ng sai l i h u

cl

ng

ng trình cho b l c Kalman, chúng ta tìm ra m t ph
căl

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

(2.22)

Xuất phát từ ph

đoán

ng nh sau

(2.21)

Ma tr n hi p ph

thái ti nă

cl

(2.20)

Ma tr n hi p ph
=

ng vƠ l i h u

− xk−

=

−

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

ng sai l i h u

Tr
lim

(2.25)

T
HP −
k H +R

ng trình (2.25), chúng ta thấy r ng, khi ma tr n hi p ph
−1

−

→0

=

−

→0

=0

c xác đ nh nh sau:

ng sai l i ti n

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 đ

lim

cl

−

ng R ti n đ n 0 thì giá tr zk là chính xác h n,

l i kém chính xác. Tr

Trang 17

ng h p khi giá tr hi p


ph

ng sai l i

cl

c Pk− ti n t i 0, giá tr

ng tr

lƠ kém chính xác trong khi đó

giá tr tiên đoán Hxk− l i đ t đ chính xác h n.
2.3.2.ăB năchấtăth ngăkêăcủaăl căKalman.

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

của phơn bố tr ng thái n u các đi u ki n (2.29) đ
tr ng thái h u

cl

c tho mƣn. Hi p ph

ng sai l i

ng trong công thức (2.25) ph n ánh s thay đ i của phơn bố

tr ng thái.
P xk zk ~N E xk , E xk − xk xk − xk

T

) = N(xk , Pk )

(2.30)

Tómăl i: B l c Kalman

cl

ng m t quá trình b ng vi c sử d ng m t d ng

của đi u khi n ph n h i, b l c

cl

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.

Trang 18


Chu trình c p nh t th i gian

Chu trình c p nh t giá tr đo

(1) D đoán tr ng thái k ti p

(1) Tính toán đ l i Kalman

(2) D đoán sai số t

(2) C p nh t giá tr

ng quan k ti p

tr đo đ

c

cl

ng v i giá

th i đi m hi n t i (

(3) C p nh t sai số t

)

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].

t

ng tr ng cho ti nă trìnhă nhi u covariance.

đơy, nó ch mức đ tin

c y của c m bi n gia tốc góc quan h v i c m bi n v n tốc góc.
=
Ch

_

0
0

_

=

0.001
0

0
0.003

ng trình b l c Kalman cho vi đi u khi n g m 2 hƠm:

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

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:

=

−

=

_ �

=
_ �

0

_ �

Ti p theo, ta c p nh t ma tr n covariance qua công thức thứ hai của chu trình
c p nh t

cl

−

ng

=

−1

+

V i A lƠ ma tr n Jacobian của

=

Chi ti t ch

vƠ có giá tr 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 đ

m ib

c th i gian dt (2.5ms), nên

c th c hi n theo chu kỳ 7.5ms.

Ta tính ma tr n C, lƠ ma tr n Gradient của giá tr đo l
đ i (hay còn g i lƠ vector hi u ch nh sai số đo) đ
C=

d ang le _m

d angle _m

d angle

d gyro _bias

Sau đó, ta th c hi n lần l

= 1

c xác đ nh nh sau:

0

t nh sau:

+ Tính sai số gi a giá tr góc đo đ

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.

6
5

Signal With Kalman Filter
Signal Without Kalman Filter

4
3

Angle (degree)

2
1
0
-1
-2
-3
-4
-5

0.5

1

1.5
Time (sec)

2

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.

Ta có th sử d ng b l c b ph (complementary filter) đ k t nối hai tín hi u
từ Accelerometer vƠ gyro thƠnh m t tín hi u duy nhất. Accelerometer đ
m t b l c thông thấp, còn gyro đ

c đ a qua

c đ a qua m t b l c thông cao, sau đó hai tín

Trang 21


hi u đƣ đ

c l c s nối v i nhau thƠnh m t tín hi u duy nhất.

ph lƠ tín toán nhanh, d thi t k . Nh

u đi m của b l c b

c đi m của b l c nƠy lƠ b n chất v n của b

l c thông cao vƠ thông thấp, có nghĩa đ l i tín hi u không b ng nhau trong toƠn d i
đo, b l ch pha rõ r t t i vùng nối tần số. H n n a giá tr
nh t th

_ �

ng xuyên d lƠm cho b l c mất tác d ng khi lƠm vi c

không đ

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

ng của dòng đi n ngõ vƠo.

Đối v i b l c Kalman, thu t ng ắl cẰ không có nghĩa nh các b l c trên.
Đơy lƠ m t gi i thu t tính toán vƠ

cl

vƠo đ

c m t giá tr ra đáng tin c y nhất cho vi c xử lý

c cung cấp t i nó đ có đ

ng thống kê tối u tất c các thông tin ngõ

ti p theo. Do v y l c Kalman có th sử d ng đ lo i b các tín hi u nhi u mƠ đ
mô hình là nh ng tín hi u nhi u tr ng trên tất c d i thông mƠ nó nh n đ
vƠo, d a trên các thống kê tr

c đo vƠ chuẩn tr c l i giá tr

cl

c

c từ ngõ

ng b ng các giá

tr đo hi n t i v i đ l ch pha gần nh không t n t i vƠ có đ l i tối thi u xấp x 0 đối

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 v t lý. Chính vì đi u nƠy lƠm b l c

Kalman tr nên ph d ng h n tất c nh ng b l c khác trong vi c xử lý tín hi u
chính xác của các c m bi n t a đ , c m bi n la bƠn, GPS, gyro,…
Hi n nay, v i s phát tri n của trí tu nhơn t o (AI), các ứng d ng AI đ
d ng trong nhi u lĩnh v c, đặc bi t lƠ đ nh h

c sử

ng trong hƠng không vũ tr , nh ng

không vì th mƠ b l c Kalman gi m vai trò, mƠ ng

c l i đó chính lƠ m t ngõ ra

đáng tin c y đ cho m ng noron xử lý vƠ ra các quy t đ nh trong các tƠu thám hi m
t hƠnh.

Trang 23


Ch

ngă3

THI TăK ăMỌăPH NG
XEăHAIăBỄNHăT ăCỂNăB NG

3.1. Môhìnhhóaxehaibánht cânb ng
Xơy d ng h ph

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]

,

V trí của bánh xe theo tr c x, tr c y

[m]

,

V trí của tr ng tơm thơn xe theo tr c
x vƠ tr c y

[m]

Kho ng cách từ tr ng tơm thơn xe đ n
tr c z của hai bánh xe

L

�=�

=

Thôngăs

0.35[m]

Bán kính bánh xe

0.075

Góc nghiêng thân xe

[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]

,

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

[Nm]

g

Gia tốc tr ng tr

ng

9.81[m/s2]

,

Moment của đ ng c nối v i bánh xe
trái, bánh xe ph i

[Nm]

+

Trang 25

0.35[m]