Báo cáo khoa học đề tài thiết kế và chế tạo imu 9 bậc

Báo cáo khoa học đề tài : Thiết kế và chế tạo imu 9 bậc

SỞ KHOA HỌC VÀ CÔNG NGHỆ THÀNH ĐOÀN

CHƯƠNG TRÌNH VƯỜN ƯƠM SÁNG TẠO KHOA HỌC VÀ CÔNG NGHỆ TRẺ

***

BÁO CÁO NGHIỆM THU

Chủ nhiệm đề tài: TS NGUYỄN VĨNH HẢO

Cơ quan chủ trì: TRUNG TÂM PHÁT TRIỂN KHOA HỌC VÀ CÔNG NGHỆ TRẺ

TP Hồ Chí Minh, tháng 06 năm 2012

Đề tài

THIẾT KẾ VÀ CHẾ TẠO IMU 9 BẬC

TÓM TẮT ĐỀ TÀI

IMU (Inertial Measurement Unit) là thiết bị dùng để đo gia tốc tịnh tiến và góc quay của một vật chuyển động trong không gian Mục tiêu của đề tài là nghiên cứu thiết kế và chế tạo thử nghiệm IMU 9 bậc với sai số góc quay nhỏ hơn 2o và tần số cập nhật 100Hz Thuật toán ước lượng tối ưu được xây dựng và lập trình trên họ vi

xử lý tốc độ cao DSP TI C2000 của Texas Instruments để tạo ra thiết bị IMU có

tần số cập nhật nhanh và kích thước nhỏ

Xuất phát từ nhu cầu thực tế ở Việt Nam khi chỉ có các ứng dụng công nghiệp phát triển mạnh, còn các ứng dụng cho tên lửa và hàng không hầu như chưa có, nên Nhóm nghiên cứu tập trung vào phân khúc sản phẩm áp dụng cho công nghiệp Với yêu cầu sai số cho phép từ 1o đến 2o thì giá thành thiết kế chế tạo sẽ rẻ và linh kiện cũng dễ tìm hơn Sản phẩm sau khi hoàn thiện có thế áp dụng được trong lĩnh vực công nghiệp, robot, lĩnh vực ô tô và máy bay mô hình

ABSTRACT

Inertial measurement unit (IMU) is a measurement device used to provide translating acceleration and angular velocity of an object in space The project’s aim is developing a complete solution of angular estimation for IMU with expected average angular error less than 2o at update frequency 100Hz An optimal estimation algorithm is formulated and embedded into the Texas Instruments DSP chip TMS320F28355 that supports the floating-point format to make the

measurement module more compact, fast update frequency, and high accurate

From current state of Vietnam where the industrial applications are well developed but the higher class such as missiles and aerospace applications are still limited, hence we focus on developing the product segment with required error in the range of 1o to 2o With the given requirements, components and materials are easy to get and designing and manufacturing costs are relatively cheap Completed product could be applied in industries, robotics, automobiles and modeling aircraft with taking into account of magnetic disturbance

MỤC LỤC

Tóm tắt đề tài I Mục lục II Danh sách các chữ viết tắt III Danh sách bảng IV Danh sách hình V Bảng quyết toán VI

PHẦN MỞ ĐẦU 1

CHƯƠNG 1 TỔNG QUAN 2

1.1 Tình hình nghiên cứu ngoài nước 2

1.2 Tình hình nghiên cứu trong nước 4

1.3 Tính cấp thiết của đề tài 4

1.4 Công trình nghiên cứu liên quan và các kết quả mới nhất 5

1.5 Các hướng giải quyết mới từ điều kiện thực tế ở Việt Nam 6

CHƯƠNG 2 NỘI DUNG VÀ PHƯƠNG PHÁP 9

2.1 Nội dung 1: Thiết kế và thi công phần cứng IMU 9

2.1 Nội dung 2: Xây dựng bộ ước lượng tối ưu 10

2.3 Nội dung 3: Cân chỉnh và đánh giá chất lượng IMU 16

CHƯƠNG 3 KẾT QUẢ VÀ THẢO LUẬN 18

3.1 Nội dung 1: Thiết kế và thi công phần cứng IMU 18

3.1 Nội dung 2: Xây dựng bộ ước lượng tối ưu 19

3.3 Nội dung 3: Cân chỉnh và đánh giá chất lượng IMU 22

CHƯƠNG 4 KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 26

4.1 Kết luận 26

4.1 Một số kết quả khác đạt được từ đề tài 27

4.3 Hướng phát triển 27

PHỤ LỤC 29

TÀI LIỆU THAM KHẢO 34

DANH SÁCH CÁC CHỮ VIẾT TẮT

VIẾT TẮT THUẬT NGỮ TIẾNG VIỆT

IMU Inertial Measurement Unit –

Thiết bị đo quán tính

INS Inertial Navigation System –

Hệ thống dẫn đường quán tính

GPS Global Navigation System –

Hệ thống định vị toàn cầu

