Nghiên cứu, thiết kế bộ lọc Kalman cải tiến

D Ạ I H Ọ C Q U Ố C G I A HÀ NỘI N g h iê n u, thiết kế lọc K a l m a n cải tiến Mã số: Ọ C 07.1 C h ủ n h iệ m dề tài: T h s T r ầ n Đ ứ c r ân ĐAI HOC QUỐC GIA HÀ NỘI TRUNG [AM IHQNG TIN THƯ VIỆN Nội - 2008 MỤC LỤC Báng giái thích chừ Viốt lãi Danh sách nhìrniỊ người ilium gia thực đe lái Danh mục bang sò li ệ u Danh mục hình Tóm tát kết nghiên cửu cuađềtài BÁOCẢỌ TỒNG K Ễ T 6.1 Dặt vấn đ ề 6.2 rống quan vấn đổ nuhicn cứu 6.3 Mục tiêu vá Nội dung Ntthiên cửu cua Đê lài .6 6.4 Dịa diêm, thời liian \ phương phápnghiên c ứu 25 6.5 Kết Cịuá nghiên c ứ u 25 6.6 Thau luận 26 6.7 Kết luận kiến n g h ị 27 6.8 Tài liệu tham khao 27 PHỤ l ụ c 28 Hãn g "iải thích c ác c h viết tắt IMl' (Inertial Measurement Unit) INS (Inertial Navigation System) GPS (Global Posiiioniiu* System) M IMS (MicroKleciroMechanieal System) Bộ dodạc quán tính I lệ tbốnụ dần đường quán tinh I lệ Ihổng dịnh vị toàn cầu i ỉộ vi điện Ur PC-box (Persional Computer box) SINS (Sirapdown INS) Máy tinh cá nhân chuyên dụ IU’ KF (Kalman Filter) I lệ dẫn dường loại gẳn chặt Bộ lọc Kalman Ni'D (North Mast Down) Các trục cúa hệ toạ dộ dần dường l ) a n h sá ch n h n g n g i th a m gia thực d c tài TT Họ tên Trần Dức Tân Học vị Thạc sỹ C o q u a n công lác Bô môn Vi diên tử ■ Vi h ộ*t h ố n sV Nhiệm vụ Chủ nhiệm dề tài Bộ môn Vi diện tư Nguyễn Thăng Thành viên Tiên sĩ Vi hệ ihống 1.oniỉ Bộ môn Vi điện tử Nguyen Phú Thuỳ Thành viên Tiến sĩ khoa học Vi hộ thống Bộ môn Vi cư diện tử í.ưu Mạnh 1iù Thành viên Cử nhân Vi hệ thống I>anh m ụ c biinj» số liệu Danh m ụ c c c hình ỉ linh 1i lộ irục loạ dộ dần dưứnự Huili2 Quỹ dạo bay cita cúc vệ tinh G PS () ỉ linh3 Cấu in k hệ thống UPS 10 ilinh4 Thuậl toán lợc Kalman 14 Hinh5 Sơ đồ thực ihuật toán Kalman 15 í linh6 Cấu trúc GPS/INS vịng mó' 16 I linh7 Cấu trúc GPS/1NS vòng kín 16 ỉ linh8 Cấu trúc tích hợp CìPS hỗ trợ 1NS .16 1linh Chương trình thicl kổ mị phongtrơn SIMUI INK/MATI , A B 20 11inh 10 1lộ INS/GPS dược kếi nối (QGTI) 05-09) 20 Hinh 11Giao diện dần đường quán tỉnh viết bàng VC-H- (mức thực th i) 2] Ị ỉìnli 12 So sánh lồi vị Irí: cấu hình phan hồi (a) cấu hình kết hựp (h )khi mấl GPS 22 1linh 13 Quỳ dạo nho ihực nghiệm 23 1linh 1-1 So sánh vận tốc theo hướng Dông vật thê chạy tlico quỳ dạo thực l ô 23 1linh 15 So sánh vận tốc theo hướng Bấc vậl the chạy theo quỹ dạo thựct ế 24 Hình 16 Góc ngiêng góc chúc vật the chav theo quỹ đạo ihực C 24 Hình ] So sánh quỹ dạo thu dưạc trcn hệ thông khácn h a u 25 Tóm till c ác kết qiiii ngh iên cứu c hí nh dề tài Két q vê khoa hoc (nhừnụ đỏng góp cùa dề tài cơng trình khoa học cơna hố) Trong thài gian thực dò tài ọ c 07-17 dề tài dà đạt dược nhữnsĩ kết qua chinh vè khoa học sau đív: r Nghiên cứu lọc ưu Kalman áp dụna vào lốn dần dường 'r Thiết ké, mơ phong mội số hộ lọc Kalman nhẩm kếl hợp 1NS GPS hiệu qua r Xây dựng phần mềm lọc Kalman xứ lý lín hiệu từ INS GPS theo thời gian tlụrc VC++ Các lòt dã cịng bố ba cơng trình khoa học sau 'I nn Due Tan Luu Manh 1la Nguyen 'Phang I ong Nguyên Phu Thuy, I luynh ! luu Tue, P erfo'm ance Im provem ent o f M E MS- B ased Sensor Applying in Inertial Navigation System , Rcseaxh Development and Application on Klectronics I dccommunicaiions and Inibmation 'Jcchnoloay No - 2007 Posts, Telematics & Information Technology Jour ml (ISSN 0866-7039) pp 19-24 T n n Dire Ian I.ưu Mạnh Hà Nguyền Thãng Long Nguyền Phú Thuỷ .Nàng Cao Chat LirợtỉỊ Hộ Thonọ, D an D ường Tích H ợp ỈN S/C P S S D ụng c ấ u Trúc Phan Hôi Vận Tốc I Liyci lập cơnạ trình I lội ntihị Cơ học toàn quốc lần thứ VIII I Nội 12/2007 tr 388-397 T IX Tail I M Ila, N I Long H M Tue, N p Thuy N ovel M EM S IN S/G PS Intvo'dtion Schem e Us ịMị Parallel Kalman Filters 2008 International Conference on Advaiccd 'Technologies for Communications ATC 2008 and RKV*08 (submitted May 2008 Kelt (Ua nhtic vu thirc tc (các sản phầm công nchệ khả năna áp dụnạ Ihực tế) Một )hần mồm ihừi lìian Ihực dược xây dựng phái triên máy tính chun dụntỊ Hệ llion* có ihc phục vụ tốt cho việc dinh vị vật the chuyên độna mộl số diêu kiện mât till hi;II từ GPS K c t .v u Y V, DCM ‘ V y» w (2 ) s in cos0 sin ụ sin^sin COSI// COS$ sin I// sill ệ sin0 sin ụ/ tCOSỘCOS/// sill ệcosO còs^sin^cosi//( sin^sin»/V COSỘsin sin I// sin^cosf// COSệ COS0 COS c o s I// dỏ V ứ i I//, IXWI ệ u ỏ c l e Tích phân V \ V | Vị) SC thu vị tri cửa vật thỏ hay bề mặt trái dất (hệ trục toạ độ dầm dường) Vĩ độ Ả kinh độ // độ cao 11 cua vật thể bay cỏ xác dịnh nhờ hệ phiương trình sau: Á • h R s i c m c u n h e d e s c r i b e d a s : 10 and k.k I A I \ ( / , (not in c lu d in g (5) UMII!’ i V e q u e i i ọ l-.rro rs i n t h e c i l ’ S a r c m o s i l v c a u s e d b y factors T h e mi cgn il ion co nf ig u tio n S clc ciisc A vailab ility error): W h e r e A n ,.) is a (n \ n ) tr a n s i t i o n m a m 's , ( i u inpu l m a trix , a n d \ \ k - l is ( r \ I is a n ( n \ r ) I ) in p u l n o ise W e ca n d e riv e d th e se m a trix b a se d o n ih e IN S e rro r e q u a tio n s • K p h cm cris J a la A n d Ih e m e a s u r e m e n t m o d e l : •Satellite cluck • M u llip aih rellev iio ii ( 6) t Ij • A tm o s p h e ric dclav • R a n d o m m e a s u re m e n t n o ise W h e r e /-k i s a ( m N I ) m e a s u r e m e n t V e c t o r , tlfc is 11 ( m s n ) • R e c e iv e r ( in c lu d e su ttw a r c ) design matrix, and V|, is (m X I ) measurement noise I h e r e a r c t h e o b j e c t p o s i t i o n a n i l v e l o c i t y (Jala m e a s u r e d b v I n t h e f i r s t b l o c k ( K l ' l ) a c o n v e n t i o n a l k u l m u n f i l l e r w i t h a s t a n d a l o n e G P S C o m p a r i n g t h e s e m e a s u r e m e n t s w ith th e dulu reduced system m e a s u r e d l\v [|)C I N S I h e p o s i t i o n e r r o r s a n d \ d o c i [ v e r r o r s c s i i m u l i o n K s t i m u i c d I N S v c l o c i l s e r r o r s a r c c o m p e n s a t e d in li r e o b t a i n e d ih c s\ slu m o u t p u t m odel is u l i l i / v d lor th e IN S v e k te m error I n I h o K I ? I h e e s t i m a t i o n o f I N S e r r o r s is, p e r f o r m e d in IV , N o viii M i;M S IN S /C il’ S I n I IXÌKA ( ION Sc Hi Mi: O ld e r lo i m p r o v e e s t i m a t i o n I h e a i m o f t h i s p a p e r i s 10 d e v e l o p o f a s p e c i f i c s c h e m e l o r I N S / C i l ’S i n t e g r a t i o n thill c u n he used sie.nal g e t s lo s t f r e q u e n t l y T h e tw o p arallel K a lm a n ill I h c c u s o t h a t C i l ’S in teg ratio n system based on i n t e r s is d e v e l o p e d a n d te s te d T h e first K a l m a n b l o c k i s a p p l i e d u> o b t a i n a I a s i c o n v e r g e n c e d u o 1« its s m a l l s ta le T h u s , i h e v e l o c i t i e s a n d p o s i t i o n o f th e v e h i c l e c a n b e q u ic k ly c o rre c te d T h e s e c o n d k l ik 'c o in m o d a ic a n d esiim aio th e h a v e th e uhilitv u tiiiu d c errors, (\|J sty le s w h ich co n sist aeeurao t»r a l i i lude rh e rc arc eight such errors ( r„ fC J vclocil\ e r r o r s ( t v s C \ r - C v i) l a n d d r i l l t e r m s ( cit.x- t i | „ Ci*,) I h e I N S e r r o r s a r c u s e d 10 c o r r c e l t h e e l e m e n t s III t h e t r a n s f o r m a t i o n m a trix ta k e n C j, a n d t h e q u a t e r n i o n I s i i m u l e d g v r o d r i l l s a r e a l s o in iu ac aiu n i in th e SIN S n av ig atio n sclicm c to F u rth erm o re, and 0 0 (J feedback schemes depending (III (ÌP S environments The INS It ỊI I) hx( /),(, /;vc , s erro r estim atio n Uni I) 0 (1 (1 u 0 0 0 ih c I N S t J I ’S system can schem e su iic li is s h o w n lienvccn ill l itt 1he t r a n s i t i o n i n u i r i s is : liviU nrvM ird I T h e IN S erro r e q u a tio n s arc u s e d a s a s y s te m m o d e l a n d ih c m e a s u re d in p u t /h j 0 d a t a l e d u> t h e i i l i c r a r e i h c d i I f a v i K e s b e t w e e n t h e I N S a n d 0 G P S |( » | W h e n O P S filter 0 w o r k s in p r e d i c t i o n m o d e a n J t h e I N S / G P S s v s i c m s w i t c h to 0 0 0 0 0 l l v t j l b i W ill'd s c h c m c 0 d a l a is n o t a v a i l a b l e , tile K a l m a n W h e r e D v it is [ h e v c l o o il v th e n a \ illation fram e 0 ( 11 h jl (1 in c tv m c m (n o rth , easl !>,/< (") « h jl in O m u l d i r e c t i o n o f d o w n ) , yơ i s one of p aram ete rs of th e am v k iliu n lu n e iio n and h\ here is (llM > s w a s u se d lor c u lib lio n a n d a lig n m e n t p u rp o s e s I’r o m t h e I N S W i is t a k e n e v e n In l lie fu s e III I ho ( i l ’S s iiH ia l b ln ck u tic p osiiio n ini.! is U ik cn \ LTV I s a ik ! I h e k l w a s ru n e v e n p ro s idl’d t)\ th e IN S until ( i l ’S sii'iuils a rc re a c q u ire d D uring u ccu racv s u c h p e r i o d s , n a v i g a t i o n e r r o r s iiKTiM.SC r u p i d l ) w i t h l i m e d u e in fo rm atio n w ith h ig h fre q u e n t} 10 (lie linic-dcponJcnl INS emu' behavior I ho reason is that each the q u atern io n arc u p d ated lo w COM V I I ' M S I.M l-s a m s i s i s III k>\\ quality sensors which provide kiruo emirs und noi.scs In ihusi.' cases, we so m etim e utiliz ed land \e h id c IN S e rro r aucuiiuiluliim m o tio n a t t r i b u t e s U) p r e v e n t I h e e q u a tio n d e r iv e d from b e h a v io r In We can O O I5625s, th e co n tin u e case see th e (iP S Iliat vchicIc mu\ I he update o o l 5(>.?5s t h e ( i l ’S I i p d u i c U i i s I lie p ro v id es l i a s ii ' a i i n n in b e t w e e n t i l ' s u p d a t i n g A t v clo cils lose u s U) UL'hicM’ h e l l e r l.v ir its p o sitio n , altitude sig n al and ihe IN S and ta il 10 c o m p u i c t h e p o s i t i o n d u r i n g t h e p e r i o d o f l o s t G I’S s ig n a l d r u m t h e 1 th u> t h e t h s o c o i i t l s ) l in a O l a i a n d \ e h i c l c v\ iIi e o m p c r i N j i c I l v C il ’ S ' s i i K a s u i v i n c m In p re s e n ts ih e this paper, u v mention 10 velticiiv and Iv k ’lH constraints c o m p a r e d u i i h i h o v a l u e s m e a s u r e d w i t h t h e i i l ’S u n i t H c a n I h e V f a n p r o v i d c t h e v i r t u a l m c u s u i v m c n i s 10 a i d i h c I M l I be W e can o n lj correct altitu d e e rro rs p rc d ic ic d citliiudc form s ilio In u U ilu d f o f Ihc IM U th e slate m a trix a pari o r th e w h o l e NNsieni u sin c ih c I h is c o r r o d e d Ill t a k e ih c in- imtiioi) Liligmncm ul' the na\ is.'aiion svsicm we use (he seen liajc cio rj u ajecio rs o ulpuli Ill ti si m > K l ; feedback blu ck can co n fig u ratio n not follow s w ith sm all e rro r fo r 100 se c o n d s I'ced h ae k ones O th erw ise, th e co m b in atio n of I n ( h i s a Mil h i n o i l s t r u c t u r e I h e f c e d b i i c k ■er ro rs b\ sto ch a stic n o ises d eterm ined sen io rs' Ihc th e d clcrm in isiic cu rd L ill\ arc alw ay s su a?v!islull> n o ise hy n o ises c a lih i'iU io n Ihe d il'llc u ll ti) I iv i l ch a e tc m iio anaiy/.iiiR the arc usualU process A llan bul We ol' [ho varian ce [he have ,V IK M S o l' Ihc iw ti c o n f i g u r a t i o n s c a n a i v c i h e b e s t r e s u l i s a s s h o w n i n I it’ h I h o Ci l’ S s i m u i l i s a v a i l a b l e a n d I h e l i v t l l i > n \ a r d K I sto ch a stic ( i l ’S Kl c o n lig u r a iio n call run g iv e th e h elle r Irajc clu rv e r r o r as th e a s i m t i ' i K i o m L ' i L T i n t o I li e a u i l u i i t ' C t > in p u u « l k > n s elim in ated th e I he feedforw ard h f i i d i n e lh > m C il'S o r th e c M c n u i l h e a d in g , m c a s i i r e m c i n s s u c h I here arc two kinds 1)1' noiso in ihc IN S: (Jcicrminislic and (if k l kl is u l i li / o c i w h e n is a p p l i e d w h e n ( Ì P S is ( U l l a g e I h o se lig iires s h o w i l i c p o s i i i u n 1)1 t h e v c h i c l e i i l o n g N o r t h a n d l i a s i d i r e c t i o n o n t h e I >1r il l i n s t e a d o f I h e t a t i i u d e a n d i h c lo n g itu d e 1h e reason is th uc can prcvuni n u m e rical in s ia h ili iic s in c a l c u l a t i n g ilic K a l m a n u a i n th e ospenmcM data Alter characterizing the IVII I errors, the i n l i t r m a i i o n o f t h e s e n o i s e s is a p p l i e d to ilic K I' b a s e d M l - M S I N S / l i l ’.s in iciỉiu iio n m odulo ill order to estim ate Ihc v e l o c i t i e s p o s i t i o n s a n d u l l i l u d f 1*1't h e o b j e c t [ | In t Jardvwarc lor th e p ro p os ed n av ig ati on sySUMÌI order [O p r o v e iho c I ' l l Ó C 111 of th e new schcm c a e x p e r i m e n t t r j j c c t t i r v is p e r f o r m e d in w h i c h C il'S s i g n a l w a s lost ill 10 seconds, l or i n s t a l l e d in a m ini v e h ic le veh icle and ihe t i p s ilic exp erim en t, |8 | I lie I M U is p l a c e d the system was is p l a c e d i n s i d e a D i s t a n c e in I h f North im) H illside ih c v e h ic le T h e IN S c o m p u l a t i o n s a n d i t s i i u e g n n i o n \s u h i l ic G P S a r c c a r r i c d o u t I is Dll i h c c o m m e r c i a l I’C b o x I l'i in IIk vase ol' c i PS UUU|!.’V Initiall> ih o v e h i c l e w a s a t rest, with ihe engine on lor aboui 3u SL'coiuls I his siaiionan data Comparison o f (he ("cedl’ack Cvitil'ianralum (ai.iml lilt' wmiltincd cine lo C iisih Ji.'UM'miiK' ihv.- lỊLiuliix ul ilK n;i\ itM iio n S Ỵ S te m t h e e x p e r i m e n t u i l h s h u n ir i c c t o i Ạ i s e x a m i n e d ( s e e I iu I) T h e v e h i c i c w a s d r i v e n l o r o n I) IS O s c u i i h I s I a' JO Sũ RI HI, ly! tu U: ‘ : AXf 11*1* f> Comparison ol N o rili F ie show s nc I ocun th e ro ll a n d p ilch o l'th e n av ig atio n M slcni c o m p u t e d a n d c o i T e e i c d bv t h e K l ' W e c a n n o i e x p c c i I h c K l to c o rre c t H >1 S lu m I ri p o l the e\|K*nincntul vehi cle I ig a n d p r e s e n t i h c \ c l u c i i i c s I i l ' k l - c o m p a r e d w i t h ih c v a l u e s m e a s u r e d w i t h l l i c C i P S u n i t Ii c a n b o s e e n o u t p u t s o r k r (dot c u rv c ) [o llo w s th e O P S s m a ll e rro r for a p p r o x i m a t c h th e u u itu d c niv en bv th e IN S p crleclh because a l t i t u d e is n o t a p a r t 1) 1' t h e m e a s u r e m e n t v e c t o r , i h e s c r e s u l t s sccn i to b e q u ite re a so n a b le for a svslcm w a s o n til cl l a n d vehiclc \e l o c i t i e s (so lid c u r v e ) w ith m in u iev I h e r e lire J im ita tio n s on choosing the path over which ihc u-h id c ran I he motion o l ' l h o v e h i c l e s h o u l d IHH h a \ e s u l i l c i c n i l v ịu % i .i H u l l s Hi! ỉ Vi ■ \ I V ; i ••• L1\ !»;; I V I yV Ni «\ i i ' i i • w ' i V ' Vv ; t \ J i l v u ’k ' 1' 11J U' u\ : \ ’ I , V \ »■' \ ' ' - i i i i c i •UV \ i > ::v \\ V > •'1 s:' j 1' s it'! ; ili ! » ! ’ i k.:lii •A ' I ' ' ' ' 1• : Li.■-•I I v - i : I M.'!: V • : \!U' • / Ỉ.:lt V / ''I \ i i ' Ị M:ỉ íỉi í ■ > ĩ y í’ ;L ■ • ; / c ì ĩ •• íí-ii íỉ ỉỉ :i i ! ì i h i ộ í i i -• : : • ,I•■ (» V N ỉilK ' ỉ í \ ị \ ‘ I ! 'i b i i ’ fi ~ ■ : ì •'! I ì'j \ ih ct i li'.i J !):ọt! h ,i ! \ \ íì\ \ ‘ ; k' • ih lii!: \ ; L ! '* 'í I ■ J \ ị r, ít' \ : \ r M - ! V !; I, » lii'i ! ' [ v i ' i i i i i i l".;ị t i e 11\\:\ s > d i i i l i\ V í iì - • V í *? ? : ! *■11:1 V - -•* '• iỉ ilỉh iỉlõl t: ’ • b ; >ú V ỉ'.-’ i:i>- iii Vl í Ĩ í! B i/li ỉ ì I ’ i i i } ì ỉ Lí í i i i i I I * H ' V { / i ỉ ỹ y ì I ^ í I V 'i J: \ ũ ( í í \ X H J ĩ J í ỉ' ; ' ỉ » ì' * í ■ì tí í ( í N V, í ii / \ V : i ị \ \ !! : iì tii' ! ;I I V >