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