Thiết kế thiết bị đo độ dài từ xa ứng dụng cảm biến quán tính

BỘ GIÁO DỤC VÀ ĐÀO TẠO ĐẠI HỌC ĐÀ NẴNG NGUYỄN NHƢ THẾ THIẾT KẾ THIẾT BỊ ĐO ĐỘ DÀI TỪ XA ỨNG DỤNG CẢM BIẾN QUÁN TÍNH LUẬN VĂN THẠC SỸ KỸ THUẬT Đà Nẵng - Năm 2018 BỘ GIÁO DỤC VÀ ĐÀO TẠO ĐẠI HỌC ĐÀ NẴNG NGUYỄN NHƢ THẾ THIẾT KẾ THIẾT BỊ ĐO ĐỘ DÀI TỪ XA ỨNG DỤNG CẢM BIẾN QUÁN TÍNH Chuyên ngành : KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA Mã số : 8520216 LUẬN VĂN THẠC SỸ KỸ THUẬT NGƢỜI HƢỚNG DẪN KHOA HỌC: TS Nguyễn Anh Duy Đà Nẵng - Năm 2018 LỜI CAM ĐOAN Tôi cam đoan cơng trình nghiên cứu riêng tơi Các số liệu, kết nêu luận văn trung thực chưa công bố công trình khác Tác giả luận văn Nguyễn Nhƣ Thế MỤC LỤC MỞ ĐẦU 1.Tính cấp thiết đề tài Tổng quan hệ thống .10 Mục tiêu nghiên cứu 11 Đối tƣợng nghiên cứu phạm vi nghiên cứu 11 4.1 Đối tƣợng nghiên cứu 11 4.2 Phạm vi nghiên cứu .11 Phƣơng pháp nghiên cứu 11 5.1 Nghiên cứu lý thuyết 11 5.2 Nghiên cứu thực nghiệm 11 Bố cục luận văn 11 CHƢƠNG 12 GIỚI THIỆU CẢM BIẾN QUÁN TÍNH, LIDAR VÀ VI ĐIỀU KHIỂN ARDUINO 12 1.1 Giới thiệu cảm biến quán tính GY-85 .12 1.2 Giới thiệu cảm biến khoảng cách LIDAR Lite V3 14 1.2.1 Tổng quan 14 1.2.2 Cảm biến khoảng cách LIDAR Lite V3 14 CHƢƠNG 15 THUẬT TỐN ĐỊNH VỊ QN TÍNH CƠ BẢN .15 2.1 Hệ trục tọa độ số khái niệm .15 2.2 Quaternion .16 2.3 Quaternion trung bình .16 2.4 Biểu diễn góc sử dụng quaternion 17 2.5 Các phƣơng trình vị trí góc .19 2.6 Phƣơng trình góc quay (góc euler) .20 2.7 Xác định góc quay từ giá trị đo .20 2.8 Các phƣơng trình cảm biến .21 2.9 Ƣớc lƣợng thay đổi vị trí 21 2.9.1 Tính 22 2.9.2 Phƣơng trình 22 2.9.3 Vận tốc vị trí .23 2.10 AHRS 23 2.10.1 Bộ lọc cho AHRS 23 2.11 Xác định góc quay từ giá trị đo .26 2.11.1 Thuật toán TRIAD 26 2.11.2 Xác định góc quay từ cảm biến .26 2.11.3 Thuật toán tổng quát .26 2.11.4 Khởi tạo góc quay ban đầu sử dụng cảm biến từ trƣờng cảm biến gia tốc 28 CHƢƠNG 29 BỘ LỌC KALMAN .29 3.1 Giới thiệu chung lọc Kalman .29 3.2 Mơ hình tốn học 30 3.2.1 Hệ thống mơ hình quan sát .30 3.2.2 Giả thiết 31 3.2.3 Nguồn gốc 31 3.2.4 Điều kiện không chệch 32 3.2.5 Hiệp phƣơng sai sai số 33 3.2.6 Độ lời Kalman .34 3.2.7 Tóm tắt phƣơng trình lọc Kalman .35 CHƢƠNG 37 THUẬT TỐN XÁC ĐỊNH THƠNG SỐ CỦA CẢM BIẾN VÀ THUẬT TỐN ĐỊNH VỊ QN TÍNH CHO BỘ LỌC KALMAN 37 4.1 Thuật tốn xác định thơng số cảm biến khoảng cách 37 4.2 Thuật toán định vị quán tính (ina) sử dụng lọc kalman .38 4.3 Ứng dụng lọc Kalman cho thuật tốn định vị qn tính 40 CHƢƠNG 42 THIẾT KẾ PHẦN CỨNG, THỰC NGHIỆM ĐO KHOẢNG CÁCH VÀ ĐÁNH GIÁ KẾT QUẢ ĐO .42 5.1 Sơ đồ chi tiết 42 5.2 Đọc liệu từ LIDAR 42 5.3 Đọc liệu từ GY-85 43 5.4 Lập trình cho vi điều khiển .45 5.5 Viết chƣơng trình cho phần mềm Matlab .45 5.6 Cài đặt điểm đo .45 5.7 Thực thu thập liệu 46 5.8 Thống kê kết đo 47 5.9 Đánh giá sai lệch .48 5.10 Nguyễn nhân sai số .49 KẾT LUẬN VÀ KIẾN NGHỊ 50 TÀI LIỆU THAM KHẢO 51 PHỤ LỤC 52 PHỤ LỤC 57 ĐO ĐỘ DÀI TỪ XA ỨNG DỤNG CẢM BIẾN QUÁN TÍNH VÀ CẢM BIẾN ĐO XA Học viên: Nguyễn Nhƣ Thế Mã số: 60.52.02.16 Chuyên ngành: Kỹ thuật điều khiển tự động hóa Khóa: 33 Trƣờng Đại học Bách khoa – ĐHĐN Tóm tắt - Cám biến qn tính (Inertial Measurement Unit – IMU) cảm biến khoảng cách đƣợc sử dụng rộng rãi nhiều lính vực kỹ thuật đời sống Trong phần trình bày tác giả thiết kế hệ thống đo xa sử dụng cảm biến khoảng cách dùng laser kết hợp với cảm biến quán tính (IMU) Bằng cách kết hợp phân tích tính tốn quỹ đạo chuyển động hệ thống (đƣợc ƣớc lƣợng từ liệu cảm biến quán tính) khoảng cách từ hệ thống đến điểm cần đo (đo đƣợc từ cảm biến khoảng cách), ta tính đƣợc tọa độ khơng gian điểm cần đo Từ liệu tính tốn thơng số hình học khác nhƣ khoảng cách, góc, diện tích… khơng gian phạm vi cho phép Kết thực nghiệm cho thấy khả ứng dụng thực tế hệ thống đƣợc đề xuất Từ khóa – Đo khoảng cách từ xa; cảm biến khoảng cách; cảm biến quán tính; khối định vị quán tính – IMU; lọc Kalman Abstract –The Inertial Measurement Unit (IMU) and distance measuring sensor is now widely used in many areas of technology and life In the presentation, the author designed a remote measurement system using a laser distance sensor combined with an inertial sensor (IMU) By combining the analysis and calculation of the motion trajectory of the system (estimated from inertial sensor data) and the distance from the system to the points to be measured (measured from distance sensors), We can calculate the spatial coordinates of the points to be measured From the data it can calculate other geometric parameters such as distance, angle, area in the space within the allowed range Experimental results show that the practical applicability of the system is proposed Key word – Remote measuring; distance measuring sensor; inertial sensor; inertial measuring unit – IMU; Kalman filter DANH MỤC CÁC KÝ HIỆU, CÁC CHỮ VIẾT TẮT LCD Lyquify Crystal Display BCS Body Coordinate System WCS World Coordinate System IMU Inertial Measurement Unit LIDAR Laser Imaging Detection And Ranging IDE Integrated Development Environment CPU Center Processcing Unit INA Inertial Navigation Algorithm ZVI Zero Velocity Interval DANH MỤC CÁC BẢNG Số hiệu 5.1 5.2 5.3 5.4 5.5 Tên bảng Thống kê kết đo thực nghiệm Biểu đồ thống kê sai lệch độ dài AB Biểu đồ thống kê sai lệch độ dài BC Biểu đồ thống kê sai lệch độ dài AC Bảng thống kê sai lệch nhỏ lớn đoạn AB, BC, AC Trang 48 49 49 50 50 DANH MỤC CÁC HÌNH Số hiệu 1.1 1.2 1.3 1.4 2.1 3.1 3.2 3.3 3.4 4.1 4.2 4.3 5.1 5.2 5.3 5.4 5.5 5.6 Tên hình Tổng quan hệ thống đề xuất Hình ảnh IMU Các cấu trúc khối IMU vi Cảm biến quán tính GY-85 Cảm biến khoảng cách LIDAR lite Hệ trục tọa độ dẫn hƣớng khối tâm Mơ hình đo lƣờng ƣớc lƣợng lọc Kalman Tín hiệu thu trƣớc sau lọc qua Kalman Mơ hình khơng gian trạng thái Tóm tắt q trình khởi tạo Kalman Phƣơng pháp xác định thông số cảm biến khoảng cách Vec tơ vị trí r không sử dụng lọc Kalman Vec tơ vị trí r có sử dụng lọc Kalman Sơ đồ thiết bị đo khoảng cách từ xa Lƣu đồ thuật toán đọc liệu từ LIDAR Lƣu đồ thuật tốn chƣơng trình đọc từ GY-85 Các điểm cần đo tƣờng Thiết bị đo độ dài từ xa Dữ liệu truyền qua giao diện serial monitor Trang 10 12 13 13 14 15 30 31 31 37 38 41 42 43 44 45 46 47 48 46 5.7 Thực thu thập liệu Ngƣời dùng cầm thiết bị đo độ dài từ xa (Hình 5.5) Ban đầu đèn laser đƣợc bật lên, ngƣời dùng vào điểm cần đo thứ nhất, sau nhấn nút record, ngƣời dùng vào điểm cần đo thứ hai, sau nhấn nút record để kết thúc việc đọc liệu Hình 5.5: Thiết bị đo độ dài từ xa Dữ liệu đƣợc vi điều khiển đọc sau truyền qua giao diện giám sát nối tiếp (Serial monitor) tốc độ 115200baud, liệu có dạng (ax, ay, az, gx, gy, gz, distance) với tần số lấy mẫu 100Hz 47 Hình 5.6: Dữ liệu truyền qua giao diện serial monitor 5.8 Thống kê kết đo Tác giả thực nghiệm đo đoạn AB, BC, AC, đoạn đƣợc thực 10 lần, vị trí đứng xa khác nhau, thống kê đo độ dài từ xa đƣợc thể dƣới bảng sau: Độ dài (m) AB BC AC Giá trị thật 2.03 Sai lệch 2.51 Sai lệch 1.51 Sai lệch Lần thứ 2.1222 -0.0922 2.4385 0.0715 1.4329 0.0771 Lần thứ 2.1441 -0.0219 2.4172 0.0213 1.3170 0.1159 Lần thứ 2.1132 0.0309 2.5683 -0.1511 1.4665 -0.1495 Lần thứ 2.1360 -0.0228 2.5893 -0.021 1.5413 -0.0748 Lần thứ 2.2123 -0.0763 2.7328 -0.1435 1.5885 -0.0472 Lần thứ 2.0694 0.1429 2.4308 0.302 1.4069 0.1816 Lần thứ 1.9037 0.1657 2.4140 0.0168 1.5218 -0.1149 Lần thứ 1.9609 -0.0572 2.3843 0.0297 1.3795 0.1423 Lần thứ 2.2109 -0.25 2.6866 -0.3023 1.5287 -0.1492 Lần thứ 10 2.1369 0.074 2.7908 -0.1042 1.4317 0.097 Bảng 5.1: thống kê kết đo thực nghiệm 48 5.9 Đánh giá sai lệch 0.2 0.15 0.1 0.05 -0.05 -0.1 -0.15 -0.2 -0.25 -0.3 Bảng 5.2: Biểu đồ thống kê sai lệch độ dài AB Trung bình sai lệch ̅ ∑ = -0.01069 0.4 0.3 0.2 0.1 -0.1 -0.2 -0.3 -0.4 Bảng 5.3: Biểu đồ thống kê sai lệch độ dài đoạn BC Trung bình sai lệch ̅ ∑ = -0.02808 49 0.2 0.15 0.1 0.05 -0.05 -0.1 -0.15 -0.2 Bảng 5.4: Biểu đồ thống kê sai lệch độ dài đoạn AC ∑ Trung bình sai lệch ̅ = 0.00783 Từ bảng 7.1 ta có bảng thống kê sai lệch nhỏ lớn độ dài đoạn AB, BC, AC nhƣ sau: Đoạn Sai lệch nhỏ Sai lệch lớn -0.0922 (4.5%) 0.1657 (8.1%) -0.3023 (12%) 0.302 (12%) -0.1495 (10%) 0.1816 (12.1%) Bảng 5.5: Bảng thống kê sai lệch nhỏ lớn đoạn AB, BC, AC Từ bảng 5.2, 5.3, 5.4, 5.5 ta kết luận thiết bị đo xa sai số tƣơng đối lớn, chƣa đáp ứng đƣợc yêu cầu đo độ dài từ xa không gian 5.10 Nguyễn nhân sai số Trong đo lƣờng, nguyên nhân sai số sai số hệ thống sai số ngẫu nhiên, ngồi cịn có nguyên nhân sau: - - Cảm biến đo độ dài LIDAR có phạm vi đo lƣờng zerovel(i-1) n=n+1; up(n)=i; end 59 if zerovel(i)>zerovel(i+1) m=m+1; down(m)=i; end end up(1)=2; down(length(down)+1)=N; % distance correction for k=1:1 load('LIDAR_distance_calib.mat') m=0; count=0 dis_vector=zeros(1,N); dis_value=zeros(1,N); dis_value=a(:,7); for i=1:length(dis_value) if dis_value(i)>0 dis_vector(i)=1; end dis_value(i)=dis_value(i)/100; end for k=1:length(up) dis_mean(k)=sum(dis_value(up(k):down(k)))/sum(dis_vector(up(k):down(k))); end floorzero=0; ym=ya; [r,v,q4,eulercom4,bghat,bahat,Qbg0,Qba0,zerovel] = personal_navigation0(ya,yg,ym,floorzero,zerovel,ghat,bghat,bahat,Qbg0,Qba0); len = norm(r(:,length(ya))) figure plot3(r(1,:),r(2,:),r(3,:)) axis equal x=[0.9994 ; 0.0135 ; 0.0311]; % Lidar point b1=quaternion2dcm(mean(q4(:,1),2))'*(dis_mean(1))*x b2=quaternion2dcm(mean(q4(:,up(2):down(2)),2))'*(dis_mean(2))*x+mean(r(:,up(2):down( 2)),2) b=[b1 b2 ] quaternion2euler(q4(:,1))*180/pi quaternion2euler(mean(q4(:,up(2):down(2)),2))*180/pi %Length len=[norm(b1-b2)] end ... từ ngun nhân trên, tơi định chọn đề tài “THIẾT KẾ THIẾT BỊ ĐO ĐỘ DÀI TỪ XA ỨNG DỤNG CẢM BIẾN QUÁN TÍNH” ” nhằm nghiên cứu kỹ thuật toán định vị quán tính, lọc Kalman, ứng dụng cảm biến quán tính, ... sau: - - Cảm biến đo độ dài LIDAR có phạm vi đo lƣờng