Luận án Tiến sĩ Kỹ thuật: Xây dựng thuật toán dẫn đường và điều khiển cho phương tiện ngầm

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

,

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

,X Y Z

,

Các mô men ngoại lực tác dụng lên AUV (Nm)

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)

K M N , ,u v w ,

,

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)

u v w , c c c

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)

,p q r ,

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

Các góc Ơle (rad)

Hệ số lực khối nước kèm (kg)

x y z , , ,  , uX 

X

,

X

,

Hệ số lực khối nước kèm chéo trục (kg/rad)

X X , vr

rr

qq

wq

X

,

X

Hệ số lực cản theo trục

u u |

|

|

v v |

X

Hệ số lực cản theo trục

|,

X X , uv

uw

w w |

bX của hệ tọa độ gắn liền (kg/m) bX của hệ tọa độ gắn liền (kg/m)

upX

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

uuX 

l

Lực đẩy động cơ (N)

plX

Hệ số lực khối nước kèm (kg)

 ,v rY Y

Hệ số lực cản (kg/m)

|v vY

|

Y Y ,wp pq

Hệ số lực khối nước kèm chéo trục bY (kg/rad)

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

uvY

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

urY

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

uuY 

h

Hệ số lực khối nước kèm (kg)

 q

Hệ số lực cản (kg/m)

|w wZ

|

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

uqZ

Hệ số lực khối nước kèm chéo trục (kg/rad)

Z Z ,vp rp

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)

uwZ

Z Z ,w

vii

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

uuZ 

s

Hệ số mô men khối nước kèm (kg.m2/rad2)

Hệ số mô men bánh lái đối với hệ tọa độ gắn liền (kg/rad)

pK  uuK 

l

Hệ số mô men khối nước kèm (kg/rad)

uuK

Hệ số mô men khối nước kèm chéo trục (kg/rad)

upK

Hệ số mô men khối nước kèm (kg.m2/rad2)



M M ,q

w

Hệ số mô men thân và thành phần bánh lái (kg)

uwM

Hệ số mô men khối nước kèm chéo trục (kg.m2/rad2)

rpM

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

uqM

Hệ số mô men khối nước kèm chéo trục (kg.m/rad)

vpM

Hệ số mô men bánh lái (kg/rad)

uuM 

s

Hệ số mô men khối nước kèm (kg.m2/rad2)



N N ,v

r

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

urN

Hệ số mô men thân và bánh lái (kg)

uvN

Hệ số mô men khối nước kèm chéo trục (kg.m/rad)

N N ,wp

pq

Hệ số mô men bánh lái (kg/rad)

uuN 

h

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

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

Lực cản (N)

B W D L 

Lực nâng (N) Mật độ nước (kg/m3) Tổng diện tích bề mặt theo hướng vận tốc AUV (m2)

fA

, 

Các góc tấn công và góc trượt ngang của AUV (rad)

Diện tích bề mặt của bánh lái (m2)

Góc ảnh hưởng của bánh lái (rad)

blS e

Khoảng cách từ trục các bánh lái đến tâm trọng lực (m)

blx

Góc quay các bánh lái hướng (rad)

h  ,h

1

2

viii

Góc quay các bánh lái sâu (rad)

s  ,s

1

2

Tổng góc bẻ lái điều khiển theo góc hướng (rad)

h

Tổng góc bẻ lái điều khiển theo góc lắc (rad)

l

Tổng góc bẻ lái điều khiển theo góc chúc ngóc (rad)

s

Ma trận quán tính của AUV

RBM

Ma trận hướng tâm Coriolis của AUV

RBC

Véc tơ ngoại lực và mô men ngoại lực tác động lên thân AUV

RB

Tâm khối của AUV trong hệ tọa độ gắn liền

bG

Ma trận mô men đường chéo theo các trục hệ tọa độ gắn liền

0I

Ma trận quán tính khối nước kèm

Ma trận hướng tâm Coriolis khối nước kèm

Ma trận lực và mô men thủy động

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

Ma trận thông số lực và mô men của bánh lái

Véc tơ lực và mô men của bánh lái

AM AC  ( ) )D  ( ( )g  )L  ( bl

Véc tơ lực và mô men của động cơ đẩy

pl

Tâm nổi của AUV

fC

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

R ,

lR

Bán kính của trái đất khi xem trái đất là hình cầu

R 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ý

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

U  

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ý

Kinh độ, vĩ độ

,l 

ix

Số siêu phức quaternion

,

Các tham số Rodrig – Hamilton

0

3

2

1

n

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

 , ,     bC

n

Ma trận chuyển từ hệ tọa độ đế sang hệ tọa độ địa lý

dC

,

Chỉ số gia tốc cảm nhận có nhiễu đo đối với hệ tọa độ gắn liền

a a a , x y

z

,

,

Chỉ số gia tốc cảm nhận đối với hệ tọa độ gắn liền

a bx

a by

a bz

,

Chỉ số gia tốc cảm nhận có nhiễu đo đối với hệ tọa độ đế

n n n , x y

z

,

,

Chỉ số gia tốc cảm nhận đối với hệ tọa độ đế

n dx

n dy

n dz

f

,

f

,

f

Chỉ số gia tốc cảm nhận đối với hệ tọa độ địa lý

N

E

D

,

Các nhiễu đo của gia tốc kế

w w w , 5

6

4

,

Chỉ số của các con quay vi cơ bao gồm độ trôi và nhiễu đo

   , q

p

r

,

Các tham số biến đổi chậm thể hiện độ trôi của con quay

b b b , p q r

,

Nhiễu đo con quay vi cơ

w w w , 2

1

3

,

Chỉ số của từ kế đối với hệ tọa độ địa lý

B B B , y

x

z

,

V

,

Chỉ số của vận tốc kế chiếu lên hệ tọa độ gắn liền

DVL V x

DVL y

DVL V z

,

Các chỉ số vận tốc đối với hệ tọa độ địa lý

V V V , E

D

N

Véc tơ tham số hệ thống



Véc tơ hồi quy

( )t

Sai số ước lượng

( )t

Hệ số quên



Véc tơ trọng số kết nối giữa lớp 3 và lớp 4 của mạng singleton

k

Véc tơ sai số trạng thái ước lượng được

ˆke

,

,

Các tập mờ

1

i i A A B 2 k k

i k

)

Véc tơ cơ sở mờ

ke ˆ(

k

Đầu ra của mạng singleton

fku

I

I

I OX Y Z

Hệ tọa độ quán tính

e

e

e

Hệ tọa độ cố định tâm trái đất

OX Y Z

x

Hệ tọa độ gắn liền

b

b b

Hệ tọa độ địa lý

0 0

0

OX Y Z OX Y Z 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

0Z ................................................. 18

Hình 1.12: Quay theo góc chúc ngóc  quanh trục 1Y ............................................. 18

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

2X ..................................................... 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

,

,

,

    đúng và ước lượng ............................................ 103

Hình 4. 5: Các giá trị

0

1

2

3

Hình 4.6: Các giá trị

ijc đú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

0

70

0 75



 

hạn

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

03 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 (

)nR quanh

điểm tiếp cận quỹ đạo mong muốn ( )T , bán kính

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

0X

ASWsR

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

AWsR

qd

nR

T

quỹ đạo mong muốn trong mặt phẳng

C

ngang

bX

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

0Y 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

0X

LOS góc chúc ngóc quỹ

nR

đạo hiệu chỉnh tại điểm

T

bắt đầu quá trình điều

C

qd

khiển

bX

0Z

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;

IZ

N

e

n ă v n ê i h t i à Đ

h c i w n e e r G

GMST

O

IY

Điểm xuân phân

Đường xích đạo

IX

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

eZ

, )l   (

0X

0Y

0Z



O

eY

l

eX

Đườ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 Y Z được gắn chặt với trái đất, thường thì hệ

0 0

0

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

0X theo hướng bắc (N), trục

0Y hướng đông (E) và trục

0Z 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

0X theo hướng bắc (N), trục

0Y hướng đông (E) và trục

0Z 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

b b

b

b

G X Y Z 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

bG ; Trục dọc

bG X hướng theo

b

chiều dọc của phương tiện ngầm, trục

bG Z hướng xuống dưới và trục

b

b bG Y 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ý

0Z được hệ tọa độ

0 0

0

mới là

1 1

1

OX Y Z một góc hướng  quanh trục OX Y Z (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

u

cos

sin

0





0

sin

 cos

0



(1.1)

 0

 0

1

v 0 w 0

u 1 v 1 w 1

    

    

    

         

    



0X

1X

1u

0u

0Y

0v

O



1v

1Y

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

0Z

Quay hệ tọa độ

OX Y Z một góc chúc ngóc  quanh trục

1 1

1

1Y được hệ tọa độ

mới là

2 2

2

OX Y Z (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:

cos

0 sin

u



2



(1.2)

0 sin





u 1 v 1 w 1

v 2 w 2

    

    

    

     0 1     0 cos   

    

2X

2u



O

1u

1X

1w

2w



2Z

1Z

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

19

Quay hệ tọa độ

OX Y Z một góc lắc  quanh trục

2 2

2

2X đưa về hệ tọa độ

OX Y Z trùng với hệ tọa độ gắn liền

3 3

3

b b

b

OX Y Z (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:

2

(1.3) 

2v

2Y

O

0 1 0 cos 0 sin 0 sin  cos   u v 2 w 2 u 3 v 3 w 3                                

3v

3w

3Y

2w

3Z



2Z



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

2X

Thực hiện ba phép quay liên tiếp bên trên ta có các ma trận quay:

0

0

1

cos

0 sin

cos

sin

0









(1.4)

;

;

,

,

,

XR 

ZR 

YR 

 cos  0

0 cos 0 sin

sin  0

sin  cos

0 sin

0 1

 



    

      

     

     

    

   0 1   0 cos   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 độ

n

địa lý với ma trận Côsin định hướng

bC được biểu diễn:





Z

,

, 

R R R X  Y ,

n C b

    

(1.5)

c c 11 12 c c 21 22 c c 32 31 cos sin sin sin sin sin

cos sin cos sin sin cos

sin sin sin cos



c  13  c  23  c  33 sin cos  cos cos  cos sin  

  cos cos  

    

cos cos               sin cos               sin   

n

Ma trận

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

1 

C

)







sang hệ tọa độ gắn liền bởi ma trận chuyển

b n

n T C ( ) b

n C ( b

T T R R R Y Z

T X

,

,

,   

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]:

k

i

 





(1.6)

j      3

0

1

2

trong đó,

    là tham số Rodrig – Hamilton; , i

0

1

2

3

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:

2

2

2

, , , j k đơn vị phức trên ba trục phức. ,





 



 



 



  ; i 1

; j k 

; k i 

.

(1.7)

k

i

*  





Số liên hợp quaternion

* được định nghĩa:

j      3

0

1

2

*

1





    

Khi đó

2      3

2 1

2 0

2 2

Ma trận chuyển của của véc tơ

bR từ hệ tọa độ gắn liền sang hệ tọa độ địa lý

nR

có thể định nghĩa thông qua số siêu phức quaternion  như sau [20]:

*









(1.8)

R n

          y

R b

R b

x

z

* y

* z

* x



Từ (1.8) thấy rằng số siêu phức quaternion

. Sử dụng quy tắc

      z

y

x

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

k

sin

(cos

j

i







      z

y

x

 2

 2

cos

cos

sin

sin

i

(cos

cos

cos

sin

sin

(cos







(1.9)

2

2

2

2

2

2

cos

cos

sin

(sin

cos

cos

cos

sin

k

j

(cos









  )(cos 2 2       sin )  2 2       sin ) sin 2 2

2

2

2

2

  sin ) sin )(cos 2 2       ) sin 2 2 2    2 2 2

2    sin ) 2 2 2

Từ (1.6) và (1.9) các tham số Rodrig – Hamilton được xác định:

   i j k j j i k k j i  i k j

cos sin sin sin cos cos  2 2 2 2

(1.10)

cos sin cos cos sin  2 2 2 2

cos cos cos sin sin  

sin cos cos cos sin sin          2 2       sin 2 2       sin 2 2 2 2    2 2 2 2 2    2 2 2    o     1     2     3 

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







cos

k

sin



  z

 2

 2

sin

cos

0

I

,

ZR 

 0

 0

1

     

    



cos

j

sin



  y

 2

 2



II

,

YR 

cos 0 sin



     

0 sin    0 1   0 cos  

cos

i

sin



  x

 2

 2





III

,

XR 

0 1 0 cos 0 sin

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]:



(1.11)

R n

n C R b b

2

2



2 0

1 3

 2

2 2



0 3 



trong đó,

(1.12)

1 2 2  0

0 3

1 2

n C b

2 2    1 2 2 2      2 2

2

2

2

2

2 1 2      0 2 2 1 2    0 1 1 





2 2       3

2 3 2  0

0 2

2 3

0 1

1 3

c 11 c 21 c 31

c 12 c 22 c 32

c 13 c 23 c 33

    

    

    

    

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)

0

k

j

  





là vận tốc góc quay tuyệt đối của hệ tọa độ địa lý;

trong đó,

i    E D

N

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:

 

1

3

2

1

N

D

E

3

q p      

D

N

E

0

3

2

0

2

(1.14)

p q r     

D

N

E

3

0

1

3

1

p q r     

D

N

E

2

1

0

2

0

p q        2 r            0 2  2            1 3  2            0 2  2 r            3 1       

22

n

Từ các tham số Rodrig-Hamilton của ma trận Côsin định hướng

bC 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:

arctg



arctg  2 2    0 3 1 2 2 2 2 1 2     0 1   

 





c 31

1 3

(1.15)

arctg

arctg



   arcsin    arcsin (2 )     2    0 2

c 21 c 11  c 32 c 33

 2 2    0 1 2 3 2 2 2 1 2     0 3                        

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 g

 

(1.16)

m

trong đó,

a



là gia tốc tuyệt đối (gia tốc đối với hệ tọa độ quán tính);

I

 d dr  dt dt 

  

I

mg 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]:

U r

a



  



U  







I

d dt

d  dt

dr dt

I

I

I

  

I

(1.17)

U U r (

)



U        



 d dr  dt dt  d  dt

N

trong đó,

đạo

 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;

d  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);

(

,

)T



U U U U , N D

E

vận tốc góc trái đất;  là ký hiệu tích véc tơ;

23

,

,

)T

 

vận tốc góc tuyệt đối của hệ tọa độ địa lý;

(    D

N

E

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:

f

g



        U

(1.18)

d  dt

N

trong đó,

là gia tốc trọng trường.

g

g

U U r (

)









m

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

(1.19)

x

y

y

x

y

z

z

y

x

z

z

x

x

y

z

m n det ( ( ) ) (    m n m n a m n m n b m n m n c )     

x

y

z

trong đó,

m m m n n n                    

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ả:

f











 

N

V E D

V U V U V D E E D D E

f











 

E

V N D

V U V U V D N N D D N

(1.20)

f

g













 

D

V E N

V U V U V N E E N N E

dV N dt dV E dt dV D dt

        

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.

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

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

D

D

D

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

,

,

, , OX OY OZ bằng 3 động cơ

Một đầu của các trục quay

D

D

D

,

,   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

1

2

3

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

b

định được ma trận Côsin đinh hướng

dC 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

n

định hướng

dC giữa hệ tọa độ đế và hệ tọa độ địa lý tại nơi máy bay thả ASWs.

n

Từ việc xác định được ma trận Côsin định hướng

dC 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ý):

OX OY OZ gắn cảm biến góc để đo góc quay

26

(

T )

C

(

T )

f

,

f

,

f

,

,



(1.21)

E

D

N

n d

n dx

n dy

n dz

trong đó,

,

,

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

N

E

D

,

,

f f f

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:

f

;

f

;

f

g









 V N

N

 V E

E

 V D

D

(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:

;

;

(1.23)

 x V  N

 y V  E

 z V  D

n

Từ việc xác định được ma trận Côsin định hướng

dC giữa hệ tọa độ đế và hệ tọa

b

độ địa lý và ma trận Côsin định hướng

dC giữa hệ tọa độ đế và hệ tọa độ gắn liền sẽ

n

xác định được ma trận Côsin định hướng

bC giữa hệ tọa độ gắn liền và hệ tọa độ

địa lý:

1 

n dx n dy n 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ế. dz

( C

)

(1.24)

n C b

b d

n d

* c 11 * c 21 * c 31

* c 12 * c 22 * c 32

* c 13 * c 23 * c 33

C 

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

N

E

D

n

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

bC :

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

V V V tâm khối của ASWs cần phải xác định ma

(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

1

q r   

3

0

(1.26)

p q r  

0

3

q p  

 2 p      0 3  2      1 2  2 r      2 1  2 p     0 3

1

2

n

Từ việc xác định được ma trận Côsin định hướng

bC sẽ xác định được các

thành phần gia tốc trong hệ tọa độ độ địa lý [46]:



(1.27)

f C a b

n b

hay viết dưới dạng ma trận:

q r           

2

2 2    1

2 0

1 2

2 2    0 3

1 3

2 2    0 2

N

f     a bx a by a bz

2

2

(1.28)

2 2    0 3

1 2

2 2    2

2 0

2 3

E

f     a bx a by a bz

  (2

2

2

  

 1  

2 2    0 2

1 3

  1 )    0 1

2 3

2 2    3

2 0

D

  

  2    0 1  1

trong đó,

f

(

f

,

f

,

f

)T



là các thành phần véc tơ gia tốc trong hệ tọa độ địa lý;

E

D

N

(

,

,

)T



là chỉ số của gia tốc kế trong hệ tọa độ gắn liền;

a b

a bx

a by

a bz

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

f     a bx a by a bz      

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:

,

,

(1.29)

p

,

,

trong đó,

p b           p  q  r w 1 q b w 2 q r b w 3 r

q

r

,

,

b b b là các tham số biến đổi chậm thể hiện độ trôi của con quay vi cơ; p

1

2

3

Gia tốc kế cho giá trị đo bao gồm gia tốc chuyển động thực và nhiễu:

a

a

; a

a

; a

a

;

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

(1.30)

x

bx

y

by

z

bz

,

,

trong đó,

      w 4 w 5 w 6

4

5

6

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.

w w w là các nhiễu đo thường có dạng ồn trắng.

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

(

X

)

)





k

k

w k

k

G X ( 1 

1 

1 

(1.31)

Z

)



k

F 1 k  h X ( k

k

k 1  v  k

trong đó,

k

1

,...,

)T

F

)

(

f

f

,



X X  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 ,k là các hàm

là ma trận hệ số nhiễu;

và bước thứ (k-1);

1

k

k

f 1

2

n

G X (

1 

)

là véc tơ hàm động học

động học phi tuyến theo biến số là véc tơ X với

F k

1

k

X (

1 

ở bước thứ k-1;

kZ là giá trị của véc tơ đầu ra (véc tơ m chiều, thường m  n);

29

)T

h

,...,



là véc tơ đo với

h m

h 1(

kh là giá trị của véc tơ đo ở bước thứ k;

kw v là véc ,k 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 :

T

w j w k ( )

( ),

I





Q . 

~

(0,

)



N Q ;

 ; 0

;

kw

k

k

2 wk





kw k ( )

 

 

T

N ~ (0,



R ; )

 ; 0

;

kv

2 vk





kv k ( )

k

( ), k ( ) I   R .  v k j v k    

 X

 X

là ma trận hiệp phương sai của véc tơ sai số đánh giá

k

k

k

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

2

2

trong đó,  là kỳ vọng toán học;

wk ,

vk 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

0X là một véc tơ ngẫu nhiên có kỳ vọng toán học biết trước

([ X ][ X ] )T     P k

  

và ma trận hiệp phương sai ban đầu

0

0





0

0X

X ]T     X ][ 0  0 P 0  [ 

kZ như

sau (hình 1.15):

Phương trình trạng thái ước lượng là [47]:

(

)

(

)





ˆ X

ˆ( X

)



F k

1 

k

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:

(1.33)

  k 1 

ˆ X X 

( k

)  1 

F  1 k  X 

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:

(

)

(

)





 

;

(1.34)

k

k

1 

T   1 k 

T G QG 1 1 k  

  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

1 

Tính toán ma trận đo bằng cách đạo hàm riêng:

H



)



(1.35)

k

ˆ X X 

( k

h  k X 

Độ 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:

(

)

(

)





1 

K

R

)





(1.36)

k

k

T k

T k

( P H H P H k

k

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

P k P k

30

)

ˆ Z

)



