Ứng dụng Quaternion và bộ lọc Kalman trong việc ước lượng hướng chuyển động của vật thể bay

Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số 48, 04 - 2017 35
ỨNG DỤNG QUATERNION VÀ BỘ LỌC KALMAN TRONG VIỆC 
ƯỚC LƯỢNG HƯỚNG CHUYỂN ĐỘNG CỦA VẬT THỂ BAY 
Nguyễn Đăng Tiến* 
Tóm tắt: Trong bài báo này, chúng tôi trình bày một phương pháp mới để ước 
lượng hướng của vật thể dựa trên cảm biến IMU và cảm biến từ trường. Cảm biến 
IMU giúp chúng ta ước lượng tư thế, trong khi đó, cảm biến từ trường ước lượng 
góc quay còn lại của vật thể. Bộ lọc Kalman được sử dụng để kết hợp các cảm biến 
cũng như làm giảm tác động của nhiễu loạn trong quá trình đo lên kết quả. Thêm 
vào đó, phương pháp lọc dựa trên Quaternion đã được trình bày để khắc phục một 
số điểm kỳ dị ở phương pháp Euler. Cuối cùng, kết quả mô phỏng đã chứng minh sự 
hiệu quả của phương pháp đề xuất. 
Từ khóa: Cảm biến IMU, Cảm biến từ trường, Bộ lọc Kalman, Quaternion. 
1. ĐẶT VẤN ĐỀ 
Lĩnh vực công nghệ quân sự trên thế giới hiện nay đã có những bước phát triển 
vượt bậc với những công nghệ vô cùng hiện đại. Để thu hẹp khoảng cách phát triên 
với các cường quốc công nghệ quân sự trên thể giới thì việc tập trung nghiên cứu 
vào các thiết bị quân sự tự động, có khả năng bảo mật cũng như có độ chính xác 
cao là một yêu cầu bức thiết. Vậy làm như thế nào để một thiết bị (như tên lửa) có 
thể di chuyển đến được mục tiêu với sai số nhỏ nhất? Câu trả lời là thiết bị dẫn 
đường trên tên lửa đạt độ chuẩn xác cao hay nói cách khác là hướng và vị trí của 
tên lửa được dự đoán một cách chuẩn xác nhất. 
Ước lượng hướng (ba góc xoay quanh trục x, y, z) được sử dụng trong hầu hết 
các thiết bị quân sự hiện đại và một trong những phương pháp xác định hướng 
ngày càng được sử dụng phổ biến là cảm biến đơn vị đo quan tính (IMU). Hệ 
thống kiểu này bao gồm 3 cảm biến vận tốc góc, 3 cảm biến gia tốc và 3 cảm biến 
từ trường [1-3]. 
Nếu chúng ta biết giá trị khởi tạo ban đầu của bộ ước lượng, hướng của đối 
tượng có thể được tính bằng phương pháp tích phân giá trị đầu ra của cảm biến vận 
tốc góc. Tuy nhiên, phương pháp này chỉ dùng được trong một khoảng thời gian 
ngắn bởi vì sai số sẽ bị cộng dồn trong quá trình tích phân. Trong [8], hướng của 
đối tượng được tính toán dựa trên cảm biến gia tốc và cảm biến từ trường. Tuy 
nhiên, trong phương pháp này sai số có thể lớn khi mà có tác động của gia tốc 
ngoại và nhiễu trong cảm biến từ trường. Như vậy, vấn để trọng tâm của các bộ 
ước lượng hướng là làm thế nào để kết hợp ba loại cảm biến gia tốc, vận tốc góc và 
từ trường. 
Như chúng ta biết, phương pháp ước lượng hướng vật thể bằng cách tiếp cận 
thông qua các góc Euler thì dễ hình dung và dễ hiểu nhưng phương pháp này mất 
nhiều thời gian tính toán và các biến trạng thái có thể gặp phải một số điểm kỳ dị 
[4-8]. Trong bài báo này, để khắc phục những vấn đề trên, phương pháp tiếp cận 
quaternion trong bộ lọc Kalman đã được đề xuất, phương pháp này không những 
tăng tốc độ tính toán của bộ lọc mà còn giải quyết được các điểm kỳ dị trong 
phương pháp Euler. 
Kỹ thuật điều khiển & Điện tử 
N. Đ. Tiến, “Ứng dụng Quaternion và bộ lọc Kalman chuyển động của vật thể bay.” 36 
2. NỘI DUNG CẦN GIẢI QUYẾT 
2.1. Giới thiệu về cảm biến IMU 
Trong một bộ IMU thường bao gồm một vài loại cảm biến như là cảm biến vận 
tốc, cảm biến gia tốc và cảm biến từ trường. Nhóm nghiên cứu đã kết hợp những 
loại cảm biến này trong quá trình ước lượng tư thế. 
Bộ cảm biến vận tốc góc có thể được sử dụng để ước lượng trực tiếp hướng của 
đối tượng. Gọi x , y , z là ba vận tốc góc đo được từ bộ cảm biến và q là 
vector quaternion. Sau đó, vector quaternion được tính dựa trên giá trị của vận tốc 
góc hiện tại và giá trị trước đó của vector quaternion qˆ [10]. 
1
ˆ
2
q q   (1) 
với  là một quaternion tinh khiết và  là ký hiệu của phép nhân quaternion. 
Phương trình (1) có thể được chuyển sang dạng sau. 
ˆ ˆ ˆ
ˆ ˆ ˆ1
ˆ ˆˆ2
ˆ ˆ ˆ
x y z
x z y
y x z
z y x
x y zw
w y zx
q
w z xy
w x yz
  
  
  
  





 (2) 
