7. Kết cấu của luận án
3.4 Hệ thống điều khiển dẫn đường AUV và thực hiện các nhiệm vụ dưới nước
dưới nước
3.4.1 Điều khiển AUV theo quỹ đạo đặt trước
Điều khiển AUV theo quỹ đạo đặt trước thường sử dụng trong trường hợp AUV khảo sát đáy biển, tìm kiếm cứu nạn, dò tìm hư hỏng trên đường ống ngầm… Việc điều khiển này giả định tốc độ AUV không thay đổi, quỹ đạo theo mặt phẳng ngang được định trước bằng các điểm chuyển hướng tạo
pn + = x + , y+ T k 1 k 1 k 1
tài này, tác giả sử dụng luật dẫn đường LOS (Line-of-Sight) để điều khiển AUV theo tuyến.
Giả sử AUV có tọa độ trong hệ trục tọa độ {n} di chuyển trong mặt phẳng nằm ngang và bám theo quỹ đạo mong muốn là một đoạn thẳng được tạo thành từ hai điểm chuyển hướng pnk = xk , yk T và
như trong hình 3.10, vận tốc và hướng đi AUV sẽ được xác định như sau:
U (t ) = x (t )2 + y (t )2 (3.51)
( t ) = a tan 2 (y (t ), x (t )) − , (3.52)
Để xác định sai số bám quỹ đạo của con tàu e(t), ta tiến hành chuyển tọa độ (x, y) của con tàu trong hệ trục tọa độ {n} sang hệ trục tọa độ tham chiếu với gốc đặt tại waypoint p nk = x k , yk T và hệ trục tọa độ tham chiếu này quay quanh trục zn của hệ trục tọa độ {n} một góc k được xác định như sau:
ak =a tan 2 (yk+1 −yk , xk+1 −xk ) (3.53)
(t)=
R p (a T
pn (t) − pn (3.54)
trong đó:
(t) = s(t), e(t) T .
s(t) : là khoảng cách along-track. e(t): là sai số bám quỹ đạo.
Theo tài liệu [31] thì giá trị ma trận R p ( k )T được tính như sau:
T =cos a −sin a (3.55)
Rp (ak ) k k
sin ak cos ak
Ta có thể xác định được sai số bám quỹ đạo của con tàu như sau:
( ) ( ) k k ( )−
y
k k
e t = − x t − x sin a + y t cos a (3.56)
Nhiệm vụ chính của hệ thống dẫn đường (Guidance) là tính toán các trạng thái mong muốn giúp cho AUV bám theo quỹ đạo được định trước, điều này cũng tương đương với việc hệ thống dẫn đường phải làm cho sai số bám
quỹ đạo e(t) tiến về 0 ( lim e(t) = 0 ). Để thực hiện được điều này thì hệ thống
t→
dẫn đường LOS sử dụng nguyên lý Lookahead-based Steering để tính toán hướng đi mong muốn d và từ đó tính ra góc mũi tàu mong muốn d cung cấp cho hệ thống máy lái tự động. Hệ thống máy lái tự động sẽ sử dụng d như là tín hiệu điều khiển đầu vào nhằm điều khiển con tàu bám theo quỹ đạo mong muốn và làm cho sai số bám quỹ đạo của con tàu giảm dần về 0.
Phương pháp điều khiển tàu bằng nguyên lý Lookahead-based Steering sẽ làm cho sai số bám quỹ đạo e(t) tiến về 0 bằng cách hướng vector vận tốc của AUV đến điểm chuyển hướng kế tiếp trên quỹ đạo mà con tàu cần phải đi đế n hay cũng chính là điể m chuyể n hướng trê n đoạn thẳng quỹ đạo mà
con tàu đang bám theo như hình 3.10.
Theo tài liệu [31] thì hướng đi mong muốn d theo nguyên lý Lookahead-based Steering sẽ được tách thành hai thành phần như sau:
d = p + r (e) (3.57) Trong đó p là góc tiếp tuyến của đoạn thẳng quỹ đạo mong muốn còn
r là góc tương quan giữa vận tốc và quỹ đạo chuyển. Góc r được thêm vào d nhằm đảm bảo rằng vector vận tốc U của con tàu luôn hướng về điểm chuyển hướng pn+ . k 1 p =ak (3.58) ( ) =(− ( ) ) (3.59) Δ
Trong đólà khoảng cách từ hình chiếu của điểm p n (t) trên đoạn thẳng quỹ đạo mong muốn cho đến điểm chuyển hướng pn + .
k 1
Từ (3.57), (3.58) và (3.59) thì ta có thể tính được hướng đi mong muốn (desired heading) theo nguyên lý Lookahead-based Steering như sau:
d = d − = p + r − (3.60)
v là góc trượt của con tàu. Tuy nhiên vì v U Trong đó = arcsin
U
nên ta có thể giả thiết như0 .
Khi đó (3.60) được viết lại thành:
pn
+
Khoảng cách từ vị trí AUV đến điểm chuyển hướng được tính như sau: n ( ) n ( ) n x − x t 2+ y − y t 2 R2 (3.62)
Đây là điều kiện để AUV chuyển sang điểm chuyển hướng tiếp theo. Trong trường hợp không gian 3 chiều (3D), độ sâu điểm chuyển hướng được tính đến:
[ − ( )]2 + [ − ( )]2 + [ − ( )]2 ≤ 2 (3.63)
Trong đề tài này, tác giả điều khiển AUV theo quỹ đạo vuông góc và zig-zag. Các kết quả mô phỏng được trình bày ở Chương 4.