DCM Direct Cosine Modulation –

Điều chế Cosine trực tiếp EKF Extended Kalman filter – Bộ

ước lượng Kalman mở rộng

DANH SÁCH BẢNG

1 Sai số 3 góc (RMS) giữa bộ ước lượng thiết kế với bộ

ước lượng tích hợp trong MTi-9

20

DANH SÁCH HÌNH

3 Sơ đồ kết nối phần cứng trong thiết bị IMU 9

4 Cấu trúc bộ ước lượng tối ưu dùng bộ lọc Kalman 10

6 Biểu diễn góc lệch của từ trường so với phương Bắc 12

9 Cấu trúc bộ ước lượng trên Matlab/Simulink 19

10 Giá trị ước lượng và sai số 3 góc khi đặt IMU đứng yên 20

11 Giá trị ước lượng và sai số 3 góc khi xoay quanh Z 21

12 Giá trị ước lượng và sai số 3 góc khi di chuyển dọc X 21

13 Giá trị ước lượng và sai số 3 góc khi chuyển động tự do 22

16 Độ lệch của tâm từ trường trục X, Y 24

17 Chương trình hiển thị kết quả trên máy tính 24

18 Thiết bị IMU 9 bậc lắp đặt trên Quadrotor 26

20 Quĩ đạo thực nghiệm mô hình tích hợp INS/ GPS 27

QUYẾT TOÁN KINH PHÍ (Phần này Trung tâm Phát triển KHCN sẽ bổ sung)

Đề tài: THIẾT KẾ VÀ CHẾ TẠO IMU 9 BẬC

Chủ nhiệm: TS Nguyễn Vĩnh Hảo

Cơ quan chủ trì: Trung tâm phát triển khoa học và công nghệ trẻ

Thời gian đăng ký trong hợp đồng: 12 tháng

Thời gian thực hiện giai đoạn 1: (Dùng cho báo cáo giám định)

Tổng kinh phí được duyệt: 80 triệu

Kinh phí cấp giai đoạn 1: (số tiền) (Theo thông báo số: /TB-KHCN ngày…) Kinh phí cấp giai đoạn 2: (số tiền) (Theo thông báo số: /TB-KHCN ngày…)

sách

Nguồn khác

I Kinh phí được cấp trong năm

II Kinh phí quyết toán trong năm

1 Công chất xám

2 Công thuê khoán

3 Nguyên, nhiên, vật liệu, dụng

cụ, phụ tùng, văn phòng phẩm

4 Thiết bị

5 Xét duyệt, giám định, nghiệm

thu

6 Hội nghị, hội thảo

7 Đánh máy tài liệu

8 Giao thông liên lạc

9 Chi phí điều hành

III Tiết kiệm 5%

IV Kinh phí chuyển sang năm sau

BÁO CÁO NGHIỆM THU

Tên đề tài: THIẾT KẾ VÀ CHẾ TẠO IMU 9 BẬC

Chủ nhiệm đề tài: TS Nguyễn Vĩnh Hảo

Cơ quan chủ trì: Trung tâm phát triển khoa học và công nghệ trẻ

Thời gian thực hiện đề tài: 12 tháng

Kinh phí được duyệt: 80 triệu đồng

Kinh phí đã cấp: 80 triệu đồng, theo TB số : TB-SKHCN ngày / /

Mục tiêu: (Theo đề cương đã duyệt)

IMU (Inertial Measurement Unit) là thiết bị dùng để đo gia tốc tịnh tiến và góc quay của một vật chuyển động trong không gian

Mục tiêu của đề tài là nghiên cứu thiết kế và chế tạo thử nghiệm IMU 9 bậc với sai số góc quay nhỏ hơn 2o và tần số cập nhật 100Hz Thuật toán ước lượng tối ưu được xây dựng và lập trình trên họ vi xử lý tốc độ cao DSP TI C2000 của Texas Instruments để tạo ra thiết bị IMU có tần số cập nhật nhanh và kích thước nhỏ

Nội dung: (Theo đề cương đã duyệt và hợp đồng đã ký)

Công việc dự kiến Công việc đã thực hiện

Đặt hàng và tích hợp cảm biến Hoàn thành

Cân chỉnh và đánh giá chất lượng Hoàn thành

Chương 1

TỔNG QUAN 1.1 Tình hình nghiên cứu ngoài nước

Ngày nay, IMU là một thiết bị không thể thiếu trong nhiều lĩnh vực như máy bay, tên lửa, tàu thủy, ô tô, robot công nghiệp, mô phỏng chuyển động của con người… Theo thống kê (từ nguồn www.yole.fr) thị trường tiêu thụ IMU trong năm

2009 có giá trị 1,85 tỉ USD, trong đó hàng không chiếm 55%; tên lửa 33%; hàng hải 4%; không gian 2%; công nghiệp, ô tô và các ứng dụng khác 6% Chính vì thị trường IMU trên thế giới rất lớn nên đã có rất nhiều công ty tham gia nghiên cứu chế tạo IMU với nhiều mức chất lượng và phạm vi ứng dụng khác nhau Điển hình trong số này là:

