Mô hình toán học hệ thống dẫn đường quán tính

Thông tin khoa học công nghệ<br /> <br /> MÔ HÌNH TOÁN HỌC HỆ THỐNG<br /> DẪN ĐƯỜNG QUÁN TÍNH<br /> Nguyễn Thái Hòa*, Đỗ Xuân Ngọc, Đào Văn Hưng, Đỗ Ngọc Tuấn<br /> Tóm tắt: Trong điều khiển thiết bị bay chúng ta phải giải quyết vấn đề dẫn<br /> đường. Một trong các phương pháp dẫn đường là dẫn đường quán tính không dùng<br /> thông tin bên ngoài vật thể chuyển động mà dùng thông tin về gia tốc của bản thân<br /> vật thể chuyển động để xác định các tham số dẫn đường. Bài báo này trình bày về<br /> một mô hình động học mới cho hệ thống dẫn đường quán tính.<br /> Từ khóa: Hệ thống dẫn đường quán tính; Hệ tọa độ quán tính; Khối đo lường quán tính; Góc Euler.<br /> <br /> 1. ĐẶT VẤN ĐỀ<br /> Hệ thống dẫn đường quán tính (INS) dựa trên nguyên lý tính quãng đường đi<br /> qua để xác định các tham số chuyển động của thiết bị bay (TBB) so với một hệ tọa<br /> độ nào đó. Ưu điểm cơ bản của hệ thống INS là xác định đồng thời các tham số<br /> chuyển động của TBB mà không cần thông tin nào từ bên ngoài nên đảm bảo tính<br /> độc lập cao. Tuy nhiên, nhược điểm của hệ thống INS là sai số định vị tăng theo<br /> thời gian. Vì vậy vấn đề hạn chế sai số trong hệ thống INS đã có nhiều nghiên cứu<br /> về lý thuyết cũng như giải pháp được ứng dụng trong thực tế. Một trong các<br /> phương pháp hạn chế sai số trong hệ thống INS là sử dụng các mô hình động lực<br /> học phi tuyến kết hợp với lọc Kalman trong thiết kế hệ thống.<br /> Bài báo này, trình bày mô hình động lực học phi tuyến sử dụng véc tơ quay cho<br /> thuật toán dẫn đường quán tính. Trong mô hình động học véc tơ quay, đầu ra của<br /> con quay hồi chuyển được đo trực tiếp và ước lượng theo cấu trúc phản hồi của véc<br /> tơ quay. Cấu trúc phản hồi có nghĩa là đầu ra của con quay hồi chuyển được xấp xỉ<br /> từ véc tơ quay trước đó. Đặc tính này cho sự liên kết chặt chẽ giữa các sai số của<br /> phép đo và ước lượng. Hơn nữa cũng tính chất này cho phép việc tính toán các góc<br /> nhanh hơn phương pháp Euler. Thông tin về vận tốc từ khối đo lường quán tính<br /> (IMU) được sử dụng làm các phép đo đầu vào. Và vì thế, vận tốc và các thành<br /> phần của véc tơ quay được sử dụng như là các biến trạng thái trong mô hình động<br /> lực học phi tuyến.<br /> 2. NGUYÊN LÝ DẪN ĐƯỜNG QUÁN TÍNH<br /> Trong nội dung này, chúng tôi trình bày tóm lược về nguyên lý dẫn đường<br /> quán tính, cũng như các phương trình động lực học dùng trong hệ thống dẫn<br /> đường quán tính.<br /> Xét các chuyển động diễn ra theo thuyết tương đối hẹp Galilean. Theo đó cơ sở<br /> toán học của dẫn đường quán tính là định luật cơ bản của Newton về quy luật cơ<br /> học chuyển động của trọng tâm TBB trong hệ tọa độ quán tính (i-frame). Quy luật<br /> này được mô tả như sau [4]:<br /> <br /> d2R <br />  n  g ( R) (1)<br /> dt 2 <br /> <br /> Trong đó R là véc tơ vị trí của TBB; n là véc tơ gia tốc nhận cảm của khối<br /> <br /> tâm TBB; g ( R) là véc tơ gia tốc trọng trường của TBB do lực hấp dẫn trái đất<br /> <br /> <br /> Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san CNTT, 04 - 2019 169<br />  Công nghệ thông tin<br /> <br /> d2R<br /> sinh ra (xét chuyển động trong trọng trường trái đất); là gia tốc tuyệt đối<br /> dt 2<br /> của TBB.<br /> Phương trình (1) thể hiện việc miêu tả toán học của một chuyển trong hệ tọa độ<br /> quán tính là đơn giản. Tuy nhiên, khi giải các bài toán chuyển động trong thực tế<br /> sẽ thuận tiện hơn khi chúng ta chuyển các hệ tọa độ quán tính sang hệ tọa độ<br /> không quán tính [3-5].<br /> Trên cơ sở phương trình (1) người ta xây dựng các mô hình toán thể hiện động<br /> lực học cho chuyển động, cũng như các thuật toán cho các hệ thống dẫn đường<br /> <br /> quán tính. Thông tin về gia tốc nhận cảm n có thể đo được bằng ba cảm biến gia<br /> <br /> tốc bố trí trực giao; thông tin về gia tốc trọng trường g ( R) thường được mô tả<br /> <br /> dưới dạng hàm đã biết của R tùy thuộc vào việc mô hình hóa trái đất.<br /> Xét phương trình (1) dưới dạng Cô-si:<br /> <br />  dV  <br />   n  g ( R)<br />  dt<br />   (2)<br /> d R V <br />  dt<br /> Khi xét các phương trình (2) này trong hệ tọa độ quay với vận tốc góc  , ta có:<br /> <br />  dV dV <br />     V<br />  dt dt<br />   (3)<br />  d R  dR   V<br /> <br />  dt dt<br /> dV dR  <br /> Trong đó , là đạo hàm của các véc tơ R và V trong hệ tọa độ quay với<br /> dt dt<br /> vận tốc góc  .<br /> Các phương trình trên là cơ sở cho phép ta xây dựng các thuật toán dẫn đường<br /> quán tính khác nhau. Thực tế để xây dựng các thuật toán số cho hệ thống dẫn<br /> đường quán tính, người ta phải lựa chọn dạng cơ sở cho hệ tọa độ dẫn đường để<br /> xác định các tham số định vị và định hướng. Hệ tọa độ lựa chọn do kiểu loại TTB,<br /> đặc điểm quỹ đạo, và nhiệm vụ cần giải quyết quy định.<br /> Thông thường đối với TBB gần trái đất, người ta hay dùng hệ tọa với mặt phẳng<br /> cơ sở là mặt phẳng ngang và được định hướng bằng các trục theo phương vị (các<br /> trục hướng về phía bắc và phía đông). Khi các trục của tọa độ đã được định vị theo<br /> một hướng xác định, người ta quay với vận tốc xác định. Và thông tin về vị trí của<br /> đối tượng được xác định bằng kinh độ, vĩ độ và độ cao trên Ellipsoid của trái đất.<br /> 3. MÔ HÌNH TOÁN HỌC HỆ THỐNG DẪN ĐƯỜNG QUÁN TÍNH<br /> Trong phần này, chúng tôi trình bày một số mô hình toán học phi tuyến mô tả<br /> mối tương quan giữa các trạng thái dẫn đường, cũng như trình bày mô hình toán<br /> học sai số cho các đo đạc trong hệ thống dẫn đường quán tính.<br /> 3.1. Mô hình động lực học vận tốc<br /> <br /> <br /> <br /> 170 N. T. Hòa, …, Đỗ Ngọc Tuấn, “Mô hình toán học hệ thống dẫn đường quán tính.”<br /> Thông tin khoa học công nghệ<br /> <br /> Phương trình (2) xét trong hệ tọa độ địa lý địa phương (ENU), hệ động lực học<br /> vận tốc được biểu diễn:<br />   ve  vv <br />  ae    2  vn sin   e u  2 vu cos  <br />   ( N  h) cos   N h <br /> ve   <br /> d    ve  vn vu<br /> v   a    2   ev sin     g  w1. (4)<br /> dt   <br /> n n<br />  ( N  h) cos   M h <br /> vu   <br /> 2 2<br />  a  ve  vn  v 2 cos  <br />  u N  h M  h e  <br /> Trong đó: g là véc tơ gia tốc trọng trường, h là độ cao, M , N là bán kính cong<br /> của trái đất được xác định theo công thức [1]:<br /> a 1  e 2  a<br /> M 3 2<br /> ,N  12<br /> 1  e2 sin 2   1  e2 sin 2  <br /> Vĩ độ  là giá trị khởi tạo ban đầu của khối đo lường quán tính (IMU), và <br /> T<br /> là tốc độ quay của trái đất. Véc tơ v enu   ve , vn , vu  là vận tốc trong hệ tọa độ<br /> ENU. Nhiễu trạng thái của mô hình động lực học, ký hiệu w1 , nhiễu này được giả<br /> định bao gồm nhiễu trắng (Gaussian white noise), sai số bán kính trái đất (sai số do<br /> phương pháp mô hình hóa trái đất), sai số về khởi tạo vị trí ban đầu, sai số hệ thống<br /> của khối IMU.<br /> Gia tốc trong hệ tọa độ ENU, ký hiệu là a enu , và được tính toán theo công thức:<br />  ae <br /> a   an   Cimu<br /> enu enu a<br /> A  a imu  b a  . (5)<br />  au <br /> enu<br /> Với Cimu là ma trận chuyển trạng thái từ hệ tọa IMU (body frame) sang hệ tọa<br /> độ ENU, và a imu là đầu ra của gia tốc kế trong hệ tọa độ IMU, và ba là sai số của<br /> gia tốc kế (sai số này là tổ hợp của ba thành phần: constant bias, turn-on bias, và<br /> random walk noise).<br /> Hệ số tỉ lệ  SFiia |i 1,2,3  và ma trận độ chệch không  MAajk | j ,k 1,2,3; j  k  của gia tốc<br /> kế được mô tả bởi:<br />  SF11a MA12a<br /> MA13a <br />  <br /> Aa  I 33   MA21a<br /> SF22a MA23a  . (6)<br />  MA31a<br /> a a <br /> MA32 SF33 <br /> <br /> Các giá trị về hệ số tỉ lệ, và độ chệch không của gia tốc kế được lấy từ đặc tính<br /> của khối đo lường quán tính IMU.<br /> enu<br /> Véc tơ quay liên quan đến ma trận chuyển trạng thái Cimu của công thức (5)<br /> được cho bởi [6]:<br /> <br /> <br /> <br /> <br /> Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san CNTT, 04 - 2019 171<br />  Công nghệ thông tin<br /> <br />  1  cos    sin    <br /> enu<br /> Cimu   cos    I 33  2<br />  . T<br />   .<br />    (7)<br />   <br />  <br /> T 12<br /> Trong đó, véc tơ quay     e ,  n , u  ,     T .  , và ma trận của phản đối<br /> xứng của véc tơ quay được cho bởi:<br />  0  u  n <br />      u 0  e  . (8)<br />   e e 0 <br /> 3.2. Mô hình động lực học trạng thái<br /> Có nhiều nghiên cứu về mô hình hệ động lực học của véc tơ quay, một trong<br /> các mô hình hệ động lực của véc tơ quay được cho dưới dạng:<br />  1 1   .sin   <br /> 2<br />    I33      T 1        imu . (9)<br />  2  .  2 1  cos    <br /> Trong đó,  imu là vận tốc góc trong hệ tọa IMU. Tuy nhiên công thức này<br /> không xét vận tốc quay của trái đất, cũng như không xét vận tốc quay của véc tơ<br /> vận tốc. Vì vậy, khi xem xét tới vận tốc quay của trái đất, hiệu ứng lực Coriolis, và<br /> gia tốc ly tâm thì phương trình động lực học (9) được xem xét dưới dạng:<br />  <br />   imu  imu  enu  enu  w2 . (10)<br />  <br /> Với<br />    sin    <br />  imu  1 .1 <br /> T<br /> <br /> 2 1  cos    <br />  I 33  1 .1T     <br />   2<br />  (11)<br />    sin    <br /> T<br />   enu  1 .1 <br /> 2 1  cos    <br />  I33  1.1   2   <br /> T<br /> <br /> <br /> <br /> Và 1  . Sai số của mô hình động lực, ký hiệu w2 được giả định là nhiễu<br /> <br /> trắng, bao gồm cả sai số bán kính trái đất và sai số nhiễu của con quay hồi quyển.<br /> Vận tốc góc  enu bao gồm vận tốc góc của trái đất enu , vận tốc góc ly tâm  a và<br /> hiệu ứng lực Coriolis  c . Theo [6] chúng ta có thể mô tả:<br />  v <br />    n <br />  0   0   M  h<br />   ve <br />  enu  enu   a   c   cos     0    . (12)<br />  N h <br /> <br />   sin   ve tan    <br />    0 <br />  N h <br />  <br /> Vận tốc góc  imu được xác định theo công thức:<br /> <br /> <br /> <br /> 172 N. T. Hòa, …, Đỗ Ngọc Tuấn, “Mô hình toán học hệ thống dẫn đường quán tính.”<br /> Thông tin khoa học công nghệ<br /> <br />  imu  A g  imu  b g  . (13)<br /> Trong đó b g là nhiễu, ký hiệu hệ số tỉ lệ  SFiig |i 1,2,3  và ma trận độ chệch<br /> không  MAjkg | j ,k 1,2,3; j  k  của con quay hồi chuyển, khi đó:<br />  SF11g MA12g<br /> MA13g <br />  <br /> Ag  I 33   MA21g SF22g<br /> MA23g  . (14)<br />  MA31g<br /> g<br /> MA32 SF33  g <br /> <br /> Các hệ số tỉ lệ, độ chệch không được lấy từ đặc tính kỹ thuật của con quay.<br /> 3.3. Mô hình đo đạc<br /> Vận tốc và tốc độ góc được sử dụng làm phép đo tham chiếu. Mô hình toán học<br /> cho phép đo vận tốc được xây dựng khá đơn giản, một trong các mô hình đó có thể<br /> mô tả như sau:<br />  ve  1 0 0   ve <br /> v   0 1 0  v   w . (15)<br />  n   n 3<br /> <br /> vu  0 0 1  vu <br /> Trong đó, sai số của mô hình w3 chính là nhiễu trắng của đầu ra các gia tốc kế.<br /> Với các mô hình động lực học đo đạc cho phương trình (15) được cho bởi biểu<br /> thức [6]:<br />   v <br />    n <br /> M h<br /> 1   <br />      ve   cos     w .<br />  imu   imu    enu (16)<br />      N h   3<br /> <br />    <br />    sin   ve tan   <br />   <br />   N  h  <br /> Trong đó w3 là nhiễu trắng phép đo của con quay hồi chuyển.<br /> Từ mô hình động lực học đo đạc, người ta thiết kế các bộ lọc để giảm thiểu các<br /> sai số. Thực tế người ta thường dùng các bộ lọc Kalman trong thiết kế hệ thống [2].<br /> 4. MÔ PHỎNG VÀ ĐÁNH GIÁ KẾT QUẢ<br /> Trong phần này, chúng tôi trình bày mô hình IMU với mục đích mô phỏng. Vì<br /> vậy các thành phần sai số được giả thiết là các quá trình Gauss-Markov bậc nhất<br /> với tự tương quan là hằng số. Hằng số tự tương quan trong mô phỏng là 7000s.<br /> Các phương trình mô tả sai số của con quay hồi chuyển, gia tốc kế được sử<br /> dụng trong mô phỏng được cho bởi:<br /> 1 1<br /> ba   ba  2 a2  a u  t  và b g   b g  2 g2  g u  t <br /> a g<br /> Với ba và b g là các sai số của gia tốc kế và con quay hồi chuyển;  a và  g hằng<br /> số tự tương quan;  a  50,  g  0.02 là độ lệch chuẩn của các nhiễu ngẫu nhiên;<br /> u  t  là hàm kỳ vọng của nhiễu trắng.<br /> <br /> <br /> Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san CNTT, 04 - 2019 173<br />  Công ngh<br /> nghệệ thông tin<br /> <br /> Các phép đo ccủa<br /> ủa gia tốc kế đđư<br /> ược<br /> ợc mô tả theo mô hình:<br />  sin  <br /> a    cos  sin   g .<br /> imu <br /> <br />   cos  cos  <br /> Với g là véc tơ gia ttốc<br /> V ốc trọng tr ờng trong hệ tọa độ quán tính;  ,  , là ba góc<br /> trường<br /> Euler đđịnh<br /> ịnh hhư<br /> ướng.<br /> ớng.<br /> Mô hình hóa phép đo vvới ới nhiễu trắng, phép đo đđư<br /> ược ợc xác định bởi:<br /> 0.01<br /> a  a  b  0.01 u  t  .<br /> enu imu a<br /> <br /> <br />  0.1 <br /> Mô hình đo đđạc<br /> ạc đối với con quay hồi chuyển đđược ợc cho bởi:<br /> 3.4 106 <br />  <br />  enu   b  b g  3.6 106  u  t <br /> 5.4 106 <br />  <br />   cos  cos <br />  0 <br /> Với    cos  cos   90  là đđầu<br /> Với b<br /> ầu ra của con quay hồi chuyển tại vĩ độ<br />  <br />   sin  <br />  với<br /> ới góc phphương<br /> ương vvịị  .<br /> Ban đầu<br /> đầu góc phphương<br /> ương vvịị đư ợc khởi tạo llàà 50 với<br /> được ới sai số 0.5, phương<br /> phương sai ccủa<br /> ủa nhiễu<br /> được ước<br /> được ớc lượng<br /> l ợng qua bộ lọc Kalman thích nghi. Độ lệch chuẩn của nhiễu chúng tôi<br /> sử<br /> ử dụng trong mô phỏng nh như<br /> ư trong B<br /> Bảng<br /> ảng 1:<br /> B ảng 1. Độ<br /> Bảng Độ lệch chuẩn của các quá trình nhi nhiễu.<br /> ễu.<br /> --5 --8<br /> Vận<br /> ận tốc đông (m/s) 2.5 x 10 Vector quay đông (rad) 4.4 x 10<br /> --5<br /> Vận<br /> ận tốc bắc (m/s) 2.5 x 10 Vector quay bbắc ắc (rad) 4.4 x 10--8<br /> Vận<br /> ận tốc llên<br /> ên (m/s) 3.3 x 10--5 Véc tơ quay lên (rad) 4.4 x 10--8<br /> <br /> <br /> <br /> <br /> 1 Góc phương vvịị tham chiếu thực vvà<br /> Hình 1. à góc phương vvị ước<br /> ước lư<br /> l ượng<br /> ợng.<br /> <br /> <br /> 174 N. T. Hòa, …, Đỗ<br /> ỗ Ngọc Tuấn<br /> Tuấn,, ““Mô<br /> Mô hình toán h<br /> học<br /> ọc hệ thống dẫn đđư<br /> ường tính.””<br /> ờng quán tính<br /> Thông tin khoa học công nghệ<br /> <br /> Kết quả mô phỏng (hình 1) cho thấy góc phương vị được tính bằng phương<br /> pháp véc tơ qoay (đường nét liền) gần giá trị thực hơn so với phương pháp tính<br /> bằng góc Euler (đường nét đứt). Góc phương vị tính theo phương pháp góc Euler<br /> nhấp nhô khoảng 140 trước khi hội tụ và phải mất tới 13 giây để hội tụ về giá trị<br /> thực. Trong khi đó đối với phương pháp véc tơ quay góc phương vị nhấp nhô<br /> khoảng 20 và mất khoảng 3 giây để hội tụ về giá trị thực.<br /> <br /> <br /> <br /> <br /> Hình 2. Sai số phép đo của phương pháp véc tơ quay.<br /> Trên Hình 2 là hình ảnh mật độ nhiễu của các phép đo mô phỏng con quay hồi<br /> chuyển trục X, trục Y và trục Z. Các giá trị trung bình của nhiễu trong phép đo<br /> trong khoảng từ 15 giây và 50 giây lần lượt tương ứng là 3.5 x 10-6 (rad/s), 3.64 x<br /> 10-6 (rad/s), và 5.44 x 10-6 (rad/s). Vì vậy các phương sai của nhiễu được thiết kế<br /> cho bộ lọc Kalman thích nghi với sai số tối đa khoảng 2.9%.<br /> 5. KẾT LUẬN<br /> Tùy theo những ứng dụng thực tiễn, dẫn đường quán tính được mô tả và mô<br /> hình hóa bằng thuật toán số và được thể hiện bằng kỹ thuật để giải quyết vấn đề<br /> được đặt ra. Ưu điểm của dẫn đường quán tính là chỉ cần dùng thông tin nội tại của<br /> bản thân vật thể chuyển động mà không cần các thông tin từ bên ngoài. Tuy nhiên,<br /> sai số của dẫn đường quán tính tăng theo thời gian. Chính vì vậy đã có rất nhiều<br /> nghiên cứu để cải thiện vấn đề sai số cho các hệ thống dẫn đường quán tính. Một<br /> trong các phương pháp hạn chế sai số trong hệ thống dẫn đường quán tính là sử<br /> dụng các mô hình động lực học phi tuyến kết hợp với lọc Kalman trong thiết kế hệ<br /> thống. Trong bài báo này, chúng tôi cũng đã trình bày về một mô hình IMU với<br /> những đánh giá kết quả thực tế thu được.<br /> TÀI LIỆU THAM KHẢO<br /> [1]. Jay A. Farrell, and Matthew Barth, The Global Positioning System & Inertial<br /> Navigation, McGraw-Hill, 1998.<br /> <br /> <br /> <br /> Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san CNTT, 04 - 2019 175<br />  Công nghệ thông tin<br /> <br /> [2]. Mohinder S. Grewal, Angus P. Andrews, Kalman Filtering Theory and<br /> Practice, Prentice Hall, 1993.<br /> [3]. Robert Grover Brown, Patrick Y.C. Hwang, Introduction to Random Signals<br /> and Applied Kalman Filter, John Wiley & Sons, 1997.<br /> [4]. Robert M. Rogers, Applied Mathematics in Integrated Navigation Systems,<br /> AIAA, 2000.<br /> [5]. Oleg Salychev, Inertial Systems in Navigation and Geophysics, Bauman<br /> MSTU Press, Moscow, 1998.<br /> [6]. Mohinder S. Grewal, Lawrence R. Weill, and Angus P. Andrews, Global<br /> Positioning systems, Inertial Navigation and Integration, Wiley Interscience,<br /> 2001.<br /> ABSTRACT<br /> A MATHEMATICAL MODEL OF INERTIAL NAVIGATION SYSTEMS<br /> In controlling flight vehicles, we have to solve navigation problems. One of the<br /> navigation methods is inertial navigation without using information from outside<br /> the moving object that uses information about the acceleration of the moving object<br /> itself to determine the navigational parameters. This paper presents a new dynamic<br /> mathematical model of inertial navigation systems.<br /> Keywords: Inertial navigation system; Inertial coordinates; Inertial measurement unit; Euler angle.<br /> <br /> Nhận bài ngày 28 tháng 12 năm 2018<br /> Hoàn thiện ngày 28 tháng 02 năm 2019<br /> Chấp nhận đăng ngày 25 tháng 3 năm 2019<br /> <br /> Địa chỉ: Viện Công nghệ thông tin/Viện KH-CNQS.<br /> *<br /> Email: thaihoa78th@gmail.com.<br /> <br /> <br /> <br /> <br /> 176 N. T. Hòa, …, Đỗ Ngọc Tuấn, “Mô hình toán học hệ thống dẫn đường quán tính.”<br />