Tài liệu Nâng cao chất lượng hệ thống tích hợp ins gps 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........................................................................... 1 CHƯƠNG II. HỆ THỐNG INS .................................................................................................. 4 2.1 Nguyên lý hoạt động của INS......................................................................................................4 2.2 Ma trận chuyển hệ tọa độ ............................................................................................................5 2.2.1 Ma trận chuyển Euler...........................................................................................................5 2.2.2 Ma trận chuyển Quaternion ..................................................................................................6 2.2.3 Tính góc Euler từ ma trận xoay.............................................................................................7 2.2.4 Tính Quaternion từ ma trận xoay ..........................................................................................8 2.2.5 Tính góc Euler từ Quaternion ...............................................................................................8 2.3 Phương trình chuyển động ..........................................................................................................9 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 của 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 quả mô phỏng .................................................................................................................... 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 bộ lọc Kalman IV DANH MỤC HÌNH Hình 1. Quỹ đạo bay của các vệ tinh GPS ................................................................................. 1 Hình 2. Cấu trúc của IMU ........................................................................................................... 4 Hình 3. Hệ thống định vị quán tính............................................................................................. 5 Hình 4. 3 góc Euler ....................................................................................................................... 5 Hình 5. Sai lệch giữa hệ tọa độ p-frame và N- frame............................................................. 11 Hình 6. Thuật toán Kalman cổ điển.......................................................................................... 14 Hình 7. Lưu đồ tính toán với bộ lọc Kalman .......................................................................... 16 Hình 8. Sơ đồ INS/GPS theo phương thức vòng hở ................................................................ 17 Hình 9. 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à vòng kín (b)....................................................... 18 Hình 11. Lưu đồ thuật toán hệ thống tích hợp INS/GPS........................................................ 19 Hình 12. Bộ lọc Kalman trong mô phỏng ................................................................................ 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 của hệ INS/GPS, Vn của GPS và sai số tuyêt đối của vận tốc. .......... 25 Hình 15. Vận tốc Ve của hệ INS/GPS, Ve của GPS, và sai số tuyệt đối. ............................. 26 Hình 16. So sánh góc hướng INS/GPS và GPS. ...................................................................... 27 Hình 17. Các góc lệch E,  N, Up . .......................................................................................... 27 Hình 18. Độ trôi của cảm biến vận tốc góc Gbx, Gby. ............................................................. 28 Hình 19. Các góc chúc ( Pitch) và 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 toán Salychev.................................................................................................... 34 Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman V DANH MỤC BẢNG Bảng 1. Lỗi gây ra bởi cảm biến quán tính.............................................................................. 10 Bảng 2. Trung vị, phương sai của khoảng cách ...................................................................... 24 Bảng 3. Sai số tuyệt đối của vận tốc theo phương Đông ....................................................... 25 Bảng 4. Sai số tuyệt đối của vận tốc theo phương Bắc .......................................................... 26 Bảng 5. Các trung vị và phương sai của góc chúc và góc nghiêng ...................................... 29 Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ 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 bộ lọc Kalman VII LỜI NÓI ĐẦU Hệ thống vi cơ điện tử - MEMS là sự kết hợp của các yếu tố cơ khí, nhiệt, sinh học, hóa học, quang học, chất lỏng, và điện tử trên cùng một đế Silicon. Thông thường dựa trên công nghệ vi chế tạo. Công nghệ MEMS hứa hẹn một cuộc cách mạng thay thế nhiều sản phẩm bằng cách kết hợp nhiều thành phần trên một chíp. Công nghệ MEMS ngày càng phát triển, các thiết bị MEMS cho độ chính xác cao, hiệu suất cao, độ tin cậy lớn và giá thành hạ. Một trong những thiết bị MEMS đã được ứng dụng gần đây về lĩnh vực định vị tọa độ vật thể đó là IMU. Thiết bị này được đưa vào sử dụng trong lĩnh vực điều khiển Robot hay trong định vị tọa độ của các 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 toán dẫn đường quán tính (INS). Hiện nay, vấn đề định vị thu hút sự quan tâm của nhiều nhà nghiên cứu và công nghiệp ở trong và ngoà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 như độ chính xác và kích thước nhỏ. Để tích hợp được INS và GPS thì chúng ta thường sử dụng bộ 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 nhưng chất lượng hệ thống là chưa cao. Mục tiêu của luận văn là cải thiện chất lượng của hệ thống tích hợp INS/GPS sử dụng một 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 của toàn hệ thống, đặc biệt là các thông số về các góc tư thế, vận tốc, vị trí. Nội dung của đề tài bao gồm các 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, bộ lọc Kalman và ứ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 bộ lọc Kalman 1 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) là hệ thống xác định vị trí dựa trên vị trí của các vệ tinh nhân tạo, do Bộ Quốc phòng Hoa Kỳ thiết kế, xây dựng, vận hành và quản lý. Hệ thống định vị toàn cầu GPS thu thập các thông tin về tọa độ (vĩ độ và kinh độ), tốc độ và độ cao của 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ên các trạm phát tín hiệuvô tuyến điện. Được biết đến nhiều nhất là các 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ỹ và biến thể với độ chính 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 1. Quỹ đạo bay của các vệ tinh GPS Hệ thống định vị toàn cầu GPS thu thập các thông tin về tọa độ (vĩ độ và kinh độ), độ cao và tốc độ của các vật thể [4]. Các vệ tinh GPS bay vòng quanh Trái Đất hai lần trong một ngày theo một quỹ đạo rất chính xác và 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 này và bằng phép tính lượng giác tính được chính xác vị trí của người dùng. Về bản chất máy thu GPS so sánh thời gian tín hiệu được phát Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman 2 đi từ vệ tinh với thời gian nhận được chúng. Sai lệch về thời gian cho biết máy thu GPS ở cách vệ tinh bao xa. Với nhiều cách đo được tới vệ tinh máy thu có thể tính được vị trí của người dùng và hiển thị lên bản đồ điện tử của máy. Hiện tại, GPS sử dụng 2 tần số L1 = 1575,42MHz và L2 = 1227,60MHz [1,6]. Các sóng mang L1, L2 được điều biến bởi 3 loại mã sau: - Mã P: là mã chính xác, có tần số 10,23 MHz, độ dài toàn phần 267 ngày. Tuy vậy người ta đã chia mã này thành các đoạn có độ dài 7 ngày và gắn cho mỗi vệ tinh trong hệ thống GPS một trong các đoạn mã như thế, cứ sau 1 tuần lại thay đổi nên khó bị giải mã để sử dụng nếu ko được phép. Mã P điều biến cả 2 sóng mang L1 và L2. - Mã C/A có tần số 1,023MHz, nó chỉ điều biến sóng mang L1, mã C/A được sử dụng cho mục đích dân sự, mỗi vệ tinh được gán 1 mã C/A riêng biệt. - Mã D là mã dùng để truyền lịch vệ tinh mới nhất, thông số của lớp khí quyển sóng điện từ truyền qua, thời gian của hệ thống, sai số đồng hồ vệ tinh, phân bố của các vệ tinh trên quỹ đạo... Nó điều biến cả 2 sóng mang L1 và L2. Khi hoạt động, tần số L1 bao gồm 2 tín hiệu số, được gọi là mã P và mã C/A. Mã P nhằm bảo vệ thông tin khỏi những sự truy nhập trái phép. Tuy nhiên, mục đích chính của các tín hiệu mã hóa là nhằm tính toán thời gian cần thiết để thông tin truyền từ vệ tinh tới một thiết bị thu nhận trên mặt đất. Sau đó, khoảng cách giữa 2 bên được tính bằng cách nhân thời gian cần thiết để tín hiệu đến nơi với tốc độ của ánh sáng là 300.000 km/giây. Tuy nhiên, tín hiệu có thể bị sai đôi chút khi đi qua bầu khí quyển. Vì vậy, kèm theo thông điệp gửi tới các thiết bị nhận, các vệ tinh thường gửi kèm luôn thông tin về quỹ đạo và thời gian. Việc sử dụng đồng hồ nguyên tử sẽ đảm bảo chính xác về sự thống nhất thời gian giữa các thiết bị thu và phát. Để biết vị trí chính xác của các vệ tinh, thiết bị thu GPS còn nhận thêm 2 loại dữ liệu mã hóa:  Dữ liệu Almanac: được cập nhật định kỳ và cho biết vị trí gần đúng của các vệ tinh trên quỹ đạo. Nó truyền đi liên tục và được lưu trữ trong bộ nhớ của thiết bị thu nhận khi các vệ tinh di chuyển quanh quỹ đạo.  Dữ liệu Ephemeris: phần lớn các vệ tinh có thể hơi di chuyển ra khỏi quỹ đạo chính của chúng. Sự thay đổi này được ghi nhận bởi các trạm kiểm soát mặt đất. Việc sửa chữa những sai số này là rất quan trọng và được đảm nhiệm bởi trạm chủ Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman 3 trên mặt đất trước khi thông báo lại cho các vệ tinh biết vị trí mới của chúng. Thông tin được sửa chữa này được gọi là dữ liệu Ephemeris. Kết hợp dữ liệu Almanac và Ephemeris, các thiết bị nhận GPS biết chính xác vị trí của mỗi vệ tinh. Nhược điểm cơ bản của các hệ thống định vị là khi bị che khuất tầm nhìn vệ tinh, tín hiệu GPS bị sai số hoặc bị mất tín hiệu. Ngoài ra còn có một số nguyên nhân sau gây ra sai số của phương pháp định vị GPS [2].  Giữ chậm của tầng đối lưu và tầng ion – Tín hiệu vệ tinh bị chậm đi khi xuyên qua tầng khí quyển.  Tín hiệu đi nhiều đường – Điều này xảy ra khi tín hiệu phản xạ từ nhà hay các đối tượng khác trước khi tới máy thu.  Lỗi đồng hồ máy thu – Đồng hồ có trong máy thu không chính xác như đồng hồ nguyên tử trên các vệ tinh GPS.  Lỗi quỹ đạo – Cũng được biết như lỗi thiên văn, do vệ tinh thông báo vị trí không chính xác.  Số lượng vệ tinh nhìn thấy – Càng nhiều vệ tinh được máy thu GPS nhìn thấy thì càng chính xác. Nhà cao tầng, địa hình, nhiễu loạn điện tử hoặc đôi khi thậm chí tán lá dầy có thể chặn thu nhận tín hiệu, gây lỗi định vị hoặc không định vị được. Nói chung máy thu GPS không làm việc trong nhà, dưới nước hoặc dưới đất.  Che khuất về hình học – Điều này liên quan tới vị trí tương đối của các vệ tinh ở thời điểm bất kì. Phân bố vệ tinh lí tưởng là khi các quả vệ tinh ở vị trí tạo các góc rộng với nhau. Phân bố xấu xảy ra khi các quả vệ tinh ở trên một đường thẳng hoặc cụm thành nhóm.  Sự giảm có chủ tâm tín hiệu vệ tinh – Là sự làm giảm tín hiệu cố ý do sự áp đặt của Bộ Quốc phòng Mỹ, nhằm chống lại việc đối thủ quân sự dùng tín hiệu GPS chính xác cao. Chính phủ Mỹ đã ngừng việc này từ tháng 5 năm 2000, làm tăng đáng kể độ chính xác của máy thu GPS dân sự. (Tuy nhiên biện pháp này hoàn toàn có thể được sử dụng lại trong những điều kiện cụ thể. Chính điều này là tiềm ẩn hạn chế an toàn cho dẫn đường và định vị dân sự). Trong những năm gần đây, có nhiều hệ thống định vị vệ tinh trên thế giới như là hệ thống GLONASS của Liên bang Nga, hệ thống Galileo của Liên minh Châu Âu. Xu hướng trong thời gian tới, thiết bị thu GPS được ứng dụng trong rất 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 vẫn đang được tiến hành. Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman 4 CHƯƠNG II. HỆ THỐNG INS Hệ thống định vị quán tính phát triển từ những năm 1960. Tiến sĩ Robert Goddard, một nhà tiên phong trong lĩnh vực công nghệ tên lửa, đưa ra ý tưởng thực nghiệm một hệ gyroscope thô sơ đã khơi dậy trào lưu nghiên cứu về INS trên thế giới. Sau đó INS được đưa vào ứng dụng trong lĩnh vực công nghệ tên lửa đầu tiên, tiếp đó mở rộng qua các nghành hàng không dân dụng và quân sự, vận tải biển…. 2.1 Nguyên lý hoạt động của INS Nguyên lý hoạt động của INS là tổng hợp các tín hiệu đo được bởi một cơ cấu đo lường quán tính IMU (Inertial measurement Unit), để xác định trạng thái hiện tại của hệ thống. Các tín hiệu đo được bởi IMU bao gồm vận tốc góc (gyros) và gia tốc (accelerometer). Cấu tạo IMU bao gồm 3 cảm biến gia tốc và 3 cảm biến vận tốc góc. Ở đây một hệ thống INS bao gồm IMU và thuật toán tính toán được tích hợp cùng. Thuật toán sử dụng trong luận văn này là thuật toán SINS của Salychev (Phụ lục 2). INS đưa ra các thông số cho người dùng như: vị trí (kinh độ, vĩ độ, cao độ), vận tốc, các góc tư thế (góc nghiêng, góc chúc, góc hướng) (xem Hình 3). Hình 2 dưới đây mô tả một khối IMU [2]. Với những IMU trước đây sử dụng cảm biến quán tính hoạt động theo nguyên tắc cơ khí, thường có kích thước lớn, hoạt động kém hiệu quả, giá thành cao và tiêu thụ nhiều năng lượng. Ngày nay khi khoa học công nghệ phát triển, đặc biệt là công nghệ vật liệu mới và công nghệ vi chế tạo đã tạo ra các cảm biến vi cơ có kích thước rất nhỏ (cỡ centimet), hoạt động hiệu quả, tiêu thụ ít năng lượng và đặc biệt là giá thành hạ. Điều này mở ra cơ hội cho việc ứng dụng các cảm biến vi cơ trong nhiều lĩnh vực đời sống. Hình 2. Cấu trúc của IMU Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman 5 Hình 3 dưới đây mô tả nguyên lí hoạt động của hệ thống INS [8]. Accelerometters Gyros INPUT Initial alignment Alignment phase OUT PUT Gravity, Non-gravitational acceleration correction Velocity Coordinate system Transformation acceleration Attitude Attitude Computing integration integration Position Navigational Phase Hình 3. Hệ thống định vị quán tính 2.2 Ma trận chuyển hệ tọa độ Trong thuật toán SINS, để đưa ra các thông số cuối cùng, chúng ta cần các phép tính toán chuyển đổi qua các hệ tọa độ với nhau. Sau đây học viên đưa ra một số ma trận chuyển đổi chính trong khi sử dụng thuật toán SINS. 2.2.1 Ma trận chuyển Euler Một vấn đề quan trọng khi xác định vật thể trong không gian 3 chiều đó là tọa độ của vật trong hệ tọa độ Đề Các hoặc là 3 góc Euler [5]. Hình 4. 3 góc Euler Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman 6 Các góc Euler (, , ) (Hình 4) thay thế cho ba chuyển động quay kết hợp, di chuyển hệ trục tham chiếu đến một hệ trục ta đang xét. Các chuyển động thành phần là chuyển động quanh một trục, có thể là , hoặc . Tương ứng với các chuyển động này, ta có ma trận xoay thành phần được định nghĩa như sau.  [  [  [     ]   ] (2.2) ]  (2.3)     (2.1) Trong đó R(Z,), R(Y, θ) và R(X, ) lần lượt là các ma trận góc xoay quanh trục x, y, z theo các góc tương ứng , , . Khi đó ta có ma trận xoay tổng quát trong không gian 3 chiều Euclide như sau: Rxyz cos   sin  0  sin cos 0 0 0 1   cos   0    sin  0 sin   1 0   1 0  0 cos  0 cos   0 sin  0   sin   cos   (2.4) Hay [                              ] (2.5) Phương trình (2.5) được sử dụng phản hồi để hiệu chỉnh lại các giá trị Quaternion trong mô hình Simulink. 2.2.2 Ma trận chuyển Quaternion Ngoài cách xác định ma trận chuyển đổi giữa các hệ tọa độ bằng góc Euler, ta có thể sử dụng tới kĩ thuật tính quaternion đơn giản hơn. Một quaternion đơn vị có thể được định nghĩa như sau: [ | ]T | Chúng ta có thể kết hợp quaternion với một chuyển động quay quanh một trục như sau: Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman 7 (2.6) Trong công thức (2.6) trên, cos ( là góc quay (giá trị tính bằng Radian) và cos ( ), cos ( ), ) là các cosin định hướng của trục. Khi đó ma trận quay của chuyển động quay trong không gian 3 chiều Euclide được xác định theo (2.7) như sau : [ ] (2.7) 2.2.3 Tính góc Euler từ ma trận xoay. Giả sử ta có một ma trận xoay biết trước là [ ] (2.8) Từ phương trình (2.5) ta được: tan = a32 /a33 , sin = -a31, tan = a21 /a11. Như vậy    { à  à  (2.9) (2.10)    { à  à Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman (2.11) 8 2.2.4 Tính Quaternion từ ma trận xoay Với các góc Euler biết trước, ta có thể tính được Quaternion được xác định theo công thức (2.13) [10]. Từ phương trình (2.7) ta thu được các phương trình sau: = = = (2.12) 1= Do đó | Giả sử rằng | √ | | √ | | √ | | √ (2.13) là dương sign( ) = sign(1) sign( ) = sign( - ) sign( ) = sign( - ) sign( ) = sign( - ) 2.2.5 Tính góc Euler từ Quaternion Từ phương trình (2.5), (2.7) ta tính được các góc Euler như sau:  [ ]  [ ] Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman (2.14) 9 2.3 Phương trình chuyển động Tư thế của vật thể trong không gian được xác định bởi 3 góc Euler [4,6]. Mối liên hệ giữa các vận tốc góc của Roll, Pitch, Yaw (p, q, r) và 3 góc Euler , ,  được tính theo công thức (2.15) sau: ̇ [ ̇ ] ̇  (       ) [ ]   (2.15) Như vậy khi tích phân các phương trình trên ta thu được các góc Euler. Các gia tốc (ax, ay , az) dọc theo các trục của hệ tọa độ vật thể liên hệ với 3 vận tốc (U, V, W) trong hệ tọa độ trái đất theo phương trình sau : ̇  ̇  ̇   (2.16)  Sau khi tích phân phương trình (2.16), chúng ta thu được U, V, W. Sử dụng ma trận Direct Cosine Matrix (DCM), chúng ta có thể chuyển từ hệ tọa độ trái đất sang hệ tọa độ định vị: ̇ [ ̇] ̇ [ ] [ ] (2.17) Trong đó : DCM phương trình (2.5) Tích phân phương trình (2.17), ta thu được vị trí của vật. Tiếp theo chúng ta có thể thu được kinh độ, vĩ độ và độ cao của vật thông qua các phương trình (2.18) sau : ̇ ̇ (2.18) ̇ Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman 10 2.4 Mô hình lỗi INS Có nhiều loại sai số trong các hệ thống INS và chủ yếu là do các cảm biến quán tính gây nên [1,6]. Bảng 1 liệt kê một số lỗi gây ra bởi các cảm biến gia tốc và vận tốc góc. Loại Gây nên sai số Lỗi vị trí khi lắp đặt cảm biến. Góc nghiêng, góc chúc và góc hướng. Độ lệch (offset) của cảm biến gia Lối ra cảm biến gia tốc sẽ bị lệch đi một giá trị tốc. không đổi. Giá trị này lại thay đổi mỗi khi tắt /bật thiết bị. Hiện tượng lệch và trôi của cảm Vật thể không chuyển động nhưng vẫn có vận tốc biến vận tốc góc (do tác động của góc không đổi. nhiệt độ). Nhiễu ngẫu nhiên. Lỗi ngẫu nhiên trong đo lường. Bảng 1. Lỗi gây ra bởi cảm biến quán tính Những lỗi trong đo gia tốc và vận tốc góc sẽ dẫn tới các lỗi tăng dần khi xác định vị trí, vận tốc, và góc tư thế của vật thể bay. Các lỗi tăng dần này được gọi là lỗi dẫn đường. Có thể nhận thấy chắc chắn rằng hệ thống dẫn đường quán tính không thể hoạt động tự trị được mà phải được kết hợp với một hệ thống khác. Các véc tơ trạng thái lỗi của INS bao gồm lỗi hệ tọa độ, lỗi vận tốc, và các lỗi tư thế. Khi các lỗi thay đổi theo thời gian, chúng được mô tả thông qua các phương trình vi phân. Đối với các góc lệch nhỏ  N,  E,  Up [7,9] là độ lệch giữa hệ tọa độ p-frame và llframe (xem Hình 5) [8]. Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman 11 Up Zp  Up North N E Yp East Xp Hình 5. Sai lệch giữa hệ tọa độ p-frame và N- frame Ma trận chuyển giữa 2 hệ tọa độ này có thể được mô tả theo (2.19), được suy ra từ dạng ma trận DCM chuẩn, và giả sử các góc này nhỏ thỏa mãn cos Φ = 1 và sin Φ = Φ [7,9].    [  ]  (2.19)  Theo phương trình Poisson, vận tốc góc trong hệ tọa độ p-frame có thể được biểu diễn theo vận tốc góc tuyệt đối trong hệ tọa độ ll-frame và các đạo hàm của các lỗi tư thế  N ,  E,  Up . [ Dựa theo sai số của cảm biến  ] [ E , N, ̇ [̇ ]  ̇ ]  Up (2.20) thì : (2.21) Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman 12 Lỗi tính toán vận tốc góc có thể thu được bằng cách vi phân các biểu thức theo hình chiếu trên hệ tọa độ ll-frame. (2.22) Trong đó là vận tốc và các lỗi tương ứng trên hệ tọa độ ll-frame. , : vĩ độ và lỗi vĩ độ. U : tốc độ quay của trái đất. Thay phương trình (2.21) và (2.22) vào (2.20) ta được : ̇ (2.23) ̇ (2.24)  ̇ (2.25) Tương tự các gia tốc cũng được biểu diễn như sau : [ Trong đó : E, N, up ] [ ] [ ] [ ] là các hệ số tỉ lệ gia tốc. BN, BE, BUp là các độ lệch 0 của gia tốc.       Đưa thêm lực Coriolis vào trong tính toán lỗi ta thu được : Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman (2.26) 13 ̇   (2.27) ̇   [ ] (2.28) Sự khác nhau giữa vận tốc góc trong hệ tọa độ p-frame và ll-frane là nguyên nhân chính gây ra độ trôi gyro và các lỗi tính toán các vận tốc góc chuyển từ ll-frame sang Eframe. Ở đây, lỗi vị trí có thể thu được từ các phương trình sau: ̇ ̇ Gỉa sử h = 0 thì (2.29) Như vậy ta có thể đơn giản các phương trình lỗi của INS như sau : ̇  ̇  ̇ ̇ ̇         Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman (2.30)