datung imu

7/29/2019 Datung Imu

1/91

H QUC GIA TP. H CH MINH CNG HA X HI CH NGHA VIT NAMTRNG I HC BCH KHOA c lp T do Hnh phcKHOA: IN IN TB MN: IU KHIN T NG

------------------o0o-----------------

NHIM V LUN VN TT NGHIP

H V TN: NG ANH TNG MSSV: 40802528

NGNH: IU KHIN T NG LP: DD08KSTD

1. ti lun vn:

XY DNG B C LNG GC QUAY BA TRC

2. Nhim v (Yu cu v ni dung v s liu ban u) : Tm hiu v nh gi cc gii thut khc nhau xy dng b c lng ba gc

quay.

Hin thc gii thut trn h thng nhng dng vi iu khin vi tn s cp nht gitr ba gc quay l 100Hz.

nh gi sai s ca h thng IMU v ci tin sai s RMS ba gc nh hn 3 Xy dng phn mm ha 3D hin th m hnh IMU trn my tnh.

3. Ngy giao nhim v lun vn : 01/09/2012

4. Ngy hon thnh nhim v : 17/12/2012

5. Ngi hng dn : TS. NGUYN VNH HO

Ni dung v yu cu LVTN c thng qua B Mn.Ngythngnm..

CH NHIM B MN NGI HNG DN CHNH(K v ghi r h tn) (K v ghi r h tn)

PHN DNH CHO KHOA, B MN :Ngi duyt (chm s b) : ............ .............n v : ........................................................

Ngy bo v : .................... ...........................im tng kt :.............................................

Ni lu tr lun vn : .............................. ....


2/91

B GIO DC V O TO CNG HA X HI CH NGHA VIT NAMI HC QUC GIA TP.HCM c lp T do Hnh phcTrng i Hc Bch Khoa *******Khoa: in in tB mn: iu khin t ng Ngy.thng.nm 2012

PHIU CHM BO V LVTN

(Dnh cho ngi hng dn)

1. H v tn SV: ..............................................................................................................MSSV: ...................................... Chuyn ngnh: iu Khin T ng

2. ti : ........................................................................................................................3. H tn ngi hng dn: TS. Nguyn Vnh Ho4. Tng qut v bn thuyt minh: ....................................................................................S trang : ............ ...... S chng : ...................

Bng s liu : ............ ...... S hnh v : ...................S ti liu tham kho : ............ ...... Phn mm tnh ton : ...................Hin vt (sn phm) : ..................5. Tng qut v cc bn v :- S bn v : Bn A0 Bn A1 Kh khc- S bn v v tay : S bn v trn my tnh :6. Nhng u im chnh ca LVTN :

....................................................................................................................................

....................................................................................................................................

....................................................................................................................................

....................................................................................................................................7. Nhng thiu st chnh ca LVTN :

....................................................................................................................................

....................................................................................................................................

....................................................................................................................................

....................................................................................................................................8. ngh : c bo v B sung thm bo v Khng c bo v 9. 3 cu hi SV phi tr li trc Hi ng :

1) ....................................................................................................................................................................................................................................................................2) ....................................................................................................................................................................................................................................................................

3) ....................................................................................................................................................................................................................................................................10. nh gi chung (bng ch : gii, kh, TB) : im ________/10

K tn (ghi r h tn)


3/91

B GIO DC V O TO CNG HA X HI CH NGHA VIT NAMI HC QUC GIA TP.HCM c lp T do Hnh phcTrng i Hc Bch Khoa *******Khoa: in in tB mn: iu khin t ng Ngy. thng. nm 2012

PHIU CHM BO V LVTN

(Dnh cho ngi phn bin)

1. H v tn SV: ..............................................................................................................MSSV: ...................................... Chuyn ngnh: iu Khin T ng

2. ti : ........................................................................................................................3. H tn ngi phn bin: ..............................................................................................4. Tng qut v bn thuyt minh: ....................................................................................S trang : ............ ...... S chng : ...................

Bng s liu : ............ ...... S hnh v : ...................S ti liu tham kho : ............ ...... Phn mm tnh ton : ...................Hin vt (sn phm) : ..................5. Tng qut v cc bn v :- S bn v : Bn A0 Bn A1 Kh khc- S bn v v tay : S bn v trn my tnh :6. Nhng u im chnh ca LVTN :

....................................................................................................................................

....................................................................................................................................

....................................................................................................................................

....................................................................................................................................7. Nhng thiu st chnh ca LVTN :

....................................................................................................................................

....................................................................................................................................

....................................................................................................................................

....................................................................................................................................8. ngh : c bo v B sung thm bo v Khng c bo v 9. 3 cu hi SV phi tr li trc Hi ng :

1) ....................................................................................................................................................................................................................................................................2) ....................................................................................................................................................................................................................................................................

3) ....................................................................................................................................................................................................................................................................10. nh gi chung (bng ch : gii, kh, TB) : im ________/10

K tn (ghi r h tn)


4/91

Nhn xt ca gio vin hng dn.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

Tp. H Ch Minh, Ngy . Thng .nm 2012

GVHD


5/91

Nhn xt ca gio vin phn bin

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

.........................................................................................................................................

Tp. H Ch Minh, Ngy . Thng .nm 2012

GVPB


6/91

i

LI CM N

Xin gi li cm n su sc n thy hng dn TS. Nguyn Vnh Ho gi ti

cng nh hng dn tn tnh gip ti hon thnh lun vn. Cm n cc thy c trong B

mn T ng ni ring v Khoa in-in t ni chung to iu kin gip v ging

dy nhit tnh trong sut cc nm hc qua.

Xin gi li cm n n cc anh ch, cc bn v cc em trong Cu lc b Nghin Cu

Khoa Hc Khoa in-in t, i hc Bch Khoa TPHCM Pay It Forward ng hnh,

gip v ng vin ti trong sut qu trnh hc tp cng nh thc hin lun vn.

c bit xin cm n n gia nh lun h tr v ng vin gip ti c ng lc

phn u trong hc tp v cuc sng.

Tp HCM, Thng 12 nm 2012

ng Anh Tng


7/91

ii

TM TT

Hng nghin cu v thut ton cc b c lng gc quay ba trc hay xy dng h

thng o qun tnh ba trc IMU (Inertial Measurement Unit) xut hin t lu do nhu cu

cn thng tin v gc nghing hay gc quay ca vt th trong khng gian gii quyt cc bi

ton v iu khin cn bng, h thng nh v dn ng, h thng m phng c ng

