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ù

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

Chuyªn ngμnh: Kü thuËt ®iÒu khiÓn vμ tù ®éng hãa

M· sè: 62 52 02 16

LuËn ¸n tiÕn sÜ kü thuËt

Ng−êi h−íng dÉn khoa häc:

1. PGS.TS TrÇn §øc ThuËn
2. TS NguyÔn Quang VÞnh

Hμ néi 2014
 i

LỜI CAM ĐOAN

Tôi xin cam đoan đây là công trình nghiên cứu của riêng tôi. Những nội dung,
số liệu và kết quả trình bày trong luận án là hoàn toàn trung thực và chưa có tác giả
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

LỜI CẢM ƠN

Công trình nghiên cứu này được thực hiện tại Viện Tên lửa, Viện Tự động hoá
thuộc Viện Khoa học và Công nghệ Quân sự - Bộ Quốc phòng.
Tác giả xin bày tỏ sự biết ơn sâu sắc tới tập thể cán bộ giáo viên hướng dẫn
khoa học:
PGS. TS Trần Đức Thuận
TS Nguyễn Quang Vịnh
Đã tận tình hướng dẫn và giúp đỡ trong suốt quá trình thực hiện luận án.
Tác giả luận án xin chân thành cảm ơn Ban Lãnh đạo, Chỉ huy Viện Khoa học
và Công nghệ Quân sự, Phòng Đào tạo Viện Khoa học và Công nghệ quân sự, Viện
Tên lửa, Viện Tự động hoá các đồng nghiệp đã luôn động viên, quan tâm và giúp
đỡ để hoàn thành luận án.
Xin chân thành cám ơn các Thầy giáo, các nhà Khoa học và gia đình đã quan
tâm giúp đỡ, đóng góp nhiều ý kiến quý báu, cổ vũ và động viên tác giả hoàn thành
công trình khoa học này.
 iii

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
1.4.2.2 Các lực và mô men khối nước kèm ..................................................... 33
1.4.2.3 Các lực và mô men thủy động ............................................................. 34
1.4.2.4 Các lực và mô men của bánh lái ......................................................... 36
1.4.3 Các yếu tố môi trường tác động lên phương tiện ngầm tự hành.................. 38
1.5 Kết luận chương 1 .............................................................................................. 39
Chương 2: XÂY DỰNG THUẬT TOÁN XÁC ĐỊNH THAM SỐ DẪN ĐƯỜNG
CHO VŨ KHÍ CHỐNG NGẦM ............................................................................... 41
 iv

2.1 Xây dựng thuật toán dẫn đường quán tính không đế cho vũ khí chống ngầm ... 42
2.1.1 Xây dựng thuật toán xác định tham số dẫn đường cho vũ khí chống ngầm
ở giai đoạn chuyển động trong khí quyển .................................................... 43
2.1.2 Xây dựng thuật toán xác định tham số dẫn đường cho vũ khí chống ngầm ở
giai đoạn chuyển động trong nước ............................................................... 50
2.2 Xây dựng thuật toán dẫn đường quán tính có đế cho vũ khí chống ngầm ......... 59
2.2.1 Xây dựng thuật toán xác định ma trận Côsin định hướng giữa hệ tọa độ đế
với hệ tọa độ địa lý theo phương pháp phối hợp véc tơ vận tốc khi thả vũ
khí chống ngầm từ máy bay phản lực .......................................................... 60
2.2.2 Xây dựng thuật toán xác định ma trận Côsin định hướng giữa hệ tọa độ đế
với hệ tọa độ địa lý theo phương pháp phối hợp véc tơ vận tốc khi thả vũ
khí chống ngầm từ máy bay lên thẳng ......................................................... 66
2.2.3 Thuật toán dẫn đường .................................................................................. 70
2.3 Kết luận chương 2 .............................................................................................. 72
Chương 3: XÂY DỰNG THUẬT TOÁN NHẬN DẠNG VÀ ĐIỀU KHIỂN CHO
VŨ KHÍ CHỐNG NGẦM ........................................................................................ 73
3.1 Phương trình chuyển động của vũ khí chống ngầm ........................................... 73
3.1.1 Phương trình chuyển động tổng quát ........................................................... 73
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
4.3.1 Nhận dạng tham số mô hình vũ khí chống ngầm theo góc chúc ngóc ...... 104
4.3.2 Nhận dạng tham số mô hình vũ khí chống ngầm theo góc hướng ............ 106
4.4 Mô phỏng đ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....................................................................................................... 109
4.4.1 Mô phỏng điều khiển vũ khí chống ngầm theo góc hướng ....................... 109
4.4.2 Mô phỏng điều khiển vũ khí chống ngầm theo góc chúc ngóc ................. 111
4.4.3 Mô phỏng điều khiển vũ khí chống ngầm theo góc lắc ............................. 113
4.4.4 Mô phỏng đ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 tổng hợp theo góc hướng, góc chúc ngóc và góc lắc ...... 114
4.5 Kết luận chương 4 ............................................................................................ 116
KẾT LUẬN VÀ KIẾN NGHỊ................................................................................. 117
1. Kết luận ......................................................................................................... 117
2. Những đóng góp mới của luận án ................................................................. 118
3. Kiến nghị ....................................................................................................... 119
DANH MỤC CÁC CÔNG TRÌNH ĐÃ CÔNG BỐ CỦA TÁC GIẢ .................... 120
TÀI LIỆU THAM KHẢO ....................................................................................... 122
PHỤ LỤC A ……………………………………………………………………...A1
PHỤ LỤC B .……………………………………………………………………...B1
 vi

DANH MỤC CÁC KÝ HIỆU, VIẾT TẮT

X ,Y , Z Các ngoại lực tác dụng lên AUV (N)

K, M , N Các mô men ngoại lực tác dụng lên AUV (Nm)
u, v, w 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)
uc , vc , wc 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)
p, q, r 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, y , z Vị trí AUV trong hệ tọa độ địa lý (m)
 ,  , Các góc Ơle (rad)
X u Hệ số lực khối nước kèm (kg)
X wq , X qq , X vr , X rr Hệ số lực khối nước kèm chéo trục (kg/rad)

X u|u| , X v|v| Hệ số lực cản theo trục X b của hệ tọa độ gắn liền (kg/m)
X w|w| , X uv , X uw Hệ số lực cản theo trục X b của hệ tọa độ gắn liền (kg/m)
X up Hệ số lực bánh lái (kg/rad)
X uul Hệ số lực của bánh lái (kg/m.rad)
X pl Lực đẩy động cơ (N)
Yv , Yr Hệ số lực khối nước kèm (kg)
Yv|v| Hệ số lực cản (kg/m)
Ywp , Ypq Hệ số lực khối nước kèm chéo trục Yb (kg/rad)
Yuv Hệ số khối nước kèm chéo trục, lực nâng và lực cản (kg/rad)
Yur Hệ số khối nước kèm chéo trục, lực nâng (kg/rad)
Yuu h Hệ số lực của bánh lái (kg/m.rad)
Z w , Z q Hệ số lực khối nước kèm (kg)
Z w|w| Hệ số lực cản (kg/m)
Zuq Hệ số lực khối nước kèm chéo trục và lực nâng (kg/rad)
Z vp , Z rp Hệ số lực khối nước kèm chéo trục (kg/rad)
Z uw Hệ số lực khối nước kèm chéo trục, lực nâng và lực cản (kg/rad)
 vii

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

K p Hệ số mô men khối nước kèm (kg.m2/rad2)
Kuul Hệ số mô men bánh lái đối với hệ tọa độ gắn liền (kg/rad)
Kuu Hệ số mô men khối nước kèm (kg/rad)
Kup Hệ số mô men khối nước kèm chéo trục (kg/rad)
M q , M w Hệ số mô men khối nước kèm (kg.m2/rad2)
M uw Hệ số mô men thân và thành phần bánh lái (kg)
M rp Hệ số mô men khối nước kèm chéo trục (kg.m2/rad2)
M uq Hệ số mô men khối nước kèm chéo trục và lực nâng (kg.m/rad)
M vp Hệ số mô men khối nước kèm chéo trục (kg.m/rad)
M uu s Hệ số mô men bánh lái (kg/rad)
N v , N r Hệ số mô men khối nước kèm (kg.m2/rad2)
Nur Hệ số mô men khối nước kèm chéo trục và lực nâng (kg.m/rad)
Nuv Hệ số mô men thân và bánh lái (kg)
N wp , N pq Hệ số mô men khối nước kèm chéo trục (kg.m/rad)
Nuu h 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 Lực cản (N)
L Lực nâng (N)
 Mật độ nước (kg/m3)
Af Tổng diện tích bề mặt theo hướng vận tốc AUV (m2)
,  Các góc tấn công và góc trượt ngang của AUV (rad)
Sbl Diện tích bề mặt của bánh lái (m2)
e Góc ảnh hưởng của bánh lái (rad)
xbl Khoảng cách từ trục các bánh lái đến tâm trọng lực (m)
 h1 ,  h 2 Góc quay các bánh lái hướng (rad)
 viii

 s1 ,  s 2 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)
M RB Ma trận quán tính của AUV
CRB Ma trận hướng tâm Coriolis của AUV
 RB Véc tơ ngoại lực và mô men ngoại lực tác động lên thân AUV
Gb Tâm khối của AUV trong hệ tọa độ gắn liền
I0 Ma trận mô men đường chéo theo các trục hệ tọa độ gắn liền
MA Ma trận quán tính khối nước kèm
CA (  ) Ma trận hướng tâm Coriolis khối nước kèm
D(  ) Ma trận lực và mô men thủy động
g ( ) 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
 pl Véc tơ lực và mô men của động cơ đẩy
Cf Tâm nổi của AUV
R  , Rl 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ý
U Véc tơ vận tốc góc trái đất
 Véc tơ vận tốc đối tượng đối với hệ tọa độ cố định tâm trái đất
 Véc tơ vận tốc dài và vận tốc góc trong hệ tọa độ gắn liền
 Véc tơ vị trí và góc Ơle trong hệ tọa độ địa lý
l,  Kinh độ, vĩ độ
 ix

 Số siêu phức quaternion

0 , 1 , 2 , 3 Các tham số Rodrig – Hamilton
Cbn Ma trận chuyển từ hệ tọa độ gắn liền sang hệ tọa độ địa lý
Cdn Ma trận chuyển từ hệ tọa độ đế sang hệ tọa độ địa lý
ax , a y , az Chỉ số gia tốc cảm nhận có nhiễu đo đối với hệ tọa độ gắn liền
abx , aby , abz Chỉ số gia tốc cảm nhận đối với hệ tọa độ gắn liền
nx , n y , nz Chỉ số gia tốc cảm nhận có nhiễu đo đối với hệ tọa độ đế
ndx , ndy , ndz Chỉ số gia tốc cảm nhận đối với hệ tọa độ đế
fN , fE , fD Chỉ số gia tốc cảm nhận đối với hệ tọa độ địa lý
w4 , w5 , w6 Các nhiễu đo của gia tốc kế
 p , q , r Chỉ số của các con quay vi cơ bao gồm độ trôi và nhiễu đo
bp , bq , br Các tham số biến đổi chậm thể hiện độ trôi của con quay
w1 , w2 , w3 Nhiễu đo con quay vi cơ
Bx , By , Bz Chỉ số của từ kế đối với hệ tọa độ địa lý

VxDVL ,VyDVL ,VzDVL Chỉ số của vận tốc kế chiếu lên hệ tọa độ gắn liền
VN ,VE ,VD Các chỉ số vận tốc đối với hệ tọa độ địa lý
 Véc tơ tham số hệ thống
 (t ) Véc tơ hồi quy
 (t ) 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
eˆk Véc tơ sai số trạng thái ước lượng được
Aki 1 , Aki 2 , Bki Các tập mờ
k ( eˆk ) Véc tơ cơ sở mờ
u fk Đầu ra của mạng singleton
OX I Y I Z I Hệ tọa độ quán tính
OX eY e Z e Hệ tọa độ cố định tâm trái đất
 x

OX bYb Z b Hệ tọa độ gắn liền

OX 0Y0 Z 0 Hệ tọa độ địa lý
AUV Autonomous Underwater Vehicle
Phương tiện ngầm tự hành
ARX Auto-Regressive-eXternal input
Cấu trúc mô hình nhận dạng ARX
ASWs Anti-Submarine Weapons
Vũ khí chống ngầm
DAFNOC Direct Adaptive Fuzzy-Neural Output Feedback Controller
Bộ điều khiển hồi tiếp đầu ra nơron mờ thích nghi trực tiếp
DVL Doppler Velocity Log
Thiết bị đo vận tốc theo hiệu ứng Dople
MIMO Multi Input Multi Output
Hệ thống có nhiều đầu vào, nhiều đầu ra
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
Hình 1.5: Quỹ đạo mong muốn và quỹ đạo hiệu chỉnh của ASWs............................. 9
Hình 1.6: Quỹ đạo hiệu chỉnh của vũ khí chống ngầm trong mặt phẳng ngang .....11
Hình 1.7: Quỹ đạo hiệu chỉnh của vũ khí chống ngầm trong mặt phẳng đứng .......11
Hình 1.8: Hệ tọa độ quán tính...................................................................................15
Hình 1.9: Quan hệ của hệ tọa độ cố định tâm trái đất và hệ tọa độ địa lý ...............16
Hình 1.10: Hệ tọa độ gắn liền ...................................................................................16
Hình 1.11: Quay theo góc hướng  quanh trục Z 0 .................................................18
Hình 1.12: Quay theo góc chúc ngóc  quanh trục Y1 .............................................18
Hình 1.13: Quay theo góc lắc  quanh trục X 2 .....................................................19
Hình 1.14: Hệ tọa độ địa lý và thiết bị dẫn đường quán tính có đế dạng giải tích ... 24
Hình 1.15: Bộ lọc Kalman phi tuyến mở rộng .........................................................30
Hình 1.16: Biễu diễn AUV trong các hệ tọa độ ........................................................31
Hình 1.17: Biễu diễn lực cản, lực nâng, góc tấn công, góc trượt bên của AUV ....34
Hình 1.18: Vị trí, góc bẻ lái của bánh lái hướng và bánh lái sâu ............................36
Hình 1.19: Vận tốc dòng chảy đại dương trong hệ 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
Hình 4. 5: Các giá trị 0 , 1 , 2 , 3 đúng và ước lượng ............................................103
Hình 4.6: Các giá trị cij đúng và tính toán được ....................................................103
Hình 4.7: Kết quả nhận dạng và sai số nhận dạng theo góc chúc ngóc .................105
Hình 4.8: Giá trị các tham số mô hình ̂ theo góc chúc ngóc ước lượng được ....105
Hình 4.9: Kết quả nhận dạng và sai số nhận dạng theo góc hướng .......................107
Hình 4.10: Giá trị các tham số mô hình ̂ theo góc hướng ước lượng được ........108
Hình 4.11: Sơ đồ mô phỏng hệ thống điều khiển ASWs theo góc hướng ................109
Hình 4.12: Kết quả mô phỏng điều khiển ASWs theo góc hướng ...........................110
Hình 4.13: Sơ đồ mô phỏng hệ thống điều khiển ASWs theo góc chúc ngóc..........111
Hình 4.14: Kết quả mô phỏng điều khiển vũ khí chống ngầm theo góc chúc ngóc .. 112
Hình 4.15: Sơ đồ mô phỏng hệ thống điều khiển ASWs theo góc lắc .....................113
Hình 4.16: Kết quả mô phỏng điều khiển vũ khí chống ngầm theo góc lắc............114
Hình 4.17: Sơ đồ mô phỏng điều khiển ASWs theo góc chúc ngóc, góc hướng và
góc lắc .....................................................................................................................115
Hình 4.18: Kết quả mô phỏng điều khiển ASWs theo góc hướng, góc chúc ngóc và
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ũ khí chống ngầm. Vì vậy, nghiên cứu về mô tả toán học, xây dựng phương trình
chuyển động, nhận dạng, dẫn đường và điều khiển cho các loại vũ khí chống ngầm
là rất cần thiết.
Vũ khí chống ngầm có thể được phóng từ các loại tàu chiến trên mặt nước, tàu
ngầm hay có thể được thả từ máy bay (máy bay phản lực hay máy bay lên thẳng).
Luận án tập trung nghiên cứu lớp đối tượng vũ khí chống ngầm thả từ máy bay.
Các thông tin về mục tiêu (tàu ngầm đối phương) được xác định bởi hệ thống
Sonar (hệ thống phao có trang bị các đầu thu sóng siêu âm và có các đầu phát vô
tuyến). Từ các thông tin này, hệ thống máy tính trên máy bay xác định thời cơ thả
ASWs sao cho sau khi ASWs rơi xuống nước tại điểm chạm nước tính toán và
chuyển động theo chương trình (quỹ đạo xoắn lò xo) sẽ phát hiện được mục tiêu.
 2

Trong trường hợp ASWs rơi xuống nước ở chế độ có mở dù thì điểm chạm nước
thực tế có thể không trùng với điểm chạm nước tính toán. Nếu sai lệch này lớn hơn
giới hạn làm việc của đầu tự dẫn thì ASWs sau khi chuyển động theo chương trình
sẽ không phát hiện được mục tiêu và ngòi nổ được kích hoạt để tự hủy (hình 1).

Hình 1: Quỹ đạo chuyển động của ASWs khi thả từ máy bay
Từ quỹ đạo trên cho thấy sai số giữa điểm chạm nước tính toán và điểm chạm
nước thực do các nguyên nhân sau: sai số thời cơ thả ASWs và sai số vị trí ASWs
chạm nước do độ lệch quỹ đạo trong quá trình ASWs chuyển động trong khí quyển.
Để tăng khả năng phát hiện mục tiêu thì sau khi chạm nước, ASWs cần phải chuyển
động về điểm tiếp cận quỹ đạo thực đã xác định trước. Điểm tiếp cận quỹ đạo thực
là điểm nằm trên quỹ đạo mong muốn ở độ sâu trước độ sâu có mục tiêu một số
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.
5. Bố cục của luận án
Những vấn đề mà luận án giải quyết được phân bổ trong các chương:
Chương 1: Tổng quan về dẫn đường và điều khiển cho phương tiện ngầm
Đây là chương tổng quan về các loại phương tiện ngầm. Các nghiên cứu về mô
tả động học, dẫn đường và điều khiển phương tiện ngầm. Các hệ tọa độ tham chiếu,
phép biến đổi tọa độ, các lực, mô men tác động lên phương tiện ngầm. Trình bày
tổng quan về dẫn đường quán tính, ứng dụng bộ lọc Kalman phi tuyến mở rộng
trong dẫn đường quán tính. Xác định các vấn đề cần giải quyết cho luận án.
Chương 2: Xây dựng thuật toán xác định tham số dẫn đường cho vũ khí
chống ngầm
Chương 2 trình bày các thuật toán dẫn đường quán tính có đế và không đế. Xây
dựng thuật toán kết hợp các thiết bị đo để xác định các tham số dẫn đường cho vũ
khí chống ngầm trong cả giai đoạn chuyển động trong khí quyển và giai đoạn
 4

chuyển động trong nước. Xây dựng thuật toán xác định ma trận Côsin định hướng
giữa hệ tọa độ đế với hệ tọa độ dẫn đường của thiết bị dẫn đường quán tính có đế
trong trường hợp thả vũ khí chống ngầm từ máy bay phản lực bay ở tốc độ lớn và
trong trường hợp thả từ máy bay lên thẳng bay tốc độ thấp.
Chương 3: Xây dựng thuật toán nhận dạng và điều khiển cho vũ khí chống ngầm
Trong chương này trình bày hệ phương trình mô tả chuyển động tổng quát trong
hệ tọa độ 6 bậc tự do và các phương trình chuyển động theo các mặt phẳng của vũ
khí chống ngầm. Đề xuất ứng dụng thuật toán bình phương tối thiểu đệ quy để ước
lượng tham số mô hình vũ khí chống ngầm dựa vào tập mẫu dữ liệu vào ra, tính
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

Trải qua nhiều thập niên phương tiện ngầm đã được phát triển rất đa dạng về chủng
loại và dùng cho nhiều mục đích khác nhau như mục đích quân sự, cứu hộ, thám hiểm
đáy đại dương, thăm dò, khai thác tài nguyên biển.
Phương tiện ngầm (Underwater Vehicle – UV) có nhiều loại khác nhau, căn cứ
vào khả năng tham gia điều khiển của con người có thể phân thành hai loại: loại có
người lái và loại không người lái. UV không người lái được chia thành UV điều khiển
từ xa và UV tự hành. Dưới đây khảo sát một số dạng UV với các ứng dụng và đặc tính
làm việc khác nhau:
- Phương tiện ngầm điều khiển từ xa :
Có nhiều kiểu thiết kế phương tiện ngầm điều khiển từ xa (Underwater
Remotely Operated Vehicles – ROV), hầu hết ROV được thiết kế dạng rô bốt
chuyển động trong nước bằng các hệ thống đẩy có hướng trục đẩy theo các mặt
phẳng khác nhau (hình 1.1).

Hình 1.1: Phương tiện ngầm điều khiển từ xa

ROV có khả năng cơ động tốt, được liên kết với vị trí điều khiển bằng cáp [22].
Các ROV đã được nghiên cứu phát triển và thương mại hóa rộng rãi trên thế giới có
thể thực hiện nhiều nhiệm vụ dưới nước như kiểm tra đường ống ngầm, các hoạt
 6

động tìm kiếm, cứu hộ, kiểm tra kỹ thuật các công trình ngầm, kiểm tra về mặt sinh
thái các tham số của môi trường nước, khai thác, nghiên cứu khoa học và phục vụ
quân sự. Vì ROV được liên kết với vị trí điều khiển bằng cáp nên thường bán kính làm
việc của ROV không xa. Hiện nay trong nước có một số nghiên cứu như rô bốt cá của
nhóm nghiên cứu vốn là những sinh viên ở ĐH Sư phạm kỹ thuật TP HCM hay đề tài
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 để
giảm lực cản thủy động. Hệ thống lái của ASWs có các máy lái điều khiển các bánh
lái gồm hai bánh lái trong mặt phẳng ngang và hai bánh lái trong mặt phẳng đứng,
các bánh lái này có thể được điều khiển riêng biệt hay theo cặp. Một số loại vũ khí
chống ngầm sử dụng bánh lái trong mặt phẳng đứng để điều khiển theo góc hướng
và đồng thời điều khiển giảm lắc, một số loại có vây điều khiển giảm lắc. Hầu hết
các loại vũ khí chống ngầm đều được trang bị đầu tự dẫn và thiết bị nổ cảm ứng,
điều này cho phép tăng khả năng phát hiện và tiêu diệt mục tiêu. Kích thước, trọng
lượng và tốc độ của ASWs tùy thuộc vào thiết kế.
Các loại vũ khí chống ngầm (tên lửa, ngư lôi) có thể được phóng đi từ tàu chiến
trên mặt nước, tàu ngầm hay thả từ máy bay. Các loại ngư lôi chống ngầm chuyển
động trong nước bởi lực đẩy của chân vịt, nguồn điện cung cấp cho động cơ để
quay chân vịt thường là nguồn ác quy vì thế ngư lôi thường có khoảng cách làm
việc ngắn, tốc độ chuyển động thấp. Các loại tên lửa chống ngầm chuyển động
trong nước bởi lực đẩy của động cơ phản lực nhiên liệu rắn vì thế tốc độ chuyển
động của tên lửa chống ngầm nhanh hơn ngư lôi chống ngầm nhưng thời gian làm
việc của động cơ thường ngắn hơn ngư lôi chống ngầm vì vậy trong giai đoạn đầu
 8

quỹ đạo chuyển động trong nước của tên lửa chống ngầm thường sử dụng lực trọng
lực để đưa tên lửa về độ sâu nhất định mới mở động cơ hành trình.
Các loại vũ khí chống ngầm hiện có của Hải quân Việt Nam hầu hết là theo
thiết kế của Nga, trong đó có một số loại được thả từ máy bay tiêu diệt tàu ngầm
như tên lửa chống ngầm AПР-2Э (hình 1.4) [13] hay ngư lôi chống ngầm AT-1MЭ.

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ị
hệ thống dẫn đường nên không xác định được tọa độ điểm chạm nước. Khi sai số
điểm chạm nước lớn hơn khoảng cách phát hiện mục tiêu của đầu tự dẫn thì sau khi
chuyển động theo chương trình ASWs không thể phát hiện được mục tiêu. Để nâng
cao khả năng phát hiện mục tiêu, vấn đề đặt ra là phải đưa thêm thiết bị dẫn đường
quán tính và một số thiết bị đo khác khi cải tiến hoặc sản xuất mới các loại vũ khí
chống ngầm. Các tài liệu đi kèm với các loại vũ khí này do đối tác bàn giao chỉ
mang tính hướng dẫn sử dụng, không thể hiện các tham số động học và thuật toán
điều khiển nó. Trong nước cũng chưa có các báo cáo nghiên cứu về mô tả động học,
dẫn đường và điều khiển các loại vũ khí này. Để tiến tới cải tiến hoặc sản xuất mới
các loại vũ khí chống ngầm như trên đòi hỏi phải có những nghiên cứu có tính phát
triển của riêng Việt Nam. Những nghiên cứu phát triển về giải pháp, học thuật
 10

thường phải kèm theo là nghiên cứu về thiết kế công nghệ. Tuy nhiên trong giới hạn
của luận án chỉ tập trung nghiên cứu về giải pháp và học thuật. Việc đưa thêm thiết
bị dẫn đường quán tính vào hệ thống điều khiển cho phép xác định vị trí của vũ khí
chống ngầm và tư thế của nó khi đang rơi ở chế độ có dù trong khí quyển và khi
chuyển động trong nước. Nhờ có thiết bị dẫn đường quán tính có thể xác định các
tham số dẫn đường tại điểm chạm nước, từ nguồn thông tin này kết hợp với thông
tin về tọa độ, góc định hướng của ASWs tại điểm tiếp cận quỹ đạo mong muốn
trước khi thả ASWs, hệ thống điều khiển trên ASWs điều khiển hiệu chỉnh quỹ đạo
nhằm đưa ASWs về điểm tiếp cận quỹ đạo mong muốn tính toán trước (hình 1.5).
Quá trình ASWs rơi trong khí quyển các bánh lái duy trì góc chúc ngóc trong giới
hạn 700  750 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.5s 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 30 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ế
tính toán tại điểm này ta thiết kế một quả cầu có bán kính cho phép ( Rn ) quanh
điểm tiếp cận quỹ đạo mong muốn (T ) , bán kính Rn phù hợp với bán kính lượn
 11

vòng của ASWs. Khi ASWs đi vào trong quả cầu này sẽ bắt đầu quay theo góc
hướng và góc chúc ngóc đi về điểm giao nhau (C ) giữa quỹ đạo mong muốn và bề
mặt quả cầu cho phép sao cho tại điểm này tư thế của ASWs trùng với tư thế (góc
hướng  qd , góc chúc ngóc qd ) đã tính toán trước. Quỹ đạo hiệu chỉnh khi này vẽ

trong mặt phẳng ngang và mặt phẳng đứng như hình 1.6 và hình 1.7.

X0

RASWs

 qd RAWs bán kính vòng xoắn lò xo của

Rn T
quỹ đạo mong muốn trong mặt phẳng
C
Xb ngang
 LOS
 LOS góc hướng quỹ đạo hiệu chỉnh

tại điểm bắt đầu quá trình điều khiển

O Y0
Hình 1.6: Quỹ đạo hiệu chỉnh của vũ khí chống ngầm trong mặt phẳng ngang
O LOS X0 LOS góc chúc ngóc quỹ
Rn đạo hiệu chỉnh tại điểm
T
C bắt đầu quá trình điều
qd
Z0
Xb khiển

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.
 12

Như vậy có hai vấn đề đặt ra luận án cần giải quyết đó là:
- Vấn đề dẫn đường, tức là vấn đề xác định vị trí, vận tốc và tư thế của vũ khí
chống ngầm khi chuyển động trong khí quyển và khi chuyển động trong nước
trong giai đoạn hiệu chỉnh quỹ đạo.
- Vấn đề tạo lệnh điều khiển hiệu chỉnh quỹ đạo.
Việc xác định vị trí và tư thế của vũ khí chống ngầm sẽ là tiền đề để xác định
lệnh điều khiển hiệu chỉnh quỹ đạo. Để tạo lệnh điều khiển cần có mô hình toán mô
tả chuyển động của vũ khí chống ngầm.
Hiện nay trên thế giới đã có nhiều công trình nghiên cứu về mô tả động học của
AUV dạng ngư lôi [15], [24], [25], [39], [40]. Hầu hết các nghiên cứu này tập trung
vào AUV loại nhỏ làm các nhiệm vụ nghiên cứu khoa học với tốc độ thấp. Chưa có
các nghiên cứu về mô tả động học theo thiết kế của vũ khí chống ngầm hiện có
trong lực lượng Hải quân. Đây là vấn đề bí mật nên không được công bố rộng rải.
Mô tả chuyển động của ASWs dựa trên các quy luật vật lý chưa thực sự mô tả
chính xác chuyển động của đối tượng thực, hơn nữa có nhiều thông số không thể
xác định chính xác, sai số mô hình lớn dẫn đến bài toán điều khiển theo mô hình
toán xây dựng với nhiều tham số giả định khi áp dụng thực tế có thể đáp ứng không
tốt, khi này có thể sử dụng thuật toán nhận dạng để ước lượng tham số mô hình. Có
nhiều phương pháp ước lượng tham số mô hình [4], [5], [11], [30], trong đó thuật
toán bình phương tối thiểu đệ quy với ưu điểm là không cần lưu giữ các dữ liệu
trong quá khứ nên khả năng cập nhật thông số nhanh [30], có thể ứng dụng cho các
loại phương tiện ngầm [28]. Vì vậy luận án đề xuất sử dụng thuật toán bình phương
tối thiểu đệ quy theo mô hình ARX (Auto-Regressive-eXternal input) để ước lượng
tham số mô hình ASWs.
Đã có những quan tâm về thiết kế bộ điều khiển cho các loại AUV khác nhau
trong những thập kỷ qua. Bộ điều khiển kinh điển PID [3], [8] đã được đề xuất cho
AUV [40]. Tuy nhiên, đối tượng hoạt động ở môi trường nước chịu tác động của
các yếu tố không biết trước như gió, dòng chảy, hướng dòng chảy, mật độ, nhiệt độ
nước và một số thông số trong bản thân AUV không được tính toán chính xác, đặc
 13

tính động học của đối tượng không phải bất biến theo thời gian như nhiên liệu bị
tiêu tốn, trọng lượng, vị trí trọng tâm thay đổi. Như vậy, mô hình động học xây
dựng được hay nhận dạng được không thể hiện chính xác mô hình thực đối tượng.
Vì thế, một bộ điều khiển PID truyền thống với các thông số của bộ điều khiển cố
định không đủ linh hoạt để điều khiển AUV nếu sai số mô hình động học và sự ảnh
hưởng của các yếu tố môi trường quá lớn. Xuất phát từ thực tế đó, các thuật toán
điều khiển hiện đại [7], [10], [12] đã được nghiên cứu ứng dụng cho AUV: điều
khiển trượt [40], điều khiển tối ưu [16], điều khiển thích nghi [14], [27], [45], điều
khiển bền vững [23], điều khiển hồi tiếp ngược [18], điều khiển hồi tiếp trạng thái
[21], [31], điều khiển hồi tiếp đầu ra [26], [29]. Điều khiển sử dụng mô hình mờ,
giải thuật di truyền hay mạng nơron [1], [2], [4], [6] gần đây cũng được nghiên cứu
phát triển mạnh cho phương tiện ngầm [14], [17], [19], [27], [38], [44], [45].
Bộ điều khiển hồi tiếp đầu ra được đề xuất cho mô hình AUV năm bậc và ba
bậc tự do có xét đến sự ảnh hưởng của dòng chảy [26] với kết quả thực nghiệm trên
AUV dạng ngư lôi kiểu nhỏ cho thấy chất lượng bám vết của hệ thống rất tốt. Tuy
nhiên, một mô hình đầy đủ trong hệ tọa độ 6 bậc tự do vẫn chưa được xét đến.
Điều khiển với kỹ thuật hồi tiếp gia tốc được đề xuất để điều khiển ngư lôi bám
theo quỹ đạo mong muốn [18]. Kết quả trong công trình minh chứng tính bền vững
của thuật toán trước các thông số bất định của mô hình ngư lôi và ảnh hưởng của
dòng chảy. Tuy nhiên, gia tốc nhận được từ hệ thống dẫn đường dùng để hồi tiếp
luôn tồn tại sai số và độ trôi vì thế kết quả điều khiển thực luôn tồn tại sai số.
Tối ưu hóa bộ điều khiển thông minh AUV theo độ sâu [16] cho thấy tính hiệu
quả của thuật toán với thời gian cập nhật thông số của bộ điều khiển nhanh. Tuy
nhiên chỉ mới thể hiện sự hoạt động độc lập theo độ sâu, điều khiển AUV thực tế có
nhiều đầu vào, nhiều đầu ra và có sự tác động qua lại giữa các đầu vào, đầu ra.
Thành phần điều khiển trượt có thể được áp dụng để giải quyết vấn đề không
chắc chắn [39]. Tuy nhiên, nó thường được giả định là biết trước. Đây cũng là một
trong những hạn chế của thuật toán vì thực tế các đặc tính phi tuyến và các yếu tố
không chắc chắn không thể giới hạn trước. Để thực hiện luật cập nhật thích nghi
 14

mạng nơron nhiều lớp được áp dụng [14]. Các trọng số của mạng được cập nhật
theo thuật toán hồi tiếp ngược. Có thể xây dựng mạng nơron để xấp xỉ các hàm
không biết trước. Tuy nhiên, không thể xác định chính xác số nơron ở lớp ẩn mà
luôn tồn tại sai số cấu trúc mạng vì thế kết hợp mạng nơron với thành phần điều
khiển chế độ trượt được áp dụng [43].
Việc kết hợp chặt chẽ giữa hệ mờ và mạng nơron trong xấp xỉ hàm với luật điều
khiển thích nghi hồi tiếp đầu ra cho hệ phi tuyến được đề xuất [41]. Trong thực tế
không phải luôn dễ dàng đo được tất cả các trạng thái của hệ thống hay chi phí cho
việc đo lường rất lớn, độ chính xác không cao khi này để điều khiển hồi tiếp phải
ước lượng các trạng thái từ các đầu ra của hệ thống. Xuất phát từ những cơ sở trên,
và tham khảo về điều khiển mờ thích nghi hồi tiếp đầu ra cho hệ phi tuyến MIMO
[36]. Thuật toán điều khiển hồi tiếp đầu ra nơron mờ thích nghi trực tiếp (Direct
Adaptive Fuzzy-Neural Output Feedback Controller-DAFNOC) cho hệ MIMO
được đề xuất cho vũ khí chống ngầm điều khiển theo góc hướng, góc chúc ngóc và
ổn định góc lắc.
Bộ điều khiển DAFNOC áp dụng cho các hệ thống trong điều kiện chỉ có đầu ra
của hệ thống đo lường được với điểm mạnh là bộ điều khiển sử dụng mạng nơron
mờ Singleton có các thông số đầu ra của mạng được cập nhật trực tuyến theo luật
cập nhật thích nghi, các thông số đầu ra của mạng cần cập nhật ít hơn một hệ mờ
với cùng số luật mờ vì thế khối lượng tính toán ít hơn nhiều so với sử dụng hệ mờ.
Bộ điều khiển DAFNOC ước lượng trực tiếp luật điều khiển nên giảm đáng kể khối
lượng tính toán so với bộ điều khiển gián tiếp. Điều này cho phép sử dụng thuật
toán DAFNOC trong những hệ thống có nhiều đầu vào, đầu ra với khối lượng tính
toán lớn. Bộ điều khiển DAFNOC có thể ứng dụng trong điều khiển vũ khí chống
ngầm (ASWs) theo từng kênh riêng biệt hay điều khiển tổng hợp đồng thời theo góc
hướng, góc chúc ngóc và ổn định góc lắc với đầu ra của ASWs được tính toán được
nhờ thuật toán xác định tham số dẫn đường.
Để giải quyết hai nhiệm vụ cơ bản nêu trên cần phải có các cơ sở học thuật về
mô tả chuyển động, dẫn đường và điều khiển vũ khí chống ngầm. Vì vậy, luận án sẽ
trình bày các cơ sở toán học có liên quan và xác định các vấn đề cần giải quyết khi
 15

thực thi việc xác định các tham số dẫn đường trong quá trình vũ khí chống ngầm
chuyển động trong khí quyển và giai đoạn chuyển động trong nước, xác định quỹ
đạo hiệu chỉnh và điều khiển vũ khí chống ngầm bám theo quỹ đạo hiệu chỉnh.

1.2 Các hệ tọa độ sử dụng trong mô tả chuyển động của phương tiện ngầm
1.2.1 Hệ tọa độ quán tính
Theo định luật Newton, hệ tọa độ quán tính là hệ tọa độ không quay hoặc
chuyển động không có gia tốc (hình 1.8) [32]:
Gốc tọa độ tại tâm trái đất;
Trục XI đi qua điểm xuân phân là giao điểm giữa quỹ đạo quay của trái đất và
đường xích đạo thiên văn;
Trục YI nằm phía bên phải của khung toạ độ trực giao;
Trục ZI hướng theo cực Bắc;

