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