ĐẠI HỌC QUỐC GIA HÀ NỘI
TRƯỜNG ĐẠI HỌC CÔNG NGHỆ VŨ VĂN NINH
NÂNG CAO CHẤT LƯỢNG HỆ THỐNG TÍCH
HỢP INS/GPS SỬ DỤNG BỘ LỌC KALMAN
NÂNG CAO CHẤT LƯỢNG HỆ THỐNG TÍCH
HỢP INS/GPS SỬ DỤNG BỘ LỌC KALMAN

Ngành: Công nghệ Điện tử - Viễn thông
Chuyên nghành : Kỹ thuật Điện tử
Mã số: 60 52 70 LUẬN VĂN THẠC SĨ

HƯỚNG DẪN KHOA HỌC: TS. TRẦN ĐỨC TÂN
HÀ NỘI – 2012
III

Nâng cao chng h thng tích hp INS/GPS s dng b lc Kalman



IV

Nâng cao chng h thng tích hp INS/GPS s dng b lc Kalman

DANH MỤC HÌNH
Hình 1. Qu o bay ca các v tinh GPS 1
Hình 2. Cu trúc ca IMU 4
Hình 3. H thnh v quán tính 5
Hình 4. 3 góc Euler 5
Hình 5. Sai lch gia h t p-frame và N- frame 11
Hình 6. Thut toán Kalman c n. 14
 tính toán vi b lc Kalman 16
 c vòng h 17
 c vòng kín 17
Hình 10. Cu trúc theo kiu vòng h (a) và vòng kín (b) 18
Hình 11. L thut toán h thng tích hp INS/GPS 19
Hình 12. B lc Kalman trong mô phng 20
Hình 13.Qu o GPS (a), INS/GPS (b), khong cách d (c). 24
Hình 14.Vn tc Vn ca h INS/GPS, Vn ca GPS và sai s i ca vn tc. 25
Hình 15. Vn tc Ve ca h INS/GPS, Ve ca GPS, và sai s tuyi. 26
Hình 16. ng INS/GPS và GPS. 27
Hình 17. Các góc lch

E
,


N
,
VI

Nâng cao chng h thng tích hp INS/GPS s dng b lc Kalman

THUT NG VIT TT

Viết tắt
Ti ếng Anh
Ti ếng Việt
IMU
Inertial Measurement Units
Kh 
GPS
Global Positioning System
H thnh v toàn cu
KF


VII

Nâng cao chng h thng tích hp INS/GPS s dng b lc Kalman LU

H th   n t - MEMS là s kt hp ca các yu t  nhit, sinh
hc, hóa hc, quang hc, cht lng,  n t trên cùng m  Silicon.  ng
da trên công ngh vi ch to. Công ngh MEMS ha hn mt cuc cách mng thay th
nhiu sn phm bng cách kt hp nhiu thành phn trên mt chíp. Công ngh MEMS
ngày càng phát trin, các thit b  chính xác cao, hiu sut cao,  tin cy
ln và giá thành h.
Mt trong nhng thit b MEMS c ng dng gv nh v ta
 vt th  Thit b này c  s dng trong c u khin Robot
  nh v t  c   n chuy ng.  i dùng d s dng,
thit b p vi thut toán dng quán tính (INS).
Hin nay, v  nh v thu hút s quan tâm ca nhiu nhà nghiên cu và công
nghip  c. Khi kt hp vi GPS, h thnh v kt hp INS/GPS có
nhi  m       c nh.  tích h c INS và GPS thì
ng s dng b lc t Kalman. Vic áp dng lc Kalman tuy
n, d thng h thc tiêu ca lu
là ci thin ch ng ca h thng tích hp INS/GPS s dng mt cu hình kt hp
INS/GPS phù hp.
Trong lu   c viên tp trung nghiên cu h thng tích hp INS/GPS vi
mc tiêu nâng cao ch ng ca toàn h th c bit là các thông s v 
th, vn tc, v trí.
Ni dung c tài bao g
-  H thnh v toàn cu GPS.



     V     
  
2  L1 = 1575,42MHz và L2 = 1227,60MHz [1,6]. Các
 
- Mã P: là mã chính xác, có tn s  dài toàn phn 267 ngày. Tuy vy
           dài 7 ngày và gn cho mi v tinh
