4. Đề xuất cỏc giải phỏp mới, nội dung và phƣơng phỏp nghiờn cứu
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.