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ế - Phạm Văn Phúc

Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 49, 06 - 2017 3 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Ế Phạm Văn Phúc1, Trần Đức Thuận2*, Nguyễn Sỹ Long2, Nguyễn Trọng Yến2, Đặng Văn Thành2 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 kế, con quay vi cơ để xác định góc quay xung quanh trục của các phương tiện 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 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 của các gia tốc kế. Từ khóa: Con quay vi cơ, Góc lắc, Bộ lọc Kalman. 1. ĐẶT VẤN ĐỀ 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ả 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ơ 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ư 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 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 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 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 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 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 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 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. 2. BỘ LỌC KALMAN VÀ THUẬT TOÁN XÁC ĐỊNH GÓC NGHIÊNG VẬT THỂ 2.1. Bộ lọc Kalman 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ệ độ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 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 rời rạc như sau [2]: 1 1 1 1 2( ) ( ) ( ), ( , ,..., ) T k k k k nX F X G X k F f f f     (1) 1( ) ( ); ( ,..., ) T k k mZ h X k h h h   (2) Trong đó: kX , 1kX  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 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 chiều có dạng tạp trắng với kỳ vọng toán học bằng 0; kZ là giá trị của véc tơ đầu ra (véc 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 đo; 1kF  là véc tơ hàm số F ở bước thứ (k-1); 1, 2 ,... nf f f là các hàm phi tuyến với biến số là véc tơ X ; kZ là véc tơ hàm số h ở bước thứ k, 1 2( , ,..., ) T mh h h h ,  v k là các véc 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 sai của véc tơ sai số đánh giá trạng thái véc tơ X như sau: Tên lửa & Thiết bị bay 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ế.” 4 k T k k k kP X X X X            , trong đó:  là ký hiệu kỳ vọng toán học. 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 Kalman [2] như sau: ( ) ( )1 1( )k k kX F X       (3)   1 1 1 ; k k k X X F X           (4)   k k k X X h H X       ; (5)     1 1 1 1 1 1 ; T T k k k k k k kP P G Q G            (6)       ;k k k kP I K H P    (7)      ;k k k k kX X K Z Z       (8)   ;k k kZ h X    (9)      1 T T k k k k k k kK P H H P H R     (10) 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 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 và chỉ khi hạng của ma trận   1 ... nT T T TH H H    bằng bậc của hệ động học, tức là:   1 ... nT T T Trank H H H n     (11) 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 trường nước 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 đị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ị đ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 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 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 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:   (12) 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 được góc nghiêng  . Tuy nhiên con quay vi cơ C cho ta tín hiệu có dạng sau: b     (13) Trong đó:  - Chỉ số do con quay đưa ra;  - Tốc độ góc quay thực của vật thể xung 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 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ơ có dạng ồn trắng. 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 đị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 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 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 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à: Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 49, 06 - 2017 5  1 1sinz g v  (14)  2 2cosz g v  (15) Trong đó: g – Giá trị gia tốc trọng trường nơi vật thể đang hoạt động, 1 2,v v - Nhiễu đo của các gia tốc kế có dạng ồn trắng. Hình 1. Vật thể trong môi trường nước và việc xác định góc nghiêng. Hình 2. Bố trí các phần tử vi cơ trong thiết bị TB. Từ (14) và (15) ta có thể xác định trực tiếp góc nghiêng . Tuy nhiên, để làm được đ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. Đ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 đúng sau:      11 1 2 2 sin sin cos cos cos g vz v z tg v z g v g               (16) Sự gần đúng của biểu thức (16) thỏa mãn khi 2 cos ,v g  tức là góc  không gần góc 090 . Từ (12) và (13) ta có: b      (17) 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:  1 / ,k k t      suy ra: A1 A2 C  g x y TB  Phương nằm ngang Tên lửa & Thiết bị bay 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ế.” 6  1k k k b t        (18) 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: 1k kb b  (19) Đặt 1 2, .x x b  Từ (18) và (19) ta có:        1 1 1 2 1 1 2, 1 1x k f x x g x k x k t t          (20)    2 2 1 2 2( ) , 1x k f x x x k   (21) Ở đây,    1 2,x k x k là các giá trị của 1 2,x x ở bước tính thứ .k Hai phương trình (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 véc tơ hàm  1 1k kF X  trong công thức (3) có dạng:             1 1 2 1 2 1 1 2 1 2 2 , 1 1 , 1k k f x x x k x k t F X f x x x k                  (22) Từ (16) ta có:    1 2 1,z h x x v tg x v    (23) Trong đó:    1 2 1,h x x tg x (24) 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 theo cần xác định các ma trận ,k kH ở các công thức (4) và (5): 1 1 1 2 2 1 2 2 / / 1 / / 0 1 k f x f x t f x f x                     (25)   21 2 1/ / 1/ cos 0kH h x h x x         (26) 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ừ 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 học (1) có dạng: 0 t G        (27) 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à: 2 ,Q     2 vR     (28) 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 toán lọc Kalman để xác định góc nghiêng  (giá trị 1( )x k ). Kết quả mô phỏng Matlab-simulink và thực nghiệm cho thấy: thuật toán cho kết quả đạt độ chính xác trong các điều kiện sau: - Thiết bị hoạt động trong giới hạn  0 060 60 .   Điều này đảm bảo cho biểu thức (16) được thỏa mãn. - 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 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 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 chuyển động quay, tỉ lệ với bình phương tốc độ quay 2 . Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 49, 06 - 2017 7 Hình 3. Sơ đồ thuật toán lọc Kalman. Kết thúc Bắt đầu 0, ,Q R X   00 1 X X k      1G X k   Xˆ k (3)  1k  (4)  Zˆ k (9)  H k (5)  P k (6)  K k (10)  Z k  P k (7) 1k k  KTPK  Xˆ k (8) Từ thiết bị đo Có Không Tên lửa & Thiết bị bay 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ế.” 8 3. KẾT LUẬN 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 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 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 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 đị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 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 vị vệ tinh GPS). TÀI LIỆU THAM KHẢO [1]. Распопов В.Я., “Микросистемная авионика”, учебное пособие. Тула " Грифик", 2010. [2]. “Фильтрация истохастическое управление в динамических системах”. Под редакциёй К. Т. Леондеса. Издательство "Мир", Москва, 1980. [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ệ thống điều khiển thiết bị bay”. NXB Khoa học và Kỹ thuật, Hà Nội, 2015. [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 đườ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 quốc lần thứ 2 về điều khiển và tự động hóa VCCA-2013. Đà Nẵng, 2013. [5]. Songlai Han, Jinling Wang. “Integrated GPS/INS navigation system with daul-rate Kalman Filter”. Springer. Oct.2011. ABSTRACT AN ALGORITHM FOR DETERMINATION THE ANGLE FOR DEVICES OPERATING IN WATER BASED ON THE COMBINATION OF ANGLE SPEED GYROS AND ACCELEROMETERS In this paper, a method of using the information of the accelerometer, micro mechanical gyros to determine the angle of rotation around the axis of the means of operation in water environment on the basis of extended Kalman filter application for nonlinear systems in order to overcome the drift of micro mechanical gyros and noise measurement of accelerometers is described. Keywords: Micromechanical gyros, Angle, Kalman. Nhận bài ngày 09 tháng 9 năm 2016 Hoàn thiện ngày 04 tháng 11 năm 2016 Chấp nhận đăng ngày 20 tháng 6 năm 2017 Địa chỉ: 1Trường Trung cấp Kỹ thuật Hải quân. 2 Viện KH-CNQS; *Email: thuandauto@yahoo.com