I HC QUC GIA H NI

TRNG I HC CNG NGH

V VN NINH

NNG CAO CHT LNG H THNG TCH

HP INS/GPS S DNG B LC KALMAN

LUN VN THC S

H NI 2012

I HC QUC GIA H NI
TRNG I HC CNG NGH

V VN NINH

NNG CAO CHT LNG H THNG TCH

HP INS/GPS S DNG B LC KALMAN
Ngnh: Cng ngh in t - Vin thng
Chuyn nghnh : K thut in t
M s: 60 52 70

LUN VN THC S
HNG DN KHOA HC: TS. TRN C TN

H NI 2012

LI CAM OAN
Ti xin cam oan nhng ni dung trnh by trong kha lun sau y c
trch dn trung thc t cc ngun ti liu tham kho, ng thi nghin cu trc
c dng pht trin cho ti ca ti cng c trch dn y v chnh xc.

Ngi cam oan

V Vn Ninh

Nng cao cht lng h thng tch hp INS/GPS s dng b lc Kalman

II

LI CM N
Li u tin tc gi mun gi li cm n chn thnh ti thy gio hng
dn, TS.Trn c Tn ch bo tn tnh, nh hng nghin cu cho ti. Thy
dnh nhiu thi gian tm huyt v gip ti hon thnh lun vn ny.
Trong thi gian hc tp v lm lun vn ti b mn MEMS, ti c to
mi iu kin thun li nht. Qua y cho ti by t lng bit n n cc thy,cc
anh, cc bn v cc em sinh vin trong b mn v s gip nhit tnh m ti nhn
c. Cm n s h tr t ti cp HQGHN m s QB.B.11-31.
Cm n cc thy trong khoa in t - Vin thng trng i Hc Cng
Ngh, i hc Quc Gia H Ni v kin thc m cc thy c trang b cho em trong
qu trnh hc tp.
Cui cng tc gi mun cm n ti gia nh, bn b c nhng li ng
vin, khch l tc gi quyt tm hon thnh lun vn ny.
Trong qu trnh hon thnh lun vn khng th trnh khi nhng thiu st.
Tc gi rt mong nhn c nhng ng gp qu bu ca thy, c v cc bn
lun vn c hon thin hn.
H Ni, thng 09 nm 2012
Hc vin
V Vn Ninh

Nng cao cht lng h thng tch hp INS/GPS s dng b lc Kalman

III

MC LC
LI CAM OAN........................................................................................................................... I
LI CM N ................................................................................................................................ II
MC LC.....................................................................................................................................III
DANH MC HNH .................................................................................................................... IV
DANH MC BNG.....................................................................................................................V
THUT NG VIT TT.......................................................................................................... VI
TM TT ................................................................................................................................... VII
CHNG I. GII THIU H THNG GPS........................................................................... 1
CHNG II. H THNG INS .................................................................................................. 4
2.1 Nguyn l hot ng ca INS......................................................................................................4
2.2 Ma trn chuyn h ta ............................................................................................................5
2.2.1 Ma trn chuyn Euler...........................................................................................................5
2.2.2 Ma trn chuyn Quaternion ..................................................................................................6
2.2.3 Tnh gc Euler t ma trn xoay.............................................................................................7
2.2.4 Tnh Quaternion t ma trn xoay ..........................................................................................8
2.2.5 Tnh gc Euler t Quaternion ...............................................................................................8
2.3 Phng trnh chuyn ng ..........................................................................................................9
2.4 M hnh li INS ....................................................................................................................... 10

CHNG III. H THNG TCH HP INS/GPS ................................................................. 14

3.1 L Thuyt ................................................................................................................................ 14
3.1.1 B lc Kalman ................................................................................................................. 14
3.1.2 H thng tch hp INS/GPS................................................................................................ 17
3.2 xut ca hc vin ................................................................................................................ 18
3.2.1 Lu thut ton h thng tch hp INS/GPS ..................................................................... 18
3.2.2 B lc Kalman cho h thng tch hp INS/GPS ................................................................... 19
3.3 Kt qu m phng .................................................................................................................... 23

CHNG IV. KT LUN ....................................................................................................... 30

TI LIU THAM KHO .......................................................................................................... 31
PH LC ..................................................................................................................................... 32

Nng cao cht lng h thng tch hp INS/GPS s dng b lc Kalman