ngi Trn th gii c rt nhiu sn phm thng mi vi chnh xc rt cao (


8/91

iii

MC LC

LI CM N ........................................................................................................................ ii

TM TT ......................................................................................................................... ii

MC LC ........................................................................................................................ iii

DANH SCH BNG BIU .................................................................................................. v

DANH SCH HNH V ...................................................................................................... vi

DANH SCH VIT TT ................................................................................................... viii

CHNG 1. TNG QUAN ................................................................................................ 1

1.1 Cc khi nim chung v b c lng gc quay ba trc ....................................... .... 1

1.2 Cc ng dng, tnh hnh nghin cu trong v ngoi nc .......... ............................... 1

1.3 Nhim v ca lun vn............ ....................... ...................... .................................... 3

1.4 Ni dung lun vn .......... ................................ ...................... ................................ .... 3

CHNG 2. THUT TON C LNG ..................................................................... 4

2.1 Tng quan v cc gii thut c lng ba gc nghing ........................................ .... 4

2.1.1 Ma trn xoay .................... ....................... ................................ .......................... 4

2.1.2 Ma trn xoay da trn ba gc Euler ............................... ....................... ............. 5

2.1.3 Ma trn xoay da trn Quaternion ........... ................................ .......................... 6

2.1.4 Cc phng trnh ng hc ..................... ...................... ....................... ........... 10

2.1.5 Cch tnh cc gc Euler t quaternion v ma trn DCM ................................ .. 11

2.2 B lc b ............................................................................................................... 11

2.2.1 Tng quan cc b lc b ........... ......................................... ............................. 11

2.2.2 Cc phng php c lng bnh phng ti thiu ....................................... .. 11

2.2.3 B lc b dng phng php Gauss-Newton............................................. ...... 13

2.2.4 B lc b dng phng php Gradient Descent ......................................... ...... 15

2.3 B lc Kalman .......... ......................................... .................................................. .. 16

2.3.1 Tng quan v b lc Kalman ...................... .................................................. .. 16


9/91

iv

2.3.2 B lc Kalman m rng dng DCM................................... ............................. 20

2.3.3 B lc Kalman m rng dng Quaternion ..................... ................................ .. 22

2.4 M phng v nh gi cc b c lng .............................. .................................. 24

2.4.1 Phng php m phng v nh gi.............................................................. .. 24

2.4.2 Kt qu v nhn xt ................................ ....................... ............................... .. 25

CHNG 3. XY DNG B C LNG ................................................................. 29

3.1 H thng IMU tch hp ARM Cortex-M4 STM32F40x v cm bin ADIS16405... 29

3.1.1 Vi iu khin ARM Cortex-M4F STM32F40x ................... ............................. 29

3.1.2 Cm bin ADIS16405 v phng php calib............................................. ...... 46

3.2 Xy dng m hnh MATLAB Simulink gii thut Kalman m rng ci tin dngDCM ..............................................................................................................................49

3.3 Lp trnh cho IMU kt hp MATLAB Simulink Embedded Coder v trnh bin dch

KeilC .............................................................................................................................. 53

3.4 Lp trnh ha m hnh 3D cho IMU dng ngn ng Python ................... ........... 55

3.4.1 Tng quan cu trc v c php ca mt chng trnh vit bng Python ........... 55

3.4.2 Cc th vin............ ................................ ...................... ................................ .. 57

3.4.3 Phn mm ha m hnh 3D cho IMU .......... ......................................... ...... 61

3.5 H thng bn xoay kim tra IMU .......... ............................... ................................ .. 62

CHNG 4. THC HIN NH GI & KT QU T C ............................. .. 64

4.1 Phng php nh gi ............. ................................ ......................................... ...... 64

4.2 Thc hin nh gi ...................... ...................... .................................................. .. 65

4.3 Kt qu .................................................................................................................. 66

CHNG 5. KT LUN .................................................................................................. 765.1 Kt lun ................................................................................................................. 76

5.2 Hng pht trin ca ti ................................ ......................................... ........... 76

TI LIU THAM KHO ................................................................................................. 77


10/91

v

DANH SCH BNG BIU

Bng 1.1 Bng so snh chnh xc cc IMU thng mi ..................... ............................... 2

Bng 2.1 Kt qu m phng i vi b lc b ............. ................................ ...................... 265

Bng 2.2 Kt qu m phng i vi b lc Kalman....................................................... ...... 26

Bng 3.1 Tm tt s chu k my khi thc hin lnh s hc dng FPU .............................. 4832

Bng 3.2 Bng h s t l cho cc gi tr cm bin ........... .................................................. .. 48

Bng 4.1 Kt qu th nghim 1 ............. ................................ ......................................... ...... 67

Bng 4.2 Kt qu th nghim 2 ............. ................................ ......................................... ...... 67


11/91

vi

DANH SCH HNH V

Hnh 1.1 H ta chun NED .............................................................................................. 1

Hnh 1.2 Ba gc nghing trong khng gian............................................................................. 1

Hnh 1.3 Mt s ng dng ca IMU ....................................................................................... 2

Hnh 1.4 Mt s IMU thng mi .......................................................................................... 2

Hnh 2.1 th xoay gc yaw ................................................................................................ 5

Hnh 2.2 Vector v im, Quarternion v vector..................................................................... 7

Hnh 2.3 B lc b QUEST .................................................................................................. 14

Hnh 2.4 B lc b Madgwick.............................................................................................. 16

Hnh 2.5 S h thng c lng ...................................................................................... 17

Hnh 2.7 Kt qu m phng Kalman cho h tuyn tnh .......... ......................................... ...... 19

Hnh 2.8. Gii thut b lc Kalman m rng ........................................................................ 20

Hnh 2.9 M hnh Kalman dng DCM ................................................................................. 20

Hnh 2.10. M hnh Kalman dng Quaternion ...................................................................... 22

Hnh 2.11 Biu ba gc v sai s ca mu EX-F2 dng Gauss-Newton ............................. 26

Hnh 2.12 Biu ba gc v sai s ca mu EX-F2 dng Gradient Descent ................... ...... 27

Hnh 2.13 Biu ba gc v sai s ca mu EX-F2 dng Kalman DCM ................... ........... 27

Hnh 2.14 Biu ba gc v sai s ca mu EX-F2 dng Kalman Quaternion ................... .. 27

Hnh 3.1 IMU-9DOF c ch to ti PTN B mn T ng HBK TPHCM.............. ...... 29

Hnh 3.2 S khi li ARM Cortex-M4 ............................................................................ 31

Hnh 3.3. Kin trc ma trn bus trong STM32F407 .............................................................. 34

Hnh 3.4 Chng trnh to file thit lp clock cho STM32F407............................................ 36

Hnh 3.5 Gin hot ng Timer c bn ............................................................................ 38

Hnh 3.6 Frame truyn ca USART ..................................................................................... 39

Hnh 3.7 Gin th t truyn SPI ...................................................................................... 42

Hnh 3.8 S khi ADIS16405 v h trc ca cc cm bin ............. ...................... ........... 46

Hnh 3.9 M hnh kt ni phn cng gia VK v ADIS16405 ......................................... .. 47Hnh 3.10 Gin xung trao i d liu trn SPI v ch c burst-mode ........................ 47

Hnh 3.11 Gin t trng trn cc trc trc khi calib ................... .................................. 49

Hnh 3.12 Gin t trng trn cc trc sau khi calib ....................... ............................... .. 49

Hnh 3.13 M hnh EKF-DCM ci tin ................................................................................ 50

Hnh 3.14 M hnh Simulink ca phng php EKF-DCM .................... ............................. 53


12/91

vii

Hnh 3.15 M hnh Compiler kt hp MATLAB Simulink v Keil C ........... ........................ 53

Hnh 3.16 Kin trc phn mm trong IMU ........................................................................... 54

Hnh 3.17 Lu gii thut x l trong IMU ................... .................................................. .. 54

Hnh 3.18 M hnh ca mt trnh bin dch (interpreted language) .......... ............................. 55

Hnh 3.19 H ta v vt th trong VPython ..................................................................... 58

Hnh 3.20 Hnh cu trong VPython ...................................................................................... 59

Hnh 3.21 Hnh hp trong VPython ...................................................................................... 59

Hnh 3.22 H ta NED v H ta trong VPython ....................... ............................... .. 60

Hnh 3.23 Kin trc phn mm hin th m hnh 3D ca IMU ........... ....................... ........... 61

Hnh 3.24 Lu gii thut phn mm ha cho IMU ..................... ...................... ........... 61

Hnh 3.25 Giao din chng trnh ha 3D dng Python ...................... ............................. 62

Hnh 3.26 Bn xoay kim tra IMU ....................................................................................... 63Hnh 3.27 Mch c xung encoder giao tip my tnh ti PTN B mn T ng, HBK

TPHCM ........................................................................................................................ 63

Hnh 4.1 File text lu d liu c lng ............................................................................... 66

Hnh 4.2 Biu ba gc quay v sai s ca mu TN1-XYZ1 (1) ........................................ .. 68




Hnh 4.6 Biu ba gc quay v sai s ca mu TN1-XYZ3 (1) ........................................ .. 70Hnh 4.7 Biu ba gc quay v sai s ca mu TN1-XYZ3 (2) ........................................ .. 70

Hnh 4.8 Biu ba gc quay v sai s ca mu khi c nhiu t trng (1) .................. ...... 71

Hnh 4.9 Biu ba gc quay v sai s ca mu khi c nhiu t trng (2) ................... ...... 71

Hnh 4.10 Biu ba gc quay v sai s ca mu TN2-S ................... .................................. 72

Hnh 4.11 Biu ba gc quay v sai s ca mu TN2-X ......................................... ........... 72

Hnh 4.12 Biu ba gc quay v sai s ca mu TN2-Y ......................................... ........... 72

Hnh 4.13 Biu ba gc quay v sai s ca mu TN2-Z ................... .................................. 73

Hnh 4.14 Biu ba gc quay v sai s ca mu TN2-XYZ ......................................... ...... 73Hnh 4.15 Biu ba gc quay v sai s ca mu TN2-AX ................................................. 74

Hnh 4.16 Biu ba gc quay v sai s ca mu TN2-AY ................................................. 74

Hnh 4.17 Biu ba gc quay v sai s ca mu TN2-AZ ..................... ............................. 74

Hnh 4.18 Biu ba gc quay v sai s ca mu TN2-AXYZ ........... ............................... .. 75

Hnh 4.19 Biu ba gc quay v sai s ca mu TN2-M (1) ............. ...................... ........... 75


13/91

viii

DANH SCH VIT TT

IMU : Inertial Measurement Unit - n v o qun tnh

INS : Inertial Navigation System H thng qun tnh dn ngDCM : Direction Cosine Matrix Ma trn cosine trc tip

KF : Kalman Filter B lc Kalman

EKF : Extended Kalman Filter B lc Kalman m rng

NED : North-East-Down H ta Bc-ng-Hng xung

ENU : East-North-Up H ta ng-Bc-Hng ln

MEMS: Microelectromechanical systems H thng vi c in t

VK : Vi iu khin


14/91

Xy dng b c lng gc quay ba trc

1

Chng 1. Tng quan1.1 Cc khi nim chung v b c lng gc quay ba trc

B c lng gc quay ba trc (hay cn gi l IMU-Inertial Measurement Unit-B o

lng qun tnh) l h thng c lng ba gc nghing Euler trong khng gian ca vt th.

Ba gc Euler c nh ngha l gc lch gia ba trc trn vt th so vi h trc chun, chiu

dng gc quay c xc nh theo qui tc bn tay phi. H trc chun c dng trong lun

vn l NED (Bc-ng-Hng Xung) gn lin mt t, h trc gn vi vt th l h trc

Descartes vi ba trc x, y, z dc theo h trc ca cc cm bin trn IMU. K hiu ba gc quay

ln lt l:

Roll: gc quay theo trc x, k hiu ( ) Pitch: gc quay theo trc y, k hiu ( )

Yaw: gc quay theo trc z, k hiu ( < < )

Hnh 1.1 H ta chun NED Hnh 1.2 Ba gc nghing trong khng gianMt h thng IMU thng thng s gm ba loi cm bin Gia tc (Accelerometer), Vn

tc gc quay (Gyroscope) v T trng (Magnetometer). Trn IMU c th c thm cm bin

nhit b nhit cho cc cm bin hoc cm bin p sut o cao Nh s pht trin

ca cng ngh MEMS cc cm bin ny u c ch to ngy cng nh gn v sai s nh, v

vy cng vic xy dng b c lng gc quay ba trc ch yu l xy dng thut ton kt

hp ba loi cm bin ny v c lng ra ba gc quay vi sai s nh nht.

1.2

Cc ng dng, tnh hnh nghin cu trong v ngoi ncCc sn phm IMU c lng ba gc quay c s dng rng ri trong cc lnh vc

nh: robot t cn bng, iu khin my bay, iu khin quadrotor, m phng chuyn ng

ngi, tng tc ngi-my, h thng dn ng INS kt hp IMU v GPS


15/91


2

Hnh 1.3 Mt s ng dng ca IMU

Trn th gii lnh vc nghin cu ny c t lu v c nghin cu rng ri cng

nh cho ra i cc sn phm thng mi t c chnh xc rt cao. V d mt s sn

phm thng mi:

Hnh 1.4 Mt s IMU thng mi

Bng 1.1 Bng so snh chnh xc cc IMU thng mi

Sn phm chnh xc ng dngHG9900-Honeywell - Gyroscope bias:


16/91


3

IMU hin nay tp trung pht trin v hon thin hai loi thut ton cho cc b IMU l b lc

b (Complementary Filter) [5], [6] v lc Kalman (Kalman Filter) [7], [8], [9], [10] hay

kt hp c hai nh [11], [12]. Trong b lc b tuy c u th hn v tnh n gin v khi

lng tnh ton t nhng vn khng hiu qu bng b lc Kalman. Vi s pht trin ca cc

dng vi iu khin h tr tnh ton s thc th khi lng tnh ton khng phi l vn ln

vi b IMU. S chnh xc ca cc IMU phn ln c quyt nh cht lng ca thut ton

c pht trin, do mc tiu chnh ca lun vn ny l xy dng v nh gi cc thut

ton hin nay chn ra thut ton vi sai s nh nht, t a ra cc ci tin hon thin

b lc.

1.3 Nhim v ca lun vnNhim v ca lun vn:

1. Tm hiu v nh gi cc gii thut khc nhau xy dng b c lng ba gc quay.

2. Hin thc gii thut trn h thng nhng dng vi iu khin vi tn s cp nht gi tr

ba gc quay l 100Hz.

3. nh gi sai s ca h thng IMU v ci tin sai s RMS ba gc nh hn 3

4. Xy dng phn mm ha 3D hin th m hnh IMU trn my tnh.

1.4 Ni dung lun vnLun vn gm nm phn:

Chng 1. Tng quan

Chng 2. Thut ton c lng

Chng 3. Xy dng b c lng.

Chng 4. Thc hin nh gi & Kt qu t c

Chng 5. Kt lun


17/91


4

Chng 2. Thut ton c lng2.1 Tng quan v cc gii thut c lng ba gc nghing2.1.1 Ma trn xoay

Ma trn xoay chuyn gi tr vector t h ta gn lin vi vt (B-body) sang hta NED (E-Earth)

= ( ) = Gi s

, l 2 vector trong ta (B) & (E) th ta c quan h:

= . Hoc: = . = . Ma trn xoay l ma trn 3x3 vi cc tnh cht sau:

Tnh cht 1: Ma trn xoay phi c chun ha (normalize) tc l tng bnh phngcc gi tr trn hng v ct bng 1. = = = 1

Tnh cht 2: Ma trn xoay phi l ma trn trc giao (orthogonal) tc l tch ca bt cmt cp vector hng hay mt cp vector ct u bng 0.. = 0 . = 0. = 0

Tnh cht 3: Tch ma trn xoay v chuyn v ca n l ma trn n v.

. = hay =


18/91


5

2.1.2 Ma trn xoay da trn ba gc EulerTa s tnh ma trn xoay da trn 3 gc euler.

Hnh 2.1 th xoay gc yawGi s ban u gi vt sao cho h trc 0xyz gn lin vi vt (B) trng vi h trc 0xyz (E

hay N-E-D) nh trn. Xt vector trong h trc (B) = (, , )th hnh chiu ca ntrong h trc (E) l vector = (, , )Gi nguyn trc z quay h trc (B) mt gc trn mt Oxy.Trong h trc (B) vector vn c gi tr l = (, , ), tuy nhin lc ny hnh chiu can xung (E) l

= (, , ). Vi:

= . . = . + . = Sp xp li h phng trnh trn ta c:

= 0 0

0 0 1

. Hay:

= 0 00 0 1 t , l ma trn xoay quanh trc z t h ta B n h ta E th:

-1 1

-1

1

x

y

(cos, sin)

(-sin , cos)


19/91


6

, = 0 00 0 1Tng t:

, = 1 0 00 0 , = 0 0 1 0 0

Vy nu xoay h trc (B) theo 3 gc ,, so vi h trc (E) th mt vector trong h trc(B) s c hnh chiu

trn h trc (E) nh sau:

= ,. , . , . = . y ch chiu dng gc xoay theo quy tc bn tay phi v 3 gc Euler c gii hn ln

lt l:

, , < < Khi ma trn xoay tnh c nh sau:

= . .. . . +... ..+ . .. . . . 2.1.3 Ma trn xoay da trn Quaterniona.i s Quaternion

Ton t hnh hc quaternion v i s quaternion c gii thiu bi nh ton hc

William Hamilton. Khi nim quaternion dng biu din mi quan h gia hai vector c

sng to t khi nim vector biu din mi quan h gia hai im .Trong i s vector ta c t mt im ton t vector s to ra duy nht mt im vichiu di l ln ca vector v hng ca l hng ca vector. Tng t tvector i s quaternion s to ra duy nht vector.


20/91


7

Hnh 2.2 Vector v im, Quarternion v vector

Ton t quaternion gm hai phn c hng (hay phn o) biu din dng vector trongkhng gian v phn v hng (hay phn thc) c biu din nh sau:

[, ] = + = + + + ( , )Vi: = = = = 1 = = = = = = Cc vector = + + trong khng gian ba chiu s c biu din dng quaternionnh sau: = + + + 0. Cn cc gi tr thc c biu din dng quaternion nhsau

= 0 + 0 + 0 + .

Mt s php ton trong i s quaternion nh sau:

t , l hai quaternion nh sau: = + + + = + + + i. Php cng:

+ = [,

] + [,

] = [ +,

+

]

+ = ( + ) + ( + ) + ( + ) + ( + )ii. Php nhn:

= [, ][, ] = [+ .+ ., . . ]


21/91


8

= ( + + + )( + + + )= ( + ) + ( + + ) + ( + + ) + ( + + ) = = (). hoc: =

= (). iii. Lng lin hp:

= [, ] [, ]

= +

iv. Dng chun ha:

() = . () = + + + v. Quaternion nghch o:( = 1) = () = ( + )Trong ch tch c hng

v v hng

. ca hai vector c cng thc sau:

= ( ) + ( ) + ( ). = + + b. p dng quaternion vo cc php quay


22/91


9

y khi quay vector xung quanh vector mt gc th ta c vector c biu dindng i s quaternion nh sau:

=

Vi: = ,0, = [,0] Khi : = cos( ) , = sin . V l quaternion lin hp ca : = + T ta c c cch quay vector da vo quaternion vi ch quaternion quay phi l

quaternion n v tc l: () = 1c.Ma trn xoay

+ Tnh ma trn xoay t sang theo hai hng (v quaternion l n v nn = + ) = = () = (). = (). ().

=

. =

2. + 2 1 2( ) 2(+) 02( + ) 2 + 2 1 2( ) 02( ) 2( + ) 2 + 2 1 00 0 0 1 .

= = 2. + 2 1 2( + ) 2() 02( ) 2

+ 2

1 2( + ) 02( + ) 2( ) 2 + 2 1 00 0 0 1 .

Ma trn xoay da trn Quaternion (vi phn thc) = 2. + 2 1 2( + ) 2( )2( ) 2 + 2 1 2( + )2( + ) 2( ) 2 + 2 1


23/91


10

2.1.4 Cc phng trnh ng hca.Phng trnh ng hc biu din bi ba gc Euler

= 1 0 0 Trong :

= 1v , , l vn tc gc cc trc x, y, zb.Phng trnh ng hc biu din dng DCM

= . [ ]Vi: [ ] = 0 0 0 = =

. .. . . +... .. +. .. . . . c.Phng trnh ng hc biu din dng Quaternion

= 12 . Khi l vector trong khng gian ba chiu ta c cng thc rt gn nh sau:

=

+

+

+

= + + + 0

= 12 = 12 (). = 12 0

= 12


24/91


11

2.1.5 Cch tnh cc gc Euler t quaternion v ma trn DCMT cc ma trn xoay trn ta tnh c cc gc Euler da vo cc h s ca ma trn DCM hoc

da vo quaternion nh sau:

Gc Euler Tnh t DCM Tnh t quaternion 2(, ) 2(2 + 2, 2 + 2 1) asin() asin(2 + 2) 2(, ) 2(2 + 2, 2 + 2 1)2.2 B lc b2.2.1 Tng quan cc b lc b

Cc b lc b u hot ng da trn nguyn l: gi tr c lng s l gi tr ph hp

nht sao cho ti thiu ha tng cc bnh phng cc sai khc t cc ngun d liu khc nhau

(phng php Least-mean-square c lng bnh phng ti thiu).

i vi IMU trong cc thut ton y c ba ngun d liu c lng gc l: Gia tc ba

trc, Vn tc gc quay ba trc v T trng ba trc. thc hin iu ny ta s dng cc

thut ton tm gi tr nh nht ca hm mc tiu l:

() = () .()

Bi ton ny cn gi l nonlinear least-square, gii th c nhiu thut ton, y trnh

by hai thut ton c dng rng ri l: Gauss-Newton v Gradient Descent.

u im ca cc b lc b l thut ton n gin, khi lng tnh ton t rt d nhng xung

cc dng vi iu khin. Tuy nhin v qu n gin nn vic ti u v ci tin gii thut rt

kh. Cc vn cn gii quyt khi gii bi ton ny l:

Tnh hi t ca gii thut iu kin m bo gi tr nh nht

2.2.2 Cc phng php c lng bnh phng ti thiua. Phng php Newton

u tin ta xem xt phng php Newton tm gi tr nh nht ca hm () tng qutsau ng dng tm gi tr nh nht ca cc hm bnh phng (nonlinear least-squares)

Ta bt u t khai trin Taylor quen thuc:


25/91


12

() = () + ( ). + 12 ( ) . (). ( ) ()Trong = (), () = ( ) v l gi tr bin ti thi im (hay bc th)

. Thc hin php o hm (hay gradient) ta c:

0 = () = + (). ( )Nu () > 0 (cn gi l Hessian) (o hm bc mt bng 0, o hm bc hai ln hnkhng tc l tm gi tr min) ta c biu thc qui nh sau: = (()). T ta c th tm gi tr nh nht ca hm() bng php lp qua 2 bc: Bc 1: tnh t phng trnh (). = Bc 2: tnh = + i vi bi ton nonlinear least-squares: cho gi tr hm = [ , , ] ca bin = [, , ] vi , phng php Newton s tm gi tr nh nht ca tng tt c cc

bnh phng sau:

( ) dng phng php Newton ta s xt hm() = () .() (cn gi l hm mc tiu).Bt u tnh gradient v Hessian ca:

() = () = 2 ().

() Ch ma trn Jacobian bin ca hm :() = =

Do : () = 2() .()Tip tc tnh ma trn Hessian ca

. Thnh phn th

(,)l:

() = () = 2 (). () + (). () t:

() = (). ()


26/91


13

() = 2((). () + ())Nh vy phng trnh qui cho trng hp ny l:

=

()

.

() + ()

.

()

.()

Trong trng hp thnh phn o hm bc hai () qu nh c th b qua khi gii thutny c gi lphng php Gauss-Newton vi phng trnh qui nh sau: = (). (). ().()Gii thut ny hi t khi lp s ln cn thit v o hm bc hai (Hessian ()) phi lunln hn khng m bo v gi tr nh nht, ngoi ra nu gi tr ban u cng gn vi gi tr

nh nht th hi t cng nhanh. xem xt tt c cc iu kin ny s rt phc tp v khng

ph hp nhng xung vi iu khin nn ta s kim chng tnh hi t da trn thc t m

phng chn s ln lp cho hp l.

b. Phng php Gradient

Bi ton t ra l tm gi tr nh nht ca hm s () bt k. Qu trnh tm gi tr nys cho gi tr ban u chy theo hng () qua phng trnh quy sau: = . ()Trong > 0 gi l ln bc nhy (step size). Gii thut dng li khi hay Vi trng hp nonlinear least-squares:

() = 2().()th phng trnh tr

thnh: = . 2 . () .()Cng tng t nh Gauss-Newton ln bc nhy v gi tr ban u s nh hng nthi gian v tnh hi t ca gii thut do ta s xem xt trong phn m phng. c bit

phng php ny ch cn tnh gradient (khng c nhn ma trn v nghch o nh phng

php Newton) nn rt n gin nhng vo vi iu khin.

2.2.3 B lc b dng phng php Gauss-Newton


27/91


14

Hnh 2.3 B lc b QUEST

gii thut ny ta s dng phng php Gauss-Newton c lng gi tr ca

quaternion t hai ngun l {Gyrometer} v {Accelerometer, Magnetometer}.Ta c phng trnh ng hc ca vn tc gc quay: = . Vi gi tr ny ta c th tnh c quaternion bng tch phn n gin: = +. (vi T l thi gian gia cc ln chy gii thut). Tuy nhin c tnh vt l ca Gyrometer l

khng chnh xc tn s thp (ch chnh xc tn s cao) do ta phi kt hp thm ngun

thng tin t {Accelerometer, Magnetometer} rt ra gi tr hp l (Accelerometer chnh xc tn s thp). Do gii thut Gauss-Newton c dng y ti thiu ha hm mc tiu() = ().()Trong : () =

cha su gi tr o t cm bin Accelerometer, Magnetometer (h gn vi IMU B) cha su gi tr c nh ca gia tc v t trng (h gn lin mt t E). Trong

thnh phn gia tc lun lun l

[0,0,1]cn thnh phn t trng thay i theo v tr

a l.

l ma trn xoay t h E sang B ()T () nh trn tnh ma trn Jacobian:


28/91


15

() = , , ,

= .

, .

, .

, .

Khi ta c phng trnh qui: = (). () . ().()Lc ny: = + . (vi l h s li)Cui cng ta s tnh c: = + . Php chun ha cui

=

. m bo tnh n v ca quaternion

2.2.4 B lc b dng phng php Gradient DescentTng t nh gii thut QUEST trn, gii thut ny ta s dng phng php

Gradient Descent c lng. t hm mc tiu nh sau:() = () .()Trong : () =

() = 2. ()

.()

Phng trnh quy: = .()Cc bc tnh tng t m hnh QUEST trn


29/91


16

Hnh 2.4 B lc b Madgwick

2.3 B lc Kalman2.3.1 Tng quan v b lc Kalman

Cc b lc Kalman da vo gii thut c lng Kalman bao gm hai phn: u tin

l d bo gi tr (prediction) da vo d liu trc , sau l hiu chnh li (correction)

nh vo cc gi tr thc t va o. B lc ny c pht trin ban u t nhu cu ca vic

theo di qu o v dn ng cho cc h thng trong khng gian ca NASA. Sau ny n

c p dng rng ri vo cc h thng khc nh x l nh, iu khin

Cc vn ca vic hin thc mt b lc Kalman gm:

Ch ra m hnh ton ca h thng. c lng c cc covariance ca nhiu: do m hnh & o lng.

Xt h thng tuyn tnh tng qut c biu din bi h phng trnh trng thi sau:

= . + . + = . + . + Trong : , , , : cc ma trn hng (phn bit vi h phi tuyn l cc hm s) : gi tr trng thi ca h thng ti thi im : ng ra ca h thng ti thi im


30/91


17

: nhiu do m hnh hay nhiu qu trnh (process noise) : nhiu do o lng (measurement noise)

Hnh 2.5 S h thng c lng

tm ng ra ca h thng i vi cc ng vo khc nhau v cc trng thi khc nhauca h thng ta phi thit lp c h phng trnh trn. Tuy nhin vic tnh ton c th dn

ra kt qu sai vi thc t do:

1. M hnh ton sai v tuyn tnh ha b qua cc thnh phn bc cao, b qua cc ilng nh nhit , ma st

2. Nhiu do o c t cm bin hay phng php oDo yu cu t ra l cn c cc cc b lc hay gii thut c lng ti u ha gi tr

tnh ton. Cc bc tnh ton trong gii thut Kalman nh sau:

Hnh 2.6. Gii thut KalmanCc k hiu dng trong ny l:

, : cc h s ca phng trnh vi phn tuyn tnh. : ma trn covariance ca nhiu c lng c ( & l gi tr trc & sau). : ma trn covariance ca nhiu do qu trnh (hip phng sai). : ma trn covariance ca nhiu do o lng.


31/91


18

: ma trn li. : ma trn nhy xc nh quan h tuyn tnh gia tn hiu ca h thng v tn hiu

c lng.

xem xt gii thut c lng Kalman ta s tm hiu v d sau:Mt chic xe c t vo gia tc = 1/ khng i, tnh ton v tr ca n sau mi = 0.1. Vi nhiu ca php o v tr l 10m ( lch chun), nhiu ca gia tc l 0.2/( lch chun)

Gii:

Ta c cc phng trnh ng hc n gin sau: (b qua cc yu t khc)

Vn tc xe: = + V tr xe:

= + +

M hnh ha h thng nh sau:

Tn hiu ng vo (iu khin): = Ng ra: = Cc bin trng thi: = [, ] Nhiu qu trnh: = ~ Nhiu o lng: = ~

Phng trnh h thng nh sau:

= 1 0 1 + 2 + = [1 0] + Nhn xt:

Covariance nhiu qu trnh: = (. ) = [ ] = = () ()() ()

=

.

/4

/2/2 = 10

2.10

2.10 4.10

Covariance nhiu o lng = (. ) = ([]. []) = () = 10 = 100T cc ma trn trn ta m phng c kt qu nh sau:


32/91


19

Hnh 2.7 Kt qu m phng Kalman cho h tuyn tnh

y ng lin nt l gi tr o raw cha c x l, ng chm v t l gi tr c

lng estimate. Ta thy sau khi qua lc Kalman th tn hiu tt hn rt nhiu so vi gi tr

raw.Mt thc t l rt nhiu cc h thng thc u l phi tuyn do khi p dng Kalman

cho cc i tng ny ta phi dng b lcKalman m rng (Extended-Kalman Filter)

Ta bt u vi m hnh tng qut ca h thng phi tuyn:

= (, , ) = ( , , ) t cc ma trn Jacobian:

=

(, , 0) = + .

= (, , 0)Cc bc tnh ton nh sau:


33/91


20

Hnh 2.8. Gii thut b lc Kalman m rng

2.3.2 B lc Kalman m rng dng DCM

Hnh 2.9 M hnh Kalman dng DCM

Xy dng KF1:

Phng trnh ng hc da vo DCM:

=

= . 0 0 0 Rt ring hng th 3 ca ma trn DCM ta c phng trnh:

= 0 0 0


34/91


21

Phng trnh o lng cho Accelerometer:

= . 00

=

. T ta xy dng c m hnh ton cho khi lc Kalman 1 nh sau:

= , , , , , = , , , , , = . + = 0 0 0 = . + = . 00

Gi s nhiu t Gyrometer 3 trc khng tng quan nhau ta s c biu thc covariance ca

nhiu qu trnh: = (. ) = 0 00 . V biu thc covariance ca nhiu o lng:

= (. ) = 00 . Ri rc ha cc covariance:

= . 0

= . . . . .Trong , l variance ca Accelerometer v Gyrometer da vo iu kin + + 1 > Xy dng m hnh cho khi Kalman 2:

= , , , , ,

=

,

,

,

,

,

= . + = 0 0 0 , = 0 0 0 = . + = [] = 0 00 . = . 00 .


35/91


22

2.3.3 B lc Kalman m rng dng Quaternion

Hnh 2.10. M hnh Kalman dng Quaternion

u tin ta c h phng trnh ng hc ca h thng nh sau:

=

1 +

= 1

+

= 1 + = 12 . . + . = 12 . + . . = 12 . + . + . = 1

2 . . .

t = [, , ] = , , , , , , (Vi : phn thc ca quaternion)Tnh ma trn Jacobian:

= =

1 0 0 0 0 0 00 1 0 0 0 0 00 0 1 0 0 0 00.5 0.5 0.5 0 0.5 0.5 0.5

0.5 0.5 0.5 0.5 0 0.5 0.50.5 0.5 0.5 0.5 0.5 0 0.50.5 0.5 0.5 0.5 0.5 0.5 0


36/91


23

= + .

= 1 0 0 0 0 0 0

0 1 0 0 0 0 00 0 1 0 0 0 00.5 0.5 0.5 1 0.5 0.5 0.50.5 0.5 0.5 0.5 1 0.5 0.50.5 0.5 0.5 0.5 0.5 1 0.50.5 0.5 0.5 0.5 0.5 0.5 1

Thm na t = , , , , , , , , th: = () = ,, . .

Vi l ma trn xoay: = 2. + 2 1 2( + ) 2( )2( ) 2 + 2 1 2( + )2( + ) 2( ) 2 + 2 1

= =

. .

= 4 2 22 0 22 2 0 = 0 2 22 4 22 2 0 =

0 2 22

0 2

2 2 4

= 4 2 22 4 22 2 4


37/91


24

= 0 0 0 00

0

Cc ma trn covariance:

= 00 0 vi = 0 00 00 0 = ([, , , , , , , , ]) Vi (): ma trn ng cho

2.4 M phng v nh gi cc b c lng2.4.1 Phng php m phng v nh gi

Ta s dng mu d liu thu c t IMU thng mi c chnh xc rt cao ca hng

Xsens bao gm cc gi tr:

Ba gc c lng Ba gi tr vn tc gc ba trc Ba gi tr gia tc ba trc Ba gi tr t trng ba trc

Ta s dng MATLAB Simulink m phng ln lt cc mu th bng bn phng php lc

lit k trn:

B lc b bng Gauss-Newton B lc b bng Gradient Descent B lc Kalman m rng bng DCM B lc Kalman m rng bng quaternion

D liu th l cc gi tr thu c t IMU () s c dng c lng ra ba gcnghing () sau so snh vi gi tr chnh xc t IMU ca ().Vi mi b lc ta s th 5 mu d liu vi cc kiu quay khc nhau c k hiu:

Xoay cm bin t do: EX-F2

Xoay cm bin t do: EX-F1 t cm bin ng im: EX-S Di chuyn cm bin dc trc x: EX-XMOVE Xoay cm bin quanh trc z: EX-Z


38/91


25

nh gi sai s ca php c lng, ta dng gi tr RMS ca sai s gia gi tr c lng

v gi tr chnh xc:

= 1

Vi: = Ch : y i vi cc sai s do chuyn t cc gc 180 ta s cho = 02.4.2 Kt qu v nhn xtCc k hiu:

G-N: B lc b bng Gauss-Newton GD: B lc b bng Gradient Descent E-KF: B lc Kalman m rng bng quaternion DCM: B lc Kalman m rng bng DCM

Bng 2.1 Kt qu m phng i vi b lc b

STT Mu th B lc bSai s RMS ()

1 EX-F2 G-N 0.8784 0.3733 0.9637GD ( = 0.1) 2.0067 0.5009 3.16642 EX-F1

G-N 3.7005 2.489 4.945GD ( = 0.4) 9.1692 7.0878 21.935

3 EX-SG-N 0.0457 0.0533 0.1271GD ( = 0.07) 0.0564 0.0664 0.0703

4 EX-XMOVEG-N 0.1724 0.7104 1.1006GD ( = 0.1) 0.597 0.5292 1.2589

5 EX-ZG-N 0.4185 0.2655 10.7354GD ( = 0.5) 0.9263 0.7639 5.0267


39/91


26

Bng 2.2 Kt qu m phng i vi b lc Kalman

STT Mu th B lc KalmanSai s RMS ()

1

EX-F2 E-KF 1.2193 0.419 4.3721

DCM 1.4836 0.7122 1.5629

2EX-F2 E-KF (***) 2.0442/9.0854 1.2628/11.2862 45.2105/8.0918

DCM 2.0167 1.0612 2.3396

3EX-S E-KF 0.1924 0.6838 1.062

DCM 0.1086 0.1115 0.3944

4EX-XMOVE E-KF 2.3144 2.0432 3.7116

DCM 0.3357 0.5617 1.9229

5EX-Z E-KF 0.8362 0.5016 31.1851

DCM 0.1762 0.1239 3.5559

Ch : trng hp (***) cho thy s nh i khi la chn ti u gc ,, trong hai trnghp.Nhn xt:

Cc phng php dng b lc b tuy n gin nhng cc h s khng thch nghic cc trng hp chuyn ng khc nhau ca IMU.

Cc b lc dng Kalman cho kt qu kh tt khi thay i cc iu kin khc nhau vb lc dng DCM th tt hn Kalman m rng v tch c nh hng ca gi tr

t trng ra khi c lng

v

.

Trong iu kin khng c gia tc ngoi th Gyrometer & Accelerometer c lnggc , kh tt cn vic c lng gc khng c tt.

Hnh 2.11 Biu ba gc v sai s ca mu EX-F2 dng Gauss-Newton


40/91


27

Hnh 2.12 Biu ba gc v sai s ca mu EX-F2 dng Gradient Descent

Hnh 2.13 Biu ba gc v sai s ca mu EX-F2 dng Kalman DCM

Hnh 2.14 Biu ba gc v sai s ca mu EX-F2 dng Kalman Quaternion


41/91


28

C ba vn (c sp xp theo mc phc tp dn) cn phi gii quyt i vi mt thut

ton c lng cho IMU l:

1. p ng tnh tt iu kin thng thng (gia tc ngoi nh, t nhiu t trng)

2. p ng ng tt ngay c khi c gia tc ngoi

3. p ng tt khi c nhiu t trng can thip

Hai phng php b v Kalman trn c bn l ging nhau khi gii quyt vn [1]

tuy nhin m hnh Kalman m rng s c u im hn khi gii quyt vn [2] v [3] v

Kalman cho php chnh nh cc covariance ca tng thnh phn gi tr dng trong m hnh.

c bit m hnh DCM cho php tch nh hng ca t trng ln hai gc roll v pitch nn

phn tip theo s tp trung pht trin v ci tin gii thutKalman m rng dng DCM cho

IMU.


42/91


29

Chng 3. Xy dng b c lng3.1 H thng IMU tch hp ARM Cortex-M4 STM32F40x v cm bin

ADIS16405

H thng IMU 9-DOF bao gm vi iu khin ARM-Cortex M4 STM32F405 h tr tnh

ton du chm ng v cm bin 9-DOF ADIS16405 (gm ba loi cm bin Gia tc ba trc,

Vn tc gc ba trc, T trng ba trc) c ch to ti phng th nghim iu Khin T

ng - B mn T ng, i hc Bch Khoa TPHCM. Sau y l hnh nh v m hnh ca

h thng ny:

Hnh 3.1 IMU-9DOF c ch to ti PTN B mn T ng HBK TPHCM

Trong lun vn ny ta s s dng phn cng IMU trn kim tra gii thut c

lng ba gc quay. Sau y ta s tm hiu v dng vi iu khin ARM Cortex-M4 v cm

bin ADIS16405 cng nh cch calib loi cm bin ny.

3.1.1 Vi iu khin ARM Cortex-M4F STM32F40x


43/91


30

a. Gii thiu vi x l ARM Cortex-M4F

Cc vi iu khin s dng li ARM ngy cng ph bin trn cc sn phm nh: di ng,

cc thit b in t cm tay, cc thit b in t, ng dng trong xe hiCc c im ca vi

iu khin dng li ARM l gi thnh r, x l tc cao, kh nng tit kim nng lng v

mt li th rt ln l c rt nhiu dng vi iu khin cung cp a dng cc ngoi vi bi cc

nh sn xut chip hng u nh TI (vi dng LM3Sx), ST (vi dng STM32x) v NXP (vi

dng LPCx). Cng vi mt cng ng dng ARM rng ln v s h tr th vin lp trnh

cng nh thit k mch mu t cc nh sn xut ln ny vic ng dng cc dng vi iu khin

ny vo sn phm tr nn d dng v nhanh chng. Do cc vi iu khin li ARM l la

chn ca nhiu nh sn xut in t hin nay.

Trong cc dng ARM, dng Cortex-M (M0, M1, M3, M4) c sn xut nhm n ng

dng nhng yu cu tc va phi, nhiu ngun ngt v p ng ngt nhanh. Cc ngoi vi

i km do cc nh sn xut vi iu khin thm vo cng rt a dng v d: LPC1769 ca NXP

s dng li ARM Cortex-M3 c cc ngoi vi nh: UART, Ethernet, USB, CAN l cc chun

truyn thng dng nhiu trong cc sn phm in t. S dng cc vi iu khin ny tip

cn vi cc ng dng nhng hay iu khin t ng trong phm vi cc n v lun vn l

la chn ph bin v ph hp vi xu hng hin nay.

Vi x l Cortex- M4F l bn nng cp ca Cortex-M4 vi kh nng tnh ton s thc

FPU (Floating Point Unit) v vn gi nguyn cc kh nng khc nn ta gi gn l vi x lCortex-M4 cp ti vi x l ny. Cc u im ca vi x l Cortex-M4:

C ch x l ngt lng nhau NVIC (Nested Vector Interrupt Controller) cho phpx l nhiu ngt xy ra ng thi vi thi gian tr thp (low latency).

B x l s thc FPU (Floating Point Unit). y hai c im quan trng cn tm hiu vic xy dng phn mm trn cc vi iu

khin dng vi x l ny linh hot v an ton hn l: c ch x l ngt NVIC v b x l s thc

FPU.

B iu khin ngt lng nhau NVIC: cho php ci t cc kh nng X l nhiu ngt (240 ngt) vi nhiu mc u tin (256 mc) Thi gian tr thp (low-latency) Tit kim nng lng

Li ARM cung cp 2 loi ngt:


44/91


31

Ngt mc (level interrupt): s c gi n khi ngt b xa. Nu khng b xa thngt ny s c kch hot li ngay khi thot ra khi chng trnh phc v ngt

ca n. Vic ny m bo khng c thi gian tr mt khi mun vi x l lun trong

ngt khi c ngt mc xy ra.

Ngt xung (pulse interrupt): nu c thm cc xung na cng n trong trng thix l th ngt cng ch x l mt ln.

Hnh 3.2 S khi li ARM Cortex-M4

B x l s thc FPU:FPU trong Cortex M4 da trn tiu chun IEEE 754 (IEEE

Standard 754 for Binary Floating-Point Arithmetic) qui nh cch x l s thc di dng

nh phn. FPU cung cp ba ch ci t:

Full-compliance mode: Hon ton theo chun IEEE-754 v phn cng


45/91


32

Flush-to-zero mode: cc s subnormal trong cc php ton s qui v 0 trc khitnh hoc cc kt qu php ton cng c qui v 0

Delault NaN mode: cc s hng l NaN s to ra cc kt qu cng l NaN. Cckiu php tnh gy ra NaN l: cn bc hai ca s m, nhn 0 v v cng, chia cho

0, chia cho v cng, phn d ca php chia 0, phn d ca php chia v cng cho

bt c s no, cng tr hai s v cng.

FPU h tr y cc php ton vi chnh xc n (single-precision) l: cng, tr,

nhn, chia, nhn cng dn, cn bc hai. Tuy nhin khng h tr cc php tnh sau: tnh ly d,

lm trn s thc sang s nguyn gi nguyn dng du chm ng (floating point number to

integer-valued floating-point number), chuyn nh phn sang thp phn, thp phn sang nh

phn, so snh trc tip gia hai s dng chnh xc n (single-precision) v chnh xc kp

(double-precicion). Cc php tnh ny phi c h tr bng cc hm th vin.

Bng thng k s chu k my tn thc hin cc php ton ph bin vi s dng du

chm ng trn FPU:

Bng 3.1 Tm tt s chu k my khi thc hin lnh s hc dng FPU

Ton hng S chu k M t

Tr tuyt i 1

Cng 1Tr 1

Nhn 1

Nhn xong cng/tr dn 3

Nhn xong cng/tr dn ri o du 3

Chia 14

Cn bc hai 14

So snh 1 Vi 0

Chuyn i 1 Gia 2 s nguyn, 2 s thc.Gi N s (Load) Double 64 bit: 1+2*N

Float 32 bit: 1+N

Double hoc float

Chuyn thanh ghi (Move) Hu ht l 1 chu k

b. Gii thiu vi iu khin STM32F40x


46/91


33

Vi iu khin STM32F40x l dng vi iu khin 32 bit ca STMicroelectronics s dng

li vi x l ARM Cortex-M4F. Cc ngoi vi h tr bi STM32F40x gm:

B nh: 1 Mbyte b nh flash, 192 Kbyte SRAM + 4 Kbyte SRAM d tr dng choRTC

in p, ngun clock: 1.8 V 3.6V, 4 Mhz 26Mhz thch anh ngoi Ch standby tit kim nng lng GPIO: 82 chn (i vi STM32F407) ADC: 12 bit, 10 bit, 8 bit hoc 6 bit DAC: 12 bit 16 b DMA, 8 knh trn mi DMA Timer: 16 bit, 32 bit H tr cc chun debug:

JTAG, SWD (Serial Wire Debug) Cortex-M4F Embedded Trace Macrocell

H tr cc giao thc truyn thng : Giao tip ni tip: I2C, USART, SPI, I2S, CAN (2.0B), SDIO, USB 2.0,

Ethernet

Giao tip song song FSMC: Flash, SRAM, PC Card 8 n 14 bit giao tip camera n 54Mbyte/s B to gi tr ngu nhin (analog random number generator) RTC

Kin trc b nh v bus: h thng bus gm c ma trn bus giao tip gm nhiu lp AHB

(Advanced High-Performance Bus) 32 bit ni vi nhau v cc bus APB (Advanced Peripheral

Bus).


47/91


34

Hnh 3.3. Kin trc ma trn bus trong STM32F407

u tin ta thy 3 ng bus S0, S1, S2 t li Cortex-M4 ng vi:

I-bus : ng ni truy cp (fetch) cc dng lnh (Instruction) t b nh Flash, SRAMhoc b nh ngoi thng qua FSMC (flexible static memory controller)

D-bus: kt ni bus d liu (Data) ca li v 64Kbyte d liu ca CCM RAM(Coupled Core Memory) vi cc b nh cha lnh hoc d liu (Flash, SRAM,

FSMC)

S-bus: kt ni bus h thng (System) ca li n cc b nh cha lnh hoc d liu(SRAM, AHB ca ngoi vi)

Tip theo l cc bus S3, S4, S5 dng cho hai b DMA1 v DMA2. Mi DMA gm c 1

ng b nh ni trc tip vo ma trn bus v 1 ng ni trc tip ra ngoi vi khng thng

qua ma trn bus. Nh c c im ny m cc DMA c th dng trao i trc tip gi tr

cc thanh ghi ca ngoi vi vo SRAM thc thi thay v thng qua li chuyn sang SRAM.

Do DMA dng cho cc ng dng cn tc trao i nhanh tit kim thi gian tnh

ton.

Cui cng l 2 bus S6, S7 l DMA dng cho Ethernet v USB. Ma trn bus s lm nhim

v qun l cc truy cp t cc master S0, S1S7. Gii thut qun l ng truyn l round-

robin tc l x l quay vng trn nguyn tc khng u tin, cc truy cp phi chia s ng

truyn trong mt thi gian c nh nu hon thnh th b xa ra cn khng s tip tc thc thi

vng lp sau. Cc bus APB l cc bus giao tip ngoi vi nn cn c cc cu (AHB/APB

Bridge) ng b APB v AHB.

B nh bao gm Flash, SRAM c chia ra thnh cc phn cho ngoi vi, chng trnh.

Cc b nh ny dng 4 loi bus AHB1, AHB2, APB1, APB2. Cc bus ny c th thit lp


48/91


35

cc tc clock khc nhau nn dn n tc cc ngoi vi khc nhau. Phn chia cc vng

nh quan trng nh sau:

SRAM gm 3 khi 112 KByte, 64 Kbyte v 12 KByte bt u 0x20000000. Victruy cp SRAM s khng tn thi gian tr hay trng thi ch (latency or wait state).

B nh Flash cho lp trnh gm 12 sector t 0x08000000 n 0x080FFFFF (1MByte). Vic truy cp n Flash s tn mt khong thi gian ch (thi gian tr ny c

th lp trnh c) ty thuc vo tc truy cp v in p cung cp cho vi iu

khin.

B nh chng trnh Bootloader bt u ti 0x00000000 c th dng vi USART1,USART3, CAN2 hoc USB ch l DFU (Device Firmware Update).

Ngun xung clock:c 3 ngun cung cp clock cho vi iu khin

HIS clock: dao ng ni 16Mhz c th dng trc tip hoc qua b PLL. HSE clock: dao ng ngoi 4Mhz n 26Mhz. PLL clock: c cung cp clock t HIS hoc HSE gm 2 phn

Main PLL: cung cp clock cho li v cc ngoi vi. PLL I2S: cung cp clock chnh xc cao cho b I2S (dng cho x l m thanh).

n gin lp trnh nh sn xut ST cung cp mt phn mm i km to file thit lp

xung clock cho STM32F407. Ngi dng ch cn nhp cc thng s cn thit sau s dng

file to ra a vo chng trnh ca mnh. Giao din chng trnh:


49/91


36

Hnh 3.4 Chng trnh to file thit lp clock cho STM32F407

GPIO (General Purpose In/Out)

Dng STM32F407 c 100 chn trong c khong 80 chn c th dng chc nng

GPIO thng thng. Cc chn ny chia lm 5 port PA, PB, PC, PD, PE; mi port c 16 chn.

Cc chn xut c th thit lp ch push-pull, open-drain hoc c in tr pull-up/pull-

down. Ngoi ra c th thit lp tc clock cho cc chn tit kim nng lng. Cc chn

nhp c th thit lp ch floating, pull-up/pull-down hoc analog. Tt c cc chn ny

u c th ci t lm ngt ngoi. c bit phn ln cc chn c tnh cht l 5V tolerant do

c th dng giao tip trc tip vi cc IC ngoi c mc logic 0-5V. V d chng trnh ci

t chn PA0 ch input c nt nhn v chn PD12 ch output xut LED, khi

c nt nhn th chuyn trng thi LED:

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;

GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;

GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;

GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;

GPIO_Init(GPIOD, &GPIO_InitStructure);


50/91


37

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);

GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;

GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;

GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;

GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;

GPIO_Init(GPIOD, &GPIO_InitStructure);

while(!GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0))

{

GPIO_ToggleBits(GPIOD, GPIO_Pin_12);

}

Timer

Nu khng tnh b SysTickTimer ca li th ngoi vi ca STM32F407 c 14 timer. Cc

timer ny u c th dng nh thi, to delay hay PWMNhng trong STM32F407 chia

thnh 3 nhm vi mt s kh nng ring chuyn dng cho cc ng dng c th.

Timer iu khin cao cp: (Advanced-control timers) TIM1 & TIM8 Timer 16 bit vi b m counter t ly li gi tr t (auto-reload) v b

chia trc 16 bit (prescaler) lp trnh c.

C th dng cho capture, PWM c dead-time Timer dng cho mc ch chung chung: (General-purpose timers)

TIM2 n TIM5: 16 hoc 32 bit. TIM9 n TIM14: 16 bit. Cc timer ny u c kh nng auto-reload v b chia trc 16 bit lp

trnh c.

Dng timer ny cho capture, PWM Timer c bn: (Basic Timer) TIM6 & TIM7

16 bit vi b m ln, b chia 16 bit. Dng cho to khng thi gian chun x l cc tc v. Dng clock cho b DAC.

