ĐẠI HỌC QUỐC GIA TP. HCM
TRƯỜNG ĐẠI HỌC BÁCH KHOA
--------------------

NGUYỄN NGỌC TUỆ

NGHIÊN CỨU THIẾT KẾ VÀ ĐIỀU KHIỂN
ROBOT DƯỚI NƯỚC DẠNG AUV
DESIGN AND CONTROL AN AUTONOMOUS
UNDERWATER VEHICLE
Chuyên ngành : Kỹ thuật cơ điện tử.
Mã số:60520114

LUẬN VĂN THẠC SĨ

TP. HỒ CHÍ MINH, tháng 5 năm 2019


CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI
TRƯỜNG ĐẠI HỌC BÁCH KHOA –ĐHQG -HCM
Cán bộ hướng dẫn khoa học : PGS.TS Võ Tường Quân
(Ghi rõ họ, tên, học hàm, học vị và chữ ký)
Cán bộ chấm nhận xét 1 : PGS.TS Nguyễn Thanh Phương
(Ghi rõ họ, tên, học hàm, học vị và chữ ký)
Cán bộ chấm nhận xét 2 : TS Nguyễn Vĩnh Hảo
(Ghi rõ họ, tên, học hàm, học vị và chữ ký)
Luận văn thạc sĩ được bảo vệ tại Trường Đại học Bách Khoa, ĐHQG Tp. HCM
ngày 01 tháng 07 năm 2019
Thành phần Hội đồng đánh giá luận văn thạc sĩ gồm:
(Ghi rõ họ, tên, học hàm, học vị của Hội đồng chấm bảo vệ luận văn thạc sĩ)
1. PGS.TS Nguyễn Tấn Tiến.

2. TS Phùng Trí Cơng
3. PGS.TS Nguyễn Thanh Phương
4. TS Nguyễn Vĩnh Hảo
5. TS Phạm Công Bằng
Xác nhận của Chủ tịch Hội đồng đánh giá LV và Trưởng Khoa quản lý chuyên
ngành sau khi luận văn đã được sửa chữa (nếu có).
CHỦ TỊCH HỘI ĐỒNG

TRƯỞNG KHOA CƠ KHÍ


Luận văn tốt nghiệp

ĐẠI HỌC QUỐC GIA TP.HCM
TRƯỜNG ĐẠI HỌC BÁCH KHOA

CỘNG HÒA XÃ HỘI CHỦ NGHĨA VIỆT
NAM Độc lập - Tự do - Hạnh phúc

NHIỆM VỤ LUẬN VĂN THẠC SĨ
Họ tên học viên: NGUYỄN NGỌC TUỆ

MSHV: 1570355

Ngày, tháng, năm sinh: 26/02/1991

Nơi sinh: TP.HCM

Chuyên ngành: Kỹ Thuật Cơ Điện Tử


Mã số : 60520114

I. TÊN ĐỀ TÀI :
Nghiên cứu thiết kế và điều khiển robot dưới nước dạng AUV.
II. NHIỆM VỤ VÀ NỘI DUNG:

III. NGÀY GIAO NHIỆM VỤ : ..............................................................................
IV. NGÀY HOÀN THÀNH NHIỆM VỤ: ...............................................................
V.

CÁN BỘ HƯỚNG DẪN : PGS. TS VÕ TƯỜNG QUÂN
Tp. HCM, ngày…. tháng…. năm 20....
CÁN BỘ HƯỚNG DẪN

CHỦ NHIỆM BỘ MÔN ĐÀO TẠO

(Họ tên và chữ ký)

(Họ tên và chữ ký)

TRƯỞNG KHOA CƠ KHÍ
(Họ tên và chữ ký)

2


Luận văn tốt nghiệp
LỜI CẢM ƠN
Xin được gửi lời cảm ơn chân thành và sâu sắc đến quý Thầy, Cô, đặc biệt nhất là PGS.TS
Võ Tường Quân, đã tạo điều kiện, tận tình hướng dẫn, truyền đạt những kiến thức và kinh

nghiệm q báu giúp tơi vững vàng hồn thành đề tài này. Đồng thời tạo nền tảng tri thức
giúp tôi tiếp tục nghiên cứu và làm việc.
Xin cảm ơn những bạn bè và đồng nghiệp luôn quan tâm và giúp đỡ tơi trong suốt q trình
học tập và nghiên cứu đề tài.
Trân trọng cảm ơn!
Tp. HCM, ngày 14 tháng 5 năm 2019

Nguyễn Ngọc Tuệ

3


Luận văn tốt nghiệp

TÓM TẮT LUẬN VĂN THẠC SĨ
Luận văn này tập trung vào việc mơ hình hóa mơ hình AUV 6 bậc tự do từ thiết kế cơ khí.
Sau đó sẽ tính tốn lại các hệ số thực nghiệm cũng như xem xét dòng chảy rối bằng phần
mềm CFD (Computational Fluid Dynamics). Thiết kế bộ điều khiển độ sâu với ba phương
pháp lặn là : lặn bằng cánh, lặn bằng đối trọng, lặn kết hợp cánh và đối trọng. Các bộ điều
khiển sẽ được dựa trên điều khiển mặt trượt, kết hợp với một số giải thuật để ước lượng một
số thông số chưa biết. Cuối cùng là mô phòng kết quả bằng Matlab.

4


Luận văn tốt nghiệp
ABSTRACT
This thesis focused on modeling the 6 DoFs AUV from the mechanical designed model.
Then I recalculate experimental equations as well as consider the stream line by using CFD
( Computational Fluid Dynamics) software. Designing the depth controller with three

method consists of diving by fin, by mass shifter mechanism and combines. I will use sliding
mode control in order to design the depth controller, combine with some estimate
algorithms. Finnally, I will simulate by Matlab.

5


Luận văn tốt nghiệp

LỜI CAM ĐOAN CỦA TÁC GIẢ
Tôi xin cam đoan : Luận văn thạc sĩ với đề tài “ Nghiên cứu thiết kế và điều khiển robot
dưới nước dạng AUV.” là cơng trình nghiên cứu của cá nhân tôi dưới sự hướng dẫn của
PGS. TS. Võ Tường Quân.
Các nội dung nghiên cứu, kết quá trong đề tài này là trung thực. Những số liệu, công thức,
phương pháp được thu thập từ các nguồn khác nhau có ghi rõ trong phần tài liệu tham khảo.
Tơi xin chịu hồn tồn trách nhiệm về cơng trình nghiên cứu của mình!
Tp. HCM, ngày 14 tháng 5 năm 2019.