• Northrop Grumman, Mỹ:

Mã hiệu: IMU600 Kích thước: 14cm x 19cm x 7cm (W x L x H) Trọng lượng: 1.5kg

Tần số cập nhật: 700Hz Sai số: 0.05o

Ứng dụng: Hàng không, tên lửa

• Honeywell, Mỹ:

Mã hiệu: HG1700 – AG59 Kích thước: 9cm x 9cm x 7cm (W x L x H) Trọng lượng: 0.8kg

Tần số cập nhật: 100Hz Sai số: 0.2o

Ứng dụng: Công nghiệp, hàng không

• Gladiator Technology, Mỹ:

Mã hiệu: Landmark 30 IMU Kích thước: 8cm x 8cm x 6cm (W x L x H) Trọng lượng: 0.4kg

Tần số cập nhật: 200Hz Sai số: 0.5o

Ứng dụng: Ô tô, ổn định hướng antenna

• Systron Donner, Mỹ:

Mã hiệu: MMQ50 IMU Kích thước: 5cm x 5cm x 7cm (W x L x H) Trọng lượng: 0.2kg

Tần số cập nhật: 50Hz Sai số: 1o

Ứng dụng: Robot, hàng hải

• Xsens Technologies, Hà Lan:

Mã hiệu: MTx Kích thước: 4cm x 5cm x 2cm (W x L x H) Trọng lượng: 0.03kg

Tần số cập nhật: 120Hz Sai số: 2o

Ứng dụng: Công nghiệp, mô phỏng

Mặc dù độ chính xác và kích thước của các IMU là khác nhau nhưng nguyên lý

và cấu trúc cơ bản để hình thành nên một IMU là giống nhau Bên trong IMU có 9 cảm biến bao gồm 3 cảm biến vận tốc góc (gyroscope), 3 cảm biến gia tốc (accelerometer), và 3 cảm biến từ trường (magnetometer) kết hợp lại với nhau trên

1 trục tọa độ 3 chiều trực giao như mô tả ở Hình 2 Hệ thống cảm biến này gọi chung là MARG (Magnetic, Angular rate, and Gravity) Đây là các cảm biến hệ vi

cơ điện tử (MEMS) có xu hướng phát triển mạnh trong vòng 10 năm trở lại

Hình 2 Cấu trúc tổng quát của một IMU

Thành phần trung tâm của IMU là một con chip vi điều khiển được lập trình để đọc 9 cảm biến và sử dụng các thuật toán ước lượng trạng thái phi tuyến tối ưu để ước lượng góc quay dựa theo 9 giá trị đo này Công việc trọng tâm ở đây là kết hợp

dữ liệu từ các cảm biến MEMS theo mô hình toán chính xác (được xây dựng nên bởi từng nhà sản xuât) để có được giá trị uớc lượng góc quay của vật thể tốt hơn là chỉ sử dụng riêng lẻ 1 hay 2 cảm biến đơn thuần Góc nghiêng theo trục của đối tượng về lý thuyết có thể đạt được bằng cách lấy tích phân dữ liệu cảm biến vận tốc góc theo thời gian Tuy nhiên, khó khăn ở đây khi sử dụng các cảm biến MARG là luôn hiện hữu độ không chính xác trong quá trình đọc dữ liệu cảm biến,

và các tác động nhiễu không mong muốn bên ngoài như các rung động có gia tốc ngoài, nhiễu từ trường từ các vật liệu sắt từ, động cơ,… Ngoài ra, sai số tích lũy rất

dễ tăng dần theo thời gian bởi sự lượng tử hóa, khâu tích phân, và sai số cảm biến

Do đó, việc kết hợp các cảm biến lại để giám sát và bù trừ cho nhau là cần thiết

Để bù lại sai số tích lũy của hệ thống từ cảm biến vận tốc góc, cảm biến gia tốc được sử dụng Cảm biến gia tốc đo gia tốc trọng trường g, được dùng để bù sai số cho những góc so với phương thẳng đứng (trục x, y) Thành phần thứ ba của hệ thống IMU là cảm biến từ trường Cảm biến từ trường đo vùng từ trường nội của khu vực trên Trái Đất, nó được dùng để hiệu chỉnh sai số của góc trên mặt phẳng nằm ngang vuông góc với phương gG (trục z)

1.2 Tình hình nghiên cứu trong nước

Mặc dù IMU đã được ứng dụng rộng rãi trên thế giới nhưng hướng nghiên cứu

và chế tạo IMU ở trong nước còn khá mới mẻ Đó là vì để chế tạo được một IMU tốt cần phải hội đủ hai yếu tố: cảm biến tốt và thuật toán ước lượng trạng thái tối

