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
TP. HỒ CHÍ MINH TP. HỒ CHÍ MINH



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

I
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 2
o
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ừ 1
o

đến 2
o
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 2
o
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 1
o
to 2
o
. 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.







II
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

III
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

















IV
DANH SÁCH BẢNG
SỐ TÊN BẢNG SỐ LIỆU TRANG
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
2 Thông số kỹ thuật của IMU 9 bậc 26

V
DANH SÁCH HÌNH
SỐ TÊN HÌNH ẢNH TRANG
1 Một số sản phẩm IMU 1
2 Cấu trúc tổng quát của một IMU 2
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
5 Hướng lệch của tù trường trái đất 12
6 Biểu diễn góc lệch của từ trường so với phương Bắc 12
7 Sơ đồ khối bộ ước lượng Kalman DCM 15
8 Sản phẩm phần cứng IMU 18
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
14 Mô hình cân chỉnh IMU 23
15 Bo mạch giao tiếp PC104CARD 23
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
19 Mô hình tích hợp INS/GPS 27
20 Quĩ đạo thực nghiệm mô hình tích hợp INS/ GPS 27











VI
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…)

TT Nội dung Kinh phí Trong đó
Ngân
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

1
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 2
o

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
Xây dựng bộ ước lượng tối ưu Hoàn thành
Cân chỉnh và đánh giá chất lượng Hoàn thành


Sản phẩm của đề tài:
Tên sản
phẩm
Thông số kỹ thuật đề tài
đặt ra
Thông số kỹ thuật hoàn
thành
IMU 9
bậc
- Kích thước: 10 x 6 x 6cm
(LxWxH)
- Trọng lượng: <0.1kg
- Tần số cập nhật: 100Hz
- Độ phân giải góc quay ngõ
ra 0.1
o

- Sai số tĩnh 1
o

(RMS

- Sai số động 2
o
(RMS)
- Kích thước: 8 x 6 x 4cm
(LxWxH)
- Trọng lượng: 90g
- Tần số cập nhật: 100Hz
- Độ phân giải góc quay ngõ ra
0.1
o

- Sai số tĩnh 1
o
(RMS)

- Sai số động 2
o
(RMS)






2
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.05
o

Ứ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.2
o

Ứ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.5
o

Ứ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ố: 1
o

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


3
• 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ố: 2
o

Ứ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
g
G
(trục z).

4
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:

Mã hiệu: IMU-01
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.

5
• Đ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 ±90
o

. 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

6
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à 2
o
và hệ thống động là 4
o

.
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
k
Q , đồng thời cập
nhật ma trận hiệp phương sai nhiễu đo lường
k
R
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.57
o
.
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.4
o
), sai số động là 2.6
o
(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ừ 2
o
đến 0.1
o
. 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.1
o
) đò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ừ 2
o
đến 1
o

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ẽ

7
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ừ 1
o
đến 2
o
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

8
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.

9
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ế

10
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

11
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
B
R
là ma trận DCM từ hệ tọa độ {B} (là hệ tọa độ gắn vào đối tượng) đến
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à:

E
BZYX
RRRR
ψ
θ
φ
=

os os sin sin os os sin sin sin os os sin
sin os sin sin sin os os os sin sin os sin
sin os sin os os

E
B
cc c c cc
Rc ccc c
ccc
ψ
θ φθ ψ φψ φψ φ ψ θ
ψ
θφθψφψφθψψφ
θθφ φθ
−+
⎡⎤
⎢⎥
=+−
⎢⎥
⎢⎥
−
⎣⎦
(1)
Vector gia tốc đo lường trong hệ tọa độ {B}, []
B
BBBT
xyz
aaaa=
G


Vector gia tốc trọng trường g
G
trong hệ tọa độ {E}, [00g]

E
T
g =
G
, với
2
9.81m/sg =
Ta có:
BETE
B
aRg=
GG


12
0sin
0 cos sin
cos cos
B
x
BET
yB
B
z
a
aR g
ag
θ
θ
φ