Nguyễn Ngọc Tuệ

6


Luận văn tốt nghiệp

MỤC LỤC
MỤC LỤC
………………………………………………………………...7
DANH MỤC HÌNH ẢNH....................................................................................8
DANH MỤC BẢNG BIỂU ................................................................................10

CHƯƠNG 1 TỔNG QUAN VỀ AUV ..............................................................11
1.1 Giới thiệu. ……………………………………………………………………...11
1.2 Lịch sử phát triển. ................................................................................................. 11
1.3 Tình hình nghiên cứu trong nước. [4] ................................................................... 12
1.4 Các AUV thương mại hiện có. ............................................................................. 13
1.5 Đối tượng nghiên cứu. .......................................................................................... 14
1.6 Các bài toán khi điều khiển AUV. ........................................................................ 15
1.7 Các phương pháp lặn. ........................................................................................... 16

CHƯƠNG 2 MƠ HÌNH TỐN HỌC CỦA AUV. .........................................18
2.1 Phân tích chuyển động AUV. ............................................................................... 18
2.2 Động học [9] ......................................................................................................... 19
2.3 Động lực học [10] ................................................................................................. 20
2.3.1 Động học vật rắn. ............................................................................................. 20
2.3.2 Lực thủy tĩnh .................................................................................................... 21
2.3.3 Thủy động lực học. .......................................................................................... 24
2.4 Mô hình AUV 6 bậc tự do. ................................................................................... 38
2.5 Dạng ma trận ......................................................................................................... 39

CHƯƠNG 3 CÁC THÔNG SỐ CỦA AUV .....................................................42
3.1 Mở đầu. ……………………………………………………………………...42
3.2 Kiểm nghiệm dòng chảy khi AUV hoạt động ...................................................... 42
3.2.1 Giả thuyết bài toán và cách thực hiện .............................................................. 42
3.2.2 Dòng chảy qua thân AUV ............................................................................... 45
3.2.3 Dịng chảy qua cánh......................................................................................... 45
3.3 Tính tốn lại hệ số lực cản 𝑪𝒅 .............................................................................. 50

CHƯƠNG 4 THIẾT KẾ BỘ ĐIỀU KHIỂN ĐỘ SÂU ...................................53
4.1 Thuật toán dẫn đường. .......................................................................................... 53
4.2 Hoạch định đường đi cho AUV. ........................................................................... 53

4.3 Điều khiển độ sâu thông qua cánh lặn. ................................................................. 54
4.4 Điều khiển độ sâu thông qua đối trọng. ................................................................ 59
4.4.1 Bộ điều khiển Backstepping cho đối trọng. ..................................................... 59
4.4.2 Bộ điều khiển adaptive sliding mode cho độ sâu thông qua đối trọng ............ 61
4.5 Bộ điều khiển độ sâu dạng elevator. ..................................................................... 64
4.5.1 Bộ điều khiển độ sâu thông qua cánh. ............................................................. 65
4.5.2 Bộ điều khiển góc θ thơng qua đối trọng......................................................... 68
4.6 Nhiễu tác động. ..................................................................................................... 72
4.7 Kết Luận ……………………………………………………………………...74

CHƯƠNG 5 KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN .................................76
5.1 Kết quả đạt được. .................................................................................................. 76
5.2 Hướng phát triển. .................................................................................................. 76

7


Luận văn tốt nghiệp

DANH MỤC HÌNH ẢNH
Hình 1.1 Remus 100 [4] .......................................................................................13
Hình 1.2 Remus 600 [4] .......................................................................................13
Hình 1.3 Remus 6000. [4] ....................................................................................14
Hình 1.4 Dẫn động AUV. [6] ..............................................................................15
Hình 1.5 AUV có đối trọng [7] ............................................................................15
Hình 1.6 Lặn chúi đầu và lặn elevator [9] ...........................................................16
Hình 2.1 Tọa độ trong khơng gian của AUV.......................................................18
Hình 2.2 Trọng tâm và tâm nổi của AUV. ..........................................................22
Hình 2.3 Cận tích phân. .......................................................................................28
Hình 2.4 Hệ số t. ……………………………………………………………….31

Hình 2.5 Giá trị β [5]. ..........................................................................................33
Hình 2.6 Góc của cánh lái. [12] ...........................................................................36
Hình 2.7 Góc của cánh lặn. [12] ..........................................................................36
Hình 2.8 Lực tác động lên con trượt. ...................................................................37
Hình 3.1 AUV ……………………………………………………………….42
Hình 3.2 Chọn kiểu phân tích. .............................................................................43
Hình 3.3 Chọn loại lưu chất. ................................................................................44
Hình 3.4 Chọn vận tốc. ........................................................................................44
Hình 3.5 Chiều quay và vận tốc quay. .................................................................44
Hình 3.6 Hình dáng dịng chảy. ...........................................................................45
Hình 3.7 Dịng chảy ở đi AUV ........................................................................45
Hình 3.8 Phương trình Bernoulli cho mặt trên và dưới cánh. .............................46
Hình 3.9 Thất tốc xảy ra trên cánh. .....................................................................47
Hình 3.10 Đồ thị lực nâng. ..................................................................................48
Hình 3.11 AoA=0. ...............................................................................................49
Hình 3.12 AoA=19o. ............................................................................................49
Hình 3.13 AoA=20o. ............................................................................................50
Hình 3.14 Diện tích rẽ nước ................................................................................51
Hình 3.15 Mục tiêu tính tốn ...............................................................................52
Hình 4.1 Thuật tốn dẫn đường LOS...................................................................53
Hình 4.2 Sơ đồ khối bộ điều khiển độ sâu. ..........................................................54
Hình 4.3 Hiện tượng chattering [14]....................................................................55
Hình 4.4 Đáp ứng x- z..........................................................................................56
Hình 4.5 Sai số z ……………………………………………………………...57
Hình 4.6 Đáp ứng θ. .............................................................................................57
Hình 4.7 Tín hiệu điều khiển. ..............................................................................57
Hình 4.8 Đáp ứng z. .............................................................................................58
Hình 4.9 Sai số z……………………………………………………………….58
Hình 4.10 Đáp ứng θ. ...........................................................................................58
Hình 4.11 Tín hiệu điều khiển. ............................................................................59

Hình 4.12 Sơ đồ khối bộ điều khiển độ sâu thông qua đối trọng. .......................61
Hình 4.13 Đáp ứng Z. ..........................................................................................63
8