ZI
N
Đài thiên văn
Greenwich

e

O GMST

Điểm xuân YI
phân
Đường xích đạo
I
X
Hình 1.8: Hệ tọa độ quán tính
1.2.2 Hệ tọa độ cố định tâm trái đất
Hệ tọa độ này quay xung quanh mặt trời với tốc độ 7.292115 x 10-5 rad/s. Mô tả
hệ tọa độ cố định tâm trái đất như hình 1.9 [32]:
Gốc tọa độ tại tâm trái đất;
Trục Xe có hướng đi qua kinh tuyến của đài thiên văn Greenwich trong mặt phẳng
xích đạo;
Trục Ye hướng 900 phía Đông của đài thiên văn Greenwich trong mặt phẳng xích đạo;
Trục Ze trục quay của tham chiếu Ellip của trái đất.
 16

Ze X0 ( , l )

Y0
Z0


O
e
e l Y
X
Đường xích đạo

Hình 1.9: Quan hệ của hệ tọa độ cố định tâm trái đất và hệ tọa độ địa lý
1.2.3 Hệ tọa độ địa lý
Có hai khái niệm về hệ tọa độ địa lý, hệ tọa độ địa lý cố định và hệ tọa độ địa lý
di động. Hệ tọa độ địa lý cố định OX 0Y0 Z 0 được gắn chặt với trái đất, thường thì hệ
tọa độ địa lý cố định có gốc tọa độ tại điểm xuất phát của phương tiện ngầm, trục
X 0 theo hướng bắc (N), trục Y0 hướng đông (E) và trục Z 0 hướng xuống dưới (D)
tạo thành tam diện thuận (hình 1.9) [39]. Vị trí của hệ tọa độ địa lý trong hệ tọa độ
cố định tâm trái đất được xác định bởi kinh độ l và vĩ độ  . Trong luận án, hệ tọa
độ này được chọn làm hệ tọa độ dẫn đường.
Hệ tọa độ địa lý di động có tâm hệ tọa độ trùng với tâm khối vật thể chuyển
động, trục X 0 theo hướng bắc (N), trục Y0 hướng đông (E) và trục Z 0 hướng xuống
dưới (D) tạo thành tam diện thuận.
1.2.4 Hệ tọa độ gắn liền

Hình 1.10: Hệ tọa độ gắn liền

Chuyển động của phương tiện ngầm có thể được mô tả trong hệ tọa độ gắn liền
6 bậc tự do Gb X bYb Z b như hình 1.10 [25], [39]:
 17

Gốc tọa độ thường chọn trùng với tâm trọng lực Gb ; Trục dọc Gb X b hướng theo
chiều dọc của phương tiện ngầm, trục Gb Z b hướng xuống dưới và trục GbYb hướng
ngang tạo thành tam diện thuận.
1.2.5 Ma trận chuyển tọa độ
Việc xác định quan hệ giữa hai hệ tọa độ thực chất là xác định ma trận chuyển
tọa độ và chuyển đổi véc tơ từ hệ tọa độ này sang hệ tọa độ khác. Có bốn phương
pháp thường được sử dụng như sau :
- Phương pháp góc Ơle : Phương pháp đo góc Ơle trực tiếp bằng các cơ cấu đo góc
(các Encoder) chỉ phù hợp với các thiết bị đặt trên mặt đất. Đối thiết bị bay hoặc
phương tiện ngầm để đo trực tiếp góc Ơle phải dùng các con quay ba bậc tự do.
Nhược điểm của con quay ba bậc tự do là khối lượng lớn và giá thành rất cao, vì
là thiết bị cơ điện tinh vi đòi hỏi độ chính xác cao trong gia công và lắp ráp.
- Phương pháp giải trực tiếp hệ phương trình phi tuyến: Việc dùng phương pháp
giải trực tiếp hệ phương trình phi tuyến gặp phải hai khó khăn về mặt toán học
đó là vấn đề phi tuyến và vấn đề điểm kỳ dị (chia 0).
- Phương pháp ma trận Côsin định hướng: Việc dùng phương pháp ma trận Côsin
định hướng sẽ phải giải số lượng lớn các phương trình vi phân.
- Phương pháp sử dụng tham số Rodrig-Hamilton: Người ta thường dùng trong
thiết bị dẫn đường quán tính không đế với các phần tử đo gồm các gia tốc kế và
các con quay vi cơ đo tốc độ quay tuyệt đối.
Các phương pháp xác định ma trận chuyển ở trên là kiến thức cơ bản đã được
trình bày trong nhiều tài liệu tham khảo. Phương pháp góc Ơle và phương pháp sử
dụng tham số Rodrig-Hamilton có liên quan sẽ được trình bày trong luận án.
1.2.5.1 Phương pháp góc Ơle
Thực hiện ba phép quay liên tiếp để biến đổi tọa độ giữa hệ tọa độ gắn liền sang
hệ tọa độ địa lý như hình 1.11 – 1.13 ta có được các góc Ơle  , , .
Quay hệ tọa độ địa lý OX 0Y0 Z 0 một góc hướng  quanh trục Z 0 được hệ tọa độ
mới là OX 1Y1Z1 (hình 1.11). Phương trình thể hiện mối quan hệ giữa hai hệ tọa độ
viết dưới dạng ma trận:
 18

 u0  cos  sin 0   u1 
    
 v0    sin cos 0   v1  (1.1)
w   0 0 1   w1 
 0 
X0 
X1

u1
u0

v0 Y0
O
v1


Y1

Hình 1.11: Quay theo góc hướng  quanh trục Z 0

Quay hệ tọa độ OX 1Y1Z1 một góc chúc ngóc  quanh trục Y1 được hệ tọa độ
mới là OX 2Y2 Z 2 (hình 1.12). Phương trình thể hiện mối quan hệ giữa hai hệ tọa độ
viết dưới dạng ma trận:
 u1   cos  0 sin    u2 
    
 v1    0 1 0   v2  (1.2)
 w    sin  0 cos    w 
 1   2 

X2

 u2
O
X1 u1

w1
w2

Z2  Z1

Hình 1.12: Quay theo góc chúc ngóc  quanh trục Y1

 19

Quay hệ tọa độ OX 2Y2 Z 2 một góc lắc  quanh trục X 2 đưa về hệ tọa độ
OX 3Y3 Z 3 trùng với hệ tọa độ gắn liền OX bYb Z b (hình 1.13). Phương trình thể hiện
mối quan hệ giữa hai hệ tọa độ viết dưới dạng ma trận:
 u2  1 0 0   u3 
    
 v2    0 cos   sin    v3  (1.3)
 w   0 sin  cos    w3 
 2 

O v2 Y2
v3 
w3
Y3
w2
Z3  Z2

Hình 1.13: Quay theo góc lắc  quanh trục X 2

Thực hiện ba phép quay liên tiếp bên trên ta có các ma trận quay:
1 0 0   cos  0 sin    cos  sin 0
RX , 
 0 cos   
 sin   ; RY ,   0 1 0  ; RZ ,   sin
 cos 0  (1.4)
0 sin  cos     sin  0 cos    0 0 1 
Các phép quay tọa độ trên cho phép chuyển từ hệ tọa độ gắn liền sang hệ tọa độ
địa lý với ma trận Côsin định hướng Cbn được biểu diễn:

 c11 c12 c13 

C  RZ , RY , RX ,
n
b  c21 c22 c23 
c31 c32 c33 
(1.5)
cos cos cos sin  sin   sin cos  cos sin cos   sin sin  
  sin cos sin sin sin   cos cos  sin sin cos   cos sin  
  sin  cos sin  cos cos  

Ma trận Cbn là ma trận trực giao nên có thể chuyển một véc tơ từ hệ tọa độ địa lý

sang hệ tọa độ gắn liền bởi ma trận chuyển Cnb  (Cbn )T  (Cbn ) 1  RXT , RYT, RZT ,
 20

1.2.5.2 Phương pháp sử dụng tham số Rodrig – Hamilton

Số siêu phức quaternion được định nghĩa [20], [35]:

  0  i1  j2  k 3 (1.6)
trong đó,
0 , 1 , 2 , 3 là tham số Rodrig – Hamilton; i, j , k đơn vị phức trên ba trục phức.

Phép nhân hai số quaternion được ký hiệu là  và tuân thủ quy tắc nhân các số
ảo sau:
i 2  j 2  k 2  1 ; i  j   j  i  k ; j  k  k  j  i ; k  i  i  k  j . (1.7)
Số liên hợp quaternion * được định nghĩa: *  0  i1  j2  k 3

Khi đó 02  12  22  32    *  1

Ma trận chuyển của của véc tơ Rb từ hệ tọa độ gắn liền sang hệ tọa độ địa lý Rn
có thể định nghĩa thông qua số siêu phức quaternion  như sau [20]:
Rn  Rb *   z   y   x Rb *z  *y  *x (1.8)

Từ (1.8) thấy rằng số siêu phức quaternion    z   y   x . Sử dụng quy tắc

nhân số ảo (1.7) ta có:

     
   z  y  x  (cos  k sin )(cos  j sin )(cos  i sin )
2 2 2 2 2 2
           
 (cos cos cos  sin sin sin )  i (cos cos sin  sin sin cos ) (1.9)
2 2 2 2 2 2 2 2 2 2 2 2
           
 j (cos sin cos  sin cos sin )  k (sin cos cos  cos sin sin )
2 2 2 2 2 2 2 2 2 2 2 2
Từ (1.6) và (1.9) các tham số Rodrig – Hamilton được xác định:
      
o  cos 2 cos 2 cos 2  sin 2 sin 2 sin 2

  cos  cos  sin   sin  sin  cos 
 1 2 2 2 2 2 2
 (1.10)
  cos  sin  cos   sin  cos  sin 
 2 2 2 2 2 2 2
      
3  sin cos cos  cos sin sin
 2 2 2 2 2 2
 21

Bảng 1.1: Quan hệ giữa các Côsin chỉ phương và các số quaternion

Thứ tự quay Góc Ơle Góc Côsin chỉ phương Các số quaternion
 cos  sin 0  
 z  cos  k sin
I  RZ ,   sin cos 0  2 2
 0 0 1 

 cos  0 sin    
 y  cos  j sin
II  RY ,   0 1 0  2 2
  sin  0 cos  

1 0 0   
 x  cos  i sin
 RX , 
 0 cos   sin   2 2
III
0 sin  cos  

Công thức (1.8) có thể viết lại dưới dạng ma trận thông qua các số siêu phức
quaternion [32]:
Rn  Cbn Rb (1.11)
 202  212  1 212  20 3 213  20 2   c11 c12 c13 
 
trong đó, Cbn   212  20 3 202  222  1 22 3  20 1   c21 c22 c23  (1.12)
 213  20 2 22 3  20 1 202  232  1   c31 c32 c33 
 
Có thể xác định các tham số Rodrig-Hamilton thông qua các vận tốc góc quay
tuyệt đối của hệ tọa độ gắn liền và hệ tọa độ địa lý [20], [46]:
       
2 (1.13)
trong đó,   0  i N  jE  kD là vận tốc góc quay tuyệt đối của hệ tọa độ địa lý;
  0  ip  jq  kr là vận tốc góc quay tuyệt đối của hệ tọa gắn liền; i, j , k đơn vị
phức trên ba trục phức.
Từ (1.13) sử dụng quy tắc nhân số ảo (1.7), các tham số Rodrig-Hamilton được
viết trong miền số thực:
20  1 p  2 q  3 r   N 1  E 3  D 2
 
21  0 p  3 q  2 r   N 0  E 2  D 3
  (1.14)
22  3 p  0 q  1r   N 3  E 1  D 0
2   p   q   r         
 3 2 1 0 N 2 E 0 D 1
 22

Từ các tham số Rodrig-Hamilton của ma trận Côsin định hướng Cbn có thể xác
định các góc Ơle: góc lắc  , góc chúc ngóc  , góc hướng  như sau:

  c21   212  20 3 

  arctg    arctg  2 
 20  21  1 
2
  c11 

   arcsin  c31    arcsin(213  20 2 ) (1.15)

  arctg  c32   arctg  22 3  20 1 
    2 
 20  23  1 
2
  c33 

1.3 Tổng quan về dẫn đường quán tính

1.3.1 Nguyên tắc dẫn đường quán tính
Ý tưởng chính của dẫn đường quán tính là dựa trên phép tích phân gia tốc. Thiết
bị đo gia tốc đối tượng là gia tốc kế. Gia tốc kế đo gia tốc cảm nhận của tâm khối
đối tượng chuyển động, trong hệ tọa độ quán tính chỉ số gia tốc kế f được biểu

diễn [20], [32]:

f  a  gm (1.16)

trong đó,
d  dr 
a là gia tốc tuyệt đối (gia tốc đối với hệ tọa độ quán tính);
dt  dt I  I

g m là gia tốc trọng trường sinh ra do lực hút trái đất.

Gia tốc tuyệt đối a được xác định từ công thức Coriolis [32]:

d  dr  d d dr
a       U  r I  U 
dt  dt I  I dt dt I dt I
(1.17)
d
     U    U  (U  r )
dt N

trong đó,
d
 là véc tơ vận tốc của đối tượng so với hệ tọa độ cố định tâm trái đất; đạo
dt N
hàm  đối với hệ tọa độ địa lý (trong trường hợp chọn làm hệ tọa độ dẫn đường);
U  (UN ,UE ,UD )T vận tốc góc trái đất;  là ký hiệu tích véc tơ;
 23

  ( N ,  E ,  D )T vận tốc góc tuyệt đối của hệ tọa độ địa lý;

r là véc tơ vị trí của đối tượng ngầm trong hệ tọa độ quán tính.

Từ phương trình (1.16), (1.17) gia tốc cảm nhận có thể được viết lại thành:
d
f    U   g (1.18)
dt N

trong đó,
g  g m  U  (U  r ) là gia tốc trọng trường.

Phương trình (1.18) là phương trình dẫn đường tổng quát.

Xem hệ tọa độ địa lý như một hệ tọa độ dẫn đường và định nghĩa phép chiếu
của phương trình (1.18) trên các trục của hệ tọa độ địa lý. Với mục đích đó, quy tắc
nhân véc tơ được giới thiệu [20], [32]:

 a b c 
 
m  n  det   mx my mz    (m y nz  mz n y )a  (mx nz  mz nx )b  (mx n y  my nx )c (1.19)
 n ny nz  
 x

trong đó, a, b, c là ba véc tơ đơn vị trên ba trục.

Dùng luật ở trên, phép chiếu của gia tốc lên các trục của hệ tọa độ địa lý được
mô tả:
 dVN
 f N  dt  EVD  DVE  U EVD  U DVE

 dVE
 fE   NVD  DVN  U NVD  U DVN (1.20)
 dt
 dVD
 f D  dt  EVN  NVE  U EVN  U NVE  g


Có hai ý tưởng khác nhau của việc phỏng hệ tọa độ dẫn đường. Ý tưởng đầu
tiên là sử dụng một cơ cấu đế con quay gồm ba trục với ba gia tốc kế được đặt trực
giao, kiểu thiết bị này được gọi là thiết bị dẫn đường quán tính có đế. Loại thứ hai
được gọi là thiết bị dẫn đường quán tính không đế cung cấp ý tưởng phân tích hệ
tọa độ dẫn đường trên bo mạch máy tính, sử dụng các phép đo lường từ các gia tốc
kế và con quay tốc độ góc được lắp đặt trực tiếp trên thân đối tượng.
 24

1.3.1.1 Dẫn đường quán tính có đế

Đế của thiết bị dẫn đường quán tính là một tấm phẳng được ổn định vị trí trong
không gian nhằm tạo ra hệ tọa độ đo (gọi là hệ tọa độ đế). Có ba loại thiết bị dẫn
đường quán tính có đế:
- Thiết bị dẫn đường quán tính có đế dạng giải tích: Loại này tạo ra một hệ tọa độ
đo ổn định trong không gian quán tính.
- Thiết bị dẫn đường quán tính có đế dạng bán giải tích: Loại này tạo ra một hệ
tọa độ đo vừa ổn định trong không gian vừa quay trong không gian với tốc độ
góc bằng tốc độ góc quay trái đất tại vị trí vũ khí rời khỏi bệ phóng.
- Thiết bị dẫn đường quán tính có đế dạng hình học: Loại này tạo ra một hệ tọa
độ đo vừa ổn định trong không gian vừa quay theo sự quay ngày đêm của trái
đất và quay theo do sự chuyển động bám theo độ cong của bề mặt trái đất.
Vũ khí chống ngầm thường rơi trong khí quyển khoảng vài phút và chuyển
động trong môi trường nước khoảng một vài phút nữa, không gian hoạt động tương
đối hẹp nên nếu sử dụng thiết bị dẫn đường quán tính có đế thì nên dùng loại đế ổn
định trong không gian quán tính, có giá thành rẻ nhất trong các thiết bị dẫn đường
quán tính có đế (rẻ hơn thiết bị dẫn đường quán tính bán giải tích và thiết bị dẫn
đường quán tính hình học – hai loại thường dùng cho vật thể chuyển động thời gian
dài và không gian rộng).

Hình 1.14: Hệ tọa độ địa lý và thiết bị dẫn đường quán tính có đế dạng giải tích
 25

Trên hình 1.12 là cấu trúc của của thiết bị dẫn đường quán tính có đế dạng giải
tích [46]. Nguyên lý hoạt động của thiết bị dẫn đường quán tính có đế dạng giải tích
như sau:
Trên tấm đế, ngoài ba gia tốc kế còn có 3 con quay ba bậc tự do, ba con quay đó
được đặt sao cho các trục chỉ hướng theo 3 phương của tam diện thuận. Khi các rôto
của ba con quay đạt tốc độ quay danh định và các con quay được mở chốt thì 3 trục
nhạy của chúng xác lập một hệ tọa độ quán tính (hệ tọa độ đế). Tấm đế quan hệ với
vỏ của AUV (tức là với hệ tọa độ gắn liền với AUV) thông qua 3 khung treo các
đăng, 3 khung này có thể quay xung quanh các trục OX D , OYD , OZ D bằng 3 động cơ
điện tương ứng. Hệ tọa độ đế trùng với hệ tọa độ gắn liền của AUV ở thời điểm mở
chốt 3 con quay ba bậc tự do. Khi AUV chuyển động trong không gian, nếu động
cơ điện không quay thì tấm đế sẽ quay theo, các cơ cấu đo của các con quay ba bậc
tự do sẽ xuất hiện tín hiệu tỉ lệ với sai lệch của vị trí tấm đế so với hệ tọa độ quán
tính đã xác lập. Các tín hiệu này được khuyếch đại và đưa đến các động cơ điện, các
động cơ quay để đưa tấm đế về trùng với hệ tọa độ quán tính đã xác lập để khử bỏ
các sai lệch. Bản chất quá trình ổn định vị trí tấm đế là quá trình tự động bám bởi 3
hệ tự động bám.
Một đầu của các trục quay OX D , OYD , OZ D gắn cảm biến góc để đo góc quay
1 , 2 , 3 của 3 khung các đăng so với vỏ AUV, tức là so với hệ tọa độ gắn liền với

AUV. Như vậy từ thông tin của 3 góc quay do ba cảm biến góc cung cấp sẽ xác
định được ma trận Côsin đinh hướng Cdb dạng (1.5) mô tả quan hệ giữa hệ tọa độ đế
và hệ tọa độ gắn liền với AUV.
Việc mở chốt ba con quay của thiết bị đế được thực hiện ở thời điểm mang tính
ngẫu nhiên (do người phi công lái máy bay thực hiện) nên hệ tọa độ đế được xác lập
sẽ là một hệ tọa độ cố định bất kỳ trong không gian quán tính. Vì vậy, để xác định
được các tham số định vị và tham số định hướng của ASWs so với hệ tọa độ dẫn
đường (hệ tọa độ địa lý nơi máy bay thả ASWs) cần phải xác định ma trận Côsin
định hướng Cdn giữa hệ tọa độ đế và hệ tọa độ địa lý tại nơi máy bay thả ASWs.
Từ việc xác định được ma trận Côsin định hướng Cdn sẽ xác định được các
thành phần của véc tơ gia tốc trong hệ tọa độ dẫn đường (hệ tọa độ địa lý):
 26

( f N , f E , f D )T  Cdn ( ndx , ndy , ndz )T (1.21)

trong đó,
f N , f E , f D là thành phần của véc tơ gia tốc trong hệ tọa độ địa lý;

ndx , ndy , ndz là thành phần của véc tơ gia tốc trong hệ tọa độ đế, được đo bởi gia tốc kế.

Các tổ hợp khí tài quân sự nói chung và đối với các loại ASWs nói riêng thường
chọn hệ toạ độ địa lý nơi ASWs rời phương tiện mang làm hệ toạ độ dẫn đường
đồng thời bỏ qua vận tốc góc quay của hệ tọa độ địa lý và vận tốc góc quay của hệ
tọa độ cố định tâm trái đất, khi đó phương trình (1.20) được viết lại:
VN  f N ; VE  f E ; VD  f D  g (1.22)

Tọa độ tâm khối x, y, z của ASWs trong hệ tọa độ địa lý được xác định bằng
cách giải hệ phương trình vi phân:
x  VN ; y  VE ; z  VD (1.23)

Từ việc xác định được ma trận Côsin định hướng Cdn giữa hệ tọa độ đế và hệ tọa

độ địa lý và ma trận Côsin định hướng Cdb giữa hệ tọa độ đế và hệ tọa độ gắn liền sẽ

xác định được ma trận Côsin định hướng Cbn giữa hệ tọa độ gắn liền và hệ tọa độ
địa lý:
 c11
* *
c12 * 
c13
 * * * 
Cbn  (Cdb ) 1 Cdn  c21 c22 c23  (1.24)
 c* c* * 
c33 
 31 32
Từ (1.24) các góc trạng thái được xác định theo công thức (1.15).
Để đế hoạt động có các thuật toán điều khiển đế và thuật toán xác định ma trận
Côsin định hướng giữa hệ tọa độ đế với hệ tọa độ dẫn đường (hệ tọa độ địa lý). Các
thuật toán điều khiển đế được cài đặt khi chế tạo thiết bị dẫn đường quán tính có đế
còn thuật toán xác định ma trận Côsin định hướng giữa hệ tọa độ đế với hệ tọa độ
dẫn đường là thuật toán được hình thành khi sử dụng thiết bị dẫn đường quán tính
có đế, vấn đề này không được chuyển giao khi mua sắm thiết bị. Vì vậy, luận án sẽ
đi sâu vào giải quyết vấn đề xây dựng thuật toán xác định ma trận Côsin định hướng
giữa hệ tọa độ đế với hệ tọa độ dẫn đường.
 27

1.3.1.2 Dẫn đường quán tính không đế

Thiết bị dẫn đường không đế bao gồm ba gia tốc kế và ba con quay vi cơ đo tốc
độ góc được cố định trong hộp gọi là đơn vị đo lường quán tính (Inertial
Measurement Unit – IMU). Khi đưa IMU vào vật thể chuyển động cần thực hiện
sao cho các trục của IMU song song với các trục của hệ tọa độ gắn liền, khi đó các
chỉ số của gia tốc kế và của con quay vi cơ là đối với hệ tọa độ gắn liền. Để xác
định các thành phần vận tốc VN ,VE ,VD tâm khối của ASWs cần phải xác định ma
trận Côsin định hướng giữa hệ tọa độ gắn liền và hệ tọa độ địa lý di động Cbn :
Trong trường hợp vùng hoạt động của phương tiện chuyển động hẹp và thời
gian hoạt động ngắn, bỏ qua vận tốc góc quay của hệ tọa độ địa lý, phương trình
(1.13) được viết lại:
   
2 (1.25)
Viết (1.25) dưới dạng các phương trình vi phân trong miền số thực như sau:
20  1 p  2 q  3 r
 
21  0 p  3 q  2 r
  (1.26)
22  3 p  0 q  1r
2   p   q   r
 3 2 1 0

Từ việc xác định được ma trận Côsin định hướng Cbn sẽ xác định được các
thành phần gia tốc trong hệ tọa độ độ địa lý [46]:
f  Cbn ab (1.27)
hay viết dưới dạng ma trận:
 
 f N  202  212  1 abx   212  203  aby   213  202  abz


 2 2

 f E   212  203  abx  20  22  1 aby   223  201  abz (1.28)

 
 f D   213  202  abx  (223  201 )aby  20  23  1 abz
2 2

trong đó,
f  ( f N , f E , f D )T là các thành phần véc tơ gia tốc trong hệ tọa độ địa lý;

ab  (abx , aby , abz )T là chỉ số của gia tốc kế trong hệ tọa độ gắn liền;

Vận tốc và tọa độ tâm khối của ASWs trong hệ tọa độ địa lý có được từ việc
giải các phương trình (1.22) và (1.23).
 28

Các góc định hướng của ASWs được xác định theo công thức (1.15).
Tuy nhiên các con quay vi cơ đo thành phần tốc độ góc thực tế thường cho các
giá trị tham số đo được bao gồm tốc độ quay thực cộng với độ trôi và nhiễu:
 p  p  bp  w1 , q  q  bq  w2 , r  r  br  w3 (1.29)

trong đó, bp , bq , br là các tham số biến đổi chậm thể hiện độ trôi của con quay vi cơ;

w1 , w2 , w3 là các nhiễu đo thường có dạng ồn trắng (nhiễu Gauss).

Gia tốc kế cho giá trị đo bao gồm gia tốc chuyển động thực và nhiễu:
a x  a bx  w4 ; a y  a by  w5 ; a z  a bz  w6 ; (1.30)

trong đó, w4 , w5 , w6 là các nhiễu đo thường có dạng ồn trắng.

Việc sử dụng thông tin về gia tốc và tốc độ góc để xác định các tham số dẫn
đường đã có nhiều nghiên cứu công bố [20], [32], [33]. Tuy nhiên, thông tin về tốc
độ góc do con quay vi cơ đo được luôn có nhiễu đo và độ trôi như (1.29) và (1.30).
Vì vậy, việc xác định các tham số dẫn đường theo phương pháp trực tiếp giải
phương trình (1.26), (1.28) luôn có sai số và sai số tăng theo thời gian.
1.3.2 Kết hợp các hệ thống định vị và dẫn đường
Để khắc phục các sai số của việc sử dụng trực tiếp thông tin từ các gia tốc kế và
con quay vi cơ xác định tham số dẫn đường, luận án đề xuất việc đưa thêm các phần tử
đo khác và áp dụng bộ lọc Kalman phi tuyến mở rộng (EKF). Vì vậy, trong chương
này luận án trình bày cơ sở toán học của bộ lọc Kalman phi tuyến mở rộng.
Nguyên lý lọc Kalman được mô tả như sau: Giả sử quá trình chuyển động của
vật thể được mô tả bởi hệ phương trình động học phi tuyến dưới dạng rời rạc như
sau [47]:
X k  Fk 1 ( X k 1 )  Gk 1 ( X k 1 ) wk 1
(1.31)
Z k  hk ( X k )  vk
trong đó, X k , X k 1 là trạng thái của véc tơ trạng thái X (véc tơ n chiều) ở bước thứ k
và bước thứ (k-1); Gk 1 ( X k 1 ) là ma trận hệ số nhiễu; F  ( f1 , f 2 ,..., f n )T là các hàm
động học phi tuyến theo biến số là véc tơ X với Fk 1 ( X k 1 ) là véc tơ hàm động học
ở bước thứ k-1; Z k là giá trị của véc tơ đầu ra (véc tơ m chiều, thường m  n);
 29

h  (h1 ,..., hm )T là véc tơ đo với hk là giá trị của véc tơ đo ở bước thứ k; wk , vk là véc
tơ nhiễu động học và nhiễu đo có dạng nhiễu trắng với kỳ vọng toán học bằng 0 và
không tương quan với trạng thái ban đầu :
wk ~ N (0, Q) ;   wk (k )   0 ;   wk ( j ), wk T (k )   I . wk
2
Q;

vk ~ N (0, R) ;   vk (k )   0 ;   vk ( j ), vk T (k )   I . vk2  R ;
 
Pk  ([ X k  X k ][ X k  X k ]T ) là ma trận hiệp phương sai của véc tơ sai số đánh giá

trạng thái véc tơ X .

trong đó,  là kỳ vọng toán học;  wk2 ,  vk2 là hiệp phương sai nhiễu động học và
nhiễu đo; Q , R là ma trận hiệp phương sai nhiễu động học và nhiễu đo; I là ma trận
đơn vị.
Trạng thái ban đầu X 0 là một véc tơ ngẫu nhiên có kỳ vọng toán học biết trước
0    X 0  và ma trận hiệp phương sai ban đầu P0   [ X 0  0 ][ X 0  0 ]T 

Thủ tục của thuật toán đánh giá trạng thái X trên cơ sở véc tơ quan sát Z k như
sau (hình 1.15):
Phương trình trạng thái ước lượng là [47]:
Xˆ k(  )  Fk 1 ( Xˆ k( 1) ) (1.32)
Tính toán ma trận chuyển trạng thái bằng cách đạo hàm riêng:
Fk 1
 k 1  X  Xˆ (  )
(1.33)
X k 1

Ma trận hiệp phương sai của của véc tơ sai số đánh giá trạng thái được tính toán:
Pk(  )   k 1 Pk(1) Tk 1  Gk 1QGkT1 ; (1.34)
Tính toán ma trận đo bằng cách đạo hàm riêng:
hk
Hk  X  Xˆ (  )
(1.35)
X k

Độ lợi Kalman được tính toán từ ma trận hiệp phương sai của véc tơ sai số đánh
giá trạng thái và nhiễu đo được tính toán:
K k  Pk(  ) H kT ( H k Pk(  ) H kT  R ) 1 (1.36)

Véc tơ đo lường ước lượng:

 30

Zˆ k  hk ( Xˆ k(  ) ) (1.37)

Cập nhật véc tơ trạng thái với bước đo lường mới:

Xˆ k(  )  Xˆ k(  )  K k ( Z k  Zˆ k ) (1.38)

Cập nhật ma trận hiệp phương sai của véc tơ sai số đánh giá trạng thái:
Pk(  )  ( I  K k H k ) Pk(  ) (1.39)

Điều kiện đánh giá được khi và chỉ khi hạng của ma trận H T T H T ...(T ) n 1 H T

bằng bậc của hệ động học, tức là: rank H T T H T ...(T ) n 1 H T  n

Q
R
Hiệu chỉnh ma trận K k
theo (1.34) và (1.36)
vk
ASWs Xk Zk Xˆ k(  ) Xˆ k(  )
hk ( X k ) Kk Fk 1 ( Xˆ k(1) )

Các phương Zˆ k Xˆ k(  )
tiện đo

hk ( Xˆ k(  ) )

Hình 1.15: Bộ lọc Kalman phi tuyến mở rộng

Như vậy để thực hiện được việc lọc Kalman cần phải thực hiện các bước sau:
- Xác định véc tơ hàm động học Fk 1 ( X k 1 )
- Xác định véc tơ hàm quan sát hk ( X k )
- Xác định ma trận hệ số nhiễu Gk 1 ( X k 1 )
- Xác định ma trận cường độ nhiễu Q, R
Xác định các tham số định hướng và tham số định vị cho phương tiện chuyển
động trên cơ sở kết hợp các phương tiện đo sử dụng bộ lọc EKF đã minh chứng tính
hiệu quả của nó [42]. Bộ lọc EKF còn được ứng dụng trong việc xác định ma trận
Côsin định hướng giữa hệ tọa độ đế và hệ tọa độ địa lý của thiết bị dẫn đường quán
tính có đế. Đây là hướng tiếp cận của luận án được trình bày trong chương 2.
 31

1.4 Mô tả động học phương tiện ngầm tự hành dạng ngư lôi

Đối với AUV thường chuyển động trong khoảng cách ngắn có thể xem bề mặt đất
là phẳng, khi đó hệ tọa độ địa lý được xem như là hệ tọa độ dẫn đường (hình 1.16).

o X0
B
u
Xb
Y0 Gb Cf
Z0
v w
W
Yb

Zb

Hình 1.16: Biễu diễn AUV trong các hệ tọa độ

Lực, mô men, vận tốc đối với hệ tọa độ gắn liền được ký hiệu bởi các véc tơ sau:
1  ( X , Y , Z )T véc tơ ngoại lực tác dụng lên AUV;

 2  ( K , M , N )T véc tơ mô men ngoại lực tác dụng lên AUV;

V  (u , v, w)T véc tơ vận tốc dài chiếu lên các trục hệ tọa độ gắn liền X b , Yb , Z b ;

  ( p, q, r )T véc tơ vận tốc góc tuyệt đối của hệ tọa độ gắn liền;

Vị trí và góc định hướng của AUV đối với hệ tọa độ địa lý:

1  ( x, y, z )T vị trí tâm khối của phương tiện ngầm trong hệ tọa độ địa lý;

 2  ( ,  , )T góc định hướng giữa hệ tọa độ gắn liền của phương tiện ngầm với

hệ tọa độ địa lý;

Ký hiệu:   (V T ,  T )T  (u , v, w, p, q, r )T ;   (1T , 2T )T  ( x, y, z ,  ,  , )T

Mối quan hệ giữa các thành phần véc tơ trong hệ tọa độ gắn liền và hệ tọa độ
địa lý [39]:
  J ( )  (1.40)
 32

1 tan  sin  tan  cos  

 Cn 033  
trong đó, J ( )   b  với J ( 2 )  0 cos   sin  
033 J ( 2 ) 
0 sin  / cos  cos  / cos  

Phương trình chuyển động của AUV dưới dạng tổng quát trong hệ tọa độ gắn
liền [15], [39]:
M RB   CRB (  )    RB (1.41)
trong đó, M RB là ma trận quán tính; CRB là ma trận hướng tâm Coriolis;  RB là véc
tơ ngoại lực và mô men ngoại lực tác động lên thân AUV.
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
Phương trình lực của AUV trong hệ tọa độ gắn liền là [39]:
Fb  mab (1.42)
trong đó, m là khối lượng của AUV; ab  V    V    Gb    (  Gb ) là gia tốc
tâm khối của AUV chiếu lên hệ tọa độ gắn liền [39]; Gb là véc tơ tọa độ tâm khối
của AUV trong hệ tọa độ gắn liền.
Phương trình mô men của AUV trong hệ tọa độ gắn liền là [39]:
M b  I 0    ( I 0   )  Gb  mab (1.43)

trong đó, I 0  diag[ I xx , I yy , I zz ]  R 33 là ma trận mô men đường chéo theo các trục

X b , Yb , Z b tại tâm trọng lực.

Thay thế gia tốc ab vào phương trình lực (1.42) và phương trình mô men (1.43),
thực hiện phép nhân véc tơ có hướng và bỏ qua các thành phần bậc cao của phương
trình mô men theo các trục X b , Yb , Z b . Xét hệ tọa độ gắn liền có gốc tọa độ tại tâm
khối Gb , ma trận quán tính và ma trận hướng tâm Coriolis được biểu diễn [39]:

m 0 0 0 0 0

0   0 mr mq 0 0 0 
0 m 0 0 0 
0 mp 0 
0 0 m 0 0 0  mr 0 0
M RB  ;  mq mp 0 0 0 0  (1.44)
 0 0 0 I xx 0 0  CRB   0 0 0 0

I zz r  I yy q 

0 0 0 0 I yy 0  0 0 0  I zz r 0 I xx p 
   
 0 0 0 0 0 I zz   0 0 0 I yy q  I xx p 0 
 33

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
Xét loại AUV được điều khiển bởi bốn bánh lái gồm hai bánh lái hướng vừa
làm nhiệm vụ điều khiển theo hướng, vừa điều khiển giảm lắc và hai bánh lái sâu
điều khiển AUV theo độ sâu hay góc chúc ngóc, truyền động quay các bánh lái này
là các máy lái điện. Ngoại lực và mô men ngoại lực tác động lên AUV được biểu
diễn [39]:
 RB  M A   C A (  )   D(  )   L(  )   g ( )   (1.45)

trong đó, M A , C A (  ) là ma trận quán tính và ma trận hướng tâm Coriolis khối nước

kèm; D(  ) là ma trận lực và mô men thủy động; g ( ) là véc tơ lực và mô men gây

ra bởi trọng lực và lực nổi; L(  ) là ma trận thông số lực và mô men của bánh lái;

   bl   pl là lực và mô men của bánh lái và động cơ đẩy.

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

Lực nổi (lực Acsimet) B tác động tại tâm nổi có tọa độ C f  ( x f , y f , z f )T đối
với hệ tọa độ gắn liền và trọng lực W tác động tại tâm khối Gb (hình 1.16).
Xét hệ tọa độ gắn liền có gốc tọa độ tại tâm khối Gb , khi này tọa độ của tâm
khối trong hệ tọa độ gắn liền là Gb  ( xb , yb , zb )T  (0,0, 0)T . Đối với AUV thì W  B ,
đồng thời tâm khối và tâm nổi được thiết kế nằm trong cùng mặt phẳng đối xứng
tức là y f  0 [39], Lực và mô men gây ra bởi trọng lực và lực nổi được biểu diễn
trong hệ tọa độ gắn liền:
 X HS   0 
Y   0 
 HS   
 Z HS   0 
