Bộ giáo dục v đo tạo Bộ quốc phòng
Viện khoa học v công nghệ quân sự

TRƯƠNG DUY TRUNG

Xây dựng THUậT TOáN dẫn đờng
v ĐIềU KHIểN CHO phơng tiện ngầm Luận án tiến sĩ kỹ thuật
H nội 2014
Bộ giáo dục v đo tạo Bộ quốc phòng
Viện khoa học v công nghệ quân sự

nào công bố trong bất cứ một công trình nào khác. Tác giả luận án Trương Duy Trung

ii







MỤC LỤC
DANH MỤC CÁC KÝ HIỆU, VIẾT TẮT vi
DANH MỤC ĐỒ THỊ, HÌNH VẼ xi
PHẦN MỞ ĐẦU 1
Chương 1: TỔNG QUAN VỀ DẪN ĐƯỜNG VÀ ĐIỀU KHIỂN CHO PHƯƠNG
TIỆN NGẦM 5
1.1 Tổng quan về phương tiện ngầm 5
1.2 Các hệ tọa độ sử dụng trong mô tả chuyển độ
ng của phương tiện ngầm 15
1.2.1 Hệ tọa độ quán tính 15
1.2.2 Hệ tọa độ cố định tâm trái đất 15
1.2.3 Hệ tọa độ địa lý 16
1.2.4 Hệ tọa độ gắn liền 16
1.2.5 Ma trận chuyển tọa độ 17
1.2.5.1 Phương pháp góc Ơle 17
1.2.5.2 Phương pháp sử dụng tham số Rodrig – Hamilton 20
1.3 Tổng quan về dẫn đường quán tính 22
1.3.1 Nguyên tắ
c dẫn đường quán tính 22
1.3.1.1 Dẫn đường quán tính có đế 24
1.3.1.2 Dẫn đường quán tính không đế 27
1.3.2 Kết hợp các hệ thống định vị và dẫn đường 28
1.4 Mô tả động học phương tiện ngầm tự hành dạng ngư lôi 31
1.4.1 Các lực, mô men quán tính và hướng tâm của phương tiện ngầm tự hành 32
1.4.2 Các lực và mô men ngoại lực tác động lên phương tiện ngầm tự hành 33
1.4.2.1 Các lực và mô men gây ra bởi tr
ọng lực và lực nổi 33

3.1.2 Phương trình chuyển động trong các mặt phẳng 76
3.1.2.1 Phương trình chuyển động trong mặt phẳng đứng 76
3.1.2.2 Phương trình chuyển động trong mặt phẳng ngang 78
3.1.2.3 Phương trình chuyển động theo góc lắc 79
3.2 Nhận dạng mô hình vũ khí chống ngầm 79
3.3 Điều khiển hồi tiếp đầu ra nơron mờ thích nghi trực tiếp cho vũ khí chống ngầm 82
3.4 Dẫn đường cho vũ khí chống ngầm hiệu ch
ỉnh quỹ đạo sau khi chạm nước 91
3.4.1 Phương pháp dẫn đường cho vũ khí chống ngầm trong mặt phẳng ngang 91
3.4.2 Phương pháp dẫn đường cho vũ khí chống ngầm trong mặt phẳng đứng 94
3.5 Kết luận chương 3 96
Chương 4: MÔ PHỎNG KIỂM NGHIỆM THUẬT TOÁN NHẬN DẠNG, DẪN
ĐƯỜNG VÀ ĐIỀU KHIỂN CHO VŨ KHÍ CHỐNG NGẦM 97
4.1 Mô phỏng xác định tham số dẫn
đường cho vũ khí chống ngầm 97
v



4.1.1 Xây dựng các hàm động học và hàm quan sát 97
4.1.2 Thực hiện bộ lọc Kalman 99
4.1.3 Kết quả mô phỏng 100
4.1.3.1 Giai đoạn vũ khí chống ngầm chuyển động trong khí quyển 100
4.1.3.2 Giai đoạn vũ khí chống ngầm chuyển động trong nước 101
4.2 Mô phỏng xác định ma trận Côsin định hướng giữa hệ tọa độ đế và hệ tọa độ
địa lý theo phương pháp phối hợp véc t
ơ vận tốc. 102
4.2.1 Trường hợp thả vũ khí chống ngầm từ máy bay phản lực 102
4.2.2 Trường hợp thả vũ khí chống ngầm từ máy bay lên thẳng 103
4.3 Mô phỏng nhận dạng tham số mô hình vũ khí chống ngầm 104