ưu trong không gian Cảm biến tốt chúng ta có thể mua được, nhưng để ước lượng trạng thái tối ưu cần phải có kiến thức toán học về vật chuyển động trong không gian và đặc biệt phải có kinh nghiệm trong việc thiết kế các bộ ước lượng số phi tuyến tối ưu

Trước nhu cầu tiềm năng đó, trong 5 năm trở lại đây, ở Việt Nam đã có một vài nhóm nghiên cứu trong các trường đại học tiến hành nghiên cứu và chế tạo thử nghiệm IMU dùng bộ lọc tuyến tính Kalman mở rộng Điển hình trong số này là sản phẩm của PTN Trọng điểm quốc gia

• PTN Trọng điểm quốc gia, ĐH Bách Khoa TP.HCM:

Kích thước: xxx Trọng lượng: 0.05kg Tần số cập nhật: 100Hz Sai số: chưa đánh giá Ứng dụng: thử nghiệm

Tuy nhiên, hạn chế của các IMU này là cho ra kết quả ước lượng không chính xác vì thuật toán ước lượng góc quay được xây dựng theo bộ lọc tuyến tính, trong khi bản chất của chuyển động quay IMU là phi tuyến Hơn nữa, khi xây dựng bộ ước lượng, các nhóm nghiên cứu chưa đánh giá đầy đủ ảnh hưởng của từ trường ngoài tác động tới kết quả ngõ ra Điều này dẫn tới IMU cho ra kết quả ước lượng không chính xác nếu đặt thiết bị này gần các nguồn phát sinh từ trường như động

cơ, các vật liệu sắt từ,… Vì vậy khả năng ứng dụng thiết bị này vào thực tế như lĩnh vực công nghiệp, robot, máy bay mô hình còn rất giới hạn

1.3 Tính cấp thiết của đề tài

Ngày nay IMU đã được ứng dụng rộng rãi trong rất nhiều lĩnh vực và là thiết bị

không thể thiếu được trong nhiều ứng dụng như:

• Máy bay: dùng IMU để giữ thăng bằng và điều khiển máy bay

• Tên lửa: dựa vào IMU để lái tên lửa đi theo một quỹ đạo vạch trước

• Tàu thủy, tàu ngầm: dùng IMU để xác định vị trí và phương hướng của tàu

• Ô tô: kết hợp IMU với hệ thống định vị toàn cầu GPS để nhận biết tọa độ của ô tô di chuyển trên đường với độ chính xác cao

• Robot: kết hợp với camera để bám theo vật hoặc gắp vật, ứng dụng trong mobile robot, giữ thăng bằng trong các robot 2 chân dạng người

• Điện ảnh: sử dụng nhiều IMU gắn vào các bộ phận của người hoặc động vật

để lưu và mô phỏng lại chuyển động của đối tượng

Thúc đẩy bởi các yếu tố đề cập ở trên cũng như nhu cầu tiềm năng của IMU là rất lớn ở Việt Nam, chúng tôi nhận thấy việc nghiên cứu chế tạo thiết bị đo quán tính 3 trục IMU là cấp thiết Hơn nữa, khi tiếp xúc với các nhóm nghiên cứu mô hình máy bay không người lái, chúng tôi thấy họ gặp rất nhiều khó khăn trong việc triển khai vì không nắm được công nghệ IMU và giá thành IMU mua ở nước ngoài khá cao (có giá từ vài nghìn USD trở lên, tại thời điểm hiện tại một IMU sử dụng ở cấp độ công nghiệp là MTx của Xsens Technologies có giá 5.000 USD) Trong khi

đó, các kết quả nghiên cứu IMU trong nước chưa thể áp dụng vào công nghiệp hoặc máy bay mô hình vì đặc tính của động cơ máy bay phát sinh nhiễu từ trường rất lớn làm cho kết quả ước lượng không chính xác

Vì vậy, khi thiết bị IMU cùng với thuật toán ước lượng phi tuyến có khử nhiễu

từ trường của nhóm nghiên cứu chúng tôi ra đời sẽ góp phần hỗ trợ các nhóm nghiên cứu robot và máy bay không người lái do giá thành thấp và thuận lợi trong việc hợp tác tích hợp IMU vào hệ thống

1.4 Công trình nghiên cứu liên quan và các kết quả mới nhất của thế giới

Hiện nay có 3 phương pháp để biểu diễn góc quay và chuyển động tịnh tiến của một IMU trong không gian Phương pháp được sử dụng rộng rãi và tương đối đơn giản là sử dụng góc Euler theo 3 trục x, y, z Tuy nhiên, biểu diễn góc theo phương pháp góc Euler có nhược điểm là IMU không thể bám theo đối tượng trong tất cả các hướng do sự suy biến của góc xoay quanh trục y khi nó tiến tới ±90o Phương pháp ma trận xoay Direction Cosine Matrix (DCM) sẽ khắc phục được trường hợp suy biến của phương pháp góc Euler Phương pháp thứ ba cũng được sử dụng rất nhiều đó là phương pháp toán tử xoay Quaternion, phương pháp này cũng khắc phục được sự suy biến của phương pháp Euler đồng thời cải thiện được khả năng tính toán Trong ba phương pháp trên, thì hai phương pháp DCM và Quaternion được sử dụng chủ yếu, là cơ sở để thiết kế các bộ ước lượng nói chung, đặc biệt là

