Xây dựng thuật toán xác định vị trí và tư thế cho phương tiện ngầm

Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 56, 08 - 2018 3 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 xO 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 xO và Oy một tam diện thuận. Trên PTN có các thiết bị xác định cự ly iD 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 O 'O 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 O 'O 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 ( 1, 2,3i ). Nhiệm vụ của máy tính trên PTN là từ các thông tin iD , i , i cần xác định vị trí và tư thế của bản thân nó Tên lửa & Thiết bị bay 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.” 4 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 độ xyzO 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.). x 0 z , x y , y 2P1P 1D 3D 1 1 ,,z ,,y ,,x 3P ,0 ,z 2D 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 2 2( ,0, )x z ; P3 3 3( ,0, )x z (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 1D là cự ly đo được giữa phương tiện ngầm và phao số 1 (gốc hệ tọa độ xyzO ), 2D , 3D 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: 1 2 2 2 2x y z D   (1) 2 2 2 22 2( )x x y z D    (2) 2 2 2 23 3 3( ) ( )x x y z z D     (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]: Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 56, 08 - 2018 5 1 1 2 2 1 2 3 1 2 1 2 ( , ,..., ) ( , ,..., ) ( , ,..., ) ( ) . . . ( , ,..., )                        n n n n n f x x x f x x x f x x x F X f x x x (4) Được xác định nghiệm theo thuật toán truy hồi như sau: ( 1) ( ) 1 ( )( )i i iqX X J F X    (5) trong đó, 1qJ  là ma trận nghịch đảo của ma trận Jacobi qJ sau: 1 1 1 1 2 2 2 2 1 2 1 2 . . . . . . . . . . . . n n q n n n n f f f x x x f f f x x x J f f f x x x                                        (6) 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: ( )( )Nif X  , với mọi 1, 2,...,i 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: 1 2 2 2 2 1( , , )    f x y z x y z D (8) 2 2 2 22 2 2( , , ) ( )    f x y z x x y z D (9) 2 2 2 23 3 3 3( , , ) ( ) ( )     f x y z x x y z z D (10) Tiến hành xây dựng ma trận Jacobi qJ dạng (6): 1 1 1 2 2 2 3 3 3 ( , , ) ( , , ) ( , , ) ( , , ) ( , , ) ( , , ) ( , , ) ( , , ) ( , , ) q f x y z f x y z f x y z x y z f x y z f x y z f x y z J x y z f x y z f x y z f x y z x y z                               (11) 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ên lửa & Thiết bị bay 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.” 6 2 3 3 2 2 2 2( ) 2 2 2( ) 2 2( )           q x y z J x x y z x x y z z (12) Ma trận nghịch đảo của ma trận Jacobi qJ : 2 2 1 2 3 3 2 3 3 3 2 3 2 3 3 3 3 2 3 2 3 3 1 1 0 2 2 ( 2 2 2 1 2 2 2                          q x x x z xz x z x z xz x z z J x yz x yz yz x x x x z x z z (13) Triển khai các phép tính truy hồi theo công thức (5) có: 1 2 2 2 2 2 2 2 2 2 2 2 1 2 2 2 2 2 2 2 1 2 2 1 1 ( 1) ( ) ( ( ), ( ), ( )) ( ( ), ( ), ( )) 2 2 1 1 ( ) [ ( ) ( ) ( ) -D ] [( ( ) ) ( ) ( ) -D ] 2 2 2 ( ) 2 2 x i x i f x i y i z i f x i y i z i x x x i x i y i z i x i x y i z i x x x D D x i x                 (14) 2 2 2 22 3 3 2 3 1 2 3 2 2 2 23 3 2 2 2 3 2 2 2 2 3 3 3 3 ( ( ) ( ) ( ) ) ( 1) ( ) [ ( ) ( ) ( ) ] 2 ( ) ( ( ) ( )) [( ( ) ) ( ) ( ) ] 2 ( ) ( ) [( ( ) ) ( ) ( ( ) ) ] 2 ( ) x z i x i z x z i x z y i y i x i y i z i D x y i z x i z x z i x i x y i z i D x y i z z i x i x y i z i z D y i z                      (15) 2 2 2 2 2 1 3 3 2 2 3 2 2 3 2 2 2 2 3 3 3 3 ( ( ) ( )) ( ( ) )( ( ) ( ) ( ) ) ( 1) ( ) 2 ( ) ( ) [( ( ) ) ( ) ( ( ) ) ] 2 ( ) D x i z x z i x z i x z x i y i z i D y i y i x y i z z i x i x y i z i z D y i z                2 2 2 22 3 1 2 3 2 2 2 23 2 2 2 3 2 2 2 2 3 3 3 3 ( 1) ( ) [ ( ) ( ) ( ) ] 2 [( ( ) ) ( ) ( ) ] 2 1 [( ( ) ) ( ) ( ( ) ) ] 2 x x z i z i x i y i z i D x z x x i x y i z i D x z x i x y i z i z D z                   (16) Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 56, 08 - 2018 7 Ở đây: ( 1)x i  , ( )x i , ( 1)y i  , ( )y i , ( 1)z i  , ( )z i là các giá trị ở các bước tính truy hồi thứ ( 1)i  và thứ i . Chọn giá trị ban đầu (1)x ; (1)y , (1)z 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 xyzO thực chất là tìm quan hệ giữa tọa độ xyzO 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 O 'O từ vị trí O về vị trí 'O . Khi này nhận được hệ tọa độ mới ' x '' '' ''O y z . Hệ tọa độ này có gốc trùng với hệ tọa độ dẫn đường ' x'y'z'O , 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 độ ' x''y''z''O 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 ' x''O một góc  nào đó để hệ ' x''y''z''O trùng khít với hệ ' x ' ' 'O y z , thì ba góc  ,  ,  được gọi là ba tư thế giữa hai hệ tọa độ xyzO 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 độ xyzO 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 zzz                                 (17) Vế phải biểu thức (17) hiệu véc tơ ( $ $ $( )Tx y z - ( * * *)Tx y z ) chính là tọa độ của điểm đó trong hệ tọa độ ' x '' '' ''O y z . Trong đó, ma trận A là ma trận cosin định hướng, có cấu trúc sau: 11 12 13 21 22 23 31 32 33 a a a A a a a a a a          (18) 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: 11 12 13; sin ; sina cos cos a a cos        (19) 21 22cos sin cos sin sin ;a a cos cos          (20) 23 31sin sin sin ; sin sin sina cos cos a cos cos             (21) 32 33sin ; sin sin sin cosa cos a cos           (22) Từ (19) và từ (20), (22) có: 13 3212 11 22 rctg(- ); rcsin(a ); rctg(- ) a a a a a a a      (23) Tên lửa & Thiết bị bay 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.” 8 Hình 2. Hệ tọa độ liên kết ' ' ' 'O x y z so với hệ tọa độ chuẩn xyz.O 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ử jia ( 1, 2,3i  ; 1, 2,3j  ) 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 iD , i , i nhận được từ các cơ cấu đo để xác định các phần tử jia 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 độ ( $ix , $ iy , $ iz , 1, 2,3, 4,...i  ) 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 iD , i , i . Từ thông tin iD , 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: ' sin ; ' ; ' sini i i i i i i i i i iy D x D cos cos z D cos       (24) Tọa độ vị trí phao thứ i trong hệ tọa độ dẫn đường (hệ tọa độ xyzO ) đượ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 ( $ix , $ iy , $ iz , 1, 2,3, 4,...i  ). Trên cơ sở tính các tham số tính được *x , *y , *z , 'ix , 'iy , 'iz (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ớ ( $ix , $ iy , $ iz , 1, 2,3, 4,...i  ) 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ó: Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 56, 08 - 2018 9 $ 11 12 13 $ 21 22 23 $ 31 32 33 ' * ' ( * ) *' ii i i i i xx a a a x y a a a y y zz a a a z                                  (25) Do cần phải xác định các phần tử jia nên đặt: 1 11 2 12 3 13 4 21 5 22; ; ; ;x a x a x a x a x a     (26) 6 23 7 31 8 32 9 33; ; ;x a x a x a x a    (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: $ $ $2 1 2 2 2 3 2( *) ( *) ( *) 'x x x y y x z z x x      (28) $ $ $2 4 2 5 2 6 2( *) ( *) ( *) 'x x x y y x z z x y      (29) $ $ $2 7 2 8 2 9 2( *) ( *) ( *) 'x x x y y x z z x z      (30) Với phao số 3 cũng sẽ nhận được 3 phương trình: $ $ $3 1 3 2 3 3 3( *) ( *) ( *) 'x x x y y x z z x x      (31) $ $ $3 4 3 5 2 6 3( *) ( *) ( *) 'x x x y y x z z x y      (32) $ $ $3 7 3 8 2 9 3( *) ( *) ( *) 'x x x y y x z z x z      (33) Tương tự phao số 4 cũng sẽ nhận được 3 phương trình: $ $ $4 1 4 2 4 3 4( *) ( *) ( *) 'x x x y y x z z x x      (34) $ $ $ 4 4 4 5 4 6 4( *) ( *) ( *) 'x x x y y x z z x y      (35) $ $ $4 7 4 8 4 9 4( *) ( *) ( *) 'x x x y y x z z x z      (36) Với 9 ẩn số 1 2 3 4 5 6 7 8 9( , , , , , , , , )x x x x x x x x x đã 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 ]: $ $ $ 2 2 2 1 $ $ $ 2 2 2 2 $ $ $ 2 2 23 $ $ $ 3 3 34 $ $ $ 5 3 3 3 6 7 8 9 ( *)( *)( *)000000 000( *)( *)( *)000 000000( *)( *)( *) ( *)( *)( *)000000 000( *)( *)( *)000 0 x x y y z zx x x y y z zx x x y y z zx x x y y z zx x x x y y z z x x x x                                            1 2 2 2 3 3 $ $ $ 33 3 3 $ $ $ 44 4 4 $ $ $ 4 4 4 4 $ $ $ 4 4 4 4 ' ' ' ' ' '00000( *)( *)( *) '( *)( *)( *)000000 '000( *)( *)( *)000 '000000( *)( *)( *) x y z x y zx x y y z z xx x y y z z yx x y y z z zx x y y z z                                                       (37) 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: TAA I (38) Trong đó, TA 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ên lửa & Thiết bị bay 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.” 10 11 12 13 11 21 31 21 22 23 12 22 32 31 32 33 13 23 33 100 010 001 a a a a a a a a a a a a a a a a a a                            (39) triển khai biểu thức (39) nhận được: 2 2 211 12 13 1a a a   (40) 11 21 12 22 13 23 0a a a a a a   (41) 11 31 12 32 13 33 0a a a a a a   (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: 2 2 21 2 3 1x x x   (43) 1 4 2 5 3 6 0x x x x x x   (44) 1 7 2 8 3 9 0x x x x x x   (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 1 2 3 4 5 6 7 8 9( , , , , , , , , )x x x x x x x x x . 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): $ $ 2 1 2 2 1 1 2 9 2 1 2 9 3 1 2 9 4 1 2 9 5 1 2 9 6 1 2 9 7 1 2 9 8 1 2 9 9 1 2 9 ( *) ( *)( , ,..., ) ( , ,..., ) ( , ,..., ) ( , ,..., ) ( ) ( , ,..., ) ( , ,..., ) ( , ,..., ) ( , ,..., ) ( , ,..., ) x x x y y xf x x x f x x x f x x x f x x x F X f x x x f x x x f x x x f x x x f x x x                                  $ 2 3 2 $ $ $ 2 4 2 5 2 6 2 $ $ $ 2 7 2 8 2 9 2 $ $ $ 3 1 3 2 3 3 3 $ $ $ 3 4 3 5 3 6 3 $ $ $ 3 7 3 8 3 9 3 2 1 2 ( *) ' ( *) ( *) ( *) ' ( *) ( *) ( *) ' ( *) ( *) ( *) ' ( *) ( *) ( *) ' ( *) ( *) ( *) ' z z x x x x x y y x z z x y x x x y y x z z x z x x x y y x z z x x x x x y y x z z x y x x x y y x z z x z x x                                  2 23 1 4 2 5 3 6 1 7 2 8 3 9 1x x x x x x x x x x x x x                                   (46) Khi đó, ma trận Jacobi qJ theo (6) đối với véc tơ hàm (46) như sau: 12 12 121 1 1 12 12 121 2 9 12 12 122 2 2 1 2 9 13 13 13 13 13 13 13 13 13 1 2 3 9 9 9 1 2 9 0 0 0 0 0 0 ... 0 0 0 0 0 0 0 0 0 0 0 0 ... 0 0 0 0 0 0 . 0 0 0 0 0 0 . 0 0 0 0 0 0 . 2 2 2 0 0 0 ... q x y zf f f x y zx x x x y zf f f x x x x y z J x y z x y z x x x f f f x x x                                         4 5 6 1 2 3 7 8 9 1 2 3 0 0 0 0 0 0 0 0 0 x x x x x x x x x x x x                             (47) Với: $ $ $ $ $ $ 12 2 12 2 12 2 13 3 13 3 13 3*; y *; *; *; y *;z *;x x x y y z z z x x x y y z z            Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 56, 08 - 2018 11 Trong thuật toán sẽ dùng Matlab để tính ma rận nghịch đảo của qJ , khi đó, ma trận nghịch đảo 1 q J có dạng: 11 12 19 21 22 29 1 91 99 . . . . . . . . . . . . q q q q q q q J q q                      (48) với ijq là các phần tử của ma trận nghịch đảo 1 q J . Theo công thức truy hồi (5) ta có: 1 1 11 1 1 2 9 12 2 1 2 9 19 9 1 2 9( 1) ( ) ( , ,... ) ( , ,... )... ( , ,... )    x i x i q f x x x q f x x x q f x x x (49) 2 2 21 1 1 2 9 22 2 1 2 9 29 9 1 2 9( 1) ( ) ( , ,... ) ( , ,... )... ( , ,... )    x i x i q f x x x q f x x x q f x x x (50) 3 3 31 1 1 2 9 32 2 1 2 9 39 9 1 2 9( 1) ( ) ( , ,... ) ( , ,... )... ( , ,... )    x i x i q f x x x q f x x x q f x x x (51) 4 4 41 1 1 2 9 42 2 1 2 9 49 9 1 2 9( 1) ( ) ( , ,... ) ( , ,... )... ( , ,... )    x i x i q f x x x q f x x x q f x x x (52) 5 5 51 1 1 2 9 52 2 1 2 9 59 9 1 2 9( 1) ( ) ( , ,... ) ( , ,... )... ( , ,... )    x i x i q f x x x q f x x x q f x x x (53) 6 6 61 1 1 2 9 62 2 1 2 9 69 9 1 2 9( 1) ( ) ( , ,... ) ( , ,... )... ( , ,... )    x i x i q f x x x q f x x x q f x x x (54) 7 7 71 1 1 2 9 72 2 1 2 9 79 9 1 2 9( 1) ( ) ( , ,... ) ( , ,... )... ( , ,... )    x i x i q f x x x q f x x x q f x x x (55) 8 8 81 1 1 2 9 82 2 1 2 9 89 9 1 2 9( 1) ( ) ( , ,... ) ( , ,... )... ( , ,... )    x i x i q f x x x q f x x x q f x x x (56) 9 9 91 1 1 2 9 92 2 1 2 9 99 9 1 2 9( 1) ( ) ( , ,... ) ( , ,... )... ( , ,... )    x i x i q f x x x q f x x x q f x x x (57) Chọn giá trị ban đầu 1(1)x , 2 (1)x , 3 (1)x , 4 (1)x , 5 (1)x , 6 (1)x , 7 (1)x , 8 (1)x , 9 (1)x 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: 1 2 3(0,0,0);P (7,0,5);P (10,0, 5);P  1 2 325, 49 ;D 17,15 ; 20,61D m m D m   0 0 0 0 2 2 3 332 ; 89 ; 7, 47 ; 86,12 ;         Hình 3. Kết quả xác định các phần tử 11a , 12 ,a 13a của ma trận cosin định hướng. Tên lửa & Thiết bị bay 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.” 12 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ử 21a , 22 ,a 23a của ma trận cosin định hướng. Hình 5. Kết quả xác định các phần tử 31a , 32 ,a 33a 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 j ,iA a    ( 1, 2,3i  ; 1, 2,3j  ) có giá trị như sau: 11 0.4924a  ; 12 0.6425a  ; 13 0.5868a   ; 21 0.1240a   ; 22 0.716a  23 0.6799a  ; 31 0.8572a  ; 32 0.2620a   ; 33 0.4324a  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 xyzO sẽ là: 050  ; 040  ; 020  (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. Nghiên cứu khoa học công nghệ Tạp chí Nghiên cứu KH&CN quân sự, Số 56, 08 - 2018 13 [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; 2Viện Khoa học và công nghệ quân sự; 3Phòng Khoa học quân sự, Học viện Phòng không - Không quân. *Email: phucanhquansg@gmail.com.