MN

Các mô men ngoại lực tác dụng lên AUV (Nm)
,,uvw
Các thành phần vận tốc chiếu lên các trục hệ tọa độ gắn liền (m/s)
,,
cc c
uvw
Các thành phần vận tốc dòng chảy đối với hệ tọa độ gắn liền (m/s)
,,pqr
Các thành phần vận tốc góc chiếu lên hệ tọa độ gắn liền (rad/s)
,,
x
yz

Vị trí AUV trong hệ tọa độ địa lý (m)
,,



Các góc Ơle (rad)u
X

Hệ số lực khối nước kèm (kg)
,,,
wq qq vr rr
XXXX


(kg/rad)
l
uu
X

Hệ số lực của bánh lái (kg/m.rad)
p
l
X Lực đẩy động cơ (N)
,
vr
YY

Hệ số lực khối nước kèm (kg)
||vv
Y Hệ số lực cản (kg/m)
,
wp pq
YY Hệ số lực khối nước kèm chéo trục
b
Y
(kg/rad)
uv
Y

Hệ số khối nước kèm chéo trục, lực nâng và lực cản (kg/rad)
ur
Y
Hệ số khối nước kèm chéo trục, lực nâng (kg/rad)


s
uu
Z

Hệ số lực của bánh lái (kg/m.rad)
p
K


Hệ số mô men khối nước kèm (kg.m
2
/rad
2
)
l
uu
K

Hệ số mô men bánh lái đối với hệ tọa độ gắn liền (kg/rad)
uu
K
Hệ số mô men khối nước kèm (kg/rad)
up
K
Hệ số mô men khối nước kèm chéo trục (kg/rad)
,
qw
M
M


Hệ số mô men khối nước kèm (kg.m
2
/rad
2
)
ur
N
Hệ số mô men khối nước kèm chéo trục và lực nâng (kg.m/rad)
uv
N
Hệ số mô men thân và bánh lái (kg)
,
wp pq
NN

Hệ số mô men khối nước kèm chéo trục (kg.m/rad)
h
uu
N

Hệ số mô men bánh lái (kg/rad)
B

Lực nổi (lực Acsimet) (N)
W

Lực trọng lực (N)
D


,
hh


Góc quay các bánh lái hướng (rad)
viii



12
,
s
s


Góc quay các bánh lái sâu (rad)
h

Tổng góc bẻ lái điều khiển theo góc hướng (rad)
l

Tổng góc bẻ lái điều khiển theo góc lắc (rad)
s

Tổng góc bẻ lái điều khiển theo góc chúc ngóc (rad)
R
B
M
Ma trận quán tính của AUV
R


Véc tơ lực và mô men liên quan đến trọng lực và lực nổi
()L


Ma trận thông số lực và mô men của bánh lái
bl

Véc tơ lực và mô men của bánh lái
p
l

Véc tơ lực và mô men của động cơ đẩy
f
C Tâm nổi của AUV
R

,
l
R

Bán kính cong của trái đất theo tham chiếu Ellip
R

Bán kính của trái đất khi xem trái đất là hình cầu
V Véc tơ vận tốc dài trong hệ tọa độ gắn liền

Véc tơ vận tốc góc trong hệ tọa độ gắn liền

Véc tơ vận tốc góc tuyệt đối của hệ tọa độ địa lý


Ma trận chuyển từ hệ tọa độ gắn liền sang hệ tọa độ địa lý
n
d
C

Ma trận chuyển từ hệ tọa độ đế sang hệ tọa độ địa lý
,,
x
yz
aaa
Chỉ số gia tốc cảm nhận có nhiễu đo đối với hệ tọa độ gắn liền
,,
bx by bz
aaa Chỉ số gia tốc cảm nhận đối với hệ tọa độ gắn liền
,,
x
yz
nnn
Chỉ số gia tốc cảm nhận có nhiễu đo đối với hệ tọa độ đế
,,
dx dy dz
nnn Chỉ số gia tốc cảm nhận đối với hệ tọa độ đế
,,
NED
f
ff
Chỉ số gia tốc cảm nhận đối với hệ tọa độ địa lý
456
,,www Các nhiễu đo của gia tốc kế