bộ lọc Kalman

Trong tất cả sản phẩm của nước ngoài và các bài báo quốc tế liên quan tới IMU, thuật toán ước lượng dựa trên Bộ lọc Kalman mở rộng được áp dụng rộng rãi trong việc ước lượng 7 biến trạng thái đầu vào gồm 3 thành phần vận tốc góc, 4 thành phần quaternion và 9 giá trị đo lường ngõ ra gồm 3 thành phần vận tốc góc, 3 thành phần gia tốc góc và 3 thành phần từ trường [1], [2], [3] Bộ lọc thiết kế đơn giản nhưng đòi hỏi tính toán nhiều, đặc biệt là ngõ ra phi tuyến Thuật toán Guass-Newton được kết hợp với bộ lọc Kalman (QUEST) để loại bỏ tính phi tuyến của ngõ ra của bộ lọc, giảm được khối lượng tính toán đáng kể Thuật toán cải tiến giảm bậc cho bộ lọc Kalman để giảm thời gian tính toán ma trận nghịch đảo, và một số kinh nghiệm trong việc đánh giá nhiễu quá trình và nhiễu đo lường cho bộ lọc Kalman mở rộng cũng đã được đề cập ở [4], [5] Nhóm tác giả mô phỏng offline trên Matlab để so sánh chất lượng của thuật toán QUEST với bộ lọc

Kalman mở rộng, sau đó thực hiện kiểm tra kết quả real-time với ứng dụng viết bằng Java Độ chính xác với hệ thống tĩnh là 2o và hệ thống động là 4o

Tác giả Angelo M Sabatini [8] đã xây dựng mô hình đo lường cảm biến thực trong đó xem xét toàn diện các thành phần bias và nhiễu Bộ lọc Kalman mở rộng dựa trên toán tử quaternion, ước lượng thêm 6 biến trạng thái gồm 3 thành phần bias accelerometer, 3 thành phần bias của magnetometer Bài báo đã đưa ra các biểu thức để cập nhật ma trận hiệp phương sai nhiễu quá trình Q k, đồng thời cập nhật ma trận hiệp phương sai nhiễu đo lường R k bằng các kiểm tra trước các dữ liệu đo lường accelerometer và magnetometer Tác giả đã kiểm tra kết quả dựa trên tập dữ liệu từ cảm biến InertiaCube2 (của hãng InterSense Inc.) với nhiều phương pháp khác nhau Mặc dù phương pháp này đòi hỏi phải tính toán rất nhiều, cộng với việc bù được ảnh hưởng nhiễu của từ trường môi trường xung quanh, đòi hỏi

độ chính xác của cảm biến, nhưng kết quả đạt được là rất tốt, sai số RMSE nhỏ nhất đạt được là 2.57o

Vấn đề bù nhiễu từ trường [6], [7] được nhóm tác giả thực hiện bằng bộ lọc Kalman bù, sai số vận tốc, và sai số nhiễu từ trường được ước lượng vào trong các phương trình trạng thái Kết quả đạt được với môi trường nhiễu từ trường nội rất tốt, sai số tĩnh (1.4o), sai số động là 2.6o (RMSE)

Bộ lọc Kalman dựa trên ma trận DCM cũng được thực hiện và so sánh với các

bộ lọc sử dụng phương pháp Euler và Quaternion [9], [10] Nhóm tác giả thiết kế 2

bộ lọc Kalman riêng biệt cho phương pháp DCM, một để ước lượng góc quay trục

x, y và một cho các góc quay trục z Bằng cách này, việc tính toán sẽ nhanh hơn do tránh được phép tính ma trận nghịch đảo có kích thước lớn Ngoài ra, phương pháp DCM cũng tránh được sai số khi xấp xỉ bậc nhất của bộ lọc Kalman mở rộng, và không bị ảnh hưởng bởi vector từ trường tham chiếu Việc kiểm tra, so sánh kết quả được thực hiện trên tiện ích Matlab Aerospace Blockset Nhóm tác giả cũng đã

mô phỏng hiện tượng suy biến của phương pháp góc Euler mà phương pháp DCM

và Quaternion không gặp phải

1.5 Các hướng giải quyết mới từ điều kiện thực tế ở Việt Nam

IMU đóng vai trò vô cùng quan trọng trong một hệ thống vì độ chính xác của

