You are on page 1of 47

Đại Học Công Nghệ.

ĐHQGHN Khoa: Điện tử-Viễn Thông
LỜI CẢM ƠN
Trước tiên em xin chân thành cảm ơn thầy giáo hướng dẫn Ths.Trần Đức Tân thuộc
Khoa Điện tử -Viễn thông đã tận tình hướng dẫn em thực hiện khoá luận tốt nghiệp này.
Em cũng xin bày tỏ lời cảm ơn sâu sắc đến các thầy giáo, cô giáo đã giảng dạy em
trong suốt những năm học Đại Học. Những kiến thức nền tảng quý báu đó sẽ là hành trang
giúp cho em vững bước trong tương lai. Đặc biệt là các thầy trong trong bộ môn MEMS đã
tận tình hướng dẫn em hoàn thành khoá luận tốt nghiệp này.
Em xin chân thành cảm ơn đề tài QC 07 -17 và các cán bộ tham gia đề tài đã giúp đỡ
em trong quá trình thực hiện luận văn.
Cuối cùng em muốn bày tỏ lòng cảm ơn đối với gia đình, bạn bè, người thân đã luôn
động viên, giúp đỡ em trong quá trình học tập và thực hiện khoá luận này.
Hà Nội, tháng 5 năm 2008
Sinh viên
Sinh Viên: Cao Ngọc Phát - 1 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
TÓM TẮT NỘI DUNG
Nhu cầu sử dụng hệ thống định vị và dẫn đường đã trở thành một nhu cầu không thể
thiếu trong cuộc sống ngày nay. Tuy nhiên hệ thống không thể đạt được sự chính xác tuyệt
đối. Vì vậy cần phải làm tăng độ chính xác của hệ thống định vị toàn cầu. Bằng cách khắc
phục được sai số của hệ thống dẫn đường (GPS) và hệ thống dẫn đường quán tính (INS) .
Hệ thống dẫn đường quán tính INS có 2 ưu điểm nổi bật khi so sánh với các hệ thống
dẫn đường khác là khả năng hoạt động tự trị và độ chính xác cao trong những khoảng thời
gian ngắn. Lỗi nghiêm trọng nhất của hệ thống INS là do các cảm biến quán tính gây ra.
Chính vì thế trong những ứng dụng thời gian dài thì hệ thống INS thường sử dụng với các
hệ thống hỗ trợ khác như hệ thống dẫn đường vô tuyến (Loran, Omega và Tacan), hệ thống
dẫn đường vệ tinh (GPS, GLONASS và Transit), JTIDS, DME…Các hệ thống này hoạt
động ổn định theo thời gian và vì thế cần tích hợp INS và các hệ thống hỗ trợ này. Sự kết
hợp GPS và INS là lý tưởng nhất vì hai hệ thống này có khả năng bù trừ nhau hiệu quả.
Trái tim của hệ thống tích hợp này chính là bộ lọc tối ưu Kalman.
Bộ lọc Kalman rất hiệu quả và linh hoạt trong việc kết hợp đầu ra bị nhiễu của cảm
biến quán tính để ước lượng trạng thái của hệ thống. Luận án này đã đề xuất một cấu trúc
lọc Kalman cải tiến bao gồm hai bộ lọc song song nhằm cải thiện độ chính xác và tốc độ
đáp ứng của hệ thống, Ngoài ra, hệ thống này còn có thể hoạt động linh hoạt giữa hai chế
độ vòng kín và vòng hở, rất hữu ích trong những tình huống mất tín hiệu từ GPS.
Sinh Viên: Cao Ngọc Phát - 2 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Bảng giải thích các chữ viết tắt
GPS (Global Positioning System) Hệ thống định vị toàn cầu
IMU (Inertial Measurement Unit) Bộ đođạc quán tính
INS (Inertial Navigation System) Hệ thống dẫn đường quán tính
KF (Kalman Filter) Bộ lọc Kalman
MEMS (MicroElectroMechanical System) Hệ vi cơ điện tử
NED (North, East, Down) Các trục của hệ toạ độ dẫn đường
PC-box (Persional Computer box) Máy tính cá nhân chuyên dụng
SINS (Strapdown INS) Hệ dẫn đường loại gắn chặt
Sinh Viên: Cao Ngọc Phát - 3 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
MỤC LỤC
Bảng giải thích các chữ viết tắt..........................................................................................3
1.1.6. Hoạt động.........................................................................................................11
1.2.1.Phương trình động học......................................................................................13
1.2.2. Ưu điểm và nhược điểm của INS......................................................................16
2.2 Các hệ tọa độ ................................................................................................................................18
2.2.1 Hệ tọa độ quán tính (Inertial Frame)..................................................................19
2.2.2 Hệ tọa độ cố định tâm trái đất (The Earth Fixed Frame)...................................19
2.2.3 Hệ tọa độ định vị (Navigation frame).....................................................................19
2.2.4 Hệ tọa độ gắn liền vật thể (Body frame)................................................................19
2.3 Phương trình định vị.....................................................................................................................20
2.4 Hệ Phương trình định vị trong hệ tọa độ cố định tâm trái đât (e-frame). ...................................21
2.5. Tổng quan về hệ cảm nhận quán tính IMU.................................................................................22
2.6 Thuật toán dẫn đường quán tính...................................................................................................23
2.7.1. Nhiễu tất định..........................................................................................................27
2.7.2 Nhiễu thống kê..........................................................................................................27
Năm 1960 R.E Kalman đã xuất bản một bài báo với tiêu đề “A New Approach to Linear Filtering
and Predication Problems”. Nghiên cứu của Kalman đã khắc phục hạn chế của bộ lọc Weiner-Hopf
trong việc giải quyết bài toán thống kê tự nhiên. Kể từ đó, danh từ bộ lọc Kalman đã ra đời. Bộ lọc
này ước lượng trạng thái của quá trình thời gian rời rạc theo phương trình sai phân tuyến tính:.....28
3.1.1.Bản chất tính toán của bộ lọc.............................................................................29
Chúng ta định nghĩa là trạng thái tiền ước lượng ở thời điểm thứ k, là trạng thái hậu ước lượng
tại thời điểm thứ k và cho ra giá trị đo zk. Chúng ta có thể định nghĩa các lỗi tiền ước lượng và lỗi
hậu ước lượng như sau:.......................................................................................................................29
3.1.2. Bản chất thống kê của bộ lọc............................................................................30
Công thức (3.8) thể hiện bản chất thống kê của tiền ước lượng quy định trên tất cả các giá trị đo
trước zk (Quy tắc Bayer).....................................................................................................................30
Sinh Viên: Cao Ngọc Phát - 4 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
4.1 Thiết bị phần cứng.........................................................................................................................38
4.1.1 Khối dẫn đường quán tính MICRO-ISU BP3010................................................38
Chương 1: Tổng quan về hệ thống dẫn đường quán tính
(INS) và hệ thống định vị toàn cầu (GPS).
1.1. Hệ thống định vị toàn cầu (GPS).
Hệ thống định vị toàn cầu (GPS-Global Positioning System) là một mạng gồm 24 vệ
tinh Navstar quay xung quanh Trái đất tại độ cao 11.000 dặm (17.600 km). Được Bộ Quốc
phòng Hoa Kỳ ấn định chi phí ban đầu vào khoảng 13 tỷ USD, song việc truy nhập tới GPS
là miễn phí đối với mọi người dùng, kể cả những người ở các nước khác. Các số liệu định
vị và định thời được sử dụng cho vô số những ứng dụng khác nhau, bao gồm đạo hàng
hàng không, đất liền và hàng hải, theo dõi các phương tiện giao thông trên bộ và tầu biển,
điều tra khảo sát và vẽ bản đồ, quản lý tài sản và tài nguyên thiên nhiên.
Với việc khắc phục được những giới hạn về độ chính xác quân sự vào tháng 3/1996,
ngày nay GPS có thể chỉ ra chính xác vị trí của các mục tiêu chỉ nhỏ bằng đồng 10 xu ở bất
kỳ nơi nào trên bề mặt trái đất.
Vệ tinh GPS đầu tiên đã được phóng vào năm 1978. Mười vệ tinh đầu tiên là các vệ
tinh ‘mở mang’, gọi là Block 1 (Lô 1). Từ năm 1989 đến năm 1993 có 23 vệ tinh khai thác,
gọi là Block 2 (Lô 2) đã được phóng lên quỹ đạo. Vệ tinh thứ 24 được phóng nốt vào năm
1994 đã hoàn thành hệ thống. Các vệ tinh được bố trí sao cho các tín hiệu từ 6 trong số đó
có thể được thu nhận gần như 100 phần trăm thời gian tại bất kỳ điểm nào trên trái đất.
Sinh Viên: Cao Ngọc Phát - 5 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Hình 1.1:Quỹ đạo các vệ tinh cua hệ thống GPS.
Trong số 24 vệ tinh của Bộ quốc phòng Mỹ nói trên, chỉ có 21 thực sự hoạt động, 3 vệ
tinh còn lại là hệ thống hỗ trợ. Tín hiệu radio được truyền đi thường không đủ mạnh để
thâm nhập vào các tòa nhà kiên cố, các hầm ngầm và hay tới các địa điểm dưới nước.
Ngoài ra nó còn đòi hỏi tối thiểu 4 vệ tinh để đưa ra được thông tin chính xác về vị trí (bao
gồm cả độ cao) và tốc độ của một vật. Vì hoạt động trên quỹ đạo, các vệ tinh đảm bảo cung
cấp vị trí tại bất kỳ điểm nào trên trái đất.
1.1.1. Cấu trúc của hệ thống định vi toàn cầu.
GPS bao gồm 3 mảng (xem hình 1.2):
 Mảng người dùng: gồm người sử dụng và thiết bị thu GPS.
 Mảng kiểm soát: bao gồm các trạm trên mặt đất, chia thành trạm trung tâm và trạm
