ĐẠ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)
- Xem thêm -