θ
φ
⎡⎤
−
⎡⎤ ⎡ ⎤
⎢⎥
⎢⎥ ⎢ ⎥
==
⎢⎥
⎢⎥ ⎢ ⎥
⎢⎥
⎢⎥ ⎢ ⎥
⎣⎦ ⎣ ⎦
⎣⎦
(2)
Giải hệ phương trình trên ta được:

22
sin
1
cos
b
y
b
bb
z
yz
a
a
aa

φ
φ
⎡
⎤
⎡⎤
=
⎢
⎥
⎢⎥
+
⎣⎦
⎣
⎦
(3)

sin
1
sin cos
cos
b
x
bb
yz
a
aa
g
θ
φ
φ
θ

⎡⎤
−
⎡⎤
=
⎢⎥
⎢⎥
+
⎢⎥
⎣⎦
⎣⎦
(4)

¾ 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
I
F
H
Z
X

Y
North
East
Magnetic
North

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
h
X và hướng Magnetic North là
ψ
Δ , góc lệch giữa
B
h
X và hướng Bắc
E

X (North) là
ψ
, trong đó
B
h
X là hình
chiếu vuông góc của hướng
B
X
của lên mặt phẳng nằm ngang. Lúc này, góc yaw
sẽ bằng:
D
ψ
ψ
=Δ + (5)

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

M
BZYX
RR RR
ψ
θφ
Δ
=
Vector từ trường trong hệ tọa độ {M} là
M
m
G

có dạng:

[0]
M
T
mH Z=
G

Vector từ trường đo được từ cảm biến trong hệ tọa độ {B} là

[]
BBBBT
xyz
mmmm=
G

Mối quan hệ giữa vector
B
m
G
và
M
m
G
xác định bởi ma trận DCM
M
B
R
:
.( )

MMB B
BZYX
mRmRRRm
ψθφ
Δ
==
GG G
(6)
1
(). . (.)
MMB B
ZBYX
RmRmRRm
ψθφ
Δ−
⇒==
GG G


Khai triển các ma trận xoay theo (1), ta được
cos cos sin sin sin cos
sin cos sin
sin cos sin cos cos
BB B
xy z
BB
yz
BB B
xx z
Hmmm

Hmm
Zmm m
ψ
θθφθφ
ψφφ
θ
θφ θφ
⎡⎤
Δ++
⎡⎤
⎢⎥
⎢⎥
−Δ= −
⎢⎥
⎢⎥
⎢⎥
⎢⎥
−+ +
⎣⎦
⎣⎦
(7)
Từ 2 thành phần đầu tiên của biểu thức (7) ở trên, ta có thể suy ra
cos sin
tan
cos sin sin sin cos
BB
yz
BB B
xy z
mm

mm m
φφ
ψ
θ
θφ θφ
−+
Δ=
++
(8)
Từ biểu thức (8) ở trên kết hợp với các biểu thức (3), (4) và (5) ta sẽ có được
trạng thái của góc trục z (góc yaw) hoàn toàn.

*Bước 2: Xác định mô hình đối tượng và mô hình đo lường cho bộ lọc Kalman
theo phương pháp DCM
Ước lượng trạng thái góc nghiêng liên quan đến việc xác định góc roll và góc
pitch bằng các sử dụng cảm biến vận tốc góc và gia tốc. Dữ liệu từ cảm biến vận
tốc góc xem như mô hình đối tượng, dữ liệu từ cảm biến gia tốc và từ trường là mô
hình đo lường.
Phương trình vi phân ma trận DCM:
() ().[ ]
EE
BB
Rt Rt
ω
=

(9)
trong đó,
[]
BBBBT

xyy
ωωωω
=
G
là vận tốc góc đo từ cảm biến gyros trong hệ tọa
độ {B}, và
0
[] 0
0
BB
zy
BB
zx
BB
yx
ω
ω
ω
ωω
ωω
⎡⎤
−
⎢⎥
=−
⎢⎥
⎢⎥
⎣⎦
(10)
Hay biểu thức (9) có thể được được viết lại

14