con. Các trạm con, vận hành tự động, nhận thông tin từ vệ tinh, gửi tới cho trạm chủ.
Sau đó các trạm con gửi thông tin đã được hiệu chỉnh trở lại, để các vệ tinh biết
được vị trí của chúng trên quỹ đạo và thời gian truyền tín hiệu. Nhờ vậy, các vệ tinh
mới có thể đảm bảo cung cấp thông tin chính xác tuyệt đối vào bất kỳ thời điểm nào.
Sinh Viên: Cao Ngọc Phát - 6 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Hình 1.2: Cấu trúc hệ thống định vị toàn cầu.
 Mảng không gian: gồm các vệ tinh hoạt động bằng năng lượng mặt trời và bay trên
quỹ đạo. Quãng thời gian tồn tại của chúng vào khoảng 10 năm và chi phí cho mỗi
lần thay thế lên đến hàng tỷ USD. Một vệ tinh có thể truyền tín hiệu radio ở nhiều
mức tần số thấp khác nhau, được gọi là L1, L2...Một đài phát thanh FM thường cần
có công suất chừng 100.000 watt để phát sóng, nhưng một vệ tinh định vị toàn cầu
chỉ đòi hỏi 20-50 watt để đưa tín hiệu đi xa 19.200 km.
1.1.2. Thành phần của hệ thống định vị toàn cầu.
Hệ thống GPS gồm có các vệ tinh, các máy thu và các hệ thống điều khiển dưới đất.
Các vệ tinh phát các tín hiệu ở tần số 1575,42 MHz để các máy thu GPS dưới mặt đất có
thể tách ra được. Các máy thu này có thể được lắp đặt trên các con tầu, các máy bay và các
xe ô tô để cung cấp thông tin định vị chính xác bất kể điều kiện thời tiết như thế nào.
Chúng phát hiện, giải mã và xử lý các tín hiệu vệ tinh GPS để xác định vị trí chính xác của
người dùng.
Đoạn điều khiển (hay đoạn mặt đất) của GPS gồm có 5 trạm giám sát không người điều
khiển đặt tại Hawaii, Kwajalein ở Thái Bình Dương, Diago Garcia ở ấn Độ Dương,
Ascension Island ở Đại Tây Dương và Colorado Springs ở Solo. Còn có một Trạm mặt đất
chính đặt tại Falcon AFB ở Colorado Springs, và 4 trạm mặt đất an-ten lớn để phát quảng
bá các tín hiệu lên các vệ tinh. Các trạm này cũng bám theo và giám sát các vệ tinh GPS.
1.1.3. Hoạt động của hệ thống định vị toàn cầu.
Sinh Viên: Cao Ngọc Phát - 7 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Với GPS, các tín hiệu từ các vệ tinh sẽ đi tới các vị trí chính xác của người dùng và
được đo theo phép tam giác đạc. Để thực hiện phép tam giác đạc, GPS đo khoảng cách
thông qua thời gian hành trình của bản tin vô tuyến từ vệ tinh tới một máy thu mặt đất. Để
đo thời gian hành trình, GPS sử dụng các đồng hồ rất chính xác trên các vệ tinh. Một khi
khoảng cách tới vệ tinh đã được đo thì việc biết trước về vị trí vệ tinh trong không gian sẽ
được sử dụng để hoàn thành tính toán. Các máy thu GPS trên mặt đất có một “cuốn niên
giám” được lưu trữ trong bộ nhớ máy tính của chúng để chỉ thị mỗi vệ tinh sẽ có mặt nơi
nào trên bầu trời vào bất kỳ thời điểm nào. Các máy thu GPS sẽ tính toán các thời gian trễ
qua tầng đối lưu và khí quyển để tiếp tục làm chính xác hơn phép đo vị trí.
Để bảo đảm chắc chắn vệ tinh và máy thu đồng bộ với nhau, mỗi vệ tinh có bốn đồng
hồ nguyên tử chỉ thời gian chính xác tới 3 ns, tức ba phần tỷ giây. Nhằm tiết kiệm chi phí,
các đồng hồ trong các máy thu dưới đất được làm ít chính xác hơn đôi chút. Bù lại, một
phép đo tầm hoạt động vệ tinh được trang bị thêm. Phép đo lượng giác chỉ ra rằng, nếu ba
số đo chính xác định vị được vị trí một điểm trong không gian ba chiều thì một phép đo thứ
tư có thể loại bỏ mọi độ chênh lệch thời gian nào đó. Phép đo thứ tư này chỉnh lại sự đồng
bộ hoá không hoàn hảo của máy thu.
Hình 1.3: Các tín hiệu từ vệ tinh, được một máy thu GPS đặt trên một chiếc ô tô thu nhận,
được sử dụng để xác định thông tin vị trí chính xác
Khối mặt đất thu nhận tín hiệu vệ tinh đi tới với tốc độ bằng tốc độ ánh sáng. Ngay như
tại tốc độ như vậy tín hiệu cũng phải mất một lượng thời gian đáng kể mới tới được máy
thu. Sự chênh lệch giữa thời điểm tín hiệu được gửi đi và thời điểm tín hiệu được thu nhận
với tốc độ ánh sáng cho phép máy thu tính được khoảng cách tới vệ tinh. Để đo lường
chính xác độ cao, kinh độ và vĩ độ, máy thu đo thời gian các tín hiệu từ một số vệ tinh
truyền tới máy thu (Hình 1.2).
Sinh Viên: Cao Ngọc Phát - 8 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
GPS sử dụng một hệ tọa độ gọi là Hệ thống Trắc địa học Toàn cầu 1984 (WGS-84 -
Worldwide Geodetic System 1984). Hệ thống này tương tự như các đường kẻ kinh tuyến và
vĩ tuyến quen thuộc thường thấy trên các bản đồ treo tường cỡ lớn. Hệ thống WGS - 84
cung cấp một khung tham chiếu gắn sẵn tiêu chuẩn hoá, cho phép các máy thu của bất kỳ
hãng sản xuất nào cũng cung cấp đúng cùng một thông tin định vị.
1.1.4. Ứng dụng của hệ thống định vị toàn cầu.
Mặc dù hệ thống GPS chỉ mới được hoàn thành vào năm 1994 nhưng nó đã thực sự tự
khẳng định mình trong những ứng dụng quân sự.
Ngày nay, GPS đã trở thành một yếu tố quan trọng của hầu như tất cả các chiến dịch
quân sự và tất cả các hệ thống vũ khí. Ngoài ra, GPS còn được sử dụng trên các vệ tinh để
đạt được các dữ liệu quỹ đạo có độ chính xác cao và để điều khiển hướng bay của các con
tầu vũ trụ.
Mặc dù hệ thống GPS lúc ban đầu được triển khai để đáp ứng các yêu cầu của giới
quân sự, nhưng người ta đang không ngừng tìm ra các cách thức mới để sử dụng những khả
năng của nó, từ cao siêu đến bình dị. Một trong số cách thức thứ nhất là sử dụng GPS cho
công tác quản lý động vật hoang dã. ở châu Phi, các máy thu GPS được sử dụng để giám
sát các đường hướng di trú của các đàn động vật lớn cho những mục đích nghiên cứu khác
nhau.
Những máy thu GPS cầm tay hiện đang được sử dụng thường ngày trong các ứng dụng
thực địa, trong đó có đòi hỏi việc thu thập thông tin chính xác, kể cả việc kiểm tra hiện
trường của các công ty phục vụ công cộng, việc vẽ bản đồ của các nhà khai thác dầu mỏ và
khí đốt và việc quy hoạch tài nguyên của các công ty lâm nghiệp.
Các khinh khí cầu có trang bị GPS đang giám sát các lỗ hổng trong tầng ô-zôn trên
các vùng cực và chất lượng không khí cũng đang được giám sát nhờ các máy thu GPS. Các
phao theo dõi lượng dầu tràn lớn trên biển phát đi các dữ liệu cần thiết nhờ sử dụng GPS.
Các nhà khảo cổ học và các nhà thám hiểm đang sử dung hệ thống này để đánh dấu các vị
trí ở xa trên biển và trên đất liền trước khi họ có thể lập quyết toán trang thiết bị và kinh
phí.
Theo dõi các phương tiện vận chuyển là một trong những ứng dụng GPS phát triển
nhanh nhất. Các đoàn tầu, các hệ thống vận chuyển công cộng, các đoàn xe tải quá cảnh,
Sinh Viên: Cao Ngọc Phát - 9 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
các chuyến xe bưu chính... có trang bị các máy thu GPS để giám sát các vị trí của chúng
vào mọi thời điểm.
Các dữ liệu GPS sẽ trở nên hữu ích hơn đối với khách hàng khi nó được liên kết với
kỹ thuật vẽ bản đồ số. Theo đó, một số hãng sản xuất ô tô đang chào hàng một phương án
chế tạo xe mới là trang bị các màn hình trình bày hành trình xe chạy do các máy thu GPS
hướng dẫn. Các màn hình này thậm chí còn có thể tháo ra đem về nhà để lập chương trình
cho một chuyến đi. Một số phương tiện xe cộ có trang bị GPS đưa ra các bảng hướng dẫn
trên màn hiển thị cho các lái xe và qua các lệnh bằng tiếng nói tổng hợp. Những tính năng
này cho phép lái xe đến được bất kỳ nơi nào anh ta muốn một cách nhanh chóng hơn và an
toàn hơn so với trước đây.
Công nghệ GPS thậm chí còn đang được sử dụng kết hợp với công nghệ mạng tế bào
để cung cấp các dịch vụ giá trị gia tăng. Với việc ấn một phím bấm trên máy điện thoại di
động mạng tế bào, có thể đàm thoại với một nhà cung cấp dịch vụ và cùng một lúc báo hiệu
tới các dịch vụ điều phối trung tâm thông báo về vị trí, của họ về các tình huống khẩn cấp
hoặc các hỏng hóc trang thiết bị.
Điều này là có thể được với Khối Định vị Mạng tế bào và Nhắn tin Khẩn (Cellular
Positioning and Emergency Messaging Unit) của hãng Motorola. Thiết bị này mở ra một kỷ
nguyên mới của an toàn di động và theo dõi các đoàn xe và các đoàn tầu biển. Các thiết bị
này được thiết kế cho các nhà tích hợp hệ thống là những người đang tạo cấu hình các
mạng tiêu dùng và thương mại khai thác qua điện thoại di động tế bào. Khối Định vị Mạng
tế bào và Nhắn tin Khẩn truyền đạt thông tin về vị trí và trạng thái của các phương tiện xe
cộ do GPS xác định, rất phù hợp để sử dụng trong các hệ thống nhằm trợ giúp cho các nhà
quản lý đường bộ, các hãng giám sát nội vụ, các công ty điện thoại di động, các công ty cho
thuê xe ô tô, các nhà khai thác đội tầu biển thương mại và các nhà sản xuất ô tô... tìm kiếm
những lợi thế cạnh tranh..
1.1.5. Mã trong hệ thống định vị toàn cầu.
Mỗi một vệ tinh trong hệ thống GPS đều có đồng hồ nguyên tử độ chính xác rất cao
để làm cơ sở cho thiết bị phát tần số chuẩn 10,23Mz. Tần số này điều biến 2 sóng mang L1
= 1575,42MHz và L2 = 1227,60MHz. Các sóng mang L1, L2 được điều biến bởi 3 loại mã
sau:
Sinh Viên: Cao Ngọc Phát - 10 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
 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.
