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

Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman THUẬT NGỮ VIẾT TẮT IMU Inertial Measurement Units Khối đo quán tính GPS Global Positioning System Hệ thống định vị to

Trang 1

ĐẠI HỌC QUỐC GIA HÀ NỘI TRƯỜNG ĐẠI HỌC CÔNG NGHỆ

Trang 2

ĐẠ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

Trang 3

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

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

Trang 4

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 G bx , G by 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

Trang 5

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

Trang 6

THUẬT NGỮ VIẾT TẮ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

Trang 7

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

Trang 8

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

Trang 9

đ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ủ

Trang 10

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

Trang 11

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

Trang 12

Hình 3 dưới đây mô tả nguyên lí hoạt động của hệ thống INS [8]

Attitude Computing

Coordinate system Transformation integration integration

OUT PUT

Navigational Phase

Attitude

Velocity Position

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

Trang 13

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.3)

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:

cos sin 0 cos 0 sin 1 0 0sin cos 0 0 1 0 0 cos sin

0 0 1 sin 0 cos 0 sin cos

Trang 14

Trong công thức (2.6) trên, là góc quay (giá trị tính bằng Radian) và cos ( ), 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 :

[

Trang 15

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:

] (2.14)

Trang 16

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 :

Trang 17

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

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

tốc

Lối ra cảm biến gia tốc sẽ bị lệch đi một giá trị 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

biến vận tốc góc (do tác động của

nhiệt độ)

Vật thể không chuyển động nhưng vẫn có vận tốc góc không đổi

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à

ll-frame (xem Hình 5) [8]

Trang 18



Up

N E

East

North

Up Zp

Xp

Yp

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]

Trang 19

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 :

Trang 20

̇  