11 12 13 11 12 13
21 22 23 21 22 23
31 31 31 31 31 31
0
0
0
EEE EEE BB
BBB BBB z y
EEE EEE B B
BBB BBB z x
EEE EEE BB
BBB BBB y x
RRR RRR
RRR RRR
RRR RRR
ω
ω
ω
ω
ωω
⎡⎤⎡⎤⎡ ⎤
−
⎢⎥⎢⎥⎢ ⎥
=−
⎢⎥⎢⎥⎢ ⎥
⎢⎥⎢⎥⎢ ⎥
−
⎣⎦⎣⎦⎣ ⎦




(11)
Nếu ta chọn cả ma trận DCM làm biến trạng thái thì tới có 9 thành phần cần
phải được cập nhật. Ở đây, do ma trận DCM có tính chất trực giao và trực chuẩn
nên ta chỉ cần cập nhật 6 thành phần của nó, cụ thể ở đây ta chọn 2 hàng cuối của
ma trận DCM để cập nhật:
31 31
32 32
33 33
0
0
0
EBBE
BzyB
EB BE
Bz xB
EBB E
ByxB
RR
RR
RR
ωω
ωω
ωω
⎡⎤⎡ ⎤⎡⎤
⎢⎥⎢ ⎥⎢⎥
=−
⎢⎥⎢ ⎥⎢⎥

⎢⎥⎢ ⎥⎢⎥
−
⎣⎦⎣ ⎦⎣⎦



(12)

21 21
22 22
23 23
0
0
0
EBBE
BzyB
EB BE
Bz xB
EBB E
ByxB
RR
RR
RR
ωω
ωω
ωω
⎡⎤⎡ ⎤⎡⎤
⎢⎥⎢ ⎥⎢⎥
=−
⎢⎥⎢ ⎥⎢⎥

⎢⎥⎢ ⎥⎢⎥
−
⎣⎦⎣ ⎦⎣⎦



(13)
Mô hình đo lường thứ nhất sử dụng vector gia tốc accelerometer được xây dựng
như sau:
31
32
33
0
0
BE
xB
BET E
yB B
BE
zB
aR
aR Rg
agR
⎡⎤ ⎡ ⎤
⎡⎤
⎢⎥ ⎢ ⎥
⎢⎥
==
⎢⎥ ⎢ ⎥
⎢⎥

⎢⎥ ⎢ ⎥
⎢⎥
⎣⎦
⎣⎦ ⎣ ⎦
,
trong đó, , ,
BBB
x
yz
aaa là 3 thành phần của vector đo lường gia tốc accelerometer
trong hệ tọa độ {B}, g là gia tốc trọng trường.
Ba biến trạng thái:
21
22
23
sin os
sin sin sin os os
os sin sin os sin
E
B
E
B
E
B
Rc
Rcc
Rc c
ψθ
φ
θψ φψ

φ
θψ ψφ
⎡⎤
⎡⎤
⎢⎥
⎢⎥
=+
⎢⎥
⎢⎥
⎢⎥
⎢⎥
−
⎣⎦
⎣⎦
, (14)
được sử dụng làm mô hình đo lường thứ hai sử dụng vector từ trường và vector gia
tốc trong biểu thức (3), (4), và (8).

* Bước 3: Xây dựng bộ lọc Kalman rời rạc cho IMU
Sử dụng các phương trình trạng thái ở biểu thức (1) đến (10), bộ ước lượng góc
quay dựa trên phương pháp DCM được thực hiện thông qua 2 khối Kalman 1 và
Kalman 2 như ở Hình 7, khối thứ nhất ước lượng trạng thái góc 2 góc quay trục x,
y (roll, pitch) và khối thứ 2 ước lượng góc quay trục z (yaw). Bằng cách này, khối
lượng tính toán sẽ nhỏ hơn là ước lượng cùng lúc cả 3 góc quay.

15
m
ω
G
Vận tốc góc

Kalman filter 1
Kalman filter 2
Gia tốc
Từ
trường
3
ˆ
E
Bx
R
m
m
G
2
ˆ
E
Bx
R
Normalization
2
3
ˆ
ˆ
E
B
x
E
B
x
R