Sai số ước lượng

Hệ số quên
k

Véc tơ trọng số kết nối giữa lớp 3 và lớp 4 của mạng singleton
ˆ
k
e
Véc tơ sai số trạng thái ước lượng được
12
,,
ii i
kk k
A
AB
Các tập mờ
ˆ
()
kk
e

Véc tơ cơ sở mờ
f
k
u
Đầu ra của mạng singleton
I
II

IMU Đơn vị đo lường quán tính
Inertial Measurement Unit
INS Inertial navigation system
Hệ thống dẫn đường quán tính
GPS Global Positioning System
Hệ thống định vị toàn cầu
PD Proportional-Derivative Controller
Bộ điều khiển PD
PID Proportional-Integral-Derivative Controller
Bộ điều khiển PID
SISO Single Input Single Output
Hệ thống có một đầu vào, một đầu ra
UV Underwater Vehicle
Phương tiện ngầm
ROV Underwater Remotely Operated Vehicles
Phương tiện ngầm điều khiển từ xa

xi



DANH MỤC CÁC BẢNG
Bảng 1.1: Quan hệ giữa các Côsin chỉ phương và các số quaternion 21
DANH MỤC HÌNH VẼ, ĐỒ THỊ
Hình 1: Quỹ đạo chuyển động của ASWs khi thả từ máy bay……….……… ………. 2
Hình 1.1: Phương tiện ngầm điều khiển từ xa 5
Hình 1.2: Phương tiện ngầm có người lái 6
Hình 1.3: Phương tiện ngầm tự hành 7
Hình 1.4: Mô hình tên lửa chống ngầm AПР
-2Э 8

ệ tọa độ địa lý 38
Hình 2.1: Chức năng của hệ thống điều khiển vũ khí chống ngầm 41
Hình 2.2: Cấu trúc hệ thống điều khiển vũ khí chống ngầm 42
Hình 2.3: Bố trí con quay vi cơ đo tốc độ góc, gia tốc kế và từ kế 43
Hình 2.4: Sơ đồ xác định tham số dẫn đường khi kết hợp con quay vi cơ, gia tốc k
ế
và từ kế 49
Hình 2.5: Sơ đồ xác định tham số dẫn đường khi kết hợp con quay vi cơ, gia tốc kế,
từ kế, vận tốc kế và cảm biến áp suất 58
xii



Hình 2.6: Quan hệ giữa các hệ tọa độ 59
Hình 2.7: Hình chiếu vận tốc góc tuyệt đối

của hệ tọa độ địa lý 61
Hình 2.8: Sơ đồ dẫn đường quán tính có đế 71
Hình 3.1: Vị trí các bánh lái trên ASWs 76
Hình 3.2: Lưu đồ thuật toán ước lượng tham số mô hình bằng phương pháp bình
phương tối thiểu đệ quy 81
Hình 3.3: Sơ đồ bộ điều khiển hồi tiếp đầu ra nơron mờ thích nghi trực tiếp 86
Hình 3.4: Cấu hình mạng nơ
ron mờ Singleton 86
Hình 3.5: Quỹ đạo hiệu chỉnh trong mặt phẳng ngang 92
Hình 3.6: Quỹ đạo hiệu chỉnh trong mặt phẳng đứng 94
Hình 4.1: Các tham số Rodrig-Hamilton khi ASWs chuyển động trong khí quyển 100
Hình 4.2: Các các góc Ơle khi ASWs chuyển động trong khí quyển 100
Hình 4.3: Các tham số Rodrig-Hamilton khi ASWs chuyển động trong nước 101
Hình 4.4: Các các góc Ơle khi ASWs chuyển động trong nước 102

góc lắc 115
1



