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ế

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ể.

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 TỐ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 sở tốn học cho việc sử dụng thông tin đo từ hệ gia tốc kế để xác định tốc độ quay vật thể chuyển động Từ tốc độ quay xác định góc tư vật thể Giải pháp toán xây dựng phương trình mơ tả quan hệ tốc độ quay với gia tốc điểm khác 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 vật Kết xác định tư vị trí vật thể khơng gian (giải tố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 MỞ ĐẦU Vấn đề dẫn đường cho vật thể chuyển động chất xác định vị trí, vận tốc tâm khối tư so với hệ tọa độ chọn làm hệ tọa độ dẫn đường Việc sử dụng phương tiện đo quay đo tốc độ góc, gia tốc kế thông tin định vị vệ tinh nhiều 20 cơng trình đề cập có nhiều ứng dụng thực hóa Do quay đo tốc độ góc ln có độ trơi khơng (có giá trị phát cho dù vật thể không quay) độ trơi biến đổi q trình hoạt động, thực tế phải có hệ đo khác để khắc phục hiệu ứng sai số tính trơi khơng 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 kết hợp quay đo tốc độ góc với hệ đa gia tốc kế để khắc phục tượng trôi không Trong báo này, nhóm tác giả đề xuất giải pháp sử dụng hệ đa gia tốc kế (không sử dụng quay thông tin định vị vệ tinh) để giải vấn đề dẫn đường Vấn đề thực có ý nghĩa thực tiễn xây dựng thiết bị dẫn đường cho phương tiện ngầm, phương tiện chuyển động nhanh mà khơng thể có thơng tin từ hệ định vị vệ tinh tốc độ quay sau: THIẾT KẾ THUẬT TỐN XÁC ĐỊNH THƠNG TIN VỀ TỐC ĐỘ QUAY GĨC TỪ THƠNG TIN HỆ ĐA GIA TỐC KẾ bày việc xác định x ,  y , z nhờ hệ đa gia Vì gia tốc kế gắn trực tiếp vật thể chuyển động, nên số gia tốc kế đưa thành phần gia tốc điểm gắn gia tốc kế đo hệ tọa độ gắn liền với vật thể (hệ tọa độ liên kết) Để xác định tham số chuyển động tâm khối vật thể so với hệ tọa độ dẫn đườ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  x0   xl  y   Ay   0  l  z0   zl  (1) Trong A gọi ma trận cosin định hướng, có cấu trúc [2, 3] Việc xác định quan hệ hệ tọa độ dẫn đường hệ tọa độ liên kết việc xác định ma trận A ba góc  , ,  (góc tư [3]) Theo [2, 3] có thơng tin tốc độ quay vật thể xung quanh trục hệ tọa độ liên kết x ,  y , z có quan hệ phần tử ma trận A với Số 22 c11  c12 z  c13 y , c12  c13 x  c11 z , c13  c11 y  c12 x , c21  c22 z  c23 y , c22  c23 x  c21 z , c23  c21 y  c22 x (2) c31  c32 z  c33 y , c32  c33 x  c31 z , c33  c31 y  c32 x Trong (2), cij phần tử ma trận A Như để xác định tư vật trình động phải có thơng tin tốc độ quay x ,  y , z Sau trình tốc kế gắn vật thể chuyển động Hình Chuyển động phức hợp vật rắn lý tưởng Để hiểu rõ chất việc xác định tốc độ góc sở hệ đa gia tốc kế, ta xem xét chuyển động vật rắn lý tưởng hình Giả sử vật rắn chuyển động phức hợp có gia tốc góc , vận tốc góc , gia tốc dài Khi phương trình mô tả gia tốc điểm P vật rắn sau [1,4]: (3) đó, gia tốc điểm P O; gia tốc hướng tâm P; gia tốc tiếp tuyến P; O tâm khối vật thể 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 [4]: (4) (5) vectơ nối điểm P với điểm O Từ (3), (4), (5) suy ra: (6) Theo [4], áp dụng công thức nhân vectơ có hướng ta có: rz  ry  x   rP   rz rx  y ry  rx z rz  ry  x   rz rx  y  rx z ry (7)   (  rPO )   rx  rx ry rz  ry  ry rx  rz  rz 0 rx  x2  y2  z2 (8) rz  ry x y  x z  y z Xem xét mơ hình hệ cảm biến gồm hệ gia tốc kế trục hình 2, hệ gia tốc kế số đặt tâm khối vật thể chuyển động Còn ba hệ gia tốc kế cịn lại đặt vị trí trục hệ tọa độ gắn liền với vật thể chuyển động Hệ tọa độ OXYZ hệ tọa độ liên kết vật thể chuyển động, tất cảm biến có trục nhạy song song với trục tương ứng hệ tọa độ liên kết 22 Hình Mơ hình hệ gia tốc kế trục Vì hệ gia tốc kế số nằm tâm khối vật thể, nên số gia tốc chuyển động dài, tức là: (9) [A1x A1 y A1z ]T  [ax0 a 0y a 0z ]T đó, A1x ; A1 y ; A1z giá trị lý tưởng hệ gia tốc kế số 1; ax0 ;a0y ;az0 gia tốc dài vật thể Hệ gia tốc kế số nằm cách tâm khối O khoảng cách r vị trí: r2 x  r; r2 y  0; r2 z  (10) Từ công thức (3), (7), (8) ta có số gia tốc kế số sau: A2 x ax0 r ( y2  z2 ) A2 y  a 0y  rz  rx y A2 z az0 r y rxz (11) A2 x ; A2 y ; A2 z giá trị lý tưởng hệ gia tốc kế số 2, x ;  y ; z vận tốc góc vật thể; gia tốc góc vật thể Hệ gia tốc kế số nằm cách tâm khối O khoảng cách: r3 x  0; r3 y  r; r3 z  (12) Từ cơng thức (3), (7), (8) ta có số gia 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ố sau: Đặt A3 x ax0 rz rx y A3 y  a y   r (x2  z2 ) A3 z az0 rx r yz (13) A3 x ; A3 y ; A3 z giá trị lý tưởng (19) Với cách đặt biến (19) từ phương trình (16), (17), (18) có hệ phương trình: hệ gia tốc kế số Hệ gia tốc kế số nằm cách tâm khối O khoảng cách: r4 x  0; r4 y  0; r4 z  r (14) Từ công thức (3), (7), (8) ta có số gia tốc kế số sau: A4 x ax0 r y rxz A4 y  a y  rx  r yz A4 z az r (x2   y2 ) (15) A4 x ; A4 y ; A4 z giá trị hệ gia tốc kế số Trừ hai vế phương trình (11) với hai vế phương trình (9) nhận được: A2 x  A1x r (   ) A2 y  A1 y  rz  rx y A2 z  A1z r y rxz y z (16) Trừ hai vế phương trình (13) với hai vế phương trình (9) nhận được: A3 x  A1x rz rx y A3 y  A1 y   r (x2  z2 ) A3 z  A1z rx r yz (17) Trừ hai vế phương trình (15) với hai vế phương trình (9) nhận được: A4 x  A1x r y rxz A4 y  A1 y  rx  r yz A4 z  A1z r (x2   y2 ) Số 22 m1   x ; m2   y ; m3   z ;  2 m4   x ; m5   y ; m6   z ;  m7   x y ; m8   x z ; m    ; y z  (18) 1) m5  m6  ( A2 x  A1x ) / r  2)  m7  m3  ( A2 y  A1 y ) / r  3)  m8  m2  ( A2 z  A1z ) / r 4) m7  m3  ( A3 x  A1x ) / r  5)  m4  m6  ( A3 y  A1 y ) / r  6) m9  m1  ( A3 z  A1z ) / r 7) m8  m2  ( A4 x  A1x ) / r  8) m9  m1  ( A4 y  A1 y ) / r  9)  m4  m5  ( A4 z  A1z ) / r (20) Hệ phương trình tuyến tính (20) hệ tuyến tính với ẩn số mi ; i  Tiến hành giải hệ nhận được: 1) m1   x  ( A1 y  A3 z  A1z  A4 y ) / 2r  2) m2   y  ( A2 z  A4 x  A1x  A1z ) / 2r 3) m    ( A  A  A  A ) / 2r z 1x 1y 2y 3x  4) m4   x  ( A4 z  A1z  A2 x  A1x  A3 y   A1 y ) / 2r  5) m5   y  ( A1z  A4 z  A2 x  A1x  A3 y   A1 y ) / 2r  6) m6   z  ( A2 x  A1x  A1z  A4 z  A3 y   A ) / 2r  1y 7) m7   x y  ( A3 x  A1x  A1 y  A2 y ) / 2r  8) m8   x z  ( A4 x  A1x  A2 z  A1z ) / 2r 9) m     ( A  A  A  A ) / 2r y z 3z 1z 1y 4y  (21) Để xác định vận tốc góc x ,  y , z vật 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 thuật lọc Kalman để xác định vận tốc góc x ,  y , z (bộ lọc Kalman cơng cụ tốn học giúp cho việc đánh giá vectơ trạng thái hệ động học sở thông tin quan sát vectơ tín hiệu đầu [2]) Để ứng dụng giải thuật Kalman vấn đề xác định x ,  y , z tiến hành xác lập trạng thái X cần đánh giá x ,  y , z , tức là: X  [x1 , x2 , x3 ]T  [x ,  y , z ]T (22) Từ ba phương trình (1, 2, 3) hệ (21) có: ( x (k )  k (k  1)) / T   x  ( A2 z  A4 x  A1x  A1z ) / 2r  ( A2 z  A4 x  A1x  A1z )T / 2r (24b) ( z   x  1x  1z )T / 2r ( A1x  A1 y  A2 y  A3 x ) / 2r  ( A1x  A1 y  A2 y  A3 x )T / 2r (24c) (1x  1 y   y   x )T / 2r Với đặt biến (22) từ ba phương trình hệ (24a), (24b), (24c) nhận được: x1 (k )  x1 (k  1)  ( A1 y  A3 z  A1z  A4 y )T / (25) 2r  (1 y   z  1z   y )T / 2r x2 (k  1)  x2 (k ) ( A2 z  A4 x  A1x  A1z )T  ( A1 y  A3 z  A1z  A4 y ) / 2r ( y (k )   y (k  1)) / T   y ( A2 z  A4 z  A1x  A1z ) (26) /2r  ( z   x  1x  1z )T / 2r (23) x3 (k )  x3 (k  1) ( z (k )   z ( k  1)) / T   z ( A1x  A1 y  A2 y  A3 x )T /  ( A1x  A1 y  A2 y  A3 x ) / 2r 2r  (1x  1 y   y   x )T / 2r Trong phương trình hệ (23) hệ (21) ký hiệu Aix , Aiy , Aiz , i =1, 2, 3, Phương trình (25), (26), (27) phương trình động học thuật tốn lọc Kalman thành phần gia tốc thực chiếu trục Ta ký hiệu số đo đọc từ gia tốc kế trục đo tương ứng, ký hiệu  ix , iy , iz , i  1, 2,3, sai số đo gia tốc kế tương ứng, T - bước thời gian rời rạc hóa phương trình vi phân hệ (23) Khi có phương trình sau: ( A1 y  A3 z  A1z  A4 y ) / 2r  ( A1 y  A3 z  A1z  A4 y )T / 2r (1 y   z  1z   y )T / 2r 24 (24a) (27) Từ phương trình (từ phương trình thứ đến phương trình thứ 9) hệ (21) nhận hệ phương trình tham chiếu sau: a) Z k  [z1 z2 z3 z4 z5 z6 ]T    h( X k )   k  b) h  [h1 ( X k ) h ( X k ) h ( X k ) h ( X ) h ( X ) h ( X )]T  k k k Trong hàm (28) zi  hi ( X k ) phương trình (từ phương trình thứ đến 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) hệ (21) cụ thể sau:  zˆ1  h1 ( X k )   x2 (k )  xˆ12 (k )  2  zˆ2  h2 ( X k )   y (k )  xˆ2 (k )  2  zˆ3  h3 ( X k )   z (k )  xˆ3 (k )  zˆ4  h4 ( X k )   x (k ) y (k )   xˆ1 (k ) xˆ2 (k )  zˆ  h ( X )   (k ) (k ) k x z   xˆ1 (k ) xˆ3 (k )   zˆ6  h6 ( X k )   y (k )z (k )  xˆ (k ) xˆ (k )  (29) Hệ phương trình (29) hệ phương trình tham chiếu giải thuật lọc Kalman Để có thơng tin đo lường thơng số tham chiếu giải thuật lọc Kalman, ta thực đọc số gia tốc kế sau thực phép tính số học theo biểu thức vế phải phương trình cuối hệ (21), tức là:  z1  ( A4 z  A1z  A2 x  A1x  A3 y   A1 y ) / 2r   z2  ( A1z  A4 z  A2 x  A1x  A3 y   A1 y ) / 2r   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 4x 1x 2z 1z   z6  ( A3 z  A1z  A1 y  A4 y ) / 2r  thuật lọc Kalman Các ma trận G (ma trận cường độ nhiễu), Q (hiệp phương sai hệ động học), R (hiệp phương sai quan sát) xác định sở hệ phương trình (25), (26), (27), (28) thông tin phương sai gia tốc kế hãng sản xuất sản phẩm cung cấp tài liệu bán sản phẩm Vấn đề giải cơng trình [1] Mấu chốt vấn đề đề cập báo xây dựng thuật tốn xác định tốc độ góc vật thể chuyển động từ hệ túy gia tốc kế, khơng có quay đo tốc độ góc, nên tránh hiệu ứng trơi khơng Trên hình 3, hình hình kết mơ thuật tốn trình bày Ở hình đường liền nét tốc độ góc giả định, đường màu xanh kết lọc Kalman từ thông tin giả gia tốc kế Các tốc độ góc giả định hàm sau: x (t )  0.005t  0.1;  y (t )  0.1sin(0.1t );  2.6 t  8000(ms) z (t )   2.6  0.006(t  8) t 8000(ms) (30) ⌢ ⌢ ⌢ Ở Aix , Aiy , Aiz ,i =1,2,3,4 số đo đọc gia tốc kế thời điểm gián đoạn thứ k Như có đủ thơng tin để thực giải Số 22 Hình Đồ thị vận tốc góc  x từ kết lọc Kalman hệ đo gia tốc kế 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 Đồ thị vận tốc góc y từ kết lọc Kalman hệ đo gia tốc kế Hình Đồ thị vận tốc góc z từ kết lọc Kalman hệ đo gia tốc kế KẾT LUẬN Qua kết mô cho thấy tất trường hợp tốc độ góc quay quanh trục, tính từ giải pháp lọc Kalman sở xử lý thông tin đo từ hệ gia tốc kế, trùng với tốc độ quay đặt Như giải pháp thuật tốn trình 26 bày có tính đắn có giá trị sử dụng Việc sử dụng hệ đa gia tốc kế tránh sử dụng quay đo tốc độ góc (ln có độ trơi khơng) khơng phải dùng thơng tin từ ngồi vật thể (như thông tin từ hệ định vị vệ tinh) 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 thuật toán xác định vận tốc góc hệ đa gia tốc kết hợp quay vận tốc góc”, Tạp chí Nghiên cứu khoa học công nghệ quân sự, Viện Khoa học 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 Thạc sĩ chuyên ngành điện tử Đại học Tổng hợp Leibniz Hannover - Đức năm 2007 Hiện tác giả 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 nghiên cứu sinh Viện Khoa học Công nghệ quân Lĩnh vực nghiên cứu: điều khiển đồng hóa cấu dịch chuyển Tác giả Đặng Tiến Trung nhận tốt nghiệp đại học chuyên ngành kỹ sư điện tự động hóa 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 Học viện Kỹ thuật quân Hiện tác giả 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 giải pháp điều khiển đại 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 ... nhờ hệ đa gia Vì gia tốc kế gắn trực tiếp vật thể chuyển động, nên số gia tốc kế đưa thành phần gia tốc điểm gắn gia tốc kế đo hệ tọa độ gắn liền với vật thể (hệ tọa độ liên kết) Để xác định. .. hình hệ cảm biến gồm hệ gia tốc kế trục hình 2, hệ gia tốc kế số đặt tâm khối vật thể chuyển động Còn ba hệ gia tốc kế lại đặt vị trí trục hệ tọa độ gắn liền với vật thể chuyển động Hệ tọa độ OXYZ... định tốc độ góc sở hệ đa gia tốc kế, ta xem xét chuyển động vật rắn lý tưởng hình Giả sử vật rắn chuyển động phức hợp có gia tốc góc , vận tốc góc , gia tốc dài Khi phương trình mô tả gia tốc điểm