R
⎡⎤
⎢⎥
⎢⎥
⎣⎦
Compass
heading
m
a
G
m
ψ
ˆ
E
B
R

Hình 7.
Sơ đồ khối bộ ước lượng Kalman DCM

Mô hình đối tượng và mô hình đo lường của bộ lọc Kalman thứ 1:
11 11
1
1111
1111
(,)
kk kk
kkkk
x
Txw

zHxv
ω
+
++++
=Φ Δ +
=+
(15)
trong đó
1
31 32 33
T
EEE
kB B B
xRRR
⎡⎤
=
⎣⎦
là vector biến trạng thái 3 1×
1
T
BBB
kxyz
zaaa
⎡⎤
=
⎣⎦
là vector đo lường 3 1
×
,
[

]
1
k
ω
Φ= ⊗,
1
3k
HgI= , (16)

với
[] []
1
.1 .
1
BB
zy
BB
zx
BB
yx
TT
IT T
TT
ωω
ωωω ω
ωω
⎡⎤
−Δ Δ
⎢⎥
⊗= + = Δ − Δ

⎢⎥
⎢⎥
−Δ Δ
⎣⎦
, (17)
và TΔ là thời gian lấy mẫu.
Giá trị nhiễu covariance của mô hình đối tượng và mô hình đo lường
11
,
kk
wv,
được cho bởi
{
}
111
13
T
kkk
QEww I
α
==, (18)
{
}
111
3
T
kkka
REvv rI==, (19)
Tương tự, đối với bộ lọc Kalman thứ 2 cũng có dạng giống bộ lọc Kalman 1,
với các biến trạng thái và đo lường lúc này sẽ là :

2
21 22 23
T
EEE
kB B B
xRRR
⎡
⎤
=
⎣
⎦
là vector biến trạng thái 3 1×

16
2
sin os
sin sin sin os os
os sin sin os sin
k
c
zcc
cc
ψθ
φ
θψ φψ
φ
θψ ψφ
⎡
⎤

⎢
⎥
=+
⎢
⎥
⎢
⎥
−
⎣
⎦
là vector đo lường 3 1× ,
Ma trận chuyển trạng thái và ma trận độ nhạy đo lường được cho bởi

[
]
2
k
ω
Φ= ⊗,
2
3k
HI
=
, (20)
và nhiễu covariance của
22
,
kk
wv lần lượt là
{

}
222
23
T
kkk
QEww I
α
==, (21)
{
}
222
3
T
kkk
REvv I
μ
==, (22)
Giá trị
12
,
α
α
được chọn để duy trì độ phẳng mượt của đáp ứng ngõ ra; ,
a
r
μ
sẽ
được chọn tùy theo mức độ ảnh hưởng nhiễu của môi trường. Sau khi tính toán
được giá trị cập nhật sau quan sát, do các biến trạng thái được ước lượng là 2 hàng
của ma trận DCM, nên ta cần chuẩn hóa các biến trạng thái này để thỏa mãn tính

chất trực chuẩn.

b. Phương án thực hiện:
Dựa trên các phương pháp xây dựng bộ ước lượng góc quay ở mục 2.2.a, nhóm
nghiên cứu đã thực hiện các công việc sau:
• Thiết kế bộ ước lượng góc quay dựa trên bộ lọc Kalman mở rộng
+
Xây dựng phương trình cập nhật trạng thái từ (15) – (22) theo bộ lọc
Kalman mở rộng
+
Thu thập các mẫu dữ liệu về chuyển động từ phần cứng IMU
• Mô phỏng và kiểm ra bộ lọc Kalman mở rộng
+
Xây dựng bộ ước lượng và mô phỏng trên Matlab với các tập mẫu dữ liệu
thu thập
+
Kiểm tra kết quả ngõ ra bộ ước lượng và hiệu chỉnh các thông số của bộ
lọc
• Hiện thực bộ ước lượng Kalman mở rộng lên phần cứng IMU
+
Lập trình thời gian thực cho bộ ước lượng Kalman mở rộng với tần số
cập nhật 100Hz
+
Kiểm tra kết quả ngõ ra bộ ước lượng và hiệu chỉnh lại thông số bộ lọc
• Xây dựng chương trình phần mềm hiển thị kết quả góc quay lên máy tính

