Nghiên cứu hệ thống định vị sử dụng vệ tinh kết hợp cảm biến quán tính

BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - LUẬN VĂN THẠC SĨ KHOA HỌC NGHIÊN CỨU HỆ THỐNG ĐỊNH VỊ SỬ DỤNG VỆ TINH KẾT HỢP CẢM BIẾN QUÁN TÍNH NGÀNH: ĐIỀU KHIỂN TỰ ĐỘNG MÃ SỐ: MAI ĐỨC THÔNG Người hướng dẫn khoa học: PGS.TS HỒNG MINH SƠN HÀ NỘI 2008 MAI ĐỨC THƠNG BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - LUẬN VĂN THẠC SĨ KHOA HỌC ĐIỀU KHIỂN TỰ ĐỘNG NGÀNH: ĐIỀU KHIỂN TỰ ĐỘNG NGHIÊN CỨU HỆ THỐNG ĐỊNH VỊ SỬ DỤNG VỆ TINH KẾT HỢP CẢM BIẾN QN TÍNH MAI ĐỨC THƠNG 2006 - 2008 Hà Nội 2008 HÀ NỘI 2008 MỤC LỤC DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ…………………………………… ĐẶT VẤN ĐỀ……………………………………………………………… Chương 1: HỆ THỐNG ĐỊNH VỊ SỬ DỤNG VỆ TINH 1.1 Khái quát hệ thống định vị sử dụng vệ tinh 1.2 Hệ trục toạ độ liên quan 1.3 Cấu trúc hệ định vị sử dụng vệ tinh 1.3.1 Vệ tinh 1.3.2 Các trạm điều khiển mặt đất 12 1.3.3 Thiết bị thu tín hiệu từ vệ tinh 13 1.4 Nguyên lý hoạt động 14 1.5 Sai số nguyên nhân gây sai số 20 Chương 2: HỆ THỐNG ĐỊNH VỊ SỬ DỤNG CẢM BIẾN QUÁN TÍNH 24 2.1 Khái quát định vị sử dụng cảm biến quán tính 24 2.2 Các hệ trục toạ độ liên quan 27 2.2.1 Hệ trục toạ độ quán tính (Earth – Centered Inertial ECI) 27 2.2.2 Hệ tọa độ tâm trái đất (Earth-Centered Earth-Fixed,e-frame) 27 2.2.3 Hệ toạ độ dẫn đường (n-frame): 28 2.2.4 Hệ toạ độ vật thể (b-frame): 29 2.3 Cơ chế tính tốn hệ INS 32 2.3.1 Cơ chế tính tốn trục 32 2.3.2 Tính tốn hai trục 33 2.3.3 Tính toán theo ba trục 36 2.4 Quá trình chỉnh ban đầu 38 Chương 3: HỆ THỐNG ĐỊNH VỊ TÍCH HỢP GPS/INS 39 3.1 Bản chất bù INS-GPS 39 3.2 Đánh giá sai số: 40 3.2.1 Sai số vận tốc: 41 3.2.2 Công thức động học sai số hướng: 44 3.3 Bộ lọc Kalman tích hợp INS/GPS 46 3.4 Các phương pháp tích hợp INS/GPS: 56 3.4.1 Tích hợp lỏng GPS/INS 58 3.4.2 Tích hợp chặt GPS/INS 60 3.5 Các thuật toán xử lý 60 3.6 Các hạn chế tích hợp INS/GPS: 63 3.7 Thiết kế thiết bị định vị kết hợp GPS/INS 64 3.7.1 Modun GPS 65 3.7.2 Modun INS 66 3.7.3 Mạch nguồn 69 3.7.4 Modun vi xử lý 69 Mai Đức Thông Cao học ĐKTĐ 2006-2008 3.7.5 Thiết kế phần mềm 72 KẾT LUẬN VÀ HƯỚNG NGHIÊN CỨU TIẾP THEO……………………73 TÀI LIỆU THAM KHẢO………………………………………………… 74 PHỤ LỤC……………………………………………………………………76 DANH MỤC CÁC HÌNH VẼ, ĐỒ THỊ Hình 1.1 Hệ toạ độ WGS84 hệ toạ độ tâm trái đất Oxyz Hình 1.2 Hệ thống định vị sử dụng vệ tinh Hình 1.3 Quỹ đạo vệ tinh 10 Hình 1.4 Cấu trúc tín hiệu GPS 11 Hình 1.5 Hoạt động trạm điều khiển 12 Hình 1.6 Sơ đồ cấu trúc thu tín hiệu GPS 14 Hình 1.7 Mã giả ngẫu nhiên 15 Hình 1.8 Khoảng cách từ hai vị trí biết 15 Hình 1.9 Kết tính lặp 18 Hình 1.10 Tín hiệu từ vệ tinh 19 Hình 1.11 Tín hiệu từ hai vệ tinh 19 Hình 1.12 Tín hiệu từ vệ tinh 20 Hình 1.13 Ảnh hưởng khí 21 Hình 1.14 Ảnh hưởng phản xạ 22 Hình 1.15 Vị trí tương đối vệ tinh máy thu 23 Hình 2.1 Hệ INS sử dụng khớp đăng 25 Hình 2.2 Hệ thống cảm biến hệ strapdown 26 Hình 2.3 Cấu tạo hệ strapdown 26 Hình 2.4 Hệ toạ độ qn tính 27 Hình 2.5 Hệ trục toạ độ ECEF 28 Hình 2.6 Hệ toạ độ dẫn đường 29 Hình 2.7 Hệ toạ độ vật thể 29 Hình 2.8 Cơ chế tính tốn theo trục 32 Hình 2.9 Hệ toạ độ vật thể hệ toạ độ dẫn đường 33 Hình 2.10 Tính tốn theo ba trục 36 Hình 2.11 Cơ chế tính tốn hệ INS 38 Hình 3.1 Q trình tính tốn lọc Kalman 49 Hình 3.2 Khoảng thời gian đo lường IMU GPS 55 Hình 3.3 Ảnh hưởng cánh tay đòn 56 Hình 3.4 Phương pháp truyền thẳng (vịng lặp mở) 57 Hình 3.5 Phương pháp phản hồi (vịng lặp đóng) 57 Hình 3.6 Tích hợp lỏng GPS/INS 59 Hình 3.7 Sơ đồ tích hợp INS/GPS tập trung (vịng mở) 61 Hình 3.8 Sơ đồ tích hợp INS/GPS tập trung (vịng đóng) 61 Hình 3.9 Sơ đồ tích hợp INS/GPS phân tán (vịng mở) 61 Mai Đức Thơng Cao học ĐKTĐ 2006-2008 Hình 3.10 Sơ đồ tích hợp INS/GPS phân tán (vịng đóng) 62 Hình 3.11 Thiết kế phần cứng 64 Hình 3.13 Cảm biến quán tính 66 Hình 3.14 Các khối chức cảm biến quán tính 67 Hình 3.15 Mạch nguồn 69 Hình 3.16 Vi xử lý PSoC 70 Hình 3.17 Ứng dụng điều khiển 73 Hình 3.18 Ứng dụng quản lý 74 Mai Đức Thông Cao học ĐKTĐ 2006-2008 ĐẶT VẤN ĐỀ Trong thực tế nay, nhiều trường hợp đòi hỏi cần phải biết xác vị trí vận tốc vật thể Một ví dụ điển hình vấn đề điều khiển robot tự hành theo quỹ đạo xác định trước, tốn tìm đường ngắn hai điểm cho phương tiện giao thông đường bộ, hay phức tạp xác định điều khiển đường bay máy bay, tên lửa không gian Ngồi ra, nhiều trường hợp địi hỏi ghi lại thơng tin hành trình di chuyển phương tiện phục vụ cho mục đích điều tra, lập đồ quản lý việc sử dụng thiết bị kiểu “hộp đen” phương tiện giao thông đường (ô tô, tàu hoả…) để lưu lại hành trình Hành trình sử dụng để phân tích tai nạn thơng tin vận tốc vị trí xảy tai nạn Một ứng dụng quan trọng quản lý hành trình đồn xe tải đồn xe vận tải mỏ than Việc ghi lại hành trình giúp người quản lý kiểm sốt số chuyến vận chuyển hợp lệ lượng nhiên liệu tiêu hao… Như vấn đề định vị vấn đề - vấn đề cảm biến, đo lường nhiều toán điều khiển quản lý Giải vấn đề tạo sở để giải nhiều yêu cầu khác phức tạp Mục đích đề tài nghiên cứu phương pháp hệ thống định vị nhằm tìm giải pháp có ý nghĩa mặt lý thuyết thực tiễn giao thơng vận tải Hiện có hai nhóm phương pháp định vị bản: • Nhóm phương pháp xác định vị trí nhờ tính chuyển động tương đối so với điểm mốc biết Phương pháp sử dụng điểm cố định mặt đất đèn hải đăng sao, hay phương pháp định vị sử dụng sóng radio dựa nguồn phát sóng có vị trí biết Tiêu biểu cho Mai Đức Thông Cao học ĐKTĐ 2006-2008 phương pháp hệ thống định vị sử dụng vệ tinh (Global Positioning System – GPS) • Phương pháp định vị sử dụng cảm biến quán tính (Inertial Navigation System - INS) dựa vận tốc vị trí ban đầu biết sau đo gia tốc góc gia tốc dịch chuyển để xác định vị trí Đây phương pháp không cần dựa vào thơng tin từ bên ngồi Một ví dụ điển hình hệ thống tàu ngầm, nơi sử dụng phương pháp dẫn đường truyền thống mơi trường nước chắn sóng radio khó để xác định điểm mốc Nhóm phương pháp thứ mà cụ thể hệ thống định vị tồn cầu sử dụng vệ tinh GPS có khả cung cấp thơng tin vị trí vận tốc xác khoảng thời gian dài, bị ảnh hưởng nguồn nhiễu thơng thường có nhược điểm tốc độ đưa kết chậm, khó áp dụng cho ứng dụng tính tốn nhanh tốn điều khiển quỹ đạo tên lửa Nhóm phương pháp thứ hai INS có khả cung cấp thơng tin vị trí xác khoảng thời gian ngắn, bị ảnh hưởng nhiều nguồn nhiễu có ưu điểm tốc độ đưa kết cao Như việc kết hợp hai phương pháp định vị ta hệ thống có ưu điểm vượt trội sau: khả cho thông tin vị trí vận tốc với độ cao xác khoảng thời gian dài, đồng thời tốc độ xuất liệu nhanh Các nghiên cứu có tác giả ngồi nước chủ yếu tập trung vào ứng dụng đơn lẻ, cụ thể sử dụng phương pháp định vị (GPS INS) mà có nghiên cứu giải vấn đề tích hợp hai phương pháp Ngồi chưa có thiết kế phần cứng phần mềm cụ thể thiết bị định vị tích hợp phù hợp với ứng dụng nước Mai Đức Thông Cao học ĐKTĐ 2006-2008 Nội dung luận văn là: tập trung nghiên cứu nội dung phương pháp định vị sử dụng vệ tinh phương pháp định vị sử dụng cảm biến qn tính từ nghiên cứu khả kết hợp chúng, cuối thiết kế thiết bị tích hợp hai phương pháp ứng dụng Luận văn bao gồm chương: Chương Định vị sử dụng vệ tinh: trình bày lý thuyết hệ thống định vị sử dụng vệ tinh hay gọi hệ thống định vị toàn cầu (GPS) bao gồm cấu trúc, nguyên lý hoạt động, sai số nguyên nhân gây sai số Chương Định vị sử dụng cảm biến quán tính: trình bày nội dung hệ thống xác định vị trí sử dụng cảm biến qn tính bao gồm cấu trúc, chế tính tốn, nguyên nhân gây sai số hệ thống Chương Kết hợp định vị sử dụng vệ tinh cảm biến qn tính: trình bày khả tíchhợp hai phương pháp để có hệ thống hồn chỉnh bao gồm chế tích hợp, phương pháp tích hợp, lọc Kalman tích hợp GPS/INS Cuối luận văn trình bày thiết kế phần cứng phần mềm cho thiết bị định vị tích hợp hai phương pháp Mai Đức Thông Cao học ĐKTĐ 2006-2008 Chương 1: HỆ THỐNG ĐỊNH VỊ SỬ DỤNG VỆ TINH Chương trình bày số kết nghiên cứu hệ thống định vị sử dụng vệ tinh bao gồm cấu trúc hệ thống, nguyên lý hoạt động, nguyên nhân gây sai số cách khắc phục từ phục vụ cho việc thiết kế hệ thống định vị tích hợp chương 1.1 Khái quát hệ thống định vị sử dụng vệ tinh Hệ thống định vị sử dụng vệ tinh hay gọi hệ thống định vị toàn cầu (Global Positioning System – GPS) hệ thống xác định vị trí đâu bề mặt trái đất dựa vào việc tính tốn khoảng cách từ điểm tới vệ tinh không gian Hệ thống thiết kế vận hành quốc phòng Mỹ ban đầu cho mục đích quân ứng dụng nhiều dân 1.2 Hệ trục toạ độ liên quan Hệ WGS84: Vấn đề cần phải xác định hệ thống định vị dẫn đường hệ trục toạ độ quy chiếu Đối với hệ thống GPS người ta thường sử dụng hệ WGS 84 Hệ trục toạ độ WGS 84 mơ hình trái đất quy chuẩn ấn định năm 1984 dùng phổ biến Hình 1.1 Hệ toạ độ WGS84 hệ toạ độ tâm trái đất Oxyz Mai Đức Thông Cao học ĐKTĐ 2006-2008 Hệ thống xấp xỉ bề mặt trái đất hình elip quay với trục quay trùng khớp với trục quay trái đất, tâm tâm trái đất kinh tuyến qua đài thiên văn Greewich Trục elip định nghĩa có độ dài khoảng 6.378.137 m trục phụ 6.356.752 m Vị trí điểm trái đất xác định theo kinh độ, vĩ độ độ cao (Φ,λ,h) Hệ trục toạ độ tâm trái đất: Hệ trục có gốc tâm trái đất, trục x,y, z trục vng góc cho mặt phẳng Oxy mặt phẳng chứa đường xích đạo Trục x qua đường kinh tuyến gốc trục z qua cực bắc hình 1.1 Các cơng thức liên hệ hệ toạ độ WGS84 (Φ,λ,h) với hệ trục toạ độ tâm trái đất (x,y,z): x = (Rn + h) cos φ cos λ y = (Rn + h) cos φ sin λ z = (Rn (1 – e2) + h) sin φ Hệ trục toạ độ thường sử dụng để xác định vị trí vệ tinh quỹ đạo vệ tinh cố định so với hệ trục 1.3 Cấu trúc hệ định vị sử dụng vệ tinh Một hệ thống định vị sử dụng vệ tinh (GPS) đầy đủ bao gồm ba phần sau: • Các vệ tinh khơng gian • Các trạm điều khiển mặt đất • Bộ thu tín hiệu ứng dụng người sử dụng Hình 1.2 minh hoạ ba thành phần hệ thống định vị toàn cầu sử dụng vệ tinh Mai Đức Thông Cao học ĐKTĐ 2006-2008 69 3.7.3 Mạch nguồn Hình 3.15 Mạch nguồn Mạch nguồn có khả đưa điện áp 5V cho vi xử lý PSoC, cảm biến INS đồng thời đưa điện áp 3.3V cho việc thực đọc/ghi thẻ nhớ nguồn cho GPS 3.7.4 Modun vi xử lý Thiết kế sử dụng vi xử lý PsoC Cypress PSoC – Programmable System on Chip - loại vi xử lý đặc biệt có tính vượt trội so với loại vi xử lý khác Với mục đích đáp ứng hầu hết nhu cầu khác người sử dụng, PSoC khơng có khả vi xử lý thông thường mà cịn cung cấp loạt mơđun phần cứng lập trình người sử dụng Mai Đức Thơng Cao học ĐKTĐ 2006-2008 70 Hình 3.16 Vi xử lý PSoC Các đặc tính chính: Bộ xử lý với kiến trúc Havard mạnh mẽ · Tốc độ CPU M8C lên tới 24MHz · Bộ nhân MAC ,8*8 ,32 bit Accumulate · Công suất thấp tốc độ cao · Dải điện áp hoạt động là: 3V đến 5.25V · Điện áp hoạt động xuống 1V sử dụng chế độ Switch Mode Pump chip · Dải nhiệt độ hoạt động công nghiệp: -40 độ C tới 85 độ C Các khối ngoại vi cao cấp (PsoC Blocks): · 12 khối analog cung cấp: o đến 14 bit ADC o bit DAC o Bộ khuếch đại lập trình o Bộ so sánh lọc lập trình · khối số cung cấp: o đến 32 bit Timer,counter,PWM o Các module CRC PRS Mai Đức Thông Cao học ĐKTĐ 2006-2008 71 · o UART song công o SPI o Kết nối tới tất chân Có khả tạo ngoại vi phức tạp nhờ tổ hợp khối lại Các loại đồng hồ xác lập trình · Mạch dao động bên 24/48MHz với độ xác 2.5% · Tuỳ chọn mạch dao động bên lên tới 24MHz · Mạch dao động bên cho Watchdog Sleep Bộ nhớ chip linh hoạt · Bộ nhớ lưu giữ chương trình Flash 16K · 256 bytes SRAM lưu liệu · ISSP · Chế độ bảo vệ linh hoạt · Mô EEPROM Flash Các cấu hình chân lập trình · Dòng điều khiển cho chân 25mA · Các chế độ HighZ,Strong,PullUp,PullDown · 12 đầu vào analog · Các ngắt định hình chân GPIO Các nguồn lực khác hệ thống · Các master,slave multi-master I2C tới 400KHz · Các timer Sleep Watchdog · Các điện áp chuẩn chip Các công cụ phát triển đầy đủ · Phần mềm thiết kế chip PsoC (PsoC Designer) · Bộ nạp mô mạch Mai Đức Thông Cao học ĐKTĐ 2006-2008 72 3.7.5 Thiết kế phần mềm Thiết kế phần mềm bao gồm phần mềm thu thập số liệu vi xử lý PsoC phần mềm mơ tính tốn, vẽ lại đường MATLAB trình bày chi tiết phần phụ lục Mai Đức Thông Cao học ĐKTĐ 2006-2008 73 KẾT LUẬN VÀ HƯỚNG NGHIÊN CỨU TIẾP THEO Luận văn tập trung nghiên cứu nội dung cụ thể phương pháp định vị sử dụng vệ tinh (GPS) phương pháp định vị sử dụng cảm biến qn tính (INS) từ đề giải pháp tích hợp hai phương pháp để thu giải pháp tin cậy Luận văn trình bày thiết kế phần cứng phần mềm thiết bị định vị tích hợp GPS INS với sai số vị trí vận tốc nhỏ Từ kết nghiên cứu nghiên cứu tốn điều khiển quỹ đạo xác để ứng dụng cho robot tự hành máy bay, tên lửa, tàu vũ trụ…theo mô hình: GPS/INS ĐIỀU KHIỂN, GIÁM SÁT… CƠ CẤU CHẤP HÀNH ứng dụng máy bay khơng người lái hình 3.17 Hình 3.17 Ứng dụng điều khiển Mai Đức Thơng Cao học ĐKTĐ 2006-2008 74 Ngồi kết hợp với phương pháp truyền thơng khơng dây (sóng radio RF, mạng thông tin di động GSM, GPRS) để xây dựng phần mềm quản lý online Web offline hình 3.18 Hình 3.18 Ứng dụng quản lý Mai Đức Thông Cao học ĐKTĐ 2006-2008 75 TÀI LIỆU THAM KHẢO Tiếng Việt Nguyễn Doãn Phước Lý thuyết điều khiển nâng cao, NXB Khoa học Kỹ thuật, 2006 Tiếng Anh Grewal, Mohinder S Global Positioning Systems, Inertial Navigation,and Integration John Wiley, New York, 2001 Gregory t French Understanding the gps - an introduction to the global positioning system , GeoResearch Inc Eskin - Design of an Inertial Navigation Unit Using MEMS sensors, January 2006 Mai Đức Thông Cao học ĐKTĐ 2006-2008 76 PHỤ LỤC Sơ đồ nguyên lý mạch Mai Đức Thông Cao học ĐKTĐ 2006-2008 77 Thiết kế phần cứng cho chip PsoC Mai Đức Thông Cao học ĐKTĐ 2006-2008 78 Source Code PsoC SD- Card // Port Select Drive Note // DO => P2[3] GlobalIn High Z Data Out // SCLK => P2[2] GlobalOut Strong Clock In // DI => P2[1] GlobalOut Strong Data In // CS => P2[0] StdCPU Strong Card Select (Active Low) // CD => P2[4] StdCPU Pull Up Card Detect (Active Low) // WP => P2[5] StdCPU Pull Up Write Protect (Active High) // #include // part specific constants and macros #include "PSoCAPI.h" // PSoC API definitions for all User Modules #include "stdlib.h" #define CARD_PRESENT #define CARD_NOT_PRESENT char myfile[] = "data.txt"; const char longStr[] ; unsigned int Total_Byte; unsigned int SoByte_Du; BOOL Transfer_Staion_Ok=FALSE; BOOL File_Open=FALSE; BOOL BigFile=FALSE; const char gps_test[]="$042501,2101.65266,10548.09747,000.0,230607*"; void main() { SW1(0);SW0(1); //Mode_Receive Setup7msPulse(); com_initialize (); Led_GPS(1); Led_INS(1); SDCard_Start(); SDCard_Select(SDCard_ENABLE); cardInfo = 0; while ( ! cardInfo ) // Wait for card to communicate { // initialize card, determine card type and file system type cardInfo = SDCard_InitCard(); PC_PutChar(cardInfo); } if((cardInfo==0x41)|(cardInfo==0xB1)) { Led_RF(0); } else while(1); SPIS_DisableSS(); NSS_Conf(1);//Enb=0; for(i_count=0;i_count1000000) BigFile=TRUE; PC_PutChar(0x66); SDCard_fclose(fp); PC_PutChar(0x77); if(BigFile) { BigFile=FALSE; SDCard_Remove(myfile); } fp = SDCard_fopen(myfile,"a+"); // Open file to append data PC_PutChar(fp); // PC_Write(Value_stData_GPS,Count_GPS_In); SDCard_fputBuff(Value_stData_GPS,Count_GPS_In,fp); PC_PutChar(0x99); SDCard_fclose(fp); GPS_EnableInt(); } SDCard_Remove(Myfile); if(Blink) { Led_GPS(0); //GPS_Valide=FALSE; } else Led_GPS(1); if(Count_7ms>50) { Count_7ms=0; // Enb_Save=TRUE; PC_PutChar(cardInfo); } ///////////////////// if(Num_Block==0) { fp = SDCard_fopen(myfile,"r+"); //Open to read Frame=FRAME_OK; if(fp==SDCard_MAXFILES) { Total_Block=0; SoByte_Du=21; Total_Byte=24; } else //Co file ton tai va dang open { Total_Block=(unsigned int)(SDCard_GetFileSize(fp)/998); SoByte_Du=(unsigned int)(SDCard_GetFileSize(fp)%998)+21; Mai Đức Thông Cao học ĐKTĐ 2006-2008 80 File_Open=TRUE; } } // if(Num_Block==0) Enb_Next_Transfer=TRUE; } } //Start Transfer } //Read_RF_Complete //While File gps.c xử lý liệu từ modu GPS #include "port.h" #include "gpsc.h" void com_initialize (void) { GPS_Start(GPS_PARITY_NONE); GPS_EnableInt(); Counter8_Start(); Counter48_Start(); } #pragma interrupt_handler GPSc_ISR void GPSc_ISR() { char ch; ch=GPS_bReadRxData(); PC_PutChar(ch); if(ch=='$') { Step=0; Enb_ReceivGPS=FALSE; } switch(Step) { case 0: Step++; break; case 1: if (ch=='G') Step++; break; case 2: if (ch=='P') Step++; break; case 3: if (ch=='R') Step++; break; case 4: if (ch=='M') Step++; break; case 5: if (ch=='C') { Step++; Mai Đức Thông Cao học ĐKTĐ 2006-2008 81 Dauphay=0; Enb_ReceivGPS=TRUE; Count_GPS_In=1; } break; } if (Enb_ReceivGPS) { switch (Dauphay) { case 1: // Thoi gian case 2: //Valid case 3: // Kinh case 5: // Vi case 7: //Toc case 8: //Ngay thang Buffer_GPS_In[Count_GPS_In++]=ch; } if (ch==',') { Dauphay++; if(Dauphay==2) { Count_GPS_In-=5; Buffer_GPS_In[Count_GPS_In++]=ch; } if(Dauphay==3) { if(Buffer_GPS_In[Count_GPS_In-2]=='A') GPS_Valide=TRUE; else GPS_Valide=FALSE; } if((Enb_ReceivGPS)&&((Count_GPS_In==80)||(ch==0x0a))) { Buffer_GPS_In[0]='$'; Buffer_GPS_In[Count_GPS_In]=0x0a; if(Count_GPS++==5) { Enb_Save=TRUE; Count_GPS=0; } } } MATLABcode cho INS Ta sử dụng m-file INS2D.m để đọc liệu từ file data.txt thẻ nhớ, tính tốn vẽ lại đường theo hai chiều XY vật clc fid = fopen('data.txt'); C = textscan(fid,'%d16%d16%d16%d16%d16%d16','delimiter',','); Mai Đức Thông Cao học ĐKTĐ 2006-2008 82 fclose(fid); ZGYRO_W=C{3}'*0.0183 %scale _ radian N=length(ZGYRO_W); T=0.5; %sample time A_X =C{4}'*0.4669 A_Y =C{5}'*0.4669 %A_XY{1}=A_X' %A_XY{2}=A_Y' %ZGYRO_W=ZGYRO_OUT*0.0183 %*75/4085, range -75:75*, ADC max 4085 %ZANG_D(1)=35; %initial angle ZANG_D(1:N)=0;ZANG_D(1)=0; A_N(1:N)=0; A_E(1:N)=0; V_N(1:N)=0;V_N(1)=0; V_E(1:N)=0;V_E(1)=0; P_N(1:N)=0;P_N(1)=0; P_E(1:N)=0;P_E(1)=0; for i=2:length(ZGYRO_W), ZANG_D(i)=ZANG_D(i-1)+ZGYRO_W(i)*T; A_N(i) = cos(ZANG_D(i)*pi/180)*A_X(i)sin(ZANG_D(i)*pi/180)*A_Y(i); A_E(i) = sin(ZANG_D(i)*pi/180)*A_X(i) +cos(ZANG_D(i)*pi/180)*A_Y(i); V_N(i) =V_N(i-1)+A_N(i)*T*9.81/1000; V_E(i) =V_E(i-1)+A_E(i)*T*9.81/1000; P_N(i) =P_N(i-1)+V_N(i)*T; P_E(i) =P_E(i-1)+V_E(i)*T; end plot(P_E,P_N,' rs','LineWidth',2,'MarkerSize',10); File INS3D.m tính tốn theo trục XYZ clc fid = fopen('data.txt'); C = textscan(fid,'%d16%d16%d16%d16%d16%d16','delimiter',','); fclose(fid); XGYRO_W=C{1}'*0.0183; %0.0183 = 75/4095 (range 75) YGYRO_W=C{2}'*0.0183; ZGYRO_W=C{3}'*0.0183; %scale _ radian A_X =C{4}'*0.4669; %0.4669 = 1912/4095 A_Y =C{5}'*0.4669;A_Z =C{6}'*0.4669; Mai Đức Thông Cao học ĐKTĐ 2006-2008 83 N=length(ZGYRO_W);T=0.5; %sampling time %ZGYRO_W=ZGYRO_OUT*0.0183 %*75/4085, range -75:75*, ADC max 4085 %ZANG_D(1)=35; %initial angle XANG_D(1:N)=0;ZANG_D(1)=0; YANG_D(1:N)=0;ZANG_D(1)=0; ZANG_D(1:N)=0;ZANG_D(1)=0; %A_NED init A_N(1:N)=0;A_E(1:N)=0;A_D(1:N)=0; %V_NED init V_N(1:N)=0;V_N(1)=0;V_E(1:N)=0;V_E(1)=0;V_D(1:N)=0;V_D(1)=0; %P_NED init P_N(1:N)=0;P_N(1)=0;P_E(1:N)=0;P_E(1)=0;P_D(1:N)=0;P_D(1)=0; for i=2:N, XANG_D(i)=XANG_D(i-1)+XGYRO_W(i)*T; YANG_D(i)=YANG_D(i-1)+YGYRO_W(i)*T; ZANG_D(i)=ZANG_D(i-1)+ZGYRO_W(i)*T; cosx=cos(XANG_D(i)*pi/180);sinx=sin(XANG_D(i)*pi/180); cosy=cos(YANG_D(i)*pi/180);siny=sin(YANG_D(i)*pi/180); cosz=cos(ZANG_D(i)*pi/180);sinz=sin(ZANG_D(i)*pi/180); C_BN= [1 0;0 cosz sinz;0 -sinz cosz]*[cosy -siny;0 0;siny cosy]*[cosx sinx 0;-sinx cosx 0;0 1]; A_XYZ(1)=A_X(i);A_XYZ(2)=A_Y(i);A_XYZ(3)=A_Z(i); A_NED=C_BN*A_XYZ; A_N(i) = A_NED(1); A_E(i) = A_NED(2); A_D(i) = A_NED(3); V_N(i) =V_N(i-1)+A_N(i)*T*9.81/1000; V_E(i) =V_E(i-1)+A_E(i)*T*9.81/1000; V_D(i) =V_D(i-1)+A_D(i)*T*9.81/1000; P_N(i) =P_N(i-1)+V_N(i)*T; P_E(i) =P_E(i-1)+V_E(i)*T; P_D(i) =P_D(i-1)+V_D(i)*T; end plot(P_E,P_N,'-mo','LineWidth',2, 'MarkerEdgeColor','k', 'MarkerFaceColor','g', 'MarkerSize',6) axis square %plot3(P_E,P_N,P_D,'mo','LineWidth',2, 'MarkerEdgeColor','k', 'MarkerFaceColor','g', 'Marker Size',6) Mai Đức Thông Cao học ĐKTĐ 2006-2008 ... kế hệ thống định vị tích hợp chương 1.1 Khái quát hệ thống định vị sử dụng vệ tinh Hệ thống định vị sử dụng vệ tinh hay cịn gọi hệ thống định vị tồn cầu (Global Positioning System – GPS) hệ thống. .. Chương 1: HỆ THỐNG ĐỊNH VỊ SỬ DỤNG VỆ TINH 1.1 Khái quát hệ thống định vị sử dụng vệ tinh 1.2 Hệ trục toạ độ liên quan 1.3 Cấu trúc hệ định vị sử dụng vệ tinh 1.3.1 Vệ tinh ... dung hệ thống xác định vị trí sử dụng cảm biến quán tính bao gồm cấu trúc, chế tính tốn, ngun nhân gây sai số hệ thống Chương Kết hợp định vị sử dụng vệ tinh cảm biến qn tính: trình bày khả tíchhợp