Luận văn tốt nghiệp
Hình 4.14 Sai số Z. ..............................................................................................63
Hình 4.15 Đáp ứng θ. ...........................................................................................63
Hình 4.16 Tín hiệu điều khiển. ............................................................................64
Hình 4.17 Ước lượng Mq .....................................................................................64
Hình 4.18 Sơ đồ khối bộ điều khiển neural network –sliding mode ...................65
Hình 4.19 Sơ đồ neural network. .........................................................................67
Hình 4.20 Đáp ứng Z ...........................................................................................70
Hình 4.21 Sai số ez ..............................................................................................70
Hình 4.22 Tín hiệu điều khiển 𝜹𝒔 ........................................................................71
Hình 4.23 Tín hiệu điều khiển Xg ........................................................................71
Hình 4.24 Đáp ứng góc θ .....................................................................................71
Hình 4.25 Giá trị ước lượng F1 ...........................................................................72
Hình 4.26 Giá trị ước lượng 𝑭𝜽...........................................................................72
Hình 4.27 Đồ thị x-z nhiễu 𝑫 = 𝟏 tác động ........................................................74
Hình 4.28 Đồ thị x-z nhiễu 𝑫 = 𝟐 tác động ........................................................74

9


Luận văn tốt nghiệp

DANH MỤC BẢNG BIỂU
Bảng
Bảng

Bảng
Bảng
Bảng
Bảng

2.1 Ký hiệu của SNAME...........................................................................18
2.2 Hệ số thực nghiệm α............................................................................26
2.3 Các thành phần ma trận Dp ..................................................................41
3.1 Bảng giá trị lực nâng và hệ số 𝒄𝒍𝜶 theo AOA ....................................48
3.2 Kết quả Cd ...........................................................................................52
4.1 Đánh giá bộ điều khiển........................................................................74

10


Luận văn tốt nghiệp

CHƯƠNG 1 TỔNG QUAN VỀ AUV
1.1 Giới thiệu.
Autonomous underwater vehicle (AUV) là một dạng mobile robot hoạt động trong môi
trường nước. AUV là thành viên của một nhóm các thiết bị khơng người lái dưới nước có
tên tiếng Anh là Unmanned underwater vehicle (UUV). Ngoài AUV, một nhánh khác của
UUV là Remotely operated underwater vehicle (ROV), khác với AUV là thiết bị có thể vận
hành hồn tồn tự động, ROV cần có thao tác điều khiển của con người [1].
AUV có nhiều ứng dụng to lớn đối với con người:
-

Thương mại: ngành cơng nghiệp hóa dầu dùng AUV để khảo sát là lập bản đồ chi
tiết của đáy biển trước khi xây dựng dàn khoan hay đặt đường ống.


-

Nghiên cứu: các nhà khoa học dùng AUV để nghiên cứu, thám hiểm hồ, biển và
đáy biển.

-

Quân sự: dò mìn, vận chuyển, tạo các điểm thơng tin liên lạc.

AUV hoạt động trong môi trường nước nên đem lại nhiều thách thức trong việc nghiên cứu
và phát triển. Đầu tiên do hoạt động trong môi trường nước nên AUV cần có các biện pháp
chống thấm. Tiếp theo là khó khăn trong việc truyền thông tin dưới nước, việc này gây ảnh
hưởng đến việc truyền nhận dữ liệu cũng như định vị AUV. Về mặt điều khiển, AUV chịu
nhiều nhiễu tác động từ bên ngoài tác động vào hệ thống như: gió, sóng, dịng chảy, lực
thủy tĩnh và thủy động. Tất cả các yếu tố trên gây khó khăn trong việc điều khiển và hoạt
động của AUV. [2]

1.2 Lịch sử phát triển.
AUV đầu tiên được phát triển tại Applied Physics Laboratory thuộc đại học Washington
vào năm 1957 bởi Stan Murphy và Bob Francois và sau đó là Terry Ewart. Một trong những
AUV đầu tiên cũng phát triển tại Massachusetts Institute of Technology vào thập niên 70
[1].
Trước 1970 – Nghiên cứu các ứng dụng đầu tiên của AUV.
AUV bắt đầu được nghiên cứu từ thập niên 60. Vài AUV được nghiên cứu tập trung vào
các ứng dụng thu thập dữ liệu.
1970 – 1980 – Khám phá tiềm năng của AUV
Suốt thập niên 70, nhiều mơ hình được chế tạo và thử nghiệm. The University of
Washington APL phát triển UARS và SPURV để thu thập dữ liệu từ vùng Arctic. The
University of New Hampshire’s Marine System Engineering Laboratory ( bây giờ là the
Autonomous Undersea Systems Institute) phát triển thiết bị EAVE cùng với sự hỗ trợ của


11


Luận văn tốt nghiệp
Hải quân Mỹ ở San Diego. Dù có nhiều thử nghiệm thất bại nhưng cũng đã đặt nền móng
cho sự phát triển của AUV.
1980 – 1990 – Thử nghiệm với các mẫu.
Thập niên 80, có những phát triển vượt bậc ở các lĩnh vực khác có ảnh hưởng tích cực đến
việc nghiên cứu AUV. Phải kể đến đó là sự phát triển của khoa học máy tính, tạo ra các máy
tính nhỏ, tiêu thụ năng lượng thấp. Điều này có nghĩa là các AUV sẽ được lập trình với các
thuật tốn điều khiển phức tạp hơn phục vụ cho quá trình tự hành.
1990 – 2000 – Phát triển Goal Driven Tech.
Trong thập niên này các AUV có thể đi đến các mục tiêu đã xác định từ trước. Một số tổ
chức bắt đầu nghiên cứu tập trung vào các ứng dụng cụ thể khác nhau, tạo tiền đề thương
mại hóa AUV. 1996 Kongsberg giới thiệu AUV thương mại đầu tiên của hãng ứng dụng
trong cả lĩnh vực dân sự và quân sự.
2000 – 2010 – Thị trường thương mại phát triển.
Đây là thời kỳ AUV được sử dụng cho nhiệm vụ thương mại. Thập kỷ này đánh dấu công
nghệ AUV từ môi trường nghiên cứu và học thuật được đưa vào ngành cơng nghiệp đại
dương. [3]