PHẦN MỞ ĐẦU
1. Đặt vấn đề
Vùng biển Việt Nam trải dài, diện tích rộng hơn cả diện tích đất liền. Đây là
nguồn tài nguyên vô cùng lớn cho cuộc công nghiệp hóa, hiện đại hóa đất nước.
Xuất phát từ vị trí và tầm quan trọng của biển đối với sự phát triển kinh tế và củng
cố quốc phòng an ninh đặt ra yêu cầu cần thiết phải nghiên cứu, phát triển các lo
ại
phương tiện thực hiện các nhiệm vụ khảo sát, khai thác tài nguyên biển và các
nhiệm vụ quốc phòng an ninh. Trong những năm qua, chúng ta đã không ngừng
củng cố và xây dựng lực lượng Hải quân để bảo vệ biển, đảo và toàn vẹn lãnh thổ,
đặc biệt chú trọng đến các phương tiện ngầm trong đó có vũ khí chống ngầm (ngư
lôi, tên lửa chống ngầm).
Trong biên chế của Hải quân Việt Nam, các lo
ại vũ khí chống ngầm
(
Anti-
Submarine Weapons
– ASWs)
hiện có đều được mua từ các nước khác, tài liệu đi
kèm chỉ mang tính hướng dẫn sử dụng, các công trình công bố của nước ngoài chỉ
mang tính khái quát chung. Hiện nay chưa có các công trình nghiên cứu trong nước
công bố về các loại vũ khí chống ngầm, nếu có thì ở dạng sơ sài, không đầy đủ.
Trước yêu cầu xây dựng lực lượng Hải quân nhân dân hiện đại đòi h
ỏi phải làm chủ
được vũ khí trang bị và hướng tới sửa chữa, cải tiến, nâng cấp, sản xuất mới các loại

vòng quay xoắn lò xo. Sau khi ASWs đi về điểm này và có tư thế trùng với tư thế
tính toán của ASWs tại điểm này sẽ tiế
p tục chuyển động theo chương trình xoắn lò
xo tìm kiếm mục tiêu.
Để giải quyết vấn đề trên, luận án đề xuất việc cải tiến trang bị thêm thiết bị dẫn
đường quán tính (có đế hoặc không đế) cho vũ khí chống ngầm được thả từ máy
3



bay. Luận án đi sâu vào hai vấn đề cơ bản đó là vấn đề dẫn đường và điều khiển vũ
khí chống ngầm cải tiến nêu trên.
2. Mục đích nghiên cứu của luận án
Xây dựng phương pháp luận để tổng hợp thuật toán dẫn đường và thuật toán
điều khiển chuyển động cho phương tiện ngầm có trang bị thiết bị dẫn đường quán
tính có
đế hoặc không đế trong giai đoạn chuyển động tự lập (Autonom).
3. Đối tượng và phương pháp nghiên cứu
- Đối tượng nghiên cứu của luận án: Hệ thống điều khiển của phương tiện ngầm tự
hành dạng ngư lôi.
- Phương pháp nghiên cứu: Áp dụng các công cụ và phương pháp toán điều khiển
hiện đại để xây dựng thuật toán dẫn đường và đi
ều khiển. Dùng kỹ thuật mô
phỏng để đánh giá.
4. Ý nghĩa khoa học và ý nghĩa thực tiễn của luận án
- Kết quả nghiên cứu của luận án là cơ sở khoa học để xây dựng phần mềm cho hệ
thống điều khiển vũ khí chống ngầm khi có trang bị thiết bị dẫn đường quán tính.
- Kết quả luận án sẽ là cơ sở
để cải tiến, hiện đại hóa vũ khí chống ngầm hiện có
và khi thiết kế chế tạo mới.