trong phn ny dng TIM6 trong nhm timer c bn (cc timer khc nu ch dng vi

chc nng nh thi th cng tng t). Nh tm tt trn nhm timer c bn l timer vi

b m ln 16 bit, b chia trc 16 bit.


51/91


38

Hnh 3.5 Gin hot ng Timer c bn

V d chng trnh thit lp TIM6 ch ngt 1s v SysTick Timer ngt 1ms:

RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6, ENABLE);

prescaleValue = (uint16_t) 8400;

TIM_TimeBaseInitStruct.TIM_Prescaler = prescaleValue - 1;

TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;

TIM_TimeBaseInitStruct.TIM_Period = 10000;

TIM_ITConfig(TIM6, TIM_IT_Update, ENABLE);

TIM_TimeBaseInit(TIM6, &TIM_TimeBaseInitStruct);

TIM_Cmd(TIM6, ENABLE);

SysTick_Config(21000);

SysTick_CLKSourceConfig(SysTick_CLKSource_HCLK_Div8);

on chng trnh ngt o trng thi bit trong ngt TIM6 v SysTick Timer

void SysTick_Handler(void)

{

static uint32_t SysTick_counter;

SysTick_counter++;

if(SysTick_counter == 1000)

{

SysTick_counter = 0;


}

}

