Nghiên cứu, xây dựng thuật toán khảo sát sai số của hệ thống dẫn đường quán tính có đế với các cảm biến định hướng tự do theo phương vị

Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số Kỷ niệm 55 năm Viện KHCNQS, 10 - 2015 73 NGHIÊN CỨU, XÂY DỰNG THUẬT TOÁN KHẢO SÁT SAI SỐ CỦA HỆ THỐNG DẪN ĐƯỜNG QUÁN TÍNH CÓ ĐẾ VỚI CÁC CẢM BIẾN ĐỊNH HƯỚNG TỰ DO THEO PHƯƠNG VỊ Lê Tuấn Anh1*, Phan Tương Lai2, Hoàng Thế Khanh1, Nguyễn Quang Vịnh1 Tóm tắt: Để tăng độ chính xác của hệ thống dẫn đường quán tính (HTDĐQT) cần thiết phải nắm rõ và xác định được mô hình các sai số tác động lên hệ thống như các sai số về vị trí, góc, vận tốc, độ trôi con quay, độ trôi gia tốc kế,...từ đó xây dựng các bộ lọc để giảm thiểu và loại bỏ sai số làm ảnh hưởng đến khả năng hoạt động của hệ thống. Bài báo này tập trung vào nghiên cứu, xây dựng mô hình toán và xác định các sai số của HTDĐQT có đế theo thời gian và xây dựng chương trình mô phỏng các sai số này để từ đó hiểu rõ bản chất các nhiễu tác động và đưa ra các thuật toán lọc phù hợp để loại bỏ hoặc giảm thiểu chúng nhằm nâng cao tốt nhất hoạt động của HTDĐQT. Từ khóa: HTDĐQT - Hệ thống dẫn đường quán tính, MCH - Ma trận cosin chỉ hướng, CQOĐĐ – Con quay ổn định đế. 1. MỞ ĐẦU Hệ thống dẫn đường quán tính đóng vai trò quan trọng trong các bài toán điều khiển chuyển động theo quỹ đạo mong muốn được ứng dụng rộng khắp trong ngành công nghiệp quốc phòng và dân sự như tên lửa, máy bay, hàng hảiViệc xác định chính xác nhất tọa độ của đối tượng chuyển động trong không gian là mục tiêu của bài toán xây dựng hệ thống dẫn đường quán tính. Để xây dựng các thuật toán lọc Kalman nhằm khử các sai số phát sinh trong quá trình hệ thống hoạt động thực tế thì việc nghiên cứu bản chất toán học và xây dựng mô hình các sai số có thể ảnh hướng đến kết quả đo của HTDĐQT là thực sự cần thiết và cần được giải quyết triệt để, khoa học. Bài báo này tập trung vào xây dựng các mô hình toán học các sai số có thể ảnh hưởng đến chất lượng của hệ thống và sử dụng các công cụ toán học, lập trình để mô phỏng các sai số tác động lên hệ thống để từ đó nắm vững được bản chất gây ra sai số của hệ thống và lựa chọn thuật toán lọc, loại trừ nhiễu hiệu quả. 2. ĐẶT BÀI TOÁN Để xây dựng các phương trình toán học mô tả các sai số của hệ thống dẫn đường quán tính ta đưa ra các hệ tọa trục tọa độ liên quan đến các đại lượng này. Các hệ tọa độ được sử dụng trong bài báo gồm: - Hệ tọa độ địa lý cơ sở O , trụcO hướng về phía đông, trụcO hướng về phía bắc, trụcO theo phương thẳng đứng hợp thành tam diện thuận [1,4]. - Hệ tọa độ liên kết với thân đối tượng c c cOx y z , trục cOx hướng theo phương vuông góc trục dọc đối tượng [4]. - Ngoài ra còn sử dụng hệ tọa độ đế Oxyz, hệ tọa độ tự do liên kết với đế theo phương vị c c cO   [4]. - Vị trí liên kết hệ tọa độ đế Oxyz phải lưu vị trí của hệ tọa độ tự do theo phương vị c c cO   - Vị trí góc hệ tọa độ c c cO   so với hệ tọa độ O được xác định bởi góc ε thay đổi liên tục theo thời gian (xem hình vẽ 1). Tên lửa & Thiết bị bay L.T.Anh, P.T.Lai, H.T.Khanh, N.Q.Vịnh,"Nghiên cứu, xây dựng thuật toán theo phương vị.” 74 Hình 1. Các hệ tọa độ và các góc động học của hệ thống. Vị trí ban đầu của hệ tọa độ đế Oxyz trùng với hệ tọa độ Oξc ηc ζc. Vận tốc tuyệt đối chuyển động của đối tượng khi chiếu lên trục Ox ký hiệu là V1, Oy là V2; gia tốc chuyển động trên 2 trục đó là W1 và W2. Thuật toán của nghiên cứu sẽ tương ứng với sơ đồ cấu trúc HTDDQT (xem hình 3). Từ sơ đồ cấu trúc đó, ta tìm được các hình chiếu vận tốc tuyệt đối của đối tượng: 1 1 10 2 2 20 0 0 ; t t V W dt V V W dt V     (1) với V10 và V20 là các giá trị vận tốc ban đầu. Mặt khác, ta luôn có: . . 1 2 1 1 2 2; dV dV V W V W dt dt     Nếu biết trước các giá trị của vận tốc tuyệt đối V1 và V2 của chuyển động ta có thể tìm được các thành phần vận tốc theo phương bắc và đông: 1 2 1 2cos sin ; sin cosE NV V V V V V       (2) Giả thiết thành phần vận tốc theo phương thẳng đứng bằng không, Khi đó các vận tốc trên được xác định bởi hình chiếu tốc độ góc tuyệt đối đế con quay ổn định lên các trục hệ tọa độ địa lý: 2 1 ;N Ex V V R R      (3) trong đó: 2 21 2 2 1 1 sin 21 sin a R h a e h e             (4) là bán kính cong trái đất theo hướng tiếp tuyến với mặt phẳng song song với mặt phẳng xích đạo. Bán kính cong chính trong mặt phẳng kinh tuyến R2 được tính theo công thức:     2 2 2 2 2 3/22 2 1 3 1 sin 21 sin e R a h a e e h e               (5) Hình chiếu của vận tốc góc con quay đế ổn định lên các trục của nó sẽ trùng với hình chiếu vận tốc góc của tam diện vuông tự do theo phương vị [4], nghĩa là: Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số Kỷ niệm 55 năm Viện KHCNQS, 10 - 2015 75 2 1 2 1 cos sin cos sin sin cos sin cos N E c N E c V V R R V V R R                               (6) Góc quay của đế tính theo công thức: 1 tanE V R          (7) Sau khi thực hiện biến đổi, ta thu được: 2 2 2 2 2 1 1 1 cos cos cos ; sinc N c N V e V e V V R a R a                         (8) Như vậy, cần phải quay đế ứng với các vận tốc góc này để nó trở về phương nằm ngang. Tốc độ thay đổi vĩ độ và kinh độ tuyệt đối tính theo công thức sau: 2 1 ; ; cos N EV V R R       (9) Sau khi lấy tích phân và biến đổi ta thu được kinh độ tương đối và thành phần vận tốc tuyến tính theo hướng đông: 1; cosE Eut V R u       (10) trong đó, u – vận tốc góc quay quanh trục của trái đất, Λ và VE là các giá trị ban đầu. THIẾT LẬP MÔ HÌNH TOÁN CÁC SAI SỐ CỦA HTDĐQT CÓ ĐẾ Từ cơ sở trên ta xây dựng phương trình khảo sát sai số HTDĐQT trong trường hợp lý tưởng. Để thuận tiện tính toán với các biến đổi toán học giữa các hệ trục tọa độ ta xây dựng ma trận cosin chỉ hướng các góc giữa các hệ trục tọa độ c c cO   và Oxyz và có tính đến các góc nhỏ i để xác định được sai số định hướng của con quay ổn định đế. Ma trận cosin chỉ hướng được viết như sau: Hình 2. Mối liên hệ giữa các góc quay của đế giữa hai hệ trục tọa độ Oxyz và .c c cO   Bảng 1. Ma trận cosin chỉ hướng giữa các hệ trục tọa độ c c cO   và .Oxyz c c c x 1 3 2 y 3 1 1 z 2 1 1 Khi kể đến các sai số i thì hình chiếu tốc độ góc của tam diện c c cO   lên trục của đế có thể viết lại khi coi 0c  . Để xác định được các giá trị này cần giả thiết rằng vận Tên lửa & Thiết bị bay L.T.Anh, P.T.Lai, H.T.Khanh, N.Q.Vịnh,"Nghiên cứu, xây dựng thuật toán theo phương vị.” 76 tốc góc tuyệt đối của CQĐOĐ bằng tổng của vận tốc tương đối và vận tốc dịch chuyển sinh ra khi hai hệ trục tọa độ chuyển động tương đối với nhau. Ta viết hình chiếu vận tốc góc tuyệt đối của CQĐOĐ lên các trục của nó như sau [5,6]: 1 2 3; ;x n y n z n                    (11) Mặt khác, các thành phần tốc độ góc tuyệt đối của CQĐOĐ được xác định bằng các mô-ment hiệu chỉnh tác động lên nó và độ trôi riêng của con quay: 2 3 3 3 2 1 1 ; ; ; ; n c c n c c n c c yx z x x y n y z z MM M H H H                                                           (12) trong đó, Mi (i=x,y,z) là các mô ment hiệu chỉnh để xác định chuyển động của trục thứ I; i là các độ trôi của đế theo trục thứ I tương ứng; H là mô-men động học của con quay. Mô-men hiệu chỉnh Mi có thể viết theo các vận tốc góc tương ứng ' , 'c c   : 1 2' ; 'x c y cM K M K    (13) trong đó: Ki là độ dốc đặc trưng của các kênh hiệu chỉnh. Khi đó, ta viết lại các tốc độ góc , ,x y z   dưới dạng: 1 2' ; ' ;x c x y c y z z K K H H              (14) Ký hiệu i k K h H  , dễ thấy để đồng nhất (13) và (14) (nếu như không tính đến độ trôi con quay) thì cần phải coi hk=1. Ta đưa ra ký hiệu sai số tương đối của kênh điều khiển là kh , khi đó 1k kh h  Đồng nhất các thành phần vế phải đối với các phương trình của 2 hệ thống các vận tốc góc viết ở trên, ta thu được phương trình chuyển động của con quay ổn định đế [5,6]:     1 3 2 3 3 1 2 1 ' 1 ' ; c c k c x c c k c y c c z h h                                              (15) Viết hình chiếu gia tốc chuyển động của đối tượng lên các trục hệ tọa độ đế dưới dạng: 2 2 2 2 2 1 1 2 2 1 , ,cc N E c c c c c c c V V V V V W R V W R W R R R R                         (16) Gia tốc biểu kiến được đo bằng các cảm biến có thể biểu diễn dưới dạng: 2 2 2 1 1 1 2 1 1 3 2 2 0 2 2 1 sin cos sin sin cos sin N E W V R u V R u R u V V g R R                          (17) Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số Kỷ niệm 55 năm Viện KHCNQS, 10 - 2015 77 2 2 2 1 1 1 3 2 2 2 2 1 3 0 1 2 1 sin cos sin sin cos sin N E W V R u R u V V V R u g R R                            trong đó, g0 là gia tốc trọng trường. Nếu đưa vào sai số do sự sai lệch giữa gia tốc trọng trường và gia tốc địa lý theo phương thẳng đứng, nhận được biểu thức:     ' 2 2 1 21 3 1 3 2 2 ' 2 0 2 1 2 1 ' 2 2 2 12 3 1 3 2 2 ' 2 0 1 1 2 1 sin cos sin cos sin 'cos 'sin ' sin cos cos sin sin 'cos 'sin ' N E N E W V V R u V V g R u R R W V V R u V V g R u R R                                                     Khi tính đến sai số dụng cụ của các cảm biến và các bộ tích phân, ta viết đạo hàm của các vận tốc tuyệt đối như sau:         ' ' ' 2 1 1 1 ' ' ' 2 2 2 1 1 sin 'cos 'sin ' 1 1 sin 'cos 'sin ' 1 a k a k V W h a R u h V V W h a R u h V                                   (18) trong đó, ,a kh h  là các hệ số tỉ lệ bất ổn định tương ứng với cảm biến và bộ tích phân, a là độ dịch không của cảm biến, V  là độ dịch không của bộ tích phân. Các bán kính cong cơ bản R1 và R2 tính dưới dạng: ' 2 2 ' 2 2 2 1 2 1 3 1 sin ' ; 1 sin ' 2 2 R a e R a e e                  (19) trong đó, ' là giá trị vĩ độ thiết bị đo. Các giá trị thiết bị đo ' ',E NV V trên cơ sở thuật toán hoạt động của hệ thống lý tưởng là: ' ' ' ' ' ' 1 2 2 2cos ' sin ', sin ' cos 'E NV V V V V V       (20) Hình chiếu vận tốc góc của tam diện địa lý lên các trục của nó là: ' ' ' ' ' ' 2 1 ;N E V V R R      (21) Các giá trị đo vĩ độ ' , kinh độ ' và góc của đế con quay ' kể đến các sai số dụng cụ và tích phân xác định bởi hệ phương trình vi phân dưới đây:       ' ' ' ' 1 , ' sec ' 1 , ' tan ' 1 , ' ' h h h ut                                             (22) Tên lửa & Thiết bị bay L.T.Anh, P.T.Lai, H.T.Khanh, N.Q.Vịnh,"Nghiên cứu, xây dựng thuật toán theo phương vị.” 78 với , ,h h h     là các hệ số tỉ lệ không ổn định của bộ tích phân ứng với các góc , ,   ; , ,    là độ dịch không của các bộ tích phân tương ứng. Các giá trị đo hình chiếu vận tốc góc tam của tam diện thuận có thể xác định bởi thế vào biểu thức của ,c c   các tham số giá trị đo. Sơ đồ cấu trúc thuật toán như hình 2: Hình 3. Cấu trúc hệ thống dẫn đường quán tính với các cảm biến tự do theo phương vị. 4. MÔ PHỎNG VÀ ĐÁNH GIÁ 4.1. Kết quả mô phỏng Giả thiết trái đất dạng elippsoid với các tham số như sau: u= 7.2921E-5 [1/s]- vận tốc góc quay quanh trục của trái đất, a=6378245 [m]- bán trục lớn của trái đất, e2=0.006934 – bình phương tiêu cự của ellip, g=9.81 [m/s2]- gia tốc trọng trường, ge=9.780318 và q=3.46775e-3 là các tham số hấp dẫn. Ta xét hai trường hợp với đầu vào được lựa chọn từ các mô hình sai số của hệ thống được đề cập ở trên.  Trường hợp 1: Điều kiện ban đầu của các sai số HTDĐQT:  Giá trị ban đầu của vĩ độ, kinh độ, góc lệch của đế: 0 , 0, 45     [độ]  Giá trị vận tốc và gia tốc ban đầu: 20 0260[ / ], 0[ / ]V m s a m s   Giá trị độ trôi con quay ban đầu: 20, 0, 0, 0.01[ / ]x y z xa m s        Độ dịch không ban đầu: 0.0001, 0, 0, 0, 0xa V          Hệ số bất ổn định tỉ lệ: 0, 0, 0, 0, 0, 0a i kh h h h h h              Trường hợp 2: Thay đổi điều kiện ban đầu của các sai số HTDĐQT: 00 , 0 , 15 , 100[ ], 0, 0, 0, 0 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 o o o o x y z a i k mV a s a V a V h h h h h h                                                 Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số Kỷ niệm 55 năm Viện KHCNQS, 10 - 2015 79 4.2. Kết quả mô phỏng thuật toán và bàn luận Thuật toán và các modules chương trình được viết bằng phần mềm Matlab/GUI + Trường hợp 1: Hình 4. Sai số vận tốc khi hệ thống hoạt động. Hình 5. Các sai số vị trí góc của hệ thống. Hình 6. Các sai số định hướng của đế. Tên lửa & Thiết bị bay L.T.Anh, P.T.Lai, H.T.Khanh, N.Q.Vịnh,"Nghiên cứu, xây dựng thuật toán theo phương vị.” 80 + Trường hợp 2: Hình 7. Sai số vận tốc khi hệ thống hoạt động. Hình 8. Các sai số vị trí góc của hệ thống. Hình 9. Các sai số định hướng của đế. 4.3. Đánh giá kết quả Từ đồ thị hình 6,7 ta thấy rằng nếu hệ thống hoạt động ổn định và chỉ chịu tác động của sai số độ trôi và độ dịch không của gia tốc kế thì các sai số về vận tốc là khá nhỏ (cỡ 0.2%), các sai số vị trí cũng rất nhỏ (sai số vĩ độ lớn nhất khoảng xấp xỉ 1góc phút sau 2 giờ hệ thống làm việc), các sai số góc rất nhỏ (sai số góc lớn nhất khoảng 0.2 góc phút). Như vậy, với thiết lập đầu vào như trường hợp 1 ta đảm bảo hệ thống hoạt động ổn định, chính xác, các sai số rất nhỏ và không ảnh hưởng đến chất lượng hệ thống. Trong hình 7, nếu ta thiết lập điều kiện đầu với các sai lệch về độ trôi gia tốc kế trên các trục, độ trôi con quay, sai số góc...thì các sai số đo tăng lên đáng kể, tuy nhiên sau 10 giờ hệ thống hoạt động thì các sai số này cũng chỉ dao động quanh giá trị 0 mà không bị tích lũy theo thời gian. Từ kết quả mô phỏng mô hình các sai số này cho phép ta xây dựng bộ lọc Kalman để loại bỏ các sai số làm ảnh hưởng đến chất lượng hệ thống. Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số Kỷ niệm 55 năm Viện KHCNQS, 10 - 2015 81 5. KẾT LUẬN - Bài báo đã đề xuất phương pháp nghiên cứu, xây dựng và khảo sát mô hình sai số của HTDĐQT có đế với các cảm biến theo phương vị. Trên cơ sở mô hình toán của các sai số hệ thống, bài báo đã đưa ra thuật toán giải và đưa ra chương trình mô phỏng các sai số tác động lên hệ thống để từ đó cho phép lựa chọn và thiết kế bộ lọc tối ưu loại bỏ hoặc giảm thiểu tối đa các sai số ảnh hưởng đến chất lượng hệ thống. - Bài báo đã đưa ra các mô hình toán sai số và bản chất vật lý gây ra các sai số này, sử dụng các công cụ toán học và lập trình hiện đại để đưa ra thuật toán tính toán và mô phỏng các sai số theo thời gian thực hoạt động của hệ thống. - Những kết quả mô phỏng và mô hình toán học cho phép sử dụng nó để thiết kế thuật toán lọc tối ưu, phi tuyến, mở rộng hay rời rạc hóa... để nâng cao độ chính xác cho HTDĐQT, giảm thiểu và loại bỏ các nhiễu ảnh hưởng đến chất lượng làm việc của hệ thống. TÀI LIỆU THAM KHẢO [1]. Кузовков Н.Т., Салычев О.С. “Инерциальная навигация и оптимальная Фильтрация”. Москва «Машинастроение», 1982 г. [2]. O.S.Salychev. “MEMS-based inertial navigation: Expectations and reality”. Published by the BMSTU Press. Moscow, Russia, 2012. [3]. “Inertial Systems in Navigation and Geophysics”. Bauman MSTU Press, Moscow 1998. [4]. “Инерциальные навигационные системы”. Л.М.Селиванова.Издотельство МГТУ им. Н.Э.Баумана, 2012г. [5]. А.А.Одинцов. “Теория и расчет гироскопических приборов.Учебное пособие”. - Киев: Высшая школа, 1985. - 392 с. [6]. Виниченко Н.Т., Кацай Д.А., Лысова А.А. “Теория гироскопических приборов”. Учебное пособие. – Челябинск: ЮурГу, 2010. - 141 с. [7]. Trần Đức Thuận và cộng sự, “Xây dựng thuật toán xác định tham số định hướng cho phương tiện chuyển động trên cơ sở kết hợp con quay tốc dộc góc với từ kế và gia tốc kế”, Tạp chí Nghiên cứu KH&CNQS, (25),tr 7-16, (2013). [8]. Trần Đức Thuận và cộng sự , “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 KH&CNQS, tr.3-13, số 34, (2014). ABSTRACT RESEARCH AND SERVEY OF INERTIAL NAVIGATION SYSTEM ERRORS MODEL FOR FREE ORIENTATION OF ACCELEROMETERS IN THE AZIMUTH The paper presents a method for improving quality of the inertial navigation system (INS) based on building mathematical model of errors and resolving it. The parallel modeling technique was used to formulate the algorithm for recognizing and evaluating noises, including mutative interferences. The paper also suggests a general approach to build filtering algorithms to eliminate and minimize INS errors in order to accuracy enhancement of the integrated INS. Keywords: Inertial Navigation System (INS), Gyro stabilized platform (GSP). Nhận bài ngày 20 tháng 7 năm 2015 Hoàn thiện ngày 05 tháng 9 năm 2015 Chấp nhận đăng ngày 10 tháng 9 năm 2015 Địa chỉ: 1 Viện Tên Lửa- Viện KH-CN quân sự; 2 Viện KH-CN quân sự ; * Email: tuananhlemta@gmail.com.