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ộ

Từ đó, luận án đư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 g

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

Tóm tắt LUậN áN TIếN Sĩ Kỹ THUậT

Hà Nội - 2015

Luận án được hoàn thành tại: Trường Đại học Giao thông Vận tải

Người hướng dẫn khoa học:

1 GS.TS Lê Hùng Lân

2 PGS.TS Nguyễn Thanh Hải

Phản biện 1: GS.TSKH Cao Tiến Huỳnh

Phản biện 2: PGS.TS Nguyễn Quang Hoan

Phẩn biện 3: PGS.TS Nguyễn Văn Liễn

Luận án sẽ được bảo vên trước Hội đồng chấm luận án tiến sĩ cấp Trường họp tại: ………

vào hồi ……… giờ, ngày … tháng …… năm 2015

Có thể tìm hiểu luận án tại:

- Thư viện trường Đại học Giao thông Vận tải

- Thư viện quốc gia

[1] Chủ trì 01 đề tài nhánh trong đề tài NCKH cấp Nhà nước (KC.06.02/06-10)

và 01 nhánh Dự án chế thử trong Chương trình khoa học công nghệ trọng điểm cấp Nhà nước (KC06.DA08/11-15); 01 đề tài cấp Bộ (B2007-04-27);

01 đề tài Dự án GDDH II (EEC 8.13); 03 đề tài cấp trường (T2009-ĐĐT-31, T2010-ĐĐT-15 và T2011-ĐĐT- 15)

[2] 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 [3] 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, trang 335 – 339

[4] 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 [5] 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

[6] Nguyễn Quang Tuấn, Ngô Thanh Bình (2011); “Công nghệ tích hợp 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

INS-[7] 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

[8] 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

[9] 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, trang 92-97

[10] 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, trang 411 – 418

[11] 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; trang 361 – 376

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ừ đó, luận án đư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, tính phi tuyến của hệ thống và xử lý thời gian thực Các sản phẩm thực tế phát triển trên cơ sở giải pháp của luận án đã được ứng dụng thành công trong thực tế

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 đường bộ là phát triển hiệu quả việc quản lý, điều hành các phương tiện và kiểm tra các trạng thái của chú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à vận tốc theo 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ố về dao động của đối tượng Thông thường các thông số này bao gồm: gia tốc, các góc nghiêng, góc hướng thể hiện hướng di chuyển của đối tượng và một số thông số về tình trạng hoạt động của đối tượng

Hiện nay, ngoài các phương pháp xác định vị trí dựa vào đặc tính chuyển động tương đối của đối tượng so với điểm mốc đã biết như hải đăng, các vì sao, radar hay sóng vô tuyến, 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 thu được từ hệ thống GPS

 Phương pháp sử dụng hệ dẫn đường quán tính (INS) Dựa trên trạng thái ban đầu

đã biết, ta đo các vector gia tốc và vận tốc góc quay theo các hướng để tính toán xác định vị trí tiếp theo của đối tượng chuyển động

Một hệ thống quản lý 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ề vị trí

và vận tốc trong khoảng thời gian dài nhưng tốc độ đưa ra dữ liệu 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 Nó còn bị ảnh hưởng bởi trọng lực, nhưng có các ưu điểm là tốc độ

tính toán cao và tính được đầy đủ được các thông số gia tốc cũng như các góc dao động theo các hướ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 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 và nhiều lĩnh vực hoạt động phục vụ con người, đặc biệt là trong lĩnh vực giao thông vận tải 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ó chất lượng tốt, nhưng có giá thành rất rẻ, cỡ 30 euro/1 thiết bị Do đó việc tích hợp INS trên các thiết bị giám sát hành trình sử dụng modul thu tín hiệu GPS là việc nên làm và sẽ dần trở thành tiêu chuẩn của hệ thống Ngoài tính năng hỗ trợ cho GPS, 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

Mục tiêu đầu tiên của luận án là nghiên cứu cơ sở khoa học và khả năng tích hợp