1.3 Tình hình nghiên cứu trong nước. [4]
AUV có khả năng áp dụng trong nhiều lĩnh vực khác nhau, đặc biệt trong quân sự và thương
mại, nên được nhiều quốc gia đầu tư phát triển. Đặc biệt, lĩnh vực quân sự hiện là khách
hàng chủ yếu của AUV-tương tự như những gì diễn ra trên bầu trời đối với phương tiện bay
không người lái (UUV).
Ở trong nước với kỷ nguyên công nghệ và nền kinh tế đa chiều, tồn cầu hóa và tri thức,
việc phát triển các hệ thống cơng nghiệp có một vai trị quan trọng trong q trình cơng
nghiệp hóa, hiện đại hóa và bảo vệ đất nước. Hệ thống điều khiển công nghiệp là một phần

của lĩnh vực sản xuất cơng nghiệp; nó ngày càng được nhiều doanh nghiệp sử dụng và phát
triển để góp phần tạo ra giá trị cạnh tranh. Đặc biệt là các hệ thống điều khiển cho các
phương tiện phục vụ cho việc khảo sát, thăm dị, khai khống tài ngun biển và bảo vệ lãnh
hải của đất nước. Hơn nữa, nó góp phần trong mục tiêu “Chiến lược Biển” mà Đảng và Nhà
nước đang đầu tư phát triển. Ngoài ra, việc nghiên cứu về đại dương cũng rất cần các phương
tiện tự hành dưới nước (AUV) nhằm mục đích nâng cao hiệu quả kinh tế xã hội trong dân
sự cũng như các trang thiết bị hải quân trong quân sự ở nước ta.
Hiện nay việc nghiên cứu hoạt động của phương tiện ngầm bắt đầu được quan tâm ở trường
ĐH Bách khoa Hà Nội và trường ĐH Bách khoa TP. HCM, trường ĐH Giao thông vận tải
TP.HCM, Học viện Hải quân. Chắc chắn việc nghiên cứu chế tạo và sử dụng phương tiện
ngầm sẽ được quan tâm nhiều hơn, góp phần giải quyết nhiều nhiệm vụ trong kỹ thuật khai
thác dầu khí, viễn thơng, trong quốc phịng và trong giao thơng đường thủy…

12


Luận văn tốt nghiệp
1.4 Các AUV thương mại hiện có.
a) Remus 100.

Hình 1.1 Remus 100 [4]
Thơng số kỹ thuật:
Đường kính : 19cm.
Chiều dài : từ 160cm.
Cân nặng trên cạn : 37kg.
Độ sâu tối đa : 100m.
Thời gian hoạt động : 8-10 giờ, tùy vào cấu hình và nhiệm vụ.
Động cơ : DC brushless dẫn động trực tiếp với chân vịt 3 cánh.
Tốc độ : lên đến 2.3m/s. Tùy thuộc cầu hình sensor.
b) Remus 600.


Hình 1.2 Remus 600 [4]
Thơng số kỹ thuật:
Đường kính : 32.4cm.
Chiều dài : 3.25m phụ thuộc vào cấu hình module.
Cân nặng trên cạn : 240kg.
Độ sâu tối đa : 600m.
Thời gian hoạt động : lên đến 24 giờ, tùy vào cấu hình và nhiệm vụ.
Động cơ : DC brushless dẫn động trực tiếp với chân vịt 2 cánh.
Tốc độ : lên đến 2.3m/s. Tùy thuộc cầu hình sensor.

13


Luận văn tốt nghiệp
Điều khiển: 3 cánh độc lập ở đi điều khiển góc roll,pitch,yaw, độ sâu, track line
Điều hướng: Inertial, Long Baseline (LBL) Acoustic, SBAS enabled GPS, Ultra Short
Baseline Acoustic and Acoustic Transponder.
c) Remus 6000.

Hình 1.3 Remus 6000. [4]
Thơng số kỹ thuật:
Đường kính : 71cm.
Chiều dài : 3.84m phụ thuộc vào cấu hình module.
Cân nặng trên cạn : 862kg.
Độ sâu tối đa : 6000m.
Thời gian hoạt động : lên đến 22 giờ, tùy vào cấu hình và nhiệm vụ.
Động cơ : DC brushless dẫn động trực tiếp với chân vịt 2 cánh.
Tốc độ : lên đến 2.3m/s. Tùy thuộc cầu hình sensor.
Điều khiển: điều khiển yaw và pitch bằng cánh. Độ sâu, track-line.

Điều hướng: Long Baseline Transducer (7-15 kHz upward looking transducer) and Dead
Reckon with ADCP Inertial Navigation System (INS).

1.5 Đối tượng nghiên cứu.
Đối tượng nghiên cứu trong luận văn này dạng AUV có bốn cánh (fin) đặt đối xứng nhau
gồm hai cánh lái (rudder), hai cánh lặn (stern plan) và một chân vịt (propeller) như hình 1.4.
Nhiệm vụ của các cánh là để điều khiển hướng còn chân vịt sẽ tạo lực đẩy cho AUV. Hai
cánh rudder kết hợp với propeller sẽ tạo góc yaw cho AUV. Như vậy góc quay rudder và
vận tốc của propeller sẽ tạo nên các chuyển động trong mặt phẳng song song với mặt phẳng
OEXEYE. Khi điều hướng AUV sẽ sử dụng cánh lái kết hợp với chân vịt tạo lực đẩy thay đổi
góc Yaw (heading).

14


Luận văn tốt nghiệp

Hình 1.4 Dẫn động AUV. [6]
Thêm vào đó AUV cịn có đối trọng để nhằm mục đích cân bằng theo chiều dọc cũng như
thay đổi trọng tâm để thực hiện việc cân bằng hoặc lặn xuống và nổi lên Hình 1.5.

Hình 1.5 AUV có đối trọng [7]

1.6 Các bài tốn khi điều khiển AUV.
Có ba bài tốn điều khiên AUV:
 Điều khiển vận tốc u.
 Điều khiển hướng (heading control).
 Điều khiển độ sâu (depth control).
Bài toán điều khiển vận tốc thường được thực hiện chung với hai bài tốn cịn để điều khiển
AUV tracking hoặc đi đến tọa độ mong muốn.

Bài toán điều khiển hướng là bài toán xác định giá trị tọa độ x-y và góc yaw (𝜓) trong mặt
phẳng XoY của AUV. Trong bài tốn này góc yaw (𝜓) ln được kiểm sốt chặt chẽ tại
mỗi tọa độ đặt của AUV vì nó bị ảnh hưởng bởi tọa độ đặt tiếp theo. Để đơn giản, AUV
thường được điều khiển đạt độ sâu trước sau đó mới điều khiển hướng ở độ sâu đó.