nó quyết định đến chất lượng của cả hệ thống Ví dụ như trong tên lửa, nếu IMU càng chính xác thì quỹ đạo của của tên lửa đi càng ổn định và chính xác, khả năng bắn trúng mục tiêu càng cao Do đó, giá thành của IMU dao động từ vài nghìn USD tới vài trăm nghìn USD tùy thuộc vào sai số của nó từ 2o đến 0.1o Thiết bị IMU trong các ứng dụng hàng không, tên lửa và không gian, yêu cầu phải có sai số rất nhỏ (< 0.1o) đòi hỏi việc chế tạo IMU rất khó khăn IMU sử dụng trong công nghiệp, ô tô và các ứng dụng khác có yêu cầu sai số từ 2o đến 1o nên việc chế tạo ít phức tạp hơn

Chất lượng của một IMU phụ thuộc chính vào 3 yếu tố Yếu tố thứ nhất là độ chính xác của 9 cảm biến gồm 3 cảm biến vận tốc góc (gyroscope), 3 cảm biến gia tốc (accelerometer), và 3 cảm biến từ trường (magnetometer) Nếu cảm biến có chất lượng tốt, đồng nghĩa với sai số đo đạc nhỏ, thì việc xác định góc nghiêng sẽ

chính xác hơn Yếu tố thứ hai là thuật toán ước lượng góc quay dựa vào 9 giá trị cảm biến này Thuật toán ước lượng thể hiện trình độ công nghệ và thương hiệu của nhà sản xuất Một thuật toán ước lượng tốt có khả năng phát hiện được các nhiễu nền (bias) của vận tốc, gia tốc và từ trường thông qua mô hình chúng bằng các phương trình biến trạng thái Các nhiễu nền này cũng được xem như là các biến trạng thái trong qua trình ước lượng Yếu tố thứ ba là quá trình cân chỉnh IMU sao cho thuật toán ước lượng góc quay không bị ảnh hưởng khi nhiệt độ môi trường thay đổi, khi có gia tốc, từ trường ngoài tác động và không bị trôi theo thời gian Xuất phát từ nhu cầu thực tế ở Việt Nam khi chỉ có các ứng dụng công nghiệp phát triển mạnh, còn các ứng dụng cho tên lửa và hàng không hầu như chưa có, nên chúng tôi tập trung vào phân khúc sản phẩm áp dụng cho công nghiệp Với yêu cầu sai số cho phép từ 1o đến 2o thì giá thành thiết kế chế tạo sẽ rẻ và linh kiện cũng dễ tìm hơn Sản phẩm sau khi hoàn thiện có thế áp dụng được trong lĩnh vực công nghiệp, robot, lĩnh vực ô tô và máy bay mô hình

Với yêu cầu sản phẩm đề ra như trên, nhóm nghiên cứu chọn các tiếp cận vấn

đề là chế tạo ra IMU có chất lượng tương đương IMU mã hiệu MTx của hãng Xsens Technologies, Hà Lan Chúng tôi xem MTx như là 1 sản phẩm chuẩn để sản phẩm IMU của nhóm cần phải đạt tới Sở dĩ chúng tôi chọn MTx vì nó là sản phẩm tương đối chuẩn mực và được sử dụng rộng rãi trong công nghiệp Ở thời điểm hiện tại, giá thành của MTx vào khoảng 3.000 USD, là tương đối đắt so với nhu cầu và thị trường sử dụng ở Việt Nam Do đó, chúng tôi lên phương án sử dụng các cảm biến thay thế để khi tích hợp thành sản phẩm IMU đóng gói hoàn chỉnh có giá khoảng 20 triệu đồng Hay nói cách khác, mục tiêu của nhóm là nghiên cứu chế tạo

ra IMU sử dụng trong công nghiệp có chất lượng tương đương với thiết bị nước

ngoài trong khi giá thành chỉ bằng 1/3 so với thiết bị ngoại nhập

Để đạt được mục tiêu này, chúng tôi đề ra phương hướng giải quyết như sau:

• Sử dụng cảm biến có chất lượng khá với giá thành hợp lý để chất lượng

đo đạc ổn định và tin cậy Không sử dụng cảm biến rẻ tiền do chất lượng không được đảm bảo và cũng không lựa chọn cảm biến quá tốt vì giá thành sẽ rất cao

• Tập trung nghiên cứu sâu vào thuật toán ước lượng để đạt được kết quả tốt nhất, thuật toán ước lượng có khả năng phát hiện và bù được các nhiễu nền (bias) của vận tốc, gia tốc, và từ trường Nhóm nghiên cứu sẽ xây dựng phương trình mô tả trạng thái và ước lượng trạng thái chuyển động của IMU trên cơ sở bộ lọc Kalman mở rộng (Extended Kalman filter) và bộ lọc phi tuyến (Unscented Kalman filter) để nâng cao độ chính xác của giá trị ước lượng Thuật toán cập nhật trạng thái sẽ được phát triển theo các phương pháp ma trận DCM và toán tử quaternion để cải thiện tốc độ của thuật toán cũng như loại bỏ được trường hợp suy biến của góc quay Euler Khi sử dụng bộ lọc phi tuyến, một vấn đề phát sinh là thời gian xử lý tăng lên rất nhiều lần so với bộ lọc Kalman mở rộng Do đó đòi hỏi phải có một vi xử lý đủ mạnh để đảm bảo tốc độ cập nhật 100Hz (tương ứng tốc độ tính toán của thuật toán phải nhỏ hơn 10ms) Vì vậy, một vi xử lý có hỗ trợ phép tính dấu phẩy động là lựa

