zunia.vn

Tuyển sinh 2024 dành cho Gen-Z

zunia.vn

» Khoa Học Tự Nhiên

Xây dựng thuật toán xác định tốc độ góc cho vật thể chuyển động bằng hệ đa gia tốc kế

Chia sẻ: ViVientiane2711 ViVientiane2711 | Ngày: | Loại File: PDF | Số trang:9

Báo xấu

41
lượt xem 2
download

Download Vui lòng tải xuống để xem tài liệu đầy đủ

Bài viết trình bày cơ sở toán học cho việc sử dụng thông tin đo được từ hệ các gia tốc kế để xác định tốc độ quay của vật thể chuyển động. Từ tốc độ quay có thể xác định được các góc tư thế của vật thể.

Chủ đề:

Bình luận(0) Đăng nhập để gửi bình luận!

Lưu

Nội dung Text: Xây dựng thuật toán xác định tốc độ góc cho vật thể chuyển động bằng hệ đa gia tốc kế

TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ NĂNG LƯỢNG - TRƯỜNG ĐẠI HỌC ĐIỆN LỰC (ISSN: 1859 - 4557) XÂY DỰNG THUẬT TOÁN XÁC ĐỊNH TỐC ĐỘ GÓC CHO VẬT THỂ CHUYỂN ĐỘNG BẰNG HỆ ĐA GIA TỐC KẾ PROPOSING AN ALGORITHM FOR DETERMINING THE ANGLE VELOCITY OF MOVING OBJECTS WITH MULTIPLE ACCELEROMETER SYSTEM Nguyễn Văn Diên1, Đặng Tiến Trung2 Trường Đại học Sư phạm kỹ thuật Hưng Yên1, Trường Đại học Điện lực2 Ngày nhận bài: 23/11/2019, Ngày chấp nhận đăng: 24/04/2020, Phản biện: PGS.TS. Nguyễn Quang Hùng Tóm tắt: Bài báo trình bày cơ sở toán học cho việc sử dụng thông tin đo được từ hệ các gia tốc kế để xác định tốc độ quay của vật thể chuyển động. Từ tốc độ quay có thể xác định được các góc tư thế của vật thể. Giải pháp của bài toán là xây dựng các phương trình mô tả quan hệ giữa tốc độ quay với các gia tốc ở các điểm khác nhau trên vật, sau đó sử dụng giải thuật lọc Kalman để xây dựng thuật toán xác định tốc độ quay của vật. Kết quả là xác định được tư thế và vị trí của vật thể trong không gian (giải quyết bài toán dẫn đường cho vật thể chuyển động). Từ khóa: Gia tốc kế, đối tượng dịch chuyển, thuật toán lọc Kalman. Abstract: Paper presents the mathematical basis of using information that is measured from the accelerometer system to determine the rotation speed of a moving object. From the rotational speed, it is possible to determine the position angles of the object. The solution of the problem is to build equations describing the relationship between the rotation speed and the acceleration at different points on the object. Then the Kalman filtering algorithm is used to build an algorithm to determine the rotation speed of the object. The posture and position of objects in space are identified. Keywords: Accelerometer, moving object, kalman filtering algorithm. 1. MỞ ĐẦU công trình đề cập và đã có nhiều ứng Vấn đề dẫn đường cho vật thể chuyển dụng được hiện thực hóa. Do các con động bản chất là xác định vị trí, vận tốc quay đo tốc độ góc luôn có độ trôi không của tâm khối và tư thế của nó so với hệ (có giá trị phát ra cho dù vật thể không tọa độ được chọn làm hệ tọa độ dẫn quay) và độ trôi này biến đổi trong quá đường. Việc sử dụng các phương tiện đo trình hoạt động, vì vậy trong thực tế phải như con quay đo tốc độ góc, gia tốc kế và có các hệ đo khác để khắc phục hiệu ứng thông tin định vị vệ tinh đã được nhiều sai số do tính trôi không của các con 20 Số 22
TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ NĂNG LƯỢNG - TRƯỜNG ĐẠI HỌC ĐIỆN LỰC (ISSN: 1859 - 4557) quay. Trong [1] đã xây dựng thuật toán các tốc độ quay đó như sau: kết hợp con quay đo tốc độ góc với hệ đa c11  c12 z  c13 y , c12  c13 x  c11 z , gia tốc kế để khắc phục hiện tượng trôi không. Trong bài báo này, nhóm tác giả c13  c11 y  c12 x , c21  c22 z  c23 y , đề xuất giải pháp chỉ sử dụng hệ đa gia c22  c23 x  c21 z , c23  c21 y  c22 x (2) tốc kế (không sử dụng con quay và thông c31  c32 z  c33 y , c32  c33 x  c31 z , tin định vị vệ tinh) để giải quyết vấn đề c33  c31 y  c32 x dẫn đường. Vấn đề này thực sự có ý nghĩa thực tiễn khi xây dựng thiết bị dẫn đường Trong (2), cij là các phần tử của ma trận A. cho các phương tiện ngầm, phương tiện Như vậy để xác định tư thế của vật trong chuyển động nhanh mà ở đó không thể có quá trình động phải có thông tin về các thông tin từ các hệ định vị vệ tinh. tốc độ quay x ,  y , z . Sau đây sẽ trình 2. THIẾT KẾ THUẬT TOÁN XÁC ĐỊNH bày việc xác định x ,  y , z nhờ hệ đa gia THÔNG TIN VỀ TỐC ĐỘ QUAY GÓC tốc kế gắn trên vật thể chuyển động. TỪ THÔNG TIN HỆ ĐA GIA TỐC KẾ Vì các gia tốc kế gắn trực tiếp ở vật thể chuyển động, nên các chỉ số do các gia tốc kế đưa ra là các thành phần của gia tốc ở các điểm gắn gia tốc kế đo trong hệ tọa độ gắn liền với vật thể (hệ tọa độ liên kết). Để xác định được tham số chuyển động của tâm khối vật thể so với hệ tọa độ dẫn Hình 1. Chuyển động phức hợp của vật rắn lý tưởng đường cần phải có thông tin gia tốc tâm khối so với hệ tọa độ dẫn đường. Để hiểu rõ bản chất việc xác định tốc độ góc trên cơ sở hệ đa gia tốc kế, ta xem xét  x0   xl  y   Ay  chuyển động của một vật rắn lý tưởng như  0  l (1) hình 1. Giả sử vật rắn chuyển động phức  z0   zl  hợp có gia tốc góc , vận tốc góc , gia Trong đó A gọi là ma trận cosin định tốc dài . Khi đó phương trình cơ bản hướng, có cấu trúc như [2, 3]. mô tả gia tốc của một điểm P bất kỳ trên Việc xác định quan hệ giữa hệ tọa độ dẫn vật rắn như sau [1,4]: đường và hệ tọa độ liên kết chính là việc (3) xác định ma trận A hoặc ba góc  , ,  (góc tư thế [3]). Theo [2, 3] nếu có thông trong đó, là gia tốc tại điểm P và O; tin về tốc độ quay của vật thể xung quanh là gia tốc hướng tâm tại P; là gia 3 trục của hệ tọa độ liên kết x ,  y , z thì tốc tiếp tuyến tại P; O là tâm khối của có quan hệ của các phần tử ma trận A với vật thể. Số 22 21
TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ NĂNG LƯỢNG - TRƯỜNG ĐẠI HỌC ĐIỆN LỰC (ISSN: 1859 - 4557) Chúng ta biết rằng [4]: (4) (5) ở đây là vectơ nối điểm P với điểm O. Từ (3), (4), (5) suy ra: (6) Hình 2. Mô hình 4 hệ gia tốc kế 3 trục Theo [4], áp dụng công thức nhân 2 vectơ có hướng ta có: Vì hệ gia tốc kế số 1 nằm ở tâm khối vật thể, nên chỉ số của nó sẽ là gia tốc 0 rz  ry  x chuyển động dài, tức là:   rP 0   rz 0 rx  y [A1x A1 y A1z ]T  [ax0 a 0y a 0z ]T (9) ry  rx 0 z (7) trong đó, A1x ; A1 y ; A1z là các giá trị lý tưởng 0 rz  ry  x của hệ gia tốc kế số 1; ax0 ;a0y ;az0 là các gia   rz 0 rx  y tốc dài của vật thể. ry  rx 0 z Hệ gia tốc kế số 2 nằm cách tâm khối O   (  rPO )  khoảng cách r và ở vị trí:  x2 r2 x  r; r2 y  0; r2 z  0 (10)  y2 0  rx  rx ry rz 0 Từ công thức (3), (7), (8) ta có chỉ số gia  z2 (8) tốc kế số 2 như sau:  ry 0  ry rx 0 rz   rz  rz 0 0 rx ry x y A2 x ax0 0 r ( y2  z2 )  x z A2 y  a 0y  rz  rx y (11)  y z A2 z az0 r y rxz Xem xét mô hình hệ cảm biến gồm 4 hệ trong đó A2 x ; A2 y ; A2 z là các giá trị lý tưởng gia tốc kế 3 trục như hình 2, trong đó hệ gia tốc kế số 1 đặt ở tâm khối vật thể của hệ gia tốc kế số 2, x ;  y ; z là các chuyển động. Còn ba hệ gia tốc kế còn lại vận tốc góc của vật thể; là các đặt ở 3 vị trí trên các trục hệ tọa độ gắn gia tốc góc của vật thể. liền với vật thể chuyển động. Hệ gia tốc kế số 3 nằm cách tâm khối O Hệ tọa độ OXYZ là hệ tọa độ liên kết của khoảng cách: vật thể chuyển động, tất cả các cảm biến r3 x  0; r3 y  r; r3 z  0 (12) có trục nhạy song song với các trục tương ứng của hệ tọa độ liên kết. Từ công thức (3), (7), (8) ta có chỉ số gia 22 Số 22
TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ NĂNG LƯỢNG - TRƯỜNG ĐẠI HỌC ĐIỆN LỰC (ISSN: 1859 - 4557) tốc kế số 3 như sau: Đặt A3 x ax0 rz rx y m1   x ; m2   y ; m3   z ;  A3 y  a y  0  r (x2  z2 ) (13) m4   x ; m5   y ; m6   z ; 0 2 2 2  (19) A3 z az0 rx r yz m7   x y ; m8   x z ; m    ; trong đó A3 x ; A3 y ; A3 z là các giá trị lý tưởng  9 y z của hệ gia tốc kế số 3. Với cách đặt biến như (19) từ các phương trình (16), (17), (18) có hệ 9 phương trình: Hệ gia tốc kế số 4 nằm cách tâm khối O 1) m5  m6  ( A2 x  A1x ) / r khoảng cách:  2)  m7  m3  ( A2 y  A1 y ) / r r4 x  0; r4 y  0; r4 z  r  (14) 3)  m8  m2  ( A2 z  A1z ) / r 4) m7  m3  ( A3 x  A1x ) / r Từ công thức (3), (7), (8) ta có chỉ số gia  tốc kế số 4 như sau: 5)  m4  m6  ( A3 y  A1 y ) / r  6) m9  m1  ( A3 z  A1z ) / r A4 x ax0 r y rxz 7) m8  m2  ( A4 x  A1x ) / r A4 y  a y  rx  0 r yz (15)  A4 z az0 0 r (x2   y2 ) 8) m9  m1  ( A4 y  A1 y ) / r  9)  m4  m5  ( A4 z  A1z ) / r (20) trong đó A4 x ; A4 y ; A4 z là các giá trị của hệ Hệ phương trình tuyến tính (20) là hệ gia tốc kế số 4. tuyến tính với 9 ẩn số mi ; i  1..9 . Tiến Trừ hai vế của phương trình (11) với hai hành giải hệ này nhận được: vế phương trình (9) nhận được: 1) m1   x  ( A1 y  A3 z  A1z  A4 y ) / 2r  A2 x  A1x r (   ) 2) m2   y  ( A2 z  A4 x  A1x  A1z ) / 2r 2 2 0 y z A2 y  A1 y  rz  rx y (16) 3) m    ( A  A  A  A ) / 2r  3 z 1x 1y 2y 3x A2 z  A1z r y rxz 4) m4   x  ( A4 z  A1z  A2 x  A1x  A3 y 2  Trừ hai vế của phương trình (13) với hai  A1 y ) / 2r  vế phương trình (9) nhận được: 5) m5   y  ( A1z  A4 z  A2 x  A1x  A3 y 2  A3 x  A1x rz rx y  A1 y ) / 2r A3 y  A1 y  0  r (x2  z2 )  6) m6   z  ( A2 x  A1x  A1z  A4 z  A3 y (17) 2 A3 z  A1z rx r yz   A ) / 2r  1y Trừ hai vế của phương trình (15) với hai 7) m7   x y  ( A3 x  A1x  A1 y  A2 y ) / 2r  vế phương trình (9) nhận được: 8) m8   x z  ( A4 x  A1x  A2 z  A1z ) / 2r 9) m     ( A  A  A  A ) / 2r A4 x  A1x r y rxz  9 y z 3z 1z 1y 4y A4 y  A1 y  rx  r yz (18) (21) A4 z  A1z 0 r (x2   y2 ) Để xác định vận tốc góc x ,  y , z của vật Số 22 23
TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ NĂNG LƯỢNG - TRƯỜNG ĐẠI HỌC ĐIỆN LỰC (ISSN: 1859 - 4557) thể xin đề xuất giải pháp ứng dụng giải ( A2 z  A4 z  A1x  A1z ) thuật lọc Kalman để xác định các vận  ( A2 z  A4 x  A1x  A1z )T / 2r (24b) tốc góc x ,  y , z (bộ lọc Kalman là một ( 2 z   4 x  1x  1z )T / 2r công cụ toán học giúp cho việc đánh giá vectơ trạng thái hệ động học trên ( A1x  A1 y  A2 y  A3 x ) / 2r cơ sở thông tin quan sát vectơ tín hiệu đầu  ( A1x  A1 y  A2 y  A3 x )T / 2r (24c) ra [2]). (1x  1 y   2 y   3 x )T / 2r Để ứng dụng giải thuật Kalman đối với vấn đề xác định x ,  y , z tiến hành Với các đặt biến như (22) từ ba phương trình của hệ (24a), (24b), (24c) nhận xác lập trạng thái X cần đánh giá là được: x ,  y , z , tức là: x1 (k )  x1 (k  1)  X  [x1 , x2 , x3 ]T  [x ,  y , z ]T (22) ( A1 y  A3 z  A1z  A4 y )T / (25) Từ ba phương trình (1, 2, 3) của hệ (21) 2r  (1 y   3 z  1z   4 y )T / 2r có: x2 (k  1)  x2 (k ) ( x (k )  k (k  1)) / T   x ( A2 z  A4 x  A1x  A1z )T (26)  ( A1 y  A3 z  A1z  A4 y ) / 2r /2r  ( 2 z   4 x  1x  1z )T / 2r ( y (k )   y (k  1)) / T   y (23) x3 (k )  x3 (k  1)  ( A2 z  A4 x  A1x  A1z ) / 2r ( z (k )   z ( k  1)) / T   z ( A1x  A1 y  A2 y  A3 x )T / (27)  ( A1x  A1 y  A2 y  A3 x ) / 2r 2r  (1x  1 y   2 y   3 x )T / 2r Trong các phương trình hệ (23) cũng như Phương trình (25), (26), (27) chính là hệ (21) các ký hiệu Aix , Aiy , Aiz , i =1, 2, 3, 4 phương trình động học trong thuật toán lọc Kalman. là các thành phần gia tốc thực chiếu trên các trục. Ta ký hiệu Từ 6 phương trình (từ phương trình thứ 4 đến phương trình thứ 9) của hệ (21) là các số đo đọc được từ các gia tốc kế trên chúng ta nhận được hệ phương trình tham các trục đo tương ứng, các ký hiệu chiếu như sau:  ix , iy , iz , i  1, 2,3, 4 là các sai số đo của các gia tốc kế tương ứng, T - bước thời a) Z k  [z1 z2 z3 z4 z5 z6 ]T  gian rời rạc hóa phương trình vi phân của   h( X k )   k hệ (23). Khi đó sẽ có các phương trình sau:  (28) b) h  [h1 ( X k ) h 2 ( X k ) h 3 ( X k ) ( A1 y  A3 z  A1z  A4 y ) / 2r  h ( X ) h ( X ) h ( X )]T  4 k 5 k 2 k ( A1 y  A3 z  A1z  A4 y )T / 2r (24a) Trong đó các hàm zi  hi ( X k ) là 6 (1 y   3 z  1z   4 y )T / 2r phương trình (từ phương trình thứ 4 đến 24 Số 22
TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ NĂNG LƯỢNG - TRƯỜNG ĐẠI HỌC ĐIỆN LỰC (ISSN: 1859 - 4557) phương trình thứ 9) của hệ (21) được cụ thuật lọc Kalman. Các ma trận G (ma trận thể như sau: cường độ nhiễu), Q (hiệp phương sai hệ động học), R (hiệp phương sai quan sát)  zˆ1  h1 ( X k )   x2 (k )  xˆ12 (k )  được xác định trên cơ sở hệ phương trình  zˆ2  h2 ( X k )   y (k )  xˆ2 (k ) 2 2 (25), (26), (27), (28) và các thông tin về   zˆ3  h3 ( X k )   z (k )  xˆ3 (k ) phương sai của các gia tốc kế do hãng sản 2 2  zˆ4  h4 ( X k )   x (k ) y (k ) xuất sản phẩm cung cấp trong tài liệu bán  sản phẩm. Vấn đề này được giải quyết  xˆ1 (k ) xˆ2 (k ) (29)  zˆ  h ( X )   (k ) (k ) như trong công trình [1].  5 5 k x z Mấu chốt vấn đề được đề cập trong bài  xˆ1 (k ) xˆ3 (k )  báo này là đã xây dựng được thuật toán  zˆ6  h6 ( X k )   y (k )z (k ) xác định tốc độ góc của vật thể chuyển  xˆ (k ) xˆ (k )  2 3 động từ hệ thuần túy các gia tốc kế, không có các con quay đo tốc độ góc, nên tránh Hệ phương trình (29) chính là hệ phương được hiệu ứng trôi không. trình tham chiếu trong giải thuật lọc Kalman. Trên các hình 3, hình 4 và hình 5 là kết quả mô phỏng thuật toán đã trình bày. Ở Để có thông tin đo lường các thông số các hình này đường liền nét là các tốc độ tham chiếu trong giải thuật lọc Kalman, ta góc giả định, còn đường màu xanh là kết thực hiện đọc các chỉ số các gia tốc kế và quả lọc Kalman từ các thông tin giả gia sau đó thực hiện các phép tính số học theo tốc kế. Các tốc độ góc giả định ở đây là các biểu thức ở vế phải của 6 phương các hàm sau: trình cuối hệ (21), tức là: x (t )  0.005t  0.1;  y (t )  0.1sin(0.1t );  z1  ( A4 z  A1z  A2 x  A1x  A3 y   2.6 khi t  8000(ms)  A1 y ) / 2r z (t )    2.6  0.006(t  8) khi t 8000(ms)  z2  ( A1z  A4 z  A2 x  A1x  A3 y   A1 y ) / 2r (30)   z3  ( A2 x  A1x  A1z  A4 z  A3 y   A1 y ) / 2r   z4  ( A3 x  A1x  A1 y  A2 y ) / 2r  z  ( A  A  A  A ) / 2r  5 4x 1x 2z 1z  z6  ( A3 z  A1z  A1 y  A4 y ) / 2r  ⌢ ⌢ ⌢ Ở đây Aix , Aiy , Aiz ,i =1,2,3,4 là các số đo đọc được của các gia tốc kế ở các thời điểm gián đoạn thứ k . Hình 3. Đồ thị vận tốc góc  x từ kết quả lọc Như vậy có đủ thông tin để thực hiện giải Kalman hệ đo gia tốc kế Số 22 25
TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ NĂNG LƯỢNG - TRƯỜNG ĐẠI HỌC ĐIỆN LỰC (ISSN: 1859 - 4557) Hình 4. Đồ thị vận tốc góc y từ kết quả lọc Kalman hệ đo gia tốc kế Hình 5. Đồ thị vận tốc góc z từ kết quả lọc Kalman hệ đo gia tốc kế 3. KẾT LUẬN bày có tính đúng đắn và có giá trị sử Qua kết quả mô phỏng cho thấy tất cả các dụng. Việc sử dụng hệ đa gia tốc kế tránh trường hợp các tốc độ góc quay quanh các không phải sử dụng các con quay đo tốc trục, được tính từ giải pháp lọc Kalman độ góc (luôn có độ trôi không) và không trên cơ sở xử lý thông tin đo được từ hệ phải dùng các thông tin từ ngoài vật thể gia tốc kế, trùng với các tốc độ quay đặt. (như thông tin từ hệ định vị vệ tinh). Như vậy giải pháp và thuật toán đã trình 26 Số 22
TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ NĂNG LƯỢNG - TRƯỜNG ĐẠI HỌC ĐIỆN LỰC (ISSN: 1859 - 4557) TÀI LIỆU THAM KHẢO [1] Nguyễn Hoàng Minh, Trần Đức Thuận, Nguyễn Sỹ Long, Nguyễn Việt Hưng. “Xây dựng phương pháp và thuật toán xác định vận tốc góc bằng hệ đa gia tốc kết hợp con quay vận tốc góc”, Tạp chí Nghiên cứu khoa học và công nghệ quân sự, Viện Khoa học và Công nghệ quân sự, số 34, 12-2014, tr 3-13. [2] Фильтрация и Стохастическое Управление в Динамических Системах. Под Редакциёй К Т Леондеса, перевод с английского. Издательство "Мир" Москва 1980. [3] В.В. Матвеев, В.Я. Распопов. «Основы Построения Бесплатформенных Инерциальных Навигационных Систем» - СПб: ГНЦ РФ ОАО «Концерн «ЦНИИ «Электроприбор», 2009. [4] Salychev O.S (1998) “Inertial Systems in Navigation and Geophysics”, Bauman MSTU Press, Moscow. Giới thiệu tác giả: Tác giả Nguyễn Văn Diên tốt nghiệp đại học chuyên ngành sư phạm kỹ thuật; nhận bằng Thạc sĩ chuyên ngành cơ điện tử tại Đại học Tổng hợp Leibniz Hannover - Đức năm 2007. Hiện nay tác giả là giảng viên Bộ môn Cơ điện tử - Trường Đại học Sư phạm kỹ thuật Hưng Yên. Từ năm 2012 là nghiên cứu sinh Viện Khoa học và Công nghệ quân sự. Lĩnh vực nghiên cứu: điều khiển và đồng bộ hóa các cơ cấu dịch chuyển. Tác giả Đặng Tiến Trung nhận bằng tốt nghiệp đại học chuyên ngành kỹ sư điện - tự động hóa tại Trường Đại học Bách khoa Hà Nội năm 2004, bảo vệ luận án Tiến sĩ năm 2019 tại Học viện Kỹ thuật quân sự. Hiện nay tác giả là giảng viên Khoa Kỹ thuật điện - Trường Đại học Điện lực. Lĩnh vực nghiên cứu: ứng dụng các giải pháp điều khiển hiện đại trong hệ thống điện. Số 22 27
TẠP CHÍ KHOA HỌC VÀ CÔNG NGHỆ NĂNG LƯỢNG - TRƯỜNG ĐẠI HỌC ĐIỆN LỰC (ISSN: 1859 - 4557) 28 Số 22

CÓ THỂ BẠN MUỐN DOWNLOAD

THÔNG TIN

TRỢ GIÚP

HỖ TRỢ KHÁCH HÀNG

Theo dõi chúng tôi

Chịu trách nhiệm nội dung:

Nguyễn Công Hà - Giám đốc Công ty TNHH TÀI LIỆU TRỰC TUYẾN VI NA

LIÊN HỆ

Địa chỉ: P402, 54A Nơ Trang Long, Phường 14, Q.Bình Thạnh, TP.HCM

Hotline: 093 303 0098

Email: support@tailieu.vn

Giấy phép Mạng Xã Hội số: 670/GP-BTTTT cấp ngày 30/11/2015 Copyright © 2022-2032 TaiLieu.VN. All rights reserved.