với ˆ ˆ ˆ, , w x y và zˆ là bốn thành phần hướng quaternion trước đó qˆ . 
Bộ cảm biến gia tốc và cảm biến từ trường có thể dùng để ước lượng hướng của 
những đối tượng di chuyển chậm. Trong đó: 
 , ,x y za a a là gia tốc tuyến tính đầu ra của cảm biến gia tốc. 
 , ,x y zm m m là từ trường đầu ra đo được từ cảm biến từ trường. 
2.2. Thuật toán ước lượng hướng của đối tượng 
Trong thuật toán này, vector cảm biến góc đóng vai trò trạng thái của hệ thống. 
Đầu ra của cảm biến gia tốc và cảm biến từ trường đóng vai trò là hệ quan sát. 
Đặt quaternion 4q ,
Te q 
 , với  1 2 3, ,
T
e q q q 
, phương trình (2) được viết 
lại như sau. 
4 3 21
3 4 12
2 1 43
1 2 34
1 1
q
2 2
x
y
z
q q qq
q q qq
q q qq
q q qq

 

  


 


 (3) 
Như vậy, phương trình trạng thái của hệ thống được thiết lập như sau: 
4 4 4 3
3 4 3 3
0q 0
0 0 (t)w
 


 (4) 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số 48, 04 - 2017 37
với (t)w là nhiễu hệ thống được giả sử là có phân bố Gaussian với hiệp phương sai 
là Q , q là vector vận tốc của q ,  là vector vận tốc góc. 
Mô hình đo sử dụng ba cảm biến gia tốc và ba cảm biến vận tốc được thiết lập 
như sau: 
3 3 0
3 3 0
(t)(q) 0
(t)0 (q)
a
g
vC ga
vC mm
 (5) 
với (t), (t)a gv v là nhiễu phép đo, (q)C là ma trận đổi từ quaternion sang ma trận 
quay được thiết lập dưới dạng sau [10]: 
2 2
1 2 2 3 1 4 2 4 1 3
2 2
2 3 1 4 1 3 3 4 1 2
2 2
2 4 1 3 3 4 1 2 1 4
2 2 1 2 2 2 2
(q) 2 2 2 2 1 2 2
2 2 2 2 2 2 1
q q q q q q q q q q
C q q q q q q q q q q
q q q q q q q q q q
 (6) 
Bởi vì phương trình (5) là phi tuyến, tuyến tính hóa phương trình quan sát ta được. 
ˆkx x
a
H
mx

 
 (7) 
Để khắc phục tác động của nhiễu và gia tốc ngoại trong bài báo này, bộ bù thích 
nghi của tác giả [9] đã được sử dụng để bù lại tác động của gia tốc ngoại. 
 0 0 0 0
1
; a a a g g gr r r r r r 
 (8) 