(1.37)

k

ˆ( ( h X  k

k

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

(

)

(

)





ˆ X

ˆ X

ˆ Z

)







(1.38)

( K Z k

k

k

k

k

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:

(

)

(

)





(

)





(1.39)

k

P k

I K H P k k

T

T

T

T

H

H

1 H

...(

T n )





Điều kiện đánh giá được khi và chỉ khi hạng của ma trận

T

T

T

T

1 

rank H

H

H

...(

T n )





bằng bậc của hệ động học, tức là:

 n

Q

R

Hiệu chỉnh ma trận

kK theo (1.34) và (1.36)

kv

)

)

ˆ ( X 

ˆ ( X 

kX

kZ

ASWs

k

k

(

)



ˆ( X

)

(

kF

) h X   k

k

1 

kK

k

1 

)

ˆ ( X 

k

ˆ kZ

Các phương tiện đo

(

)

)

ˆ( kh 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

F k

1

k

( X

1 

(

- Xác định véc tơ hàm quan sát

h X ) k k

)

- Xác định ma trận hệ số nhiễu

1

k

k

( G X

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

X0

o

B

Xb

u

Y0

Gb

fC

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:

(

X Y Z ,

,

)T

véc tơ ngoại lực tác dụng lên AUV;

  1

(

K M N

,

,

)T

véc tơ mô men ngoại lực tác dụng lên AUV;

  2

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 Y Z ; , b

b

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ý:

x y z ( ,

, )T

  1

vị trí tâm khối của phương tiện ngầm trong hệ tọa độ địa lý;

,

)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

2

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

T

T

,

,

,

x y z ( , ,





) ,  

,

T T )

u v w p q r ( , , ) ,

,

,



T V ( 



;

Ký hiệu:

T T ( )    2

T 1

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

n

J

( ) 

J

)

0

  cos

  sin



trong đó,

với

(  2

bC 0

)

0 3 3  J (  2

3 3 

    

   

 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

M

)

(   

liền [15], [39]:  C 



(1.41)

RB

RB

RB

trong đó,

RBM là ma trận quán tính;

RBC 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]:

(1.42)

F ma b

b

)

 

(   

là gia tốc

a b

G b

G b

trong đó, m là khối lượng của AUV;

tâm khối của AUV chiếu lên hệ tọa độ gắn liền [39];

bG 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]:



) 



(1.43)

     0

I 0(

M I  b

G ma  b

b

I

diag I [

,

I

,

I

]

3 3 R 





trong đó,

là ma trận mô men đường chéo theo các trục

zz

yy

xx

0

,

,

b

b

b

X Y Z tại tâm trọng lực.

Thay thế gia tốc

ba 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 Y Z . Xét hệ tọa độ gắn liền có gốc tọa độ tại tâm

b

b

b

khối

bG , ma trận quán tính và ma trận hướng tâm Coriolis được biểu diễn [39]:





(1.44)

;

M



RB

C



RB

0 mr mq mp mr 0 mq mp 0  0 0 0 0 0 0 0 0 0

0 0 0 I q  yy I p xx 0



m 0 0 0 0 0

0 m 0 0 0 0

0 0 m 0 0 0

0 0 0 I xx 0 0

0 0 0 0 I yy 0

0 0 0 0 0 I

0 0 0 0 I r  zz I q yy

0 0 0 I r zz 0 I p xx

zz

         

         

         

         

 V  V       

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]:

M

C

D

L

g

( )

(

)

( )  

( )      













(1.45)

RB

A

(

)

,

trong đó,

AM C  là ma trận quán tính và ma trận hướng tâm Coriolis khối nước

A

kèm;

( )D  là ma trận lực và mô men thủy động;

(

ra bởi trọng lực và lực nổi;

( )g  là véc tơ lực và mô men gây )L  là ma trận thông số lực và mô men của bánh lái;





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

   bl pl

   A

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

C

(

x

,

y

,

z

)T



Lực nổi (lực Acsimet) B tác động tại tâm nổi có tọa độ

đối

f

f

f

f

với hệ tọa độ gắn liền và trọng lực W tác động tại tâm khối

bG (hình 1.16).

bG , khi này tọa độ của tâm

Xét hệ tọa độ gắn liền có gốc tọa độ tại tâm khối T (0,0, 0)

T )

(

,

,





. Đối với AUV thì W B ,

khối trong hệ tọa độ gắn liền là

x y z b b b

G 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

0

tức là

fy  [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 0

Y

HS

0

Z

HS

g



( ) 

(1.46)

cos

sin  

K

HS

sin

cos cos

M

z B f

HS

x B f cos



sin  

N

z B f   x B f

HS

         

         

          

           

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











X u X wq X qq X vr X rr qq

vr

rr



Y v Y r Y ur Y wp Y pq 



wq 

A 

 u  

wp

pq

 r

 v

ur a





Z w Z q Z uq Z vp Z rp

Z











vp

rp

A

 w

uq

 q

a

(1.47)

K



 K p  p

A





M M w M q M uw M vp M rp M uq













vp

rp

 q

A

uq

uw a

a



 w 

N













N v N r N uv N wp N pq N ur wp

pq

 v

 r

A

uv a

ur a

  Y  A        

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

0

0

0

0

0

 u 0

0

0

0

0

Y  v 0

Z

0

Z

Y  r 0

(1.48)

M

A

0

0

 w 0

 q 0

0

K

0

0

 p 0

0

M

M

 q

0

 w 0

0

0

N

N

 v

 r

          

         

X w X q X v X r rr

wq

qq

vr

0 0

0 0

0 0

0 Y w wp

 Y p pq

 Y u ur a

0

0

0

0

Z v Z r  rp

vp

Z u uq a

C



(1.49)

A

0

0

0

0

0

0

0

0

0



M u M r M v rp vp

uw a

M u uq a

0

0

N w wp

N p pq

N u uv a

N u ur a

         

         

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

L

D

bG

 

V

bX

bY

bZ

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

theo hướng vận

Lực cản liên quan đến mật độ nước , tổng diện tích bề mặt

fA

tốc V của AUV (hình 1.17) [15]:

2

D

(1.50)

C A V D f

1  2

2

2

2

V

u

v

w







trong đó,

;

) theo đường Parabol

DC liên quan đến góc tấn công (

2

b

c



a   

có dạng

 với

,a b c là các hằng số. ,

DC

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ể

tg

[

rad

]

 



được mô tả trong mặt phẳng ngang

và góc trượt bên 

bX Z là

b

w u

tg

[

rad

]

 



.

trong mặt phẳng đứng

b bX Y được mô tả

v u

,

Tổng lực cản theo các trục

b

b

b

|

|

|

X

|

|

X Y Z là: , | 









X uv X uw X v v uw

vv

X w w | w w |

d

X u u | u u |

|



v v |

|

uv Y uv Y v v |  uv d

(1.51)

Z

|

|



Z uw Z w w 

d

w w | |

uw d

  Y  d  

trong đó,

X

A c X ) ;

X

A b X ) ;

X

);

( 

( 

( 

 



 



 



|

u u |

f

uw

uv

f

w w | |

v v |

|

A a )( f

1 2

1 2

c 2

1 2

(1.52)

Z

A b Z ) ;

( 

( 



 

 

 

|

w w | |

f

A c ) f

Y uv d

ww d

1 2

1 2

    Y v v | 

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ố

LC , MC liên

quan đến góc tấn công  và góc trượt bên  [15].

2

2

L

M

;

(1.53)

C A V L

f

C A V M f

1  2

1  2

Lực nâng và mô men chúc ngóc khi xét trong mặt phẳng

bX Z từ phương trình

b

(1.53) ta có:

2

2

L

Z

w





) cos  

với

(1.54)

l

A C u ( L

f

L



1 2

C  C    

36

2

2

M

M

w

u (







) 

với

l

A C f M

M



1 2

C  C    

Tương tự trong mặt phẳng

b bX Y :

2

2

2

2

v

N

v

u (







) cos  







) 

;

(1.55)

Y l

f

l

A C f M

A C u ( L 



1 2

1 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ó:

Z

M

N

 









;

(1.56)

L

f

f M

A C 

A C 

Y uv l

uw l

uv l

uw l

1 2

1 2

Từ (1.52) đến (1.56) ma trận lực và mô men thủy động được mô tả:

X

|

u

|

|

|

w

|



w w | |

uw

u u | | 0

X u X |

X u X 0

0 0 0 0 0 0

uv 

| v | v v | v Y u | 

v v |

|

 Y u Y uv d

uv l

Z u Z

|

|

0 0 0

0

0



w Z u 

w w | |

uw d

uw l

(1.57)

D



( ) 

0 0

0 0

0 0 0 0 0 0

0 M u uw l

0

0

0 0 0

N u uv l

         

         

1.4.2.4 Các lực và mô men của bánh lái

Xb

blx

s

bZ

Xb

blx

h

bY

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]:

2

M



u



;

(1.58)

bl

x L bl bl

L bl

C S L

 bl e



bl

1 2

37

Trong đó,

blS là diện tích bề mặt của

blLC  là tốc độ thay đổi hệ số lực nâng và

)

bánh lái.

là góc ảnh hưởng của bánh lái (hình 1.18).

e rad (

v





Đối với bánh lái hướng, góc ảnh hưởng của bánh lái là

, đối với

   h

e

x r blh u

bls



. Thông thường khoảng cách từ trục

bánh lái sâu góc ảnh hưởng là

   s

e

w x q  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

x bl

x blh

x bls

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:

X

u [

]

 





uu x up 





X uu X up 

l

blh

2  l

bl

2  l

C S L 

blh

X u uu  l

uu l

up l

1 2

uv

u

u [

]













blh

2  h

x ur bl

Y uu

2  h

C S L 

blh

 h

Y uv Y ur  uv h

ur h

1 2

Z

Z

u

u [

]

 





uw x uq 





Z uw Z uq 

s

bls

2  s

bl

2  s

uu

uq

C S L 

bls

 s

uw s

s

(1.59)

K

]

C S x u [





K uu K up 

 





uu x u p 

2  l

l

blh bl

2  l

bl

L 

K u uu  l

uu l

up l

blh

M

C S x u [

uw x uq M u ]

 









M uw M uq 



s

bls bl

2  s

bl

uu

2  s

uq

L 

bls

 s

uw s

s

N

uv

C S x u [

]

 









N uv N ur 



h

blh bl

2  h

x ur bl

2  h

L 

blh

N u uu  h

uv h

ur h

1 2 1 2 1 2 1 2

    Y h             

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:

0

0

0

0

X u uu l

X u up l

0

0

0

0

Y u uv h

Y u ur h

0

0

0

0

Z u uq

Z u uw s

s

L



( ) 

(1.60)

0

0

0

0

K u uu l

K u up l

0

0

0

0

M u uq

M u uw s

s

0

0

0

0

N u uv h

N u ur h

          

          

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à:

2

T

(

X

2 u Y ,

2 u Z ,

2 u K ,

2 u M ,

2 u N ,

u

)



(1.61)

 bl

 l

uu

 h

uu

uu

 s

uu

 l

uu

 s

uu

 h

 h

 l

 s

 l

 s

 h

38









với,

;

trong đó

   h h 1

h

2

   h h 1

2

l

h  là góc bẻ lái các bánh lái hướng;

,h

2

1





trong đó

s

s  là góc bẻ lái các bánh lái sâu.

,s

   1 s s

2

2

1

Đố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

(

, 0, 0, 0, 0, 0)T

thường là không đổi và được biểu diễn:

(1.62)

  pl

plX

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.

N

cu





D

cw

E

cv

Vc

Hình 1.19: Vận tốc dòng chảy đại dương trong hệ tọa độ địa lý

u

,

,

Vận tốc dòng chảy

N c

E v w trong hệ c

D c

cV được phân tích thành các thành phần

tọa độ địa lý (hình 1.19) thông qua các góc  và  [39].

u

cos

cos

sin

cos  

sin  









;

;

(1.63)

N c

V c

E v c

V c

D w c

V c

T

(

,

,

)



Vận tốc dòng chảy đối với hệ tọa độ gắn liền được ký hiệu

b V c

u v w c c

c

,

,

,

trong đó

X Y Z . , b

b

b

u v w là các thành phần vận tốc dòng chảy theo các trục c

c

c

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ý

n

được mô tả thông qua ma trận chuyển

bC :

T

,

,

)

,



(1.64)

u v w ( c c

c

n T u ) ( C ( b

N c

E D T v w , ) c c

39

T

T

T

u v w ( , ,

)

(

,

,

)

(

u

,

)







Vận tốc tương đối của AUV khi này

.

V td

u v w c c

c

v w , td td

td

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

X

X

X

X







uu

uu

u u |

up

l

up l

|; Y

Y

Y

;











uv

uv

uv

Y ur

uv l

h

d

Y ur a

Y ur h

Z

Z

Z

;

Z

Z

Z

Z











uw

uq

uq

uq

uw l

uw s

uw d

a

s

(1.65)

K

K

;

K

K





uu

uu

up

l

up l

M

M

M

M

;

M

M

M











uw

uq

uq

uq

uw l

uw s

uw a

a

s

N

N

N

N

;

N

N

N











uv

uv

uv

ur

a

h

uv l

ur a

ur h

  Y         

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à:

ext

HS

2  l

uv

uw

uu

 u

v v |

|



 X X X uu X u X uv X uw X v v | |          X vr vr X u uu  l

up

qq

rr

pl

w w | |

| |       X w w X up X wq X qq X rr X wq

HS

2  h

wp

pq

uv

v v |

|

 h

 Y u | |         Y ext Y uu  Y ur Y uv Y v v Y v Y wp Y pq Y r    r v ur

ext

HS

2  s

uu

uw

vp

w w | |

rp

 q

 s

(1.66)

 | | Z Z Z u          Z uw Z uq Z vp uq  Z w w Z w Z q Z rp  w

ext

HS

uu

uu

2  l

up

 l

 K K K      K uu K p K u  p

ext

HS

2  s

uq

vp

uw

rp

 w

 q

  M M M uw M uq M vp M w M q M rp         M u uu  s

ext

HS

2  h

wp

pq

uv

ur

 v

    

 N N         N ur N uv N v N wp N pq N r   r N u uu  h             

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

2T

Xb

1A

1T

1C

2A

3T

2C

3A

Yb

3C

Zb

,

,

C C C - các con quay vi cơ

2

3

2

3

1

2

3

, , , , T T T - các từ kế; 1 A A A - các gia tốc kế; 1

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

;

;

;

p

;











x 2

;  1

 3

(2.1)

;

 2 ;

;

 0 ; q

r

b





x 3 

x 4 

x 5 

x 1 x 6

x 7

x 8

x 9

x 10

p

b q

b r

Biến X khi đó là véc tơ

6

2

9

4

8

7

3

10

Từ các phương trình (1.29) viết lại:

X ( , , , , , , , , )T  x x x x x x x x x x , 1 5

(2.2)

p

Thay các phương trình (2.2) vào các phương trình (1.26) ta có:

(

b

)

)

)













(2.3)

p

 2      0 p

1

w 1

(   2 q

b w  2 q

(   3 r

b w  3 r

(

)

)

)













(2.4)

p

 2    1 p

0

b w  1

(   3 q

b w  2 q

(   2 r

b w  3 r

(

b

)

)

)















(2.5)

p

 2    2 p

3

w 1

(   0 q

b w  2 q

(   1 r

b w  3 r

(

b

)

)

)

 













(2.6)

p

 2    3 p

2

w 1