15


Luận văn tốt nghiệp
Bài toán điều khiển độ sâu giúp cho AUV đạt được độ sâu mong muốn hay nói cách khác
là xác định giá trị tọa độ x-z trong mặt phẳng XoZ . Khác với điều khiển hướng, khi AUV
đạt được độ sâu thì góc pitch (𝜃) mong muốn là bằng 0. Một vấn đề đặt ra nửa là phải giữ
được ổn định độ sâu trong quá trình điều hướng. Ta thấy bài toán độ sâu thường được giải
quyết trước bài toán điều khiển hướng.
AUV dạng torpedo thường được thiết kế đối xứng nên hai bài toán hướng và độ sâu sẽ tương
đối giống nhau. Như vậy ta thấy bài tốn điều khiển độ sâu có thể xem là tiền đề của điều
khiển hướng vì thế trong luận văn này tôi sẽ tập trung vào việc thiết kế bộ điều khiển độ
sâu.

1.7 Các phương pháp lặn.
Kỹ thuật lặn của các thiết bị lặn dưới nước được chia làm hai loại trong tài liệu [7] đó là lặn
tĩnh (static diving) và lặn động (dynamic diving). Lặn tĩnh là phương pháp bơm nước vào
hoặc các bồn chứa (ballast tank) để thay đổi trọng lượng của thiết bị lặn. Phương pháp này
thường được dùng cho các tàu ngầm lớn. Cịn AUV thì sử dụng phương pháp lặn động. Ban
đầu AUV sẽ chìm trong nước nhưng lơ lững ở tầng nước mặt. Nhờ vào góc nghiêng của
stern plane và propeller để tạo góc pitch. Như vậy góc nghiêng của stern plane và vận tốc
của propeller sẽ tạo nên chuyển động lặn cho AUV. Ngồi ra cịn có các AUV hoặc ROV
lặn bằng hoạt động của các propeller.
Trong luận văn này sẽ tập trung trình bày hai cách lặn đó là lặn chúi đầu và lặn dạng elevator.
Trong khi cách lặn chúi đầu được sử dụng với các khoảng cách lặn xa thì lặn dạng elevator

được dùng ở khoảng cách ngắn [9].

Hình 1.6 Lặn chúi đầu và lặn elevator [9]

16


Luận văn tốt nghiệp
Cách lặn chúi đầu có thể thực hiện bằng cánh lặn hoặc đối trọng. Cách lặn elevator thì phối
hợp giữa cánh và đối trọng, cánh đóng vai trị tạo lực để lặn cịn đối trọng có nhiệm vụ giữ
cho cho AUV luôn song song với mặt phẳng xOy.

17


Luận văn tốt nghiệp

CHƯƠNG 2 MƠ HÌNH TỐN HỌC CỦA AUV.
2.1 Phân tích chuyển động AUV.
Một AUV sẽ có sáu bậc tự do (6 Dof). Với sáu bậc tự do ta sẽ có thể xác định vị trí và hướng
của AUV trong khơng gian. Trong hình 2.1 cho ta thấy tọa độ của AUV trong không gian
với hệ tọa độ ObXbYbZb là tọa độ địa phương (body-fixed frame), OEXEYEZE là tịa độ tồn
cục hay hệ tọa độ gắn với trái đất (earth-fixed frame, e-frame), và bảng 1.1 chú giải các ký
hiệu theo The Society of Naval Architects and Marine Engineers (SNAME) [5].

Hình 2.1 Tọa độ trong khơng gian của AUV.
Theo SNAME sáu thành phần chuyển động của AUV gồm ba chuyển động tịnh tiến và ba
chuyển động xoay. Ba chuyển động tịnh tiến là: tịnh tiến theo phương Xb (surge), tịnh tiến
theo phương Yb (sway), tịnh tiến theo phương Zb (Heave). Ba chuyển động quay là quay
quanh trục Xb (roll), quay quanh trục Yb (pitch), quay quanh trục Zb (yaw). Ứng với mỗi

chuyển động theo phương Xb, Yb, Zb là chuyển vị xb, yb, zb. Tương tự với các chuyển động
quay ta cũng có góc quay ϕ, θ, ψ theo thứ tự là các góc roll, pitch, yaw.
Bảng 2.1 Ký hiệu của SNAME
Lực và

Bậc tự do
1

Surge

moment

Vận tốc dài và Vị trí và các
vận tốc góc
góc Euler

X

u

x

18


Luận văn tốt nghiệp
2

Sway


Y

v

y

3

Heave

Z

w

z

4

Roll

K

p

ϕ

5

Pitch


M

q

θ

6

Yaw

N

r

ψ

Ta có thể mơ tả các chuyển động trong bảng 1.1 thành dạng vector như sau [5]:
𝜂 = [𝜂1

𝜂 2 ]𝑇

𝜂1 = [𝑥

𝑦

𝑧 ]𝑇

𝜂2 = [𝜙

𝜃


𝜓]𝑇

𝜈 = [𝜈1

𝜈2 ]𝑇

𝜈1 = [𝑢

𝑣

𝑤]𝑇

𝜈2 = [𝑝

𝑞

𝑟 ]𝑇

𝜏 = [𝜏1

𝜏 2 ]𝑇

𝜏1 = [𝑋

𝑌

𝑍 ]𝑇

𝜏 2 = [𝐾


𝑀

𝑁 ]𝑇

Với η là vector biểu diễn vị trí và hướng của AUV trong hệ tọa độ toàn cục, ν là vector biểu
diễn vận tốc dài và vận tốc góc trong hệ tọa độ địa phương, và τ là vector biểu diễn lực và
moment tác dụng lên AUV trong hệ tọa độ địa phương.

2.2 Động học [9]
Phép biến đổi trục tọa độ sau đây cho ta quan hệ giữa vận tốc trong hệ tọa độ toàn cục và
vận tốc trong hệ tọa độ địa phương:
𝜂̇ 1 = 𝐽1 (𝜂2 )𝜈1 ,

(2.1)

Với 𝐽1 (𝜂2 ) là ma trận chuyển đổi,
c(𝜓) c(𝜃) −s(𝜓) c(𝜙) + s(𝜙) s(𝜃) c(𝜓) s(𝜓) s(𝜙) + s(𝜃) c(𝜓) c(𝜙)
𝐽1 (𝜂2 ) = [s(𝜓) c(𝜃) c(𝜓) c(𝜙) + s(𝜙) s(𝜃) s(𝜓) −c(𝜓) s(𝜙) + s(𝜃) s(𝜓) c(𝜙)]
− s(𝜃)
s(𝜙) c(𝜃)
c(𝜙) c(𝜃)