trong đó, 0ar , 0gr là phương sai của cảm biến gia tốc và cảm biến vận tốc góc. Bộ 
bù thích nghi được tính toán dựa trên tiêu chuẩn [9]. 
2 2 2(a ,a ,a ) 1x y z x y zf a a a  (9) 
với  là một số dương. 
Hình 1. Quy trình hoạt động của bộ lọc Kalman mở rộng. 
-Trạng thái hệ thống kế tiếp 
 ˆk k kx x
  
-Sai số hiệu phương sai kế tiếp 
 1
T
k k k kP P Q
   
Qúa trình dự đoán 
Quá trình hiệu chỉnh 
- Tuyến tính hóa phương trình phi tuyến 
ˆ
( )
k
k
x x
f x
H
x

 
- Độ lợi mạch lọc Kalman 
 T Tk k kK P H HP H R 
- Giá trị hiệu chỉnh 
 1 1ˆ ˆ ˆk k k k kx x K z Hx 
- Hiệu chỉnh sai số hiệp phương sai 
 k k kP I K H P
Kỹ thuật điều khiển & Điện tử 
N. Đ. Tiến, “Ứng dụng Quaternion và bộ lọc Kalman chuyển động của vật thể bay.” 38 
Trong phương pháp bộ bù thích nghi này. Khi mà đối tượng nằm dưới sự tác 
động của gia tốc ngoại, thì bộ lọc Kalman sẽ tin tưởng nhiều hơn vào tín hiệu đo 
được từ gyroscope vì khi đó tín hiệu đo được từ cảm biến gia tốc sẽ chứa thành 
phần không mong muốn của gia tốc ngoại. Như vậy, phương trình trạng thái và 
phương trình quan sát của bộ lọc ước lượng hướng đã được xây dựng xong. Cuối 
cùng, bộ lọc ước lương hướng được xây dựng như hình 1. 
3. MÔ PHỎNG, TÍNH TOÁN, THẢO LUẬN 
3.1. Phương pháp, công cụ mô phỏng 
Để chứng minh sự hiệu quả của bộ lọc ước lượng đề xuất, nhóm nghiên cứu của 
chúng tôi đã tiến hành mô phỏng trên phần mềm Matlab. Mô hình của IMU được 
lấy từ toolbox Gimbal IMU of Matlab Aerospace Blockset, kiểu mô phỏng fixstep, 
với bước tính 0.001s và phương pháp giải ODE4. Các thông số của IMU được mô 
phỏng dựa trên bộ XSen’s MTi 28A53G35 IMU [11] với phương sai tĩnh của cảm 
biến gia tốc và cảm biến vận tốc góc lần lượt là 0 1.7735 004a er và 
0 8.4053 005g er . Thêm vào đó, để tăng độ thuyết phục trong quá trình mô 
phỏng chúng tôi có cho thêm tác động của gia tốc ngoại với dải tác động là 
220 /m s (hình 2) lên đối tượng cần ước lượng hướng. 
Hình 2. Biểu đồ gia tốc ngoại tác động lên đối tượng cần được ước lượng hướng. 
3.2. Kết quả mô phỏng và bình luận 
Kết quả mô phỏng được trình bày ở hình 3 và 4. Trong hình 3 là kết quả của bộ 
lọc ước lượng tiêu chuẩn. Nhìn vào kết quả chúng ta có thể thấy là dưới tác động 
của gia tốc ngoại thì bộ lọc thông thường không cho ta kết quả mong muốn. Trong 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số 48, 04 - 2017 39
khi đó, bộ lọc của nhóm nghiên cứu (hình 4) có tính năng nhận diện gia tốc ngoại 
thông qua bộ bù thích nghi, do đó, tác động của gia tốc ngoại đã bị làm suy yếu. 
Thêm vào đó, bộ lọc đề xuất sử dụng phương pháp quaternion nên thời gian xử lý 
sẽ nhanh hơn (0.55 s) so với phương pháp Euler (0.78 s), đồng thời, các điểm kỳ dị 
trong phương pháp Euler cũng được loại bỏ. Từ kết quả mô phỏng cho thấy, khả 
năng ứng dụng rất cao của bộ lọc lên các thiết bị quân sự đòi hỏi độ chính xác cao 
trong quá trình hiện đại hóa quân sự của Việt Nam. 
Hình 3. Kết quả ước lượng của bộ lọc ước lượng hướng thông thường. 
Hình 4. Kết quả ước lượng của bộ lọc ước lượng hướng đề xuất bởi nhóm tác giả. 
Kỹ thuật điều khiển & Điện tử 
N. Đ. Tiến, “Ứng dụng Quaternion và bộ lọc Kalman chuyển động của vật thể bay.” 40 
4. KẾT LUẬN 
Trong bài báo này chúng tôi vừa đề xuất một bộ lọc Kalman mở rộng dựa trên 
phương pháp quaternion cho việc ước lượng hướng của những vật thể bay không 
xác định. Phương pháp trên không những có khả năng xử lý sẽ nhanh hơn, loại bỏ 
được những điểm kỳ dị trong phương pháp Euler mà còn có khả năng ước lượng 
chuẩn xác dưới sự tác động của gia tốc ngoại thông qua bộ bù thích nghi. Kết quả 
mô phỏng đã chứng minh khả năng vượt trội của bộ lọc đề xuất so với bộ lọc tiêu 
chuẩn. Trong tương lai chúng tôi sẽ ứng dụng phương pháp này cho quá trình thực 
nghiệm trên các thiết bị quân sự đòi hỏi độ chính xác cao. 
TÀI LIỆU THAM KHẢO 
[1]. B. Barshan et al, “Evaluation of a Solid-State Gyroscope for Robotic 
Application,” IEEE Trans. On Instrumentation and Measurement, Vol. 44, No. 
1(1994), pp.61-67. 
[2]. B. Barshan et al, “Inertial Navigation System for Mobile Robots,” IEEE Trans. 
Robot. Automat., Vol. 11, No. 3(1995), pp.328-342. 
[3]. P. Bristeau et al, “Trajectory estimation for a hybrid rocket,” AIAA Guidance 
Navigration and Control Conference, Chicago, US 2009. 
[4]. T. S. Bruggemann et al, “GPS Fault Detection with IMU and Aircraft 
Dynamics,” IEEE Transactions on Aerospace and Electronic Systems, Vol. 
47, No. 1, pp. 305-316. 
[5]. F. Qin et al, “Performance assessment of a low-cost inertial measurement unit 
based ultra-tight global navigation satellite system/inertial navigation system 
integration for high dynamic applications,” IET Radar, Sonar Navigat., vol. 8, 
no. 7, pp. 828–836, 2014. 
[6]. A. Golovan et al, “Small satellite attitude determination based on GPS/IMU 
data fusion,” ICNPAA 2014: 10th International Conference on Mathematical 
Problems in Engineering Aerospace and Sciences AlP Conference 
Proceedings, vol. 1637, pp. 341-348, 2014. 
[7]. R. G. Brown et al, “Introduction to Random Signals and Applied Kalman 
Filtering,” 1997, John Wiley & Sons. 
[8]. H. Rehbinder et al, “Drift-free attitude estimation for accelerated rigid 
bodies” Science Direct - Automatica, April 2004, Vol. 40, No. 4, pp. 653-659. 
[9]. Y. S. Suh, et al, “Attitude Estimation Adaptively Compensating External 
Acceleration,” JSME International Journal, 2006, Series C, Vol.49, No 1. 
[10]. Sabatini, A.M., “Quaternion-Based Extended Kalman Filter for 
Determining orientation by inertial and magnetic sensing”, IEEE 
Transactions on Biomedical Engineering (Volume: 53, Issue: 7, July 2006). 
[11]. https://www.xsens.com. 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số 48, 04 - 2017 41
ABSTRACT 
APPLYING QUATERNION AND KALMAN FILTER IN ESTIMATING 
DIRECTION OF FLYING OBJECT 
 In this paper, a new method for object direction estimation through IMU 
and magnetic sensor is presented. IMU sensor was used to obtain the 
attitude of the object while the yaw angle is estimated by magnetic sensor. 
The extended Kalman filter is used to handle these sensors’ output and to 
reduce the effect of the measurement noise. Moreover, quaternion based 
method have been used to solve the singularity problem which may appear in 
the Euler method. Finally, the simulation results demonstrate the 
effectiveness of the proposed method. 
Keywords: IMU sensor, Magnetic sensor, Kalman filter, Quaternion. 
Nhận bài ngày 03 tháng 3 năm 2017 
Hoàn thiện ngày 04 tháng 4 năm 2017 
Chấp nhận đăng ngày 05 tháng 4 năm 2017 
Địa chỉ: Trường Đại học Kỹ thuật - Hậu cần Công an nhân dân. 
 *Email: dangtient36@gmail.com