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 .8 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 toá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ị qn 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 Tiếng Anh Tiếng Việt IMU Inertial Measurement Units Khối đo quán tính GPS Global Positioning System Hệ thống định vị toàn cầu KF Kalman Filter Bộ lọc Kalman INS Inertial Navigation System Hệ thống dẫn đường quán tính SINS Strapdown Inertial Navigation System Thuật toán dẫn đường quán tính kiểu gắn chặt DCM Direct Cosin Matrix Ma trận hướng Cosine b-frame Body frame Hệ tọa độ vật thể ll-frame Local level frame Hệ tọa độ địa phương N- frame Navigation frame Hệ tọa độ dẫn đường 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 qn 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 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 toà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ị tồ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ị toà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 toà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 cịn 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 mặt đất trước thông báo lại cho vệ tinh biết vị trí chúng Thơng tin sửa chữa gọi liệu Ephemeris Kết hợp liệu Almanac Ephemeris, thiết bị nhận GPS biết xác vị trí vệ tinh Nhược điểm hệ thống định vị bị che khuất tầm nhìn vệ tinh, tín hiệu GPS bị sai số bị tín hiệu Ngồi cịn có số ngun nhân sau gây sai số phương pháp định vị GPS [2]  Giữ chậm tầng đối lưu tầng ion – Tín hiệu vệ tinh bị chậm xuyên qua tầng khí  Tín hiệu nhiều đường – Điều xảy tín hiệu phản xạ từ nhà hay đối tượng khác trước tới máy thu  Lỗi đồng hồ máy thu – Đồng hồ có máy thu khơng xác đồng hồ ngun tử vệ tinh GPS  Lỗi quỹ đạo – Cũng biết lỗi thiên văn, vệ tinh thơng báo vị trí khơng xác  Số lượng vệ tinh nhìn thấy – Càng nhiều vệ tinh máy thu GPS nhìn thấy xác Nhà cao tầng, địa hình, nhiễu loạn điện tử đơi chí tán dầy chặn thu nhận tín hiệu, gây lỗi định vị khơng định vị Nói chung máy thu GPS khơng làm việc nhà, nước đất  Che khuất hình học – Điều liên quan tới vị trí tương đối vệ tinh thời điểm Phân bố vệ tinh lí tưởng vệ tinh vị trí tạo góc rộng với Phân bố xấu xảy vệ tinh đường thẳng cụm thành nhóm  Sự giảm có chủ tâm tín hiệu vệ tinh – Là làm giảm tín hiệu cố ý áp đặt Bộ Quốc phòng Mỹ, nhằm chống lại việc đối thủ quân dùng tín hiệu GPS xác cao Chính phủ Mỹ ngừng việc từ tháng năm 2000, làm tăng đáng kể độ xác máy thu GPS dân (Tuy nhiên biện pháp hồn tồn sử dụng lại điều kiện cụ thể Chính điều tiềm ẩn hạn chế an tồn cho dẫn đường định vị dân sự) Trong năm gần đây, có nhiều hệ thống định vị vệ tinh giới hệ thống GLONASS Liên bang Nga, hệ thống Galileo Liên minh Châu Âu Xu hướng thời gian tới, thiết bị thu GPS ứ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 31 TÀI LIỆU THAM KHẢO Tiếng Việt Trần Đức Tân, Nghiên cứu, thiết kế lọc Kalman cải tiến, đề tài QC 07.17, Đại học Quốc gia Hà Nội Lưu Mạnh Hà, Nghiên cứu tích hợp hệ dẫn đường quán tính hệ định vị toàn cầu (INS/GPS), Luận văn thạc sĩ, Trường đại học Công nghệ, Đại học Quốc Gia Hà Nội, tr.21-35 Luu Manh Ha, Tran Duc Tan, Chu Duc Trinh, Nguyen Thang Long, Nguyen Dinh Duc, INS/GPS Navigation for Land Applications via GSM/GPRS Network, IEICE ICDV 2011, pp 86-90 Tiếng Anh Tan, T.D Ha, L.M Long, N.T Tue, H.H Thuy, N.P, 2008, Novel MEMS INS/GPS Integration Scheme Using Parallel Kalman Filters, ATC, System Integration, 2008 IEEE/SICE International Symposium, pp 72-76 T D Tan, H H Tue, N T Long, N P Thuy, N V Chuc, Designing Kalman Filters for Integration of Inertial Navigation System and Global Positioning System, the 10th biennial Vietnam Conference on Radio & Electronics, Hanoi, 2006, pp 226-230 Vikas Kumar N, 2004, Integration of Inertial Navigation System and Global Positioning System Using Kalman Filtering, M.Tech Dissertation, Indian Institute Of Technology, Bombay, July 2004, pp 8-14 Salycheva, A.O., 2004 Medium Accuracy INS/GPS Integration in Various GPS Environment, M.Sc Thesis University of Calgary, Geomatic Engineering Dept, pp 62-67; 78-79; 94-95 Xiaoying Kon, Inertial Navigation System Algorithms for low cost IMU, Doctor thesis at the University of Sydney, August 2000, pp 11-19 Oleg S Salychev, Inertial Systems in Navigation and Geophysics, Bauman MSTU Press, 1998, pp 91-95 10 Wen zhang, Mounir Ghogho, and Baolun Yuan, Mathematical Model and Matlab Simulation of Strapdown Inertial Navigation System, Modelling and Simulation in Engineering, September 2011, 25 pages Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman 32 PHỤ LỤC Các hệ tọa độ 1.1 Hệ tọa độ quán tính (i- Frame) - Gốc hệ tọa độ: trùng với tâm trái đất - Trục z trục quay trái đất có chiều dương hướng lên bắc cực - Trục x, trục y tùy ý thuộc mặt phẳng xích đạo cho trục x, y, z theo hệ thuận 1.2 Hệ tọa độ cố định tâm trái đất (e-frame) -Gốc tọa độ: tâm trái đất - Trục x : có chiều dương quay phía giao điểm đường kinh tuyến đường xích đạo - Trục z : trục quay trái đất có chiều dương hướng lên bắc cực - Trục y tích có hướng trục z trục x cho hệ tọa độ cố định tâm trái đất hệ thuận 1.3 Hệ tọa độ gắn liền vật thể (b-frame) (Hình 20) - Gốc hệ tọa độ : tâm gia tốc IMU trực giao - Trục x : hướng vng góc mặt phải IMU - Trục y : hướng vng góc vởi mặt đối diện IMU - Trục z : có chiều dương hướng lên Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman 33 Hình 20 Hệ tọa độ vật thể 1.4 Hệ tọa độ định vị (N-frame) - Gốc hệ tọa độ: trùng tâm vật thể - Trục E: theo hướng đông - Trục N: theo hướng bắc - Trục Up: hướng lên vng góc bề mặt trái đất-hệ ENU (Hình 21) Ngồi cịn sử dụng hệ định vị NED có trục D hướng xuống tâm trái đất Hình 21 Hệ tọa độ định vị Thuật toán SINS [2] Thuật toán SINS Salychev chia làm hai phần Phần thứ xử lý thông tin cảm biến gia tốc Phần thứ hai xử lý thông tin cảm biến vận tốc góc Bộ liệu cảm biến gia tốc sử dụng cho việc tính tốn độ lệch cảm biến vận tốc Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman 34 góc, lỗi tỷ lệ lỗi khởi tạo Sau bù lỗi ta tính độ tăng gia tốc theo công thức: Wxb , yb , zb   t k  hN (A.1) a xb , yb , zb dt tk Với: xb , y b , z b - Hệ tọa độ gắn liền a xb , yb , zb - Đầu cảm biến gia tốc hN1 - Chu kỳ lấy mẫu Các trình tương tự sử dụng cho liệu thu từ cảm biến vận tốc góc Ở đây, trước hết tất lỗi độ lệch, lỗi tỷ lệ lỗi khởi tạo bù trừ Độ tăng gia tốc tính theo cơng thức: xb , yb , zb Ở đây: xb , yb , zb  t k  hN xb , yb , zb tk dt (A 2) đầu cảm biến vận tốc góc Thơng thường tốc độ liệu từ cảm biến gia tốc cảm biến vận tốc góc có tần số cao, từ 100Hz đến 600Hz ứng dụng thực tế lại khơng cần tần số cao mà vào khoảng 40-60Hz Để giảm tần số khung liệu, cần sử dụng tiền tích phân cho liệu từ cảm biến gia tốc cảm biến vận tốc góc Ví dụ tiền tích phân cảm biến gia tốc Gia tốc đối tượng tuyệt đối viết dạng công thức lực Coriolis: dV dt I  dV dt b  b V dV dt I  đạo hàm tổng thể vector vận tốc tuyệt đối hệ tọa độ quán tính dV dt b  đạo hàm phần vector vận tốc tuyệt đối hệ tọa độ gắn liền b (A 3) vân tốc góc tuyệt đối hệ tọa độ gắn liền Hình chiếu thành phần riêng lên hệ tọa độ gắn liền coi đầu cảm biến gia tốc Do đó, tích phân chúng xem xét tích phân hệ tọa độ gắn liền Từ phương trình (A.3) viết dạng đạo hàm phần vận tốc Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman 35 dV dt b  dV dt I  b V Lấy tích phân hai vế với khoảng thời gian hN3 = 8hN, ta được: t k  hN dV t k  hN dV xb xb  dt   dt   t t k k dt dt t k  hN dV yb t k  hN dV yb t k  hN dt  dt  tk tk tk  dt dt t k  hN dV t k  hN dV t k  hN zb zb  dt   dt   tk tk tk dt dt  t k  hN tk V zb dt V yb  yb V zb  zb V xb  xb zb xb yb V xb dt V zb dt Hay: W xb ,k  W xb ,k 1  W yb ,k 1 zb, k  W zb,k 1 yb , k  W xb ,k W yb ,k  W yb ,k 1  W zb,k 1 xb , k  W xb ,k 1 zb, k  W yb ,k W zb,k  W zb,k 1  W xb ,k 1 yb , k  W yb ,k 1 xb , k  W zb,k W zb,k  W zb,k 1  W xb ,k yb , k  W yb ,k xb , k  W zb,k W yb ,k  W yb ,k 1  W zb,k xb , k  W xb ,k zb, k  W yb ,k W xb ,k  W xb ,k 1  W yb ,k zb, k  W zb,k yb , k  W xb ,k (A 4) với điều kiện ban đầu Wxb  Wyb  Wzb  ; Quá trình diễn số bước (ở bước) Tích phân theo phương pháp cách văn hoa gọi bù lỗi vận tốc (sculling compensation) Sau trình bù lỗi vận tốc ta tính độ tăng vận tốc hệ tọa độ gắn liền sau:  W x  W xb     N  W y   Cb W yb   W  W zb    Với CbN ma trận chuyển đổi từ hệ tọa độ gắn liền xb , yb , z b sang hệ tọa độ cấp địa phương x, y, z Việc xác định ma trận chuyển CbN thực thuật tốn tính tư vật thể Bước thuật toán dẫn đường xác định ma trận chuyển đổi hệ tọa độ cấp địa phương hệ tọa độ gắn liền Ở đây, ta cần phải sử dụng phương Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman 36 trình Poisson Nhưng hệ thống thực tế cần phải sử dụng đến kỹ thuật tính quaternion Đối với thuật tốn cấp địa phương q trình quaternion gồm có hai bước Bước xác định quaternion hệ tọa độ gắn liền hệ tọa độ cấp địa phương, với điều kiện hệ tọa độ cấp địa phương khơng thay đổi vị trí suốt khoảng thời gian lấy mẫu cuối trước Điều có nghĩa hệ tọa độ cấp địa phương xem xét hệ quy chiếu qn tính suốt mẫu Phương trình quaternion chuyển đổi từ hệ quy chiếu khơng q tính (hệ tọa độ gắn liền) sang hệ quy chiếu quán tính có dạng sau: (A 5) QnP1  Qnf  Ở đây: Q f quaternion sau Q P quaternion trước     1i   j   3k Quaternion phép quay nhỏ thể dạng vector quay sau     cos ;    xb     1 sin  ;     yb     2 sin  ;     zb     3 sin  ;     Bước thứ hai việc chỉnh lại quaternion theo thay đổi hệ tọa độ cấp địa phương khơng gian qn tính khoảng thời gian mẫu cuối Việc chỉnh lỗi có thê xem phép chuyển từ hệ tọa độ quán tính sang hệ tọa độ cấp địa phương Giá trị quaternion tính sau: (A 6) Qnf1  m*Qnp1 Với: m*  m0  m1i  m2 j  m3 k Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman 37 quaternion giá trị quay nhỏ hệ tọa độ định vị hệ tọa độ qn tính Điều mơ tả vector quay hệ tọa độ Vector quay trường hợp trình bày dạng phương trình vi phân sau:           với vận tốc góc quay hai khung  Do m* mơ tả sau m*  m0  m1i  m2 j  m3 k với  h  m0*  cos N     x  hN  m1*  sin      y  hN  m2*  sin     m3*  với x  , z y ,  h  sin  N    z hình chiếu vận tốc góc tuyệt đối hệ tọa độ cấp địa phương hN3 : chu kỳ lấy mẫu Quá trình xem xét có dạng hồi quy đầu phương trình (A.6) lối vào phương trình (A.5) mẫu Việc phân chia tính tốn quaternion làm hai bước có lý cụ thể Thật vậy, phương trình (A.5) mơ tả việc chuyển đổi hệ tọa độ gắn liền hệ tọa độ qn tính coi chuyển động quay nhanh Các góc hệ tọa độ có giá lớn Ngược lại chuyển đổi hệ tọa độ quán tính hệ tọa độ cấp địa phương coi chuyển động quay chậm Rõ ràng việc tính tốn quaternion cơng thức dẫn đến việc cộng gộp hai vận tốc góc có giá trị khác Vận tốc góc có bậc lớn gấp ba lần bốn lần so với vận tốc góc kia, điều dẫn đến lỗi máy tính tính tốn Hơn nữa, việc phân chia bước tính tốn 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    P  q1     q 2P    P  q3  k 1              q0f  f   q1   q 2f    q3f       (A.7)  q3P  m0      q 2P   m1  q1P  m2     q0P  k 1  m3  (A.8) q0f  f q1 q 2f  f q3  q0P   P   q1  q 2P   P  k 1 q3  q1P q0P  q 2P q3P  q3P q 2P q0P  q1P Để 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ó:             2 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 t n  hN t n  hN   dt    dt   (A 9) tn tn 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     xb   j41     yb      zb   j 41    j 1    P1  P2     xb  j      2   P1 yb  j   3        j zb   3    yb 3     zb 3 xb 2    yb 2   P3    zb 2  xb  4    P1  P2  yb 4    30   zb 4    xb 4   yb 4     zb 4   xb 3    yb 3     zb 3 xb (A 10) 4   yb 4    zb 4   xb Với:   zb  j  yb  j     Pj   zb  j   xb  j ; j  1,2,3,4  yb  j   xb  j     j      j   độ tăng góc tính theo cơng thức công thức (A.2) yb  j   zb  j  xb 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 ,  j  cần có giá trị đo lân cận với mẫu thời gian lẫy mẫu hN1 tức  j  k hN1   k 1 hN1  Sử dụng giá trị vector quay vừa xác định tính quaternion:      1i   j   k Với         cos ;    xb     sin  ;     yb     sin  ;      zb    sin  ;    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      384  r xb   r yb   r zb r  1 (A 12)  4   42 3840 Các giá trị quaterion cần phải thoả mãn điều kiện chuẩn hoá: q   q12  q22  q32  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ư:      q02  q12  q22  q32   Thì cần tính lại qˆ N 1     q N 1 1   2 1   q n1 (A 13) Cịn khơng có lỗi thì: qˆ N 1  q N 1 Xem xét mối liên hệ quaternion ma trận chuyển đổi CbN mô tả sau: c11  qˆ 02  qˆ12  qˆ 22  qˆ 32 c12  2qˆ1 qˆ  qˆ qˆ  c13  2qˆ1 qˆ  qˆ qˆ  c 21  2qˆ1 qˆ  qˆ qˆ  c 22  qˆ 02  qˆ 22  qˆ12  qˆ 32 c 23  2qˆ qˆ  qˆ qˆ1  (A 14) c31  2qˆ1 qˆ  qˆ qˆ  c32  2qˆ1 qˆ  qˆ qˆ  c33  qˆ 02  qˆ 32  qˆ12  qˆ 22 với c11, c12 , c13 , c21, c22 , c23 , c31, c32 , c33 phần tử ma trân CbN chuyển hệ tọa độ gắn liền hệ tọa độ cấp địa phương 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 b12  y   b    21 b22  z  b31 b32 b13   x E  b23   y E  b33   z E  với: b11   sin cos sin   sin cos  b12  sin sin sin   cos cos  b13  cos sin  b21   sin cos cos   sin sin  b22   sin sin cos   cos sin  (A 15) b23  cos cos  b31  cos cos b32  cos sin b33  sin với vĩ độ, kinh độ  góc phương vị tính công thức:   VE tg R với V E vận tốc theo hướng đông hệ cấp địa phương so với trái đất, R bán kính trái đất Góc phương vị góc lệch trục z góc hướng bắc Hình chiếu vận tốc vật thể so với trái đất lên mặt phẳng cấp địa phương có dạng : V x  W x*   V y 2U z  V z  y  2U y dt t t0 (A 16) t V y  W y*   V x 2U z  V z  x  2U x dt t0 với: Wx* , W y* 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 x   y  Vy Ry  Vx e b13b23 a Vx V y  e b13b23 Rx a (A 17) b 1 H  1  e  e b13   Rx a a  33 b2 1 H  1  e 33  e b23   Ry a a  với: e- độ lệch tâm trái đất; a- 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: x  x U x y  y U y z Uz (A 18) với: U x  Ub13 ,U y  Ub23 ,U z  Ub33 ; 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  VE R cos 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: B NE Phương trình Puasson dạng ma trận sau: B NE  BNE  N E (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  N E      y  0 x y    x   dạng hồi quy phương trình Puasson: b12  N   b12  N  1   y b32  N  1hN b22 ( N )  b22  N  1   x b32  N  1hN b32 ( N )  b32  N  1   y b12  N  1   x b22  N  1hN b13 ( N )  b13  N  1   y b33  N  1hN (A 19) b23 ( N )  b23  N  1   x b33  N  1hN b33 ( N )  b33  N  1   y b13  N  1   x b23  N  1hN b31 ( N )  b12  N b23  N   b22  N b13  N  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 VN  Vy cos   Vx sin  VE  Vy sin   Vx cos  Lưu đồ thuật toán Lưu đồ thuật tốn SINS Salychev trình bày Hình 22 Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman (A 20) 44 IMU Gxb xb Gyb Gzb yb zb IMU a xb Axb Ayb Azb a yb a xb a zb 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 xb , yb , zb  t k  h N 1  hN1 xb , yb , zb dt Wxb , yb , zb  tk  hN 1 a xb , yb , zb dt tk tk hN1 Wxb, yb , zb Chỉnh lỗi góc (Coning) Bù lỗi vận tốc Sculling hN3  xb  zb  yb Wxb W yb hN3 Wzb Tính Quaternion -1 Qnf1  Qnf  qi fm         cos ;    xb     sin  ;     yb     sin  ;      zb    sin  ;    q 0p q1p Tính lại độ tăng vận tốc hệ tọa độ cấp địa phương Wx,y  W x  W xb     n W y   Cb W yb   W  W zb    Wx q3p q 2p Hình 22 Thuật toán Salychev C ij Wy Wz Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman 45 q1p q 0p q 2p Wx q 3p W y Wz Tính Quaternion -2 Qnf1  m*Qnp1 bij 0 Tính vận tốc Vx,y  h  m0*  cos N     x  h  m1*  sin  N     y  h  m2*  sin  N    m3*   q if z B E N Tính ma trận BNE  h  sin  N    Vx, y bij q0f q if q 2f q1f q 2f Tính vận tốc q0p Chuẩn hố tham số quaternion Tính ma trận CbN VE  V y sin   Vx cos  Cij Tính tốn tư   arctg VN  V y cos   Vx sin  VN , VE C 32 ; C0 2 C  C 31  C 33 hN3 g  arctg   arc  g C 31 C 33 C12 C 22  Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng lọc Kalman