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.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ƣ sau:
(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ƣ sau:
(3.22)
{ ( )
} Trong đú: là giỏ trị COG trong hệ tọa độ trỏi đất tham chiếu G.
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( )
COGY = = sin( ) (3.23)
Sau đú ta tớnh toỏn hiệu chỉnh gúc yaw theo cụng thức:
Yaw( ) = rxxCOGY – ryxCOGX (3.24)
( ) coscos ( ) cossin ( )
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:
Yaw( ) = Yaw( ) [ ] (3.25) Yaw( )= [ ( ) ( )] [ ]