1.1.6. Hoạt động
Tần số L1 chứa đựng 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ủ
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.
Sinh Viên: Cao Ngọc Phát - 11 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Nguyên nhân sai số
Sai số của phương pháp đinh vị GPS chủ yếu là do 6 nguyên nhân dưới đây (không kể
sai số nhân tạo SA đã được cựu tổng thống Bill Clinton ra lệnh tắt):
 Dữ liệu Ephemeris.
 Đồng hồ vệ tinh.
 Trễ ở tầng điện ly.
 Trễ ở tầng đối lưu.
 Nhiễu đa đường.
 Máy thu (bao gồm cả phần mềm).
Lỗi dữ liệu Ephemeris xảy ra khi thông điệp của GPS không truyền chính xác vị trí của
vệ tinh và vì thế ảnh hưởng tới độ chính xác khi xác định khoảng cách. Lỗi này sẽ tăng theo
thời gian từ lần cập nhật cuối cùng của trạm điều khiển. Lỗi do đồng hồ vệ tinh ảnh hưởng
tới cả những người dùng mã C/A hay mã P, lỗi này gây ra sai số 1 – 2 mét sau khi cập nhật
12 giờ. Lỗi đo lường gây sai số khoảng cách cỡ vài mét.Trễ ở tầng điện ly và đối lưu gây
nên trễ pha khi tính toán khoảng cách (pseudorange). Lỗi này có thể loại trừ khi sử dụng
các máy thu dùng mã P có 2 băng tần. Với L1 và L2 có tần số khác nhau, tầng điện ly sẽ
làm chúng có độ trễ khác nhau. Đó là cơ sở cho phương pháp loại trừ sai số này. Lỗi đa
đường gây ra bởi các tín hiệu bị phản xạ qua các chướng ngại khác nhau tới máy thu. Hiện
tượng này sẽ trầm trọng hơn nếu có nhiều chướng ngại và lỗi khoảng cách gây ra có thể tới
15 mét.
Vai trò chính của GPS đó là cung cấp chính xác các thông số vị trí và vận tốc của vật
thể bay. GPS có thể được sử dụng để hỗ trợ cho các hệ thống dẫn đường khác mà tiêu biểu
là sự kết hơp GPS / INS.
1.2. Hệ thống dẫn đường quán tính (INS).
Một số khái niệm cơ bản sau :
 Quán tính: là bản chất của vật thể mà khi không có lực tác động thì nó sẽ chuyển
động tịnh tiến đều hoặc chuyển động vòng tròn đều.
Sinh Viên: Cao Ngọc Phát - 12 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
 Hệ quy chiếu quán tính: hệ quy chiếu mà ba định luật Newton được áp dụng và
bảo toàn.
 Cảm biến quán tính: gồm 2 loại là gia tốc kế và cảm biến vận tốc góc (còn gọi con
quay vi cơ).
 Hệ thống dẫn đường quán tính: là hệ thống sử dụng các cảm biến vận tốc góc và
cảm biến gia tốc để ước lượng vị trí, vận tốc, độ cao và vận tốc thay đổi độ cao của
vật thể bay.
Hình 1.4: Trục toạ độ của hệ thống dẫn đường quán tính
Hệ thống INS gồm ba cảm biến vận tốc góc cho phép xác định vận tốc góc nghiêng,
góc chúc và góc hướng trong hệ toạ độ vật thể bay (xem hình 1.4). Hệ thống INS cũng có
thể có thêm ba cảm biến gia tốc cho phép xác định gia tốc theo ba trục của hệ toạ độ vật thể
bay này. Hiện nay có hai cấu trúc dẫn đường quán tính tiêu biểu nhằm xác định các góc Ơle
từ các cảm biến vận tốc góc là cấu trúc gắn chặt (gimble) và cấu trúc nổi (strapdown). Cấu
trúc strapdown hiện được sử dụng rộng rãi hơn, trong đó các cảm biến gia tốc và vận tốc
góc được gắn chặt vào vật thể bay. Các giá trị gia tốc thu được từ các cảm biến gia tốc
được hiệu chỉnh với vận tốc quay của trái đất và gia tốc trọng trường nhằm xác định vị trí
và vận tốc chính xác của vật thể bay.
1.2.1. Phương trình động học.
Sinh Viên: Cao Ngọc Phát - 13 -
Góc
hướng
Z
Góc trúc
Y
Góc nghiêng
X
khối tâm
0
1 sin cos
0 cos sin
tan tan
s 0 sin cos ec sec
p
q
r
θ θ
θ
φ φ φ
θ φ φ
θ ψ φ φ
1
¸ _ 1
1
1
· −
1
1

1
1
¸ , ¸ ]
¸ ]
&
&
&
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Định hướng của vật thể bay với hệ trục quả đất cố định được xác định bởi ba góc Ơle
φ θ ψ , ,
(hình 1.5). Phương trình (1.1) biểu diễn sự liên hệ giữa các vận tốc góc nghiêng,
vận tốc góc chúc và vận tốc góc hướng ( ký hiệu là p,q và r) và 3 góc Ơle:

(1.1)

Tích phân phương trình (1.1) ta sẽ thu được 3 góc Ơle.
Hình 1.5. Ba góc Ơle
Các gia tốc a
x
, a
y
và a
z
của vật thể bay dọc theo 3 trục toạ độ vật thể bay liên hệ với
vận tốc U, V và W trên hệ trục quả đất cố định (XYZ) theo hệ phương trình (1.2):
Sinh Viên: Cao Ngọc Phát - 14 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
φ θ
φ θ
θ
.cos g.cos - W.p .
.sin g.cos - W.p .
g.sin W.q .
.
.
.
+ − ·
+ − ·
+ − + ·
q U a W
r U a V
r V a U
z
y
x
(1.2)
Thực hiện tích phân
. .
,V U

.
W
sẽ thu được các vận tốc U, V và W. Sau đó đổi hệ trục
toạ độ nhờ sử dụng ma trận cosin trực tiếp DCM để thu được vận tốc theo hướng bắc,
hướng đông và hướng về trái đất (người ta gọi đó là hệ trục toạ độ dẫn đường,
xem hình 1.4).

1
1
1
]
1

¸

·
1
1
1
1
]
1

¸

·
1
1
1
1
]
1

¸

W
.
.
.
V
U
DCM
V
V
V
Z
Y
X
T
D
E
N
(1.3)
Ở đó :

1
1
1
]
1

¸

− +
+ −

·
θ φ ψ φ ψ θ φ ψ φ ψ θ φ
θ φ ψ φ ψ θ φ ψ φ ψ θ φ
θ ψ θ ψ θ
cos cos cos sin sin sin cos sin sin cos sin cos
cos sin cos cos sin sin sin sin cos cos sin sin
sin sin cos cos cos
DCM
Tích phân V
N
,V
E
và V
D
sẽ thu được vị trí của vật thể bay trên bề mặt trái đất (hệ trục
toạ độ dẫn đường). Vĩ độ λ , kinh độ
µ
và độ cao H của vật thể bay có thể xác định nhờ hệ
phương trình sau:
D
earth
E
earth
N
V H
R
V
R
V
− · · ·
&
&
&
λ
µ λ
cos
(1.4)
Sinh Viên: Cao Ngọc Phát - 15 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Hình 1.6: Hệ trục toạ độ dẫn đường
1.2.2. Ưu điểm và nhược điểm của INS
 Ưu điểm của hệ INS
- Hoạt động tự trị.
- Tần số cập nhật cao.
- Các sai số có đặc tính thay đổi chậm, ít chịu ảnh hưởng bên ngoài.
- Có khả năng đo các góc định hướng.
 Nhược điểm
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. Dưới đây là 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ây nên sai số về góc góc nghiêng , góc trúc và góc
hướng.
- Lỗi về độ lệch của cảm biến gia tốc dẫn lối gia của 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 khi bật hoặc tát thiết bị.
Sinh Viên: Cao Ngọc Phát - 16 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
- 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 độ) gây nên
vật 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 gây nên lổi ngẩu nhiên trong đo lường.
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à vận tốc của vật thể bay (do việc lấy tích phân). Các lỗi tăng dần này được gọi là lỗi
dẫn đường, bao gồm 9 lỗi (3 lỗi vị trí, 3 lỗi vận tốc, 3 lỗi tư thế). 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.

