Thiết kế bộ lọc kalman để tính toán ước lượng đường di chuyển của thiết bị khảo sát

ĐẠI HỌC QUỐC GIA HÀ NỘI TRƯỜNG ĐẠI HỌC CÔNG NGHỆ - - VƯƠNG HẢI TÚ THIẾT KẾ BỘ LỌC KALMAN ĐỂ TÍNH TỐN ƯỚC LƯỢNG ĐƯỜNG DI CHUYỂN CỦA THIẾT BỊ KHẢO SÁT LUẬN VĂN THẠC SĨ CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ, TRUYỀN THÔNG HÀ NỘI – 2016 ĐẠI HỌC QUỐC GIA HÀ NỘI TRƯỜNG ĐẠI HỌC CÔNG NGHỆ - - VƯƠNG HẢI TÚ THIẾT KẾ BỘ LỌC KALMAN ĐỂ TÍNH TỐN ƯỚC LƯỢNG ĐƯỜNG DI CHUYỂN CỦA THIẾT BỊ KHẢO SÁT Ngành: Công nghệ Kỹ thuật điện tử, truyền thông Chuyên ngành: Kỹ thuật Điện tử Mã số: 60520203 LUẬN VĂN THẠC SĨ CÔNG NGHỆ KỸ THUẬT ĐIỆN TỬ, TRUYỀN THÔNG NGƯỜI HƯỚNG DẪN KHOA HỌC: PGS.TS TRẦN ĐỨC TÂN HÀ NỘI - 2016 LỜI CAM ĐOAN Tôi xin cam đoan cơng trình nghiên cứu thân, xuất phát từ thực tế yêu cầu công việc Các số liệu thu thập kết nêu luận văn trung thực, có nguồn gốc rõ ràng chưa công bố trước Các thơng tin trích dẫn sử dụng luận văn ghi rõ nguồn gốc Tác giả Vương Hải Tú MỤC LỤC LỜI CAM ĐOAN MỤC LỤC DANH MỤC CÁC KÝ HIỆU VÀ TỪ VIẾT TẮT .4 DANH MỤC CÁC BẢNG DANH MỤC CÁC HÌNH VẼ .6 MỞ ĐẦU CHƯƠNG 1: HỆ THỐNG ĐỊNH VỊ BẰNG SÓNG ÂM .9 1.1 Các phương pháp định vị sóng âm .9 1.1.1 Phương pháp đường sở dài LBL 1.1.2 Phương pháp đường sở ngắn SBL .10 1.1.3 Phương pháp đường sở cực ngắn USBL 10 1.1.4 Các hệ thống kết hợp 11 1.2 Tần số sử dụng hệ thống định vị sóng âm 11 1.3 Các thành phần hệ thống định vị sóng âm 12 1.4 Sai số hệ thống định vị sóng âm 13 1.4.1 Sai số tương đối hệ thống đường sở dài LBL 13 1.4.3 Các nguồn nhiễu ảnh hưởng tới hệ thống định vị sóng âm 14 1.4.4 Tầm nhìn thẳng 15 1.4.5 Hiện tượng đường truyền sóng bị bẻ cong vùng mù 15 CHƯƠNG 2: NGUYÊN LÝ CỦA HỆ THỐNG ĐỊNH VỊ USBL VÀ PHƯƠNG PHÁP ĐỊNH VỊ THỦ CÔNG .18 2.1 Phương pháp định vị thủ công 18 2.2 Các thu phát sóng âm .19 2.3 Lan truyền sóng âm 20 2.4 Tín hiệu sóng âm 22 2.4.1 Tín hiệu nguyên gốc 22 2.4.2 Tín hiệu điều chế tần số (FM hay gọi CHIRP) .23 2.4.3 Tín hiệu điều chế pha (PSK) .24 2.5 Nguyên lý hệ thống USBL 25 2.6 Ảnh hưởng độ nghiêng góc xoay thu phát 27 2.7 Hiệu chỉnh hệ thống USBL 27 2.7.1 Phương pháp hiệu chỉnh tĩnh 28 2.7.2 Phương pháp hiệu chỉnh động 28 CHƯƠNG 3: XỬ LÝ KẾT QUẢ ĐO BẰNG BỘ LỌC BÙ VÀ KALMAN .30 3.1 Thu thập liệu .30 3.2 Đánh giá liệu .31 3.3 Bộ lọc bù Kalman 35 3.3.1 Bộ lọc bù 35 3.3.2 Bộ lọc Kalman 35 3.4 Kết sau áp dụng lọc 39 3.4.1 Sử dụng lọc bù 39 3.4.2 Sử dụng lọc Kalman .41 KẾT LUẬN 43 TÀI LIỆU THAM KHẢO 44 DANH MỤC CÁC KÝ HIỆU VÀ TỪ VIẾT TẮT Ký hiệu Đơn vị USBL Ý nghĩa Ultra Short Base Line SBL Short Base Line LBL Long Base Line AHRS Altitude and heading reference system α độ Hướng phương vị theo phương ngang từ điểm thả cáp tới cá đo R m Khoảng cách theo phương ngang từ điểm thả cáp tới cá đo DANH MỤC CÁC BẢNG Bảng 1.1 - Chiều dài sở sử dụng hệ thống định vị sóng âm .9 Bảng 1.2 - Phạm vi hoạt động sóng âm theo dải tần số [2, tr.7] .11 Bảng 1.3 - Sai số tương đối theo dải tần số [2, tr.10] 13 DANH MỤC CÁC HÌNH VẼ Hình 1.1 - Hệ thống định vị đường sở dài LBL [2, tr.5] Hình 1.2 - Hệ thống định vị đường sở ngắn SBL [2, tr.4] 10 Hình 1.3 - Hệ thống định vị USBL [2, pp.3] 11 Hình 1.4 - Thành phần hệ thống định vị sóng âm [2, tr.8] .12 Hình 1.5 - Kết đo hai thiết bị đo trực tiếp (Valeport MidasSVP) đo gián tiếp (Valeport MidasCTD) điều kiện 16 Hình 1.6 - Quan hệ tốc độ âm độ sâu nước 16 Hình 1.7 - Thay đổi nhiệt độ theo độ sâu (ở vùng biển có độ sâu 75m, đo thiết bị Valeport MidasCTD) 17 Hình 2.1 - Phương pháp xác định khoảng cách (range) góc phương vị (bearing) từ điểm thả tới cá đo 18 Hình 2.2 - Hình dạng bố trí phần tử áp điện thường gặp [3, pp.9] 19 Hình 2.3 - Phần tử áp điện trước sau phân cực [3, pp.9] 19 Hình 2.4 - Hình dạng vùng lan truyền sóng âm [3, pp.11] 20 Hình 2.5 - Hình dạng búp sóng thực tế bao gồm búp sóng búp sóng phụ [3, pp.12] .21 Hình 2.6 - Mặt cắt cánh sóng tạo dãy phần tử áp điện thẳng hàng [4, pp.33] 21 Hình 2.7 - Các vị trí giao thoa tăng cường triệt tiêu tạo thành cánh sóng [4, pp.27] .21 Hình 2.8 - Phổ tần số biên độ tương quan tín hiệu sóng âm gốc [5, tr.3] 22 Hình 2.9 - Phổ tần số biên độ tương quan tín hiệu điều chế tần số [5, tr.3] 23 Hình 2.10 - Tín hiệu sóng âm điều chế pha (cịn gọi tín hiệu băng rộng) 24 Hình 2.11 - Phổ tần số biên độ tương quan tín hiệu sóng âm điều chế pha [5, tr.3] 24 Hình 2.12 - Hệ thống định vị sóng âm USBL [6, tr.2] 25 Hình 2.13 - Bộ phát đáp Sonardyne Coastal gắn cá đo quét ngang âm bề mặt EdgeTech 4200MP 25 Hình 2.14 - Bộ thu phát Sonardyne 8024 Wideband gắn tàu khảo sát 25 Hình 2.15 - Xác định góc dựa vào độ trễ thời gian .26 Hình 2.16 - Hai cách bố trí đơn giản thu thu phát USBL 27 Hình 2.17 - Sắp xếp thu phát hệ thống USBL Sonardyne (5 thu xếp hình ngũ giác phát giữa) 27 Hình 2.18 - Phương pháp hiệu chỉnh tĩnh .28 Hình 2.19 - Phương pháp hiệu chỉnh động 29 Hình 3.1 - Lắp đặt thiết bị tàu 30 Hình 3.2 - Một vài điển hình hệ thống USBL hoạt động không tốt, vẽ hệ tọa độ phẳng North-East 31 Hình 3.3 - Khoảng cách từ điểm thả tới cá đo tính theo phương pháp thủ công USBL .32 Hình 3.4 - Tương quan hướng cá đo, hướng phương vị α hệ thống USBL, hướng phương vị tàu hướng tàu chạy 33 Hình 3.5 - Tương quan độ lệch hướng tàu chạy CMG, hướng cá đo hướng phương vị từ điểm thả cáp tới cá đo tính hệ thống USBL 34 Hình 3.6 - Hướng phương vị từ điểm thả cáp tới cá mô hình quan hệ tuyến tính với hướng cá hướng tàu chạy .34 Hình 3.7 – Sơ đồ lọc bù [8, tr.2] 35 Hình 3.8 – Một trường hợp lọc bù lọc hoạt động với nhiễu .35 Hình 3.9 – Minh họa hoạt động lọc bù 35 Hình 3.10 – Sơ đồ thuật toán lọc Kalman rời rạc 38 Hình 3.11 – Sơ đồ áp dụng lọc Kalman 38 Hình 3.13 - Sai lệch tọa độ (East) cá đo phương pháp tính thủ cơng, USBL trước sau lọc 39 Hình 3.12 - Tọa độ (East) cá đo theo phương pháp thủ công, USBL sau lọc .39 Hình 3.14 - Tọa độ (North) phương pháp thủ công, USBL sau lọc 40 Hình 3.15 - Sai lệch tọa độ (North) phương pháp tính thủ cơng USBL trước sau lọc 40 Hình 3.16 - Đường cá đo sau lọc, vẽ hệ tọa độ phẳng North-East 41 Hình 3.17 - So sánh kết (sai lệch tọa độ North) lọc thơng thấp Kalman 41 Hình 3.18 - So sánh kết (sai lệch tọa độ East) lọc thơng thấp Kalman .42 Hình 3.19 - Kết vị trí sau áp dụng lọc Kalman, vẽ hệ tọa độ phẳng North-East 42 MỞ ĐẦU Công nghiệp khai thác dầu mỏ giới sớm, khoảng năm ký 19 phát triển mạnh mẽ vào nửa cuối thể kỷ 20 tiếp tục với phát triển thiết bị điện tử Sự đời internet bùng nổ thơng tin tồn cầu địi hỏi người tiến hành xây dựng cơng trình biển ngày nhiều Ngành dịch vụ đo đạc khảo sát biển đời từ để phục vụ cho việc thăm dò, thiết kế, xây dựng tu bảo dưỡng cơng trình hạ tầng biển Các loại thiết bị thăm dị, đo đạc kể đến như: - - Các thiết bị đo thủy hải văn: đo sâu cánh sóng, nhiều cánh sóng (Singlebeam/Multibeam Echo-Sounder), quan trắc thủy triều, dịng chảy, sóng biển Các thiết bị đo địa vật lý: đo quét ngang âm bề mặt (Side-scan sonar), đo âm địa chấn (Sub-bottom profiler) Các thiết bị đo từ trường Các thiết bị đo đạc nghiên cứu địa chất đáy biển, thiết bị đo quan trắc môi trường nước biển Hầu hết thiết bị đo sử dụng sóng âm, dải tần siêu âm từ cỡ 10kHz đến 500khz, thiết bị đo âm địa chấn có dải tần thấp (vài kHz) phải truyền qua lớp đất đá sâu đáy biển Các thiết bị đo lắp đặt tàu khảo sát Một số khác (như máy đo quét ngang âm bề mặt, đo từ trường, hay đo âm địa chấn) yêu cầu phải trì khoảng cách với đáy biển để đảm bảo độ phân giải dải đo, giảm thiểu ảnh hưởng nguồn nhiễu từ tàu, sai số sóng biển, thả kéo theo tàu sợi cáp (vừa chịu lực truyền dẫn tín hiệu), tùy thuộc vào độ sâu nước vùng khảo sát, chiều dài thả cáp lến tới 4000-5000m Một yêu cầu đo đạc xác định vị trí, phổ biến sử dụng hệ thống định vị toàn cầu vi sai DGPS cho độ xác lên tới 10-20cm Các thiết bị lắp đặt tàu xác định vị trí từ hệ thống DGPS dựa vào khoảng cách tới ăng-ten GPS phương vị tàu Các thiết bị kéo theo tàu đòi hỏi phức tạp hơn, khoảng cách ngắn sử dụng chiều dài cáp để tính theo cách tương tự Ở nước sâu chiều dài cáp lớn cần sử dụng hệ thống định vị sóng âm USBL (ultra-short baseline) hay LBL (long baseline) Tuy nhiên lúc hệ thống hoạt động tốt, phụ thuộc vào việc lắp đặt xác, lựa chọn hệ thống phù hợp chất lượng hệ thống AHRS Luận văn đề cập đến vấn đề tìm giải pháp định vị tốt trường hợp khơng có hệ thống định vị sóng âm, hệ thống hoạt động không tốt, sử dụng thông tin chuyển động từ cảm biến gắn thiết bị kéo theo tàu 31 âm bề mặt xấp xỉ 10 lần/giây số liệu dẫn đường lần/giây Toàn số liệu lọc bớt đưa tốc độ lần/giây Vị trí điểm thả cáp tính từ vị trí ăng-ten GPS dựa theo sơ họa tàu đo đạc từ trước 3.2 Đánh giá liệu Bộ số liệu thu thập đường khảo sát có chiều dài khoảng 5.3km, điều kiện thời tiết xấu dẫn đến hệ thống USBL hoạt động không tốt số thời điểm Trong luận văn đề xuất ước lượng góc hướng phương vị α theo hai cách: - - Bằng góc hướng ngược với hướng cá đo: Cảm biến góc hướng cá ổn định gần với góc hướng α xác định hệ thống USBL Tuy có thời điểm hướng chạy tàu thay đổi lớn, hướng cá đo dao động mạnh có độ lệch so với góc hướng α Bằng quan hệ tuyến tính với hướng cá đo hướng tàu chạy: 𝛼 = 𝑎×ℎướ𝑛𝑔 𝑡à𝑢 𝑐ℎạ𝑦 + (1 − 𝑎)×ℎướ𝑛𝑔 𝑐á đ𝑜 + 𝑏 (3.1) Trong hệ số a độ lệch b xác định so sánh với kết hệ thống USBL Góc hướng α sau lọc lọc thơng thấp để loại bỏ nhiễu tần số cao Đường tàu Đường cá (định vị thủ công) Đường cá (USBL) Hình 3.2 - Một vài điển hình hệ thống USBL hoạt động khơng tốt, vẽ hệ tọa độ phẳng North-East 32 Hình 3.3 - Khoảng cách từ điểm thả tới cá đo tính theo phương pháp thủ cơng USBL Ở thời điểm tốt vị trí cá đo tính theo phương pháp thủ công hệ thống USBL trùng khớp (hình 3.2), nhiên nhiều thời điểm số liệu vị trí tính hệ thống USBL dao động với biên độ vào cỡ khoảng 3-4m với hướng thay đổi đột ngột Đây xác định kết bất thường cá đo có thiết kế thủy động học cho phép hoạt động di chuyển ổn định Số liệu hướng cá đo thời điểm cho thấy hướng thân cá đo ổn định thời điểm Hình 3.3 thể khoảng cách R tính theo phương pháp thủ cơng (đường màu xanh) tính hệ thống USBL (đường màu đỏ) Đường màu vàng chiều dài thả cáp, đường màu đen độ sâu cá đo, đo cảm biến Với phương pháp tính thủ cơng, khoảng cách từ điểm thả tới cá đo R ổn định phụ thuộc vào chiều dài thả cáp độ sâu cá Với hệ thống USBL, khoảng cách R đo bị nhiễu (trong trường hợp xảy theo chu kỳ) bám sát khoảng cách R so với phương pháp tính thủ cơng tương quan phù hợp với thay đổi chiều dài thả cáp 33 Hình 3.4 - Tương quan hướng cá đo, hướng phương vị α hệ thống USBL, hướng phương vị tàu hướng tàu chạy Hướng cá đo (đã cộng thêm 180 độ, đường màu xanh) bị thay đổi hướng tàu chạy thay đổi đột ngột nhiễu động môi trường thân cảm biến, nhiên ln có xu hướng trở sát với hướng dây cáp (cũng hướng từ điểm thả tới cá đo xác định hệ thống USBL, đường màu đỏ) Đường màu đen vàng tương ứng hướng tàu hướng chạy tàu dịch lên 30 20 độ để việc hiển thị rõ ràng Đây sở đề xuất thứ để tính góc hướng α Trong hình 3.5 mô tả sai lệch hướng hướng cá đo góc hướng α tính hệ thống USBL (đường màu đen), sai lệch hướng cá đo góc hướng α tính hệ thống USBL so với hướng chạy tàu (tương ứng đường màu xanh đỏ) Cả ba sai lệch thay đổi xung quanh giá trị khẳng định tương quan ba đại lượng hướng Ở thời điểm hướng tàu chạy thay đổi mạnh, sai lệch ba đại lượng hướng tương ứng tăng lên đáng kể Đây sở để luận văn đưa đề xuất thứ để xác định góc hướng α phương pháp thủ cơng (cơng thức 3.1) Giá trị góc phương vị α tính sau lọc lọc thơng thấp có hàm truyền H(z) = (1-k)/(1-kz-1) Bằng số thử nghiệm dựa kiểm chứng với kết hệ thống USBL, hệ số a = 0,4 độ lệch b = (độ) xác định hệ số tốt số liệu sử dụng Hệ số k hàm truyền lọc thông thấp chọn k=0.85 Kết cho thấy tốt đối chiếu với kết hệ thống USBL (hình 3.6) tạo khác biệt lớn kết phương pháp định vị thủ cơng 34 Hình 3.5 - Tương quan độ lệch hướng tàu chạy CMG, hướng cá đo hướng phương vị từ điểm thả cáp tới cá đo tính hệ thống USBL Hình 3.6 - Hướng phương vị từ điểm thả cáp tới cá mô hình quan hệ tuyến tính với hướng cá hướng tàu chạy Trong hình 3.6, đường màu xanh nước biển, màu xanh da trời, màu đen tương ứng hướng tàu chạy, hướng cá đo, góc hướng α tính hệ thống USBL Đường màu đỏ góc hướng α tính đề xuất thứ cách tính thủ cơng 35 3.3 Bộ lọc bù Kalman 3.3.1 Bộ lọc bù Hình 3.7 – Sơ đồ lọc bù [8, tr.2] Hình 3.8 – Một trường hợp lọc bù lọc hoạt động với nhiễu Bộ lọc bù lọc hoạt động miền tần số với hai đầu vào x, y phép đo bị nhiễu tín hiệu z ẑ ước lượng z sau lọc Giả thiết phép đo x bị nhiễu chủ yếu tần số thấp, phép đo y nhiễu chủ yếu tần số cao Khi lọc thơng thấp G(s) sử dụng cho phép đo y để loại bỏ nhiễu tần số cao, đồng thời lọc [1-G(s)] lọc thông cao loại bỏ nhiễu tần số thấp phép đo x Ước lượng ẑ tổng hai thành phần phép đo x y lọc bỏ nhiễu Bộ lọc bù sử dụng nhiều cho hệ cảm biến dẫn đường quán tính để kết hợp kết phép đo gia tốc góc gia tốc tịnh tiến HPF ∑ LPF Hình 3.9 – Minh họa hoạt động lọc bù Một biến thể lọc bù (hình 3.8) lọc G(s) hoạt động với đầu vào nhiễu từ hai phép đo: y-x = n2-n1 sử dụng luận văn để ước lượng vị trí cá đo Đầu vào x vị trí cá đo thu từ phương pháp tính thủ cơng y vị trí cá đo thu từ hệ thống USBL Bộ lọc thông thấp chọn có hàm truyền H(z) = (1-k)/(1-kz-1) 3.3.2 Bộ lọc Kalman Khác với lọc bù áp dụng miền tần số, lọc Kalman hoạt động miền thời gian quan tâm tới đặc trưng thống kê hệ thống thay hàm truyền hay miền tần số Bộ lọc Kalman sử dụng phương trình tốn học để mơ tả ước lượng tối ưu trạng thái hệ thống, sử dụng hai mơ hình: mơ hình động học mơ hình đo lường (nhóm nghiên cứu có số cơng trình lọc Kalman liệt kê 11-15 phần Tài liệu tham khảo] 36 Biểu diễn liên tục theo thời gian mơ hình động học có dạng [9, tr.25]: 𝑥̇ (𝑡 ) = 𝐹 (𝑡 )𝑥(𝑡 ) + 𝐵 (𝑡 )𝑢(𝑡 ) + 𝐺 (𝑡 )𝑣(𝑡) (3.2) Trong x(t) véc tơ trạng thái hệ thống, F(t) ma trận động học, G(t) ma trận điều khiển đầu vào, u(t) véc tơ điều khiển đầu vào, G(t) ma trận điều khiển nhiễu v(t) nhiễu q trình Và mơ hình đo lường: 𝑧(𝑡 ) = 𝐻 (𝑡 )𝑥 (𝑡 ) + 𝑤(𝑡) (3.3) Trong z(t) véc tơ đo lường, H(t) ma trận đo w(t) nhiễu đo Biểu diễn rời rạc theo thời gian, mơ hình động học có dạng: 𝑥𝑘 = 𝐹𝑘 𝑥𝑘−1 + 𝐵𝑘 𝑢𝑘 + 𝐺𝑘 𝑣𝑘 (3.4) Trong xk véc tơ trạng thái đặc trưng cho hệ thống thời điểm k kích thước Nx1, Fk ma trận chuyển trạng thái có kích thước NxN xác lập mối liên hệ trạng thái hệ thống thời điểm k thời điểm k-1 trước đó, uk véc tơ điều khiển đầu vào kích thước Rx1, Bk ma trận kích thước NxR thể tác động đầu vào lên trạng thái hệ, Gk ma trận kích thước NxQ thể tác động nhiễu trình lên trạng thái hệ vk véc tơ nhiễu trình kích thước Qx1 Mơ hình đo lường có dạng: 𝑧𝑘 = 𝐻𝑘 𝑥𝑘 + 𝑤𝑘 (3.5) Trong zk véc tơ đo lường có kích thước Mx1, Hk ma trận kích thước MxN xác lập mối quan hệ véc tơ đo trạng thái hệ wk véc tơ nhiễu đo kích thước mx1 wk vk giả thiết nhiễu trắng, phân bố chuẩn, không tương quan với nhau: 𝑝(𝑣 ) ~ 𝑁(0, 𝑄) (3.6) 𝑝(𝑤) ~ 𝑁(0, 𝑅) (3.7) Q, R tương ứng ma trận hiệp phương sai nhiễu trạng thái nhiễu đo giả thiết số Bộ lọc Kalman hoạt động theo hai bước: bước dự đoán bước cập nhật Trong bước dự đoán, lọc Kalman thực dự đoán trạng thái hiệp phương sai lỗi trạng thái k dựa vào thơng tin có trạng thái k-1 trước 𝑥̂𝑘|𝑘−1 = 𝐹𝑘 𝑥̂𝑘−1|𝑘−1 + 𝐵𝑘 𝑢𝑘 (3.8) 𝑃𝑘|𝑘−1 = 𝐹𝑘 𝑃𝑘−1|𝑘−1 𝐹𝑘 𝑇 + 𝐺𝑘 𝑄𝑘 𝐺𝑘𝑇 (3.9) 37 Trong 𝑥̂𝑘|𝑘−1 , 𝑃𝑘|𝑘−1 ước lượng trạng thái hiệp phương sai lỗi hệ thống thời điểm k dựa vào thông tin từ thời điểm k-1, 𝑥̂𝑘−1|𝑘−1 , 𝑃𝑘−1|𝑘−1 ước lượng tốt trạng thái hiệp phương sai lỗi thời điểm k-1 Trong bước cập nhật trạng thái hiệp phương sai lỗi thời điểm k tiếp tục dự đoán dựa giá trị ướng lượng bước dự đốn trước thơng tin phép đo zk có thời điểm k Giả thiết trạng thái ước lượng 𝑥̂𝑘|𝑘 tổ hợp tuyến tính có trọng số giá trị ước lượng bước dự đoán giá trị đo zk: 𝑥̂𝑘|𝑘 = 𝑊𝑘′ 𝑥̂𝑘|𝑘−1 + 𝑊𝑘 𝑧𝑘 (3.10) Wk’ Wk ma trận trọng số (khuếch đại) thay đổi theo thời gian, chọn cho tối thiểu trung bình bình phương sai số ước lượng Sai số ước lượng thời điểm k (bước cập nhật): 𝑥̃𝑘|𝑘 = 𝑥̂𝑘|𝑘 − 𝑥𝑘 (3.11) = 𝑊𝑘′ 𝑥̂𝑘|𝑘−1 + 𝑊𝑘 𝑧𝑘 − 𝑥𝑘 (kết hợp với 3.5 3.10) = 𝑊𝑘′ (𝑥̃𝑘|𝑘−1 + 𝑥𝑘 ) + 𝑊𝑘 (𝐻𝑘 𝑥𝑘 + 𝑤𝑘 ) − 𝑥𝑘 = (𝑊𝑘′ + 𝑊𝑘 𝐻𝑘 − 1)𝑥𝑘 + 𝑊𝑘′ 𝑥̃𝑘|𝑘−1 + 𝑊𝑘 𝑤𝑘 (3.12) Chúng ta biết để trung bình bình phương sai số ước lượng trung bình sai số ước lượng phải tối thiểu Trong theo giả thiết E(wk) = E(𝑥̃𝑘|𝑘−1 | 𝑍 𝑘−1 ) = 0, ta chọn 𝑊𝑘′ + 𝑊𝑘 𝐻𝑘 − = để tối thiểu trung bình sai số ước lượng E(𝑥̃𝑘|𝑘 | 𝑧𝑘 ) Do 𝑊𝑘′ = − 𝑊𝑘 𝐻𝑘 Thay vào (3.10) (3.12) ta có: 𝑥̂𝑘|𝑘 = (1 − 𝑊𝑘 𝐻𝑘 )𝑥̂𝑘|𝑘−1 + 𝑊𝑘 𝑧𝑘 = 𝑥̂𝑘|𝑘−1 + 𝑊𝑘 (𝑧𝑘 − 𝐻𝑘 𝑥̂𝑘|𝑘−1 ) (3.13) 𝑥̃𝑘|𝑘 = 𝑊𝑘′ 𝑥̃𝑘|𝑘−1 + 𝑊𝑘 𝑤𝑘 = (1 − 𝑊𝑘 𝐻𝑘 )𝑥̃𝑘|𝑘−1 + 𝑊𝑘 𝑤𝑘 (3.14) 𝐻𝑘 𝑥̂𝑘|𝑘−1 dự đoán phép đo trạng thái k Hiệp phương sai sai số ước lượng [9, tr.48]: 𝑇 | 𝑍 𝑘 ) = (1 − 𝑊𝑘 𝐻𝑘 )𝑃𝑘|𝑘−1 (1 − 𝑊𝑘 𝐻𝑘 )𝑇 + 𝑊𝑘 𝑅𝑘 𝑊𝑘𝑇 𝑃𝑘|𝑘 = 𝐸(𝑥̃𝑘|𝑘 𝑥̃𝑘|𝑘 (3.15) 𝑊𝑘 hệ số khuếch đại Kalman chọn để tối thiểu hóa trung bình bình phương sai 𝑇 số ước lượng 𝐿𝑘 = 𝐸(𝑥̃𝑘|𝑘 𝑥̃𝑘|𝑘 ) nhỏ −1 𝑊𝑘 = 𝑃𝑘|𝑘−1 𝐻𝑘𝑇 [𝐻𝑘 𝑃𝑘|𝑘−1 𝐻𝑘𝑇 + 𝑅𝑘 ] (3.16) Các phương trình (3.8), (3.9), (3.16), (3.13) (3.15) phương trình lọc Kalman rời rạc 38 𝑥̂𝑘|𝑘−1 = 𝐹𝑘 𝑥̂𝑘−1|𝑘−1 + 𝐵𝑘 𝑢𝑘 𝑃𝑘|𝑘−1 = 𝐹𝑘 𝑃𝑘−1|𝑘−1 𝐹𝑘 𝑇 + 𝐺𝑘 𝑄𝑘 𝐺𝑘𝑇 Bước dự báo 𝑊𝑘 = 𝑃𝑘|𝑘−1 𝐻𝑘𝑇 [𝐻𝑘 𝑃𝑘|𝑘−1 𝐻𝑘𝑇 + 𝑅𝑘 ] Bước cập nhật −1 𝑥̂𝑘|𝑘 = 𝑥̂𝑘|𝑘−1 + 𝑊𝑘 (𝑧𝑘 − 𝐻𝑘 𝑥̂𝑘|𝑘−1 ) 𝑃𝑘|𝑘 = (1 − 𝑊𝑘 𝐻𝑘 )𝑃𝑘|𝑘−1 (1 − 𝑊𝑘 𝐻𝑘 )𝑇 + 𝑊𝑘 𝑅𝑘 𝑊𝑘𝑇 Hình 3.10 – Sơ đồ thuật tốn lọc Kalman rời rạc Bộ lọc Kalman ứng dụng thay lọc thông thấp mô hình lọc bù hình 3.8 Vị trí tính pp thủ công + + + _ + Vị trí tính hệ thống USBL + Vị trí ước lượng Bộ lọc Kalman Hình 3.11 – Sơ đồ áp dụng lọc Kalman Phương trình trạng thái phương trình đo viết tương đồng sau: [ 𝑑𝑁𝑘 𝑑𝑁 ] = [ ] [ 𝑘−1 ] + [ ] 𝑣𝑘 𝑑𝐸𝑘 𝑑𝐸 01 𝑘−1 𝑑𝑁𝑘(đ𝑜) 𝑑𝑁 [ ] = [ ] [ 𝑘 ] + 𝑤𝑘 𝑑𝐸𝑘(đ𝑜) 𝑑𝐸𝑘 (3.17) (3.18) Các ma trận véc tơ trạng thái, ma trận hiệu phương sai sai số ước lượng, ma trận hiệp phương sai véc tơ nhiễu trạng thái nhiễu đo khởi tạo ban đầu: 10 𝑋0 = [ ] ; 𝑃0 = [ ] ; 𝑄 = [0.32 0.32 ]; 𝑅 = [1.52 1.52 ] 01 39 3.4 Kết sau áp dụng lọc 3.4.1 Sử dụng lọc bù Hình 3.12 - Tọa độ (East) cá đo theo phương pháp thủ công, USBL sau lọc Hình 3.13 - Sai lệch tọa độ (East) cá đo phương pháp tính thủ cơng, USBL trước sau lọc 40 Hình 3.14 - Tọa độ (North) phương pháp thủ công, USBL sau lọc Hình 3.15 - Sai lệch tọa độ (North) phương pháp tính thủ cơng USBL trước sau lọc 41 Đường tàu Đường cá (định vị thủ công) Đường cá (USBL) Đường cá (sau lọc) Hình 3.16 - Đường cá đo sau lọc, vẽ hệ tọa độ phẳng North-East 3.4.2 Sử dụng lọc Kalman Hình 3.17 - So sánh kết (sai lệch tọa độ North) lọc thông thấp Kalman 42 Hình 3.18 - So sánh kết (sai lệch tọa độ East) lọc thông thấp Kalman Đường tàu Đường cá (định vị thủ công, cách tính 2) Đường cá (USBL) Đường cá (sau lọc Kalman) Hình 3.19 - Kết vị trí sau áp dụng lọc Kalman, vẽ hệ tọa độ phẳng North-East 43 KẾT LUẬN Dựa số liệu thu thập khẳng định tương quan chặt chẽ chiều dài thả cáp khoảng cách tương đối R từ điểm thả cáp tới cá đo, hướng từ điểm thả cáp tới cá đo hướng tàu chạy, hướng cá đo Khoảng cách R xác định phương pháp thủ công tương đối ổn định xác dựa so sánh với kết đo hệ thống USBL Trong hướng dây cáp có độ ổn định thấp sai lệch nhiều so với kết hệ thống USBL Luận văn khẳng định mối liên hệ tuyến tính góc hướng từ điểm thả cáp tới cá đo với hướng cá hướng tàu chạy Do hướng tàu chạy tính từ hệ thống định vị tồn cầu GPS nên số liệu có chứa thành phần tần số cao, lọc thông thấp giúp loại bỏ thành phần Kết thu được kiểm chứng với số liệu đo hệ thống USBL (hoàn toàn độc lập với tất số liệu khác) Phát giúp cho phương pháp định vị thủ cơng có độ xác cao trường hợp hệ thống USBL hoạt động gián đoạn không tốt Khi áp dụng lọc bù với đầu vào kết phương pháp tính thủ công kết đo hệ thống USBL giải tốt bất thường gián đoạn hết đo hệ thống USBL Về lọc Kalman giải tốt lọc thông thấp Hướng nghiên cứu tập trung vào sử dụng cảm biến quán tính gắn cá đo để phát triển tốn dẫn đường qn tính sử dụng lọc Kalman áp dụng cho xác định vị trí cá đo 44 TÀI LIỆU THAM KHẢO Tiếng Anh Jonathan Martin, Subsea positioning through the ages, www.sonardyne.com Keith Vickery (1998), Acoustic Positioning Systems “A Practical Overview Of Current Systems”, Sonardyne, Inc Airmar Technology Corporation, Theory of Operations L3 Communications SeaBeam Instruments (2000), Multibeam Sonar Theory of Operation S.Adrián-Martínez, M.Ardid, M.Bou-Cabo, I.Felis, C.Llorens, J.A.MartínezMora, M.Salda, Acoustic signal detection through the cross-correlation method in experiments with different signal to noise ratio and reverberation conditions Sonadyne, Wideband™ Fusion LBL and USBL Sonardyne, Fusion USBL System Manual Walter T Higgins, Jr., A Comparison of Complementary and Kalman Filtering, IEEE Tranactions On Aerospace And Electronic Systems Vol Aes-1 1, No Hugh Durrant-Whyte (2011), Introduction to Estimation and the Kalman Filter 10 R.L Kirlin, W.S Lu, B Hedstrom, C.M Leung (1993), Towfish orientation and position estimation, IEEE Journal of Oceanic Engineering ( Volume: 18, Issue: 3, Jul 1993 ) 11 Tran, D T., Luu, M H., Nguyen, T L., Nguyen, D D., & Nguyen, P T (2014) Land-vehicle mems INS/GPS positioning during GPS signal blockage periods Journal of Science, Vietnam National University, Hanoi., 23(4), 243251 12 Tan, T D., Tue, H H., Long, N T., Thuy, N P., & Van Chuc, N (2006, November) Designing Kalman filters for integration of inertial navigation system and global positioning system In The 10th biennial Vietnam Conference on Radio & Electronics, REV-2006 Hanoi, November (pp 6-7) 13 Tan, T D., Ha, L M., Long, N T., Thuy, N P., & Tue, H H (2007) Performance Improvement of MEMS-Based Sensor Applying in Inertial Navigation Systems Research-Development and Application on Electronics, Telecommunications and Information Technology, (2), 19-24 14 Tan, T D., Ha, L M., Long, N T., Tue, H H., & Thuy, N P (2008, December) Novel MEMS INS/GPS Integration Scheme Using Parallel Kalman Filters In Proceedings of the 2008 IEEE International Symposium on System Integration(pp 72-76) 15 Tan, T D., Ha, L M., Long, N T., Tue, H H., & Thuy, N P (2007, October) Feedforward Structure Of Kalman Filters For Low Cost Navigation In 45 International Symposium on Electrical-Electronics Engineering (ISEE2007) (pp 1-6) 16 Dean Steinke (2003), Design and Simulation of a Kalman filter for ROV Navigation, University of Victoria 17 Magne Mandt, Kenneth Gade, Bjørn Jalving, Integrating DGPS-USBL position measurements with inertial navigation in the HUGIN 3000 AUV ... HÀ NỘI TRƯỜNG ĐẠI HỌC CÔNG NGHỆ - - VƯƠNG HẢI TÚ THIẾT KẾ BỘ LỌC KALMAN ĐỂ TÍNH TỐN ƯỚC LƯỢNG ĐƯỜNG DI CHUYỂN CỦA THIẾT BỊ KHẢO SÁT Ngành: Công nghệ Kỹ thuật điện tử, truyền thông Chuyên... thuật toán lọc Kalman rời rạc Bộ lọc Kalman ứng dụng thay lọc thơng thấp mơ hình lọc bù hình 3.8 Vị trí tính pp thủ cơng + + + _ + Vị trí tính hệ thống USBL + Vị trí ước lượng Bộ lọc Kalman Hình... α tính hệ thống USBL Đường màu đỏ góc hướng α tính đề xuất thứ cách tính thủ cơng 35 3.3 Bộ lọc bù Kalman 3.3.1 Bộ lọc bù Hình 3.7 – Sơ đồ lọc bù [8, tr.2] Hình 3.8 – Một trường hợp lọc bù lọc