void TIM6_DAC_IRQHandler(void)

{

if(TIM_GetFlagStatus(TIM6, TIM_FLAG_Update) == SET)


52/91


39

{

TIM_ClearFlag(TIM6, TIM_FLAG_Update);


}

}

USART

Giao thc truyn thng ni tip UART qu ph bin trong cc thit b hin nay.

Trong lun vn ny khi UART s c dng giao tip truyn d liu t IMU ln my

tnh. STM32F407 c 4 b USART (1,2,3,6) v 2 b UART (4,5) truyn thng song cng, bt

ng b hoc ng b. y em quan tm n truyn thng bt ng b trao i d liu

vi IMU. D liu c ci t 8 bit, 1 stop bit v khng dng parity. c bit cc b USART

ny h tr DMA nn rt thun li cho vic nhn v gi lng ln d liu m khng lm tn

nhiu thi gian ca li vi x l.

Hnh 3.6 Frame truyn ca USART

Cc bc ci t gi v nhn qua USART trong phn mm:

Bt clock cho bus APB ca ngoi vi USART. Bt clock cho cc Port ca cc chn lin quan l TX, RX. Ci t chc nng USART cho cc chn lin quan TX, RX. Ci t cc thng s cho b USART. V d: baud rate 115200, d liu 8 bit, 1 stop

