Nghiên cứu khoa học công nghệ
ỨNG DỤNG QUATERNION 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 cảm biến từ trường. Cảm biến
IMU giúp chúng ta ước lượng 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ả 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.
Tạp chí Nghiên cứu KH&CN quân sự, Số 48, 04 - 2017
35
ˆ
1
ˆ
ˆ ˆ
ˆ
w
ω
ω
ˆ
ˆ
w
x
x z y
ω
x
+ z
ˆ ˆ
ˆ
w
y
2
y
w
x
ˆ ˆ ˆ ˆ
ˆ
1
q
q
q
q
2
2
3
q
1
1
q
1
2
ω
4
=
Kỹ thuật điều khiển & Điện tử
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
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].
với ω
q = 2 qω (1)
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.
xωx yωy zωz
q = = 1 ˆωy + yωx zωz
z ˆωz + ˆωy ˆωx
(2)
với w, x, y 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 đó:
(ax,ay ,az ) là gia tốc tuyến tính đầu ra của cảm biến gia tốc.
(mx,my ,mz ) 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 q = eT ,q4
, với
e =q ,q2 ,q3 T , phương trình (2) được viết
lại như sau.
1 q4
q = 2 = 1 q3
4 q
q3
q4
q
q2
q2 ωx
q1 ωy Θω
q3 z
(3)
Như vậy, phương trình trạng thái của hệ thống được thiết lập như sau:
q 044
ω 034
Θ43 0
033 w(t)
(4)
36
N. Đ. Tiến, “Ứng dụng Quaternion bộ lọc Kalman… chuyển động của vật thể bay.
=
+
2
1
1
1
1
2
1
1
1
1
2
1
a
ˆ
1
a a a
r r
r
r
r
2 2 2
x
ˆ
ˆ
( )
k k
k k
( )
− −
ˆ ˆ ˆ
k k
Nghiên cứu khoa học công nghệ
với w(t) là nhiễu hệ thống được giả sử là có phân bố Gaussian với hiệp phương sai
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:
a C(q)
m 033
033 g0 va (t)
C(q)m0 vg (t)
(5)
với va (t),vg (t)
là nhiễu phép đo, C(q) là ma trận đổi từ quaternion sang ma trận
quay được thiết lập dưới dạng sau [10]:
2q2 +2q2 1
C(q) 2q2q3 2q q4
2q2q4 +2q q3
2q2q3 +2q q4
2q2 +2q3 1
2q3q4 2q q2
2q2q4 2q q3
2q3q4 +2q q2
2q2 +2q4 1
(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.
H = x m x=xk
(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.
r = r 0 r 0;
g = g0 g0
(8)
trong đó, a0 , g0 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].
f (ax,ay ,az ) = ax +ay +az 1
(9)
với
là một số dương.
Quá trình hiệu chỉnh
Qúa trình dự đoán
-Trạng thái hệ thống kế tiếp
- Tuyến tính hóa phương trình phi tuyến
Hk = f (x)
x=xk
xk = Φk xk
- Độ lợi mạch lọc Kalman
Kk = PHT HPHT + R
-Sai số hiệu phương sai kế tiếp
P= Φk P1ΦkT +Q
- Giá trị hiệu chỉnh
xk = xk 1 + Kk zk Hxk 1
- Hiệu chỉnh sai số hiệp phương sai
P = (I Kk H)P
Hình 1. Quy trình hoạt động của bộ lọc Kalman mở rộng.
Tạp chí Nghiên cứu KH&CN quân sự, Số 48, 04 - 2017
37