toán góc tham chiếu sử dụng thông tin từ hệ thống dẫn đường và đề xuất thuật toán
điều khiển thích nghi hồi tiếp đầu ra nơron mờ thích nghi trực tiếp cho vũ khí chống
ngầm có 3 đầu vào, 3 đầu ra trong điều kiện có sai số mô hình và ảnh hưởng bởi
dòng chảy đại dương.
Chương 4: Mô phỏng kiểm nghiệm thuật toán nhận dạng, dẫn đường và
điều khiển cho vũ khí chống ngầm
Sử dụng công cụ Matlab - Simulink mô phỏng kiểm nghiệm, khảo sát, đánh giá
các thuật toán xác định tham số dẫn đường đã xây dựng ở chương 2. Mô phỏng
nhận dạng tham số mô hình và điều khiển hồi tiếp đầu ra nơron mờ thích nghi trực
tiếp theo tham số của một chủng loại vũ khí chống ngầm.
Nội dung chính của luận án
được công bố trong 15 bài báo trong đó có 8 bài
công bố trên Tạp chí Nghiên cứu khoa học và công nghệ quân sự, thuộc Viện KH-
CN quân sự/BQP, 06 bài báo tại các hội nghị tự động hóa và cơ điện tử toàn quốc,
01 bài báo tại hội nghị quốc tế. Luận án được trình bày trong 119 trang A4, trong đó
có 52 hình vẽ và đồ thị, 2 phụ lục. 5



Chương 1
TỔNG QUAN VỀ DẪN ĐƯỜNG VÀ ĐIỀU KHIỂN
CHO PHƯƠNG TIỆN NGẦM

1.1 Tổng quan về phương tiện ngầm

nghiên cứu thiết kế chế tạo, thử nghiệm thiết bị điều khiển cho rô bốt ngầm mini hoạt
động trong vùng nước nông của Học viện Hải quân. Tuy nhiên các nghiên cứu trong
nước chưa được đưa vào sản xuất ứng dụng thực tiễn hay thương mại hóa.
- Phương tiện ngầm có người lái:

Hình 1.2: Phương tiện ngầm có người lái
Tàu ngầm là dạng phương tiện ngầm có người lái và đồng thời cũng có chế độ lái
tự hành như dạng phương tiện ngầm tự hành (hình 1.2) [39]. Hiện nay trên thế giới
đã có nhiều công trình nghiên cứu về mô hình động học, dẫn đường và điều khiển
cho phương tiện ngầm dạng tàu ngầm [24], [25], [39], các công bố của nghiên cứu
này dựa trên các mô hình đơ
n giản, chưa phải là các mô hình thực tế của các tàu
ngầm hiện đại hiện có.
- Phương tiện ngầm tự hành:
Phương tiện ngầm tự hành (Autonomous Underwater Vehicle – AUV) có nhiều
dạng thiết kế và dùng cho nhiều mục đích khác nhau như hiện các nhiệm vụ khảo
sát, khai thác tài nguyên biển và các nhiệm vụ quốc phòng an ninh. Về kết cấu,
cũng có nhiều loại khác khau, có kiểu thiết kế dạng rô bốt chuyể
n động trong nước
7



bằng các hệ thống đẩy có hướng trục đẩy theo các mặt phẳng khác nhau, có kiểu
thiết kế dạng ngư lôi dùng động cơ đẩy dạng chân vịt (hình 1.3).

Hình 1.3: Phương tiện ngầm tự hành 
Vũ khí chống ngầm (ASWs) là một trong các loại AUV dùng để tiêu diệt các
loại tàu ngầm. Vũ khí chống ngầm có thiết kế dạng ngư lôi thường có kết cấu dạng
thân tròn xoay dài, phía đầu có dạng parabol tròn xoay và đuôi có dạng thon dần để


Hình 1.4: Mô hình tên lửa chống ngầm AПР-2Э
Cả hai loại vũ khí chống ngầm này trong hệ thống điều khiển có một con quay
ba bậc tự do để xác định góc lắc và đầu tự dẫn thủy âm dạng thụ động có thể phát
hiện được mục tiêu có khoảng cách đến 1500m. Luận án đi sâu vào chủng loại vũ
khí chống ngầm này.
Nguyên lý hoạt động của loại vũ
khí chống ngầm này được mô tả như sau: Trên
cơ sở thông tin mục tiêu nhận được từ các phao thủy âm cảnh giới vùng biển, máy
bay sẽ bay đến vùng có mục tiêu ngầm và thả vũ khí chống ngầm. Giả thiết vũ khí
chống ngầm được thả ở chế độ có dù và rơi đúng điểm chạm nước tính toán (hình
1.5). Sau khi chạm nước vũ khí chống ngầm hoạt động theo chương trình tạo ra qu
ỹ
đạo xoắn hình lò xo. Trong quá trình chuyển động xoắn nếu đầu tự dẫn bắt được
tính hiệu thủy âm của mục tiêu thì sẽ tự động chuyển qua chế độ điều khiển tự dẫn
để đưa vũ khí chống ngầm đến gặp mục tiêu. Trong trường hợp sau khi chuyển
động theo chương trình vẫn không bắt được mục tiêu thì vũ khí chống ngầm sẽ tự
động kích ho
ạt khối thuốc nổ (phần chiến đấu) để tự hủy.
Nguyên nhân của việc không bắt được mục tiêu có thể là:
9