2
CHNG I. GII THIU H THNG GPS
H thng nh v ton cu (Global Positioning System GPS) l h thng xc nh v tr da trn v tr ca cc v tinh
nhn to, do B Quc phng Hoa K thit k, xy dng, vn
hnh v qun l. H thng nh v ton cu GPS thu thp cc
thng tin v ta (v v kinh ), tc v cao ca vt
th. H thng GPS gm 24 v tinh phng ln qu o tri t
(hnh 1).
Cc h thng dn ng truyn thng hot ng da trn
cc trm pht tn hiu v tuyn in. c bit n nhiu nht l
cc h thng sau: LORAN (LOng RAnge Navigation) hot
ng gii tn 90-100 kHz ch yu dng cho hng hi, hay
TACAN (TACtical Air Navigation) dng cho qun i M v
bin th vi chnh xc thp VOR/DME VHF
(Omnidirectional Range/Distance Measuring Equipment) dng
cho hng khng dn dng

Hnh 1. Qu o bay ca cc v tinh GPS

Cc v tinh GPS bay vng quanh Tri t hai ln trong mt ngy
theo mt qu o rt chnh xc v pht tn hiu c thng tin
xung Tri t. Cc my thu GPS nhn thng tin ny v bng
php tnh lng gic tnh c chnh xc v tr ca ngi dng.

3
V bn cht my thu GPS so snh thi gian tn hiu c pht i
t v tinh vi thi gian nhn c chng. Sai lch v thi gian
cho bit my thu GPS cch v tinh bao xa. Ri vi nhiu qung
cch o c ti nhiu v tinh my thu c th tnh c v tr ca
ngi dng v hin th ln bn in t ca my.
Sau y l mt s nguyn nhn chnh gy ra sai s ca phng
php inh v GPS :

Nhiu do hin tng Fading Hin tng ny xy ra

thng xuyn trong thng tin v tuyn. Ti ni thu s thu
c nhiu tn hiu phn x t nh hay cc i tng
khc.
Li xung nhp ng h my thu ng h c trong my
thu khng chnh xc nh ng h nguyn t trn cc v
tinh GPS.
Li qu o Do v tinh thng bo v tr khng chnh
xc.
S lng v tinh quan st c Cng nhiu v tinh
c my thu GPS nhn thy th cng chnh xc. Cc a
hnh phc tp,cc cng trnh cao tng gy li nh v
hoc khng nh v c.
V tr cc v tinh Trong qu trnh chuyn ng,c th
cc v tinh s nm trn mt ng thng hoc cm thnh
nhm, iu ny nh hng ti cch tnh ton v tr da
trn hnh hc v lm sai lch kt qu.
S suy gim tn hiu v tinh c ch tm Mc d GPS
c cung cp min ph nhng nhiu tnh nng khc vn
mang tnh thng mi. S lm gim tn hiu c do s
p t ca B Quc phng M, nhm chng li vic i
th qun s dng tn hiu GPS chnh xc cao.

4
CHNG II. GII THIU H THNG INS
2.1 Nguyn l hot ng ca INS
Nguyn l hot ng ca INS l tng hp cc tn hiu o c
bi mt c cu o lng qun tnh IMU (Inertial measurement
Unit), xc nh trng thi hin ti ca h thng. Cc tn hiu
o c bi IMU bao gm vn tc gc (gyros) v gia tc
(accelerometer). Cu to IMU bao gm 3 cm bin gia tc v 3
cm bin vn tc gc. y mt h thng INS bao gm IMU v
thut ton tnh ton c tch hp cng. Thut ton s dng trong
lun vn ny l thut ton SINS ca Salychev (Ph lc 2). INS
a ra cc thng s cho ngi dng nh: v tr (kinh , v ,
cao ), vn tc, cc gc t th (gc nghing, gc chc, gc
hng) (xem Hnh 3).
Hnh 2 di y m t mt khi IMU [2].

Hnh 2. Cu trc ca IMU

Hnh 3 di y m t nguyn l hot ng ca h thng INS [8].

5
Accelerometters

Gyros

INPUT

Initial alignment

Alignment phase
OUT PUT

Gravity,
Non-gravitational
acceleration correction

acceleration

Attitude

Attitude
Computing

Velocity
Coordinate
system
Transformation

integration

integration

Position

Navigational Phase

Hnh 3. H thng nh v qun tnh

2.2 Ma trn chuyn h ta
2.2.1 Ma trn chuyn Euler
Mt vn quan trng khi xc nh vt th trong khng gian 3
chiu l ta ca vt trong h ta Cc hoc l 3 gc
Euler.

Hnh 4. 3 gc Euler
Cc gc Euler ( , ,) thay th cho ba chuyn ng quay kt
hp, di chuyn h trc tham chiu n mt h trc ta ang xt.

6
Cc chuyn ng thnh phn l chuyn ng quanh mt trc, c
th l X, Y, hoc Z

(2.5)
2.2.2 Ma trn chuyn.Quaternion
Mt quaternion n v c th c nh ngha nh sau:
T

Chng ta c th kt hp quaternion vi mt chuyn ng quay

quanh mt trc nh sau:

Khi ma trn quay ca chuyn ng quay trong khng gian 3

chiu Euclide c xc nh nh sau :