trong h thng GPS m, c sau 1 tun li ti nên
khó b gi s dng nu bin c 2 sóng mang L1 và
L2.
- Mã C/A có tn s 1,023MHz, nó ch u bi      c s
dng cho m, mi v c gán 1 mã C/A riêng bit.
-  truyn lch v tinh mi nht, thông s ca lp khí quyn sóng
n t truyn qua, thi gian ca h thng, sai s ng h v tinh, phân b ca các
v tinh trên qu u bin c 2 sóng mang L1 và L2.
Khi hong, tn s L1 bao gm 2 tín hiu sc gi là mã P và mã C/A. Mã P
nhm bo v thông tin khi nhng s truy nhp trái phép. Tuy nhiên, ma
các tín hiu mã hóa là nhm tính toán thi gian cn thi thông tin truyn t v tinh ti
mt thit b thu nhn trên m   ng cách gic tính bng cách
nhân thi gian cn thi tín hii t ca ánh sáng là 300.000 km/giây.
Tuy nhiên, tín hiu có th b       u khí quyn. Vì vy, kèm
  p gi ti các thit b nhn, các v  ng gi kèm luôn thông tin v
qu o và thi gian. Vic s dng h nguyên t s m bo chính xác v s thng
nht thi gian gia các thit b thu và phát.
 bit v trí chính xác ca các v tinh, thit b thu GPS còn nhn thêm 2 loi d
liu mã hóa:
 D lic cp nhnh k và cho bit v trí ga các v tinh
trên qu o. Nó truy      trong b nh ca thit b thu
nhn khi các v tinh di chuyn quanh qu o.

ca B Quc phòng M, nhm chng li vi i th quân s dùng tín hiu GPS
chính xác cao. Chính ph M  ng vic này t      
   chính xác ca máy thu GPS dân s. (Tuy nhiên bin pháp này hoàn
toàn có th c s dng li trong nhu kin c thu này là tim
n hn ch an toàn cho d nh v dân s).
Trong nh có nhiu h thnh v v tinh trên th gi
thng GLONASS ca Liên bang Nga, h thng Galileo ca Liên minh Châu Âu. Xu
ng trong thi gian ti, thit b c ng dng trong rt nhic. Mc
tiêu nghiên cu phát trin h GPS giá r, nh gn, d s dng v c tin hành.
4

Nâng cao chng h thng tích hp INS/GPS s dng b lc Kalman H THNG INS
H thnh v quán tính phát trin t nh. Ti
mc công ngh tên lng thc nghim mt h
 u v INS trên th gii. c
  ng d   c công ngh tên l u tiên, tip   rng qua các
nghành hàng không dân dng và quân s, vn ti bi
2.1 Nguyên lý hoạt động của INS
Nguyên lý hong ca INS là tng hp các tín hic bi mu 
         nh trng thái hin ti ca h
thng. Các tín hi  c bi IMU bao gm vn tc góc (gyros) và gia tc
(accelerometer). Cu to IMU bao gm 3 cm bin gia tc và 3 cm bin vn tc góc. 
 t h thng INS bao gm IMU và thu    c tích hp cùng. Thut
toán s dng trong lu t toán SINS ca Salychev (Ph lc 2). 
các thông s  ), vn tc, các góc 
 ng) (xem Hình 3).
Hình 2 dmô t mt khi IMU [2]. Vi nhng IMU s dng cm

Attitude
Velocity
Position

Hình 3. H thnh v quán tính
2.2 Ma trận chuyển hệ tọa độ
Trong thut toán     các thông s cui cùng, chúng ta cn các phép
tính toán chuyi qua các h t vi nhau. t s ma trn
chuyi chính trong khi s dng thut toán SINS.
2.2.1 Ma trn chuyn Euler
Mt v  quan trnh vt th trong không gian 3 chi
ca vt trong h t  Các hoc là 3 góc Euler [5].

Hình 4. 3 góc Euler
6

Nâng cao chng h thng tích hp INS/GPS s dng b lc Kalman

   (, , ) (Hình 4)          
             
. 
   .













 












(2.3)
  R(Z,

    

) l t là các ma trn góc xoay quanh trc x, y, z
ng , , .
n xoay tng quát trong không gian 3 chi
cos sin 0 cos 0 sin 1 0 0
sin cos 0 0 1 0 0 cos sin
0 0 1 sin 0 cos 0 sin cos
xyz
R












T























Trong công thc (2.6) trên,  là góc quay (giá tr tính bng Radian) và cos (


), cos (


),
cos (


) là các   ng ca trc.    n quay ca chuy ng quay
trong không gian 3 chinh theo (2.7) 










 

















































(2.8)
T  c: tan = a
32
/a
33
, sin = -a
31
, tan = a
21
/a
11
. y







(2.9)

 

 (2.10)








 



 






 




 








= 














= 















 













 



 



)
sign(

) = sign(

-

)
sign(

) = sign(

-

)
2.2.5 Tính góc Euler t Quaternion
T   (2.7






























(2.14)
9

Nâng cao chng h thng tích hp INS/GPS s dng b lc Kalman

2.3 Phương trình chuyển động
 ca vt th nh bi 3 góc Euler [4,6]. Mi liên
h gia các vn tc góc ca Roll, Pitch, Yaw (p, q, r) và 3 góc Euler , ,  c tính
theo công thc (2.15) sau:






(2.15)
   c các góc Euler.
Các gia tc (a
x
, a
y
, a
z
) dc theo các trc ca h t vt th liên h vi 3 vn tc
(U, V, W) trong h t  











 



(2.16)







(2.17)
 :  5)
  c v trí ca vt. Tip theo chúng ta có th
,    cao ca vt thông qua các  (2.18) sau :









 






(2.18)





10


phân.
i vi các góc lch nh 
N
, 
E
, 
Up
[7,9] là  lch gia h t p-frame và ll-
frame (xem Hình 5) [8].
11

Nâng cao chng h thng tích hp INS/GPS s dng b lc Kalman




Up


East
North
Up
Zp
Xp
Yp

Hình 5. Sai lch gia h t p-frame và N- frame
Ma trn chuyn gia 2 h t này có th c mô t theo (2.19), c suy ra t
dng ma trn DCM chun, và gi s các góc này nh tha mãn cos 
[7,9].




































(2.21)












12

Nâng cao chng h thng tích hp INS/GPS s dng b lc Kalman

Li tính toán vn tc góc có th  c bng cách vi phân các biu thc theo
hình chiu trên h t ll-frame.
















là vn tc và các lng trên h t ll-frame.
,   và li  .
U : t quay ct.
  (2.21) và (2.22) vào (2.20) c :
























  


 





 (2.25)
  các gia tc biu di


















(2.26)
  
E
, 
N
, 
up
là các h s t l gia tc.

B
N,
B
E
, B
Up
 lch 0 ca gia tc.








































c Coriolis vào trong tính toán lc :
13



 











 (2.27)
















 (2.28)
S khác nhau gia vn tc góc trong h t p-frame và ll-frane là nguyên nhân
 trôi gyro và các li tính toán các vn tc góc chuyn t ll-frame sang E-
frame.  i v trí có th c t  sau:

  
















Ga s h = 0 thì












































  

 14

Nâng cao chng h thng tích hp INS/GPS s dng b lc Kalman

. H THNG TÍCH HP INS/GPS
Khi s dng riêng r tng thit b INS, GPS thì mi thit b u bc l m,
m riêng và khó khc phm ca nó. S tích hp ca GPS vi mt h
th nh v quán tính INS ci thin ch ng và khuy m ca tng h thng: S
d      lch không (bias) cho các thit b quán tính, và INS có th
c s d ci thinh v và thu thp d liu ca máy thu GPS. Khi kt hp INS
và GPS và s dng b lc Kalman, chúng ta có th   c c li ca INS và
GPS. i s dng bit thông tin v trí t.
3.1 Lý Thuyết
3.1.1 B lc Kalman

k
.

A
k,k-1
là ma trn chuy c (nxn);
G
k,k-1
là ma tru vào c (nxr);
w
k-1
là ma trn nhi c (rx1)
Vector ng có dng :








(3.2)
 z
k
ng, c (mx1); H
k
là ma tr, c (mxn)
w và v có hàm phân b theo hàm Gauss vi trung bình bng 0 và ma trn hi
(covariance) lt là Q và R.
w~N(0,Q);

k
ng :
x
k
+
= x
k
-
+ K
k
*(z
k
 H
k
*x
k
-
) (3.5)
Vi K
k
 khui Kalman  c tính theo công thc sau:










Do h th nh v làm vic  ch  thi gian thc nên chúng ta chuy
trên v dng ri rc theo thi gian:










(3.9)
 
k
là ma trn chuyn, và có dng




 

 (3.10)
Và ma trn hing sai liên quan ti w
k
s là :




































Hình 7.  tính toán vi b lc Kalman
17

Nâng cao chng h thng tích hp INS/GPS s dng b lc Kalman

3.1.2 H thng tích hp INS/GPS
Khi tích hp 2 h thng INS vi GPS, chúng ta có th s dng 2 mô hình kt hp:
 c lng (Hình 8) và cht (Hình 9) c mô t [2].

Hình 8.  c vòng h

Hình 9.  INS/GPS theo c vòng kín
Trong lu     s dng mô hình vòng h và dùng b l  
 ng li INS. L     ng, có th bù cho h thng INS theo 2 kiu
Feedback (phn hi, kín) hay Feedforward (h) (Hình 10) [3].
INS
GPS
Kalman
Tín hiệu lí tưởng
+ nhiễu INS
Tín hiệu lí tưởng
+ nhiễu GPS
nhiễu INS
+
_
nhiễu GPS - nhiễu INS
lối ra được hiệu chỉnh


E
,

N
,


Up
, và  trôi c
(G
bx
, G
by
). Các li v trí và vn tc này bù cho h thng INS  t qu v trí và
vn tc chính xác .
3.2.1  thut toán h thng tích hp INS/GPS