(2.2)

Ghi chú : c là cos, s là sin.
Ma trận 𝐽1 (𝜂2 ) là ma trận thuần nhất.
Phép biến đổi hệ trục sau đây cho ta qua hệ giữa vận tốc góc trong hệ tọa độ toàn cục và
vận tốc trong hệ toạ độ địa phương:
𝜂̇ 2 = 𝐽2 (𝜂2 )𝜈2 ,


(2.3)

Với
1
𝐽2 (𝜂2 ) = [0
0

sin(𝜙) tan(𝜃)
cos(𝜙)
sin(𝜙) / cos(𝜃)

cos(𝜙) tan(𝜃)
−sin(𝜙)
]
cos(𝜙) / cos(𝜃)

(2.4)

𝜋

Trong ma trận 𝐽2 (𝜂2 ) có điểm kỳ dị tại góc pitch 𝜃 = ± . Tuy nhiên, trong thực tế góc pitch
2

hầu như khơng vào vùng kỳ dị đó.

19


Luận văn tốt nghiệp

Kết hợp (1.1) và (1.3) ta có động học của AUV:
𝜂̇
𝐽 (𝜂 )
[ 1] = [ 1 2
𝜂̇ 2
03×3

03×3 𝜈1
][ ]
𝐽2 (𝜂2 ) 𝜈2

𝜂̇ = 𝐽(𝜂 )𝜈.

(2.5)

2.3 Động lực học [10]
Phương trình động lực học tổng quát của AUV có dạng như sau:
[𝑀𝑅𝐵 − 𝑀𝐴 ]𝑣̇ + 𝐶 (𝑣 )𝑣 + 𝐷 (𝑣 )𝑣 + 𝑔(𝜂 ) = 𝜏

(2.6)

Với
𝑀𝑅𝐵 : ma trận khối lượng.
𝑀𝐴 : ma trận khối lượng cộng gộp.
𝐶 (𝑣 ): ma trận coriolis.
𝐷 (𝑣 ): ma trận lực cản.
Phương trình (1.6) có thể được viết dưới dạng :
𝑣̇ = [𝑀𝑅𝐵 + 𝑀𝐴 ]

−1


(𝐶 (𝑣 )𝑣 + 𝐷 (𝑣 )𝑣 + 𝑔(𝜂 ) + 𝜏)

(2.7)

2.3.1 Động học vật rắn.
Từ công thức Newton-Euler cho vật rắn có khối lượng m ta có phương trình cân bằng lực
và moments:
𝐸
𝐸
𝐸
𝐸
𝑚[𝜈̇ 𝑂𝑏 + 𝜔̇ 𝑂𝑏
× 𝑟𝑂𝑏 + 𝜔𝑂𝑏
× 𝜈𝑂𝑏 + 𝜔𝑂𝑏
× (𝜔𝑂𝑏
× 𝑟𝑂𝑏 )] = 𝑓𝑂𝑏 ,
𝐸
𝐸
𝐸
𝐸
𝐼𝑂 𝜔̇ 𝑂𝑏
+ 𝜔𝑂𝑏
× 𝐼𝑂 𝜔𝑂𝑏
+ 𝑚𝑟𝑂𝑏 × (𝜈̇ 𝑂𝑏 + 𝜔𝑂𝑏
× 𝜈𝑂𝑏 ) = 𝑚𝑂𝑏 ,

(2.8)

Với (các ký hiệu này theo bảng 1.1)

𝑓𝑂𝑏 = [𝑋
𝑚𝑂𝑏 = [𝐾
𝜈𝑂𝑏 = [𝑢

𝑍]𝑇 là tổng hợp lực tác động vào AUV.

𝑌
𝑀
𝑣

𝑁]𝑇 là tổng moment tác động vào AUV.
𝑤]𝑇 là vận tốc dài của AUV.

𝐸
𝜔𝑂𝑏
= [𝑝 𝑞 𝑟]𝑇 là vận tốc góc của AUV.
𝑟𝑂𝑏 = [𝑥𝑔 𝑦𝑔 𝑧𝑔 ]𝑇 là vector tọa độ trọng tâm CG (center of gravity).

𝐼𝑥
𝐼𝑜 = [−𝐼𝑦𝑥
−𝐼𝑧𝑥

−𝐼𝑥𝑦
𝐼𝑦
−𝐼𝑧𝑦

−𝐼𝑥𝑧
−𝐼𝑦𝑧 ] là moment quán tính.
𝐼𝑧


Các thành phần 𝐼𝑥𝑦 , 𝐼𝑥𝑧 , 𝐼𝑦𝑧 = 0 vì AUV đối xứng qua mặt phẳng xz và xy.
Từ (2.8) phương trình động học vật rắn sáu bậc tự do của AUV:
𝑚[𝑢̇ − 𝑣𝑟 + 𝑤𝑞 − 𝑥𝑔 (𝑞2 + 𝑟 2 ) + 𝑦𝑔 (𝑝𝑞 − 𝑟̇ ) + 𝑧𝑔 (𝑝𝑟 + 𝑞̇ )] = ∑ 𝑋

(2.9)

20


Luận văn tốt nghiệp
𝑚[𝑣̇ − 𝑤𝑟 + 𝑢𝑟 − 𝑦𝑔 (𝑟 2 + 𝑝2 ) + 𝑧𝑔 (𝑞𝑟 − 𝑝̇ ) + 𝑥𝑔 (𝑝𝑞 + 𝑟̇ )] = ∑ 𝑌
𝑚[𝑤̇ − 𝑢𝑞 + 𝑣𝑝 − 𝑧𝑔 (𝑞2 + 𝑞2 ) + 𝑥𝑔 (𝑝𝑟 − 𝑞̇ ) + 𝑦𝑔 (𝑟𝑞 + 𝑝̇ )] = ∑ 𝑍
𝐼𝑥𝑥 𝑝̇ + (𝐼𝑧𝑧 − 𝐼𝑦𝑦 )𝑞𝑟 + 𝑚[𝑦𝑔 (𝑤̇ − 𝑢𝑞 + 𝑣𝑝) − 𝑧𝑔 (𝑣̇ − 𝑤𝑝 + 𝑢𝑟)] = ∑ 𝐾
𝐼𝑦𝑦 𝑞̇ + (𝐼𝑥𝑥 − 𝐼𝑧𝑧 )𝑟𝑝 + 𝑚[𝑧𝑔 (𝑢̇ − 𝑣𝑟 + 𝑤𝑞) − 𝑥𝑔 (𝑤̇ − 𝑢𝑞 + 𝑣𝑝)] = ∑ 𝑀
𝐼𝑧𝑧 𝑟̇ + (𝐼𝑦𝑦 − 𝐼𝑥𝑥 )𝑝𝑞 + 𝑚[𝑥𝑔 (𝑣̇ − 𝑤𝑝 + 𝑢𝑟) − 𝑧𝑔 (𝑢̇ − 𝑣𝑟 + 𝑤𝑞)] = ∑ 𝑁
Phương trình (2.9) có thể được viết thành:
𝑀𝑅𝐵 𝑣̇ + 𝐶𝑅𝐵 (𝑣 )𝑣 = 𝜏