(   1 q

b w  2 q

(   0 r

b w  3 r

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

w 3

(  p

(2.7)

x w  8 1 )

(  q )





x 3 



x 4 ) 

x 10 



 

x 2

x 3

x 8

x 9

x w  2 9 x  4

x 10

(  r x w x w x w 2 3 3

1

2

4

x 2 (  p

(  q

(  r

2

)

)

)















(  q

(2.8)

(  r )

)

 x 2 

(  p 



x 3 



w 3 

x 4 

x w  2 9 



x 1 (  p

x 1

x w  8 1 ( )  r

x 3

x 8

x 10

x 10 x 4

x 9

(  q

x w x w x w 1 3 2

1

3

4

2

)

)

)















(  p

(2.9)

(  r )

)

 x 3 

(  q 



x 2 



w 3 

x 4 

x w  1 8 



x 1 (  q

x 1

x w  9 2 ( )  r

x 2

x 9

x 10

x 10 x (  4 p

x 8

x w x w x w 1 2 1

2

3

4

2

)

)

)















(  p

(2.10)

x 10 )

(  q )

)

 x 4 

(  r 



x 2 



x 3 

x w  1 8 



x 1 (  r

x 1

x 10

w 3 (  q

x 2

x 9

x w  2 9 (   p

x 3

x 8

x w x w x w 1 2 1

3

2

3

p ; ;         b w q 1  p  q b w r q 2  r b w  r 3

Tách các số hạng có thừa số nhiễu

1

2

3

(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

, , w w w trong các phương trình vi phân

hàm động học

2

3

)

1) / 2

[

1)(

1))

1) (

1))









T x k ( 















x k ( ) 1

2

 p

x k ( 8

x k ( 3

 q

x k ( 9

(2.11)

1  1)(

(

k

1))] / 2

k 

x k ( 1 





f X ( 1 x k ( 4

 r

x 10

, f , f , f 1 f : 4

T

(2.12)

1 k  1)(

) 1) / 2 [ 1)( 1)) 1) ( ( k 1))            x k ( ) 2 x k ( 1  p x k ( 8 x k ( 3  r x 10

 x k ( 2    1))] / 2 f X ( 2 x k ( 4  q x k ( 9

45

1)(

1))

1)(

(

k

1))

)



















1)  

 q

x k ( 9

x k ( 2

 r

x 10

x k ( ) 3

(2.13)

k 1  1)( 

x k ( 3 





T x k [ ( 1 1))] / 2

f X ( 3 x k ( 4

 p

x k ( 8

1) / 2

1)(

(

k

1))

1)(

1))

)























T x k [ ( 1

 r

x 10

x k ( 2

 q

x k ( 9

x k ( ) 4

(2.14)

1 k  1)(



x k ( 4 





1))] / 2

f X ( 4 x k ( 3

 p

x k ( 8

Ở đâ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: ,

(2.15)

k

1 

)   x k ( ) 5 f X ( 5 x k ( 5

(2.16)

k

1 

)   x k ( ) 6 f X ( 6 x k ( 6

(2.17)

k

1 

)   1)  1)   1) x k ( ) 7 f X ( 7 x k ( 7

Độ trôi

q

r

, , b b b cũng có thể viết dưới dạng: p

(2.18)

k

1 

)    1) x k ( ) 8 f X ( 8 x k ( 8

(2.19)

k

1 

)

(

k ( )

k

(2.20)





 1)

x 10

f X ( 10

x 10

k

1 

Như vậy từ các phương trình (2.11) đến (2.20) cho đầy đủ các hàm động học

)    1) x k ( ) 9 f X ( 9 x k ( 9

.

2

1

k

1 

của véc tơ hàm

Tiếp theo xác định ma trận chuyển vị

bằng cách lấy đạo

, f ,..., ) f 1 f 10 F k X (

(

)

ˆ X X 



k

1 

hàm riêng

, nhận được [47]:

(2.21)

1,10



i

j

,

; 1,10









k

1 

   ij

 

  k 1  F  1 k  X 

f  i x 

j

j

   

   

trong đó,

f  i x 

/ 2

/ 2

/ 2

T

T

T

;

;

;

 

 

 

 12

 13

 14

1)) 1)) ( 1)) k       x 10 (  r x k ( 8 x k ( 9 (  p (  q

(

1))





T

/ 2

1)

T

/ 2

1)

T

/ 2

;

;

T

/ 2







  p

x k ( 8

 18

; 19  

x k 3(

; 110  

x k 4(

21

1)  kx 2 (

/ 2

/ 2

1)

/ 2

T

T

T

;

;

 



  r

23

 24

; 28   

x k 1(

( ( 1)) 1)) k     x 10 x k ( 9 (  q

1)

/ 2

1)

/ 2

/ 2

T

T

T

;

;





  q

  29

x k 4 (

; 210   

x k 3(

31

( 1))   x k ( 9

/ 2

1)

/ 2

T

T

;

T

/ 2

;

 



  p

 32

34

; 38   

x k 4 (

( 1)) ( 1)) k     x 10 (  r x k ( 8

1)

T

/ 2

1)

T

/ 2

;

T

/ 2

;

  





  r

39

x k 1(

; 310  

x k 2 (

41

( ( 1)) k   x 10

1)

/ 2

T

T

/ 2

;

T

/ 2

;



 

  q

42

 43

; 48  

x k 3(

( 1)) 1))     (  p x k ( 9 x k ( 8

46

1)

T

/ 2

1)

T

/ 2

  





49

x k 2 (

; 410   

x k 1(

; ii 1  ;

i

; 1,10

j

.

tất cả các





ij khác đều bằng 0,

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:

1,10

(2.22)

[ / 2 T 1)]  1)   1)    g 1 w x k ( 1 2 w x k ( 2 3 w x k ( 3 4

(2.23)

2

/ 2 g T 1)] [   1)   1)    w x k ( 1 1 w x k ( 3 3 w x k ( 2 4

(2.24)

3

/ 2 g T 1)] [   1)   1)    w x k ( 2 1 w x k ( 3 2 w x k ( 1 4

(2.25)

4

Lấy đạo hàm riêng

nhận được ma trận hệ số nhiễu [47]:

g  w 

,i j

(2.26)

1,3

,

i

; 1,10

j





G g  ij

 

 

trong đó,

[ / 2 g T 1)]  1)   1)    w x k ( 1 3 w x k ( 2 2 w x k ( 3 1

;

;

;

;

21

1) / 2 1) / 2 1) / 2 1) / 2 T T T g T          g 11 x k 2 ( g 12 x k 3( g 13 x k 4 ( x k 1(

;

;

;

;

22

23

31

32

1) / 2 1) / 2 1) / 2 1) / 2 g T g T g T g T            x k 4 ( x k 3( x k 4 ( x k 1(

;

;

;

;

33

41

42

43

các hệ số

ijg 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

1) / 2 1) / 2 1) / 2 1) / 2 g T g T g T g T            x k 2 ( x k 3( x k 2 ( x k 1(

sai của các con quay vi cơ

3

1

Vấn đề tiếp theo là tìm các hàm quan sát

ih . Như trên đã nêu trong ASWs có ba

, w w w . , 2

từ kế đo ba thành phần 1

2

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:

,

,

B B B (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

x

y

z

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

, z z , z trong hệ tọa độ gắn liền của véc tơ từ trường trái đất 3

2

x

y

z

n

bC , tức là:

, , , B B B có quan hệ mật thiết với nhau qua ma trận Côsin định hướng z z , 1 z và 3

47

T

(

,

T )

,

,

)



(2.27)

n T C ( ) ( b

B B B y z

x

z z , 1

2

z 3

n

Triển khai biểu thức (2.27) với ma trận

bC như (1.12) ta có:

(2

2

1)

(2

)

(2

)

(2.28)

y

2    1

2 0

2    0 3

1 2

2    0 2

1 3

B     B x B z z 1

(2

)

(2

2

1))

(2

2

)

(2.29)

y

2

2    0 3

1 2

2    2

2 0

   0 1

2 3

(2

)

(2

2

)

B

(2

2

1)









(2.30)

z 3

2    0 2

1 3

   0 1

2 3

2    3

2 0

B x

y

B z

Với cách đặt biến (2.1), từ (2.28) đến (2.30) ta có ba hàm quan sát:

)

(2 ( ) 2 ( ) 1)

( ) 2 ( )

( ))











y

( ) z k  1

2 x k 2

x k x k (2 ( ) 2

x k x k B 1 4

3

(2.31)

2 x k 1 ( ) 2 ( )

( ))

h X ( 1 



k x k x k (2 ( ) 2

B x x k x k B z 1 3

4

)

( ))

( ) 2 ( )

(2 ( ) 2 ( ) 1)

B













z k ( ) 2

2 x k 1

2 x k 3

y

(2.32)

x k x k B 1 4 x ( ))

h X ( 2 



x k x k (2 ( ) 2 3 k x k x k (2 ( ) ( ) 2 ( ) 3

4

x k x k B 1 2 z

)

( ) 2 ( )

( ))

( ) 2 ( )

( ))

z B     B x B z

( ) z k  3

( h X 3

k

(2 ( ) x k x k 2

4

(2 ( ) x k x k 3

4

y

(2.33)

(2 ( ) 2 ( ) 1)

    x k x k B 1 3 x x k x k B 1 2

2 x k 1

2 x k 4

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

g

,

g

,

g (các thành phần này có thể xác định bằng cách đọc

thành phần như sau

x

0

y

0

z

0

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à:

f

f

f

g













,

,

f

f

f

, trong đó

là hình chiếu gia tốc

a bx

N

; g a x

by

E

; g a y bz

D

z

N

E

D

,

,

tuyệt đối của ASWs xuống các trục của hệ tọa độ gắn liền, còn

g g g là hình z

x

y

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ì

0;

0;

0

f

f

f





 . Trong trường hợp không xét đến nhiễu đo, ba gia tốc kế đo

N

E

D

   B z

(

,

,

T )

(

,

,

T )

(

,

,

T )

z

. Khi đó có thể coi:

ba thành phần



g g g y

x

z

z 5

z 6

4

a bx

a by

a bz

(

z

,

T )

g

,

g

,

g

T )

(2.34)



z z , 5

x

y

z

6

4

0

0

0

n T C ( ) ( b

n

Triển khai biểu thức (2.34) với ma trận

bC như (1.12) và cách đặt biến như (2.1)

ta có:



48

)

(2 ( ) 2 ( ) 1)

g

( ) 2 ( )

( ))











0

x k x k (2 ( ) 2

x k x k g 1 4

3

0

x

y

(2.35)

( ) 2 ( )

2 x k 1 

( ) z k  4 

h X ( 4 k x k x k (2 ( ) 2

2 x k 2 x k x k g ( )) 1 3

4

0

z

)

( ) 2 ( )

( ))

(2 ( ) 2 ( ) 1)

g













z k ( ) 5

3

2 x k 3

2 x k 1

0

0

y

x

(2.36)

( ) 2 ( )

( ))

x k x k (2 ( ) 2 



h X ( 5 k x k x k (2 ( ) 3

x k x k g 1 4 x k x k g 1 2

4

0

z

( ) 2 ( )

( ))

( ) 2 ( )

( ))



) 







z k ( ) 6

h X ( 6

k

x k x k (2 ( ) 2

x k x k g 1 3

4

x

0

x k x k (2 ( ) 3

x k x k g 1 2

4

y

0

(2.37)

(2 ( ) 2 ( ) 1)

g







2 x k 1

2 x k 4

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ố 7 z

z z con quay vi cơ đo được sẽ là: 8

9

)

(2.38)

( ) z k 7

( h X 7

k

( ) x k 5

( ) x k 8

p

)



 





(2.39)

k

q b q

( ) z k 8

( h X 8

( ) x k 6

( ) x k 9

)

( ) k

p b     

(2.40)

k

( ) z k 9

( h X 9

( ) x k 7

Từ (2.31 – 2.33), (2.35 – 2.37) và (2.38 – 2.40) ta có đầy đủ 9 hàm quan sát

     r b r x 10

(

,...,

)

của véc tơ hàm

, h h 1 2

9

h h X . ( )k

Việc xác định ma trận đo

bằng cách lấy đạo hàm riêng

nhận

k

(

)



k

h  i x 

j

H

,

i

1,9;

j

1,10







được [47]:

.

(2.41)

h ij

k

 

 

trong đó,

;

4

4



 ( ) 2

 ( ) 2



 ( ) 2

 ( ) 2

x

y

z

x

y

z

h 11

B x k 1

B x k 4

B x k ( ) 3

h ; 12

B x k 2

B x k 3

B x k ( ) 4

;

2

2



 ( ) 2



 ( ) 2

y

z

z

h 13

B x k 2

B x k ( ) 1

h ; 14

B x k y 1

B x k ( ) 2

;

;

2

  2

 ( ) 4

 ( ) 2



( ) 2 

x

z

x

z

h 22

B x k 3

B x k ( ) 1

h 21

B x k 4

B x k y 1

B x k ( ) 2

;

;

2



 ( ) 4

 ( ) 2

2  

( ) 2 

x

z

x

y

z

h 24

B x k 1

B x k ( ) 3

h 23

B x k 2

B x k 3

B x k ( ) 4

;

2

2



 ( ) 2

 ( ) 4



 ( ) 2

x

y

z

x

h 31

B x k 3

B x k 2

B x k ( ) 1

h ; 32

B x k 4

B x k ( ) y 1

2

2



 ( ) 2



 ( ) 2

 ( ) 4

;

x

y

x

y

z

h 33

B x k 1

B x k ( ) 4

h ; 34

B x k 2

B x k 3

B x k ( ) 4

;

( );

2

( );

( );

2

( );

2

2  



2  





h 41

g x k 0 3

h 42

g x k 0 4

h 43

g x k 0 1

h 44

g x k 0 2

h 51

g x k ( ) 0 2

z

z

z

z

z

;

2

2

2

4

4











h 52

g x k ( ) 0 1

h ; 53

g x k ( ) 0 4

h ; 54

g x k ( ) 0 3

h ; 61

g x k ( ) 0 1

h ; 64

g x k ( ) 0 4

z

z

z

z

z











 ; tất cả các

h 1010

h 99

h 88

h 710

h 69

h 58 1

ijh khác đều bằng 0.

H  h  k ˆ X X X  

49

Q

Lọc EKF

R

Hiệu chỉnh ma trận kK

3 con quay vi cơ

(

,...,

)T

Các phương tiện đo

kZ

ˆ ( ) X 

k

(

)



3 từ kế

ˆ( X

)

kF

1 

kK

k

1 

ˆ ( ) X 

k

ˆ kZ

3 gia tốc kế

(

)

ya

)

xa

za

ˆ( kh X 

k

 z 1 z 9

ˆ ( ) X 

k

n

bC

M

,

,

,       3

1

0

2

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









 

 arctg

1 3

arctg





   (2 arcsin    ) 2 2    0 3 1 2 2 2 2 1 2     0 1 2    0 2 

DV

g

DV

z

(0)



(0) 

Df

2 2    0 1 2 3 2 2 2 1 2     0 3      





EV

EV

(0)y

(0) 

Ef

z





NV

NV

(0)x

(0) 

Nf

x





y

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ó:

X

X

X

(

(

(

(

(

;

;

;

;

)k

)k

)k

)kX

)kX

1

2

2

3

4

; 5 z

(

z

)

z

)









z z           v T v T v T z 1 h 1 h 2 z 3 h 3 h 4 w 4 h 5 w 5

;

.

;

;

)kX

7

h X ( 7

k

w 1

)kX

w 2

9

h X ( 9

k

w 3

6

8 (

z   h  h 6 w 6 z 8

50

trong đó,

,

,

w w w là nhiễu đo của ba gia tốc kế và

,

,

5

4

6

1

3

2

v T

v T

v là nhiễu đo ba từ kế; T

,

,

w w w là nhiễu đo của ba con quay vi cơ.

1

2

3

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ố (

đã được xác

k

k

k

k

1 

1 

1 

1 

định) để thực hiện thủ tục lọc Kalman (hình 2.4). Kết quả ước lượng theo lưu đồ

,

,

,

    , từ đó xác định 3 góc định hướng

0

1

2

3

giải thuật bộ lọc Kalman phi tuyến mở rộng nhận được 4 tham số Rodrig-Hamilton ,  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

(2.42)

M



0 1 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0

0 0 0 1 0 0 0 0 0 0

     

     

(

)

(



( X ), ( ( ), Q R , F k h X G X ), k

Hàm

được thiết lập từ các phương trình (2.11-2.20), trong đó

)  là

kF

1 

k

k

1 

1 

(

)

ˆ X ˆ( X )

1k  . Hàm

được thiết lập từ các

véc tơ trạng thái ước lượng ở bước thứ

k

(

)

) ˆ( kh X 

phương trình (2.31-2.33) và các phương trình (2.35-2.40), trong đó

k

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

ˆ X  là véc tơ

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ó:

(2.43)

bx

x

by

bz

z

y

Thay các phương trình (2.43) vào các phương trình (1.28) ta có:

a a ; a a ; a a       w 4 w 5 w 6

N

y

z

x

2    1

2 0

1 2

1 3

f =(2 2 ) (2 )(a ) (2 )(a ) 1)(a       w 4 2    0 3 w 5 2    0 2 w 6

(2.44)

x

E

z

y

1 2

2    2

2 0

2 3

f =(2 )(a ) (2 2 ) (2 2 )(a )   1)(a     2    0 3 w 4 w 5    0 1 w 6

x

y

D

z

1 3

2 3

2    3

2 0

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 )(a ) (2 2 )(a ) (2 2 )     1)(a   2    0 2 w 4    0 1 w 5 w 6     

N

y

z

x

2    1

2 0

1 2

1 3

(2.45)

2 (2 )a (2 )a 1)a     V = =(2 f N 2    0 3 2    0 2

2    1

2 0

1 2

1 3

(2 2 (2 ) (2 )  1)    w 4 2    0 3 w 5 2    0 2 w 6

 V = =(2 f E

E

x

z

y

1 2

2    2

2 0

2 3

(2.46)

)a (2 2 (2 2 )a  1)a   2    0 3    0 1

1 2

2    2

2 0

2 3

g

=(2

)a

(2

)a

(2

2







1)a 

 V = f D

D

x

y

z

2    0 2

1 3

2    0 1

2 3

2    3

2 0

(2.47)

(2

)

(2

2

(2

2







1) 

2    0 2

1 3

w 4

)    0 1

2 3

w 5

2    3

2 0

w g  6

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

;

;

;

;

;

b















x 4

x 5

p

x 6

b q

x 7

b r

(2.48)

;

;

;

z







 3 

x 1 x 8

 0 V N

x 2 x 9

;  1 V E

x 3 x 10

 2 V D

x 11

X

(

,

,

,

,

,

,

,

,

,

)T



Biến X khi đó là véc tơ

x x x x x x x x x x , 1 5

2

3

4

6

7

8

9

10

x 11

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

(  p

(  r

(2.49)

x w  5 1 )

(  q )

)





x 3 



x 4 

 

x w  7 3 



x 2 (  p

x 2

x 3

x 5

(  q

x 6

x w  2 6 x  4

(  r

x 7

x w x w x w 2 3 3

1

2

4

(2 ) (2 2 (2 2 )   1)   2    0 3 w 4 w 5    0 1 w 6

52

2

)

)

)













(2.50)

(  r )

 x 2 

(  p 



x 3 



x 4 ) 

x w  6 2 



x 1 (  p

x 1

x w  5 1 ( )  r

x 3

x 8

x 10

x w  3 7 x  4

(  q

x 9

(  q x w x w x w 1 1 3 2

3

4

2

)

)

)













(  p

(2.51)

(  r )

 x 3 

(  q 



x 2 



x 4 ) 

x w  5 1 



x 1 (  q

x 1

x w  6 2 ( )  r

x 2

x 9

x 10

x w  3 7 x  4

(  p

x 8

x w x w x w 1 2 1

2

3

4

2

)

)

)













(  p

(2.52)

(  q )

)

 x 4 

(  r 



x 2 



x 3 

x w  5 1 



x 1 (  r

x 1

x w  3 7 x ) 2

x 10

(  q

x 9

x w  2 6 x  3

(  p

x 8

x w x w x w 1 2 1

3

2

3

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:

1)a

(2

2

)a

(2

2

)a













y

z

x

 x 8

2 x =(2 1

2 x 2 2

x x 2 3

x x 1 4

x x 2 4

x x 1 3

(2.53)

2

1)

(2

2

(2

2















2 (2 x 1

2 x 2

w 4

x x 2 3

) x x w 1 4 5

x x 2 4

) x x w 1 3 6

=(2

2

)a

1)a

(2

2

)a













x

z

y

 x 9

x x 2 3

x x 1 4

2 x (2 1

2 x 2 3

x x 3 4

x x 1 2

(2.54)

(2

2

1)

(2

2















x x 2 3

) x x w 1 4 4

2 (2 x 1

2 2 x 3

w 5

x x 3 4

) x x w 1 2 6

=(2

2

)a

(2

2

)a











x

y

z

 x 10

x x 2 4

x x 1 3

x x 3 4

x x 1 2

2 x (2 1

2 2 1)a x  4

(2.55)

(2

2

(2

2













x x 2 4

) x x w 1 3 4

x x 3 4

) x x w 1 2 5

2 (2 x 1

2 2 1) x  4

w g  6

,

,

,

,

,

w w w w w w trong các phương trình vi

Tách các số hạng có thừa số nhiễu

1

2

3

4

5

6

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

,

f

,

f

,

f

,

f

,

f

,

:

phương pháp Ơle ta có các hàm động học

f 1

2

3

4

8

9

f 10

k

)

(

1)

1)

1)





1)  



1)  







f x ( 1 k

x k ( ) 1

x k ( 1

x k ( 2

x k ( 2

x k ( 5

1 

 

   p

T 2

(2.56)

k

k

(

1)

1)

1)

(

1)

1)

1)



1)  







1)  











x k ( 3

x k ( 3

x k ( 6

x k ( 4

x k ( 4

x k ( 7

  r

   q

 

T 2

T 2

)

(

1)

1)

1)

k





1)  



1)  





( f x 2 k

( ) x k 2

( x k 2

( x k 1

( x k 1

( x k 5

1 

   p

 

T 2

(2.57)

k

k

(

1)

1)

1)

(

1)

1)

1)





1)  







1)  









  r

x k ( 3

x k ( 3

x k ( 7

x k ( 4

x k ( 4

x k ( 6

   q

 

T 2

T 2

)

(

1)

1)

1)

k





1)  



1)  





( f x 3 k

( ) x k 3

( x k 3

( x k 4

( x k 4

( x k 5

1 

   p

 

T 2

(2.58)

(

1)

1)

1)

(

1)

1)

1)

k

k





1)  







1)  









  r

( x k 2

( x k 2

( x k 7

( x k 1

( x k 1

( x k 6

   q

 

T 2

T 2

(

1)

1)

1)

k



1)  



1)  







( f x 4 k

1

( ) x k 4

( x k 4

( x k 1

( x k 1

( x k 7

  r

)  

T 2

(2.59)

(

1)

1)

1)

(

1)

1)

1)

k

k





1)  







1)  







( x k 2

( x k 2

( x k 6

( x k 3

( x k 3

( x k 5

   q

 

 

   p

T 2

T 2

53

)

1)

T





1)  

 



a k ( x

f x ( k 8

x k ( 8

2 x k 2 ( 1

2 x k 1) 2 ( 2

1 

T

1)

1)

1)

1)



   







(2.60)

x k 2 ( 2

x k ( 3

x k 1) 2 ( 1

x k ( 4

T

1)

1)

1)

1)





 







x k ( ) 8  

 1) 1     a k (  y 

a k ( z

x k 2 ( 2

x k ( 4

x k 1) 2 ( 1

x k ( 3

T

)

1)





1)  

 



a k ( y

f x ( k 9

x k ( 9

2 x k 2 ( 1

2 x k 1) 2 ( 3

1 

T

1)

1)

1)

1)



   







(2.61)

a k ( x

x k 2 ( 2

x k ( 3

x k 1) 2 ( 1

x k ( 4

T

1)

1)

1)

1)





 







x k ( ) 9  

 1) 1      

a k ( z

x k 2 ( 3

x k ( 4

x k 1) 2 ( 1

x k ( 2

2 x k 2 ( 1

1 

( ) k ( ) ( k T 1)   1)        x k a k ( z f 10 x 10

(2.62)

T 1) 1)  1) 1  1) 1)      a k ( x   1) 2 ( x k   1 x k ( 3 x k 2 ( 2 x k ( 4

2 x k 1) 2 ( 4  

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

T 1) 1) 1) Tg       1)   x 10   a k ( y x k ( 2 x k 2 ( 3 x k ( 4 x k 1) 2 ( 1

Trong trường hợp

với

q

r

k

j

1

, , ) b b b biến đổi chậm các hàm động học p f x  (

được coi:

5, 7 j 

(2.63)

1

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

được viết dưới dạng rời rạc:

11f

  1) f x ( j k x k ( ) j x k ( j )  

(

)



10

f 11

(2.64) x  k 1 Như vậy các phương trình từ (2.56) đến (2.64) cho đầy đủ các hàm động học

k ( ) Tx 1) k k ( ( 1)     x 11 x 11

.

2

1

k

1 

của véc tơ hàm

Tiếp theo xác định ma trận chuyển vị

bằng cách lấy đạo

, f ,..., ) f 1 f 11 F k X (

(

)

ˆ X X 



k

1 

, nhận được [47]:

(2.65)

hàm riêng

1,11



i

j

,

; 1,11









k

1 

   ij

 

f  i x 

f  i x 

j

j

   

   

  1 k  F  1 k  X 

;

;

 13

q k (

trong đó,  p k (    12



  



1)

/ 2

1)

/ 2;

1)

/ 2;

T

T

T

1)  







;

  











 14

r k (

x k ( 7

  15

x k 2 (

  16

x k 3(

1)

/ 2

T

1) T / 2 1) T / 2 1)    1)    x k ( 5 x k ( 6



;

;





  17

x k 4 (

21

p k (

  



1) T / 2 1)    x k ( 5

;

;

  



 24

q k (

23

r k (

  



1)

/ 2

1)

/ 2

1)

/ 2

T

T

T

  





  



;

;

;













25

x k 1(

  26

x k 4 (

27

x k 3(

1) T / 2 1)    1) T / 2 1)    x k ( 6 x k ( 7

54

1)

/ 2

T

1)  



;

;

  



31

q k (

 32

r k (

x k ( 7

  



1) T / 2 1)    x k ( 6

  

;

34

p k (

; 35   

; 36

  



1)

/ 2

T

1)  



1) T / 2 1) T / 2 1) T / 2 1)      x k ( 5 x k 4 ( x k 1(

1)

T

/ 2

1)  



;

;

  



41

r k (

x k ( 7

  37

q k (

42

x k ( 6

 ;  



1)

T

/ 2

1)  



1) T / 2  x k 2 (

  

;

;

;

p k (

 43

x k ( 5

  45

46

  



1) T / 2 1) T / 2   x k 3( x k 2 (

  

;

47

1) T / 2  x k 1(

  81

1) 1) 2 1) 1) 2 1)         ; 1) a k ( x a k ( y a k ( z Tx k 4 ( 1 Tx k ( 4 Tx k ( 3

  82

1) 1) 2 1) 1) 2 1)         ; 1) Tx k 4 ( 2 a k ( x Tx k ( 3 a k ( y Tx k ( 4 a k ( z

  83

1) 1) 2 1)      ; 1) a k ( y a k ( z Tx k 2 ( 2 Tx k ( 1

84

1) 1) 2 1)      ; 1) Tx k 2 (    1 a k ( y Tx k ( 2 a k ( z

  91

1) 1) 4 1) 1) 2 1)         ; 1) a k ( x a k ( y a k ( z Tx k 2 ( 4 Tx k ( 1 Tx k ( 2

  92

1)

1) 4

1)

1) 2

1)



 



 



 ; 1)

a k ( x

a k ( y

a k ( z

  93

Tx k 2 ( 2

Tx k ( 3

Tx k ( 4

1) 1) 2 1)      ; 1) Tx k 2 ( 3 a k ( x Tx k ( 1 a k ( z

  94

1) 1) 2 1)      ; 1) Tx k 2 ( 1 a k ( x Tx k ( 3 a k ( z

101

1) 1) 2 1) 1) 4 1)         ; 1) a k ( x a k ( y a k ( z Tx k 2 (    3 Tx k ( 2 Tx k ( 1

  102

1) 1) 2 1)      ; 1) Tx k 2 ( 4 a k ( x Tx k ( 1 a k ( y

103

1) 1) 2 1)      ; 1) Tx k 2 (    1 a k ( x Tx k ( 4 a k ( y

  104

1,11

1,

i

, còn tất cả các



  ii

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

1) 1) 2 1) 1) 4 1)         ; 1) Tx k 2 ( 2 a k ( x Tx k ( 3 a k ( y Tx k ( 4 a k ( z

1

2

3

4

5

6

, , , , , w w w w w w được viết lại dưới dạng rời rạc theo phương pháp Ơle:

(2.66)

1) / 2 1) / 2 1) / 2       g 1 x k ( 2 w T 1 x k ( 3 w T 2 x k ( 4 w T 3

(2.67)

2

g 1) / 2 1) / 2 1) / 2        x k ( 1 w T 1 x k ( 4 w T 2 x k ( 3 w T 3

(2.68)

3

g 1) / 2 1) / 2 1) / 2        x k ( 4 w T 1 x k ( 1 w T 2 x k ( 2 w T 3

(2.69)

4

g 1) / 2 1) / 2 1) / 2       x k ( 3 w T 1 x k ( 2 w T 2 x k ( 1 w T 3

55



8

2 x k 2 ( 1

2 x k 1) 2 ( 2

(2.70)

g T 1) 1) 1)            w T  4 x k ( 3 x k 1) 2 ( 1 x k ( 4 w 5

 x k ( 3

T 1) 1) 1)       x k 2 ( 2     x k 2 ( 2 x k ( 4  1) 1  x k 1) 2 ( 1 w 6

9

2 x k 2 ( 1

2 x k 1) 2 ( 3

(2.71)

g T 1) 1) 1)            x k ( 3 x k 1) 2 ( 1 x k ( 4 w 5    1) 1 

 x k 2 ( 2 

 w T  4  1)

T

1)

1)

1)

 

 

 



 







g 10

2 x k 2 ( 1

2 x k 1) 2 ( 4

w T  6

x k ( 4

x k 1) 2 ( 1

x k ( 3

w 4

(2.72)

T

1)

1)

1)



 





x k 2 ( 2 

   x k 2 ( 3

 1) 1  x k 1) 2 (  1

 x k ( 2

w 5

x k ( 4

Lấy đạo hàm riêng

nhận được ma trận hệ số nhiễu [47]:

g  w 

T 1) 1)       x k 2 ( 3 x k ( 4 x k 1) 2 ( 1 x k ( 2 w 6

(2.73)

G

1,6

i

j

,

; 1,11









g ij

 

 

g  i w 

j

   

   

trong đó,

,i j

T

T

T

g

T













 



;

;

;

;

g 11

x k 2 (

g 12

x k 3(

g 13

x k 4 (

x k 1(

21

1) / 2 1) / 2 1) / 2 1) / 2

g

T

g

T

g

T

g

T





 



 



 



;

;

;

;

22

x k 4 (

x k 3(

23

x k 4 (

31

x k 1(

32

1) / 2 1) / 2 1) / 2 1) / 2

g

T

g

T

g

T

g

T









 



 



;

;

;

;

33

x k 2 (

x k 3(

41

x k 2 (

42

x k 1(

43

1) / 2 1) / 2 1) / 2 1) / 2

g

g

 

 

 

 



 





T x k (2 ( 2

x k ( 3

x k 1) 2 ( 1

x k ( 4

85

84

2 T x k (2 ( 1

2 x k 1) 2 ( 2

1) 1); 1) 1) 1));

g

 



 





86

T x k (2 ( 2

x k ( 4

x k 1) 2 ( 1

x k ( 3

1) 1) 1));

g

g

 



 





 

 

 

94

T x k (2 ( 2

x k ( 3

x k 1) 2 ( 1

x k ( 2

95

2 T x k (2 ( 1

2 x k 1) 2 ( 3

1) 1) 1)); 1) 1);

g

 



 





96

T x k (2 ( 3

x k ( 4

x k 1) 2 ( 1

x k ( 3

1) 1) 1));

 



 





1) 1) 1));

 



 





 

 

 

g 104 g 105

T x k (2 ( 3 T x k (2 ( 3

x k ( 4 x k ( 4

x k 1) 2 ( 1 x k 1) 2 ( 1

x k ( 2 x k ( 2

g 106

2 T x k (2 ( 1

2 x k 1) 2 ( 4

Các hệ số

ijg 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ế.

1) 1) 1)); 1) 1);

z z ,

Vấn đề tiếp theo là tìm các hàm quan sát. Các hàm quan sát 1

2

z do ba từ kế 3

,

đo được như trường hợp ASWs rơi trong khí quyển đã trình bày, tức

z z , 1

2

z xác 3

đị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

DVL

,

,

V

V

z



sát

.

4

z 5

z , vận tốc đo được từ vận tốc kế cho chỉ số 6

DVL V ( x

DVL y

DVL T V ) z

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:

T

,

,

,

)

V



(2.74)

DVL V ( x

DVL y

DVL T V ) z

n T C ) ( ( b

V V V , D

N

E

n

Triển khai biểu thức (2.74) với ma trận

bC như (1.12). Với cách đặt biến như

(2.48), ba hàm quan sát có được là:

, ,













k

h X ( 4

2 x k 2

x k x k (2 ( ) 2

x k x k x k ( ) 1 4

9

3

(2.75)

) (2 ( ) 2 ( ) 1) ( ) 2 ( ) ( ))

k ( )





2 z k x k ( ) 4 1 x k x k (2 ( ) ( ) 2 ( ) 2

x k ( ) 8 x k x k x 1 3

10

4

)

( ))

( ) 2 ( )

(2 ( ) 2 ( ) 1)















k

h X ( 5

8

2 x k 1

2 x k 3

x k ( ) 9

(2.76)

x k x k x k ( ) 1 4 ( ))

k ( )





z k x k x k ( ) (2 ( ) 5 2 3 x k x k (2 ( ) ( ) 2 ( ) 3

4

x k x k x 1 2

10

( ))







k

h X ( 6

(2.77)

8 (2 ( ) 2 ( ) 1)

k ( )











z k x k x k ( ) (2 ( ) 6 2 4 x k x k (2 ( ) ( ) 2 ( ) 3

x k x k x k ( ) 1 3 x k x k x k ( )) ( ) 1 2

2 x k 1

2 x k 4

9

4

x 10

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

7z

) ( ) 2 ( ) ( ))

k ( )





(2.78)

k

h X ( 7

z k ( ) 7

x 11

)

h

Từ (2.31 – 2.33) và (2.75 – 2.78) ta có đầy đủ 7 hàm quan sát

của

h h , 1 2

7

véc tơ hàm

h X . ( )k

H



Việc xác định ma trận đo

bằng cách lấy đạo hàm riêng

k

(

)



k

h  k ˆ X X X  

h  i x 

j

( ,..., )

H

i

j







.

(2.79)

nhận được [47]:

h ij

k

 

 

trong đó,

, 1, 7; 1,11

j





như trường hợp ASWs rơi trong khí quyển đã xét;

ijh i ,

1,3; 1, 4

k ( )







;

h 41

x k x k 4 ( ) 1

x k x k 4

x k x 3

8

9

10

( ) 2 ( ) ( ) 2 ( )

k ( )

k ( )











;

;

h 42

x k x k 4 ( ) 2

x k x k 3

x k x 4

8

9

10

h 43

x k x k 2 ( ) 2

x k x 1

9

10

( ) 2 ( ) ( ) 2 ( ) ( ) 2 ( )

k ( )









;

 ;

h 44

x k x k 2 ( ) 1

x k x 2

9

10

h 48

2 x k 1

2 x k 2

( ) 2 ( ) 2 ( ) 2 ( ) 1









;

;

h 49

x k x k 2 ( ) 2

x k x k ( ) 1

3

4

h 410

x k x k 2 ( ) 2

x k x k ( ) 1

4

3

( ) 2 ( ) ( ) 2 ( )

k ( )

k ( )











;

;

h 51

x k x k 4 ( ) 1

x k x k 4

x k x 2

9

8

10

h 52

x k x k 2 ( ) 3

x k x 1

8

10

( ) 2 ( ) ( ) 2 ( ) ( ) 2 ( )

k ( )

k ( )







 



;

h 53

x k x k 2 ( ) 2

x k x k 3

x k x 4

8

9

10

; 54 h

x k x k 2 ( ) 1

x k x 3

8

10

( ) 4 ( ) ( ) 2 ( ) ( ) 2 ( )

57









;

 ;

h 58

x k x k 2 ( ) 2

x k x k ( ) 1

3

4

h 59

2 x k 3

2 x k 1

( ) 2 ( ) 2 ( ) 2 ( ) 1

k ( ) 2 ( )











;

;

h 510

x k x k 2 ( ) 3

x k x k ( ) 1

2

4

h 61

x k x 4 ( ) 1

10

x k x k 3

x k x k ( ) 2

8

9

( ) 2 ( ) ( ) 2 ( )









;

;

h 62

x k x k 2 ( ) 4

x k x k ( ) 1

8

9

h 63

x k x k 2 ( ) 1

x k x k ( ) 4

9

8

( ) 2 ( ) ( ) 2 ( )

k ( )







;

h 64

x k x k 2 ( ) 2

x k x k 3

x k x 4

9

8

10

( ) 2 ( ) ( ) 4 ( )









;

h 68

x k x k 2 ( ) 2

x k x k ( ) 1

4

3

; 69 h

x k x k 2 ( ) 3

x k x k ( ) 1

4

2

( ) 2 ( ) ( ) 2 ( )





 tất cả các

ijh khác đều bằng 0.

h 610

2 x k 4

2 x k 1

 ; 711 1. h

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ó:

(

X

(

X

(

X

(

X

(

X

z

z





















;

;

;

;

;

)k

)k

)k

)k

)k

v T

v T

v T

v V

v V

z 1

h 1

1

2

h 2

2

z 3

h 3

3

4

h 4

1

z 5

h 5

2

(

X

2 ( ) 2 ( ) 1

z

z

v









;

.

)k

v V

k

z

6

h 6

3

7

h X 7 (

)

,

,

trong đó,

là nhiễu đo của vận tốc kế

v V

v V

v V

1

2

3

1

3

2

v T

v T

v là nhiễu đo của 3 từ kế; T

và

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

, ,

X

Q R ,

Như vậy đã có đầy đủ yếu tố (

đã được xác

F k

h X G X ), k

k

k

k

k

1 

1 

1 

1 

đị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

,

,

,

,

,

    , các thành phần vận tốc

V V V và độ sâu của ASWs trong hệ tọa độ

0

1

2

3

N

E

D

địa lý. Từ đó xác định được 3 góc định hướng

)

độ tâm khối của ASWs trong mặt phẳng ngang ( ,

trình vi phân ở biểu thức (1.23) theo sơ đồ hình 2.5, trong đó các ma trận

,  theo công thức (1.15) và tọa , x y bằng cách giải hai phương ,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 

(2.81)

 0 0 0 0 0 0 0 0 0 0 1

      

0 0 0 0 0 0 0 1 0 0 0

.

(2.82)

0 0 0 0 0 0 0 0 1 0 0

S

0 0 0 0 0 0 0 0 0 1 0

     

    

( ), ( ( ),

58

Q

R

Lọc EKF Các phương tiện đo

phương tiện đo quán tính

Hiệu chỉnh kK ma trận

3 con quay vi cơ 3 gia tốc kế

z



z 1

7

kZ

k

(

)



kF

1 

kK

k

1 

k

ˆ kZ

(

)

( ,..., )T ˆ ( ) X  ) ˆ( X 3 từ kế 3 vận tốc kế ˆ ( ) X 

k

) ˆ( kh X  Cảm biến áp suất phương tiện đo không quán tính







arctg

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

k

1

0

2





 

   (2 arcsin

   )

M

1 3



arctg





ˆ ( ) X  , , ,       3 2 2    0 3 1 2 2 2 2 2 1     0 1 2    0 2

  

  

N

z

2 2    0 1 2 3 2 2 2 1 2     0 3



EV

y



EV

(0)y



S

NV

x



NV   DV

(0)x

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

(

)

(



được thiết lập từ các phương trình (2.56-2.64), trong đó

)  là

Hàm

kF

1 

k

k

1 

1 

(

)

) ˆ( X ˆ X

1k  . Hàm

được thiết lập từ các

véc tơ trạng thái ước lượng ở bước thứ

k

(

)

) ˆ( kh X 

phương trình (2.31-2.33) và các phương trình (2.75-2.78), trong đó

k

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

ˆ X  là véc tơ

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

n

tham số dẫn đường cho ASWs cần phải xác định ma trận Côsin định hướng

dC 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

n

n

dC cũng cho phép xác định được ma trận Côsin định hướng

bC 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).

n

Việc xác định ma trận Côsin định hướng

dC đượ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 Y Z )  Hệ tọa độ gắn liền với ASWs (

OX Y Z )  hệ tọa độ gắn

D D D

b b

b

liền với phương tiện mang (

OX Y Z )  hệ tọa độ địa lý (

m m m

0 0 0

OX Y Z ) (hình 2.6).

mX

0X   (Bắc)

bX

DY

mY

DX

bY

O

0Y   (Đô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 OZ OZ OZ vuông góc với mặt phẳng của trang

D

0

b

m

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à

n C C C , khi đó: m

m b

b d

C



(2.83)

n d

b m n C C C m b d

, ,

Từ 3 góc

1

2

3

b

trận

dC tương tự biểu thức (1.5) như sau:

(2.84)

sin sin



cos sin 1 sin sin 1

1

cos sin 1 sin sin 1

sin sin   3 1 sin cos   3

1

b dC

cos cos   2 1 sin cos   1 2 sin   2

sin cos       3 3 1 2 cos cos       3 2 3 sin cos   3

2

cos     2 3 cos     3 2 cos cos   3

2

    

    

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

m

m

bC được xác định một lần khi thiết kế vị trí và phương treo ASWs, tức là

bC 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 –

n

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

mC .

n

Tuy nhiên phương pháp đo trực tiếp ma trận định hướng

dC 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

n

xác định ma trận Côsin định hướng

dC 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

n

quán tính để thực hiện nhiệm vụ xác định ma trận Côsin định hướng

dC .

, ,   do 3 cảm biến góc của thiết bị đế cung cấp sẽ xác định ma

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.85)

Trong miền số thực các tham số Rodrig-Hamilton được viết:





(2.86)

 2         0 2

D

N

E

3

1

 



(2.87)

D

N

E

 2         1 3

0

2





(2.88)

D

N

E

 2         2 0

1

3

 



(2.89)

D

N

E

 2         3 1

2

0

0X

eZ

N   NV

EV

0Y

E

DV

D   0Z 

O

R

eY

l

eX

Đường xích đạo

2   

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]:

cos

sin

U

tg

U





 





(2.90)

; 

;

 N

 D

   E

V N R

V E R

V E R



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















(2.91)

V N

V E

V D

x 1

 0

x 2

x 3

 2

x 4

 3

x 5

x 6

x 7

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:

; ; ; ; ; ;  1

U (

U (









tg   



(2.92)

 x 1

x 2

x 3

x 4

x 6 R

x 6 R

x 5 R

2 cos ) sin )

U ( cos

 

 



(2.93)

 x 2

x 1

x U  3

x 4

x 6 R

x 5 R

x 6 tg    R

2 ) ( sin )

U (  

tg   



 

(2.94)

 x 3

x 1

x U  2

x 4

x 6 R

x 5 R

x 6 R

2 sin ) ( cos )

62



tg   

 

(2.95)

 x 4

x U (  1

x U  2

x 3

x 5 R

x 6 R

x 6 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:

n













(2.96)

x

n dx

y

n dy

z

n dz

w n ; 4

w n ; 5

w 6

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

2 sin ) ( cos )











(2.97)

n dx

x

dy

y

z

n w n 4

n w n 5 dz

n w  6

; ;

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

trong đó,

4

5

6

n

Viết lại (1.21) với

dC có dạng (1.12):

, ,

f







(2.98)

N

n 1) dx

n dy

n dz

2    1

2 0

1 2

1 3

=(2

)

(2

2

(2

2

)

f







(2.99)

2    0 3

1 2

2    2

2 0

   0 1

2 3

E

n dx

n 1) dy

n dz

=(2

)

(2

2

)

(2

2

f







(2.100)

D

n dx

n dy

n 1) dz

2    0 2

1 3

   0 1

2 3

2    3

2 0

=(2 2 (2 ) (2 ) 2    0 3 2    0 2

, thay các phương trình (2.97) vào các phương trình

Đặt

3

1

w w w 5 6

4

(2.98) đến (2.100) ta có:

=(2

2

(2

2

)

)(

)

(2

)(

)

f

n

n

n

1)( 













(2.101)

N

y

z

x

2    1

2 0

   1 2 0 3

1

 2

2    0 2

1 3

 3

=(2

)(

2

)

)

(2

2

)(

)

f

n

n

n







1)( 







(2.102)

E

x

z

y

2    0 3

1 2

2 (2    2

2 0

1

 2

   0 1

2 3

 3

=(2

)(

(2

2

)

)(

)

(2

2

)

f

n

n

n











1)( 



(2.103)

2    0 2

1 3

   2 3 0 1

1

 2

2    3

2 0

 3

D

x

y

z

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

)(

)

n 1)(

n

n















(2.104)

 V =(2 N

y

z

x

2    1

2 0

   1 2 0 3

1

 2

2    0 2

1 3

 3

)(

2

)

1)(

)

(2

2

)(

)

n

n

n















(2.105)

 V =(2 E

x

z

y

2    0 3

1 2

2 (2    2

2 0

1

 2

   0 1

2 3

 3

)(

(2

2

)

)(

2

)

1)(

)

n

g



















(2.106)

 V =(2 D

n x

y

n z

2    0 2

1 3

   2 3 0 1

1

2 (2    3

2 0

2

 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:

) , ( , , ) ( ,     2

n

n

n

















y

z

x

 x 5

2 x 1

2 x 2

 1

x x 2 3

x x 1 4

 2

x x 2 4

x x 1 3

 3

(2 2 1)( )( )( ) ) 2(  ) 2( 

n















(2.107)

x

y

z

2 x 1

2 x 2

x x 2 3

x x n 1 4

x x 2 4

x x n ) 1 3

(2 2 1) 2( ) 2(















2 x 1

2 x 2

 1

x x 2 3

x x 1 4

x x 2 4

x x 1 3

(2 2 1) 2( 2( )  2 )  3

63

(2

2

1)(

n

)(

n

)(

n

)









) 2( 





) 2( 





x

z

y

 x 6

2 x 1

2 x 3

 2

x x 2 3

x x 1 4

 1

x x 3 4

x x 1 2

 3

(2

2

1)

n

2(

)

2(

)















(2.108)

x

z

y

2 x 1

2 x 3

x x 2 3

x x n 1 4

x x 3 4

x x n 1 2

(2

2

1)

2(

2(

 













2 x 1

2 x 3

 2

x x 2 3

x x 1 4

)  1

x x 3 4

x x 1 2

)  3

n

n

n

g



















x

y

z

 x 7

2 x 1

2 x 4

 3

x x 2 4

x x 1 3

 1

x x 3 4

x x 1 2

 2

(2 2 1)( )( )( ) ) 2(  ) 2( 

n

g

















(2.109)

z

x

y

2 x 1

2 x 4

x x 2 4

x x n ) 1 3

x x 3 4

x x n 1 2

(2 2 1) 2( 2( )

 













2 x 1

2 x 4

 3

x x 2 4

x x 1 3

x x 3 4

x x 1 2

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

(2 2 1) 2( 2( )  1 )  2

f

f

động học

f 1

2

3

f : 4

1)

)

)

1)

[( cos U





1)  

 



k

( f X 1

( ) x k 1

( x k 1

( x k 2

1 

(2.110)

T 2 1)

1)

sin

1)]

U



1)  



tg   

( x k 3

( x k 4

( x k  6 R

x k (  6 R ( x k  5 R

  

  

, , ,



U











k

f X ( 2

x k ( ) 2

x k ( 2

x k ( 1

1 

T 2

(2.111)

1) ) cos ) 1) 1)   [ ( 

x k ( 6 R 1)





U

tg  







( x k 3

x k ( 4

x k ( 5 R

x k ( 6 R

  

  

1) sin 1)] 1)  



U





tg   



k

f x ( 3

x k ( ) 3

x k ( 3

x k ( 1

1 

T 2

x k ( 6 R

(2.112)

1) ) sin ) 1) 1)   [ ( 





U









x k ( 2

x k ( 4

x k ( 5 R

x k ( 6 R

  

  

1) 1) cos 1)] 1)  





tg   



k

f X ( 4

x k ( ) 4

x k ( 4

x k ( 1

x k ( 2

1 

T 2

x k (  5 R

x k (  6 R

 U  

  

(2.113)

1) 1) ) ) sin 1) [ 1)   1)  



 



x k ( 3

x k (  6 R

 U  

  

1) cos 1)]

Tách các số hạng có thừa số nhiễu

1

2

3

(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ó

, ,   trong các phương trình vi phân

f

f

các hàm động học

5

6

f : 7

, ,

T





 



n k ( x

f x ( 5 k

x k ( 5

2 x k 2 ( 1

2 x k 1) 2 ( 2

1 

) 1) 1)  

T



   







(2.114)

n k ( y

x k 2 ( 2

x k ( 3

x k 1) 2 ( 1

x k ( 4

T

1) 1) 1) 1)





 







x k ( ) 5  

 1) 1      

n k ( z

x k 2 ( 2

x k ( 4

x k 1) 2 ( 1

x k ( 3

1) 1) 1) 1)

64

T





 



n k ( y

f x ( 6 k

x k ( 6

2 x k 2 ( 1

2 x k 1) 2 ( 3

1 

) 1) 1)  

T









   

(2.115)

n k ( x

x k 2 ( 2

x k ( 3

x k 1) 2 ( 1

x k ( 4

1) 1) 1) 1)

T





 







n k ( z

x k 2 ( 3

x k ( 4

x k 1) 2 ( 1

x k ( 2

1) 1) 1) 1)

T





 



n k ( z

f x ( 7 k

x k ( 7

2 x k 2 ( 1

2 x k 1) 2 ( 4

1 

) 1) 1)  

T









   

(2.116)

n k ( x

x k 2 ( 2

x k ( 4

x k 1) 2 ( 1

x k ( 3

T

Tg

1) 1) 1) 1)



 





x k ( ) 6   x k ( ) 7  

 1) 1        1) 1      

n k ( y

x k 2 ( 3

x k 1) 2 ( 1

x k ( 4

x k ( 2

1) 1)  

Ma trận chuyển vị được xác định bằng cách lấy đạo hàm riêng

, nhận được

f  i x 

j

(2.117)

[47]:

,

1, 7;

1, 7

i

j

 







   ij

 

j

  f i   x    

trong đó,

cos

[(

U

sin

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

UT [(

R

T

R





1) /

1) /

;





11 1  ; 12 

; 13 

x k ( 6

x k ( 6

] / 2 ] / 2    tg

R

T

R

T

tg

R



1) / 2

1) / 2

1)] / 2





1) 



14

; 15   

; 16 

x k 5( T

x k 4(

x k ( 2

x k ( 3

cos

[     0  ; ; 17

UT [(

R

T

R

 

1) /

1) / 2

;





x k 5(

x k ( 6

; 22 1  ; 23

[(

U

sin

] / 2      21

T

R

T

R

 

;

;

1) /

1) / 2





25

x k ( 6

x k 3(

] / 2   tg     24

T

tg

R

 

1)]2

;

1) 



27

x k ( 1

x k ( 4

[  0  ;  26

T

R

T

R

tg  

 



U [

sin

1) /

1) / 2





; 32

; 33 1  ;

x k 5(

x k ( 6

cos

] / 2    31

UT [(

R

T

R



1) /

1) / 2

;





; 35  

x k ( 6

x k 2 (

R

] / 2   34



T tg [ 

1)] / 2

1) 



x k ( 1

x k ( 4

  36 0  ; ; 37

T

R

U

tg   

1) / 2

T

R

] / 2

;



 

1) /



x k 5(

; 42 

x k ( 6

[( sin   41

T

R



1) / 2

UT [(

R

] / 2

;



 

1) /



 43

x k 1(

x k ( 6

T tg [

R

cos  1  ; ; 44   45



1)] / 2

;

1) 



47

x k ( 2

x k ( 3

T 4

T 2



 0  ;  46

n









;

  51

n x

n z

y

x k ( 1

x k ( 4

Tx k 2 ( 3

T 4

T 2



1) 1) 1)

n

n

n









;

  52

x

y

z

x k ( 2

x k ( 3

Tx k 2 ( 4













  53

; 55 1  ; 56 0  ; 57 0  ;

( Tx k 2 2

2 ( Tx k 1

; 54 ( Tx k 2    1

2 ( Tx k 2

1) n y

1) n z

1) n y

1) n z

T 2

T 2

T 2

T 2

1)

1)

n

1)

;

1)

n

1)

n

;

















1) 1) 1)

x

z

n x

y

n z

x k ( 4

x k ( 1

Tx k 2 ( 2

x k ( 3

x k ( 1

  62   61

65

T 2

T 4

;

1)

1)

n

1)

















; 64  

n 1) x

n 1) z

n x

y

n z

x k ( 2 T 1

x k ( 2 T 3

x k ( 2

x k ( 3

Tx k 2 ( 4

T 2

n

T 2

n

n

  63

1)

1)

1)

;











65 0  ; 66 1  ; 67 0  ;

71

x

y

z

x k ( 3

x k ( 2

Tx k 4 ( 1

T 2

T 2

1)

1)

n

;













  

n 1) x

n 1) y

n x

y

x k ( 4

x k ( 1

; 73 x k ( 2 T    1

x k ( 2 T 4











; 75 0  ; 76 0  ; 77 1  ;

  74

n 1) x

n 1) y

n 1) z

x k ( 2 T 2

x k ( 2 T 3

x k ( 4 T 4

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:

  72

g

 

 

 







5

(2.118)

1) 1] 1) 1)

2 x k 1) 2 ( 2 1)]















g

1) 1]

1)

1)

1)]

 

1) 1) 1) 1)]



