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


14

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.
Nâng cao chất lượng hệ thống tích hợp INS/GPS sử dụng bộ lọc Kalman

15

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

(3.1)
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 :

(3.2)
Ở đâ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)

P k = E(e k*ekT )

(3.4)

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

Giá trị cập nhật trạng thái xk được ước lượng :

xk+ = xk- + Kk*(zk – Hk*xk-)

(3.5)

Với Kk độ khuếch đại Kalman và được tính theo công thức sau:
(3.6)
Thay phương trình (3.2) vào (3.5) ta được :
P k = (I – Kk.Hk).P k-

(3.7)

Phương trình (3.1) cũng tương đương với phương trình vi phân sau :

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

(3.8)


16

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:



(3.9)

Trong đó Φk là ma trận chuyển, và có dạng như sau :





(3.10)

Và ma trận hiệp phương sai liên quan tới wk sẽ là :
[

]



 

Dưới đây là lưu đồ thuật toán Kalman (Hình 7) [7]:

Hình 7. Lưu đồ tính toán với 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


17

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].
Tín hiệu lí tưởng + nhiễu INS

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

INS
nhiễu INS

_
GPS

+

nhiễu GPS - nhiễu INS

Kalman

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

(a)

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

18

INS

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

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

nhiễu INS

_
GPS

+

nhiễu GPS - nhiễu INS

Bộ lọc
Kalman

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

(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

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