CHƯƠNG 2 BÀI THÍ NGHIỆM ĐO LƯỜNG QUÁN TÍNH IMU
2.3.1.Lắp đặt thiết bị
2.3. TRÌNH TỰ THỰC HIỆN THÍ NGHIỆM
2.3.1.Lắp đặt thiết bị
Phần 1
1. Kiểm tra bàn xoay về mặt cơ học xem có xoay tốt ở 3 trục X, Y, Z hay không. Đảm bảo bàn xoay không bị cản trở bởi các chướng ngại vật khi xoay quanh các trục.
2. Lắp ráp thiết bị IMU lên bàn xoay sao cho 3 trục X, Y, Z của IMU và bàn xoay cùng phương và chiều với nhau (xem kí hiệu các trục được vẽ trên bàn xoay).
3. Nạp chương trình cho IMU (nếu chưa nạp)
4. Dùng dây 4 sợi nối IMU với RS-232: nguồn 3.3V/GND gắn vào chân Vin/GND của IMU (Chú ý: gắn đúng chiều dịng điện như đã chú thích trên dây nguồn!). Dây Tx/Rx từ RS232 gắn vào chân Tx/Rx của IMU
Phần 2
1. Gắn RS232 vào máy tính thơng qua cổng USB.
52
2. Click chuột phải vào My Computer -> Manage -> Computer Management -> Device Manager để xem IMU được kết nối hay chưa? ở COM thứ mấy? Ví dụ như trong hình vẽ là COM 2.
Hình 2-8: Kiểm tra xem IMU được kết nối hay chưa ?
3. Click khởi động giao diện của IMU Pololu CHR-6dm: AHRSInterface.exe. Chọn COM2 sau đó click vào biểu tượng kết nối màu xanh như hình bên dưới
53
Hình 2-9: Khởi động giao diện của IMU Pololu CHR-6dm
4. Lưu dữ liệu thí nghiệm bằng cách log file: Dialogs -> Log
54
Chú ý: file dữ liệu được lưu trữ dưới dạng .log, có thể lưu lại dưới dạng .txt để dùng
Matlab đọc dễ dàng khi ước lượng góc nghiêng Euler bằng Kalman Filter. 2.3.2. Nạp chương trình lên IMU Pololu CHR-6dm
Phần 1: Nạp chương trình thí nghiệm đo góc nghiêng trên bàn xoay
1. Tháo XBee ra khỏi RS232
2. Nối dây nạp chương trình vào RS232 và IMU như 2 hình vẽ bên dưới
Hình 2-11: Nối dây nạp chương trình vào RS232
55
3. Mở chương trình FlashLoader. Làm các bước như hình bên dưới. Trong mục
Download from file chọn đường dẫn đến file CHR6dm.hex trong thư mục
CHR6dmRelease để nạp file này vào IMU. Đợi đến khi kết thúc nạp thì click vào nút Close chương trình FlashLoader.
4. Sau khi nạp chương trình cho IMU xong thì tháo dây nạp chương trình ra, rồi sử dụng IMU bình thường theo các bước đã hướng dẫn ở trên.
56
Hình 2-14: Sau khi nạp chương trình cho IMU xong (2)
Phần 2: Nạp chương trình thí nghiệm đo góc nghiêng và vị trí GPS trên máy bay
Tương tự như 4 bước nạp chương trình ở phần 1 tuy nhiên đường dẫn đến file CHR6dm.hex là khác nhau. File CHR6dm.hex nằm ở thư mục có sử dụng thêm GPS(AHRSInterface_source_chrobitics.com_gps_20120424)
Thiết lập baudrate cho Xbee
57
Hình 2-15. Xbee được gắn vào RS232
2. Mở chương trình X-CTU thực hiện theo các bước Bước 1: Chọn tab Modem Configuration.
Bước 2: Nhấn nút Read sẽ thấy mục Interface Date Rate Bước 3: Chọn baurate là 115200.
Bước 4: Nhấn Write để thiết lập baudrate mong muốn cho Xbee.
58
3. Nhấn tab Read một lần nửa để đọc xem XBee có được thiết lập baudrate như mong muốn hay chưa.
Chú ý: trường hợp chương trình báo lỗi thì nhấn nút màu đỏ trên RS232 rồi thả ra.
2.4. CHƯƠNG TRÌNH MATLAB
Ứng dụng code MATLAB, được cung cấp trong tài liệu thí nghiệm ta thực hiện tính tốn cho từng file kết quả đo được thông qua bộ IMU.
clear; clc;
sensor_data = load('IMU_DATA_2020_NHOM5_A01_YAW.txt'); time = sensor_data(:,1); % s
yaw = sensor_data(:,2); % deg pitch = sensor_data(:,3); % deg roll = sensor_data(:,4); % deg yaw_rate = sensor_data(:,5); % deg/s pitch_rate = sensor_data(:,6); % deg/s roll_rate = sensor_data(:,7); % deg/s mag_x = sensor_data(:,8); % int mag_y = sensor_data(:,9); % int mag_z = sensor_data(:,10);% int gyro_x = sensor_data(:,11); % deg/s gyro_y = sensor_data(:,12); % deg/s gyro_z = sensor_data(:,13); % deg/s accel_x = sensor_data(:,14); % int15 accel_y = sensor_data(:,15); % int15 accel_z = sensor_data(:,16); % int15 % IMU parameters
59 accel_bias = [0,0,0]'; mag_bias = [-20,-22,-9]'; gyro_alignment = [1 0 0;0 1 0;0 0 1]; accel_alignment = [1 0 0;0 1 0;0 0 1]; accel_ref = [0,0,-6000]'; % int15 mag_ref = [255,0,666]'; mag_cal = [0.001218163 0 0;... 0 0.001197028 0;... 0 0 0.001296065]; % EKF Init P = [0.01 0 0;0 0.01 0;0 0 0.01]; Q = [0.01 0 0;0 0.01 0;0 0 0.01]; Racc = [1000 0 0;0 1000 0;0 0 1000]; Rmag = [0.00001 0 0;0 0.00001 0;0 0 0.00001];
% Compute initial roll and pitch angles N = length(time); % number of data samples
%T = 1/50; % Prediction step should nominally run at roughly 400Hz or greater d2r = pi/180; phi_est = zeros(N,1); theta_est = zeros(N,1); psi_est = zeros(N,1);
60
phi = -180/pi*atan(accel_y(1)/accel_z(1)); % deg theta = -180/pi*atan(accel_x(1)/accel_z(1)); % deg psi = 0; % rad phi_est(1) = phi; theta_est(1) = theta; psi_est(1) = psi; for i = 2:N-1
% Elapsed time since last prediction dT = time(i) - time(i-1);
T = dT/1000;
% Retrieve angular rates from sensor_data p = gyro_x(i+1); % deg/s q = gyro_y(i+1); r = gyro_z(i+1); cos_phi = cos(phi*pi/180); tan_theta = tan(theta*pi/180); sin_phi = sin(phi*pi/180); cos_theta = cos(theta*pi/180); sin_theta = sin(theta*pi/180);
% Compute expected angle rates based on gyro outputs phi_dot = p + sin_phi*tan_theta*q + cos_phi*tan_theta*r; theta_dot = cos_phi*q - sin_phi*r;
psi_dot = (sin_phi/cos_theta)*q + (cos_phi/cos_theta)*r;
% EKF_Predict
61
theta = theta + T*theta_dot;% deg psi = psi + T*psi_dot; % deg
% Linearize propogation equation for covariance estimation A(1,1) = q*cos_phi*tan_theta - r*sin_phi*tan_theta;
A(1,2) = r*cos_phi*(tan_theta*tan_theta + 1) + q*sin_phi*(tan_theta*tan_theta + 1); A(1,3) = 0;
A(2,1) = -r*cos_phi - q*sin_phi; A(2,2) = 0;
A(2,3) = 0;
A(3,1) = (q*cos_phi)/cos_theta - (r*sin_phi)/cos_theta;
A(3,2) = (r*cos_phi*sin_theta)/(cos_theta*cos_theta) + (q*sin_phi*sin_theta)/(cos_theta*cos_theta); A(3,3) = 0;
% Compute new covariance: P = P + T*(AP + PA^T + Q) P = P + T*(A*P + P*A' + Q);
% Update states estimates in data structure cos_phi = cos(phi*pi/180); cos_theta = cos(theta*pi/180); cos_psi = cos(psi*pi/180); sin_phi = sin(phi*pi/180); sin_theta = sin(theta*pi/180); sin_psi = sin(psi*pi/180);
% Build rotation matrix from inertial frame to body frame (for
% computing expected sensor outputs given yaw, pitch, and roll angles) R(1,1) = cos_psi*cos_theta;
62 R(1,2) = cos_theta*sin_psi; R(1,3) = -sin_theta; R(2,1) = cos_psi*sin_phi*sin_theta - cos_phi*sin_psi; R(2,2) = cos_phi*cos_psi + sin_phi*sin_psi*sin_theta; R(2,3) = cos_theta*sin_phi; R(3,1) = sin_phi*sin_psi + cos_phi*cos_psi*sin_theta; R(3,2) = cos_phi*sin_psi*sin_theta - cos_psi*sin_phi; R(3,3) = cos_phi*cos_theta; % Retrieve accel_ref
acc_ref = accel_alignment*(accel_ref - accel_bias);
% Compute expected accelerometer output based on yaw, pitch, roll acc_hat = R*acc_ref;
% Compute C matrix for Kalman gain calculation C(1,1) = 0;
C(1,2) = -acc_ref(3)*cos_theta - acc_ref(1)*cos_psi*sin_theta - acc_ref(2)*sin_psi*sin_theta; C(1,3) = acc_ref(2)*cos_psi*cos_theta - acc_ref(1)*cos_theta*sin_psi;
C(2,1) = acc_ref(1)*(sin_phi*sin_psi + cos_phi*cos_psi*sin_theta) - acc_ref(2)*(cos_psi*sin_phi - cos_phi*sin_psi*sin_theta) + acc_ref(3)*cos_phi*cos_theta;
C(2,2) = acc_ref(1)*cos_psi*cos_theta*sin_phi - acc_ref(3)*sin_phi*sin_theta + acc_ref(2)*cos_theta*sin_phi*sin_psi;
C(2,3) = -acc_ref(1)*(cos_phi*cos_psi + sin_phi*sin_psi*sin_theta) - acc_ref(2)*(cos_phi*sin_psi - cos_psi*sin_phi*sin_theta);
C(3,1) = acc_ref(1)*(cos_phi*sin_psi - cos_psi*sin_phi*sin_theta) - acc_ref(2)*(cos_phi*cos_psi + sin_phi*sin_psi*sin_theta) - acc_ref(3)*cos_theta*sin_phi;
C(3,2) = acc_ref(1)*cos_phi*cos_psi*cos_theta - acc_ref(3)*cos_phi*sin_theta + acc_ref(2)*cos_phi*cos_theta*sin_psi;
C(3,3) = acc_ref(1)*(cos_psi*sin_phi - cos_phi*sin_psi*sin_theta) + acc_ref(2)*(sin_phi*sin_psi + cos_phi*cos_psi*sin_theta);
63
% Compute Kalman gain: L = PC^T * (R + CPC^T)^(-1) L = (P*C') * ((Racc + C*P*C')^(-1));
% Compute new covariance P = (eye(3)-L*C)*P;
% Retrieve accelerations from sensor_data acc_vect = [accel_x(i);accel_y(i);accel_z(i)]; acc_vect = accel_alignment*(acc_vect-accel_bias); % Correction correction = L*(acc_vect-acc_hat); % EKF_Acceleration update phi = phi + correction(1); theta = theta + correction(2); psi = psi + correction(3);
% "Unroll" angle estimates to be in the range from -360 to 360 degrees if phi > 360,phi = phi - 360;end;
if phi < -360,phi = phi + 360;end; if theta > 360,theta = theta - 360;end; if theta < -360,theta = theta + 360;end; if psi > 360,psi = psi - 360;end; if psi < -360,psi = psi + 360;end;
64
m_ref = mag_cal*(mag_ref-mag_bias);
% Compute C based on magnetic field data in inertial frame C(1,1) = 0; C(1,2) = 0; C(1,3) = m_ref(2)*cos_psi - m_ref(1)*sin_psi; C(2,1) = 0; C(2,2) = 0; C(2,3) = -m_ref(1)*cos_psi - m_ref(2)*sin_psi; C(3,1) = 0; C(3,2) = 0; C(3,3) = 0;
% Rotate reference vector into vehicle-1 frame R(1,1) = cos_psi; R(1,2) = sin_psi; R(1,3) = 0; R(2,1) = -sin_psi; R(2,2) = cos_psi; R(2,3) = 0; R(3,1) = 0; R(3,2) = 0; R(3,3) = 1;
% Compute expected magnetic field vector mag_hat = R*m_ref;
% Retrieve magnetic field from sensor_data mag_vect = [mag_x(i);mag_y(i);mag_z(i)];
65
mag_vect = mag_cal*(mag_vect-mag_bias);
% Build rotation matrix from body frame to vehicle-1 frame (ie. only yaw remains uncorrected) R(1,1) = cos_theta; R(1,2) = sin_phi*sin_theta; R(1,3) = cos_phi*sin_theta; R(2,1) = 0; R(2,2) = cos_phi; R(2,3) = -sin_phi; R(3,1) = -sin_theta; R(3,2) = cos_theta*sin_phi; R(3,3) = cos_phi*cos_theta;
% Rotate measurement into vehicle-1 frame mag_vect = R*mag_vect;
% Compute Kalman gain: L = PC^T * (R + CPC^T)^(-1) L = P*C'*((Rmag + C*P*C')^(-1));
% Compute new covariance P = (eye(3)-L*C)*P; % Correction correction = L*(mag_vect-mag_hat); % EKF_Acceleration update phi = phi + correction(1,1); theta = theta + correction(2,1); psi = psi + correction(3,1);
66
% "Unroll" angle estimates to be in the range from -360 to 360 degrees if phi > 360,phi = phi - 360;end;
if phi < -360,phi = phi + 360;end; if theta > 360,theta = theta - 360;end; if theta < -360,theta = theta + 360;end; if psi > 360,psi = psi - 360;end; if psi < -360,psi = psi + 360;end; phi_est(i+1) = phi; theta_est(i+1) = theta; psi_est(i+1) = psi; end figure(1); plot(time,yaw,'b',time,psi_est,'r');xlabel('Samples');ylabel('Psi');legend('yaw','est'); figure(2); plot(time,pitch,'b',time,theta_est,'r');xlabel('Samples');ylabel('Theta');legend('pitch','est'); figure(3); plot(time,roll,'b',time,phi_est,'r');xlabel('Samples');ylabel('Phi');legend('roll','est');
2.5. KẾT QUẢ THÍ NGHIỆM VÀ ƯỚC LƯỢNG GĨC
Dựa trên số liệu thu được từ cảm biến của mơ hình thí nghiệm IMU ta xác định góc trạng thái Euler (Roll, Pitch, Yaw) trên bàn xoay 3 trục. Sau đó so sánh kết quả đó với các thơng số của nhà sản xuất trên IMU.
2.5.1. Trường hợp làm thí nghiệm ước lượng góc ROLL:
Đưa tập tin số liệu trường hợp Roll vào code Matlab đã cung cấp ta vẽ được 2 đồ thị:
67
- Đồ thị thực hiện thí nghiệm thu được
Hình 2-17: Đồ thị tương quan thông số nhà sản xuất và thực nghiệm đo góc Roll (Thay đổi góc Roll)
Nhận xét:
- Kết quả đồ thị tương quan thơng số cho góc Roll cho thấy kết quả đo thực nghiệm khá bám với thơng số của nhà sản xuất.
- Nhìn chung vẫn còn sai số giữa 2 đồ thị, đặt biệt là tại các biên độ ở đỉnh có sai số lớn.
- Đồ thị có tính chất điều hịa, nhưng lúc đầu do chưa hẳn ổn định tầng số nên có biên độ thấp hơn.
- Với biên độ dao động cực đại của đồ thị góc Roll của đồ thị est nằm trong khoảng (-60;63) độ. Phần đồ thị roll có biên độ trong khoảng (-63;60) độ
2.5.2. Trường hợp làm thí nghiệm ước lượng góc PITCH
68
Hình 2-18: Đồ thị tương quan thông số nhà sản xuất và thực nghiệm đo góc Pitch (Thay đổi góc Pitch)
Nhận xét:
- Kết quả đồ thị tương quan thơng số cho góc Pitch cho thấy kết quả đo thực nghiệm khá bám với thông số của nhà sản xuất.
- Vẫn còn sai số giữa 2 đồ thị, nhất là tại các biên độ ở đỉnh có sai số lớn. Ở đỉnh
tại mẫu 4
6 10 sự sai số lớn chênh lệch khoảng 12o.
- Với biên độ dao động cực đại của đồ thị góc Pitch của đồ thị est nằm trong khoảng (-54;40) độ. Phần đồ thị roll có biên độ trong khoảng (-54;30) độ.
2.5.3. Trường hợp làm thí nghiệm ước lượng góc YAW
69
Hình 2-19. Đồ thị tương quan thông số nhà sản xuất và thực nghiệm đo góc Yaw (Thay đổi góc Yaw)
Nhận xét:
- Kết quả đồ thị tương quan thông số cho góc Yaw cho thấy kết quả đo thực nghiệm khá bám với thông số của nhà sản xuất.
- Vẫn còn sai số giữa 2 đồ thị, nhất là tại các biên độ ở đỉnh có sai số lớn. Có xuất hiện sự lệch hình dạng của 2 đồ thị tại những mẫu đầu tiên, do là chưa ổn định tần số ban đầu nên gây ra sự sai số này.
- Quan sát thấy phần đồ thị nằm trên trục hồn nên các giá trị góc Yaw đều là giá trị dương.
- Với biên độ dao động cực đại của đồ thị góc Yaw của đồ thị est nằm trong khoảng (163;0) độ. Phần đồ thị roll có biên độ trong khoảng (160;0) độ.
2.5.4. Trường hợp làm thí nghiệm ước lượng góc Roll, Pitch, Yaw kết hợp – MIX Thực hiện thay đổi cả 3 góc Roll, Pitch, Yaw kết quả đi bằng cảm biến IMU được đưa vào tập tin kết quả. Đưa tập tin số liệu trường hợp Pitch vào code Matlab đã cung cấp ta vẽ được 2 đồ thị:
70
Hình 2-20. Đồ thị tương quan thơng số nhà sản xuất và thực nghiệm đo góc Roll (Thay đổi Roll, Pitch, Yaw)
71
Hình 2-22. Đồ thị tương quan thơng số nhà sản xuất và thực nghiệm đo góc Yaw (Thay đổi Roll, Pitch, Yaw)
Nhận xét:
- Kết quả đồ thị tương quan thơng số cho góc Roll, Pitch, Yaw cho thấy kết quả đo thực nghiệm khá bám với thông số của nhà sản xuất. Kết quả thõa mãn ở trường hợp thay đổi cả 3 góc Euler này.
- Vẫn còn sai số giữa 2 đồ thị, nhất là tại các biên độ ở đỉnh có sai số lớn.
• Đồ thị Roll: có dạng khơng tuần hồn. Biên độ lúc đầu nhỏ từ từ tăng lên
đến mẫu 4
4 10 biên độ tăng cực đại. Khoảng biên độ dao động từ (-60; 60) độ đối với est, (-42; 58) độ đối với đồ thị roll.
• Đồ thị Pitch: có dạng khá tuần hồn ở đầu về sau biên độ dao động tăng
mạnh ở khoảng mẫu 4
4.8 10 . Khoảng biên độ dao động từ (-78; 40) độ
đối với est, (-76; 30) độ đối với đồ thị pitch.
• Đồ thị Yaw: ban đầu chưa ổn định tầng số nên bị lệch nhiều, dần tuần hoàn. Khoảng biên độ dao động dương (thuộc bề trên trục hoành) từ (37; 150) độ đối với est, (37; 140) độ đối với đồ thị pitch.
2.5.5. Kết luận
- Nhìn chung, sự tương quan giữa 2 đồ thị thực nghiệm và thông số nhà sản xuất khá tốt, kết quả khá bám nhau. Nhưng vẫn còn sai số, nhất là ở các đỉnh. Nguyên
72
nhân là do độ nhạy của cảm biến chưa được tốt, nên khi kết thúc một chu kì thì cảm biến bắt kết quả chậm so với đồ thị thông số của nhà sản xuất.
- Ngoài ra, Do là khơng cố định được hồn tồn các trục khác nên có thể gây sai số do quá trình thực hiện thao tác.
- Chú ý khi làm thí nghiệm thay đổi góc với mơ hình cần lắc cho góc dao động ổn định trước khi thực hiện thu nhận kết quả. Đặc biệt, muốn đạt được kết quả tốt dễ xem xét nên thao tác với bàn xoay tạo ra biên độ dao động đủ lớn và chu kì dài.
73
TÀI LIỆU THAM KHẢO
[1] ĐẶNG TRUNG DUẨN, BÀI THÍ NGHIỆM VỀ THIẾT BỊ ĐO LƯỜNG QUÁN TÍNH
(IMU) TRÊN BÀN XOAY 3 TRỤC.
[2] YOUNGJOO KIM AND HYOCHOONG BANG, INTRODUCTION TO KALMAN
74
CHƯƠNG 3. BÀI THÍ NGHIỆM ẢNH HƯỞNG CỦA SỰ TẬP TRUNG ỨNG SUẤT TRUNG ỨNG SUẤT
3.1. MỤC ĐÍCH THÍ NGHIỆM
Thí nghiệm này nhằm xác định sự khác nhau về quá trình rạn nứt và độ lớn lực phá hủy của các hình dạng vết cắt khác nhau (với kích thước chiều rộng, chiều sâu của vết cắt là như nàu) và các độ mở góc khác nhau đối với vết cắt hình chữ V tương ứng với các hệ số tập trung ứng suất khác nhau.
Yêu cầu xác định quá trình rạn nứt và độ lớn lực phá hủy do ảnh hưởng của dạng sợi ngắn gia cường được sử dụng trong composites, bằng cách dùng mẫu giấy thử được bấm kim bấm.
3.2. CƠ SỞ LÝ THUYẾT
Trong nhiều trường hợp bất kì, sự thay đổi về tiết diện (cross-section) đều làm thay đổi sự phân bố ứng suất và gia tăng ứng suất. Tính bất liên tục này được gọi là sự gia tăng ứng suất và những vùng này được gọi vùng tập trung ứng suất. Với cùng 1 lực tác động, vùng nào có sự tập trung ứng suất lớn hơn thì ở đó có ứng suất lớn hơn.
Với Kt được gọi là hệ số tập trung ứng suất : - o t K max
= cho ứng suất theo phương pháp tuyến (kéo, nén, uốn).
- o t K max
= cho ứng suất cắt (xoắn).
Trong đó:
• max và max là ứng suất lớn nhất.
• o và o là ứng suất danh nghĩa (nominal stress).
75
• Đối với vết cắt hình chữ U (U-notch) chịu tải kéo:
Hình 3-1: Hệ số tập trung ứng suất Ktn đối với tấm phẳng chịu kéo với vết cắt hình chữ U ở một cạnh (từ dữ liệu