bit, khng dng Parity, cho php b thu v b pht, cho php c ngt bo nhn

xong v gi xong, cho php s dng DMA b thu v b pht.

Ci t DMA: (c chi tit trong phnDMA) Trng hp gi:

Ngun gi l buffer cha d liu mun gi. ch n l thanh ghi d liu ca USART USART_DR.


53/91


40

Trng hp nhn: th ngc li. Ci t ngt cho b thu v b pht ( phi xa cc c trong ngt).

V d chng trnh echo dng USART1:

RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE) ;

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);

//enable alternate function at TX,RX pins

GPIO_PinAFConfig(GPIOB, GPIO_PinSource6, GPIO_AF_USART1);

GPIO_PinAFConfig(GPIOB, GPIO_PinSource7, GPIO_AF_USART1);

// Init GPIO

GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;




//TX PIN


GPIO_Init(GPIOB, &GPIO_InitStructure);

//RX PIN


GPIO_Init(GPIOB, &GPIO_InitStructure);

//Init Uart properties

USART_InitStructure.USART_BaudRate = 115200;

USART_InitStructure.USART_WordLength = USART_WordLength_8b;

USART_InitStructure.USART_StopBits = USART_StopBits_1;

USART_InitStructure.USART_Parity = USART_Parity_No;

USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;

USART_InitStructure.USART_HardwareFlowControl =

USART_HardwareFlowControl_None;

USART_Init(USART1, &USART_InitStructure);

USART_Cmd(USART1, ENABLE);

while(1)

{while(USART_GetFlagStatus(USART1, USART_FLAG_RXNE) == RESET);

getChar = USART_ReceiveData(USART1);

USART_SendData(USART1, getChar);

}


54/91


41

SPI

Giao thc ni tip ng b SPI dng nhiu trong giao tip vi cc loi cm bin. Trong

giao thc ny mt chip ng vai tr Master v nhiu chip ng vai tr Slave. Giao thc SPI

yu cu 4 dy:

CS: (Chip Select) chn Slave giao tip vi Master. TX: xut d liu (cn gi l MOSI: Master Out Slave In). RX: nhn d liu (cn gi l MISO: Master In Slave Out). CLK: chn clock ng b d liu c iu khin bi Master.

Qu trnh truyn nhn trn ng truyn SPI tm tt nh sau:

Qu trnh truyn:

Master s ko chn CS xung mc thp chn Slave. Sau d liu 8 bit c dch t song song sang ni tip trn ng TX.

Ty vo loi cm bin th ban u cn thit lp bit Read/Write v a ch

thanh ghi trong Slave.

Qu trnh nhn: Master phi ko chn CS xung mc thp chn Slave. Sau bt u truyn d liu 8 bit bo cho Slave a ch thanh ghi cn c

v.

Cc chu k sau Master phi tip tc gi clock trn ng CLK bng cchghi gi tr gi khng c ngha ra TX (dummy write). V b thu s c gi

tr gi v trn RX t Slave.

Cc bc ci t gi v nhn bng SPI trong phn mm:

Bt clock cho khi ngoi vi SPI, cho cc port ng vi cc chn c lin quan nSPI.

Ci t chc nng chn CS l dng digital, output cn cc chn CLK, TX, RX lchc nng SPI.

Thit lp cc thng s cho SPI: truyn 2 hng, d liu 8 bit hay 16 bit, bit LSB itrc v ng vai tr l Master.

Cho php SPI v DMA trn SPI. Ci t DMA cho SPI: a ch ngun, a ch ch, di khi d liu Ci t cc ngt truyn v nhn hay DMA.


55/91


42

Hnh 3.7 Gin th t truyn SPI

V d chng trnh ci t SPI:

RCC_APB2PeriphClockCmd(RCC_APB2Periph_SPI1, ENABLE);

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);

GPIO_PinAFConfig(GPIOA, GPIO_PinSource7, GPIO_AF_SPI1); //MOSI