hệ thống GPS và INS Từ đó, luận án đã đưa ra một số khả năng phát triển ứng dụng thực tế trên cơ sở cập nhật công nghệ mới Trên cơ sở này, luận án đã thiết kế chế tạo thành công thiết bị tích hợp GPS/INS sử dụng các thiết bị và giải pháp xử lý dữ liệu tiên tiến nhằm nâng cao chất lượng hệ thống giám sát Đối tượng áp dụng của luận án

là các phương tiện giao thông đường bộ, cụ thể là xe bus

Trong trường hợp mất tín hiệu GPS 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 riêng tín hiệu INS

mà không có sự hỗ trợ của các hệ thống khác thì kết quả định vị sẽ không còn chính xác nữa Luận án này 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 các trường hợp đối tượng hoạt động trong môi trường đóng kín hay mất GPS tín hiệu trong khoảng thời gian 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.

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 thiết kế thiết bị tích hợp GPS/INS nhằm cải thiện chất lượng của các hệ thống giám sát phương tiện chuyển động 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

 Ý 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ộ

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 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 mới 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

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

1.1 Hệ thống GPS

GPS là hệ thống định vị toàn cầu của Mĩ, gồm: Các vệ tinh không gian, Các trạm điều khiển mặt đất và Bộ thu tín hiệu Người sử dụng dùng module thu để nhận thông tin từ GPS, xử lý và đưa ra thông số giám sát đối tượng chuyển động

1.2 Hệ thống INS

Hệ thống hỗ trợ điển hình cho GPS trong giám sát chuyển động là hệ thống đo lường quán tính INS Dựa trên trạng thái ban đầu đã biết, người sử dụng dùng INS đo gia tốc và góc quay theo các hướng, từ đó tính toán xác định vị trí tiếp theo và các thông số thể hiện trạng thái của đối tượng chuyển động

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

Tích hợp các công nghệ dẫn đường sẽ cho ra một hệ thống hoạt động tốt hơn Việc tiền xử lý dữ liệu INS và bù dữ liệu giữa hai hệ thống GPS/INS sẽ tạo ra một hệ thống có độ chính xác cao hơn, đồng thời có được các thông số thể hiện trạng thái chuyển động của đối tượng Giải pháp bù dữ liệu để nâng cao chất lượng hệ thống giám sát thường sử dụng bộ lọc Kalman

CHƯƠNG 2 LỌC KALMAN VÀ MATLAB TOOL-BOX

2.1 Lọc Kalman, EKF và UKF

Chương này trình bày về lọc Kalman, EKF và UKF Sau khi phân tích lý thuyết, luận án tiến hành mô phỏng các thuật toán lọc sử dụng tín hiệu phát trộn nhiễu ngẫu nhiên trên cơ sở phát triển hệ thống thư viện Matlab Tool-box Kalman của Jouni Hartikainen [31] Các kết quả này được tiếp tục phát triển ứng dụng trong chương 4 cho đối tượng thực tế là xe bus

2.2 Phát triển Tool-box và kết quả mô phỏng

Để làm sáng tỏ các lý thuyết về lọc KF, EKF và UKF nhằm đưa vào áp dụng trong phần thiết kế cho đối tượng cụ thể trong chương sau, luận án triển khai các thuật

toán lọc và phát triển Tool-box [31] trên cơ sở sử dụng hàm phát sine ngẫu nhiên theo

mô hình vận tốc Wiener [53] Các tham số hệ số biên độ, vận tốc góc và độ lớn của góc được thay đổi, trộn với nhiễu ngẫu nhiên lớn và hạn biên tín hiệu

k k k

k k k k k

• a k là hệ số biên độ tại bước tính thứ k

• là nhiễu Gaussian ồn trắng đơn biến tại bước tính thứ k





Hình 2.7, 2.8: Kết quả mô phỏng với tín hiệu phát trộn nhiễu ngẫu nhiên

