Nâng cao chất lượng hệ thống tích hợp INS,GPS sử dụng bộ lọc kalman

ĐẠ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 LUẬN VĂN THẠC SĨ HÀ NỘI – 2012 ĐẠ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 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 MỤC LỤC LỜI CAM ĐOAN I LỜI CẢM ƠN II MỤC LỤC III DANH MỤC HÌNH IV DANH MỤC BẢNG V THUẬT NGỮ VIẾT TẮT VI TÓM TẮT VII CHƯƠNG I GIỚI THIỆU HỆ THỐNG GPS CHƯƠNG II HỆ THỐNG INS 2.1 Nguyên lý hoạt động INS 2.2 Ma trận chuyển hệ tọa độ 2.2.1 Ma trận chuyển Euler 2.2.2 Ma trận chuyển Quaternion 2.2.3 Tính góc Euler từ ma trận xoay 2.2.4 Tính Quaternion từ ma trận xoay 2.2.5 Tính góc Euler từ Quaternion 2.3 Phương trình chuyển động 2.4 Mơ hình lỗi INS 10 CHƯƠNG III HỆ THỐNG TÍCH HỢP INS/GPS 14 3.1 Lý Thuyết 14 3.1.1 Bộ lọc Kalman 14 3.1.2 Hệ thống tích hợp INS/GPS 17 3.2 Đề xuất học viên 18 3.2.1 Lưu đồ thuật tốn hệ thống tích hợp INS/GPS 18 3.2.2 Bộ lọc Kalman cho hệ thống tích hợp INS/GPS 19 3.3 Kết mô 23 CHƯƠNG IV KẾT LUẬN 30 TÀI LIỆU THAM KHẢO 31 PHỤ LỤC 32 Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman IV DANH MỤC HÌNH Hình Quỹ đạo bay vệ tinh GPS Hình Cấu trúc IMU Hình Hệ thống định vị quán tính Hình góc Euler Hình Sai lệch hệ tọa độ p-frame N- frame .11 Hình Thuật tốn Kalman cổ điển .14 Hình Lưu đồ tính tốn với lọc Kalman 16 Hình Sơ đồ INS/GPS theo phương thức vòng hở 17 Hình Sơ đồ INS/GPS theo phương thức vịng kín .17 Hình 10 Cấu trúc theo kiểu vịng hở (a) vịng kín (b) .18 Hình 11 Lưu đồ thuật tốn hệ thống tích hợp INS/GPS 19 Hình 12 Bộ lọc Kalman mô .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 hệ INS/GPS, Vn GPS sai số tuyêt đối vận tốc 25 Hình 15 Vận tốc Ve hệ INS/GPS, Ve GPS, sai số tuyệt đối 26 Hình 16 So sánh góc hướng INS/GPS GPS 27 Hình 17 Các góc lệch E, N,Up 27 Hình 18 Độ trơi cảm biến vận tốc góc Gbx, Gby 28 Hình 19 Các góc chúc ( Pitch) nghiêng (Roll) 28 Hình 20 Hệ tọa độ vật thể 33 Hình 21 Hệ tọa độ định vị .33 Hình 22 Thuật tốn Salychev 34 Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman V DANH MỤC BẢNG Bảng Lỗi gây cảm biến quán tính 10 Bảng Trung vị, phương sai khoảng cách 24 Bảng Sai số tuyệt đối vận tốc theo phương Đông 25 Bảng Sai số tuyệt đối vận tốc theo phương Bắc 26 Bảng Các trung vị phương sai góc chúc góc nghiêng 29 Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman VI THUẬT NGỮ VIẾT TẮT Viết tắt IMU In GPS G KF K INS In SINS S S DCM D b-frame B ll-frame L N- frame N Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman VII LỜI NÓI ĐẦU Hệ thống vi điện tử - MEMS kết hợp yếu tố khí, nhiệt, sinh học, hóa học, quang học, chất lỏng, điện tử đế Silicon Thông thường dựa công nghệ vi chế tạo Công nghệ MEMS hứa hẹn cách mạng thay nhiều sản phẩm cách kết hợp nhiều thành phần chíp Cơng nghệ MEMS ngày phát triển, thiết bị MEMS cho độ xác cao, hiệu suất cao, độ tin cậy lớn giá thành hạ Một thiết bị MEMS ứng dụng gần lĩnh vực định vị tọa độ vật thể IMU Thiết bị đưa vào sử dụng lĩnh vực điều khiển Robot hay định vị tọa độ phương tiện chuyển động Để người dùng dễ sử dụng, thiết bị IMU tích hợp với thuật tốn dẫn đường quán tính (INS) Hiện nay, vấn đề định vị thu hút quan tâm nhiều nhà nghiên cứu cơng nghiệp ngồi nước Khi kết hợp với GPS, hệ thống định vị kết hợp INS/GPS có nhiều ưu điểm độ xác kích thước nhỏ Để tích hợp INS GPS thường sử dụng lọc tối ưu Kalman Việc áp dụng lọc Kalman tuyến tính có ưu điểm đơn giản, dễ thực thi chất lượng hệ thống chưa cao Mục tiêu luận văn cải thiện chất lượng hệ thống tích hợp INS/GPS sử dụng cấu hình kết hợp INS/GPS phù hợp Trong luận văn này, họ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ất lượng tồn hệ thống, đặc biệt thơng số góc tư thế, vận tốc, vị trí Nội dung đề tài bao gồm chương sau: - - Chương I : Hệ thống định vị toàn cầu GPS - Chương II : Hệ thống dẫn đường quán tính INS Chương III : Hệ thống tích hợp INS/GPS, lọc Kalman ứng dụng - Chương IV : Kết luận Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman CHƯƠNG I GIỚI THIỆU HỆ THỐNG GPS Hệ thống định vị toàn cầu (tiếng Anh: Global Positioning System - GPS) hệ thống xác định vị trí dựa vị trí vệ tinh nhân tạo, Bộ Quốc phòng Hoa Kỳ thiết kế, xây dựng, vận hành quản lý Hệ thống định vị toàn cầu GPS thu thập thông tin tọa độ (vĩ độ kinh độ), tốc độ độ cao vật thể Hệ thống GPS gồm 24 vệ tinh phóng lên quỹ đạo trái đất (xem Hình 1) [3] Các hệ thống dẫn đường truyền thống hoạt động dựa trạm phát tín hiệuvô tuyến điện Được biết đến nhiều hệ thống sau: LORAN – (LOng RAnge Navigation) – hoạt động giải tần 90-100 kHz chủ yếu dùng cho hàng hải, hay TACAN – (TACtical Air Navigation) – dùng cho quân đội Mỹ biến thể với độ xác thấp VOR/DME – VHF (Omnidirectional Range/Distance Measuring Equipment ) – dùng cho hàng khơng dân dụng Hình Quỹ đạo bay vệ tinh GPS Hệ thống định vị tồn cầu GPS thu thập thơng tin tọa độ (vĩ độ kinh độ), độ cao tốc độ vật thể [4] Các vệ tinh GPS bay vòng quanh Trái Đất hai lần ngày theo quỹ đạo xác phát tín hiệu có thơng tin xuống Trái Đất Các máy thu GPS nhận thơng tin phép tính lượng giác tính xác vị trí người dùng Về chất máy thu GPS so sánh thời gian tín hiệu phát Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman từ vệ tinh với thời gian nhận chúng Sai lệch thời gian cho biết máy thu GPS cách vệ tinh bao xa Với nhiều cách đo tới vệ tinh máy thu tính vị trí người dùng hiển thị lên đồ điện tử máy Hiện tại, GPS sử dụng tần số L1 = 1575,42MHz L2 = 1227,60MHz [1,6] Các sóng mang L1, L2 điều biến loại mã sau: - Mã P: mã xác, có tần số 10,23 MHz, độ dài tồn phần 267 ngày Tuy người ta chia mã thành đoạn có độ dài ngày gắn cho vệ tinh hệ thống GPS đoạn mã thế, sau tuần lại thay đổi nên khó bị giải mã để sử dụng ko phép Mã P điều biến sóng mang L1 L2 - Mã C/A có tần số 1,023MHz, điều biến sóng mang L1, mã C/A sử dụng cho mục đích dân sự, vệ tinh gán mã C/A riêng biệt - Mã D mã dùng để truyền lịch vệ tinh nhất, thơng số lớp khí sóng điện từ truyền qua, thời gian hệ thống, sai số đồng hồ vệ tinh, phân bố vệ tinh quỹ đạo Nó điều biến sóng mang L1 L2 Khi hoạt động, tần số L1 bao gồm tín hiệu số, gọi mã P mã C/A Mã P nhằm bảo vệ thông tin khỏi truy nhập trái phép Tuy nhiên, mục đích tín hiệu mã hóa nhằm tính tốn thời gian cần thiết để thơng tin truyền từ vệ tinh tới thiết bị thu nhận mặt đất Sau đó, khoảng cách bên tính cách nhân thời gian cần thiết để tín hiệu đến nơi với tốc độ ánh sáng 300.000 km/giây Tuy nhiên, tín hiệu bị sai đơi chút qua bầu khí Vì vậy, kèm theo thông điệp gửi tới thiết bị nhận, vệ tinh thường gửi kèm thông tin quỹ đạo thời gian Việc sử dụng đồng hồ nguyên tử đảm bảo xác thống thời gian thiết bị thu phát Để biết vị trí xác vệ tinh, thiết bị thu GPS nhận thêm loại liệu mã hóa:  Dữ liệu Almanac: cập nhật định kỳ cho biết vị trí gần vệ tinh quỹ đạo Nó truyền liên tục lưu trữ nhớ thiết bị thu nhận vệ tinh di chuyển quanh quỹ đạo  Dữ liệu Ephemeris: phần lớn vệ tinh di chuyển khỏi quỹ đạo chúng Sự thay đổi ghi nhận trạm kiểm soát mặt đất Việc sửa chữa sai số quan trọng đảm nhiệm trạm chủ Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman nhằm mục đích đưa lời giải thích mang tính vật lý rõ ràng cho việc mô máy tính Cụ thể, phương trình (A.5) đưa mô không gian ổn định (không điều khiển được) Phương trình thứ hai phản ánh điều khiển không gian ổn Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman 38 định với mục đính đồng với hệ tọa độ cấp địa phương m - quaternion xét việc tính ảnh mơ men quay cảm biến vận tốc góc Phương trình quaternion (A.5) (A.6) viết lại dạng nhân ma trận sau q0P   q P     qP    q q        q q    (A.8)     q q q  k 1  q  Để làm rõ phương trình quaterion (A.5) cần phải tính độ tăng vector quay  Từ phương trình sai phân góc quay ta có:  với góc quay nhỏ thì: Tích phân phương trình với khoảng thời gian tích phân hN3, ta thu  (A 9) Thành phần thứ hai xem “chỉnh lỗi góc” (coining corection), phản ánh vector  không trùng với vector Thuật tốn khác trình bày phương thức chỉnh lỗi góc dựa việc tính tốn xấp xỉ số học phương trình (A.9) Thuật tốn gồm có bước cụ thể sau Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman 39  (A 10)        P P Với:   P j   j   xb      zb  yb j  zb j     j   độ tăng góc tính theo cơng thức cơng thức (A.2) Do đó, thuật tốn bốn bước sử dụng cho vector quay Để hồ hợp tính tốn góc tính tốn vận tốc với bước hN3 = 8hN1, giá trị đo lân cận với mẫu thời gian lẫy mẫu hN1 tức  j  k Sử dụng giá trị vector quay vừa xác định tính quaternion:   Với    cos           yb    2   Chuỗi khai triển xấp xỉ hàm sin cho ta giá trị quarternion: Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman (A 11)  40  01    8384  r xb  r yb 2   r zb r  Các giá trị quaterion cần phải thoả mãn điều kiện chuẩn hố: q q Trong q trình tính tốn xảy lỗi tính tốn đại lượng xấp xỉ, cần phải chuẩn hố lại quarternion Nếu như:   q Thì cần tính lại (A 13) ˆ q N 1 Còn khơng có lỗi thì: qˆ Xem xét mối liên hệ quaternion ma trận chuyển đổi C mô tả sau: địa phương với gắn liền hệ tọa độ cấp (A 14) CbN chuyển hệ tọa độ Ma trận chuyển đổi hệ tọa độ cố định tâm trái đất hệ tọa độ cấp địa phương: Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman 41 x b11   y   b  21   z   b  31  với: b11 sin b12  sin b13  cos b21 sin b22 sin b23  cos b31 b32 b33 với (A 15)  cos cos  cos sin  sin vĩ độ, kinh độ  góc phương vị tính cơng thức:  cấp địa phương so với trái đất, R bán kính với trái góc hướng bắc đất Góc phương vị góc lệch trục so với trái đất lên mặt phẳng cấp địa phương có z Hình chiếu vận tốc vật thể dạng : Vx  2U  2U Wx* , Wy* Vy với: tổng độ tăng vận tốc U x ,U y ,U z hình chiếu vector vận tốc góc trái đất lên hệ tọa độ cấp địa phương x ,  y hình chiếu vector vận tốc góc hệ tọa độ cấp địa phương lên trái đất, công thức sau: Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman 42 Vy   x Ry V  x  y   Rxa 213a   Rx 1 b H  1  e 33  e b    1 b H 1  e 33  e b    Rya 223a  với: e- độ lệch tâm trái đất; alà bán trục trái đất; H- độ cao vật thể Hình chiếu vận tốc góc tuyệt đối hệ tọa độ cấp địa phương lên trục có dạng: U x x Uy y z với: U x U vận tốc góc trái đất Để tính tọa độ vị trí thuật tốn c ấp địa phương ta khơng thể lấy tích phân trực tiếp vận tốc Nguyên nhân liên quan đến lượng giá trị lớn lấy tích phân  trường hợp độ cao tương đối lớn Để giải vấn đề cần sử dụng phương trình Puasson Xét ma trận chuyển đổi từ hệ tọa độ cấp địa phương sang hệ tọa độ cố định tâm trái đất: BNE Phương trình Puasson dạng ma trận sau: (phương trình Puasson) Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman 43 Với  dạng hồi quy phương trình Puasson: N   b b 12 12 (N )  b b 22 22 (N )  b b 32 (A 19) 32 (N )  b b 13 13 (N )  b b 23 23 (N )  b b 33 33 (N )  b b 31 12 Sau xác định phần tử mâ trận chuyển, ta xác định thơng số vận tốc theo hướng bắc hướng đông trái đất theo cơng thức V N V E Lưu đồ thuật tốn Lưu đồ thuật tốn SINS Salychev trình bày Hình 22 (A 20) Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman 44 IMU G IMU xb a xb xb Bù nhiễu cảm biến vận tốc góc Bù nhiễu cảm biến gia tốc Tính độ tăng góc Tính độ tăng vận tốc t hN1 xb, yb, zb  W xb, yb,zb Chỉnh lỗi góc (Coning) hN3   Tính Quaternion -1 Q q    hN3 B ù l ỗ i v ậ n t ố c S c u l l i n g W Tính lại độ tăng vận tốc hệ tọa độ cấp địa phương Wx  W  y W  W x Hình 22 Thuật toán Salychev Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman Tính Quaternion -2 Qf n1 m m hN3 q q m  m   45 Wx Wy Wz bij 0 Tính vận tốc Vx,y Chuẩn hố tham số quaternion Tính ma trận BNE Tính ma trận B E C b i j ij Tính vận tốc Tính tốn tư V  C3 N V C0  C321  C323 N E    ar ct g C C 33  arc C C 22 V Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman ... thuật toán Kalman (Hình 7) [7]: Hình Lưu đồ tính tốn với lọc Kalman Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman 17 3.1.2 Hệ thống tích hợp INS/GPS Khi tích hợp hệ thống INS... Chương II : Hệ thống dẫn đường quán tính INS Chương III : Hệ thống tích hợp INS/GPS, lọc Kalman ứng dụng - Chương IV : Kết luận Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman CHƯƠNG... ứng dụng nhiều lĩnh vực Mục tiêu nghiên cứu phát triển hệ GPS giá rẻ, nhỏ gọn, dễ sử dụng tiến hành Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman CHƯƠNG II HỆ THỐNG INS Hệ thống