x k ( 3 x k 1) 2 (   1  







6

( x k 3

x k ( 3 1) 2 ( x k 1

 2

(2.119)

1)

1)

1)]



x k ( 4 2 1) 2 ( x k   3  



 2    3 ( x k 4

T x k [2 ( 2 x k ( 4 [2 ( T x k 2 

 3 1)

 1 T x k [2 ( 2  1 

g

 

1) 2 ( x k 1 1) 1]  





 





7

x k ( 4

x k 1) 2 ( 1

x k ( 3

(2.120)

1) 1)]  2

( x k 2 T x k [2 ( 2 1)]



( x k 4 2 x k 1) 2 (   4 1)  







2 T x k [2 ( 1 x k 2 ( 1 2 [2 ( T x k 1 [2 ( T x k 3 2 T x k [2 ( 1 T x k [2 ( 3

x k ( 4

x k ( 1

x k ( 2

các

Ma trận hệ số nhiễu được xác định bằng cách lấy đạo hàm riêng

g  i   j

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

(2.121)

G

1,3

i

j

,

; 1,7









g ij

 

 

g  i   j

   

   

trong đó,

1)  1 1)  3

g

g

 

 

 



 





  ; 1) 1]

;

51

2 T x k [2 ( 1

2 x k 1) 2 ( 2

T x k [2 ( 2

x k ( 3

x k 1) 2 ( 1

x k ( 4

52

1) 1) 1)]

g

 



 



 



 





 ; 1)]

;

g 53

T x k [2 ( 2

x k ( 4

x k 1) 2 ( 1

x k ( 3

T x k [2 ( 2

x k ( 3

x k 1) 2 ( 1

x k ( 4

61

1) 1) 1) 1) 1)]

g

g

 

 

 



 





  ; 1) 1]

;

62

2 T x k [2 ( 1

2 x k 1) 2 ( 3

T x k [2 ( 3

x k ( 4

x k 1) 2 ( 1

x k ( 2

63

1) 1) 1)]

 



 





 



 





;

;

g 71

T x k [2 ( 2

x k ( 4

x k 1) 2 ( 1

x k ( 3

g 72

T x k [2 ( 3

x k ( 4

x k 1) 2 ( 1

x k ( 2

1) 1) 1)] 1) 1) 1)]

g

 

 

 

; Các hệ số

73

2 T x k [2 ( 1

2 x k 1) 2 ( 4

ijg 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ế.

1) 1)]

Trên máy bay luôn có các thiết bị đo vận tốc

V V V ta có véc tơ đo:

N

E

D

(

,

T )

T )



,

, khi đó các hàm quan sát có được là:

, z z 1

2

z 3

, V V V ( D

N

E

, ,







(2.122)

h X ( 1

k

x 5

h X ( 2

k

x 6

h X ( 3

k

x 7

) ; ) ; )

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

v v v , tức là: 1

3

2

, ,

z

















;

;

(2.123)

z 1

h X ( 1

k

x 5

v 1

2

h X ( 2

k

x 6

v 2

z 3

h X ( 3

k

x 7

 v 3

nhận được [47]:

Việc xác định ma trận đo bằng cách lấy đạo hàm riêng

h  i x 

j



) ) )

H



(2.124)

k

ˆ ( )  k

h  k X X X  

0 1 0 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.

0 0 1 0 0 0 0 1 0 0 0 0 0 0     

X

Q R ,

Như vậy, đã có đầy đủ các yếu tố (

đã được xác

F k

h X G X ), k

k

k

k

k

 1

 1

 1

 1

đị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

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

1 2

2

2

2





2 0

1 3

C



0 3 



(2.125)

n d

0 3

1 2

    0 2 2 1 2    0 1 2 2 1 2 

 2 

 

2 2    1 1 2 2 2 2 2 2      2 0 2 2 2 2       3

2 3 2  0

0 2

2 3

1 3

0 1

c 11 c 21 c 31

c 12 c 22 c 32

c 13 c 23 c 33

    

    

    

    

Đâ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.

, , ,     và từ đó xác định được ma trận Côsin định

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

Khi thả ASWs từ máy bay lên thẳng thì các thành phần

có giá trị vô

V V ,N E 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

const

;

const

;

const

;

const









(2.126)

 0

 1

 2

 3

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

    không còn thực hiện được, khi này đòi hỏi cách

0

1

2

3

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ị

j

1,3)



trên máy bay lên thẳng để xác định các phần tử

của ma trận Côsin định

c ij i ( ,

n

hướng

dC .

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à:







(2.127)

c n 11

dx

c n 12

dy

c n 13

dz

 NV



(2.128)





 V E

c n 21

c n 22

c n 23

dx

dy

dz



g







(2.129)

c n 31

dx

c n 32

dy

c n 33 dz

 V D

Thay các giá trị

,

,

n dx

n dy

n theo các phương trình (2.97) vào các phương trình dz

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

)













(2.130)

c n 11 x

c n 12 y

c n 13 z

( c w c w c w 11 4 12 13 6

5

 V N

(

)













(2.131)

c n 21

x

c n 22

y

c n 23

z

c w c w c w 21 22 6

23

5

4

 V E

g

)













(2.132)



c n 31 x

c n 32 y

c n 33 z

( c w c w c w 31 4 32 5 33 6

 V D

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

,

,

V V V 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 về

N

E

D

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

j

1,3)



.

xác định các phần tử

c ij i ( ,

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

(0)

dt













c 11

c 12

c 13

dV N

( ) V T N

V N

n dt x

n dt y

n dt z











dV N dt

0

0

0

0

0

(2.133)

[

]







4

5

6

T  c w dt 11 0

T  c w dt 12 0

T  c w dt 13 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:

T

T

T

(0)]

V T V [ ( ) N

N







y

n dt c x 11

n dt c 12

n dt c z 13







 T

1 T

1 T

1 T

0

0

0

  

  

  

  

  

(2.134)

T

T

   T

]

[ 





c 11

w dt 4

c 12

w dt 5

c 13

w dt 6







1 T

1 T

1 T

0

0

0

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

{





} 0 

;

;

{ } 0 w dt M w  4

4

{ } 0 w dt M w  5

5

w dt M w  6

6







1 lim T T

1 lim T T

1 lim T T

0

0

0

,

,

w w w là các nhiễu dạng ồn trắng.

vì

4

5

6

Khi đó phương trình (2.134) sẽ trở thành:

T

T

T

(0)]

V T [ ( ) N

V N

n dt c

n dt c

n dt c







(2.135)

x

y

z

11

12

13







 T

1 T

1 T

1 T

0

0

0

  

  

  

  

  

  

T 

và tính gần đúng các thừa số

Chia khoảng thời gian T thành N khoảng

T N

tích phân trong biểu thức (2.135) sẽ có dạng:

T

N

N

N

ndt



 



n T i

n i

n i









1 T

1 T

T  T

1 N

i

i

i

1 

1 

1 

0

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

N

N

N

(0)









(2.136)



 ( ) V T N

V N

( ) n i x

( ) n i y

( ) n i z

c 11

c 12

c 13







1 N

1 N

1 N

1 T

i

i

i

1 

1 

1 

  

  

  

  

  

  

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:

2

N

2

N

2

N

(2 ) T









(2.137)



 V N

( ) V T N

( ) n i x

c 11

( ) n i y

c 12

( ) n i z

c 13







1 N

1 N

1 N

1 T

i N 1  

1 i N  

1 i N  

  

  

  

  

  

  

69

3

N

3

N

3

N

1

1

1

1

(3 ) T

(2 ) T









(2.138)





V N

V N

( ) n i x

c 11

( ) n i y

c 12

( ) n i z

c 13







T

N

N

N

i

2

N

i

2

N

i

2

N



1 



1 



1 

,

,

Nếu coi 11 c

c 12

c là ba nghiệm cần tìm thì ba phương trình (2.136 – 2.138) là 13

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)

X

(

,

,

)T

                 

trong đó,

c 11

c 12

c 13

N

N

N









i

i

i

1 

1 

1 

T

(0)] /



N

N

N

2

2

2

n i ( ) x n i ( ) y n i ( ) z 1 N 1 N 1 N

N (2 )

( )] /

T

;

B









i N 1  

i N 1  

i N 1  

V [ N (3 )

N T (2 )] /

T

V T V [ ( ) N T V T  T V  N

V [ N

    

    

N

N

N

3

3

3

A ( ) n i x ( ) n i y ( ) n i z 1 N 1 N 1 N







i N 2 

1 

i N 2 

1 

i N 2 

1 

,

,

Như vậy đã xác định được các phần tử 11 c

c 12

c của ma trận Côsin định hướng 13

n

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

)

T

(0)) /



1



A

)) /

T

T (2 )





(2.140)

V N V T ( N

T (3 )

T (2 )) /

T



c 11 c 12 c 13

V T ( ( N V ( N V ( N

V 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ử

n

,

,

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

c 21

c 22

c của ma trận 23

T

(0)) /



1 

A

T (2 )

( )) /

T





(2.141)

V E V T E

T (3 )

T (2 )) /

T



V T ( ( ) E V ( E V ( E

V E

c 21 c 22 c 23

n i ( ) x n i ( ) y n i ( ) z 1 N 1 N 1 N                   

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ử

n

,

,

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

c 31

c 32

c của ma trận 33

                   

1 

(0)) /  T g 

(2.142)

A T (2 ) ( )) /   T g  V D V T 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

T (3 ) T (2 )) /  T g  V T ( ( ) D V ( D V ( D V D c 31 c 32 c 33                    

các gia tốc

y

z

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

, , n n n không đổi hoặc có giá trị bằng 0 (chuyển động đều) trong quá x

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

n

Ma trận Côsin định hướng

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

định thông qua các tham số Rodrig – Hamilton

    khi thả từ máy bay phản

0

1

2

3

, , ,

khi thả từ máy bay lên thẳng theo công

lực hay thông qua các phần tử

c ij i ( ,

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

=(2

2

n 1)

(2

)

n

(2

)

n













(2.143)

2    1

2 0

2    0 3

1 2

2    0 2

1 3

c n 11

c n 12

c n 13

N

x

y

z

x

y

z

f

=(2

)

n

(2

2

n 1)

(2

2

)

n









(2.144)





E

x

z

y

2    0 3

1 2

2    2

2 0

   0 1

2 3

x

y

z

c n 21

c n 22

c n 23

f

=(2

)

n

(2

2

)

n

(2

2

n 1)













(2.145)

D

x

y

z

x

y

z

2    0 2

1 3

   0 1

2 3

2    3

2 0

c n 31

c n 32

c n 33

Từ (1.22), vận tốc tâm khối của ASWs trong hệ tọa độ địa lý được xác định:

j 1,3) 

(2.146)

N

E

D

 V N

 V E

 V D

Khi đó tọa độ tâm khối của ASWs trong hệ tọa độ địa lý được xác định:

f ; f ; f g    

 y V 

(2.147)

 z V  D

 x V  N

; ;E

71

ASWs

Giai đoạn ASWs còn ở trên máy bay

1

2

3

Máy bay

3 vận tốc kế

Cơ cấu đế 3 gia tốc kế

Thuật toán xác n định ma trận dC

xn

yn



zn

arctg





n

* c 21 * c 11

  

bC



n

arcsin



 

   * c 31

dC

 



(0)x

arctg





NV

Df

Ef

Nf



x   

  * c 32 * c 33

  

  





NV

(0)

b

EV

(0)y

1





dC

1   2   3





EV

g

(0) y )b dC  (

DV

z







z





(0) (0)

Giai đoạn ASWs rơi trong khí quyển và chuyển động có điều khiển trong nước

DV

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

n

Trong quá trình ASWs rời vật mang, ma trận Côsin định hướng

dC 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

b

dC 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

n

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

(2.148)

n d

b n C C d b

1

C

Nhân hai vế phương trình (2.148) với

dC  có: (

1 

)b

(2.149)

n C b

b d

n d

* c 11 * c 21 * c 31

* c 12 * c 22 * c 32

* c 13 * c 23 * c 33

C ( ) C 

          

72

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

 

arctg

arctg

 

 

* 31

 arcsin c 

;

  ;

(2.150)

* c 21 * c 11

* c 32 * c 33

  

  

  

  

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ý):



(

(

X

|

v

|

(

X

X

)









w X 

|

|

(

uu

u

w

up









m X u X )  u

v v |

|

X u v ) uv

uw

|

w w |

uu

X w wq

up

mw q )

(

X

)







r X 







X q qq

v X r mv rr

vr

pl

2  l

X u uu  l



(

Y (

|

v

|

)

q









p Y 







 r

p



Y u v mpw  uv

0

v v |

|

Y u mu r Y u ( ) ur

uu

2  h

m Y v Y )  r  v

Y w wp

pq

 h





(

)

(

Z

|

w

|

Z u w

)

mv

Z uq Z

u

m Z w Z q mqu 













(

)

p





uw

uq

2  s

uu

 w

 q

|

w w |

Z v Z r  rp

vp

 s

(3.1)

(

I

 K p K

)

p

cos







sin  

(

)

uu K u 

I

I

qr







xx

 p

z B f

uu

up

yy

zz

2  l

K u uu  l

(

I

 M q M

)











(

)

(

)

uw M v M r p

I

I

rp











 M w  w

yy

 q

M uq uq

uw

vp

rp

xx

zz

z

sin

cos

B



cos  







2  s

f

x B f

M u uu  s

I

N

(

)

 r N uv

p

I

I

pq

cos















sin  

(

)







 N v  v

zz

 r

uv

N ur ur

x B f

N w wp

xx

yy

N pq pq



2  h

N u uu  h

               

sin sin

cos

(cos

(cos

sin cos

v

w

cos   



cos

sin cos

(sin sin sin

(sin sin cos

cos

v

w



sin sin )       sin )      

sin cos )       cos )       cos cos

sin

 

(3.2)

cos

   (cos sin ) v w      tan sin tan cos r      sin r





 sin sec

 r

cos sec

q

  

 



 x u   0   y u   0   u z   0   p q       q     

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

C

M

C

D

L

g















 

(3.3)

RB

RB

  A

A

Phương trình (3.3) được viết lại:

( ) ) ( ( )   ( )   ( )      

M

C

D

L

g











 

(3.4)

M M M

C





C C 



trong đó,

;

A

RB

A

RB

Kết hợp (3.4) và (1.40) ta có hệ phương trình:

M

C

D

( )

L

g

)

(

 



( )  



( )      







(3.5)

( )

 J   



J

1( )  

(3.6)

Từ (3.5) có thể viết

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

1 

1 



J

J



  J

 

( )[ ( ) ] ( )    



(3.7)

1

1

1

1 











( ) ) ( ( )   ( )      

D

C

 J

L

g

J

J

J

J

MJ







Thay các phương trình (3.6), (3.7) vào (3.4) khi đó (3.4) được viết lại: 1    ( ) ] ( )[ ( ) ( )      

 (         

1

1



1 





MJ

[

MJ

C

D

L

)]

J

( )













 ( ) 

 ( ) ( ) J  

( ) 

( ) 

( 

( ) g   



(3.8) (3.9)

TJ  ( )

ta có:

Nhân hai vế (3.9) với

1

T

T



1 





1 

J

MJ

J

MJ

C

D

L

J









( ) ( ) ( ) ) ( ) ( )

 ( ) 

 J ( ) ( )  

(3.10)

T

T





 ( ) 

J

J





g ( ) ( )  

( )  ( )[  ( )  ( )  ( )] 

( ) 

75





 

 ( ) 

(3.11)

M  ( ) 

C 

g 

trong đó,

1

T





M

J

MJ



, ( )    

T



1 

J

MJ

C

D

L

)]









( )[ 

 ( ) ( ) J  

( ) 

( ) 

( 

C ) , (  

T

 J

( ) ( ) g  

g ( )  

( ) 

TJ 

M

M

0



( ) 

( ) 

 [39], khi đó (3.11) được viết lại:

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



T 

M

)

,

M

M







 

 ( ) (  

(3.12)

1  

C 

1  

( ) g ( )   

1  

( )  





Từ (3.5) và (3.12) với

hệ phương trình (3.5) được viết lại:

   pl bl

( )  ( )  ( )  

(3.13)

( )

M

M

M

J

M

J

 ( ) (  







T  ( )   pl

T  ( )   bl

1  

C 

1  

g ( ) ( )   

1  

1  

 J         

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)

1s ,

2s và hai bánh lái trong mặt phẳng đứng (bánh lái hướng)

1h ,

2h (hình

3.1). Các bánh lái hướng

1h ,

2h 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

1s ,

2s đ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à

1

2

 s

  s

Đị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 đó:

2



(3.14)

 l

   h h 1 2

h



(3.15)

 h

   h 2 1 2

khi đó,





(3.16)

   h

1h

l





(3.17)

   h

2h

l

) , ( ) ( )

76

1h

2s

1s

Y

Y

2h

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

0

Giả thiết ASWs chuyển động không trượt nghĩa là lực bên

Y  , 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

u

u

, các thông số ban đầu

 . 0

chuyển động thẳng của ASWs không đổi

0

q  0 0

77

q

Khi đó

, trong mặt phẳng đứng véc tơ vận tốc của ASWs được viết lại

[23].

u ( , 0,

w q , 0,

, 0)T

 

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:

Z

|

|

0 0 0



|

| w w

Z u uw td

 w

w td q

 w

 q





 x

z

m Z  M  0 0 0

Z   q I M  yy 0 0 0

0 0 0 0 0 0 1 0 0 0 1 0 0 0 1

w td M u uw td 0 0 0

0 0 0 0 0 0 0 0 0 0 0 0

Z u uq td M u uq td 1 0 0

 w    q       x    z 

       

       

       

               

       

       

Z

uu

2 u td

 s

sin

cos

0  

z B f

x B f

2 M u uu td  s

(3.18)





sin cos

cos sin

0 0 0

u td u  td

       

          

     s    

       

0 w   td w   td 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





(3.19)

sin

(

1  ) [





 q  



 

cos ] 

uw

uq

I M  yy

M w M u w M u q z B td

td

td

b

x B b

(





uu

s

u

u



 vì dòng chảy đại dương được coi là hằng

         w q  1 2   I M M u )  s  yy q td  là hằng số; trong đó,

0

w tdw

c

u td

số.



Đặt biến trạng thái



 

 x 1

x  1  x   2

Đầ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

y

Đầu ra hệ thống

x có được từ thiết bị dẫn đường quán tính theo tính toán

1

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:

[

g x u d

( )

]

 x A x B f x ( ) 







0

k

(3.20)

k y C x



T k

78

trong đó:

;

;

okA

kB

kC

0 1

   

  

f x ( )

(

1  ) [

sin









x B 

cos ] 

 

I M  yy

 q

M w M u w M u q z B td

uw td

uq td

w

b

b

1 

g x ( )

(





uu

yy

 q

2 td

I M M u )

s

0 0 0      1   1      0  

Các thành phần

được giải từ phương trình (3.18)

tdw q q ,

,

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

u

u

, góc hướng nhỏ, các thông số ban đầu

 . Khi này có thể

là không đổi

0

r  0 0

r 

xem

, 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

u v ( ,

, 0, 0, 0, )T

r



[23].

diễn

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:

|





v v |

Y u uv td

Y u ur td

Y  r N

I

v td r



 v

 r

zz





m Y   v N 0 0 0

 0 0 0

0 0 0 0 0 0 1 0 0 0 1 0 0 0 1

| v  td N u uv td 0 0 0

mu  td N u ur td 1 0 0

0 0 0 0 0 0 0 0 0 0 0 0 0 0 0

 x y

       

 v      r          x      y  

       

               

       

Y  |       

2 Y u uu td  h

2 N u uu td  h

(3.21)





0

sin cos

cos sin

0 0 0 v   td v   td

u td u td

0 0

         

       

       

     h    

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:

(3.22)

1





0

1  ) [

I

N

I

N

r  r  











zz

 r

N v N u v td td

uv

 v

N u r ur

td

zz

 r

uu

N u

2  td h

h

      

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

trong đó,

v tdv

Đặt biến trạng thái



 

  x 1

x  1  x   2

( ] ( )

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

y

Đầu ra hệ thống

x có được theo tính toán của thuật toán xác định tham số

1

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 đó,

1  ) [

f x ( )

I

N









 

 r

N v N u v td td

uv

v

N u r ur td

zz

1 

g x ( )

(

I

)



N   r

zz

uu

2 td

N u

h

 v v ,

r

Các thành phần

được giải từ phương trình (3.21)

( ]

,td

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]:

(3.23)

1

1 



p  p

I

K

I

K











 p

xx

z B b

 p

xx

uu

2  td

K u

l

l

       

Đặt biến trạng thái



 

  x 1

x  1  x   2

Đầ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

y

Đầu ra hệ thống

x có được theo tính toán của thuật toán xác định tham số

1

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 đó:

1 

( ) sin ( )

f x ( )

I



K   p

xx

z B  sin b

1 

g x ( )

(

I

)



K   p

xx

uu

2 td

K u

l

( )

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.

y k . Với nhiễu mô hình ( )

Cho hệ thống có đầu vào là

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:

( )u k , đầu ra là ( )d k giả thiết

y k ( )

d k ( )

 









(3.24)

a y k ( 1

a y k ( 2

b u k ( 1

b u k ( 2

ký hiệu,

2) 2) 1)   1)  

k ( )  

véc tơ tham số hệ thống

(3.25)

a 1

a 2

b 1

b 2

( )T

k ( )

y k (

y k (

u k (

u k (











véc tơ hồi quy

(3.26)

   

y k ( )

k ( )

d k ( )

( ) k  

T 

Với ký hiệu trên, (3.24) được viết lại dưới dạng

(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ứ

1) 2) 1) 2) T 

( )d k .

Trong biểu thức (3.27) nhiễu

( )k và nhiễu

đầu ra của hệ thống khi biết đầu vào và đầu ra trong quá khứ

( )d k không thể biết trước nên chỉ có thể dự báo

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:

( )k . Để nhấn mạnh

k ( )

(3.28)

/ ) ˆ( y k ˆ T   ˆ  k ( )

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

N 1,

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:

( )

NZ

u { (1),

y

u N y N ),



(3.29)

(1),..., ( ( )}

81

Bắt đầu

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

( )k

, ˆ (0)

Khởi tạo

1k 

(

( ) k

( ) y k

( ) k







T 

ˆ 1) k  

k

k  1



1) ( ) k 

L k ( )



k P k ( ) (





P k ( T ( ) k  

1) ( ) k 

P k (

k P k ( ) (

1)





P k ( )

P k (



1)  

k P k ( ) (





1 t ( ) 

T k 1) ( )   T k ( )  

1) ( ) k 

   

   

k ( )

1)

k

L k

ˆ ˆ (     

k ( ) ( )

(0)P

Điều kiện dừng

SAI

Kết thúc

ĐÚNG

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

k

N 1,

thiểu đệ quy với

(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 ( )

y k ( )

k ( )







T 

(3.30)

( ˆ   k 1)

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:



1) ( ) k 

L k ( )



(3.31)

k P k ( ) (





P k ( T ( ) 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:

P k (

k P k ( ) (





P k ( )

P k (



(3.32)

k P k ( ) (





T k 1) ( )   T k ( )  

k 1) ( ) 

   

   

1) 1)   1 t ( ) 

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

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

(0)P

đầu thường được chọn

là ma trận đơn vị; ˆ (0) có thể được khởi tạo ngẫu nhiên

k ( )

k

,



quanh giá trị 0, hệ số quên

  và được chọn trong khoảng 0.95-0.99 [30].

1) ˆ ˆ (     

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:



(3.34)

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

G z ( ) z b z b  2 1 a z a   1 2

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 A x B F x G x u d ( )

( )

[

]









0

(3.35)

T

y C 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,

T )

T )





; đầu ra hệ thống là các góc

góc chúc ngóc và góc lắc

h

u , , , , u u u ( 1 2 3 (    s l

T )





T ( ) ,  

đo được từ thiết bị dẫn

hướng, góc chúc ngóc và góc lắc

đườ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

y ( , , , y y y 1 2 3

T )

T )





.

2; nhiễu ngoài được xét là dòng chảy đại dương

3

c

Véc tơ trạng thái:

T



d ( , , , , d d d 1 2 u v w ( c c

T )





 y y y y y y , 1 2 3

 1

3

2

x ( , , , , ( , , , , , ) x 11 x 12 x 21 x 22 x 31 x 32



02

[ , ] A 0 diag A A A , 01 03



1

2

, , ] B diag B B B [ 3



2

1

, , ] C diag C C C [ 3

84

;

;

okA

kB

kC

   

  

F x M

( )

)

,

M

M

( )

J







 ( ) (  

T  ( )   pl

1  

C 

1  

( ) g ( )   

1  

T

1 



0 1 0 0 0      1   1      0  



bl

Viết lại các hàm

G x u M ( ) ( ) ( ) J   

đầu ra, bậc 2:

F x ( )

( ),

( ),



( ) T 



f x 1

f x 2

f x 3

G x ( )

( ),

( ),







G x G x G x ( ) 2

1

3

( )F x và ( )G x dưới dạng ma trận cho hệ thống có 3 đầu vào, 3





1

2

3

k

k

k

g x g ( ), x g ( ), ( ) T  x G x ( ) k





) là các hàm phi tuyến trơn.

kf

kjg

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ó

k 1,3; j 1,3 x và ( ) x ( ( )

được dùng trong toàn bộ mục

mô tả tổng quát như phương trình (3.35) với

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ì

1,3 k 

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

(

,

,

T )

,

T )





sao cho khi ASWs chuyển động theo các góc mong

y in

( ,    r

r

r

y in 1

y in 2

y in 3

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

1k  .

tín hiệu mong muốn (

   và có khả năng thích nghi với sự ảnh hưởng của

r

r

r

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]:













(3.36)

min

max

 n

 l

 

0 x ( ) x ( )

( )G x

trong đó,

n x và ( )

l x tương ứng là giá trị riêng nhỏ nhất và lớn nhất của

M

M

0



( ) 

( ) 

Vì

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



T 

( )

85

Đặt:



;

 y in

 y in 1

 y in 2

 y in 3

 y in 1

 y in 2

 y in 3

( , , , , , )T ( , , )T Y in y in 1 y in 2 y in 3

Sai số bám

 ; ˆ x

 . Với ˆe và ˆx là ước lượng của e và x

ˆ x e Y  in e Y  in

T )

T )

T )

T )









;

 e e e e e e , 3 1 2

 2

 1

3

21

11

31

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]:

*

( )[

F x ( )

]

1  u G x







(3.37)

r ( ) y in

T K e c

e ( , , , , ( , , , , , ( , , ( E E E , , e 11 e 12 e 21 e 22 e 31 e 32 E 1 e e e 1 2 3





với

là véc tơ độ lợi hồi tiếp được

trong đó,

2

3

2

c

c

c

T ck

ck

chọn sao cho

thỏa mãn định lý Hurwitz.

K [ , ] K ( ) diag K K K , c 1 K K , ck 1

k

T ck

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

(

,

)T



y   ,

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 ˆe  [41]:

A 0 B K k









0

0

1

(3.38)

T

 ˆ T ˆ ˆ e A e BK e K E E 1 c ˆ ˆ E C e  1

trong đó,

ˆ ( )



là véc tơ độ lợi bộ quan sát trạng thái;

0

02

03

K [ , ] diag K K K , 01





được chọn sao cho

thỏa mãn định lý Hurwitz.

0

k

k 0 1

k 0 2

T k



 e

K ( K , K )T A ok K C k 0

và

Sai số bộ quan sát được định nghĩa:

 E 1

ˆ E E  1

1

Sơ đồ bộ điều khiển DAFNOC như hình 3.3 với luật điều khiển được đề nghị:

ˆ e e  



 v

(3.39)

f

trong đó,

u u



là thành phần để khử nhiễu ngoài và sai số mô hình;

u

u (

,

u

,

u

)T



là thành phần xấp xỉ luật điều khiển (3.37).

1

2

3

f

f

f

f

v ( , , )T v v v 1 2 3

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

1 1kA

1k

1ˆke

1k

. . .

h 1

kA

fku

. . .

1 2kA

kh

kh

2ˆke

. . .

h 2

kA

Lớp 1

Lớp 3

Lớp 4

Lớp 2

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

Để tính

fku 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



. Cấu hình cơ bản của mạng nơron mờ Singleton gồm có h luật Nếu -

1

2

( , )T ˆ e k ˆ e k ˆ e k

) được viết [4]:

Thì và cơ chế suy luận mờ. Luật Nếu – Thì thứ i (với

i

i

i

fku là

iR : Nếu

1ˆke là

kA và

1

2ˆke là

kA thì

2

kB

i

i

i

trong đó,

1

2

kA ,

kA và

kB là các tập mờ

Mạng nơron mờ Singleton có bốn lớp:

i h 1,



( , )T

Tại lớp 1: Nút là các đầu vào, đại diện véc tơ ngôn ngữ vào

;

1

2

ˆ e k ˆ e k ˆ e k

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

(3.40)

 

 i A k

 i A kj

j

1 

)

được chọn dạng hàm Gauss với độ rộng

Hàm liên thuộc của biến mờ

ˆ( A e kj

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

) ˆ( e kj

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ờ

h  R

1

 k

k

trong đó,

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

2

)

ˆ( e

kj

 A



i kj

1

j



i

h

 1

(với

)

(3.41)

)



 ki

ˆ( e k

2

h

(

)]

ˆ e

kj

 A

  [

i kj

1

i



1

j



) ,..., , )T ˆ( e k (    k kh 2

Tại lớp 4: Đầu ra đại diện giá trị đầu ra của mạng

fku . Giữa lớp 3 và lớp 4 được nối

h



 , các thông số này có thể điều

đầy đủ bởi véc tơ thông số

1

2

k

k

 kh

(

chỉnh được.

) 1.    ki

ki là điểm vạch mà tại đó

i B k

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:

2

h

(

)]

ˆ e

kj

 A

  [  ki

i kj

i

1



j

1



(3.42)

)

u





T   k k

ˆ( e k

fk

2

h

(

)]

ˆ e

kj

 A

  [

i kj

i

1



j

1



, ,..., )T R (    k

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

cần thực

1

2

k

k

 kh

hiện một số phép biến đổi sau :

, ,..., )T (    k

Thay (3.39) và

 vào (3.35), khi đó phương trình (3.35) được viết lại:

(

e

)

B F x G x u G x v d

( )

( )

( )











 Y in

 e A Y   in

0

f

 

 

(3.43)

T

(

e

)







y in

E C Y in

1

e x Y  in

 e A e B F x G x u G x v d 















0

f

 Y in

 

 



T

( ) ( ) ( ) A Y 0 in

1

E C e 

























T  ( ) e A e BK e B F x c

 y in

T c

 Y in

 By in

0

 

 



(3.44)

T

ˆ ˆ ( ) ( ) K e G x u G x v d f A Y in 0

1

E C e 



















T  ( ) e A e BK e B F x c

 y in

T c

0

 

 



(3.45)

T

ˆ ˆ ( ) ( ) K e G x u G x v d f

1

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

*

E C e 

 ( ) e A e BK e B G x u G x u G x v d













T c

0

f

 

 



(3.46)

T

ˆ ( ) ( )

1

 e

     ˆ e e

Thay

vào (3.46) khi đó:

*

E C e 

 e

  ˆ e A e  











0

T c

f

 

 

(3.47)

T

ˆ ( ) ( ) ( ) ( ) ˆ e BK e B G x u G x u G x v d 





 ˆ  E E C e  1 1

T

T



( ) ˆ e

 e

* A K C e B G x u G x u G x v d

















 T  ˆ e A e BK e K C e   c

0

0

0

0

f

 

 



(3.48)

T

  E C e  1

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

T



ˆ ˆ ( ( ) ( ) ( ) )

* A K C e B G x u G x u G x v d













0

0

f



(3.49)

T

 e   E C e  1

( ( ) ( ) ( ) ) [ ]

89



vào (3.49) viết lại cho thành phần thứ k (

):

f

f

/ u 1,3 k  ˆ( ) u e 













k

T  K C e k k

0

k

kk

k

* k

fk

 k

k

 B g u 

 

(3.50)

Thay  e k  E



T  C e k k

k

1

( ) ( / ) d A 0 g u kk ˆ e k g v kk k













k

T  )] K C e k k

0

k

kk

k

* k

fk

 k

k

 B g u 

 



(3.51)

 E



k

1

T  C e k k

(

1  )]

(

/

)

d













 E k

1

T [ C sI k

A 0

k

T K C 0 k k

kk

k

g u kk

fk

ˆ e k

 k

g v kk k

k

 

*  B g u  k



(3.52)

(

/

)

d









kk

k

g u kk

fk

ˆ e k

 k

g v kk k

k

*  ( ) H s g u  k

 

(

)]







Với

H s ( ) k

T C sI [ k

A 0

k

T K C 0 k k

1 B k

[ ( ( / ) sI d A 0 g u kk ˆ e k g v kk k

Giả thiết 2: Cho

ke và ˆke thuộc tập

keU ,

ˆkeU thỏa mản điều kiện [41]:

r k

{

:||

U

R

m





|| 

  }

e k

e k

e k

e k

r k

U

R

m

{

:||





|| 

  }

ˆ e k

ˆ e k

ˆ e k

ˆ e k

trong đó, ˆke là ước lượng của

ke và

kem ,

ˆkem là các thông số thiết 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:

[sup

|

/

) |]

u





*  k

M

* k

ˆ ( u e k k

 k







arg min  k

 k

ˆ e Ue e Ue , k k k

ˆ k

h

M

R

:||

}





|| 

với

trong đó,

{  k

 k

m  k

k

 k

m là hằng số.













T  K C e k k

*  k

 k

kk

kk

fk

fk

0

k

k

k

k

k

k

 B g u 

 

(3.53)

Theo giả thiết 2, phương trình (3.50) có thể được viết lại:  e k  E



T  C e k k

k

1

)





trong đó,

là sai số xấp xỉ

w k

g u kk

* k

g u kk

fk

* ˆ( e  / k k

Khi đó (3.53) có thể viết lại như sau:

u

u

(

)

(

/

)

(

/

)











g v w d 



A 0

k

T  K C e k k

0

k

kk

ˆ e k

fk

*  k

ˆ e k

fk

 k

kk

k

k

k

 e k





 B g  k

 

(3.54)

 E



T  C e k k

k

1

( ) ( ) ( ) / / g v w d  g u kk A 0 ˆ e k ˆ e k











k

T  K C e k k

0

k

kk

* T k

T k

kk

k

k

k

 B g  k

 

(3.55)



 e k  E



T  C e k k

k

1

(

)

(

)









g v w d 



k

T  K C e k k

k

 T   k kk k

ˆ e k

kk

k

k

k

A 0

0

 B g  k

 

(3.56)



  e k  E



1

T  C e k k

k

( ) ) ( ) g v w d  A 0 (     k ˆ e k

90





trong đó,

.

 * T T    j j

T j

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











1

k

k

T k

 T   k kk k

kk

k

k

k

T  C sI  k

 

 B g  k

 

(3.57)

( ) ( ) g v w d  A 0 K C 0 k ˆ e k







k

 T   kk k k

kk

k

k

k

 

 

( ) ( ) H s g g v w d  ˆ e k

 E









(3.58)

1

k

k

k

 T   k k

fk

fk

trong đó,

v

fk

 k

kk

1( ) L s g v k

( )[ ( ) ] ( ) H s L s v w ˆ e k







;

fk

k

 

( ) ( ) w ˆ e k w d  k  T   k k ˆ e k

.

;  1 T   ( ) L s g    k kk k k 1  ( ) L s k k

 k

) ( ) ( ˆ e k ˆ e k

được chọn sao cho

là hàm truyền chắc chắn ổn định và

1( ) kL s

k

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

( ) ( ) )(sLk H s L s là k

Giả sử rằng

  sao cho

kL s ( )

k

k

thích hợp. Theo cách đặt như (3.58) khi đó (3.56) có thể viết:

(

)

(

)

v

w











k

T  K C e k k

k

B L s ( ) k

k

ˆ e k

fk

fk

A 0

0

 T    k k

 

(3.59)

  e k  E



1

T  C e k k

k

( ) ( ) H s L s là hàm truyền thực dương chặt s b 1









fk

fk

 



(3.60)

) v w  A e ck k B ck ˆ( e k  T    k k



1

T  C e k k

k

trong đó,

  e k  E





;

k

T

( A ck )T k A 0 K C k 0



ckB

(0 ) b 1

Giả thiết 3:

fkw được giả sử thỏa mản điều kiện [41]:

|

w

(3.61)

fk

|  k

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

(

)

(||

(

m hay

 E

|| 

|| 

) 0) 

|| neáu

vaø

k

 k

 k

T   k k 1

ˆ e k

 k

(3.62)

  k

(

Pr(

))

(

m  k  E

|| 

) 0 

|| neáu

vaø

1

 E   k k 1  E   k k

k

ˆ e k ˆ e k

 k

T   k k 1

k ˆ e k

k

m  k

    

Với

k  là thông số thích nghi thiết kế. Thông số

k

)

 E k

Pr(

(

))

(

)





(3.63)

1

1

 E   k k

k

ˆ e k

 E   k k

k

ˆ e k

 k

 k

T ˆ( e   1 k k k 2 || ||  k

0 m thỏa mãn giả thiết 2.

 

và ||

Khi đó ||

k

k

k

Thành phần để khử nhiễu ngoài và sai số mô hình

kv được cho bởi [41]:

0

|



| 

neáu

vaø

k

1

k

1

0

|

 E  E



| 

vaø

(3.64)

v k

k

1

k

1

 k  k

 E  E  E

|

 | 

neáu neáu

k

k

1

 k

  k     k   E /    1 k k

/

trong đó,

k là hằng số dương. ;

k  

k

min

Đị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.

|| 2 ||   k m m

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

.

ASWsR

nR 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 độ

(

)

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ệ

x y , 0 0

(

)

là điểm bắt đầu điều khiển hiệu chỉnh quỹ

x y , 1 1

tọa độ địa lý; Điểm P có tọa độ

đạ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 độ

là điểm tiếp cận với quỹ đạo mong muốn, tọa độ này đã được xác

x y , 2

2

đị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

(

)

là điểm giao nhau giữa vòng tròn cho phép với quỹ

bay; Điểm C có tọa độ

x y , 3 3

đạ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

x y

)

rời khỏi máy bay; ( ,

là tọa độ tức thời của ASWs.

dòng chảy

X N 0 ( )

ASWsR

qd

q

nR T

qn

C

qn

nL

bX

ASWs

n

n

LOS

x y ( , )

P

O

0Y

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:

arctan





(3.65)

LOS

 

y 2 x 2

y 1 x 1

  

  

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:

arctan





(3.66)

ASWs

y 2 x 2

  

 y  x  

Sai lệch góc hướng LOS và ASWs được tính:

0

0

( )

180

180

t  











với

(3.67)

LOS

n

ASWs

n

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:

2

2

(

x

y

)

)









x 2

2

t ( )

( ) sin(

t ( ))



nL ( y Sai số quỹ đạo khi này được tính:

.

(3.68) (3.69)

 n

L t n

 n

Quá trình điều khiển ASWs theo góc hướng ở giai đoạn này sao cho góc hướng

0





, tức là sai số

ASWs

LOS

n  .

Để 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:

arctan

K

( ) t dt

  





(3.70)

r

ASWs

In

 n



( ) t  n  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:

arctan



(3.71)

 q

y 3 x 3

  

 y  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:

0

0

90

90









với

(3.72)

qn

t ( ) qn

   qd q

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:

2

2

(

y

x

)

)









x 3

y 3

t ( )

( ) sin(

t ( ))



qnL ( Sai số quỹ đạo khi này được tính:

(3.73) (3.74)

 qn

L t qn

 qn

Quá trình điều khiển ASWs theo góc hướng ở giai đoạn này sao cho góc hướng

0

  , tức là sai số

qd

q

qn  . Để giải quyết vấn đề trên thì hướng đi mong muốn

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

t ( )

K

arctan

t dt ( )





(3.75)

   q

r

In

 qn



 qn  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

z z , 1

2

z là tọa độ 3

,P T C

,

theo trục

0OZ của các điểm

tương ứng như hình 3.6.

O

X N 0 ( )

P

LOS

d

qd

ASWs d

dL

nR T

C

qd

qd

bX

0Z

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:

arctan





(3.76)

LOS

 

z 2 x 2

z 1 x 1

  

  

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:

arctan





(3.77)

ASWs

z 2 x 2

  

 z  x  

Sai lệch góc chúc ngóc LOS và ASWs được tính:

95

0

0

180

180











 

với

(3.78)

t ( ) d

LOS

ASWs

d

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:

2

2

(

x

z

)

)









x 2

2

t ( )

( ) sin(

t ( ))



dL ( z Sai số quỹ đạo khi này được tính:

.

(3.79) (3.80)

 d

L t d

 d

Quá trình điều khiển ASWs theo góc chúc ngóc ở giai đoạn này sao cho góc

0





chúc ngóc

, tức là sai số

LOS

d  . Để giải quyết vấn đề trên thì góc chúc

ASWs

ngóc mong muốn được đề xuất:

arctan

K

( ) t dt





  

(3.81)

r

ASWs

Id

 d



t ( )  d  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:

arctan



(3.82)

 q

z 3 x 3

  

 z  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

0

0

90

90









trong giai đoạn quay được tính:

với

(3.84)

qd

t ( ) qd

   q

qd

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

2

2

(

x

z

)

)







x 3

t ( )

( ) sin(

t ( ))



qdL ( z  3 Sai số quỹ đạo khi này được tính:

(3.85) (3.86)

 qd

L t qd

 qd

Quá trình điều khiển ASWs theo góc chúc ngóc ở giai đoạn này sao cho góc

0

  , tức là sai số

chúc ngóc

q

qd

qd  . Để 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:

t ( )

arctan

K

( ) t dt





(3.87)

   q

r

Id

 qd



 qd  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ý

B B B , y z

x

,p q r không có nhiễu và độ trôi ,

- Thiết lập các giá trị tốc độ góc thực

- Thiết lập gia tốc cảm nhận thực a , a , a

bx

by

bz không có nhiễu và độ trôi

(0),

   (0) (0),

- Khởi tạo góc Ơle ban đầu

lập điều kiện ban đầu cho các

tham số Rodrig-Hamilton

- Thiết

(0),

(0),

(0),

(0)

o    theo công thức (1.10) với các góc Ơle đã cho

3

2

1

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

(0),

(0),

(0)

V N

V E

V D

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



* 1

  

(hiện tượng trôi

số tính toán liên quan đến không thỏa mãn đẳng thức

đị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

r

q





 

1

3

2

p      0

p

q

r









0

3

(4.1)

p

q

r









3

0

p

q

r









 2  0  2      2 1 1  2      2 1 2  2      1 3 3

0

2

      

(









 là hệ số hiệu chuẩn. ) 1

trong đó,

2 2      1 3

2 0

2 2

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:

k

(

k ( )

p k (

1)

k ( )

q k (

1)

k ( )

r k (

1)

k ( )

k ( )

1)  















 0

 1

 2

 3

k ( )   0





k

(

k ( )

(

p k (

1)

k ( )

r k (

1)

k ( )

q k (

1)

k ( )

k ( )

1)  















 1

 0

 2

 3

k ( )   1





(4.2)

k

(

k ( )

q k ( (

1)

k ( )

r k (

1)

k ( )

p k (

1)

k ( )

k ( )

1)  















 2

 0

 1

 3

k ( )   2





k

(

k ( )

r k ( (

1)

k ( )

q k (

1))

k ( )

p k (

1)

k ( )

k ( )













1)  

 0

 1

 2

( ) k   3

 3



T 2 T 2 T 2 T   2

   0    1     2     3 

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:

,

,

của con quay vi 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 b b b được đặt tùy ý song phải thỏa mãn điều kiện p

q

r

,

,

,

,

,

;

;

. Nhiễu

w w w w w w là hàm nhiễu ngẫu nhiên

b  r r

1

2

3

4

5

6

b  p

p

b  q q

đượ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.

,

,

   có nhiễu và trôi.

- Thiết lập các giá trị đo được của con quay tốc độ góc

p

q

r

,

,

- Thiết lập các giá trị đo được của gia tốc kế

a a a có nhiễu. x

y

z

- 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:

T

10

X

(0),

(0),

(0),

p

(0), (0), (0),

q

r

,

R





0

o

b b b , q r

p

    1 2



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

T

X

(0),

(0),

(0),

,

,

(0),

(0),

z (0), (0)



11  R

0

1

2

o

b b b V , p q N

r

V E

V D

 (0),     3



- 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á (ma trận có các phần tử

trạng thái

0P , ma trận này thường được chọn

P O 0

10 10x

cho

bằng 0) cho trường hợp ASWs chuyển động trong khí quyển và

P O 0

11 11x

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

Giá trị ước lượng

Giá trị thực

0

1

0.5

1.05

0.4

1

0.3

0.2

0.95

0.9

0.1

t(s)

t(s)

0

10

20

30

40

50

0

0

10

20

30

40

50

0.85

3

2

0.3

0.2

t(s)

0.2

0

0.1

-0.2

t(s)

0

-0.1

-0.2

0

10

20

30

40

50

10

20

30

40

50

0

-0.4 -0.6

Hình 4.1: Các tham số Rodrig-Hamilton khi ASWs chuyển động trong khí quyển

Giá trị thực

Giá trị tính toán được

)

rad (

1

t(s)

0

-1

10

20

30

40

50

0

)

rad (

1

t(s)

0

-1

10

20

30

40

50

0

)

rad (

1

t(s)

0

-1

10

20

30

40

50

0

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.

Giá trị thực

Giá trị ước lượng

0

1

1.2

0.6

0.4

1.1

1

0.2

t(s)

0.9

0

t(s)

0.8

-0.2

50

100

0

50

100

0

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

50

100

0

50

100

0

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.

Giá trị thực

Giá trị tính toán được

)

rad (

1

t(s)

0

-1

20

40

60

80

100

0

)

rad (

2

t(s)

0

-2

20

40

60

80

100

0

)

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

(0)  

(0)  

(0)  

;

;

, giá trị các góc Ơle

góc Ơle đúng ban đầu có giá trị

 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ị ước lượng

-7

0

Giá trị thực  1

Sai số

x 10

0

0.8392

0.1756

1

0.839

0.1752

-1

t(s)

t(s)

t(s)

0.8388

0.1748

-3

0

0

25

50

25

50

50

0

-7

2

3

x 10

25 Sai số

2

0.507

6

0.0982

0.506

2

0.0979

t(s)

t(s)

t(s)

0.505

-2

0.0976

0

25

50

25

50

0

0

25

50

,

,

,

Hình 4. 5: Các giá trị

    đúng và ước lượng

0

1

2

3

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

ijc tính toán

c13

c12

ijc đúng  c11

0.89

0.015

0.48

0.88

0.47

0.01

0.87

0.46

0.005

0

2

0

0

2

2

1 c22

1 c21

1 c23

0.344

0.94

-0.19

0.92

0.342

-0.195

-0.2

0.9

0.34

0

1

2

0

2

1

0

2

1 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

2

0

1

Hình 4.6: Các giá trị

ijc đú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ố

ijc

i ( ,

1,3)

j 

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 ( )

k

k

2)

(

k

(

k

2)



 

1)  





1)  



(4.3)

a (  1

a (  2

b  1 s

b  2 s

k ( )

k ( )

k ( )

T  

 , trong đó:

hay có thể viết

t ( )

k

1)

k

2)

t (

1)

t (



( 



(  







là véc tơ hồi quy;

  

2) T 

 s

 s

T

 

là tham số mô hình cần ước lượng.





a 1

a 2

b 1

b 2

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

/

)

k ( )

ˆ T  

ˆ  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:

(0)P

4 4 I 

;

- Ma trận tạm

105

4 1

ˆ (0) 0   

(ma trận có các phần tử bằng 0)

- Tham số mô hình ước lượng

0.988



.

- Hệ số quên được chọn cố định

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

)

rad (

1

0.5

Số mẫu

0

-0.5

-1

Vartheta thuc Vartheta nhan dang

-1.5

400

800

1200

1600

2000

0

rad

)

( 

Sai số nhận dạng 

0.1

0.05

Số mẫu

0

-0.05

-0.1

400

800

1200

1600

2000

0

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

1

0.5

Số mẫu

0

-0.5

-1

-1.5

a1 a2 b1 b2

-2

-2.5

2000

1200

1600

800

400

0 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 ( )

1)

k

(

ˆ     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

G z ( )



(4.5)

dạng được viết dưới dạng hàm truyền rời rạc là:

z 0.005678  2 z z 1.776

0.0144 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 ( )

k

k

2)

(

k

d

(

k

2)



 

1)  





1)  



(4.6)

c (  1

c (  2

d  1 h

 2 h

k ( )

k ( )

T k ( )  

 , trong đó:

hay có thể viết

k ( )

k

1)

k

2)

(

k

1)

(

k



( 



(  







là véc tơ hồi quy;

  

2) T 

 h

 h

T

d

 

là tham số mô hình cần ước lượng.





c 1

c 2

d 1

2

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

/

)

k ( )

ˆ ( 

ˆ T  

ˆ 

(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:

(0)P

4 4 I 

Ma trận tạm ban đầu

;

Tham số mô hình ước lượng ˆ (0) được khởi tạo ngẫu nhiên quanh giá trị 0;

0.98



Hệ số quên được chọn

.

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

400

800

1600

2000

0

(

rad

)



1200 Sai số nhận dạng 

0.4

0.2

Số mẫu

0

-0.2

-0.4

400

800

1200

1600

2000

0

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

G z ( )



(4.8)

được viết dưới dạng hàm truyền rời rạc là:

2

z  1.98 z

0.3152 z 

0.004359 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

1

0.5

Số mẫu

0

-0.5

-1

c1 c2 d1 d2

-1.5

-2

-2.5

200

400

600

800

1000

1200

1400

1600

1800

2000

0

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

0 ( 45

có tọa độ

với góc định hướng

)

,

0 0  , 18 ,3 )

(

)

,

(100,100, 20)

, x y z  1 1

1

,      ( 1

1

1

(

)

,

(1100,1100,120)

đến điểm tiếp cận quỹ đạo mong muốn ( )T có tọa độ

.

x y z  , 2 2

2

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

m 50

thì bắt đầu quay theo góc hướng và góc chúc ngóc về điểm giao nhau giữa

nR

quỹ đạo mong muốn với bề mặt quả cầu cho phép có

tọa độ

(

)

,

(1050,1150,140)

với góc

trạng

thái mong muốn

tại điểm này

x y z  , 3 3

3

0

(135

)

,

. Nhiễu ngoài là ảnh hưởng của dòng chảy đại dương

0 0 , 18 ,0 ) 

,     ( qd

qd

qd

(m/s).

T (4, 4, 0.5)

T )

,

,



u v w c c

c

giả sử được xét tại thời điểm 20s: ( 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

gồm hai bánh lái

u  h

(vì trong trường hợp này

h  được giả thiết là quay theo cặp nghĩa là

,h

1

2

 h 1

  h

2

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:

(12,16);

(60,120).

T 1

TK  01

cK 

0.0025.



- 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

- Hệ số của luật thích nghi:

1 1 

- Hàm liên thuộc của biến mờ

dạng hàm Gauss là:

)

j

1ˆ( e i A 1 j

2

exp

0.25





)



ˆ( e 1

j

ˆ e 1

j

c 1

j

 1









 i A 1

j

  

  

trong đó, tâm và độ rộng hàm Gauss là:

( 0.5

0.25 0 0.25 0.5)



c   1

5  . ; 1



- Hàm truyền ổn định (bộ lọc):

L 1

s

1

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.

 (độ)

400

800

1200

0

150

X

100

 mong muốn  thực tế  quỹ đạo

P

quỹ đạo thực tế phương đường thẳng tới điểm tiếp cận quỹ đạo mong muốn T

50

thời điểm có nhiễu

46.4

t(s)

400

0

44.8

19.5

24

-50

25

50

0

h (độ)

30

800

t(s)

0

T

C

-30

1200

25

50

0

Y

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

. Mô

gồm hai bánh lái

s  được giả thiết là quay theo cặp, nghĩa là

,s

1

2

 s 1

  s

2

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:

(120, 20);

(820, 990).

T 2

TK  02

cK 

0.005.



- Các thông số của thành phần điều khiển khử sai số mô hình:

 2

20;  2

9000

- Hệ số của luật thích nghi:

  2

)

- Hàm liên thuộc của biến mờ

dạng hàm Gauss là:

j

2ˆ( e i A 1 j

2

exp

0.25





)



ˆ( e 2

j

ˆ e 2

j

c 2

j

 2









j

 i A 2

  

  

trong đó, tâm và độ rộng hàm Gauss là:

0.5 0 0.5 1)



;

.

25 

c   ( 1 2

2



- Hàm truyền ổn định (bộ lọc):

L 2

1

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.

800

400

1200

0

X

P

50

Quỹ đạo thực Phương đường thẳng LOS

T

100

C

150

Z (độ)

s (độ)

t(s)

0

40

Thời điểm xét nhiễu

-5

20

t(s)

0

-10

-15

-20

 mong muốn  thực  quỹ đạo

1s 2s

-20

0

10

10

30

30

20

20

0

-40 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ỏ

18

(độ) nên thời gian đáp

để đi về góc chúc ngóc theo quỹ đạo mong muốn

qd  

ứ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:

(360, 60);

(25, 350).

- Độ lợi hồi tiếp và bộ quan sát:

cK 

T 3

TK  03

0.1.



- 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

30

- Hệ số của luật thích nghi:

  3

)

dạng hàm Gauss là:

- Hàm liên thuộc của biến mờ

j

3ˆ( e i A 3 j

2

exp

0.25





)



ˆ( e 3

j

ˆ e 3

j

c 3

j

 3









 i A 3

j

  

  

trong đó, tâm và độ rộng hàm Gauss là:

( 10

5 0 5 10)



;

2  .

c   3

3



- Hàm truyền ổn định (bộ lọc):

L 3

s

1 

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  gồm hai bánh lái hướng

l

(khi này không xét đến

h  được giả thiết là quay theo cặp nghĩa là

,h

2

1

 h 1

  h

2

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

2

 mong muốn  thực tế

1

-0.5

t(s)

0

1.5

0

-1

0

10

20

30

40

50

5

h (độ) 4

t(s)

1h 2h

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

,

   h

1h

l





như công thức (3.16) và (3.17).

   h

2h

l

115

(độ)

(độ)

0

t(s)

5 t(s) 0

-5

-10

0

30

10

40

0

50

500

0

 mong muốn  DAFNOC  quỹ đạo  PD 10

20

30

40

 mong muốn  DAFNOC  PD 20 500

1000

1000

1500

50 1500

-10   -20     -30   -40 150

(độ)

Y

X

50

 mong muốn  DAFNOC  quỹ đạo  PD

100

50

Thuc LOS P T C

46

t(s)

0

150

Z

42

28

19

100               -50

50

0

10

20

30

40

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

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

    phù hợp với tham số thật giả định

0

1

2

3

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

,

,

,

    trong bài toán xác định tham số

để xác định các tham số Rodrig-Hamilton

0

1

2

3

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

,

,

,

    trong bài toán xác định tham số dẫn đường cho vũ khí chống ngầm ở

0

1

2

3

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.

(2002), “Output Feedback Control of

[41] W.Y.Wang, Y.G.Leu, T.T.Lee,

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à

070

040

, theo mặt phẳng đứng là

; 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

0

0

22

0.5

hạn lớn nhất

. 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

cho tên lửa khi chuyển

070

độ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ả

Giá trị

Đơn vị

Kí 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

175

blx



1025

Mật độ nước

mm kg/m3

Diện tích bề mặt của bánh lái

0.016236

m2

blS

Góc bẻ lái lớn nhất

22

độ

max

0.4

kg

uX 

Hệ số lực khối nước kèm theo trục dọc của hệ tọa độ gắn liền

Hệ số lực cản theo trục

|

bX

-3

kg/m

Hệ số lực cản theo trục

0.1

kg/m

bX

Hệ số lực cản theo trục

|

bX

-52

kg/m

Hệ số lực cản theo trục

30

kg/m

bX

Hệ số lực cản theo trục

|u uX uvX |v vX uwX |w wX

|

bX

-52

kg/m

Hệ số lực khối nước kèm chéo trục

-27

kg/rad

Hệ số lực khối nước kèm chéo trục

1.8

kg/rad

Hệ số lực khối nước kèm chéo trục

0

kg/rad

0

kg/rad

Hệ số lực khối nước kèm chéo trục Lực đẩy động cơ

7000

N

Hệ số lực của bánh lái

13.48

kg/m.rad

l

Hệ số lực khối nước kèm

-676

kg

wqX rrX qqX vrX plX uuX  vY

Hệ số lực khối nước kèm

-92

kg

rY

Hệ số lực khối nước kèm chéo trục

wpY

270

kg/rad

A3

Hệ số lực khối nước kèm chéo trục

pqY

-100

kg/rad

Hệ số lực cản

|

|v vY

-20.27

kg/m

-220

kg/rad

uvY

Hệ số khối nước kèm chéo trục, lực nâng và lực cản

-100

kg/rad

urY

Hệ số khối nước kèm chéo trục, lực nâng

Các lực của bánh lái

13.48

kg/m.rad

h

Hệ số lực khối nước kèm

-676

kg

uuY  wZ 

Hệ số lực khối nước kèm

-92

kg

Hệ số lực cản

|

qZ  |w wZ

20.27

kg/m

uqZ

-620

kg/rad

Hệ số lực khối nước kèm chéo trục và lực nâng

Hệ số lực khối nước kèm chéo trục

vpZ

-270

kg/rad

Hệ số lực khối nước kèm chéo trục

rpZ

-270

kg/rad

-10

kg/rad

uwZ

Hệ số lực khối nước kèm chéo trục và lực nâng và lực cản

Hệ số lực của bánh lái

13.48

kg/m.rad

s

Hệ số mô men khối nước kèm

-676

kg.m2/rad2

Hệ số mô men bánh lái

23.59

kg/rad

l

uuZ  pK  uuK 

upK

-100

kg/rad

Hệ số mô men khối nước kèm chéo trục

Hệ số mô men khối nước kèm

-100

kg/rad

uuK

Hệ số mô men khối nước kèm

-406.6

kg.m2/rad2

Hệ số mô men khối nước kèm

92

kg.m2/rad2

qM  wM 

362

kg

uwM

Hệ số mô men thân và thành phần bánh lái hướng

uqM

-125

kg.m/rad

Hệ số mô men khối nước kèm chéo trục và lực nâng

Hệ số mô men khối nước kèm

vpM

10

kg.m/rad

A4

rpM

140

kg.m2/rad2

Hệ số mô men khối nước kèm chéo trục

Hệ số mô men bánh lái

-23.59

kg/rad

s

Hệ số mô men khối nước kèm

92

kg.m2/rad2

uuM  vN 

Hệ số mô men khối nước kèm

676

kg.m2/rad2

rN 

-90

kg.m/rad

urN

Hệ số mô men khối nước kèm chéo trục và lực nâng

Hệ số mô men thân và bánh lái

-78

kg

uvN

wpN

100

kg.m/rad

Hệ số mô men khối nước kèm chéo trục

pqN

-140

kg.m/rad

Hệ số mô men khối nước kèm chéo trục

Hệ số mô men bánh lái

-23.59

kg/rad

h

uuN 

0I

Ma trận mô men đường chéo theo các trục hệ tọa độ gắn liền

[70.83, 70.83, 1.83]

Tọa độ tâm nổi của ASWs

fC

[0.007, 0, 0.015]

B1

PHỤ LỤC B

Bu t ( )

Ax t ( )

 x t ( )





(0)

x

có điều kiện đầu

Bổ đề 1 [41]: Tính bị chặn của nghiệm hệ tuyến tính ổn định. Cho hệ thống

x 0

n

m

u R

n n A R 

n m B R 

trong đó,

x R ,

,

,

.

u t ( )

, cho

Giả sử A là ma trận Hurwitz và

L 2

e

0 và

0 là các hằng số dương,

0





||

A te (

0 ( ) t  e  

   )  || 

trong đó,

thỏa mãn điều kiện

 [0, ]l

 2 l 

0

0

t

  0

||

x t ( ) ||

e

||

||

||





Thì ta có:

(B.1)

u t

 0

x 0

|| 2 

B || ||  0 2    0

t

T

t ( )    

||

u

d

e

||

1/2 ( ) ( ) u )   

trong đó, giá trị

tu

|| 2

tu

|| 2 

 được tính theo công thức:

  (

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à

kE t tiến tới 0 khi t   .

1( )

Chứng minh:

Định nghĩa hàm Lyapunov của hệ thống như sau:

3

V

(B.2)

  V k

k

1 





trong đó,

(B.3)

V k

T   e P e k k k

  T   k k

1 2

1 2  k

với

T P k

kV ta có :

0.  Đạo hàm theo thời gian hàm P k

(B.4)

Từ (3.60) và (B.4) ta có:

    V k  T   e P e k k k  T   e P e k k k    T   k k 1 2 1 2 1  k

(B.5)

T   e A P P A e  k k k

T ck

ck

k

T  e P B k k ck

fk

fk

( ) ) ( ] w v       V k  T [   k k ˆ e k    T   k k 1 2

Bởi vì

k

k

T P k

 

A P P A  k ck

Q k

Q

0

với

(B.6)

 T k Q k

k C



T ck P B k ck

k

sLsH )( )( 0 1  k là hàm truyền thực dương chặt, nên tồn tại  sao cho: P k

B2

Từ (B.5) và (B.6) ta có:

(B.7)

T   e Q e k k k

fk

fk

1

( ) ] v w        V k  E k  T [   k k ˆ e k    T   k k 1 2 1  k

, theo (3.64) thì

, mặt khác theo giả

Trong trường hợp

kE 

k

1

min

 E

v

w





] 0 

0 /  v     k k

thiết 3 |

. Do đó

.

1[

k

fk

fk

fk

fk

fk

khi này

0

, theo (3.64) thì

, mặt khác theo giả thiết 3

Trong trường hợp

kE 

1

w v w |  k

  k

 E

v

w

v k





] 0 

. Do đó

.

fk

fk

fk

1[

k

fk

fk

khi này

| w v w |  k

thì (B.7) có thể được viết lại:

Như vậy trong cả hai trường hợp

kE 

1

kE 

1

hay

0 0

(B.8)

 V k

T   e Q e k k k

 E k

   T   k k

 T ˆ( e  1 k k k

)     1 2 1  k

2 ||

 Q E ( ) |

2 |

(B.9)

Sự thật

 e k

k

k

min

min

1

) ||   Q ( k

kQ

trong đó, giá trị riêng nhỏ nhất min (

2 |

) 0  , nên từ (B.7) và (B.8) có:

 E

(B.10)

 V k

 ( ) | Q E k

k

k

   T   k k

min

1

 T   1 k k

( )      ˆ e k 1 2 1  k

vì

 * T T    j j

T j

arg min

[sup

|

/

) |]

u





Mặt khác theo giả thiết 2 thì

*  k

M

* k

ˆ ( u e k k

 k







 k

 k

ˆ e Ue e Ue , k k k

ˆ k

*

 

Tức là

, khi đó

 phương trình (B.9) được viết lại:

k 

 T  j

T   j

2 |

0

 E

(B.11)

 V k

 ( ) | Q E k

k

k

  T   k k

min

1

 T   1 k k

 E

) 0 

, theo (3.62) phương trình (B.10) được viết lại:

Trường hợp

T k

k

ˆ( e  1 k k

2 |

( )      ˆ e k 1 2 1  k

T )]

(B.12)

min

k

k

1

k

1

( ) (  ( ) | Q E  E       V k ˆ e k [  k ˆ e k   k  T     k k   1 2

T )]

Vì

nên từ (B.12) có:

 T   k k

  k

2 |

(B.13)

( ) ( 0   ˆ e k [  k ˆ e k

k

min

1

 E

(

|| 

) 0 

||

vaø

Trường hợp

, theo (3.63) phương trình (B.11)

 k

T   1 k k

k

ˆ e k

m  k

được viết lại:

 V k  ( ) | Q E k 1   2

B3

)

 E k

 E

2 |

(

)

(

T )]



 







(B.14)

min

1

1

 V k

 Q E ( ) | k

k

k

ˆ e k

[  k

ˆ e k

  k

 T   k k

 T     k k

 

1 2

T ˆ( e   1 k k k 2 || ||  k

)

 E k

2 |





(B.15)

 V    k

 ( ) | Q E k

k

 T   k k

min

1

1 2

T ˆ( e   1 k k k 2 || ||  k

 E

) 0 

Trong

trường hợp này

;

và

nên

k

T k

k

ˆ( e  1 k k

k

k

 E

)

k

0



khi đó phương trình (B.14) có thể được viết như (B.13).

 T   k k

ˆ( e  1 k k 2 || ||

T k  k

Từ (B.2) và (B.13) cho thấy

và

, nhưng không đảm bảo

|| || 2 ||   ||   k m m

 ke t ( )

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,

bị chặn,

 kE t 1( ) L L

.

 kE t 1( )

L

Tích phân 2 vế (B.13) ta được:





2 |

dt

 kE t 1( )

(B.16)

min

1

V k

 ( ) | Q E k

k

0

1 2

     

  

0



2 |

dt





(B.17)

1

  | E k

V (0) k (1/ 2)

) )

( V   k Q ( k min

0

,

Bởi vì vế phải của (B.16) bị chặn nên

theo bổ đề Barlalat [37] ta có

 kE t ( )

1

L 2

( ) |

0



.

 E t k 1

lim | t 

Với định nghĩa hàm Lyapunov hệ thống như (B.2) khi này:

3

3

2 |

 V





(B.18)

min

1

 ( ) | Q E k

k

   V k



1 2

k

k

1 

1 

0

khi t  

Chứng tỏ

kE t  1( )

Tiếp theo xét phương trình sai số động học bộ quan sát (3.38). Đặt

2

2

u

g

ˆ( e

)





g v w d 



, theo luật cập nhật (3.62), (3.63) và giả thiết

k

 T   j kj j

j

kj

j

k

k





j

j

1 

1 



1,3 cho thấy

là ma trận Hurwitz, khi đó theo bổ đề

ku bị chặn. Mặt khác

A 0

k

K C 0 k

T k

1 ta có:



B4

t

  0

k

||

( ) ||

e

||

(0) ||

||

u





(B.19)

 e t k

k

 e k

k

 0

|| 2  k

|| B ||  k k 0 2    k k

0

là ma trận Hurwitz và

bị chặn, từ phương trình động

Bởi vì

A 0

k

B K k

T ck

ke t ( )





bị chặn. Vì

do đó

học bộ quan sát trạng thái (3.38) cho thấy ˆ ( ) ke t

 e k

e k

ˆ e k

0

( )

khi t   dẫn tới

E e 1,k k

kE t  1( )

ˆ,k x x k

ky t theo

L và

L . Sự giới hạn của

t ( )

bởi

.

kE t và 1( )

inky