Sinh Viên: Cao Ngọc Phát - 17 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Chương 2: Lý thuyết hệ thống dẫn đường tích hợp GPS/INS

2.1 Giới thiệu về hệ dẫn đường quán tính.
Dẫn đường quán tính dựa trên cơ sở tính toán vị trí, vận tốc và hướng của một vật thể
chuyển động bằng các sử dụng các thông số đo được từ các cảm biến như cảm biến gia tốc
và cảm biến vận tốc góc. Các cảm biến quán tính và đo chuyển động đều dựa vào các định
luật vật lý về tự nhiên và không chịu ảnh hưởng của các tín hiệu điện hay điện từ bên
ngoài, điều này là cơ sở cho cảm biến quán tính hoạt động đáng tin cậy và khó bị nhiễu.
Cảm biến quán tính hoạt động dự trên định luật II Newton:
F
dt
d m ) ( . x&
·
(2.1)
Với :
F là vectơ tổng các lực tác dụng lên vật thể.
m là khối lượng vật thể.
x& là đạo hàm bậc nhất quãng đường chuyển động của vật thể (vận tốc chuyển động).
Phương trình (2.1) cần được đưa về dạng liên hệ với vectơ gia tốc trọng trường g.
x& & m = F + mg (2.2)
Gia tốc sinh ra do lực F tác dụng lên vật là a =
m
F
, thay vào phương trình (2.2) ta có:
x& & = a + g (2.3)
Các phương trình này được ấn định cho hệ tọa độ quán tính Newton. Đối với hệ tọa độ
thứ i thì phương trình (2.3) có dạng.

i
x& & = g
i
(
i
x
) + a
i
(2.4)
Với
i
x là vị trí trong hệ tọa độ thứ i, g
i
là gia tốc trọng trường của hệ tọa độ thứ i phụ
thuộc vào vị trí x, a
i
là gia tốc thành phần được đo bởi các cảm biến gia tốc.
Từ các phường trình này có thể xác định được vị trí x
i
và vân tốc của vật thể.
2.2 Các hệ tọa độ
Sinh Viên: Cao Ngọc Phát - 18 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
2.2.1 Hệ tọa độ quán tính (Inertial Frame).
Hệ tọa độ quán tính là một hệ tọa độ tuân theo 3 định luật của Newton. Gốc của hệ tọa
độ quán tính và hướng của các trục là tuỳ ý. Để cho thuận lợi, hệ tọa độ quá tính thường
được định nghĩa trùng với tâm của trái đất. Hệ tọa độ này cũng được gọi là hệ tọa độ thứ i
để tránh nhầm lẫn với hệ tọa độ quán tính lí tưởng. Tất cả các phép đo quán tính đều có
quan hệ đến hệ tọa độ này. Các thông số trong hệ tọa độ này sẽ có ký tự biểu tượng thứ i, ví
dụ
i
x .
2.2.2 Hệ tọa độ cố định tâm trái đất (The Earth Fixed Frame)
Hệ tọa độ cố định tâm trái đất (e-frame) có gốc tọa độ là tâm của trái đất. Trục x được
định nghĩa có chiều dương quay về phía giao điểm của đường kinh tuyến 0 và đường xích
đạo, trục z là trục quay của trái đất có chiều dương hướng lên bắc cực, trục y là tích có
hướng của trục z và trục x sao cho hệ tọa độ cố định tâm trái đất là một hệ thuận. Các thông
số trong hệ tọa độ này sẽ có ký tự biểu tượng e, ví dụ x
e
.
2.2.3 Hệ tọa độ định vị (Navigation frame)
Hệ tọa độ định vị được sử dụng để mô tả sự chuyển động của vật thể theo các hướng
bắc, đông, và hướng đi lên vuông góc bề mặt trái đất-hệ ENU (hình 2.1). Ngoài ra hệ định
vị còn có thể định nghĩa trục hướng đi lên thành trục hướng đi xuống, tức là đi thẳng vào
tâm trái đất-Hệ NED. Các thông số trong hệ tọa độ định vị sẽ có biểu tượng n, ví dụ x
n
.
Khi chuyển đổi giữa 2 hệ quy chiếu ENU và NED cần phải chú ý đổi các giá trị của x, y và
z.
Hình 2.1: Hệ tọa độ định vị
2.2.4 Hệ tọa độ gắn liền vật thể (Body frame).
Sinh Viên: Cao Ngọc Phát - 19 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Hệ tọa độ gắn liền vật thể lấy vật thể làm gốc hệ tọa độ, từ đó mô tả các chuyển động
theo các hướng trước, sau, trái phải, lên, xuống và các góc quay Euler
2.3 Phương trình định vị.
Giả sử a là một hệ tọa độ bất kỳ quay quanh hệ tọa độ quán tính i với vận tốc góc là
a
ia
ω . Mối liên hệ khi chuyển từ hệ tọa độ a sang hệ tọa độ quán tính i là:
x
i
=
i
a
C x
a
(2.5)
Với
i
x
là vị trí trên hệ tọa độ i, là 1 vectơ 3 thành phần.

a
x
là vị trí trên hệ tọa độ i, là 1 vectơ 3 thành phần.
i
a
C là ma trận chuyển từ hệ tọa độ a sang hệ tọa độ quán tính i. Lấy đạo hàm theo thời
gian của ma trận chuyển đổi ta được:
a
ia
i
a
i
a
C C Ω ·
&
(2.6)
Với
a
ia
Ω là ma trận phản đối xứng được tạo bởi các thành phần của
a
ia
ω = (
3 2 1
, , ω ω ω
):

,
_

¸
¸



· Ω
0
0
0
1 2
1 3
2 3
ω ω
ω ω
ω ω
a
ia
(2.7)
Ma trận chuyển đổi đạo hàm cấp 2 của hai hệ tọa độ là:
a
ia
a
ia
i
a
a
ia
i
a
i
a
C C C Ω Ω + Ω ·
& & &
(2.8)
Đạo hàm phương trình (2.5) sử dụng các phương trình (2.6) và (2.8) ta được:
x& & =
a i
a
a i
a
a i
a
C C C x x x & & &
& & &
+ + 2
= ( )
a a
ia
a
ia
a
ia
i
a
a a
ia
i
a
a i
a
C C C x x x Ω Ω + Ω + Ω +
&
& & & 2 (2.9)
Giải lấy
a
x& & và kết hợp với (2.4) và sử dụng tính trực giao của ma trận
i
a
C
, ta thu được
hệ phương trình động học trong hệ tọa độ a:
Sinh Viên: Cao Ngọc Phát - 20 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
( )
a a a a
ia
a
ia
a
ia
a a
ia
a
g a x x x + + Ω Ω + Ω − Ω − ·
&
& & & 2 (2.10)
Với
i i
a
a
C a a · và ) (
i i a
i
a
C x g g · . Hệ phương trình (2.10) là ba phương trình vi phân
bậc 2, có thể chuyển đổi thành sáu phương trình vi phân bậc 1 như sau:
( )
a a
dt
d
a a a a
ia
a
ia
a
ia
a a
ia
a
dt
d
x x
g a x x x
&
&
& &
·
+ + Ω Ω + Ω − Ω − · 2
(2.11)
Đây chính là hệ phương trình định vị đối với một hệ tọa độ bất kỳ.
2.4 Hệ Phương trình định vị trong hệ tọa độ cố định tâm trái đât (e-frame).
Trái đất chỉ quay quanh trục z , do đó trong hệ tọa độ cố định tâm trái đất e-frame thì
chỉ có thành phần
e
ω ω ·
3
là khác không, còn các thành phần còn lại đều bằng không. Do
e
ω là một hằng số nên
e
ie

&
= 0, và
( )
e e
dt
d
e e e e
ie
e
ie
e
ie
e e
ie
e
dt
d
x x
g a x x x
&
&
& &
·
+ + Ω Ω + Ω − Ω − · 2
(2.12)
với

,
_

¸
¸

· Ω
0 0 0
0 0
0 0
e
e
e
ie
ω
ω
(2.13)
với
e
ω
là vận tốc quay của trái đất.
e
a
được chuyển đổi từ gia tốc mà các cảm biến gia trốc đo được trong hệ tọa độ gắn
liền vật thể.

b e
b
e
C a a · (2.14)
Ma trận chuyển đổi
e
b
C
được xác định bằng cách tính tích phân các vận tốc góc thu
được từ các cảm biến vận tốc góc.
b
eb
e
b
e
b
C C Ω ·
&
(2.15)
Sinh Viên: Cao Ngọc Phát - 21 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Thành phần
b
eb
Ω có thể được viết dưới dạng của các vận tốc góc đo được từ các cảm
biến (
b
eb
ω ) như sau:
e
ie
b
e
b
ib
b
eb
C ω ω ω − ·
(2.16)
Các phương trình từ (2.12) đến (2.16) là các phương trình vi phân bậc nhất mô tả mối
liên hệ giữa vị trí, vận tốc và tư thế của vật thể với dữ liệu thu được từ khối IMU là
b
a và
b
ib
ω . Một cách tổng quát có thể hiểu các phương trình vi phân được viết dưới dạng:
)) ( ), ( ( ) ( t t f t a x x · &
(2.17)
với
) (t a
bao gồm cả a
b

b
ib
ω .
2.5. Tổng quan về hệ cảm nhận quán tính IMU.
Những khối IMU thời kì đầu sử dụng những cảm biến quán tính hoạt động theo
nguyên tắc cơ khí. Những cảm biến cơ khí này 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, cùng với sự tiến bộ của
khoa học công nghệ, đặ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 một khả năng rộng lớn cho việc ứng dụng
các cảm biến vi cơ trong nhiều lĩnh vực đời sống.
Một khối vi cơ IMU được cấu tạo từ các cảm biến vi cơ, thường là 3 cảm biến gia tốc
và 3 cảm biến vận tốc góc, hoặc cũng có thể là 1 cảm biến gia tốc 3 chiều kết hợp với 3
cảm biến vận tốc góc. Các cảm biến vi cơ kết cấu hỗ trợ với nhau theo cấu trúc gắn liền
(hình 2.2a) hoặc theo cấu trúc nổi (hình 2.2b), từ đó có thể xác định được 3 thành phần
chuyển động quay và tịnh tiến của vật thể.
Sinh Viên: Cao Ngọc Phát - 22 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Hình 2.2: Các cấu trúc của khối IMU vi cơ.
Điểm khác nhau cơ bản của hai kiểu cấu trúc này đó là: với kiểu gắn liền thì các cảm
biến không bị thay đổi hướng theo đối tượng chuyển động, còn trong kiểu Strapdown thì
các cảm biến được gắn chặt với vật chuyển động, do đó sẽ thay đổi trang thái chuyển động
theo vật đó. Trên thực tế khối IMU có cấu trúc kiểu Strapdown được sử dụng rộng rãi hơn
bởi cấu trúc này đơn giản và có giá thành chế tạo thấp với độ chính xác có thể chấp nhận
được.Khi kết hợp các cảm biến vi cơ thành một cấu trúc tổng thể thì thường tạo ra sai số.
Sai số mắc phải trong việc sử dụng các cảm biến vi cơ này có ở 2 cấp độ, cấp độ cảm biến
và cấp độ nhóm cảm biến. Ở cấp độ cảm biến là sai số của từng cảm biến cấu tạo tên khối
IMU, còn ở cấp độ nhóm cảm biến là sai số tổ hợp của nhóm cảm biến với nhau .
2.6 Thuật toán dẫn đường quán tính.
Thuật toán dẫn đường quán tính sẽ tính toán vận tốc, tư thế vật thể trong hệ tọa độ
định vị. Dữ liệu vào là các thông tin ban đầu về vị trí và dữ liệu thu được từ khối IMU .
Lưu đồ thuật toán được mô tả trong hình 2.3 và 2.4 với các phương trình chi tiết sau:
Các ký hiệu trong lưu đồ:
zb yb xb zb yb xb
a a a , , , , , ω ω ω
: các vân tốc góc và gia tốc thu được từ các cảm biến.
Sinh Viên: Cao Ngọc Phát - 23 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
h
N1
: thời gian lấy tính phân.
h
N3
: bước thời cập nhật của hệ thống INS.
k j i
3 2 1 0
λ λ λ λ λ ∆ + ∆ + ∆ + ∆ · ∆
k m j m i m m m
3 2 1 0
*
∆ + ∆ + ∆ + ∆ · ∆
N
b
C là ma trận chuyển từ hệ tọa độ gắn liền vật thể sang hệ tọa độ định vị.
E
N
B là ma trận chuyển từ hệ tọa độ định vị sang hệ tọa độ cố định tâm trái đất
ψ γ ϑ , ,
là 3 góc quay Roll, Pitch, và Yaw.
ε λ ϕ , ,
là vĩ độ, kinh độ và góc phương vị.
V
x,y,y
là các vận tốc trong hệ tọa độ cố định tâm trái đất.
D E N
V V V , , ,là các vận tốc trong hệ tọa độ định vị.
Sinh Viên: Cao Ngọc Phát - 24 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
zb yb xb , ,
α
Hình 2.3 Thuật toán dẫn đường quán tính
Sinh Viên: Cao Ngọc Phát - 25 -
Tính Quaternion -1 TT
xb
∆Φ
yb
∆Φ
zb
∆Φ
Tính lại độ tăng vận tốc
trong hệ tọa độ định vị t
xb
W
yb
W
zb
W
h
N
3
fm
i
q
Liên kết
Liên kết
y x,
W ∆
ij
C
p
q
1
p
q
0
p
q
2
p
q
3
x
W ∆
y
W ∆
z
W ∆
xb
ω
yb
ω
zb
ω
xb
a
yb
a
xb
a
h
N1
Chỉnh lỗi góc (Coning) Bù lỗi vận tốc Sculling
zb yb xb
W
, ,

h
N
3
h
N1
Bù nhiễu của cảm biến vận
tốc góc theo mô hình (2.18)
Bù nhiễu của cảm biến gia
tốc theo mô hình (2.18)
Tính độ tăng về góc T Tính độ tăng về vận tốc T
IMU I
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Hinh2.4: Thuật toán dẩn đường quán tính.
Sinh Viên: Cao Ngọc Phát - 26 -
h
N3
h
N3 NN
Tính Quaternion -2 TT
p
q
0
p
q
1
p
q
2
p
q
3
p
q
0
y
W ∆
z
W ∆
x
W ∆
h
N3
ij
C
f
q
0
f
q
1
f
q
2
f
q
2
ϑ
γ ψ
ϕ λ ε
E N
V V ,
Chuẩn hoá các tham số
quaternion
Tính ma trận
Tính tọa độ TTT
Tính toán tư thế T
ij
b
y x
V
,
Tính vận tốc V
x,y
Tính ma trận
z y x , ,
ω
( ) 0
ij
b
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
2.7. Các loại nhiễu ảnh hưởng đến khối IMU.
Khối IMU chịu ảnh hưởng của hai loại nhiễu là nhiễu tất định và nhiễu thống kê.
2.7.1. Nhiễu tất định
Các loại nhiễu tất định của khối IMU gồm có độ lệch vận tốc góc, độ lệch gia tốc, lỗi tỉ
lệ vận tốc góc, lỗi tỉ lệ gia tốc, lỗi vị trí.
2.7.2 Nhiễu thống kê
Có thể liệt kê các loại nhiễu thống kê chính tác động đến hệ thống dẫn đường quán tính
như sau:
Ồn lượng tử: sinh ra khi chuyển tín hiệu tương tự sang dạng số. Nguyên nhân của của
nó là sự sai khác giữa mức lượng tử số và biên độ thật của tín hiệu tương tự. Có thể hạn chế
ồn lượng tử bằng các phương pháp mã hóa, thay đổi tần số lấy mẫu hoặc tăng thêm mức
lượng tử.
Nhiễu trắng : là nguyên nhân chính gây ra lỗi thống kê của các khôi IMU và có công
suất không đổi trên toàn dải tần. Bản chất của các lỗi bước góc ngẫu nhiên (đối với cảm
biến vận tốc góc) và bước vận tốc ngẫu nghiên (đối với cảm biến gia tốc) đều xuất phát từ
nhiễu trắng.
Bước ngẫu nhiên: Đây là loại nhiễu không có nguồn gốc rõ ràng, và có thể thẫy loại
nhiễu này tăng theo hàm mũ với khoảng tương quan thời gian dài. Đối với cảm biến vận
tốc góc có bước vận tốc góc ngẫu nhiên, đối với cảm biến gia tốc có bước gia tốc ngẫu
nhiên.
Nhiễu rung :Nhiễu rung là loại nhiễu ở tần số thấp, gây nên sự mất ổn định của độ
lệch, nguồn gốc của loại nhiễu này là do các linh kiện điện tử nhạy với rung ngẫu nhiên.
Sinh Viên: Cao Ngọc Phát - 27 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Chương 3: Áp dụng bộ lọc Kalman (hai bộ lọc Kalman) vào
bài toán dẫn đường
3.1. Bộ lọc Kalman.
Năm 1960 R.E Kalman đã xuất bản một bài báo với tiêu đề “A New Approach to
Linear Filtering and Predication Problems”. Nghiên cứu của Kalman đã khắc phục hạn chế
của bộ lọc Weiner-Hopf trong việc giải quyết bài toán thống kê tự nhiên. Kể từ đó, danh từ
bộ lọc Kalman đã ra đời. Bộ lọc này ước lượng trạng thái
n
x ℜ ∈ của quá trình thời gian rời
rạc theo phương trình sai phân tuyến tính:
1 1 1 − − −
+ + ·
k k k k
w Bu Ax x (3.1)
Với việc đo
n
z ℜ ∈
k k k
v Hx z + · (3.2)
Biến ngẫu nhiên w
k
và v
k
biểu diễn nhiễu đo và nhiễu quá trình. Trong thuật toán lọc
Kalman, đặc tính thống kê của hai biến này phải được biết trước. Chúng ta giả sử các biến
này độc lập có phổ trắng và phân bố Gauss.
P(W))∼N(0,Q) (3.3)
P(R)∼N(0,R)
Trong thực tế, ma trận hiệp phương sai nhiễu quá trình Q và ma trận hiệp phương sai
nhiễu đo R phải thay đổi theo từng thời điểm, tuy nhiên chúng ta có thể giả sử là hằng số.
Ma trận A(nxn) trong phương trình sai phân (3.1) là ma trận chuyển trạng thái từ thời
điểm trước (k-1) sang thời điểm hiện tại (k). Chú ý rằng, trong thực tế A có thể thay đổi
theo từng thời điểm. Nhưng chúng ta cũng có thể giả sử nó là hằng số. Ma trận B (nx1) là
ma trận điều khiển có lối vào
n
u ℜ ∈ . Ma trận H(mxn) trong phương trình (3.2) là ma trận
đo lường. Trong thực tế H có thể thay đổi theo từng thời điểm, ở đây chúng ta giả sử là
hằng số.
Sinh Viên: Cao Ngọc Phát - 28 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
3.1.1.Bản chất tính toán của bộ lọc.
Chúng ta định nghĩa n
k x ℜ ∈


là trạng thái tiền ước lượng ở thời điểm thứ k,
n
k x ℜ ∈


là trạng thái hậu ước lượng tại thời điểm thứ k và cho ra giá trị đo z
k
. Chúng ta có thể định
nghĩa các lỗi tiền ước lượng và lỗi hậu ước lượng như sau:



− · k
k k
x x e
(3.4)
k
k k
x x e

− ·
(3.5)
Ma trận hiệp phương sai lỗi tiền ước lượng:
] [
T
k k
e e E P
− − −
· (3.6)
Ma trận hiệp phương sai lỗi hậu ước lượng:
] [
T
k k k
e e E P · (3.7)
Xuất phát từ phương trình cho bộ lọc Kalman, chúng ta đi tìm ra một phương trình tính
toán trạng thái hậu ước lượng
k x

như là một tổ hợp tuyến tính của trạng thái tiền ước
lượng


k x
và sự khác nhau giữa giá trị đo thực tế z
k
và giá trị tiên đoán
k x H

được chỉ
trong phương trình sau.
) (



∧ ∧
− + · k
k
k k x H z K x x
(3.8)
Giá trị
) (


− k
k
x H z
trong công thức (3.8) được gọi là giá trị sai khác giữa giá trị
tiên đoán


k x H
và giá trị thực tế z
k
đo được. Giá trị này bằng 0 nghĩa là hai giá trị hoàn toàn
đồng nhất với nhau.
Ma trận K (mxn) trong phương trình (3.8) gọi là hệ số khuếch đại Kalman nhằm mục
đích tối thiểu hoá hiệp phương sai lỗi hậu ước lượng (3.7). Độ khuếch đại Kalman có thể
được xác định bởi phương trình sau:
R H HP
H P
R H HP H P K
T
k
T
k T
k
T
k k
+
· + ·


− − − 1
) ( (3.9)
Sinh Viên: Cao Ngọc Phát - 29 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Quan sát phương trình (3.9), chúng ta thấy rằng, khi ma trận hiệp phương sai lỗi đo
lượng R tiến tới 0 thì hệ số khuếch đại K được xác định như sau:
1
0
lim


·

H k
k
p
k
Trường hợp khác, khi hiệp phương sai lỗi tiền ước lượng

k
P
tiến tới 0 thì:
0 lim
0
·


k
p
k
k
Khi hiệp phương sai lỗi đo lường R tiến đến 0 thì giá trị z
k
là chính xác hơn, trong khi
giá trị tiên đoán


k x H
lại kém chính xác. Trường hợp, khi giá trị hiệp phương sai lỗi ước
lượng trước

k
P
tiến tới 0, giá trị z
k
là kém chính xác trong khi đó giá trị tiên đoán


k x H
lại
đạt độ chính xác hơn.
3.1.2. Bản chất thống kê của bộ lọc.
Công thức (3.8) thể hiện bản chất thống kê của tiền ước lượng


k x
quy định trên tất cả
các giá trị đo trước z
k
(Quy tắc Bayer).
k
k
x x E

· ] [
k
T
k
k
k
k
P x x x x E · − −
∧ ∧
] ) )( [(
Trạng thái hậu ước lượng trong phương trình (3.8) phản ánh giá trị trung bình (mômen
bậc 1) của phân bố trạng thái nếu các điều kiện (3.3) được thoả mãn. Hiệp phương sai lỗi
trạng thái hậu ước lượng trong công thức (3.6) phản ánh sự thay đổi của phân bố trạng thái
(mômen bậc 2).
) | (
k k
z x P
∼ ) , ( ]) ) )( [( ], [ (
k
k
T
k
k
k
k k
P x N x x x x E x E N
∧ ∧ ∧
· − −
Nói tóm lại, Bộ lọc Kalman ước lượng một quá trình bằng việc sử dụng một dạng của
điều khiển phản hồi: bộ lọc ước lượng trạng thái quá trình tại một vài thời điểm và sau đó
quan sát phản hồi trong dạng của nhiễu đo. Các phương trình trong bộ lọc Kalman chia
thành hai nhóm: các phương trình cập nhật thời gian và các phương trình cập nhật đo.
Các phương trình cập nhật thời gian có nhiệm vụ dự đoán trước trạng thái hiện tại và
hiệp phương sai lỗi ước lượng cho thời điểm tiếp theo. Các phương trình cập nhật đo có
Sinh Viên: Cao Ngọc Phát - 30 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
nhiệm vụ phản hồi, ví dụ việc kết hợp chặt chẽ giá trị đo mới vào giá trị tiền ước lượng để
thu được những cải tiến trong giá trị hậu ước lượng.
Phương trình cập nhật thời gian có thể gọi là phương trình tiên đoán, trong khi phương
trình cập nhật đo có thể coi như phương trình của bộ sửa sai.


Hình 3.1: Thuật toán lọc Kalman.
Các phương trình cập nhật thời gian của bộ lọc Kalman rời rạc
1
1





+ ·
k
k k Bu x A x
Q A AP P
T
k k
+ ·


1
Các phương trình cập nhật đo của bộ lọc Kalman rời rạc:
1
) (
− − −
+ · R H HP H P K
T
k
T
k k
) (



∧ ∧
− + · k
k k
k x H z K x x

− ·
k k k
P H K I P ) (
Sinh Viên: Cao Ngọc Phát - 31 -
Cập nhật
thời gian
(tiên đoán)
Cập nhật đo
(sửa sai)
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông

Hình 3.2: Sơ đồ thực hiện thuật toán Kalman.
3.2. Xây dựng bộ lọc Kalman cải tiến.
3.2.1. Hệ thống dẫn đường tích hợp GPS / INS.
Hệ thống dẫn đường quán tính INS có 2 ưu điểm nổi bật khi so sánh với các hệ thống
dẫn đường khác là khả năng hoạt động tự trị và độ chính xác cao trong những khoảng thời
gian ngắn. Lỗi nghiêm trọng nhất của hệ thống dẫn đường quán tính INS là do các cảm
Sinh Viên: Cao Ngọc Phát - 32 -
Trạng thái
ước lượng
được cập nhật x
k
Z
k
Khởi tạo
Tính hệ số
khuếch đại Kalman k
Cập nhật ước lượng C
Tiên đoán TT
Cập nhật Hiệp phương
sai s
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
biến quán tính gây ra. Chính vì thế trong những ứng dụng thời gian dài thì hệ thống dẫn
đường quán tính INS thường sử dụng với các hệ thống hỗ trợ khác như hệ thống dẫn đường
vô tuyến (Loran, Omega và Tacan), hệ thống dẫn đường vệ tinh (GPS, GLONASS và
Transit), JTIDS, DME…Các hệ thống này hoạt động ổn định theo thời gian và vì thế cần
tích hợp INS và các hệ thống hỗ trợ này. Sự kết hợp GPS và INS là lý tưởng nhất vì hai hệ
thống này có khả năng bù trừ nhau hiệu quả. Trái tim của hệ thống tích hợp này chính là bộ
lọc tối ưu Kalman.
Bộ lọc Kalman rất hiệu quả và linh hoạt trong việc kết hợp đầu ra bị nhiễu của cảm biến
quán tính để ước lượng trạng thái của hệ thống không ổn định. Tín hiệu bị nhiễu từ các cảm
biến quán tính và GPS bao gồm các thông tin về vị trí, vận tốc, toạ độ của vật thể bay.
Những tác nhân làm hệ thống không ổn định là nhiễu do cảm biến, do người sử dụng và
nhiễu do môi trường. Bộ lọc Kalman dùng để ước lượng lỗi do các cảm biến quán tính gây
ra và ta xây dựng được vectơ trạng thái
k

của bộ lọc từ các lỗi này. Các giá trị thu được từ
GPS dùng để xây dựng vectơ đo lường z. Sau khi mô hình hoá các lỗi này thì thực hiện chu
trình Kalman với các ước lượng vectơ trạng thái và ma trận tương quan tại thời điểm ban
đầu. Cấu trúc này được gọi là cấu trúc GPS hỗ trợ INS và các lỗi được xử lý theo kiểu vòng
mở và vòng đóng như mô tả trong hình 3.3 và 3.4.
Cấu trúc vòng mở cho phép thực thi dễ dàng hơn nhưng cấu trúc vòng kín lại cho kết
quả chính xác hơn.
Hình 3.3: Cấu trúc GPS/INS vòng mở.
Sinh Viên: Cao Ngọc Phát - 33 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Hình 3.4: Cấu trúc GPS/INS vòng kín.
3.2.2. Bộ loc kalman cải tiến.
Trong khoá luận này, cấu trúc lọc Kalman được phát triển bao gồm hai bộ lọc song
song được sử dụng rất linh hoạt trong hệ thống dẫn đường. Khi tín hiệu GPS thu được tốt
thì hệ thống INS/GPS hoạt động với cấu hình vòng kín. Khi tín hiệu GPS bị mất thì hệ
thống dễ dàng chuyển sang cấu hình vòng mở. Khi tín hiệu GPS lại được khôi phục thì hệ
thống lại trở về cấu hình vòng kín. Việc chuyển linh hoạt giữa hai cấu hình vòng kín và
vòng hở giúp nâng cao chất lượng của toàn bộ hệ thống và khắc phục được nhược điểm của
cả hai cấu hình riêng lẻ.
Hình 3.5: Bộ lọc kalman song song.
Sinh Viên: Cao Ngọc Phát - 34 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Bộ lọc Kalman thứ nhất (KF1) được thiết lập với véctơ 3 trạng thái là sai số vận tốc
(e
VN
, e
VE
, e
VD
), véctơ đo lường là hiệu giữa vận tốc của GPS và INS. Tốc độ cập nhật của
vận tốc GPS là 1 Hz và của INS là 64 Hz. Vì thế tốc độ cập nhật của bộ lọc Kalman có thể
lựa chọn là 1 Hz. Chúng ta có thể lựa chọn tốc độ cập nhật cao hơn cho đầu ra của bộ lọc
Kalman này (chẳng hạn là 2 s) với giả thiết rằng khoảng thời gian 0.5 s thì vận tốc của vật
thể chuyển động là không thay đổi. Ưu điểm của bộ lọc ít trạng thái KF1 này là tốc độ hội
tụ nhanh, tránh được các sai số như bộ lọc Kalman không hội tụ hoặc sai sót khi thiết lập
mô hình trạng thái. Đầu ra của KF1 được sử dụng để bù trừ cho đầu ra của hệ thống.
Bộ lọc Kalman thứ hai KF2 có thể ước lượng tám trạng thái của hệ thống bao gồm:
các lỗi vận tốc trên hệ toạ độ dẫn đường (e
VN
, e
VE
, e
VD
), độ trôi của các con quay vi cơ (G
Bx
,
G
By
, G
Bz
) và các lỗi góc nghiêng (Tn, Te). Lỗi ước lượng của INS được sử dụng để hiệu
chỉnh ma trận chuyển hệ toạ độ
N
b
C và véctơ quaternion. Các ước lượng về độ trôi của con
quay vi cơ được phản hồi về khối dẫn đường quán tính SINS.
Ta có A
k,k-1
là ma trận chuyển trạng thái và bởi vì khoảng thời gian này nhỏ (tức là tốc
độ cập nhật của INS là lớn - ở đây là 64 Hz), chúng ta có thể xấp xỉ A
k,k-1
như sau:
t F I e A
t F
k k
∆ + ≈ ·


) (
1 ,
(3.10)
1
1
1
1
1
1
1
1
1
1
1
]
1

¸





·

β
β
β
N
N
N
N N N
N N N
k k
h
h
h
Dvd
Dvd
C h C h C h
C h C h C h
A
0 0 0 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0 0 0
0 0 0 0 0
0 0 0 0 0
23 22 21
13 12 11
1 ,
(3.11)
Ở đó Dvd là độ tăng vận tốc theo hướng tâm trái đất của hệ trục toạ độ dẫn đường,
β

là tham số của hàm tương quan, h
N
=1/64 s, và ma trận hiệp phương sai ứng với w
k
là:
[ ] t A GQG A w w E Q
T
k
T
k
T
k k k
∆ ≈ · (3.12)
Ở đó:
Sinh Viên: Cao Ngọc Phát - 35 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
[ ]
2 2 2 2 2 2
x x x az ay ax
diag Q
ω ω ω
σ σ σ σ σ σ ·
Xét phương trình đo lường:
z
k
= H
k
x
k
+ v
k
(3.13)
Ở đó:

,
_

¸
¸



·
GPS INS
GPS INS
GPS INS
k
Vd Vd
Ve Ve
Vn Vn
z
,
( )
5 3 3 3
0
× ×
· I H
k


[ ] [ ]
2 2 2
Vd Ve Vn
T
k k k
diag v v E R σ σ σ · ·
Là ma trận hiệp phương sai của v
k
.
Trong trường hợp tín hiệu GPS bị mất hoặc không đáng tin cậy, thông tin thu được sẽ
được lấy từ khối INS cho tới khi tín hiệu GPS được khôi phục. Trong khoảng thời gian đó,
các lỗi dẫn đường sẽ tăng nhanh chóng theo thời gian do nhược điểm của các cảm biến vi
cơ điện tử. Vì thế, hệ thống của chúng ta cần phải tận dụng được các phép đo ảo dựa vào
tính chất động học của vật thể chuyển động (ở đây tập trung vào các phương tiện chuyển
động trên mặt đất). Cụ thể, trong khoá luận có thể tận dụng những ràng buộc về vận tốc và
độ cao của vật thể. Trong nhiều tình huống thì có thể coi độ cao của phương tiện chuyển
động bằng 0, tuy nhiên không thể áp dụng điều kiện này trên toàn bộ tuyến đường.
Ràng buộc của vận tốc:
0 ) (
0 ) (
·
·
t V
t V
b
z
b
x
(3.14)
Phương trình 21 được chuyển sang hệ dẫn đương như sau:
1
1
1
]
1

¸

·
1
1
1
]
1

¸

b
z
b
y
b
x
V
V
V
n
b
D
E
N
C
V
V
V
(3.15)
Với các quãng đường dài, phương tiện chuyển động cần dừng lại để cập nhật lỗi.
Trong trường hợp này thì các vận tốc trở về 0. Lúc này thì các vận tốc đầu ra của INS chính
Sinh Viên: Cao Ngọc Phát - 36 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
là các lỗi vận tốc. Việc xác định xem phương tiện đã hoàn toàn dừng lại hay chưa sẽ dựa
vào vận tốc của GPS hoặc đầu ra của cảm biến gia tốc. Tín hiệu gia tốc sẽ được xử lí bằng
phương pháp cửa sổ trượt với kích thước cửa sổ cỡ vài giây.Ngưỡng xác định xe dừng hẳn
là 0.05 m/s. Khi phương tiện dừng hoàn toàn, vận tốc theo hướng Đông và Bắc sẽ đưa vào
bộ lọc. Cách xử lí này khác với cách xử lí thông thường là véctơ đo lường được tính bằng
hiệu số giữa vận tốc của INS và GPS. Để cho việc ước lượng lỗi INS chính xác hơn thì
véctơ đo lường cần được gia tăng so với khi phương tiện chuyển động nhằm thu được độ
khuếch đại Kalman lớn hơn.
Khi mất GPS, các góc tư thế của vật thể nhờ việc dự đoán véctơ trạng thái. Ngoài ra,
để phục vụ cho việc hiệu chỉnh góc tư thế của hệ thống thì chúng ta còn có thể sử dụng
thông tin góc hướng từ GPS hoặc cảm biến từ. Với ứng dụng 2D thì góc hướng tính theo
GPS được sử dụng với điều kiện rằng vận tốc chuyển động lúc này phải đủ lớn. Góc hướng
của GPS được tính như sau:
GPS
N
GPS
E
GPS
V
V
tg H
1 −
·
(3.16)
Có thể phân loại các sai số trong các khối dẫn đường quán tính thành các sai số tất
định và các sai số ngẫu nhiên. Các sai số tất định có thể loại trừ khá dễ dàng nhờ các
phương pháp căn chuẩn thiết bị. Tuy nhiên, loại bỏ các sai số ngẫu nhiên trong hệ thống
dẫn đương lại là một vấn đề phức tạp. Trước tiên, chúng ta phải đặc trưng được các nhiễu
ngẫu nhiên tác động lên hệ thống sử dụng phương pháp mật độ phổ công suất, phương
pháp phương sai Allan hoặc kết hợp cả hai phương pháp này. Phương pháp xác định các
thông số nhiễu đã được trình bày chi tiết trong báo cáo của đề tài QGTĐ 05-09, và được
nêu lại ở phần nhiễu cũa khối IMU. Sau khi đặc trưng được sai số của khối IMU thì các
thông số này sẽ được sử dụng trong các khối lọc Kalman nhằm bù trừ sai số .
Chương 4: Các kết qủa thực nghiệm.
Sinh Viên: Cao Ngọc Phát - 37 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông

4.1 Thiết bị phần cứng.
4.1.1 Khối dẫn đường quán tính MICRO-ISU BP3010.
MICRO-ISU BP3010 là một khối IMU được tích hợp 3 cảm biến vận tốc góc
ADXR300, và 3 cảm biến gia tốc ADXL210E có bù nhiệt theo mô hình gắn liền. Sáu lối ra
của 6 cảm biến này được đưa qua một mạch tích phân sáu đường để thu được 3 độ tăng về
góc dThx, dThy, dThz và 3 độ tăng vận tốc là dVx, dVy, dVz. Các vi điều khiển trong khối
IMU này tổng hợp dữ liệu tính Coning và Sculling và giao tiếp với bên ngoài qua giao diện
RS232. Khung truyền dữ liệu ra ngoài được cập nhật với các tốc độ khác nhau là 8Hz,
16Hz, 32Hz và 64Hz, nhưng tốt nhất vẫn là 64Hz. Thông tin chi tiết được trình bày trên
trang Web của nhà sản xuất BEC Navigation System .
Hình 4.1: Hệ INS/GPS được kết nối (QGTĐ 05-09).
4.1.2 Thiết bị thu GPS
Thiết bị thu GPS HI-204E (Hình 4.2) của hãng Haicom có các đặc điểm sau:
 Có 12 kênh dẫn đường
 Độ nhạy tối thiểu -165 dB.
 Truyền dữ liệu qua cổng USB .
Sinh Viên: Cao Ngọc Phát - 38 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
 Chuẩn truyền NMEA-108 với tốc độ 4800 baud
 Tốc độ cập nhật khung dữ liệu 1Hz
 Sai số định vị khi không có SA là 25m.
Các thông tin thu từ GPS theo chuẩn NMEA-108: GGA, GLL, GSA, GSV, RMC, và VTG
mang thông tin về thời gian chuẩn, kinh độ , vĩ độ, tư thế, vận tốc, và một số thông tin
khác.
Thông tin chi tiết được trình bày trên trang Web của nhà sản xuất Haicom [8].
Hình 4.2: Thiết bị thu GPS HI-204E.

4.2. Kết quả thực nghiệm.
4.2.1. Xây dựng hệ tích hợp INS/GPS theo thời gian thực.
a. Thiết kế và mô hình hoá hệ thống dẫn đường tích hợp GPS / INS: mức hệ thống
Công cụ SIMULINK rất thuận tiện và hiệu quả trong việc thiết kế và mô phỏng hệ thống
dẫn đường ở mức hệ thống và có thể hỗ trợ trực tiếp cho mức thực thi. Toàn bộ hệ thống
dẫn đường đã được mô phỏng và kiểm tra nhờ ngôn ngữ lập trình nhúng trong môi trường
SIMULINK/MATLAB,đã tận dụng được ưu điểm của cả SIMULINK và lập trình với các
lệnh M-files. Điều này sẽ rất thuận tiện để phát triển chương trình thời gian thực. Ngoài các
tập dữ liệu mô phỏng, chương trình này còn có thể xử lí các dữ liệu thực (đo trực tiếp từ
GPS và INS trên các quỹ đạo thực nghiệm). Rất nhiều tập dữ liệu khác nhau đã đuợc thử
nghiệm trên chương trình này.
Sinh Viên: Cao Ngọc Phát - 39 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Hình 4.3: Chương trình thiết kế, mô phỏng trên SIMULINK/MATLAB.
b. Thiết kế và mô hình hoá hệ thống dẫn đường tích hợp GPS / INS: mức thực thi
Thuật toán dẫn đường quán tính và lọc Kalman được xây dựng trên ngôn ngữ C và
VC++ để thu thập dữ liệu từ khối IMU, GPS và la bàn từ phục vụ cho việc thực hiện dẫn
đường quán tính theo thời gian thực. Bài toán xử lý thời gian thực là bài toán quản lý tài
nguyên phần cứng và tối ưu hoá phần mềm. Công việc phải làm là đồng thời thu thập dữ
liệu từ GPS, INS, la bàn từ rồi thực hiện tính toán trên INS và thực hiện lọc Kalman. Hệ
dẫn đường quán tính có tốc độ thu dữ liệu của GPS là 480x0baud, tốc độ dữ liệu từ khối
IMU là 38400 baud và tốc độ truyền khung INS 64Hz thì việc thu thập dữ liệu và xử lý
cũng là một thách thức.
Giao diện chương trình VC++ được mô tả trong hình 4.4. Với giao diện này người
dùng có thể quan sát được các góc định hướng thể hiện trạng thái vật thể và các vận tốc, vị
trí hiện tại của vật thể đo bởi hệ GPS/INS cũng như các thông tin thu được từ GPS.
Hiện tại vấn đề xây dựng chương trình thời gian thực cho quỹ đạo ngoài phòng thí
nghiệm đã được xây dựng xong và đưa vào thử nghiệm cho một số kết quả rất khả quan.
Để đảm bảo độ tin cậy của hệ thống thì thử nghiệm chạy thời gian thực với nhiều quỹ đạo
khác nhau và nhiều điều kiện khác nhau rồi phân tích.
Sinh Viên: Cao Ngọc Phát - 40 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Hình 4.4: Giao diện dẫn đuờng quán tính viết bằng VC++ (mức thực thi).
4.2.2. Kết quả.
Để thực hiện trên các quỹ đạo ngoài phòng thí nghiệm thì GPS và hệ thống được lắp
đặt trên một ôtô. Lúc đầu thì ôtô ở trạng thái đứng yên, nổ máy trong khoảng 100 s. Thao
tác này cho ta tập dữ liệu tất định phục vụ cho việc căn chuẩn và chỉnh vị trí của INS.
Tốc độ cập nhật từ các cảm biến quán tính là 64 Hz, từ GPS là 1 Hz, và từ Kalman là 2
Hz nhằm thu được kết quả có độ chính xác cao hơn.
Để làm rõ tính ưu việt của cấu trúc thực thi mới thì trên quỹ đạo thực nghiệm ngắt bỏ
tín hiệu GPS trong 100 giây (từ giây thứ 650 tới 750). Hình 4.5.a mô tả quỹ đạo của vật thể
sử dụng cấu trúc phản hồi. Có thể thấy rằng đầu ra của hệ thống (đường liền) có sự sai khác
rất lớn so với quỹ đạo của GPS. Mô hình hệ thống kết hợp thể hiện rõ ưu điểm trên hình
4.1.b với sai số vị trí là nhỏ nhất. Ở đó, cấu hình vòng kín được sử dụng khi thu được tín
hiệu từ GPS và sẽ chuyển sang cấu hình vòng hở lúc mất GPS. Hình 4.5.a và b đều mô tả
thông tin vị trí theo quãng đường đối tượng chuyển động theo hướng Bắc và hướng Đông
thay cho hiển thị kinh độ, vĩ độ.
Sinh Viên: Cao Ngọc Phát - 41 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
(a)
(b)
Hình 4.5: So sánh lỗi vị trí: cấu hình phản hồi (a) và cấu hình kết hợp (b )khi mất GPS.
Sinh Viên: Cao Ngọc Phát - 42 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Nhằm dễ dàng đánh giá chất lượng của hệ dẫn đường thì một thí nghiệm khác được
thực hiện với quỹ đạo nhỏ và phương tiện chuyển động trong khoảng thời gian khoảng 80s.
Hình 4.6: Quỹ đạo nhỏ thực nghiệm
Hình 4.7 và 4.8 mô tả vận tốc của KF khi so sánh với vận tốc của GPS. Thấy rằng đầu
ra của KF (nét đứt) bám sát vận tốc của GPS với sai số rất nhỏ trong khoảng thời gian xấp
xỉ 3 phút.
Hình 4.7: So sánh vận tốc theo hướng Đông khi vật thể chạy theo quỹ đạo thực tế.
Sinh Viên: Cao Ngọc Phát - 43 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
Hình 4.8: So sánh vận tốc theo hướng Bắc khi vật thể chạy theo quỹ đạo thực tế.
Hình 4.9 là các góc ngiêng và góc chúc của hệ thống sau khi đã được tính toán và
hiệu chỉnh nhờ bộ lọc Kalman. Trong khoảng 10 giây đầu lúc phương tiện chuẩn bị chuyển
động thì góc nghiêng và góc chúc gần như không đổi. Khi chuyển động, các giá trị góc vẫn
chạy quanh điểm cân bằng với độ lệch nhỏ. Các kết quả này là hợp lí với tình huống chạy
thử nghiệm trên quỹ đạo hai chiều khá bằng phẳng.
Hình 4.9: Góc ngiêng và góc chúc khi vật thể chạy theo quỹ đạo thực tế.
Quỹ đạo 2-D được trình bày trên hình 4.10. Lưu ý là ở đây chúng ta không trình bày
với thứ nguyên rad của kinh độ, vĩ độ. Nguyên nhân đó là tránh sự mất ổn định trong tính
Sinh Viên: Cao Ngọc Phát - 44 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
toán. Có thể thấy quỹ đạo của hệ tích hợp vòng kín bám rất sát với quỹ đạo của GPS với lỗi
nhỏ (< 2 m) trong suốt 1 vòng kín.
Hình 4.10: So sánh quỹ đạo thu được trên các hệ thống khác nhau.
KẾT LUẬN
Trong quá trình thực hiện khoá luận em đã thu được một số kết quả như sau:
- Tìm hiểu được cấu trúc, thành phần, hoạt động và ứng dụng của hệ thống định vi
toàn cầu GPS.
- Tìm hiểu được tổng quan hệ thống dẫn đường quán tính INS.
- Nắm bắt lý thuyết của hệ thống dẫn đường quán tính.
- Tìm hiểu cấu trúc lọc tối ưu Kalman nói chung và bộ lọc Kalman cải tiến trong bài
toán dẫn đường.
- Tìm hiểu và xây dựng bộ lọc Kalman cải tiến cho hệ dẫn đường quán tính trên mô
hình Matlab - Simulink.
- Xây dựng hệ thống dẫn đường quán tính sử dụng bộ lọc Kalman cải tiến theo thời
gian thực bằng ngôn ngữ VC++.
Sinh Viên: Cao Ngọc Phát - 45 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
- Thực nghiệm đánh giá chất lượng hệ thống dẫn đường đã xây dựng trên các quỹ đạo
thực tế.
Tài liệu tham khảo.
[1] Đề tài QGTĐ 05-09. Chủ biên GS.TS Nguyễn phú Thuỳ. Năm 2007
[2] Đề tài QC 07.17 Chủ biên thạc sỹ Trần Đức Tân. Năm 2008.
[3] Vikas Kumar N, Integration of Inertial Navigation System and Global
Positioning System Using Kalman Filtering, M.Tech. Dissertation, Indian
Institute of Technology, Bombay, July 2004.
[4] Oleg S. Salychev, Applied Inertial Navigation: Problems and Solutions,
BMSTU Press, Moscow Russia, 2004.
[5] Georey J.Bulmer, “In MICRO-ISU BP3010 An OEM Miniature Hybrid 6
Degrees-. Of-Freedom Inertial Sensor Unit”, Gyro Symposium, Stuttgart
16th-17th September, 2003.
[6] Peter S. Maybeck, Stochastic models, estimation, and control, Academic Press,
Vol. 1, 1994.
Sinh Viên: Cao Ngọc Phát - 46 -
Đại Học Công Nghệ. ĐHQGHN Khoa: Điện tử-Viễn Thông
[7] Haiying Hou, Modeling inertial sensors errors using Allan variance, UCEGE
reports number 20201, Master's thesis, University of Calgary, September
2004.
[8] Wang, J., Lee, H.K., Rizos, C., “GPS/INS Integration: A Performance
Sensitivity Analysis”, Wuhan University Journal of Nature Sciences, 8(2B), pp.
508-516.
[9] Gyro, Accelerometer Panel of the IEEE Aerospace, and Electronic Systems
Society, Draft recommended practice for inertial sensor test equipment,
instrumentation, data acquisition and analysis, In IEEE Std Working Draft P1554/
D14.
[10] Panzieri, S., Pascucci, F., Ulivi, G., “An Outdoor navigation system using
GPS and Inertial Platform”, IEEE ASME Transactions on Mechatronics, Vol. 7.
(2002).
Sinh Viên: Cao Ngọc Phát - 47 -