g ( )     (1.46)
 K HS   z f B cos  sin  
 M HS   z f B sin   x f B cos  cos  
   
 N HS    x f B cos  sin  
1.4.2.2 Các lực và mô men khối nước kèm

Nhìn chung các đối tượng chuyển động dưới nước có tốc độ thấp, khi đó lực và
mô men tạo ra bởi khối nước kèm được mô tả [39]:
 34

 X A  X u u  X wq wq  X qq qq  X vr vr  X rr rr

YA  Yv v  Yr r  Yura ur  Ywp wp  Ypq pq
 Z  Z w  Z q  Z uq  Z vp  Z rp
 A w q uqa vp rp
(1.47)

 K A  K p p
 M  M w  M q  M uw  M vp  M rp  M uq
 A w q uwa vp rp uqa

 N A  N v v  N r r  N uva uv  N wp wp  N pq pq  N ura ur

Từ (1.47) ma trận quán tính và ma trận hướng tâm khối nước kèm được mô tả:
 X u 0 0 0 0 0
0 Yv 0 0 0 Yr 

0 0 Z w 0 Z q 0
MA    (1.48)
0 0 0 K p 0 0
0 0 M w 0 M q 0
 
 0 N v 0 0 0 N r 

0 0 0 0 X wq w  X qq q X vr v  X rr r 
0 0 0 Ywp w Ypq p Yura u 

0 0 0 Z vp v  Z rp r Z uqa u 0 
CA    (1.49)
0 0 0 0 0 0 
 
0 0 M uwa u M rp r  M vp v M uqa u 0 
0 N u 0 N wp w N pq p N ura u 
 uva

1.4.2.3 Các lực và mô men thủy động

L
D

Gb

V  Xb

Yb
Zb

Hình 1.17: Biễu diễn lực cản, lực nâng, góc tấn công, góc trượt bên của AUV
 35

Lực cản liên quan đến mật độ nước  , tổng diện tích bề mặt A f theo hướng vận
tốc V của AUV (hình 1.17) [15]:
1
D  CD Af V 2 (1.50)
2

trong đó, V  u 2  v 2  w2 ; CD liên quan đến góc tấn công ( ) theo đường Parabol
có dạng C D  a 2  b  c với a, b, c là các hằng số.
Giả sử rằng vận tốc v và w là nhỏ so với vận tốc u , góc tấn công  có thể
w
được mô tả trong mặt phẳng ngang X b Zb là tg    [rad ] và góc trượt bên 
u
v
trong mặt phẳng đứng X bYb được mô tả tg     [rad ] .
u
Tổng lực cản theo các trục X b , Yb , Z b là:
 X d  X u|u|u | u |  X uv uv  X uwuw  X vv v | v |  X w|w| w | w |

Yd  Yuvd uv  Yv|v|v | v | (1.51)

 Z d  Z uwd uw  Z w|w| w | w |

trong đó,

 1 1 1 c
 X u|u|   2 (  Af )c; X uw  X uv   2 (  Af )b; X w|w|  X v|v|   2 (  Af )(a  2 );
 (1.52)
Y  Z   1 (  A )b; Z  Y   1 (  A )c
 v|v| w| w|
2
f wwd uvd
2
f

Lực nâng L tác động tại tâm áp lực tạo ra vuông góc với hướng dòng chảy khi
AUV chuyển động trong nước (hình 1.17). Lực này đối với tâm nổi tạo ra mô men
chúc ngóc M . Cả lực nâng và mô men quay tỉ lệ thuận với các hệ số CL , CM liên
quan đến góc tấn công  và góc trượt bên  [15].

1 1
L  C L A f V 2 ; M   CM A f V 2 (1.53)
2 2

Lực nâng và mô men chúc ngóc khi xét trong mặt phẳng X b Z b từ phương trình
(1.53) ta có:
1 C
Zl   Af CL (u 2  w2 ) cos  với CL  L (1.54)
2 
 36

1 C
Ml   Af CM  (u 2  w2 ) với CM   M
2 

Tương tự trong mặt phẳng X bYb :

1 1
Yl   Af CL (u 2  v 2 )  cos  ; N l   Af CM  (u 2  v 2 )  (1.55)
2 2

Giả sử rằng vận tốc u lớn hơn rất nhiều so với v và w ta có:
1 1
Yuvl   Z uwl   Af CL ; M uwl  N uvl   Af CM  (1.56)
2 2

Từ (1.52) đến (1.56) ma trận lực và mô men thủy động được mô tả:
 X u|u| | u | X uv u  X v|v| | v | X uwu  X w|w| | w | 0 0 0
 0 Yuvd u  Yv|v| | v | Yuvl u 0 0 0 0 

 0 0 Z uwd u  Z w|w| | w |  Z uwl u 0 0 0
D(  )    (1.57)
 0 0 0 0 0 0
 
 0 0 M uwl u 0 0 0
 0 Nuvl u 0 0 0 0 

1.4.2.4 Các lực và mô men của bánh lái

xbl Xb
s

Zb

xbl Xb
h
Yb

Hình 1.18: Vị trí, góc bẻ lái của bánh lái hướng và bánh lái sâu

Hai bánh lái nằm ngang điều khiển AUV theo độ sâu hay góc chúc ngóc được
quay bởi hai máy lái điện theo cặp, hai bánh đứng điều khiển AUV theo góc hướng
và góc lắc được quay bởi hai máy lái điện độc lập, công thức thực nghiệm cho lực
nâng và mô men của bánh lái là [15]:
1
Lbl   CLbl Sbl eu 2 ; M bl  xbl Lbl (1.58)
2
 37

Trong đó, CL là tốc độ thay đổi hệ số lực nâng và Sbl là diện tích bề mặt của
bl

bánh lái.  e (rad ) là góc ảnh hưởng của bánh lái (hình 1.18).
v  xblh r
Đối với bánh lái hướng, góc ảnh hưởng của bánh lái là  e   h  , đối với
u
w  xbls q
bánh lái sâu góc ảnh hưởng là  e   s  . Thông thường khoảng cách từ trục
u
các bánh lái hướng và bánh lái sâu đến tâm trọng lực là bằng nhau xbl  xblh  xbls .
Từ các phương trình lực nâng và mô men ta có hệ số thủy động theo các trục hệ
tọa độ gắn liền:

 1
 X l   2  CLblh Sblh [u  l  uu  xbl up]  X uul u  l  X uul uu  X upl up
2 2


Y  1  C S [u 2  uv  x ur ]  Y u 2  Y uv  Y ur
 h 2 L blh blh h bl uu h h uvh urh


 Z s   1  CL Sbls [u 2 s  uw  xbl uq ]  Z uu u 2 s  Z uw uw  Z uq uq
 2 bls s s s

 (1.59)
 K   1  C S x [u 2  uu  x up ]  K u 2  K uu  K up
L blh blh bl uu l
 l 2
l bl l uul upl

 1
 M s    CLbls Sbls xbl [u 2 s  uw  xbl uq]  M uu s u 2 s  M uws uw  M uqs uq
 2
 1
 N h    CLblh Sblh xbl [u  h  uv  xbl ur ]  N uu h u  h  N uvh uv  N urh ur
2 2

 2

Từ (1.59) thông số lực và mô men bánh lái theo các trục đối với hệ tọa độ gắn
liền được viết dưới dạng ma trận:

 X uul u 0 0 X upl u 0 0 
 
 0 Yuvh u 0 0 0 Yurh u 
 0 0 Z uws u 0 Z uqs u 0 
L(  )    (1.60)
 K uu u 0 0 K upl u 0 0 
 l 
 0 0 M uws u 0 M uqs u 0 
 
 0 N uvh u 0 0 0 N urh u 

Lực và mô men bánh lái theo các trục đối với hệ tọa độ gắn liền là:

 bl  ( X uu  l u 2 , Yuu  h u 2 , Z uu  s u 2 , K uu  l u 2 , M uu  s u 2 , N uu  hu 2 )T

l h s l s h
(1.61)
 38

với,  h   h 2   h1 ;  l   h 2   h1 trong đó  h1 ,  h 2 là góc bẻ lái các bánh lái hướng;

 s   s 2   s1 trong đó  s1 ,  s 2 là góc bẻ lái các bánh lái sâu.

Đối với AUV có hệ động lực là động cơ tên lửa, lực đẩy tạo ra đối với AUV
thường là không đổi và được biểu diễn:  pl  ( X pl , 0, 0, 0, 0, 0)T (1.62)

1.4.3 Các yếu tố môi trường tác động lên phương tiện ngầm tự hành

AUV chuyển động ở độ sâu lớn nên chỉ xét đến sự ảnh hưởng của dòng chảy
đại dương, không xét đến ảnh hưởng của sóng, gió và các yếu tố môi trường khác
như sự thay đổi nhiệt độ, mật độ nước.
ucN


wcD
vcE

Vc

Hình 1.19: Vận tốc dòng chảy đại dương trong hệ tọa độ địa lý
Vận tốc dòng chảy Vc được phân tích thành các thành phần ucN , vcE , wcD trong hệ
tọa độ địa lý (hình 1.19) thông qua các góc  và  [39].

ucN  Vc cos  cos  ; vcE  Vc cos  sin  ; wcD  Vc sin  (1.63)

Vận tốc dòng chảy đối với hệ tọa độ gắn liền được ký hiệu Vcb  (uc , vc , wc )T
trong đó uc , vc , wc là các thành phần vận tốc dòng chảy theo các trục X b , Yb , Z b .
Mối quan hệ giữa vận tốc dòng chảy trong hệ tọa độ gắn liền và hệ tọa độ địa lý
được mô tả thông qua ma trận chuyển Cbn :

(uc , vc , wc )T  (Cbn )T (ucN , vcE , wcD )T (1.64)

 39

Vận tốc tương đối của AUV khi này Vtd  (u , v, w)T  (uc , vc , wc )T  (utd , vtd , wtd )T .
Kết hợp các phương trình (1.47), (1.51), (1.56) và (1.59) các thành phần lực và
mô men thủy động được viết lại:

 X uu  X uul  X u|u | ; X up  X upl


Yuv  Yuvl  Yuvh  Yuvd ; Yur  Yura  Yurh
Z  Z  Z  Z ; Z uq  Z uqa  Z uqs
 uw uwl uws uwd
(1.65)

 K uu  K uul ; K up  K upl
M  M  M  M ; M uq  M uqa  M uqs
 uw uwa uws uwl

 N uv  N uv  N uv  N uv ; N ur  N ura  N urh
 a h l

Từ các thành phần lực và mô men trong công thức (1.45) và với cách viết như
(1.65), tổng các ngoại lực và mô men ngoại lực tác động lên AUV là:

 X ext  X HS  X uu u 2 l  X uu uu  X u u  X uv uv  X uwuw  X v|v|v | v |  X vr vr 

 l

  X w|w| w | w |  X up up  X wq wq  X qq qq  X rr rr  X pl

 Yext  YHS  Yuu h u  h  Yur ur  Yuvuv  Yv|v|v | v | Yv v  Ywp wp  Ypq pq  Yr r
2


 Z ext  Z HS  Z uu s u  s  Z uwuw  Z uq uq  Z vp vp  Z w|w| w | w |  Z w w  Z q q  Z rp rp (1.66)
2


 K ext  K HS  Kuu uu  K p p  K uul u  l  K up
2


 M ext  M HS  M uu s u  s  M uwuw  M uq uq  M vp vp  M w w  M q q  M rp rp
2

 N  N  N u 2  N ur  N uv  N v  N wp  N pq  N r
 ext HS uu h h ur uv v wp pq r

1.5 Kết luận chương 1

Qua phân tích tổng quan về phương tiện ngầm và hệ thống dẫn đường, điều
khiển nó cho phép rút ra các kết luận sau:
1. Chủng loại vũ khí tên lửa, ngư lôi chống ngầm (vũ khí chống ngầm) là loại
vũ khí quan trọng trong chiến tranh hiện đại. Tuy nhiên, trong hệ thống điều
khiển của các chủng loại vũ khí chống ngầm hiện có tại Việt Nam chưa được
trang bị thiết bị dẫn đường nên khi tác chiến trong điều kiện thời tiết phức
tạp rất khó khăn. Vì vậy cần phải cải tiến, hiện đại hóa bằng cách trang bị
thêm thiết bị dẫn đường quán tính và phải giải quyết một số vấn đề có tính
học thuật.
 40

2. Việc trang bị thiết bị dẫn đường quán tính không đế có ưu điểm giá thành rẻ.
Tuy nhiên phải có các giải pháp khắc phục các nhược điểm mà hiện nay công
nghệ chế tạo chưa giải quyết được đó là độ trôi tín hiệu đầu ra của các phần tử
đo trong thiết bị dẫn đường quán tính. Việc đưa thêm các phần tử đo khác đòi
hỏi kèm theo các thuật toán xử lý đồng bộ. Đây là vấn đề còn mới và có nhiều
giải pháp thực thi khác nhau.

3. Việc trang bị thiết bị dẫn đường quán tính có đế tuy giá thành cao nhưng khắc
phục được các nhược điểm cơ bản của thiết bị dẫn đường quán tính không đế.
Tuy nhiên, cần xây dựng thuật toán xác định ma trận Côsin định hướng giữa hệ
tọa độ đế và hệ tọa độ địa lý. Đây thực sự là vấn đề còn mới ở Việt Nam. Các
tài liệu nước ngoài khi chuyển giao vũ khí có thiết bị dẫn đường quán tính
không đề cập đến vấn đề này. Vì vậy, đòi hỏi phải nghiên cứu vấn đề xác định
ma trận Côsin định hướng giữa hệ tọa độ đế và hệ tọa độ địa lý khi ứng dụng
thiết bị dẫn đường quán tính có đế.
4. Để hiệu chỉnh quỹ đạo chuyển động của vũ khí chống ngầm khi trang bị thêm
thiết bị dẫn đường quán tính cần phải xây dựng thuật toán tạo lệnh điều khiển
cho vũ khí chống ngầm ở giai đoạn chuyển động dưới nước, đưa vũ khí chống
ngầm về điểm tiếp cận quỹ đạo mong muốn tính toán trước đảm bảo xác xuất
phát hiện mục tiêu là lớn nhất.
Những vấn đề nêu trên sẽ lần lượt được giải quyết ở các chương sau của bản
luận án này.
 41

Chương 2
XÂY DỰNG THUẬT TOÁN XÁC ĐỊNH THAM SỐ DẪN ĐƯỜNG
CHO VŨ KHÍ CHỐNG NGẦM

Trong các bài toán dẫn đường cho các phương tiện chuyển động nói chung và
vũ khí chống ngầm nói riêng luôn có hai vấn đề: xác định tham số định hướng và
xác định tham số định vị. Vấn đề xác định tham số định hướng là xác định tương
quan về góc giữa hệ tọa độ gắn liền với vật thể và hệ tọa độ địa lý (được chọn làm
hệ tọa độ dẫn đường), vấn đề xác định tham số định vị là xác định vị trí tâm khối và
vận tốc của nó so với hệ tọa độ địa lý. Luận án dựa trên nguyên tắc về dẫn đường
quán tính để xây dựng thuật toán dẫn đường quán tính không đế xác định tham số
dẫn đường cho vũ khí chống ngầm trong cả hai giai đoạn: giai đoạn chuyển động
trong khí quyển và giai đoạn chuyển động trong nước. Luận án còn đề xuất thuật
toán xác định ma trận Côsin định hướng giữa hệ tọa độ đế và hệ tọa độ địa lý trong
trường hợp ASWs được trang bị thiết bị dẫn đường quán tính không đế.
Trên hình 2.1 mô tả chức năng hệ thống điều khiển cho vũ khí chống ngầm.
Nhiệm vụ cuối cùng của hệ thống điều khiển là quay các bánh lái (máy lái) ML1,
ML2, ML3, ML4 sao cho vũ khí chống ngầm tiếp cận đến mục tiêu ở tư thế ngòi nổ
tiếp xúc được kích hoạt nhằm tiêu diệt mục tiêu.

Hình 2.1: Chức năng của hệ thống điều khiển vũ khí chống ngầm
Cấu trúc của hệ thống điều khiển cải tiến có trang bị thiết bị dẫn đường quán
tính không đế cho vũ khí chống ngầm được thể hiện trên hình 2.2. Đối với hệ thống
điều khiển có thiết bị dẫn đường quán tính có đế thì trên hình 2.2 thay vào vị trí
thiết bị dẫn đường quán tính không đế và thiết bị đo không quán tính là thiết bị dẫn
đường quán tính có đế đã trình bày ở chương 1. Hệ thống điều khiển thực hiện hai
 42

nhiệm vụ cơ bản, nhiệm vụ xác định tham số dẫn đường và nhiệm vụ tạo lệnh điều
khiển. Từ cấu trúc hệ thống điều khiển cải tiến này cho thấy có hai cụm thuật toán
chính cần phải giải quyết đó là thuật toán dẫn đường và thuật toán điều khiển.

Hình 2.2: Cấu trúc hệ thống điều khiển vũ khí chống ngầm
Chương 2 luận án sẽ giải quyết các thuật toán dẫn đường ở cả giai đoạn rơi
trong khí quyển và cả giai đoạn chuyển động tự lập từ điểm chạm nước thực về
điểm tiếp cận quỹ đạo mong muốn. Các thuật toán liên quan đến việc tạo lệnh điều
khiển sẽ được giải quyết trong chương 3.
2.1 Xây dựng thuật toán dẫn đường quán tính không đế cho vũ khí chống
ngầm
Vũ khí chống ngầm thả từ máy bay hoạt động ở hai giai đoạn: giai đoạn chuyển
động trong khí quyển và giai đoạn chuyển động trong nước. Quá trình ASWs
chuyển động trong khí quyển theo quỹ đạo rơi có dù. Ở chế độ rơi có dù vận tốc
 43

chuyển động của ASWs biến thiên nhỏ. Giai đoạn chuyển động trong nước có sử
dụng thiết bị dẫn đường quán tính được tính từ điểm sau khi ASWs chạm nước
xuống độ sâu 20m chuyển động về điểm tiếp cận quỹ đạo mong muốn đã trình bày
trong chương 1.
2.1.1 Xây dựng thuật toán xác định tham số dẫn đường cho vũ khí chống
ngầm ở giai đoạn chuyển động trong khí quyển
Giai đoạn chuyển động rơi theo quỹ đạo có dù trong khí quyển, vận tốc chuyển
động của ASWs biến thiên nhỏ. Trong trường hợp này luận án đề xuất phương án
ứng dụng bộ lọc Kalman phi tuyến mở rộng ước lượng trực tiếp các tham số Rodrig
- Hamilton trên cơ sở các thông tin quan sát do con quay vi cơ, gia tốc kế, từ kế
cung cấp.
Giả thiết trên vũ khí chống ngầm có các phần tử đo sau (hình 2.3):
- Ba con quay vi cơ gắn theo các phương của hệ tọa độ gắn liền có thể đo các
thành phần vận tốc góc quay của hệ tọa độ gắn liền;
- Ba gia tốc kế được gắn theo các phương của hệ tọa độ gắn liền có thể đo các gia
tốc cảm nhận theo các phương của hệ tọa độ gắn liền;
- Ba từ kế đo ba thành phần trong hệ tọa độ gắn liền của véc tơ từ trường trái đất
nơi vũ khí chống ngầm đang hoạt động;

Gb
T2
T1 A1 C1 Xb
A2 T3
C2
Yb A3

C3

Zb

T1 , T2 , T3 - các từ kế; A1 , A2 , A3 - các gia tốc kế; C1 , C2 , C3 - các con quay vi cơ

Hình 2.3: Bố trí con quay vi cơ đo tốc độ góc, gia tốc kế và từ kế

Đặt các biến của véc tơ trạng thái X :

 44

x1  0 ; x2  1 ; x3  2 ; x4  3 ; x5  p;
(2.1)
x6  q; x7  r ; x8  bp ; x9  bq ; x10  br

Biến X khi đó là véc tơ X  ( x1 , x2 , x3 , x4 , x5 , x6 , x7 , x8 , x9 , x10 )T

Từ các phương trình (1.29) viết lại:
p   p  bp  w1 ; q  q  bq  w2 ; r  r  br  w3 (2.2)

Thay các phương trình (2.2) vào các phương trình (1.26) ta có:
20  1 ( p  bp  w1 )  2 (q  bq  w2 )  3 (r  br  w3 ) (2.3)

21  0 ( p  bp  w1 )  3 (q  bq  w2 )  2 (r  br  w3 ) (2.4)

22  3 ( p  bp  w1 )  0 (q  bq  w2 )  1 (r  br  w3 ) (2.5)

23  2 ( p  bp  w1 )  1 (q  bq  w2 )  0 (r  br  w3 ) (2.6)

Với cách đặt biến như (2.1) các phương trình (2.3) đến (2.6) được viết lại:
2 x1   x2 ( p  x8  w1 )  x3 (q  x9  w2 )  x4 (r  x10  w3 )
(2.7)
  x2 ( p  x8 )  x3 (q  x9 )  x4 (r  x10 )  x2 w1  x3 w2  x4 w3

2 x2  x1 ( p  x8  w1 )  x3 (r  x10  w3 )  x4 (q  x9  w2 )

(2.8)
 x1 ( p  x8 )  x3 (r  x10 )  x4 (q  x9 )  x1w1  x3 w3  x4 w2

2 x3  x1 (q  x9  w2 )  x2 (r  x10  w3 )  x4 ( p  x8  w1 )

(2.9)
 x1 (q  x9 )  x2 (r  x10 )  x4 ( p  x8 )  x1w2  x2 w3  x4 w1

2 x4  x1 (r  x10  w3 )  x2 (q  x9  w2 )  x3 ( p  x8  w1 )

(2.10)
 x1 (r  x10 )  x2 (q  x9 )  x3 ( p  x8 )  x1w3  x2 w2  x3 w1

Tách các số hạng có thừa số nhiễu w1 , w2 , w3 trong các phương trình vi phân
(2.7) đến (2.10), sau đó rời rạc các số hạng còn lại theo phương pháp Ơle ta có các
hàm động học f1 , f 2 , f3 , f 4 :
x1 (k )  f1 ( X k 1 )  x1 (k  1) / 2  T [ x2 (k  1)( p  x8 ( k  1))  x3 (k  1) (q  x9 ( k  1))
(2.11)
 x4 (k  1)(r  x10 (k  1))] / 2

x2 (k )  f 2 ( X k 1 )  x2 (k  1) / 2  T [ x1 (k  1)( p  x8 (k  1))  x3 (k  1) (r  x10 ( k  1))

(2.12)
 x4 (k  1)(q  x9 (k  1))] / 2
 45

x3 (k )  f3 ( X k 1 )  x3 (k  1)  T [ x1 (k  1)(q  x9 (k  1))  x2 ( k  1)(r  x10 ( k  1))

(2.13)
 x4 (k  1)( p  x8 (k  1))] / 2

x4 (k )  f 4 ( X k 1 )  x4 (k  1) / 2  T [ x1 (k  1)(r  x10 (k  1))  x2 ( k  1)(q  x9 ( k  1))

(2.14)
 x3 (k  1)( p  x8 (k  1))] / 2

Ở đây, T là bước tính khi rời rạc hóa.

Đối với bộ lọc Kalman phi tuyến mở rộng [47], các tham số không được mô tả
bằng phương trình vi phân như các tốc độ góc p, q, r sẽ được xác định như sau:
x5 (k )  f5 ( X k 1 )  x5 (k  1) (2.15)
x6 (k )  f 6 ( X k 1 )  x6 (k  1) (2.16)
x7 (k )  f 7 ( X k 1 )  x7 (k  1) (2.17)
Độ trôi bp , bq , br cũng có thể viết dưới dạng:

x8 (k )  f8 ( X k 1 )  x8 (k  1) (2.18)
x9 (k )  f9 ( X k 1 )  x9 (k  1) (2.19)
x10 (k )  f10 ( X k 1 )  x10 (k  1) (2.20)
Như vậy từ các phương trình (2.11) đến (2.20) cho đầy đủ các hàm động học
f1 , f 2 ,..., f10 của véc tơ hàm Fk 1 ( X k 1 ) .
Fk 1
Tiếp theo xác định ma trận chuyển vị  k 1  X  Xˆ k 1 (  )
bằng cách lấy đạo
X
f i  
hàm riêng , nhận được [47]: k 1   fi   ij  , i  1,10; j  1,10 (2.21)
x j  x j 
trong đó,
12  ( p  x8 (k  1))T / 2 ; 13  (q  x9 (k  1))T / 2 ; 14  (r  x10 (k  1))T / 2 ;
18  x2 (k  1)T / 2 ; 19  x3 (k  1)T / 2 ; 110  x4 (k  1)T / 2 ; 21  ( p  x8 (k  1))T / 2 ;
23  (r  x10 (k  1))T / 2 ; 24  (q  x9 (k  1))T / 2 ; 28   x1 (k  1)T / 2 ;
29  x4 (k  1)T / 2 ; 210   x3 (k  1)T / 2 ; 31  (q  x9 (k  1))T / 2 ;
32  (r  x10 (k  1))T / 2 ; 34  ( p  x8 (k  1))T / 2 ; 38   x4 (k  1)T / 2 ;
39   x1 (k  1)T / 2 ; 310  x2 (k  1)T / 2 ; 41  (r  x10 (k  1))T / 2 ;
42  (q  x9 (k  1))T / 2 ; 43  ( p  x8 (k  1))T / 2 ; 48  x3 (k  1)T / 2 ;
 46

49   x2 (k  1)T / 2 ; 410   x1 (k  1)T / 2 ; ii  1 ;

tất cả các ij khác đều bằng 0, i  1,10; j  1,10 .

Từ các phương trình (2.7) đến (2.10) các hàm chứa nhiễu động học được viết lại
dưới dạng rời rạc theo phương pháp Ơle:
g1  [ w1 x2 (k  1)  w2 x3 (k  1)  w3 x4 (k  1)]T / 2 (2.22)
g 2  [ w1 x1 (k  1)  w3 x3 (k  1)  w2 x4 (k  1)]T / 2 (2.23)
g3  [ w2 x1 (k  1)  w3 x2 (k  1)  w1 x4 (k  1)]T / 2 (2.24)
g 4  [ w1 x3 (k  1)  w2 x2 (k  1)  w3 x1 (k  1)]T / 2 (2.25)
gi
Lấy đạo hàm riêng , nhận được ma trận hệ số nhiễu [47]:
w j

G   gij  , i  1,10; j  1,3 (2.26)

trong đó,
g11  x2 (k  1)T / 2 ; g12  x3 (k  1)T / 2 ; g13  x4 (k  1)T / 2 ; g 21   x1 (k  1)T / 2 ;

g 22  x4 (k  1)T / 2 ; g 23   x3 (k  1)T / 2 ; g31   x4 (k  1)T / 2 ; g32   x1 (k  1)T / 2 ;

g33  x2 (k  1)T / 2 ; g 41   x3 (k  1)T / 2 ; g 42   x2 (k  1)T / 2 ; g 43   x1 (k  1)T / 2 ;

các hệ số gij khác đều bằng 0.

Ma trận Q là ma trận đường chéo với các giá trị trên đường chéo là các phương
sai của các con quay vi cơ w1 , w2 , w3 .
Vấn đề tiếp theo là tìm các hàm quan sát hi . Như trên đã nêu trong ASWs có ba
từ kế đo ba thành phần z1 , z2 , z3 trong hệ tọa độ gắn liền của véc tơ từ trường trái đất
nơi ASWs đang hoạt động. Giả sử từ trường trái đất vùng ASWs hoạt động không
thay đổi và có các giá trị lần lượt theo ba phương của hệ tọa độ địa lý như sau:
Bx , By , Bz (các giá trị này được đo một lần bằng các từ kế khi hệ tọa độ gắn liền

trùng với hệ tọa độ địa lý hoặc được đo bằng cách nào đó). Dễ dàng nhận thấy giữa
z1 , z2 , z3 và Bx , By , Bz có quan hệ mật thiết với nhau qua ma trận Côsin định hướng

Cbn , tức là:

 47

( z1 , z2 , z3 )T  (Cbn )T ( Bx , By , Bz )T (2.27)

Triển khai biểu thức (2.27) với ma trận Cbn như (1.12) ta có:

z1  (202  212  1) Bx  (212  20 3 ) By  (213  20 2 ) Bz (2.28)

z2  (212  20 3 ) Bx  (202  222  1)) By  (22 3  20 1 ) Bz (2.29)

z3  (213  20 2 ) Bx  (22 3  20 1 ) By  (202  232  1) Bz (2.30)

Với cách đặt biến (2.1), từ (2.28) đến (2.30) ta có ba hàm quan sát:
z1 (k ) h 1 ( X k )  (2 x12 (k )  2 x22 (k )  1) Bx  (2 x2 ( k ) x3 (k )  2 x1 (k ) x4 ( k )) By
(2.31)
 (2 x2 (k ) x4 (k )  2 x1 (k ) x3 (k )) Bz

z2 (k )  h2 ( X k )  (2 x2 (k ) x3 (k )  2 x1 (k ) x4 ( k )) Bx  (2 x12 ( k )  2 x32 (k )  1) By
(2.32)
 (2 x3 (k ) x4 (k )  2 x1 (k ) x2 (k )) Bz

z3 (k ) h3 ( X k )  (2 x2 (k ) x4 (k )  2 x1 (k ) x3 (k )) Bx  (2 x3 (k ) x4 ( k )  2 x1 ( k ) x2 ( k )) By
(2.33)
 (2 x12 (k )  2 x42 (k )  1) Bz

Giả thiết trong vùng hoạt động của ASWs véc tơ gia tốc trọng trường có
phương và giá trị không đổi. Chiếu véc tơ đó xuống hệ tọa độ địa lý có được các
thành phần như sau g x 0 , g y 0 , g z 0 (các thành phần này có thể xác định bằng cách đọc

gia tốc kế khi làm cho hệ tọa độ gắn liền trùng với hệ tọa độ địa lý và cho ASWs bất
động). Gia tốc kế không đo gia tốc chuyển động mà cho chỉ số của gia tốc cảm nhận
(gia tốc biểu kiến) [32], tức là:
abx  f N  g x ; aby  f E  g y ; abz  f D  g z , trong đó f N , f E , f D là hình chiếu gia tốc

tuyệt đối của ASWs xuống các trục của hệ tọa độ gắn liền, còn g x , g y , g z là hình

chiếu của véc tơ gia tốc trọng trường xuống các trục tương ứng của hệ tọa độ gắn
liền. Dễ dàng nhận thấy trong trường hợp thả ASWs từ máy bay ở chế độ có dù thì
f N  0; f E  0; f D  0 . Trong trường hợp không xét đến nhiễu đo, ba gia tốc kế đo

ba thành phần ( z4 , z5 , z6 )T  (abx , aby , abz )T  ( g x , g y , g z )T . Khi đó có thể coi:

( z4 , z5 , z6 )T  (Cbn )T ( g x 0 , g y 0 , g z 0 )T (2.34)

Triển khai biểu thức (2.34) với ma trận Cbn như (1.12) và cách đặt biến như (2.1)
ta có:
 48

z4 (k )  h 4 ( X k )  (2 x12 (k )  2 x22 (k )  1) g x 0  (2 x2 (k ) x3 (k )  2 x1 (k ) x4 (k )) g y 0
(2.35)
 (2 x2 (k ) x4 (k )  2 x1 (k ) x3 (k )) g z 0

z5 (k )  h5 ( X k )  (2 x2 (k ) x3 (k )  2 x1 (k ) x4 (k )) g x 0  (2 x12 (k )  2 x32 (k )  1) g y 0
(2.36)
 (2 x3 (k ) x4 (k )  2 x1 (k ) x2 (k )) g z 0

z6 (k )  h6 ( X k ) (2 x2 (k ) x4 (k )  2 x1 (k ) x3 (k )) g x 0  (2 x3 (k ) x4 (k )  2 x1 (k ) x2 (k )) g y 0
(2.37)
 (2 x12 (k )  2 x42 (k )  1) g z 0

Theo các biểu thức (1.29) trong trường hợp các con quay vi cơ không xét đến
nhiễu đo, các chỉ số z7 , z8 , z9 con quay vi cơ đo được sẽ là:
z7 (k )  h7 ( X k )  p  bp  x5 (k )  x8 (k ) (2.38)

z8 (k )  h8 ( X k )  q  bq  x6 (k )  x9 (k ) (2.39)

z9 (k )  h9 ( X k )  r  br  x7 (k )  x10 (k ) (2.40)
Từ (2.31 – 2.33), (2.35 – 2.37) và (2.38 – 2.40) ta có đầy đủ 9 hàm quan sát
(h1 , h2 ,..., h 9 ) của véc tơ hàm h( X k ) .

hk hi
Việc xác định ma trận đo H k  X  Xˆ k (  )
bằng cách lấy đạo hàm riêng nhận
X x j
được [47]: H k   hij  , i  1,9; j  1,10 . (2.41)

trong đó,
h11  4 Bx x1 (k )  2 By x4 (k )  2 Bz x3 (k ) ; h12  4 Bx x2 (k )  2 By x3 (k )  2 Bz x4 (k ) ;

h13  2 By x2 (k )  2 Bz x1 (k ) ; h14  2 By x1 (k )  2 Bz x2 (k ) ;

h21  2 Bx x4 (k )  4 By x1 (k )  2 Bz x2 (k ) ; h22  2 Bx x3 (k )  2 Bz x1 (k ) ;

h23  2 Bx x2 (k )  4 By x3 (k )  2 Bz x4 (k ) ; h24  2 Bx x1 (k )  2 Bz x3 (k ) ;

h31  2 Bx x3 (k )  2 By x2 (k )  4 Bz x1 (k ) ; h32  2 Bx x4 (k )  2 By x1 (k ) ;

h33  2 Bx x1 (k )  2 By x4 (k ) ; h34  2 Bx x2 (k )  2 By x3 (k )  4 Bz x4 (k ) ;

h41  2 g z 0 x3 (k ); h42  2 g z 0 x4 (k ); h43  2 g z 0 x1 (k ); h44  2 g z 0 x2 (k ); h51  2 g z 0 x2 (k ) ;

h52  2 g z 0 x1 (k ) ; h53  2 g z 0 x4 (k ) ; h54  2 g z 0 x3 (k ) ; h61  4 g z 0 x1 (k ) ; h64  4 g z 0 x4 (k ) ;

h1010  h99  h88  h710  h69  h58  1 ; tất cả các hij khác đều bằng 0.
 49

Lọc EKF
Các phương Q
tiện đo R
Hiệu chỉnh
3 con ma trận K k
quay vi cơ
Z k  ( z1 ,..., z9 )T Xˆ k(  )
3 từ kế Kk Fk 1 ( Xˆ k(1) )

Zˆ k Xˆ k(  )
3 gia tốc
kế

ax ay az hk ( Xˆ k(  ) )

Xác định tham số dẫn đường Xˆ k(  )

Cbn M
0 , 1 , 2 , 3

 2   20 3 
  arctg  12 2 
 20  21  1 
2

   arcsin(213  20 2 )

 2   20 1 
  arctg  22 3 
 20  23  1 
2

g VD
VD (0) z (0)
fD  
z
 
VE (0) VE
 y (0)
fE y
 
VN (0) VN
fN  x(0)
x
 
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ế
Giả sử nhiễu đo của các từ kế, gia tốc kế và con quay vi cơ là ồn trắng thì ta có:
z1  h1 ( X k )  vT 1 ; z2  h2 ( X k )  vT 2 ; z3  h3 ( X k )  vT 3 ; z4  h4 ( X k )  w4 ; z5  h5 ( X k )  w5 ;

z6  h6 ( X k )  w6 ; z7  h7 ( X k )  w1 ; z8  h8 ( X k )  w2 ; z9  h9 ( X k )  w3 .
 50

trong đó,
vT 1 , vT 2 , vT 3 là nhiễu đo ba từ kế; w4 , w5 , w6 là nhiễu đo của ba gia tốc kế và
w1 , w2 , w3 là nhiễu đo của ba con quay vi cơ.
Nếu nhiễu đo của các từ kế, gia tốc kế và con quay vi cơ không tương quan với
nhau thì ma trận R là ma trận đường chéo với các giá trị trên đường chéo là các
phương sai của các từ kế, gia tốc kế và con quay vi cơ.
Như vậy đã có đầy đủ yếu tố ( Fk 1 ( X k 1 ), hk ( X k ), Gk 1 ( X k 1 ), Q, R đã được xác
định) để thực hiện thủ tục lọc Kalman (hình 2.4). Kết quả ước lượng theo lưu đồ
giải thuật bộ lọc Kalman phi tuyến mở rộng nhận được 4 tham số Rodrig-Hamilton
0 , 1 , 2 , 3 , từ đó xác định 3 góc định hướng  ,  , theo công thức (1.15). Các
thành phần của véc tơ gia tốc trong hệ tọa độ địa lý được xác định từ các chỉ số gia
tốc kế theo công thức (1.28), vận tốc và tọa độ tâm khối của ASWs trong hệ tọa độ
địa lý có được bằng cách giải phương trình vi phân (1.22), (1.23) như sơ đồ hình
2.4, trong đó ma trận M được xác định:
1 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 
M  (2.42)
0 0 1 0 0 0 0 0 0 0
 
0 0 0 1 0 0 0 0 0 0

