Xem mẫu
- Nghiên cứu khoa học công nghệ
XÂY DỰNG THUẬT TOÁN XÁC ĐỊNH VỊ TRÍ VÀ TƯ THẾ
CHO PHƯƠNG TIỆN NGẦM
Phạm Văn Phúc1*, Trần Đức Thuận2, Nguyễn Việt Anh3, Nguyễn Quang Vịnh2
Tóm tắt: Bài báo đưa ra một cách tiếp cận mới trong việc xác định vị trí và tư
thế của phương tiện ngầm thông qua việc đo đạc một số tham số giữa phương tiện
ngầm và các phao thu phát tín hiệu, trên cơ sở ứng dụng thuật toán truy hồi
Newton-Raphson. Kết quả mô phỏng đã khẳng định được tính đúng đắn của thuật
toán đề xuất.
Từ khóa: Thuật toán truy hồi; Phao thu phát tín hiệu; Phương tiện ngầm.
1. ĐẶT VẤN ĐỀ
Để điều khiển chuyển động của bất cứ phương tiện nào đều phải xác định được vị trí
của tâm khối và tư thế của nó so với hệ tọa độ nào đó được chọn làm hệ quy chiếu dẫn
đường. Đối với phương tiện trên không, trên mặt đất hoặc mặt biển có thể dùng các con
quay cơ hoặc con quay vi cơ, con quay lazer kết hợp với các thông tin hỗ trợ từ các trạm
dẫn đường hoặc thông tin định vị vệ tinh để xác định vị trí và tư thế cho phương tiện. Vấn
đề nay đã được đề cập tương đối đầy đủ trong các nghiên cứu trước đây [3], [4], [5]. Tuy
nhiên đối với PTN vấn đề xác định vị trí và tư thế sẽ khác, vì môi trường nước các tin hiệu
vô tuyến bị hấp thụ.
Hiện nay các phương tiện ngầm trong quân đội ta thường là được nhập ngoại. Các hãng
nước ngoài thường bán phương tiện đồng bộ với các thiết bị phục vụ dẫn đường kèm theo.
Các thiết bị dẫn đường này sẽ làm nhiệm vụ xác định vị trí và tư thế của phương tiện
ngầm.
Để làm chủ trong khai thác vận hành PTN đã nhập và tiến tới tự thiết kế chế tạo các
chủng loại PTN cần phải nghiên cứu vấn đề dẫn đường nêu trên. Trong bài báo này, nhóm
tác giả sẽ nghiên cứu vấn đề dẫn đường cho PTN làm nhiệm vụ tìm kiếm hoặc cảnh giới
trong không gian ngầm trong vùng được phân công với một cự ly vừa phải. Vấn đề này có
đặc điểm riêng, khác với vấn đề dẫn đường cho tầu ngầm hoặc ngư lôi.
2. NỘI DUNG
2.1. Phương pháp dẫn đường cho phương tiện ngầm
Giả sử phương tiện ngầm làm nhiệm vụ giám sát quá trình xây dựng công trình có phần
dưới ngầm, khảo sát và thăm dò đáy đại dương, hoặc cảnh giới phần ngầm của vùng biển
được định vị bằng các phao thủy âm.
Trên hình 1 mô tả một vùng biển có 3 phao thủy âm. Hệ tọa độ Oxyz với gốc tọa độ
tại phao số 1, trục Ox là đường thẳng nối phao số 1 với phao số 2, trục Oy vuông góc với
mặt nước biển (phương thẳng đứng), trục Oz vuông góc với mặt phẳng Oxy và tạo với
Ox và Oy một tam diện thuận.
Trên PTN có các thiết bị xác định cự ly Di từ PTN đến các phao tương ứng và các
thiết bị đo góc hợp bởi đường nối tâm khối PTN và phao so với hệ tọa độ liên kết của PTN
O ' x ' y ' z ' , đó là các góc tà (góc i -góc hợp gữa đường OO ' với mặt phẳng O ' x ' y ' ) và
góc phương vị (góc i -góc hợp giữa hình chiếu của đường OO ' xuống mặt phẳng
O ' x ' y ' với trục O ' x ' ), ở đây i là chỉ số thứ tự các phao ( i 1, 2,3 ). Nhiệm vụ của máy
tính trên PTN là từ các thông tin Di , i , i cần xác định vị trí và tư thế của bản thân nó
Tạp chí Nghiên cứu KH&CN quân sự, Số 56, 08 - 2018 3
- Tên lửa & Thiết bị bay
so với hệ tọa độ dẫn đường Oxyz , tức là xác định vị trí của gốc hệ tọa độ O ' x ' y ' z ' so
với hệ tọa độ Oxyz và xác định ma trận cosin mô tả quan hệ giữa hai hệ tọa độ Oxyz và
O ' x ' y ' z ' . Các thông tin này là cơ sở để giải quyết các vấn đề khác (vấn đề điều khiển
hoạt động cho bản thân PTN, vấn đề cảnh giới v.v.).
y
P3
P1 P2
0
x
D3
D1 y ,,
z D2
, 1
y
,
x
1
x ,,
0,
z ,,
z,
Hình 1. Mô tả phương pháp dẫn đường cho phương tiện ngầm.
2.2. Xây dựng thuật toán xác định vị trí và tư thế cho phương tiện ngầm
Giả thiết rằng 3 phao thủy âm được cố định tại mỗi vị trí bằng các thiết bị chuyên dùng.
Sự ảnh hưởng của dòng chãy đại dương, sóng, gió lên các phao là không đáng kể, vì vậy,
có thể xem các phao đứng yên tại một điểm trong suốt quá trình khảo sát thuật toán. Trước
khi PTN hoạt động trong môi trường nước cần nạp vào máy tính (hoặc vi xử lý) của thiết bị
điều khiển thông tin về tọa độ vị trí của 3 phao thu phát sóng siêu âm P1 (0, 0, 0) ; P2
( x2 , 0, z2 ) ; P3 ( x3 , 0, z3 ) (vì cả ba phao đều nằm trên mặt biển- mặt phẳng 0xz , nên tọa độ
y của chúng đều bằng 0). Gọi tọa độ tâm khối thiết bị ngầm (tọa độ điểm O ' - Gốc hệ tọa
độ O ' x ' y ' z ' ) là ( x, y, z ) , gọi D1 là cự ly đo được giữa phương tiện ngầm và phao số 1
(gốc hệ tọa độ Oxyz ), D2 , D3 là cự ly đo được giữa tâm khối phương tiện và phao số 2,
số 3 tương ứng. Từ kiến thức toán có các phương trình sau:
x 2 y 2 z 2 D12 (1)
( x x2 ) 2 y 2 z 2 D22 (2)
2 2 2 2
( x x3 ) y ( z z3 ) D 3 (3)
Giải hệ phương trình (1), (2), (3) sẽ xác định được tọa độ tâm khối PTN. Về mặt toán
học có nhiều thuật toán xác định nghiệm phương trình trên. Trong bài báo này xin đề xuất
áp dụng phương pháp số Newton-Raphson để xây dựng thuật toán xác định nghiệm hệ
phương trình nêu trên. Theo phương pháp này, hệ phương trình phi tuyến [2]:
4 P. V. Phúc, …, N. Q. Vịnh, “Xây dựng thuật toán xác định vị trí … phương tiện ngầm.”
- Nghiên cứu khoa học công nghệ
f1 ( x1 , x2 ,..., xn )
f ( x , x ,..., x )
2 1 2 n
f3 ( x1 , x2 ,..., xn )
F ( X ) . (4)
.
.
f ( x , x ,..., x )
n 1 2 n
Được xác định nghiệm theo thuật toán truy hồi như sau:
X ( i 1) X ( i ) J q1 F ( X ( i ) ) (5)
trong đó, J q1 là ma trận nghịch đảo của ma trận Jacobi J q sau:
f1 f1 f
x ... 1
x2 xn
1
f2 f2 f2
x ...
x2 xn
1 (6)
Jq .
.
.
fn fn
...
fn
x1 x2 x n
Việc thực hiện phép truy hồi (5) được dừng ở bước thứ N khi thỏa mãn điều kiện sau:
f i ( X ( N ) ) , với mọi i 1, 2,..., n (7)
trong đó, là số dương bé tùy ý ( 0 ).
Để áp dụng thuật toán Newton-Raphson từ ba phương trình (1), (2), (3) xây dựng ba
phương trình mới sau:
f1 ( x, y, z ) x 2 y 2 z 2 D12 (8)
f 2 ( x, y, z ) ( x x2 ) 2 y 2 z 2 D22 (9)
f3 ( x, y, z ) ( x x3 ) 2 y 2 ( z z3 ) 2 D32 (10)
Tiến hành xây dựng ma trận Jacobi J q dạng (6):
f1 ( x, y, z ) f1 ( x, y, z ) f1 ( x, y, z )
x y z
f 2 ( x, y, z ) f 2 ( x, y, z ) f 2 ( x, y, z )
Jq (11)
x y z
f3 ( x, y, z ) f3 ( x, y, z ) f3 ( x, y, z )
x y z
Thực hiện các phép lấy đạo hàm riêng đối với các hàm số (8), (9), (10) và thay vào (11)
nhận được sau:
Tạp chí Nghiên cứu KH&CN quân sự, Số 56, 08 - 2018 5
- Tên lửa & Thiết bị bay
2 x 2 y 2z
J q 2( x x2 ) 2 y 2 z (12)
2( x x3 ) 2 y 2( z z3 )
Ma trận nghịch đảo của ma trận Jacobi J q :
1 1
0
2 x2 2 x2
( x2 z xz3 x3 z x2 z3 xz3 x3 z z
J q1 (13)
2 x2 yz3 2 x2 yz3 2 yz3
x x3 x3 1
2 x2 z3 2 x2 z3 2 z3
Triển khai các phép tính truy hồi theo công thức (5) có:
1 1
x(i 1) x(i) f1 ( x(i), y(i), z (i)) f 2 ( x(i), y(i), z (i))
2 x2 2 x2
1 1
x(i) [x(i)2 y(i)2 z (i)2 -D12 ] [(x(i) x2 )2 y(i)2 z (i)2 -D22 ] (14)
2 x2 2 x2
x2 D12 D22
2 x(i)
2 2 x2
( x z (i) x(i) z3 x3 z (i) x2 z3 )
y(i 1) y(i) 2 [x(i)2 y(i)2 z (i)2 D12 ]
2 x2 y(i) z3
( x(i) z3 x3 z (i))
[(x(i) x2 )2 y(i)2 z (i)2 D22 ] (15)
2 x2 y(i) z3
z (i)
[(x(i) x3 )2 y(i)2 ( z (i) z3 )2 D32 ]
2 y(i) z3
D12 ( x(i ) z3 x3 z (i )) ( x2 z (i ) x2 z3 )(x(i ) 2 y (i ) 2 z (i ) 2 D22 )
y (i 1) y (i )
2 x2 y (i ) z3
z (i )
[(x(i ) x3 ) 2 y (i ) 2 ( z (i ) z3 ) 2 D32 ]
2 y (i ) z3
x x
z (i 1) z (i ) 2 3 [x(i ) 2 y (i ) 2 z (i ) 2 D12 ]
2 x2 z3
x3
[(x(i ) x2 ) 2 y (i ) 2 z (i ) 2 D22 ] (16)
2 x2 z3
1
[(x(i ) x3 ) 2 y (i ) 2 ( z (i ) z3 ) 2 D32 ]
2 z3
6 P. V. Phúc, …, N. Q. Vịnh, “Xây dựng thuật toán xác định vị trí … phương tiện ngầm.”
- Nghiên cứu khoa học công nghệ
Ở đây: x(i 1) , x(i ) , y (i 1) , y (i ) , z (i 1) , z (i ) là các giá trị ở các bước tính truy
hồi thứ (i 1) và thứ i .
Chọn giá trị ban đầu x(1) ; y (1) , z (1) cho quá trình tính truy hồi một bộ giá trị tùy ý
nào đó, sau đó, sử dụng 3 biểu thức truy hồi (14), (15), (16) sẽ xác định được nghiệm hệ
phương trình (8), (9), (10), gọi x * , y * , z * là tọa độ tâm khối PTN (là kết quả tính truy
hồi), đây chính là vị trí của PTN cần xác định.
Việc xác định tư thế của PTN so với hệ tọa độ dân đường Oxyz thực chất là tìm quan
hệ giữa tọa độ Oxyz với hệ tọa độ O ' x ' y ' z ' . Tiến hành dịch chuyển tịnh tiến hệ tọa độ
Oxyz theo đường thẳng nối OO ' từ vị trí O về vị trí O ' . Khi này nhận được hệ tọa độ
mới O ' x '' y '' z '' . Hệ tọa độ này có gốc trùng với hệ tọa độ dẫn đường O ' x'y'z' , xong các
trục của nó song song theo từng cặp với hệ tọa độ Oxyz ( Ox / / Ox '' , Oy / / O ' y '' ,
Oz / / O ' z '' ). Nếu chúng ta quay hệ tọa độ O ' x''y''z'' xung quanh trục O ' y '' một góc
nào đó, sau đó tiếp tục quay nó xung quanh trục O ' z '' một góc nào đó, và cuối cùng
tiếp tục quay nó xung quanh trục O ' x'' một góc nào đó để hệ O ' x''y''z'' trùng khít với
hệ O ' x ' y ' z ' , thì ba góc , , được gọi là ba tư thế giữa hai hệ tọa độ Oxyz và
O ' x ' y ' z ' (hình 2).
Theo [1] một điểm nào đó có tọa độ ( x $ , y $ , z $ ) trong hệ tọa độ Oxyz thì tọa độ của
nó trong hệ tọa độ O ' x ' y ' z ' sẽ là ( x$ , y$ , z$ ) và được xác định như sau:
x$ x$ x *
$
y$ A( y y * ) (17)
z$ z *
z$
Vế phải biểu thức (17) hiệu véc tơ ( ( x $ y $ z $ )T - ( x * y * z*)T ) chính là tọa độ của
điểm đó trong hệ tọa độ O ' x '' y '' z '' .
Trong đó, ma trận A là ma trận cosin định hướng, có cấu trúc sau:
a11a12 a13
A a21a22 a23 (18)
a31a32 a33
Cũng theo [1] các phân tử của ma trận A quan hệ với các góc , , như sau:
a11 cos cos ; a12 sin ; a13 sin cos (19)
a21 cos sin cos sin sin ; a22 cos cos (20)
a23 sin sin cos cos sin ; a31 cos sin sin sin cos (21)
a32 cos sin ; a33 sin sin sin cos cos (22)
Từ (19) và từ (20), (22) có:
a13 a
arctg(- ); arcsin(a12 ); arctg(- 32 ) (23)
a11 a22
Tạp chí Nghiên cứu KH&CN quân sự, Số 56, 08 - 2018 7
- Tên lửa & Thiết bị bay
Hình 2. Hệ tọa độ liên kết O ' x ' y ' z ' so với hệ tọa độ chuẩn Oxyz.
Như vậy, việc xác định tư thế PTN được quy về tìm ba góc , , hoặc xác định 9
phần tử aij ( i 1, 2,3 ; j 1, 2,3 ) của ma trận A (gọi là ma trận cosin định hướng).
Trong bài báo này, nhóm tác giả sẽ đề xuất giải pháp, sử dụng các thông tin Di , i , i
nhận được từ các cơ cấu đo để xác định các phần tử aij ma trận cosin định hướng A . Các
phao thu phát sóng thủy âm được gắn chặt tương đối vào các cọc hoặc các đối tượng cố
định (cầu cảng, cột thu phát sóng vô tuyến hoặc cột đường dây dẫn điện vv) để đảm bảo
các tọa độ ( xi$ , yi$ , zi$ , i 1, 2,3, 4,... ) không thay đổi trong suốt thời gian phương tiện
ngầm hoạt động.
Nhờ các máy thu và phát sóng siêu âm gắn trên các phao cố định và máy thu và phát
đặt trên phương tiện ngầm sẽ đo được các thông tin Di , i , i .
Từ thông tin Di , i , i có thể xác định tọa độ vị trí phao thứ i trong hệ tọa độ liên
kết gắn liền với phương tiện ngầm (hệ tọa độ O ' x ' y ' z ' ) như sau:
y 'i Di sin i ; x 'i Di cos i cosi ; z 'i Di cos i sin i (24)
Tọa độ vị trí phao thứ i trong hệ tọa độ dẫn đường (hệ tọa độ Oxyz ) được xác định và
ghi vào bộ nhớ máy tính hệ thống điều khiển của phương tiện ngầm trước khi chuyển động
vào môi trường nước ( xi$ , yi$ , zi$ , i 1, 2,3, 4,... ).
Trên cơ sở tính các tham số tính được x * , y * , z * , x 'i , y 'i , z 'i (theo thuật toán truy
hồi (14),(15),(16) và theo các biểu thức (24)) và các tham số đã được nạp vào bộ nhớ ( xi$ ,
yi$ , zi$ , i 1, 2,3, 4,... ) cần xác định tư thế của phương tiện ngầm so với hệ tọa độ dẫn
đường, cụ thể là xác định ma trận cosin định hướng A.
Theo phương trình (17) ta có:
8 P. V. Phúc, …, N. Q. Vịnh, “Xây dựng thuật toán xác định vị trí … phương tiện ngầm.”
- Nghiên cứu khoa học công nghệ
$
x 'i a11a12 a13 xi x *
y ' a a a ( y $ y * )
(25)
i 21 22 23 i
z ' a a a z$ z *
i 31 32 33 i
Do cần phải xác định các phần tử aij nên đặt:
x1 a11 ; x2 a12 ; x3 a13 ; x4 a21 ; x5 a22 (26)
x6 a23 ; x7 a31 ; x8 a32 ; x9 a33 (27)
Vì phao số 1 được dùng làm gốc tọa độ, nên với phao số 2 từ cách đặt biến như (26),
(27) triển khai (25) nhận được 3 phương trình:
( x2$ x*) x1 ( y2$ y*) x2 ( z2$ z*) x3 x '2 (28)
( x2$ x*) x4 ( y2$ y*) x5 ( z2$ z*) x6 y '2 (29)
( x2$ x*) x7 ( y2$ y*) x8 ( z2$ z*) x9 z '2 (30)
Với phao số 3 cũng sẽ nhận được 3 phương trình:
( x3$ x*) x1 ( y3$ y*) x2 ( z3$ z*) x3 x '3 (31)
$ $ $
( x x*) x4 ( y y*) x5 ( z z*) x6 y '3
3 3 2 (32)
$ $ $
( x x*) x7 ( y y*) x8 ( z z*) x9 z '3
3 3 2 (33)
Tương tự phao số 4 cũng sẽ nhận được 3 phương trình:
( x4$ x*) x1 ( y4$ y*) x2 ( z4$ z*) x3 x '4 (34)
( x4$ x*) x4 ( y4$ y*) x5 ( z4$ z*) x6 y '4 (35)
$ $ $
( x x*) x7 ( y y*) x8 ( z z*) x9 z '4
4 4 4 (36)
Với 9 ẩn số ( x1 , x2 , x3 , x4 , x5 , x6 , x7 , x8 , x9 ) đã có 9 phương trình tuyến tính, nên lời giải
sẽ tồn tại và được xác định dưới dạng ma trận như sau [ 2 ]:
1
( x $ x*)( y2$ y*)( z2$ z*)000000
x1 2 x '2
000( x2$ x*)( y2$ y*)( z2$ z*)000
x2 $ $ $ y '2
x3 000000( x2 x*)( y2 y*)( z2 z*) z '2
$ $ $
x4 ( x3 x*)( y3 y*)( z3 z*)000000 x '3
x 000( x$ x*)( y $ y*)( z $ z*)000 y ' (37)
5 3 3 3 3
x6 000000( x3$ x*)( y3$ y*)( z3$ z*) z '3
$
x7 ( x4 x*)( y4 y*)( z4 z*)000000 x '4
$ $
x8 000( x$ x*)( y $ y*)( z $ z*)000 y '4
4 4 4
x9 000000( x4$ x*)( y4$ y*)( z4$ z*) z '4
Như vậy, để xác định được ma trận cosin định hướng bằng hệ phương trình tuyến tính
(tức là xác định tư thế phương tiện ngầm) cần phải có 4 phao thu phát sóng siêu âm. Để
giảm số lượng phao tiếp tục xem xét tính chất của ma trận cosin định hướng A .
Theo [1], ma trận cosin định hướng A có tính chất:
AAT I (38)
T
Trong đó, A là ma trận chuyển vị của ma trận A , I là ma trận đơn vị. Triển khai
công thức (38) có:
Tạp chí Nghiên cứu KH&CN quân sự, Số 56, 08 - 2018 9
- Tên lửa & Thiết bị bay
a11a12 a13 a11a21a31 100
a a a a a a 010 (39)
21 22 23 12 22 32
a31a32 a33 a13 a23 a33 001
triển khai biểu thức (39) nhận được:
a112 a122 a132 1 (40)
a11a21 a12 a22 a13 a23 0 (41)
a11a31 a12 a32 a13a33 0 (42)
Với cách đặt biến (26), (27) ba phương trình (40), (41), (42) được viết lại như sau:
x12 x22 x32 1 (43)
x1 x4 x2 x5 x3 x6 0 (44)
x1 x7 x2 x8 x3 x9 0 (45)
Kết hợp các phương trình (28), (29), (30), (31), (32), (33) với ba phương trình (43),
(44), (45) nhận được 9 phương trình để giải nhằm xác định các phần tử của ma trận cosin
định hướng A . Tuy nhiên, hệ phương trình không còn tuyến tính nữa (3 phương trình phi
tuyến). Để khắc phục vấn đề này sẽ lại áp dụng thuật toán truy hồi Newton-Raphson để
tìm bộ nghiệm ( x1 , x2 , x3 , x4 , x5 , x6 , x7 , x8 , x9 ) .
Trước tiên, từ các phương trình (28), (29), (30), (31), (32), (33), (43), (44), (45) cần xây
dựng véc tơ hàm số dạng (4):
( x $ x*) x1 ( y2$ y*) x2 ( z2$ z*) x3 x '2
f1 ( x1 , x2 ,..., x9 ) 2
f ( x , x ,..., x ) ( x2$ x*) x4 ( y2$ y*) x5 ( z2$ z*) x6 y '2
2 1 2 9
$ $ $
f 3 ( x1 , x2 ,..., x9 ) ( x2 x*) x7 ( y2 y*) x8 ( z2 z*) x9 z '2
$ $ $
f 4 ( x1 , x2 ,..., x9 ) ( x3 x*) x1 ( y3 y*) x2 ( z3 z*) x3 x '3 (46)
F ( X ) f 5 ( x1 , x2 ,..., x9 ) ( x3$ x*) x4 ( y3$ y*) x5 ( z3$ z*) x6 y '3
f 6 ( x1 , x2 ,..., x9 ) ( x3$ x*) x7 ( y3$ y*) x8 ( z3$ z*) x9 z '3
f ( x , x ,..., x ) 2
7 1 2 9 x x22 x32 1
1
f8 ( x1 , x2 ,..., x9 ) x x x x x x
1 4 2 5 3 6
f 9 ( x1 , x2 ,..., x9 ) x1 x7 x2 x8 x3 x9
Khi đó, ma trận Jacobi J q theo (6) đối với véc tơ hàm (46) như sau:
f1 f1 f1 x12 y12 z12 0 0 0 0 0 0
...
x x2 x9 0 0 0 x12 y12 z12 0 0 0
1
f 2 f 2 f 2 0 0 0 0 0 0 x12 y12 z12
x ...
x2 x9 x13 y13 z13 0 0 0 0 0 0 (47)
1
J q . 0 0 0 x13 y13 z13 0 0 0
. 0 0 0 0 0 0 x13 y13 z13
. 2 x1 2 x2 2 x3 0 0 0 0 0 0
f9 f9 f9 x4
...
x5 x6 x1 x2 x3 0 0 0
x1 x2 x9 x7 x8 x9 0 0 0 x1 x2 x3
Với:
x12 x2$ x*; y12 y2$ y*; z12 z2$ z*; x13 x3$ x*; y13 y3$ y*; z13 z3$ z*;
10 P. V. Phúc, …, N. Q. Vịnh, “Xây dựng thuật toán xác định vị trí … phương tiện ngầm.”
- Nghiên cứu khoa học công nghệ
Trong thuật toán sẽ dùng Matlab để tính ma rận nghịch đảo của J q , khi đó, ma trận
nghịch đảo J q1 có dạng:
q11 q12 . . q19
q
21 q22 . . q29
. .
J q1 (48)
. .
.
q91 . . . q99
với qij là các phần tử của ma trận nghịch đảo J q1 .
Theo công thức truy hồi (5) ta có:
x1 (i 1) x1 (i ) q11 f1 ( x1 , x2 ,...x9 ) q12 f 2 ( x1 , x2 ,...x9 )... q19 f 9 ( x1 , x2 ,...x9 ) (49)
x2 (i 1) x2 (i ) q21 f1 ( x1 , x2 ,...x9 ) q22 f 2 ( x1 , x2 ,...x9 )... q29 f 9 ( x1 , x2 ,...x9 ) (50)
x3 (i 1) x3 (i ) q31 f1 ( x1 , x2 ,...x9 ) q32 f 2 ( x1 , x2 ,...x9 )... q39 f 9 ( x1 , x2 ,...x9 ) (51)
x4 (i 1) x4 (i ) q41 f1 ( x1 , x2 ,...x9 ) q42 f 2 ( x1 , x2 ,...x9 )... q49 f 9 ( x1 , x2 ,...x9 ) (52)
x5 (i 1) x5 (i ) q51 f1 ( x1 , x2 ,...x9 ) q52 f 2 ( x1 , x2 ,...x9 )... q59 f 9 ( x1 , x2 ,...x9 ) (53)
x6 (i 1) x6 (i ) q61 f1 ( x1 , x2 ,...x9 ) q62 f 2 ( x1 , x2 ,...x9 )... q69 f 9 ( x1 , x2 ,...x9 ) (54)
x7 (i 1) x7 (i ) q71 f1 ( x1 , x2 ,...x9 ) q72 f 2 ( x1 , x2 ,...x9 )... q79 f 9 ( x1 , x2 ,...x9 ) (55)
x8 (i 1) x8 (i ) q81 f1 ( x1 , x2 ,...x9 ) q82 f 2 ( x1 , x2 ,...x9 )... q89 f 9 ( x1 , x2 ,...x9 ) (56)
x9 (i 1) x9 (i ) q91 f1 ( x1 , x2 ,...x9 ) q92 f 2 ( x1 , x2 ,...x9 )... q99 f 9 ( x1 , x2 ,...x9 ) (57)
Chọn giá trị ban đầu x1 (1) , x2 (1) , x3 (1) , x4 (1) , x5 (1) , x6 (1) , x7 (1) , x8 (1) , x9 (1) cho
quá trình tính truy hồi một bộ giá trị tùy ý nào đó, sau đó sử dụng 9 biểu thức truy hồi từ
(49) đến (57) sẽ xác định được nghiệm hệ phương trình (46). Đây chính là các tham số cần
tìm mô tả tư thế của phương tiện ngầm.
3. KẾT QUẢ MÔ PHỎNG
Giả sử 3 phao thủy âm được cố định tại các vị trí có các tham số như sau:
P1 (0, 0, 0); P2 (7, 0,5); P3 (10, 0, 5); D1 25, 49m; D 2 17,15m; D3 20, 61m
2 320 ; 2 890 ; 3 7, 47 0 ; 3 86,120 ;
Hình 3. Kết quả xác định các phần tử a11 , a12 , a13 của ma trận cosin định hướng.
Tạp chí Nghiên cứu KH&CN quân sự, Số 56, 08 - 2018 11
- Tên lửa & Thiết bị bay
Trên các hình 3 đến hình 6 thể hiện kết quả xác định các phần tử của ma trận A trên
ngôn ngữ Matlab trong trường hợp này.
Hình 4. Kết quả xác định các phần tử a21 , a22 , a23 của ma trận cosin định hướng.
Hình 5. Kết quả xác định các phần tử a31 , a32 , a33 của ma trận cosin định hướng.
Từ các đồ thị kết quả mô phỏng cho thấy ma trận cosin định hướng A aij ,
( i 1, 2,3 ; j 1, 2,3 ) có giá trị như sau:
a11 0.4924 ; a12 0.6425 ; a13 0.5868 ;
a21 0.1240 ; a22 0.716
a23 0.6799 ; a31 0.8572 ; a32 0.2620 ; a33 0.4324
Khi này, các góc định hướng của phương tiện ngầm so với hệ tọa độ dẫn đường Oxyz
sẽ là:
500 ; 400 ; 200 (58)
Nhận xét: Khi sử dụng thuật toán từ (49) đến (57) chỉ còn 3 phao thu phát tín hiệu siêu
âm, giảm được 1 phao so với thuật toán (37), nhưng vẫn xác định được các tham số mô tả
tư thế của phương tiện ngầm.
4. KẾT LUẬN
Vấn đề dẫn đường cho PTN khác với vấn đề dẫn đường cho các phương tiện chuyển
động trong không gian trên mặt nước (khi có tín hiệu của các hệ thống định vị vệ tinh) và
vấn đề này còn mới ở Việt Nam. Các kết quả do bài báo đề xuất có thể là cơ sở để làm chủ
trong khai thác vận hành các PTN nhập ngoại và là cơ sở để thiết kế chế tạo hệ thống dẫn
đường cho các phương tiện hoạt động ngầm.
Thuật toán được đề xuất sẽ là cơ sở để xây dựng phần mềm cho máy tính trên khoang
của hệ thống điều khiển phương tiện ngầm.
TÀI LIỆU THAM KHẢO
[1]. Trần Đức Thuận, Bùi Ngọc Mỹ. “ Thiết bị dẫn đường quán tính và đo cao trong hệ
thống điều khiển thiết bị bay”. NXB Khoa học và Kỹ thuật, Hà Nội, 2015.
12 P. V. Phúc, …, N. Q. Vịnh, “Xây dựng thuật toán xác định vị trí … phương tiện ngầm.”
- Nghiên cứu khoa học công nghệ
[2]. Nguyễn Minh Chương, Nguyễn Văn Khải, Nguyễn Văn Tuấn, Nguyễn Tường, Khuất
Văn Ninh. “ Giải tích số”. NXB Giáo dục, Hà Nội, 2003.
[3]. Bùi Xuân Khoa.“ Các hệ thống vô tuyến dẫn đường hàng không”. NXB Khoa học và
Kỹ thuật, Hà Nội, 2008.
[4]. Trần Đức Thuận, Trương Duy Trung, Nguyễn Quang Vịnh, Nguyễn Sỹ Long, Trần
Xuân Kiên, Bùi Hồng Huế, Nguyễn Văn Diên, “ 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 độ góc
với từ kế và gia tốc kế”. Tạp chí nghiên cứu KH-CNQS, Số 25, T6-2013, tr.7-16.
[5]. Trần Đức Thuận, Bùi Hồng Huế, Trương Duy Trung, Trần Xuân Kiên, “ Ứng dụng
bộ lọc Kalman phi tuyến mở rộng xây dựng thuật toán xác định tham số định hướng
trên cơ sở kết hợp con quay tốc độ góc với từ kế và gia tốc kế”. Hội nghị Cơ điện tử
Toàn quốc lần thứ 6, T10-2012, tr.488-494.
ABSTRACT
AN ALGORITHM FOR DETERMINATION THE LOCATION AND POSITION
FOR UNDERWATER VEHICLES
In the paper, a new approach to determining the location and position of
underwater vehicles through the measurement of some parameters between
underwater vehicles and buoys, based on the application of algorithms retrieve
Newton-Raphson is presented. Simulation results confirm the correctness of the
proposed algorithm.
Keywords: Newton-Raphson; Buoys; Underwater vehicles.
Nhận bài ngày 30 tháng 3 năm 2018
Hoàn thiện ngày 22 tháng 4 năm 2018
Chấp nhận đăng ngày 10 tháng 8 năm 2018
Địa chỉ: 1Trường Cao Đẳng Kỹ thuật Hải quân;
2
Viện Khoa học và công nghệ quân sự;
3
Phòng Khoa học quân sự, Học viện Phòng không - Không quân.
*
Email: phucanhquansg@gmail.com.
Tạp chí Nghiên cứu KH&CN quân sự, Số 56, 08 - 2018 13
nguon tai.lieu . vn
