Ứ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