intTypePromotion=1

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

Chia sẻ: ViSumika2711 ViSumika2711 | Ngày: | Loại File: PDF | Số trang:11

0
15
lượt xem
0
download

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

Mô tả tài liệu
  Download Vui lòng tải xuống để xem tài liệu đầy đủ

Bài viết đư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

Chủ đề:
Lưu

Nội dung Text: 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 />
ADSENSE
ADSENSE

CÓ THỂ BẠN MUỐN DOWNLOAD

 

Đồng bộ tài khoản
2=>2