GPIO_PinAFConfig(GPIOA, GPIO_PinSource6, GPIO_AF_SPI1); //MISO

GPIO_PinAFConfig(GPIOA, GPIO_PinSource5, GPIO_AF_SPI1); //SCK

GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;



GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN;

// MOSI


GPIO_Init(GPIOA, &GPIO_InitStructure);

// MISO



// SCK


56/91


43



// SPI Configuration

SPI_InitStructure.SPI_Direction = SPI_Direction_2Lines_FullDuplex;

SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b;SPI_InitStructure.SPI_CPOL = SPI_CPOL_Low;

SPI_InitStructure.SPI_CPHA = SPI_CPHA_1Edge;

SPI_InitStructure.SPI_NSS = SPI_NSS_Soft;

SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_4;

SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB;

SPI_InitStructure.SPI_CRCPolynomial = 7;

SPI_InitStructure.SPI_Mode = SPI_Mode_Master;

SPI_Init(SPI1, &SPI_InitStructure);

SPI_Cmd(SPI1, ENABLE);

// Chip Select

GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3 ;

GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;




GPIO_Init(GPIOE, &GPIO_InitStructure);

Chng trnh trao i d liu trn bus SPI:

uint8_t mySpiExchangeByte(SPI_TypeDef* SPIx, uint8_t byte){

while (SPI_I2S_GetFlagStatus(SPIx, SPI_I2S_FLAG_TXE) == RESET);

SPI_I2S_SendData(SPIx, byte);

while (SPI_I2S_GetFlagStatus(SPIx, SPI_I2S_FLAG_RXNE) == RESET);

return SPI_I2S_ReceiveData(SPIx);

}

DMA

DMA (Direct Memory Access) cho php truyn d liu tc cao gia ngoi vi v ccvng nh, gia cc vng nh vi cc vng nh. D liu c chuyn i trn bus ring ca

DMA nn khng cn c s can thip ca li vi x l, trong lc li vn thc hin cc tc v

khc nn tit kim nhiu thi gian cho tnh ton. Cc c im ca DMA trong STM32F407:

C 2 b DMA (DMA1, DMA2) mi b iu khin 8 stream, trn mi stream c bghp knh x l 8 yu cu (request).


57/91


44

Mi stream c th ci t dng cho chuyn d liu 3 nhm t: Ngoi vi ti vng nh (peripheral-to-memory). Vng nh ti ngoi vi.

Vng nh ti vng nh. C 4 b FIFO dng cho:

FIFO mode: phn mm chn cc mc dng FIFO , , hoc yFIFO.

Direct mode: chuyn d liu trc tip khng quan FIFO. Cc stream c th ci t 4 cp u tin: rt cao, cao, trung bnh, thp (very

high, high, medium, low).

S lng khi d liu truyn trn DMA c th ln ti 65535. C 5 c i km qu trnh truyn DMA kim sot l:

Truyn c mt na ( Half-transfer reached). Truyn xong hon ton (Transfer complete). Truyn c li (Transfer error). FIFO c li (FIFO error). Truyn ch trc tip c li (Direct mode error).

Cc bc ci t s dng DMA:

Ci t trc ngoi vi s dng (nh USART v SPI trn) trong cho php ngtDMA.

Thit lp DMA: Chn knh DMA. Chn a ch gi v nhn cho ngun v ch. a ch c th l thanh ghi

trong phn cng hoc a ch bin lu tr t t.

Chn dng FIFO hay khng v 4 mc tng ng. Mt s ty chn khc nu cn.

Thit lp cc thng s nh trn DMA trn knh ny. Cho php ngt hoc khng ngt 5 c bo DMA trn. Mi khi mun ta s cho php DMA.

V d chng trnh ci t s dng DMA trong truyn d liu trn USART1_TX:


58/91


45

// When using rx_dma do not enable receive interrupt

USART_ITConfig(USART1, USART_IT_RXNE, DISABLE);

USART_Init(USART1, &USART_InitStructure);

// Enable usart1

USART_Cmd(USART1, ENABLE);

// Enable TX-RX DMA request

USART_DMACmd(USART1, USART_DMAReq_Tx|USART_DMAReq_Rx, ENABLE);

// Enable DMA2 Stream7-Channel4

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2, ENABLE);

DMA_Usart1TxStruct.DMA_Channel = DMA_Channel_4;

DMA_Usart1TxStruct.DMA_DIR = DMA_DIR_MemoryToPeripheral;

DMA_Usart1TxStruct.DMA_Memory0BaseAddr = (uint32_t)usart1TxBuffer;

DMA_Usart1TxStruct.DMA_PeripheralBaseAddr = (uint32_t)USART1 + 0x04;

DMA_Usart1TxStruct.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;

DMA_Usart1TxStruct.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;

DMA_Usart1TxStruct.DMA_Priority = DMA_Priority_High;

DMA_Usart1TxStruct.DMA_FIFOMode = DMA_FIFOMode_Enable;

DMA_Usart1TxStruct.DMA_FIFOThreshold = DMA_FIFOThreshold_Full;

DMA_Usart1TxStruct.DMA_MemoryInc = DMA_MemoryInc_Enable;

DMA_Usart1TxStruct.DMA_PeripheralInc = DMA_PeripheralInc_Disable;

DMA_Usart1TxStruct.DMA_Mode = DMA_Mode_Normal;

DMA_Usart1TxStruct.DMA_BufferSize = (uint32_t)10 ;

DMA_Usart1TxStruct.DMA_MemoryBurst = DMA_MemoryBurst_Single;

DMA_Usart1TxStruct.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;

//Enable DMA Stream Transfer Complete interrupt

DMA_ITConfig(DMA_USART1TX_STREAM, DMA_IT_TC, ENABLE);

DMA_Init(DMA_USART1TX_STREAM, &DMA_Usart1TxStruct);

// Enable DMA Interrupt DMA2_STREAM7

NVIC_InitStruct.NVIC_IRQChannel = DMA2_Stream7_IRQn;

NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 1;NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0;

NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;

NVIC_Init(&NVIC_InitStruct);

Chng trnh nhn d liu trn ngt DMA USART1_TX DMA2-Stream7


59/91


46

void DMA2_Stream7_IRQHandler(void)

{

if (DMA_GetFlagStatus(DMA_USART1TX_STREAM, DMA_FLAG_TCIF7)== SET)

{

DMA_ClearFlag(DMA_USART1TX_STREAM, DMA_FLAG_TCIF7);GPIO_ToggleBits(GPIOD, GPIO_Pin_12);

}

}

3.1.2 Cm bin ADIS16405 v phng php caliba. Gii thiu cm bin ADIS16405

Cm bin ADIS16405 l h thng o qun tnh y bao gm ba loi Gia tc, Vn tc

gc v T trng. Gi tr ca cc loi cm bin c calib khi sn xut v c bit c

b theo nhit nn rt thun li cho vic kim tra gii thut (khng b nh hng bi sai s

do nhit ). Cm bin ADIS16405 cung cp giao thc SPI tc cao giao tip nn thun

tin cho vic tch hp IMU nh gn.

Hnh 3.8 S khi ADIS16405 v h trc ca cc cm bin

b. Giao tip gia VK STM32F405 v cm bin ADIS16405


60/91


47

Hnh 3.9 M hnh kt ni phn cng gia VK v ADIS16405

Giao tip gia STM32F405 v ADIS16405 l ng giao tip ni tip SPI 16-bit. Mi gi tr

ca cm bin s cha trong cc thanh ghi 16-bit c a ch 7-bit.

Hnh 3.10 Gin xung trao i d liu trn SPI v ch c burst-mode

Mt frame truyn ca SPI t VK s gm 16 bit, trong bit th 16 s quy nh c hay ghi

(bit 0: c, bit 1: ghi), 7 bit tip theo cha a ch thanh ghi mun giao tip v 8 bit cui l gitr ghi vo hoc khng quan tm nu l c. Gi tr c v s c nhn trong chu k tip

theo.

/ A[6:0] DC[7:0]


61/91


48

V d: mun c gi tr thanh ghi XACCL_OUT a ch 0x0A ta s ghi d liu ra SPI

0x0A00 v c v16-bit d liu tip theo bng cch ghi ra gi tr dummy byte 0x0000.

ADIS16405 h tr ch c Burst-mode tin cho vic c lin tip nhiu byte. Ban

u s phi ghi ra ng SPI gi tr 0x3E00 sau ln lt ghi ra dummy byte 0x0000 vc v cc gi tr t thanh ghi 0x02 (SUPPLY_OUT)n thanh ghi 0x18 (AUX_ADC). Gi

tr 16-bit sau khi c v dng s b 2 14-bit (bit du l bit s 14, 13 bit thp l s) nn phi

chuyn qua s nguyn trc khi nhn vi h s t l ng vi ring tng thanh ghi.

Bng 3.2 Bng h s t l cho cc gi tr cm bin

c. Calib cm bin

Cc gi tr ca cc loi cm bin Accelerometer v Gyroscope c calib trong khisn xut nn khng cn phi calib li. Tuy nhin do t trng thay i theo cc vng a l

khc nhau, b nhiu bi cc thit b in t, hay kim loi gn nn phi calib li cm bin

T trng. Cc gi tr offset s c thm vo sau khi thu gi tr t cm bin sao cho t

trng trn ba trc x,y,z to thnh hnh cu. Sau y l gin t trng trn cc trc trc

v sau khi calib.


62/91


49

Hnh 3.11 Gin t trng trn cc trc trc khi calib

Hnh 3.12 Gin t trng trn cc trc sau khi calib

Nh trn hnh v ln lt cc offset ca t trng trn ba trc l:

= 0.025 = 0.025 = 0.01 3.2 Xy dng m hnh MATLAB Simulink gii thut Kalman m rng ci

tin dng DCMTng t nh m hnh DCM phn hai, m hnh ny thm ba thng s c lng ba gi

tr gia tc ngoi , , tng kh nng thch nghi cho b c lng IMU khi c giatc ngoi.


63/91


50

Hnh 3.13 M hnh EKF-DCM ci tin

M hnh KF1:

Phng trnh ng hc da vo DCM: (: ma trn xoay chuyn ta t h B-IMU sang N-Earth)

= = . 0 0 0

Rt ring hng th 3 ca ma trn DCM ta c phng trnh:

= 0

0

0

ci tin gii thut y ta thm khu c lng gia tc ngoi vi phng trnh ng hc

nh sau:

= . Vi [0,1]

Phng trnh o lng cho Accelerometer:

= . 001 +

= +

M hnh KF1 s dng cc bin trng thi hng s ba ca ma trn DCM. T ta xy dng

c m hnh ton cho khi lc Kalman 1 nh sau:

= , , , , , , , , = , , , , ,


64/91


51

= . + = 0 00 0 00 0 (1 ) = + Vi

= 0

0 0

= . + = 0 0 0Gi s nhiu t Gyrometer 3 trc khng tng quan nhau ta s c biu thc covariance v

dng ri rc ca nhiu qu trnh:

