Tài liệu Nghiên cứu xác định định hướng không gian của thiết bị bay theo các phép đo từ trường trái đất [tt]

1 MỞ ĐẦU 1. Đặt vấn đề. Đối tượng nghiên cứu trong luận án là thiết bị bay trinh sát không người lái (TBBTSKNL) với hệ thống điều khiển trên khoang là hệ dẫn đường quán tính không đế. Vấn đề là cần nghiên cứu khả năng tạo ra hệ thống cảm biến - điều khiển định vị TBB có độ chính xác bảo đảm yêu cầu, giá thành rẻ và quan trọng là khối lượng nhỏ. Việc sử dụng từ trường Trái đất trong định hướng dẫn đường cho TBB bằng cách đo chính xác hướng và độ lớn vector từ trường Trái đất nhờ những cảm biến nhỏ gọn. Những vi cảm biến từ trường có độ chính xác cao hiện được sử dụng rộng rãi trong lĩnh vực hàng không, hàng hải. Chúng cho phép tạo ra những thiết bị dẫn đường, định hướng và điều khiển cực kỳ hấp dẫn về kích thước, trọng lượng và giá thành. 2. Mục tiêu nghiên cứu của luận án: Đưa ra hướng khắc phục khó khăn khi sử dụng thông tin từ trường. Khả năng ứng dụng các bộ cảm biến từ trường trong hệ thống điều khiển định hướng không gian cho TBB. Chứng minh bằng thực nghiệm khả năng sử dụng thông tin từ trường Trái để điều khiển TBBTSKNL. 3. Phương pháp nghiên cứu: Các nội dung được nghiên cứu kết hợp phương pháp nghiên cứu và phân tích lý thuyết; Phương pháp toán vector, hình học giải tích; Phương pháp mô hình hóa toán học bằng máy tính; Phương pháp lọc phi tuyến và xử lý tín hiệu tối ưu, thích nghi; Phương pháp thực nghiệm đối chứng. 4. Nội dung của luận án Luận án bao gồm: 126 trang + 42 trang phục lục, 58 hình vẽ, 6 bảng biểu được trình bày trong 4 chương, 22 đề mục lớn nhỏ. Chương 1. Hệ dẫn đường quán tính không đế và bài toán định hướng thiết bị bay trinh sát không người lái. 2 Chương 2. Thông tin từ trường trái đất và khả năng sử dụng trong bài toán điều khiển định hướng TBBKNL Chương 3. Tổng hợp hệ thống đo vận tốc góc của thiết bị bay dựa trên lọc phi tuyến tối ưu Chương 4. Thực nghiệm đánh giá khả năng thực tế hóa hệ đo –xử lý thông tin vận tốc góc của TBB theo từ trường trái đất. Chương I HỆ DẪN ĐƯỜNG QUÁN TÍNH KHÔNG ĐẾ VÀ BÀI TOÁN ĐỊNH HƯỚNG THIẾT BỊ BAY TRINH SÁT KHÔNG NGƯỜI LÁI 1.1. Hệ thống dẫn đường quán tính Chuyển động của TBBKNL được mô tả bằng những phương trình vi phân theo định luật 2 Niu-tơn: m⋅ d 2r dt 2 = F và j⋅ dω =M dt Trong đó: F = F(α, β, V, u ) (1.2) M = M(α, β, V, u ) (1.1) (1.3) Ở đây: α và β - góc tấn công và góc trượt tương ứng của TBB trong dòng khí; V - vận tốc dòng khí tương đối so với TBB; u - vector điều khiển (vị trí của cánh lái liệng, cánh lái độ cao, cần ga điều chỉnh vòng quay động cơ để thay đổi tốc độ và nhiều cơ cấu khác). 1.2. Cấu trúc hệ dẫn đường cho máy bay không người lái Hầu hết TBBKNL sử dụng phương pháp dẫn đường quán tính (DĐQT), có sơ đồ cấu trúc thể hiện trên sơ đồ khối hình 1.2 1.3. Hệ điều khiển định hướng thân TBBKNL 1.3.1 Nguyên lý làm việc và cấu trúc Cấu trúc của một hệ ĐKĐH trong hệ ĐKQT không đế mô tả trên hình 1.3. 3 Khối cảm biến Đo VTG - Cảm biến ωx1 - Cảm biến ωy1 - Cảm biến ωz1 Đo GTT - Cảm biến Ax - Cảm biến Ay - Cảm biến Az GPS - Kinh độ λ(t) - Vĩ độ ϕ(t) - Độ cao H(t) Khí áp kế - Độ cao H(q) - Vận tốc bay V(t) Khối xử lý tín hiệu Lọc t/h VTG - Bộ lọc ωx1 - Bộ lọc ωy1 - Bộ lọc ωz1 Khối đo tọa độ góc - Thuật toán đo ωx1, ωy1, ωz1 - Thuật toán biến đổi tọa độ ϑ, ψ, γ Khối tạo lệnh ĐK Thuật toán tạo lệnh - đ/kh độ cao λϑ - đ/kh hướng λψ - đ/kh liệng λγ Khối tạo th/số ĐK - Thuật toán so sánh ∆ϑ, ∆ψ, ∆γ - Thuật toán xác định ∆’ϑ, ∆’ψ, ∆’γ λϑ λψ λγ ϑprog ψprog γprog Hình 1.3. Cấu trúc hệ ĐKĐH cho TBBKNL 1.3.2. Mô tả toán học quá trình xử lý thông tin ĐKĐH a) Phương trình động hình học Việc chuyển từ hệ TĐQT sang hệ TĐLK được thực hiện bởi 3 lần quay liên tiếp: X q Yq Z q ϑ, ψ , γ → X 1 Y1 Z 1 , C với C là ma trận cos-sin định hướng so với hệ TĐQT. cosϑ.cosψ −cosϑ.sinψ.sinγ −sinϑ.cosγ cosϑ.sinψ.cosγ +sinϑ.sinγ C = sinϑ.cosψ sinϑ.sinψ.sinγ + cosϑ.cosγ sinϑ.sinψ.cosγ −cosϑ.sinγ (1.4) −sinψ cosψ.sinγ cosψ.cosγ xq x1 yq = C y1 Khi đó: zq (1.5) z1 ω = ψ& + ϑ& + γ& (1.6) Hoặc viết dưới dạng khác: ϑ& = ( 1 ω y1 sin γ + ωz1 cos γ cos ψ ψ& = ω y 1 cos γ − ω z 1 sin γ ) (1.8) (1.9) 4 γ& = ωx1 + tgψ(ωy1 sin γ + ωz1 cos γ) (1.10) Các phương trình (1.8) và (1.10) có những điểm đặc biệt khi mà góc hướng ψ= π ± n.π . 2 Điểm đặc biệt đó có thể tránh được, theo phép biến đổi Quaternion. Các phương trình xác định tham số Roth-Hamington (1.11) – (1.15): Ma trận C thay cho ma trận cos-sin định hướng (1.4) có dạng. C11 C12 C = C 21 C 22 C31 C32 C13 C 23 C33 (1.14) Các góc gật, hướng và liệng được xác định bởi các công thức sau: ϑ = arcsin C 21; ψ = −arctg C31 C ; γ = − arctg 23 C11 C 22 (1.16) 1.4. Ý nghĩa của việc ổn định định hướng không gian cho TBB trinh sát Sai lệch tâm của ảnh dưới mặt đất khi có các sai số góc theo các trục: a) OX: ∆x = H o [tgε / 2 − tg (ε / 2 − ∆ϑ)] (1.17a) b) OZ: ∆Z = H o [tgβ / 2 − tg (β / 2 − ∆γ )] (1.18) Trong đó: Ho là độ cao bay tại thời điểm chụp ảnh; ε, β tương ứng với góc ε, β tương ứng với góc mở của camera trinh sát theo hai trục OX và OZ; ∆ϑ, ∆γ tương ứng với các sai lệch góc định hướng của TBB theo hai kênh gật và liệng. Sai lệch tâm của ảnh là: ∆ R = ∆x 2 + ∆z2 (1.19) 1.5. Đặt vấn đề cần nghiên cứu Để sử dụng TBBKNL vào mục đích trinh sát mục tiêu trên mặt đất, nếu ta coi các kênh điều khiển gật, hướng và liệng tường minh về cấu trúc, tham số và có chất lượng làm việc đáp ứng yêu cầu, thì vấn đề đặt ra cần nghiên cứu trong luận án như sau. Để nâng cao chất lượng ổn định các góc định hướng thân TBBTSKNL 5 trong những điều kiện cần giảm trọng lượng kết cấu trên khoang để tăng khối lượng nhiên liệu (tăng thời gian bay) và đảm bảo ảnh trinh sát từ trên không có chất lượng cao, ta cần phải nghiên cứu khả năng sử dụng và tích hợp những loại cảm biến công nghệ mới có trọng lượng siêu nhỏ nhưng bảo đảm chất lượng thông tin cao. Hệ thống thông tin dẫn đường TBBTSKNL ngoài yêu cầu về kích thước trọng lượng nhỏ phải là hệ dẫn đường quán tính không đế khả thi, có thể tạo thành sản phẩm thực tế. Như vậy luận án phải giải quyết những bài toán sau: Bài toán thứ nhất. Biện luận, chứng minh khả năng sử dụng các loại cảm biến công nghệ mới đo VTT Trái đất vào mục đích xác định vận tốc các góc định hướng thân TBB. Bài toán thứ hai. Nghiên cứu các biện pháp xử lý thông tin từ cảm biến đã lựa chọn dựa trên các phương pháp lọc, xử lý tối ưu. Tổng hợp và chứng minh nhờ mô phỏng trên máy tính các phương án lọc tối ưu, từ đó lựa chọn được bộ lọc đáp ứng tốt nhất yêu cầu chất lượng thông tin dưới tác động của nhiễu và môi trường. Bài toán thứ ba. Chứng minh bằng thực nghiệm đối với phương án lựa chọn và tích hợp các cảm biến; phương án lọc tối ưu; khả năng hiện thực hóa hệ thống thông tin dẫn đường bằng thiết bị và thuật toán được xây dựng. Kết luận chương 1 Chương I đã làm rõ những vấn đề sau: - Ảnh hưởng của sai số định hướng thân TBB tới chất lượng thông tin trinh sát, từ đó làm rõ yêu cầu về chất lượng của các hệ thống ổn định định hướng. - Vấn đề cần nghiên cứu của luận án dựa trên những kết quả phân tích hệ thống dẫn đường quán tính không đế mà trong đó quan trọng hơn cả là xác định được ba bài toán cần giải. - Cấu trúc, nguyên lý làm việc của hệ thống và các thuật toán dẫn đường quán tính. Quy trình xử lý các thông tin định hướng TBB. 6 Chương II THÔNG TIN TỪ TRƯỜNG TRÁI ĐẤT VÀ KHẢ NĂNG SỬ DỤNG TRONG BÀI TOÁN ĐIỀU KHIỂN ĐỊNH HƯỚNG TBBKNL 2.1. Những khái niệm cơ bản về từ trường Trái đất và cảm biến Từ trường Trái đất được đặc trưng bởi 3 đại lượng: cường độ, độ lệch và độ dốc vector cường độ từ trường. Từ trường Trái đất có một thành phần song song với bề mặt Trái đất và luôn chỉ về một hướng cực từ Bắc. Đây chính là cơ sở cho tất cả la bàn từ sử dụng để dẫn đường và định hướng. 2.2. Bản chất và đặc tính của sai số khi đo từ trường trên thiết bị bay Sai số đo VTT có 2 loại: sai số cố định, và sai số tạp. 2.3. Những khó khăn khi sử dụng từ trường Trái đất để định hướng TBB Khó khăn do: ảnh hưởng từ trường của TBB; tính không đồng nhất trên bề mặt Trái đất; Không thể xác định định hướng TBB theo VTT Trái đất nếu chỉ sử dụng từ trường như nguồn thông tin chính. 2.4. Khả năng sử dụng thông tin từ trường Trái đất trong điều khiển định hướng cho thiết bị bay 2.4.1. Tính chất đa trị của định thức Jacobi về khả năng sử dụng đơn thuần thông tin từ trường Trái đất. H1 = A (ϑ, ψ , γ ).H (2.2) Ở đây: H1 – vector đo được của từ trường Trái đất H trong hệ TĐLK; A – ma trận chuyển; ϑ, ψ, γ - các góc định hướng của hệ TĐLK; H – VTT đã biết trong hệ tọa độ địa tâm. cos ψ. cos ϑ.H x + sin ϑ.H y + sin ψ. cos ϑ.H z   (sin ψ. sin γ − cos ψ. sin ϑ. cos γ ).H + cos ϑ. cos γ.H +  x y    H1 =  + (cos ψ. sin γ − sin ψ. sin ϑ. cos γ ).H z   (sin ψ . cos γ + cos ψ . sin ϑ . sin γ ). H − cos ϑ . sin γ . H + x y     + (cos ψ. cos γ − sin ψ. sin ϑ. sin γ ).H z   (2.4) 7 nếu coi Hz = 0 khi đó H1 mà TBB đo được có dạng sau: cos ψ . cos ϑ.H x + sin ϑ.H y H 1 = (sin ψ . sin γ − cos ψ . sin ϑ. cos γ ).H x + cos ϑ. cos γ .H y (sin ψ . cos γ + cos ψ . sin ϑ. sin γ ).H x − cos ϑ. sin γ.H y (2.5) Ma trận Jacobi của vector H1  ∂H x1  ∂ϑ  ∂H 1 (ϑ , ψ , γ )  ∂ H y1 =  ∂ (ϑ , ψ , γ ) ∂ϑ  H ∂ z1   ∂ ϑ ∂ H x1 ∂ψ ∂ H y1 ∂ψ ∂ H z1 ∂ψ ∂H x1 ∂γ ∂ H y1 ∂γ ∂ H z1 ∂γ         Det[∂H1(ϑ, ψ, γ ) / ∂ (ϑ, ψ, γ )] = 0 Y1 Trục mà khi quay quanh nó, bộ đo vector không cảm biến được 0 X1 H Z1 Hình 2.9. Giải thích không có khả năng định vị chỉ bằng các phép đo từ trường T ⇒, (ϑ, ψ, γ ) có vô số nghiệm. 2.4.2. Mô hình đo VTT Trái đất kết hợp với nguồn thông tin độc lập khác để ĐKĐH cho TBB 1. Đặt vấn đề. Y1 Nếu coi VTT Trái đất H1 là xác định và bất biến, ω thì có thể lấy nó làm gốc để xét sự quay của TBB O Z1 −ω tương đối so với VTT. − ωtt 2. Mô hình toán học phép đo VTT trong hệ TĐLK − ωp X1 H1 a. Phương pháp xác định vận tốc góc của vector Hình 2.10. Sự quay của VTT trong hệ TĐLK từ trường Vector − ω cần đo đặc trưng cho sự quay của VTT H1 nếu ta coi H1 là vector bán kính, thì: [ dH1 / dt = − ω × H1 ] (2.6) có thể biểu diễn bằng một định thức: i dH1 = − ω1x dt H1x j − ω1y H1y k − ω1z (2.7) H1z Từ (2.7) ta có: 8  dH 1x / dt = − ω1y .H1z + ω1z .H1y   dH 1y / dt = + ω1x .H1z − ω1z .H1x  dH / dt = − ω .H + ω .H 1x 1y 1 y 1x  1z (2.8) Det[(2.8)] = 0, phép đo theo (2.8) có kết quả đa trị. b. Khắc phục tính đa trị bằng cách bổ sung thông tin vận tốc góc từ các phép đo độc lập. Do (2.8) đa trị nên cần phải bổ sung thông tin cho hệ này. + Bổ sung thông tin theo thành phần ω1x Đưa thêm vào (2.8) giá trị đo độc lập VTG ω1x, ký hiệu là W. Hệ phương trình (2.8) khi đó sẽ có dạng mới:  dH 1x / dt = − ω1 y .H1z + ω1z .H1y   dH 1 y / dt = W .H1z − ω1z .H1x  dH / dt = − W .H + ω .H 1z 1y 1 y 1x  (2.9) biểu thức tính trực tiếp ω1y và ω1z là:  ω1z = ( W.H 1z − dH 1 y / dt ) / H 1 x ω  1 y = ( W.H 1y + dH 1z / dt ) / H 1 x Hệ phương trình (2.10) chỉ giải được khi (2.10) H1x ≠ 0 . + Bổ sung thông tin theo ω1y hoặc ω1z ta cũng lập được các hệ phương trình (2.12), (2.14) tương tự như (2.10), các hệ này chỉ giải được khi hoặc H1y ≠ 0 H1z ≠ 0 . 3. Phương án sử dụng duy nhất một cảm biến VTG độc lập là con quay thẳng đứng Các hệ phương trình (2.10), (2.12), (2.14) vô nghiệm khi (H1x, H1y, H1z=0). Nếu dùng hai cảm biến VTG độc lập, đặt vuông góc với nhau, thì với định hướng bất kỳ của TBB, ta sẽ chọn 1 cặp phương trình để xác định thành phần thứ 3 còn lại của vetor VTG. Tuy nhiên ta có thể sử dụng 1 cảm biến VTG độc lập làm nguồn thông tin bổ sung bằng cách chọn vị trí đặt trục nhạy của cảm biến VTG theo một trong 3 trục (X1, Y1, Z1) của hệ TĐLK. 9 Lựa chọn: đối với vùng gần xích đạo của Trái đất VTT có thành phần thẳng đứng lớn hơn các thành phần ngang, tốt nhất nên đặt trục nhạy của cảm biến VTG dọc theo trục OY1 của hệ TĐLK trên TBB. Khả năng H1y=0 chỉ khi góc liệng của TBB bằng góc nghiêng của VTT (góc nghiêng của VTT ≥60o). Như vậy TBB sẽ không thể có góc liệng lớn như vậy do hạn chế về quá tải vật bay, góc liệng của TBB ≤ 30o. 2.4.3. Mô phỏng các phép đo VTG của TBB bằng phương pháp tính thẳng VTT có thông tin bổ sung của cảm biến độc lập Cho trước những dữ liệu đầu vào mô hình: Sai số TBBP tương đối của tạp âm ε = −5dB , ∆t=0.1s; υH=-60o; H1x=H1y=H1z=0; ϑ=0, ψ=0, γ=0; ω1x=ω1y=ω1z=5o/s. Sử dụng biểu thức (2.4), (2.11), (2.12), (2.25), (2.26) tính toán bằng phần mềm MatLab, có bổ sung cảm biến VTG ω1y ta nhận được những kết quả mô phỏng trên hình 2.1. a) Thành phần H1x hướng theo trục OX1 (có can nhiễu tạp mức -5dB) b) Thành phần vận tốc góc ω1x(nhiễu đo ω1x tăng tại các điểm H1y≈0) c) Thành phần H1y hướng e) Thành phần H1z hướng theo trục OY1(có can nhiễu theo trục OZ1 (có can nhiễu tạp mức -5dB) tạp mức -5dB) d) Thành phần vận tốc góc e) Thành phần H1z hướng theo ω1y (không có nhiễu đo do đặt trục (nhiễu đo ω1z tăng tại các điểm H1y≈0) ω1y=const) Hình 2.11. Kết quả mô phỏng đo các thành phần VTT và VTG của TBB Từ kết quả mô phỏng cho phép ta rút ra một số kết luận sau: 1. Tính đơn trị của các phép đo VTT so với các trục định hướng của TBB chỉ có khi sử dụng thêm các phép đo VTG bằng một hoặc hai cảm biến độc 10 lập. Tuy nhiên, ngay trong trường hợp này, vẫn tồn tại những miền nghiệm đa trị, nếu như không có sự lựa chọn cách đặt cảm biến VTG bổ sung. 2. Sử dụng duy nhất một cảm biến VTG có trục nhạy hướng theo trục OY1 của hệ TĐLK trên TBB. 3. Sai số khi tính các vector VTG thông qua đo VTT là sai số tính đạo hàm của VTT. Sai số này được đặc trưng bởi sai số nhiễu tạp của bộ đo (hình 2b, 2f). Để giảm ảnh hưởng của sai số nhiễu tạp, ta phải sử dụng các phương pháp lọc phi tuyến tối ưu. Kết luận chương 2 1. Bài toán điều khiển và ổn định định hướng TBB phụ thuộc rất nhiều vào độ chính xác ổn định trục thẳng đứng hệ dẫn đường quán tính. Phần tử cảm biến chính của hệ ĐKĐH truyền thống đo vị trí trục thẳng đứng là con quay cơ - điện. Giá thành, khối lượng và kích thước hình học của nó chiếm tỷ trọng đáng kể đối với phần còn lại của hệ thống. 2. Việc đo vector từ trường không cho phép xác định định hướng của TBB một cách đơn trị. Nguyên nhân xuất phát từ bản chất hình học của phép đo, mà không thể khắc phục được bằng các thuật toán biến đổi. Để giải được bài toán đo VTG thông qua đo VTT cần phải sử dụng thêm những phép đo độc lập đối với một hoặc 2 góc định hướng TBB. 3. Có khả năng xây dựng một hệ ĐKĐH không dùng con quay cơ - điện mà thay vào đó là các cảm biến VTG kết hợp với cảm biến ba trục đo từ trường Trái đất được chế tạo trên cơ sở công nghệ mới, có kích thước và giá thành thấp. 4. Để hiện thực hóa hệ thống ĐKĐH cho TBB sử dụng thông tin từ trường Trái đất thì cần tổng hợp hệ thống đo – xử lý thông tin có độ chính xác cao là rất quan trọng và quan trọng hơn cả là tổng hợp các bộ lọc số tối ưu phi tuyến cho phép giảm tối đa các sai số tương quan yếu, nhất là sai số tính đạo hàm theo các thành phần của VTT. 11 Chương III TỔNG HỢP HỆ THỐNG ĐO VẬN TỐC GÓC CỦA THIẾT BỊ BAY DỰA TRÊN LỌC PHI TUYẾN TỐI ƯU 3.1. Bài toán lọc các thành phần VTG Ta xây dựng bài toán trên cơ sở lý thuyết lọc phi tuyến. - Kênh quan sát được mô tả bằng phương trình sau: y ( t ) = s[λ ( t ), t ] + n tap ( t ) (3.1) với: y(t) là các giá trị đo VTT theo các trục hệ TĐLK [ y H1x , y H1y , yH1z ], có kèm theo nhiễu tạp; { } s[λ ( t ), t ] = H1 = H1x , H1y , H1z T là các giá trị đo thực của VTT (không bị ảnh hưởng của nhiễu tạp); λ(t) là vector các tham số đánh giá; n tap ( t ) là vector sai số nhiễu dạng tạp trắng có ma trận mật độ phổ dạng đường chéo N/2. Vector đánh giá các tham số λ(t) được mô tả như sau: { } λ ( t ) = H1x , H1y , H1z , ω1x , ω1y , ω1z T (3.2) Phương trình xác suất tiên nghiệm, mô tả các thành phần của vector đánh giá có dạng: dH1x / dt = −ω1y .H1z + ω1z .H1y dH1y / dt = +ω1x .H1z − ω1z .H1x dH1z / dt = −ω1x .H1y + ω1y .H1x (3.3) ω1x , ω1y và ω1z , mô tả bằng các quá trình Markov bậc nhất: dω1x / dt = −(1 / T1x )ω1x + n ht / 1x dω1y / dt = − 1 / T1y ω1y + n ht / 1y ( ) dω1z / dt = −(1 / T1z )ω1z + n ht / 1z (3.4) 3.2. Thuật toán lọc phi tuyến các thành phần vector VTG của TBB 3.2.1. Mô hình toán học 12 Sử dụng thuật toán lọc tĩnh cận tuyến tính để lọc phi tuyến các thành phần VTG của TBB. Dạng cuối cùng của thuật toán lọc như sau: dH1*x / dt = −ω1*y.H1*z + ω1*z.H1*y + FH1x H1*y / dt = +ω1*x.H1*z − ω1*z.H1*x + FH1y H1*z / dt = −ω1*x.H1*y + ω1*y.H1*x + FH1z dω1x / dt = −(1 / T1x ).ω1*x + Fω1x ( ) dω1y / dt = − 1 / T1y .ω1*y + Fω1y dω1z / dt = −(1 / T1z ).ω1*z + Fω1z (3.6) Khi sử dụng thuật toán lọc phi tuyến cận tối ưu ta có: FH 1x = K H  Y H 1x ( t ) − H 1*x    *   FH 1 y = K H  Y H 1 y ( t ) − H 1 y    *   FH 1z = K H  Y H 1z ( t ) − H 1z    *  Fω1x = K ω  Y H 1 y ( t ) − H 1 y  + K ω  Y H 1z ( t ) − H 1*z      * *    Fω1 y = K ω  Y H 1x ( t ) − H 1x  + K ω  Y H 1z ( t ) − H 1z      * *    Fω1z = K ω  Y H 1x ( t ) − H 1x  + K ω  Y H 1 y ( t ) − H 1 y      (3.8) 3.2.2. Mô phỏng thuật toán lọc trên máy tính Dữ liệu đầu vào chung cho tất cả các thử nghiệm là: độ nghiêng của VTT bằng -60o; Vị trí góc ban đầu hệ TĐLK ϑ=0, ψ=0, γ=0; ω1X = ω1Y = ω1Z = 5o / s ; K H = 1 , K ω = 1 , Tω1x = Tω1y = Tω1z = 1s . Đánh giá kết quả khảo sát: a) Kết luận về tính hội tụ và phân kỳ của thuật toán. - Với thuật toán lọc phi tuyến có hệ số không đổi là phân kỳ. - Với thuật toán lọc phi tuyến động (hệ số thay đổi) là thuật toán hội tụ nhưng độ chính xác chưa cao. - Với thuật toán lọc phi tuyến với tín hiệu sai số tiên nghiệm thuật toán hội tụ (hình 3.3). b) Kết luận về các tính chất động học của thuật toán. Thuật toán lọc phi tuyến với tín hiệu sai số tiên nghiệm (hình 3.3b và 3.3f) cho thấy sai số tĩnh theo các trục X1 và Z1. và những vùng tăng gần vùng phân kỳ của thuật toán lọc tĩnh (hình 3.2b và 3.2f). c) Kết luận về ảnh hưởng của sai số tạp âm tới thuật toán 13 Khi có nhiễu tác động dưới dạng sai số TBBP=5dB, xuất hiện sai số đo VTG những vùng không mong đợi (hình 3.3b và 3.3f). 3.3. Các thuật toán lọc phi tuyến khác trong tổng hợp bộ đo VTG của TBB 3.4.1. Sử dụng thuật toán lọc Kalman rời rạc mở rộng - EKF - Phương trình mô tả động học của hệ thống: x k = f k −1 ( x k −1 , t k −1 ) + w k −1 (3.9) - Phương trình phép đo: z k = h k ( x k ) + v k x̂ 0 = E[x 0 ] T  X 0 = E ( x 0 − x̂ 0 )( x 0 − x̂ 0 ) [ + Khởi tạo bộ lọc: P (3.10) ] (3.11) + Phương trình ước lượng trạng thái dự báo: x̂ k ( − ) = f k − 1 (x̂ k − 1 ( + ) ) (3.12) + Phương trình tính phép đo dự báo: ẑ k = h k (x̂ k ( −) ) + PT hiệu chỉnh dự báo phép đo: x̂k (+) = x̂k (−) + Hk (zk − ẑk ), Hk ≈ (3.13) ∂hk ∂x x=x̂ (−) k ( 3.14) + Ma trận phương sai tiên nghiệm: P k ( − ) = A k Pk ( + ) A T k + Q k +1 , Ak ≈ df k dx x = x) ( −) k −1 (3.15) + Phương trình tính hệ số khuếch đại lọc: K k (− ) = Pk (− ) H T k [H k P k ( − ) H Tk + R k ] −1 (3.16) + Phương trình tính ma trận phương sai hậu nghiệm: Pk (+) = [I − K k H k ]Pk (−) (3.17) 3.4.2. Sử dụng thuật toán lọc Kalman Unscented Filter (UKF) Thuật toán UKF để ước lượng trạng thái và tham số của hệ động học có tính phi tuyến lớn. Thuật toán này tương đương như thuật toán (EKF) nhưng độ chính xác cao hơn. Các phương trình thực hiện thuật toán lọc Kalman Unscented: + Khởi tạo bộ lọc: giống biểu thức (3.11) 14 + Tính các điểm sigma: [ ϑk −1 = x̂ k −1, x̂ k +1 + (1 + λ)Px k−1, x̂ k −1 − (1 + λ)Px k−1 ] (3.19) + Các phương trình cập nhật thời gian: (3.20) + Các phương trình cập nhật phép đo: (3.21) Trong (3.20) và (3.21), Qk và Rk tương ứng với ma trận phương sai của nhiễu kênh trạng thái và nhiễu kênh đo, còn các trọng số có thể được tính theo phép biến đổi Unscented. 3.4.3. Kết hợp thuật toán lọc thích nghi và thuật toán lọc Kalman Unscented thích nghi (MS-AUKF). Thuật toán MS-AUKF được xây dựng trên cơ sở hai bộ lọc UKF song song (hình 3.4), gồm UKF-master và UKF-slaver, UKF-slaver ước lượng phương sai nhiễu sử dụng những thay đổi (innovations) được tạo bởi UKFmaster. Hai UKF độc lập trong cấu trúc MS-AUKF. x̂ k −1 UKF-Master Cập nhật thời gian x̂ k|k −1 Cập nhật phép đo x̂ k trạng thái vk innovation θˆ k −1 yk UKF-Slaver Cập nhật thời gian θˆ k|k −1 Cập nhật phép đo θ̂ k phương sai nhiễu Hình 3.4. Sơ đồ cấu trúc của bộ lọc MS-AUKF ký hiệu Q vk , θik là phần tử đường chéo thứ i của ma trận phương sai nhiễu đo v 1 2 m nghĩa là: Q k = diag(θ k , θ k ,...θ k ) Khi động học của θk đã biết : θ k = f (θ k −1 ) + Wθk (3.23) (3.24) thì (3.24) là mô hình tham chiếu của UKF-slaver. Khi không biết động học của θk , thì sử dụng mô hình nhiễu tác động: θ k = θ k −1 + Wθk (3.25) 15 w θk được với giả thiết là tạp trắng Gauss có kỳ vọng = 0. Phương sai của những thay đổi tạo bởi UKF-master là tín hiệu quan sát đối với UKF-slaver, khi đó mô hình quan sát có dạng:  2n  S k = g ( θ k ) = diag  ∑ w ic ( γ i , k k −1 − y k k −1 )( γ i , k k −1 − y k k −1 ) T + Q kv −1  i = 0  Phép đo của Ŝk (3.26) T nhận được bởi UKF-slaver là: Sk = diag(v k v k ) với v k = y k − yk|k −1 là innovation và yk là phép đo thực. Bởi vậy thuật toán đệ quy của UKF-slaver có thể được thực hiện như sau: + Slave-I – Khởi tạo bộ lọc (3.27) + Slave-II – Tính các điểm sigma (3.28) + Slave-III – Cập nhật thời gian (3.29) + Slave IV – Cập nhật phép đo (3.30) 3.4. Mô phỏng đánh giá các mô hình hệ thống xác định VTG đã xây dựng Điều kiện thực hiện mô phỏng giống như chương 2. Số liệu ban đầu: ω1x = ω1y = ω1z = 0.1(rad / s) ; các góc ban đầu bằng 0; VTG của TBB ω1y đo nhờ một cảm biến VTG độc lập bổ sung. Sử dụng phần mềm MatLaB để lập trình, ta nhận được những kết quả sau: Theo quy trình các thuật toán lọc đã trình bày ở các mục 3.3.1 – 3.3.3, sử dụng phần mềm MatLaB để lập trình, ta nhận được những kết quả sau 1 1 0 Sai so w*1x H1y Sai so 1 0.5 0 -0.5 -1 w*1z 0.5 0 -0.5 0 10 20 30 40 50 60 70 80 90 -1 100 0 10 20 30 40 50 60 70 80 90 -1 100 0 10 20 30 40 a) Khi không có nhiễu phép đo VTT không có sai số 1 70 80 90 100 0 w1z 0.5 H1y W1x 60 1 1 0.5 0 -0.5 -1 50 Thoi gian(s) 0 -0.5 0 10 20 30 40 50 60 70 80 90 100 -1 0 10 20 30 40 50 60 70 80 90 100 -1 0 10 20 30 40 50 Thoi gian(s) 60 70 80 90 b) Khi có nhiễu phép đo VTT có sai số lớn tại những điểm H1y=0 Hình 3.5 Sai số đánh giá các VTT và VTG khi sử dụng thuật toán EKF. Mô phỏng theo phương pháp thực nghiệm Monte Carlo. 100 16 Sai số đánh giá các VTT và VTG sử dụng thuật toán EKF mô phỏng theo phương pháp thực nghiệm thống kê Monte-Carlo khi nhiễu tác động nhỏ thể hiện trên hình 3.6. Khi nhiễu tác động lớn ở giai đoạn cuối, được thể hiện trên hình 3.7.(a, b, c, d, e, f) -3 -3 x 10 1.5 0.5 0 -0.5 -1 2 Sai so danh gia w1x 0 10 20 30 40 50 60 Thoi gian(s) 70 80 90 0.5 0 -0.5 -1 -2 100 Sai so danh gia w1z 1 0.5 0 -0.5 -1 -1.5 0 10 20 30 40 50 60 Thoi gian(s) 70 80 90 -2 100 d. Sai số đánh giá ω1x a. Sai số đánh giá H1x x 10 1.5 1 -1.5 -1.5 -2 -3 x 10 1.5 1 Sai so danh gia w1x Sai so danh gia H1x 2 Sai so danh gia H1x Sai so danh gia w1z 2 0 10 20 30 40 50 60 Thoi gian(s) 70 80 90 100 f. Sai số đánh giá ω1z Hình 3.6 Sai số đánh giá các VTT và VTG khi sử dụng thuật toán EKF. Mô phỏng theo phương pháp thực nghiệm Monte Carlo. -3 H1x 0 -0.5 -1 -1.5 -2 1 0.5 0 -0.5 -1 -1.5 0 10 20 30 40 50 60 Thoi gian(s) 70 80 -2 90 100 w1z 1.5 Sai so danh gia w1z Sai so danh gia w1x 0.5 x 10 2 W1x 1.5 1 -3 x 10 2 1.5 Sai so danh gia H1x -3 x 10 2 1 0.5 0 -0.5 -1 -1.5 0 10 20 30 40 50 60 Thoi gian(s) 70 80 -2 90 100 0 10 20 30 40 50 60 Thoi gian(s) 70 80 90 100 a. Sai số đánh giá H1x d. Sai số đánh giá ω1x f. Sai số đánh giá ω1z Hình 3.7 Sai số đánh giá các VTT và VTG khi sử dụng thuật toán EKF. Mô phỏng theo phương pháp thực nghiệm Monte Carlo. Sai số đánh giá các VTT và VTG khi sử dụng thuật toán UKF. Mô phỏng theo phương pháp thực nghiệm Monte Carlo được thể hiện trên hình 3.8 -3 -3 x 10 2 Sai so danh gia H1x 1.5 1 Sai so danh gia w1x Sai so danh gia H1x 2 0.5 0 -0.5 -1 -1.5 10 20 30 40 50 60 Thoi gian(s) 70 80 90 100 a. Sai số đánh giá H1x Sai so danh gia w1z 1.5 1 0.5 0 -0.5 -1 -1.5 0 x 10 Sai so danh gia w1x 1.5 -2 -3 x 10 Sai so danh gia w1z 2 -2 1 0.5 0 -0.5 -1 -1.5 0 10 20 30 40 50 60 Thoi gian(s) 70 80 90 100 d. Sai số đánh giá ω1x -2 0 10 20 30 40 50 60 Thoi gian(s) 70 80 90 100 f. Sai số đánh giá ω1z Hình 3.8 Sai số đánh giá các VTT và VTG khi sử dụng thuật toán UKF . Mô phỏng theo phương pháp thực nghiệm Monte Carlo. Chất lượng của 2 bộ lọc EKF và UKF được so sánh với nhau thông qua việc so sánh sai số đánh giá các thành phần VTT và VTG của hai bộ lọc chạy đồng thời trong cùng một điều kiện thử nghiệm. Kết quả mô phỏng khi hai thuật toán EKF và UKF chạy đồng thời được thể hiện trên hình 3.9. 17 -3 -3 x 10 Sai so H1x EKF 4 Sai so H1x UKF -3 x 10 Sai so w1x EKF 4 Sai so w1x UKF 3 3 2 2 2 1 0 -1 Sai so w1z 3 Sai so w1x Sai so H1x 4 1 0 -1 0 -2 -2 -3 -3 -4 0 -4 0 -4 0 10 20 30 40 50 60 70 80 90 100 Thoi gian(s) d. Sai số đánh giá ω1x Sai so w1z UKF -1 -3 a. Sai số đánh giá H1x Sai so w1z EKF 1 -2 10 20 30 40 50 60 70 80 90 100 Thoi gian(s) x 10 10 20 30 40 50 60 70 80 90 100 Thoi gian(s) f. Sai số đánh giá ω1z Hình 3.9 Sai số đánh giá các VTT và VTG khi sử dụng thuật toán EKF và UKF Ở nửa đầu của quá trình khi nhiễu tác động nhỏ, kết quả ở cả hai phương pháp UKF và MS-AUKF sai số đánh giá có thể coi là giống nhau. Ở nửa sau của quá trình ta cho nhiễu tác động lớn thì phương pháp MSAUKF có hiệu quả hơn so với phương pháp UKF (hình 3.10). -3 -3 x 10 1.5 2 Sai so H1x(UKF) Sai so H1x(MSAUKF) 1.5 2 Sai so w1x(UKF) Sai so w1x(MSAUKF) 0.5 0 -0.5 0 -0.5 0.5 0 -0.5 -1 -1 -1 -1.5 -1.5 10 20 30 40 50 60 70 80 90 100 Thoi gian(s) a. Sai số đánh giá H1x -2 0 Sai so w1z(UKF) Sai so w1z(MSAUKF) 1 0.5 -1.5 -2 0 x 10 1.5 1 Sai so w1x Sai so H1x 1 -3 x 10 Sai so w1z 2 10 20 30 40 50 60 70 80 90 100 Thoi gian(s) d. Sai số đánh giá ω1x -2 0 10 20 30 40 50 60 70 80 90 100 Thoi gian(s) f. Sai số đánh giá ω1z Hình 3.10 Sai số đánh giá các VTT và VTG khi sử dụng thuật toán MS-AUKF. Mô phỏng theo phương pháp thực nghiệm Monte Carlo. Đánh giá kết quả khảo sát: 1. Nếu đo từ trường không có sai số thì có thể tính trực tiếp các thành phần VTG ω1x và ω1z (hình 3.5a). Nếu phép đo từ trường có sai số rất nhỏ, thì các phép tính trực tiếp vẫn có sai số rất lớn (hình 3.5b). Như vậy không thể sử dụng tính trực tiếp các thành phần VTG vì luôn tồn tại sai số đo. 2. Sử dụng thuật toán EKF hoặc UKF ta có thể hoàn toàn ước lượng không chỉ các thành phần VTG không được quan sát ω1x, ω1z mà còn cả chính bản thân các phép đo từ trường H1x, H1y, H1z. Chất lượng của thuật toán UKF là tốt hơn so với thuật toán EKF nhưng không đáng kể (xem các hình 3.9). 3. Khi nhiễu đo tăng lên tại thời điểm t=50(s), thì sai số ước lượng của cả hai thuật toán EKF và UKF đều tăng rất lớn (hình 3.7a, d, f, hình 3.10a, d, f). 18 4. Sử dụng thuật toán UKF thích nghi (MS-AUKF) ta có thể giải quyết hầu hết nhược điểm của các thuật toán EKF và UKF trước sự thay đổi của nhiễu đo (hình 3.10a,d,f). Kết luận chương 3 Trên cơ sở các phép đo VTT và thông tin VTG bổ sung từ cảm biến độc lập ω1y . Trong chương 3 tác giả đã: + Tổng hợp được mô hình bộ lọc phi tuyến tĩnh (hệ số cố định) và động (có tín hiệu sai số tiên nghiệm). Khảo sát hai mô hình lọc phi tuyến đã nêu và rút ra những kết luận quan trọng liên quan tới tính hội tụ, phân kỳ, tính chất động học và sai số do tác động của nhiễu tạp đo gây ra. + Xây dựng và tổng hợp được 3 mô hình bộ lọc Kalman rời rạc tối ưu là EKF, UKF và MS-AUKF đều có khả năng lọc, xử lý tốt đối với nhiễu đo có đặc trưng thống kê ổn định. + Cả 3 phương án lọc Kalman rời rạc đều hội tụ tốt với sai số nhỏ. Tính chống nhiễu và tính chất động học được cải thiện tốt hơn bộ lọc phi tuyến động. Tuy nhiên khi tình huống nhiễu thay đổi chỉ có bộ lọc Kalman mở rộng thích nghi (MS-AUKF) vẫn đáp ứng được những yêu cầu về động học và sai số. Chương IV THỰC NGHIỆM ĐÁNH GIÁ KHẢ NĂNG THỰC TẾ HÓA HỆ ĐO – XỬ LÝ THÔNG TIN VẬN TỐC GÓC CỦA THIẾT BỊ BAY THEO TỪ TRƯỜNG TRÁI ĐẤT 4.1. Mô tả thực nghiệm Thí nghiệm tiến hành trên giá quay PTN giống như TBBKNL chuyển động trong chế độ bay bằng thực tế. 4.2. Thiết kế và tổ chức phần cứng thực nghiệm Cảm biến đo từ trường ba trục loại HMC2003 và cảm biến VTG bổ sung loại LY510ALH được sử dụng để thiết kế mạch thử nghiệm, chọn ADC trên cơ sở Platform ElVIS-II của NI 19 Sơ đồ chức năng kết nối phần cứng hình 4.11(a,b) Bộ nguồn chất lượng cao Giá quay 3 bậc tự do gắn board cảm biến từ trường trái đất + cảm biến VTG Board ELVIS II biến đổi AD và ghép nối với máy tính Máy tính cá nhân sử lý và thực hiện các thuật toán lọc Kết quả sử lý (H1x. H1y, H1z) ω1x. ω 1y ,ω 1z Hình 4.11.a Sơ đồ chức năng kết nối phần cứng 4.3. Thiết kế và xây dựng phần mềm thực nghiệm Mô hình rời rạc của bộ lọc như sau: [ H 1x ( k + 1) = H 1x (k ) + T * − ω1 y (k )H 1z (k ) + ω1z (k ) H 1y (k ) H 1 y ( k + 1) = H 1 y (k ) + T * [ω1x (k )H 1z ( k ) − ω1z ( k )H 1x (k )] [ H 1z (k + 1) = H 1z (k ) + T * − ω1z (k )H 1 y (k ) + ω1 y (k )H 1x (k ) ] (4.1) ] ω 1 x ( k + 1 ) = exp( − T / T1 x ) ω 1 x ( k ) + w x ( k ) ω 1 y ( k + 1 ) = exp( − T / T1 y ) ω 1 y ( k ) + w y ( k ) ω 1 z ( k + 1 ) = exp( − T / T1 z ) ω 1 z ( k ) + w z ( k ) viết hệ (4.1) dưới dạng ma trận – vector: x ( k + 1 ) = f k ( x ( k ), t k ) + w ( k ) (4.2) Ở đây: x(k) = [H1x (k) H1y (k) H1z (k) H1x (k) ω1x (k) ω1y (k) ω1z (k)]T (4.3) vector đo được biểu diễn như sau z(k ) = H  1x (k ) H 1y (k ) H 1z ( k ) ω 1y (k )   T + v(k ) (4.4) Các quan sát (phép đo) ở (4.4) ta sẽ ước lượng vector trạng thái x(k) ở (4.3) mà động học được mô tả bởi hệ phương trình (4.1). 4.3.1. Phần mềm và kết quả lọc theo thuật toán EKF a) Kết quả đo H1x b) Kết quả đo H1y c) Kết quả đo H1z Hình 4.18. Kết quả đo VTT khi sử dụng bộ lọc EKF (đơn vị gauss) (đường màu đỏ là đo trước lọc; màu trắng là sau lọc) a) Kết quả đo ω1x b) Kết quả đo ω1y c) Kết quả đo ω1z -3 Hình 4.19. Kết quả đo các thành phần VTG (đơn vị đo 10 rad/s) 20 a) Kết quả đo H1x b) Kết quả đo H1y c) Kết quả đo H1z Hình 4.20. Kết quả đo VTT khi sử dụng bộ lọc UKF (đơn vị gauss) (đường màu đỏ là đo trước lọc; màu trắng là sau lọc) 4.3.2. Phần mềm và kết quả lọc theo thuật toán UKF a) Kết quả đo ω1x b) Kết quả đo ω1y c) Kết quả đo ω1z Hình 4.21. Kết quả đo các thành phần VTG (đơn vị đo 10-2rad/s) a) Kết quả đo H1x b) Kết quả đo H1y c) Kết quả đo H1z Hình 4.22. Kết quả đo các thành phần VTT có lọc bias (đơn vị đo gauss) (đường màu đỏ là đo trước lọc; màu trắng là sau lọc) a) ω1x b) ω1y c) ω1z Hình 4.23. Kết quả lọc UKF các thành phần VTG (đơn vị đo độ/s) (đường màu đỏ là đo trước lọc; màu trắng là sau lọc) 4.3.2. Phần mềm thuật toán lọc Kalman thích nghi (MS-AUKF) Giải pháp phần mềm nâng cao chất lượng của bộ lọc bằng cách ước lượng sai số bias của cảm biến VTG sẽ được bổ sung vào vector trạng thái. Khi đó vector trạng thái là T xnew(k) = H1x (k) H1y (k) H1z (k) ω1x (k) ω1y(k) ω1z (k) xbias (k)   (4.6) ở đây xbias(k) là bias của cảm biến VTG. Ma trận độ nhạy phép đo H sẽ là ω1ydo (k) = ω1y (k) + x bias (k) + vω1y (k) Bias của MEMs là: (4.7) x& bias (t) = w th/bias (t) Hay dưới dạng rời rạc hóa x bias (k + 1) = x bias (k) + Tw th /bias (k) (4.8) (4.9)