Hàm Fk 1 ( Xˆ (  ) ) được thiết lập từ các phương trình (2.11-2.20), trong đó Xˆ (  ) là

k 1 k 1

véc tơ trạng thái ước lượng ở bước thứ k  1 . Hàm hk ( Xˆ (  ) ) được thiết lập từ các
k

phương trình (2.31-2.33) và các phương trình (2.35-2.40), trong đó Xˆ (  ) là véc tơ

k

trạng thái ước lượng ở bước thứ k .

2.1.2 Xây dựng thuật toán xác định tham số dẫn đường cho vũ khí chống
ngầm ở giai đoạn chuyển động trong nước
Giai đoạn vũ khí chống ngầm chuyển động trong nước có gia tốc, khi đó phương
trình (2.34) không đúng nên thuật toán như trường hợp chuyển động rơi trong khí
quyển không thể áp dụng. Trong trường hợp này, luận án đề xuất phương án ứng
dụng bộ lọc Kalman phi tuyến mở rộng ước lượng trực tiếp các tham số dẫn đường
trên cơ sở các thông tin quan sát do con quay vi cơ, gia tốc kế, từ kế, vận tốc kế và
 51

cảm biến áp suất cung cấp. Thuật toán này cho phép kế thừa các thiết bị đo đã dùng ở
giai đoạn chuyển động trong khí quyển (hình 2.3), chỉ thêm các phần tử đo sau:
- Vận tốc kế đo 3 thành phần vận tốc trong hệ tọa độ gắn liền. Vận tốc kế
(Doppler Velocity Log – DVL) được gắn trực tiếp trên thân ASWs phát và thu
sóng siêu âm nhờ hiệu ứng Dopler của sóng siêu âm phản xạ từ đáy biển hoặc
phản xạ từ mặt biển khi hoạt động trong môi trường nước.
- Cảm biến áp suất đo độ sâu của ASWs so với bề mặt nước.
Từ các phương trình (1.30) viết lại có:
a bx  a x  w4 ; a by  a y  w5 ; a bz  a z  w6 (2.43)

Thay các phương trình (2.43) vào các phương trình (1.28) ta có:
 f N =(202  212  1)(a x  w4 )  (212  20 3 )(a y  w5 )  (213  20 2 )(a z  w6 )

 f E =(212  20 3 )(a x  w4 )  (20  22  1)(a y  w5 )  (22 3  20 1 )(a z  w6 ) (2.44)
2 2


 f D =(213  20 2 )(a x  w4 )  (22 3  20 1 )(a y  w5 )  (20  23  1)(a z  w6 )
2 2

Thay các phương trình (2.44) vào các phương trình (1.22), các thành phần vận
tốc trong hệ tọa độ địa lý sẽ là:
 =f =(2 2  2 2  1)a  (2   2  )a  (2   2  )a
VN N 0 1 x 1 2 0 3 y 1 3 0 2 z
(2.45)
 (202  212  1)w4  (212  20 3 ) w5  (213  20 2 ) w6
 =f =(2   2  )a  (2 2  2 2  1)a  (2   2  )a
VE E 1 2 0 3 x 0 2 y 2 3 0 1 z
(2.46)
 (212  20 3 ) w4  (202  222  1)w5  (22 3  20 1 ) w6
 =f  g =(2   2  )a  (2   2  )a  (2 2  2 2  1)a
VD D 1 3 0 2 x 2 3 0 1 y 0 3 z
(2.47)
 (213  202 )w4  (223  201 )w5  (202  232  1)w6  g

Đặt các biến của véc tơ trạng thái X :

x1  0 ; x2  1 ; x3  2 ; x4  3 ; x5  bp ; x6  bq ; x7  br
(2.48)
x8  VN ; x9  VE ; x10  VD ; x11  z

Biến X khi đó là véc tơ X  ( x1 , x2 , x3 , x4 , x5 , x6 , x7 , x8 , x9 , x10 , x11 )T

Với cách đặt biến như (2.48) các phương trình (2.3) đến (2.6) được viết lại :
2 x1   x2 ( p  x5  w1 )  x3 (q  x6  w2 )  x4 (r  x7  w3 )
(2.49)
  x2 ( p  x5 )  x3 (q  x6 )  x4 (r  x7 )  x2 w1  x3 w2  x4 w3
 52

2 x2  x1 ( p  x5  w1 )  x3 (r  x7  w3 )  x4 (q  x6  w2 )

(2.50)
 x1 ( p  x8 )  x3 (r  x10 )  x4 (q  x9 )  x1w1  x3 w3  x4 w2

2 x3  x1 (q  x6  w2 )  x2 (r  x7  w3 )  x4 ( p  x5  w1 )

(2.51)
 x1 (q  x9 )  x2 (r  x10 )  x4 ( p  x8 )  x1w2  x2 w3  x4 w1

2 x4  x1 (r  x7  w3 )  x2 (q  x6  w2 )  x3 ( p  x5  w1 )

(2.52)
 x1 (r  x10 )  x2 (q  x9 )  x3 ( p  x8 )  x1w3  x2 w2  x3 w1

Với cách đặt biến như (2.48), các phương trình (2.45) đến (2.47) được viết lại:
x8 =(2x12  2x22  1)a x  (2 x2 x3  2 x1 x4 )a y  (2 x2 x4  2 x1 x3 )a z
(2.53)
 (2x12  2x22  1)w4  (2 x2 x3  2 x1 x4 ) w5  (2 x2 x4  2 x1 x3 ) w6

x9 =(2 x2 x3  2 x1 x4 )a x  (2x12  2x32  1)a y  (2 x3 x4  2 x1 x2 )a z

(2.54)
 (2 x2 x3  2 x1 x4 ) w4  (2x12  2x32  1)w5  (2 x3 x4  2 x1 x2 ) w6

x10 =(2 x2 x4  2 x1 x3 )a x  (2 x3 x4  2 x1 x2 )a y  (2x12  2x42  1)a z

(2.55)
 (2 x2 x4  2 x1 x3 )w4  (2 x3 x4  2 x1 x2 )w5  (2x12  2x42  1)w6  g

Tách các số hạng có thừa số nhiễu w1 , w2 , w3 , w4 , w5 , w6 trong các phương trình vi

phân (2.49) đến (2.52) và (2.53) đến (2.55), sau đó rời rạc các số hạng còn lại theo
phương pháp Ơle ta có các hàm động học f1 , f 2 , f3 , f 4 , f8 , f9 , f10 :
T
 p (k  1) x2 (k  1)  x2 (k  1) x5 (k  1)  
f1 ( xk 1 )  x1 (k )  x1 (k  1) 
2
(2.56)
T T
 q (k  1) x3 (k  1)  x3 (k  1) x6 (k  1)   r (k  1) x4 (k  1)  x4 (k  1) x7 (k  1)
2 2
T
 p (k  1) x1 (k  1)  x1 (k  1) x5 (k  1) 
f 2 ( xk 1 )  x2 (k )  x2 (k  1) 
2 (2.57)
T T
 r (k  1) x3 (k  1)  x3 (k  1) x7 (k  1)  q (k  1) x4 (k  1)  x4 (k  1) x6 (k  1) 
2 2
T
 p (k  1) x4 (k  1)  x4 (k  1) x5 (k  1) 
f3 ( xk 1 )  x3 (k )  x3 (k  1) 
2 (2.58)
T T
 r (k  1) x2 (k  1)  x2 (k  1) x7 (k  1)  q (k  1) x1 (k  1)  x1 (k  1) x6 (k  1) 
2 2
T
r (k 1) x1 (k 1)  x1 (k 1) x7 (k 1)
f 4 ( xk 1 )  x4 (k )  x4 (k  1) 
2
(2.59)
T T
 q (k  1) x2 (k  1)  x2 (k  1) x6 (k  1)    p (k  1) x3 (k  1)  x3 (k  1) x5 (k  1) 
   
2 2
 53

f8 ( xk 1 )  x8 (k )  x8 (k  1)  T  2 x12 (k  1)  2 x22 (k  1)  1 ax (k  1)

 T  2 x2 (k  1) x3 (k  1)  2 x1 (k  1) x4 (k  1) a y (k  1) (2.60)
 T  2 x2 (k  1) x4 (k  1)  2 x1 (k  1) x3 (k  1) az (k  1)

f 9 ( xk 1 )  x9 (k )  x9 (k  1)  T  2 x12 (k  1)  2 x32 (k  1)  1 a y (k  1)

 T  2 x2 (k  1) x3 (k  1)  2 x1 (k  1) x4 (k  1) ax (k  1) (2.61)
 T  2 x3 (k  1) x4 (k  1)  2 x1 (k  1) x2 (k  1) az (k  1)

f10 ( xk 1 )  x10 (k )  x10 (k  1)  T  2 x12 (k  1)  2 x42 (k  1)  1 az (k  1)

 T  2 x2 (k  1) x4 (k  1)  2 x1 (k  1) x3 (k  1) ax (k  1) (2.62)
 T  2 x3 (k  1) x4 (k  1)  2 x1 (k  1) x2 (k  1) a y (k  1)  Tg

Ở đây, T là bước tính khi rời rạc hóa.

Trong trường hợp bp , bq , br biến đổi chậm các hàm động học f j ( xk 1 ) với

j  5, 7 được coi:
f j ( xk 1 )  x j (k )  x j (k  1) (2.63)
Với cách đặt biến như (2.48), từ các phương trình (1.23) ta có hàm động học
theo độ sâu f11 được viết dưới dạng rời rạc:
f11 ( xk 1 )  x11 (k )  x11 (k  1)  Tx10 (k  1) (2.64)
Như vậy các phương trình từ (2.56) đến (2.64) cho đầy đủ các hàm động học
f1 , f 2 ,..., f11 của véc tơ hàm Fk 1 ( X k 1 ) .
Fk 1
Tiếp theo xác định ma trận chuyển vị  k 1  X  Xˆ k 1 (  )
bằng cách lấy đạo
X

f i  
hàm riêng , nhận được [47]: k 1   fi   ij  , i  1,11; j  1,11 (2.65)
x j  x j 

trong đó,
12    p (k  1)  x5 (k  1)  T / 2 ; 13   q (k  1)  x6 (k  1)  T / 2 ;

14   r (k  1)  x7 (k  1)  T / 2 ; 15   x2 (k  1)  T / 2; 16   x3 (k  1)  T / 2;

17   x4 (k  1)  T / 2 ; 21   p (k  1)  x5 (k  1)  T / 2 ;

23  r (k  1)  x7 (k  1)  T / 2 ; 24   q (k  1)  x6 (k  1)  T / 2 ;

25    x1 (k  1)  T / 2 ; 26   x4 (k  1)  T / 2 ; 27    x3 (k  1)  T / 2 ;

 54

31  q (k  1)  x6 (k  1)  T / 2 ; 32   r (k  1)  x7 (k  1)  T / 2 ;

34   p (k  1)  x5 (k  1)  T / 2 ; 35   x4 (k  1)T / 2 ; 36   x1 (k  1)T / 2 ;

37  x2 (k  1)T / 2 ; 41  r (k  1)  x7 (k  1)  T / 2 ; 42  q (k  1)  x6 (k  1)  T / 2 ;

43    p (k  1)  x5 (k  1)  T / 2 ; 45  x3 (k  1)T / 2 ; 46   x2 (k  1)T / 2 ;

47   x1 (k  1)T / 2 ;

81  4Tx1 (k  1)ax (k  1)  2Tx4 (k  1)a y (k  1)  2Tx3 (k  1)az (k  1) ;

82  4Tx2 (k  1)ax (k  1)  2Tx3 (k  1)a y (k  1)  2Tx4 (k  1)az (k  1) ;

83  2Tx2 (k  1)a y (k  1)  2Tx1 (k  1)az (k  1) ;

84  2Tx1 (k  1)a y (k  1)  2Tx2 (k  1)az (k  1) ;

91  2Tx4 (k  1)ax (k  1)  4Tx1 (k  1)a y (k  1)  2Tx2 (k  1)az (k  1) ;

92  2Tx3 (k  1)ax (k  1)  2Tx1 (k  1)az (k  1) ;

93  2Tx2 (k  1)ax (k  1)  4Tx3 (k  1)a y (k  1)  2Tx4 (k  1)az (k  1) ;

94  2Tx1 (k  1)ax (k  1)  2Tx3 (k  1)az (k  1) ;

101  2Tx3 (k  1)ax (k  1)  2Tx2 (k  1)a y (k  1)  4Tx1 (k  1)az (k  1) ;

102  2Tx4 (k  1)ax (k  1)  2Tx1 (k  1)a y (k  1) ;

103  2Tx1 (k  1)ax (k  1)  2Tx4 (k  1)a y (k  1) ;

104  2Tx2 (k  1)ax (k  1)  2Tx3 (k  1)a y (k  1)  4Tx4 (k  1)az (k  1) ;

ii  1, i  1,11 , còn tất cả các ij khác đều bằng 0.

Từ các phương trình (2.49) đến (2.55) các hàm với các số hạng có thừa số nhiễu
w1 , w2 , w3 , w4 , w5 , w6 được viết lại dưới dạng rời rạc theo phương pháp Ơle:

g1  x2 (k  1) w1T / 2  x3 (k  1) w2T / 2  x4 (k  1) w3T / 2 (2.66)

g 2   x1 (k  1) w1T / 2  x4 (k  1) w2T / 2  x3 (k  1) w3T / 2 (2.67)
g3   x4 (k  1) w1T / 2  x1 (k  1) w2T / 2  x2 (k  1) w3T / 2 (2.68)
g 4  x3 (k  1) w1T / 2  x2 (k  1) w2T / 2  x1 (k  1) w3T / 2 (2.69)
 55

g8  T  2 x12 (k  1)  2 x22 (k  1)  1 w4  T  2 x2 (k  1) x3 (k  1)  2 x1 (k  1) x4 (k  1) w5

(2.70)
 T  2 x2 (k  1) x4 (k  1)  2 x1 (k  1) x3 (k  1) w6

g9  T  2 x2 (k  1) x3 (k  1)  2 x1 (k  1) x4 (k  1)  w4  T  2 x12 (k  1)  2 x32 (k  1)  1 w5

(2.71)
 T  2 x3 (k  1) x4 (k  1)  2 x1 (k  1) x2 (k  1) w6

g10  T  2 x12 (k  1)  2 x42 (k  1)  1 w6  T  2 x2 (k  1) x4 (k  1)  2 x1 (k  1) x3 (k  1) w4

(2.72)
 T  2 x3 (k  1) x4 (k  1)  2 x1 (k  1) x2 (k  1) w5

gi
Lấy đạo hàm riêng , nhận được ma trận hệ số nhiễu [47]:
w j

 g 
G   i    gij  , i  1,11; j  1,6 (2.73)
 wj 
trong đó,
g11  x2 (k  1)T / 2 ; g12  x3 (k  1)T / 2 ; g13  x4 (k  1)T / 2 ; g 21   x1 (k  1)T / 2 ;

g 22  x4 (k  1)T / 2 ; g 23   x3 (k  1)T / 2 ; g31   x4 (k  1)T / 2 ; g32   x1 (k  1)T / 2 ;

g33  x2 (k  1)T / 2 ; g 41  x3 (k  1)T / 2 ; g 42   x2 (k  1)T / 2 ; g 43   x1 (k  1)T / 2 ;

g84  T (2 x12 (k  1)  2 x22 (k  1)  1); g85  T (2 x2 (k  1) x3 (k  1)  2 x1 (k  1) x4 (k  1));

g86  T (2 x2 (k  1) x4 (k  1)  2 x1 (k  1) x3 (k  1));

g94  T (2 x2 (k  1) x3 (k  1)  2 x1 (k  1) x2 (k  1)); g95  T (2 x12 (k  1)  2 x32 (k  1)  1);

g96  T (2 x3 (k  1) x4 (k  1)  2 x1 (k  1) x3 (k  1));

g104  T (2 x3 (k  1) x4 (k  1)  2 x1 (k  1) x2 (k  1));
g105  T (2 x3 (k  1) x4 (k  1)  2 x1 (k  1) x2 (k  1)); g106  T (2 x12 (k  1)  2 x42 (k  1)  1);

Các hệ số gij khác đều bằng 0.

Nếu nhiễu đo của các con quay vi cơ và các gia tốc kế không tương quan lẫn
nhau thì ma trận Q là ma trận đường chéo với các giá trị trên đường chéo là các
phương sai của các con quay vi cơ và gia tốc kế.
Vấn đề tiếp theo là tìm các hàm quan sát. Các hàm quan sát z1 , z2 , z3 do ba từ kế
đo được như trường hợp ASWs rơi trong khí quyển đã trình bày, tức z1 , z2 , z3 xác
định theo các biểu thức (2.31), (2.32) và (2.33).
 56

Từ thông tin ba thành phần vận tốc trong hệ tọa độ gắn liền cho ta ba hàm quan
sát z4 , z5 , z6 , vận tốc đo được từ vận tốc kế cho chỉ số V DVL  (VxDVL , VyDVL , VzDVL )T .
Mối liên hệ giữa vận tốc trong hệ tọa độ gắn liền và vận tốc trong hệ tọa độ địa lý
được viết:
(VxDVL , VyDVL , VzDVL )T  (Cbn )T (VN , VE , VD )T (2.74)
Triển khai biểu thức (2.74) với ma trận Cbn như (1.12). Với cách đặt biến như
(2.48), ba hàm quan sát có được là:
h4 ( X k )  z4 (k )  (2 x12 (k )  2 x22 (k )  1) x8 (k )  (2 x2 (k ) x3 (k )  2 x1 (k ) x4 ( k )) x9 (k )
(2.75)
 (2 x2 (k ) x4 (k )  2 x1 (k ) x3 (k )) x10 (k )

h5 ( X k )  z5 (k )  (2 x2 (k ) x3 (k )  2 x1 ( k ) x4 (k )) x8 ( k )  (2 x12 (k )  2 x32 (k )  1) x9 (k ) 
(2.76)
 (2 x3 ( k ) x4 ( k )  2 x1 ( k ) x2 (k )) x10 ( k )

h6 ( X k )  z6 (k )  (2 x2 (k ) x4 (k )  2 x1 (k ) x3 (k )) x8 ( k )
(2.77)
 (2 x3 (k ) x4 (k )  2 x1 (k ) x2 (k )) x9 (k )  (2 x12 (k )  2 x42 ( k )  1) x10 ( k )

Cảm biến áp suất đo độ sâu của ASWs so với mặt nước cho hàm quan sát z7
h7 ( X k )  z7 (k )  x11 (k ) (2.78)
Từ (2.31 – 2.33) và (2.75 – 2.78) ta có đầy đủ 7 hàm quan sát (h1 , h2 ,..., h 7 ) của
véc tơ hàm h( X k ) .
hk hi
Việc xác định ma trận đo H k  X  Xˆ k (  )
bằng cách lấy đạo hàm riêng
X x j

nhận được [47]: H k   hij  , i  1, 7; j  1,11 . (2.79)

trong đó,
hij , i  1,3; j  1, 4 như trường hợp ASWs rơi trong khí quyển đã xét;

h41  4 x1 (k ) x8 (k )  2 x4 (k ) x9 (k )  2 x3 (k ) x10 ( k ) ;

h42  4 x2 (k ) x8 (k )  2 x3 (k ) x9 (k )  2 x4 (k ) x10 ( k ) ; h43  2 x2 (k ) x9 (k )  2 x1 (k ) x10 (k ) ;

h44  2 x1 (k ) x9 (k )  2 x2 (k ) x10 (k ) ; h48  2 x12 (k )  2 x22 (k )  1 ;

h49  2 x2 (k ) x3 (k )  2 x1 (k ) x4 (k ) ; h410  2 x2 (k ) x4 (k )  2 x1 (k ) x3 (k ) ;

h51  4 x1 (k ) x9 (k )  2 x4 (k ) x8 (k )  2 x2 (k ) x10 ( k ) ; h52  2 x3 (k ) x8 (k )  2 x1 (k ) x10 (k ) ;

h53  2 x2 (k ) x8 (k )  4 x3 (k ) x9 (k )  2 x4 ( k ) x10 ( k ) ; h54  2 x1 (k ) x8 (k )  2 x3 (k ) x10 (k ) ;

 57

h58  2 x2 (k ) x3 (k )  2 x1 (k ) x4 (k ) ; h59  2 x12 (k )  2 x32 (k )  1 ;

h510  2 x3 (k ) x4 (k )  2 x1 (k ) x2 (k ) ; h61  4 x1 (k ) x10 (k )  2 x3 (k ) x8 (k )  2 x2 (k ) x9 ( k ) ;

h62  2 x4 (k ) x8 (k )  2 x1 (k ) x9 (k ) ; h63  2 x1 (k ) x8 (k )  2 x4 (k ) x9 (k ) ;

h64  2 x2 (k ) x8 (k )  2 x3 (k ) x9 (k )  4 x4 (k ) x10 (k ) ;

h68  2 x2 (k ) x4 (k )  2 x1 (k ) x3 (k ) ; h69  2 x3 (k ) x4 (k )  2 x1 (k ) x2 (k ) ;

h610  2 x12 (k )  2 x42 (k )  1 ; h711  1. tất cả các hij khác đều bằng 0.

Giả sử nhiễu đo của từ kế, vận tốc kế và cảm biến áp suất là ồn trắng thì ta có:
z1  h1 ( X k )  vT 1 ; z2  h2 ( X k )  vT 2 ; z3  h3 ( X k )  vT 3 ; z4  h4 ( X k )  vV 1 ; z5  h5 ( X k )  vV 2 ;

z6  h6 ( X k )  vV 3 ; z7  h7 ( X k )  vz .

trong đó, vT 1 , vT 2 , vT 3 là nhiễu đo của 3 từ kế; vV 1 , vV 2 , vV 3 là nhiễu đo của vận tốc kế

và vz là nhiễu đo của cảm biến áp suất.
Nếu nhiễu đo của các từ kế, các vận tốc kế và cảm biến áp suất không tương
quan với nhau thì ma trận R là ma trận đường chéo với các giá trị trên đường chéo
là các phương sai của các từ kế, các vận tốc kế và cảm biến áp suất.
Như vậy đã có đầy đủ yếu tố ( Fk 1 ( X k 1 ), hk ( X k ), Gk 1 ( X k 1 ), Q, R đã được xác
định) để thực hiện thủ tục lọc Kalman (hình 2.5). Kết quả ước lượng theo lưu đồ
giải thuật bộ lọc Kalman phi tuyến mở rộng nhận được 4 tham số Rodrig-Hamilton
0 , 1 , 2 , 3 , các thành phần vận tốc VN , VE ,VD và độ sâu của ASWs trong hệ tọa độ
địa lý. Từ đó xác định được 3 góc định hướng  ,  , theo công thức (1.15) và tọa
độ tâm khối của ASWs trong mặt phẳng ngang ( x, y ) bằng cách giải hai phương
trình vi phân ở biểu thức (1.23) theo sơ đồ hình 2.5, trong đó các ma trận M , N , S
được xác định:
1 0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 0 
M  (2.80)
0 0 1 0 0 0 0 0 0 0 0
 
0 0 0 1 0 0 0 0 0 0 0

N   0 0 0 0 0 0 0 0 0 0 1 (2.81)
0 0 0 0 0 0 0 1 0 0 0
S  0 0 0 0 0 0 0 0 1 0 0  . (2.82)
0 0 0 0 0 0 0 0 0 1 0 
 58

Lọc EKF
Các phương tiện đo Q
R
phương tiện đo quán tính
Hiệu chỉnh
ma trận K k
3 gia tốc 3 con quay
kế vi cơ

Z k  ( z1 ,..., z7 )T Xˆ k(  )
3 từ kế 3 vận Kk Fk 1 ( Xˆ k(1) )
tốc kế
Zˆ k Xˆ k(  )
Cảm biến áp suất
phương tiện đo không
quán tính hk ( Xˆ k(  ) )

 Xác định tham số dẫn đường

 2   203 
  arctg  12 2 
 20  21  1 
2

 0 , 1 , 2 , 3 Xˆ k(  )
   arcsin(213  20 2 ) M

  2   201 
  arctg  22 3 
 20  23  1 
2

N
z y (0)
 VE
y

VE
 x(0) VN S
x

VN
VD

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
Hàm Fk 1 ( Xˆ (  ) ) được thiết lập từ các phương trình (2.56-2.64), trong đó Xˆ (  ) là
k 1 k 1

véc tơ trạng thái ước lượng ở bước thứ k  1 . Hàm hk ( Xˆ (  ) ) được thiết lập từ các
k

phương trình (2.31-2.33) và các phương trình (2.75-2.78), trong đó Xˆ (  ) là véc tơ k

trạng thái ước lượng ở bước thứ k .

 59

2.2 Xây dựng thuật toán dẫn đường quán tính có đế cho vũ khí chống ngầm
Khi sử dụng thiết bị dẫn đường quán tính có đế dạng giải tích để xác định các
tham số dẫn đường cho ASWs cần phải xác định ma trận Côsin định hướng Cdn giữa
hệ tọa độ đế và hệ tọa độ địa lý, từ đó sẽ xác định được các thành phần của véc tơ
gia tốc trong hệ tọa độ địa lý theo công thức (1.21), xác định được vận tốc và tọa độ
tâm khối của ASWs theo công thức (1.22) và (1.23). Từ việc xác định được ma trận
Cdn cũng cho phép xác định được ma trận Côsin định hướng Cbn giữa hệ tọa độ gắn

liền và hệ tọa độ địa lý theo công thức (1.24), từ đó có thể tính toán được các góc
định hướng theo công thức (1.15).
Việc xác định ma trận Côsin định hướng Cdn được thực hiện theo hai công đoạn,
công đoạn thô và công đoạn tinh. Công đoạn thô được xác định thông qua các thiết
bị đo trong bản thân thiết bị dẫn đường quán tính và thiết bị đo khác của máy bay
(phương tiện mang và thả vũ khí chống ngầm). Dễ dàng nhận thấy quan hệ giữa hệ
tọa độ đế và hệ tọa độ dẫn đường (hệ tọa độ địa lý) được thông qua các quan hệ sau:
Hệ tọa độ đế ( OX DYD Z D )  Hệ tọa độ gắn liền với ASWs ( OX bYb Z b )  hệ tọa độ gắn
liền với phương tiện mang ( OX mYm Z m )  hệ tọa độ địa lý ( OX 0Y0 Z 0 ) (hình 2.6).

Xm

X0
(Bắc)

Xb
YD

XD
Ym
Yb

O
Y0
(Đông)
X

Hình 2.6: Quan hệ giữa các hệ tọa độ

 60

Trong hình 2.6 các trục OZ D , OZ b , OZ m , OZ 0 vuông góc với mặt phẳng của trang
giấy nên không được thể hiện. Gọi ma trận Côsin định hướng của 3 quan hệ trên lần
lượt là Cdb , Cbm , Cmn , khi đó:
Cdn  Cdb Cbm Cmn (2.83)
Từ 3 góc 1 , 2 , 3 do 3 cảm biến góc của thiết bị đế cung cấp sẽ xác định ma

trận Cdb tương tự biểu thức (1.5) như sau:

cos1 cos2 cos1 sin 2 sin 3  sin 1 cos3 cos1 sin 2 cos3  sin 1 sin 3 
Cdb   sin 1 cos2 sin 1 sin 2 sin 3  cos1 cos3 sin 1 sin 2 cos3  cos1 sin 3  (2.84)
  sin 2 cos2 sin 3 cos2 cos3 

Khi còn nằm trên máy bay thì ASWs được treo chặt trên máy bay nên ma trận
Cbm được xác định một lần khi thiết kế vị trí và phương treo ASWs, tức là Cbm phải

được ghi trong tài liệu kỹ thuật của tổ hợp máy bay thả ASWs. Trên máy bay phải
có thiết bị đo các góc định hướng của máy bay với các phương Bắc – Nam, Đông –
Tây, thẳng đứng. Từ thông tin của các thiết bị này sẽ xác định được ma trận Cmn .

Tuy nhiên phương pháp đo trực tiếp ma trận định hướng Cdn vừa nêu có độ
chính xác không cao vì các sai số của nhiều thiết bị đo. Luận án đưa ra thuật toán
xác định ma trận Côsin định hướng Cdn theo cách tiếp cận khác (xác định tinh) có sử
dụng kết quả của công đoạn thô làm điều kiện đầu.
Trong trường hợp ASWs thả từ máy bay, trước khi ASWs rời khỏi máy bay để
hoạt động độc lập thì ASWs được liên kết chặt với máy bay, khi này có thể kết hợp
thông tin từ các thiết bị đo vận tốc của máy bay với thông tin của thiết bị dẫn đường
quán tính để thực hiện nhiệm vụ xác định ma trận Côsin định hướng Cdn .
2.2.1 Xây dựng thuật toán xác định ma trận Côsin định hướng giữa hệ tọa độ
đế với hệ tọa độ địa lý theo phương pháp phối hợp véc tơ vận tốc khi thả
vũ khí chống ngầm từ máy bay phản lực
Vì hệ tọa độ đế của thiết bị dẫn đường quán tính dạng giải tích được ổn định
trong không gian quán tính nên dễ dàng nhận thấy tốc độ quay của hệ tọa độ đế
  0 , khi đó phương trình (1.13) sẽ có dạng:
 61

    
2 (2.85)
Trong miền số thực các tham số Rodrig-Hamilton được viết:
20   N 1  E 3  D 2 (2.86)

21   N 0  E 2  D 3 (2.87)

22   N 3  E 1  D 0 (2.88)

23   N 2  E 0  D 1 (2.89)
X0
e
N
Z
VN
VE
Y0
E
VD
D
Z
O 0
R e
e l Y
X
Đường xích đạo

Hình 2.7: Hình chiếu vận tốc góc tuyệt đối  của hệ tọa độ địa lý
xuống các trục của nó
Phép chiếu của vận tốc góc tuyệt đối  của hệ tọa độ địa lý xuống các trục của
nó cho mô hình trái đất hình cầu có bán kính R (hình 2.7) được xác định [20]:
VE V V
N   U cos  ; E   N ; D   E tg   U sin  (2.90)
R R R
Đặt các biến của véc tơ trạng thái X :
x1  0 ; x2  1 ; x3  2 ; x4  3 ; x5  VN ; x6  VE ; x7  VD (2.91)
Thay (2.90) vào các phương trình (2.86 – 2.89) với cách đặt biến như (2.91),
các phương trình (2.86 – 2.89) được viết lại:
x6 x x
2 x1  (U cos   ) x2  (U sin   tg  6 ) x3  5 x4 (2.92)
R R R
x6 x x
2x2  (U cos   ) x1  5 x3  (U sin   tg 6 ) x4 (2.93)
R R R
x6 x x
2x3  (U sin   tg  ) x1  5 x2  (U cos   6 ) x4 (2.94)
R R R
 62

x5 x x
2x4  x1  (U sin   tg  6 ) x2  (U cos   6 ) x3 (2.95)
R R R

Gia tốc kế đo gia tốc chuyển động trong hệ tọa độ đế bao gồm gia tốc chuyển
động thực và nhiễu đo:
nx  ndx  w4 ; n y  ndy  w5 ; nz  ndz  w6 (2.96)

Viết lại các phương trình (2.96):

ndx  nx  w4 ; ndy  n y  w5 ; ndz  nz  w6 (2.97)

trong đó, w4 , w5 , w6 là nhiễu đo của gia tốc kế thường có dạng ồn trắng.

Viết lại (1.21) với Cdn có dạng (1.12):

f N =(202  212  1)ndx  (212  20 3 )ndy  (213  20 2 )ndz (2.98)

f E =(212  20 3 )ndx  (202  222  1)ndy  (22 3  20 1 )ndz (2.99)

f D =(213  20 2 )ndx  (22 3  20 1 )ndy  (202  232  1)ndz (2.100)

Đặt (1 , 2 , 3 )  ( w4 , w5 , w6 ) , thay các phương trình (2.97) vào các phương trình
(2.98) đến (2.100) ta có:
f N =(202  212  1)(nx  1 )  (212  20 3 )(n y  2 )  (213  20 2 )(nz  3 ) (2.101)

f E =(212  20 3 )(nx  1 )  (202  222  1)(n y  2 )  (22 3  20 1 )( nz  3 ) (2.102)

f D =(213  202 )(nx  1 )  (22 3  20 1 )(n y  2 )  (202  232  1)(nz  3 ) (2.103)

Thay các phương trình (2.101) đến (2.103) vào các phương trình (1.22), phương
trình vận tốc tâm khối của ASWs được viết lại:
 =(2 2  2 2  1)(n   )  (2   2  )(n   )  (2   2  )(n   )
V (2.104)
N 0 1 x 1 1 2 0 3 y 2 1 3 0 2 z 3

 =(2   2  )(n   )  (2 2  2 2  1)(n   )  (2   2  )(n   )

V (2.105)
E 1 2 0 3 x 1 0 2 y 2 2 3 0 1 z 3

 =(2   2  )(n   )  (2   2  )(n   )  (2 2  2 2  1)(n   )  g

V (2.106)
D 1 3 0 2 x 1 2 3 0 1 y 2 0 3 z 3

Theo cách đặt biến (2.91) các phương trình (2.104) đến (2.106) được viết lại:
x5  (2 x12  2 x22  1)(nx  1 )  2( x2 x3  x1 x4 )(n y  2 )  2( x2 x4  x1 x3 )(nz  3 )
 (2 x12  2 x22  1)nx  2( x2 x3  x1 x4 )n y  2( x2 x4  x1 x3 )nz (2.107)
 (2 x  2 x  1)1  2( x2 x3  x1 x4 )2  2( x2 x4  x1 x3 )3
2
1
2
2
 63

x6  (2 x12  2 x32  1)(n y  2 )  2( x2 x3  x1 x4 )(nx  1 )  2( x3 x4  x1 x2 )( nz  3 )

 (2 x12  2 x32  1)n y  2( x2 x3  x1 x4 )nx  2( x3 x4  x1 x2 )nz (2.108)
 (2 x12  2 x32  1)2  2( x2 x3  x1 x4 )1  2( x3 x4  x1 x2 )3

x7  (2 x12  2 x42  1)(nz  3 )  2( x2 x4  x1 x3 )(nx  1 )  2( x3 x4  x1 x2 )(ny  2 )  g

 (2 x12  2 x42  1)nz  2( x2 x4  x1 x3 )nx  2( x3 x4  x1 x2 )ny  g (2.109)
 (2 x  2 x  1)3  2( x2 x4  x1 x3 )1  2( x3 x4  x1 x2 )2
2
1
2
4

Rời rạc các phương trình (2.92) đến (2.95) theo phương pháp Ơle ta có các hàm
động học f1 , f 2 , f3 , f 4 :
T x (k 1)
f1 ( X k 1 )  x1 (k )  x1 (k 1)  [(U cos   6 ) x2 (k 1)
2 R
(2.110)
 x (k 1)  x5 (k 1)
 U sin   tg  6  x3 (k 1)  x4 (k 1)]
 R  R

T x ( k  1)
[  (U cos   6
f 2 ( X k 1 )  x2 ( k )  x2 ( k  1)  ) x1 ( k  1)
2 R
(2.111)
x ( k  1)  x ( k  1) 
 5 x3 ( k  1)   U sin   tg  6  x4 ( k  1)]
R  R 

T x ( k  1)
[ (U sin   tg  6
f 3 ( xk 1 )  x3 ( k )  x3 ( k  1)  ) x1 ( k  1)
2 R
(2.112)
x ( k  1)  x ( k  1) 
 5 x2 ( k  1)   U cos   6  x4 ( k  1)]
R  R 
T x (k 1)  x (k 1) 
f4 ( X k 1)  x4 (k )  x4 (k 1)  [ 5 )x1 (k 1)  U sin   tg 6  x2 (k 1)
2 R  R 
(2.113)
 x (k 1) 
 U cos   6  x3 (k 1)]
 R 

Tách các số hạng có thừa số nhiễu 1 , 2 , 3 trong các phương trình vi phân
(2.107) đến (2.109), sau đó rời rạc các số hạng còn lại theo phương pháp Ơle ta có
các hàm động học f5 , f 6 , f 7 :

f5 ( xk 1 )  x5 (k )  x5 (k  1)  T  2 x12 (k  1)  2 x22 (k  1)  1 nx (k  1)

 T  2 x2 (k  1) x3 (k  1)  2 x1 (k  1) x4 (k  1) ny (k  1) (2.114)
 T  2 x2 (k  1) x4 (k  1)  2 x1 (k  1) x3 (k  1) nz (k  1)
 64

f 6 ( xk 1 )  x6 (k )  x6 (k  1)  T  2 x12 (k  1)  2 x32 (k  1)  1 n y (k  1)

 T  2 x2 (k  1) x3 (k  1)  2 x1 (k  1) x4 (k  1) nx (k  1) (2.115)
 T  2 x3 (k  1) x4 (k  1)  2 x1 (k  1) x2 (k  1) nz (k  1)
f 7 ( xk 1 )  x7 (k )  x7 (k  1)  T  2 x12 (k  1)  2 x42 (k  1)  1 nz ( k  1)
 T  2 x2 (k  1) x4 (k  1)  2 x1 (k  1) x3 (k  1) nx (k  1) (2.116)
 T  2 x3 (k  1) x4 (k  1)  2 x1 (k  1) x2 (k  1) ny (k  1)  Tg