(2.10)

Với:

𝑀𝑅𝐵

𝑚
0
0
=
0
𝑚𝑧𝑔

[−𝑚𝑦𝑔

0
𝑚
0
−𝑚𝑧𝑔
0
𝑚𝑥𝑔

0
0
𝑚
𝑚𝑦𝑔
−𝑚𝑥𝑔
0

𝐶𝑅𝐵 (𝜈) = [

0
𝑚𝑧𝑔 −𝑚𝑦𝑔
−𝑚𝑧𝑔
0
𝑚𝑥𝑔
𝑚𝑦𝑔 −𝑚𝑥𝑔
0
𝐼𝑥
−𝐼𝑥𝑦 −𝐼𝑥𝑧
−𝐼𝑦𝑥
𝐼𝑦
−𝐼𝑦𝑧

−𝐼𝑧𝑥 −𝐼𝑧𝑦
𝐼𝑧
]

03×3
𝐶𝑅𝐵 (𝜈)2

𝐶𝑅𝐵 (𝜈)1
]
𝐶𝑅𝐵 (𝜈)3

(2.11)

(2.12)

Với
𝑚(𝑦𝑔 𝑞 + 𝑧𝑔 𝑟)
𝐶𝑅𝐵 (𝜈)1 = [−𝑚(𝑦𝑔 𝑝 + 𝑤)
−𝑚(𝑧𝑔 𝑝 − 𝑣)
−𝑚(𝑦𝑔 𝑞 + 𝑧𝑔 𝑟)
𝐶𝑅𝐵 (𝜈)2 = [ 𝑚(𝑦𝑔 𝑝 − 𝑤)
𝑚(𝑧𝑔 𝑝 + 𝑣)
0
𝐶𝑅𝐵 (𝜈)3 = [ 𝐼𝑦𝑧 𝑞 + 𝐼𝑥𝑧 𝑝 − 𝐼𝑧 𝑟
−𝐼𝑦𝑧 𝑟 − 𝐼𝑥𝑦 𝑝 + 𝐼𝑦 𝑞

−𝑚(𝑥𝑔 𝑞 − 𝑤)
𝑚(𝑧𝑔 𝑟 + 𝑥𝑔 𝑝)
−𝑚(𝑧𝑔 𝑞 + 𝑢)


−𝑚(𝑥𝑔 𝑟 + 𝑣)
−𝑚(𝑦𝑔 𝑟 − 𝑢) ]
𝑚(𝑥𝑔 𝑝 + 𝑦𝑔 𝑞)

𝑚(𝑥𝑔 𝑞 + 𝑤)
−𝑚(𝑧𝑔 𝑟 + 𝑥𝑔 𝑝)
𝑚(𝑧𝑔 𝑞 − 𝑢)
−𝐼𝑦𝑧 𝑞 − 𝐼𝑥𝑧 𝑝 + 𝐼𝑧 𝑟
0
𝐼𝑥𝑧 𝑟 + 𝐼𝑥𝑦 𝑞 − 𝐼𝑥 𝑝

𝑚(𝑥𝑔 𝑟 − 𝑣)
𝑚(𝑦𝑔 𝑟 + 𝑢) ]
−𝑚(𝑥𝑔 𝑝 + 𝑦𝑔 𝑞)
𝐼𝑦𝑧 𝑟 + 𝐼𝑥𝑦 𝑝 − 𝐼𝑦 𝑞
−𝐼𝑥𝑧 𝑟 − 𝐼𝑥𝑦 𝑞 + 𝐼𝑥 𝑝]
0

2.3.2 Lực thủy tĩnh
Các thiết bị hoạt động dưới nước chịu tác động của hai lực tĩnh cơ bản là trọng lực được đặt
tại trọng tâm và lực nổi hay còn gọi là lực đẩy Archimède được đặt ở tâm nổi (center of

21


Luận văn tốt nghiệp
buoyance). Các AUV thường được thiết kế cẩn thận để trọng tâm và tâm nổi trùng nhau.
Tâm nổi thường được chọn là tâm của AUV. [8]

Hình 2.2 Trọng tâm và tâm nổi của AUV.

Trọng lực và lực Archimède được tính như cơng thức 1.10
𝑊 = 𝑚𝑔
𝐵 = 𝜌𝑔𝑉

(2.13)

Với:
W: trọng lực.
m: khối lượng.
g: gia tốc trọng trường.
B: lực đẩy Archimède.
ρ: khối lượng riêng.
V: thế tích AUV chiếm chỗ trong nước.
Trọng lực trong hệ tọa độ toàn cục:
𝜏 𝐺 = [0

0

𝑊 ]𝑇

(2.14)

Trọng lực trong hệ tọa độ địa phương:
𝜏𝐺′ = 𝐽1−1 (𝜂2 )𝜏𝐺

(2.15)

Với
𝜏𝐺′


𝑐(𝜓) 𝑐(𝜃)
= [−𝑠(𝜓) 𝑐(𝜙) + 𝑠(𝜙) 𝑠(𝜃) 𝑐(𝜓)
𝑠(𝜓) 𝑠(𝜙) + 𝑠(𝜃) 𝑐(𝜓) 𝑐(𝜙)

𝑠(𝜓)𝑐(𝜃)
− 𝑠(𝜃)
0
𝑐(𝜓) 𝑐(𝜙) + 𝑠(𝜙) 𝑠(𝜃) 𝑠(𝜓) 𝑐(𝜃)𝑠(𝜙) ] [ 0 ]
−𝑐(𝜓) 𝑠(𝜙) + 𝑠(𝜃) 𝑠(𝜓) 𝑐(𝜙) 𝑐(𝜙) 𝑐(𝜃) 𝑊