chọn cần thiết để đảm bảo tần số cập nhật ngõ ra 100Hz theo mục tiêu đề tài đặt ra

• Xây dựng mô hình cân chỉnh IMU chính xác để có giải pháp triệt tiêu ảnh hưởng của các yếu tố bên ngoài tác động tới chất lượng IMU như nhiệt độ môi trường; gia tốc, từ trường ngoài tác động; và khả năng sai số tích lũy theo thời gian Chúng tôi đặc biệt chú ý tới khả năng khử nhiễu

từ trường ngoài để sản phẩm IMU áp dụng được trong các môi trường có

sử dụng động cơ như trong công nghiệp, ô tô, máy bay mô hình

Chương 2

NỘI DUNG VÀ PHƯƠNG PHÁP

Đề giải quyết các vấn đề nêu ra ở Chương 1 cũng như yêu cầu ban đầu của mục tiêu đề tài, nhóm nghiên cứu tập trung giải quyết 3 nội dung chính sau:

2.1 Nội dung 1: Thiết kế và thi công phần cứng IMU

a Phương pháp nghiên cứu và kỹ thuật sử dụng

Hình 3 Sơ đồ kết nối phần cứng trong thiết bị IMU

Thiết bị IMU bao gồm 3 cảm biến vận tốc góc + 3 cảm biến gia tốc + 3 cảm biến từ trường được kết nối tới khối Vi xử lý trung tâm như ở Hình 3 Các cảm biến cùng loại đặt vuông góc với nhau theo 3 trục x, y, và z để tạo thành một hệ tọa

độ trực giao Vi xử lý đọc 9 giá trị cảm biến này thông qua bộ biến đổi AD 16 bit

có độ chính xác cao và tốc độ biến đổi nhanh Thuật toán ước lượng cài đặt trên Vi

xử lý sẽ ước lượng gia tốc tịnh tiến và góc quay của đối tượng chuyển động dựa trên 9 giá trị đo này Một cảm biến nhiệt độ sẽ được lắp đặt kèm theo để thuật toán ước lượng có khả năng bù trừ các nhiễu nền từ các cảm biến Cuối cùng các thông

số này được gửi đi thông qua cổng RS232 tới máy tính hoặc tới các phần cứng ứng dụng khác

Vi xử lý là thành phần trung tâm của phần cứng IMU, yêu cầu phải có tốc độ xử

lý nhanh để thực hiện được các phép tính ma trận nghịch đảo kích thước lớn của bộ ước lượng phi tuyến Với tốc độ phát triển công nghệ trên thế giới, hiện nay có nhiều họ vi xử lý thực hiện được công việc tính toán phức tạp này Tuy nhiên, họ

Vi xử lý DSP (digital signal processor), cụ thể là dòng sản phẩm TMS320F28335, của hãng Texas Instruments là nổi trội hơn cả TMS320F28335 hoạt động ở xung nhịp 150MHz, có khả năng xử lý phép tính dấu chấm động chỉ trong 1 chu kỳ máy,

và khả năng giao tiếp ngoại vi mạnh Do đó, Nhóm nghiên cứu quyết định thiết kế

bo mạch phần cứng IMU dựa trên chip TMS320F28335 để tận dụng sức mạnh tính toán trong việc xây dựng các bộ ước lượng góc quay phi tuyến phức tạp

b Phương án thực hiện:

Trong nội dung này, nhóm nghiên cứu đã thực hiện các công việc sau:

• Thiết kế bo mạch phần cứng IMU

+ Thiết kế khối Vi xử lý trung tâm TMS320F28335

+ Thiết kế khối giao tiếp Vi xử lý với 3 cảm biến vận tốc góc

+ Thiết kế khối giao tiếp Vi xử lý với 3 cảm biến gia tốc

+ Thiết kế khối giao tiếp Vi xử lý với 3 cảm biến từ trường

+ Thiết kế khối giao tiếp Vi xử lý với máy tính

• Gia công bo mạch phần cứng IMU

+ Xử lý tập tin gia công bo mạch phần cứng IMU

+ Kiểm tra chất lượng bo mạch phần cứng IMU

+ Hàn linh kiện và kiểm tra thử nghiệm IMU

• Lập trình giao tiếp cảm biến và truyền thông nối tiếp cho IMU

+ Lập trình giao tiếp phần cứng TMS320F28335 với các cảm biến

+ Lập trình giao tiếp phần cứng TMS320F28335 với máy tính thông qua chuẩn RS232

2.2 Nội dung 2: Xây dựng bộ ước lượng tối ưu

a Phương pháp nghiên cứu và kỹ thuật sử dụng

