Xây dựng thuật toán xác định góc lắc cho phương tiện chuyển động trên cơ sở kết hợp con quay tốc độ góc với gia tốc kế

Nghiên cứu khoa học công nghệ<br /> <br /> XÂY DỰNG THUẬT TOÁN XÁC ĐỊNH GÓC LẮC CHO<br /> PHƯƠNG TIỆN CHUYỂN ĐỘNG TRÊN CƠ SỞ KẾT HỢP<br /> CON QUAY TỐC ĐỘ GÓC VỚI GIA TỐC KẾ<br /> Phạm Văn Phúc1, Trần Đức Thuận2*, Nguyễn Sỹ Long2,<br /> Nguyễn Trọng Yến2, Đặng Văn Thành2<br /> Tóm tắt: Bài báo trình bày một phương pháp sử dụng thông tin của các gia tốc<br /> kế, con quay vi cơ để xác định góc quay xung quanh trục của các phương tiện<br /> chuyển động trên cơ sở ứng dụng bộ lọc Kalman để kết hợp thông tin của hai phần<br /> tử đo quán tính nhằm khắc phục được yếu tố trôi của con quay vi cơ và nhiễu đo<br /> của các gia tốc kế.<br /> Từ khóa: Con quay vi cơ, Góc lắc, Bộ lọc Kalman.<br /> <br /> 1. ĐẶT VẤN ĐỀ<br /> Việc xác định góc nghiêng của các vật thể hoạt động nói chung đã được nhiều tác giả<br /> nghiên cứu. Hiện nay, nhiều tác giả [1,3,4,5] đã nghiên cứu ứng dụng các cơ cấu đo vi cơ<br /> quán tính kết hợp với việc đo véc tơ từ trường trái đất hoặc các thiết bị đo vận tốc (như<br /> thiết bị GPS) để xác định góc nghiêng vật thể. Việc phải kết hợp các thiết bị đo là để khắc<br /> phục yếu tố độ trôi tham số đo của phần tử vi cơ quán tính đo vận tốc góc. Tuy nhiên, đối<br /> với vật thể hoạt động trong môi trường nước có một số đặc điểm sau: Không có thông tin<br /> của GPS và chuyển động chậm nên khó có khả năng dùng thông tin về vận tốc để hiệu<br /> chỉnh; Không đo được từ trường trái đất vật thể chuyển động vì có quá nhiều vật thể nhiễm<br /> từ và sinh từ. Trong bài báo này, nhóm tác giả đề xuất một giải pháp sử dụng bộ lọc<br /> Kalman [2] kết hợp thông tin của phần tử vi cơ quán tính đo vận tốc góc với gia tốc kế đo<br /> véc tơ gia tốc trọng trường trái đất để xác định góc nghiêng của vật thể chuyển động.<br /> 2. BỘ LỌC KALMAN VÀ THUẬT TOÁN XÁC ĐỊNH GÓC NGHIÊNG VẬT THỂ<br /> 2.1. Bộ lọc Kalman<br /> Bộ lọc Kalman là một công cụ toán học giúp cho việc đánh giá véc tơ trạng thái hệ<br /> động học trên cơ sở thông tin quan sát véc tơ tín hiệu đầu ra. Giả sử quá trình chuyển động<br /> của vật thể hoặc quá trình công nghệ được mô tả bởi hệ phương trình động học dưới dạng<br /> rời rạc như sau [2]:<br /> X k  Fk 1 ( X k 1 )  G ( X k 1 ) (k ), F  ( f1 , f 2 ,..., f n )T (1)<br /> Z k  h( X k )  (k ); h  (h1 ,..., hm )T (2)<br /> Trong đó: X k , X k 1 là trạng thái của véc tơ trạng thái X (véc tơ n chiều) ở bước thứ k<br /> và bước thứ (k-1); G là ma trận nhiễu tích thước  nxl  ;  ( k ) là véc tơ nhiễu động học l<br /> chiều có dạng tạp trắng với kỳ vọng toán học bằng 0; Z k là giá trị của véc tơ đầu ra (véc<br /> tơ m chiều, thường thì m  n ). Véc tơ đầu ra này có thể đo được bằng các phương tiện<br /> đo; Fk 1 là véc tơ hàm số F ở bước thứ (k-1); f1, f 2 ,... f n là các hàm phi tuyến với biến<br /> số là véc tơ X ; Z k là véc tơ hàm số h ở bước thứ k, h  (h1 ,h2 ,..., hm )T , v  k  là các véc<br /> tơ nhiễu đo m chiều, có dạng tạp trắng với kỳ vọng toán học bằng 0. Ma trận hiệp phương<br /> sai của véc tơ sai số đánh giá trạng thái véc tơ X như sau:<br /> <br /> <br /> Tạp chí Nghiên cứu KH&CN quân sự, Số 49, 06 - 2017 3<br />  Tên lửa & Thiết bị bay<br />   T<br /> Pk    X k  X k   X k  X k  , trong đó:  là ký hiệu kỳ vọng toán học.<br /> Thủ tục của thuật toán đánh giá trạng thái X trên cơ véc tơ quan sát (đo được) Z theo<br /> Kalman [2] như sau:<br />  <br /> X k(  )  Fk 1 ( X k( 1) ) (3)<br /> F<br />  k 1  k 1 X  X    ; (4)<br /> X k 1<br /> <br /> <br />  hk<br /> Hk     ; (5)<br /> X X  X k<br /> Pk     k 1 Pk1  Tk 1  G k 1Q k 1G kT1 ; (6)<br /> <br /> Pk     I  K k H k  Pk  ; (7)<br />   <br /> X k    X k    K k Z k  Z k ;<br />   (8)<br />  <br /> <br /> Z k  hk X k   ;  (9)<br /> 1<br /> K k  Pk   H kT  H P  H<br /> k k<br />  T<br /> k  Rk  (10)<br /> Trong đó, I là ma trận đơn vị. Bản chất của bộ lọc Kalman là xác định đánh giá trạng<br /> <br /> thái X sao cho hiệp phương sai đạt giá trị nhỏ nhất có thể. Điều kiện đánh giá được khi<br /> n 1<br /> và chỉ khi hạng của ma trận H T T H T ...    H T bằng bậc của hệ động học, tức là:<br /> n 1<br /> rank H T T H T ...    HT  n (11)<br /> <br /> 2.2. Thuật toán kết hợp phương tiện đo xác định góc nghiêng vật thể hoạt động ở môi<br /> trường nước<br /> Giả sử có vật thể hoạt động trong môi trường nước được thể hiện trên hình 1. Cần xác<br /> định góc nghiêng  của sàn vật thể so với phương nằm ngang. Trên vật thể có gắn thiết bị<br /> đo vi cơ quán tính TB. Thiết bị TB (hình 2) bao gồm một con quay vi cơ C đo vận tốc góc<br /> lắc vật thể  , hai gia tốc kế đo các thành phần gia tốc theo các phương ox và oy. Gia tốc<br /> kế A1 đo gia tốc theo phương ox, còn gia tốc kế A2 đo gia tốc theo phương oy. Dễ dàng<br /> nhận thấy góc nghiêng  có liên hệ với vận tốc quay  theo biểu thức sau:<br />    (12)<br /> Nhìn vào biểu thức (12) ta thấy chỉ cần tích phân tín hiệu từ con quay C là xác định<br /> được góc nghiêng  . Tuy nhiên con quay vi cơ C cho ta tín hiệu có dạng sau:<br />     b   (13)<br /> Trong đó:  - Chỉ số do con quay đưa ra;  - Tốc độ góc quay thực của vật thể xung<br /> quanh trục oz (vuông góc với mặt phẳng oxy); b - Tham số độ trôi của con quay (biến đổi<br /> chậm, có thể coi là hằng số chưa rõ ở từng giai đoạn cụ thể);  - Nhiễu của con quay vi cơ<br /> có dạng ồn trắng.<br /> Từ biểu thức (13) cho thấy không thể dùng trực tiếp tín hiệu từ con quay vi cơ để xác<br /> định góc nghiêng  . Do phương đo của gia tốc kế A1, A2 vuông góc với bán kính từ tâm<br /> quay nên gia tốc pháp tuyến không ảnh hưởng đến chỉ số đo của nó; Gia tốc tiếp tuyến<br /> và gia tốc dài của chuyển động tâm khối cũng rất ít ảnh hưởng đến các chỉ số của gia tốc<br /> kế A1, A2 [2]. Vì vậy, có thể thấy các chỉ số z1,z2 của gia tốc kế A1, A2 sẽ là:<br /> <br /> <br /> 4 P. V. Phúc, T. Đ. Thuận, …, “Xây dựng thuật toán xác định góc lắc… với gia tốc kế.”<br /> Nghiên cứu khoa học công nghệ<br /> <br /> z1  g sin    v1 (14)<br /> z2  g cos    v2 (15)<br /> Trong đó: g – Giá trị gia tốc trọng trường nơi vật thể đang hoạt động, v1 , v2 - Nhiễu đo<br /> của các gia tốc kế có dạng ồn trắng.<br /> <br /> <br /> <br /> <br /> TB<br /> <br /> Phương nằm ngang<br /> <br /> <br /> <br /> <br /> Hình 1. Vật thể trong môi trường nước và việc xác định góc nghiêng.<br /> <br /> y x<br /> A1<br /> <br /> <br /> C<br /> <br /> <br /> <br /> g<br /> A2<br /> Hình 2. Bố trí các phần tử vi cơ trong thiết bị TB.<br /> Từ (14) và (15) ta có thể xác định trực tiếp góc nghiêng  . Tuy nhiên, để làm được<br /> điều này cần phải biết chính xác giá trị gia tốc trọng trường nơi vật thể đang hoạt động.<br /> Điều này không phải lúc nào cũng thực hiện được. Từ (14) và (15) cho ta biểu thức gần<br /> đúng sau:<br /> z1 g sin    v1 sin  v1<br /> z     tg    v (16)<br /> z2 g cos    v2 cos  g cos <br /> Sự gần đúng của biểu thức (16) thỏa mãn khi v2  g cos  , tức là góc  không gần<br /> góc 900 . Từ (12) và (13) ta có:<br />     b   (17)<br /> Giải phương trình vi phương (17) theo phương pháp Ơle với bước tính t nhận được:<br />   k  k 1  / t , suy ra:<br /> <br /> <br /> <br /> Tạp chí Nghiên cứu KH&CN quân sự, Số 49, 06 - 2017 5<br />  Tên lửa & Thiết bị bay<br /> <br /> k  k 1   k  b    t (18)<br /> Tham số độ trôi của con quay b biến đổi chậm nên có thể chấp nhận đẳng thức sau:<br /> bk  bk 1 (19)<br /> Đặt x1   , x2  b. Từ (18) và (19) ta có:<br /> x1  k   f1  x1 , x2   g1  x1  k  1  x2  k  1 t  t (20)<br /> <br /> x2 (k )  f 2  x1 , x2   x2  k  1 (21)<br /> Ở đây, x1  k  , x2  k  là các giá trị của x1 , x2 ở bước tính thứ k . Hai phương trình<br /> (20), (21) tạo nên hệ thống phương trình (1) của thuật toán lọc Kalman. Dễ dàng nhận thấy<br /> véc tơ hàm Fk 1  X k 1  trong công thức (3) có dạng:<br />  f  x , x    x  k  1  x2  k  1 t <br /> Fk 1  X k 1    1 1 2    1  (22)<br />  f 2  x1 , x2    x2  k  1 <br /> Từ (16) ta có: z  h  x1 , x2   v  tg  x1   v (23)<br /> <br /> Trong đó: h  x1 , x2   tg  x1  (24)<br /> <br /> Phương trình (23) tạo nên hệ thống phương trình (2) của thuật toán lọc Kalman. Tiếp<br /> theo cần xác định các ma trận  k , H k ở các công thức (4) và (5):<br /> <br />  f / x1 f1 / x2  1 t <br /> k   1   (25)<br /> f 2 / x1 f 2 / x2  0 1 <br /> H k   h / x1 h / x2   1/ cos 2 x1 0  (26)<br /> Như vậy, hàm quan sát ở bài toán lọc Kalman nay là phi tuyến (biểu thức (24)). Từ<br /> các phương trình (1) và (20), (21) dễ dàng nhận thấy ma trận G trong phương trình động<br /> học (1) có dạng:<br />  t <br /> G  (27)<br /> 0<br /> Theo [2] các ma trận cường độ nhiễu Q, R là phương sai của nhiễu  , v, tức là:<br /> Q   2  , R   v 2  (28)<br /> Như vậy, đã đủ yếu tố để thực hiện thuật toán lọc Kalman. Trên hình 3 là sơ đồ thuật<br /> toán lọc Kalman để xác định góc nghiêng  (giá trị x1 (k ) ).<br /> Kết quả mô phỏng Matlab-simulink và thực nghiệm cho thấy: thuật toán cho kết quả<br /> đạt độ chính xác trong các điều kiện sau:<br />  <br /> - Thiết bị hoạt động trong giới hạn 600  600 . Điều này đảm bảo cho biểu thức<br /> (16) được thỏa mãn.<br /> - Việc đặt hai gia tốc kế A1, A2 trong thiết bị TB đảm bảo tính vuông góc với các trục<br /> oy và ox. Nếu điều này không đạt được thì trong chỉ số của các gia tốc kế ngoài thành<br /> phần như các biểu thức (14), (15) còn có thành phần phản ánh gia tốc pháp tuyến của<br /> chuyển động quay, tỉ lệ với bình phương tốc độ quay  2 .<br /> <br /> <br /> 6 P. V. Phúc, T. Đ. Thuận, …, “Xây dựng thuật toán xác định góc lắc… với gia tốc kế.”<br /> Nghiên cứu khoa học công nghệ<br /> <br /> <br /> Bắt đầu<br /> <br /> <br /> <br /> Q, R, X 0<br /> <br /> <br /> <br /> X   0  X 0<br /> k 1<br /> <br /> <br /> <br /> G  X   k  1<br /> <br /> <br /> Xˆ   k  (3)<br /> <br /> <br />   k  1 (4)<br /> <br /> <br /> Zˆ  k  (9)<br /> <br /> <br /> H  k  (5)<br /> <br /> <br /> P  k  (6)<br /> <br /> <br /> K  k  (10)<br /> <br /> Từ thiết bị đo<br /> Z k <br /> <br /> <br /> Xˆ   k  (8)<br /> <br /> <br /> P  k  (7)<br /> <br /> k  k 1<br /> <br /> <br /> Có Không<br /> Kết thúc KTPK<br /> <br /> <br /> Hình 3. Sơ đồ thuật toán lọc Kalman.<br /> <br /> <br /> <br /> Tạp chí Nghiên cứu KH&CN quân sự, Số 49, 06 - 2017 7<br />  Tên lửa & Thiết bị bay<br /> <br /> 3. KẾT LUẬN<br /> Bằng giải pháp ứng dụng bộ lọc Kalman phi tuyến đã kết hợp các thông tin của con<br /> quay vi cơ đo tốc độ góc có độ trôi với các gia tốc kế đo tương quan của hai thành phần<br /> véc tơ gia tốc trọng trường (mà không cần biết chính xác giá trị gia tốc trọng trường) cho<br /> phép xác định góc nghiêng của vật thể chuyển động. Thuật toán này có giá trị cho việc xác<br /> định góc cren của các chủng loại tên lửa quay quanh trục dọc hoặc các vật thể hoạt động<br /> trong môi trường nước, mà ở đó không có thông tin của các thiết bị ngoài khác (như định<br /> vị vệ tinh GPS).<br /> TÀI LIỆU THAM KHẢO<br /> [1]. Распопов В.Я., “Микросистемная авионика”, учебное пособие. Тула " Грифик",<br /> 2010.<br /> [2]. “Фильтрация истохастическое управление в динамических системах”. Под<br /> редакциёй К. Т. Леондеса. Издательство "Мир", Москва, 1980.<br /> [3]. Trần Đức Thuận, Bùi Ngọc Mỹ. “Thiết bị dẫn đường quán tính và đo cao trong hệ<br /> thống điều khiển thiết bị bay”. NXB Khoa học và Kỹ thuật, Hà Nội, 2015.<br /> [4]. Dương Bạch Phi, Ngô Khánh Hiếu, Nguyễn Vĩnh Hào. “Phát triển hệ thống dẫn<br /> đường tích hợp GPS/INS cho máy bay mô hình”. Tr.162-167, Kỷ yếu Hội nghị toàn<br /> quốc lần thứ 2 về điều khiển và tự động hóa VCCA-2013. Đà Nẵng, 2013.<br /> [5]. Songlai Han, Jinling Wang. “Integrated GPS/INS navigation system with daul-rate<br /> Kalman Filter”. Springer. Oct.2011.<br /> <br /> ABSTRACT<br /> AN ALGORITHM FOR DETERMINATION THE ANGLE FOR DEVICES<br /> OPERATING IN WATER BASED ON THE COMBINATION OF ANGLE SPEED<br /> GYROS AND ACCELEROMETERS<br /> In this paper, a method of using the information of the accelerometer, micro<br /> mechanical gyros to determine the angle of rotation around the axis of the means of<br /> operation in water environment on the basis of extended Kalman filter application<br /> for nonlinear systems in order to overcome the drift of micro mechanical gyros and<br /> noise measurement of accelerometers is described.<br /> Keywords: Micromechanical gyros, Angle, Kalman.<br /> Nhận bài ngày 09 tháng 9 năm 2016<br /> Hoàn thiện ngày 04 tháng 11 năm 2016<br /> Chấp nhận đăng ngày 20 tháng 6 năm 2017<br /> Địa chỉ: 1Trường Trung cấp Kỹ thuật Hải quân.<br /> 2<br /> Viện KH-CNQS;<br /> *Email: thuandauto@yahoo.com<br /> <br /> <br /> <br /> <br /> 8 P. V. Phúc, T. Đ. Thuận, …, “Xây dựng thuật toán xác định góc lắc… với gia tốc kế.”<br />