Kết quả mô phỏng với tín hiệu phát trộn nhiễu ngẫu nhiên (Hình 2.7, 2.8) cho thấy quỹ đạo có biên độ dao động quanh điểm cân bằng và luôn có hướng trở về bám sát quỹ đạo thực sau khi bị dao động cưỡng bức Trên đồ thị hình 2.8, các điểm chấm (blue) là các giá trị đo đạc bị tác động của nhiễu, đường gạch (green) 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 vẫn thu được giá trị sau lọc quanh điểm ổn định Bộ lọc UKF cho kết quả tốt hơn, sai lệch nhỏ và ổn định hơn so với EKF, với biên độ dao động cưỡng bức của UKF có thể tăng từ 5 lên tới giá trị 15

Những kết quả này được 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 Đây là đóng góp mới ban đầu của luận án đã được 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]

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

CHO HỆ THỐNG INS 3.1 Phương pháp tự động hiệu chỉnh các phần tử của ma trận quay

Chuyển động của một đối tượng được 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/Globle) Chuyển động của đối tượng có thể biểu diễn bằng ba phép quay liên tục, tính toán trên cơ sở ma trận quay R (Rotation matrix)

Ma trận quay R còn được gọi là ma trận cosine chỉ phương DCM, bởi vì mỗi phần tử được biểu diễng bằng cosine của các góc quay cơ sở roll, pitch và yaw (góc Euler)

Ma trận quay R có 9 phần tử , hay còn gọi là 9 thành phần (element), trong đó

các phần tử luôn phụ thuộc nhau Đặ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, ta 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:

Hình 3.3: Biểu diễn ma trận quay R dạng vector, đặc tính trực giao và tích chéo

Ma trận quay mô tả sự định hướng của hệ tọa độ quy chiếu này so với hệ tọa độ quy chiếu khác Với cách biểu diễn 9 phần tử của R thông qua 6 vector (x b , y b , z b ,

x e , y e , z e) như trên, ta có thể xoay đối tượng thông qua xoay các vector bằng cách nhân

chúng với ma trận quay R 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)

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

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

Giải pháp cho vấn đề lệch trực giao 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 Đị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 Xử lý sai số trên cơ sở những bước tính toán liên tiếp thông qua việc kiểm soát hiện tượng lệch trực giao, qua đó sẽ kiểm soát được hiện tượng trôi dữ liệu dẫn đến sai số tích lũy của INS

Cuối năm 2009, trong dự án của Stalino Electronics [88], William Premerlani và Sergiu Baluta giới thiệu giải pháp tự động hiệu chỉnh ma trận quay sử dụng MEMS

INS 6-DOF kết hợp tín hiệu GPS, còn gọi là lọc bổ sung DCM, có mô hình như sau:

Hình 3.4: Cấu trúc DCM của William Premerlani và Sergiu Baluta

Nhận xét rằng trong trường hợp mất tín hiệu GPS, thiết bị tích hợp GPS/INS này sẽ bị mất dữ liệu góc hướng Như vậy, việc cập nhật các phần tử của ma trận

quay trong thuật toán lọc DCM trên từ các giá trị cập nhật omega để tính góc yaw

của họ bị phá vỡ Hơn nữa, tốc độ nhận dữ liệu góc hướng từ GPS so với INS là quá chậm, và dữ liệu góc hướng từ tín hiệu GPS là không rất chính xác vì ăng ten của

module GPS không phải luôn luôn song song với hệ tham chiếu G trong thời gian di

chuyển của đối tượng Như vậy, cấu trúc này có thể bị phá vỡ trong quá trình hoạt động và cho kết quả chậm và không chính xác Luận án đã phát triển và hoàn thiện phương pháp này trên cơ sở cập nhật và tính toán theo ba vector từ trường từ IMU Mag cơ sở tích hợp trên INS 9-DOF

3.2 Phát triển thuật toán DCM trên cơ sở MEMS INS 9-DOF

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

Các MEMS INS xoay cùng với đối tượng mang, tạo ra tín hiệu tỷ lệ thuận với tốc