Hình 4 Cấu trúc bộ ước lượng tối ưu dùng bộ lọc Kalman mở rộng

Thuật toán ước lượng tối ưu tính góc quay φ θ ψ, , xung quanh trục x, y, z được trình bày như ở Hình 4 Giá trị vận tốc góc kèm thông tin nhiễu của 3 trục được đưa trực tiếp vào bộ lọc Kalman mở rộng nhằm giảm thời gian tính toán Bộ lọc

xây dựng trên cơ sở các phép toán quaternion (hoặc phép biến đổi DCM) để tránh các trường hợp suy biến của ma trận chuyển đổi Giá trị gia tốc và từ trường của 3 trục sau khi chuyển đổi sang quaternion (hoặc DCM) được đưa vào bộ lọc Kalman như các đại lượng đo Dựa vào 6 đại lượng đo này kèm thông tin nhiễu đo xác định

từ mô hình thực tế, bộ lọc phi tuyến thực hiện thuật toán ước lượng cập nhật trạng thái ngõ ra Giá trị quaternion (hoặc DCM) ˆq của 3 góc sau khi qua ma trận chuyển đổi sẽ cho ra 3 giá trị góc quay Euler ˆ ˆ ˆφ θ ψ, , để thuận tiện cho các ứng dụng khác cần sử dụng thiết bị này

Để xây dựng bộ ước lượng góc nghiêng cho IMU sử dụng bộ lọc Kalman, ba bước cần thiết đầu tiên phải tiến hành là:

B1: Chọn các biến trạng thái phù hợp với chuyển động của hệ thống như góc quay, gia tốc tịnh tiến, nhiễu nền vận tốc, gia tốc, từ trường Các biến trạng thái sẽ biểu diễn đặc tính động của hệ thống và được ước lượng bởi bộ lọc Kalman mở rộng hoặc bộ lọc phi tuyến

B2: Chọn tín hiệu ngõ vào và tín hiệu đo đạc cho hệ thống Cụ thể như ở Hình

4, ta chọn tín hiệu ngõ vào là 3 giá trị vận tốc góc được lấy từ 3 cảm biến vận tốc góc, và 6 tín hiệu đo đạc từ 3 cảm biến gia tốc + 3 cảm biến từ trường

B3: Xác định giá trị nhiễu tác động vào mô hình mô tả biến trạng thái Trong trường hợp này ta phải xác định giá trị nhiễu bằng cách tính các ma trận hiệp phương sai của các sai số phép đo vận tốc góc, gia tốc và từ trường Các giá trị này sẽ được xác định và tính toán từ thực nghiệm

*Bước 1: Hệ thống tham chiếu góc nghiêng theo mô hình DCM (Direction

Cosine Matrix)

¾ Góc roll và pitch quay quanh trục x, y

Gọi E

hệ tọa độ {E} (là hệ tọa độ mặt đất) Ma trận chuyển đổi từ hệ tọa độ {B} đến hệ tọa độ {E} là:

0 sin

0 cos sin

cos cos

B x

B z

z

a a

φφ

b x

¾ Góc yaw quay quanh trục z

Từ trường của Trái Đất đặc trưng bởi vector vector từ trường gồm hướng và cường độ của nó tại một vùng nào đó trên Trái Đất Cường độ từ trong mặt phẳng nằm ngang, là tổng của 2 thành phần vector X và Y Độ lệch của hướng bắc cực từ

so với hướng bắc địa lý được gọi là độ lệch từ hay góc lệch từ (declination), ký

hiệu là D Góc giữa cường độ từ trong mặt phẳng ngang H với tổng cường độ từ F, gọi là góc nghiêng hay góc từ khuynh (inclination), ký hiệu là I Giá trị của góc lệch từ D, và góc nghiêng từ khuynh I tại một vùng nào đó trên Trái Đất có thể xác

định bởi mô hình WMM 2010 (World Magnetic Model 2010) khi ta biết vĩ độ (latitude) và kinh độ (longtitude) của nó Khi tham khảo mô hình WMM, ta thấy từ trường không chỉ thay đổi theo vị trí mà còn thay đổi theo thời gian qua từng năm

D

ψ ψ Δ

B h

X

Hình 5 Hướng lệch của tù trường Hình 6 Biểu diễn góc lệch của từ

trái đất trường so với phương Bắc của trái đất

Chúng ta sẽ xác định hướng lệch trong mặt phẳng ngang bằng dữ liệu từ cảm biến từ trường và các góc nghiêng roll và pitch

Gọi hệ tọa độ từ trường là {M} Hệ tọa độ {M} xác định bằng cách xoay hệ tọa

độ {E} quanh trục Z một góc là D Góc lệch giữa B

sẽ bằng:

D

Ma trận DCM chuyển từ hệ tọa độ {B} sang hệ tọa độ {M} được xác định:

Khai triển các ma trận xoay theo (1), ta được

cos cos sin sin sin cos

sin cos sin cos cos

cos sin sin sin cos