Ở đây T là bước tính rời rạc hóa.
f i
Ma trận chuyển vị được xác định bằng cách lấy đạo hàm riêng , nhận được
x j

 
[47]:    fi   ij  , i  1, 7; j  1, 7 (2.117)
x  j 
trong đó,
11  1 ; 12  T [(U cos   x6 (k  1) / R] / 2 ; 13  T [(U sin   tg  x6 (k  1) / R ] / 2 ;

14  Tx5 (k 1) / 2R ; 15  Tx4 (k 1) / 2R ; 16  T [ x2 (k 1)  tg  x3 (k 1)] / 2R ; 17  0 ;

21  T [(U cos   x6 (k  1) / R ] / 2 ; 22  1 ; 23  Tx5 (k  1) / 2 R ;

24  T [(U sin   tg  x6 (k  1) / R ] / 2 ; 25  Tx3 (k  1) / 2 R ;

26  T [ x1 (k  1)  tg  x4 (k  1)]2 R ;  27  0 ;

31  T [U sin   tg  x6 ( k  1) / R ] / 2 ; 32  Tx5 (k  1) / 2 R ; 33  1 ;

34  T [(U cos   x6 (k  1) / R] / 2 ; 35  Tx2 (k  1) / 2 R ;

36  T [tg  x1 (k  1)  x4 (k  1)] / 2 R ; 37  0 ;

41  Tx5 (k  1) / 2 R ; 42  T [(U sin   tg  x6 (k  1) / R] / 2 ;

43  T [(U cos   x6 (k 1) / R] / 2 ; 44  1 ; 45  Tx1 (k  1) / 2 R ;

46  T [tg  x2 (k  1)  x3 (k  1)] / 2 R ; 47  0 ;

51  4Tx1 (k  1)nx  2Tx4 (k  1)ny  2Tx3 (k  1)nz ;

52  4Tx2 ( k  1) nx  2Tx3 ( k  1) n y  2Tx4 ( k  1) nz ;

53  2Tx2 (k 1)ny  2Tx1 (k 1)nz ; 54  2Tx1 (k 1)ny  2Tx2 (k 1)nz ; 55  1; 56  0 ; 57  0 ;

61  2Tx4 (k  1)nx  2Tx1 (k 1)ny  2Tx2 (k  1)nz ; 62  2T x3 ( k  1) n x  2T x1 ( k  1) n z ;

 65

63  2Tx2 (k 1)nx  4Tx3 (k 1)ny  2Tx4 (k 1)nz ; 64  2Tx1 (k 1)nx  2Tx3 (k 1)nz ;

65  0 ; 66  1; 67  0 ; 71   2T x3 ( k  1) n x  2T x2 ( k  1) n y  4Tx1 ( k  1) n z ;

72  2Tx4 (k  1)nx  2Tx1 (k  1)ny ; 73  2Tx1(k 1)nx  2Tx4 (k 1)ny ;

74  2Tx2 (k 1)nx  2Tx3 (k 1)ny  4Tx4 (k 1)nz ; 75  0 ; 76  0 ; 77  1;

Từ các phương trình (2.107) đến (2.109) các hàm chứa nhiễu động học được
viết lại dưới dạng rời rạc theo phương pháp Ơle:
g5  T [2 x12 (k  1)  2 x22 (k  1)  1]1  T [2 x2 (k  1) x3 (k  1)
(2.118)
 2 x1 (k  1) x4 (k  1)]2  T [2 x2 (k  1) x4 (k  1)  2 x1 (k  1) x3 (k  1)]3
g 6  T [2 x12 (k  1)  2 x32 (k  1)  1]1  T [2 x2 (k  1) x3 (k  1)  2 x1 (k  1) x4 (k  1)]2
(2.119)
 T [2 x3 (k  1) x4 (k  1)  2 x1 (k  1) x2 (k  1)]3
g 7  T [2 x12 (k  1)  2 x42 (k  1)  1]1  T [2 x2 (k  1) x4 (k  1)  2 x1 (k  1) x3 (k  1)]2
(2.120)
 T [2 x3 (k  1) x4 (k  1)  x1 (k  1) x2 (k  1)]3
gi
Ma trận hệ số nhiễu được xác định bằng cách lấy đạo hàm riêng các
 j

phương trình (2.118 – 2.120) nhận được [47]:

 g 
G   i    gij  , i  1,7; j  1,3 (2.121)
  j 
trong đó,
g51  T [2 x12 (k  1)  2 x22 (k  1)  1] ; g52  T [2 x2 (k  1) x3 (k  1)  2 x1 (k  1) x4 (k  1)] ;

g53  T[2x2 (k 1) x4 (k 1)  2x1 (k 1) x3 (k 1)] ; g61  T [2x2 (k 1) x3 (k 1)  2 x1 (k 1) x4 (k 1)] ;

g 62  T [2 x12 (k  1)  2 x32 (k  1)  1] ; g 63  T [2 x3 (k  1) x4 (k  1)  2 x1 (k  1) x2 (k  1)] ;

g71  T[2x2 (k 1) x4 (k 1)  2x1 (k 1) x3 (k 1)] ; g72  T[2x3 (k 1) x4 (k 1)  2x1 (k 1) x2 (k 1)] ;

g 73  T [2 x12 (k  1)  2 x42 (k  1)  1)] ; Các hệ số gij khác đều bằng 0.

Ma trận Q là ma trận đường chéo với các phần tử đường chéo là phương sai của
các nhiễu đo của các gia tốc kế.
Trên máy bay luôn có các thiết bị đo vận tốc VN ,VE ,VD ta có véc tơ đo:

( z1 , z2 , z3 )T  (VN ,VE ,VD )T , khi đó các hàm quan sát có được là:

h1 ( X k )  x5 ; h2 ( X k )  x6 ; h3 ( X k )  x7 (2.122)
 66

Tuy nhiên, các phương tiện đo vận tốc cho kết quả có nhiễu đo dạng ồn trắng
v1 , v2 , v3 , tức là:

z1  h1 ( X k )  x5  v1 ; z2  h2 ( X k )  x6  v2 ; z3  h3 ( X k )  x7  v3 (2.123)
hi
Việc xác định ma trận đo bằng cách lấy đạo hàm riêng nhận được [47]:
x j

1 0 0 0 0 0 0
h  
Hk  k X  Xˆ k (  )
 0 1 0 0 0 0 0
  (2.124)
X
0 0 1 0 0 0 0

Từ (2.123) dễ dàng nhận thấy ma trận R là ma trận đường chéo với các phần tử
đường chéo là các phương sai của nhiễu đo vận tốc.
Như vậy, đã có đầy đủ các yếu tố ( Fk 1 ( X k 1 ), hk ( X k ), Gk 1 ( X k 1 ), Q, R đã được xác
định) để thực hiện thủ tục lọc Kalman (hình 1.15). Sau khi thực hiện lọc Kalman sẽ
nhận được các tham số 0 , 1 , 2 , 3 và từ đó xác định được ma trận Côsin định
hướng giữa hệ tọa độ quán tính (hệ tọa độ đế) khi còn nằm trên máy bay phản lực
với hệ tọa độ dẫn đường (hệ tọa độ địa lý) ở mọi nơi, mọi thời điểm. Tức là:
 202  212  1 212  20 3 213  20 2   c11 c12 c13 
 
Cdn   212  203 202  222  1 22 3  20 1   c21 c22 c23  (2.125)
 213  20 2 22 3  20 1 202  232  1   c31 c32 c33 
 
Đây chính là thủ tục của phương pháp xác định ma trận Côsin định hướng giữa
hệ tọa độ đế khi còn nằm trên máy bay phản lực với hệ tọa độ địa lý theo phương
pháp phối hợp véc tơ vận tốc trong trường hợp thả ASWs từ máy bay phản lực.
2.2.2 Xây dựng thuật toán xác định ma trận Côsin định hướng giữa hệ tọa độ
đế với hệ tọa độ địa lý theo phương pháp phối hợp véc tơ vận tốc khi thả
vũ khí chống ngầm từ máy bay lên thẳng
VN VE
Khi thả ASWs từ máy bay lên thẳng thì các thành phần , có giá trị vô
R R
cùng nhỏ. Mặt khác vận tốc quay của trái đất U có giá trị nhỏ (U = 150/giờ =
7,27.10-5 rad/giây), như vậy khi đó có thể coi các vế phải của các phương trình
(2.86 – 2.89) đều bằng không, tức là:
 67

0  const ; 1  const ; 2  const ; 3  const (2.126)

Như vậy giữa hệ tọa độ đế và hệ tọa độ dẫn đường (hệ tọa độ địa lý) không có
sự chuyển động tương đối với nhau. Việc áp dụng thuật toán lọc Kalman phi tuyến
mở rộng để xác định 0 , 1 , 2 , 3 không còn thực hiện được, khi này đòi hỏi cách
tiếp cận khác để xác định ma trận Côsin định hướng giữa hệ tọa độ đế và hệ tọa độ
dẫn đường. Trong trường hợp này luận án đề xuất thuật toán kết hợp gia tốc kế trên
cơ cấu đế của thiết bị dẫn đường quán tính có đế và vận tốc kế thường được trang bị
trên máy bay lên thẳng để xác định các phần tử cij (i, j  1,3) của ma trận Côsin định

hướng Cdn .
Phương trình mô tả sự thay đổi vận tốc chuyển động của máy bay lên thẳng
theo định luật Newton sẽ là:
VN  c11ndx  c12 ndy  c13 ndz (2.127)

VE  c21ndx  c22 ndy  c23ndz (2.128)

VD  c31ndx  c32 ndy  c33 ndz  g (2.129)

Thay các giá trị ndx , ndy , ndz theo các phương trình (2.97) vào các phương trình

(2.127) đến (2.129) nhận được:

VN  c11nx  c12ny  c13nz  (c11w4  c12 w5  c13w6 ) (2.130)

VE  c21nx  c22 n y  c23nz  (c21w4  c22 w5  c23 w6 ) (2.131)

VD  c31nx  c32ny  c33nz  (c31w4  c32w5  c33w6 )  g (2.132)

Nhiệm vụ của việc xác định ma trận Côsin định hướng giữa hệ tọa độ đế và hệ
tọa độ dẫn đường (hệ tọa độ địa lý) sẽ được quy về bài toán sau: Trên cơ sở thông
tin về VN ,VE ,VD do các phần tử đo vận tốc của máy bay lên thẳng cung cấp và thông
tin do các gia tốc kế đặt trên đế của thiết bị dẫn đường quán tính cung cấp cần phải
xác định các phần tử cij (i, j  1,3) .

Tiến hành tích phân theo thời gian hai vế của phương trình vi phân (2.130) nhận
được:
 68

T T T T T
dV
0 dtN dt  0 dVN  VN (T )  VN (0)  c11 0 nx dt  c12 0 ny dt  c13 0 nz dt
T T T
(2.133)
 [c11  w4 dt  c12  w5 dt  c13  w6 dt ]
0 0 0

Chia hai vế của đẳng thức (2.133) cho khoảng thời gian tích phân T và nhận được:
[VN (T )  VN (0)]  1  1T  1T 
T
   nx dt  c11    ny dt  c12    nz dt  c13
T T 0  T 0  T 0 
T T T
(2.134)
1 1 1
[c11  w4 dt  c12  w5 dt  c13  w6 dt ]
T0 T0 T0

Nếu thời gian tích phân đủ lớn thì theo định nghĩa về kỳ vọng toán học có:
T T T
1 1 1
lim
T  T 
0
w4dt  M{w4}  0 ; lim  w5 dt  M {w5}  0 ; lim  w6 dt  M {w6 }  0
T  T
0
T  T
0

vì w4 , w5 , w6 là các nhiễu dạng ồn trắng.

Khi đó phương trình (2.134) sẽ trở thành:
[VN (T )  VN (0)]  1  1T  1T 
T
   nx dt  c11    n y dt  c12    nz dt  c13 (2.135)
T T 0  T 0  T 0 
T
Chia khoảng thời gian T thành N khoảng T  và tính gần đúng các thừa số
N
tích phân trong biểu thức (2.135) sẽ có dạng:
T
1 1 N T N
1 N


T 0
ndt  
T i 1
ni T 
T
n
i 1
i 
N
n
i 1
i

Khi đó phương trình (2.135) sẽ có dạng sau:

1 1 N
 1 N
 1 N

T
VN (T )  VN (0)   
N

i 1
nx (i )  c11  
 N

i 1
n y (i )  c12  
 N
 n (i)  c
i 1
z 13 (2.136)

Tiếp tục tiến hành thực hiện phép tích phân 2 vế phương trình (2.131) trong
khoảng thời gian từ T đến 2T và từ 2T đến 3T, tương tự cách biến đổi trên nhận
được:
1 1 2N
 1 2N
 1 2N

T
VN (2T )  VN (T )   
N

i  N 1
nx (i )  c11  
 N

i  N 1
n y (i )  c12  
 N
 n (i)  c
i  N 1
z 13 (2.137)
 69

VN (3T )  VN (2T )     1  1 

1 1 3N 3N 3N

T N
 nx (i )  c11  
 N
 n y (i )  c12  
 N
 nz (i )  c13

(2.138)
i  2 N 1 i  2 N 1 i  2 N 1

Nếu coi c11 , c12 , c13 là ba nghiệm cần tìm thì ba phương trình (2.136 – 2.138) là
ba phương trình đại số tuyến tính, có thể viết dưới dạng ma trận như sau:
AX  B (2.139)
trong đó, X  (c11 , c12 , c13 )T

 1 N 1 N 1 N 
 N  nx (i)  ny (i)
N i 1

N i 1
nz (i) 
 [VN (T )  VN (0)] / T   i 1

 1 2N
1 2N 1 2N 
B   [VN (2T )  VN (T )] / T  ; A    nx (i)  ny (i)  nz (i) 
 [V (3T )  V (2T )] / T   N i  N 1 N i  N 1 N i  N 1 
 N N   1 3N 
1 3N 1 3N
  nx (i)
 N i 2 N 1
 ny (i)
N i 2 N 1

N i 2 N 1
nz (i) 

Như vậy đã xác định được các phần tử c11 , c12 , c13 của ma trận Côsin định hướng

Cdn giữa hệ tọa độ đế và hệ tọa độ địa lý được xác định:

 c11   (VN (T )  VN (0)) / T 

 c   A1  (V (2T )  V (T )) / T  (2.140)
 12   N N 
c   (V (3T )  V (2T )) / T 
 13   N N 
Tương tự cách làm nêu trên đối với phương trình vi phân (2.131), các phần tử
c21 , c22 , c23 của ma trận Cdn giữa hệ tọa độ đế và hệ tọa độ địa lý được xác định:

 c21   (VE (T )  VE (0)) / T 

 c   A1  (V (2T )  V (T )) / T  (2.141)
 22   E E 
c   (V (3T )  V (2T )) / T 
 23   E E 
Tương tự cách làm nêu trên đối với phương trình vi phân (2.132), các phần tử
c31 , c32 , c33 của ma trận Cdn giữa hệ tọa độ đế và hệ tọa độ địa lý được xác định :

 c31   (VD (T )  VD (0)) / T  g 

  1  
 c32   A  (VD (2T )  VD (T )) / T  g  (2.142)
c   (V (3T )  V (2T )) / T  g 
 33   D D 
Từ các biểu thức (2.140), (2.141) và (2.142) dễ dàng có các nhận xét sau: Nếu
các gia tốc nx , n y , nz không đổi hoặc có giá trị bằng 0 (chuyển động đều) trong quá

trình xác định ma trận Côsin định hướng giữa hệ tọa độ đế và hệ tọa độ địa lý, khi
 70

này ma trận A sẽ suy biến (định thức bằng 0) và trong trường hợp này không xác
định được nghiệm dạng (2.140), (2.141) và (2.142). Như vậy, để xác định ma trận
Côsin định hướng giữa hệ tọa độ đế và hệ tọa độ địa lý, máy bay phải chuyển động
với gia tốc thay đổi tức là phải cơ động. Nhận xét trên phù hợp với hướng dẫn sử
dụng tổ hợp tên lửa hành trình đối hải có thiết bị dẫn đường quán tính có đế được
đối tác nước ngoài chuyển giao.
Dùng phương pháp phối hợp véc tơ vận tốc cho phép xác định ma trận Côsin
định hướng giữa hệ tọa độ đế và hệ tọa độ địa lý cung cấp thông tin cơ sở để thiết bị
dẫn đường quán tính thực hiện việc xác định các tham số dẫn đường khi ASWs
chuyển động.
Việc xác định ma trận Côsin định hướng giữa hệ tọa độ đế và hệ tọa độ địa lý
được thực hiện khi ASWs còn nằm trên máy bay, sau khi ASWs rời máy bay sẽ
chuyển sang giai đoạn thực hiện thuật toán dẫn đường.
2.2.3 Thuật toán dẫn đường
Sau khi ASWs rời máy bay, hệ thống điều khiển chuyển sang giai đoạn xác
định tham số dẫn đường (khi rơi trong khí quyển có dù và khi chuyển động có điều
khiển trong nước).
Ma trận Côsin định hướng Cdn giữa hệ tọa độ đế và hệ tọa độ địa lý được xác
định thông qua các tham số Rodrig – Hamilton 0 , 1 , 2 , 3 khi thả từ máy bay phản
lực hay thông qua các phần tử cij (i, j  1,3) khi thả từ máy bay lên thẳng theo công
thức (2.125). Khi đó theo phương trình (1.21), các thành phần của véc tơ gia tốc
trong hệ tọa độ địa lý được xác định từ các giá trị đo của gia tốc kế:
f N =(202  212  1)nx  (212  20 3 ) n y  (213  20 2 )nz  c11nx  c12 n y  c13 nz (2.143)

f E =(212  203 ) nx  (202  222  1)n y  (22 3  20 1 )nz  c21nx  c22 n y  c23nz (2.144)

f D =(213  20 2 )nx  (22 3  20 1 ) n y  (202  232  1)nz  c31nx  c32 n y  c33 nz (2.145)

Từ (1.22), vận tốc tâm khối của ASWs trong hệ tọa độ địa lý được xác định:
VN  f N ; VE  f E ; VD  f D  g (2.146)
Khi đó tọa độ tâm khối của ASWs trong hệ tọa độ địa lý được xác định:
x  VN ; y  VE ; z  VD (2.147)
 71

Giai đoạn ASWs

ASWs
còn ở trên máy bay
1 2 3
Máy bay
Cơ cấu đế
Thuật toán xác
3 gia tốc kế 3 vận tốc kế
định ma trận Cdn

nx ny nz 
 c* 
  arctg  21
* 
Cbn  c11 

C n
d    arcsin c31
*

 c*  
x(0) VN (0)   arctg  32
 fN fE fD * 
x   c33 
VN
 
1
y  y (0) VE (0)
 b 1
(C ) Cdb 2
d

VE
  3

VD (0) g
z (0)   Giai đoạn ASWs rơi trong khí
z 
quyển và chuyển động có điều
  khiển trong nước
VD

Hình 2.8: Sơ đồ dẫn đường quán tính có đế

Trong quá trình ASWs rời vật mang, ma trận Côsin định hướng Cdn giữa hệ tọa
độ đế và hệ tọa độ địa lý không thay đổi và là tích giữa ma trận Côsin định hướng
Cdb giữa hệ tọa độ đế và hệ tọa độ gắn liền của ASWs với ma trận Côsin định hướng

Cbn giữa hệ tọa độ gắn liền ASWs và hệ tọa độ địa lý, tức là:

Cdn  CdbCbn (2.148)

Nhân hai vế phương trình (2.148) với (Cdb ) 1 có:

 c11
* *
c12 * 
c13
 * * * 
Cbn  (Cdb ) 1 Cdn  c21 c22 c23  (2.149)
 c* c* * 
c33 
 31 32
 72

Các góc định hướng của ASWs được xác định [32]:

 c*   c32
*

  arctg  21
*  ;    arcsin 
 c *
31

 ;   arctg  * 
(2.150)
 c11   c33 

Sơ đồ dẫn đường quán tính có đế chỉ ra trên hình 2.8.

2.3 Kết luận chương 2
Từ các kết quả chương 2 cho phép rút ra các kết luận sau:
1. Do các phần tử đo quán tính thực tế luôn có độ trôi và nhiễu đo, hai yếu tố này
sẽ gây ra sai số khi xác định tham số dẫn đường. Vì vậy phải sử dụng các
phương tiện đo khác để hiệu chỉnh các sai số. Đây thực sự là vấn đề còn mới ở
Việt Nam và cấp thiết khi khai thác, sử dụng hoặc cải tiến chế tạo mới loại vũ
khí chống ngầm. Vì vấn đề này là vấn đề bí mật quân sự nên ít được công bố,
đòi hỏi phải có các nghiên cứu của riêng Việt Nam.
2. Luận án đã xây dựng thuật toán xác định tham số dẫn đường cho vũ khí chống
ngầm ở giai đoạn chuyển động trong khí quyển trên cơ sở sử dụng từ kế kết hợp
với thông tin của gia tốc kế, thông tin các phần tử đo vận tốc góc và giai đoạn
chuyển động trong nước dùng vận tốc kế, cảm biến áp suất, từ kế kết hợp với
thông tin của gia tốc kế, thông tin các phần tử đo vận tốc góc. Thuật toán kết
hợp các phương tiện đo này đã giải quyết vấn đề sai số và độ trôi của thiết bị
dẫn đường quán tính không đế. Đây là đóng góp mới của luận án và được công
bố trong các công trình [6], [7], [8], [14], [15] của tác giả.
3. Thuật toán dẫn đường quán tính có đế luôn liên quan đến vấn đề xác định ma
trận Côsin định hướng giữa hệ tọa độ đế và hệ tọa độ địa lý. Vấn đề này cũng là
một vấn đề phức tạp và chưa được công bố rộng rãi, đặc biệt khi vũ khí chống
ngầm được thả từ máy bay.
4. Luận án đã xây dựng thuật toán xác định ma trận Côsin định hướng giữa hệ tọa
độ đế và hệ tọa độ địa lý của thiết bị dẫn đường quán tính có đế dùng thông tin
về vận tốc từ các thiết bị đo của máy bay trong cả hai trường hợp thả vũ khí
chống ngầm từ máy bay phản lực và từ máy bay lên thẳng. Đây là đóng góp
mới của luận án và được công bố trong công trình [10] của tác giả.
 73

Chương 3
XÂY DỰNG THUẬT TOÁN NHẬN DẠNG VÀ ĐIỀU KHIỂN
CHO VŨ KHÍ CHỐNG NGẦM
Vũ khí chống ngầm có trang bị thiết bị dẫn đường quán tính sẽ xác định được
các tham số định hướng và định vị của nó. Thông tin về tham số định vị được sử
dụng để tính toán quỹ đạo hiệu chỉnh, thông tin về tham số định hướng được xem
như là đầu ra đo được của ASWs và được sử dụng để tổng hợp lệnh điều khiển quay
các bánh lái đảm bảo cho ASWs đi theo quỹ đạo hiệu chỉnh đã xác định trước.
Trong chương này, tác giả đề xuất ứng dụng phương pháp bình phương tối thiểu đệ
quy để ước lượng tham số mô hình của ASWs, từ đó tổng hợp các lệnh điều khiển
hồi tiếp đầu ra nơron mờ thích nghi trực tiếp cho ASWs dựa vào tham số mô hình
nhận dạng được.
3.1 Phương trình chuyển động của vũ khí chống ngầm
3.1.1 Phương trình chuyển động tổng quát
Từ (1.40), (1.41), (1.45) và (1.66) phương trình chuyển động tổng quát của
ASWs trong hệ tọa độ 6 bậc tự do (hệ tọa độ gắn liền và hệ tọa độ địa lý):
( m  X u )u  X uu uu  ( X v|v| | v |  X uv u )v  ( X uwu  X |w|w | w |) w  X up up  ( X wq w 

  X qq q  mw) q  ( X vr v  X rr r  mv ) r  X pl  X uu u 2 l
l


( m  Yv )v  Yr r  (Yv|v| | v | Yuv u0 )v  mpw  Ywp wp  Ypq pq  (Yur u  mu ) r  Yuu u  h
2
h

( m  Z ) w  Z q  mqu  ( Z | w |  Z u ) w  ( Z v  Z r  mv ) p  Z uq  Z u 2
 w q | w| w uw vp rp uq uu
s s
 (3.1)
( I xx  K p ) p  K uu uu  K up up  ( I yy  I zz ) qr  z f B cos  sin   K uu u  l
2
l

  M w  ( I  M ) q  M uw  ( M v  M r ) p  ( I  I ) rp  M uq 
 w yy q uw vp rp zz xx uq

  z f B sin   x f B cos  cos   M uu u  s2

 s

  N v v  ( I zz  N r ) r  N uv uv  N wp wp  ( I xx  I yy ) pq  N pq pq  N ur ur  x f B cos  sin 

  N uu u 2 h
h

 x  u0 cos cos   v(cos sin  sin   sin cos  )  w(cos sin  cos   sin sin  )
 y  u sin cos   v(sin sin  sin   cos cos  )  w(sin sin  cos   cos sin  )
 0

 z  u0 sin   v(cos  sin  )  w cos  cos 


  p  q tan  sin   r tan  cos 
  q cos   r sin  (3.2)

  q sin  sec  r cos  sec
 74

Từ các phương trình (3.1), (3.2) đối với ASWs cho phép xây dựng bài toán điều
khiển tổng hợp ASWs theo các kênh.
Quỹ đạo chuyển động của ASWs trong nước là rất phức tạp. Với các tính chất
động hình học của ASWs ta có thể phân tích thành quỹ đạo trong mặt phẳng ngang
và trong mặt phẳng đứng. Trong mặt phẳng đứng ASWs được điều khiển theo góc
chúc ngóc, trong mặt phẳng ngang ASWs được điều khiển theo góc hướng, ngoài ra
ASWs còn được điều khiển ổn định góc lắc với đầu vào điều khiển là góc quay
bánh lái hướng.
Với đầu vào là thông số của một chủng loại ASWs, đầu ra điều khiển là góc
hướng, góc lắc, góc chúc ngóc hoàn toàn có thể xác định được nhờ thiết bị dẫn
đường quán tính theo thuật toán xác định tham số dẫn đường ở chương 2 và được
mô phỏng kiểm chứng ở mục 4.2.
Dưới dạng tổng quát (1.41) và (1.45) phương trình động học ASWs được viết lại:
M RB   CRB (  )   M A   C A (  )   D(  )   L(  )   g ( )   (3.3)
Phương trình (3.3) được viết lại:
M   C (  )   D(  )   L(  )   g ( )   (3.4)
trong đó, M  M A  M RB ; C  C A  CRB
Kết hợp (3.4) và (1.40) ta có hệ phương trình:
M   C (  )   D(  )   L(  )   g ( )  
(3.5)
  J ( ) 

Từ (3.5) có thể viết   J 1 ( ) (3.6)

Đạo hàm hai vế (3.6) ta có:

  J 1 ( )[  J ( ) J 1 ( ) ] (3.7)

Thay các phương trình (3.6), (3.7) vào (3.4) khi đó (3.4) được viết lại:
MJ ( )[  J ( ) J 1 ( ) ]  C (  ) J 1 ( )  D(  ) J 1 ( )  L(  ) J 1 ( )  g ( )  
1
(3.8)

 MJ 1 ( )  [ MJ 1 ( ) J ( )  C (  )  D (  )  L(  )]J 1 ( )  g ( )   (3.9)

Nhân hai vế (3.9) với J T ( ) ta có:

J T ( ) MJ 1 ( )  J T ( )[ MJ 1 ( ) J ( )  C (  )  D(  )  L(  )]J 1 ( )

(3.10)
 J T ( ) g ( )  J T ( )
 75

 M  ( )  C (  , )  g ( )   (3.11)

trong đó,
M  ( )  J T ( ) MJ 1 ( )

C (  , )  J T ( )[ MJ 1 ( ) J ( )  C (  )  D (  )  L(  )]

g ( )  J T ( ) g ( )

  J T ( )

Trong hệ tọa độ địa lý M  ( )  M T ( )  0 [39], khi đó (3.11) được viết lại:

  M 1 ( )C (  , )  M 1 ( ) g ( )  M 1 ( ) (3.12)

Từ (3.5) và (3.12) với    pl   bl hệ phương trình (3.5) được viết lại:

  J ( ) 
 (3.13)
  M  ( )C (  , )  M  ( ) g ( )  M  ( ) J ( ) pl  M  ( ) J ( ) bl
1 1 1 T 1 T

Hệ phương trình (3.13) có dạng bậc 2 mô tả chuyển động của ASWs có 6 bậc
tự do.
ASWs được điều khiển bởi hai bánh lái trong mặt phẳng nằm ngang (bánh lái
sâu)  s1 ,  s 2 và hai bánh lái trong mặt phẳng đứng (bánh lái hướng)  h1 ,  h 2 (hình
3.1). Các bánh lái hướng  h1 ,  h 2 vừa điều khiển ASWs theo góc hướng và điều
khiển giảm lắc bởi các máy lái độc lập. Các bánh lái sâu  s1 ,  s 2 điều khiển ASWs
theo góc chúc ngóc bởi hai máy lái theo cặp, nghĩa là  s1   s 2 .
Định nghĩa góc quay bánh lái để điều khiển tên lửa theo góc hướng là  h , góc
quay bánh lái điều khiển góc lắc là  l , trong đó:
 h1   h 2
l  (3.14)
2
 h 2   h1
h  (3.15)
2
khi đó,
 h1   l   h (3.16)
 h2  l   h (3.17)
 76

 h1

 s2  s1

Y Y

 h2
Z Z

Hình 3.1: Vị trí các bánh lái trên ASWs

Điều khiển tổng hợp ASWs gồm điều khiển theo góc hướng, theo góc chúc
ngóc và điều khiển giảm lắc.
3.1.2 Phương trình chuyển động trong các mặt phẳng
Phương trình chuyển động của ASWs có 6 bậc tự do là hệ nhiều đầu vào, nhiều
đầu ra với khối lượng tham số cần phải tính toán nhiều, phức tạp, một số tham số
cần phải tính toán bằng thực nghiệm, vì thế xây dựng bài toán điều khiển trong
trường hợp này rất khó khăn. Chuyển động của ASWs trong nước được coi là 3
chuyển động độc lập nhau (chuyển động theo góc hướng trong mặt phẳng ngang,
chuyển động theo góc chúc ngóc trong mặt phẳng đứng và chuyển động quay quanh
trục dọc ASWs), thực tế 3 chuyển động có liên quan đến nhau và ảnh hưởng lẫn
nhau, nếu giả thiết chuyển động là độc lập sẽ gây ra các sai số, tuy nhiên nhờ có hệ
thống điều khiển giảm lắc nên có thể bỏ qua các sai số này.
Một số công trình nghiên cứu [15], [23], [39] đề xuất phương pháp hạn chế bậc
tự do để đơn giản hóa hệ phương trình (3.1), (3.2) về các phương trình trong mặt
phẳng ngang và mặt phẳng đứng mà không mất tính tổng quát đặc tính động học của
ASWs theo từng mặt phẳng, cho phép xây dựng bài toán nhận dạng và điều khiển
theo từng mặt phẳng làm cơ sở để xây đựng bài toán điều khiển tổng hợp ASWs.
3.1.2.1 Phương trình chuyển động trong mặt phẳng đứng
Giả thiết ASWs chuyển động không trượt nghĩa là lực bên Y  0 , tất cả các lực,
mô men được biểu diễn trong mặt phẳng đứng, độ nghiêng bên bằng không, vận tốc
chuyển động thẳng của ASWs không đổi u  u0 , các thông số ban đầu 0  q0  0 .
 77

Khi đó   q , trong mặt phẳng đứng véc tơ vận tốc của ASWs được viết lại
  (u , 0, w, 0, q , 0)T [23].

Với các điều kiện trên, từ (3.1), (3.2) chuyển động của ASWs trong mặt
phẳng đứng khi có xét đến ảnh hưởng của dòng chảy đại dương được mô tả bởi
hệ phương trình:

 m  Z w  Z q 0 0 0  w   Z|w|w | wtd |  Zuwutd Z uq utd 0 0 0  wtd 

   
  M w I yy  M q 0 0 0  q   M uwutd M uqutd 0 0 0  q 
 0 0 1 0 0      0 1 0 0 0    
     
 0 0 0 1 0  x   0 0 0 0 0  x 
 0 0 0 0  
1  z  
   0 0 0 0 0  z 

 0   Zuu s utd2 
   
 z f B sin   x f B cos    M uu s utd 
2

 0    0  s (3.18)
   
 utd cos   wtd sin    0 
 u sin   w cos    
 td td   0 
Từ phương trình chuyển động trong mặt phẳng đứng (3.18), phương trình
chuyển động theo góc chúc ngóc được viết lại:

  q

  q  ( I yy  M q ) [ M w w   M uwutd wtd  M uq utd q  zb B sin   xb B cos  ]
1
(3.19)

  ( I yy  M q )1 M uu s utd2  s
trong đó, utd  u0  uc là hằng số; w td  w vì dòng chảy đại dương được coi là hằng
số.
 x1  
Đặt biến trạng thái 
 x2  x1  

Đầu vào điều khiển là góc bẻ lái điều khiển theo góc chúc ngóc u   s
Đầu ra hệ thống y  x1 có được từ thiết bị dẫn đường quán tính theo tính toán
của thuật toán xác định tham số dẫn đường ở chương 2.
Phương trình (3.19) được viết lại dưới dạng hệ phương trình:
x  A0 k x  Bk [ f ( x )  g ( x )u  d ]
(3.20)
y  CkT x
 78

trong đó:
0 1  0 1
Aok    ; Bk    ; Ck   
0 0  1 0
f ( x )  ( I yy  M q ) 1[ M w w  M uwutd wtd  M uq utd q  zb B sin   xb B cos  ]

g ( x )  ( I yy  M q ) 1 M uu s utd2

Các thành phần wtd , q , q được giải từ phương trình (3.18)

3.1.2.2 Phương trình chuyển động trong mặt phẳng ngang
Tương tự xét trong mặt phẳng ngang với giả thiết vận tốc ASWs theo trục dọc
là không đổi u  u0 , góc hướng nhỏ, các thông số ban đầu  0  r0  0 . Khi này có thể
xem   r , vận tốc dài và vận tốc góc của ASWs trong hệ tọa độ gắn liền được biểu
diễn   (u, v, 0, 0, 0, r )T [23].

Với các điều kiện trên, từ (3.1), (3.2) chuyển động của ASWs trong mặt phẳng
ngang được mô tả bởi hệ phương trình sau:
 m  Yv Yr 0 0 0   v  Y|v|v | vtd | Yuv utd  mutd  Yur utd 0 0 0   vtd 
 N    
 v I zz  N r 0 0 0   r   N uv utd N ur utd 0 0 0   r 
 0 0 1 0 0      0 1 0 0 0    
     
 0 0 0 1 
0  x   0 0 0 0 0  x 
 0 0 0 0   
1  y    0 0 0 0 0   y 
  Yuu h utd 
2
 0
   
 0 2
  Nuu h utd 
 0    0 h (3.21)
   
utd cos  vtd sin   0 
utd sin  vtd cos   0 
   

Từ phương trình chuyển động ASWs trong mặt phẳng ngang (3.21), ta có
phương trình chuyển động theo góc hướng:
  r
   (3.22)
  r  ( I zz  N r ) [ N v v  N uvutd vtd  Nur utd r ]  ( I zz  N r ) Nuu h utd  h
1 1 2

trong đó, vtd  v vì dòng chảy đại dương được coi là hằng số.

 x1  
Đặt biến trạng thái 
 x2  x1  
 79

Đầu vào điều khiển là góc bẻ lái điều khiển theo góc hướng u   h
Đầu ra hệ thống y  x1 có được theo tính toán của thuật toán xác định tham số
dẫn đường ở chương 2.
Phương trình (3.22) được viết lại dưới dạng hệ phương trình (3.20):
trong đó,
f ( x )  ( I zz  N r ) 1[ N v v  N uv utd vtd  N ur utd r ]

g ( x )  ( I zz  N r ) 1 N uu h utd2

Các thành phần v, vtd , r được giải từ phương trình (3.21)

3.1.2.3 Phương trình chuyển động theo góc lắc

Từ phương trình (3.1), (3.2) dùng phương pháp hạn chế bậc tự do khi đó
phương trình chuyển động của ASWs theo góc lắc có thể được viết [23]:
  p
   (3.23)
  p  ( I xx  K p ) zb B sin   ( I xx  K p ) Kuul utd  l
1 1 2

 x1  
Đặt biến trạng thái 
 x2  x1  
Đầu vào điều khiển là góc bẻ lái điều khiển ổn định góc lắc u   l
Đầu ra hệ thống y  x1 có được theo tính toán của thuật toán xác định tham số
dẫn đường ở chương 2.
Phương trình (3.23) được viết lại dưới dạng hệ phương trình (3.20) , trong đó:
f ( x )  ( I xx  K p ) 1 zb B sin 

g ( x )  ( I xx  K p ) 1 K uul utd2

3.2 Nhận dạng mô hình vũ khí chống ngầm

Nhận dạng tham số (hay còn gọi là nhận dạng hệ thống) là bài toán liên quan
đến việc sử dụng dữ liệu đo thực tế của đối tượng điều khiển để lập mô hình của đối
tượng đó. Có nhiều phương pháp khác nhau để ước lượng tham số mô hình có thể
sử dụng trong nhận dạng mô hình hệ thống như: phương pháp ước lượng đệ quy,
 80

bình phương tối thiểu đệ quy, sai số dự đoán đệ quy… Luận án sử dụng phương
pháp bình phương tối thiểu đệ quy được phát triển từ phương pháp bình phương nhỏ
nhất với các tham số được ước lượng liên tục tại mỗi thời điểm, ưu điểm của
phương pháp này là không cần lưu trữ dữ liệu trong quá khứ nên khả năng cập nhật
thông số nhanh, khối lượng tính toán ít, kết quả nhận được vẫn đảm bảo yêu cầu.
Dựa vào phương trình chuyển động theo góc chúc ngóc (3.19) và theo góc
hướng (3.22) của ASWs, cấu trúc mô hình lựa chọn để nhận dạng là mô hình bậc 2.
Cho hệ thống có đầu vào là u (k ) , đầu ra là y (k ) . Với nhiễu mô hình d (k ) giả thiết
là nhiễu trắng (nhiễu Gauss), mô hình đơn giản nhưng không mất tính tổng quát
được chọn là mô hình ARX:
y (k )  a1 y (k  1)  a2 y (k  2)  b1u (k  1)  b2u (k  2)  d (k ) (3.24)
ký hiệu,

véc tơ tham số hệ thống (k )  (a1 a2 b1 b2 )T (3.25)

véc tơ hồi quy  (k )    y (k  1)  y (k  2) u (k  1) u (k  2) 

T
(3.26)

Với ký hiệu trên, (3.24) được viết lại dưới dạng y (k )   T (k )(k )  d (k ) (3.27)

Biểu thức (3.27) cho thấy có thể tính được đầu ra y (k ) nếu biết tham số hệ thống
(k ) , đầu vào, đầu ra trong quá khứ  (k ) và nhiễu d (k ) .

Trong biểu thức (3.27) nhiễu d (k ) không thể biết trước nên chỉ có thể dự báo
đầu ra của hệ thống khi biết đầu vào và đầu ra trong quá khứ  (k ) . Để nhấn mạnh

giá trị dự báo phụ thuộc vào tham số (k ) , ta viết bộ dự báo dưới dạng hồi quy
tuyến tính:
ˆ )   T ( k )
yˆ(k /  ˆ (k ) (3.28)
Cần xác định tham số ˆ (k ) sao cho giá trị dự báo yˆ (k / ˆ ) càng gần giá trị đo

y (k ), (k  1, N ) càng tốt.

Giả sử thu thập được N mẫu dữ liệu dùng để nhận dạng mô hình:

Z N  {u (1), y (1),..., u ( N ), y ( N )} (3.29)

 81

Bắt đầu

Đọc hệ số quên  và véc tơ hồi quy  (k )

Khởi tạo P(0) , ˆ (0)

k 1

ˆ (k  1)
 ( k )  y ( k )   T ( k ) k  k 1

P(k  1) (k )
L( k ) 
 (k )   T (k ) P(k  1) (k )

1  P(k  1) (k ) T (k ) P(k  1) 

P(k )   P(k  1)  
 (t )   (k )   T (k ) P(k  1) (k ) 

ˆ (k )  
 ˆ (k  1)  L(k ) (k )

SAI
Điều kiện dừng

ĐÚNG

Kết thúc

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
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 với k  1, N (hình 3.2) gồm các bước sau [30]:
Bước 1. Tính sai số đầu ra:
Sai số đầu ra của bước nhận dạng thứ k được tính theo phương trình:
ˆ (k  1)
 ( k )  y ( k )   T ( k ) (3.30)
 82

Bước 2. Tính ma trận tạm:

Bước này chỉ mang nhiều ý nghĩa về thủ pháp lập trình, ma trận tạm được tính
theo công thức:
P( k  1) ( k )
L(k )  (3.31)
 (k )   T (k ) P(k  1) (k )

Bước 3. Cập nhật ma trận hiệp phương sai:

Để tránh trường hợp phải nghịch đảo ma trận, sử dụng ma trận hiệp phương sai
P (k ) làm ma trận trung gian:

1  P(k  1) (k ) T (k ) P (k  1) 
P (k )   P (k  1)   (3.32)
 (t )   (k )   T (k ) P(k  1) (k ) 

Bước 4. Cập nhật các tham số mô hình:

Các tham số mô hình ở bước nhận dạng thứ k được cập nhật theo công thức:
ˆ (k )  
 ˆ (k  1)  L(k ) (k ) (3.33)
Khi sử dụng thuật toán trên để ước lượng tham số mô hình của ASWs, điều kiện
đầu thường được chọn P(0) là ma trận đơn vị; ˆ (0) có thể được khởi tạo ngẫu nhiên
quanh giá trị 0, hệ số quên  (k )   , k và được chọn trong khoảng 0.95-0.99 [30].

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

Theo lưu đồ giải thuật ước lượng tham số mô hình (hình 3.2), sau khi thỏa mãn
điều kiện dừng thì các tham số mô hình ước lượng được ̂ sẽ gần đúng với tham số
mô hình theo công thức (3.25). Từ các tham số mô hình ước lượng được có thể mô tả
chuyển động của ASWs dưới dạng hàm truyền rời rạc:
b1 z  b2
G( z)  (3.34)
z  a1 z  a2
2

Từ phương trình hàm truyền rời rạc với các tham số đã ước lượng được (3.34)
cho phép xây dựng bộ điều khiển theo mô hình này. Nếu quá trình nhận dạng cho
tham số mô hình chính xác với mô hình đối tượng thì có thể áp dụng các phương
pháp điều khiển hệ tuyến tính [9] để xây dựng bộ điều khiển. Thực tế mô hình nhận
dạng được không hoàn toàn đúng với mô hình thực, mặt khác các yếu tố ảnh hưởng
 83

của môi trường trong quá trình điều khiển không giống với điều kiện môi trường
trong quá trình nhận dạng. Vì vậy, bộ điều khiển kinh điển cho mô hình tuyến tính
nhận dạng được có thể không đáp ứng được khi áp dụng cho đối tượng thực. Việc
xây dựng bộ điều khiển dựa trên các công cụ và phương pháp điều khiển hiện đại là
cần thiết. Mặt khác, bộ điều khiển khi xây dựng cho mô hình nhận dạng được khi áp
dụng cho đối tượng thực cần phải hiệu chỉnh lại thông số bộ điều khiển cho phù
hợp. Quá trình hiệu chỉnh thông số bộ điều khiển được thực hiện trong giai đoạn thử
nghiệm của quá trình sản xuất hay cải tiến vũ khí chống ngầm. Trong luận án tác
giả sử dụng số liệu thực nghiệm của một loại ASWs để nhận dạng tham số và sử
dụng mô hình phi tuyến dựa trên mô tả vật lý (3.19) và (3.22) để hiệu chỉnh thông
số bộ điều khiển.
Phương trình hàm truyền rời rạc bậc 2 nhận dạng được (3.34) có thể biến đổi
đưa về dạng hàm truyền liên tục, tiếp tục đặt biến trạng thái để đưa về dạng hệ
phương trình trạng thái (3.20). Như vậy, một cách tổng quát phương trình động học
của ASWs trong các mặt phẳng chuyển động dạng (3.20) có thể viết lại dưới dạng
hệ phương trình MIMO vuông 3 đầu vào, 3 đầu ra, bậc 2:
x  A0 x  B[ F ( x )  G ( x )u  d ]
(3.35)
y  CT x

trong đó, đầu vào điều khiển là các góc bẻ lái điều khiển ASWs theo góc hướng,
góc chúc ngóc và góc lắc u  (u1 , u2 , u3 )T  ( h ,  s ,  l )T ; đầu ra hệ thống là các góc

hướng, góc chúc ngóc và góc lắc y  ( y1 , y2 , y3 )T  ( , ,  )T đo được từ thiết bị dẫn

đường quán tính theo tính toán của thuật toán xác định tham số dẫn đường ở chương
2; nhiễu ngoài được xét là dòng chảy đại dương d  (d1 , d 2 , d3 )T  (uc , vc , wc )T .
Véc tơ trạng thái:
x  ( y1 , y1 , y2 , y 2 , y3 , y3 )T  ( x11 , x12 , x21 , x22 , x31 , x32 )T

A0  diag[ A01 , A02 , A03 ]

B  diag[ B1 , B2 , B3 ]

C  diag[C1 , C2 , C3 ]
 84

0 1  0 1
Aok    ; Bk    ; Ck   
0 0  1 0
F ( x )  M 1 ( )C (  , )  M 1 ( ) g ( )  M 1 ( ) J T ( ) pl

G ( x )u  M1 ( ) J T ( ) bl

Viết lại các hàm F ( x ) và G ( x ) dưới dạng ma trận cho hệ thống có 3 đầu vào, 3
đầu ra, bậc 2:
F ( x )   f1 ( x ), f 2 ( x ), f3 ( x ) 
T

G ( x )   G1 ( x ), G2 ( x ), G3 ( x ) 

Gk ( x )   g k1 ( x ), g k 2 ( x ), g k 3 ( x ) 
T

f k ( x ) và g kj ( x ) ( k  1,3; j  1,3 ) là các hàm phi tuyến trơn.

Luận án sẽ trình bày thuật toán điều khiển cho ASWs có 3 đầu vào, 3 đầu ra có
mô tả tổng quát như phương trình (3.35) với k  1,3 được dùng trong toàn bộ mục
3.3. Trong trường hợp điều khiển đơn kênh theo hàm truyền nhận dạng được (3.34)
thì k  1 .
Trước khi ASWs rời khỏi máy bay, thông tin về tọa độ điểm tiếp cận quỹ đạo
mong muốn và thông tin về tọa độ, góc định hướng tại điểm giao nhau giữa quỹ đạo
mong muốn với quả cầu cho phép thiết kế trước đã biết. Thông tin về tọa độ của
ASWs được xác định liên tục từ thiết bị dẫn đường quán tính. Từ các thông tin trên,
khối tạo đầu ra mong muốn (hình 3.3) sẽ tính toán các góc mong muốn
yin  ( yin1 , yin2 , yin3 )T  ( r , r ,  r )T sao cho khi ASWs chuyển động theo các góc mong

muốn này thì quỹ đạo của nó sẽ bám theo quỹ đạo hiệu chỉnh.
Vấn đề đặt ra là thiết kế bộ điều khiển sao cho các đầu ra của hệ thống bám theo
tín hiệu mong muốn ( r ,r ,  r ) và có khả năng thích nghi với sự ảnh hưởng của
dòng chảy đại dương trong điều kiện chỉ có đầu ra đo được.
Giả thiết 1: Ma trận G ( x ) xác định dương thỏa mãn điều kiện [5]:
0   min   n ( x )   l ( x )   max   (3.36)
trong đó,  n ( x ) và  l ( x ) tương ứng là giá trị riêng nhỏ nhất và lớn nhất của G ( x )

Vì M  ( )  M T ( )  0 [39] và theo cách đặt như (3.35) thì giả thiết trên là hợp lý.
 85

Đặt:
Yin  ( yin1 , yin1 , yin2 , yin2 , yin3 , yin3 )T ; yin  ( 
yin1 ,  yin3 )T
yin2 , 

Sai số bám e  Yin  x ; eˆ  Yin  xˆ . Với ê và x̂ là ước lượng của e và x

e  (e1 , e1 , e2 , e2 , e3 , e3 )T  (e11 , e12 , e21 , e22 , e31 , e32 )T ; E1  (e1 , e2 , e3 )T  ( E11 , E21 , E31 )T

Nếu các hàm F ( x ) và G ( x ) đã biết chắc chắn và không có nhiễu ngoài d thì
theo tiêu chuẩn Lyapunov và dựa trên tiếp cận tương đương chắc chắn, luật điều
khiển là [41]:
u *  G 1 ( x )[  F ( x )  yin( r )  K cT e ] (3.37)
trong đó, K c  diag[ K c1 , K c 2 , K c 3 ] với K ckT  ( K ck1 , K ck 2 ) là véc tơ độ lợi hồi tiếp được
chọn sao cho A0 k  Bk K ckT thỏa mãn định lý Hurwitz.
Tuy nhiên trong thực tế do thông tin không đầy đủ nên ta không thể có được mô
hình toàn chính xác của đối tượng điều khiển, do đó luật điều khiển lý tưởng (3.37)
không thể thực hiện được. Mặt khác bởi vì hệ thống (3.35) giả thiết chỉ có đầu ra
y  ( ,  ,  )T là đo lường được vì vậy phải xây dựng bộ quan sát trạng thái để ước

lượng các trạng thái của hệ thống.

Tuy nhiên, thuật toán đề xuất không ước lượng trực tiếp véc tơ trạng thái mà
ước lượng véc tơ sai số trạng thái ê [41]:
eˆ  A0 eˆ  BK cT eˆ  K 0 ( E1  Eˆ1 )
(3.38)
Eˆ  C T eˆ
1

trong đó,
K 0  diag[ K 01 , K 02 , K 03 ] là véc tơ độ lợi bộ quan sát trạng thái;

K 0 k  ( K 0 k 1 , K 0 k 2 )T được chọn sao cho Aok  K 0 k CkT thỏa mãn định lý Hurwitz.

Sai số bộ quan sát được định nghĩa: e  e  eˆ và E1  E1  Eˆ1

Sơ đồ bộ điều khiển DAFNOC như hình 3.3 với luật điều khiển được đề nghị:
u  uf  v (3.39)
trong đó,
v  (v1 , v2 , v3 )T là thành phần để khử nhiễu ngoài và sai số mô hình;

u f  (u f 1 , u f 2 , u f 3 )T là thành phần xấp xỉ luật điều khiển (3.37).

 86

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

Ak11
k1
eˆk 1
...  k1
h
A k1
u fk

...
1
A  kh
k2
kh
eˆk 2

...
h
A k2

Lớp 1 Lớp 2 Lớp 3 Lớp 4

Hình 3.4: Cấu hình mạng nơron mờ Singleton

Để tính u fk bộ điều khiển DAFNOC sử dụng cấu trúc của mạng nơron mờ

Singleton (hình 3.4) với đầu vào của mạng là các các sai số trạng thái ước lượng
theo các trạng thái tương ứng với các phương trình có đầu vào, đầu ra thứ k :
 87

eˆk  (eˆk1 , eˆk 2 )T . Cấu hình cơ bản của mạng nơron mờ Singleton gồm có h luật Nếu -

Thì và cơ chế suy luận mờ. Luật Nếu – Thì thứ i (với i  1, h ) được viết [4]:
Ri : Nếu eˆk1 là Aki 1 và eˆk 2 là Aki 2 thì u fk là Bki

trong đó, Aki 1 , Aki 2 và Bki là các tập mờ

Mạng nơron mờ Singleton có bốn lớp:
Tại lớp 1: Nút là các đầu vào, đại diện véc tơ ngôn ngữ vào eˆk  (eˆk1 , eˆk 2 )T ;
Tại lớp 2: Các nút thể hiện giá trị hàm thành viên của tổng số các biến ngôn ngữ.
2
 A    A (eˆkj )
i i (3.40)
k kj
j 1

Hàm liên thuộc của biến mờ  Ai (eˆkj ) được chọn dạng hàm Gauss với độ rộng
kj

và tâm hàm Gauss được chọn cố định, điều này cho phép giảm thời gian tính toán
của mạng đảm bảo được tính thời gian thực của hệ thống điều khiển.
Tại lớp 3: Các nút là giá trị của véc tơ cơ sở mờ:
Véc tơ cơ sở mờ k ( eˆk )  (k1 , k 2 ,..., kh )T  R h

trong đó, ki là giá trị của đầu ra mỗi nút ở lớp 3:
2


j 1
Akji
( eˆkj )
 ki ( eˆk )  h 2
(với i  1  h ) (3.41)
 [   A i ( eˆkj )]
i 1 j 1
kj

Tại lớp 4: Đầu ra đại diện giá trị đầu ra của mạng u fk . Giữa lớp 3 và lớp 4 được nối

đầy đủ bởi véc tơ thông số  k  ( k1 , k 2 ,..., kh )T  R h , các thông số này có thể điều

chỉnh được.  ki là điểm vạch mà tại đó  Bki ( ki )  1.

Sử dụng luật suy diễn max – prod, mờ hóa singleton và giải mờ theo trung bình
trọng tâm, đầu ra của mạng nơron mờ có thể biểu diễn như sau:
h 2

  [ 
i 1
ki
j 1
Akji
( eˆkj )]
u fk  h 2
  kT  k ( eˆk ) (3.42)
 [   A i ( eˆkj )]
i 1 j 1
kj
 88

Như vậy, để thực hiện luật điều khiển DAFNOC cho vũ khí chống ngầm cần
phải xây dựng 3 mạng nơron mờ với ngõ vào của mỗi mạng là véc tơ sai số trạng
thái ước lượng tương ứng. Điểm khác giữa thuật toán này so với hệ mờ là số trọng
số kết nối ngõ ra của mạng ít hơn trọng số kết nối mệnh đề kết luận của hệ mờ nên
khối lượng tính toán ít hơn.
Để xác định luật cập nhật các thông số của mạng  k  ( k1 , k 2 ,..., kh )T cần thực
hiện một số phép biến đổi sau :
Thay (3.39) và x  Yin  e vào (3.35), khi đó phương trình (3.35) được viết lại:

Yin  e  A0 (Yin  e )  B  F ( x )  G ( x )u f  G ( x ) v  d 
(3.43)
yin  E1  C T (Yin  e )

e  A0 e  B   F ( x )  G ( x )u f  G ( x ) v  d   Yin  A0Yin

E1  C T e

e  A0 e  BKcT eˆ  B   F ( x )  yin  KcT eˆ  G( x )u f  G( x ) v  d   Yin  A0Yin  Byin

 (3.44)
E1  C T e

e  A0 e  BK cT eˆ  B   F ( x )  yin  K cT eˆ  G ( x )u f  G ( x ) v  d 
 (3.45)
E1  C T e

Thay (3.37) vào (3.45) ta có:

e  A0 e  BK cT eˆ  B G ( x )u *  G ( x )u f  G ( x ) v  d 
 (3.46)
E1  C T e

Thay e  e  eˆ vào (3.46) khi đó:

e  eˆ  A0 ( e  eˆ )  BK cT eˆ  B G ( x )u *  G ( x )u f  G ( x ) v  d 

(3.47)
E1  Eˆ1  C T ( e  eˆ )

e  eˆ  A0 eˆ  BKcT eˆ  K 0C T e  ( A0  K 0C T ) e  B G ( x )u *  G( x )u f  G( x ) v  d 

 (3.48)
E1  C T e

Thay (3.38) vào (3.48) ta có:

e  ( A0  K 0C T ) e  B[G ( x )u *  G ( x )u f  G ( x ) v  d ]
 (3.49)
E1  C T e
 89

Thay u f  u f ( eˆ /  ) vào (3.49) viết lại cho thành phần thứ k ( k  1,3 ):
ek  ( A0 k  K 0 k CkT ) ek  Bk  g kk uk*  g kk u fk ( eˆk /  k )  g kk vk  d k 
(3.50)
E k 1  CkT ek

[ sI  ( A0 k  K 0 k CkT )]ek  Bk  g kk uk*  g kk u fk ( eˆk /  k )  g kk vk  d k 

 (3.51)
E k 1  CkT ek

E k1  CkT [sI  ( A0k  K0k CkT )]1 Bk  gkk uk*  gkk u fk ( eˆk / k )  gkk vk  dk 
 (3.52)
 H k (s)  gkk uk*  gkk u fk ( eˆk / k )  gkk vk  dk 

Với H k ( s )  CkT [ sI  ( A0 k  K 0 k CkT )]1 Bk

Giả thiết 2: Cho ek và eˆk thuộc tập U ek , U eˆk thỏa mản điều kiện [41]:

U ek  {ek  R rk :|| ek || mek  }

U eˆk  {eˆk  R rk :|| eˆk || meˆk  }

trong đó, eˆk là ước lượng của ek và me , meˆ là các thông số thiết kế.
k k

Véc tơ tham số  k* được cập nhật trực tuyến để tiến tới giá trị tiệm cận tối ưu:
 k*  arg min  M  [sup e Ue , eˆ Ueˆ | u k*  u k ( eˆk /  k ) |]
k k k k k k

trong đó, M   { k  R h :||  k || m } với m là hằng số.

k k k

Theo giả thiết 2, phương trình (3.50) có thể được viết lại:
ek  ( A0 k  K 0 k CkT ) ek  Bk  g kk u fk ( eˆk /  k* )  g kk u fk ( eˆk /  k )  g kk vk  wk  d k 
(3.53)
E k 1  CkT ek

trong đó, wk  g kk uk*  g kk u fk ( eˆk /  k* ) là sai số xấp xỉ

Khi đó (3.53) có thể viết lại như sau:
ek  ( A0 k  K 0 k CkT ) ek  Bk  g kk  u fk ( eˆk /  k* )  u fk ( eˆk /  k )   g kk vk  wk  d k 
(3.54)
E k 1  CkT ek

ek  ( A0 k  K 0 k CkT ) ek  Bk  g kk ( k*T   kT )k ( eˆk )  g kk vk  wk  d k 

 (3.55)
E k 1  CkT ek

ek  ( A0 k  K 0 k CkT ) ek  Bk  g kkkT  k ( eˆk )  g kk vk  wk  d k 

 (3.56)
E k 1  CkT ek
 90

trong đó, Tj   *j T   Tj .

Dùng phương pháp thiết kế thực dương chặt Lyapunov để phân tích tính ổn
định của (3.56) và đưa ra luật cập nhật thích nghi trực tiếp cho  k . Phương trình
(3.56) có thể được viết lại:
1
E k 1  CkT  sI  ( A0 k  K 0 k CkT )  Bk  g kkkT k ( eˆk )  g kk vk  wk  d k 
(3.57)
 H k ( s )  g kkkT  k ( eˆk )  g kk vk  wk  d k 

 E k 1  H k ( s ) Lk ( s )[kT k ( eˆk )  v fk  w fk ] (3.58)

trong đó,
v fk  Lk1 ( s ) g kk vk ;
w fk  Lk1 ( s )  g kkkT k ( eˆk )  wk  d k   kT k ( eˆk ) ;
k ( eˆk )  Lk1 ( s)k ( eˆk ) .

Lk (s ) được chọn sao cho Lk1 ( s ) là hàm truyền chắc chắn ổn định và H k ( s ) Lk ( s ) là

hàm truyền thực dương chặt thích hợp.

Giả sử rằng Lk ( s)  s  b1 sao cho H k ( s) Lk ( s) là hàm truyền thực dương chặt
thích hợp. Theo cách đặt như (3.58) khi đó (3.56) có thể viết:
ek  ( A0 k  K 0 k CkT ) ek  Bk Lk ( s ) kT k ( eˆk )  v fk  w fk 
(3.59)
E k 1  CkT ek

ek  Ack ek  Bck kT k ( eˆk )  v fk  w fk 

 (3.60)
E k 1  CkT ek

trong đó,
Ack  ( A0 k  K 0 k CkT ) ;

BckT  (0 b1 )

Giả thiết 3: w fk được giả sử thỏa mản điều kiện [41]:

| w fk |  k (3.61)

trong đó,  k là các hằng số dương.

Các tham số kết nối của mạng được cập nhật theo luật thích nghi [41]:
 91

  k E k 1k ( eˆk ) neáu ||  k || mk hay (||  k || m k vaø E k 1 kT k ( eˆk )  0)
k   (3.62)
Pr(   ( eˆ ))
E neá u ||  || m vaø E  T  ( eˆ )  0
 k k 1 k k k  k k 1 k k k

Với  k  0 là thông số thích nghi thiết kế. Thông số m thỏa mãn giả thiết 2. k

E k 1 kT k ( eˆk )
Pr( k E k 1k ( eˆk ))   k E k1k ( eˆk )   k k (3.63)
||  k ||2

Khi đó ||  k || m và || k || 2m

k k

Thành phần để khử nhiễu ngoài và sai số mô hình vk được cho bởi [41]:

 k neáu E k1  0 vaø | E k1 |  k

vk     k neáu E k1  0 vaø | E |  k1 k (3.64)
  E /  neáu | E k 1 |  k
 k k1 k
trong đó,  k   k /  min ;  k là hằng số dương.
Định lý và chứng minh tính ổn định của thuật toán đề nghị được trình bày trong
phụ lục B.
Mạng nơron mờ Singleton được sử dụng trong bộ điều khiển DAFNOC thực
chất là mờ hóa mạng truyền thẳng, chỉ có các tham số ngõ ra của mạng được cập
nhật theo luật điều khiển thích nghi (3.62), (3.63) với các tham số cần cập nhật ít
hơn hệ mờ có cùng số luật mờ, không huấn luyện mạng, cấu trúc của mạng là cố
định và không cập nhật các tham số hàm liên thuộc của biến mờ. Vì vậy khối lượng
tính toán của mạng nơron mờ Singleton ít, đảm bảo được tính thời gian thực trong
điều khiển.

3.4 Dẫn đường cho vũ khí chống ngầm hiệu chỉnh quỹ đạo sau khi chạm nước
3.4.1 Phương pháp dẫn đường cho vũ khí chống ngầm trong mặt phẳng ngang
Quỹ đạo tham chiếu của ASWs là đường thẳng từ điểm bắt đầu điều khiển
(điểm sau khi ASWs chạm nước lặn đến độ sâu 20m) đến điểm tiếp xúc với quả cầu
cho phép và quỹ đạo cong từ điểm này về điểm giao nhau giữa bề mặt quả cầu cho
phép với quỹ đạo mong muốn.
Quỹ đạo hiệu chỉnh chiếu lên mặt phẳng ngang như hình 3.5 [23]. Quỹ đạo
mong muốn chiếu lên mặt phẳng ngang là vòng tròn có bán kính RASWs . Rn là bán
 92

kính vòng tròn lớn nhất của quả cầu cho phép. Trên hình 3.5 điểm O có tọa độ
( x0 , y0 ) là điểm ASWs rời khỏi máy bay, tọa độ này được xem là điểm gốc của hệ

tọa độ địa lý; Điểm P có tọa độ ( x1 , y1 ) là điểm bắt đầu điều khiển hiệu chỉnh quỹ
đạo của ASWs, tọa độ này được xác định từ thiết bị dẫn đường quán tính; Điểm T
có tọa độ ( x2 , y2 ) là điểm tiếp cận với quỹ đạo mong muốn, tọa độ này đã được xác
định trước khi thả ASWs và được cập nhật cho ASWs tại thời điểm rời khỏi máy
bay; Điểm C có tọa độ ( x3 , y3 ) là điểm giao nhau giữa vòng tròn cho phép với quỹ
đạo mong muốn, tọa độ và góc định hướng mong muốn của ASWs tại điểm này
cũng được xác định trước khi thả ASWs và được cập nhật cho ASWs tại thời điểm
rời khỏi máy bay; ( x, y ) là tọa độ tức thời của ASWs.

X 0 (N ) dòng chảy
RASWs

Rn
 qd
q
 qn T
C  qn
Ln Xb
 ASWs
n
n
 LOS
( x, y )

O Y0

Hình 3.5: Quỹ đạo hiệu chỉnh trong mặt phẳng ngang
Góc hướng quỹ đạo từ điểm bắt đầu điều khiển P đến điểm tiếp cận quỹ đạo
mong muốn T được tính toán:
 y2  y1 
 LOS  arctan   (3.65)
 x2  x1 
 93

Góc hướng từ vị trí tức thời ASWs tới điểm tiếp cận quỹ đạo mong muốn T
được tính toán theo định nghĩa về góc hướng:
 y2  y 
 ASWs  arctan   (3.66)
 x2  x 
Sai lệch góc hướng LOS và ASWs được tính:
 n (t )   LOS  ASWs với 1800   n  1800 (3.67)
Khoảng cách từ vị trí tức thời của ASWs đến điểm tiếp cận quỹ đạo mong muốn
T trong mặt phẳng ngang:

Ln  ( x2  x ) 2  ( y2  y ) 2 (3.68)
Sai số quỹ đạo khi này được tính:  n (t )  Ln (t ) sin( n (t )) . (3.69)
Quá trình điều khiển ASWs theo góc hướng ở giai đoạn này sao cho góc hướng
 ASWs   LOS , tức là sai số  n  0 .

Để quỹ đạo của ASWs luôn bám theo quỹ đạo LOS thì góc hướng đi mong
muốn được đề xuất:
  n (t ) 
 r   ASWs  arctan    K In   n (t )dt (3.70)
 n 
trong đó,  n là thông số được chọn phụ thuộc vào bán kính lượn vòng của ASWs
trong mặt phẳng ngang.
Khi ASWs đi vào trong quả cầu cho phép thì ASWs bắt đầu quay theo hướng
trong mặt phẳng ngang đi về góc hướng quỹ đạo  qd tại điểm C đã biết trước.

Góc hướng từ vị trí tức thời ASWs ở giai đoạn chuyển động quay tới điểm giao
nhau C giữa quỹ đạo mong muốn và bề mặt quả cầu cho phép được tính toán theo
định nghĩa về góc hướng:
 y3  y 
 q  arctan   (3.71)
 x3  x 
Sai lệch góc hướng quỹ đạo tại điểm C và góc hướng quay ở vị trí tức thời
trong giai đoạn quay được tính:
 qn (t )   qd  q với 900   qn  900 (3.72)
 94

Khoảng cách từ vị trí tức thời của ASWs đến điểm C trong mặt phẳng ngang:

Lqn  ( x3  x) 2  ( y3  y ) 2 (3.73)
Sai số quỹ đạo khi này được tính:  qn (t )  Lqn (t ) sin( qn (t )) (3.74)

Quá trình điều khiển ASWs theo góc hướng ở giai đoạn này sao cho góc hướng
 q   qd , tức là sai số  qn  0 . Để giải quyết vấn đề trên thì hướng đi mong muốn

giai đoạn này được đề xuất:

  qn (t ) 
 r   q  arctan    K In   qn (t ) dt (3.75)
 n 
3.4.2 Phương pháp dẫn đường cho vũ khí chống ngầm trong mặt phẳng đứng
Tương tự, quỹ đạo hiệu chỉnh chiếu lên mặt phẳng đứng với z1 , z2 , z3 là tọa độ
theo trục OZ 0 của các điểm P, T , C tương ứng như hình 3.6.

O
X 0 (N )

P LOS
d
ASWs
 qd
d Rn
Ld T
C qd

Z0 X b  qd

Hình 3.6: Quỹ đạo hiệu chỉnh trong mặt phẳng đứng
Góc chúc ngóc quỹ đạo từ điểm bắt đầu điều khiển P đến điểm tiếp cận quỹ
đạo mong muốn T được tính toán:
 z2  z1 
LOS  arctan   (3.76)
 x2  x1 
Góc chúc ngóc từ vị trí tức thời của ASWs tới điểm tiếp cận quỹ đạo mong
muốn T được tính toán theo định nghĩa về góc chúc ngóc:
 z2  z 
ASWs  arctan   (3.77)
 x2  x 
Sai lệch góc chúc ngóc LOS và ASWs được tính:
 95

 d (t )  LOS  ASWs với 1800   d  1800 (3.78)

Khoảng cách từ vị trí tức thời của ASWs đến điểm tiếp cận quỹ đạo mong muốn
T trong mặt phẳng đứng:

Ld  ( x2  x ) 2  ( z2  z ) 2 (3.79)
Sai số quỹ đạo khi này được tính:  d (t )  Ld (t ) sin( d (t )) . (3.80)
Quá trình điều khiển ASWs theo góc chúc ngóc ở giai đoạn này sao cho góc
chúc ngóc ASWs  LOS , tức là sai số  d  0 . Để giải quyết vấn đề trên thì góc chúc
ngóc mong muốn được đề xuất:
  d (t ) 
r  ASWs  arctan    K Id   d (t )dt (3.81)
 d 
trong đó,  d là thông số được chọn phụ thuộc vào bán kính lượn vòng của ASWs
trong mặt phẳng đứng.
Khi ASWs đi vào trong quả cầu cho phép thì ASWs bắt đầu quay theo góc chúc
ngóc trong mặt phẳng đứng đi về góc chúc ngóc quỹ đạo qd tại điểm C đã biết trước.
Góc chúc ngóc từ vị trí tức thời của ASWs ở giai đoạn chuyển động quay tới điểm
giao nhau C giữa quỹ đạo mong muốn và bề mặt quả cầu cho phép được tính toán:
 z3  z 
q  arctan   (3.82)
 x3  x 
Sai lệch góc chúc ngóc quỹ đạo tại điểm C và góc chúc ngóc ở vị trí tức thời
trong giai đoạn quay được tính:  qd (t )  qd  q với 900   qd  900 (3.84)

Khoảng cách từ vị trí tức thời của ASWs đến điểm C trong mặt phẳng đứng:

Lqd  ( x3  x) 2  ( z3  z ) 2 (3.85)
Sai số quỹ đạo khi này được tính:  qd (t )  Lqd (t ) sin( qd (t )) (3.86)

Quá trình điều khiển ASWs theo góc chúc ngóc ở giai đoạn này sao cho góc
chúc ngóc q  qd , tức là sai số  qd  0 . Để giải quyết vấn đề trên thì góc chúc

ngóc mong muốn giai đoạn này được đề xuất:

  qd (t ) 
r  q  arctan    K Id   qd (t )dt (3.87)
 d 
 96

3.5 Kết luận chương 3

Các kết quả của chương 3 rút ra các kết luận sau:
1. Nhờ việc ổn định góc lắc ở giá trị 0 nên có thể phân tích chuyển động của vũ
khí chống ngầm thành hai chuyển động thành phần: Thành phần chuyển động
trong mặt phẳng đứng và thành phần chuyển động trong mặt phẳng ngang. Từ
đó ứng dụng phương pháp nhận dạng tham số mô hình và phương pháp điều
khiển hiện có để tổng hợp lệnh điều khiển cho vũ khí chống ngầm. Nội dung
này được công bố trong công trình [5] của tác giả.
2. Ứng dụng thuật toán nhận dạng theo phương pháp bình phương tối thiểu đệ quy
có thể ước lượng được tham số mô hình mô tả chuyển động theo hai mặt phẳng
cho vũ khí chống ngầm trên cơ sở thu thập dữ liệu đầu vào của các cơ cấu lái và
dữ liệu đầu ra về góc hướng và góc chúc ngóc do thiết bị dẫn đường quán tính
cung cấp. Tham số mô hình nhận dạng được là cơ sở quan trọng để tổng hợp
lệnh điều khiển cho các cơ cấu lái. Đây là đóng góp mới của luận án và được
công bố trong công trình [9] của tác giả.
3. Từ các thông tin về tham số định vị của vũ khí chống ngầm do thiết bị dẫn
đường quán tính cung cấp kết hợp với thông tin về tọa độ điểm tiếp cận quỹ đạo
mong muốn, bán kính quả cầu cho phép, tọa độ và góc định hướng theo quỹ đạo
mong muốn tại điểm giao nhau giữa quỹ đạo mong muốn và bề mặt quả cầu cho
phép đã biết trước có thể tính toán các góc hướng và góc chúc ngóc mong muốn
sao cho quỹ đạo của vũ khí chống ngầm sẽ đi theo quỹ đạo hiệu chỉnh.
4. Từ thông tin về tham số định hướng của vũ khí chống ngầm cho phép tổng hợp
các lệnh điều khiển cho các cơ cấu lái theo luật điều khiển hồi tiếp đầu ra nơron
mờ thích nghi trực tiếp. Thông tin về góc lắc thu được từ thiết bị dẫn đường
quán tính giúp cho việc bổ sung lượng điều khiển cho các cơ cấu lái để ổn định
góc lắc ở vị trí 0 nhằm duy trì tính độc lập của hai chuyển động ở hai mặt phẳng
đứng và mặt phẳng ngang. Đây là đóng góp mới của luận án và được công bố
trong công trình [11], [13], [14] của tác giả.
 97

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

Mô phỏng các thuật toán xác định tham số dẫn đường, thuật toán nhận dạng tham
số mô hình và thuật toán điều khiển hồi tiếp đầu ra nơron mờ thích nghi trực tiếp trên
phần mềm Matlab-Simulink với tham số đầu vào mô phỏng là tham số của một loại vũ
khí chống ngầm cụ thể có trong biên chế của lực lượng Hải quân Việt Nam.
4.1 Mô phỏng xác định tham số dẫn đường cho vũ khí chống ngầm
4.1.1 Xây dựng các hàm động học và hàm quan sát
Để tiến hành mô phỏng cần phải xây dựng các hàm động học và hàm quan sát.
Hàm quan sát được thiết lập trong điều kiện không bị ảnh hưởng bởi độ trôi và nhiễu
đo của các con quay tốc độ góc và gia tốc kế. Trình tự các bước thực hiện như sau:
Thiết lập các điều kiện ban đầu:
- Thiết lập giá trị ban đầu về từ trường trong hệ tọa độ địa lý Bx , By , Bz

- Thiết lập các giá trị tốc độ góc thực p, q, r không có nhiễu và độ trôi
- Thiết lập gia tốc cảm nhận thực a bx , a by , a bz không có nhiễu và độ trôi

- Khởi tạo góc Ơle ban đầu  (0), (0), (0)

- Thiết lập điều kiện ban đầu cho các tham số Rodrig-Hamilton
o (0), 1 (0), 2 (0), 3 (0) theo công thức (1.10) với các góc Ơle đã cho

- Thiết lập các thành phần vận tốc ban đầu trong hệ tọa độ địa lý
VN (0), VE (0), VD (0)

Xây dựng các hàm quan sát:

- Giải các phương trình vi phân (1.26) theo phương pháp số có thể xuất hiện sai
số tính toán liên quan đến không thỏa mãn đẳng thức   *  1 (hiện tượng trôi
định mức quaternion). Để tự động định mức lại số quaternion, theo tài liệu cơ
học chuyên ngành thay cho việc giải các phương trình vi phân (1.26) sẽ thực
hiện giải các phương trình vi phân [32]:
 98

20   p1  q2  r 3  0

 
21  p0  r 2  q3  1
  (4.1)
22  q0  r 1  p3  2
2  r   q  p  
 3 0 1 2 3

trong đó,   (02  12  22  32 )  1 là hệ số hiệu chuẩn.

Giải các phương trình vi phân (4.1) theo phương pháp Ơle để tính các tham số
Rodrig-Hamilton trong điều kiện con quay vi cơ cho giá trị đo không có nhiễu:
 T
0 (k  1)  0 (k )  2  p (k  1)1 (k )  q (k  1)2 (k )  r (k  1)3 (k )   (k )0 (k ) 

 (k  1)   (k )  T  ( p (k  1) (k )  r (k  1) (k )  q (k  1) (k )   (k ) (k ) 
 1 1
2
0 2 3 1
(4.2)

 (k  1)   (k )  T  (q (k  1) (k )  r (k  1) (k )  p (k  1) (k )   (k ) (k ) 
 2 2
2
0 1 3 2

 T
3 (k  1)  3 (k )   (r (k  1)0 (k )  q (k  1))1 (k )  p (k  1)2 (k )   (k )3 (k )
 2

trong đó, T là thời gian lấy mẫu.

- Xác định các hàm quan sát của từ kế theo các phương trình (2.28 – 2.30) với
các tham số Rodrig-Hamilton đã xác định trong điều kiện không có nhiễu.
- Xác định các hàm quan sát của gia tốc kế theo các phương trình (2.35 – 2.37)
với các tham số Rodrig-Hamilton đã xác định trong điều kiện không có nhiễu.
- Xác định các hàm quan sát của con quay vi cơ trên cơ sở giả thiết vận tốc góc
quay không bao gồm nhiễu đo theo các phương trình (2.38 – 2.40).
- Giải các phương trình vi phân (1.22) theo phương pháp Ơle với các các tham số
Rodrig-Hamilton được tính ở trên và gia tốc cảm nhận đo được không có nhiễu
đo để xác định các thành phần vận tốc trong hệ tọa độ địa lý. Giải phương trình
(2.74) để xác định các thành phần vận tốc trong hệ tọa độ gắn liền từ các thành
phần vận tốc trong hệ tọa độ địa lý và các các tham số Rodrig-Hamilton được
tính ở trên chưa xét nhiễu.
- Xác định hàm quan sát theo độ sâu dựa vào phương trình (2.78) trong điều kiện
gia tốc kế và con quay vi cơ cho giá trị đo lý tưởng.
- Xác định các ma trận đo theo (2.41) cho trường hợp ASWs chuyển động trong
khí quyển và theo (2.79) cho trường hợp ASWs chuyển động trong nước.
 99

Xây dựng các hàm động học:

- Thiết lập độ trôi và nhiễu của con quay vi cơ và nhiễu đo của gia tốc kế: Độ trôi
của con quay vi cơ bp , bq , br được đặt tùy ý song phải thỏa mãn điều kiện
bp   p ; bq  q ; br  r . Nhiễu w1 , w2 , w3 , w4 , w5 , w6 là hàm nhiễu ngẫu nhiên
được lấy từ chương trình tạo hàm ngẫu nhiên của hệ ngôn ngữ mô phỏng với kỳ
vọng toán học bằng 0.
- Thiết lập các giá trị đo được của con quay tốc độ góc  p , q , r có nhiễu và trôi.
- Thiết lập các giá trị đo được của gia tốc kế ax , a y , az có nhiễu.
- Xác định ma trận chuyển vị theo công thức (2.21) cho trường hợp ASWs
chuyển động trong khí quyển và theo công thức (2.65) cho trường hợp ASWs
chuyển động trong nước.
- Xác định ma trận hệ số nhiễu theo công thức (2.26) cho trường hợp ASWs
chuyển động trong khí quyển và theo công thức (2.73) cho trường hợp ASWs
chuyển động trong nước.
4.1.2 Thực hiện bộ lọc Kalman
Để thực hiện thuật toán lọc Kalman cần phải xác định các giá trị ban đầu:
- Thiết lập điều kiện ban đầu cho véc tơ trạng thái ước lượng theo điều kiện đầu
các thành phần đã thiết lập ở bước xây dựng hàm động học và hàm quan sát.
trường hợp ASWs chuyển động trong khí quyển:
X 0   o (0), 1 (0), 2 (0), p (0), q (0), r (0), bp , bq , br   R10
T

trường hợp ASWs chuyển động trong nước:

X 0   o (0), 1 (0), 2 (0), 3 (0), bp , bq , br ,VN (0),VE (0),VD (0), z(0)   R11
T

- Thiết lập điều kiện ban đầu ma trận hiệp phương sai của véc tơ sai số đánh giá
trạng thái P0 , ma trận này thường được chọn P0  O10 x10 (ma trận có các phần tử
bằng 0) cho trường hợp ASWs chuyển động trong khí quyển và P0  O11x11 cho
trường hợp ASWs chuyển động trong nước.
- Thiết lập ma trận trận hiệp phương sai nhiễu động học Q . Ma trận Q thường là
ma trận đường chéo với các giá trị trên đường chéo là phương sai của nhiễu
động học đã thiết lập ở bước xây dựng hàm động học.
- Thiết lập ma trận trận hiệp phương sai nhiễu đo R .
Thực hiện các bước theo trình tự của bộ lọc Kalman như lưu đồ giải thuật ở
hình 1.15.
 100

4.1.3 Kết quả mô phỏng

4.1.3.1 Giai đoạn vũ khí chống ngầm chuyển động trong khí quyển
0 Giá trị thực 1 Giá trị ước lượng
1.05 0.5

0.4
1
0.3
0.95
0.2
0.9
0.1
t(s) t(s)
0.85 0
0 10 20 30 40 50 0 10 20 30 40 50
2 3
0.2 0.3
t(s) 0.2
0
0.1
-0.2 t(s)
0
-0.4
-0.1

-0.6 -0.2
0 10 20 30 40 50 0 10 20 30 40 50

Hình 4.1: Các tham số Rodrig-Hamilton khi ASWs chuyển động trong khí quyển
 (rad ) Giá trị thực Giá trị tính toán được
1
t(s)
0

-1
0 10 20 30 40 50
 (rad )
1
t(s)
0

-1
0 10 20 30 40 50
 (rad )
1
t(s)
0

-1
0 10 20 30 40 50

Hình 4.2: Các các góc Ơle khi ASWs chuyển động trong khí quyển
Mô phỏng bằng ngôn ngữ mô phỏng Matlab trong trường hợp áp dụng bộ lọc
Kalman phi tuyến mở rộng ước lượng trực tiếp các tham số Rodrig-Hamilton trên
cơ sở các thông tin quan sát do con quay vi cơ, gia tốc kế và từ kế cung cấp.
 101

Từ kết quả hình 4.1 cho thấy với giá trị khởi tạo của các tham số Rodrig-
Hamilton thực giả định khác với giá trị ban đầu của tham số ước lượng vẫn cho kết
quả ước lượng bám sát các tham số thực giả định.
Trên cơ sở các tham số Rodrig-Hamilton ước lượng được, tính toán các góc Ơle
theo công thức (1.5) cho kết quả như hình 4.2. Từ kết quả này cho thấy giá trị tính
toán được gần đúng với giá trị giả định và sai số không tăng theo thời gian.
Từ kết quả mô phỏng cho thấy có thể ứng dụng thuật toán đề xuất xác định
các tham số định hướng của vũ khí chống ngầm trong giai đoạn chuyển động
trong khí quyển.
4.1.3.2 Giai đoạn vũ khí chống ngầm chuyển động trong nước
Mô phỏng thuật toán xác định tham số dẫn đường cho ASWs trong trường hợp
kết hợp con quay tốc độ góc với từ kế, gia tốc kế, vận tốc kế và cảm biến áp suất.
Từ kết quả hình 4.3 cho thấy các tham số Rodrig-Hamilton ước lượng được
bám sát các tham số thực giả định.
Trên cơ sở các tham số Rodrig-Hamilton ước lượng được, tính toán các góc Ơle
theo công thức (1.5) cho kết quả như hình 4.3. Từ kết quả trên cho thấy giá trị tính
toán được gần đúng với giá trị giả định và sai số không tăng theo thời gian.

0 Giá trị thực 1 Giá trị ước lượng

1.2 0.6

1.1 0.4

1 0.2
t(s)
0.9 0
t(s)
0.8 -0.2
0 50 100 0 50 100
2 3
0.2 0.3
t(s)
0 0.2

0.1
-0.2 t(s)
0
-0.4
-0.1
-0.6 -0.2
0 50 100 0 50 100

Hình 4.3: Các tham số Rodrig-Hamilton khi ASWs chuyển động trong nước
 102

Việc ứng dụng bộ lọc Kalman phi tuyến mở rộng cho phép thiết lập thuật toán
xác định các tham số dẫn đường trong điều kiện con quay vi cơ đo tốc độ góc có độ
trôi, nhiễu, gia tốc kế có nhiễu và thông tin quan sát được của các từ kế, vận tốc kế,
cảm biến áp suất cũng có nhiễu. Từ kết quả mô phỏng cho thấy có thể ứng dụng
thuật toán đề xuất để xác định các tham số dẫn đường của vũ khí chống ngầm trong
giai đoạn chuyển động trong nước.
 (rad ) Giá trị thực Giá trị tính toán được
1
t(s)
0

-1
0 20 40 60 80 100
 (rad )
2
t(s)
0

-2
0 20 40 60 80 100
 (rad )
1
t(s)
0

-1
0 20 40 60 80 100

Hình 4.4: Các các góc Ơle khi ASWs chuyển động trong nước
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.
4.2.1 Trường hợp thả vũ khí chống ngầm từ máy bay phản lực
Để thực hiện mô phỏng cần tạo ra hai véc tơ là véc tơ gia tốc và véc tơ vận tốc.
Gia tốc được thiết lập có xét nhiễu đo có dạng ồn trắng (nhiễu Gauss). Giả sử các
  
góc Ơle đúng ban đầu có giá trị  (0)  ;  (0)  ;  (0)  , giá trị các góc Ơle
3 9 15
thay đổi chậm phụ thuộc vào tốc độ góc quay của trái đất và tốc độ chuyển động
của máy bay phản lực trong quá trình xác định ma trận Côsin định hướng giữa hệ
tọa độ đế và hệ tọa độ địa lý.
Kết quả mô phỏng như hình 4.5 cho trường hợp thả ASWs từ máy bay phản lực
cho thấy các tham số Rodrig – Hamilton ước lượng phù hợp với tham số giả định.
Từ kết quả mô phỏng trên cho thấy có thể áp dụng bộ lọc Kalman phi tuyến mở
 103

rộng để ước lượng các tham số Rodrig – Hamilton, từ đó cho phép xác định ma trận
Côsin định hướng giữa hệ tọa độ đế và hệ tọa độ địa lý trong trường hợp thả ASWs
từ máy bay phản lực.
Giá trị thực Giá trị ước lượng
0 1 x 10 Sai số 0
-7

0.8392 0.1756 1

0.839 0.1752 -1

t(s) t(s) t(s)

0.8388 0.1748 -3
0 25 50 0 25 50 0 25 50
2 3 x 10
-7
Sai số 2
0.507 0.0982 6

0.506 0.0979 2

t(s) t(s) t(s)

0.505 0.0976 -2
0 25 50 0 25 50 0 25 50

Hình 4. 5: Các giá trị 0 , 1 , 2 , 3 đúng và ước lượng

4.2.2 Trường hợp thả vũ khí chống ngầm từ máy bay lên thẳng

cij đúng cij tính toán

c11 c12 c13
0.48 0.015 0.89

0.47 0.01 0.88

0.46 0.005 0.87

0 1 2 0 1 2 0 1 2
c21 c22 c23
0.344 0.94 -0.19

0.342 0.92 -0.195

0.34 0.9 -0.2

0 1 2 0 1 2 0 1 2
c31 c32 c33
-0.78 0.396 0.44

-0.8 0.394 0.42

-0.82 0.392 0.4

0 1 2 0 1 2 0 1 2

Hình 4.6: Các giá trị cij đúng và tính toán được
 104

Quá trình thu thập dữ liệu trong khoảng thời gian 30s với giả thiết các góc Ơle
  
đúng giữa hệ tọa độ đế và hệ tọa độ dẫn đường có giá trị   ; ; , các
3 9 15
góc này cố định trong thời gian mô phỏng thu thập dữ liệu. Trong mỗi khoảng thời
gian 10s khác nhau gia tốc được giả thiết là khác nhau phù hợp với điều kiện đã
trình bày ở chương 2. Sau khi thu thập dữ liệu thực hiện tính toán các chỉ số cij

(i, j  1,3) của ma trận Côsin định hướng giữa hệ tọa độ đế và hệ tọa độ dẫn đường.

Từ kết quả (hình 4.6) cho thấy tính đúng đắn của thuật toán đề xuất.
4.3 Mô phỏng nhận dạng tham số mô hình vũ khí chống ngầm
Dữ liệu đầu vào để thực hiện mô phỏng là các thông số của một chủng loại tên
lửa chống ngầm (phụ lục A) có trong biên chế của lực lượng Hải quân hiện nay.
4.3.1 Nhận dạng tham số mô hình vũ khí chống ngầm theo góc chúc ngóc
Mô hình đối tượng sử dụng mô phỏng để thu thập dữ liệu là phương trình
chuyển động trong mặt phẳng đứng (3.18). Mô phỏng thu thập 2000 mẫu dữ liệu
với đầu vào là góc bẻ lái bánh lái điều khiển theo góc chúc ngóc  s cho trước sao
cho đầu ra hệ thống là góc chúc ngóc  thay đổi trong giới hạn cho phép của tên
lửa chống ngầm.
Từ (3.19) cho thấy phương trình chuyển động theo góc chúc ngóc là phương
trình vi phân bậc hai, vì thế mô tả tương quan giữa tín hiệu vào – ra theo cấu trúc
mô hình ARX được viết:
 (k )  a1 (k  1)  a2 (k  2)  b1 s (k  1)  b2 s (k  2) (4.3)

hay có thể viết  (k )   T (k )(k ) , trong đó:

 (t )    (k  1)  (k  2)  s (t  1)  s (t  2)  là véc tơ hồi quy;

T

   a1 a2 b1 b2  là tham số mô hình cần ước lượng.

T

Bộ dự báo dưới dạng hồi quy tuyến tính trong trường hợp này được viết:
ˆ (k / 
ˆ )   T ( k )
ˆ (k ) (4.4)
Để dùng thuật toán đệ quy ước lượng tham số hệ thống cần thiết lập điều kiện đầu:
- Ma trận tạm P(0)  I 44 ;
 105

- Tham số mô hình ước lượng ˆ (0)  041 (ma trận có các phần tử bằng 0)
- Hệ số quên được chọn cố định   0.988 .

 (rad ) Kết quả nhận dạng 

1

0.5
Số mẫu
0

-0.5

-1 Vartheta thuc
Vartheta nhan dang
-1.5
0 400 800 1200 1600 2000
 (rad ) Sai số nhận dạng 
0.1

0.05
Số mẫu
0

-0.05

-0.1
0 400 800 1200 1600 2000

Hình 4.7: Kết quả nhận dạng và sai số nhận dạng theo góc chúc ngóc
̂
1.5

0.5
Số mẫu
0

-0.5

-1 a1
a2
-1.5
b1
-2 b2

-2.5
0 400 800 1200 1600 2000

Hình 4.8: Giá trị các tham số mô hình ̂ theo góc chúc ngóc ước lượng được
Thực hiện thuật toán đệ quy ước lượng tham số theo các bước từ (3.30) đến
(3.33) (hình 3.2), khi tham số ước lượng càng gần đúng với tham số thực thì sai số
giữa đầu ra thực tế và đầu ra ước lượng  (k ) càng bé, khi đó theo (3.33) thì
ˆ (k )  
 ˆ (k  1) tức là kết quả ước lượng hội tụ. Thuật toán ước lượng tham số ngoài
 106

việc cho kết quả nhận dạng với độ chính xác cao còn phải đảm bảo tính hội tụ nhanh.
Trên hình 4.8 cho thấy các tham số mô hình ước lượng được ̂ hội tụ sau 800 mẫu
dữ liệu nhận dạng. Kết quả nhận dạng và sai số nhận dạng theo góc chúc ngóc như
hình 4.7, thời điểm bắt đầu nhận dạng có sai số lớn do các tham số ước lượng khởi
tạo chưa đúng với tham số hệ thống, điều này phù hợp với thực tế. Sai số nhận dạng
sau khi hội tụ nhỏ.
Từ kết quả hình 4.7 và hình 4.8 cho thấy sử dụng thuật toán đề xuất cho kết quả
ước lượng hội tụ nhanh và nhận được tham số mô hình ước lượng:
ˆ   1.776 0.7898 0.005678 0.0144 T


Theo (4.3) tham số mô hình của tên lửa chống ngầm theo góc chúc ngóc nhận
0.005678 z  0.0144
dạng được viết dưới dạng hàm truyền rời rạc là: G ( z )  (4.5)
z 2  1.776 z  0.7898
Mô hình (4.5) được sử dụng để xây dựng bộ điều khiển cho ASWs theo góc
chúc ngóc.
4.3.2 Nhận dạng tham số mô hình vũ khí chống ngầm theo góc hướng
Mô hình đối tượng sử dụng mô phỏng để thu thập dữ liệu là phương trình
chuyển động trong mặt phẳng ngang (3.21) với thông số của một chủng loại tên lửa
chống ngầm. Mô phỏng thu thập 2000 mẫu dữ liệu với đầu vào là góc bẻ lái bánh
lái hướng  h được thiết lập sao cho đầu ra là góc hướng  thay đổi trong giới hạn
làm việc cho phép của tên lửa chống ngầm. Từ (3.22) cho thấy phương trình chuyển
động theo góc hướng là phương trình vi phân bậc hai, vì thế mô tả tương quan giữa
tín hiệu vào – ra theo cấu trúc mô hình ARX được viết:
 (k )  c1 (k  1)  c2 (k  2)  d1 h (k  1)  d 2 h (k  2) (4.6)

hay có thể viết  (k )   T (k )(k ) , trong đó:

 (k )    (k  1)  (k  2)  h (k  1)  h (k  2)  là véc tơ hồi quy;

T

   c1 c2 d 2  là tham số mô hình cần ước lượng.

T
d1

Bộ dự báo dưới dạng hồi quy tuyến tính trong trường hợp này được viết:
ˆ )   T ( k )
ˆ (k /  ˆ (4.7)
 107

Để dùng thuật toán đệ quy ước lượng tham số hệ thống cần thiết lập điều kiện đầu:
Ma trận tạm ban đầu P(0)  I 44 ;

Tham số mô hình ước lượng ˆ (0) được khởi tạo ngẫu nhiên quanh giá trị 0;
Hệ số quên được chọn   0.98 .
Thực hiện thuật toán đệ quy ước lượng tham số theo các bước từ (3.30) đến
(3.33) (hình 3.2). Trên hình 4.10 cho thấy các tham số mô hình ước lượng được ̂
hội tụ sau 400 mẫu dữ liệu nhận dạng. Kết quả nhận dạng và sai số nhận dạng theo
góc hướng như hình 4.9. Thời điểm bắt đầu nhận dạng cho sai số lớn do các tham số
ước lượng khởi tạo chưa đúng với tham số hệ thống, điều này phù hợp với thực tế.
Sai số nhận dạng sau khi hội tụ nhỏ.

 (rad ) Kết quả nhận dạng 

10

Số mẫu
0

-10
Psi
Psi nhan dang
-20
0 400 800 1200 1600 2000
 (rad ) Sai số nhận dạng 
0.4
0.2
Số mẫu
0
-0.2
-0.4
0 400 800 1200 1600 2000

Hình 4.9: Kết quả nhận dạng và sai số nhận dạng theo góc hướng

Từ kết quả hình 4.9 và hình 4.10 cho thấy sử dụng thuật toán đề xuất cho kết
quả ước lượng hội tụ nhanh. Tham số mô hình ước lượng là:
ˆ   1.98 0.9802 0.3152 0.004359 T


Theo (4.6) tham số mô hình của tên lửa chống ngầm theo góc hướng nhận dạng
0.3152 z  0.004359
được viết dưới dạng hàm truyền rời rạc là: G ( z )  (4.8)
z 2  1.98 z  0.9802
 108

Mô hình (4.8) được sử dụng để xây dựng bộ điều khiển cho ASWs theo
góc hướng.

̂
1.5

0.5
Số mẫu
0

-0.5 c1
c2
-1
d1
-1.5 d2

-2

-2.5
0 200 400 600 800 1000 1200 1400 1600 1800 2000

Hình 4.10: Giá trị các tham số mô hình ̂ theo góc hướng ước lượng được
Qua kết quả mô phỏng ta thấy tốc độ hội tụ tham số mô hình nhanh (hình 4.8;
4.10), sai số nhận dạng theo góc chúc ngóc (hình 4.7) và theo góc hướng (hình 4.9)
khi đã hội tụ là rất nhỏ. Điều đó chứng tỏ các tham số nhận được từ lớp mô hình
trên thích hợp với bài toán chuyển động của ASWs trong các mặt phẳng riêng biệt.
Để chính xác hơn nữa, cần phải mô phỏng, tính toán tổng hợp trên cả 2 mặt phẳng
với các giả thiết tối ưu.
Phương trình chuyển động trong mặt phẳng ngang và mặt phẳng đứng của
ASWs có hai bánh lái điều khiển theo góc chúc ngóc và hai bánh lái điều khiển theo
góc hướng kết hợp điều khiển giảm lắc dựa trên các tính chất vật lý của đối
tượng và môi trường nước đối tượng hoạt động có nhiều thông số đối tượng
không thể tính toán chính xác, sai số mô hình lớn dẫn đến bài toán điều khiển mô
phỏng dựa trên mô hình này khi áp dụng thực tế có thể đáp ứng không tốt. Vì
thế, luận án đã xây dựng thuật toán bình phương tối thiểu đệ quy nhận dạng mô
hình tham số phương tiện ngầm.
Trên cơ sở phương trình chuyển động trong các mặt phẳng đã xây dựng, tiến
hành mô phỏng theo thông số của một chủng loại tên lửa chống ngầm (phụ lục
A) thả từ máy bay để thu thập tập mẫu dữ liệu kiểm chứng thuật toán nhận dạng.
 109

Từ kết quả nhận dạng cho thấy thuật toán đề xuất có thể ứng dụng tốt để nhận
dạng mô hình tham số của ASWs.
4.4 Mô phỏng đ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
Quỹ đạo mong muốn là đường thẳng từ điểm bắt đầu điều khiển ASWs giả thiết
có tọa độ ( x1 , y1 , z1 )  (100,100, 20) với góc định hướng ( 1 ,1 ,  1 )  (450 , 180 ,30 )
đến điểm tiếp cận quỹ đạo mong muốn (T ) có tọa độ ( x2 , y2 , z2 )  (1100,1100,120) .
Khi ASWs đi đến cách điểm T một khoảng bằng bán kính quả cầu cho phép
Rn  50m thì bắt đầu quay theo góc hướng và góc chúc ngóc về điểm giao nhau giữa
quỹ đạo mong muốn với bề mặt quả cầu cho phép có tọa độ
( x3 , y3 , z3 )  (1050,1150,140) với góc trạng thái mong muốn tại điểm này
( qd ,qd ,  qd )  (1350 , 180 ,00 ) . Nhiễu ngoài là ảnh hưởng của dòng chảy đại dương

giả sử được xét tại thời điểm 20s: (uc , vc , wc )T  (4, 4, 0.5)T (m/s).
4.4.1 Mô phỏng điều khiển vũ khí chống ngầm theo góc hướng
Trong mặt phẳng ngang tín hiệu điều khiển là góc bẻ lái u   h gồm hai bánh lái
 h1 ,  h 2 được giả thiết là quay theo cặp nghĩa là  h1   h 2 (vì trong trường hợp này
chưa xét đến quá trình điều khiển theo góc lắc). Mô phỏng hệ thống điều khiển theo
tham số mô hình nhận dạng được (4.8) và hiệu chỉnh thông số bộ điều theo phương
trình chuyển động (3.22) của một chủng loại tên lửa chống ngầm (phụ lục A) với
đầu ra hệ thống là góc hướng được tính toán từ hệ thống dẫn đường (hình 4.11).

Hình 4.11: Sơ đồ mô phỏng hệ thống điều khiển ASWs theo góc hướng
 110

Thông số bộ điều khiển theo góc hướng với luật điều khiển DAFNOC được chọn:
- Độ lợi hồi tiếp và bộ quan sát: K cT1  (12,16); K 01T  (60,120).
- Các thông số của thành phần điều khiển khử sai số mô hình: 1  0.4;1  0.0025.
- Hệ số của luật thích nghi:  1  1
- Hàm liên thuộc của biến mờ  Ai (eˆ1 j ) dạng hàm Gauss là:
1j


   
2
 Ai (eˆ1 j )  exp  0.25 eˆ1 j  c1 j 1 
1j
 
trong đó, tâm và độ rộng hàm Gauss là:
c1  (0.5 0.25 0 0.25 0.5) ; 1  5 .

1
- Hàm truyền ổn định (bộ lọc): L1 
s 1
- Thông số kết nối đầu ra của mạng Singleton 1 được khởi tạo ngẫu nhiên quanh
giá trị 0.

0 400 800 1200  (độ)

150
quỹ đạo thực tế X  mong muốn
phương đường thẳng tới  thực tế
100  quỹ đạo
P điểm tiếp cận quỹ đạo
mong muốn T 50
400
46.4
thời điểm có nhiễu t(s)
0
44.8
-50 19.5 24
0 25 50
 h (độ)
800 30

t(s)
0
T
C
1200 -30
Y 0 25 50

Hình 4.12: Kết quả mô phỏng điều khiển ASWs theo góc hướng

Kết quả mô phỏng cho thấy khi có ảnh hưởng của dòng chảy đại dương, các
thông số của bộ điều khiển được cập nhật trực tuyến nên góc quay bánh lái sẽ thay
đổi đảm bảo đáp ứng hệ thống ít bị tác động và nhanh chống bám theo quỹ đạo
 111

mong muốn (hình 4.12). Quỹ đạo của ASWs tại thời điểm bắt đầu mô phỏng có sai
lệch do góc hướng ban đầu không trùng với phương của quỹ đạo hiệu chỉnh. Tuy
nhiên do hệ thống thích nghi nhanh nên sau một thời gian ngắn quỹ đạo của ASWs
bám theo quỹ đạo mong muốn.
Khi ASWs đi vào trong vòng tròn có bán kính bằng bán kính của quả cầu cho
phép thì bắt đầu quay theo góc hướng về góc hướng mong muốn  qd và hướng tới
điểm giao nhau giữa quỹ đạo mong muốn và bề mặt quả cầu cho phép C chiếu lên
mặt phẳng ngang. Điều kiện dừng mô phỏng là khi ASWs đã đi về góc hướng mong
muốn  qd , từ kết quả hình 4.12 cho thấy khi ASWs đạt điều kiện về góc hướng thì
vị trí còn một lượng sai lệch nhất định, theo tính toán mô phỏng thì khoảng sai lệch
theo phương ngang là 5m. Vì ASWs có đầu tự dẫn làm việc trong giới hạn 1500m
nên sai lệch này có thể chấp nhận được.
4.4.2 Mô phỏng điều khiển vũ khí chống ngầm theo góc chúc ngóc

Hình 4.13: Sơ đồ mô phỏng hệ thống điều khiển ASWs theo góc chúc ngóc
Điều khiển theo góc chúc ngóc với tín hiệu điều khiển là góc bẻ lái u   s
gồm hai bánh lái  s1 ,  s 2 được giả thiết là quay theo cặp, nghĩa là  s1   s 2 . Mô
phỏng hệ thống điều khiển theo tham số mô hình nhận dạng được (4.5) và hiệu
chỉnh thông số bộ điều theo phương trình chuyển động (3.19) của một chủng loại
tên lửa chống ngầm (phụ lục A) với đầu ra hệ thống là góc chúc ngóc được tính
toán từ hệ thống dẫn đường (hình 4.13).
Thông số bộ điều khiển theo góc chúc ngóc với luật điều khiển DAFNOC
được chọn:
 112

- Độ lợi hồi tiếp và bộ quan sát: K cT2  (120, 20); K 02T  (820, 990).
- Các thông số của thành phần điều khiển khử sai số mô hình:  2  20;  2  0.005.
- Hệ số của luật thích nghi:  2  9000
- Hàm liên thuộc của biến mờ  Ai (eˆ2 j ) dạng hàm Gauss là:
1j


   
2
 Ai (eˆ2 j )  exp  0.25 eˆ2 j  c2 j  2 
2j
 
trong đó, tâm và độ rộng hàm Gauss là:
c2  (1 0.5 0 0.5 1) ;  2  25 .

1
- Hàm truyền ổn định (bộ lọc): L2 
s 1
- Thông số kết nối đầu ra của mạng Singleton  2 được khởi tạo ngẫu nhiên
quanh giá trị 0.
400 800 1200
0
X
P Quỹ đạo thực
50
Phương đường thẳng
LOS

100
T

C
150
Z
 (độ) t(s)  s (độ)
0 40
Thời điểm xét nhiễu
-5 20
t(s)
-10 0
 mong muốn
-15  thực -20  s1
 quỹ đạo  s2
-20 -40
0 10 20 30 0 10 20 30

Hình 4.14: Kết quả mô phỏng điều khiển vũ khí chống ngầm theo góc chúc ngóc
Ứng dụng bộ điều khiển DAFNOC mô phỏng điều khiển ASWs theo góc chúc
ngóc cho kết quả như hình 4.14. Từ kết quả mô phỏng cho thấy, tại thời điểm bắt
đầu mô phỏng, góc chúc ngóc ASWs không trùng với góc chúc ngóc mong muốn
 113

nên có sai số quỹ đạo, tuy nhiên quỹ đạo của hệ thống nhanh chóng bám theo quỹ
đạo mong muốn. Hệ thống có khả năng thích nghi tốt trong điều kiện có ảnh hưởng
của dòng chảy đại dương.
Trong giai đoạn chuyển động quay vì góc chúc ngóc thay đổi trong giới hạn nhỏ
để đi về góc chúc ngóc theo quỹ đạo mong muốn qd  18 (độ) nên thời gian đáp
ứng nhanh và sai lệch vị trí nhỏ.
4.4.3 Mô phỏng điều khiển vũ khí chống ngầm theo góc lắc
Thông số bộ điều khiển theo góc lắc với luật điều khiển DAFNOC được chọn:
- Độ lợi hồi tiếp và bộ quan sát: K cT3  (360, 60); K 03T  (25, 350).
- Các thông số của thành phần điều khiển khử sai số mô hình: 3  0.1;  3  0.1.
- Hệ số của luật thích nghi:  3  30
- Hàm liên thuộc của biến mờ  Ai (eˆ3 j ) dạng hàm Gauss là:
3j


   
2
 Ai (eˆ3 j )  exp  0.25 eˆ3 j  c3 j  3 
3j
 
trong đó, tâm và độ rộng hàm Gauss là:
c3  (10 5 0 5 10) ;  3  2 .
1
- Hàm truyền ổn định (bộ lọc): L3 
s4
- Thông số kết nối đầu ra của mạng Singleton 3 được khởi tạo ngẫu nhiên quanh
giá trị 0.

Hình 4.15: Sơ đồ mô phỏng hệ thống điều khiển ASWs theo góc lắc
Tín hiệu điều khiển theo góc lắc là góc bẻ lái u   l gồm hai bánh lái hướng
 h1 ,  h 2 được giả thiết là quay theo cặp nghĩa là  h1   h 2 (khi này không xét đến
 114

quá trình điều khiển theo góc hướng). Sơ đồ mô phỏng theo phương trình động học
(3.23) với tham số mô phỏng của một chủng loại tên lửa chống ngầm (phụ lục A)
với đầu ra hệ thống là góc lắc được tính toán từ hệ thống dẫn đường như hình 4.15.
Từ kết quả điều khiển như hình 4.16 cho thấy đáp ứng của hệ thống theo góc
lắc nhanh, sau khoảng thời gian 1.5s tên lửa đã ổn định được góc lắc.

 (độ)
3
3
 mong muốn
2  thực tế
1
-0.5 t(s)
0 1.5
0

-1
0 10 20 30 40 50
 h (độ)
5
4  h1
h2 t(s)
0
-4
0 0.2
-5
0 10 20 30 40 50

Hình 4.16: Kết quả mô phỏng điều khiển vũ khí chống ngầm theo góc lắc
Các trường hợp mô phỏng điều khiển theo từng kênh riêng biệt ở trên đã đơn
giản hóa bài toán chuyển động tổng hợp, vì thế đã loại bỏ một số thông số của hệ
thống, giảm đặc tính phi tuyến của hệ thống và chưa xét đến sự ảnh hưởng, tác động
qua lại giữa các kênh điều khiển. Để có thể ứng dụng thực tế cần phải xây dựng bộ
điều khiển tổng hợp đồng thời theo các kênh đã xét ở trên.

4.4.4 Mô phỏng đ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 tổng hợp theo góc hướng, góc chúc ngóc và góc lắc
Mô hình đối tượng được mô phỏng theo phương trình chuyển động tổng hợp
(3.13). Vấn đề đặt ra là điều khiển ASWs đi về điểm giao nhau giữa bề mặt quả cầu
cho phép với quỹ đạo mong muốn và có các góc định hướng trùng với góc định
hướng mong muốn của ASWs tại điểm tiếp cận này ngay khi ASWs lặn theo lực
trọng lực đến độ sau 20m sau khi chạm nước. Sơ đồ mô phỏng như hình 4.17. Các
bánh lái quay theo tín hiệu điều khiển theo góc hướng và góc lắc  h1   l   h ,
 h 2   l   h như công thức (3.16) và (3.17).
 115

Hình 4.17: Sơ đồ mô phỏng điều khiển ASWs theo góc chúc ngóc, góc hướng
và góc lắc
 (độ)  (độ)
0 5
t(s) t(s)
-10 0
-20
 mong muốn  mong muốn
 DAFNOC -5  DAFNOC
-30  quỹ đạo  PD
 PD -10
0 10 20 30 40 50
-40 0
0 10 20 30 40 50 500 500
 (độ) 1500 1000 1000
1500
150
 mong muốn Y
 DAFNOC 50 Thuc
X
100
 quỹ đạo LOS
 PD P
100 T
50
46 C
t(s)
0
150 Z
42
19 28
-50
0 10 20 30 40 50

Hình 4.18: Kết quả mô phỏng điều khiển ASWs theo góc hướng, góc chúc ngóc và
góc lắc
Thông số bộ điều khiển MIMO dựa trên các thông số theo từng kênh điều khiển
SISO (theo góc hướng, góc chúc ngóc và góc lắc) đã thiết kế, điều này thể hiện tính
 116

kế thừa của việc đơn giản hóa hệ thống theo từng mặt phẳng để thiết kế bộ điều
khiển tổng hợp. Kết quả mô phỏng như hình 4.18.
Khi thực hiện bộ điều khiển tổng hợp ASWs (bộ điều khiển MIMO) đồng thời
theo cả góc hướng, góc chúc ngóc và góc lắc dựa trên các thông số của từng kênh
điều khiển (các bộ điều khiển SISO) đã thiết kế hệ thống vẫn có thể thích nghi tốt
trước các tác động qua lại giữa các kênh điều khiển tại thời điểm bắt đầu mô phỏng
(P) và khi quay đồng thời theo hướng và góc ngóc về điểm giao nhau (C) giữa quỹ
đạo mong muốn với bề mặt quả cầu cho phép. Hệ thống có khả năng thích nghi tốt
đối với ảnh hưởng của dòng chảy đại dương xét ở thời điểm 20s. So sánh với điều
khiển PD cho thấy tại thời điểm có ảnh hưởng bởi dòng chảy đại dương kết quả
điều khiển theo góc hướng sử dụng bộ điều khiển DAFNOC ít bị tác động và nhanh
chóng bám theo quỹ đạo mong muốn hơn kết quả sử dụng bộ điều khiển PD.
4.5 Kết luận chương 4
Từ các kết quả mô phỏng cho phép rút ra các kết luận sau:
1. Qua kết quả mô phỏng cho thấy thuật toán ứng dụng bộ lọc Kalman kết hợp các
thông tin của các phương tiện đo không quán tính và quán tính cho ước lượng
các tham số Rodrig-Hamilton 0 , 1 , 2 , 3 phù hợp với tham số thật giả định
trong điều kiện con quay vi cơ có độ trôi và nhiễu đo, các phương tiện đo đều
có nhiễu đo.
2. Kết quả mô phỏng thủ tục xác định ma trận Côsin định hướng giữa hệ tọa độ đế
và hệ tọa độ địa lý cho thiết bị dẫn đường quán tính có đế theo phương pháp
phối hợp véc tơ vận tốc trong điều kiện có xét nhiễu đo của gia tốc kế cho thấy
kết quả tính toán theo thuật toán đề nghị phù hợp với giá trị giả định cho cả hai
trường hợp thả vũ khí chống ngầm từ máy bay phản lực và máy bay lên thẳng.
3. Kết quả mô phỏng nhận dạng các tham số mô hình mô tả hai kênh chuyển động
của vũ khí chống ngầm theo thuật toán bình phương tối thiểu đệ quy cho thấy
thời gian hội tụ nhanh, sai số nhận dạng sau khi hội tụ nhỏ.
4. Ứng dụng bộ điều khiển hồi tiếp đầu ra nơron mờ thích nghi trực tiếp trong điều
khiển đơn kênh và điều khiển tổng hợp một chủng loại vụ khí chống ngầm cho
thấy đáp ứng hệ thống có thời gian xác lập nhanh, tính thích nghi nhanh và có
khả năng đáp ứng với biên độ nhiễu lớn. Hệ điều khiển nhiều đầu vào, nhiều
đầu ra dựa trên nguyên tắc tổng hợp các hệ đơn kênh nên khối lượng tính toán
và tham số cần cập nhật ít có thể cho phép ứng dụng điều khiển thực.
 117

KẾT LUẬN VÀ KIẾN NGHỊ

1. Kết luận
Từ các kết luận của chương trong luận án cho phép rút ra các kết luận sau:
1. Đưa thiết bị dẫn đường quán tính và một số thiết bị đo vào hệ thống điều
khiển của vũ khí chống ngầm thả từ máy bay lên thẳng hay máy bay phản lực cho
phép hiệu chỉnh quỹ đạo chuyển động đưa vũ khí chống ngầm về điểm tiếp cận với
quỹ đạo mong muốn trong điều kiện điểm chạm nước có sai số.
2. Bổ sung từ kế, vận tốc kế, cảm biến áp suất cho phép ứng dụng thiết bị dẫn
đường quán tính không đế để xác định các tham số dẫn đường cho vũ khí chống
ngầm khi rơi trong khí quyển và khi chuyển động trong môi trường nước. Nhờ việc
áp dụng thuật toán lọc Kalman phi tuyến mở rộng để xây dựng thuật toán kết hợp
thông tin của các phương tiện đo quán tính với phương tiện đo không quán tính (vận
tốc kế, từ kế, cảm biến áp suất) để xác định các tham số dẫn đường (tham số định vị
và tham số định hướng) trong điều kiện các con quay vi cơ có độ trôi và nhiễu đo,
các thiết bị đo có nhiễu.
3. Có thể kết hợp thông tin về vận tốc của máy bay với các thông tin của các
gia tốc kế đặt trên đế của thiết bị dẫn đường quán tính có đế để xác định ma trận
Côsin định hướng giữa hệ tọa độ đế và hệ tọa độ địa lý cho thiết bị dẫn đường quán
tính có đế.
4. Đối với vũ khí chống ngầm nhờ có thuật toán ổn định góc lắc nên có thể
phân chuyển động của nó thành hai chuyển động độc lập: chuyển động trong mặt
phẳng đứng và chuyển động trong mặt phẳng ngang.
Áp dụng phương pháp bình phương tối thiểu đệ quy có thể xây dựng thuật toán
nhận dạng tham số mô hình chuyển động của vũ khí chống ngầm theo hai mặt
phẳng: mặt phẳng đứng và mặt phẳng ngang.
Áp dụng thuật toán điều khiển hồi tiếp đầu ra nơron mờ thích nghi trực tiếp để tổng
hợp lệnh điều khiển cho vũ khí chống ngầm chuyển động đồng thời theo góc hướng,
góc chúc ngóc và ổn định góc lắc trong điều kiện đầu ra đo được bởi thiết bị dẫn
đường quán tính.
5. Các kết quả mô phỏng cho thấy tính đúng đắn của các thuật toán được đề
xuất: thuật toán lọc Kalman kết hợp con quay vi cơ với gia tốc kế, từ kế, vận tốc
kế và cảm biến áp suất xác định tham số định hướng và tham số định vị, thuật
 118

toán kết hợp thông tin của các gia tốc kế đặt trên đế của thiết bị dẫn đường quán
tính và vận tốc kế của máy bay để xác định ma trận Côsin định hướng giữa hệ tọa
độ đế và hệ tọa độ địa lý cho thiết bị dẫn đường quán tính có đế, thuật toán nhận
dạng tham số mô hình và thuật toán đ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.
6. Luận án đã thực hiện một chu trình hoàn chỉnh từ xây dựng mô hình toán
chuyển động, mô hình hóa, mô phỏng, đề xuất thuật toán nhận dạng tham số mô hình
và xây dựng thuật toán dẫn đường, thuật toán điều khiển cho vũ khí chống ngầm khi
rơi trong khí quyển và khi chuyển động về điểm tiếp cận quỹ đạo mong muốn.
2. Những đóng góp mới của luận án
- Áp dụng bộ lọc Kalman phi tuyến mở rộng và phương pháp phối hợp véc tơ
vận tốc xây dựng thuật toán xác định tham số dẫn đường cho vũ khí chống ngầm
trong giai đoạn chuyển động trong khí quyển và chuyển động dưới nước trên cơ sở
kết hợp thiết bị dẫn đường quán tính với từ kế, vận tốc kế và cảm biến áp suất, cụ
thể là:
+ Áp dụng lọc Kalman phi tuyến mở rộng xây dựng thuật toán kết hợp tham số đo
của con quay vi cơ có độ trôi và nhiễu với các tham số đo của gia tốc kế, từ kế có nhiễu
để xác định các tham số Rodrig-Hamilton 0 , 1 , 2 , 3 trong bài toán xác định tham số
dẫn đường cho vũ khí chống ngầm ở giai đoạn rơi có dù trong khí quyển. Mục đích
cuối cùng là xác định vị trí và tư thế của vũ khí chống ngầm khi nó chạm nước.
+ Áp dụng lọc Kalman phi tuyến mở rộng xây dựng thuật toán kết hợp tham số
đo của con quay vi cơ có độ trôi và nhiễu với các tham số đo của gia tốc kế, từ kế,
vận tốc kế và cảm biến áp suất có nhiễu để xác định các tham số Rodrig-Hamilton
0 , 1 , 2 , 3 trong bài toán xác định tham số dẫn đường cho vũ khí chống ngầm ở

giai đoạn chuyển động trong môi trường nước phục vụ cho việc điều khiển hiệu
chỉnh quỹ đạo cho vũ khí chống ngầm.
+ Áp dụng phương pháp phối hợp véc tơ vận tốc do các phương tiện đo vận tốc
trên máy bay cung cấp với thông tin từ gia tốc kế của thiết bị dẫn đường quán tính
có đế dạng giải tích xây dựng thuật toán xác định ma trận Côsin định hướng giữa hệ
 119

tọa độ đế và hệ tọa độ địa lý (hệ tọa độ dẫn đường) trong bài toán xác định tham số
dẫn đường cho vũ khí chống ngầm ở cả hai giai đoạn (chuyển động rơi có dù trong
khí quyển và chuyển động trong môi trường nước) phục vụ cho việc điều khiển hiệu
chỉnh quỹ đạo cho vũ khí chống ngầm.
- Đề xuất một giải pháp nhận dạng tham số mô hình và áp dụng bộ điều khiển
hồi tiếp đầu ra nơron mờ thích nghi trực tiếp có sử dụng thông tin từ thiết bị dẫn
đường quán tính cho điều khiển vũ khí chống ngầm, cụ thể là:
+ Đề xuất một giải pháp nhận dạng tham số mô hình của vũ khí chống ngầm áp
dụng phương pháp bình phương tối thiểu đệ quy có sử dụng thông tin từ thiết bị dẫn
đường quán tính.
+ Đề xuất áp dụng bộ điều khiển hồi tiếp đầu ra nơron mờ thích nghi trực tiếp
có sử dụng thông tin từ thiết bị dẫn đường quán tính cho việc điều khiển vũ khí
chống ngầm đồng thời theo góc hướng, góc chúc ngóc và ổn định góc lắc trong giai
đoạn điều khiển hiệu chỉnh quỹ đạo.
3. Kiến nghị
Kết quả trong luận án dừng lại ở mô phỏng. Đề nghị các cơ quan chức năng
của Bộ quốc phòng cho phép từng bước đưa kết quả của luận án vào các đề tài triển
khai ứng dụng khi cải tiến, hiện đại hóa vũ khí chống ngầm.
 120

DANH MỤC CÁC CÔNG TRÌNH ĐÃ CÔNG BỐ CỦA TÁC GIẢ

[1] Nguyễn Quang Vịnh, Lê Anh Tuấn, Trương Duy Trung, Nguyễn Hoàng, Đồng
Văn Tấn, Nguyễn Đức Thành, (2011), “Khảo sát một số tham số quan trọng
của khối đo lường quán tính IMU AHRS400”, Tạp chí Nghiên cứu KH&CN
Quân sự, Viện KH&CN Quân sự, (9), tr.213-217.
[2] Nguyễn Quang Vịnh, Trương Duy Trung, (2011), “Điều khiển hồi tiếp ngõ ra
nơron mờ thích nghi trực tiếp hệ cánh tay hai khớp nối”, Tạp chí Nghiên cứu
KH&CN Quân sự, Viện KH&CN Quân sự, (9), tr.93-101.
[3] Nguyễn Quang Vịnh, Trương Duy Trung, Phan Tương Lai, (2011), “Điều
khiển hồi tiếp ngõ ra nơron mờ thích nghi gián tiếp hệ cánh tay hai khớp
nối”, Hội nghị toàn quốc lần thứ nhất về Điều khiển và Tự động hóa,
tr.167-172.
[4] Nguyễn Quang Vịnh, Phan Tương Lai, Trương Duy Trung, Vũ Huy Trung,
(2012), “Bộ lọc cho hệ thống đo lường quán tính IMU AHRS400”, Tạp chí
Nghiên cứu KH&CN Quân sự, Viện KH&CN Quân sự, (21), tr.7-14.
[5] Trần Đức Thuận, Nguyễn Quang Vịnh, Trương Duy Trung, (2012), “Mô hình
động học chuyển động của ngư lôi”, Tạp chí Nghiên cứu KH&CN Quân sự,
Viện KH&CN Quân sự, (21), tr.28-35.
[6] Trần Đức Thuận, Bùi Hồng Huế, Trương Duy Trung, Trần Xuân Kiên, (2012),
“Ứng dụng 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 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ử Toàn quốc lần thứ 6, tr 488-494.
[7] Trương Duy Trung, Nguyễn Quang Vịnh, Nguyễn Quang Hùng, Trần Đức
Thuận, (2013), “Xây dựng thuật toán xác định tham số dẫn đường cho phương
tiện chuyển động trên cơ sở kết hợp con quay tốc độ góc với từ kế, gia tốc kế
và vận tốc kế”, Tạp chí Nghiên cứu KH&CN Quân sự, Viện KH&CN Quân sự,
(21), tr 3-12.
[8] Trần Đức Thuận, Trương Duy Trung, Nguyễn Quang Vịnh, Nguyễn Sĩ Long,
Trần Xuân Kiên, Bùi Hồng Huế, Nguyễn Văn Diên, (2013), “Xây dựng thuật
toán xác định tham số định hướng cho phương tiện chuyển động trên cơ sở kết
 121

hợp con quay tốc độ góc với từ kế và gia tốc kế”, Tạp chí Nghiên cứu KH&CN
Quân sự, Viện KH&CN Quân sự, (25), tr 7-16.
[9] Trần Đức Thuận, Nguyễn Quang Vịnh, Trương Duy Trung, (2013), “Mô hình
hóa và nhận dạng tên lửa chống ngầm”, Tạp chí Nghiên cứu KH&CN Quân sự,
Viện KH&CN Quân sự, (28), tr3-11.
[10] Trương Duy Trung, Trần Đức Thuận, Nguyễn Quang Vịnh, (2013), “Thuật
toán đặt điều kiện ban đầu cho cơ cấu đế của hệ thống dẫn đường quán tính
theo phương phương pháp phối hợp véc tơ vận tốc”, Hội nghị toàn quốc lần
thứ hai về Điều khiển và Tự động hóa, tr 269-275.
[11] Trương Duy Trung, Trần Đức Thuận, Nguyễn Quang Vịnh, (2013), “Điều
khiển hồi tiếp ngõ ra nơron mờ thích nghi trực tiếp cho một dạng phương tiện
ngầm tự hành”, Hội nghị toàn quốc lần thứ hai về Điều khiển và Tự động hóa,
tr 222-228.
[12] Trương Duy Trung, Trần Đức Thuận, Bùi Hồng Huế, Nguyễn Văn Diên,
(2013), “Xây dựng thuật toán xác định tham số dẫn đường cho phương tiện
chuyển động trên cơ sở kết hợp con quay tốc độ góc với từ kế, gia tốc kế và
định vị vệ tinh GPS”, Hội nghị toàn quốc lần thứ hai về Điều khiển và Tự
động hóa, tr 37-42.
[13] Phạm Văn Phúc, Đặng Xuân Kiên, Trương Duy Trung, (2013), “Thiết kế hệ
thống điều khiển ngư lôi sử dụng bộ điều khiển hồi tiếp ngõ ra nơron mờ thích
nghi trực tiếp”, Hội nghị toàn quốc lần thứ hai về Điều khiển và Tự động hóa,
tr 283-289.
[14] Truong Duy Trung, Tran Duc Thuan, Nguyen Quang Vinh, Nguyen Vinh Hao,
Truong Viet Chuong, (2013), “Guidance, navigation and control of
Autonomous underwater vehicles”, International Symposium on Electrical and
Electronics Engineering ISBN 978-604-73-2039-4, pp 44-49.
[15] Trương Duy Trung, Nguyễn Quang Vịnh, (2014), “Xây dựng thuật toán xác
định tham số dẫn đường cho vũ khí chống ngầm”, Tạp chí Nghiên cứu KH&CN
Quân sự, Viện KH&CN Quân sự, chấp nhận đăng số 31 tháng 6 năm 2014.
 122

TÀI LIỆU THAM KHẢO

Tiếng Việt
[1] Bùi Công Cường, Nguyễn Doãn Phước (2001) Hệ mờ mạng nơron & ứng dụng,
Nhà xuất bản khoa học kỹ thuật, Hà Nội.
[2] Phạm Hữu Đức Dục, (2009), Mạng nơron & Ứng dụng trong Điều khiển, Nhà
xuất bản Khoa học và Kỹ thuật – Hà Nội .
[3] Nguyễn Thị Phương Hà, Huỳnh Thái Hoàng (2003), Lý thuyết điều khiển tự
động, Nhà xuất bản ĐHQG Tp. Hồ Chí Minh
[4] Huỳnh Thái Hoàng, (2006), Hệ thống điều khiển thông minh, Nhà xuất bản
ĐHQG Tp. Hồ Chí Minh.
[5] Huỳnh Thái Hoàng (2005), Các thuật toán tối ưu bền vững để nhận dạng và
điều khiển thích nghi hệ thống động, Luận án Tiến sĩ Kỹ thuật, ĐHBK Tp.Hồ
Chí Minh.
[6] Phan Xuân Minh, Nguyễn Doãn Phước. (2002) Lý thuyết điều khiển mờ, Nhà
xuất bản khoa học kỹ thuật, Hà Nội.
[7] Nguyễn Thương Ngô (1998), Lý thuyết điều khiển tự động hiện đại, Nhà xuất
bản khoa học kỹ thuật, Hà Nội.
[8] Phạm Công Ngô (1998), Lý thuyết điều khiển tự động, Nhà xuất bản khoa học
kỹ thuật, Hà Nội.
[9] Nguyễn Thương Ngô (2000), Lý thuyết điều khiển tự động hệ tuyến tính, Nhà
xuất bản khoa học kỹ thuật, Hà Nội.
[10] Nguyễn Thương Ngô (2003), Lý thuyết điều khiển tự động thông thường và
hiện đại-hệ phi tuyến-hệ ngẫu nhiên, Nhà xuất bản khoa học kỹ thuật, Hà
Nội.
[11] Nguyễn Doãn Phước, Phan Xuân Minh (2001), Nhận dạng hệ thống điều
khiển, Nhà xuất bản khoa học kỹ thuật, Hà Nội.
[12] Nguyễn Doãn Phước, Phan Xuân Minh, Hán Thành Trung (2003), Lý thuyết
điều khiển phi tuyến, NXB Khoa học và kỹ thuật, Hà Nội.
[13] “Thuyết minh kỹ thuật Tên lửa AПР-2Э” (2003), Cục kỹ thuật, Quân chủng
Phòng Không – Không Quân.
 123

Tiếng Anh
[14] A. Forouzantabar,B. Gholami, M. Azadi, (2012), “Adaptive Neural Network
Control of Autonomous Underwater Vehicles”, World Academy of Science,
Engineering and Technology 67, pp 304-309.
[15] Alexander V. Inzartsev, (2008), Underwater Vehicles, ISBN 978-953-7619-49-
7, I-Tech, Vienna, Austria.
[16] A. Faruq, S. Abdullah, M. Shah, (2011), “Optimization of An Intelligent
Controller for An Unmanned Underwater Vehicle”, TELKOMNIKA, Vol.9,
No.2, pp. 245~256.
[17] B. Raeisy, A. Safavi, R. Khayatian, (2012), “Optimized Fuzzy Control Degign
of An Autonomous Underwater Vehicle”, Iranian Journal of Fuzzy Systems
Vol. 9, No. 2, pp 25-41.
[18] C. Vuilmet, (2006), “A MIMO Backstepping Control with Acceleration Feedback
for Torpedo”, The 38th Southeastern Symposium on System Theory Tennessee
Technological University Cookeville, TN, USA, pp 157-162.
[19] D. Ven, C. Flanagan, D. Toal, (2005), “Neural Networkcontrol of Underwater
Vehicles”, Engineering Applications of Artificial Intelligence 18, pp 533–547.
[20] D. H. Titterton, J. L. Weston, (2004), Strapdown Inertial Navigation Technology,
2nd Edition, The Institution of Electrical Engineers, ISBN 0863413587.
[21] D. H. Shin, S. T. Kwon, S. H. Park, M. G. Joo, (2013), “ Fuzzy State Feedback
Control for Way-Point Tracking of Autonomous Underwater Vehicle”,
International Journal of Control and Automation, Korea Vol. 6, No. 1, pp 119-130.
[22] Graham LeBlanc, (2011), Design and Simulation of A Control Continuum for
Tetherless Underwater Vehicles, Master thesis, Dalhousie University
Halifax, Nova Scotia.
[23] G .N. Robert, R. Sutton, (1996), “Advance in Unmanned Marine Vehicles”,
Control of Engineering Series 69, pp. 92-101.
[24] Hung Duc Nguyen, Riaan Pienaar, Dev Ranmuthugala, William West, (2011)
“Modelling, Simulation and Control of Underwater Vehicles”, The first
Vietnam Conference on Control and Automation, pp 150-159.
 124

[25] J.E.G Refsnes, (2007), Nonlinear Model-Based Control of Slender Body AUVs,
Thesis for the degree of philosophy doctor, Trondheim.
[26] J. Refsnes, A. J. Sørensen, K. Y. Pettersen, (2007), “ Output Feedback Control
of an AUV with Experimental Results”, Conference on Control &
Automation, Athens – Greece, pp 1-8.
[27] J. H. Li, P. M. Lee, B. H. Jun, (2004), “A Neural Network Adaptive Controller
for Autonomous Diving Control of An Autonomous Underwater Vehicle”,
International Journal of Control, Automation, and Systems, vol. 2, no. 3, pp.
374-383.
[28] K. M. Fauske, F. Gustafsson, Ø. Hegrenæs, (2007), “Estimation of AUV
Dynamics for Sensor Fusion”, The 10th International Conference on
Information Fusion, Canada, pp 9-12.
[29] Khac Duc Do, J. Pan, (2009), Control of Ships and Underwater Vehicles, e-ISBN
978-1-84882-730-1 Springer Dordrecht Heidelberg London New York.
[30] L. Ljung, (2007), System Identification Theory for The User, second edition,
Prentice Hall PRT Upper Saddle River, NJ 07458.
[31] M. Santhakumar, T. Asokan, (2009), “Non-linear Adaptive Control System for
an Underactuated Autonomous Underwater Vehicle using Dynamic State
Feedback”, International Journal of Recent Trends in Engineering, India,
Vol 2, No. 5, pp 380-384.
[32] Oleg S. Salychev, (1988), Inertial systems in navigation and geophysics,
Bauman MSTU Press Moscow.
[33] P. Kaniewski, G. S. Kaliski, (2005), “Integrated Positioning System for AUV”,
Molecular and Quantum Acoustics (26), pp 115-128.
[34] P .A. Ioannou, J. Sun, (1996), Robust Adaptive Control, Prentice-Hall,
Englewood Cliffs, NJ.
[35] Robert M. Rogers, (2003), Applied Mathematics in Integrated Navigation
Systems, American Institute of Aeronautics and Astronautics, Inc. 1801
Alexander Bell Drive, Reston, VA 20191-4344, Second Edition.
[36] S. Tong, T. Wang, J.T. Tang, (2005), “Fuzzy Adaptive Output Tracking
Control of Nonlinear System”, Fuzzy Sets and Systems 156, pp 285-299.
 125

[37] S.S. Sastry, M. Bodson, (1989), Adaptive Control: Stability, Convergence, and
Robustness, Prentice-Hall, Englewood Cliffs, NJ.
[38] T. S. TSAY, (2010), “Intelligent Guidance and Control Laws for An Autonomous
Underwater Vehicle”, Wseas transactions on systems, pp 463-475.
[39] T. I.Fossen, (1994), Guidance and Control of Ocean Vehicles, Chichester:
John Wiley & Sons.
[40] T. I. Fossen, (2002), Marine Control Systems, Marine Cybernetics, ISBN 82-
93256-00-2.
[41] W.Y.Wang, Y.G.Leu, T.T.Lee, (2002), “Output Feedback Control of
Nonlinear Systems Using Direct Adaptive Fuzzy – Neural Controller”,
Fuzzy Set and Systems, pp 341-358.
[42] X. Yun, E. R. Bachmann, (2006), “Design, Implementation, and Experimental
Results of a Quaternion-Based Kalman Filter for Human Body Motion
Tracking”, IEEE Transactions on Robotics, vol. 22, no. 6, pp 1216-1227.
[43] Y. J. Mon, C. M. Lin, (2012), “Supervisory Recurrent Fuzzy Neural Network
Guidance Law Design for Autonomous Underwater Vehicle”, International
Journal of Fuzzy Systems, Vol. 14, No. 1, pp 54-64.
[44] Z. Tang, Q. He, S. Wang, J. Shen, J. Luo, (2012), “Predictive Fuzzy PID
Control Method for Underwater Vehicles”, Journal of Computational
Information Systems 8, China, pp 3635-3642.
[45] Z. Qin, J. Gu, (2010) “Adaptive Control of Autonomous Underwater Vehicle
Base on The Fuzzy Neural Network”, Journal of Automation, Mobile
Robotics & Intelligent Systems, pp 104-111.

Tiếng Nga
[46] В.В. Матвеев, В.Я. Распопов (2009), Основы построения
бесплатформенных инерциальных навигационных систем, Москва.
[47] К. Т. Леондес, (1980), Фильтрация и стохастическое управление в
динамических системах, Мир, Москва.
 A1

PHỤ LỤC A

MỘT SỐ THAM SỐ CỦA MỘT CHỦNG LOẠI TÊN LỬA CHỐNG NGẦM

Tên lửa chống ngầm được thả từ máy bay tiêu diệt tàu ngầm, có các đặc
tính kỹ chiến thuật cơ bản sau:
- Chiều dài: 3700 mm.
- Trọng lượng: 575 kg.
- Đường kính thân: 350 mm.
- Khoảng cách từ trục bánh lái đến tâm trọng lực: 175mm.
- Độ sâu hành trình: tới 600m.
- Vùng hoạt động: theo hướng 00 – 3600; theo góc lắc: -500 đến 500
- Tốc độ: tới 115 km/h.
- Giản đồ định hướng của hệ thống an ten thu phát: theo mặt phẳng ngang là
70 0 , theo mặt phẳng đứng là 40 0 ; khoảng cách đến 1500m.
- Động cơ: động cơ tên lửa nhiên liệu rắn với trọng lượng nhiên liệu 165kg, thời
gian làm việc động cơ 55s, lực đẩy ở điều kiện tiêu chuẩn 6500-7500N.
- Máy lái: có 4 máy lái điện quay 4 bánh lái.
- Bánh lái: bốn bánh lái có kích thước bằng nhau 164 mm x 99 mm với góc giới
hạn lớn nhất 220  0.50 . Hai bánh lái điều khiển tên lửa theo độ sâu, hai bánh lái
điều khiển tên lửa vừa theo hướng vừa điều khiển giảm lắc.
- Khối các thiết bị điều khiển:
 Hệ thống truyền cảm về góc chúc ngóc: Hệ thống gồm một con quay ba bậc
tự do dùng để định hướng và tạo góc chúc ngóc 700 cho tên lửa khi chuyển
động trong không khí.
 Hệ thống truyền cảm tốc độ góc: Hệ thống gồm có ba con quay hai bậc tự do
để đo vận tốc góc của tên lửa theo các trục của hệ tọa độ gắn liền.
 Hệ thống truyền cảm theo độ sâu: Làm việc theo nguyên tắc áp lực nước biển
tác dụng lên bề mặt biến trở làm dịch chuyển con chạy của biến trở.
 A2

Một số tham số sử dụng để mô phỏng:

Mô tả Kí Giá trị Đơn vị

hiệu
Chiều dài thân L 3700 mm
Đường kính thân R 350 mm
Khối lượng m 575 kg
Vận tốc u 31.94 m/s
Khoảng cách trục bánh lái đến tâm nổi xbl 175 mm
Mật độ nước  1025 kg/m3
Diện tích bề mặt của bánh lái Sbl 0.016236 m2
Góc bẻ lái lớn nhất  max 22 độ
Hệ số lực khối nước kèm theo trục X u 0.4 kg
dọc của hệ tọa độ gắn liền
Hệ số lực cản theo trục X b X u|u| -3 kg/m
Hệ số lực cản theo trục X b X uv 0.1 kg/m
Hệ số lực cản theo trục X b X v |v | -52 kg/m
Hệ số lực cản theo trục X b X uw 30 kg/m
Hệ số lực cản theo trục X b X w|w| -52 kg/m
Hệ số lực khối nước kèm chéo trục X wq -27 kg/rad
Hệ số lực khối nước kèm chéo trục X rr 1.8 kg/rad
Hệ số lực khối nước kèm chéo trục X qq 0 kg/rad
Hệ số lực khối nước kèm chéo trục X vr 0 kg/rad
Lực đẩy động cơ X pl 7000 N
Hệ số lực của bánh lái X uul 13.48 kg/m.rad
Hệ số lực khối nước kèm Yv -676 kg
Hệ số lực khối nước kèm Yr -92 kg
Hệ số lực khối nước kèm chéo trục Ywp 270 kg/rad
 A3

Hệ số lực khối nước kèm chéo trục Ypq -100 kg/rad

Hệ số lực cản Yv|v| -20.27 kg/m
Hệ số khối nước kèm chéo trục, lực Yuv -220 kg/rad
nâng và lực cản
Hệ số khối nước kèm chéo trục, lực Yur -100 kg/rad
nâng
Các lực của bánh lái Yuu h 13.48 kg/m.rad
Hệ số lực khối nước kèm Z w -676 kg
Hệ số lực khối nước kèm Z q -92 kg
Hệ số lực cản Z w|w| 20.27 kg/m
Hệ số lực khối nước kèm chéo trục và Z uq -620 kg/rad
lực nâng
Hệ số lực khối nước kèm chéo trục Z vp -270 kg/rad
Hệ số lực khối nước kèm chéo trục Z rp -270 kg/rad
Hệ số lực khối nước kèm chéo trục và Z uw -10 kg/rad
lực nâng và lực cản
Hệ số lực của bánh lái Z uu s 13.48 kg/m.rad
Hệ số mô men khối nước kèm K p -676 kg.m2/rad2
Hệ số mô men bánh lái K uul 23.59 kg/rad
Hệ số mô men khối nước kèm chéo Kup -100 kg/rad
trục
Hệ số mô men khối nước kèm K uu -100 kg/rad
Hệ số mô men khối nước kèm M q -406.6 kg.m2/rad2
Hệ số mô men khối nước kèm M w 92 kg.m2/rad2
Hệ số mô men thân và thành phần M uw 362 kg
bánh lái hướng
Hệ số mô men khối nước kèm chéo M uq -125 kg.m/rad
trục và lực nâng
Hệ số mô men khối nước kèm M vp 10 kg.m/rad
 A4

Hệ số mô men khối nước kèm chéo M rp 140 kg.m2/rad2

trục
Hệ số mô men bánh lái M uu s -23.59 kg/rad
Hệ số mô men khối nước kèm N v 92 kg.m2/rad2
Hệ số mô men khối nước kèm N r 676 kg.m2/rad2
Hệ số mô men khối nước kèm chéo N ur -90 kg.m/rad
trục và lực nâng
Hệ số mô men thân và bánh lái N uv -78 kg
Hệ số mô men khối nước kèm chéo N wp 100 kg.m/rad
trục
Hệ số mô men khối nước kèm chéo N pq -140 kg.m/rad
trục
Hệ số mô men bánh lái N uu h -23.59 kg/rad
Ma trận mô men đường chéo theo các I0 [70.83, 70.83,
trục hệ tọa độ gắn liền 1.83]
Tọa độ tâm nổi của ASWs Cf [0.007, 0,
0.015]
 B1

PHỤ LỤC B
Bổ đề 1 [41]: Tính bị chặn của nghiệm hệ tuyến tính ổn định.
Cho hệ thống x (t )  Ax (t )  Bu (t ) có điều kiện đầu x (0)  x0
trong đó, x  R n , u  R m , A  R nn , B  R nm .
Giả sử A là ma trận Hurwitz và u (t )  L2 e , cho  0 và 0 là các hằng số dương,

  [0,  l ] trong đó, 0   l  2 0 thỏa mãn điều kiện || e A(t  ) || 0 e  0 ( t  )

|| B || 0
Thì ta có: || x (t ) || 0 e t || x0 || 
0
|| ut ||2 (B.1)
2 0  
t
trong đó, giá trị || ut ||2 được tính theo công thức: || ut ||2  (  e  (t  ) u T ( )u ( )d )1/2
0

Định lý [41]: Tính hội tụ của hệ thống điều khiển theo phương pháp hồi
tiếp đầu ra nơron mờ thích nghi trực tiếp.
Xét hệ thống có mô tả động học như (3.35) thỏa mãn các giả thiết 1,3 với bộ
quan sát trạng thái (3.38), luật cập nhật (3.62), (3.63) thành phần để khử nhiễu
ngoài và sai số mô hình (3.64), luật điều khiển (3.39). Khi đó tất cả các tín hiệu của
vòng lặp kín hệ thống bị chặn và Ek1 (t ) tiến tới 0 khi t   .
Chứng minh:
Định nghĩa hàm Lyapunov của hệ thống như sau:
3
V   Vk (B.2)
k 1

1 1 T 
trong đó, Vk  ekT Pk ek  k k (B.3)
2 2 k
với Pk  PkT  0. Đạo hàm theo thời gian hàm Vk ta có :
1 1 1 
Vk  ekT Pk ek  ekT Pk ek  kT k (B.4)
2 2 k
Từ (3.60) và (B.4) ta có:
1 1 
Vk  ekT ( AckT Pk  Pk Ack ) ek  ekT Pk Bck [kT k ( eˆk )  v fk  w fk ]  kT k (B.5)
2 k
Bởi vì H k ( s ) Lk ( s ) là hàm truyền thực dương chặt, nên tồn tại Pk  PkT  0 sao cho:

AckT Pk  Pk Ack  Qk

với Qk  QkT  0 (B.6)
Pk Bck  Ck
 B2

Từ (B.5) và (B.6) ta có:

1 1 
Vk   ekT Qk ek  E k 1[kT k ( eˆk )  v fk  w fk ]  kTk (B.7)
2 k

Trong trường hợp E k1  0 , theo (3.64) thì vk   k   k /  min , mặt khác theo giả

thiết 3 | w fk |  k khi này v fk  w fk . Do đó E k1[v fk  w fk ]  0 .

Trong trường hợp E k1  0 , theo (3.64) thì vk    k , mặt khác theo giả thiết 3

| w fk |  k khi này v fk  w fk . Do đó E k 1[v fk  w fk ]  0 .

Như vậy trong cả hai trường hợp E k 1  0 hay E k 1  0 thì (B.7) có thể được viết lại:
1 1 
Vk   ekT Qk ek  E k 1kT k ( eˆk )  kT k (B.8)
2 k

Sự thật min (Qk ) || ek ||2  min (Qk ) | E k1 |2 (B.9)

trong đó, giá trị riêng nhỏ nhất min (Qk )  0 , nên từ (B.7) và (B.8) có:
1 1 
Vk   min (Qk ) | E k 1 |2  E k 1kT k ( eˆk )  kT k (B.10)
2 k

vì Tj   *j T   Tj

Mặt khác theo giả thiết 2 thì  k*  arg min  k M  k [sup ek Uek , eˆk Ueˆk | u k*  u k ( eˆk /  k ) |]

Tức là k*  0 , khi đó Tj  Tj phương trình (B.9) được viết lại:
1 1
Vk   min (Qk ) | E k 1 |2  E k 1kT k ( eˆk )  kT k (B.11)
2 k

Trường hợp E k1 kT k ( eˆk )  0 , theo (3.62) phương trình (B.10) được viết lại:
1
Vk   min (Qk ) | E k 1 |2  E k 1 kT k ( eˆk )  [k ( eˆk )]T k  (B.12)
2

Vì kT k ( eˆk )  [k ( eˆk )]T k  0 nên từ (B.12) có:

1
Vk   min (Qk ) | E k 1 |2 (B.13)
2
Trường hợp ||  k || m k
vaø E k 1 kT k ( eˆk )  0 , theo (3.63) phương trình (B.11)

được viết lại:

 B3

1 E k 1 kT k ( eˆk ) T 
Vk   min (Qk ) | E k 1 |2  E k 1 kT k ( eˆk )  [k ( eˆk )]T k   k k (B.14)
2 ||  k ||2

1 E k 1 kT k ( eˆk )
 Vk   min (Qk ) | E k 1 |2   kTk (B.15)
2 ||  k ||2

Trong trường hợp này ||  k || mk ; || k || 2mk và E k 1 kT k ( eˆk )  0 nên
E k 1 kT k ( eˆk )
 kT k  0 khi đó phương trình (B.14) có thể được viết như (B.13).
||  k ||2

Từ (B.2) và (B.13) cho thấy E k1 (t )  L và ek (t )  L , nhưng không đảm bảo

rằng hội tụ bởi vì tất cả các biến ở vế phải của (3.60) bị chặn, E k1 (t ) bị chặn,

E k1 (t )  L .

Tích phân 2 vế (B.13) ta được:


 1 
Vk 0
    min (Qk ) | E k 1 |2  dt (B.16)
0 
2 

Vk (0)  Vk ()
  | E
0
k1 |2 dt 
(1/ 2)min (Qk )
(B.17)

Bởi vì vế phải của (B.16) bị chặn nên E k1 (t )  L2 , theo bổ đề Barlalat [37] ta có

lim | E k 1 (t ) | 0 .
t 

Với định nghĩa hàm Lyapunov hệ thống như (B.2) khi này:
3 3
1
V   Vk   min (Qk ) | E k 1 |2 (B.18)
k 1 k 1 2

Chứng tỏ E k1 (t )  0 khi t  
Tiếp theo xét phương trình sai số động học bộ quan sát (3.38). Đặt
2 2
uk   g kjTj  j ( eˆj )   g kj v j  wk  d k , theo luật cập nhật (3.62), (3.63) và giả thiết
j 1 j 1

1,3 cho thấy uk bị chặn. Mặt khác A0 k  K 0 k CkT là ma trận Hurwitz, khi đó theo bổ đề

1 ta có:
 B4

|| Bk || 0 k
|| ek (t ) || 0 k e 0 k t || ek (0) ||  || uk ||2 k (B.19)
2 0 k   k

Bởi vì A0 k  Bk K ckT là ma trận Hurwitz và ek (t ) bị chặn, từ phương trình động

học bộ quan sát trạng thái (3.38) cho thấy eˆk (t ) bị chặn. Vì ek  ek  eˆk do đó

Ek1 , ek  L và E k1 (t )  0 khi t   dẫn tới xk , xˆk  L . Sự giới hạn của yk (t ) theo

bởi Ek1 (t ) và yink (t ) .