(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 E-frame Ở đâ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 :

Trang 21

CHƯƠNG III HỆ THỐNG TÍCH HỢP INS/GPS

Khi sử dụng riêng rẽ từng thiết bị INS, GPS thì mỗi thiết bị đều bộc lộ ưu điểm, nhược điểm riêng và khó khắc phục nhược điểm của nó Sự tích hợp của GPS với một hệ thống định vị quán tính INS cải thiện chất lượng và khuyết điểm của từng hệ thống: Sử dụng GPS để căn chuẩn độ lệch không (bias) cho các thiết bị quán tính, và INS có thể được sử dụng để cải thiện định vị và thu thập dữ liệu của máy thu GPS Khi kết hợp INS

và GPS và sử dụng bộ lọc Kalman, chúng ta có thể ước lượng được cả lỗi của INS và GPS Do đó cho người sử dụng biết thông tin vị trí tốt hơn

3.1 Lý Thuyết

3.1.1 Bộ lọc Kalman

Để định vị đối tượng chuyển động ta thường sử dụng các công cụ tính toán dự báo đến điểm tiếp theo của đối tượng dựa trên cơ sở sử dụng bộ lọc Kalman Bộ lọc Kalman bao gồm các phương pháp tính toán truy hồi hiệu quả cho phép ước đoán trạng thái của một quá trình

Mô hình của bộ lọc Kalman (Hình 6) bao gồm 2 bước sau [2]:

 Bước dự đoán: trạng thái tiếp theo của hệ thống được dự báo bởi các giá trị đo trước

 Bước cập nhật: các trạng thái hiện tại của hệ thống được đánh giá bởi các số liệu

đo được tại thời điểm đó

Hình 6 Thuật toán Kalman cổ điển

Trang 22

Giả sử bộ lọc Kalman tuyến tính như sau [6]:

Trong đó: xk là vector trạng thái tại thời điểm k

Ak,k-1 là ma trận chuyển có kích thước (nxn);

Gk,k-1 là ma trận đầu vào có kích thước (nxr);

wk-1 là ma trận nhiều đầu vào có kích thước (rx1)

Vector đo lường có dạng :

Ở đây zk là vector đo lường, kích thước (mx1); Hk là ma trận đo, kích thước (mxn)

w và v có hàm phân bố theo hàm Gauss với trung bình bằng 0 và ma trận hiệp phương sai

(covariance) lần lượt là Q và R

w~N(0,Q);

v~N(0,R);

Giả sử xk- và xk+ lần lượt là tiên nghiệm và hậu nghiệm ước lượng của véc tơ trạng

thái x tại thời điểm k và đã biết ma trận hiệp phương sai lỗi xk thì lỗi ước lượng được xác định như sau:

ek = xk - xk- (3.3)

Ma trận hiệp phương sai sẽ là :

Pk = E(ek*ekT) (3.4) Giá trị cập nhật trạng thái xk được ước lượng :

xk +

= xk -

Pk = (I – Kk.Hk).Pk- (3.7) Phương trình (3.1) cũng tương đương với phương trình vi phân sau :

Trang 23

Do hệ thống định vị làm việc ở chế độ thời gian thực nên chúng ta chuyển phương trình

trên về dạng rời rạc theo thời gian:

Hình 7 Lưu đồ tính toán với bộ lọc Kalman

Trang 24

3.1.2 Hệ thống tích hợp INS/GPS

Khi tích hợp 2 hệ thống INS với GPS, chúng ta có thể sử dụng 2 mô hình kết hợp:

phương thức lỏng (Hình 8) và chặt (Hình 9) và được mô tả dưới đây [2]

Hình 8 Sơ đồ INS/GPS theo phương thức vòng hở

Hình 9 Sơ đồ INS/GPS theo phương thức vòng kín

Trong luận văn này, tác giả sử dụng mô hình vòng hở và dùng bộ lọc Kalman để ước lượng lỗi INS Lỗi INS sau khi ước lượng, có thể bù cho hệ thống INS theo 2 kiểu

Feedback (phản hồi, kín) hay Feedforward (hở) (Hình 10) [3]

INS

Tín hiệu lí tưởng + nhiễu INS

Tín hiệu lí tưởng + nhiễu GPS

nhiễu INS +

_ nhiễu GPS - nhiễu INS

lối ra được hiệu chỉnh

(a)

Trang 25

Kalman Tín hiệu lí tưởng + nhiễu INS

Tín hiệu lí tưởng + nhiễu GPS

nhiễu INS +

_ nhiễu GPS - nhiễu INS

lối ra được hiệu chỉnh INS

(b) Hình 10 Cấu trúc theo kiểu vòng hở (a) và vòng kín (b)

3.2 Đề xuất của học viên

Trong mô hình lỗi INS mục 2.4 đã trình bày trước đó, học viên sử dụng phương trình lỗi INS này làm mô hình hệ thống và sử dụng 2 bộ lọc Kalman song song, từ đó đưa

ra lưu đồ tính toán cho hệ thống tích hợp INS/GPS được mô tả như Hình 11 Các dữ liệu

thô từ IMU cung cấp, dữ liệu này được tính toán theo thuật toán SINS để đưa ra các thông

số vận tốc, vị trí, các góc tư thế Lối vào của khối ước lượng là độ chênh lệch của vị trí và vận tốc giữa INS và GPS Lối ra của khối ước lượng bao gồm các thông số: lỗi vị trí, lỗi vận tốc INS đã được ước lượng, các góc E , N ,Up, và độ trôi của các con quay vi cơ (Gbx, Gby ) Các lỗi vị trí và vận tốc này bù cho hệ thống INS để đưa ra kết quả vị trí và

vận tốc chính xác hơn

3.2.1 Lưu đồ thuật toán hệ thống tích hợp INS/GPS

Trang 26

IMU

Thuật toán SINS

Khối ước lượng Lọc Dự đoán GPS

Chính xác lại vị trí, vận tốc

Hình 11 Lưu đồ thuật toán hệ thống tích hợp INS/GPS

Trong lưu đồ thuật toán (Hình 11), khối ước lượng bao gồm 2 bộ lọc Kalman song

song với 2 mục đích: thứ nhất là để ước lượng các giá trị lỗi vị trí và vận tốc để bù cho hệ thống INS, thứ 2 là cập nhật quaternion để hiệu chỉnh các góc hướng Bộ lọc Kalman sẽ được đề cập trong phần sau đây

3.2.2 Bộ lọc Kalman cho hệ thống tích hợp INS/GPS

Như đề cập trong mục 2.4 trước đó, tác giả sử dụng các phương trình lỗi INS như

mô hình hệ thống, và các thành phần đo lường đưa tới các lối vào của bộ lọc là sự sai lệch giữa vận tốc và vị trí của INS với GPS Khi tín hiệu GPS mất, bộ lọc Kalman làm việc theo chế độ dự đoán

Định dạng
Số trang	52
Dung lượng	1,82 MB