- Trang Chủ
- Tự động hoá
- Ứ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
Xem mẫu
- Nghiên cứu khoa học công nghệ
Ứ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.
Tạp chí Nghiên cứu KH&CN quân sự, Số 48, 04 - 2017 35
- 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 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
qˆ q (1)
2
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.
w xˆ x yˆ y zˆ z
x
1 wˆ x yˆ z zˆ y
q (2)
y 2 wˆ y zˆ x xˆ z
z wˆ z xˆ y yˆx
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 đó:
a , a , a là gia tốc tuyến tính đầu ra của cảm biến gia tốc.
x y z
m , m , m là từ trường đầu ra đo được từ cảm biến từ trường.
x y z
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
Đặt quaternion q e T , q4 , với e q1 , q 2 , q3 , phương trình (2) được viết
lại như sau.
q1 q4 q3 q2
q
1 q q4 q1 x 1
q 2 3 (3)
q3 2 q2 q1 q4 y 2
q4 q1 q2 q3 z
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 43 0
0 033 w(t)
(4)
34
36 N. Đ. Tiến, “Ứng dụng Quaternion và bộ lọc Kalman… chuyển động của vật thể bay.”
- 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
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:
a C (q) 033 g 0 va (t)
m 0 (5)
33 C (q) m0 vg (t)
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]:
2q12 2q22 1 2q2 q3 2q1q4 2q2 q4 2q1q3
C (q) 2q2 q3 2q1q4 2q12 2q32 1 2q3 q4 2q1q2 (6)
2q2 q4 2q1q3 2q3 q4 2q1q2 2q12 2q42 1
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.
a
H (7)
x m x xˆ
k
Để 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.
1
ra ra 0 ra 0 ; rg rg 0 rg 0 (8)
trong đó, ra 0 , rg 0 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 (a x , a y , a z ) ax2 a y2 az2 1 (9)
với là một số dương.
Quá trình hiệu chỉnh
- Tuyến tính hóa phương trình phi tuyến
Qúa trình dự đoán
f ( x)
Hk
-Trạng thái hệ thống kế tiếp x x xˆ
k
- Độ lợi mạch lọc Kalman
xˆ k xk
k
K k Pk H T HPk H T R
-Sai số hiệu phương sai kế tiếp - Giá trị hiệu chỉnh
Pk k Pk 1 k T Q xˆk xˆk1 K k zk Hxˆk1
- Hiệu chỉnh sai số hiệp phương sai
Pk I K k H Pk
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
- Kỹ thuật điều khiển & Điện tử
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à ra 0 1.7735e 004 và
rg 0 8.4053e 005 . 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à
20m / s 2 (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
38 N. Đ. Tiến, “Ứng dụng Quaternion và bộ lọc Kalman… chuyển động của vật thể bay.”
- Nghiên cứu khoa học công nghệ
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ả.
Tạp chí Nghiên cứu KH&CN quân sự, Số 48, 04 - 2017 39
- Kỹ thuật điều khiển & Điện tử
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.
40 N. Đ. Tiến, “Ứng dụng Quaternion và bộ lọc Kalman… chuyển động của vật thể bay.”
- Nghiên cứu khoa học công nghệ
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
Tạp chí Nghiên cứu KH&CN quân sự, Số 48, 04 - 2017 41
nguon tai.lieu . vn