2.3. Nội dung 3: Cân chỉnh và đánh giá chất lượng IMU
a. Phương pháp nghiên cứu và kỹ thuật sử dụng
Sau khi bộ ước lượng ở Nội dung 2 đã được xây dựng hoàn chỉnh, cần thiết
phải có một mô hình cơ khí xoay 3 trục để cân chỉnh sai số ước lượng.

17
Hơn nữa, thông số thường xuyên ảnh hưởng tới nhiễu nền của cảm biến vận tốc
góc và gia tốc là nhiệt độ. Ngoài ra, chúng ta cần phải đánh giá và bù sai số cho
các loại nhiễu như: sai số tích lũy từ việc tích phân giá trị vận tốc góc, ảnh hưởng
của gia tốc ngoài lên cảm biến gia tốc, nhiễu từ trường.

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ế hệ thống cơ khí cân chỉnh IMU
+
Thiết kế mô hình cơ khí 3 trục xoay có gắn encoder
• Gia công hệ thống cơ khí cân chỉnh IMU
+
Gia công mô hình cơ khí 3 trục xoay
+
Lắp đặt encoder lên mô hình cơ khí
+
Gia công bo mạch đọc encoder và giao tiếp với máy tính
• Thiết kế bo mạch đọc encoder
+
Thiết kế khối đọc encoder 24 bit
+
Thiết kế khối giao tiếp bo mạch với máy tính
• Xây dựng chương trình phần mềm cân chỉnh và đánh giá chất lượng IMU
+
Viết chương trình đồng bộ mô hình cân chỉnh với thiết bị IMU, đánh giá
chất lượng

18
Chương 3
KẾT QUẢ VÀ THẢO LUẬN

3.1. Nội dung 1: Thiết kế và thi công phần cứng IMU
a. Kết quả

Như đã đề cập ở mục 1.5, để đảm bảo chất lượng của IMU thì phần cứng của
IMU mà cụ thể là 3 loại cảm biến vận tốc góc, gia tốc và từ trường phải có sai số

nhỏ và độ phân giải cao. Tuy nhiên, để phù hợp với nhu cầu thực tế ở Việt Nam,
yêu cầu giá thành của IMU phải không quá đắt. Do đó, Nhóm nghiên cứu đã lựa
chọn cảm bi
ến như sau:
• Cảm biến vận tốc góc và gia tốc: từ hãng Analog Devices, mã hiệu
ADIS16354. Cảm biến này có một số tính năng nổi trội so với các hãng khác
như: tầm đo vận tốc góc lớn (300
0
/s), tầm đo gia tốc 1.8g, tần số cập nhật
lớn (912Hz), độ phân giải cao (14bit), độ trôi theo nhiệt độ thấp, độ trực
giao của 3 trục lớn.
• Cảm biến từ trường: từ hãng Honeywell, mã hiệu HMC5843. Cảm biến này
có một số tính năng nổi trội như: tầm đo từ trường 4gauss, tần số cập nhật
50Hz, độ phân giải 12bit, trọng lượng và kích thước nhỏ.
• Vi xử lý trung tâm: từ hãng Texas Instruments, mã hiệu TMS320F28335.
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. Vi xử lý giao
tiếp với ADIS16354 qua chuẩn SPI và giao ti
ếp với HMC5843 qua chuẩn
I2C.

Hình 8.
Sản phẩm phần cứng IMU

Kết quả phần cứng IMU như ở Hình 8, sản phẩm có kích thước 8 x 6 x 4cm
(LxWxH) và trọng lượng 90g. Mô tả chi tiết và sơ đồ mạch điện phần cứng IMU
được trình bày cụ thể ở phần Phụ lục.