(2.7)
2.2.3.Tnh gc Euler t ma trn xoay.
Gi s ta c mt ma trn xoay bit trc l
(2.8)
T phng trnh (2.5) ta c : tan = a32 /a33 ,sin = -a31, tan =
a21 /a11 .Nh vy

2.2.4.Tnh Quaternion t ma trn xoay

T cc gc Euler bit trc ,ta c th tnh c Quaternion c
xc nh theo cng thc sau
T phng trnh (2.6) ta thu c cc phng trnh sau:
=
=
=
1 =
Do

(2.11)

(2.12)

Gi s rng

l dng
sign(

) =sign(1)

sign(

) =sign(

sign(

) =sign(

(2.13)

8
sign(

) =sign(

2.2.5.Tnh gc Euler t Quaternion

T phng trnh (2.5),(2.6) ta tnh c cc gc Euler nh sau:

(2.14)
2.3.Phng trnh chuyn ng
T th ca vt th trong khng gian c xc nh bi 3 gc
Euler. Mi lin h gia cc vn tc gc ca Roll, Pitch, Yaw
(p,q,r) v 3 gc Euler

,, c tnh theo cng thc sau :

(2.15)
Nh vy khi tch phn cc phng trnh trn ta thu c cc gc
Euler.
Cc gia tc (ax ay az) dc theo cc trc ca h ta vt th lin
h vi 3 vn tc (U,V,W) trong h ta tri t theo phng
trnh sau :

(2.16)

Sau khi tch phn phng trnh (2.16), chng ta thu c U,V,W.
S dng ma trn Direct Cosine Matrix(DCM), chng ta c th
chuyn t h ta tri t sang h ta nh v:

(2.17)
Tch phn phng trnh (2.17), ta thu c v tr ca vt. Tip
theo chng ta c th thu c kinh , v v cao ca vt
thng qua cc phng trnh sau :

(2.11)

2.4.M hnh li INS

Hu ht cc li INS u do li cm bin qun tnh. C nhiu li
do thit lp,hiu chnh Gyros v gia tc ca INS.
i vi cc gc lch nh N ,E, Up l lch gia h ta pframe v ll-frame, ma trn chuyn gia 2 h ta ny c th
c m t theo (2.12), c suy ra t dng ma trn DCM chun,
v gi s cc gc ny nh tha mn cos = 1 v sin = .
Up
Zp

Up

North

Yp

East

Xp

Hnh 5. Sai lch gia h ta p-frame v N- frame

10

(2.12)
Theo phng trnh Poisson, vn tc gc trong h ta p-frame
c th c biu din theo vn tc gc tuyt i trong h ta
ll-frame v cc o hm ca cc li t th N , E, Up.
(2.13)
Ta c th n gin cc phng trnh li t (2.12), (2.13) nh sau

11
CHNG III.H THNG TCH HP INS/GPS
3.1.L Thuyt
3.1.1 B lc Kalman
M hnh ca b lc Kalman bao gm 2 bc :
+ Bc d on : trng thi tip theo ca h thng c d bo
bi cc gi tr o trc
+ Bc cp nht : cc trng thi hin ti ca h thng c nh
gi bi cc s liu o c ti thi im .

Hnh 6. Thut ton Kalman c in.

Gi s b lc Kalman tuyn tnh nh sau:
(3.1)
Trong
xk l vector trng thi c n chiu.
Ak,k-1 l ma trn chuyn c kch thc (nxn);
Gk,k-1 l ma trn u vo c kch thc (nxr);
wk-1 l ma trn nhiu u vo c kch thc (rx1)
Vector o lng c dng :
(3.2)

12
Gi s xk- v xk+ ln lt l tin nghim v hu nghim c lng
ca vc t trng thi x ti thi im k.v bit ma trn hip
phng sai li xk th li c lng c xc nh nh sau:
ek = xk - xk(3.3)
Ma trn hip phng sai s l :
Pk = E(ek*ekT)
(3.4)
Gi tr cp nht trng thi xk c c lng :
xk+ = xk- + Kk*(zk Hk*xk-)
(3.5)
Vi Kk khuch i Kalman v c tnh theo cng thc sau:
(3.6)
Thay phng trnh (3.2) vo (3.5) ta c :
Pk = (I Kk.Hk).Pk-

Hnh 7. Lu tnh ton vi b lc Kalman

(3.7)

13
3.1.2 H thng tch hp INS/GPS
Khi tch hp 2 h thng INS vi GPS, chng ta c th s
dng 2 m hnh kt hp: phng thc lng (Hnh 8) v cht
(Hnh 9) v c m t di y [2].

Hnh 8. S INS/GPS theo phng thc vng h

Hnh 9. S INS/GPS theo phng thc vng kn

Trong lun vn ny, tc gi s dng m hnh vng h v
dng b lc Kalman c lng li INS. Li INS sau khi c
lng, c th b cho h thng INS theo 2 kiu Feedback (phn
hi, kn) hay Feedforward (h) (Hnh 10) [3].

14
Tn hiu l tng + nhiu INS

li ra c hiu chnh

INS
nhiu INS

_
GPS

nhiu GPS - nhiu INS

Kalman

Tn hiu l tng + nhiu GPS

(a)
INS

li ra c hiu chnh

Tn hiu l tng + nhiu INS

nhiu INS

_
GPS

nhiu GPS - nhiu INS

B lc
Kalman

Tn hiu l tng + nhiu GPS

(b)
Hnh 10. Cu trc theo kiu vng h (a) v vng kn (b)
3.2 xut ca hc vin
Da vo m hnh li INS mc (2.5), hc vin xut s dng 2
b lc Kalman song song (nh hnh 3.1) : b lc Kalman1 c
lng li v tr v vn tc, b lc Kalman 2 c tc dng hiu
chnh li Quaternion v phn hi li tnh ton cc thng s trong
thut ton SINS.

15
3.2.1.Lu thut ton
IMU
axb

wxb

ayb azb

w yb wzb

,g ,
XGPS(0)

Thut ton SINS

XGPS , VGPS

XINS , VINS

GPS
Gb_xy

XINS - XGPS
VINS - VGPS

Qout

Khi c lng
Lc

D on

dXINS dVINS
Chnh xc li v tr,
vn tc

X V

Hnh 11. Lu thut ton h thng tch hp INS/GPS.

16
3.2.2 B lc Kalman cho h thng tch hp INS/GPS
dE

dE+

NINS
Egps

dN

dN+
KF1
9 trng thi

EINS

VeINS
Vn

Gb_x
Gb_y

dVn
dVe

gps
gps

Ve

KF2
8 trng thi

VnINS

Up

dVe+
dVn+

H thng ra

Ngps
ae
an

dV_ned
Qout

Thut ton SINS

Hnh 12. B lc Kalman trong m phng

u ra ca b lc Kalman bao gm cc thng s sau: li v tr, li
vn tc, v li t th cng nh tri ca vn tc gc. Cc li v
tr v vn tc b cho cc li ra ca h thng.
Da vo cc phng trnh li trong mc 2.5, ta xy dng b lc
Kalman1 c vc t trng thi nh sau:
x1 =[E,N, VE, VN,N ,E ,Up ,Gbx, Gby];
v ma trn chuyn s l :

17

Ma trn o lng
H1 =

3.3.Kt qu m phng
Vi m hnh tch hp INS/GPS s dng b lc Kalman m ti
a ra, kt qu m phng h thng INS/GPS bao gm cc thng
tin v tr ca GPS v ca h INS/GPS; tc ca h thng GPS
v ca h INS/GPS; gc hng ca GPS v cc gc t th ca h
INS/GPS. Vi cch s dng 2 b lc Kalman song song nh m
hnh trc y nhng s trng thi trong KF1 tng ln vc t 9
trng thi, ng thi vc t o lng cng tng ln, do ch
lc Kalman chnh xc ln, c th.

(a)

(b)

18

(c)
Hnh 13.Qu o GPS (a), INS/GPS (b), khong cch d (c).

Hnh 14.Vn tc Vn ca h INS/GPS, Vn ca GPS v sai s tuyt

i ca vn tc.

19

Hnh 15. Vn tc Ve ca h INS/GPS, Ve ca GPS, v sai s

tuyt i.

Hnh 16. So snh gc hng INS/GPS v GPS.

20

Hnh 17. Cc gc lch E, N,Up.

Hnh 18. tri ca cm bin vn tc gc Gbx, Gby.

21

Hnh 19. Cc gc chc ( Pitch) v nghing (Roll).

22
KT LUN
Lun vn t c nhng kt qu nh sau:

Lun vn trnh by mt cch h thng cc khi nim,

nghin cu c bn v h thng GPS, INS.

Lun vn trnh by v h thng tch hp INS/GPS s

dng b lc Kalman trn c s cu hnh xut ca hc
vin.

Lun vn m phng kt qu h thng tch hp

INS/GPS vi cu hnh xut .
Nhng iu cn thiu st v phng hng pht trin ca
ti:

Mi ch a ra m hnh m phng trn Matlab, cha

c ng dng vo thc t.

Vic p dng m hnh b lc Kalman tuyn tnh chp

nhn sai s tuyn tnh ha m hnh h thng.

Mc d s trng thi ca b lc tng thm nhng s

lng vn cn hn ch c th c lng c nhiu
thng s khc nh trng thi v cc gc t th,
quaternion, h s t l gc, trng thi tri ca cm bin
gia tc vv