độ quay Khi biết các điều kiện ban đầu và thứ tự quay theo thời gian của các vector,

ta có thể biểu diễn công thức trực giao 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

Xử lý theo các hàng/cột của ma trận quay R và xem xét chúng như vector quay, ta có:

(3.14)

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 tính toán được theo 3 trụ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 góc quay dưới dạng một dạng ma trận như sau:

[

Trong thiết kế của William Premerlani và Sergiu Baluta, 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 với các tham số như sau:

Kp_ROLLPITCH = 0.02, Ki_ROLLPITCH = 0.00002;

Kp_YAW = 1.2, Ki_YAW = 0.00002

Luận án này sử dụng cùng các IMU cơ sở trên, được tích hợp tạo nên INS 9-DOF razor stick Các hệ số ban đầu của bộ điều chỉnh PI được lựa chọn như trên, được cập nhật và tiếp tục hiệu chỉnh trong suốt quá trình hoạt động của đối tượng Chúng được

phối hợp tính toán với hàm trọng lượng w trong các vòng lặp phản hồi và hệ số COG

(Course Over Ground) Ở chế độ dao động nhỏ, bất thường, hoặc có gia tốc 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, bằng công thức giới hạn giải tín hiệu (constrain):

√

| |

Điều chỉnh các tham số cho vector_scale để tính toán các tham số mới KpRP, KiRP theo hàm trọng lượng Acc_w sử dụng hàm constrain như trên Trong chế độ

làm việc bình thường, giá trị tính toán thực tế của hàm trọng lượng nằm trong khoảng

giá trị từ 0 → 1 Tham số Kp_YAW, Ki_YAW được hiệu chỉnh qua hệ số COG trên cơ

sở cập nhật 3 vector Mag

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” (Orthogonal, ký hiệu là ) gọi là "Chuẩn hóa" (Normalization) 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 R Kết quả là một phạm vi sai

lệch của hàng X và Y bị quay so với nhau, như sau:

;cossincossin

sin

cossinsinsincos

sin sin cos cos cos sin cos

yz yy yx

r r

r Y

sin sin cos cos cos sin cos cos

sin cos sin sin

cos sin sin sin cos cos

2 Y

Error X r

r Y

yz yy

r Z

zz zy zx

(3.19) Tiếp đó ta “Tái chuẩn hóa” (Renormalization, ký hiệu là ~) 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 Nhận thấy rằng độ

lớn sẽ không bao giờ khác nhau nhiều hơn 1, vì vậy có thể tái cấu trúc thông qua khai triển chuỗi Taylor mở rộng [88] Kết quả tái cấu trúc 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

Theo lý thuyết William Premerlani [88], sai số của góc yaw từ la bàn hay từ 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 Với việc sử dụng INS 9-DOF cập nhật và tính toán theo các vector từ trường thay thế cho giá trị lấy từ tín hiệu GPS, luận án đã đưa ra giải pháp giải quyết triệt để được vấn đề

độ chính xác của góc hướng và mất góc hướng

Hình 3.5, 3.6: Điều chỉnh trượt góc hướng (yaw) và góc quay (roll-pitch) Trong luận án, giá trị Mag COG được tính toán trên hệ INS 9-DOF trên cơ sở cập

nhật 3 vector từ trường Mag như sau:

  (3.22)

 

{

   

}

Trong đó: là giá trị COG trong hệ tọa độ trái đất tham chiếu G

Các vector tham chiếu từ các vector vận tốc ngang bình thường hóa (Normalized

horizontal velocity vector) được tính bằng cách dùng cosin và sin của COG trong hệ tham chiếu G như sau:

COGY = = sin

( ) coscos cossin

Để đ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 như sau:

Yaw = Yaw [

Yaw =

[ ] [

]

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

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 (e-frame), ta có thể tính toán các vector vận tốc trong khung tham chiếu P (b-frame) 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 ( ) Giá trị nàylà tích chéo của vector con quay hồi chuyển và vector vận tốc: