Tài liệu ứng dụng bộ lọc kalman trong hệ thống dẫn đường điện tử

Bé gi¸o dôc vµ ®µo t¹o Tr-êng ®¹i häc b¸ch khoa hµ néi --------------------------------------------- TrÇn Thä Tu©n øng dông bé läc Kalman trong hÖ thèng dÉn ®-êng ®iÖn tö LuËn v¨n th¹c sÜ : ĐiÖn tö viÔn th«ng Hµ néi - 2004 Bé gi¸o dôc vµ ®µo t¹o Tr-êng ®¹i häc b¸ch khoa hµ néi -------------------------------------------------------------------------------------------- TrÇn Thä Tu©n øng dông bé läc Kalman trong hÖ thèng dÉn ®-êng ®iÖn tö Chuyªn ngµnh : §iÖn tö LuËn v¨n th¹c sÜ : ĐiÖn tö viÔn th«ng Ng-êi h-íng dÉn khoa häc: TS. NguyÔn Quèc Trung Hµ néi - 2004 I C¸c Ký hiÖu Ký hiÖu ý nghÜa x Vi ph©n theo thêi gian cña tham sè vÞ trÝ x. Trong trêng hîp ë ®©y cã ®¬n vÞ lµ [m/s] x Vi ph©n cÊp hai theo thêi gian cña tham sè vÞ trÝ x. §¬n vÞ lµ [m/s2] F TuyÕn tÝnh ho¸ ph¬ng tr×nh hÖ thèng cho bëi F aN Tham sè gia tèc theo h-íng B¾c trong hÖ to¹ ®é dÉn ®êng [m/s2] aE Tham sè gia tèc theo híng §«ng trong hÖ to¹ ®é dÉn ®êng [m/s2] aD Tham sè gia tèc theo híng xuèng díi trong hÖ to¹ ®é dÉn ®êng [m/s2] xk PhÇn tö thø k cña mét chuçi, x=[x1,x2,...xn]T bi BiÓu thÞ ®é sai lÖch c0 VËn tèc truyÒn ¸nh s¸ng 299792458[m/s] cij BiÓu thÞ phÇn tö cña ma trËn C. i,j=1-3. Cbn Ma trËn chuyÓn ®æi tõ hÖ to¹ ®é dÉn ®-êng sang hÖ to¹ ®é khung Cnb Ma trËn chuyÓn ®æi tõ hÖ to¹ ®é khung sang hÖ to¹ ®é dÉn ®-êng Di §é dÞch tÇn Doppler øng víi vÖ tinh thø i E=[e0,e1,e2,e3 C¸c tham sè biÓu thÞ theo Quaternion ] f Ma trËn hÖ thèng trong miÒn thêi gian liªn tôc, x =fx ge Vector gia tèc träng tr-êng [m/s2] Iixi Ma trËn ®¬n vÞ kÝch th-íc ixi K Ma trËn hÖ sè khuÕch ®¹i Kalman L Vector nh×n th¼ng tõ vÞ trÝ a ®Õn vÞ trÝ b, l=(b-a)/|b-a| L1,L2 TÇn sè lµm viÖc cña hÖ thèng GPS, L1=1575,42[Mhz], L2=1227,60[Mhz]. 0ixj Ma trËn 0 víi i hµng vµ j cét p VËn tèc quay ®o ®-îc xung quanh trôc x cña hÖ to¹ ®é khung [rad/s] q VËn tèc quay ®o ®-îc xung quanh trôc y cña hÖ to¹ ®é khung [rad/s] r VËn tèc quay ®o ®-îc xung quanh trôc z cña hÖ to¹ ®é khung [rad/s] II P Ma trËn hiÖp biÕn tr¹ng th¸i Q Ma trËn hiÖp biÕn xö lý R Ma trËn hiÖp biÕn phÐp ®o Re B¸n kÝnh xÝch ®¹o cña tr¸i ®Êt, 6379137[m] t BiÕn biÓu thÞ thêi gian [s] v BiÕn biÓu thÞ vËn tèc [m/s] vD Tham sè vËn tèc theo ph-¬ng th¼ng ®øng trong hÖ to¹ ®é dÉn ®-êng [m/s] vE Tham sè biÓu thÞ vËn tèc theo h-íng §«ng trong hÖ to¹ ®é dÉn ®-êng [m/s] vN Tham sè biÓu thÞ vËn tèc theo h-íng B¾c trong hÖ to¹ ®é dÉn ®-êng [m/s] z Vector ®o l-êng  Gia tèc gãc,  =    [rad/s2] (u-v) Hµm xung Delta rêi r¹c, (0)=1,(n)=0 n0. ion Thêi gian trÔ trong tÇng ®iÖn ly [s] trop Thêi gian trÔ trong tÇng ®èi l-u [s] u Sai sè ph©n do sai lÖch cña ®ång hå thiÕt bÞ thu[s] u §é dÞch cña ®ång hå thiÕt bÞ thu [s/s] i Sai sè vÒ kho¶ng c¸ch gi¶ ®Õn vÖ tinh thø i [m]   i Sai sè vÒ tèc ®é biÕn ®æi kho¶ng c¸ch gi¶ ®Õn vÖ tinh thø i [m/s] t Thêi gian gi÷a c¸c mÉu [s]  Kinh ®é [rad]  B-íc sãng [m]  VËn tèc gãc, ë ®©y e VËn tèc quay cña tr¸i ®Êt, 7,2921151467E-5[rad/s]  VÜ ®é [rad]  Gãc nghiªng c¸nh [rad]  VËn tèc thay ®æi gãc nghiªng c¸nh, theo c¸ch biÓu thÞ gãc Euler [rad/s]  Ma trËn chuyÓn ®æi tr¹ng th¸i rêi r¹c theo thêi gian c Ma trËn chuyÓn ®æi tr¹ng th¸i rêi r¹c theo thêi gian cña tham sè tr¹ng th¸i ®ång hå   [,, ] [rad/s] GPS.  Gãc h-íng, theo ph-¬ng chÝnh B¾c th× b»ng 0, [rad/s] III  VËn tèc thay ®æi gãc h-íng, [rad/s]  Kho¶ng c¸ch gi¶ [m]  VËn tèc thay ®æi kho¶ng c¸ch gi¶ [m/s]  §é lÖch chuÈn  Gãc ngÈng [rad]  VËn tèc thay ®æi gãc ngÈng [rad/s] IV C¸c tõ viÕt t¾t Tõ viÕt t¾t ý nghÜa AAV Ph-¬ng tiÖn hµng kh«ng vËn hµnh tù ®éng (Autonomous Aerial Vehicle) ADC Bé chuyÓn ®æi t-¬ng tù - sè (Analog to Digital Converter) C/A M· C/A (Course/Acquisition) DAC Bé chuyÓn ®æi sè - t-¬ng tù (Digital to Analog Converter) DGPS HÖ thèng ®Þnh vÞ vÖ tinh vi sai (Differential GPS) DOP Tham sè suy gi¶m ®é chÝnh x¸c (Dilution of Precision) ECEF HÖ to¹ ®é g¾n víi tr¸i ®Êt (Earth Centered Earth Fixed Frame) ECI HÖ to¹ ®é qu¸n tÝnh t©m tr¸i ®Êt (Earth Centered Inertial Frame) EKF Bé läc Kalman më réng (Extended Kalman Filter) GDOP Tham sè suy gi¶m ®é chÝnh x¸c vÒ kho¶ng c¸ch h×nh häc (Geometric DOP) GPS HÖ thèng ®Þnh vÞ toµn cÇu (Global Positioning System) IMU Khèi ®o qu¸n tÝnh (Inertial Measuring Unit) INS HÖ thèng dÉn ®-êng qu¸n tÝnh (Inertial Navigation System) KF Bé läc Kalman (Kalman Filter) PDOP Tham sè suy gi¶m ®é chÝnh x¸c vÒ vÞ trÝ (Position DOP) PPS DÞch vô dÉn ®-êng chÝnh x¸c (Precise Positioning Service) PSD MËt ®é phæ c«ng suÊt (Power Spectrum Density) RMS Gi¸ trÞ trung b×nh b×nh ph-¬ng (Root Mean Square) SA ChÕ ®é lµm viÖc cã sai sè chñ ®Þnh cña hÖ thèng GPS (Selective Availability) SPS DÞch vô dÉn ®-êng chuÈn (Standard Positioning Service) TAS VËn tèc giã (True Air Speed) TDOP Tham sè suy gi¶m ®é chÝnh x¸c vÒ thêi gian (Time DOP) UAV M¸y bay kh«ng ng-êi l¸i (Unmanned Aerial Vehicle) VDOP Tham sè suy gi¶m ®é chÝnh x¸c theo ph-¬ng ®øng (Vertical DOP) WGS-84 HÖ to¹ ®é tr¾c ®Þa toµn cÇu ra ®êi n¨m 1984 ( World Geodetic System 1984) ZOH Gi÷ mÉu kh«ng (Zero and Hold) V Danh môc c¸c b¶ng B¶ng 4.1. C¸c tham sè chuyÓn ®æi 55 B¶ng 4.2. C¸c tham sè tÝnh to¸n vÞ trÝ cña thiÕt bÞ thu 58 B¶ng 5.1. C¸c ph-¬ng tr×nh cËp nhËt thêi gian cña bé läc Kalman rêi r¹c 75 B¶ng 5.2. Ph-¬ng tr×nh cËp nhËt kÕt qu¶ ®o cña bé läc Kalman rêi r¹c 75 B¶ng 5.3. Ph-¬ng tr×nh cËp nhËt thêi gian cña bé läc Kalman EKF 81 B¶ng 5.4. Ph-¬ng tr×nh cËp nhËt phÐp ®o cña bé läc Kalman EKF 81 VI Danh môc c¸c h×nh H×nh 2.1. HÖ to¹ ®é khung B (Body Frame) 6 H×nh 2.2. HÖ to¹ ®é vïng NED 6 H×nh 2.3. HÖ to¹ ®é ECI vµ ECEF 7 H×nh 2.4. Trôc quay cña ph-¬ng tiÖn 9 H×nh 2.5. H×nh vÏ biÓu thÞ a) gãc nghiªng c¸nh, b) gãc ngÈng vµ c) gãc h-íng 10 H×nh 2.6. Gãc nghiªng c¸nh 11 H×nh 2.7. Gãc ngÈng (Pitch) 11 H×nh 2.8. Gãc h-íng mòi (Yaw) 12 H×nh 2.9. H-íng chuyÓn ®éng thùc cña hÖ thèng 13 H×nh 2.10. Gãc lÖch gi÷a h-íng chuyÓn ®éng vµ h-íng mòi hÖ thèng() 14 H×nh 2.11. C¸c vector hÖ to¹ ®é tÝch chÐo víi [p,q,r]T 15 H×nh 2.12. Gi¶n ®å khèi biÓu diÔn qu¸ tr×nh tÝnh ma trËn Cbn 16 H×nh 2.13. S¬ ®å khèi biÓu thÞ qu¸ tr×nh biÓu diÔn gãc Euler 20 H×nh 2.14. S¬ ®å khèi biÓu thÞ qu¸ tr×nh biÓu diÔn Quaternion 23 H×nh 2.15. Vector t¹o ra khi quay vector quanh trôc q 24 H×nh 3.1. M« h×nh lµm viÖc cña c¶m biÕn gia tèc 31 H×nh 3.2. M« h×nh lµm viÖc cña c¶m biÕn quay 32 H×nh 3.3. VÞ trÝ l¾p ®Æt khèi IMU trªn hÖ thèng 35 H×nh 3.4. CÊu tróc vÞ trÝ bé c¶m biÕn Tetrad 36 H×nh 3.5. CÊu tróc phÇn cøng bé c¶m biÕn Tetrad 36 H×nh 3.6. CÊu tróc a)Strapdown vµ b)Gimbal 38 H×nh 3.7. M« h×nh xö lý tÝn hiÖu trong hÖ thèng Gimbal 39 H×nh 3.8. M« h×nh xö lý tÝn hiÖu trong hÖ thèng Strapdown 40 H×nh 3.9. M« h×nh tÝnh to¸n trong hÖ thèng INS 42 H×nh 3.10. Vector gia tèc h-íng tÊm 43 VII H×nh 4.1. QuÜ ®¹o cña c¸c vÖ tinh trong hÖ thèng GPS 46 H×nh 4.2. Nguyªn t¾c x¸c ®Þnh vÞ trÝ cña thiÕt bÞ thu 47 H×nh 4.3. VÞ trÝ cña thiÕt bÞ thu trong tr-êng hîp kh«ng lý t-ëng 48 H×nh 4.4. M« h×nh truyÒn sãng cña tÝn hiÖu GPS 49 H×nh 4.5. ¶nh h-ëng cña hiÖn t-îng truyÒn nhiÒu ®-êng lªn hµm t-¬ng quan m· C/A 51 H×nh 4.6. M« h×nh biÓu thÞ sai sè cña ®ång hå th¹ch anh 54 H×nh 4.5. ThiÕt bÞ thu GPS nhËn tÝn hiÖu tõ bèn vÖ tinh kh¸c nhau 58 H×nh 4.6. Sù ¶nh h-ëng cña hiÖu øng Doppler 62 H×nh 4.7. Kho¶ng c¸ch gi¶ vµ vËn tèc kho¶ng c¸ch gi¶ 62 H×nh 4.8. VÞ trÝ Anten trªn m¸y bay 63 H×nh 5.1. Chu tr×nh lµm viÖc cña bé läc Kalman rêi r¹c 75 H×nh 5.2. BiÓu ®å gi¶i thuËt lµm viÖc cña bé läc Kalman 76 H×nh 5.3. S¬ ®å m« t¶ qu¸ tr×nh lµm viÖc cña bé läc Kalman 76 H×nh 5.4. Nguyªn lý lµm viÖc cña bé läc Kalman më réng 82 H×nh 5.5. L-u ®å thuËt to¸n lµm viÖc cña bé läc Kalman më réng EKF 83 H×nh 6.1. M« h×nh cÊu tróc cã håi tiÕp 85 H×nh 6.2. M« h×nh cÊu tróc kh«ng cã håi tiÕp 86 H×nh 6.3. S¬ ®å khèi cÊu tróc hÖ thèng GPS ghÐp trªn ph-¬ng tiÖn mÆt ®Êt 87 H×nh 6.4. CÊu tróc ghÐp láng GPS/INS 89 H×nh 6.5. S¬ ®å cÊu tróc hÖ thèng GPS/INS ghÐp chÆt 91 H×nh 7.1. M« h×nh m« pháng mèi quan hÖ gi÷a gãc Euler vµ Quaternion 93 H×nh 7.2. KÕt qu¶ m« pháng 94 H×nh 7.3. KÕt qu¶ m« pháng thuËt to¸n läc Kalman ®èi víi h»ng sè, R=0.1 95 H×nh 7.4. KÕt qu¶ m« pháng thuËt to¸n läc Kalman ®èi víi h»ng sè, R=5 96 H×nh 7.5. KÕt qu¶ m« pháng thuËt to¸n läc Kalman ®èi víi h»ng sè, R=0.001 97 H×nh 7.6. KÕt qu¶ m« pháng thuËt to¸n läc Kalman ®èi víi tham sè biÕn ®æi, R=0.1 98 H×nh 7.7. KÕt qu¶ m« pháng thuËt to¸n läc Kalman víi tham sè biÕn ®æi, R=5 99 H×nh 7.8. KÕt qu¶ m« pháng thuËt to¸n läc Kalman víi tham sè biÕn ®æi, R=0.001 99 H×nh 7.9. M« h×nh -íc l-îng kho¶ng c¸ch sö dông bé läc EKF 101 H×nh 7.10. QuÜ ®¹o chuyÓn ®éng thùc tÕ vµ -íc l-îng cña ph-¬ng tiÖn 102 H×nh 7.11. Sai sè -íc l-îng kho¶ng c¸ch 103 VIII H×nh 7.12. VÞ trÝ thùc tÕ, vÞ trÝ ®o vµ vÞ trÝ -íc l-îng 104 1 Ch-¬ng 1 - giíi thiÖu DÉn ®-êng ®· xuÊt hiÖn tõ rÊt l©u d-íi c¸c d¹ng h×nh th¸i kh¸c nhau. Loµi chim, loµi ong vµ hÇu nh- tÊt c¶ mäi loµi ®éng vËt kh¸c trong tù nhiªn ®Òu cã kh¶ n¨ng dÉn ®-êng bÈm sinh. Víi con ng-êi, h×nh th¸i dÉn ®-êng s¬ khai nhÊt lµ dÉn ®-êng thiªn v¨n, tøc lµ dùa vµo vÞ trÝ cña mÆt trêi vµ c¸c v× sao ®Ó x¸c ®Þnh ra ph-¬ng h-íng. Tr¶i qua hµng triÖu n¨m cïng víi sù ph¸t triÓn cña khoa häc kü thuËt con ng-êi ®· ph¸t minh ra c¸c ph-¬ng thøc dÉn ®-êng míi, víi ®é chÝnh x¸c vµ tÝnh s½n sµng cao h¬n rÊt nhiÒu. HiÖn nay tån t¹i rÊt nhiÒu c¸c ph-¬ng thøc dÉn ®-êng kh¸c nhau nh- ®Þnh vÞ b»ng n¨ng l-îng nhiÖt, quang, sãng siªu ©m, sãng ®iÖn tõ... Tuy nhiªn, -u ®iÓm h¬n h¼n vÉn lµ ph-íng ph¸p dÉn ®-êng ®iÖn tö. Ph-¬ng ph¸p dÉn ®-êng ®iÖn tö dùa trªn c¬ së cña c¸c hÖ thèng tÝnh to¸n, ®iÒu khiÓn kÕt hîp víi c¸c thiÕt bÞ thu ph¸t sãng ®iÖn tõ ®Ó thùc hiÖn qu¸ tr×nh x¸c ®Þnh vÞ trÝ, ®iÒu khiÓn vµ dÉn ®-êng cho hÖ thèng chuyÓn ®éng ®óng quÜ ®¹o. Trªn thùc tÕ, b¶n th©n hÖ thèng dÉn ®-êng ®iÖn tö còng cã rÊt nhiÒu c¸c hÖ thèng kh¸c nhau. Do ®Æc thï riªng cña mçi hÖ thèng nh- : kü thuËt sö dông trong hÖ thèng, ®é chÝnh x¸c, tÝnh s½n sµng vµ kh¶ n¨ng ¸p dông vµo thùc tÕ nªn trong khu«n khæ cña luËn v¨n t¸c gi¶ chØ ®Ò cËp ®Õn hai hÖ thèng hiÖn nay ®ang ®-îc c¸c nhµ khoa häc trªn thÕ giíi quan t©m vµ ph¸t triÓn nhiÒu nhÊt. Thø nhÊt, ®ã lµ hÖ thèng dÉn ®-êng qu¸n tÝnh INS (Inertial Navigation System). HÖ thèng dÉn ®-êng qu¸n tÝnh lµm viÖc dùa trªn c¬ së cña c¸c ®Þnh luËt vÒ chuyÓn ®éng cña Newton. B»ng c¸c bé c¶m biÕn gia tèc hÖ thèng INS cã thÓ tù x¸c ®Þnh ®-îc tham sè gia tèc chuyÓn ®éng cña hÖ thèng, tõ ®ã tÝnh to¸n ra c¸c tham sè chuyÓn ®éng kh¸c nh- vËn tèc, vÞ trÝ, ®é cao. -u ®iÓm cña hÖ thèng INS ®ã chÝnh lµ tÝnh bÝ mËt cao, cã thÓ lµm viÖc trong m«i tr-êng kh¾c nghiÖt (kh«ng thÓ hoÆc khã thùc hiÖn viÖc truyÒn sãng ®iÖn tõ). V× vËy hÖ thèng INS th-êng ®-îc sö dông trong c¸c hÖ thèng dÉn ®-êng cña qu©n sù, hÖ thèng dÉn ®-êng vò trô hay hÖ thèng dÉn ®-êng cho tµu ngÇm. 2 Thø hai, ®ã lµ hÖ thèng dÉn ®-êng vÖ tinh GPS. Trªn thÕ giíi tån t¹i rÊt nhiÒu hÖ thèng dÉn ®-êng vÖ tinh kh¸c nhau nh-ng hÖ thèng GPS næi bËt h¬n c¶ bëi tÝnh phæ biÕn vµ ®é chÝnh x¸c cña nã. HÖ thèng GPS lµm viÖc dùa trªn c¬ së thu nhËn tÝn hiÖu ph¸t tõ c¸c vÖ tinh chuyÓn ®éng trªn quÜ ®¹o tham chiÕu chuÈn. Trªn c¬ së c¸c tÝn hiÖu thu nhËn ®-îc nµy thiÕt bÞ thu GPS sÏ tÝnh to¸n ra kho¶ng c¸ch ®Õn c¸c vÖ tinh tham chiÕu råi tõ ®ã x¸c ®Þnh ra vÞ trÝ cña m×nh. Víi ®Æc tÝnh chÝnh x¸c vµ tÝnh s½n sµng cao nªn hÖ thèng GPS ®-îc dïng trong rÊt nhiÒu c¸c hÖ thèng dÉn ®-êng kh¸c nhau nh- giao th«ng ®-êng bé, hµng h¶i vµ ®Æc biÖt lµ trong lÜnh vùc hµng kh«ng. Hai hÖ thèng dÉn ®-êng ®iÖn tö ®Ò cËp ë trªn cã rÊt nhiÒu c¸c -u ®iÓm nh-ng vÉn tån t¹i nh÷ng nh-îc ®iÓm kh¸c nhau. VÝ dô nh-, hÖ thèng GPS yªu cÇu ph¶i lµm viÖc trong ®iÒu kiÖn thiÕt bÞ thu GPS vµ c¸c vÖ tinh n»m trong tÇm nh×n th¼ng. Sè l-îng vÖ tinh quan s¸t cña thiÕt bÞ thu t¹i mäi thêi ®iÓm tèi thiÓu lµ 4 vÖ tinh. Trong khi ®ã hÖ thèng INS lµm viÖc dùa trªn c¬ së th«ng tin ®-a ®Õn tõ c¸c bé c¶m biÕn, do ®ã yªu cÇu vÒ c¸c bé c¶m biÕn ph¶i cã ®é chÝnh x¸c cao. Ngoµi ra, sai sè cña hÖ thèng INS cã tÝnh chÊt tÝch luü theo thêi gian, v× thÕ gi¸ thµnh cña mét hÖ thèng INS cã chÊt l-îng võa ph¶i còng lµ rÊt cao. §Ó gi¶m nh÷ng yªu cÇu lµm viÖc hÕt søc kh¾t khe ®èi víi mçi hÖ thèng dÉn ®-êng, gi¶m chi phÝ cho mçi hÖ thèng dÉn ®-êng nh-ng vÉn ®¶m b¶o ®-îc tÝnh chÝnh x¸c cña hÖ thèng ng-êi ta ®· ®-a ra m« h×nh hÖ thèng ghÐp. HÖ thèng ghÐp ®-îc cÊu t¹o tõ hai hÖ thèng riªng lÎ GPS, INS trªn c¬ së thuËt to¸n cña bé läc Kalman. Tuú thuéc vµo yªu cÇu cña tõng hÖ thèng dÉn ®-êng kh¸c nhau mµ ta sö dông c¸c cÊu tróc ghÐp kh¸c nhau. Víi sù kÕt hîp nµy, c¸c nh-îc ®iÓm cña mçi hÖ thèng hÇu nh- ®· ®-îc kh¾c phôc hoµn toµn, do ®ã tÝnh n¨ng cña hÖ thèng dÉn ®-êng ®-îc c¶i thiÖn râ rÖt. Néi dung cña luËn v¨n nµy ®Ò cËp ®Õn nh÷ng vÊn ®Ò xoay quanh hÖ thèng ghÐp GPS vµ INS víi sù trî gióp cña bé läc Kalman. Trong luËn v¨n t¸c gi¶ tr×nh bµy vÒ nguyªn t¾c lµm viÖc cña c¸c hÖ thèng, nguyªn t¾c ghÐp cña hÖ thèng. PhÇn cuèi cña luËn v¨n lµ ch-¬ng tr×nh m« pháng nh»m ®¸nh gi¸ ®Æc tÝnh cña hÖ thèng dÉn ®-êng ghÐp. LuËn v¨n víi tªn gäi : “øng dông bé läc Kalman trong hÖ thèng dÉn ®-êng ®iÖn tö“ ®­îc chia thµnh c¸c phÇn nh­ sau: 3 Ch-¬ng 2. Ph-¬ng ph¸p biÓu diÔn vÞ trÝ trong kh«ng gian ë c¸c hÖ thèng dÉn ®-êng. Kh¸i niÖm vÒ c¸c hÖ to¹ ®é sö dông trong dÉn ®-êng. BiÓu diÔn vµ tham sè c¬ b¶n ®Ó biÓu diÔn vÞ trÝ cña hÖ thèng trong kh«ng gian. L-îc ®å tÝnh to¸n chuyÓn ®æi gi÷a c¸c hÖ to¹ ®é tham chiÕu kh¸c nhau. Ch-¬ng 3. HÖ thèng dÉn ®-êng qu¸n tÝnh INS. Nguyªn lý lµm viÖc cña hÖ thèng dÉn ®-êng qu¸n tÝnh INS. CÊu hÖ thèng dÉn ®-êng INS. Ph-¬ng tr×nh tÝnh to¸n vµ c¸c sai sè trong hÖ thèng INS. Ch-¬ng 4. HÖ thèng dÉn ®-êng vÖ tinh GPS. CÊu tróc cña hÖ thèng GPS. Nguyªn t¾c lµm viÖc cña hÖ thèng GPS. Ph-¬ng tr×nh tÝnh to¸n vÞ trÝ, sai sè vµ h¹n chÕ sai sè trong hÖ thèng GPS. Ch-¬ng 5. Bé läc Kalman rêi r¹c. Ph-¬ng tr×nh tr¹ng th¸i cña hÖ thèng liªn tôc theo thêi gian, rêi r¹c theo thêi gian. HÖ thèng tuyÕn tÝnh, phi tuyÕn. ThuËt to¸n Kalman vµ nguyªn lý lµm viÖc cña bé läc Kalman. Bé läc Kalman më réng. Ch-¬ng 6. øng dông bé läc Kalman trong c¸c hÖ thèng dÉn ®-êng. CÊu tróc vµ nguyªn lý lµm viÖc cña c¸c hÖ thèng dÉn ®-êng cã sö dông bé läc Kalman. HÖ thèng ghÐp GPS/INS. Ch-¬ng 7. M« pháng. Môc ®Ých vµ cÊu tróc ch-¬ng tr×nh m« pháng. KÕt qu¶ m« pháng. §©y lµ mét ®Ò tµi cßn hÕt søc míi mÎ, hiÖn ®ang nhËn ®-îc sù quan t©m cña rÊt nhiÒu nhµ khoa häc trªn thÕ giíi vµ nã vÉn cßn ®ang trong thêi gian nghiªn cøu vµ ph¸t triÓn. ViÖc kiÓm nghiÖm vµ triÓn khai thùc tÕ ®Ó ®¸nh gi¸ hÖ thèng lµ hÕt søc khã kh¨n. KÕt qu¶ ®-a ra ë ®©y lµ dùa trªn c¬ së m« pháng vµ kÕt qu¶ cña mét sè c¸c bµi b¸o khoa häc ®· ®-îc c«ng bè. 4 T¸c gi¶ xin bµy tá lßng biÕt ¬n s©u s¾c ®èi víi sù gióp ®ì cña TS. NguyÔn Quèc Trung, c¸c thÇy c« gi¸o khoa §iÖn tö ViÔn th«ng tr-êng §¹i häc B¸ch Khoa Hµ Néi cïng b¹n bÌ ®ång nghiÖp ®· gióp ®ì hoµn thµnh luËn v¨n nµy. Trong qu¸ tr×nh lµm luËn v¨n ch¾c ch¾n kh«ng tr¸nh khái nh÷ng s¬ suÊt vÒ néi dung vµ h×nh thøc, rÊt mong nhËn ®-îc sù gãp ý cña ®éc gi¶. 5 Ch-¬ng 2 - BiÓu diÔn täa ®é vÞ trÝ 2.1. HÖ to¹ ®é tham chiÕu Mét khung (frame) ®-îc gäi lµ mét hÖ to¹ ®é tham chiÕu. C¸c hÖ to¹ ®é tham chiÕu ®-îc dïng ®Ó biÓu diÔn vµ m« t¶ tÝnh chÊt chuyÓn ®éng cña ph-¬ng tiÖn cã g¾n hÖ thèng INS. Khi thùc hiÖn qu¸ tr×nh dÉn ®-êng ta cÇn Ýt nhÊt lµ hai hÖ to¹ ®é. Mét hÖ to¹ ®é dïng ®Ó biÓu diÔn cÊu tróc cña ph-¬ng tiÖn cÇn dÉn ®-êng(vehicle), vµ mét hÖ to¹ ®é dïng ®Ó biÓu diÔn qu¸ tr×nh dÉn ®-êng(map). Th«ng th-êng cã n¨m hÖ to¹ ®é tham chiÕu kh¸c nhau ®Ó biÓu diÔn tÝnh chÊt chuyÓn ®éng cña ph-¬ng tiÖn. 2.1.1. HÖ to¹ ®é khung (Body Frame, kÝ hiÖu lµ B) §©y lµ hÖ to¹ ®é c¬ së dïng biÓu diÔn cho c¸c c¶m biÕn qu¸n tÝnh. C¸c trôc cña hÖ to¹ ®é nµy t-¬ng tù nh- cÊu tróc khung cña ph-¬ng tiÖn(vehicle). HÖ to¹ ®é nµy ®-îc coi nh- hÖ to¹ ®é tham chiÕu cã c¸c trôc trùc giao g¾n víi ph-¬ng tiÖn cÇn dÉn ®-êng. Trôc xB cã h-íng chØ vÒ phÝa tr-íc, trôc yB cã h-íng vu«ng gãc vÒ bªn ph¶i so víi trôc xB, cßn trôc zB vu«ng gãc víi mÆt ph¼ng t¹o bëi hai trôc xB, yB vµ cã h-íng chØ xuèng phÝa d-íi. Th«ng th-êng hÖ to¹ ®é nµy cã gèc trïng víi t©m khèi cña ph-¬ng tiÖn hoÆc ®-îc x¸c ®Þnh bëi giao cña c¸c trôc cña ®ång hå ®o gia tèc hay c¸c con quay håi chuyÓn. VËn tèc cña ph-¬ng tiÖn trong hÖ to¹ ®é khung ®-îc biÓu diÔn b»ng c¸c tham sè [u, v, ]. 6 H×nh 2.1. HÖ to¹ ®é khung B (Body Frame) 2.1.2. HÖ to¹ ®é vïng NED (Local Level Frame, kÝ hiÖu lµ NED) §©y lµ hÖ to¹ ®é trùc giao dïng cho qu¸ tr×nh dÉn ®-êng. Gèc ®-îc x¸c ®Þnh nh- hÖ to¹ ®é khung, trïng víi t©m khèi cña hÖ thèng. Trôc N chØ theo h-íng b¾c, trôc E chØ theo h-íng ®«ng vµ vu«ng gãc víi trôc N, trôc D h-íng xuèng d-íi theo ph-¬ng cña gia tèc träng tr-êng t¹i vÞ trÝ hiÖn t¹i cña ph-¬ng tiÖn. Hai trôc N vµ E t¹o thµnh mÆt ph¼ng tiÕp xóc víi bÒ mÆt tr¸i ®Êt t¹i vÞ trÝ xÐt. Z N E §-êng kinh tuyÕn gèc D y z Y x X MÆt ph¼ng xÝch ®¹o H×nh 2.2. HÖ to¹ ®é vïng NED 7 2.1.3. HÖ to¹ ®é tr¾c ®Þa toµn cÇu WGS-84 (kÝ hiÖu lµ W) §©y lµ mét hÖ to¹ ®é cÇu, sö dông hai gãc vµ ®é cao ®Ó biÓu diÔn vÞ trÝ cña mét ®iÓm. Th«ng th-êng hÖ to¹ ®é nµy ®-îc sö dông trong tr-êng hîp dÉn ®-êng ë kho¶ng c¸ch xa. C¸c vÞ trÝ øng víi mùc n-íc biÓn ®-îc biÓu thÞ víi ®é cao b»ng kh«ng.  Kinh ®é : øng víi ®-êng kinh tuyÕn gèc(®-êng kinh tuyÕn ®i qua Lu©n §«n, Anh) gãc kinh ®é b»ng kh«ng. Gãc kinh ®é cã gi¸ trÞ n»m trong kho¶ng tõ 03600 vµ ®-îc tÝnh quay theo h-íng ®«ng. Gi¸ trÞ cña gãc kinh ®é biÓu thÞ ®é lÖch vÒ h-íng §«ng hay h-íng T©y so víi ®-êng kinh tuyÕn gèc.  VÜ ®é : T¹i mÆt ph¼ng xÝch ®¹o th× vÜ ®é cã gi¸ trÞ b»ng kh«ng. Gãc vÜ ®é b»ng 900 øng víi ®iÓm cùc b¾c, vµ b»ng -900 øng víi ®iÓm cùc nam. Gi¸ trÞ cña gãc vÜ ®é biÓu thÞ ®é lÖch so víi mÆt ph¼ng xÝch ®¹o.  §é cao h: BiÓu thÞ ®é cao so víi mùc n-íc biÓn.  C¸c tham sè ®-îc ®Þnh nghÜa trong hÖ to¹ ®é WGS-84: ➢ B¸n kÝnh trôc lín: a = 6378,137(km)  = 7292115,1467E-10(rad/s) ➢ VËn tèc gãc cña tr¸i ®Êt:  e ➢ H»ng sè gia tèc träng tr-êng:  = 3986005E8 (m3/s2) Tõ c¸c tham sè nµy ta cã thÓ tÝnh ra ®-îc hÖ sè uèn cong cña bÒ mÆt tr¸i ®Êt vµ b¸n kÝnh trôc nhá. 2.1.4. HÖ to¹ ®é qu¸n tÝnh ECI (Earth Center Inertial Frame, kÝ hiÖu lµ I) HÖ to¹ ®é nµy dïng ®Ó tham chiÕu tham sè qu¸n tÝnh. §©y lµ mét hÖ to¹ ®é kh«ng quay, kh«ng cã gia tèc øng víi kh«ng gian qu¸n tÝnh. B»ng c¸ch bá qua sù chuyÓn ®éng cña tr¸i ®Êt xung quanh mÆt trêi, gèc cña hÖ to¹ ®é I ®-îc gi¶ thiÕt ®Æt t¹i t©m khèi cña tr¸i ®Êt. Hai trôc xI vµ yI n»m trªn mÆt ph¼ng xÝch ®¹o cña tr¸i ®Êt. Trôc xI chØ theo h-íng mét ng«i sao ®-îc gäi lµ ®iÓm ph©n xu©n (Vernal Equinox). Trôc yI t¹o víi hai trôc xI vµ zI mét hÖ to¹ ®é vu«ng gãc víi h-íng cña trôc yI n»m 8 vÒ phÝa bÕn ph¶i cña mÆt ph¼ng xz. Trôc zI trïng víi trôc quay cña tr¸i ®Êt vµ cã h-íng däc theo cùc B¾c. ZECEF = ZECI  N §-êng kinh tuyÕn gèc MÆt ph¼ng xÝch ®¹o YECEF YECI XECI §iÓm ph©n xu©n (t-t0) XECEF H×nh 2.3. HÖ to¹ ®é ECI vµ ECEF 2.1.5. HÖ to¹ ®é ECEF ( Earth Center Earth Frame, kÝ hiÖu lµ E hay ECEF): HÖ to¹ ®é nµy ®-îc g¾n cè ®Þnh víi tr¸i ®Êt vµ chuyÓn ®éng theo sù quay cña tr¸i ®Êt. HÖ to¹ ®é E th-êng ®-îc sö dông trong hÖ thèng GPS vµ ®-îc gäi lµ hÖ to¹ ®é WGS-84. Th«ng th-êng c¸c tham sè ®o cña hÖ thèng GPS ®-îc biÓu diÔn trong hÖ to¹ ®é nµy. Do tÝnh chÊt cña c¸c trôc xE, yE vµ zE trong hÖ to¹ ®é E nªn ®«i khi nã ®-îc gäi lµ hÖ to¹ ®é Decac tr¸i ®Êt. §©y lµ mét hÖ to¹ ®é trùc giao víi gèc to¹ ®é ®Æt t¹i t©m khèi cña tr¸i ®Êt. Trôc xE chØ theo h-íng kinh tuyÕn gèc (cã gi¸ trÞ kinh ®é b»ng 0) vµ n»m trªn mÆt ph¼ng xÝch ®¹o. Trôc zE trïng víi trôc quay cña tr¸i ®Êt vµ cã h-íng theo cùc B¾c. Trôc yE hîp víi hai trôc xE vµ zE t¹o thµnh mét hÖ to¹ ®é trùc giao vµ cã h-íng vÒ phÝa bªn ph¶i cña mÆt ph¼ng t¹o bëi hai trôc xE vµ zE. 9 2.2. TÝnh quay cña hÖ thèng Trong phÇn nµy ta ®Ò cËp ®Õn ba lo¹i quay kh¸c nhau cña hÖ thèng. §ã lµ: quay theo trôc däc ph-¬ng tiÖn hay cßn gäi lµ l¾c ngang - thay ®æi gãc nghiªng c¸nh (roll), quay theo trôc ngang cña ph-¬ng tiÖn hay cßn gäi lµ l¾c däc - thay ®æi gãc ngÈng (pitch) vµ quay theo trôc th¼ng ®øng hay cßn gäi lµ xoay - thay ®æi gãc h-íng (yaw). Ngoµi ra trong phÇn nµy còng ®Ò cËp ®Õn c¸c ph-¬ng ph¸p biÓu diÔn gãc kh¸c nhau nh- lµ biÓu diÔn theo gãc Euler, ma trËn Cbn vµ biÓu diÔn theo gãc sè phøc suy réng, gäi lµ gãc Quaternion. Trôc ®øng (Yaw) Trôc ngang h-íng mòi (Roll) Trôc ngang h-íng c¸nh (Pitch) H×nh 2.4. Trôc quay cña ph-¬ng tiÖn Ta ®øng trong hÖ t¹o ®é dÉn ®-êng (®øng trªn mÆt ®Êt) vµ quan s¸t sù chuyÓn ®éng cña ph-¬ng tiÖn. VÊn ®Ò chóng ta cÇn quan t©m ë ®©y chÝnh lµ tham sè gia tèc vµ tèc ®é chuyÓn ®éng cña ph-¬ng tiÖn quan s¸t. §©y chÝnh lµ c¸c th«ng tin cÇn thiÕt ®Ó thùc hiÖn qóa tr×nh dÉn ®-êng. Trªn ph-¬ng tiÖn chuyÓn ®éng ta g¾n khèi IMU ®Ó ®o c¸c gi¸ trÞ tham sè gia tèc [ax, ay, az] vµ tham sè vËn tèc quay [p, q, r]. Ta cÇn thùc hiÖn viÖc chuyÓn ®æi c¸c vector nµy sang hÖ to¹ ®é dÉn ®-êng. Ma trËn chuyÓn ®æi ®-îc sö dông ë ®©y chÝnh lµ ma trËn Cbn. KÝ hiÖu “bn” ë ®©y biÓu thÞ qu¸ tr×nh chuyÓn ®æi c¸c vector tõ hÖ to¹ 10 ®é khung(Body frame) sang hÖ to¹ ®é dÉn ®-êng(Navigation frame). TÝnh chÊt quay cña ph-¬ng tiÖn ®-îc biÓu thÞ bëi ba gãc, gäi lµ c¸c gãc Euler [, , ]. Trong kü thuËt hµng kh«ng, c¸c gãc nµy ®-îc gäi lµ gãc nghiªng c¸nh(roll), gãc ngÈng(pitch) vµ gãc h-íng(yaw). TÝnh chÊt quay ®-îc biÓu diÔn trªn c¬ së hÖ to¹ ®é trôc.