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

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

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


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

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

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

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

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

. 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

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

đề 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


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

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
,,
φ
θψ

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
T
g =
G
, với
2
9.81m/sg =
Ta có:
BETE
B
aRg=
GG12
0sin
0 cos sin
cos cos
B
x
BET
yB
B
z
a
aR g
ag
θ
θ
φ

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

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

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

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
:
.( )

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

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





(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
ωω
ωω
ωω
⎡⎤⎡ ⎤⎡⎤
⎢⎥⎢ ⎥⎢⎥
=−
⎢⎥⎢ ⎥⎢⎥

⎢⎥⎢ ⎥⎢⎥
−
⎣⎦⎣ ⎦⎣⎦



(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
⎡⎤ ⎡ ⎤
⎡⎤
⎢⎥ ⎢ ⎥
⎢⎥
==
⎢⎥ ⎢ ⎥
⎢⎥

φ
θψ ψφ
⎡⎤
⎡⎤
⎢⎥
⎢⎥
=+
⎢⎥
⎢⎥
⎢⎥
⎢⎥
−
⎣⎦
⎣⎦
, (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

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

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

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


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
ψθ
φ
θψ φψ
φ
θψ ψφ
⎡
⎤

}
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


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