= (. ) = 0 0 00 . 00 0 = . . . . 0 . . . 00 0 .

V biu thc covariance ca nhiu o lng: = (. ) = 00 . Trong , l variance tnh ca Accelerometer v Gyrometer.T ba gi tr c lng , , tnh c hai gc , nh sau: = () = (, )

M hnh KF2:

m hnh KF2 ta dng cc bin trng thi l hng hai ca ma trn DCM:

= 0 0 0

t:

= 0 0 0 Mt khc

,

, c tnh t gc

, v ba gi tr t Magnetometer theo cc cng thc

sau: = . + . . + .. = . = + ,cos = +


65/91


52

= ., = . + . . ,= . +.. = , , , , , = , , , , , = . + = 0 0 0 = + = . 0 = . + = [] = 0 00 . = . . . . . . . . .

= .

0

0 .

T ba gi tr , , tnh c cc s hng cn li ca ma trn xoay da vo tch huhng: = = . . = . . = . . = ( , )

Trong b KF1 cc h s covariance ca nhiu o lng c chn l gi tr variance

ca cc cm bin khi tnh, cc h s covariance ca nhiu qu trnh c chn bng phngphp th nhiu ln sao cho sai s ca IMU l tt nht. Trong b KF2 cc h s covariance ca

nhiu o lng v nhiu qu trnh cng c th nhiu ln sao cho sai s nh nht.


66/91


53

Hnh 3.14 M hnh Simulink ca phng php EKF-DCM

3.3 Lp trnh cho IMU kt hp MATLAB Simulink Embedded Coder vtrnh bin dch KeilC

Hnh 3.15 M hnh Compiler kt hp MATLAB Simulink v Keil C


67/91


54

Mt trong nhng kh khn ln ca vic hin thc b lc Kalman trn h thng nhng l

m hnh sau khi m phng trn MATLAB phi c chuyn sang ngn ng C. Tuy nhin

MATLAB cung cp mt cng c rt mnh h tr ngi dng l MATLAB Embedded

Coder. Cng c ny cho php chuyn t m hnh MATLAB Simulink sang ngn ng C (hoc

C++). T cc file c bin dch (dng .h v .c) kt hp vi cc file driver vit cho

STM32F40x ta s c b source code bng ngn ng C v c th bin dch ra file .hex dng

cho VK. Kin trc ca phn mm trn board IMU nh sau:

Hnh 3.16 Kin trc phn mm trong IMU

Gii thut cho IMU c chy vi tn s 100Hz theo lu gii thut sau:

Hnh 3.17 Lu gii thut x l trong IMU

Trong thi gian chy cc tc v trong vng lp 10ms l 8.5ms


68/91


55

3.4 Lp trnh ha m hnh 3D cho IMU dng ngn ng PythonPython l trnh bin dch cp cao (interpreted language), chng trnh khi vit xong s

c chy trc tip bi trnh bin dch m khng cn dch (compile) trc. Cc c php v

tnh cht ca ngn ng Python ging cc ngn ng lp trnh hng i tng khc nh: C#,C++, JavaTuy nhin mt u im rt ln ca Python l m ngun m nn c s dng

ngy cng rng ri trn th gii. Ngn ng Python t c s dng trong cc i hc Vit

Nam, tuy nhin theo khuynh hng trn th gii th cc phn mm m ngun m s ngy

cng pht trin. V cc l do trn m trong lun vn ny em s dng Python (version 2.7)

lm phn mm ha m phng 3D cho IMU, ng thi cng tm hiu cch s dng v mt

s th vin km theo ca loi ngn ng mi m ny.

Hnh 3.18 M hnh ca mt trnh bin dch (interpreted language)

3.4.1 Tng quan cu trc v c php ca mt chng trnh vit bng Pythona. Bin, biu thc, biu thc iu kin, v lnh

Tng t nh cc ngn ng khc Python cng cung cp cc kiu bin, cc ton t v

cc biu thc iu kin nh int, float, string, objectcng, tr, nhn, chia, mif, for,

whileSau y l v d mt s chng trnh nh vit bng Python lm r hn cu trc ca

chng trnh phn mm vit bng Python

Chng trnh 1:Hello Worldtrong Python

print Hello World!

Chng trnh 2: ngi dng nhp hai kch thc rng, di tnh chu vi v din tch hnh chnht

def circle(width, height):

return 2 * (width + height)

def area(width, height):

return (width * height)


69/91


56

w = input("Width : ")

h = input("Height : ")

print 'Cirle: ', circle(w,h)

print 'Area : ', area(w,h)

Chng trnh 3: vit hm tnh qui dy sFibonacci, sau tnh trong trng hp n=6

def fibonacci(n):

""" f(n) = f(n-1) + f(n-2) """

if not isinstance(n, int):

print "Inavalid value"

return -1

if n == 0 or n == 1:

return 1

else:res = fibonacci(n-1) + fibonacci(n-2)

return res

print fibonacci(6)

chng trnh 1 lnhprints in chui Hello World! ra mn hnh. chng trnh 2

hm input s xut ra tn cc kch thc v ngi dng phi nhp s vo tnh. C hai

chng trnh con tnh din tch v chu vi l hm circle v area. chng trnh 3 l hm

quy (recursion) tnh chui s Fibonacci. Trong hm c dng biu thc iu kin if loi

tr trng hp n khng phi l s nguyn.

b. Class& Thread

Tng t nh cc ngn ng lp trnh hng i tng khc Python cng cung cp cch

xy dng cc class ca ring mnh. Trong chng trnh ny ta s xy dng ba class k tha

class Threadc sn ca Python chy song song vi nhau l:

Class qun l giao tip cng ni tip vi board IMU (CommThread): c d liu tIMU thng qua UART, kim tra d liu hp l hay khng v gi d liu gc qua hai

class khc thng quan Queue hin th ha. Class qun l ha m hnh 3D ca IMU (Graph3DThread): class ny s nhn d

liu chuyn qua t CommThreadsau hin th ln cc ca s ha 3D.

Class qun l ha gin theo thi gian ca IMU (Graph2DThread) : class ny snhn d liu chuyn qua t CommThreadsau hin th ln cc ca s v th theo

thi gian ba gc Euler.


70/91


57

V d chng trnh to class CommThread, chng trnh s m port ni tip sau lp vng

ch khi nhn c d liu t IMU s gi d liu ny qua hai Threadcn li thng qua queue

x l :

class CommThread(threading.Thread):

def __init__(self, queue3D, queue2D):

threading.Thread.__init__(self)

self.queue3D = queue3D

self.queue2D = queue2D

self.com = serial.Serial(port='COM1', baudrate=115200)

def run(self):

while True:

line = self.com.readline()

words = string.split(line)

if len(words) >= 12:

self.queue3D.put(words)

self.queue2D.put(words)

def close(self):

self.com.close()

3.4.2 Cc th vinBn thn ngn ng Python khng h tr y cc giao tip phn cng hay ha 3D

nn cc lp trnh vin trn th gii to ra cc m ngun m h tr cc ng dng ny.

Trong lun vn ny em dng hai b th vin:

PySerial: h tr giao tip cng ni tip VPython: h tr hin th cc m hnh 3D v th theo thi gian

a.PySerial

Th vinPySerialc pht trin bi Chris Liechti nhm mc ch to ra mt interface

n gin giao tip gia Python v phn cng thng qua cng ni tip. Cc c im ca

th vin ny l: H tr cc giao tip ni tip bao gm: UART, TCP/IP Cng chung mt interface cho tt c hai kiu giao tip trn H tr cc kiu truy cp khc nhau nh: c, vit tng byte, chui API n gin d s dng

Mt s interface h tr bi th vin l:


71/91


58

Interface Chc nng__init__() khi ng cc gi tr cho cng nh tn cng, baudrate, s bit, thi gian

timeoutopen() m cngclose() ng cng

read(size) c s bytewrite(data) vit ra d liureadline() c cho ti khi nhn k t \nwriteline(string) vit string ra cng

V d chng trnh m port ni tip, c d liu v xut ra mn hnh dngPySerial:

import serial

com = serial.Serial(port="COM1", baudrate=115200, timeout=1)

data = com.readline()

print data

com.close()

b. VPython

Th vin VPython c pht trin bi nhm sinh vin trng Carnegie Mellon, M nhm

mc ch h tr cc m phng 3D, gip ngi dng Python tn t thi gian hn trong vic

pht trin chng trnh ha. Th vin cung cp cc hnh khi 3D nh: hnh cu, hnh hp,

mi tn, hnh elip, textv cc bin v tr vt trong khng gian cng nh h trc quay vt

th

H ta trong Python chia lm ba trc: x hng sang phi, y hng ln, z vung gc

hng ra mn hnh. V tr cng nh kch c ca cc vt th c chng trnh t ng scale

sao cho ph hp vi mn hnh. Trong mn hnh hin th c th phng to, nh bng cch dng

chut phi.

Hnh 3.19 H ta v vt th trong VPython


72/91


59

Cc vt th trong VPython c xc nh bi tn, v tt c u c thuc tnh v tr cng

nh kch thc. V d chng trnh to cc hnh cu tip xc nhau:

from visual import*

ball = sphere(pos=(0,0,0), radius=2, color=color.red)

ball1 = sphere(pos=(0,4,0), radius=2, color=color.green)

ball2 = sphere(pos=(0,-4,0), radius=2, color=color.green)

ball3 = sphere(pos=(4,0,0), radius=2, color=color.green)

ball4 = sphere(pos=(-4,0,0), radius=2, color=color.green)

Hnh 3.20 Hnh cu trong VPython

y ta s tm hiu i din mt vt th l hnh hp r hn cch dng th vin ny.

Hnh 3.21 Hnh hp trong VPython

Cc thuc tnh quan trng ca hnh hp trong VPython:

Kch c:Length-Chiu di, Width-Chiu rng,Height-Chiu Cao Hng: axis-vector song song chiu di ca hnh hp, up-vector song song vi chiu

cao ca hnh hp

V tr:pos-vector v tr ca tm hnh hp Mu sc: color-mu sc ca hnh hp (green, red, blue,)

hin th m hnh 3D ca IMU trong VPython ta s to trc i tng hnh hp sau

thay i hng quay ca hnh hp ging nh hng quay ca IMU bng cch tnh ra hai


73/91


60

vector thuc tnh hng up, axis ca hnh hp t d liu u vo l ba gc xoay nhn c t

IMU. Cch tnh hai vectorup, axis nh sau:

u tin ta c ma trn xoay chuyn vector t h trc IMU sang h trc chun NED:

= . .. . .+... ..+. .. . . . Hay vit gn li:

= Vectorup trong h trc IMU s l: = (0,0,1)Vectoraxis trong h trc IMU s l: = (1,0,0)T tnh c ta hai vector ny trong NED l:

= . = (, , ) = . = ( , , )

Tuy nhin hai h trc NED v h trc trong Vpython (VP) khc nhau do ta phi chuyn li

ta ca hai

datung imu

Documents