- Trong quá trình rơi trong khí quyển do điều kiện thời tiết (gió) vũ khí chống
ngầm chạm nước ở vị trí xa điểm chạm nước tính toán.
- Vũ khí chống ngầm rời khỏi máy bay không đúng thời cơ thả tính toán.

Hình 1.5: Quỹ đạo mong muốn và quỹ đạo hiệu chỉnh của ASWs
Vũ khí chống ngầm thả từ máy bay hiện có trong biên chế không được trang bị

00
70 75
sao cho khi chạm nước phần đầu của ASWs sẽ tiếp xúc trước. Điểm
chạm nước thực được xác định tại thời điểm có lực va chạm giữa vũ khí chống
ngầm và bề mặt nước, thời điểm này các bánh lái cũng được quy không để tránh lực
va đập và cơ cấu dù được tách ra khỏi vũ khí chống ngầm. Dẫn đường cho ASWs
trong nước bắt đầu t
ừ thời điểm này. Sau khi chạm nước 0.5
s
kênh điều khiển góc
lắc mở để đảm bảo ổn định góc lắc trong giới hạn
0
3

nhằm duy trì sự chuyển động
độc lập của ASWs trong mặt phẳng ngang và mặt phẳng đứng. ASWs lặn theo quán
tính đến độ sâu
20m
thì bắt đầu điều khiển về điểm tiếp cận quỹ đạo mong muốn
tính toán trước.
Luận án đi sâu vào vấn đề xác định quỹ đạo cần hiệu chỉnh và điều khiển vũ khí
chống ngầm chuyển động theo quỹ đạo hiệu chỉnh này.
Mỗi loại vũ khí chống ngầm khi quay trở trong nước bao giờ cũng nằm trong
giới hạ
n của bán kính lượn vòng cho phép vì thế ASWs không thể chuyển động
thẳng từ điểm ở độ sâu 20m sau khi chạm nước đến điểm tiếp cận quỹ đạo mong
muốn sao cho tư thế của ASWs tại điểm tiếp cận quỹ đạo mong muốn trùng với tư
thế tính toán trước tại điểm này.
Để ASWs vừa tiếp cận được quỹ đạo mong muốn vừ
a có tư thế trùng với tư thế



Hình 1.6: Quỹ đạo hiệu chỉnh của vũ khí chống ngầm trong mặt phẳng ngang Hình 1.7: Quỹ đạo hiệu chỉnh của vũ khí chống ngầm trong mặt phẳng đứng
Như vậy điểm kết thúc quá trình dẫn đường và điều khiển hiệu chỉnh quỹ đạo là
điểm giao nhau giữa quỹ đạo mong muốn và bề mặt quả cầ
u cho phép
C
, luận án
chỉ giới hạn giải quyết các vấn đề dẫn đường và điều khiển khi ASWs đi đến điểm
này, quá trình tìm kiếm mục tiêu theo chương trình tiếp theo không xét.
Từ những phân tích ở trên cho thấy cần phải liên tục xác định vị trí và tư thế của
vũ khí chống ngầm trong quá trình rơi trong khí quyển và khi chuyển động dưới
nước ở giai đoạn hiệu chỉnh quỹ
đạo. Việc trang bị thêm thiết bị dẫn đường quán
tính và một số thiết bị đo khác hoàn toàn khả thi.
A
SWs
R


góc hướng quỹ đạo hiệu chỉnh
tại điểm bắt đầu quá trình điều khiển
b
X
0
X
0
Z

n
R

LOS


qd


O
T
C
LOS

góc chúc ngóc quỹ
đạo hiệu chỉnh tại điểm
bắt đầu quá trình điều
khiển