2. Kết quả thực nghiệm trong điều kiện bình thường
2.3. Trường hợp 3: Cảm biến MARG chuyển động cùng lúc các trục
Trong thực tế xảy ra rất nhiều trường hợp chuyển động, chuyển động theo từng trục đơn chỉ là một trường hợp, phần lớn trong các trường hợp là chuyển động kết hợp giữa các trục. Vì vậy để xét được đáp ứng của bộ ước lượng một cách tổng quát hơn, ta xét tiếp trường hợp cảm biến MARG với chuyển động cùng lúc cả ba trục.
(a)
(b)
0 1 2 3 4 5 6 7 8 9 10
-20 -10 0 10
Wx(Deg/s)
Measured Value of Gyroscope
0 1 2 3 4 5 6 7 8 9 10
-20 -10 0 10
Wy(Deg/s)
0 1 2 3 4 5 6 7 8 9 10
-20 0 20
Wz(Deg/s)
Time(sec)
0 1 2 3 4 5 6 7 8 9 10
-0.3 -0.2 -0.1 0
Ax(G)
Measured Value of Accelerometer
0 1 2 3 4 5 6 7 8 9 10
0 0.2 0.4
Ay(G)
0 1 2 3 4 5 6 7 8 9 10
0.5 1
Az(G)
Time(sec)
Hình 4.14 Dữ liệu lấy từ cảm biến cảm biến MARG mô phỏng (a) Dữ liệu vận tốc (gyroscope); (b) Dữ liệu cảm biến gia tốc (Accelerometer);(c) Dữ liệu cảm biến la bàn
(compass)
Hình 4.15 Góc Roll sau khi qua ước lượng và được so sánh với góc Roll thực tế
Hình 4.16 Góc Pitch sau khi qua ước lượng và được so sánh với góc Pitch thực tế
0 1 2 3 4 5 6 7 8 9 10
50 100
Mx(mGa)
Measured Value of compass
0 1 2 3 4 5 6 7 8 9 10
-30 -20 -10 0
My(mGa)
0 1 2 3 4 5 6 7 8 9 10
0 20 40
Mz(mGa)
Time(sec)
0 1 2 3 4 5 6 7 8 9 10
-5 0 5 10 15 20 25 30 35
40 Roll() (DCM-KALMAN)
time(sec)
deg
Raw Roll Estimated Roll Reference Roll
0 1 2 3 4 5 6 7 8 9 10
-5 0 5 10 15 20
25 Pitch()(DCM-KALMAN)
time(sec)
deg
Raw Pitch Estimated Pitch Reference Pitch
(c)
Hình 4.17 Góc Yaw sau khi qua ước lượng và được so sánh với góc Yaw thực tế
Hình 4.18 Sai số của ba góc sau khi ước lượng so với góc thực tế
Với trường hợp 3, cả ba góc được mô phỏng chuyển động cùng lúc với vận tốc nhỏ và độ lớn góc chuyển động là gần như nhau. Quan sát hình 4.15, hình 4.16, hình 4.17 ta thấy cả ba góc đều có nhiễu trong chuyển động của từng góc và với hình 4.14 ta sẽ quan sát rõ hơn nhiễu từ dữ liệu đo được của cảm biến MARG mô phỏng. Với lượng nhiễu khá lớn nhưng sau khi qua bộ ước lượng góc ước lượng cho độ chính xác tốt hơn và gần
giống với góc thực tế. Với sai số trung bình (eRMS) và sai số lớn nhất (𝑒𝑀𝑎𝑥) lần lượt cho từng góc thông qua hình: 0.3263 và 0.98608 với góc Roll, 0.28952 và 0.83 với góc Pitch, 0.23118 và 0.76211 với góc Yaw. Ta thấy ngay cả khi ba góc cùng chuyển động sai số vẫn rất nhỏ và với giá trị sai số lớn nhất vẫn nhỏ hơn 2°, sai số trung bình vẫn nhỏ hơn 0.4° trong trường hợp này. Nếu nhận xét kĩ hơn, ta thấy góc Yaw trước khi ước lượng
0 1 2 3 4 5 6 7 8 9 10
-5 0 5 10 15 20 25 30 35
yaw()(DCM-KALMAN)
time(sec)
deg
Raw Yaw Estimated Yaw Reference Yaw
0 1 2 3 4 5 6 7 8 9 10
-0.50.50
Error Roll()(DCM-KALMAN) e-RMS = 0.3263, e-Max = 0.98608
deg
Error Roll
0 1 2 3 4 5 6 7 8 9 10
-0.5 0 0.5
Error Pitch()(DCM-KALMAN) e-RMS = 0.28952, e-Max = 0.83007
deg
Error Pitch
0 1 2 3 4 5 6 7 8 9 10
-0.5 0 0.5
Error Yaw()(DCM-KALMAN) e-RMS = 0.23118, e-Max = 0.76211
time(sec)
deg
Error Yaw
đã được kết hợp với cảm biến từ trường đã cho kết quả là nhiễu ít hơn hai góc Roll và Pitch. Sau khi qua ước lượng, kết quả còn tốt hơn với độ chính xác cao hơn.
2.4. Trường hợp 4: cảm biến MARG chuyển động với vận tốc lớn (mô phỏng chuyển động lắc trong thực tế )
Qua những trường hợp trên ta đã từng bước đánh giá thuật toán đối với cảm biến MARG khi cố định trong mặt phẳng, chuyển động trong từng trục cũng như chuyển động trong nhiều trục cùng lúc. Tuy nhiên trong các trường hợp đều xảy ra với chuyển động chậm và góc thay đổi chưa lớn. Vậy nếu cảm biến chuyển động trong trường hợp giả thiết ở đây là chuyển động lắc hay là chuyển động với vận tốc lớn và góc thay đổi lớn. Ta sẽ có một cái nhìn tổng quát hơn đối với thuật toán trong trường hợp mà nhiều thuật toán ước lượng khác cho sai số rất lớn.
0 1 2 3 4 5 6 7 8 9 10
-200 0 200
Wx(Deg/s)
Measured Value of Gyroscope
0 1 2 3 4 5 6 7 8 9 10
-10 0 10
Wy(Deg/s)
0 1 2 3 4 5 6 7 8 9 10
-20 0 20
Wz(Deg/s)
Time(sec)
0 1 2 3 4 5 6 7 8 9 10
-0.1 -0.05 0 0.05
Ax(G)
Measured Value of Accelerometer
0 1 2 3 4 5 6 7 8 9 10
-1 0 1
Ay(G)
0 1 2 3 4 5 6 7 8 9 10
-1 0 1
Az(G)
Time(sec)
(a)
(b)
Hình 4.19 Dữ liệu lấy từ cảm biến cảm biến MARG mô phỏng (a) Dữ liệu vận tốc (gyroscope); (b) Dữ liệu cảm biến gia tốc (Accelerometer);(c) Dữ liệu cảm biến la bàn
Hình 4.20 Góc Roll sau khi qua ước lượng và được so sánh với góc Roll thực tế
Hình 4.21 Góc Pitch sau khi qua ước lượng và được so sánh với góc Pitch thực tế
0 1 2 3 4 5 6 7 8 9 10
50 100
Mx(mGa)
Measured Value of compass
0 1 2 3 4 5 6 7 8 9 10
-0.5 0 0.5 1
My(mGa)
0 1 2 3 4 5 6 7 8 9 10
-1 0 1
Mz(mGa)
Time(sec)
0 1 2 3 4 5 6 7 8 9 10
-200 -150 -100 -50 0 50 100 150 200
Roll() (DCM-KALMAN)
time(sec)
deg
Raw Roll Estimated Roll Reference Roll
0 1 2 3 4 5 6 7 8 9 10
-6 -4 -2 0 2 4 6
Pitch()(DCM-KALMAN)
time(sec)
deg
Raw Pitch Estimated Pitch Reference Pitch
(c)
Hình 4.22 Góc Yaw sau khi qua ước lượng và được so sánh với góc Yaw thực tế
Hình 4.23 Sai số của ba góc sau khi ước lượng so với góc thực tế
Trường hợp này góc Roll được mô phỏng với chuyển động như là chuyển động lắc, với góc thay đổi lớn từ khoảng gần 180 đến -180 trong khoảng thời gian 2 giây (dựa vào hình 4.20), hai góc Pitch và Yaw được giữ cố định. Tần số trích mẫu sử dụng để lấy dữ
liệu trong mô phỏng là 100 Hz (tương đương thời gian lấy mẫu là 0.01 giây). Để có thể ước lượng chính xác với chuyển động nhanh thì thời gian lấy mẫu nhanh cho dữ liệu đáp ứng tốt hơn với sự thay đổi đột ngột của chuyển động. Dựa vào Hình 4.23 ta thấy sai số trung bình (𝑒𝑅𝑀𝑆) và sai số lớn nhất (𝑒𝑀𝑎𝑥) của góc Roll lần lượt là 1.5941 và 2.675 vẫn rất nhỏ. Đối với sai số lớn nhất đạt được vẫn nhỏ hơn sai số đặt ra của đề tài là 3° và sai số trung bình nhỏ hơn 2°. Với khả năng ước lượng góc cho độ chính xác cao ngay cả khi với chuyển động ở tốc độ cao và độ biến thiên vị trí lớn thuật toán cho thấy
lợi thế hơn những thuật toán ước lượng khác về độ chính xác trong nhiều trường hợp và đã đáp ứng tốt mục tiêu mà đề tài đặt ra.
0 1 2 3 4 5 6 7 8 9 10
-1 -0.5 0 0.5 1 1.5 2
2.5 yaw()(DCM-KALMAN)
time(sec)
deg
Raw Yaw Estimated Yaw Reference Yaw
0 1 2 3 4 5 6 7 8 9 10
-2 0 2
Error Roll()(DCM-KALMAN) e-RMS = 1.5941, e-Max = 2.675
deg
Error Roll
0 1 2 3 4 5 6 7 8 9 10
-0.50.50
Error Pitch()(DCM-KALMAN) e-RMS = 0.32209, e-Max = 0.93357
deg Error Pitch
0 1 2 3 4 5 6 7 8 9 10
0 1 2
Error Yaw()(DCM-KALMAN) e-RMS = 0.1555, e-Max = 2.3006
time(sec)
deg
Error Yaw
CHƯƠNG V: THỰC NGHIỆM
1. Phần cứng 1.1. Board cảm biến MARG
Sử dụng board điều khiển “Multiwii and Megapirate AIO Flight Controller w/FTDI (ATmega 2560) V2.0” (flight control) sử dụng cho multicopter. Board được thiết kế với chíp xử lý là ATMega 2560 Microcontroller và gồm các cảm biến: biến gia tốc gốc và vận tốc góc MPU 6050, cảm biến từ trường HMC 5883L và cảm biến áp suất độ chính
xác cao MS5611-01BA01.
Board được thiết kế theo board Adruno 2560 nên cho phép lập trình và giao tiếp dễ dàng với máy tính và thiết bị ngoại vi. Với IMU được tích hợp theo chuẩn I2C giúp dễ dàng giao tiếp với các cảm biến và nhận dữ liệu.
Hỗ trợ chuẩn giao tiếp Micro USB và sử dụng chip FT232RQ USB-UART giúp nạp code trực tiếp cho vi xử lý không qua board nạp. Dữ liệu được truyền nhận trực tiếp giữa board và máy tính.
Hình 5.1 Board “Multiwii and Megapirate AIO Flight Controller w/FTDI (ATmega 2560) V2.0” tích hợp cảm biến MPU6050 , cảm biến la bàn HCM5883L và cảm biến áp
suất MS5611-01BA01 1.1.1. Cảm biến MPU6050
MPU-6050 là cảm biến của hãng InvenSense. MPU-6050 là một trong những giải pháp cảm biến chuyển động đầu tiên trên thế giới có tới 6 (mở rộng tới 9) trục cảm biến tích hợp trong một chip duy nhất.
Cảm biến MPU6050 có các đặc điểm kỹ thuật sử dụng như sau:
- Nguồn cung cấp cho cảm biến từ 3-5V do trên board “Multiwii and Megapirate AIO Flight Controller w/FTDI (ATmega 2560) V2.0” đã có sẵn LDO chuyển nguồn 5V - 3.3V.
- Hỗ trợ giao tiếp với chuẩn truyền I2C có thể lên đến 400kHz.
- Thiết kế MPU6050 có sử dụng bộ chuyển đổi tương tự - số (Anolog to Digital Converter - ADC) 16-bit để cho ra kết quả chi tiết về vận tóc góc, gia tốc, nhiệt độ. Có
thể chuyển định dạng ngõ ra thành chuỗi thập phân để phù hợp với định dạng dữ liệu khi đưa vào Matlab.
- Thông số con quay hồi chuyển ba trục (3-axis MEMS Gyroscope).
Con quay hồi chuyển: ±250 ±500 ±1000 ±2000 (degree per second). Trong đề tài
thiết lập ở ±250.
Độ nhạy: 131 LSB/(°/s)
Tần số ngõ ra: 100 Hz
Nhiễu : 0.05 °/s – rms
- Thông số cảm biến gia tốc 3 chiều (3-axis MEMS accelerometer)
Cảm biến gia tốc: ±2 ±4 ±8 ±16 (g), trong đề tài thiết lập ở ± 2(°/s)
Độ nhạy: 16,384 LSB/g
Tần số ngõ ra: 100Hz
Nhiễu : 400 g/√Hz
Từ các giá trị mà cảm biến MPU6050 trả về. Ta sẽ dùng các công thức sẽ đề cập bên dưới để tính toán:
- Đối với dữ liệu lấy về từ cảm biến vận tốc Gyro:
GyroRate = gyroRead / [LSB Sensitivity]
Trong đó:
GyroRate: Vận tốc góc đơn vị là (degree/second) GyroRead: Dữ liệu (16bit) đọc về từ Gyro của MPU6050 [LSB Sensitivity]: Hệ số suy ra vận tốc góc, nó phụ thuộc vào việc thiết đặt chế độ
hoạt động của Gyro trong MPU6050 ở đây ta thiết lập ở ±250 (°/s).
Tương tự với dữ liệu lấy về từ cảm biến gia tốc Accelerometer nhưng với [LSB Sensitivity] là ± 2g.
1.1.2. Cảm biến la bàn HCM 5883L
Cảm biến la bàn HMC5883L do hãng Honeywell sản xuất, cảm biến la bàn số 3 trục này được sử dụng trong môi trường có từ tính thấp.
- Nguồn cung cấp cho cảm biến từ 2.16 - 3.6V.
- Cảm biến la bàn HMC5883L giao tiếp lấy dữ liệu qua chuẩn I2C.
- Tầm đo: ±1.3 – 8 gauss
- Độ phân giải: 5 milli-gauss
- Độ nhạy: 230 LSb/gauss
- Tần số ngõ ra: 100Hz.
- Hỗ trợ chuẩn giao tiếpI2C.
1.2. Cảm biến góc có độ chính xác cao.
Cảm biến đo góc “Inclination sensor INY360D-F99-2I2E2-V17” của hãng Pepperl- Fuchs cho ra tín hiệu góc trực tiếp với tín hiệu ngõ ra cả analog và digital. Với độ phân giải nhỏ và cho phép đo được góc nghiêng theo hai trục Roll và Pitch cùng lúc với độ chính xác cao.
Thông số cảm biến góc “Inclination sensor INY360D-F99-2I2E2-V17”:
- Loại: cảm biến góc 2 trục
- Phạm vi đo: 0 ... 360 °
- Độ chính xác tuyệt đối: ≤ ± 0.5 °
- Độ phân giải: ≤ 0.1 °
- Độ chính xác lặp lại : ≤ ± 0.1 °
- Ảnh hưởng của nhiệt độ: ≤ 0.027 °/K
- Điện áp hoạt động: 10...30 VDC
- Ngõ ra analog: 4…20mA.
- Khối lượng : 240 g
- Tiêu chuẩn bảo vệ: IP68 / IP69K
- Thiết lập từ nhà máy: Ngõ ra analog (X) -45 ° ... 45 °; Ngõ ra analog (Y) -45 ° ... 45 °
Hình 5.2 Cảm biến góc với độ chính xác cao (INY360D-F99-2I2E2-V17)
1.3. Mô hình máy bay Quadrotor
Hình 5.3 mô hình quadrotor sử dụng khung Tarot FY650 IRON MAN 650
Thống số mô hình:
- Động cơ sử dụng: 4 động cơ SunnySky X2212-13 980KV
- Cánh quạt: bốn cánh quạt Fun 10x4.7 (2 cánh thuận, 2 cánh nghịch)
- ESC: 4 ESC HobbyWing SkyWalker 40A
- Bộ điều khiển bay: APM 2.6
- Pin : 2 viên pin Li-Po Wild Scorpion 2200mAh - 35C
- GPS: Ubox super precision LEA-6h GPS and Compass
- Bộ thu phát sóng radio: Telemetry radios (433MHz)
- Modun đo nguồn: 10s Power sensor and BEC Module ( XT-60 connectors)
- Khối lượng toàn bộ mô hình: 1,6 kg
- Tải trọng mang theo tối đa của mô hình: 2 kg
- Kích thước: 460 x 460 x 260( đã bao gồm chiều cao động cơ)
- Không gian làm việc: 714 x 714 x 260( với cánh 10x 4.7 )
- Thời gian bay: 7 Phút
2. Kết quả thực nghiệm trong điều kiện bình thường.
Để đánh giá được độ chính xác của thuật toán, ta sử dụng một cảm biến đo góc
“Inclination sensor INY360D-F99-2I2E2-V17” có độ chính xác cao để tiến hành đánh giá. Với cảm biến góc 2 trục ta sẽ có được góc Roll và góc Pitch đúng nhất để làm chuẩn.
Cảm biến góc và cảm biến MARG được gắn trên cùng một mặt phẳng và đặt sát nhau. Khi đó sai số về vị trí lắp đặt là nhỏ nhất. Ở đây để cho việc thực nghiệm chính
xác và hiệu quả nhất, board điều khiển (flight control) bao gồm IMU và cảm biến góc nghiêng được lắp trên mô hình bay quadcopter để tiến hành ước lượng và đo đạc các có Euler (Roll, Pitch, Yaw).
Hình 5.4 Cảm biến MARG và cảm biến góc nghiêng được lắp trên mô hình
Quadrotor để tiến hành làm thực nghiệm
Ta tiến hành quay toàn bộ mô hình theo hai trục Roll, Pitch và Yaw. Ta sẽ thu được dữ liệu từ các cảm biến trên IMU và dữ liệu về góc chuẩn từ cảm biến góc. Tất cả các dữ liệu được đưa vào Matlab để tiến hành mô phỏng đánh giá thuật toán. Với dữ liệu đầu vào là dữ liệu thu thập được từ khối IMU là thông số thực tế đã bao gồm nhiễu, ta
tiến hành ước lượng các góc Euler dựa vào thuật toán đã mô tả phía trên. Sau khi đã ước lượng, sử dụng dữ liệu thu được từ cảm biến góc làm chuẩn (Reference) ta sẽ đánh giá được độ chính xác của góc ước lượng so với góc chuẩn. Từ đó đánh giá được tính hiệu
quả và chính xác của thuật toán trong điều kiện thực tế.
Nhưng vì điều kiện thực hiện đề tài không cho phép, nên hiện tại vẫn chưa có cảm biến nào đủ độ chính xác cao để đo được dữ liệu góc Yaw. Vì vậy đề tài sử dụng dữ liệu từ cảm biến la bàn trong khối IMU kết hợp với dữ liệu hai góc Roll và Pitch từ cảm biến góc để có được góc Yaw theo như công thức 3.14 ở trên. Như vậy ta đã đủ 3 góc Roll, Pitch, Yaw làm chuẩn. Cảm biến la bàn ở đây được đặt trong tình trạng lý tưởng nhất
có thể như tránh xa các nguồn điện gây nhiễu, đặt ở không gian mở.
Phương pháp thử nghiệm:
- Điều kiện tĩnh: board MARG giữ cố định với nhiễu là nhiễu từ cả 3 cảm biến.
- Điều kiện động: Di chuyển board ( theo từng trục, kết hợp các trục và di chuyển theo dạng lắc ). Nhiễu gồm có nhiễu của 3 cảm biến, nhiễu gia tốc ngoài và nhiễu chuyển động.
Giống như trên mô phỏng, ta sử dụng sai số trung bình 𝑒𝑅𝑀𝑆 và sai số lớn nhất 𝑒𝑀𝑎𝑥 để quan sát được giá trị sai số một cách rõ ràng nhất, giúp đánh giá độ chính xác một cách chính xác hơn.
2.1. Trường hợp 1: Cảm biến MARG được giữ cố định trong mặt phẳng
Trong trường hợp này, ta xét cảm biến MARG được giữ cố định trên mặt phẳng cùng với cảm biến góc nhằm xem xét độ chính xác của thuật toán trong điều kiện tĩnh.
Cảm biến góc với độ phân giải nhỏ nhất là 0.1°.
0 1 2 3 4 5 6 7 8 9 10
-0.05 0 0.05 0.1
Wx(Deg/s)
Measured Value of Gyroscope
0 1 2 3 4 5 6 7 8 9 10
-0.1 -0.05 0 0.05
Wy(Deg/s)
0 1 2 3 4 5 6 7 8 9 10
-0.05 0 0.05 0.1
Wz(Deg/s)
Time(sec)
(b)
Hình 5.5 Dữ liệu lấy từ cảm biến cảm biến MARG thực tế (a) Dữ liệu vận tốc (Gyroscope); (b) Dữ liệu cảm biến gia tốc (Accelerometer); (c) Dữ liệu cảm biến la bàn
Hình 5.6 Góc Roll sau khi qua thuật toán ước lượng được so sánh với góc Roll chưa
qua xử lý và góc Roll làm chuẩn lấy từ cảm biến góc
0 1 2 3 4 5 6 7 8 9 10
-4 -2 0 2x 10-3
Ax(G)
Measured Value of Accelerometer
0 1 2 3 4 5 6 7 8 9 10
-4 -2 0 2x 10-3
Ay(G)
0 1 2 3 4 5 6 7 8 9 10
0.995 1 1.005
Az(G)
Time(sec)
0 1 2 3 4 5 6 7 8 9 10
-118 -116 -114 -112
Mx(mGa)
Measured Value of compass
0 1 2 3 4 5 6 7 8 9 10
-232 -230 -228
My(mGa)
0 1 2 3 4 5 6 7 8 9 10
402 404 406 408
Mz(mGa)
Time(sec)
0 1 2 3 4 5 6 7 8 9 10
-0.2 -0.1 0 0.1 0.2
Roll() (DCM-KALMAN)
time(sec)
deg
Raw Roll Reference Roll Estimated Roll
(c)
Hình 5.7 Góc Pitch sau khi qua thuật toán ước lượng được so sánh với góc Pitch
chưa qua xử lý và góc Pitch làm chuẩn lấy từ cảm biến góc
Hình 5.8 Góc Yaw sau khi qua thuật toán ước lượng được so sánh với góc Yaw chưa
qua xử lý và góc Yaw làm chuẩn lấy từ cảm biến góc
Hình 5.9 Sai số của ba góc sau khi qua thuật toán ước lượng so với góc làm chuẩn
lấy từ cảm biến góc
0 1 2 3 4 5 6 7 8 9 10
-0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2
Pitch()(DCM-KALMAN)
time(sec)
deg
Raw Pitch Reference Pitch Estimated Pitch
0 1 2 3 4 5 6 7 8 9 10
-0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8
yaw()(DCM-KALMAN)
time(sec)
deg
Raw Yaw Reference Yaw Estimated Yaw
0 1 2 3 4 5 6 7 8 9 10
-0.2 0 0.2
Error Roll()(DCM-KALMAN) e-RMS = 0.10813, e-Max = 0.34937
deg
Error Roll
0 1 2 3 4 5 6 7 8 9 10
-0.10.10.20
Error Pitch()(DCM-KALMAN) e-RMS = 0.088966, e-Max = 0.23904
deg
Error Pitch
0 1 2 3 4 5 6 7 8 9 10
-0.5 0 0.5
Error Yaw()(DCM-KALMAN) e-RMS = 0.26488, e-Max = 0.72978
time(sec)
deg
Error Yaw
Dựa vào đồ thị Hình 5.6, Hình 5.7 ta thấy giá trị hai góc Roll và Pitch sau khi qua thuật toán ước lượng đã giảm được nhiễu khá tốt. Giá trị ban đầu của hai góc dao động trong khoảng 0.2°, sau khi qua thuật toán giá trị chỉ giao động trong khoảng 0.05° bám rất tốt so với đường chuẩn là đường 0° được xét trong trường hợp này. Giá trị của cảm biến góc với độ phân giải nhỏ nhất 0.1° cho thấy độ chính xác cao sẽ là cơ sở để đánh giá độ chính xác thực tế của thuật toán trong điều kiện thực. Với góc Yaw hình 5.8, sau khi kết hợp với dữ liệu cảm biến la bàn (cảm biến từ trường) cho giá trị góc ban đầu dao động với giá trị tối đa trong khoảng 0.8°, sau khi qua thuật toán giá trị dao động tối đa trong khoảng 0.35°. Giá trị sai số tối đa (eMax) trong trường hợp này cho ba góc lần lượt là 0.34937; 0.23904; 0.72978 đều nhỏ hơn yêu cầu đề tài. Cả ba góc đều cho giá trị tốt hơn cả giá trị làm chuẩn vì giá trị chuẩn bị giới hạn bởi độ phân giải nhỏ nhất là 0.1°.
Từ đây có thể kết luận thuật toán ước lượng trong trường hợp tĩnh cho đáp ứng tốt trong điều kiện tĩnh.
2.2. Trường hợp 2: Cảm biến MARG chuyển động trên từng trục đơn
Trong trường hợp này cảm biến MARG được cho chuyển động theo từng trục riêng rẽ để đánh giá độ chính xác trên từng trục rõ ràng hơn. Chuyển động được bắt đầu từ góc Roll sau đó là góc Pitch, góc Yaw được chuyển động sau cùng. Tuy nhiên góc Yaw vẫn chuyển động do trong quá trình làm thực nghiệm khi quay hai góc kia làm góc Yaw quay theo. Từ trường hợp này trở xuống ta sẽ xem xét thuật toán ở trạng thái động.
(a)
0 5 10 15 20
-20 0 20
Wx(Deg/s)
Measured Value of Gyroscope
0 5 10 15 20
-20 0 20
Wy(Deg/s)
0 5 10 15 20
-2020400
Wz(Deg/s)
Time(sec)
Hình 5.10 Dữ liệu lấy từ cảm biến cảm biến MARG thực tế (a) Dữ liệu vận tốc (Gyroscope); (b) Dữ liệu cảm biến gia tốc (Accelerometer);(c) Dữ liệu cảm biến la bàn
Hình 5.11 Góc Roll sau khi qua thuật toán ước lượng được so sánh với góc Roll
chưa qua xử lý và góc Roll làm chuẩn lấy từ cảm biến góc
0 5 10 15 20
-0.4 -0.2 0 0.2 0.4
Ax(G)
Measured Value of Accelerometer
0 5 10 15 20
-0.4 -0.20.20.40
Ay(G)
0 5 10 15 20
0.9 0.95 1
Az(G)
Time(sec)
0 5 10 15 20
80 100 120 140
Mx(mGa)
Measured Value of compass
0 5 10 15 20
-400 -350 -300
My(mGa)
0 5 10 15 20
300 400 500
Mz(mGa)
Time(sec)
0 5 10 15 20
-30 -20 -10 0 10 20 30
Roll() (DCM-KALMAN)
time(sec)
deg
Raw Roll Estimated Roll Reference Roll
(c) (b)