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 33 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 33 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 />
I33 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 33 1 .1T <br />
2<br />
(11)<br />
sin <br />
T<br />
enu 1 .1 <br />
2 1 cos <br />
I33 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 33 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 />
ve 1 0 0 ve <br />
v 0 1 0 v w . (15)<br />
n n 3<br />
<br />
vu 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 />
ba 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 106 <br />
<br />
enu b b g 3.6 106 u t <br />
5.4 106 <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 />