22


Luận văn tốt nghiệp

𝜏𝐺′

−𝑠𝑖𝑛(𝜃)𝑊
= [ 𝑐𝑜𝑠(𝜃)𝑠𝑖𝑛(𝜙)𝑊 ]
𝑐𝑜𝑠(𝜃)𝑐𝑜𝑠(𝜙)𝑊

(2.16)

Phương trình moment gây ra bởi trọng lực:
𝑦𝐺 cos(𝜃) cos(𝜙) 𝑊 − 𝑧𝐺 cos(𝜃) sin(𝜙) 𝑊
𝑀𝐺 = [ −𝑧𝐺 sin(𝜃) 𝑊 − 𝑥𝐺 cos(𝜃)cos(𝜙)𝑊 ]
𝑥𝐺 cos(𝜃) sin(𝜙) 𝑊 + 𝑦𝐺 sin(𝜃)𝑊

(2.17)


Tương tự ta có lực Archimede trong hệ tọa độ toàn cục:
𝜏 𝐺 = [0

0

−𝐵]𝑇

(2.18)

Lực đẩy Archimede trong hệ tọa độ địa phương:
𝜏𝐵′ = 𝐽1−1 (𝜂2 )𝜏𝐵

𝜏𝐵′

𝑐(𝜓) 𝑐(𝜃)
= [−𝑠(𝜓) 𝑐(𝜙) + 𝑠(𝜙) 𝑠(𝜃) 𝑐(𝜓)
𝑠(𝜓) 𝑠(𝜙) + 𝑠(𝜃) 𝑐(𝜓) 𝑐(𝜙)

𝜏𝐺′

𝑠(𝜓)𝑐(𝜃)
𝑐(𝜓) 𝑐(𝜙) + 𝑠(𝜙) 𝑠(𝜃) 𝑠(𝜓)
−𝑐(𝜓) 𝑠(𝜙) + 𝑠(𝜃) 𝑠(𝜓) 𝑐(𝜙)

sin(𝜃)𝐵
= [ −cos(𝜃)sin(𝜙)𝐵 ]
−cos(𝜃)cos(𝜙)𝐵

(2.19)


− 𝑠(𝜃)
0
𝑐(𝜃)𝑠(𝜙) ] [ 0 ]
𝑐(𝜙) 𝑐(𝜃) −𝐵

(2.20)

Phương trình moment gây ra bởi lực đẩy Archimede:
−𝑦𝐺 cos(𝜃) cos(𝜙) 𝐵 + 𝑧𝐺 cos(𝜃) sin(𝜙) 𝐵
𝑧𝐺 sin(𝜃) 𝐵 + 𝑥𝐺 cos(𝜃)cos(𝜙)𝐵
]
𝑀𝐵 = [
−𝑥𝐺 cos(𝜃) sin(𝜙) 𝐵 − 𝑦𝐺 sin(𝜃)𝐵

(2.21)

Gọi g(η) là lực và moment thủy tĩnh gây ra bởi lực trọng trường và lực đẩy Archimède.

23


Luận văn tốt nghiệp
𝑔 (𝜂 )
(𝑊 − 𝐵)sin(𝜃)
−(𝑊 − 𝐵) cos(𝜃) sin(𝜙)
−(𝑊 − 𝐵 ) cos(𝜃) cos(𝜙)
= − −(𝑦 𝑊 − 𝑦 𝐵) cos(𝜃) cos(𝜙) + (𝑧 𝑊 − 𝑧 𝐵) cos(𝜃) sin(𝜙)
𝑔
𝑏
𝑔

𝑏
(𝑧𝑔 𝑊 − 𝑧𝑏 𝐵) sin(𝜃) + (𝑥𝑔 𝑊 − 𝑥𝑏 𝐵) cos(𝜃) cos(𝜙)
[

−(𝑥𝑔 𝑊 − 𝑥𝑏 𝐵) cos(𝜃) sin(𝜙) − (𝑧𝑔 𝑊 − 𝑧𝑏 𝐵) sin(𝜃)

(2.22)

]

Với (xb, yb, zb) là tọa độ tâm nổi.

2.3.3 Thủy động lực học.
Lực thủy động gồm ba thành phần là :
Khối lượng cộng gộp (added mass): là khối lượng được cộng thêm vào hay còn gọi là khối
lượng ảo gây ra bởi quán tính của chất lỏng xung quanh.
Giảm chấn thủy động lực học (hydrodynamic damping): là lực cản của nước.
Lực thủy tĩnh (hydrostatic): lực đẩy Archimède.
Vector lực và moment thủy động lực học τH có dạng như sau: [5]
𝜏𝐻 = 𝑀𝐴 𝑣̇ + 𝐷 (𝑣 )𝑣 + 𝑔(η)

(2.23)

2.3.3.2 Khối lượng cộng gộp Ma và lực Coriolis cộng gộp.
Khối lượng cộng gộp có thể xem là khối lượng ảo được cộng thêm vào hệ bởi vì khi tăng
tốc hay giảm tốc thân AUV phải kéo theo một thế tích nước xung quanh nó. [9]
Ma trận khối lượng cộng gộp có dạng như sau:

𝑀𝐴 = −


𝑋𝑢̇
𝑌𝑢̇
𝑍𝑢̇

𝑋𝑣̇
𝑌𝑣̇
𝑍𝑣̇

𝑋𝑤̇
𝑌𝑤̇
𝑍𝑤̇

𝐾𝑢̇
𝑀𝑢̇
[ 𝑁𝑢̇

𝐾𝑣̇
𝑀𝑣̇
𝑁𝑣̇

𝐾𝑤̇
𝑀𝑤̇
𝑁𝑤̇

𝑋𝑝̇
𝑌𝑝̇
𝑍𝑝̇
𝐾𝑝̇
𝑀𝑝̇
𝑁𝑝̇


𝑋𝑞̇
𝑌𝑞̇
𝑍𝑞̇
𝐾𝑞̇
𝑀𝑞̇
𝑁𝑞̇

𝑋𝑟̇
𝑌𝑟̇
𝑍𝑟̇
𝐾𝑟̇
𝑀𝑟̇
𝑁𝑟̇ ]

(2.24)

Vì AUV đối xứng theo mặt phẳng xy và xz nên:
𝑋𝑢̇
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.25)

24