Luận án Tiến sĩ Kỹ thuật: Nâng cao chất lượng cho các thiết bị định vị dẫn đường sử dụng GPS phục vụ bài toán giám sát quản lý phương tiện giao thông đường bộ

Bé GI¸O DôC Vμ §μO T¹O

TR¦êNG §¹I HäC GIAO TH¤NG VËN T¶I

NG¤ THANH B×NH

N¢NG CAO CHÊT L¦îNG CHO C¸C THIÕT BÞ §ÞNH VÞ

DÉN §¦êNG Sö DôNG GPS PHôC Vô BμI TO¸N GI¸M S¸T

QU¶N Lý PH¦¥NG TIÖN GIAO TH¤NG §¦êNG Bé ngμnh: Kü thuËt ®iÒu khiÓn vμ tù ®éng hãa

M· sè: 62520216

LUËN ¸N TIÕN SÜ Kü THUËT Hμ Néi - 2015

Bé GI¸O DôC Vμ §μO T¹O TR¦êNG §¹I HäC GIAO TH¤NG VËN T¶I

NG¤ THANH B×NH

N¢NG CAO CHÊT L¦îNG CHO C¸C THIÕT BÞ §ÞNH VÞ

DÉN §¦êNG Sö DôNG GPS PHôC Vô BμI TO¸N GI¸M S¸T

QU¶N Lý PH¦¥NG TIÖN GIAO TH¤NG §¦êNG Bé ngμnh: Kü thuËt ®iÒu khiÓn vμ tù ®éng hãa

M· sè: 62520216

LUËN ¸N TIÕN SÜ Kü THUËT

Ng−êi h−íng dÉn khoa häc: GS.TS Lª Hïng L©n

PGS.TS. NguyÔn Thanh H¶i Hμ Néi - 2015

LỜI CAM ĐOAN

Tôi xin cam đoan đây là công trình nghiên cứu của riêng tôi, tất cả các ý

tƣởng tham khảo từ kết quả nghiên cứu công bố trong các công trình khác đều

đƣợc nêu rõ trong luận án. Các số liệu, kết quả nêu trong luận án là hoàn toàn

trung thực và chƣa từng đƣợc ai công bố trong bất kỳ công trình khoa học nào

khác.

Tác giả

LỜI CẢM ƠN

Lời đầu tiên, tôi xin gửi lời cảm ơn chân thành đến hai ngƣời thầy là GS.TS. Lê

Hùng Lân và PGS.TS. Nguyễn Thanh Hải về sự hƣớng dẫn tận tình của các thầy trong

quá trình tôi học tập, nghiên cứu và thực hiện luận án này.

Tôi cũng xin gửi lời cảm ơn sự giúp đỡ, cộng tác đầy nhiệt tình của các giáo sƣ

và đồng nghiệp Nhóm Cơ – Điện tử (Mechatronics Research Group), Khoa Kỹ thuật

Công nghiệp (Industrial Engineering), Trƣờng Đại học Trento – Italia, đặc biệt là giáo

sƣ Francesco Biral.

Nhân đây, tôi cũng xin gửi lời cảm ơn đến các thầy cô và đồng nghiệp trong

Khoa Điện - Điện Tử, Trƣờng Đại học Giao thông Vận tải; Vụ Khoa học công nghệ -

Bộ Giao thông vận tải; Viện Hàn lâm Khoa học và Công nghệ Việt Nam; Phòng thí

nghiệm – Trung tâm khoa học công nghệ GTVT; Viện Khoa học GTVT; Công ty

Than Đèo Nai; Công ty Thông tin Tín hiệu đƣờng sắt Hà nội; Công ty Vận tải Trento

– Italia (Trentino Trasporti) đã đóng góp ý kiến, tạo điều kiện làm việc thuận lợi và

hợp tác triển khai ứng dụng kết quả của luận án.

Tôi xin cảm ơn sự tài trợ kinh phí cho việc nghiên cứu để hoàn thiện nội dung

luận án này từ Bộ Giáo dục và Đào tạo, Trƣờng Đại học Giao thông Vận tải, Tổ chức

World Bank, Đại sứ quán Ấn Độ và Quỹ học bổng Erasmus Mundus.

Nhân dịp này, tôi xin gửi lời cảm ơn đến các thành viên trong gia đình cùng

những ngƣời bạn của tôi, những ngƣời đã hết lòng giúp đỡ, động viên, tạo điều kiện

thuận lợi trong suốt thời gian qua để tôi có đƣợc cơ hội hoàn thành tốt luận án này.

iii

MỤC LỤC

LỜI CAM ĐOAN ........................................................................................................... i

LỜI CẢM ƠN ................................................................................................................ ii

MỤC LỤC ..................................................................................................................... iii

DANH MỤC BẢNG .................................................................................................... vii

DANH MỤC HÌNH VẼ ............................................................................................... vii

DANH MỤC CÁC KÝ HIỆU, CHỮ VIẾT TẮT .......................................................... x

MỞ ĐẦU ..................................................................................................................... xiv

1. Giới thiệu tóm tắt luận án .................................................................................... xiv

2. Đặt bài toán.......................................................................................................... xiv

3. Tính cấp thiết và lý do chọn đề tài ....................................................................... xv

4. Mục đích, đối tƣợng, phạm vi nghiên cứu và kết quả mong đợi của luận án ..... xvi

4. Bố cục của luận án .............................................................................................. xvii

5. Ý nghĩa khoa học và thực tiễn của luận án .......................................................... xix

6. Tính sáng tạo và kết quả nghiên cứu ................................................................... xix

7. Những đóng góp của luận án ............................................................................... xix

TỔNG QUAN VỀ BÀI TOÁN GIÁM SÁT ................................................................. 1

1. Khái quát về bài toán giám sát sử dụng GPS ......................................................... 1

2. Phân tích đánh giá các công trình khoa học liên quan mật thiết đến đề tài nghiên

cứu trên thế giới .......................................................................................................... 2

3. Phân tích đánh giá các công trình khoa học liên quan mật thiết đến đề tài nghiên

cứu tại Việt Nam ........................................................................................................ 4

4. Đề xuất các giải pháp mới, nội dung và phƣơng pháp nghiên cứu ........................ 6

CHƢƠNG 1. HỆ THỐNG GPS VÀ CÁC HỆ THỐNG HỖ TRỢ ............................... 8

1.1. Hệ thống GPS ...................................................................................................... 8

1.1.1. Hệ trục toạ độ ................................................................................................ 8

1.1.2. Cấu trúc của một hệ định vị sử dụng vệ tinh ................................................ 9

1.1.3. Nguyên lý hoạt động ................................................................................... 10

1.1.4. Sai số và các nguyên nhân gây sai số ......................................................... 11

1.2. Hệ thống INS ..................................................................................................... 12

1.2.1. Các hệ tọa độ (Frames) ............................................................................... 12

1.2.2. Các thành phần đặc trƣng cho chuyển động ............................................... 14

1.2.3. Ma trận chuyển vị giữa các hệ tọa độ ......................................................... 16

1.2.4. Định vị sử dụng cảm biến quán tính ........................................................... 17

1.2.5. Sai số và quá trình căn chỉnh ban đầu ........................................................ 18

1.3. Hệ thống định vị tích hợp GPS/INS .................................................................. 20

1.3.1. Bản chất bù giữa INS và GPS ..................................................................... 20

1.3.2. Các phƣơng pháp tích hợp GPS/INS .......................................................... 21

1.3.3. Bài toán giám sát và đối tƣợng áp dụng ..................................................... 23

1.4. Kết luận chƣơng 1 ............................................................................................. 24

CHƢƠNG 2. LỌC KALMAN VÀ MATLAB TOOL-BOX ....................................... 25

2.1. Lọc Kalman KF ................................................................................................. 25

2.1.1. Bản chất toán học ........................................................................................ 25

2.1.2. Bộ lọc Kalman rời rạc ................................................................................. 26

2.2. Bộ lọc Kalman mở rộng EKF ............................................................................ 28

2.2.1. Bộ lọc Kalman mở rộng dạng 1 .................................................................. 29

2.2.2. Bộ lọc Kalman mở rộng dạng 2 .................................................................. 30

2.3. Bộ lọc UKF........................................................................................................ 31

2.4. Đánh giá các thuật toán lọc Kalman và kết quả mô phỏng ............................... 34

2.5. Kết luận chƣơng 2 ............................................................................................. 38

CHƢƠNG 3. PHƢƠNG PHÁP TỰ ĐỘNG HIỆU CHỈNH MA TRẬN QUAY CHO

HỆ THỐNG INS .......................................................................................................... 39

3.1. Giới thiệu phƣơng pháp tự động hiệu chỉnh ma trận quay ................................ 39

3.1.1. Nhận định chung ......................................................................................... 39

3.1.2. Nguyên tắc sử dụng trận quay R trong kiểm soát và định hƣớng .............. 46

3.1.3. Phát triển phƣơng pháp hiệu chỉnh mới trên cơ sở MEMS INS đa trục .... 48

3.2. Phát triển phƣơng pháp tự động hiệu chỉnh ma trận quay trên cơ sở MEMS INS

9-DOF ....................................................................................................................... 52

3.2.1. Nguyên tắc tính toán ................................................................................... 52

3.2.2. Tái chuẩn hóa .............................................................................................. 55

3.2.3. Hiệu chỉnh trôi dạt góc yaw ........................................................................ 60

3.2.4. Hiệu chỉnh trôi góc roll-pitch ..................................................................... 64

3.2.5. Phản hồi điều khiển bù sai lệch .................................................................. 66

3.2.6. Kết quả thử nghiệm với MEMS INS 9-DOF .............................................. 68

3.3. Kết luận chƣơng 3 ............................................................................................. 71

CHƢƠNG 4. THIẾT KẾ HỆ THỐNG TÍCH HỢP GPS/INS ..................................... 72

4.1. Giải pháp sử dụng lọc và bù dữ liệu cho thiết bị tích hợp GPS/INS ................. 72

4.1.1. Giải pháp sử dụng lọc Kalman trên thiết bị xe ........................................... 72

4.1.2. Phƣơng pháp tính toán trên miền rời rạc .................................................... 75

4.1.3. Giải pháp bù dữ liệu ................................................................................... 78

4.2. Đề xuất giải pháp phân tán cho hệ thống tích hợp GPS/INS ............................ 81

4.3. Thiết kế hệ thống tích hợp GPS/INS với MEMS INS 9-DOF và bus CAN ..... 82

4.4. Ứng dụng phƣơng pháp hiệu chỉnh các phần tử ma trận quay trong phát triển

firmware cho hệ thống nhúng trên xe bus ................................................................ 86

4.5. Thiết kế bộ lọc UKF và kết quả thực tế trên hệ thống giám sát tại trạm ứng

dụng cho xe bus ........................................................................................................ 93

4.5.1. Đối tƣợng và mô hình xe bus...................................................................... 93

4.5.2. Xây dựng bộ lọc UKF cho xe bus .............................................................. 96

4.5.3. Kết quả của hệ thống giám sát .................................................................. 100

4.6. Kết luận chƣơng 4 ........................................................................................... 107

KẾT LUẬN VÀ KIẾN NGHỊ ................................................................................... 109

I. Kết luận về luận án .............................................................................................. 109

II. Kiến nghị và hƣớng nghiên cứu tiếp theo .......................................................... 109

DANH MỤC CÁC CÔNG TRÌNH CÔNG BỐ CỦA TÁC GIẢ LIÊN QUAN ĐẾN

LUẬN ÁN .................................................................................................................. 110

TÀI LIỆU THAM KHẢO .......................................................................................... 111

A. TIẾNG VIỆT ..................................................................................................... 111

B. TIẾNG ANH ...................................................................................................... 111

C. INTERNET ........................................................................................................ 118

PHỤ LỤC 1: CÁC CÔNG VIỆC HOÀN THÀNH TRONG THỜI GIAN THỰC

HIỆN LUẬN ÁN ........................................................................................................... II

1. Các công trình khoa học đã công bố ..................................................................... II

2. Các đề tài, dự án, nhiệm vụ khác đã chủ trì hoặc tham gia ................................... V

PHỤ LỤC 2: KẾT QUẢ ĐÁNH GIÁ TẠI TRENTO, ITALIA ................................. VII

PHỤ LỤC 3: MỘT SỐ DỮ LIỆU VÀ ĐOẠN MÃ CHƢƠNG TRÌNH ..................... IX

1. Cấu trúc dữ liệu .................................................................................................... IX

2. Chƣơng trình DCM và dữ liệu ở chế độ tĩnh .................................................... XIII

3. Script Matlab cho chƣơng trình tại trạm ........................................................ XXVI

vii

DANH MỤC BẢNG

Bảng 1: Nguồn gốc và nguyên nhân gây sai số GPS ................................................... 12

Bảng 2: Các lỗi chính của hệ strapdown ...................................................................... 18

Bảng 3: Các báo cáo khoa học đã công bố .................................................................. IV

Bảng 4: Các đề tài, dự án, nhiệm vụ khoa học ............................................................. VI

Bảng 5: Cấu trúc dữ liệu GPS ...................................................................................... IX

Bảng 6: Cấu trúc dữ liệu INS ....................................................................................... XI

Bảng 7: Cấu trúc dữ liệu FMS .................................................................................... XII

Bảng 8: Dữ liệu INS bị trôi khi không sử dụng thuật toán tự động hiệu chỉnh ma trận

quay tính từ bƣớc tính thứ nhất (B8.1), 500 (B8.2), 1000 (B8.3), 2000 (B8.4), 3000

(B8.5), 4000 (B8.6) ..................................................................................................... XX

Bảng 9: Dữ liệu INS không bị trôi khi sử dụng thuật toán thuật toán tự động hiệu

chỉnh ma trận quay tính từ bƣớc tính thứ nhất (B9.1), 500 (B8.2), 1000 (B8.3), 2000

(B8.4), 3000 (B8.5), 4000 (B8.6) ............................................................................ XXIII

DANH MỤC HÌNH VẼ

Hình 1. 1: Hệ toạ độ tâm trái đất Oxyz .......................................................................... 8

Hình 1. 2: Vệ tinh GPS IIIC và quỹ đạo các vệ tinh trong không gian ....................... 10

Hình 1. 3: Tín hiệu từ vệ tinh và nguyên tắc xác định vị trí đối tƣợng ........................ 11

Hình 1. 4: Hệ tọa độ vật thể và các thành phần đặc trƣng cho chuyển động ............... 12

Hình 1. 5: Hệ toạ độ quán tính ECI .............................................................................. 13

Hình 1. 6: Hệ tọa độ CCS và hệ NWU ........................................................................ 14

Hình 1. 7: Các góc quay roll, pitch, yaw ...................................................................... 15

Hình 1. 8: Góc trƣợt α .................................................................................................. 15

Hình 1. 9: Góc trƣợt β .................................................................................................. 16

viii

Hình 2. 1: Mô hình hoá hoạt động của lọc Kalman ..................................................... 25

Hình 2. 2: Cơ chế lọc Kalman ...................................................................................... 25

Hình 2. 3: Bản chất hoạt động của bộ lọc Kalman ...................................................... 26

Hình 2. 4: Biểu diễn hàm trọng lƣợng w (Weighted sigma-points) ............................. 31

Hình 2. 5: Tính toán điểm sigma cho lọc UKF ............................................................ 32

Hình 2. 6: Sơ đồ biến đổi UT ....................................................................................... 33

Hình 2. 7: Kết quả sử dụng bộ lọc KF và EFK ............................................................ 37

Hình 2. 8: Kết quả sử dụng bộ lọc UKF ...................................................................... 37

Hình 3. 1: Ví dụ mô tả chuyển động của đối tƣợng và đặc điểm của R ...................... 41

Hình 3. 2: Quay hệ tọa độ vật thể P so với hệ tọa độ trái đất G .................................. 42

Hình 3. 3: Lệch trực giao và tích chéo ......................................................................... 45

Hình 3. 4: Cấu trúc bộ lọc chống trôi các phần tử của ma trận quay William

Premerlani và Sergiu Baluta ........................................................................................ 51

Hình 3. 5: Điều chỉnh trƣợt góc hƣớng (yaw correction) ............................................ 62

Hình 3. 6: Hiệu chỉnh góc quay roll-pitch ................................................................... 65

Hình 3. 7: Cơ chế của thuật toán lọc các phần tử ma trận quay................................... 67

Hình 3. 8: Dữ liệu INS bị trôi khi không sử dụng thuật toán DCM ............................. 69

Hình 3. 9: Dữ liệu INS không bị trôi khi có sử dụng thuật toán DCM ........................ 70

Hình 4. 1: Phƣơng pháp truyền thẳng vòng lặp mở ..................................................... 72

Hình 4. 2: Phƣơng pháp phản hồi vòng lặp đóng ........................................................ 73

Hình 4. 3: Sơ đồ tích hợp GPS/INS tập trung vòng mở ............................................... 73

Hình 4. 4: Sơ đồ tích hợp GPS/INS tập trung vòng đóng ............................................ 74

Hình 4. 5: Sơ đồ tích hợp GPS/INS phân tán vòng mở ............................................... 74

Hình 4. 6: Sơ đồ tích hợp GPS/INS phân tán vòng đóng ............................................ 74

Hình 4. 7: Mô hình bộ lọc Kalman lọc sai số .............................................................. 75

Hình 4. 8: Mô hình trạng thái trong miền z .................................................................. 76

Hình 4. 9: Khoảng thời gian đo lƣờng IMU và GPS ................................................... 78

Hình 4. 10: Cấu trúc bộ bù dữ liệu INS trên cơ sở áp dụng nguyên lý mờ ................. 80

Hình 4. 11: Khái quát đối tƣợng và thiết bị sử dụng .................................................... 83

Hình 4. 12: Các thiết bị hợp thành cơ bản ................................................................... 83

Hình 4. 13: Xử lý dữ liệu trong hệ thống ..................................................................... 85

Hình 4. 14: Thiết bị thực tế và lập trình cho thiết bị .................................................... 86

Hình 4. 15: Vị trí gắn Radar và triển khai lắp ráp thiết bị trên xe bus ......................... 86

Hình 4. 16: Hệ tọa độ vật thể P và hệ tọa độ quy chiếu G (NWU) ............................. 87

Hình 4. 17: Lƣu đồ chƣơng trình trên hệ thống nhúng Arduino .................................. 92

Hình 4. 18: Lƣu đồ thuật toán lọc UKF ....................................................................... 97

Hình 4. 19: Hành trình thực tế của xe bus ................................................................. 100

Hình 4. 20: Phóng to một đoạn hành trình thể hiện quỹ đạo GPS và UKF ............... 100

Hình 4. 21: Một đoạn quỹ đạo xe bus thể hiện quá trình lọc và hiệu chỉnh .............. 101

Hình 4. 22: Hiệu chỉnh quỹ đạo chính xác trong trƣờng hợp GPS có nhiễu lớn ....... 102

Hình 4. 23: Đồ thị vận tốc thực và hiệu chỉnh vận tốc của xe suốt dọc tuyến ........... 103

Hình 4. 24: Hành trình của xe và điều chỉnh quỹ đạo tƣơng ứng tại các thời điểm tính

toán ............................................................................................................................. 104

Hình 4. 25: Một số thông số chuyển động của xe bus (phần I) ................................. 105

Hình 4. 26: Một số thông số chuyển động của xe bus (phần II) ................................ 106

DANH MỤC CÁC KÝ HIỆU, CHỮ VIẾT TẮT

1. Các Ký hiệu

sai số hoặc giá trị hiệu chỉnh của một biến

hàm Dirac delta

véc tơ tốc độ góc

góc cuốn (roll), còn gọi là góc cren hay góc nghiêng

góc trúc (pitch)

góc hƣớng (yaw) hay góc đảo lái

véc tơ gia tốc biểu kiến

véc tơ trọng lực

véc tơ quaternion

véc tơ trực giao (Orthogonal)

véc tơ tái cấu trúc (Normalized)

ma trận hiệp phƣơng sai của véc tơ sai số hệ thống

ma trận mật độ phổ

véc tơ vị trí

véc tơ sai số hệ thống liên tục về mặt thời gian

véc tơ vận tốc

véc tơ số gia vận tốc

hàm trọng lƣợng

véc tơ nhiễu hệ thống

Véc tơ trạng thái thiết bị

Véc tơ đo lƣờng (véc tơ số đo)

Ma trận thiết kế của mô hình

Trục dài của elipsoid tham chiếu

Ma trận thiết kế bộ điều khiển

Độ lệch bias

Véc tơ sai số phép đo

Tích chéo hoặc dạng ma trận phản đối xứng

Ma trận động học (ma trận dự báo)

Ma trận thiết kế (ma trận khuếch đại)

gx, gy, gz

Gia tốc trọng trƣờng

Gia tốc theo trục x, y, z trong hệ tọa độ vật thể (b-frame)

Chiều cao của elipsoid

Ma trận thiết kế của phép đo

Ma trận đơn vị

Hệ số khuếch đại của bộ lọc Kalman

Bán kính cong của mặt phẳng cắt tạo bởi trục z và trục phƣơng Bắc

Bán kính cong của mặt phẳng cắt tạo bởi trục z và trục phƣơng Đông

Ma trận hỗ tƣơng quan của véc tơ trạng thái

Ma trận quay của véc tơ hoặc hệ tọa độ

Hệ số tỷ lệ s

α_Alpha Góc dốc theo hƣớng di chuyển của xe

Góc trƣợt giữa hƣớng thực và góc hƣớng

Góc lệch giữa Heading và Yaw (còn gọi là góc Sideslip)

xii

2. Các từ viết tắt

Acc Gia tốc kế (Accelerator)

AGD Hệ tọa độ địa lý Úc (Australian Geodetic Datum)

CAN Hệ thống bus Controller Area Network

CCS Hệ trục tọa độ Cartesian Coordinate System

DCM Ma trận cosin chỉ phƣơng (Direction Cosine Matrix)

DOF Bậc tự do, hay còn gọi là trục di chuyển của INS (Degrees Of

Freedom)

ECEF Hệ tọa độ tâm trái đất ECEF (Earth – Centered Earth-Fixed, e-frame)

ECI Hệ tọa độ quán tính ECI (Earth – Centered Inertial, i-frame)

EKF Bộ lọc Kalman mở rộng (Extended Kalman Filter)

ED Hệ tọa độ địa lý châu Âu (European Datum)

ENU Hệ tọa độ dẫn đƣờng Đông – Bắc – Xa tâm (Local East – North – Up

coordinates)

GDA Hệ tọa độ địa lý Úc (Geodetic Datum of Australian)

GIS Hệ thống thông tin địa lý (Geographic Information System)

GMT Giờ trung bình quốc tế (Greenwich Mean Time)

GNSS Hệ thống vệ tinh dẫn đƣờng toàn cầu (Global Navigation Satellite

Systems)

GPS Hệ thống định vị toàn cầu (Global Positioning System)

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

GRS Hệ quy chiếu GRS (Geodetic Reference System)

Gyro Con quay hồi chuyển (Gyroscope)

IMU Khối đo lƣờng quán tính (Inertial Measurement Unit)

xiii

INS Hệ dẫn đƣờng quán tính (Inertial Navigation System)

KF Bộ lọc Kalman (Kalman Filter)

LSE Phƣơng pháp bình phƣơng tối thiểu (Least Squares Errors)

MEMS Hệ vi cơ điện tử (MicroElectroMechanical Systems)

N/A Không có sẵn hoặc không đƣợc nối chân linh kiện

NAD Hệ tọa độ địa lý bắc Mĩ (the North American Datum)

NED Hệ tọa độ dẫn đƣờng Bắc – Đông – Hƣớng tâm (Local North – East –

Down coordinates)

NWU Hệ trục toạ độ North – West – Up hay còn gọi là North – West –

Zenith coordinate

OSGB Hệ tọa độ địa lý Anh (the Ordnance Survey of Great Britain)

PVA Vị trí, vận tốc và góc định hƣớng (Position, Velocity, Attitude)

RTK Động học thời gian thực (Real Time Kinematic)

R Ký hiệu ma trận quay (Rotation matrix)

SPKF Bộ lọc Kalman dạng SPKF (Sigma-Piont Kalman Filter)

UAV Thiết bị bay tự động không ngƣời lái (Unmanned Aerial Vehicle)

UKF Bộ lọc Kalman dạng UKF (Unscented Kalman filter)

UT Chuyển đổi Unsented Transform cho bộ lọc UKF

UTC Giờ phối hợp quốc tế (Universal Time Coordinate)

WGS Hệ trục tọa độ địa lý (The World Geodetic System)

xiv

MỞ ĐẦU

1. Giới thiệu tóm tắt luận án

Luận án đi sâu nghiên cứu giải quyết vấn đề tồn tại trong suốt thời gian qua là

vấn đề trôi dữ liệu của INS, từ đó đƣa ra giải pháp thiết kế hệ thống tích hợp GPS/INS

trên cơ sở cấu trúc phân tán nhằm nâng cao chất lƣợng cho các thiết bị định vị dẫn

đƣờng sử dụng GPS phục vụ bài toán giám sát phƣơng tiện giao thông đƣờng bộ.

Bằng cách phát triển phƣơng pháp tự động hiệu chỉnh ma trận quay cho thiết bị trên

xe và bộ lọc UKF cho thiết bị tại trạm, giải pháp phân tán của luận án đã giải quyết

đƣợc vấn đề sai số tích lũy của INS 9-DOF, tính phi tuyến của hệ thống và xử lý thời

gian thực. Các sản phẩm của luận án đã đƣợc ứng dụng trên thực tế thành công.

Nội dụng của luận án đƣợc chia thành phần tổng quan và 4 chƣơng chính, bao

gồm: 109 trang (không kể mở đầu, tài liệu tham khảo và phụ lục), 93 tài liệu tham

khảo, 9 bảng, 52 hình vẽ và đồ thị.

2. Đặt bài toán

Mục tiêu của bài toán giám sát phƣơng tiện giao thông vận tải là phát triển hiệu

quả việc quản lý, điều hành các phƣơng tiện này và kiểm tra các trạng thái của đối

tƣợng lúc đang hoạt động bình thƣờng cũng nhƣ khi có sự cố. Chất lƣợng của hệ

thống giám sát không những thể hiện ở tính chính xác về vị trí, vận tốc và thời gian,

mà còn thể hiện ở việc giám sát đƣợc các thông số chuyển động của đối tƣợng, đặc

biệt là các thông số thể hiện sự dao động của đối tƣợng và một số thông số đặc trƣng

của đối tƣợng đó. Các thông số đặc trƣng này phụ thuộc vào đối tƣợng cần giám sát,

thông thƣờng là gia tốc, các góc nghiêng thể hiện trạng thái chuyển động của đối

tƣợng, góc hƣớng và một số thông số về tình trạng của đối tƣợng.

Hiện nay, ngoài các phƣơng pháp xác định vị trí nhờ đặc tính chuyển động

tƣơng đối so với điểm mốc đã biết nhƣ đèn hải đăng hay các ngôi sao trên bầu trời,

hoặc sử dụng radar hay sóng radio/vô tuyến dựa trên những nguồn phát sóng có vị trí

xác định trƣớc, ngƣời ta dùng hai nhóm phƣơng pháp cơ bản để định vị giám sát:

 Phƣơng pháp xác định vị trí thông qua việc xác định tọa độ của đối tƣợng trên cơ

sở các hệ tọa độ định vị vệ tinh nhƣ GPS, GLONASS, GALILEO, COMPASS,

IRNSS, QZSS. Luận án này sử dụng thông tin từ hệ thống GPS.

 Phƣơng pháp dẫn đƣờng quán tính (INS) dựa trên trạng thái ban đầu đã biết, ta đo

gia tốc và góc quay để tính toán xác định vị trí tiếp theo. Đặc điểm nổi bật của

phƣơng pháp này là cho ta biết trạng thái chuyển động của đối tƣợng thông qua

các thông số về gia tốc, góc quay, độ nghiêng và hƣớng.

Một hệ thống giám sát gồm hai thành phần cơ bản là thiết bị thu dữ liệu gắn

trên đối tƣợng chuyển động và thiết bị giám sát tại trạm. Thiết bị gắn trên đối tƣợng

chuyển động thƣờng sử dụng module thu tín hiệu từ hệ thống GPS, qua đó xác định vị

trí và vận tốc của đối tƣợng mang. Hệ thống GPS cung cấp thông tin vị trí và vận tốc

trong khoảng thời gian dài nhƣng tốc độ đƣa ra kết quả chậm. Hệ thống giám sát sẽ

mất đi tính năng giám sát khi mất tín hiệu GPS. Hỗ trợ cho hệ GPS trong bài toán

giám sát thƣờng sử dụng hệ thống INS kết hợp thêm các hệ thống khác. Hệ INS chỉ

đƣa ra thông tin về vị trí và vận tốc chính xác trong khoảng thời gian ngắn do sai số

tích lũy, bị ảnh hƣởng bởi trọng lực, nhƣng có ƣu điểm là tốc độ đƣa ra kết quả cao

với tính toán đầy đủ theo các phƣơng chuyển động và các góc dao động.

Giải quyết đƣợc vấn đề trôi dữ liệu của hệ thống INS sẽ mang yếu tố quyết

định và mở ra một hƣớng đi mới trong thiết kế, chế tạo thiết bị tích hợp GPS/INS. Từ

đó, kết hợp cả hai phƣơng pháp GPS và INS ta sẽ đƣợc một hệ thống có ƣu điểm tốt

hơn với khả năng cho ra thông tin về vị trí và vận tốc với độ chính xác trong khoảng

thời gian dài, đồng thời có đƣợc thông tin về trạng thái chuyển động của đối tƣợng.

3. Tính cấp thiết và lý do chọn đề tài

Hiện nay ngoài các ứng dụng trong quân sự, hệ thống định vị toàn cầu GPS

đƣợc sử dụng trong nhiều ngành, nhiều lĩnh vực hoạt động phục vụ con ngƣời. Chỉ nói

riêng trong ngành giao thông đã có nhiều ứng dụng quan trọng của GPS nhƣ: Tự động

định vị phƣơng tiện giao thông AVL (Automatic Vehicle Location); Tự động dẫn

đƣờng các phƣơng tiện giao thông AGV (Automatic Guided Vehicle). Tại Việt nam

việc nghiên cứu ứng dụng kỹ thuật GPS đã đƣợc triển khai và có những thành công

nhất định trong lĩnh vực địa chính, bản đồ, quản lý giám sát phƣơng tiện giao thông.

xvi

Trong xu hƣớng hiện đại hoá ngành giao thông vận tải có thể nói việc đƣa vào sử

dụng GPS là điều tất yếu do những ƣu điểm nổi bật về tính hiệu quả và giá thành ngày

càng giảm. Yêu cầu đặt ra là ngoài việc xác định chính xác vị trí và tốc độ chính xác

theo thời gian, thiết bị cần giám sát liên tục các thông số về trạng thái chuyển động

của đối tƣợng nhƣ khoảng di chuyển, góc hƣớng, gia tốc, độ nghiêng, độ cao. Đây là

những yếu tố mà các thiết bị trong hệ thống giám sát hiện tại chƣa đƣợc tích hợp.

Các thiết bị giám sát hành trình tại Việt Nam cho phƣơng tiện giao thông

đƣờng bộ hiện tại chỉ sử dụng hệ thống GPS. Tính đến thời điển này các thiết bị

MEMS INS 9-DOF hiện đại có giá thành rất rẻ, cỡ 30 euro/1 thiết bị, có chất lƣợng

tốt. Do đó việc tích hợp INS trong hệ thống giám sát hành trình sử dụng GPS là việc

nên làm và sẽ dần trở thành chuẩn thiết bị. Ngoài tính năng hỗ trợ cho hệ thống GPS,

hệ thống INS còn cung cấp thêm các thông số đặc trƣng cho chuyển động, qua đó

nâng cao chất lƣợng hệ thống giám sát.

4. Mục đích, đối tƣợng, phạm vi nghiên cứu và kết quả mong đợi của luận án

Luận án nghiên cứu ứng dụng các công nghệ mới để thiết kế chế tạo thiết bị

nhằm nâng cao chất lƣợng cho các thiết bị định vị dẫn đƣờng sử dụng GPS phục vụ

bài toán giám sát quản lý phƣơng tiện giao thông đƣờng bộ. Yêu cầu của hệ thống cần

giám sát chính xác vị trí, vận tốc và các tham số thể hiện trạng thái chuyển động của

đối tƣợng. Đáp ứng nhu cầu này, luận án đặt ra nhiệm vụ nghiên cứu các cơ sở khoa

học của GPS và các thiết bị hỗ trợ, trong đó điển hình là hệ thống INS, và đề xuất các

giải pháp tích hợp trên cơ sở cập nhật công nghệ mới nhằm nâng cao chất lƣợng hệ

thống giám sát ứng dụng trong quản lý các phƣơng tiện giao thông đƣờng bộ, cụ thể là

ứng dụng cho xe bus.

Để phục vụ mục đích này, tác giả đã từng bƣớc nghiên cứu cập nhật công nghệ,

các giải pháp tích hợp hệ thống dựa trên công nghệ GPS và công nghệ vi cơ điện tử

MEMS INS, từ đó đề xuất giải pháp tích hợp và xử lý dữ liệu phù hợp. Với mục tiêu

đƣa ra ra giải pháp thiết kế thiết bị tích hợp hai hệ thống GPS và INS ứng dụng cho xe

bus, bao gồm nhiệm vụ thu thập, xử lý dữ liệu gắn trên xe với hệ thống MEMS INS

tiên tiến và xây dựng thuật toán lọc trên cơ sở lọc Kalman để nâng cao độ chính xác

về vận tốc và quỹ đạo chuyển động của xe. Ngoài ra kết quả còn yêu cầu đƣa ra giá trị

xvii

một số thông số cần giám sát trong suốt quá trình hoạt động về trạng thái dao động,

góc hƣớng, gia tốc, góc nghiêng và độ cao.

Trong trƣờng hợp mất tín hiệu GPS kéo dài hoặc thiết bị chạy trong môi trƣờng

đóng kín nhƣ trong nhà, trong đƣờng hầm hay những khu vực bị che khuất trong một

khoảng thời gian dài, hệ thống tích hợp GPS/INS lúc này chỉ làm việc với tín hiệu INS

và sẽ mất đi khả năng định vị. Luận án nghiên cứu về công nghệ và các thiết bị sử

dụng GPS ứng dụng cho phƣơng tiện giao thông đƣờng bộ, chƣa đề cập tới trƣờng

hợp thiết bị chạy trong môi trƣờng đóng kín hay mất GPS kéo dài.

Kết quả mong đợi của luận án là sẽ kiểm soát đƣợc các sai số tích lũy của INS,

từ đó đƣa ra giải pháp thiết kế thiết bị tích hợp GPS/INS nhằm loại bỏ những sai lệch

quỹ đạo của phƣơng tiện giao thông đƣờng bộ trên cơ sở sử dụng bộ lọc Kalman.

Thiết bị sẽ đƣợc ứng dụng cho hệ thống giám sát xe bus.

4. Bố cục của luận án

Luận án bao gồm phần tổng quan, kết luận, phụ lục và 4 chƣơng chính.

- Tổng quan về bài toán giám sát: Trong phần này luận án đã khái quát đặc

điểm của công nghệ định vị dẫn đƣờng sử dụng GPS và INS, đánh giá các công trình

nghiên cứu liên quan mật thiết đến đề tài luận án đã đƣợc công bố trong và ngoài

nƣớc, tình hình nghiên cứu trên thế giới, tình hình nghiên cứu tại Việt Nam. Tiếp đó

luận án phân tích những hạn chế của những hệ thống hiện tại, những vấn đề còn tồn

tại và định hƣớng giải quyết sử dụng hệ thống tích hợp GPS/INS kết hợp với thuật

toán lọc và hiệu chỉnh dữ liệu.

- Chƣơng 1: Trình bày khái quát về hệ thống GPS và hệ thống INS, bản chất bù

giữa INS – GPS và khái quát về các phƣơng pháp tích hợp GPS/INS. Sau khi khái

quát bài toán áp dụng cho hệ thống điều khiển giám sát quản lý thông vận tải ứng

dụng cho các phƣơng tiện mặt đất, luận án chỉ ra mô hình đƣợc lựa chọn trong luận án

là xe bus với một số thông số yêu cầu cần đƣợc giám sát.

- Chƣơng 2: Trình bày về lọc Kalman và các khái niệm mở rộng, bao gồm lọc

Kalman mở rộng EKF và lọc Kalman dạng UKF. Sau khi khái quát lý thuyết, luận án

mô phỏng các thuật toán lọc dựa trên hệ thống thƣ viện Matlab tool-box [79]. Những

xviii

kết quả này đƣợc tác giả tiếp tục phát triển với thuật toán hạn biên và tính toán trên

miền rời rạc z-Domain, là đóng góp mới ban đầu của luận án [63, P1.1.4]. Một số

đoạn mã (script) trong thƣ viện này đƣợc phát triển ứng dụng trong phát triển bộ lọc

UKF cho xe bus ở chƣơng 4.

- Chƣơng 3: Giới thiệu và phát triển hoàn thiện giải pháp tự động hiệu chỉnh

ma trận quay R (Rotation matrix), hay còn gọi là cơ chế lọc bổ sung DCM. Sự ra đời

của dòng cảm biến MEMS INS Razor 9-DOF (2012) đã tạo ra một bƣớc tiến đột phá

trong lĩnh vực thiết kế hệ thống tích hợp GPS/INS. Với thiết bị tiên tiến này, luận án

đã phát triển và ứng dụng thành công phƣơng pháp xử lý chống trôi các phần tử của

ma trận quay dựa trên đặc tính trực giao của các vector trên cơ sở cập nhập vecor từ

trƣờng tích hợp. Khác với lọc Kalman xử lý dữ liệu ở bƣớc sau, tức là lấy kết quả đã

bị tích lũy sai số của INS để tính toán, đây là một giải pháp mới trong thiết kế hệ

thống sử dụng MEMS INS bởi khả năng giải quyết vấn đề trôi dữ liệu ở cấp độ đo

lƣờng, hiệu chỉnh dữ liệu thô để tính toán lại các góc quay roll, pitch, yaw của MEMS

INS, từ đó tính toán chính xác hơn giá trị vận tốc, khoảng di chuyển của đối tƣợng rồi

mới đƣa về trạm để tính toán với bộ lọc.

- Chƣơng 4: Phần đầu của chƣơng này khái quát các giải pháp tính tích hợp

GPS/INS trƣớc đây và ứng dụng phƣơng pháp tính toán trên miền rời rạc cho các hệ

thống tích hợp GPS/INS kết hợp bù dữ liệu trên cơ sở nguyên lý mờ [P1.1.12].

Cập nhật công nghệ INS mới, luận án giới thiệu một giải pháp mới trong thiết

kế hệ thống tích hợp GPS/INS trên cơ sở hệ thống phân tán. Xử lý dữ liệu cho hệ

thống tích hợp đƣợc chia thành nhiệm vụ cho thiết bị trên xe (xử lý chống trôi dữ liệu)

với phƣơng pháp hiệu chỉnh các phần tử của ma trận quay R, và nhiệm vụ tại trạm (xử

lý lọc Kalman). Phần phát triển ứng dụng trên hệ thống thực tế dựa trên cơ cở lý

thuyết chƣơng 3, phát triển trên nền tảng hệ thống nhúng Arduino cho thiết bị gắn trên

đối tƣợng chuyển động là xe bus. Để hoàn thiện thuật toán lọc UKF, hệ thống cần

đƣợc cung cấp thêm các thông số khác làm giá trị tham chiếu. Giải pháp trong luận án

này là sử dụng các thông số tham chiếu đƣợc lấy từ bus CAN (chuẩn J1939 FMS) của

xe kết hợp với các thông số từ radar. Với các tham số bổ sung này, luận án đã phát

xix

triển thành công thuật toán lọc UKF linh hoạt với hai chế độ hoạt động tùy thuộc tín

hiệu đầu vào, ứng dụng trong thực tế cho đối tƣợng xe bus [P1.1.14].

- Kết luận và kiến nghị.

5. Ý nghĩa khoa học và thực tiễn của luận án

 Ý nghĩa khoa học: Đóng góp giải pháp mới trong tích hợp GPS/INS nhằm cải

thiện chất lƣợng của các thiết bị giám sát phƣơng tiện. Giải pháp không chỉ dừng ở lý

thuyết mà còn đƣa ra đƣợc sản phẩm cụ thể để thử nghiệm, kiểm chứng [P1.1.4,

P1.1.12, P1.1.14].

 Ý nghĩa thực tiễn: Giải pháp và sản phẩm của luận án có thể ứng dụng trong

các hệ thống quản lý, giám sát đối tƣợng giao thông đƣờng bộ [P1.1.14, P1.2.6].

6. Tính sáng tạo và kết quả nghiên cứu

 Đề xuất phƣơng pháp tính toán trên miền rời rạc z-Domain kết hợp bù dữ liệu

để hiệu chỉnh quỹ đạo trên cơ sở logic mờ cho các hệ tích hợp sử dụng INS 6-DOF.

Các hệ thống hiện đại sử dụng INS 9-DOF sẽ đƣợc áp dụng phƣơng pháp tự động hiệu

chỉnh các phần tử ma trận quay.

 Đƣa ra giải pháp mới tích hợp hệ thống GPS/INS dựa trên nguyên tắc điều

khiển phân tán, chia công việc xử lý chống trôi dữ liệu cho INS tại thiết bị trên xe và

đƣa bộ lọc UKF về xử lý tại trạm. Hệ thống đã đƣợc áp dụng thực tế cho kết quả giám

sát tốt, quỹ đạo đã loại bỏ đƣợc các nhiễu cho dạng giống quỹ đạo thực. Ngoài ra, hệ

thống giám sát còn kiểm soát đƣợc các thông số về tình trạng chuyển động của xe.

7. Những đóng góp của luận án

 Phát triển và ứng dụng thành công phƣơng pháp xử lý chống trôi các phần tử

của ma trận quay cho MEMS INS 9-DOF.

 Xây dựng thành công bộ lọc UKF với hai chế độ hoạt động riêng biệt ở các tần

số lấy mẫu khác nhau, tự động chuyển đổi theo đặc điểm của tín hiệu đầu vào.

TỔNG QUAN VỀ BÀI TOÁN GIÁM SÁT

1. Khái quát về bài toán giám sát sử dụng GPS

Một hệ thống giám sát gồm hai thành phần cơ bản là thiết bị gắn trên đối tƣợng

chuyển động, còn gọi là thiết bị trên xe hay thiết bị xe (Vehicle devices) và thiết bị

giám sát tại trạm (Station devices). Thiết bị xe đƣợc lắp đặt module thu GPS hoặc

thiết bị tích hợp GPS/INS để thu thập và xử lý dữ liệu đo đạc. Dữ liệu sau bƣớc tiền

xử lý đƣợc đƣa về thiết bị trạm tính toán hiệu chỉnh để hiển thị bản đồ số phục vụ

công tác quản lý giám sát.

Vấn đề khó khăn khi sử dụng các hệ thống INS là các số liệu tính toán của INS

bị trôi theo thời gian do sai số tích lũy. Giải quyết đƣợc vấn đề sai số của hệ thống

INS sẽ mang lại một hƣớng đi mới cho lớp bài toán này. Hệ thống INS thông thƣờng

chỉ đƣa ra thông tin về vị trí và vận tốc chính xác trong khoảng thời gian ngắn và bị

ảnh hƣởng bởi trọng lực. Do sự trôi tín hiệu đầu ra của bản thân IMU, cộng thêm sự

rung động của thiết bị mang, đáp ứng của hệ thống INS sẽ có sai số lớn và tăng dần

theo thời gian. Tính chất cộng dồn sai số tích lũy làm cho hầu nhƣ các hệ INS đơn trục

trƣớc đây không làm việc độc lập đƣợc. Các hệ INS giá rẻ thƣờng đƣợc áp dụng cho

các thiết bị tốc độ cao và dao động lớn, cho kết quả có thể tin cậy đƣợc trong khoảng

thời gian ngắn. Để hoạt động đƣợc trong thời gian dài, các hệ thống INS cần đƣợc kết

hợp với hệ thống khác để chỉnh định lại kết quả đo đạc.

Kết hợp hai hệ thống GPS và INS thành hệ thống tích hợp để hai hệ thống hỗ

trợ nhau cùng hoạt động là một giải pháp khả thi và nên làm. Hệ thống tích hợp sẽ có

chất lƣợng cao hơn với khả năng cho ra thông tin về vị trí và vận tốc chính xác hơn

trong khoảng thời gian dài, đồng thời có đƣợc các thông tin về trạng thái chuyển động

của đối tƣợng.

Các hệ thống tích hợp GPS/INS thƣờng sử dụng bộ lọc Kalman để nâng cao

chất lƣợng. Đây là vấn đề đƣợc quan tâm và phát triển mạnh trong khoảng hơn 10

năm trở lại đây. Tuy nhiên, những định hƣớng nghiên cứu trong lĩnh vực này luôn vận

động, thay đổi, phát triển phụ thuộc vào công nghệ chế tạo cảm biến tích hợp nên hệ

thống INS, công nghệ chế tạo chip vi xử lý, chip đồng xử lý, các sản phẩm nhúng và

các phƣơng pháp xử lý dữ liệu. Khi hệ thống INS đa trục ra đời, các nghiên cứu cho

các hệ thống đơn trục trƣớc đây trở nên phần nào lạc hậu, các công thức tính toán

cũng dần đƣợc loại bỏ. Theo đánh giá của các chuyên gia, khi các sản phẩm nhúng

tiêu chuẩn mã nguồn mở ra đời cùng với hệ thống thƣ viện phong phú của nó, việc tự

thiết kế chế tạo các sản phầm đơn lẻ từ các chip rời sẽ dần dần không còn đƣợc chú

trọng phát triển nhiều nữa. Những thiết kế này sẽ dần chuyển sang hƣớng vào các các

ứng dụng mang tính chuyên dụng. Vì vậy, cách tổ chức công việc cho các phần trong

hệ thống và sử dụng công cụ gì để xây dựng nên hệ thống trở thành vấn đề vô cùng

quan trọng, quyết định phần lớn sự thành công hay thất bại của sản phẩm. Vấn đề này

đƣợc phân tích thông qua các công trình khoa học trong và ngoài nƣớc liên quan mật

thiết tới đề tài trong phần tiếp theo.

2. Phân tích đánh giá các công trình khoa học liên quan mật thiết đến đề tài

nghiên cứu trên thế giới

Trong thời gian trƣớc đây, để đƣa hệ thống tích hợp GPS/INS vào thực tế, các

tác giả không những gặp phải những vấn đề lý thuyết với các công thức tính toán phức

tạp dẫn đến việc lập trình rất khó đáp ứng đƣợc thời gian thực, mà còn bị hạn chế bởi

công nghệ với các cảm biến rời, tín hiệu đƣa ra là tín hiệu tƣơng tự [44, 47]. Một số

dạng IMU thƣờng sử dụng nhƣ IMU của Crossbow, Honeywell, Analog Device,

Watson với một số cảm biến tiêu biểu nhƣ Analog Device ADIS16201, Watson DMS-

EGP02, Honeywell HG1700/HG1900. Rất nhiều các công trình nghiên cứu thiết kế

chế tạo thiết bị [83], cũng nhƣ luận án tiến sĩ [84] nghiên cứu về lĩnh vực này và đã có

những thành công đáng kể. Các đối tƣợng ứng dụng thƣờng là ô tô [19, 20, 55, 56, 57,

65], robot [21, 22, 23], xe tự hành (Autonomous Mobile Vehicle) [29, 32, 37, 38, 58],

hay đối tƣợng bay [50, 57, 60, 88]. Các mô hình tính toán dựa trên cơ sở tích hợp đa

cảm biến, tiêu biểu có thể kể ra nhƣ: Stefano Panzieri (2002) [61], Basil (2004) [9],

Goodall (2004) [11], Eun-Hwan Shin (2005) [19], Umar Iqbal (2008) [65], Dah-Jing

Jwo (2008) [13, 14], Naser El-Sheimy (2008) [46]; Mark Petovello (2008) [39, 40,

41]; Tomoji Takasu (2008) [64], Wei Chen (2009) [70], Nguyễn Quang Tuấn (2009)

[48], Wei Chen (2009) [70], Debo Sun (2010) [16], Christopher (2010) [12].

Các phƣơng pháp tính toán này chủ yếu sử dụng cảm biến tích hợp thành đa

trục nên phải tính toán chuyển đổi để lấy vị trí trọng tâm hệ cảm biến, áp dụng cho các

mô hình liên tục với các công thức tính toán rất phức tạp và hầu hết chỉ triển khai trên

các công cụ mô phỏng. Trong các thiết kế cụ thể thƣờng sử dụng thêm các tín hiệu hỗ

trợ nhƣ camera, la bàn, sóng radio hay sóng truyền hình số… để hỗ trợ cho bộ lọc

Kalman trên vi xử lý gắn trên đối tƣợng chuyển động. Tại thời điểm đó việc khảo sát

đánh giá sai số của INS rất đƣợc coi trọng [51, 52], thậm chí việc hoàn thành đƣợc

việc đánh giá sai số của một INS để ứng dụng cho một đối tƣợng cụ thể đã là một luận

án tiến sĩ [42, 45].

Một hƣớng phát triển dạng lọc Kalman thích nghi (Adaptive Extended Kalman

Filter, AEKF), đƣợc phát triển [26] với các ứng dụng cho mô hình Tightly- coupled

Integrated GPS/INS [60] hay Loosely-Coupled Integrated GPS/INS [71]. Một số thiết

kế thực tế sử dụng các công cụ nhúng mạnh nhƣ Rapid STM32 Blockset [36], có khả

năng tự cấu trúc hệ thống nhƣ các hệ thống FPGA, SOC [8]. Mộ số phƣơng pháp xử

lý phức tạp đƣợc giới thiệu nhƣ sử dụng nguyên lý mờ (fuzzy) [27, 62, 66, 67, 68, 69,

72], hay cấu trúc bền vững (robust) [28], tự trị (autonomous) [74], lai (hybrid) [74]

hay đa mô hình (multi-model) [72]. Tuy nhiên thiết bị thực tế trong các thiết kế này

chƣa đáp ứng đƣợc độ phức tạp của mô hình, bỏ qua những tính toán phức tạp. Một số

hệ thống phát triển các bộ làm trơn (Smoother) [31, 33, 34], hoặc giải pháp Seamless

[35] để thể hiện quỹ đạo một cách liên tục hơn. Một số thiết kế đã đƣợc triển khai trên

thiết bị trong thực tế, tuy nhiên khi triển khai nhiều tác giả chỉ sử dụng mô hình tiêu

chuẩn, loại bỏ các thành phần phi tuyến tính toán phức tạp trong mô hình lý thuyết để

đáp ứng vấn đề thời gian thực. Đối với các bộ lọc phát triển cho đối tƣợng thực tế xử

lý nhiều tín hiệu đầu vào thƣờng chỉ sử dụng lọc Kalman. Các mô hình sử dụng EKF

hay UKF chỉ đƣợc áp dụng với số lƣợng ít các tín hiệu thực.

Tháng 8/2008, hãng Analog Deveice giới thiệu MEMS INS tích hợp 6-DOF, và

đầu năm 2009 chính thức đƣa ra thị trƣờng [7]. Kể từ đây lớp bài toán này đƣợc mở ra

một hƣớng xử lý mới với công cụ vi xử lý mạnh hơn, cảm biến đa trục và tính toán

trên miền rời rạc. Tại thời điểm này, một số ý tƣởng về tự chỉnh MEMS INS bắt đầu

xuất hiện nhƣ tích hợp la bàn điện tử trên cơ sở xử lý các vector quaternien của

Nguyễn Hồ Quốc Phƣơng (2009) [91], sử dụng góc hƣớng từ GPS của William

Premerlani và Paul Bizard (2009) [88], Valerie Renaudin (2010) [66], hay của Sergiu

Baluta (2010) trong dự án điều khiển bay của Stalion Electronics [87]. Những nghiên

cứu này bắt đầu tập trung vào việc xử lý trôi dữ liệu của INS, tuy nhiên kết quả còn

khá nhiều hạn chế, chủ yếu là các kết quả khảo sát INS đa trục và các kết quả lý

thuyết hoặc dƣới dạng bản thảo để đƣa ra thảo luận (Draft of theory).

Đầu năm 2011 hãng SparkFun Electronics cho ra đời cảm biến MEMS INS 9-

DOF với độ chính xác cao, và đến tháng 8 năm 2012 hoàn thiện dòng sản phẩm lấy

tên là Razor 9-DOF Sensor [89, 90]. Giải pháp phát triển ứng dụng trên cơ sở hệ

MEMS INS 9-DOF này đã tạo ra thay đổi một cách đáng kể về phƣơng pháp tiếp cận

với hệ INS. Kể từ thời điểm này, các nghiên cứu bắt đầu tập trung vào giải pháp xử lý

chống trôi dữ liệu cho hệ thống INS để nâng cao chất lƣợng cho hệ thống INS. Các

kết quả đầu ra sau khi đƣợc xử lý chống trôi mới tiếp tục đƣợc xử lý hiệu chỉnh và đƣa

vào lọc. Cũng chính điều này làm cho các thiết kế chế tạo sản phẩm sử dụng INS đơn

trục với các tính toán phức tạp trƣớc đây trở nên lạc hậu. Các phƣơng pháp xử lý dữ

liệu và công thức tính toán cho hệ thống tích hợp GPS/INS giờ đã thay đổi. Giải pháp

xử lý dữ liệu cũng nhƣ các công thức tính toán cho hệ thống đơn trục trƣớc đây đã

đƣợc loại bỏ khỏi các hệ thống mới làm cho các công thức trở nên khác biệt.

Kể từ thời điểm này các hạn chế của hệ thống tích hợp GPS/INS do việc trôi dữ

liệu của INS gây ra đã có cơ sở để giải quyết triệt để dựa trên sự ra đời của các cảm

biến tích hợp hiện đại 9-DOF. Luận án này đã cập nhật công nghệ mới để phát triển

thành công giải pháp xử lý chống trôi các phần tử ma trận quay cho MEMS INS 9-

DOF, từ đó tính toán lại các góc quay chính xác. Các kết quả với hệ thống MEMS

INS 6-DOF phần nào trở nên lạc hậu, đƣợc đề cập trong luận án dƣới dạng giải pháp

để phân biệt và làm nổi bật giải pháp mới này.

3. Phân tích đánh giá các công trình khoa học liên quan mật thiết đến đề tài

nghiên cứu tại Việt Nam

Tại Việt nam đã có một số nghiên cứu đề cập tới vấn đề này, điển hình là đề tài

nghiên cứu khoa học cấp nhà nƣớc “Nghiên cứu thiết kế, chế tạo thiết bị định vị vệ

tinh phục vụ giám sát, quản lý phƣơng tiện giao thông đƣờng bộ, đƣờng sắt” mã số

KC.06.02/06-10 [2]. Trong đề tài này, tác giả tham gia và chủ trì nhánh số 6 “Nghiên

cứu thiết kế, chế tạo thiết bị định vị vệ tinh phục vụ giám sát, quản lý phƣơng tiện

giao thông đƣờng bộ, đƣờng sắt”. Nhiệm vụ này yêu cầu thiết kế chế tạo thiết bị tích

hợp GPS/INS sử dụng MEMS INS 6-DOF tích hợp với tín hiệu GPS, áp dụng trong

giám sát phƣơng tiện đƣờng sắt. Thiết bị đo kiểm tra dao động của tàu, qua đó đánh

giá về tình trạng nền đƣờng và các đoạn đƣờng sắt cần duy tu bảo dƣỡng. Một trong

những sản phẩm trong giai đoạn đầu của luận án là kết quả của việc tiếp tục phát triển

giải pháp này với phƣơng pháp tính toán trên miền rời rạc kết hợp với bù dữ liệu trên

cơ sở nguyên lý mờ. Kết quả này đã đƣợc báo cáo tại hội thảo toàn quốc về Cơ – Điện

tử VCM 2012 [P1.1.12].

Ngoài ra có một số luận án tiến sĩ trong nƣớc nghiên cứu về lĩnh vực này, điển

hình nhƣ: “Về một phƣơng pháp nhận dạng chuyển động phƣơng tiện cơ giới quân sự

sử dụng đa cảm biến” [1]. Luận án này đã bƣớc đầu giải quyết việc nâng cao độ chính

xác của quỹ đạo chuyển động của phƣơng tiện cơ giới quân sự đƣờng bộ sử dụng

MEMS INS 6-DOF, đƣa ra giải pháp xử lý dữ liệu của hệ thống tích hợp GPS/INS

bằng cách sử dụng logic mờ nhận dạng đối tƣợng đứng yên hay chuyển động, từ đó

đƣa ra cách tính toán phù hợp. Một số nghiên cứu khác nhƣ các công trình của Trƣơng

Duy Trung cho các thiết bị trên biển [6] hay Nguyễn Sỹ Long cho thiết bị bay hành

trình đối hải [5], luận án tiến sĩ “Nâng cao chất lƣợng hệ dẫn đƣờng thiết bị bay trên

cơ sở áp dụng phƣơng pháp xử lý thông tin kết hợp” sử dụng lọc Kalman xử lý dữ liệu

áp dụng cho thiết bị bay [3]. Vấn đề ở đây là một số thiết kế cố gắng diễn giải mô hình

đối tƣợng cũng nhƣ các biến đổi dƣới dạng công thức toán học và các ma trận cụ thể

để thực hiện tất cả trên chip vi xử lý. Điều này dẫn đến việc đƣa ra các công thức quá

phức tạp mà khi phát triển hệ thống ngoài thực tế lại bị bỏ qua. Thực chất, một số vấn

đề chỉ mang tính chất lý thuyết và nhanh chóng lỗi thời khi công nghệ thay đổi. Đặc

biệt khi thiết bị 9-DOF ra đời, những tính toán chuyển đổi trục tọa độ về trùng một

tâm cũng nhƣ các thuật toán mờ phức tạp, hay sử dụng mạng neuron áp dụng để cải

thiện tính chính xác cho giải pháp này đều đƣợc loại bỏ trong thiết kế thiết bị để đáp

ứng đƣợc yếu tố thời gian thực.

Ngoài các công trình đã đề cập ở trên, tác giả còn chủ trì đề tài dự án giáo dục

đại học giai đoạn 2 với nhiệm vụ “Nghiên cứu thiết kế thiết bị định vị kết hợp hệ

thống GPS và hệ thống đo quán tính (INS) trên công nghệ vi cơ điện tử (MEMS)”, mã

số: EEC 8.13 [P1.2.6]. Các kết quả nghiên cứu này là tiền đề cho những thiết kế thiết

bị trên xe sử dụng INS 6-DOF với giải pháp tính toán trên miền rời rạc và bù dữ liệu

trên cơ sở áp dụng logic mờ, đƣa ra phƣơng pháp bù dữ liệu linh hoạt và đáp ứng

đƣợc khả năng tính toán thời gian thực. Giải pháp này đƣợc đánh giá là một sang tạo

có ý nghĩa thực tiễn trong thiết kế, chế tạo thiết bị. Đề xuất này giải quyết vấn đề sai

lệch quỹ đạo trong hệ thống tích hợp GPS/INS có cấu trúc truyền thống, qua đó nâng

cao chất lƣợng giám sát thiết bị chuyển động và làm cơ sở so sánh với giải pháp tiếp

theo của luận án.

4. Đề xuất các giải pháp mới, nội dung và phƣơng pháp nghiên cứu

Vấn đề còn tồn tại trong các hệ thống tích hợp GPS/INS là sự trôi dữ liệu của

INS do tính chất cộng dồn sai số và độ phức tạp của hệ thống dẫn đến các bộ lọc để

hiệu chỉnh dữ liệu trên cơ sở lọc Kalman không đáp ứng đƣợc yếu tố thời gian thực.

Cập nhật sự phát triển của công nghệ MEMS INS khi xuất hiện MEMS INS 9-

DOF, luận án đề xuất giải pháp mới là xử lý dữ liệu kiểu phân tán. Trong giải pháp

này các công việc đƣợc chia thành phần xử lý dữ liệu trên xe và phần xử lý dữ liệu tại

trạm giám sát một cách hợp lý trên nguyên tắc tận dụng ƣu thế của từng phần. Thiết bị

trên xe sử dụng MEMS INS 9-DOF với giải pháp tự chỉnh bằng giải thuật xử lý chống

trôi các phần tử ma trận quay R. Các phần tử của ma trận R đƣợc hiệu chỉnh thông qua

quá trình tái chuẩn hóa tiếp tục đƣợc sử dụng tính các góc quay, từ đó tính toán đƣợc

chính xác hơn vận tốc và khoảng di chuyển của đối tƣợng. Các kết quả này đƣợc cập

nhật theo mỗi chu kỳ tính toán của hệ thống INS.

Giải pháp tự động hiệu chỉnh ma trận quay trên cơ sở cập nhật vector từ trƣờng

của INS 9-DOF là một điểm mới nổi bật của luận án so với các giải pháp tích hợp hệ

GPS/INS trƣớc đây. Khả năng hiệu chỉnh ma các phần tử trận quay R đã giải quyết

vấn đề trôi dữ liệu ở cấp độ đo lƣờng cho MEMS INS, hiệu chỉnh dữ liệu ngay từ số

liệu tính toán góc quay của các IMU. Phƣơng pháp này hiệu quả hơn xây dựng lọc

Kalman trên chip vi xử lý gắn trên xe do bộ lọc Kalman xử lý dữ liệu ở mức sau

(khoảng cách, vận tốc), là dữ liệu sử dụng giá trị góc quay đã bị sai lệch cộng dồn để

tính toán. Trong giải pháp này, ở phần thiết bị trên xe tác giả dành không gian nhớ cho

nhiệm vụ xử lý chống trôi các phần tử của ma trận quay R và đƣa phần xử lý lọc UKF

về xử lý tại trạm giám sát. Đây là cách xử lý dữ liệu hiệu quả hơn so với việc xử lý tất

cả thu thập dữ liệu, lọc Kalman trên vi xử lý.

Phần lọc Kalman sẽ là phần phát triển tại trạm giám sát. Đây cũng chính là

điểm mới thứ hai của luận án khi đã phát triển và ứng dụng thành công bộ lọc UKF tự

động chuyển đổi các chế độ hoạt động theo tín hiệu đầu vào, áp dụng cho xe bus.

Trong phần xử lý dữ liệu tại trạm này, luận án biểu diễn mô hình đối tƣợng thông qua

các hàm trạng thái, và mô hình đo đạc đƣợc biểu diễn dƣới dạng ma trận số. Thực chất

đối tƣợng xe bus trong thực tế là phi tuyến nên việc biểu diễn mô hình động học của

đối tƣợng thông qua các hàm mô tả đối tƣợng là bắt buộc. Cơ sở của giải pháp này

dựa trên sự hỗ trợ mạnh mẽ về tính toán toán học của Matlab cũng nhƣ các thƣ viện

công cụ (tool-box) hỗ trợ của nó [30, 43, 44, 50, 73, 78, 79, 80], qua đó đƣa phần công

việc tiêu tốn dung lƣợng bộ nhớ lớn, cần xử lý phức tạp về xử lý tại trạm. Điều này

làm cho chƣơng trình trên vi xử lý trở nên đơn giản hơn, tiết kiệm bộ nhớ cho vi xử lý,

giành phần bộ nhớ này cho mục đích khác và giúp tăng tốc độ xử lý cho hệ thống thu

thập dữ liệu.

Với các đề xuất và giải pháp mới và phƣơng pháp xử lý nêu trên, luận án đã

giải quyết triệt để những vấn đề tồn tại về trôi dữ liệu INS, tính phi tuyến của hệ thống

và đáp ứng đƣợc vấn đề xử lý thời gian thực. Tất cả các vấn đề về lý thuyết, giải pháp

đã đƣợc phân tích, biểu diễn dƣới dạng các công thức, lƣu đồ và giải thuật cụ thể. Kết

quả của luận án ngoài các công việc lý thuyết còn đƣợc đƣa vào thực tế thiết kế ứng

dụng thành công trong thực tế áp dụng cho hệ thống giám sát quản lý xe bus. Kết quả

mới này đã đƣợc báo cáo tại hôi thảo nSTf 2014 (NACENTECH, 2014) [P1.1.14].

CHƢƠNG 1. HỆ THỐNG GPS VÀ CÁC HỆ THỐNG HỖ TRỢ

1.1. Hệ thống GPS

1.1.1. Hệ trục toạ độ

Định vị giám sát có mục đích quản lý một hệ chuyển động so với một hệ tọa độ

địa lý của trái đất. Có nhiều dạng hệ tọa độ địa lý, bao gồm: WGS 84, 72, 64 và 60;

NAD27 Bắc Mỹ, NAD83 Bắc Mỹ; OSGB36 Anh; ED50 châu Âu [77]. Ngoài ra còn

một số hệ tọa độ khác chỉ đƣợc phổ biến tại một số vùng lãnh thổ nhƣ AGD66;

GDA84; ANS; GDA94, GDA 2000. [81].

Hệ trục tọa độ tâm trái đất ECEF là dạng hệ tọa độ Đề-các (Cartesian

Coordinate System) có gốc là tâm trái đất, 3 trục x, y, z là 3 trục vuông góc sao cho

mặt phẳng Oxy là mặt phẳng chứa đƣờng xích đạo. Trục x đi qua đƣờng kinh tuyến

gốc và trục z đi qua cực bắc (Hình 1.1).

Mô hình vật lý chuẩn của trái đất sử dụng trong các ứng dụng GPS là hệ tọa độ

trắc địa WGS84 của bộ quốc phòng Mỹ. Trong bài toán giám sát, các tính toán sẽ quy

đổi giá trị của hệ thống GPS ở hệ toạ độ WGS84 sang hệ tọa độ tâm trái đất.

Hình 1. 1: Hệ toạ độ tâm trái đất Oxyz

1.1.2. Cấu trúc của một hệ định vị sử dụng vệ tinh

Hệ thống vệ tinh dẫn đƣờng toàn cầu GNSS bao gồm ba hệ thống vệ tinh dẫn

đƣờng chính: GPS do Mĩ chế tạo và hoạt động từ năm 1994; GLONASS do Nga chế

tạo và hoạt động từ năm 1995; và hệ thống GALILEO do Liên hiệp Âu Châu (EU)

chế tạo và đƣợc đƣa vào sử dụng năm 2008. Hệ thống này bắt đầu đƣợc đƣa vào hoạt

động một cách chính thức năm 2014. Hiện tại, hệ thống này đang đƣợc phát triển

mạnh mẽ và dự kiến hoàn thiện năm 2019. Ngoài ra Trung Quốc đang phát triển một

hệ thống định vị vệ tinh riêng là hệ thống Beidou dành cho khu vực châu Á và Tây

Thái bình dƣơng, và hệ thống COMPASS là hệ thống định vị toàn cầu, dự kiến đƣa

vào hoạt động năm 2020. Ấn Độ cũng đang phát triển hệ thống của mình có tên là

IRNSS có phạm vi hoạt động trong khu vực Ấn Độ và vùng biển Bắc Ấn, dự kiến đi

vào hoạt động trong năm 2014. Nhật Bản cũng phát triển hệ thống của mình có tên là

QZSS có phạm vi hoạt động trong khu vực Châu Á và Châu Đại dƣơng. Trong luận án

này tác giả sử dụng tín hiệu của hệ GPS. Một hệ thống định vị sử dụng vệ tinh GPS

- Phần không gian (Space segment): Các vệ tinh trong không gian

- Phần điều khiển (Control segment): Các trạm điều khiển mặt đất

- Phần ngƣời sử dụng (User segment): Bộ thu tín hiệu trong các ứng dụng của ngƣời

đầy đủ bao gồm ba phần chính với các thành phần cơ bản sau [77]:

sử dụng (NSD)

Cho tới nay đã có bẩy thế hệ vệ tinh khác nhau, bao gồm: Block I, Block II,

Block IIA, Block IIR, Block IIRM, Block IIF (có các loại GPS IIF, GPS IIF-2, GPS

IIF-3, GPS IIF-4) và Block III (có các loại Block IIIA, Block IIIB, Block IIIC) [77].

Những vệ tinh thế hệ sau đƣợc trang bị thiết bị hiện đại hơn, có độ tin cậy cao hơn,

thời gian hoạt động lâu hơn. Tính đến thời điểm hiện tại, hệ thống GPS sử dụng một

hệ thống 36 vệ tinh, bao gồm 31 vệ tinh đang hoạt động và 5 vệ tinh dự phòng, phân

tán trên 6 quỹ đạo tròn trong không gian (Hình 1.2). Mặt phẳng quỹ đạo vệ tinh GPS

nghiêng so với mặt phẳng xích đạo một góc 55 độ. Quỹ đạo của vệ tinh có bán kính

khoảng 26.560 km. Vệ tinh bay quanh quỹ đạo với chu kì xấp xỉ 11 giờ 58 phút. Theo

lý thuyết, các quỹ đạo đƣợc tính toán sao cho ở bất cứ đâu trên trái đất thì một bộ thu

tín hiệu vệ tinh có thể nhận đƣợc tín hiệu của ít nhất 6 vệ tinh cùng một lúc nếu không

có gì ngăn cản tín hiệu. Ngoài ra các vệ tinh đƣợc bố trí trong không gian để có thể

thoả mãn yêu cầu về vị trí hình học tƣơng đối để có thể tính ra vị trí chính xác nhất.

Vệ tinh thế hệ mới nhất là GPS IIIC, có khả năng thực hiện cả tín hiệu quân sự và tín

hiệu dân dụng.

Hình 1. 2: Vệ tinh GPS IIIC và quỹ đạo các vệ tinh trong không gian

Hệ GPS sử dụng hệ thống “Giờ phối hợp quốc tế” UTC (Coordinate Universal

Time), là một chuẩn quốc tế về ngày giờ thực hiện bằng phƣơng pháp nguyên tử. Nó

đƣợc dựa trên chuẩn cũ là giờ trung bình Greenwich GMT (Greenwich Mean Time)

do hải quân Anh đặt ra vào thế kỷ thứ 19, sau đó đƣợc đổi tên thành giờ quốc tế UT

(Universal Time). Múi giờ trên thế giới đƣợc tính bằng độ lệch âm hay dƣơng so với

giờ quốc tế này.

1.1.3. Nguyên lý hoạt động

Để xác định chính xác vị trí của một điểm trong hệ thống GPS, cần sử dụng ít

nhất 4 vệ tinh, có nghĩa là một máy thu GPS khi liên lạc đƣợc với nhiều hơn 4 vệ tinh

thì có thể cho biết vị trí chính xác của máy thu đó (Hình 1.3). Đồng bộ thời gian là vấn

đề vô cùng quan trọng trong hệ thống GPS. Có sự lệnh thời gian giữa thiết bị thu và

phát tín hiệu là do hầu hết các thiết bị định vị GPS có giá thành tƣơng đối rẻ, cỡ xấp xỉ

100 USD, vì thế nên đồng hồ thời gian không thể là loại có độ chính xác cao. Tốc độ ánh sáng xấp xỉ 3.108 m/s, nếu đồng hồ của thiết bị nhận GPS có sai số là 0,001s hay 1ms thì sẽ gây ra một sai số về khoảng cách là 0,001 3.108 m = 300.000 m hay 300

km. Đây là một trong lý do rất khó áp dụng các hệ thống thu phát tín hiệu khác nhƣ

RF hay truển hình số để thay thế GPS.

Hình 1. 3: Tín hiệu từ vệ tinh và nguyên tắc xác định vị trí đối tƣợng

NMEA là chuẩn giao thức đƣợc sử dụng phổ biến nhất trong các máy thu GPS

hiện nay. Đƣợc phát triển bởi Hiệp hội Điện tử hàng hải quốc gia (National Marine

Electronics Association), đến nay NMEA có 4 phiên bản, đó là: NMEA 1.5, NMEA

2.0, NMEA 2.3 và NMEA 3.01. Giao diện truyền thông của máy thu GPS đƣợc định

nghĩa trong NMEA là chuẩn RS-232, tốc độ truyền dữ liệu phổ biến là 4.800 baud,

một số máy thu GPS hiện nay có thể truyền dữ liệu với tốc độ 9.600 baud và cao hơn.

Các thông điệp gửi đi từ máy thu GPS có độ dài tối đa là 82 ký tự mã ASCII và đƣợc

gọi là các câu (sentence). Số lƣợng thông điệp là khác nhau đối với mỗi phiên bản

giao thức. Một máy thu GPS có thể gửi đi khoảng 26 loại thông điệp khác nhau. Các

loại thông điệp đƣợc phân biệt với nhau bằng 5 ký tự đầu tiên ngay sau dấu $. [85].

1.1.4. Sai số và các nguyên nhân gây sai số

Độ chính xác của một hệ thống quản lý giám sát sử dụng GPS không phải luôn

là một con số cố định, tức là chúng ta không thể khẳng định rằng hệ thống này “chính

xác bao nhiêu mét”. Độ chính xác của hệ thống quản lý giám sát các thiết bị sử dụng

GPS phụ thuộc vào nhiều yếu tố, trong đó hai yếu tố chính là module thu tín hiệu GPS

và bản đồ số thể hiện kết quả chuyển động của đối tƣợng. Thực chất ta chỉ có thể nói

độ chính xác của module thu tín hiệu GPS đƣợc sử dụng trong thiết kế chứ không phải

là độ chính xác của hệ thống GPS. Số liệu mà module GPS đƣa ra lại phụ thuộc vào

chất lƣợng của module, cấu trúc module có sử dụng đồng xử lý hỗ trợ hay không, hay

đặc điểm chuyển động của đối tƣợng mang module GPS tại thời điểm đó là đứng yên,

chuyển động nhỏ, chuyển động thẳng, chuyển động cong. Ngoài ra, chất lƣợng của hệ

thống bản đồ số cũng là yếu tố quan trọng khi nói về độ chính xác của hệ thống giám

sát sử dụng GPS. Có rất nhiều tác động khác nữa có thể làm giảm độ chính xác của bộ

thu GPS bao gồm sai số thời gian từ đồng hồ trên vệ tinh, sai số vị trí của vệ tinh, ảnh

hƣởng của tầng điện ly và tầng khí quyển, phản xạ bởi các bề mặt…

Tổng hợp sai số và độ ảnh hƣởng của các nhân tố tới tính chính xác của GPS

đƣợc liệt kê trong bảng sau: [1, 18]

Bảng 1: Nguồn gốc và nguyên nhân gây sai số GPS

Hiệu ứng tầng điện ly ± 5 mét

Sự thay đổi trong quỹ đạo vệ tinh ± 2.5 mét

Sai số của đồng hồ của vệ tinh ± 2 mét

Hiệu ứng đa đƣờng (Sai số do phản xạ) ± 1 mét

Hiệu ứng đối lƣu ± 0.5 mét

Lỗi tính toán và làm tròn ± 1 mét

1.2. Hệ thống INS

1.2.1. Các hệ tọa độ (Frames)

 Hệ toạ độ vật thể (b-frame)

Hệ tọa độ vật thể Body frame (b-frame), còn gọi là Vehicle frame hay Plane

(hệ tọa độ P), là hệ tọa độ cơ bản của INS (Hình 1.4). Đây là hệ trục toạ độ trực giao

tƣơng ứng với các hƣớng chuyển động roll-pitch-heading của vật thể. Vận tốc của đối

tƣợng chuyển động trong hệ tọa độ vật thể ứng với 3 trục x, y, z.

Hình 1. 4: Hệ tọa độ vật thể và các thành phần đặc trƣng cho chuyển động

 Hệ tọa độ quán tính ECI

Hệ trục toạ độ quán tính ECI (Hình 1.5) cố định trong không gian đối với các

vật thể trên quỹ đạo của trái đất, vì vậy nó thƣờng đƣợc sử dụng để xác đinh vị trí của

các vệ tinh nhân tạo trên quỹ đạo. Gốc của hệ trục nằm ở tâm trái đất, mặt phẳng XY

trùng với mặt phẳng chứa đƣờng xích đạo, trục X có hƣớng từ tâm trái đất đến điểm

xuân phân – là giao điểm giữa quỹ đạo quay của trái đất và đƣờng xích đạo của mặt

trời, và trục Y vuông góc với trục X theo hƣớng .

Hình 1. 5: Hệ toạ độ quán tính ECI

 Hệ toạ độ dẫn đƣờng (n-frame)

Hệ tọa độ dẫn đƣờng (n-frame) là hệ tọa độ đo đạc cục bộ với tâm của nó trùng

với hệ toạ độ của cảm biến, trục x chỉ về hƣớng đo đạc phía bắc, trục z vuông góc trực

giao với đƣờng elipsoid tham chiếu hƣớng xuống dƣới, và trục y nằm bên phải của

khung trực giao. Các tính toán cho hệ thống dẫn đƣờng thƣờng dựa trên hai hệ tọa độ

là Bắc – Đông –Xuống NED (North – East – Down) và Đông – Bắc – Lên ENU (East

– North – Up). Thuận lợi của hệ toạ độ ENU là cao độ sẽ tăng lên khi đi lên. Thuận lợi

của hệ toạ độ NED là quay bên phải là chiều dƣơng đối với trục x, và các trục là tƣơng

ứng với toạ độ góc roll, pitch và heading của phƣơng tiện khi mà đối tƣợng chuyển

động nằm trên mặt phẳng hƣớng về hƣớng Bắc. Về cơ bản hai hệ ENU và NED có

tính chất gần nhƣ nhau, chỉ khác nhau về giá trị của thông số là ngƣợc dấu do tính chất

đảo trục tọa độ. Trong các bài toán điều khiển bay, rất nhiều các thông số liên quan tới

vật thể bay tới từ các đối tƣợng nằm bên dƣới nó. Do đó, sẽ là hợp lý để xác định

chiều đi xuống nhƣ một số có giá trị dƣơng. Điều này phù hợp với các tọa độ của hệ

NED. Vì vậy hệ NED trở nên rất phổ biến với các kết quả nghiên cứu có thể dễ dàng

tìm kiếm cũng nhƣ kết hợp với nhau. Hệ trục toạ độ này sẽ quay cùng với trái đất nên

thƣờng đƣợc sử dụng để xác định vị trí theo ba trục, từ đó có thể dễ dàng hơn khi

chuyển đổi sang kinh độ, vĩ độ.

Trong các thiết kế mới gần đây, nhất là đối với các thiết kế của châu Âu, hệ tọa

độ NWU (North – West – Up hay còn gọi là North – West – Zenith coordinate

system) thƣờng đƣợc sử dụng trên cơ sở hệ tọa độ CCS (Cartesian Coordinate

System). Đối với các hệ không gian thƣờng sử dụng hệ tọa độ AAC (Altitude -

Azimuth Coordinate System) [82].

Hình 1. 6: Hệ tọa độ CCS và hệ NWU

1.2.2. Các thành phần đặc trƣng cho chuyển động

Các thành phần cơ bản thể hiện trạng thái chuyển động của một đối tƣợng bao

gồm gia tốc và góc quay theo các hƣớng, bao gồm góc quay (roll, pitch, yaw) và góc

trƣợt (α, β) trong hệ quy chiếu đó [60].

 Góc quay Roll, Pitch, Yaw

Hình 1. 7: Các góc quay roll, pitch, yaw

 Góc trƣợt α và β

Khi chuyển động của đối tƣợng không theo góc pitch , ta có sự khác biệt giữa

hƣớng thực của đối tƣợng và góc hƣớng (true attack và attack angle). Góc trƣợt này

gọi là , đặc trƣng cho độ lệch giữa véc tơ tốc độ thực của đối tƣợng với véc tơ gốc

của góc pitch (Hình 1.8).

Hình 1. 8: Góc trƣợt α

Xét về mặt toán học, góc trƣợt này đƣợc biểu diễn theo công thức:

(1.4)

Ngoài góc trƣợt , đối tƣợng chuyển động còn chịu tác động của hiện tƣợng

lệch góc hƣớng, góc lệch (sideslip), là độ lệch giữa Heading và Yaw (Hình 1.9).

Hình 1. 9: Góc trƣợt β

Xét về mặt toán học, góc trƣợt này đƣợc biểu diễn theo công thức:

(1.5)

Các tính toán đối với hệ thống INS trƣớc đây thƣờng bỏ qua hai góc trƣợt này.

Đây cũng là một trong những nguyên nhân gây ra sai số lớn khi tính toán với hệ thống

INS, đặc biệt khi áp dụng cho các đối tƣợng bay hay đối tƣợng chuyển động có hiện

tƣợng trƣợt văng. Giải pháp thiết kế với INS 9-DOF trong luận án đã giải quyết triệt

để đƣợc vấn đề này.

1.2.3. Ma trận chuyển vị giữa các hệ tọa độ

Trong bài toán định vị dẫn đƣờng cho một vật thể chuyển động, có nhiều yếu tố

cần quan tâm bao gồm: gia tốc, vận tốc, khoảng di chuyển, các góc quay và hƣớng

chuyển động (góc hƣớng). Ngƣời ta đƣa ra các khái niệm hệ tọa độ quy chiếu, đó là

Hệ tọa độ quán tính (i-frame), Hệ tọa độ trái đất (e-frame) hay G (Ground), Hệ tọa độ

dẫn đƣờng (n-frame), Hệ tọa độ vật thể (b-frame) hay còn gọi là hệ tọa độ gắn liền của

đối tƣợng P (Plane). Chuyển động của vật thể lúc đó là sự di chuyển của hệ quy chiếu

này so với hệ quy chiếu khác. Ví dụ tốc độ di chuyển của vật thể (transport rate) là tốc

độ quay của hệ toạ độ dẫn đƣờng (n-frame) đối với hệ toạ độ trái đất (e-frame). Đại

lƣợng này khác với vận tốc (velocity) và là yếu tố chỉ có ý nghĩa về mặt giá trị. Các hệ

quy chiếu đƣợc liên hệ với nhau thông qua ma trận cosine chỉ phƣơng DCM, là ma

trận chuyển đổi hệ trục tọa độ, ký hiệu là R (ma trận quay Rotation) với các phần tử

đƣợc biểu diễn theo các góc Euler [88]. Đại lƣợng này ký hiệu là . đặc trƣng cho

chuyển động của hệ tọa độ f1 (frame 1) với hệ tọa độ f2 (frame 2). Tính toán ma trận

quay R đƣợc mô tả chi tiết trong thiết kế hệ thống với INS cụ thể, đƣợc trình bày chi

tiết trong chƣơng tiếp theo.

1.2.4. Định vị sử dụng cảm biến quán tính

Hệ thống dẫn đƣờng quán tính INS là một công cụ dẫn đƣờng sử dụng thiết bị

tính toán và các phần tử cảm ứng sự di chuyển để xác định vị trí và tốc độ một cách

liên tục. Tốc độ (Transporrt rate) của phƣơng tiện chuyển động đƣợc quyết định bởi

xu hƣớng (Attitude) và vận tốc di chuyển (Velocity). Một hệ định vị sử dụng cảm biến

quán tính cơ bản bao gồm hai loại cảm biến là gia tốc kế Acc (Acceleremetor) và con

quay hồi chuyển Gyro (Gyroscope). Con quay hồi chuyển Gyro sẽ đo góc quay và gia

tốc kế Acc sẽ đo gia tốc di chuyển. Về mặt lý thuyết, với điều kiện ban đầu xác định

về vận tốc và vị trí thông qua một hệ tọa độ định vị (thƣờng đƣợc xác định từ GPS) thì

tích phân kết quả từ gia tốc kế sẽ thu đƣợc vận tốc và tích phân vận tốc sẽ thu đƣợc

khoảng cách di chuyển. Đây cũng chính là lý do khi mất đi thông tin của hệ tọa định

vị GPS thì hệ thống sẽ mất đi khả năng định vị chính xác. Gyro sẽ giúp xác định

hƣớng dịch chuyển thông qua các hƣớng của gia tốc. Kết hợp của số đo gia tốc và góc

quay trên cơ sở các hệ quy chiếu ta sẽ xác định đƣợc hƣớng và vị trí của phƣơng tiện

chuyển động.

Có ba loại công nghệ mà hệ thống dẫn đƣờng quán tính áp dụng, bao gồm: hệ

Gimbaled (Gimbaled gyrostabilized platforms), hệ Fluid-suspended (Fluid-suspended

gyrostabilized platforms) và hệ Strapdown (Strapdown gyrostabilized platforms) [15].

Hai hệ thống thƣờng đƣợc sử dụng là Gimbaled và Strapdown, tùy theo tính năng và

chất lƣợng yêu cầu của đối tƣợng chuyển động. Hệ Gimbaled thƣờng sử dụng các INS

cơ đắt tiền cho các thiết bị hiện đại đòi hỏi độ chính xác và an toàn rất cao. Các thiết

kế trong luận án này sử dụng INS dạng MEMS, cấu trúc Strapdown.

Hệ Strapdown là khái niệm về phƣơng tiện chủ đƣợc sử dụng để đề cập đến

những vật thể mà hệ INS gắn chặt lên đó. Các cảm biến quán tính Gyro có thể đƣợc

gắn trên một hệ thống các trục quay trơn để chúng có thể giữ hƣớng cố định khi

phƣơng tiện chủ di chuyển, hoặc chúng có thể đƣợc gắn trực tiếp lên phƣơng tiện chủ.

Các hệ MEMS thay thế các khối dao động cơ bằng các phần tử cơ điện tử có độ chính

xác cao. Trong hệ Strapdown, đầu tiên hệ thống sẽ đo gia tốc theo hệ trục toạ độ gắn

trên vật thể P, sau đó sẽ chuyển vị về hệ trục tọa độ quy chiếu G. Nhƣ vậy hệ

Strapdown sẽ sử dụng một IMU không bị cách ly với phƣơng tiện chủ. Nó thay thế

các khớp cách ly nhƣ ở hệ Gyro cơ bằng một quá trình tính toán phần mềm. Ƣu điểm

của hệ thống MEMS là giá thành rẻ hơn so với hệ cơ do không cần sử dụng các khớp

cách ly, nhƣng nhƣợc điểm là chúng cần phải hoạt động với tốc độ quay cao để có

đƣợc số liệu đầu ra đủ biến thiên độ nhạy. Đối với vật thể bay trong trƣờng hợp

chuyển động với khoảng cách lớn, thay đổi vùng địa lý, ta cần tính đến gia tốc

Corriolis do ảnh hƣởng của chuyển động quay trái đất.

1.2.5. Sai số và quá trình căn chỉnh ban đầu

Có nhiều loại sai số trong các hệ thống INS, chủ yếu là do các cảm biến quán

tính gây nên. Các sai số này bao gồm: sai số độ lệch (Bias), hệ số tỉ lệ (Scale factor),

độ ổn định đầu ra (Output stability), độ nhạy nhiệt (Thermal sensitivity), độ nhạy với

từ tính (Magnetic sensitivity), ảnh hƣởng của lực ly tâm (Centrifuge), sai số do khả

năng chịu va đập (Shock survivability) [1, 36]. Thông thƣờng trong hệ INS Strapdown

ta quan tâm tới 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 nhƣ đƣợc

trình bày trong bảng 2.

Bảng 2: Các lỗi chính của hệ strapdown

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 tốc Đầu ra cảm biến gia tốc sẽ bị lệch đi

một giá trị không đổi. Giá trị này lại

thay đổi mỗi khi tắt/bật thiết bị.

Hiện tƣợng lệch và trôi của cảm biến Vật thể không chuyển động nhƣng

vận tốc góc (do tác động của nhiệt độ) vẫn có vận tốc góc thay đổi

Nhiễu ngẫu nhiên Lỗi ngẫu nhiên trong đo lƣờng

Hai sai số cơ bản là Drift (độ trôi) và Bias (độ lệch), là những thay đổi trong

các khoảng thời gian khác nhau gây ra các nhiễu cho phép đo. Điều này dẫn đến tình

huống cảm biến sẽ cho các thông số khác nhau trong các thời điểm khác nhau ngay cả

khi đối tƣợng chuyển động hoàn toàn tƣơng tự nhau trong mỗi thời điểm. Xử lý đƣợc

hai thông số này là một trong những yếu tố quyết định nâng cao chất lƣợng cho hệ

thống INS. Khi tính toán với các hệ thống INS đơn trục trƣớc đây, hai tham số này

thƣờng bị bỏ qua.

Những sai số của hệ thống INS trong đo gia tốc và vận tốc góc sẽ dẫn tới các

sai số của các phần tử trong ma trận quay, từ đó gây sai số cho các góc quay roll, pitch

và yaw ở bƣớc tính toán tiếp theo. Điều này gây sai số tăng dần khi xác định vị trí và

vận tốc của vật thể do việc lấy tích phân, gọi là sai số dẫn đƣờng. Vì vậy xử lý chống

trôi dữ liệu INS là một vấn đề mang tính quyết định cho chất lƣợng của hệ thống INS.

Đây là lý do mà các hệ thống INS rẻ tiền không thể hoạt động độc lập trong khoảng

thời gian dài đƣợc. Trƣớc khi hệ INS đi vào hoạt động cần phải thiết lập một số điều

kiện cho hệ thống (quá trình Calib), và phải xác định các giá trị ban đầu về các thông

số giới hạn đƣa ra theo các trục. Ngoài ra cũng cần phải xác định đƣợc vị trí đầu của

phƣơng tiện, thƣờng đƣợc xác định từ GPS, từ đó tính toán ra các vị trí tiếp theo tạo ra

quỹ đạo chuyển động của đối tƣợng.

Khi triển khai thực tế, các sai lệch của hệ thống tích hợp chủ yếu do các yếu tố

sau quyết định:

- Sai số do đối tƣợng và đặc điểm chuyển động, trong đó vấn đề vi phạm điều kiện

bờ (banking angles) là một trong các yếu tố chính gây sai lệch kết quả đo. Khi đối

tƣợng lên tục đảo chiều quay nhanh quanh trục của góc yaw sẽ gây sai lệch góc

pitch. Up-deflection là độ lệch đứng cần hiệu chỉnh, là giá trị không đo đƣợc trực

tiếp, tính toán phụ thuộc vào giới hạn góc banking angles. Với giải pháp tự động

chống trôi ma trận quay trong luận án xử lý triệt để cả vấn đề này.

- Sai số do đặc điểm cấu tạo của INS, chủ yếu do Gyro gây ra bao gồm: Lỗi số

(Numerrical errors); Trôi dữ liệu Gyro (Gyro drift); và Sai lệch offset của Gyro

(Gyro offset).

- Sai số do vi xử lý và phƣơng pháp tính toán gây ra, bao gồm: Sai số tích hợp

(Integration errors), phụ thuộc vào phƣơng pháp tính toán, bƣớc thời gian khác

nhau; và sai số lƣợng tử hóa (Quantization errors) do cách sử dụng, làm tròn các

biến, giá trị, chuyển đổi ADC/DAC hay hiện tƣợng xử lý bị tràn dữ liệu.

Những sai số này phụ thuộc vào hệ INS lựa chọn trong thiết kế, đặc trƣng cho

hiệu suất của INS đó. Ngoài ra còn có ảnh hƣởng sai số do nhiệt độ, độ trễ từ và trễ

nhiệt (magnetic – thermal hysteresis), sự chấn động (vibration). Khi nhiệt độ của các

IMU thay đổi, sự ảnh hƣởng của Drift và Bias sẽ thay đổi theo, cho đến khi nhiệt độ

đạt đến trạng thái ổn định hoặc đƣợc giữ nguyên. Với vật thể bay khi thay đổi độ cao

sẽ có thay đổi lớn về nhiệt độ nên cần đặc biệt chú ý tới tham số này. Ngoài các yếu tố

chính về trôi các tín hiệu đầu ra của INS dẫn đến sai lệch tính toán các phần tử của ma

trận quay R, đây cũng là những yếu tố gây ra sai số cho hệ INS strapdown gắn trên vật

thể chuyển động, đặc biệt là khi có hiện tƣợng trƣợt văng.

1.3. Hệ thống định vị tích hợp GPS/INS

1.3.1. Bản chất bù giữa INS và GPS

Các hệ thống dẫn đƣờng tích hợp nhiều công nghệ sẽ có lợi hơn trong các ứng

dụng xác định vị trí và dẫn đƣờng bởi vì chúng cho phép thực hiện việc trộn dữ liệu

tại cấp độ đo lƣờng. Việc tích hợp các công nghệ dẫn đƣờng nhƣ GPS/INS sẽ mang

đến một hệ thống hoạt động tốt hơn so với các hệ thống chỉ dùng GPS hoặc INS.

Thêm vào đó, việc bù dữ liệu cho nhau giữa hai hệ thống trên sẽ tạo ra một hệ thống

có độ chính xác cao hơn.

Trong các ứng dụng dựa trên quy chiếu địa lý, vị trí và vận tốc nhận đƣợc từ

GPS sẽ có các kết quả đo đạc tốt hơn khi đƣợc cập nhật thêm số đo từ INS. Tƣơng tự

nhƣ vậy, hệ INS sẽ cung cấp dữ liệu về vị trí và vận tốc chính xác cho việc thu thập

dữ liệu từ GPS và tự thu thập dữ liệu giữa các chu kỳ cập nhật GPS hay khi không

nhận đƣợc dữ liệu từ GPS, cũng nhƣ cung cấp đƣợc các tham số về các góc dao động

và hƣớng di chuyển của đối tƣợng. Hệ thống GPS xác định tọa độ ban đầu của hệ

thống tích hợp, cung cấp số liệu khởi tạo và cập nhật cho hệ INS làm việc. Trong

khoảng thời gian giữa các lần cập nhật tín hiệu GPS ta vẫn xác định đƣợc vị trí của đối

tƣợng dựa trên các thông số của INS. Nhƣ vậy hệ INS cần sự ổn định trong thời gian

dài của hệ GPS, trong đó tọa độ từ hệ GPS đƣợc sử dụng làm các số đo cập nhật cho

vị trí sinh ra bởi hệ INS. Mặt khác, thông tin vị trí và hƣớng có độ chính xác cao trong

thời gian ngắn của hệ INS sẽ cung cấp một quỹ đạo nội suy giữa các lần cập nhật dữ

liệu của hệ GPS.

Hệ thống INS thông thƣờng bao gồm hai kiểu cảm biến quán tính là các gia tốc

kế để đo độ tăng của gia tốc dài và con quay hồi chuyển để đo độ tăng của vận tốc

góc. Khác với các thiết kế trƣớc đây, trong luận án này tác giả sử dụng cảm biến tích

hợp đa trục 9-DOF nên có thể bỏ qua khoảng cách của các cảm biến Gyro và Acc.

Trƣớc đây, việc tích hợp giữa GPS và INS thƣờng cài đặt bộ lọc Kalman trong các sơ

đồ khác nhau nhƣ tích hợp theo cặp lỏng, chặt, và có thể đƣợc thực hiện trong các

vòng đóng hoặc mở ngay trên vi xử lý. Hệ thống tích hợp GPS/INS trong luận án này

đƣợc thiết kế kiểu phân tán, với khả năng xử lý chống trôi dữ liệu cho INS. Giải pháp

này kết hợp với các tín hiệu bổ sung cho bộ lọc đƣợc lấy từ bus CAN theo chuẩn

J1939 FMS của xe để xử lý lọc Kalman dạng UKF tại trạm.

1.3.2. Các phƣơng pháp tích hợp GPS/INS

Tính đến thời điểm này đã có khá nhiều giải pháp khác nhau có thể áp dụng

cho việc trộn dữ liệu giữa hệ thống GPS và hệ thống INS, phụ thuộc vào đặc điểm của

INS và Vi xử lý. Các kiểu tích hợp có thể đƣợc phân loại bởi mức độ mà dữ liệu của

mỗi thành phần có thể hỗ trợ chức năng cho các thành phần khác. Các kiểu trộn dữ

liệu này đƣợc trình bày trong nhiều tài liệu về hệ thống đa cảm biến, đã đƣợc đánh giá

trong phần Tổng quan về bài toán giám sát. Nếu chỉ xét đến hỗ trợ của các sai số sau

khi đƣợc đánh giá đến giá trị đầu ra ta sẽ có phƣơng pháp tích hợp truyền thẳng và

phản hồi. Trong phƣơng pháp truyền thẳng hoạt động của hệ thống quán tính sẽ hoạt

động độc lập mà không quan tâm đến các dữ liệu bên ngoài. Phƣơng pháp phản hồi

các đánh giá sai số trong các thành phần dẫn đƣờng sẽ đƣợc phản hồi ngƣợc trở lại

quá trình tính toán. Nhƣợc điểm của phƣơng pháp truyền thẳng là quá trình tính toán

cộng dồn sai số có thể dẫn đến sự tăng không có giới hạn của sai số dẫn đến việc bộ

lọc sẽ nhận đƣợc các giá trị đo lƣờng có sai số lớn. Nguyên nhân này là một vấn đề

của bộ lọc tuyến tính vì quá trình tuyến tính hoá chỉ có thể tiến hành với các sai số

nhỏ. Khi kết hợp GPS/INS dựa trên kiến trúc xử lý của hệ thống và mức độ trộn dữ

liệu ta sẽ có các dạng tích hợp cặp lỏng, cặp chặt, phân tán và tập trung [1, 44].

 Tích hợp lỏng GPS/INS

Tích hợp cặp lỏng là phƣơng pháp đơn giản nhất trong các thiết kế trƣớc đây.

Trong phƣơng pháp này, GPS và các IMU sẽ đƣa ra dữ liệu một cách độc lập về vị trí,

vận tốc và hƣớng. Hai dữ liệu độc lập sau đó sẽ đƣợc kết hợp lại tạo thành một dữ liệu

trộn giữa GPS/INS. Bộ lọc Kalman sử dụng kết quả của tín hiệu GPS để ƣớc lƣợng sai

số cảm biến mà INS không thể tự ƣớc lƣợng đƣợc. Cách làm này áp dụng cho các hệ

thống INS đơn trục cổ điển, đƣợc gọi là hệ INS hỗ trợ bởi GPS do các giá trị của hệ

tích hợp INS đƣợc tính toán dùng kết quả của tín hiệu GPS làm thông số tham chiếu.

Các cảm biến INS chất lƣợng thấp phải đƣợc khảo sát từ trƣớc trong một số

chuyển động đặc trƣng cho việc mất tín hiệu GPS và đƣa vào các bảng bù dữ liệu.

Trong trƣờng hợp này, tích hợp lỏng GPS/INS đƣợc coi là gồm cả quá trình phản hồi

và do vậy hiệu số giữa kết quả của INS và GPS sẽ đƣợc phản hồi tới các cảm biến

quán tính để thực hiện việc căn chỉnh. Các bảng bù, luật bù với các hệ số bù khác

nhau đƣợc xây dựng trên cơ sở thực nghiệm đối với mỗi hệ thống INS và không sử

dụng cho hệ thống INS khác đƣợc.

 Tích hợp chặt GPS/INS

Tích hợp chặt GPS/INS là một tích hợp có độ phức tạp cao hơn so với tích hợp

lỏng. Ngoài các lợi ích nhƣ kết hợp lỏng đem lại, các hệ thống tích hợp chặt sẽ có một

kết quả định vị chính xác hơn. Phƣơng pháp kết hợp lỏng là rất phổ thông vì có tính

module hoá cao, tuy nhiên phƣơng pháp kết hợp chặt nhận đƣợc sự quan tâm nhiều

hơn bởi vì tốc độ tính toán của các thiết bị hiện nay ngày càng đƣợc tăng cao.

Có hai loại thuật toán xử lý cơ bản, là xử lý tập trung và xử lý phân tán. Trong

xử lý tập trung, dữ liệu thô của cảm biến sẽ đƣợc kết hợp xử lý tại trung tâm để tính ra

vị trí. Xử lý phân tán là phƣơng pháp tuần tự xử lý, tức là các bộ xử lý của các hệ

thống riêng lẻ sẽ cung cấp kết quả sau đó đƣợc kết hợp với các mức độ tối ƣu khác

nhau bởi một bộ xử lý chủ. Chi tiết về giải pháp tích hợp sẽ đƣợc phân tích trong

chƣơng 4, làm tiền đề để so sánh với phƣơng án thiết kế mới của luận án.

1.3.3. Bài toán giám sát và đối tƣợng áp dụng

Hệ thống giám sát bao gồm hai phần: một bộ thu thập dữ liệu và điều khiển gắn

trên đối tƣợng và một phần mềm giám sát đối tƣợng trên cơ sở bản đồ số tại trung

tâm. Khả năng phát triển của sản phẩm trong hệ thống điều khiển giám sát là rất lớn,

ứng dụng cho nhiều dạng đối tƣợng khác nhau trong quản lý phƣơng tiện giao thông

vận tải đƣờng bộ, đƣờng sắt, đƣờng biển và đƣờng hàng không. Luận án tập trung và

nghiên cứu giải pháp và phát triển thiết bị định vị tích hợp GPS/INS áp dụng cho

phƣơng tiện giao thông vận tải đƣờng bộ, cụ thể là xe bus.

Hệ thống giám sát, quản lý hành trình xe có thể đƣợc xây dựng theo hai

phƣơng án: không trực tuyến (off-line) và trực tuyến (on-line). Hai phƣơng án này

khác nhau ở chỗ thông tin của xe về toạ độ và vận tốc có đƣợc truyền trực tuyến đến

ngƣời quản lý hay không.

Phương án off-line: các thông tin về xe nhƣ vị trí và vận tốc không đƣợc truyền

ngay về trung tâm mà đƣợc lƣu trữ lại tại thiết bị trên xe (còn gọi là hộp đen). Sau khi

kết thúc hành trình các thông tin này đƣợc lấy ra phục vụ công tác thống kê, hậu kiểm.

Phƣơng pháp này là đơn giản về kỹ thuật, giá thành rẻ do không cần đến thiết bị và

đƣờng truyền số liệu trực tuyến. Tuy nhiên nó có nhƣợc điểm là không cho phép giám

sát trực tuyến và điều hành tức thời. Phƣơng án này thƣờng áp dụng cho các phƣơng

tiện chở hàng thông dụng không cần đòi hỏi cao về tình trạng trực tuyến và chỉ thích

hợp cho mục đích hậu kiểm.

Phương án on-line: cho phép truyền trực tuyến thông tin xe về trung tâm, giúp

ngƣời quản lý giám sát trực tiếp hành trình xe và có thể có những quyết định điều

hành kịp thời. Ƣu điểm của phƣơng pháp này là cho phép giám sát trực tuyến hành

trình xe và điều hành tức thời. Phƣơng pháp này phức tạp hơn về kỹ thuật, có giá

thành cao hơn do cần đến thiết bị và đƣờng truyền số liệu trực tuyến, thích hợp cho

mục đích giám sát, điều hành trực tiếp. Ngoài việc vẫn lƣu trữ dữ liệu trực tiếp dƣới

dạng hộp đen, các thiết bị này cần gắn thêm module thu phát tín hiệu

GPRS/GSM/Wifi để truyền nhận thông tin về trạm, đảm bảo không những không mất

tín hiệu vị trí mà cả không mất thông tin truyền từ đối tƣợng về trạm.

1.4. Kết luận chƣơng 1

Chất lƣợng của hệ thống giám sát quyết định bởi độ chính xác về quỹ đạo

chuyển động và khả năng quản lý các thông số về trạng thái chuyển động của các đối

tƣợng. Độ chính xác của hệ thống giám sát sử dụng GPS phụ thuộc chủ yếu vào

module thu tín hiệu GPS. Đại lƣợng này không phải là một giá trị bất biến mà phụ

thuộc vào trạng thái và đặc tính chuyển động của đối tƣợng nhƣ đứng yên, chuyển

động thẳng, chuyển động cong, di chuyển nhỏ hay di chuyển với vận tốc lớn, độ rung

lắc của xe. Ngoài ra, độ chính xác còn phụ thuộc vào nhiều yếu tố khách quan nhƣ

thời tiết, địa hình. Để nâng cao chất lƣợng của thiết bị giám sát sử dụng GPS, phƣơng

pháp sử dụng hệ tích hợp GPS/INS kết hợp với các hệ thống hỗ trợ là một phƣơng

pháp có thể thực hiện đƣợc và nên làm. Lý do là ngoài việc hiệu chỉnh chính xác quỹ

đạo của đối tƣợng chuyển động, hệ thống tích hợp GPS/INS còn quản lý đƣợc các

thông số dao động của đối tƣợng, qua đó nâng cao chất lƣợng của hệ thống giám sát.

Phƣơng pháp này đòi hỏi thiết bị trên xe phải xử lý dữ liệu của hệ thống INS một cách

hiệu quả để giảm sai số cho hệ thống. Điều này phụ thuộc vào khả năng của INS, vi

xử lý sử dụng và phƣơng pháp xử lý các dữ liệu thô của INS.

Do sự hạn chế về công nghệ chế tạo INS nên khi thiết kế thiết bị trên cơ sở lựa

chọn vi xử lý hay hệ thống chúng, ta cần xem xét lựa chọn giải pháp cũng nhƣ thiết bị

một cách hợp lý để đáp ứng đƣợc yếu tố thời gian thực. Các hệ thống tích hợp

GPS/INS thƣờng đƣợc xây dựng trên cơ sở sử dụng bộ lọc Kalman để nâng cao độ

chính xác. Hiện nay, bộ lọc Kalman đã đƣợc phát triển thành các Tool-box trên

Matlab thuận lợi cho việc phát triển ứng dụng. Tuy nhiên hiện tại thƣ viện này mới chỉ

đƣợc áp dụng cho các đối tƣợng mang tính lý thuyết, thuần phi tuyến hoặc tuyến tính.

Trong chƣơng tiếp theo, tác giả sẽ khai thác và phát triển thƣ viện này, ứng dụng vào

đối tƣợng của luận án là phƣơng tiện đƣờng bộ cho đối tƣợng Phi tuyến – Tuyến tính,

áp dụng cho xe bus, sử dụng mô hình xe của Biral [10, 24]. Giải pháp này có thể đƣợc

phát triển áp dụng rộng rãi cho hệ thống quản lý giám sát các đối tƣợng chuyển động

khác.

CHƢƠNG 2. LỌC KALMAN VÀ MATLAB TOOL-BOX

2.1. Lọc Kalman KF

2.1.1. Bản chất toán học

Năm 1960, R.E. Kalman đã công bố một giải pháp truy hồi để giải quyết bài

toán lọc thông tin rời rạc tuyến tính lấy tên là lọc Kalman [25]. Từ đó đến nay cùng

với sự phát triển của tính toán kỹ thuật số, bộ lọc Kalman (KF) đã trở thành chủ đề

nghiên cứu sôi nổi và đƣợc ứng dụng trong nhiều ngành kỹ thuật công nghệ khác

nhau. Tài liệu cơ sở về KF này đƣợc phát triển hoàn thiện và cập nhật năm 2006 [76].

Một cách khái quát, lọc Kalman là một tập hợp các phƣơng trình toán học mô tả một

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á

Tín hiệu đo

Bộ lọc Kalman

Mô hình hệ thống

trình sao cho độ lệch giữa giá trị thực và giá trị ƣớc đoán là nhỏ nhất.

Hình 2. 1: Mô hình hoá hoạt động của lọc Kalman

Bộ lọc Kalman kinh điển đƣa ra một giải pháp đệ quy (closed form) đánh giá

những hệ thống động rời rạc tuyến tính hoá theo thời gian. Lọc Kalman bao gồm hai

bƣớc: bƣớc Dự báo (prediction), ở đó các 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 đó; và bƣớc Cập nhật (Update ), ở đó 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 2.2).

Hình 2. 2: Cơ chế lọc Kalman

Hình 2. 3: Bản chất hoạt động của bộ lọc Kalman

2.1.2. Bộ lọc Kalman rời rạc

Một mô hình tuyến tính hoá theo thời gian của đối tƣợng trong hệ thống giám

sát đƣợc diễn tả với công thức tuyến tính hoá liên tục nhƣ sau [25]:

(2.1)

y(t) = Hx(t) + r(t)

Trong đó:

 Điều kiện đầu là: x(0)~N(m(0),P(0))

 F và L ma trận hằng với đặc tính hoạt động của mô hình

 H là ma trận đo đạc, phụ thuộc vào cấu trúc cụ thể của đối tƣợng

 w(t) là ồn trắng với cƣờng độ phổ là Qc

 r(t) là sai số đo đạc, thể hiện theo ma trận H thông qua ma trận cột Rk

Không gian trạng thái tuyến tính rời rạc hoá theo thời gian cho các mô hình

trên đƣợc biểu diễn theo hệ phƣơng trình:

(2.2)

Trong đó:

 là trạng thái của hệ thống ở bƣớc k

 là giá trị đo đạc tại thời điểm k

 là nhiễu xử lý tại bƣớc k-1

là nhiễu đo đạc tại bƣớc k 

là ma trận chuyển (transition matrix) của mô hình động 

là ma trận mô hình đo đạc (mesurement model matrix) 

Theo Jouni, Särkkä (2011) [31], sử dụng lọc Kalman cho mô hình trên Ak và Qk

có dạng:

(2.3)

(2.4)

Trong đó: là bƣớc tính.

(2.5)

Với :

 Dự báo - Prediction (kf_predict):

(2.6)

 Cập nhật - Update (kf_update):

(2.7)

Trong đó:

 và là giá trị dự báo trung bình và ma trận hiệp phƣơng sai của trạng thái

riêng biệt tại thời điểm k trƣớc khi có giá trị đo đạc (Tiền nghiệm)

 và là giá trị đánh giá trung bình và ma trận hiệp phƣơng sai của trạng thái

riêng biệt tại thời điểm k sau khi có giá trị đo đạc (Hậu nghiệm)

 là giá trị mới đƣa vào (thặng dƣ đo đạc) tại thời điểm bƣớc k

 là ma trận hiệp phƣơng sai dự báo đo đạc tại bƣớc k

 là hệ số khuếch đại lọc, chính là độ gia tăng cần tìm của mạch lọc Kalman trong

mỗi ƣớc đoán

Đặc thù bài toán ở đây là quản lý giám sát đối tƣợng chuyển động, tức là chƣa

xét đến thành phần điều khiển tác động lên đối tƣợng nhƣ đối với lớp bài toán điều

khiển vật thể chuyển động, ví dụ nhƣ điều khiển máy bay không ngƣời lái. Trong

trƣờng hợp này, ta có thành phần .

(2.8)

Thông thƣờng trong mô hình PVT, vị trí và vận tốc nhận đƣợc từ tính toán của

hệ INS có thể đƣợc coi là các số đo trong bộ lọc Kalman, thực chất là giá trị tính toán

đƣợc từ các trục dịch chuyển và quay của cảm biến INS. Nhƣ vậy cái khó của lọc

Kalman là làm thể nào để mô hình hoá các trạng thái và đo đạc để có đƣợc hai phƣơng

trình của (2.2), tức là biết A, B, H. Các đối tƣợng thực tế thƣờng là các đối tƣợng phi

tuyến, tức là không có phƣơng trình tuyến tính cho A, B mà các thông số này đƣợc

biểu diễn thông qua hàm trạng thái của đối tƣợng. Nhƣ vậy thực tế ta phải sử dụng bộ

lọc EKF (Extended Kalman Filter) và UKF (Unscented Kalman Filter).

2.2. Bộ lọc Kalman mở rộng EKF

Thực tế trong tự nhiên tồn tại nhiều hệ thống phi tuyến, ở đó các tính toán của

bộ lọc Kalman truyền thống không áp dụng đƣợc. Mô hình lọc phi tuyến có nhiễu nhƣ

(2.9)

Trong đó:

 là trạng thái của hệ thống ở bƣớc k

 là giá trị đo đạc tại thời điểm k

 là nhiễu tác động lên đối tƣợng

 là nhiễu đo đạc

 f là hàm (phi tuyến) động học của mô hình

 h là hàm (phi tuyến) véc tơ đo đạc của mô hình

Lý thuyết về EKF dựa trên tuyến tính hoá mô hình phi tuyến quanh điểm tham

chiếu bằng cách sử dụng chuỗi Taylor mở rộng. Áp dụng lọc Kalman cho hệ phi tuyến

EKF sẽ có các bƣớc tính toán giống nhƣ trƣờng hợp tuyến tính với các hệ số tính toán

khác, bằng các triển khai tƣơng tự với các công thức của EKF. Có hai dạng bộ lọc

Kalman mở rộng, ký hiệu là EKF1, EKF2, với xấp xỉ phân tán của trạng thái nhận

đƣợc bởi quan sát , trộn nhiễu Gauss:

2.2.1. Bộ lọc Kalman mở rộng dạng 1

 Dự báo – Prediction (ekf_predict1):

(2.10)

 Cập nhật - Update (ekf_update1):

(2.11)

Trong đó:

và là ma trận Jacobi của hàm f và h, với:

(2.12)

(2.13)

Sự khác nhau giữa hai bộ lọc KF và EKF1 là ma trận và trong KF đƣợc

thay thế bởi ma trận Jacobi và trong EKF1. Giá trị ƣớc đoán

mean và giá trị thặng dƣ của ƣớc đoán cũng đƣợc tính toán khác trong EKF1

theo công thức trên.

2.2.2. Bộ lọc Kalman mở rộng dạng 2

 Dự báo - Prediction (ekf_predict2):

(2.14)

 Cập nhật - Update (ekf_update2):

(2.15)

Trong đó: và là ma trận Jacobi của hàm f và h nhƣ

EKF1 trong công thức (2.12 và 2.13). và là ma trận Hessian

của và , nhƣ sau:

(2.16)

(2.17)

là unit véc tơ của trục tọa độ i, tức là nó có giá trị 1 tại vị trí i, và

giá trị 0 tại các vị trí khác.

2.3. Bộ lọc UKF

Bộ lọc UKF là một dạng lọc ứng dụng cho các đối tƣợng có tính chất phi tuyến,

trong đó sử dụng chuyển đổi UT (Unscented Transform) đƣa ra một xấp xỉ Gauss để

lọc tối ƣu các mô hình lọc phi tuyến theo tính hiệp biến [53, 54]. Các bộ lọc UKF sử

dụng kỹ thuật lấy mẫu bằng cách chọn một thiết lập tối thiểu của các điểm lấy mẫu

(gọi là điểm sigma) xung quanh giá trị trung bình. Những điểm sigma này sau đó đƣợc

tính toán thông qua các hàm phi tuyến, từ đó giá trị trung bình và hiệp phƣơng sai của

ƣớc lƣợng này đƣợc tính toán lại. Trạng thái của hệ thống và các ma trận hiệp phƣơng

sai đƣợc xác định dựa trên giá trị trung bình và hiệp phƣơng sai của các điểm chuyển

đổi sigma. Giá trị trung bình và hiệp phƣơng sai lấy xấp xỉ bằng cách sử dụng một

hàm trọng lƣợng trung bình và hiệp phƣơng sai của điểm sigma kết tiếp.

Bộ lọc đƣợc phát triển dựa trên nguyên lý Sigma-Point Kalman Filters

(Rudolph van der Merwe, 2004) [53], đƣợc diễn giải nhƣ sau:

Hình 2. 4: Biểu diễn hàm trọng lƣợng w (Weighted sigma-points)

Phƣơng trình và các tham số tính toán:

(2.18)

( ) ( )

Hình 2. 5: Tính toán điểm sigma cho lọc UKF

Xây dựng 2L+1 điểm sigma-points, * + bao trọn giá trị trung bình và

Với:

i = 0

i = 1, …, L

hiệp phƣơng sai của biến ngẫu nhiên trƣớc:

̅ (√( )

j = L+1, …, 2L

̅ (√( )

(2.19)

Trong đó:

 hàm trọng lƣợng, đƣợc kết hợp với điểm sigma-point thứ i, với: ∑

 k là tham số tỉ lệ

là cột / hàng (thứ i) của ma trận vuông  (√ ) (√( ) ) , với (√ )

gốc trong ma trận hiệp phƣơng sai.

̅ ∑

Hiệp phƣơng sai và hiệp phƣơng sai chéo của y đƣợc tính nhƣ sau:

∑ ∑

(2.20)

Hình 2. 6: Sơ đồ biến đổi UT

Nguyên tắc các bƣớc tính toán lọc UKF nhƣ sau:

 Khởi tạo:

̂ , - ,( ̂ )( ̂ ) -

, - , ̂ - ̂

(2.21)

) - [

)(

]

 Lặp: k = 1, … ,

 Đặt t = k-1

]

 Tính toán điểm sigma-points:

[ ̂

√

√ ̂

(2.22)

 Cập nhật thời gian (Time-update)

( | ∑ ̂

) |

(2.23)

∑ ∑

( |

)( |

)

 Cập nhật đo đạc (Measurement-update)

| ( | ∑ ̂

( | )( | )

̃ ∑ ∑

( | )( | )

∑ ∑

(2.24)

)

( ̂ ̃  Tham số và kết quả tính toán

̂ ̂

( ) ( ) - , - , ,( )

2.4. Đánh giá các thuật toán lọc Kalman và kết quả mô phỏng

Trƣớc khi ứng dụng phƣơng pháp lọc UKF theo Tool-box trong thực tế cho đối

tƣợng xe bus cụ thể, luận án tiến hành khảo sát đánh giá tính đáp ứng của các bộ lọc

và phát triển Tool-box này với các hàm chặn hạn biên. Trong phần này luận án triển

khai các thuật toán lọc và áp dụng thƣ viện Tool-box trên Matlab sử dụng hàm phát

sine ngẫu nhiên ( ) ( ), với các tham số hệ số biên độ, vận tốc góc và

độ lớn góc thay đổi và đƣợc trộn với nhiễu ngẫu nhiên lớn và hạn biên tín hiệu. Khi

thay thế hàm phát sine này bằng mô hình và tín hiệu thu đƣợc từ các cảm biến gắn

trên xe bus ngoài thực tế ta sẽ vẽ đƣợc quỹ đạo chuyển động thực của đối tƣợng.

Theo mô hình vận tốc Wiener [53, 54], vector trạng thái đƣợc biểu diễn nhƣ

(2.25)

(2.26)

Trong đó:

 là tham số góc của hàm sin tại thời điểm k, với

 là vận tốc góc trong bƣớc tính thứ k, với

 là hệ số biên độ tại bƣớc tính thứ k,

 rk là nhiễu Gaussian ồn trắng đơn biến

Nhƣ vậy, ta có phƣơng trình động học cho mô hình liên tục trong miền thời

gian nhƣ sau:

= (2.27)

Hàm ồn trắng , với các biến và là cƣờng độ xáo trộn

vector vận tốc của nhiễu, trong mô phỏng ta chọn q1 = 0.2, và q2 = 0.1.

Từ công thức (2.3) và (2.27), theo Jouni Hartikainen [31], ta có:

(2.28)

Với: là khoảng cách bƣớc tính, trong mô phỏng chọn .

Từ (2.4), ta có: , với nhiễu ngẫu

nhiên Gauss ta tính đƣợc:

(2.29)

Với mô hình đo đạc ta có:

Đạo hàm vector đo đạc, ta tính đƣợc ma trận Jacobi và Hesian nhƣ sau:

(2.30)

(2.31)

Trong mô phỏng này luận án sử dụng tín hiệu phát hình sine với tần số và biên

độ thay đổi, trộn nhiễu ngẫu nhiên. Trong thực tế, dựa trên đặc điểm vật lý của đối

tƣợng, ta có thể xây dựng các giá trị hay hàm chặn hạn biên nhằm loại bỏ tín hiệu bất

thƣờng trƣớc khi đƣa vào bộ lọc Kalman để giảm giá trị (sai số đột biến) cần xử lý

làm tăng tốc độ tính toán của chip, từ đó tăng khả năng đáp ứng của hệ thống. Cụ thể,

các phƣơng tiện giao thông mặt đất, gia tốc lớn nhất , với . Chọn

, tức là luôn thỏa mãn điều kiện gia tốc lớn nhất . Để đơn giản,

đối với phƣơng tiện giao thông vận tải đƣờng bộ ta tính toán với gia tốc theo hai

phƣơng trên mặt phẳng ngang bị chặn bởi gia tốc , vận tốc bị chặn dƣới

300 km/h. Ta biết tốc độ của xe, trong khoảng thời gian 1 bƣớc tính ta sẽ tính

đƣợc giá trị hạn biên của vận tốc (thực chất là ), tức là khoảng cách giữa

hai điểm đo liên tục trong một bƣớc tính không đƣợc vƣợt quá giá trị này. Nhân giá trị

với thời gian một bƣớc tính ta có giá trị hạn biên của vị trí . Tại thời

điểm giá trị đo mà cộng nhiễu vƣợt quá biên độ ta lấy giá trị đó bằng .

Tính tƣơng tự với sẽ loại bỏ đƣợc những giá trị sai số đột biến (loại giá trị quá

dƣơng hoặc quá âm vƣợt ngƣỡng ). Phƣơng pháp này nhằm loại bỏ việc tính

toán những số liệu sai số bất thƣờng, không những giúp giảm khối lƣợng tính toán mà

còn làm tăng độ chính xác của kết quả do không xử lý bù kết quả tính gần đúng với

kết quả tính sai quá lớn vƣợt ngƣỡng cho phép. Phƣơng án này đƣợc áp dụng cho thiết

kế hệ thống tích hợp GPS/INS với INS 6-DOF ở phần 1 của chƣơng 4.

Trong mô phỏng khảo sát và phát triển Tool-box này, để làm rõ các đặc điểm

của lọc EKF, UKF với các tính chất hạn biên, ta sử dụng hàm phát hình sine với biên

độ, tần số có thể thay đổi và giá trị hạn biên thay vì hàm. Nhƣ vậy tức là với tín hiệu

phát hình sine, ta thay đổi tham số góc, vận tốc góc và biên độ, với giới hạn của mỗi

bƣớc tính phù hợp với mỗi bộ lọc EKF hay UKF. Khi triển khai thực tế, đây chính là

những yếu tố cần thử nghiệm với thiết bị thực để có đƣợc các thông số phù hợp đối

với mỗi đối tƣợng.

Hình 2. 7: Kết quả sử dụng bộ lọc KF và EFK

Hình 2. 8: Kết quả sử dụng bộ lọc UKF

Kết quả mô phỏng với tín hiệu phát trộn nhiễu ngẫu nhiên thu đƣợc cho thấy độ

dao động quanh điểm cân bằng, quỹ đạo luôn có hƣớng trở về bám sát quỹ đạo thực tế

sau khi bị dao động cƣỡng bức. Trên đồ thị (Hình 2.7 và hình 2.8), các điểm chấm

(xanh) là các giá trị đo đạc bị tác động của nhiễu, đƣờng gạch (xanh) là tín hiệu thực,

đƣờng liền (đỏ) là kết quả của bộ lọc. Với bộ lọc EKF biên độ dao động của nhiễu có

những giá trị đƣợc tăng gấp 5 lần ta vẫn thu đƣợc giá trị sau lọc quanh điểm ổn định.

Bộ lọc UKF cho kết quả tốt, sai lệch nhỏ và nhanh chóng trở về ổn định hơn so với

EKF với biên độ dao động cƣỡng bức có thể tăng từ 5 lên tới giá trị 15. Những giá trị

này ngoài thực tế có thể là sai số bất thƣờng của các IMU do các tác động đột ngột của

nhiễu ngẫu nhiên, hoặc khi bị lật trục INS . Kết quả thu đƣợc của bộ lọc UKF

tốt hơn so với bộ lọc EKF. Đây là cơ sở cho việc phát triển thuật toán lọc UKF cho

phƣơng tiện giao thông đƣờng bộ ở chƣơng 4, trong đó sử dụng script predict và

update của thƣ viện này áp dụng cho đối tƣợng thực ở hai chế độ (Mode) hoạt động

với đầy đủ tín hiệu vào (Ts = 1s) và giữa các khoảng cập nhật GPS (Ts = 20 ms) .

2.5. Kết luận chƣơng 2

Đối với các hệ tích hợp GPS/INS đơn giản, tín hiệu đo đạc là INS tần số cao và

tín hiệu tham chiếu là GPS. Khi mất tín hiệu GPS, hệ thống chỉ tính bằng INS có sai

số tích lũy lớn tăng theo thời gian và nhanh chóng sai lệch. Lúc này để sử dụng bộ lọc

Kalman trong hệ thống tích hợp GPS/INS ta cần thêm một hệ thống tham chiếu mới

có tính năng định vị kết hợp với một số hệ thống hỗ trợ khác. Đây là lý do hệ thống

GPS/INS nếu không có các hệ hỗ trợ khác sẽ mất đi khả năng định vị khi mất GPS

kéo dài. Giữa các khoảng thời gian lấy tín hiệu GPS, hoặc trong trƣờng hợp tín hiệu

GPS chập chờn, đối tƣợng có mô hình phi tuyến lớn, bộ lọc UKF với các cảm biến

phụ trợ đƣợc tích hợp thêm một cách phù hợp vẫn có thể xử lý đƣợc trong khoảng thời

gian nhỏ. Với nhiều dạng tín hiệu tham chiếu sẽ có thể áp dụng đƣợc nhiều bộ lọc

Kalman cho từng dạng tín hiệu, tạo ra các mode hoạt động khác nhau.

Tool-box Matlab đƣợc sử dụng trong phần này của luận án đƣợc phát triển với

các bộ chặn hạn biên tín hiệu trên cơ sở phân tích các giới hạn vật lý của đối tƣợng cụ

thể trong thực tế. Đây là những kết quả mới ban đầu đã đƣợc ghi nhận trong báo cáo

tại hội thảo quốc tế AICI2009 (Vol.3, IEEE) tổ chức tại Trung Quốc [63]. Khác với

những kết quả mô phỏng trên lý thuyết sử dụng các tín hiệu phát trộn nhiễu ngẫu

nhiên, các thiết kế và tính toán cụ thể đều phải dựa trên một đối tƣợng có mô hình xác

định với những công thức tính toán và phƣơng trình cụ thể, sử dụng chip vi xử lý cụ

thể với các tần số lấy mẫu nhất định. Ngoài ra hệ thống còn phụ thuộc vào tần số lấy

mẫu của các hệ thống phụ trợ. Trong chƣơng tiếp theo, luận án sẽ đề cập tới thiết kế

chi tiết với các cách xử lý dữ liệu cụ thể.

CHƢƠNG 3. PHƢƠNG PHÁP TỰ ĐỘNG HIỆU CHỈNH MA TRẬN

QUAY CHO HỆ THỐNG INS

3.1. Giới thiệu phƣơng pháp tự động hiệu chỉnh ma trận quay

3.1.1. Nhận định chung

Từ năm 2009, là thời điểm sau khi ra đời các cảm biến tích hợp 6-DOF với đầy

đủ 3 trục Acc và 3 trục Gyro, một hƣớng nghiên cứu mới để hiệu chỉnh các MEMS

INS đƣợc hình thành. Đã có số kết quả bƣớc đầu đƣa ra dƣới dạng lý thuyết [91], hay

dạng tài liệu trao đổi [88]. Cũng có một vài công trình hoàn thiện ở mức độ đơn giản,

nhƣ dự án của Sebastian Madgwick đƣợc bắt đầu từ 2010, hay của Sergiu Baluta trong

dự án dự án Starlino Electronics project [87, 88]. Bắt đầu từ thời điểm này, kết quả

nghiên cứu đã đƣợc triển khai và công bố rộng rãi [92, 93].

Đến cuối năm 2012, với sự ra đời của MEMS INS 9-DOF, phƣơng pháp tự

động điều chỉnh ma trận quay đã có cơ hội để phát triển hoàn thiện. Trƣớc khi đi vào

giải pháp và thiết kế chi tiết, luận án khái quát lý thuyết cơ bản về sử dụng ma trận

quay trong kiểm soát và định hƣớng trên cơ sở các tài liệu trên và nguyên lý về tính

toán ma trận. Từ đó ứng dụng các vector từ trƣờng của INS 9-DOF để hoàn thiện

phƣơng pháp tự động hiệu chỉnh ma trận quay và phát triển giải pháp phân tán trong

thiết kế hệ thống quản lý giám sát phƣơng tiện giao thông vận tải đƣờng bộ, cụ thể là

ứng dụng cho xe bus.

Các vector chỉ phƣơng có thể đƣợc biến đổi giữa các khung tham chiếu xoay

với một ma trận 3x3, gọi là ma trận quay R (Rotation matrix). Kết quả chuyển động

thể hiện trong khung đối tƣợng tham chiếu b-frame, hay còn gọi là khung P (Plane),

so với khung tham chiếu mặt đất e-frame, hay còn gọi là khung G (Ground). Có thể

xoay vector bằng cách nhân chúng với ma trận quay R. Ta quy ƣớc vector Q là vector

đại diện, ví dụ khoảng di chuyển, vận tốc hay gia tốc. Các thành phần của Q đƣợc ký

hiệu bằng các chỉ số theo các phƣơng của các trục tọa độ Qx, Qy, Qz. Vector Q ở khung

P (Plane) là QP, ở khung G là QG. Sự liên hệ giữa hai vector của hai khung tham chiếu

này đƣợc tính toán theo ma trận R nhƣ sau:

] (3.1) [ ] ; [

QG = RQP

Ma trận cosin chỉ phƣơng DCM chính là ma trận quay R (Rotation matrix),

đƣợc thể hiện qua các góc Euler, đƣợc tính toán nhƣ sau:

] [

(3.2)

(3.3)

Ba cột của ma trận R là kết quả biến đổi của ba vector theo ba trục của đối

tƣợng (P) so với khung tham chiếu mặt đất (G). Ba hàng của ma trận R là kết quả biến

đổi của ba vector khung tham chiếu mặt đất (G) so với khung tham chiếu đối tƣợng

(P). Ma trận R chứa tất cả các thông tin cần thiết để thể hiện tính định hƣớng của đối

tƣợng so với với khung tham chiếu trái đất. Ma trận R cũng đƣợc gọi là ma trận cosine

chỉ phƣơng, bởi vì mỗi phần tử là cosine của các góc quay cơ sở roll, pitch và yaw.

Mặc dù có thể thấy rằng có 9 phần tử khác nhau trong ma trận R nhƣng thực sự chỉ có

phần tử 3 độc lập, bởi vì trong sáu thành phần gọi là trực giao thỏa mãn điều kiện ba

vector cột vuông góc với nhau và độ lớn của mỗi vector cột bằng một. Hai quá trình

xử lý chống trôi ma trận quay là Quá trình xử lý trực giao (Orthogonal) và Quá trình

chuẩn hóa hay bình thƣờng hóa (Normalization). Công thức (3.1) và công thức (3.2)

diễn tả làm thế nào để xoay một vector đo đạc trong khung tham chiếu của đối tƣợng

(P) vào khung tham chiếu của mặt đất (G). Công thức này thể hiện sự chuyển đổi giữa

các hệ tọa độ thông qua ma trận cosines chỉ phƣơng. Công thức (3.3) là khai triển của

công thức (3.1), khai triển cụ thể thông qua phép nhân ma trận.

Ma trận quay R mô tả sự định hƣớng của một hệ quy chiếu đối với một hệ quy

chiếu khác. Các cột của ma trận R chứa các vector đơn vị trong một hệ quy chiếu và

có thể đƣợc ánh xạ biến đổi từ các hệ quy chiếu khác. Một vector trong một hệ quy

chiếu này có thể đƣợc chuyển vào hệ quy chiếu khác bằng cách nhân nó với ma trận

quay R. Việc chuyển đổi ngƣợc đƣợc thực hiện bởi các phép nghịch đảo và chuyển vị

ma trận. Vector đơn vị I với chiều dài bằng 1 đƣợc sử dụng trong tính toán tích chấm

(dot product) và tích chéo (cross product). Một nhóm quay là nhóm của tất cả các

phép quay có thể, trong đó bất kỳ hai phép quay nào trong nhóm có thể đƣợc kết hợp

để tạo ra các phép quay khác, và mỗi phép quay có một phép quay nghịch đảo.

Hình 3. 1: Ví dụ mô tả chuyển động của đối tƣợng và đặc điểm của R

Có thể để mô tả hoạt động của quá trình chuyển động của đối tƣợng so với hệ

tọa độ trái đất thông qua một độ dịch chuyển trọng tâm (chuyển động của trung tâm

của trọng lực) và xoay (thay đổi hƣớng quanh trọng tâm của lực hấp dẫn). Chuyển

động định hƣớng của nó đối với trái đất có thể đƣợc mô tả bằng cách xác định một

phép quay quanh các trục. Bằng cách bắt đầu với mốc nhất định (vị trí “chuẩn” đầu

tiên, hay còn gọi là mốc tham chiếu) với định hƣớng theo một hƣớng xác định (xác

định từ la bàn hoặc GPS), vị trí của các đối tƣợng sẽ đƣợc sẽ đƣợc xác định bằng cách

áp dụng các luân chuyển liên tục thông qua các phép nhân ma trận. Bất kỳ vị trí nào

cũng có thể đƣợc mô tả nhƣ một phép quay từ vị trí "mốc chuẩn" này.

Để mô tả sự chuyển động của đối tƣợng, ta cần xác định trạng thái của đối

tƣợng so với một hệ quy chiếu phù hợp, thƣờng là hệ tọa độ trái đất (e-frame), còn

đƣợc ký hiệu là hệ tọa độ G (Ground). Định hƣớng của đối tƣợng thƣờng đƣợc mô tả

bằng ba phép quay liên tục theo thứ tự theo các trục. Các góc quay đƣợc gọi là các góc

Euler, bao gồm góc roll, pitch và yaw. Chuyển động của đối tƣợng thể hiện bởi

chuyển động quay của hệ tọa độ vật thể P so với hệ quy chiếu G này. Chuyển động

của đối tƣợng đƣợc đƣợc diễn tả bằng phép quay trong hệ tọa độ thuận tay phải (two

right – handed coordinate systems) nhƣ sau [88]:

1. Xoay đối tƣợng đối với trục zb của nó một góc ψ

2. Xoay đối tƣợng đối với trục yb của nó một góc θ

3. Xoay đối tƣợng đối với trục xb của nó một góc

Trong đó luôn thỏa mãn các công thức sau:

Hình 3. 2: Quay hệ tọa độ vật thể P so với hệ tọa độ trái đất G

] [

(3.4)

Với:

] (3.5) [

Lý do mà các nghịch đảo của ma trận xoay R (ký hiệu theo cách này là ri,j)

bằng chuyển vị của nó là vì tính đối xứng của các phần tử. Các phần tử của ma trận

xoay là các hàm cosine giữa các cặp trục, một trong khung đối tƣợng P và một khung

mặt đất G. Tính toán nghịch đảo là tƣơng đƣơng với việc trao đổi vai trò của mặt đất

G và khung tham chiếu đối tƣợng P, tƣơng tự việc chuyển vị các hàng và cột.

Quá trình chuyển động của đối tƣợng thể hiện tƣơng đƣơng với việc xoay các

ma trận liên tục. Ta có thể nhân các quay ma trận với nhau ở mỗi lần xoay và có đƣợc

một ma trận xoay cuối cùng tƣơng đƣơng với việc áp dụng tất cả các phép quay liên

tiếp. Cần chú ý tới thứ tự các phép quay, luôn áp dụng các phép quay liên tiếp ở phía

bên trái của các kết quả. Lý do mà chúng ta phải lƣu ý về các chuỗi các hoạt động

nhân khi quay ma trận là phép nhân ma trận là không giao hoán. Do đó thứ tự của các

phép nhân ma trận là rất quan trọng. Điều này tƣơng đƣơng với việc quay các khung

tham chiếu. Ví dụ, nếu có ba ma trận luân chuyển, từ vị trí A đến B, từ B đến C, và từ

C đến D, ta có thể tính toán ma trận liên tiếp sẽ đi từ hƣớng A đến D nhƣ sau:

(3.6)

Trong đó:

RBA = xoay ma trận quay tính từ A tới B

RCB = xoay ma trận quay tính từ B tới C

(3.7) RDC = xoay ma trận quay tính từ C tới D

RDA = xoay ma trận quay tính từ A tới D

Hai khái niệm đƣợc sử dụng hiệu quả trong tính toán ma trận quay R và tính

các phần tử của nó để điều hƣớng là Tích chấm vector (dot product) và Tích chéo

vector (cross product). Tích chấm của hai vector A và B, ký hiệu là , là một tích

vô hƣớng, tính bằng cách thực hiện một phép nhân ma trận, là kết quả của một vector

hàng của ma trận A với vector cột của ma trận B. Nó chỉ ra rằng tích chấm vector tạo

ra một kết quả tƣơng đƣơng với các độ lớn của hai vector và cosin của góc giữa

chúng.

(3.8) , - [ ]

(3.9) | || | ( )

Tích chấm có tính chất giao hoán: A ⋅ B = B ⋅ A

Tích chéo của hai vectors A and B, ký hiệu là , là một vector đƣợc tính

toán theo công thức:

( )

(3.10) ( )

( )

Tích chéo vector là vector vuông góc với cả hai vector và độ lớn của nó tỷ lệ

thuận với độ lớn của các vector và sine của góc giữa chúng. Trong tính toán cập nhật

ma trận quay cần xử lý hiệu chỉnh các tham số của các phần tử để đảm bảo tính chất

vuông góc này của các tích chéo, gọi là xử lý trực giao (Orthogonal).

Với: k là vector đơn vị trực giao của 2 vector u và v (unit vector orthogonal)

Nhƣ vậy ta có:

(3.11) ( ) ( )

| | | || | ( )

Các tích chéo có tính chất phản giao hoán:

Một trong những đặc tính quan trọng của ma trận quay R là tính trực giao của

các vector thành phần, có nghĩa là nếu hai vector vuông góc trong một khung tham

khảo (nhóm quay) thì chúng sẽ vuông góc trong mỗi khung tham chiếu. Ngoài ra, độ

dài của một vector là không đổi trong mọi hệ quy chiếu. Các sai số của INS, chủ yếu

là do lỗi số (Numerical errors) làm vi phạm thuộc tính này. Ví dụ các hàng và cột phải

đại diện cho vector đơn vị có cƣờng độ bằng 1, tuy nhiên do lỗi số có thể gây ra làm

chúng sẽ có giá trị nhỏ hơn hoặc lớn hơn. Quá trình tích lũy nếu không có sự điều

chỉnh sẽ làm cho chúng có thể bị suy biến đến không, hoặc tăng rất lớn. Có thể hình

dung là các hàng và cột luôn phải vuông góc với nhau, tuy nhiên lỗi số có thể làm cho

chúng "nghiêng" với nhau, nhƣ hình dƣới đây:

Hình 3. 3: Lệch trực giao và tích chéo

Ma trận quay có 9 phần tử , hay còn gọi là 9 thành phần (element), trong đó

trên thực tế chỉ có 3 phần tử trong số đó là độc lập. Đặc tính trực giao của các ma trận

quay về toán học có nghĩa là bất kỳ các cặp cột (hoặc hàng) của ma trận là vuông góc,

và tổng các bình phƣơng của các phần tử trong mỗi cột (hoặc hàng) bằng 1. Vì vậy, có

thể hạn chế 9 yếu tố về 6 yếu tố (x, y, z) thông qua các phép biến đổi nhƣ sau:

Nhƣ vậy, một trong những vấn đề cơ bản trong xử lý trôi dữ liệu INS chính là

đảm bảo tính trực giao này. Ngoài sai số trực giao do phép lấy tích chéo, trong hệ

thống INS số luôn tồn tại những sai số đặc trƣng là các lỗi số (Numerical errors). Các

sai số này cũng góp phần gây lên hiện tƣợng lệch trực giao khi tính toán các thành

phần trong ma trận quay, kể cả khi có các tín hiệu chính xác từ các con quay hồi

chuyển (Gyro). Ở các hệ thống INS tƣơng tự, các sai số này sẽ phụ thuộc vào vi xử lý

đƣợc sử dụng, trong đó tần số lấy mẫu (sample rate) và ADC là những yếu tố chính.

Các sai số khác gây ra sai số cho quá trình xử lý dữ liệu của INS bao gồm:

• Lỗi tích hợp: Tích hợp hệ thống số sử dụng một bƣớc thời gian xác định và dữ

liệu đƣợc lấy mẫu tại các tần số lấy mẫu nhất định không phải là ƣớc số của

bƣớc thời gian này. Thông thƣờng các tính toán bỏ qua các khoảng thời gian

lệch khung này. Lỗi này tỷ lệ thuận với gia tốc quay của hệ thống, tức là khi

đối tƣợng đƣợc gia tốc quay nhanh trong khoảng thời gian lệch này.

• Lỗi lƣợng tử hóa: Khi làm việc với hệ thống số cần quan tâm tới các biến/thanh

ghi chứa ghi các giá trị. Các phần tử kỹ thuật số này có độ dài nhất định, vì vậy

sẽ sinh ra một lỗi lƣợng tử khi chuyển đổi tƣơng tự sang số, và sinh ra bất cứ

khi nào thực hiện một phép tính mà không bảo vệ tất cả các bit của kết quả.

3.1.2. Nguyên tắc sử dụng trận quay R trong kiểm soát và định hƣớng

(1). Để kiểm soát độ dốc (pitch) của đối tƣợng, ta cần biết tƣ thế dốc (pitch attitude)

của nó, đại diện bởi phần tử rzx. Bằng cách lấy tích chấm của trục lăn X (roll axis) của

nó với mặt đất theo chiều dọc Z (grond vertical), ta tính đƣợc một trong những cosine

chỉ phƣơng là rzx. Nó bằng với sine của góc giữa trục lăn X và mặt phẳng nằm ngang

(horizontal plane) trong khung trái đất tham chiếu G. Vì vậy, phần tử này của ma trận

là một dấu hiệu trực tiếp kiểm soát độ dốc cho dù trục lăn X của đối tƣợng có song

song với mặt đất hay không, và có thể đƣợc sử dụng trực tiếp trong một vòng phản hồi

để kiểm soát độ dốc. Khi đối tƣợng nằm ngang không dốc, rzx sẽ bằng 0.

(2). Để kiểm soát việc xoay (roll) của đối tƣợng, ta cần phải biết tƣ thế bờ ranh giới

(bank attitude) của nó, đại diện bởi phần tử rzy. Bằng cách lấy tích chấm trong trục dốc

Y (pitch axis) của nó với mặt đất theo chiều dọc Z (grond vertical), ta tính đƣợc một

trong những cosines chỉ phƣơng là rzy. Nó bằng với sine của góc giữa trục dốc Y và

mặt phẳng nằm ngang (horizontal plane) trong khung trái đất tham chiếu G. Vì vậy,

phần tử của ma trận là một dấu hiệu trực tiếp kiểm soát độ xoay cho dù trục dốc Y của

đối tƣợng có song song với mặt đất hay không, và có thể đƣợc sử dụng trực tiếp trong

một vòng phản hồi để kiểm soát độ xoay. Khi đối tƣợng nằm ngang không xoay, rzy sẽ

bằng 0.

(3). Để điều hƣớng (yaw), ta cần biết tƣ thế trệch (yaw attitude) của đối tƣợng với

hƣớng chuyển động mong muốn Z. Giá trị này tìm đƣợc bằng cách lấy tích chéo

(cross product) của trục lăn X của đối tƣợng với một vector chuyển động theo hƣớng

mong muốn. Vector trục lăn X của đối tƣợng là cột đầu tiên của ma trận R. Trong

trƣờng hợp này ta chỉ quan tâm tới các thành phần nằm ngang, vì vậy ta thiết lập thành

phần Z bằng không, có vector đại diện là [rxx ryx 0]. Ta lấy tích chéo của vector đó với

vector hƣớng mong muốn để có đƣợc sine của góc lệch, và lấy tích chấm để có đƣợc

cosine của góc lệch. Điều này đúng ngay cả khi đối tƣợng xoay lộn ngƣợc (up side

down). Để kiểm tra đối tƣợng có bị xoay theo hƣớng ngƣợc lại so với hƣớng mong

muốn (opposite direction) hay không, ta lấy tích chấm (dot product) của trục lăn X với

vector hƣớng mong muốn. Nếu nó là giá trị âm thì đối tƣợng di chuyển lệch hơn 90 độ

so với hƣớng mong muốn, tức là đang di chuyển ngƣợc.

(4). Để kiểm tra đối tƣợng có bị lộn ngƣợc (up side down) hay không, ta kiểm tra các

dấu hiệu của tích chấm của trục trệch Z với phƣơng thẳng đứng. Nếu nó nhỏ hơn 0 thì

đối tƣợng đang bị lộn ngƣợc. Tích chấm của trục trệch Z (yaw axis) của đối tƣợng với

phƣơng thẳng đứng là phần tử ma trận rzz của ma trận xoay R. Khi đối tƣợng đang

chuyển động tƣơng đối ngang bằng (không dốc), phần tử này có giá trị xấp xỉ bằng 1.

Khi đối tƣợng bị lộn ngƣợc, phần tử này có giá trị xấp xỉ khoảng -1. Khi đối tƣợng

đƣợc nghiêng đi sang một bên một góc 90 độ, phần tử này có giá trị 0. Yếu tố này

thƣờng đƣợc áp dụng khi kiểm soát đối tƣợng bay nhào lộn trong không gian.

(5). Để tìm tốc độ quay của đối tƣợng xung quanh trục trái đất thẳng đứng, ta biến đổi

vector quay Gyro vào khung trái đất tham chiếu G, và lấy tích chấm với trục thẳng

đứng. Điều này tƣơng đƣơng với việc lấy tích chấm của hàng thứ ba của ma trận R với

các vector quay của con quay hồi chuyển Gyro. Tỷ lệ quay của đối tƣợng xung quanh

trục trái đất theo chiều dọc bằng .

3.1.3. Phát triển phƣơng pháp hiệu chỉnh mới trên cơ sở MEMS INS đa trục

Trong thời gian trƣớc đây, đã có rất nhiều phƣơng pháp cố gắng giải quyết vấn

đề tích hợp hệ thống INS với hệ thống GPS. Do đặc điểm công nghệ tại các thời điểm

đó chỉ chế tạo đƣợc INS tƣơng tự (Analog) đơn trục nên các phƣơng pháp tính toán rất

phức tạp, nhiều khi chỉ mang tính lý thuyết. Để xử lý dữ liệu tích hợp, các thiết kế

thƣờng sử dụng các dạng lọc Kalman, bao gồm KF, EKF, UKF trên chip vi xử lý. Với

kỳ vọng sự phát triển nhanh của công nghệ vi xử lý có thể giải quyết đƣợc mọi tính

toán phức tạp, nhiều thiết kế còn sử dụng cả các thuật toán thông minh trên cơ sở sử

dụng mạng neuron để hiệu chỉnh dữ liệu. Giờ đây, các nhà thiết kế nhận ra rằng những

thuật toán dựa trên các phƣơng pháp “thông minh” để xử lý tín hiệu tích hợp GPS/INS

trên chip cho các hệ thống đơn trục trƣớc đây đã không còn tính thực tiễn nữa. Ở các

thời điểm đó, họ hy vọng rằng với sự ra đời của các chip với tốc độ tính toán siêu cao

sẽ giải quyết đƣợc vấn đề bù sai số và tính toán thời gian thực cho các hệ thống

MEMS INS đơn trục bằng các thuật toán xử lý cho đối tƣợng phi tuyến phức tạp, mà

không lƣờng trƣớc đƣợc sự phát triển của công nghệ chế tạo MEMS INS. Khi hệ

thống MEMS INS đa trục xuất hiện và bắt đầu từ thời điểm hoàn thiện công nghệ chế

tạo INS 9-DOF, quan điểm này đã đƣợc thay đổi hoàn toàn.

Kể từ khi MEMS INS 6-DOF ra đời (2008), hƣớng nghiên cứu về hệ tích hợp

GPS/INS đã bắt đầu đƣợc thay đổi một cách đáng kể về bản chất. Các giải pháp cho

những hệ thống MEMS INS trƣớc đây tích hợp từ cảm biến rời với các công thức tính

toán dịch chuyển trọng tâm khối INS đã đƣợc loại bỏ. Các hệ thống tích hợp GPS/INS

trƣớc thời điểm này (2008) đã trở nên lạc hậu, không phải chỉ do sự ra đời MEMS INS

mới có nhiều bậc tự do hơn mà là do hƣớng thay đổi của giải pháp xử lý dữ liệu. Thay

vì tính toán giá trị vận tốc và khoảng cách di chuyển từ các tín hiệu đo trực tiếp rồi

dùng lọc Kalman để hiệu chỉnh so với các tín hiệu tham chiếu, chẳng hạn GPS, các

nghiên cứu hiện đại [92] sẽ tập trung vào giải quyết vấn đề xử lý hiện tƣợng trôi dữ

liệu của hệ thống MEMS INS để tính toán dữ liệu trên hệ thống MEMS INS theo cách

tốt hơn thông qua các thuật toán lặp hiệu chỉnh RTL (Return To Load), rồi sau đó mới

sử dụng dữ liệu đã đƣợc tinh chỉnh này tính toán ra các tham số góc roll, pitch, yaw,

từ đó tính ra vận tốc và khoảng cách dịch chuyển.

Tuy nhiên, giải pháp thỏa đáng cho các vấn đề sau đây chƣa đƣợc giải quyết

triệt để:

 Vấn đề nhiễu trong xử lý các góc (Mixing): Sự thay đổi nhanh góc yaw sẽ vi

phạm điều kiện bờ gây nhiễu cho tín hiệu ra của góc pitch, gọi là hiện tƣợng

banking angles. Thêm vào đó, khoảng độ lệch đứng Up-deflection là tham số

cần đƣợc hiệu chỉnh phụ thuộc vào banking angles, là thông số không đo đƣợc

trực tiếp cần đƣợc tính toán và điều chỉnh cũng gây ra sai số.

 Tác động gia tốc: Một gia tốc kế đo gia tốc sẽ chịu tác động của gia tốc trọng

trƣờng. Gia tốc là kết quả của phép chia lực cho khối lƣợng, đƣợc tính bằng

tổng của tất cả các lực nâng, đẩy, kéo, cộng với lực hấp dẫn rồi chia cho khối

lƣợng tổng của đối tƣợng và vật mang. Tất cả các lực khí động học đều ảnh

hƣởng tới gia tốc trong quá trình chuyển động. Nhƣ vậy, gia tốc là một tham số

gây nhiễu và cũng chịu tác động của nhiễu. Đặc biệt, khi đối tƣợng thay đổi rất

nhanh góc chếch lên hoặc xuống trong thời gian ngắn trong khi tăng tốc thì giá

trị đầu ra của một gia tốc kế Acc trong MEMS INS sẽ không thay đổi. Đây là

một trong những nguyên nhân quan trọng gây ra sai số của các MEMS INS.

 Trôi các phần tử ma trận quay R: Các thành phần của ma trận quay bị trôi gây

sai số tích lũy. Các tính toán sử dụng công thức R thông thƣờng sẽ gây sai số

tích lũy cho các phần tử chủ yếu bởi sai số của Gyro, từ đó tính toán góc roll,

pitch, yaw có chứa sai số tích lũy. Nhƣ vậy, các tham số vận tốc và khoảng di

chuyển bị cộng dồn sai số theo thời gian làm sai lệch giá trị tính toán của hệ

MEMS INS.

Giải pháp mới đƣa ra một phƣơng pháp dựa trên nguyên tắc "hoàn toàn tôn

trọng đặc tính phi tuyến của các thành phần quay" trong ma trận quay R và sử dụng

phƣơng pháp bù để hiệu chỉnh, gọi là phƣơng pháp tự động điều chỉnh ma trận quay

R. Một số tài liệu gọi đây là phƣơng pháp Lọc bổ sung ma trận quay DCM [B17, 88].

Mục đích của giải thuật hiệu chỉnh các phần tử cho ma trận quay R là để có những

bƣớc tính toán liên tiếp ổn định thông qua việc kiểm soát hiện tƣợng lệch trực giao,

qua đó kiểm soát hiện tƣợng trôi dữ liệu dẫn đến sai số tích lũy của MEMS INS. Giải

thuật cung cấp khả năng ổn định sử dụng chức năng “Quay lại để nạp” (RTL – Return

To Load). Firmware làm việc dựa trên tính năng RTL thông qua việc hiệu chỉnh các

phần tử của ma trận quay R sau mỗi bƣớc nạp lại (reload). Thuật toán sử dụng tín hiệu

từ các con quay hồi chuyển (Gyro), gia tốc kế (Acc) từ các IMU và thông tin GPS để

hiệu chỉnh dựa trên nguyên tắc sau:

 Tín hiệu từ các con quay hồi chuyển Gyro đƣợc sử dụng là nguồn chính của

thông tin định hƣớng. Các thông số hiệu chỉnh lấy từ tín hiệu tham chiếu khác.

Tùy thuộc vào các tín hiệu tham chiếu này sẽ quyết định cấu trúc của bộ lọc

các phần tử ma trận quay R.

 Các lỗi số trong tích hợp sẽ dần dần vi phạm các hạn chế trực giao mà ma trận

quay R phải đáp ứng. Vì vậy cần thực hiện thƣờng xuyên các điều chỉnh nhỏ để

các yếu tố của ma trận đáp ứng đƣợc các ràng buộc.

 Tổng hợp các sai số, bao gồm các lỗi số (Numerical errors), trôi dữ liệu của con

quay hồi chuyển (gyro drift), và giá trị mức ban đầu của con quay hồi chuyển

(Gyro offset) sẽ làm hệ thống dần dần tích lũy sai số ảnh hƣởng tới quá trình

tính toán các phần tử trong ma trận quay R, từ đó ảnh hƣởng tới kết quả tính

toán của hệ thống INS. Bộ điều khiển PI thƣờng đƣợc sử dụng để triệt tiêu sai

lệch tĩnh của hệ thống. Bộ điều khiển PI đƣợc tổng hợp với mô hình:

( ) ( ) ( )

( ) ( ) ∫ ( )

Giải pháp cho vấn đề là làm tiêu tan sai lệch nhanh hơn so với việc chúng có

thể đƣợc tạo ra thông qua việc sử dụng các vector tham chiếu để phát hiện các sai lệch

tiêu cực. Kết hợp một bộ điều khiển phản hồi tỷ lệ - tích phân (PI) với các tham số Kp

và Ki đƣợc tính toán phù hợp trong quá trình Calib INS, điều chỉnh các sai lệch đƣợc

phát hiện bởi các tín hiệu tham chiếu so với tín hiệu đầu vào Gyro và Acc. Các thông

số này ảnh hƣởng tới thời gian tăng (rise time), độ vọt (overshoot) và thời gian xác lập

(settling time) của đáp ứng. Các tham số Kp và Ki này đƣợc khảo sát phụ thuộc vào

đặc điểm của cảm biến các IMU sử dụng trong hệ thống MEMS INS. Thực tế các

tham số của bộ PI này không cần tới chính xác cao [88], nhất là khi áp dụng đối với

các phƣơng tiện giao thông vận tải đƣờng bộ. Các tham số này đƣợc hiểu chỉnh trong

quá trình tính toán theo hàm trọng lƣợng và hàm chặn hạn biên, phụ thuộc vào đặc

điểm chuyển động đặc biệt của đối tƣợng, áp dụng cho đối tƣợng bay nhào lộn trong

không gian. Tín hiệu GPS đƣợc sử dụng để phát hiện sai lệch góc yaw, và các gia tốc

kế của MEMS INS đƣợc sử dụng để phát hiện góc pitch và roll. Cơ chế này hoạt động

chậm phụ thuộc vào tín hiệu GPS, tức là không hoạt động độc lập đƣợc và sẽ bị phá

vỡ khi hệ thống mất tín hiệu GPS.

Hình 3. 4: Cấu trúc bộ lọc chống trôi các phần tử của ma trận quay William

Premerlani và Sergiu Baluta

Trong lý thuyết DCM của William Premerlani và Sergiu Baluta [88], họ sử

dụng MEMS INS 6-DOF kết hợp với tín hiệu từ GPS. Trong trƣờng hợp mất tín hiệu

GPS, thiết bị sẽ bị mất dữ liệu góc hƣớng. Nhƣ vậy, cập nhật ma trận quay trong thuật

toán DCM từ các giá trị cập nhật omega của họ bị phá vỡ. Hơn nữa, tốc độ nhận dữ

liệu góc hƣớng của GPS so với INS là quá chậm và dữ liệu không phải là rất tốt vì ăng

ten của nó không phải luôn luôn song song với hệ thống tham chiếu G trong thời gian

di chuyển của các đối tƣợng. Với việc sử dụng INS 9-DOF cập nhật các vector từ

trƣờng thay thế, luận án đã đƣa ra giải pháp giải quyết triệt để đƣợc vấn đề này.

3.2. Phát triển phƣơng pháp tự động hiệu chỉnh ma trận quay trên cơ sở MEMS

INS 9-DOF

3.2.1. Nguyên tắc tính toán

Con quay hồi chuyển MEMS INS xoay cùng với đối tƣợng, tạo ra tín hiệu tỷ lệ

thuận với tốc độ quay. Sự liên quan của mức độ thay đổi theo thời gian của các phần

tử trong ma trận cosine chỉ phƣơng DCM đến các tín hiệu con quay hồi chuyển đƣợc

thể hiện thông qua phƣơng trình vi phân phi tuyến. Mục tiêu của giải pháp là để tính

toán ma trận cosine chỉ phƣơng mà không cần thực hiện bất kỳ xấp xỉ vi phạm các yếu

tố phi tuyến của các phƣơng trình này.

Khi biết các điều kiện ban đầu và thứ tự theo thời gian của các véc tơ quay, ta

có thể biểu diễn công thức trực giao (3.11) theo dạng các vector quay nhƣ sau:

( ) ( ) ∫ ( ) ( )

( ) ( )

(3.12)

Trong đó:

r(0) là giá trị khởi đầu của vector, biểu thị vị trí đầu của đối tƣợng

là giá trị biểu thị sự thay đổi trong vector ∫ ( ) ( )

Giải pháp đề ra là xử lý công thức (3.12) cho các hàng hoặc các cột của ma trận

quay R và xem xét chúng nhƣ vector quay. Nhận thấy rằng các vector thể hiện trạng

thái của đối tƣợng mà ta muốn theo dõi và vector quay không đƣợc đo trong cùng một

hệ quy chiếu. Ta muốn theo dõi các trục của đối tƣợng trong khung trái đất tham chiếu

G, nhƣng các phép đo của con quay hồi chuyển đƣợc thực hiện trong khung P. Trong

khung tham chiếu đối tƣợng P, khung trái đất G quay một góc bằng và ngƣợc hƣớng

với chiều quay của đối tƣợng trong khung tham chiếu trái đất G. Vì vậy ta có thể theo

dõi các trục trái đất nhƣ đã tính trong khung đối tƣợng P bằng cách nghịch đảo của

các tín hiệu con quay hồi chuyển. Nhƣ vậy để có đƣợc trạng thái, ta có thể đảo dấu trở

lại và trao đổi các yếu tố thông qua các tích chéo (cross product):

( ) ( )

(3.13) ( ) ( ) ( ) ∫ ( )

Các vector trong công thức (3.13) là các hàng của ma trận R trong công thức

(3.1). Nhƣ vậy, ta có:

(3.14)

( ) ( ) ( ) ( ) ( ) ( )

Tín hiệu tốc độ quay ɷ đƣợc tính toán đƣa ra cần đƣợc điều chỉnh bằng cách bù

phần trôi dạt thông tin thông qua bộ điều khiển phản hồi tỷ lệ - tích phân PI, bù cho tín

hiệu của các con quay hồi chuyển tạo ra để có đƣợc tốc độ quay đúng. Các hệ số của

bộ điều khiển PI đƣợc lựa chọn trên cơ sở phƣơng pháp thực nghiệm trong quá trình

Calib INS. Các tham số này phụ thuộc đặc tính tín hiệu ra của cảm biến cơ sở Acc,

Compass và Gyro, ngoài ra còn phụ thuộc vào hệ thống nhúng sử dụng. Trong thiết kế

của William Premerlani và trong dự án của Starlino Electronics [87, 88], các tác giả đã

khảo sát, Calib các cảm biến cơ sở Acc ADXL345 và Gyro ITG-3200, chọn ra các hệ

số của PI phù hợp để sử dụng phƣơng pháp tự động hiệu chỉnh ma trận quay cho INS

6-DOF kết hợp với GPS, với các tham số nhƣ sau:

Kp_ROLLPITCH = 0.02, Ki_ROLLPITCH = 0.00002;

Kp_YAW = 1.2, Ki_YAW = 0.00002.

Theo lý thuyết của William Premerlani các hệ số này không cần phải lựa chọn

rất chính xác [88]. Luận án này lựa chọn các tham số nhƣ trên vì sử dụng cùng các

cảm biến Gyro và Acc cơ sở, tuy nhiên các hệ số còn đƣợc đƣợc hiệu chỉnh trong quá

trình tính toán. Giải pháp tự động hiệu chỉnh ma trận quay trong luận án sử dụng hai

bộ PI cho điều chỉnh góc Yaw và điều chỉnh góc Roll-Pitch, với tham số đƣợc lựa

chọn kết hợp khi Calib INS. Các hệ số này đƣợc tính toán hiệu chỉnh trong chƣơng

trình, phụ thuộc vào hàm trọng lƣợng w thông qua sử dụng hàm chặn cƣỡng bức

constrain, và hệ số COG trên cơ sở cập nhật số liệu từ la bàn tích hợp trong khung trái

đất tham chiếu. Đây chính là phần phát triển trên cơ sở kế thừa phƣơng pháp của

William Premerlani trên cơ sở cập nhật ba vector từ trƣờng nội tại của INS 9-DOF.

Tín hiệu này là tín hiệu nội tại của INS, không nhƣ trong lý thuyết của William

Premerlani dùng tín hiệu GPS. Điều này giúp INS có thể làm việc độc lập không phụ

thuộc tín hiệu bên ngoài.

Các hệ số này đƣợc phối hợp tính toán với hàm trọng lƣợng trong các vòng lặp

phản hồi hiệu chỉnh. Ở chế độ dao động nhỏ, bất thƣờng, hoặc có gia tốc quá lớn nhƣ

đối với hệ bay, hàm trọng lƣợng đƣợc lựa chọn nhƣ sau:

 Với gia tốc thực < 0.5 G → Hàm trọng lƣợng có giá trị = 0

 Với gia tốc thực = 1 G → Hàm trọng lƣợng có giá trị = 1

 Với gia tốc thực > 1.5 G → Hàm trọng lƣợng có giá trị = 0

Trong trƣờng hợp hệ thống hoạt động bình thƣờng, giá trị của hàm trọng lƣợng

đƣợc tính theo giá trị gia tốc thực, theo công thức giới hạn giải tín hiệu (constrain)

√( ) ( ) ( )

nhƣ sau:

(3.15) ( | | )

Điều chỉnh các tham số cho vector scale để hiệu chỉnh các tham số KpRP,

KiRP theo hàm trọng lƣợng Acc_w đƣợc tính theo hàm constrain nhƣ trên. Với gia tốc

thực, hàm trọng lƣợng có giá trị thực tính toán theo (3.15) nằm trong khoảng giá trị từ

0 → 1.

Nhƣ vậy, giá trị tốc độ quay thực tế sẽ đƣợc tính toán theo công thức:

(3.16) ( ) ( ) ( )

Trong đó:

ɷGyro(t) là giá trị vận tốc góc của gyro 3 trục đo tính toán đƣợc

ɷCor (t) là giá trị hiệu chỉnh vận tốc góc, ɷCorrection

Lặp lại phƣơng trình (3.14) cho mỗi trục trái đất, ta có công thức bù các phần

tử của R theo tín hiệu Gyro dƣới dạng một dạng ma trận nhƣ sau:

(3.17) ] ( ) ( ) [

Trong đó: ;

;

Phƣơng trình (3.15) tích lũy sai số làm tròn và con quay hồi chuyển trôi, lệch

offset sẽ gây lỗi. Kết quả tính toán này trôi một vài độ mỗi giây. Trong phần tiếp theo

ta sẽ sử dụng phƣơng pháp tái chuẩn hóa từ các tọa độ trực giao, sau đó tính toán hiệu

chỉnh để loại bỏ các sai lệch rồi mới quay lại bƣớc (3.17) để cập nhật lại các phần tử

của ma trận quay R.

3.2.2. Tái chuẩn hóa

Quá trình thực hiện hiệu chỉnh để đảm bảo các điều kiện trực giao gọi là "Tái

chuẩn hóa" (Normalization). Có một số cách khác nhau để thực hiện điều này, tùy

thuộc vào nhiều yếu tố nhƣ hệ thống MEMS INS, hệ thống nhúng hay vi xử lý cũng

nhƣ đối tƣợng áp dụng. Trong luận án này, giải pháp đƣợc sử dụng dựa trên góc của

vector từ trƣờng kết hợp với các vector gia tốc của MEMS INS.

Trƣớc hết ta tính toán tích chấm (dot product) của hàng X và Y của ma trận.

Kết quả là một phạm vi sai lệch của hàng X và Y quay so với nhau:

(3.18)

Bố trí một nửa số lỗi cho từng hàng X và Y, và khoảng xoay hàng X và Y theo

(3.19)

hƣớng đối diện với cặp nối chéo, ta có:

Độ lớn của mỗi hàng và cột của ma trận R xấp xỉ bằng 1. Ta chia đều các lỗi

đối với mỗi vector làm cho các sai lệch còn lại sau khi điều chỉnh thu đƣợc sẽ thấp

hơn so với việc để sai lệch hoàn toàn cho một vector.

Bƣớc tiếp theo của giải thuật là điều chỉnh hàng Z của ma trận để trực giao với

(3.20)

hàng X và Y. Thiết lập hàng Z là tích chéo (cross product) của hàng X và Y:

( )

Bƣớc cuối cùng trong quá trình tái chuẩn hóa là tái cấu trúc các hàng của ma

trận R để đảm bảo rằng mỗi thành phần đơn vị đều có độ lớn bằng 1. Một trong những

cách chúng ta có thể làm điều đó là để phân chia mỗi phần tử của mỗi hàng cho căn

bậc hai của tổng các bình phƣơng của các phần tử trong hàng đó. Tuy nhiên, có một

cách dễ dàng hơn để làm điều đó, bởi nhận thấy rằng độ lớn sẽ không bao giờ khác

nhau nhiều hơn 1, vì vậy luận án sử dụng thông qua khai triển chuỗi Taylor mở rộng.

Kết quả tái cấu trúc (Renormalization, ký hiệu là ~) là các phƣơng trình điều chỉnh độ

lớn cho các vector hàng về giá trị 1 nhƣ sau:

( )

(3.21) ( )

)

(

)

(

)

(

( )

(

)

(

)

(

)

(

)

(

)

(

)

(

)

(

)

3.2.3. Hiệu chỉnh trôi dạt góc yaw

Kể cả trong trƣờng hợp con quay hồi chuyển Gyro trong MEMS INS có chất

lƣợng tốt, tùy thuộc vào cảm biến và đặc điểm chuyển động của đối tƣợng, tín hiệu ra

là các góc quay vẫn cần đƣợc đƣợc hiệu chỉnh bù khoảng từ vài độ mỗi giây để hiệu

chỉnh sự trôi dạt của chúng [88, 92]. Công việc cần đƣợc thực hiện là sử dụng hệ tham

chiếu định hƣớng khác để phát hiện sai lệch offset của con quay hồi chuyển và sử

dụng một vòng phản hồi trở lại con quay hồi chuyển để bù cho những sai lệch trong

phát hiện lỗi cho các thông tin trƣớc của Gyro. Quá trình thực hiện theo các bƣớc nhƣ

(1) Sử dụng vector tham chiếu định hƣớng để phát hiện sai lệch. Tính toán một

vector quay tái cấu trúc (renormalized) sẽ đƣa ra các giá trị đo đạc và tính toán

bù cho các vector tham chiếu trong liên kết. Yêu cầu chính cho một vector

tham chiếu định hƣớng là nó không trôi. Tín hiệu góc yaw từ GPS hoặc la bàn

có thể đảm bảo điều này, tuy nhiên các tín hiệu tham chiếu này có tốc độ rất

chậm so với INS. Vì vậy, giải pháp sử dụng INS 9-DOF là cần thiết và khi sử

dụng tín hiệu Mag của INS 9-DOF cần chú ý Calib chuẩn thông số này.

(2) Hiệu chỉnh của vector sai lệch quay trở lại bù tín hiệu thông qua một bộ điều

khiển phản hồi tỷ lệ tích phân PI để tạo ra một sự hiệu chỉnh góc quay cho con

quay hồi chuyển (công thức 3.17). Tùy thuộc vào đặc điểm của Gyro trong

MEMS INS mà áp dụng bộ PI với các hệ số phù hợp.

(3) Tùy thuộc vào quy ƣớc dấu cho các lỗi quay trong thiết kế có thể dƣơng hoặc

âm, giá trị đầu ra của bộ điều khiển PI sẽ đƣợc lấy để hiệu chỉnh với các tín

hiệu thực tế của con quay hồi chuyển Gyro. Kết hợp với giá trị trọng số phù

hợp, ta sẽ tính đƣợc các phần tử cập nhật tái cấu trúc (renormalized) của ma

trận quay R cho bƣớc kế tiếp. Sử dụng các giá trị cập nhật này tính toán góc

roll, pitch, yaw, từ đó tính toán đƣợc giá trị vận tốc và khoảng di chuyển chính

xác hơn. Sự khác biệt giữa hƣớng đối tƣợng đang trỏ và hƣớng nó đang chuyển

động sẽ đƣợc coi nhƣ là một lỗi ở đầu vào để điều khiển điều phản hồi chỉnh

quá trình trôi dạt. Kết quả là ma trận quay R sẽ thích ứng với các tác động bất

thƣờng (gió, nẩy, rung lắc xê dịch ngang…), và chỉnh góc quay của đối tƣợng

một góc cần thiết để giữ cho nó di chuyển dọc theo quá trình mong muốn của

đối tƣợng.

Hình 3. 5: Điều chỉnh trƣợt góc hƣớng (yaw correction)

Trong đó:

xbp: chiếu của xb (P) trên mặt phẳng xy (G)

COG: vector góc hƣớng trên mặt đất (Course Over Ground)

ψ : góc hƣớng đánh giá (estimated yaw)

ψm: góc hƣớng đo đạc (measured yaw)

Lỗi quay giữa góc yaw từ tín hiệu Mag (hoặc la bàn hay của tín hiệu GPS so

với vector nằm ngang trên mặt đất) và giá trị chiếu trên mặt phẳng nằm ngang của trục

lăn X (roll axis) của IMU cho thấy độ lớn của quá trình trôi dạt. Nhƣ vậy, vector từ

trƣờng theo các phƣơng x, y và vector góc hƣớng từ trƣờng (Magnetic Heading), hay

còn gọi là COG (Correction/Course Over Ground) của P so với G đƣợc tính toán nhƣ

)

(3.22)

(

)

} { (

là giá trị COG trong hệ tọa độ trái đất tham chiếu G. Trong đó:

Hiệu chỉnh bù quay là thành phần Z của tích chéo của cột X trong ma trận quay

R và hƣớng vector trên mặt đất. Đầu tiên, ta tạo các vector tham chiếu từ các vector

vận tốc ngang bình thƣờng hóa. Điều này có thể đƣợc thực hiện bằng cách lấy cosine

và sine của góc hƣớng trên góc mặt đất (góc CorrectionGround), trong khung tham

chiếu trái đất G, tính toán theo vector từ trƣờng:

) COGX = = cos(

(3.23) ) COGY = = sin(

Sau đó ta tính toán hiệu chỉnh góc yaw theo công thức:

)

(3.24) ) = rxxCOGY – ryxCOGX Yaw(

(

) )

Tuy nhiên, công thức (3.24) cung cấp sự điều chỉnh góc yaw trong khung trái

đất tham chiếu G. Để điều chỉnh trôi dạt con quay hồi chuyển Gyro drift, ta cần tính

toán vector hiệu chỉnh của đối tƣợng (b-frame) ở khung tham chiếu vật thể P. Giá trị

này nhận đƣợc bằng cách nhân giá trị hiệu chỉnh yaw correction trong khung tham

chiếu trái đất G với hàng Z của ma trận R:

] (3.25) ) [ Yaw( ) = Yaw(

)= Yaw(

] )] [ [ ( ) (

3.2.4. Hiệu chỉnh trôi góc roll-pitch

Gia tốc đƣợc sử dụng cho hiệu chỉnh trôi góc roll-pitch bởi vì chúng không bị

trƣợt. Về nguyên lý, cách một gia tốc kế thông thƣờng hoạt động là nó đo độ võng của

một vật có khối lƣợng nhỏ treo bằng lò xo. Các tần số dao động tự nhiên của gia tốc là

rất cao, do đó nó có thể đáp ứng một cách nhanh chóng. Độ lệch phụ thuộc vào tổng

số lực tác động lên khối treo này, tức là phụ thuộc chủ yếu vào đặc điểm của đối

tƣợng và đặc điểm chuyển động của đối tƣợng. Nguyên lý này đƣợc áp dụng thành

công trong chế tạo ra các cảm biến vi cơ điện tử MEMS với các đặc điểm tƣơng tự.

Có rất nhiều gia tốc kế tốt trên thị trƣờng của các hãng khác nhau. Các gia tốc kế này

không bị phê phán nhƣ con quay hồi chuyển, bởi vì bất kỳ sự sai lệch offset của chúng

không tạo ra một lỗi tích lũy nhƣ con quay hồi chuyển. Phép đo gia tốc là một phép đo

trực tiếp định hƣớng, trong khi một con quay hồi chuyển là một phép đo tỷ lệ thời gian

của sự thay đổi định hƣớng. Vector gia tốc ly tâm tƣơng đƣơng với tích chéo (cross

product) của vector tốc độ quay với vector vận tốc. Coi vector vận tốc của đối tƣợng

song song với phƣơng X trong khung P, sử dụng mặt đất làm hệ quy chiếu G, ta có thể

tính toán các vector vận tốc trong khung tham chiếu P nhƣ là vận tốc trên mặt đất của

đối tƣợng theo hƣớng X. Trong khung tham chiếu P, ta tính toán gia tốc ly tâm

( ) là tích chéo của vector con quay hồi chuyển và vector vận tốc:

(3.26)

[ ]

Giá trị đầu ra thực của gia tốc đƣợc tính bằng giá trị gia tốc do tác động của lực

hấp dẫn trừ đi giá trị gia tốc thực. Để phục hồi một ƣớc tính của lực hấp dẫn đã đƣợc

điều chỉnh cho gia tốc ly tâm, ta cần thêm các ƣớc tính tăng tốc ly tâm. Do đó, đo

lƣờng tham chiếu của trọng lực trong hệ tọa độ vật thể P (b-frame) đƣợc tính bởi:

(3.27)

] [

Ngoài các đo lƣờng tham chiếu của lực hấp dẫn, chúng ta cần một ƣớc tính dựa

trên hàng Z của ma trận cosine chỉ phƣơng R. Đó là sự chiếu của khung tham chiếu G

dọc theo trục của khung tham chiếu đối tƣợng P, đƣợc biểu diễn trên cơ sở việc lật

mặt phẳng bằng phép tính atan2 nhƣ sau:

Hình 3. 6: Hiệu chỉnh góc quay roll-pitch

Trong đó:

(xbe ybe zbe) – Hệ tọa độ vật thể đánh giá (estimated body frame)

(xb yb zb) – Hệ tọa độ vật thể (body frame)

– Vector trục z trái đất đánh giá (estimated earth z vector) zee

ze – Vector trục z trái đất (earth z vector)

) đƣợc tính bằng cách lấy

Nhƣ vậy vector hiệu chỉnh góc quay roll-pitch trong khung tham chiếu P (b-

frame), ký hiệu là hay (

tích chéo (cross product) hàng Z của ma trận quay R với vector tham chiếu trọng lực

bình thƣờng hóa , nhƣ sau:

[ ]

[

(3.28) ]

3.2.5. Phản hồi điều khiển bù sai lệch

Trong trƣờng hợp đối tƣợng di chuyển đảo hƣớng một khoảng rất nhỏ liên tục,

gia tốc của các MEMS INS có thể trở thành bão hòa. Nói cách khác, sự tăng tốc thực

tế có thể nằm ngoài phạm vi xử lý của gia tốc kế. Trong trƣờng hợp này, sai số sẽ

đƣợc phát sinh và tích lũy trong đánh giá các góc quay roll-pitch. Tƣơng tự nhƣ vậy,

các con quay hồi chuyển có thể trở thành bão hòa trong trƣờng hợp đối tƣợng quay đổi

chiều rất nhanh một góc nhỏ liên tục. Có thể hạn chế đƣợc điều này bằng cách sử

dụng khối phản hồi kiểm soát để hạn chế sai lệch đảo hƣớng. Trong thực tế, một

trƣờng hợp cụ thể có thể là khi xe đứng yên nhƣng nổ máy bị rung nhẹ liên tục. Lúc

này có thể Gyro vẫn đo đƣợc rung động trong khi các giá trị gia tốc có thể nằm ngoài

dải đo (thực ra là có giá trị rất nhỏ), hoặc cả hai. Vector hiệu chỉnh góc yaw tạo ra bởi

công thức (3.24) sẽ đƣợc kết hợp với vector hiệu góc chỉnh roll-pitch tính từ các

vector gia tốc Acc và Mag thành một vector tổng hợp sử dụng để bù cho việc trôi dữ

liệu (drift) của MEMS INS.

Vector hiệu chỉnh trôi góc quay (roll-pitch) đƣợc điều chỉnh phụ thuộc vào hàm

trọng lƣợng , sử dụng phƣơng pháp hạn biên phù hợp trên cơ sở hàm giới hạn

constrain rồi đƣợc đƣa vào bộ điều khiển phản hồi PI. Thông tin phản hồi điều khiển

đƣợc tích hợp với tín hiệu ra cơ sở của con quay hồi chuyển Gyro để tạo ra một vector

đúng làm đầu vào của phƣơng trình (3.16). Theo lý thuyết, hệ số này gọi là “Giá trị

làm đúng hoàn toàn” (TotalCorrection, ký hiệu là TCor). Các góc quay roll, pitch và

tại bƣớc tính này sau khi đã đƣợc tính toán hiệu chỉnh với các giá trị

yaw tại bƣớc tính này sẽ đƣợc tính toán lại theo các giá trị cập nhật của các phần tử

ma trận quay

làm đúng hoàn toàn .

(3.29)

Luận án đƣa ra một giải pháp mới ứng dụng cho hệ INS 9-DOF giải quyết

thành vấn đề trôi dữ liệu trong hệ INS. Về cơ bản, sự ra đời của MEMS INS 9-DOF

(2012) đã thay đổi về bản chất hƣớng tiếp cận trên. Kể từ thời điểm này, giải pháp lọc

các phần tử ma trận quay đã có cơ hội hoàn thiện và tạo ra một bƣớc đột phá trong

thiết kế hệ thống tích hợp GPS/INS. Tín hiệu mà INS đƣa ra là tín hiệu của 9-DOF

theo các hƣớng, bao gồm 3 giá trị của góc quay Gyro, 3 giá trị gia tốc Acc và 3 giá trị

so với phƣơng từ trƣờng trái đất Mag. Những giá trị này đƣợc đƣa ra với tốc độ cao tại

cùng một bƣớc tính, đáp ứng tốt với các tín hiệu nhỏ, tuy nhiên khi đối tƣợng chuyển

động MEMS INS vẫn gây ra hiện tƣợng trôi dữ liệu. Phát triển firmware để xử lý vấn

đề này là một giải pháp đang đƣợc đặc biệt quan tâm trong thời gian hiện nay. Trong

luận án này, tác giả phát triển một firmware dựa trên nguyên tắc cập nhật bù trôi dữ

liệu trên cơ sở cập nhật ba vector từ trƣờng. Giải thuật chi tiết đƣợc trình bày theo cơ

chế tính toán nhƣ sau:

Hình 3. 7: Cơ chế của thuật toán lọc các phần tử ma trận quay

Sau đó ta hiệu chỉnh các giá trị đầu ra của con quay hồi chuyển bằng cách thêm

các vector điều chỉnh cho các tín hiệu thô rồi đƣa trở lại vào phƣơng trình cập nhật

xoay R để tính toán lại các phần tử của nó, nhƣ trong công thức (3.17). Tại thời điểm

này, đã hoàn thành đƣợc một vòng tính toán đầy đủ. Ở các bƣớc tính toán tiếp theo ta

lặp lại toàn bộ quá trình tính toán này tạo nên thuật toán hiệu chỉnh các thành phần ma

sau khi đã đƣợc tính toán hiệu chỉnh với các

trận quay. Các góc quay roll, pitch và yaw sẽ đƣợc tính toán lại theo các giá trị cập

nhật của các phần tử ma trận quay

giá trị làm đúng hoàn toàn này.

Nhận thấy rằng trong mô hình tính toán của William Premerlani và Sergiu

Baluta nhƣ trong hình 3.4, tín hiệu GPS có tần số lấy mẫu chậm hơn rất nhiều so với

tín hiệu INS, đôi khi còn bị mất, nên việc sử dụng GPS để tính toán hiệu chỉnh INS

nhiều khi không đáp ứng đƣợc và phá vỡ cấu trúc của thuật toán. MEMS INS đƣa ra 9

loại tín hiệu đồng thời trong cùng một chu kỳ lấy mẫu, tức là có thể tính toán sử dụng

tín hiệu từ ba vector từ trƣờng Mag thay cho góc hƣớng từ GPS để kết hợp với ba

vector Gyro và ba vector Acc trong cùng một chu kỳ tính toán. Vì vậy việc thay đổi

tín hiệu tham chiếu này với thuật toán cập nhật và xử lý dữ liệu phù hợp đã giải quyết

đƣợc triệt để vấn đề cấu trúc bị phá vỡ hay góc hƣớng chậm và không chính xác từ tín

hiệu GPS. Kể từ thời điểm này, với giải pháp xử lý dữ liệu của luận án, hệ thống

MEMS INS số có thể hoạt động độc lập với một firmware xử lý trôi dữ liệu phù hợp.

Các thiết kế cũng nhƣ các giải thuật trƣớc đây sẽ cần biến đổi theo xu hƣớng này.

3.2.6. Kết quả thử nghiệm với MEMS INS 9-DOF

Khảo sát MEMS INS 9-DOF razor stick ở chế độ tĩnh, ta thu đƣợc kết quả rất

khả quan. Các giá trị đo đạc cụ thể đƣợc ghi trong file dữ liệu khá lớn, trong đó một

phần dữ liệu tại các thời điểm tính toán đặc biệt đƣợc liệt kê trong các bảng dữ liệu ở

phụ lục 3.

Mặc dù trong cả hai trƣờng hợp có và không sử dụng thuật toán tự động hiệu

chỉnh ma trận quay trên, dữ liệu đo đạc của các cảm biến Acc, Mag, Gyro đƣa ra có sự

dao động nhẹ một vài độ. Thực chất các giá trị Acc_x, Acc_y, Acc_z; Gyro_x, Gyro_y,

Gyro_z; Mag_x, Mag_y, Mag_z là những tín hiệu đầu ra của các IMU cơ sở, bao gồm

Gyro ITG-3200 (MEMS triple-axis gyro), Acc ADXL345 (triple-axis accelerometer),

và la bàn HMC5883L (triple-axis magnetometer). Các giá trị của các IMU cơ sở

không phụ thuộc vào việc có hay không sử dụng thuật toán tự động hiệu chỉnh ma trận

quay này. Sự khác biệt đƣợc thể hiện rõ ràng khi sử dụng các tín hiệu cơ sở này để

tính toán các góc quay roll, pitch và yaw.

Hình 3. 8: Dữ liệu INS bị trôi khi không sử dụng thuật toán DCM

Trong trƣờng hợp không sử dụng thuật toán hiệu chỉnh ma trận quay, các tín

hiệu góc roll, pitch, yaw của INS bị trôi khá nhanh (Hình 3.8). Các dữ liệu không ổn

định, liên tục tăng/giảm theo thời gian. Các sai lệch này khác nhau đối với từng INS

cụ thể, dù các INS đó đều cùng loại. Với MEMS INS 9-DOF đang khảo sát này, xét trong khoảng thời gian 2 phút, góc roll sai lệch tới 500, góc pitch sai lệch dao động trong khoảng 300 và góc yaw sai lệch tới 1200. Điều này dẫn đến sai số tích lũy lớn

khi tính toán các phần tử ma trận quay, từ đó gây ra sai lệch lớn về vận tốc và vị trí.

Hình 3. 9: Dữ liệu INS không bị trôi khi có sử dụng thuật toán DCM

Nhận thấy rằng việc trôi dữ liệu của MEMS INS 9-DOF này không xảy ra khi

sử dụng thuật toán tự động hiệu chỉnh ma trận quay đã đƣợc phát triển trong luận án.

Cũng với MEMS INS 9-DOF này, khi áp dụng thuật toán DCM của luận án, ở chế độ

tĩnh các giá trị của góc roll, pitch và yaw đều không bị trôi. Giá trị các góc này dao động quanh điểm cân bằng, giá trị biến thiên lớn nhất dƣới 40 đối với góc roll, 60 đối với góc pitch và 60 đối với góc yaw (Hình 3.9). Các giá trị này không bị cộng dồn theo

thời gian, tự động hiệu chỉnh dao động quanh điểm cân bằng và ổn định.

3.3. Kết luận chƣơng 3

Chƣơng này của luận án khái quát lý thuyết cơ bản về sử dụng ma trận quay

trong kiểm soát và định hƣớng trên cơ sở nguyên lý về tính toán ma trận. Đây là

phƣơng pháp đƣợc phát triển trên cơ sở kế thừa phƣơng pháp của William Premerlani

thông qua việc cập nhật và tính toán lại theo ba vector từ trƣờng nội tại của INS 9-

DOF. Sự kế thừa đƣợc thể hiện trong việc lựa chọn hệ số cho bộ PI do cùng sử dụng

IMU Gyro và Acc cơ sở. Tuy nhiên, không chỉ sử dụng các hệ số này một cách cố

định, giải pháp trong luận án có thể điều chỉnh các hệ số của hai bộ PI này một cách

phù hợp trong quá trình chuyển động của đối tƣợng. Thực chất, điểm khác biệt nổi bật

của giải pháp tự động hiệu chỉnh ma trận quay của luận án là không nhƣ trong lý

thuyết của William Premerlani tính toán giá trị góc hƣớng từ tín hiệu GPS, tín hiệu

góc hƣớng trong luận án là tín hiệu nội tại của INS lấy từ IMU tích hợp HMC5883L

(Triple-axis magnetometer) và đƣợc tính toán phù hợp. Nhƣ vậy, trong mô hình của

William Premerlani, cấu trúc tự động hiệu chỉnh ma trận quay kém chính xác hơn và

sẽ bị phá vỡ khi mất tín hiệu GPS. Với sự phát triển của giải pháp trong luận án này,

cập nhật tín hiệu góc hƣớng chính xác giúp INS 9-DOF có thể làm việc chính xác hơn

và độc lập không phụ thuộc tín hiệu bên ngoài.

Điều này có nghĩa rằng với việc cập nhật công nghệ sử dụng MEMS INS 9-

DOF, luận án đã giải quyết đƣợc vấn đề khó khăn đã tồn tại trong suốt thời gian trƣớc

đây, là việc trôi dữ liệu khi thiết kế hệ thống sử dụng INS. Từ đó luận án giới thiệu

một giải pháp mới trong thiết kế hệ thống quản lý giám sát phƣơng tiện chuyển động,

là giải pháp thiết kế dựa trên cơ sở công nghệ phân tán. Công việc sẽ đƣợc phân chia

thành phần việc trên thiết bị chuyển động (đƣợc tích hợp bộ xử lý chống trôi ma trận

quay) và phần việc tại trạm quản lý giám sát (đƣợc tích hợp bộ lọc Kalman). Đây là

một giải pháp hợp lý nhằm tận dụng tối đa khả năng của mỗi thành phần trong hệ

thống. Thiết kế cụ thể của giải pháp này ứng dụng cho đối tƣợng thực tế đã đƣợc chế

tạo và thử nghiệm thành công, đƣợc trình bày trong chƣơng tiếp theo.

CHƢƠNG 4. THIẾT KẾ HỆ THỐNG TÍCH HỢP GPS/INS

4.1. Giải pháp sử dụng lọc và bù dữ liệu cho thiết bị tích hợp GPS/INS

4.1.1. Giải pháp sử dụng lọc Kalman trên thiết bị xe

Các giải pháp trƣớc đây cho hệ thống tích hợp GPS/INS tập chung xử lý dữ

liệu cho đối tƣợng chuyển động. Cảm biến các loại và module thu tín hiệu GPS đều

đƣợc tích hợp trên thiết bị gắn trên đối tƣợng. Các dữ liệu thu đƣợc đƣợc xử lý ngay

trên vi xử lý của thiết bị này. Giải pháp hiệu chỉnh dữ liệu thông thƣờng sử dụng bộ

lọc Kalman hoặc kalman mở rộng EKF. Các dữ liệu sau khi xử lý đƣợc đƣa về trạm

giám sát để hiển thị vị trí của đối tƣợng trên bản đồ số.

Các dạng tích hợp hệ thống có thể đƣợc phân loại bởi mức độ mà dữ liệu của

mỗi thành phần có thể hỗ trợ chức năng cho các thành phần khác trong hệ thống [4].

Khi xét đến ảnh hƣởng của các sai số sau khi đƣợc đánh giá đến giá trị đầu ra, ta sẽ có

phƣơng pháp tích hợp phản hồi và truyền thẳng. Trong phƣơng pháp phản hồi, các

đánh giá sai số trong các thành phần dẫn đƣờng sẽ đƣợc phản hồi ngƣợc trở lại quá

trình cơ học. Trong phƣơng pháp truyền thẳng hoạt động của hệ thống quán tính sẽ

hoạt động độc lập không có hỗ trợ, không quan tâm đến sự tồn tại của bộ lọc cũng nhƣ

là các dữ liệu bên ngoài [1]. Khi kết hợp GPS/INS dựa trên dựa trên kiến trúc xử lý

của hệ thống và mức độ trộn dữ liệu ta sẽ có các dạng tích hợp cặp lỏng, cặp chặt,

TH đầu ra INS + Sai số

CT Cơ học

Đầu ra đã hiệu chỉnh

phân tán và tập trung.

INS

Sai số INS (đánh giá)

Bộ lọc Kalman

Số đo z tham chiếu

Sai số INS - Sai số GPS

Hình 4. 1: Phƣơng pháp truyền thẳng vòng lặp mở

Đầu ra INS + Sai số INS

CT Cơ học

INS

Sai số INS (đánh giá)

Bộ lọc Kalman

Số đo z tham chiếu

GPS

Sai số INS - Sai số GPS

-

Hình 4. 2: Phƣơng pháp phản hồi vòng lặp đóng

Đối với các dạng thiết kế hệ thống tích hợp GPS/INS gắn trên thiết bị chuyển

động dạng này, có hai phƣơng pháp cơ bản xử lý dữ liệu là: tập trung và phân tán.

Trong phƣơng pháp xử lý tập trung, dữ liệu thô của các cảm biến IMU sẽ đƣợc kết

hợp xử lý tại trung tâm để thu đƣợc lời giải về vị trí. Kiểu xử lý này thƣờng đƣợc gắn

với hệ thống tích hợp chặt. Xử lý phân tán là phƣơng pháp tuần tự xử lý, tức là các bộ

xử lý của các hệ thống riêng lẻ sẽ cung cấp lời giải riêng sau đó đƣợc kết hợp với các

mức độ tối ƣu khác nhau bởi một bộ xử lý chủ. Khi thiết kế cách hệ thống tích hợp

GPS/INS dựa trên dựa trên kiến trúc xử lý của hệ thống và mức độ trộn dữ liệu, ta sẽ

CT cơ học

đã hiệu chỉnh

chưa hiệu chỉnh

có các dạng tích hợp cặp lỏng, cặp chặt, phân tán và tập trung.

INS

Pha sóng mang, đốple

Lọc Kalman GPS/INS

và giả mã của GPS

GPS

Hình 4. 3: Sơ đồ tích hợp GPS/INS tập trung vòng mở

CT cơ học

chưa hiệu chỉnh

đã hiệu chỉnh

INS

KF phản hồi

Pha sóng mang, đốple

Lọc Kalman GPS/INS

và giả mã của GPS

GPS

CT cơ học

KF INS

Số liệu chưa hiệu chỉnh

Số liệu đã hiệu chỉnh

Hình 4. 4: Sơ đồ tích hợp GPS/INS tập trung vòng đóng

INS

KF GPS

GPS

Hình 4. 5: Sơ đồ tích hợp GPS/INS phân tán vòng mở

INS

CT cơ học

Số liệu đã hiệu chỉnh

Reset

KF INS

KF phản hồi

GPS

KF GPS

Hình 4. 6: Sơ đồ tích hợp GPS/INS phân tán vòng đóng

Trong triển khai thực tế, nhằm mục đích đáp ứng yếu tố thời gian thực, các

dạng bộ lọc khác nhau đƣa ra các công cụ tính toán khác nhau, chủ yếu áp dụng cho

mô hình PVT (Position Velocity Time) với hệ tích hợp GNSS và INS kết hợp thêm

với các hệ cảm biến khác. Hệ thống tích hợp đƣợc nghiên cứu nhiều nhất là tích hợp

GPS/INS xét với hệ thống liên tục trong miền thời gian (continuos time-invariant state

space-model), có mô hình nhƣ sau [41]:

(4.1)

Áp dụng lọc Kalman lọc sai số cho mô hình PVA này tính toán theo cơ chế sau:

Hình 4. 7: Mô hình bộ lọc Kalman lọc sai số

4.1.2. Phƣơng pháp tính toán trên miền rời rạc

Theo Mark Petovello [41], mô hình PVA (Position Velocity Acceleration) điển

hình xét trong miền s (s-Domain) đƣợc diễn giải theo toán tử Laplace. Tuy nhiên ta có

thể thấy rằng mô hình trên chƣa phản ánh chính xác đƣợc trạng thái thực của hệ thống,

với cả phần biểu diễn rời rạc và liên tục. Mô hình này không sẵn sàng cho diễn giải

các hệ thống số, tức là sẽ rất phức tạp khi khai triển lập trình để tính gần đúng nhất mô

hình toán học với những công thức toán học chi tiết. Đây cũng chính là lý do phát

triển những bộ lọc Kalman mở rộng EKF với các khai triển toán học phức tạp, điển

hình nhƣ Eun-Hwuan Shin, 2005 [19]. Một số mô hình lọc nhƣ UKF, sử dụng mờ,

neuron cố gắng tính toán các thông số phức tạp, khi đƣa vào lập trình trên chip sẽ gặp

trở ngại bởi vấn đề đáp ứng thời gian thực, và khi phát triển thực tế thì thƣờng chỉ áp

dụng với số lƣợng ít các tham số và bỏ qua các thành phần phức tạp.

Bản chất của quá trình đặt vấn đề trƣớc đây là dựa trên những phƣơng pháp

kinh điển xử lý dữ liệu của mô hình liên tục theo thời gian (Time-invariant) trong

miền s. Trong phƣơng pháp này, giải pháp xử lý dữ liệu cho các cảm biến số là tính

toán trong miền rời rạc z (z-Domain), ở đó các trạng thái và nhiễu đƣợc tính toán bởi

các bƣớc rời rạc hóa k và tính toán trạng thái s đƣợc tính toán bởi z. Cơ sở lý thuyết

của biến đổi s-z này dựa trên nguyên tắc IID (Independent and Identically Disbuited

random variables) của Papoulis [49] và Mark Petovello [41], cho kết quả mô hình

PVA nhƣ sau:

Hình 4. 8: Mô hình trạng thái trong miền z

Triển khai tính toán theo mô hình rời rạc hóa trong miền z với vi xử lý tốc độ

cao, ta giả thiết rằng rằng nhiễu trong khoảng thời gian chuyển giữa hai bƣớc tính

là không đổi, khi đó mô hình PVA trong miền z lúc này đƣợc biểu diễn

(4.2)

, -

dƣới dạng:

] [ ] ] [

]

, - , - [ , - , - , - , - [

Trong đó chính là nhiễu số của hệ thống. Thực tế ta sẽ khảo sát nhiễu này

bằng cách để đối tƣợng đứng yên (trạng thái tĩnh) và chuyển động theo một số quỹ đạo

điển hình, đo đạc các thông số đầu ra INS và lƣu lại dƣới dạng bảng sai số theo thời

gian. Đây là tham số ngẫu nhiên, bị ảnh hƣởng bởi dải nhiệt độ làm việc và các thông

số độ lệch bias (acclerator), độ trôi drift (gyro), góc lệch ban đầu . Thông số

này nhân với một bƣớc tính sinh ra sai số gia tốc tính toán vô hƣớng. Khi tính toán cần

xác định giá trị này theo mỗi bƣớc tính tại mỗi thời điểm, từ đó tính ra giá trị bù tƣơng

ứng cho vị trí, vận tốc tại mỗi thời điểm đó theo nguyên tắc xác định độ sai lệch góc

hƣớng của la bàn và góc tính toán từ INS. Lập trình tính toán theo công thức (4.2), với

thành phần bù là các giá trị đƣợc quét theo bảng tham số khảo sát trên cơ sở nguyên lý

mờ theo nguyên tắc Hệ chuyên gia.

Phƣơng pháp tính toán trên miền rời rạc đƣợc phát triển mạnh mẽ bởi các cảm

biến số MEMS INS đƣợc dùng thay thế cho cảm biến rời Gyro và Acc cơ điện. Đề

xuất của luận án về việc hạn biên nhƣ trên giúp giảm các sai số bất thƣờng, tăng tốc

độ tính toán của chip xử lý trong hệ thống tích hợp GPS/INS sử dụng cảm biến số bởi

những sai số bất thƣờng là nguyên nhân chính gây sai số lớn làm lệch quỹ đạo đi xa

khởi quỹ đạo thực. Các hệ thống bị sai lệch rất nhanh sau khi mất tín hiệu GPS bởi vì

sai số tích lũy của INS và cách bù nhiễu cho hệ thống. Việc lấy bù đơn thuần nhƣ các

hệ thống đã đƣợc thiết kế trƣớc đây bị hạn chế do các phƣơng pháp này liên tục lấy bù

mà không loại bỏ sai số bất thƣờng làm cho hệ thống thay vì bù sai số lại tính cộng

dồn cả sai số rất lớn vào.

Ngoài hiện tƣợng trôi số liệu của INS, một nguyên nhân khác nữa gây sai số

lớn trong hệ tích hợp GPS/INS là khi hệ thống đứng yên, nhƣng xe vẫn nổ máy gây

rung lắc. Lúc này MEMS INS vẫn tính toán các giá trị bình thƣờng và tích hợp với dữ

liệu GPS. Các tính toán thông thƣờng luôn cập nhật sai số theo cùng một quy luật mà

không đề cập tới trƣờng hợp này. Bản chất của sai số MEMS INS là sai số tích lũy, vì

vậy những sai số này cộng thêm nhiễu do GPS và sai số tích lũy của INS luôn đƣợc

cộng dồn vào kết quả. Phƣơng pháp xử lý là tạo ra một giới hạn độ lệch vị trí theo

kinh độ và vĩ độ theo thời gian để phát hiện vật thể đứng yên hay chuyển động, từ đó

đƣa ra quyết định ngắt việc bù sai số INS khi hệ GPS phát hiện hệ thống đứng yên

trong khoảng thời gian dài. Điều này hạn chế đƣợc sai số tích lũy của INS trong hệ

thống tích hợp trong trƣờng hợp có tín hiệu GPS và hệ thống GPS phát hiện hệ tích

hợp đứng yên. Lúc này kết quả nếu luôn cộng thêm một đại lƣợng biến thiên có tính

chất tích lũy của hệ INS thì việc tích hợp INS trong trƣờng hợp này lại phản tác dụng,

cần loại tham số bù của hệ INS ra khỏi hệ thống trong trƣờng hợp hệ không di chuyển.

4.1.3. Giải pháp bù dữ liệu

Các thuật toán lọc Kalman đƣợc đề cập ở trên khi đƣa vào thực tế ứng dụng

đều gặp một trở ngại về sự chính xác của mô hình toán học. Với mỗi đối tƣợng khác

nhau cần khảo sát, nhận dạng từ đó đƣa ra phƣơng trình toán học cụ thể áp dụng cho

đối tƣợng đó. Thêm vào đó là cần khảo sát để có đƣợc nhiễu chính xác của bộ điều

khiển gắn trên thiết bị đó. Trên cơ sở này mới lấy đƣợc các hệ số tính toán cho bộ lọc

Kalman P0, A, H, q, r.

Thuật toán tính toán trên miền rời rạc kết hợp với hạn biên tín hiệu đã làm giảm

đáng kể khối lƣợng tính toán so với các dạng bộ lọc Kalman trƣớc đây, làm cho các

thuật toán trở lên thực tế hơn và có thể lập trình triển khai trên chip vi xử lý đáp ứng

đƣợc điều kiện thời gian thực. Trong một số thiết kế trƣớc đây [19] sử dụng phƣơng

pháp nội suy Largrang để tính toán hiệu chỉnh kết quả. Trong thực tế, các giá trị đo

GPS và IMU sẽ đƣợc lấy ra trong các khoảng thời gian khác nhau. Theo cách này, vị

trí và vận tốc của IMU có thể đƣợc nội suy từ các dữ liệu trƣớc đó, sau đó số đo của

GPS sẽ đƣợc lấy ra để tính ra vector . Giả sử rằng các số đo IMU đƣợc lấy ra tại

thời điểm tk-1 và tk còn số đo GPS đƣợc lấy ra tại thời điểm tGPS. Sau đó các công thức

nội suy tuyến tính có thể đƣợc áp dụng để lấy ra vị trí và vận tốc của IMU tại thời

điểm lấy giá trị GPS.

(4.3)

(4.4)

Hình 4. 9: Khoảng thời gian đo lƣờng IMU và GPS

Các ứng dụng có tính động học cao sẽ đòi hỏi một mô hình nội suy bậc cao

đƣợc áp dụng công thức nội suy Lagrang để tính toán lại vị trí:

(4.5)

(4.6)

Vì cả hai cảm biến không thể đƣợc lắp đặt tại cùng một vị trí trên xe mà có một

khoảng cách giữa hai cảm biến nên giá trị vận tốc và vị trí của IMU và GPS sẽ khác

nhau. Ta gọi đó là ảnh hƣởng cánh tay đòn, có tác động phụ thuộc vào vị trí lắp đặt

cảm biến và mỗi đối tƣợng cụ thể. Có thể hạn chế các ảnh hƣởng này bằng cách lắp

đặt cảm biến INS tại tâm của đối tƣợng chuyển động và ăng ten GPS đƣợc đặt trên

trục đứng của cảm biến INS.

Để phát triển hoàn thiện hơn phƣơng pháp này, luận án đƣa ra một phƣơng

pháp mới bù dữ liệu cho đối tƣợng chuyển động trên cở sở áp dụng nguyên lý mờ. Ý

tƣởng của phƣơng pháp là thay vì tính toán dựa trên cơ sở chuỗi Lagrang nhƣ trên,

thuật toán đƣa ra một nguyên tắc bù phù hợp dựa trên cơ sở nguyên lý mờ nhằm mục

đích điều chỉnh giá trị của mỗi bƣớc tính dựa trên đặc thù của cảm biến sử dụng, của

bộ điều khiển và đối tƣợng áp dụng. Bộ điều khiển đƣợc thiết kế theo nguyên tắc

chung này sẽ không đòi hỏi yêu cầu khắt khe về độ chính xác của mô hình toán học

mà sử dụng phƣơng pháp tính toán trên miền rời rạc kết hợp với các luật bù mềm dẻo.

Với mỗi lớp đối tƣợng cụ thể, khi áp dụng bộ điều khiển này ta cần khảo sát, chạy thử

theo các dạng quỹ đạo đặc trƣng khác nhau và trong các điều kiện khác nhau từ đó rút

ra các luật bù và dữ liệu bù phù hợp. Với ý tƣởng này, ta có thể xây dựng đƣợc các bộ

điều khiển rất linh động. Các tâp mờ và các luật điều khiển tƣơng ứng sẽ đƣợc xây

dựng phù hợp với các trƣờng hợp cụ thể cho bộ điều khiển. Khi đƣa vào thực tế, tùy

thuộc vào mỗi đối tƣợng áp dụng ta có thể điều chỉnh cho phù hợp.

Đề xuất này của luận án trên cơ sở phƣơng pháp tính toán trên miền rời rạc và

hạn biên tín hiệu và phát triển một bộ bù mờ cho hệ thống này với giá trị đầu vào là

các độ sai lệch của góc hƣớng và độ sai lệch về tốc độ của xe tính toán đƣợc từ các

thiết bị đo khác nhau, ví dụ nhƣ độ lệch từ tính toán INS so với tính từ tín hiệu GPS.

Độ sai lệch này cũng chính là một trong các yếu tố thể hiện sự thay đổi chuyển động

của vật mang. Từ đó ta quyết định bù sai số cho hệ thống thông qua giá trị trọng số và

độ lệch của bảng tham số bù. Điều này tạo nên sự linh động của bộ điều khiển có thể

áp dụng cho nhiều dạng đối tƣợng khác nhau.

Hình 4. 10: Cấu trúc bộ bù dữ liệu INS trên cơ sở áp dụng nguyên lý mờ

Bộ bù mờ trong trƣờng hợp này hoạt động trên cơ sở chỉ cập nhật đƣợc tín hiệu

góc lệch giữa góc đo đạc của la bàn và góc tính toán của INS. Hai tín hiệu đầu vào của

bộ bù mờ là giá trị tốc độ tính toán từ hệ thống INS và sai lệch góc giữa thông số đo

đạc từ la bàn điện tử và giá trị tính toán đƣợc từ các thông số về góc lệch của cảm biến

INS. Một tín hiệu đầu ra là hệ số quyết định giá trị tham số bù. Yếu tố quyết định chất

lƣợng của thiết bị giám sát ở đây sẽ là miền chia tín hiệu đầu vào, luật bù và bảng

tham số bù, không bị bó hẹp nhƣ các thiết bị giám sát trƣớc đây là phụ thuộc mô hình

thiết bị cụ thể. Trong trƣờng hợp đặc biệt khi tín hiệu GPS chập chờn trong thời gian

ngắn, bộ bù mờ vẫn có thể sử dụng đƣợc theo nguyên tắc trên với một tín hiệu đầu vào

là sai lệch góc hƣớng, tuy nhiên cần điều chỉnh các luật cho phù hợp thông qua khảo

sát thực tế với mỗi đối tƣợng cụ thể. Thực chất trong cả hai trƣờng hợp này, hệ thống

hoạt động nhƣ một hệ chuyên gia phù hợp với mỗi mô hình đối tƣợng cụ thể.

Phần này trong luận án triển khai tính toán trên miền rời rạc, là một phƣơng

pháp có thể triển khai trên thực tế cho các chip vi xử lý thông dụng đáp ứng đƣợc đòi

hỏi về tốc độ tính toán. Kết hợp với các yếu tố hạn biên tín hiệu, tạo một miền giá trị

gốc và quét tín hiệu đầu ra để kết luận về trạng thái của thiết bị, từ đó đƣa ra phƣơng

pháp bù phù hợp với trạng thái đó. Những yếu tố này làm giảm đáng kể các số liệu

cần tính toán, giúp làm trơn quỹ đạo và kết quả đáp ứng đƣợc yêu cầu thời gian thực.

Sản phẩm của luận án đã đƣợc triển khai thiết kế chế tạo, chạy thử nghiệm bƣớc đầu

đƣa vào thực tế. Các kết quả đã đƣợc ứng dụng trong đề tài của dự án giáo dục đại học

giai đoạn 2, mã số EEC8.13, và báo cáo tại hội thảo toàn quốc về Cơ-Điện tử

VCM2012 [P1.2].

4.2. Đề xuất giải pháp phân tán cho hệ thống tích hợp GPS/INS

Các giải pháp thiết kế hệ thống tích hợp GPS/INS trƣớc đây xử lý dữ liệu chấp

nhận sai số tích lũy của INS và sử dụng bộ lọc Kalman hay các cơ chế bù để hiệu

chỉnh kết quả. Các cảm biến, INS và module thu tín hiệu GPS đƣợc gắn trên đối tƣợng

chuyển động. Dữ liệu thu đƣợc sẽ đƣợc xử lý và lọc ngay trên vi xử lý của thiết bị

này. Dữ liệu sau khi lọc đƣợc đƣa về trạm giám sát để hiển thị. Điều này gây khó khăn

trong vấn đề thời gian thực đối với vi xử lý của thiết bị trên xe, trong khi thiết bị tại

trạm là những máy tính mạnh thì chỉ làm công việc đơn giản là hiển thị bản đồ số.

Phần tiếp theo giới thiệu một giải pháp công nghệ hoàn toàn khác biệt với những ý

tƣởng mới là xử lý phân tán dữ liệu. Giải pháp này chia công việc lọc hiệu chỉnh

chống trôi dữ liệu cho MEMS INS cho thiết bị trên xe, giúp cho hệ thống MEMS INS

có khả năng tự chỉnh để hoạt động độc lập đƣợc, và đƣa xử lý lọc Kalman về trạm.

Cơ sở cho ý tƣởng theo nhƣ phân tích ở chƣơng 3 nhằm chống trôi INS dựa

trên sự đầy đủ các thông tin Gyro, Acc, Mag tại cùng một gốc tọa độ là tâm của đối

tƣợng tại cùng một thời điểm tính toán, từ đó dựa vào các yếu tố trực giao để tính toán

tái cấu trúc (Orthogonal) các vector và nguyên tắc bình thƣờng hóa (Normalized) để

tính toán lại các phần tử cho ma trận quay R. Các kết quả này đƣợc sử dụng để tính

toán lại các góc quay cơ sở, vận tốc và khoảng di chuyển dựa trên các quy luật khác

nhau. Những tiền đề cho ý tƣởng hiệu chỉnh chống trôi dữ liệu INS chỉ đƣợc hình

thành từ sau năm 2009 khi INS 6-DOF ra đời, tuy nhiên ở thời điểm này các hệ thống

MEMS INS chƣa thể phát triển đƣợc khả năng tự hiệu chỉnh. Tham số hiệu chỉnh góc

hƣớng đƣợc cập nhật với tín hiệu la bàn [91] hoặc lấy từ GPS [88]. Phƣơng pháp này

chỉ đƣợc thực sự phát triển mạnh mẽ từ cuối năm 2012 khi INS 9-DOF ra đời với đầy

đủ thông số góc hƣớng theo các phƣơng từ trƣờng trái đất.

Luận án đã phát triển phƣơng pháp tự động hiệu chỉnh ma trận quay và ứng

dụng thiết kế hệ thống trên cơ sở giải pháp phân tán. Giải pháp mới trong luận án

đƣợc thể hiện rõ trong ứng dụng thực tế với yêu cầu thiết kế một hệ tích hợp GPS/INS

mới trên cơ sở sử dụng các thiết bị MEMS INS 9-DOF tiên tiến, áp dụng bộ lọc các

thông số cho ma trận quay R và hoàn thiện thuật toán lọc UKF tại trạm. Cụ thể, công

việc bao gồm: phát triển phần cứng và hoàn thiện chƣơng trình phần mềm cho hệ

thống nhúng của thiết bị gắn trên xe sử dụng hệ tích hợp GPS/INS với phƣơng pháp tự

hiệu chỉnh ma trận quay; hoàn thiện module phần mềm giám sát định vị trên cơ sở sử

dụng bộ lọc UKF cho đối tƣợng phi tuyến với các chế dộ hoạt động khác nhau có thể

thay đổi linh động theo tín hiệu đầu vào của hệ thống, áp dụng cho phƣơng tiện giao

thông vận tải đƣờng bộ là xe bus.

4.3. Thiết kế hệ thống tích hợp GPS/INS với MEMS INS 9-DOF và bus CAN

Hệ thống tích hợp GPS/INS trong luận án đƣợc xây dựng từ các thiết bị hiện

đại, bao gồm: MEMS IMU 9-dof Razor, GPS module EM408, hệ thống nhúng

Arduino R3 cho phiên bản thứ nhất và Arduino YUN cho phiên bản thứ hai. Ngoài ra

nó còn đƣợc tích hợp kết nối với sản phẩm chuyên dụng nhƣ Rada AC20TRW và

datalogger CTAG. Để hoàn thiện thuật toán lọc UKF áp dụng cho phƣơng tiện giao

thông vận tải, ngoài các thông số đo đạc của GPS và INS, thiết kế cần có thêm các

thông số về vận hành xe làm các thông số tham chiếu. Đƣợc sự cho phép của công ty

Trentino Trasporti, tác giả đƣợc phép truy cập vào hệ thống xe bus CAN theo chuẩn

J1939 FMS để có đƣợc một số thông số chuyển động của xe bus Van Hool A330 và

MAN Lion's City Hybrid A37. Các thông số đƣợc phép truy cập và xử lý bao gồm:

Vận tốc bánh xe; Gia tốc; Khoảng cách di chuyển; Hƣớng di chuyển; Tốc độ đồng hồ;

Tốc độ động cơ; Nhiệt độ môi trƣờng xung quanh; Tỷ lệ nhiên liệu thiêu thụ; Mức độ

tiết kiệm nhiên liệu; Trạng thái cửa; Vị trí Pedal; Tình trạng phanh xe; Gia tốc đạp

phanh; và một số thông số khác. Mô hình này có đầy đủ các thông số để phát triển các

dạng bộ lọc Kalman.

Với việc tích hợp radar và sự cho phép truy cập bus CAN, luận án đã có thêm

hệ thống tham chiếu để phát triển thành công một hệ tích hợp GPS/INS mới và thuật

toán giám sát UKF với khả năng xử lý thành phần phi tuyến cho mô hình đối tƣợng và

tuyến tính cho thành phần đo đạc. Thiết bị gắn trên xe có khả năng tiếp nhận các

thông tin để xác định vị trí và theo dõi trạng thái vận hành của xe bus. Vi điều khiển

trong hệ thống sẽ truy cập sử dụng tín hiệu từ bus CAN và tín hiệu từ cảm biến laser

để hiệu chỉnh cho INS, quản lý các thông số vận hành xe nhƣ vị trí, tốc độ, gia tốc,

khoảng di chuyển, cao độ, góc dốc của xe và một số thông số khác. Dữ liệu đƣợc tổ

chức thành các file độc lập tại datalogger và đƣa về trung tâm quản lý giám sát.

Hình 4. 11: Khái quát đối tƣợng và thiết bị sử dụng

Cấu trúc phần cứng các thiết bị hợp thành của hệ thống lắp đặt trên xe bao gồm

các thiết bị cơ bản sau :

Hình 4. 12: Các thiết bị hợp thành cơ bản

Datalogger là khối lƣu xử lý và lƣu trữ dữ liệu trung tâm, có thể xử lý thông tin

liên lạc với nhiều thiết bị chủ yếu dựa trên giao thức CAN. Tại đây có thẻ nhớ để lƣu

trữ dữ liệu GPS cũng nhƣ các dữ liệu vận hành xe. Có một số loại Datalogger khác

nhau, luận án này chọn Datalogger CTAG, trên cơ sở đã hoạt động cho kết quả tốt

trong giai đoạn đầu của dự án euroFOT [86]. Datalogger này có thể xử lý 4 đầu vào và

một đƣờng CAN nối tiếp. Ngoài ra nó còn có một khe cắm thẻ nhớ SD/SDHC, có khả

năng nhận dữ liệu GPS trực tiếp và có khả năng giao tiếp GPRS cũng nhƣ các chức

năng thực thi các kịch bản đơn giản bằng ngôn ngữ Python.

MEMS INS Razor 9-DOF Sensor Stick là một cảm biến loại nhỏ, có chất lƣợng

cao với 9 bậc tự do (DOF). Nó đƣợc tích hợp bởi cảm biến gia tốc Acc 3 trục

ADXL345, từ kế Mag 3 trục HMC5883L, và con quay hồi chuyển Gyro 3 trục MEMS

ITG-3200. Giao thức truyền tin đƣợc sử dụng là I2C, đƣợc thiết kế với 4 chân tiêu

chuẩn giúp dễ dàng tháo lắp và Calib. Thiết bị MEMS INS 9-DOF này ra đời đã thay

đổi cách tiếp cận hệ thống INS, tạo ra một làn gió mới trong thiết kế chế tạo các sản

phẩm dựa trên công nghệ MEMS INS. Thiết kế hệ thống từ lúc này không cần tích

hợp thêm la bàn rời, cũng nhƣ việc loại bỏ đƣợc các công thức toán học rắc rối để xử

lý các thông số đơn trục thành đa trục đã nâng cao khả năng tính toán cho vi xử lý.

Những yếu tố này làm cho các công thức toán học tính toán hệ thống INS trở nên khác

biệt, việc xử lý dịch trục về tâm hệ thống và việc bù sai số cho các hệ INS đơn trục trở

nên không còn nhiều ý nghĩa nữa.

Khối xử lý tín hiệu INS sử dụng trong thiết kế mới này là bo mạch Arduino R3

cho phiên bản thứ nhất và Arduino YUN cho phiên bản thứ 2. Đây cũng là một giải

pháp mới trong thiết kế các sản phẩm nhúng với thiết kế tiêu chuẩn module hóa và mã

nguồn mở. Hiện tại rất nhiều sản phẩm nhúng đƣợc khuyến khích phát triển trên nền

tảng Arduino, đặc biệt là các hệ thống nhúng sản xuất tại Italia. Arduino là một sản

phẩm nhúng phát triển trên nền tảng bo mạch tích hợp mã nguồn mở. Arduino có thể

đƣợc sử dụng để phát triển các đối tƣợng tƣơng tác độc lập hoặc có thể đƣợc kết nối

với phần mềm trên máy tính tiêu chuẩn, ví dụ nhƣ Flash, Processing, MaxMSP. Đây

là các IDE mã nguồn mở, hiện nay có thể đƣợc tải về miễn phí cho cá hệ thống Mac

OS X, Windows và Linux. Arduino R3 là một bo mạch mới ra đời năm 2013, sử dụng

vi xử lý ATmega16U2 để cho phép truyền tải dữ liệu với tốc độ nhanh và bộ nhớ lớn.

Bo mạch Arduino R3 cũng có thêm hai chân mở rộng SDA và SCL bên cạnh chân

Aref. Cấu trúc này làm cho nó dễ dàng hơn trong thiết kế phần cứng, giúp ngƣời sử

dụng có thể thiết kế các shield của mình sử dụng MEMS INS 9-DOF Sensor dựa trên

tiêu chuẩn I2C một cách thuận lợi hơn.

GPS Module sử dụng trong thiết kế là loại US GlobalSat EM408 GPS module.

Đây là module chế tạo trên cơ sở chipset StarIII. Ngoài ăng ten nối ngoài, mỗi module

đƣợc tích hợp bên trong một ăng ten và một quả pin cho phép nhanh chóng tự động

chỉnh định dữ liệu thu đƣợc từ vệ tinh. Mỗi module có 20 kênh, mỗi kênh có độ nhạy -

159dBm. Theo nhƣ quảng cáo chào bán thiết bị, module này đƣợc cho là đủ khả năng

để bắt đƣợc tín hiệu khóa vệ tinh trong hẻm núi, trong khu vực đô thị, hoặc thậm chí

trong một số tòa nhà. Một cable dẫn năm chân đơn giản đƣợc thiết kế làm dây cấp

nguồn, tích cực module và dẫn tín hiệu, trong đó hai chân tín hiệu TX và RX theo

chuẩn nối tiếp UART để sử dụng cho các câu NMEA hoặc dịch nhị phân SiRF.

Thiết bị đo giãn cách cự ly và phát hiện đối tƣợng di chuyển sử dụng trong hệ

thống là Radar TRW AC20. Đây là một sản phẩm thƣơng mại giá thành cao, có độ

chính xác cao, có khả năng xử lý thông tin nội bộ và gửi chúng đến vi xử lý trung tâm.

Radar TRW AC20 theo dõi hoạt động tốt trong các điều kiện thời tiết và trên phạm vi

bán kính quét từ một vài xăng-ti-mét tới 200 mét. Vật thể di chuyển có thể đƣợc theo

dõi một cách chính xác khi chúng đi qua phạm vi bán kính quét sóng của Radar TRW

AC20. Radar này có thể theo dõi đến 8 track. Trong luận án này, tần số lấy mẫu của

radar, GPS, FMS, và IMU lần lƣợt là là 40ms, 1s, 20ms và 20ms.

Hình 4. 13: Xử lý dữ liệu trong hệ thống

Hình 4. 14: Thiết bị thực tế và lập trình cho thiết bị

Hình 4. 15: Vị trí gắn Radar và triển khai lắp ráp thiết bị trên xe bus

4.4. Ứng dụng phƣơng pháp hiệu chỉnh các phần tử ma trận quay trong phát

triển firmware cho hệ thống nhúng trên xe bus

Giám sát một đối tƣợng chuyển động là tính toán các thông số chuyển động của

đối tƣợng ở hệ tọa độ vật thể P (b-frame) so với một hệ quy chiếu G (Global), thƣờng

là hệ tọa độ trái đất G (Ground hay e-frame). Giải pháp trong luận án quy ƣớc hệ tọa

độ vật thể là Oxyz, có các vector đơn vị là (i, j, k) và hệ quy chiếu (NWU) là OXYZ,

có các vector đơn vị là (I, J, K). Trong hệ tọa độ NWU này, I là vector chỉ hƣớng bắc,

K là vector chỉ thiên (Zenith vector) và J là vector chỉ hƣớng tây. Hệ tọa độ vật thể P

quy ƣớc theo cảm biến IMU, vì vậy trong thiết kế cần bố trí chiều của IMU một cách

phù hợp. Luận án lựa chọn hệ tọa độ hiển thị hƣớng các trục X, Y, Z tƣơng ứng so với

hình ảnh của chip vật lý hoặc thiết bị, các trục X, Y, Z nhƣ trong tài liệu kỹ thuật của

hãng GitHub. Tuy nhiên điều này tùy thuộc vào từng cảm biến cụ thể và cần đƣợc

điều chỉnh trong quá trình Calib.

Hình 4. 16: Hệ tọa độ vật thể P và hệ tọa độ quy chiếu G (NWU)

Hệ tọa độ quy chiếu G (NWU) có vector đơn vị (I, J, K) và hệ tọa độ vật thể P

(b-frame) có vector đơn vị (i, j, k) có thể đƣợc biểu diễn nhƣ sau:

IG = {1,0,0} T, JG={0,1,0} T , KG = {0,0,1} T (4.7)

iB = {1,0,0} T, jB={0,1,0} T , kB = {0,0,1} T (4.8)

Đối với hệ tọa độ NWU, vector gia tốc trọng trƣờng hƣớng về tâm trái đất sẽ có chiều ngƣợc với vector Zenith (KB). Ba trục đầu ra gia tốc là A = {Ax , Ay , Az}, và giả

sử rằng không có gia tốc ngoài tác động hoặc đã đƣợc hiệu chỉnh thì ta có thể ƣớc tính KB = - A. Tƣơng tự với từ trƣờng, với M = {Mx , My , Mz }, IB chỉ cực bắc, ta có IB = M, JB = KB IB. Nhƣ vậy từ các vector gia tốc, góc quay và vector từ trƣờng, ta có thể

tính đƣợc ma trận quay R, biểu diễn qua các góc Euler gọi là DCM, thể hiện thông qua DCMB và DCMG, với: DCMG = DCMBT = [IB, JB, KB]T

, - [ ] [ ]

( ) ( ) ( )

( ) ( ) ( ) ( ) ( ) ( ) (4.9)

, - [ ] [ ]

( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )

(4.10)

Nhƣ vậy, sự chuyển động của hệ tọa độ vật thể so với hệ quy chiếu thể hiện ở

G} T = { I.i, J.i, K.i}T G} T = { I.j, J.j, K.j}T , G} T = { I.k, J.k, K.k}T

khoảng các dịch chuyển r với vector đơn vị:

G , iy G , jy G , ky

G , iz G , jz G , kz

iG = {ix jG= {jx kG= {kx

B} T ;

Khoảng di chuyển của đối tƣợng đƣợc tính toán nhƣ sau:

B, ry

B, rz

G } T ;

rB= { rx

G , ry

G , rz

rG = { rx

] [ (4.11) [ ] [ ]

Trong đó: DCM là ma trận cosine chỉ phƣơng (Direction Cosine Matrix), thể hiện sự

quay giữa hai hệ tọa độ.

0, và từ giá trị

Tại thời điểm t0 ta biết vector chỉ thiên của hệ tọa độ vật thể KB

Gyro đƣa ra ta sẽ tính toán đƣợc vận tốc góc quay ɷ = {ɷx , ɷy , ɷz }. Với ɷg là vận

1G so với phƣơng chỉ

tốc góc tính toán đƣợc từ Gyro, dθg là góc dịch chuyển, ta có: dθg = dt ɷg. Nhƣ vậy trong khoảng thời gian dt hệ tọa độ vật thể quay một góc nhỏ KB

thiên (Zenith), và vị trí tiếp theo của vector chỉ thiên sau khoảng thời gian dt là:

1G ≈ KB

0 + v.dt = KB

0 + dt (ɷg KB

0) = KB

0 + ( dθg KB

KB (4.12)

1A từ các giá trị đọc ra của

Tƣơng tự với Gyro, ta tính toán với vector gia tốc KB

cảm biến gia tốc của IMU. Từ giá trị này ta tính đƣợc giá trị của vận tốc góc ɷa và

khoảng thay đổi dθa = dt ɷa

0 va / | KB

0|2

(4.13) ɷa = KB

1A - KB

0) / dt, trong đó KB

0 là vector đơn vị có | KB

0|2 = 1, ta có:

Với: va = (KB

0 (KB

1A - KB

(4.14) dθa = dt ɷa = KB

IMU đƣợc sử dụng trong thiết kế này là loại MEMS 9-DOF Razor có ba trục từ

trƣờng. Từ các thông số của ba trục này, góc dịch chuyển của vector từ trƣờng đƣợc

tính toán theo công thức:

0 (IB

1M - IB

(4.15) dθm = dt ɷm = IB

Đánh giá theo hàm trọng lƣợng: dθ = (sa dθa + sg dθg + sm dθm) / (sa + sg + sm),

ta cập nhật ma trận DCM1 theo công thức:

1 ≈ IB

0 + ( dθ IB

0) ,

1 ≈ KB

0 + ( dθ KB

KB (4.16)

1 ≈ JB

0 + ( dθ JB

Phát triển trên hệ thống nhúng Arduino, luận án sử dụng hàm ghìm constrain

để xử lý hàm trọng lƣợng, kết hợp với các hệ số Kp, Ki đã đƣợc khảo sát phù hợp

(chƣơng 3), từ đó tính ra giá trị điều chỉnh cho các góc quay. Nhƣ vậy tại thời điểm t0

1. Áp dụng tƣơng tự cho các bƣớc tiếp theo, với ký hiệu TCT là quá

1, KB

1, JB

có ma trận DCM0 , tại thời điểm t1 ta tính đƣợc ma trận DCM1 với sự cập nhật của các vector IB

trình tái cấu trúc Normalizeed[a] , biểu diễn vector đơn vị đồng hƣớng với vector a,

giải thuật của luận án thực hiện chƣơng trình hiệu chỉnh trên cơ sở nguyên lý lọc bổ

sung RTL.

Ký hiệu Err đặc trƣng cho sai lệch (Error) trong tính toán hiệu chỉnh trục, ta

tính toán sai số theo công thức:

1 JB

1 ) / 2

(4.17) Err = ( IB

B ITG 1

 Tính toán trực giao:

’ = IB ’ = JB

B JTG 1

1 – Err JB 1 1 – Err IB

’

B KTG 1

B ’ = ITG 1

B ’ JTG 1

(4.18)

 Tính toán tái cấu trúc:

’] =

B ITCT 1

B ’’ = TCT[ITG 1

’

B = 0.5 (3 - ITG 1

B ’ ITG 1

B ’) ITG 1

’] =

B JTCT 1

B ’’ = TCT[JTG 1

’

B = 0.5 (3 - JTG 1

B ’ JTG

B ’) JTG 1

’] =

(4.19)

B 1

B ’’ = TCT[KTG 1

’

B = 0.5 (3 - KTG 1

’ KTG

B 1

’) KTG

B 1

KTCT

Trong đó:

TCT: là quá trình tái cấu trúc (renormalization)

TG: là quá trình tính toán trực giao (orthogonal)

 Tính toán cập nhật:

Tính toán giá trị cập nhật các phần tử ma trận quay R theo mỗi bƣớc tính của

Omega_Vector, trong đó là phần tử tƣơng ứng Corrected Gyro_Vector data:

(4.20)

rồi tính toán các góc Euler:

)

Từ giá trị cập nhật các phần tử ma trận quay trên, ta cập nhật lại ma trận quay

(

)

(4.21) (

(

Lặp lại quá trình tính toán các phần tử của ma trận quay R tại các thời điểm sau

mỗi bƣớc xử lý tín hiệu của INS, ta có ma trận cosine chỉ phƣơng DCM đƣợc tổ chức

’’), KTCT (Kreormalized

’’), JTCT (Jreormalized

B i

lại, cập nhật thay đổi phù hợp với các phần tử đã đƣợc hiệu chỉnh theo từng bƣớc tính ’’) từ các vector tái cấu trúc ITCT (Ireormalized

theo công thức (4.19) tƣơng ứng.

Các phần tử trong ma trận quay R sẽ không bị trôi gây sai số tích lũy bởi vì các

vector này đƣợc điều chỉnh vào vị trí trực giao tuyệt đối (orthogonal), sau đó đƣợc

thông qua quá trình tái chuẩn hóa (renormalized) quyết định bởi số liệu nhận từ gia

tốc Acc và vector từ trƣờng để hiệu chỉnh lại. Chúng cũng không chịu tác động tích

lũy của nhiễu từ bên ngoài do giải pháp sử dụng dữ liệu con quay hồi chuyển Gyro để

cập nhật lại. Số liệu thu đƣợc từ cảm biến từ trƣờng cũng đƣợc dùng để hiệu chỉnh

trƣợt cho góc yaw, còn gọi là quá trình hiệu chỉnh trƣợt góc hƣớng đối tƣợng (drifting

heading). Từ các dữ liệu đã đƣợc hiệu chỉnh tái chuẩn hóa này, ta tính toán các các giá

trị cập nhật các phần tử ma trận quay (4.20), từ đó tính ra các góc Euler: roll, pitch,

yaw (4.21) và tính toán lại vận tốc và khoảng di chuyển của đối tƣợng.

Phần này triển khai cụ thể các vấn đề lý thuyết trong chƣơng 3 trên một MEMS

INS 9-DOF cụ thể trên một hệ thống nhúng cụ thể. Giải thuật tính toán và các công

thức có thể áp dụng cho các INS 9-DOF loại khác hay với các hệ vi xử lý khác. Các

dữ liệu của MEMS INS 9-DOF luôn đƣợc cập nhật trong mỗi bƣớc tính, kết hợp với

dữ liệu nhận đƣợc từ CAN bus (FMS J1939) và GPS theo thuật toán lọc UKF để hiệu

chỉnh chính xác quỹ đạo và vận tốc của đối tƣợng đƣa vào hiển thị kết quả cuối cùng

trong hệ thống giám sát tại trạm. Dữ liệu đƣợc ghi trong các file trong suốt quá trình

hoạt động, dễ dàng truy cập để xem xét tính chất chống trôi dữ liệu của phƣơng pháp

này. Cấu trúc dữ liệu cụ thể đƣợc thể hiện ở phụ lục 3. Bộ lọc UKF sẽ đƣợc đề cập chi

tiết trong phần tiếp theo.

Hình 4. 17: Lƣu đồ chƣơng trình trên hệ thống nhúng Arduino

4.5. Thiết kế bộ lọc UKF và kết quả thực tế trên hệ thống giám sát tại trạm ứng

dụng cho xe bus

4.5.1. Đối tƣợng và mô hình xe bus

Phƣơng trình trạng thái tổng quát:

(̇ ) ( ) ( ) ( )

(4.23)

̇ ( ) ( ) ( ) ( )

(4.24)

Với bài toán giám sát ta có thành phần điều khiển B = 0 và D = 0. Khác với các

mô hình trƣớc đây, trong đó các tác giả luôn sử dụng hoặc mô hình tuyến tính hoặc

mô hình phi tuyến, trong luận án này tác giả đề ra giải pháp mô hình lai Phi tuyến

(Với mô hình hệ thống) và Tuyến tính (với mô hình đo đạc). Đề xuất mới này tận

dụng ƣu thế về mô tả động học của đối tƣợng trên Matlab với các công cụ hỗ trợ mạnh

mẽ về toán học của nó. Phần mô hình đo đạc đƣợc biểu diễn dƣới dạng các ma trận số.

Mô hình phi tuyến của hệ thống:

Đối tƣợng xe bus trong luận án là đối tƣợng phi tuyến. Nhƣ vậy, ta không biểu

diễn F dƣới dạng ma trận số cụ thể và không áp dụng các công thức toán học với ma

trận nhƣ đối với hệ tuyến tính đƣợc. Trạng thái của hệ thống đƣợc mô tả bằng phƣơng

trình động học phi tuyến của đối tƣợng dƣới dạng:

( )

(4.25)

{ }

Trong đó, biểu diễn theo các giá trị đạo hàm:

 ̇

 ̇ ( )

 ̇

 ̇ ( )

 ̇ ( ) , với là góc nghiêng từ GPS

 ̇ là vận tốc góc nghiêng từ GPS

 d_Alon: Đạo hàm Acc, là hƣớng chuyển động của xe khi đặt thiết bị song song

với sàn xe

 d_YawRate: Đạo hàm vận tốc góc Yaw của thiết bị

Đối với mô hình xe bus của hệ thống này, hàm f là hàm phi tuyến và không

biểu diễn đƣợc dƣới dạng ma trận số. Trong chƣơng trình Matlab, hàm này đƣợc mô

tải bởi các phƣơng trình trong file mô tả động học đối tƣợng

@busses_f_enhanced. Trạng thái dự báo của bƣớc tiếp theo (predicted state)

đƣợc tính toán bằng các công thức Euler.

(4.26)

Theo mô hình Jouni Hartikainen và Simo Särkkä (2011) [31] nhƣ đã khảo sát

Tool-box trong chƣơng 2, ma trận Q đƣợc tính theo công thức:

Trong đó L và Q có giá trị:

[

]

[

]

( )

(4.27)

(4.28)

Mô hình đo đạc là mô hình tuyến tính, đƣợc biểu diễn bởi phƣơng trình với ma

trận H và vector rk cụ thể, nhƣ sau :

{

}

(4.29)

Trong đó , các giá trị lấy từ tín hiệu đo đạc:

 Distance : Khoảng di chuyển của xe [m], cập nhật lấy từ CAN (FMS)

 X : Vĩ độ, từ tín hiệu GPS

 Y: Kinh độ, từ tín hiệu GPS

 Head: góc hƣớng Heading, từ tín hiệu GPS

 Speed: Tốc độ, từ tín hiệu GPS sử dụng form GPRMA

 Altitude: Độ cao so với mốc đã chọn

 Inclination: Độ nghiêng, tính toán từ GPS sử dụng form GPGGA

 Alon: Gia tốc theo hƣớng chuyển động của xe, tính từ INS khi đặt thiết bị song

song sàn xe

 YawRate: Vận tốc góc Yaw

[

]

; (4.30)

( ( )) ( ( )) ( ( )) ( ( )) ( ( )) ( ( )) ( ( )) ( ( )) ( ( )) [ ]

Với: std là đạo hàm chuẩn (standard deviation)

4.5.2. Xây dựng bộ lọc UKF cho xe bus

Trong hệ thống này tác giả phát triển bộ lọc UKF cho xe bus thu đƣợc kết quả

giám sát tốt hơn cùng nhiều thông số đặc trƣng cho chuyển động của đối tƣợng. Trong

thiết kế này, Xk biểu diễn thông qua hàm @ busses_f_enhanced và Yk là phƣơng

trình đo dạc tuyến tính, đƣợc diễn giải theo công thức (4.29). Tính toán dự báo và cập

theo công thức:

nhật các bƣớc của UKF đƣợc diễn dải nhƣ sau:

và

- Dự báo: Tính toán

̂ [ ̂ ]

(4.31) , ⋅⋅⋅ - √ [ √ √ ] ̂ ( )

]

- √ [ √

√

- Cập nhật: Tính toán , , theo công thức:

⋅⋅⋅ )

, (

- -

, ,

(4.32)

Từ đó tính toán đƣợc giá trị khuếch đại lọc , giá trị trung bình cập nhật trạng

thái và hiệp phƣơng sai nhƣ sau:

, -

(4.33)

Fil_FiltraggioPreUKF, ukf_predict và ukf_update, chi tiết liệt kê trong phụ

Các bƣớc tính UKF đƣợc triển khai trong các script Filtering_autobus,

lục 3. Hệ thống hoạt động theo hai chế độ và tự động chuyển đổi theo tín hiệu vào:

- Mode 1: Y = Y[0,…,8]; Ts = 1s

- Mode 2: Y = Y[0,…,8]; Ts = 1s, tại các thời điểm cập nhật tín hiệu GPS

Y = Y[8,9]; Ts = 20ms, tại các bƣớc tính với tín hiệu từ bus CAN

Mô hình phi tuyến

2.1

2.2

0

Sigma points

1

Tính dự báo

2.3

2

Tính thông số

2.4

3

Hình 4.18.1. Tính toán dự báo trạng thái

Tính đánh giá

4

3.1

Tính ma trận hiệp ph.sai

3.2

5

3.2

Hình 4.18: Lƣu đồ thuật toán lọc UKF (Bao gồm 4.21.1 – 4.21.3)

3.3

4.1

3.4 4.2 3.2

Hình 4.18.3. Tính toán đánh giá trạng thái

Hình 4.18.2. Tính toán dự báo đo đạc và ma trận hiệp phƣơng sai

Hình 4. 18: Lƣu đồ thuật toán lọc UKF

Giải thuật cho thuật toán lọc UKF:

 Bước 0: Khởi tạo mô hình hệ thống

 Bƣớc 1: Tính toán dự báo trạng thái, xác định và tƣơng tự nhƣ tính toán

với bộ lọc Kalman.

 Bước 2: Tính toán giá trị dự báo

(2.1). Định nghĩa 2l điểm sigma:

Trong đó:

với: l là số các phần tử của trạng thái

là căn bậc hai tích chéo ma trận vuông gốc (square-root) của ,

với

(2.2). Sử dụng các điểm sigma trên tính toán phi tuyến theo công thức:

(2.3). Tính toán véc tơ trạng thái dự báo:

(2.4). Tính toán ma trận hiệp phƣơng sai:

 Bước 3: Tính toán giá trị đo đạc dự báo và ma trận hiệp phƣơng sai

(3.1). Từ các điểm sigma tại khung [2.1] (công thức 3, 4, 5), ta tính toán các

điểm sigma của giá trị đo đạc theo công thức:

(3.2). Sử dụng giá trị sigma vừa tính đƣợc tính toán giá trị dự báo:

(3.3). Sử dụng hai giá trị và ở trên, thay vào công thức tính toán ma

trận hiệp phƣơng sai dự báo theo công thức:

(3.4). Sử dụng các giá trị , , và tính toán ma trận hiệp phƣơng sai

chéo đo đạc dự báo theo công thức:

 Bước 4: Khi có đƣợc giá trị đo đạc mới , ta tính toán giá trị cập nhật theo

các bƣớc sau:

(4.1). Sử dụng và ở trên tính toán hệ số khuếch đại lọc theo công

thức:

(4.2). Sử dụng giá trị và giá trị đo đạc thực tế tính toán giá trị

đánh giá theo công thức:

 Bước 5: Sử dụng và tính toán ma trận hiệp phƣơng sai theo công

thức:

Tƣơng tự nhƣ thuật toán lọc Kalman, với thuật toán lọc UKF ta tính đƣợc giá

trị [Khung 4], [Khung 5]. Thay hai giá trị này vào công thức tính và

của bƣớc tiếp theo [Khung 2], ta tính đƣợc trạng thái tiếp theo của hệ thống ở bƣớc

. Lặp lại các bƣớc tính toán này [từ Khung 2 tới Khung 5], ta sẽ tính đƣợc các

bƣớc liên tiếp của chuyển động. Tập hợp kết quả của các bƣớc tính này chính là quỹ

đạo của chuyển động.

100

4.5.3. Kết quả của hệ thống giám sát

Hình 4. 19: Hành trình thực tế của xe bus

Hình 4. 20: Phóng to một đoạn hành trình thể hiện quỹ đạo GPS và UKF

Nhận thấy rằng quỹ đạo nhận đƣợc từ GPS là đƣờng nối của các điểm chấm

(xanh), tuy nhiên tại một số thời điểm các nhiễu này khá lớn, các giá trị rời xa quỹ đạo

thực theo các phƣơng khác nhau làm sai lệch quỹ đạo của xe. Trên hình 4.20 thể hiện

rõ sự hiệu chỉnh chính xác quỹ đạo của xe theo giải pháp UKF mới của luận án là

đƣờng gạch (đen) thể hiện độ chính xác tốt hơn hẳn so với quỹ đạo GPS là các điểm

101

chấm (xanh). Kết quả lọc nâng cao chất lƣợng giám sát chuyển động thể hiện rõ trên

các quỹ đạo, ở đó quỹ đạo GPS (chấm xanh) có sai lệch lớn nhất và quỹ đạo tốt nhất

là UKF (gạch đen).

• Y-axis (Trục đứng): Vĩ độ • X-axis (Trục ngang): Kinh độ

Kết quả lọc và hiệu chỉnh quỹ đạo chuyển động tại đƣờng vòng

Hành trình và hiệu chỉnh các quỹ đạo sau khi lọc của xe bus: - Gạch rời đen: UKF - Chấm xanh: GPS

Hình 4. 21: Một đoạn quỹ đạo xe bus thể hiện quá trình lọc và hiệu chỉnh

Xét một vài đoạn quỹ đạo tại một số đoạn đặc biệt trên hình 4.22. Các tín hiệu

nhiễu cho hệ thống GPS gây ra sai lệch quỹ đạo lớn thƣờng xảy ra khi xe chuyển động

bất thƣờng ở những vị trí thay đổi đột ngột tốc độ và hƣớng di chuyển (Hình 4.22_3),

xe rung lắc mạnh hay ở vị trí dừng tại các trạm (Hình 4.22_2) và tại các vị trí xe đi

vào vùng bị che khuất hay xe dừng tại các cột đèn giao thông (Hình 4.22_1). Giải

pháp mới trong luận án đã lọc và đƣa ra quỹ đạo chính xác của xe thể hiện rõ tại

những vị trí này, loại bỏ đi phần quỹ đạo sai lệch và đƣa ra chính xác quỹ đạo chuyển

động của xe.

Tại các khúc cua, quỹ đạo UKF (gạch đen) tạo ra các nét đứt (hình 4.21). Điều

này có nghĩa trong giữa hai khoảng thời gian cập nhật dữ liệu GPS, UKF làm việc với

chế độ 2 (Mode 2, Ts = 20 ms) vẫn có khả năng hiệu chỉnh quỹ đạo bám theo hƣớng

chuyển động thực nhƣng quỹ đạo sẽ bị sai lệch dần. Khi có tín hiệu GPS nó sẽ hiệu

chỉnh lại quỹ đạo tạo ra đoạn dịch gấp khúc. Điều này là do hệ thống nhúng chƣa đủ

mạnh và tốc độ đƣa ra dữ liệu chậm của MEMS INS này. Hệ thống vẫn có khả năng

tính toán khi mất GPS trong thời gian ngắn, tuy nhiên độ chính xác không cao.

102

Tín hiệu GPS (chấm xanh) có nhiễu lớn làm sai lệch quỹ đạo chuyển động

Kết quả lọc khi tín hiệu GPS có nhiễu lớn tại một trạm dừng

Kết quả lọc khi tín hiệu GPS có nhiễu lớn tại đoạn cua

Hình 4. 22: Hiệu chỉnh quỹ đạo chính xác trong trƣờng hợp GPS có nhiễu lớn

103

Hình 4. 23: Đồ thị vận tốc thực và hiệu chỉnh vận tốc của xe suốt dọc tuyến

Trong đó: X-axis (Trục ngang) : Khoảng di chuyển của xe bus, [m]

Y-axis (Trục đứng) : vận tốc của xe bus, [km/h]

: Các điểm dừng của xe bus dọc theo tuyến

Đồ thị hình 4.22, hình 4.23 thể hiện rõ việc nâng cao chất lƣợng của hệ thống

giám sát, trong đó giá trị tốc độ đƣợc trải trên trục ngang theo quỹ đạo chuyển động

của xe. Trên hình 4.23_1 thể hiện rõ vận tốc thực của xe và các điểm đỗ dọc theo hành

trình, tính theo đơn vị thông thƣờng km/h. Hình 4.23_2 thể hiện các đồ thị vận tốc đo

đạc, vận tốc sau lọc và vận tốc tính toán cuối cùng. Đồ thị này giúp ngƣời giám sát

104

theo dõi hoạt động của xe, vận tốc xe cũng nhƣ quá trình tuân thủ hành trình của xe tại

các điểm đỗ nhƣ có bỏ bến hay không hoặc có đỗ dọc đƣờng hay không.

Đánh giá giá trị và điều chỉnh quỹ đạo

Hình 4. 24: Hành trình của xe và điều chỉnh quỹ đạo tƣơng ứng tại các thời

điểm tính toán

Trong đó: Y-axis (Trục dọc): Vĩ độ

X-axis (Trục ngang): Kinh độ

Điều chỉnh lùi vị trí

Điều chỉnh tiến vị trí

Điều chỉnh dịch phải ( + ) và dịch trái ( )

Trên hình 4.24 thể hiện quá trình hiệu chỉnh về quỹ đạo, bao gồm khoảng di

chuyển và vị trí của đối tƣợng chuyển động dọc theo quỹ đạo thể hiện trên bản đồ số.

Tại các thời điểm xe chuyển động bình thƣờng trong điều kiện tốt, các giá trị về

khoảng di chuyển và vận tốc không có sự thay đổi nhiều ở các kết quả từ GPS và các

105

kết quả của bộ lọc, thậm chí có nhiều đoạn quỹ đạo trùng nhau. Các kết quả này có sự

sai lệch rõ nét tại các vị trí bất thƣờng về vị trí nhƣ trong hình 4.22, chênh lệch dịch

chuyển khoảng cách tới hơn 10m nhƣ hình 4.22_2, hay sai lệch về vận tốc tới gần

1m/s nhƣ trong hình 4.23_2. Phần bên phải trên hình 4.24_1 thể hiện quá trình hiệu

chỉnh vị trí của quá trình lọc dọc theo quỹ đạo. Quá trình này đƣợc ghi liên tục tại mỗi

bƣớc tính toán thông qua việc lƣu các giá trị và tạo các file ảnh. Tập hợp các file ảnh

tạo thành một file dạng video thể hiện rõ quá trình lọc, ghi vào cơ sở dữ liệu. Đƣờng

chấm (màu xanh) ở phần bên trái thể hiện giá trị trực tiếp từ tính toán từ tín hiệu GPS.

Các điểm chấm là các vị trí tại các thời điểm nhận đƣợc tín hiệu GPS. Phần bên phải

thể hiện các hiệu chỉnh về hƣớng và giá trị tại thời điểm tính toán.

Hình 4. 25: Một số thông số chuyển động của xe bus (phần I)

Trong đó:

X-axis: Time [s]

Y-axis: Distance [m]

Các đồ thị sắp xếp theo thứ tự từ trên xuống dƣới:

- Đồ thị 1: Khoảng cách dịch chuyển của xe [m]

- Đồ thị 2: Tốc độ xe [m/s], biểu diễn trong 5 phút

- Đồ thị 3: Theta, là góc hƣớng Yaw [Rad]

- Đồ thị 4: ThetaDot là vận tốc góc hƣớng YawRate [Rad/s]

106

Hình 4. 26: Một số thông số chuyển động của xe bus (phần II)

Trong đó:

X-axis: Time [s]

Y-axis: Distance [m]

Các đồ thị sắp xếp theo thứ tựu từ trên xuống dƣới:

- Đồ thị 1: Tốc độ xe [m/s] - Đồ thị 2: Gia tốc theo trục z-axis [m/s2]

- Đồ thị 3: Góc nghiêng Alpha [Rad]

- Đồ thị 4: Độ cao so với mốc (Altitude)

Nhận thấy rằng các đồ thị hình 4.25, hình 4.26 thể hiện rõ một số thông số

chuyển động của xe dọc theo hành trình. Ngoài các thông số thông thƣờng nhƣ vị trí

và vận tốc, hệ thống giám sát trong luận án còn hiển thị một số thông số đặc trƣng của

xe nhƣ: khoảng di chuyển của xe, góc hƣớng, vận tốc góc hƣớng, gia tốc, góc

nghiêng, độ cao so với mốc. Điều này thể hiện rõ quá trình vận hành xe, qua đó giúp

nâng cao chất lƣợng của hệ thống giám sát, giúp cho quá trình quản lý, điều hành và

giám sát xe đƣợc hiệu quả hơn, cũng nhƣ giúp đánh giá về phong cách lái xe.

107

4.6. Kết luận chƣơng 4

Luận án đã phát triển thành công phƣơng pháp tính toán trên miền rời rạc kết

hợp bù dữ liệu trên cơ sở sử dụng logic mờ cho các hệ thống INS không có vector từ

trƣờng (giới hạn dƣới 6-DOF). Luận án đƣa ra giải pháp bù dữ liệu INS bằng cách so

sánh góc yaw tính toán đƣợc từ INS với góc của một la bàn điện tử tích hợp trong hệ

thống. Góc lệch giữa la bàn và góc yaw của INS, kết hợp với các đặc điểm chuyển

động của đối tƣợng, là tín hiệu đầu vào cho bộ điều khiển mờ để bù dữ liệu. Thuật

toán này cần đƣợc khảo sát thực tế với mỗi đối tƣợng cụ thể để đƣa ra các luật bù cho

phù hợp. Kết quả này đã đƣợc báo cáo tại Hội thảo toàn quốc về Cơ Điện tử VCM

2012. [PHỤ LỤC, Bảng 3]

Sự ra đời của dòng cảm biến MEMS INS Razor 9-DOF (2012) đã tạo ra một

bƣớc tiến đột phá trong lĩnh vực thiết kế hệ thống tích hợp GPS/INS. Với thiết bị tiên

tiến này, luận án đã phát triển và ứng dụng thành công phƣơng pháp xử lý chống trôi

các phần tử của ma trận quay R, từ đó thiết kế thành công thiết bị tích hợp GPS/INS

với độ chính xác cao. Khác với lọc Kalman xử lý dữ liệu ở bƣớc sau, tức là lấy kết quả

đã bị tích lũy sai số của INS để tính toán, đây là một giải pháp mới trong thiết kế hệ

) sau

thống sử dụng MEMS INS bởi khả năng xử lý dữ liệu ở cấp độ đo lƣờng khi điều

chỉnh chống trôi các phần tử ma trận quay (Ri,j) thành các giá trị cập nhật (

mỗi bƣớc tính để tính toán chính xác các giá trị đầu ra của hệ thống INS. Phƣơng pháp

này giúp INS 9-DOF có thể làm việc độc lập và tự hiệu chỉnh nó, khác với các giải

pháp trƣớc đây là dùng GPS để hiệu chỉnh lại tín hiệu trong khi vẫn cộng dồn sai số

của INS theo thời gian.

Với sự thành công của phƣơng pháp tự động hiệu chỉnh ma trận quay, luận án

trình bày một giải pháp mới thiết kế hệ thống tích hợp GPS/INS trên cơ sở cấu trúc

phân tán đối với các hệ INS 9-DOF. Giải pháp phân tán này chia các công việc xử lý ở

các phần khác nhau của hệ thống nhằm tận dụng tối đa khả năng của mỗi thành phần.

Đối với đối tƣợng trên xe, luận án phát triển phƣơng pháp hiệu chỉnh các tham số của

ma trận quay cho firmware của hệ thống nhúng và đƣa vào lập trình cho hệ thống

nhúng Arduino R3/YUN.

Trong phần giám sát thiết bị tại trạm, luận án đã phát triển thành công bộ lọc

UKF linh động tự động chuyển đổi chế độ hoạt động theo tín hiệu đầu vào. Giải thuật

108

của bộ lọc UKF đƣợc trình bày dƣới dạng các thuật toán lập trình và triển khai cụ thể

trên Matlab áp dụng cho mô hình tích hợp, là mô hình phi tuyến của đối tƣợng và

phƣơng trình tuyến tính của mô hình đo đạc, ứng dụng cho đối tƣợng cụ thể là xe bus.

Bộ lọc UKF đã nâng cao chất lƣợng quỹ đạo chuyển động của xe bus, đồng thời giám

sát đƣợc một số thông số chuyển động và trạng thái vận hành của xe theo yêu cầu.

Một hƣớng phát triển đƣợc quan tâm là ứng dụng cho các hệ thống chuyển

động trong trƣờng hợp mất GPS trong khoảng thời gian ngắn. Lúc này hệ thống cần

có một hệ quy chiếu tiêu chuẩn khác thay cho GPS, ví dụ nhƣ hệ thống phát sóng

radio ba điểm công suất cao nhƣ hệ RTK-GPS [64]. Tuy nhiên trở ngại của phƣơng

pháp này là rất khó khăn trong vấn đề đồng bộ thời gian. Thêm vào đó, phát sóng

radio công suất cao trong khu vực có dân cƣ cũng không phải là giải pháp tốt. Thực

chất thuật toán UKF trong luận án cũng có thể phần nào giải quyết vấn đề này với hai

mode hoạt động. Khi hoạt động trong mode 2 với tín hiệu tham chiếu lấy từ bus CAN,

trong khi chƣa cập nhật đƣợc tín hiệu từ GPS hệ thống vẫn có khả năng định vị bám

theo chuyển động thực tế, tuy nhiên chất lƣợng chƣa cao do tốc độ chậm của vi xử lý

và INS.

Kết quả nghiên cứu của bao gồm cả cơ sở lý thuyết về giải pháp mới, thiết kế

phần cứng và phần mềm, đã đƣợc đƣa vào ứng dụng thực tế trong quản lý giám sát xe

bus và đƣợc đánh giá cao tại một đất nƣớc có nền khoa học phát triển nhƣ Italia. Một

loạt các thiết bị dạng này đã đƣợc phát triển và ứng dụng trong hệ thống quản lý giám

sát phƣơng tiện giao thông vận tải tại Trento, Italia. Nó đã đƣợc thử nghiệm thành

công trên 6 xe bus, gồm 3 xe MAN Lion’s City Hybrid A37 và 3 xe Van Hool A330

Hybrid bus. Trên cơ sở đánh giá kết quả của chƣơng trình quản lý giám sát hai loại xe

thử nghiệm này, kết quả của luận án ngoài việc nâng cao chất lƣợng giám sát quản lý

xe bus còn có thể giám sát và đánh giá hoạt động của các xe lai Hybrid Diesel-Điện

trên một tuyến đƣờng núi về đƣờng đi, tốc độ, hiệu suất sử dụng năng lƣợng, từ đó bố

trí lịch trình hoạt động của xe một cách hợp lý. Ngoài ra, thiết bị còn đƣợc sử dụng để

đánh giá và phân tích ảnh hƣởng của phong cách lái xe trên hiệu suất xe trong các điều

kiện giao thông khác nhau.

109

KẾT LUẬN VÀ KIẾN NGHỊ

I. Kết luận về luận án

Luận án có giá trị khoa học với việc đƣa ra giải pháp thiết kế hệ thống phân tán

sử dụng INS 9-DOF tự động hiệu chỉnh gắn trên đối tƣợng chuyển động và bộ lọc

UKF tại trạm giám sát giúp nâng cao chất lƣợng hệ thống. Giải pháp phân tán trong

thiết kế hệ thống sử dụng hệ INS 9-DOF là một giải pháp mới hiệu quả so với các giải

pháp tích hợp GPS/INS trƣớc đây.

Luận án đã phát triển thành công phƣơng pháp tự động hiệu chỉnh ma trận quay

cho INS 9-DOF, xử lý dữ liệu ở cấp độ đo lƣờng giúp INS có thể tự hiệu chỉnh chống

trôi dữ liệu đƣa ra kết quả chính xác hơn trƣớc khi đƣa vào lọc. Sản phẩm tích hợp

GPS/INS sử dụng INS 9-DOF đã đƣợc thử nghiệm ứng dụng thực tế cho kết quả tốt.

Hệ thống giám sát tại trạm sử dụng lọc UKF đã loại bỏ đƣợc những giá trị sai lệch cho

ra kết quả quỹ đạo đúng với thực tế. Ngoài ra, hệ thống còn kiểm soát đƣợc một số

tham số thể hiện trạng thái chuyển động của xe.

II. Kiến nghị hƣớng nghiên cứu tiếp theo

Hƣớng phát triển cần đƣợc quan tâm là ứng dụng cho các hệ thống chuyển

động trong trƣờng hợp mất GPS. Lúc này hệ thống cần có một hệ quy chiếu tiêu

chuẩn chính xác khác cũng có khả năng định vị thay cho GPS. Khả năng phát triển

ứng dụng của thiết bị là khá đa dạng, nhƣ để giám sát và quản lý các phƣơng tiện

đƣờng bộ, đƣờng thủy, đƣờng sắt và phƣơng tiện bay.

Kết quả của luận án này cần đƣợc kết hợp với một số công việc khác nhƣ

truyền thông, định vị trên các loại bản đồ số khác nhau cũng nhƣ quản lý cơ sở dữ liệu

trên Web để tạo nên một hệ thống quản lý giám sát hoàn chỉnh.

110

DANH MỤC CÁC CÔNG TRÌNH CÔNG BỐ CỦA TÁC GIẢ LIÊN

QUAN ĐẾN LUẬN ÁN

1. Ngô Thanh Bình (2009); “Phát triển công cụ làm trơn RTS trong xác định quỹ đạo

chuyển động”; Tạp chí khoa học GTVT, số 27, trang 112 – 117.

2. Thanh Binh Ngo, Hung Lan Le, Thanh Hai Nguyen (2009); “Survey of Kalman

Filters and Their Application in Signal Processing”; Hội thảo quốc về Tính toán

thông minh, AICI09 - The 2009 International Conference on Artificial Intelligence

and Computational Intelligence (AICI'09), pp. 335 – 339.

3. Ngô Thanh Bình (2010); “Phát triển công cụ lọc UKF (Unscented Kalman Filter)

trong xử lý tín hiệu”; Tạp chí Khoa học GTVT, số 32, trang 27 – 33.

4. Ngô Thanh Bình (2011); “Tính toán hệ INS trên miền rời rạc”; Tạp chí Khoa học

GTVT, số tháng 7/2011, trang 129 – 134.

5. Nguyễn Quang Tuấn, Ngô Thanh Bình (2011); “Công nghệ tích hợp INS-GPS

trong giám sát giao thông”; Tạp chí khoa học GTVT, Bộ Giao thông Vận tải, số

tháng 9, trang 39 – 43.

6. Le Hung Lan, Nguyen Thanh Hai, Nguyen Quang Tuan, Ngo Thanh Bình (2011);

“Introduction TBN Methode in z-Domain Using in Signal Procesing”; Tạp chí

khoa học quốc tế MADI – SWJTU – UTC số 3, trang 13 – 21.

7. Ngô Thanh Bình (2011); “Xử lý góc Heading trong hệ thống tích hợp GPS/INS”;

Tạp chí Khoa học GTVT, số 36, trang 62 – 67.

8. Thanh Binh Ngo, Thanh Hải Nguyễn (2012); “Design of a system for management

and monitoring of vehicles transporting solid waste in open-cast coal mines”; Tạp

chí khoa học quốc tế: Journal of Vietnamese Environment, Vol.3 No.2, pp. 92-97.

9. Thanh Binh Ngo (2012); “Improvement of the quality of integrated INS/GPS

devices based on fuzzy logic”; Hội thảo toàn quốc về Cơ Điện tử: The 6th Vietnam

Conference on Mechatronics; Hanoi, 14-15 Dec. 2012. VCM2012, pp. 411 – 418.

10. Binh T. Ngo, Francesco Biral, Lan H. Le, Hai T. Nguyen (2014); “Improvement of

the Quality of Vehicles Positioning and Management Systems Based on using

MEMS INS 9-DOF and GPS Devices”; International conference nSTf 2014,

NACENTECH, Hanoi, pp. 361 – 376.

111

TÀI LIỆU THAM KHẢO

A. TIẾNG VIỆT

1. Phạm Hải An (2010), Về một phương pháp nhận dạng chuyển động phương tiện

cơ giới quân sự sử dụng đa cảm biến, Luận án TSKT Viện KH-CN Quân sự -

Học viện Kỹ thuật Quân sự.

2. Nguyễn Thanh Hải và các cộng sự (2007 - 2008), Nghiên cứu thiết kế, chế tạo thiết

bị định vị vệ tinh phục vụ giám sát, quản lý phương tiện giao thông đường bộ,

đường sắt; Đề tài khoa học công nghệ cấp nhà nƣớc – Chƣơng trình KC.06.02:

Nghiên cứu, phát triển và ứng dụng công nghệ tiên tiến trong sản xuất các sản

phẩm xuất khẩu chủ lực, mã số KC.06.02/06-10.

3. Phạm Tuấn Hải (2011), Nâng cao chất lượng hệ dẫn đường thiết bị bay trên cơ sở

áp dụng phương pháp xử lý thông tin kết hợp, Luận án TSKT 62.52.64.01 - Học

viện Kỹ thuật Quân sự.

4. Lê Hùng Lân và các cộng sự (2004 - 2006), Nghiên cứu ứng dụng công nghệ Tự

động hoá trong quản lý, điều hành giao thông đô thị, Đề tài khoa học công nghệ

cấp nhà nƣớc – Chƣơng trình KC.03, mã số KC.03.21.

5. Nguyễn Sỹ Long (2013), Xây dựng thuật toán xác định tham số dẫn đường và luật

điều khiển cho một lớp thiết bị bay hành trình đối hải, Luận án TSKT

6. Trƣơng Duy Trung (2014), Các công trình khoa học đã công bố của luận án tiến

62.52.64.01 - Viện Khoa học và Công nghệ Quân sự.

sĩ, Viện Khoa học Công nghệ Quân sự.

B. TIẾNG ANH

7. Analog Devices – adi-mems (2008), iSensor: Analog Devices; Updated

Confidential Information in October 2008.

8. Alex Garcia-Quinchia, Yi-Guo, Elena Martin, Carles Ferrer (2009), “A System-

On-Chip (SOC) Platform to Integrated Inertial Navigation Systems & GPS”,

112

EEE International Symposium on Industrial Electronics (ISlE 2009) Seoul

Olympic Parktel, Seoul, Korea, pp. 603 – 608.

9. Basil, H., Anathasayanam, M. and Puri, S. (2004), “Adaptive Kalman Filter

Tuning in Integration of Low-Cost MEMSGPS/INS, AIAA Guidance,

Navigation and Control”, Conference – Providence, RI, pp. 16-19.

10. Biral, F., Galvani, M. , Zucchelli, M. , Giacomelli, G. (2013), “Objective

performance evaluation on mountain routes of diesel-electric hybrid busses”,

Mechatronics (ICM), 2013 IEEE International Conference, pp. 394-399.

11. C. Goodall, N. El-Sheimy (2004), Intelligent tuning of a Kalman filter using Low-

Cost MEMS inertial sensors, Doctor’s thesis in the Uni. of Calgary

12. Christopher L. Goodall (2010), Improving Usability of Low-Cost INS/GPS

Navigation Systems using Intelligent Techniques, Doctor’s thesis in the

university of Calgary

13. Dah-Jing Jwo and Shun-Chieh Chang (2008), “Application of Optimization

Technique for GPS Navigation Kalman Filter Adaptation”, D.-S. Huang et al.

(Eds.): ICIC 2008, LNCS 5226, pp. 227–234

14. Dah-Jing Jwo, Fu-I Chang (2007), “A Fuzzy Adaptive Fading Kalman Filter for

GPS Navigation”, D.-S. Huang, L. Heutte, and M. Loog (Eds.): ICIC 2007,

LNCS 4681, pp. 820–831

15. David H. Titterton, John L. Weston (2004), Strapdown Inertial Navigation

Technology - 2nd Edition, The Institution of Electrical Engineers.

16. Debo Sun, Mark G. Petovello, and M. Elizabeth Cannon (2008), “GPS/Reduced

IMU with a Local Terrain Predic tor in Land Vehicle Navigation”, Hindawi

Publishing Corporation, International Journal of Navigation and Observation,

Article ID 813821.

17. Erick Macias, Daniel Torres, Sourabh Ravindran (2012), Nine-Axis Sensor Fusion

Using the Direction Cosine Matrix Algorithm on the MSP430F5xx Family, Texas

Instruments - Application Report SLAA518A–February 2012.

18. Elliott D. Kaplan, Christopher J. Hegarty (2006), Understanding GPS Principles

and Applications - Second Edition, Artech House.

113

19. Eun - Hwan Shin, Xiao Ji Niu (2006), Kalman Filter face off – Extended vs.

Unscented Kalman Filters for Intergrated GPS and MEMS Inertial, Mobile

sensor systems reserch group, Calgary.

20. Eun-Hwan Sin (2001), Accuracy Improvement of low cost GPS/INS for land

application, Doctor’s thesis in the university of Calgary.

21. Fabrizio Zendri (2010), Development of a research vehicle able to perform

autonomous manoeuvres, PhD Thesis of University of Trento.

22. Fabrizio Zendri (2011), Implementing state estimation methods and planning

algorithms on microcontroller architectures, and development of their

verification tools, Zendri - Report - Assegno di ricerca, University of Trento.

23. Faraz M. Mirzaei, Stergios I. Roumeliotis (2008), “A Kalman Filter-Based

Algorithm for IMU-Camera Calibration: Observability Analysis and

1143 – 1156.

Performance Evaluation”, IEEE Transactions On Robotics, Vol. 24, No. 5, pp.

24. Francesco Biral, Michele Zucchelli (2013); Progetto, “Valutazione oggettiva delle

prestazioni di autobus ibridi diesel-elettrico per mezzo di prove sperimentali”,

Relazione Intermedia, University of Trento, 2013.

25. Greg Welch, Gary Bishop (Updated July 24, 2006), An Introduction to the Kalman

Filter, Chapel Hill, NC 27599-3175.

26. Hang Shi , Zhou Wu, Baosheng Liu (2006), “An Adaptive Filter for INS/GPS

Integrated Navigation System; IMACS Multiconference on Computational

Engineering in Systems Applications", CESA, Beijing, China. Vol 1, pp. 651 –

654.

27. Hua Song, Hong-Yue Zhang, C. W. Chan (2009), Fuzzy fault tree analysis based

on T–S model with application to INS/GPS navigation system; Soft Comput

2009, pp. 31–40.

28. Jamshaid Ali, Muhammad Ushaq (2009), A consistent and robust Kalman ﬁlter

design for in-motion alignment of inertial navigation system, Center for Control

and Instrumentation, National Engineering and Scientiﬁc Commission,

Islamabad, Pakistan.

114

29. Jianchen Gao (2007), Development of a Precise GPS/INS/On-Board Vehicle

Sensors Integrated Vehicular Positioning System, Doctor’s thesis in the

university of Calgary.

30. Jim Ledin (2004); Embedded Control Systems in C/C++: An Introduction for

Software Developers Using MATLAB; CMP Books, ISBN:1578201276.

31. Jouni, Sarkka (2008), Optimal filtering with Kalman filters and smoother, Helsinki

Uni. of Technology, P.O.Box 9203, FIN-02015 TKK, Espoo, Finland

32. Jun Zhou, Hamidreza Bolandhemmat (2007), “Integrated INS/GPS System for an

Autonomous Mobile Vehicle”, Proceedings of the 2007 IEEE International

Conference on Mechatronics and Automation August 5 - 8, 2007, Harbin, China,

pp. 694 – 699.

33. Kai-Wei Chiang, et al. (2008), “An ANN–RTS smoother scheme for accurate

INS/GPS integrated attitude determination”, GPS Solut, pp .199–208

34. Kai-Wei Chiang, Yun-Wen Huang (2007), “An intelligent navigator for seamle

INS/GPS integrated land vehicle navigation applications”, Department of

Geomatics, National Cheng-Kung University, Taiwan, pp722–733.

35. Kesh Bakhru (2005), “A Seamless Tracking Solution for Indoor and Outdoor

Position Location”, Kesh Bakhru Cubic Defense Applications San Diego, CA.

Vol.3, pp. 2029 – 2033.

36. Krisada Sangpetchsong (2009), “Developing a Tilt Sensor System Using Rapid

STM32 Blockset”, Rapid STM32 Team, Mathworks Inc.

37. Lars B. Cremean, et al. (2007), “Alice: An Information-Rich Autonomous Vehicle

for High-Speed Desert Navigation”, M. Buehler, K. Iagnemma, and S. Singh

(Eds.): DARPA’05, STAR 36, pp. 437–482.

38. Libor Preucil, Roman Mazl (2006), “Vehicle Localization Using Inertial Sensors

and GPS”, S. Yuta et al. (Eds.): Field and Service Robotics, STAR 24, pp. 135–

144

39. Mark. G. Petovello, K. O’Keefe, G. Lachapelle, M. E. Cannon (2009),

“Consideration of time-correlated errors in a Kalman ﬁlter applicable to GNSS”,

J Geod 83. pp. 51–56.

115

40. Mark G. Petovello (2003), Real-Time Integration of a Tactical-Grade IMU and

GPS for High-Accuracy Positioning and Navigation, Doctor’s thesis in the

university of Calgary.

41. Mark Potovello (2010), GNSS Solution: A fully digital model for Kalman filter,

Inside GNSS, Geomatics Engineering, Calgary.

42. Minha Park (2004), Error Analysis and Stochastic Modeling of MEMS based

Inertial Sensors for Land Vehicle Navigation Applications, Doctor’s thesis in the

university of Calgary.

43. Mohinder S. Grewal, Angus P. Andrew (2001), Kalman Filtering - Theory &

Practice using MatLab, John Wiley & Son Inc., Hoboken, New Jersey.

44. Mohinder S. Grewal, Lawrence R. Weill, Angus P. Andews (2007), Global

Posittioning systems, Innertial navigation, and Integration, John Wiley & Sons,

Inc., Hoboken, New Jersey.

45. Muhammad Haris Afzal, Valérie Renaudin, Gérard Lachapelle (2011), Use of

Earth’s Magnetic Field for Mitigating Gyroscope Errors Regardless of Magnetic

Perturbation, University of Calgary.

46. Naser El-Sheimy (2008), The Potential of Partial IMUs for Land Vehicle

Navigation, Inside GNSS.

47. Nasrin Bourghani Farahani, Nader Pouryaie (2008), GPS and Low Cost Sensors in

Navigation, R. Sandau et al. (eds.), Small Satellites for Earth Observation;

Springer Science + Business Media B.V. 2008.

48. Nguyen Quang Tuan (2009), Building the positioning system with the digital

signal DVB-T of DTV, Doctor’s thesis in SWJTU, China.

49. Papoulis (1997), Sinal Analysis, McGraw-Hill, ISBN 0-07-048460-0.

50. Qin Wang, Chris Rizos, Yong Li, Shiyi Li (2008), Application of a Sigma-point

IEEE.

51. Ravindra Babu, Jinling Wang (2009), “Ultra-tight GPS/INS/PL integration: a

system concept and performance analysis”, GPS Solut, pp. 75–82.

52. Ravindra Babu, Jinling Wang, Gottapu Rao (2008), “Analysis of Ultra-tight

GPS/INS Integrated System for Navigation Performance”, IEEE-International

116

Conference on Signalprocessing, Communications and Networking Madras

Institute ofTechnology, Anna University Chennai India, pp. 234-237

53. Rudolph van der Merwe (2004), Sigma-Point Kalman Filters for Probabilistic

Inference in Dynamic State-Space Models, Doctor of Philosophy in Electrical

and Computer Engineering.

54. Rudolph van der Merwe, Eric A. Wan (2004), Sigma-Point Kalman Filters for

Integrated Navigation, Adaptive Systems Lab, OGI School of Science &

Engineering, Oregon Health & Science University.

55. Salah Sukkarieh (2000), Low Cost, High Integrity, Aided Inertial Navigation

Systems for Autonomous Land Vehicles, Doctor’s thesis in the University of

Sydney.

56. Sam eh Nassar, Naser El-Sheimy (2006); “A combined algorithm of improving

INS error modeling and sensor measurements for accurate INSGPS navigation”,

GPS Solut 10, pp. 29–39.

57. Sameh Nassar (2003), Improving the Inertial Navigation System (INS) Error

Model for INS and INS/DGPS Applications, Doctor’s thesis in the university of

Calgary.

58. Sang-il Ko, Jong-suk Choi, Byoung-hoon Kim (2006); “Performance

Enhancement of Indoor Mobile Localization System using Unscented Kalman

Filter”; SICE-ICASE International Joint Conference 2006, Oct. 18-21, 2006 in

Bexco, Busan, Korea. pp. 1355 – 1360.

59. Saurabh Godha (2006), Performance Evaluation of Low Cost MEMS-Based IMU

Integrated With GPS for Land Vehicle Navigation Application, Doctor’s thesis in

the university of Calgary.

60. Seven Rönnbäck (2000), Development of a GPS/INS navigation loop for an UAV,

The Lulea University of Technology.

61. Stefano Panzieri, Federica Pascucci, Giovanni Ulivi (2002), An Outdoor

Navigation System Using GPS and Inertial Platform, IEEE/ASME Transactions

on Mechatronics, Vol. 7, No. 2.

62. Steven R. Swanson (1998), A Fuzzy Navigational State Estimator for GPSRNS

Integration, NASA Johnson Space Center Houston, TX 77058.

117

63. Thanh Binh Ngo, Hung Lan Le, Thanh Hai Nguyen (2009), “Survey of Kalman

Filters and Their Application in Signal Processing”, AICI’09, China, pp. 335-

339.

64. Tomoji Takasu (2009), Development of the low-cost RTK-GPS receiver with an

open source program package RTKLIB, Tokyo University of Marine Science and

Technology, Japan.

65. Umar Iqbal, Aime Francis Okou, and Aboelmagd Noureldin (2008), “An

Integrated Reduced Inertial Sensor System - RISS / GPS for Land Vehicle”, 1-

66. Valerie Renaudin, Muhammad Haris Afzal, Gerard Lachapelle (2010), Complete

Triaxis Mag netometer Calibration in the Magnetic Domain, Hindawi Publishing

Corporation Journal of Sensors.

67. W. Abdel –Hamid, T. Abdel azim, N. El-S heimy, G. Lach apelle (2006),

“Improvement of MEMS-IMU/GPS performance using fuzzy modeling”, GPS

Solut. 10, pp. 1–11.

68. Walid Abdel-Hamid (2005), Accuracy Enhancement of Integrated MEMS-

IMU/GPS Systems for Land Vehicular Navigation Applications, Doctor’s thesis

in the university of Calgary.

69. Walid Abdel-Hamid, Aboelmagd Noureldin, and Naser El-Sheimy (2007);

Adaptive Fuzzy Prediction of Low-Cost Inertial-Based Positioning Errors; IEEE

Transactions on Fuzzy Systems, Vol. 15, No. 3, pp. 519 – 529.

70. Wei Chen, Zhongqian Fu, Ruizhi Chen, Yuwei Chen, Octavian Andrei, Tuomo

Kröger, Jianyu Wang (2009), “An Integrated GPS and Multi-Sensor Pedestrian

Positioning System for 3D Urban Navigation”, Urban Remote Sensing Joint

Event, pp. 1 – 6.

71. Xiyuan Chen, Xuefen Zhu, and Zigang Li (2007), “Application for GPS/SINS

Loosely-Coupled Integrated System by a New Method Based on WMRA and

RBFNN”, D.-S. Huang, L. Heutte, and M. Loog (Eds.), pp. 394–403

72. Xu Tianlai, Cui Pingyuan (2007), “Fuzzy Adaptive Interacting Multiple Model

Algorithm for INS/GPS”, Proceedings of the 2007 IEEE International

Conference on Mechatronics and Automation, Harbin, China, pp. 2963 – 2967.

118

73. Yi Cao (2008), Learning the Kalman Filter in Simulink Examples, Cranfield

University – Matlab Central.

74. Yun-Wen Huang, Kai-Wei Chiang (2008), “An intelligent and autonomous

MEMS IMU/GPS integration scheme for low cost land navigation applications”,

75. Zhiqiang Du, Zhihua Cai, Leichen Chen, Huihui Deng (2009), “An Evolutionary

GPS Solut 12, pp.135–146

Algorithm and Kalman Filter Hybrid Approach for Integrated Navigation”,

Advances in Computation and Intelligence, Springer, pp. 211–216.

76. http://www.cs.unc.edu/~welch/kalman/

77. http://en.wikipedia.org/wiki/Global_Positioning_System

78. http://www.mathworks.com/matlabcentral/

79. http://www.lce.hut.fi/research/mm/ekfukf/

80. http://eqworld.ipmnet.ru/en/software.htm#matlab

81. https://dept.astro.lsa.umich.edu/ugactivities/Labs/coords/index.html

82. http://www.gmat.unsw.edu.au/snap/gps/clynch_pdfs/coorddef.pdf

83. http://www.insidegnss.com/

84. http://plan.geomatics.ucalgary.ca/search_pubs_results.php

85. http://www.movable-type.co.uk/scripts/latlong.html

86. http://www.eurofot-ip.eu/

87. http://www.starlino.com/imu_guide.html

88. http://www.starlino.com/dcm_tutorial.html

89. https://www.sparkfun.com/categories/160

90. https://github.com/ptrbrtz/razor-9dof-ahrs/wiki/Tutorial

91. http://www.jucs.org/jucs_15_4/a_dcm_based_orientation

92. https://code.google.com/p/imumargalgorithm30042010sohm/

93. https://code.google.com/hosting/search?q=label:AHRS

C. INTERNET

PHỤ LỤC 1: CÁC CÔNG VIỆC HOÀN THÀNH TRONG THỜI GIAN

THỰC HIỆN LUẬN ÁN

1. Các công trình khoa học đã công bố

Là tác

giả Nơi công bố Năm Tên công trình Số hoặc là (tên tạp chí đã đăng công công (bài báo, công trình) đồng TT trình) bố

tác giả

1 Lập trình nhúng với vi Tác giả Tạp chí khoa học GTVT, số 2007

xử lý AVR 20, 56-60

2 Thiết kế chế tạo bộ Tác giả Tạp chí khoa học GTVT, số 2008

KIT vi xử lý đa năng 22, 129 - 135

áp dụng cho giảng dạy

và nghiên cứu khoa học

3 Phát triển công cụ làm Tác giả Tạp chí khoa học GTVT, số 2009

trơn RTS trong xác 27, 112 - 117

định quỹ đạo chuyển

động

4 Survey of Kalman Tác giả Hội thảo quốc về Tính toán 2009

Filters and Their thông minh (AICI’09):

Application in Signal The 2009 International

Processing Conference on Artificial

Intelligence and Computational

Intelligence

AICI.2009, pp 335-339

http://www.computer.org/csdl/

proceedings/aici/2009/3816/03/

3816c335-abs.html

III

5 Phát triển công cụ lọc Tác giả Hội thảo khoa học kỷ niệm 65 2010

UKF (Unscented năm thành lập trƣờng GTVT

Kalman Filter) trong số 32, 11 - 17

xử lý tín hiệu

6 Tính toán hệ INS trên Tác giả Tạp chí Khoa học GTVT, số 2011

miền rời rạc 34, 129 - 134

7 Công nghệ tích hợp Đồng Tạp chí khoa học Bộ giao 2011

INS-GPS trong giám tác giả thông, số tháng 9, 39 – 43

sát giao thông

8 Introduction TBN Đồng Tạp chí khoa học quốc tế 2011

Methode in z-Domain tác giả MADI – SWJTU – UTC số 3,

Using in Signal 13 – 21

Procesing http://lib.madi.ru/fel/fel1/fel11

P003.pdf

9 Xử lý góc Heading Tác giả Tạp chí Khoa học GTVT, số 2012

trong hệ thống tích hợp 36, 62 - 67

GPS/INS

10 Design of a system for Tác giả Tạp chí khoa học quốc tế: 2012

management and Journal of Vietnamese

monitoring of vehicles Environment, Vol.3 No.2, pp.

transporting solid 92-97.

waste in open-cast coal http://openaccess.tu-

mines. dresden.de/ojs/index.php/jve/ar

ticle/view/52/45

11 Management and Tác giả Tạp chí khoa học quốc tế: 2012

monitoring of air and Journal of Vietnamese

water pollution by Environment, Vol.3 No.1, pp.

using GIS technology. 50-54.

http://openaccess.tu-

dresden.de/ojs/index.php/jve/ar

ticle/view/43/36

12 Improvement of the 2012

quality of integrated Tác giả Hội thảo toàn quốc về Cơ Điện tử: The 6th Vietnam

INS/GPS devices based Conference on Mechatronics;

on fuzzy logic. Hanoi, 14-15 Dec. 2012.

VCM2012, pp 411 – 418

https://www.researchgate.net/p

ublication/237010067_Improve

ment_of_the_quality_of_integr

ated_INSGPS_devices_based_

on_fuzzy_logic

13 Design and Tác giả Tạp chí khoa học quốc tế: 2013

implementation of an Journal of Vietnamese

automatic hydrological Environment, Vol. 4, No. 2,

monitoring system for pp. 34-42

hydropow-er plants http://dx.doi.org/10.13141/JVE

14 Improvement of the Tác giả International conference 2014

Quality of Vehicles NACENTECH: nSTf 2014

Positioning and

Management Systems

Based on using MEMS

INS 9-DOF and GPS

Devices

Bảng 3: Các báo cáo khoa học đã công bố

2. Các đề tài, dự án, nhiệm vụ khác đã chủ trì hoặc tham gia

Tình trạng đề

Thời gian Thuộc chƣơng tài

TT Tên đề tài, dự án, nhiệm (bắt đầu – trình (Đã nghiệm

vụ khác đã chủ trì kết thúc) (nếu có) thu, chưa

nghiệm thu)

1 Đề tài NCKH Cấp bộ

Chủ trì. B2007-04-27:

Đã nghiệm Thiết kế, chế tạo bộ KIT vi 2007 – 2008 thu xử lý đa năng phục vụ

(2008) giảng dạy và nghiên cứu

khoa học

2 Đề tài NCKH Cấp nhà KC.06/06-10:

nước Nghiên cứu, Chủ nhiệm

KC.06.02/06-10: phát triển và nhánh 6

Nghiên cứu thiết kế, chế 2008 – 2009 ứng dụng công Đã nghiệm

tạo thiết bị định vị vệ tinh nghệ tiên tiến thu

phục vụ giám sát, quản lý trong sản xuất (2010)

các sản phẩm phƣơng tiện giao thông

xuất khẩu chủ đƣờng bộ, đƣờng sắt

lực.

3 Đề tài NCS 2009

T2009-ĐĐT-31:

Chủ trì. Phát triển thuật toán

Đã nghiệm Kalman và thiết kế thiết bị 2009

thu thử nghiệm ứng dụng

(2009) trong bài toán định vị

phƣơng tiện mặt đất

Chủ trì. 4 Đề tài NCS 2010

Đã nghiệm T2010-ĐĐT-15:

thu Nghiên cứu ứng dụng hệ

(2010) đa vi vử xử lý trong điều 2010

khiển chuyển động.

Chủ trì. 5 Đề tài NCS 2011

Đã nghiệm T2011-ĐĐT- 15 2011

thu Kết hợp La bàn điện tử

(2011) trong hệ thống INS/GPS

6 Đề tài Dự án GDDH GD2

Chủ trì. EEC 8.13

Nghiên cứu thiết kế thiết Dự án giáo dục Đã nghiệm

thu bị định vị kết hợp hệ thống 2009-2011 đại học, giai

(2011) GPS và hệ thống đo quán đoạn 2

tính (INS) trên công nghệ

vi cơ điện tử (MEMS)

DA chế thử - Cấp nhà nước: KC06.DA08/11-15

KC.06/11-15:

Nghiên cứu, Chủ nhiệm Hoàn thiện công nghệ chế 2012 - 2013 phát triển và nhánh tạo thiết bị kiểm soát hành ứng dụng công Đã nghiệm trình phƣơng tiện giao nghệ tiên tiến thu thông ứng dụng công nghệ trong sản xuất (2013) GPS. các sản phẩm

xuất khẩu chủ

lực.

Bảng 4: Các đề tài, dự án, nhiệm vụ khoa học

VII

PHỤ LỤC 2: KẾT QUẢ ĐÁNH GIÁ TẠI TRENTO, ITALIA

VIII

PHỤ LỤC 3: MỘT SỐ DỮ LIỆU VÀ ĐOẠN MÃ CHƢƠNG TRÌNH

1. Cấu trúc dữ liệu

TimeStamp [s];

- Hours [h];

- Minutes [min];

Seconds [s];

LatitudeDeg [°];

LatitudeMin [min];

LongitudeDeg [°];

LongitudeMin [min];

Speed [km/h;

- Heading Angle [°];

- Day;

- Month;

Year;

#Num. of Satellites;

- Altitude [m];

- Height of Geoid [m]

 Cấu trúc dữ liệu GPS

Bảng 5: Cấu trúc dữ liệu GPS

...

Time stamp [s]

Yaw angle [°]

- Roll angle [°]

- Pitch angle [°]

- Accelerometer X axis

[m/s^2]

- Accelerometer Y axis

[m/s^2]

- Accelerometer Z axis

[m/s^2]

- Gyroscope 'X' axis [°]

- Gyroscope 'Y' axis [°]

- Gyroscope 'Z' axis [°]

- Magnetometer 'X' axis [°]

- Magnetometer 'Y' axis [°]

- Magnetometer 'Z' axis [°]

 Cấu trúc dữ liệu INS

...

Bảng 6: Cấu trúc dữ liệu INS

Time stamp [s]

- WheelSpeed [m/s]

- Parking Brake Status []

- Brake Status []

- Accelerator Pedal Position [%]

Total Fuel Consumed [l]

 Cấu trúc dữ liệu FMS

XII

- -

Fuel Level [%] Engine Speed [rpm]

Total Distance [m]

- Direction []

Tachograph speed [m/s]

- Ambient Temperature [°C]

Fuel Rate [l/h]

Instant. Fuel Economy [km/l]

- DoorStatus []

- Bellow Pressure FL [kPa]

- Bellow Pressure FR [kPa]

- Bellow Pressure RL [kPa]

- Bellow Pressure RR [kPa]

...

Bảng 7: Cấu trúc dữ liệu FMS

XIII

/* This my file is a part for the Razor Stick INS 9-DOF Firmware */

// ------------------------------------------------------------

void Compass_Heading()

{

float mag_x;

float mag_y;

float cos_roll;

float sin_roll;

float cos_pitch;

float sin_pitch;

cos_roll = cos(roll);

sin_roll = sin(roll);

cos_pitch = cos(pitch);

sin_pitch = sin(pitch);

// Tilt compensated magnetic field X

mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch +

magnetom[2] * cos_roll * sin_pitch;

// Tilt compensated magnetic field Y

mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;

// Magnetic Heading

MAG_Heading = atan2(-mag_y, mag_x); // P so với G

}

2. Chƣơng trình DCM và dữ liệu ở chế độ tĩnh

XIV

// -------------------------------------------

// Computes the dot product of two vectors

float Vector_Dot_Product(const float v1[3], const float v2[3]) // dot product

{

float result = 0;

for(int c = 0; c < 3; c++)

{

result += v1[c] * v2[c];

}

return result;

}

// Computes the cross product of two vectors

// out has to different from v1 and v2!

void Vector_Cross_Product(float out[3], const float v1[3], const float v2[3])

//cross product

{

out[0] = (v1[1] * v2[2]) - (v1[2] * v2[1]);

out[1] = (v1[2] * v2[0]) - (v1[0] * v2[2]);

out[2] = (v1[0] * v2[1]) - (v1[1] * v2[0]);

}

// Multiply the vector by a scalar

void Vector_Scale(float out[3], const float v[3], float scale) // Tinh Ty le -

tich vo huong, khac voi dot product

{

for(int c = 0; c < 3; c++)

{

out[c] = v[c] * scale;

}

// Adds two vectors

void Vector_Add(float out[3], const float v1[3], const float v2[3])

{

for(int c = 0; c < 3; c++)

{

out[c] = v1[c] + v2[c];

}

// Multiply two 3x3 matrices: out = a * b

void Matrix_Multiply(const float a[3][3], const float b[3][3], float out[3][3])

{

for(int x = 0; x < 3; x++) // rows – quet hang

{

for(int y = 0; y < 3; y++) // columns – quet cot

{

out[x][y] = a[x][0] * b[0][y] + a[x][1] * b[1][y] + a[x][2] * b[2][y];

}

// Multiply 3x3 matrix with vector: out = a * b

void Matrix_Vector_Multiply(const float a[3][3], const float b[3], float out[3])

{

for(int x = 0; x < 3; x++)

{

out[x] = a[x][0] * b[0] + a[x][1] * b[1] + a[x][2] * b[2];

}

// Init rotation matrix using euler angles

void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll)

{

float c1 = cos(roll);

float s1 = sin(roll);

float c2 = cos(pitch);

float s2 = sin(pitch);

float c3 = cos(yaw);

float s3 = sin(yaw);

// Euler angles, right-handed, intrinsic, XYZ convention

// (which means: rotate around body axes Z, Y', X'')

m[0][0] = c2 * c3;

m[0][1] = c3 * s1 * s2 - c1 * s3;

m[0][2] = s1 * s3 + c1 * c3 * s2;

m[1][0] = c2 * s3;

m[1][1] = c1 * c3 + s1 * s2 * s3;

m[1][2] = c1 * s2 * s3 - c3 * s1;

m[2][0] = -s2;

m[2][1] = c2 * s1;

m[2][2] = c1 * c2;

XVI

}

// DCM algorithm chay ngon roai ;)))

// -------------------------------------------

void Normalize(void)

{

float error=0;

float temporary[3][3];

float renorm=0;

error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; // my eq

4.18,19

Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq 4.19

Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq 4.19

Vector_Add(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq 4.19

Vector_Add(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq 4.19

Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c=

a x b //my eq 4.20

renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //my

eq 4.21

Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);

renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //my

eq 4.21

Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);

renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //my

eq 4.21

Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);

}

// -------------------------------------------

void Drift_correction(void)

{

float mag_heading_x;

float mag_heading_y;

float errorCourse;

//Compensation the Roll, Pitch and Yaw drift.

// PI

XVII

static float Scaled_Omega_P[3];

static float Scaled_Omega_I[3];

float Accel_magnitude;

float Accel_weight;

//*****Roll and Pitch***************

// Calculate the magnitude of the accelerometer vector

Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] +

Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);

Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity. // My e.4.27

// Dynamic weighting of accelerometer info (reliability filter)

// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)

Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); //

// hieu chinh roll-pitch qua bo PI

Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]);

//adjust the ground of reference My e.4.28

Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);

Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);

// My eq 4.29

Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);

//*****YAW***************

// make the gyro YAW drift correction based on compass magnetic heading

mag_heading_x = cos(MAG_Heading); // My eq 4.22

mag_heading_y = sin(MAG_Heading); // My eq 4.22

errorCourse=(DCM_Matrix[0][0]*mag_heading_y) -

(DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error -> my eq4.23

Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw

correction to the XYZ rotation of the aircraft, depeding the position. My eq4.24

// hieu chinh yaw qua bo PI

Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01 proportional of YAW.

P (tỉ lệ) của Yaw // My eq 4.29

Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.

Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001 Integrator -> I

(tích phân) của Yaw // My eq 4.29

Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I

}

/**************************************************/

XVIII

void Accel_adjust(void)

{

Accel_Vector[1] += Accel_Scale(speed_3d*Omega[2]); // Centrifugal force on

Acc_y = GPS_speed*GyroZ

Accel_Vector[2] -= Accel_Scale(speed_3d*Omega[1]); // Centrifugal force on

Acc_z = GPS_speed*GyroY

}

/**************************************************/

void Matrix_update(void)

{

Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll

Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch

Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw

Accel_Vector[0]=accel[0];

Accel_Vector[1]=accel[1];

Accel_Vector[2]=accel[2];

Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional

term

Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term

//Accel_adjust(); //Remove centrifugal acceleration

// Drift correction -> Update Omega – OK chay ngon roai :))

Update_Matrix[0][0]=0;

Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z

Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y

Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z

Update_Matrix[1][1]=0;

Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];//-x

Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];//-y

Update_Matrix[2][1]=G_Dt*Omega_Vector[0];//x

Update_Matrix[2][2]=0;

Matrix_Multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c

for(int x=0; x<3; x++) //Matrix Addition (update)

{

for(int y=0; y<3; y++)

{

XIX

DCM_Matrix[x][y]+=Temporary_Matrix[x][y];

}

void Euler_angles(void)

{

pitch = -asin(DCM_Matrix[2][0]);

roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);

yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);

}

/* This my file is part of the Razor AHRS Firmware */

void output_custom() // BinhNGO's new form

{

if (output_format == OUTPUT__FORMAT_TEXT)

{

Serial.print("$INS");Serial.print(",");

Serial.print("Y");Serial.print(TO_DEG(yaw));Serial.print(",");

Serial.print("P");Serial.print(TO_DEG(pitch)); Serial.print(",");

Serial.print("R");Serial.print(TO_DEG(roll)); Serial.print(",");

Serial.print("Ax"); Serial.print(accel[0]); Serial.print(",");

Serial.print("Ay");Serial.print(accel[1]); Serial.print(",");

Serial.print("Az"); Serial.print(accel[2]); Serial.print(",");

Serial.print("Mx"); Serial.print(magnetom[0]); Serial.print(",");

Serial.print("My"); Serial.print(magnetom[1]); Serial.print(",");

Serial.print("Mz"); Serial.print(magnetom[2]); Serial.print(",");

Serial.print("Gx"); Serial.print(gyro[0]); Serial.print(",");

Serial.print("Gy"); Serial.print(gyro[1]); Serial.print(",");

Serial.print("Gz"); Serial.print(gyro[2]); Serial.println("*");

}

// ...

Bảng 8: Dữ liệu INS bị trôi khi không sử dụng thuật toán tự động hiệu chỉnh

ma trận quay tính từ bƣớc tính thứ nhất (B8.1), 500 (B8.2), 1000 (B8.3),

(B8.1): Y-11.11, P3.58, R3.96

(B8.2) : Y-2.17, P-18.54, R-27.26

Dữ liệu tại bƣớc tính 1, 2 và 3 là nhiễu ngẫu nhiên khi khởi tạo hệ thống. Dữ liệu thay đổi từ bƣớc tính 1 tới 30 là khoảng 10 cho các góc yaw, pitch và roll. Cứ mỗi 30 bƣớc tính tiếp theo (trong một khung hình) dữ liệu sai lệch khoảng 10 – 20 cho các góc yaw, pitch và roll.

2000 (B8.4), 3000 (B8.5), 4000 (B8.6)

XXI

Dữ liệu tiếp tục tích lũy thay đổi trong suốt quá trình khảo sát, đến bƣớc tính 500, các góc

yall, pitch roll thay đổi giá trị từ (Y-11.11, P3.58, R3.96) tới (Y-2.17, P-18.54, R-27.26).

(B8.3): Y20.22, P-31.51, R-67.11

(B8.4): Y64.05, P-9.19, R-146.54

Từ bƣớc tính 500 đến bƣớc tính 1000, các góc yall, pitch và roll thay đổi giá trị từ (Y-2.17,

P-18.54, R-27.26) tới (Y20.22, P-31.51, R-67.11).

Từ bƣớc tính 1000 đến bƣớc tính 2000, các góc yall, pitch và roll thay đổi giá trị từ

(Y20.22, P-31.51, R-67.11) tới (Y64.05, P-9.19, R-146.54).

XXII

(B8.5): Y56.17, P36.35, R147.86

(B8.6): Y-1.26, P34.44, R49.25

Từ bƣớc tính 2000 đến bƣớc tính 3000, các góc yall, pitch và roll thay đổi giá trị từ

(Y64.05, P-9.19, R-146.54) tới (Y56.17, P36.35, R147.86).

Từ bƣớc tính 3000 đến bƣớc tính 4000, các góc yall, pitch và roll thay đổi giá trị từ

(Y56.17, P36.35, R147.86) tới (Y-1.26, P34.44, R49.25).

Trong chế độ tĩnh mà các góc quay cơ sở này bị tích lũy sai số, hoặc luôn tăng, hoặc luôn

giảm, làm sai lệch các tính toán tiếp theo dẫn tới sai lệch về vận tốc và khoảng dịch

chuyển. Đây cũng là lý do các hệ INS trƣớc đây không làm việc độc lập đƣợc.

XXIII

Bảng 9: Dữ liệu INS không bị trôi khi sử dụng thuật toán thuật toán tự động

hiệu chỉnh ma trận quay tính từ bƣớc tính thứ nhất (B9.1), 500 (B8.2), 1000

(B9.1): Y-14.58, P2.22, R1.86

(B9.2): Y-14.36, P2.48, R2.10

Dữ liệu tại bƣớc tính số 1 là nhiễu ngẫu nhiên khi khởi tạo hệ thống. Dữ liệu không bị sai

số tích lũy, tức là không thay đổi cộng dồn trong suốt quá trình khảo sát mà chỉ thay đổi rất

(B8.3), 2000 (B8.4), 3000 (B8.5), 4000 (B8.6)

XXIV

nhỏ quanh điểm cân bằng. Các dữ liệu này tự động đƣợc hiệu chỉnh, các giá trị liên tục có giá trị xấp xỉ nhau tăng và giảm nhỏ dƣới 10 tới max là trên 20.

Từ bƣớc tính đầu đến bƣớc tính 500, các góc yall, pitch và roll thay đổi tăng giảm giá trị từ

(Y-14.58, P2.22, R1.86) tới (Y-14.36, P2.48, R2.10).

(B9.3): Y-14.19, P2.58, R2.29

(B9.4): Y-14.13, P2.73, R2.51

Từ bƣớc tính 500 đến bƣớc tính 1000, các góc yall, pitch và roll thay đổi tăng giảm giá trị

từ (Y-14.36, P2.48, R2.10) tới (Y-14.19, P2.58, R2.29).

XXV

Từ bƣớc tính 1000 đến bƣớc tính 2000, các góc yall, pitch và roll thay đổi tăng giảm giá trị

từ (Y-14.19, P2.58, R2.29) tới (Y-14.13, P2.73, R2.51).

(B9.5): Y-14.03, P2.77, R2.25

(B9.6): Y-14.04, P2.81, R2.51

Từ bƣớc tính 2000 đến bƣớc tính 3000, các góc yall, pitch và roll thay đổi tăng giảm giá trị

từ (Y-14.13, P2.73, R2.51) tới (Y-14.03, P2.77, R2.25).

Từ bƣớc tính 3000 đến bƣớc tính 4000, các góc yall, pitch và roll thay đổi tăng giảm giá trị

từ (Y-14.03, P2.77, R2.25) tới (Y-14.04, P2.81, R2.51).

XXVI

 Matlab function for busses

% ---------------------------------- % Dynamical model function for busses function x = busses_f_enhanced(xw,param) dt = param{1}; x = xw(1:9,:); dot_x = zeros(size(x)); dot_x(1,:) = x(5,:); dot_x(2,:) = x(5,:).*cos(x(4,:)); dot_x(3,:) = -x(5,:).*sin(x(4,:)); dot_x(4,:) = x(9,:); dot_x(5,:) = x(8,:)-9.8*sin(x(7,:)); dot_x(6,:) = x(5,:).*sin(x(7,:)); dot_x(7,:) = zeros(1,size(x,2)); dot_x(8,:) = zeros(1,size(x,2)); dot_x(9,:) = zeros(1,size(x,2)); % Euler integration x = x + dt * dot_x; % ---------------------------------- % Jacobian of the state transition function in busses function da = busses_df_dx_enhanced(x,param) % Jacobian for ukf dt = param{1}; df = zeros(9,9); df(1,5) = 1; df(2,5) = cos(x(4)); df(2,4) = -x(5).*sin(x(4)); df(3,5) = -sin(x(4)); df(3,4) = -x(5).*cos(x(4)); df(4,9) = 1; df(5,7) = -9.8.*cos(x(7)); df(5,8) = 1; df(6,5) = sin(x(7)); df(6,7) = x(5).*cos(x(7)); da = eye(size(df,1)) + dt * df; %... % ---------------------------------- % Filtering_autobus_2.m % Script Filtering UKF fprintf ('Entering in the script UKF\n'); fprintf ('\n'); %% generation parameters % Matrix for the states not determined L = [0 0 0 ; 0 0 0 ; 0 0 0 ; 0 0 0 ; 0 0 0 ; 0 0 0 ; 1 0 0 ; 0 1 0 ;

3. Script Matlab cho chƣơng trình tại trạm

XXVII

0 0 1 ]; % Parameters for the dynamic model d_param = {dt2}; h_param = {dt2}; % Initial covariance matrix (take the measures on the states) P0 = diag([0.1 0.25 0.7 0.01 0.001 5 0.01 0.01 0.001]); % Covariance matrix were not determined (white noise) Qc = diag([0.002 0.1 0.005]); std^2 di alpha_dot, Acc_dot, Omega_dot % Initial values of the states (I assume by the measures) m0=[0; GPSX2(IndexGPSIn); GPSY2(IndexGPSIn); mean(GPSHead2(IndexGPSIn:IndexGPSIn+5)); 0 ; GPSZ2(IndexGPSIn); 0 ; IMUAccLong2(IndexGPSIn); 0]; % calculation of Q Q = L*Qc*L'*dt2; % Handles the functions containing the dynamic and derivatives %(Required view the non-linearity) func_f = @busses_f_enhanced; func_df = @busses_df_dx_enhanced; % Matrix Measures %(sufficient for linear) H = diag([1 1 1 1 1 1 1 1 1]); % Initialization state and covariance P = P0; m = m0; % Covariance matrix of measures % sFMS XGPS YGPS HeadGPS VGPS ZGPS AlphaGPS ALonIMU OmegaIMU R = diag([100 0.2+1/(3*m(5)+1) 0.6+1.5/(3*m(5)+1) 0.01 0.01 5 0.1 0.1 0.005]); % Initializing arrays of results MM_EKF = zeros(size(m,1),size(Y2,2)); PP_EKF = zeros(size(m,1),size(m,1),size(Y2,2)); VV_EKF = zeros(size(m,1),size(Y2,2)); % Parameters for the block of the filter at low speed h=0; StopFix2=1; sogliav=1/3.6; %% Unscented Kalman filter fprintf('Running UKF...'); Stopi=0; m = m0; P = P0; MM_UKF = zeros(size(m,1),size(Y2,2)); PP_UKF = zeros(size(m,1),size(m,1),size(Y2,2));

XXVIII

VV_UKF = zeros(size(m,1),size(Y2,2)); % Filtering with UKF for k=1:size(Y2,2) % Non-augmented UKF [m,P] = ukf_predict1(m,P,func_f,Q,{dt2}); if mod(k*dt2,dt)==0 h=dt2/dt*k; [m,P] = ukf_update1(m,P,Y(:,h),H,R); else [m(8:9),P(8:9,8:9)] = ukf_update1(m(8:9),P(8:9,8:9),Y2(:,k),diag([1 1]),diag([0.1 0.005])); end % Augmented UKF with same sigma points for predict and update steps %[m,P,X_s,w] = ukf_predict3(m,P,func_f,Qc*dt,R,d_param); %[m,P] = ukf_update3(m,P,Y(:,k),H,R,X_s,w,d_param); %H va aumentato! % Block the filter at low speed if StopFix2==1 && k>1 && h>1 if Y(5,h-1)sogliav && Stopi~=0 Stopi=0; end if Stopi>0 m = MM_UKF(:,k-1); P = PP_UKF(:,:,k-1); m(5)=0; end end MM_UKF(:,k) = m; PP_UKF(:,:,k) = P; VV_UKF(:,k) = diag(P); end fprintf('Done!\n'); % ---------------------------------- % Fil_FiltraggioPreUKF_2 fprintf('Enter in the script of data processing: \n'); fprintf('\n'); % Imposed sampling frequencies for all data dt=1; % period GPS (1s) dt2=0.2; % Period dt2 = 0.1 StopFix = 0; % The input data are changed by imposing restrictions during a stop DistFix = 0; % Distance is used as the integral of the speed fprintf ('Extracting data ...'); % I pull the information needed to UKF GPSVel = getData(GPS,'Speed'); GPSTn = getData(GPS,'t'); GPSHead = getData(GPS,'Heading');

XXIX

GPSlat = getData(GPS,'LatDeg')+getData(GPS,'LatMin')/60; GPSlon = getData(GPS,'LonDeg')+getData(GPS,'LonMin')/60; GPSAlt = getData(GPS,'Alt'); FMSTime = getData(FMS,'t'); FMSSpeed = getData(FMS,'Vehicle speed'); FMSTotalDistance = getData(FMS,'TotalDist'); IMUTime = getData(IMU,'t'); % IMU varies as a function of the vehicle if (veicolo == 725)||(veicolo == 726) IMUOmega = -getData(IMU,'Gyrz'); %*0.06957 deg/s IMUAccLong = getData(IMU,'Accx'); %*0.004*9.8 m/s2 xGPS=7; yGPS=-1; xIMU=7; yIMU=1; elseif (veicolo == 727) IMUOmega = -getData(IMU,'Gyrz'); IMUAccLong = -getData(IMU,'Accy'); xGPS=7; yGPS=-1; xIMU=7; yIMU=1; elseif (veicolo == 729) IMUOmega = getData(IMU,'Gyrz'); IMUAccLong = -getData(IMU,'Accy'); xGPS=7; yGPS=-1; xIMU=7; yIMU=0; else warning('Omega and Acc may be invalid'); IMUOmega = getData(IMU,'Gyrz'); IMUAccLong = -getData(IMU,'Accy'); xGPS=7; yGPS=-1; xIMU=7; yIMU=0; end % Extract information Ancillary GPSSat = getData(GPS,'SatNum'); GPSTime = getData(GPS,'hours')+getData(GPS,'minutes')/60; FMSPedalPos = getData(FMS,'PedalPos'); if (veicolo == 725)||(veicolo == 726)||(veicolo == 727) FMSTotalFuel = getData(FMS,'TotalFuel'); else FMSFuelRate = getData(FMS,'FuelRate'); end if Radar>0 % Radar tRadar = getData(RAD,'t'); %Check Radar if tRadar(end)<(IMUTime(end)-30) warning('Radar data error: probably an interruption is occured before the end, consider the exclusion of this log') NoRadar = 1;

XXX

return end %Extract Tracks %... %% Calculate the missing information %XYZ --> change with new function in case you want to impose origin, north, west, n [XYZ, origin, nord, ovest, n] = GPS2XYZ(GPSlon,GPSlat,GPSAlt); GPSX = XYZ(:,1); GPSY = XYZ(:,2); GPSZ = XYZ(:,3); % Precision ground tilt Vsoglia = 8; %10km/h GPSAlpha = []; AlphaTime = []; for i=1:(length(GPSAlt)-1) if GPSVel(i)>Vsoglia GPSAlpha = [GPSAlpha atan((XYZ(i+1,3)-XYZ(i,3))./((XYZ(i+1,1)- XYZ(i,1)).^2+(XYZ(i+1,2)-XYZ(i,2)).^2).^0.5)]; AlphaTime = [AlphaTime GPSTn(i)]; end end % Approximation fuel consumption instantaneous fuel consumption VH %... fprintf('Done!\n') %% Conditioning and data conversion Dt1 % Interpolated to measure all Dt1 fprintf ('Filtering data1 ...'); %--------------------------GPS--------------------------- % The GPS data is lacking in some circumstances, it is therefore necessary % interpolate the data for those moments (default = linear) GPSTn2 = GPSTn(1):dt:GPSTn(length(GPSTn)); GPSVel2 = interp1(GPSTn,GPSVel,GPSTn2); GPSX2 = interp1(GPSTn,GPSX,GPSTn2); GPSY2 = interp1(GPSTn,GPSY,GPSTn2); GPSZ2 = interp1(GPSTn,GPSZ,GPSTn2); GPSHead2 = interp1(GPSTn,GPSHead,GPSTn2)*2*pi/360; GPSSat2 = interp1(GPSTn,GPSSat,GPSTn2,'nearest'); GPSTime2 = interp1(GPSTn,GPSTime,GPSTn2); % Filter and interpolated alpha windowSize = 3; GPSAlpha = filtfilt(ones(1,windowSize)/windowSize,1,GPSAlpha); GPSAlpha2 = interp1(AlphaTime,GPSAlpha,GPSTn2,'linear',0); % I eliminate jumps nell'heading so as to have continuity in the derivative turn=0;

XXXI

for i=2:length(GPSHead2) if (GPSHead2(i-1)<(turn*2*pi+0.8))&&(GPSHead2(i)>(turn*2*pi+2*pi- 0.8)) GPSHead2(i:length(GPSHead2))= GPSHead2(i:length(GPSHead2))-2*pi; turn = turn - 1; else if (GPSHead2(i-1)>(turn*2*pi+2*pi- 0.8))&&(GPSHead2(i)<(turn*2*pi+0.8)) GPSHead2(i:length(GPSHead2))= GPSHead2(i:length(GPSHead2))+2*pi; turn = turn + 1; end end end %--------------------------FMS--------------------------- % For the FMC is not interpolate but to reduce the sampling frequency to that of the GPS for i=2:(length(FMSTime)-1) if FMSTime(i)<=FMSTime(i-1) FMSTime(i)= mean([FMSTime(i-1) FMSTime(i+1)]); end end % Prior to resample the data transformed by distance in a continuous variable and then ricampiono NewTime(1) = 0; ist=1; for i=1:length(FMSTime) if FMSTotalDistance(i)~=0 NewTotDist(1) = FMSTotalDistance(i); ist=i; break end end j=2; for i=ist:(length(FMSTime)-1) if (FMSTotalDistance(i+1) > FMSTotalDistance(i)) NewTime(j) = FMSTime(i+1); NewTotDist(j) = FMSTotalDistance(i+1); j=j+1; end end FMSTotalDistance2 = interp1(NewTime,NewTotDist,GPSTn2,'pchip'); % I correct errors of extrapolation for i=2:length(GPSTn2) if FMSTotalDistance2(i)

XXXII

Fc = 5; % Cutoff Frequency % Construct an FDESIGN object and call its BUTTER method. h = fdesign.lowpass('N,F3dB', N, Fc, Fs); Hd = design(h, 'butter'); FMSSpeed = filtfilt(Hd.sosMatrix,Hd.ScaleValues,FMSSpeed); FMSSpeed2 = interp1(FMSTime,FMSSpeed,GPSTn2); % Prior to resample the transformed data on the fuel consumed in a continuous variable and then ricampiono if (veicolo == 725)||(veicolo == 726)||(veicolo == 727) NewTime2(1) = 0; ist=1; for i=1:length(FMSTime) if FMSTotalFuel(i)~=0 NewTotFuel(1) = FMSTotalFuel(i); ist=i; break end end j=2; for i=ist:(length(FMSTime)-1) if (FMSTotalFuel(i+1) > FMSTotalFuel(i)) NewTime2(j) = FMSTime(i+1); NewTotFuel(j) = (FMSTotalFuel(i+1)+FMSTotalFuel(i))/2; j=j+1; end end NewTime2(j) = FMSTime(end); NewTotFuel(j) = FMSTotalFuel(end); FMSTotalFuel2 = interp1(NewTime2,NewTotFuel,GPSTn2,'pchip'); else NewTotFuel = cumtrapz(FMSTime,FMSFuelRate)/3600; FMSTotalFuel2 = interp1(FMSTime,NewTotFuel,GPSTn2); end % The other data already "continuous" them ricampiono only FMSPedalPos2 = interp1(FMSTime,FMSPedalPos,GPSTn2); %--------------------------IMU--------------------------- % Even for the IMU one can speak of subsampling % Check monotony for the IMU for i=2:(length(IMUTime)-1) if IMUTime(i)<=IMUTime(i-1) IMUTime(i)= mean([IMUTime(i-1) IMUTime(i+1)]); end end % Influenced the IMU prefiltrando numerical errors present thresholds accelerometers AccThres = 500; DeltaAcc = 10; Intmean = 5; % thresholds gyroscope GyrThres = 500; GyrThres2 = 0.001;

XXXIII

DeltaGyrZ = 30; DeltaGyr = 50; % could be replaced with find (abs (diff)> threshold) for i=2:(length(IMUOmega)-1) if ((IMUOmega(i)>GyrThres)||(IMUOmega(i)<-GyrThres)||((IMUOmega(i)>- GyrThres2)&&(IMUOmega(i)((IMUOmega(i- 1))+DeltaGyrZ))||((IMUOmega(i))<((IMUOmega(i-1))- DeltaGyrZ))||((IMUOmega(i))>((IMUOmega(i+1))+DeltaGyrZ))||((IMUOmega(i))< ((IMUOmega(i+1))-DeltaGyrZ))) IMUOmega(i)=IMUOmega(i-1); end if (i>Intmean)&&(i<(length(IMUAccLong)-Intmean)) if (IMUAccLong(i)>AccThres)||(IMUAccLong(i)<- AccThres)||(((IMUAccLong(i))>((IMUAccLong(i- 1))+DeltaAcc))||((IMUAccLong(i))<((IMUAccLong(i-1))- DeltaAcc))||((IMUAccLong(i))>((IMUAccLong(i+1))+DeltaAcc))||((IMUAccLong( i))<((IMUAccLong(i+1))-DeltaAcc))) if ((IMUAccLong(i)<(nanmean(IMUAccLong(i-Intmean:i- 1))+DeltaAcc))&&(IMUAccLong(i)>(nanmean(IMUAccLong(i-Intmean:i-1))- DeltaAcc)))||((IMUAccLong(i)<(nanmean(IMUAccLong(i+1:i+Intmean))+DeltaAcc ))&&(IMUAccLong(i)>(nanmean(IMUAccLong(i+1:i+Intmean))-DeltaAcc))) else IMUAccLong(i)=IMUAccLong(i-1); end end end end % I correct offset given by the average nothing for the gyro and the accelerometer IMUAccLong(isnan(IMUAccLong)) = 0; IMUOmega(isnan(IMUOmega)) = 0; IMUAccLongZero = mean(IMUAccLong); IMUOmegaZero = mean(IMUOmega); IMUOmega = IMUOmega - IMUOmegaZero; IMUAccLong = IMUAccLong - IMUAccLongZero; % Filter with a low pass and / or with moving average and / or with lpc % does not change much for the result seen that the filter EKF should % already do this work PreFilter = 1; %0=NONE,1=Lowpass,2=LPC,3=MovingAverage,4=ALL(1->2->3) if PreFilter>0 if PreFilter==1||PreFilter==4 % Low Pass Fs = 50; % Sampling Frequency N = 2; % Order Fc = 10; % Cutoff Frequency % Construct an FDESIGN object and call its BUTTER method. h = fdesign.lowpass('N,F3dB', N, Fc, Fs); Hd = design(h, 'butter'); %IMUOmega = filter(Hd,IMUOmega); IMUOmega = filtfilt(Hd.sosMatrix,Hd.ScaleValues,IMUOmega); %IMUAccLong = filter(Hd,IMUAccLong); IMUAccLong = filtfilt(Hd.sosMatrix,Hd.ScaleValues,IMUAccLong); elseif PreFilter==2||PreFilter==4

XXXIV

%filtro "Dinamico" lpc coeff = lpc(IMUOmega,3); IMUOmega = filter([0 -coeff(2:end)],1,IMUOmega); coeff = lpc(IMUAccLong,3); IMUAccLong = filter([0 -coeff(2:end)],1,IMUAccLong); elseif PreFilter==3||PreFilter==4 % Media mobile windowSize = 5; IMUOmega = filter(ones(1,windowSize)/windowSize,1,IMUOmega); IMUAccLong = filter(ones(1,windowSize)/windowSize,1,IMUAccLong); end end % I sync Omega GPSOmegaEst=diff(GPSHead2)/dt; DT=0; DTdiffmin=0; diffmin=100000000000; while DT<(5) % DT max 5 seconds diffO = 0; for GPSi = round((length(GPSTn2))/2)- 300:round((length(GPSTn2))/2)+300 IMUi=find(IMUTime>=GPSTn2(GPSi)+DT,1); gap2=(IMUOmega(IMUi)*0.06957*2*pi/360-GPSOmegaEst(GPSi))^2; diffO=diffO+gap2; end if diffO

XXXV

fprintf('Done!\n') % ---------------------------------- % ukf_predict1.m % function [M,P,D] = ukf_predict1(M,P,f,Q,f_param,alpha,beta,kappa,mat) %Check arguments % if nargin < 2 error('Too few arguments'); end if nargin < 3 f = []; end if nargin < 4 Q = []; end if nargin < 5 f_param = []; end if nargin < 6 alpha = []; end if nargin < 7 beta = []; end if nargin < 8 kappa = []; end if nargin < 9 mat = []; end % % Apply defaults % if isempty(f) f = eye(size(M,1)); end if isempty(Q) Q = zeros(size(M,1)); end if isempty(mat) mat = 0; end % ---------------------------------- % Do transform and add process noise % tr_param = {alpha beta kappa mat}; [M,P,D] = ut_transform(M,P,f,f_param,tr_param); P = P + Q; % ---------------------------------- % ukf_update1.m %

XXXVI

function [M,P,K,MU,S,LH] = ukf_update1(M,P,Y,h,R,h_param,alpha,beta,kappa,mat) % % Check that all arguments are there % if nargin < 5 error('Too few arguments'); end if nargin < 6 h_param = []; end if nargin < 7 alpha = []; end if nargin < 8 beta = []; end if nargin < 9 kappa = []; end if nargin < 10 mat = []; end % % Apply defaults % if isempty(mat) mat = 0; end % % Do transform and make the update % tr_param = {alpha beta kappa mat}; [MU,S,C] = ut_transform(M,P,h,h_param,tr_param); S = S + R; K = C / S; M = M + K * (Y - MU); P = P - K * S * K'; if nargout > 5 LH = gauss_pdf(Y,MU,S); end