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

f x x k x k T x k x k a k
T x k x k x k x k a k
T x k x k x k x k a k Tg
 (2.62) 
Ở đây, T là bước tính khi rời rạc hóa. 
Trong trường hợp , ,p q rb b b biến đổi chậm các hàm động học 1( )j kf x với 
5,7j được coi: 
1( ) ( ) ( 1)j k j jf x x k x k (2.63) 
Với cách đặt biến như (2.48), từ các phương trình (1.23) ta có hàm động học 
theo độ sâu 11f được viết dưới dạng rời rạc: 
11 11 1011 1 ( ) ( 1) ( 1)( )k x k x k Tx kf x (2.64) 
Như vậy các phương trình từ (2.56) đến (2.64) cho đầy đủ các hàm động học 
1 2 11, ,...,f f f của véc tơ hàm 1 1( )k kF X . 
Tiếp theo xác định ma trận chuyển vị 
1
1
ˆ1 ( )k
k
k X X
F
X 
  bằng cách lấy đạo 
hàm riêng i
j
f
x

 , nhận được [47]: 1 ij 1,11 1,11, ;k ij
f i j
x
  
  (2.65) 
trong đó, 12 5( 1) ( 1) / 2p k x k T  ; 13 6( 1) ( 1) / 2q k x k T  ; 
 14 7( 1) ( 1) / 2r k x k T  ; 15 2 ( 1) / 2;x k T 16 3 ( 1) / 2;x k T 
 17 4 ( 1) / 2x k T ; 21 5( 1) ( 1) / 2p k x k T  ; 
 23 7( 1) ( 1) / 2r k x k T  ; 24 6( 1) ( 1) / 2q k x k T  ; 
 25 1( 1) / 2x k T ; 26 4 ( 1) / 2x k T ; 27 3 ( 1) / 2x k T ; 
54 
 31 6( 1) ( 1) / 2q k x k T  ; 32 7( 1) ( 1) / 2r k x k T  ; 
 34 5( 1) ( 1) / 2p k x k T  ; 35 4 ( 1) / 2x k T ; 36 1( 1) / 2x k T ; 
37 2 ( 1) / 2x k T ; 41 7( 1) ( 1) / 2r k x k T  ; 42 6( 1) ( 1) / 2q k x k T  ; 
 43 5( 1) ( 1) / 2p k x k T  ; 45 3( 1) / 2x k T ; 46 2 ( 1) / 2x k T ; 
47 1( 1) / 2x k T ; 
81 1 4 34 ( 1) ( 1) 2 ( 1) ( 1) 2 ( 1) ( 1)x y zTx k a k Tx k a k Tx k a k ; 
82 2 3 44 ( 1) ( 1) 2 ( 1) ( 1) 2 ( 1) ( 1)x y zTx k a k Tx k a k Tx k a k ; 
83 2 12 ( 1) ( 1) 2 ( 1) ( 1)y zTx k a k Tx k a k ; 
84 1 22 ( 1) ( 1) 2 ( 1) ( 1)y zTx k a k Tx k a k ; 
91 4 1 22 ( 1) ( 1) 4 ( 1) ( 1) 2 ( 1) ( 1)x y zTx k a k Tx k a k Tx k a k ; 
92 3 12 ( 1) ( 1) 2 ( 1) ( 1)x zTx k a k Tx k a k ; 
93 2 3 42 ( 1) ( 1) 4 ( 1) ( 1) 2 ( 1) ( 1)x y zTx k a k Tx k a k Tx k a k ; 
94 1 32 ( 1) ( 1) 2 ( 1) ( 1)x zTx k a k Tx k a k ; 
101 3 2 12 ( 1) ( 1) 2 ( 1) ( 1) 4 ( 1) ( 1)x y zTx k a k Tx k a k Tx k a k ; 
102 4 12 ( 1) ( 1) 2 ( 1) ( 1)x yTx k a k Tx k a k ; 
103 1 42 ( 1) ( 1) 2 ( 1) ( 1)x yTx k a k Tx k a k ; 
104 2 3 42 ( 1) ( 1) 2 ( 1) ( 1) 4 ( 1) ( 1)x y zTx k a k Tx k a k Tx k a k ; 
1,111,ii i , còn tất cả các ij khác đều bằng 0. 
Từ các phương trình (2.49) đến (2.55) các hàm với các số hạng có thừa số nhiễu 
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: 
1 2 1 3 2 4 3( 1) / 2 ( 1) / 2 ( 1) / 2g x k wT x k w T x k w T (2.66) 
2 1 1 4 2 3 3( 1) / 2 ( 1) / 2 ( 1) / 2g x k wT x k w T x k w T (2.67) 
3 4 1 1 2 2 3( 1) / 2 ( 1) / 2 ( 1) / 2g x k wT x k w T x k w T (2.68) 
4 3 1 2 2 1 3( 1) / 2 ( 1) / 2 ( 1) / 2g x k wT x k w T x k w T (2.69) 
55 
 
 
2 2
8 1 2 4 2 3 1 4 5
2 4 1 3 6
2 ( 1) 2 ( 1) 1 2 ( 1) ( 1) 2 ( 1) ( 1)
2 ( 1) ( 1) 2 ( 1) ( 1)
g T x k x k w T x k x k x k x k w
T x k x k x k x k w
 (2.70) 
 
 
2 2
9 2 3 1 4 4 1 3 5
3 4 1 2 6
2 ( 1) ( 1) 2 ( 1) ( 1) 2 ( 1) 2 ( 1) 1
2 ( 1) ( 1) 2 ( 1) ( 1)
g T x k x k x k x k w T x k x k w
T x k x k x k x k w
 (2.71) 
 
 
2 2
10 1 4 6 2 4 1 3 4
3 4 1 2 5
2 ( 1) 2 ( 1) 1 2 ( 1) ( 1) 2 ( 1) ( 1)
2 ( 1) ( 1) 2 ( 1) ( 1)
g T x k x k w T x k x k x k x k w
T x k x k x k x k w
 (2.72) 
Lấy đạo hàm riêng ,i
j
g
w

 nhận được ma trận hệ số nhiễu [47]: 
 ij 1,11 1,6, ;i
j
G g g i j
w
  (2.73) 
trong đó, 
11 2 ( 1) / 2g x k T ; 12 3( 1) / 2g x k T ; 13 4 ( 1) / 2g x k T ; 21 1( 1) / 2g x k T ; 
22 4 ( 1) / 2g x k T ; 23 3( 1) / 2g x k T ; 31 4 ( 1) / 2g x k T ; 32 1( 1) / 2g x k T ; 
33 2 ( 1) / 2g x k T ; 41 3( 1) / 2g x k T ; 42 2 ( 1) / 2g x k T ; 43 1( 1) / 2g x k T ; 
2 2
84 1 2(2 ( 1) 2 ( 1) 1);g T x k x k 85 2 3 1 4(2 ( 1) ( 1) 2 ( 1) ( 1));g T x k x k x k x k 
86 2 4 1 3(2 ( 1) ( 1) 2 ( 1) ( 1));g T x k x k x k x k 
94 2 3 1 2(2 ( 1) ( 1) 2 ( 1) ( 1));g T x k x k x k x k 2 295 1 3(2 ( 1) 2 ( 1) 1);g T x k x k 
96 3 4 1 3(2 ( 1) ( 1) 2 ( 1) ( 1));g T x k x k x k x k 
104 3 4 1 2(2 ( 1) ( 1) 2 ( 1) ( 1));g T x k x k x k x k 
105 3 4 1 2(2 ( 1) ( 1) 2 ( 1) ( 1));g T x k x k x k x k 2 2106 1 4(2 ( 1) 2 ( 1) 1);g T x k x k 
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ế. 
Vấn đề tiếp theo là tìm các hàm quan sát. Các hàm quan sát 1 2 3, ,z z z do ba từ kế 
đo được như trường hợp ASWs rơi trong khí quyển đã trình bày, tức 1 2 3, ,z z z xác 
định theo các biểu thức (2.31), (2.32) và (2.33). 
56 
Từ thông tin ba thành phần vận tốc trong hệ tọa độ gắn liền cho ta ba hàm quan 
sát 4 5 6, ,z z z , vận tốc đo được từ vận tốc kế cho chỉ số DVL DVL DVL T( , , )DVL x y zV V V V . 
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: 
DVL DVL DVL T T( , , ) ( ) ( , , )n Tx y z b N E DV V V C V V V (2.74) 
Triển khai biểu thức (2.74) với ma trận nbC như (1.12). Với cách đặt biến như 
(2.48), ba hàm quan sát có được là: 
2 2
4 4 1 2 8 2 3 1 4 9
2 4 1 3 10
( ) ( ) (2 ( ) 2 ( ) 1) ( ) (2 ( ) ( ) 2 ( ) ( )) ( )
(2 ( ) ( ) 2 ( ) ( )) ( )
kh X z k x k x k x k x k x k x k x k x k
x k x k x k x k x k
 (2.75) 
2 2
5 5 2 3 1 4 8 1 3 9
3 4 1 2 10
( ) ( ) (2 ( ) ( ) 2 ( ) ( )) ( ) (2 ( ) 2 ( ) 1) ( )
(2 ( ) ( ) 2 ( ) ( )) ( )
kh X z k x k x k x k x k x k x k x k x k
x k x k x k x k x k
 (2.76) 
6 6 2 4 1 3 8
2 2
3 4 1 2 9 1 4 10
( ) ( ) (2 ( ) ( ) 2 ( ) ( )) ( )
(2 ( ) ( ) 2 ( ) ( )) ( ) (2 ( ) 2 ( ) 1) ( )
kh X z k x k x k x k x k x k
x k x k x k x k x k x k x k x k
 (2.77) 
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 
7 7 11( ) ( ) ( )kh X z k x k (2.78) 
Từ (2.31 – 2.33) và (2.75 – 2.78) ta có đầy đủ 7 hàm quan sát 1 2 7( , ,..., )h h h của 
véc tơ hàm ( )kh X . 
Việc xác định ma trận đo ˆ ( )kkk X X
hH
X 
  bằng cách lấy đạo hàm riêng 
i
j
h
x

 
nhận được [47]: , 1,7; 1,11k ijH h i j . (2.79) 
trong đó, 
, 1,3; 1, 4ijh i j như trường hợp ASWs rơi trong khí quyển đã xét; 
41 1 8 4 9 3 104 ( ) ( ) 2 ( ) ( ) 2 ( ) ( )h x k x k x k x k x k x k ; 
42 2 8 3 9 4 104 ( ) ( ) 2 ( ) ( ) 2 ( ) ( )h x k x k x k x k x k x k ; 43 2 9 1 102 ( ) ( ) 2 ( ) ( )h x k x k x k x k ; 
44 1 9 2 102 ( ) ( ) 2 ( ) ( )h x k x k x k x k ; 2 248 1 22 ( ) 2 ( ) 1h x k x k ; 
49 2 3 1 42 ( ) ( ) 2 ( ) ( )h x k x k x k x k ; 410 2 4 1 32 ( ) ( ) 2 ( ) ( )h x k x k x k x k ; 
51 1 9 4 8 2 104 ( ) ( ) 2 ( ) ( ) 2 ( ) ( )h x k x k x k x k x k x k ; 52 3 8 1 102 ( ) ( ) 2 ( ) ( )h x k x k x k x k ; 
53 2 8 3 9 4 102 ( ) ( ) 4 ( ) ( ) 2 ( ) ( )h x k x k x k x k x k x k ; 54 1 8 3 102 ( ) ( ) 2 ( ) ( )h x k x k x k x k ; 
57 
58 2 3 1 42 ( ) ( ) 2 ( ) ( )h x k x k x k x k ; 2 259 1 32 ( ) 2 ( ) 1h x k x k ; 
510 3 4 1 22 ( ) ( ) 2 ( ) ( )h x k x k x k x k ; 61 1 10 3 8 2 94 ( ) ( ) 2 ( ) ( ) 2 ( ) ( )h x k x k x k x k x k x k ; 
62 4 8 1 92 ( ) ( ) 2 ( ) ( )h x k x k x k x k ; 63 1 8 4 92 ( ) ( ) 2 ( ) ( )h x k x k x k x k ; 
64 2 8 3 9 4 102 ( ) ( ) 2 ( ) ( ) 4 ( ) ( )h x k x k x k x k x k x k ; 
68 2 4 1 32 ( ) ( ) 2 ( ) ( )h x k x k x k x k ; 69 3 4 1 22 ( ) ( ) 2 ( ) ( )h x k x k x k x k ; 
2 2
610 1 42 ( ) 2 ( ) 1h x k x k ; 711 1.h tất cả các ijh khác đều bằng 0. 
Giả sử nhiễu đo của từ kế, vận tốc kế và cảm biến áp suất là ồn trắng thì ta có: 
1 1 1( )k TXz h v ; 2 2 2( )k TXz h v ; 3 3 3( )k TXz h v ; 4 4 1( )k VXz h v ; 5 5 2( )k VXz h v ; 
6 6 3( )k VXz h v ; 7 7 ( ) .k zz h X v 
trong đó, 1 2 3, ,T T Tv v v là nhiễu đo của 3 từ kế; 1 2 3, ,V V Vv v v là nhiễu đo của vận tốc kế 
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. 
Như vậy đã có đầy đủ yếu tố ( 1 1 1 1( ), ( ), ( ), ,k k k k k kF X h X G X Q R đã được xác 
định) để thực hiện thủ tục lọc Kalman (hình 2.5). Kết quả ước lượng theo lưu đồ 
giải thuật bộ lọc Kalman phi tuyến mở rộng nhận được 4 tham số Rodrig-Hamilton 
0 1 2 3, , ,    , các thành phần vận tốc , ,N E DV V V và độ sâu của ASWs trong hệ tọa độ 
địa lý. Từ đó xác định được 3 góc định hướng , ,   theo công thức (1.15) và tọa 
độ tâm khối của ASWs trong mặt phẳng ngang ( , )x y bằng cách giải hai phương 
trình vi phân ở biểu thức (1.23) theo sơ đồ hình 2.5, trong đó các ma trận , ,M N S 
được xác định: 
1 0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 0 0
M
 (2.80) 
 0 0 0 0 0 0 0 0 0 0 1N (2.81) 
0 0 0 0 0 0 0 1 0 0 0
0 0 0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 0 0 0 1 0
S
. (2.82) 
58 
Hình 2.5: Sơ đồ xác định tham số dẫn đường khi kết hợp con quay vi cơ, 
gia tốc kế, từ kế, vận tốc kế và cảm biến áp suất 
Hàm 
1
( )
1
ˆ( )
kk
F X 
 được thiết lập từ các phương trình (2.56-2.64), trong đó 1( )ˆ kX là 
véc tơ trạng thái ước lượng ở bước thứ 1k . Hàm ( )ˆ( )
kk
h X được thiết lập từ các 
phương trình (2.31-2.33) và các phương trình (2.75-2.78), trong đó ( )ˆ
k
X là véc tơ 
trạng thái ước lượng ở bước thứ k . 
kK   1( )1 ˆ( )kkF X 
( )ˆ( )
kk
h X  
Hiệu chỉnh 
ma trận kK 
R Q  
1 7( ,..., )
T
kZ z z  
ˆ
kZ  
( )ˆ
k
X  
( )ˆ
k
X  
( )ˆ
k
X  
      M  0 1 2 3
, , ,     
(0)y  
y
1 2 0 3
2 2
0 1
1 3 0 2
2 3 0 1
2 2
0 3
arcsin
2 2
2 2 1
(2 2 )
2 2
2 2 1
arctg
arctg


   
 
   
   
 
  
  
  
      N  
       S  
(0)x  
x
z
3 gia tốc 
kế 
3 con quay 
vi cơ 
phương tiện đo quán tính 
Cảm biến áp suất 
3 vận 
tốc kế 
3 từ kế 
phương tiện đo không 
quán tính 
Các phương tiện đo 
NV  
EV  
Lọc EKF 
Xác định tham số dẫn đường 
NV  
EV  
DV  
59 
2.2 Xây dựng thuật toán dẫn đường quán tính có đế cho vũ khí chống ngầm 
Khi sử dụng thiết bị dẫn đường quán tính có đế dạng giải tích để xác định các 
tham số dẫn đường cho ASWs cần phải xác định ma trận Côsin định hướng ndC 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
dC cũng cho phép xác định được ma trận Côsin định hướng nbC giữa hệ tọa độ gắn 
liền và hệ tọa độ địa lý theo công thức (1.24), từ đó có thể tính toán được các góc 
định hướng theo công thức (1.15). 
Việc xác định ma trận Côsin định hướng ndC đượ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 độ đế ( D D DOX Y Z ) Hệ tọa độ gắn liền với ASWs ( b b bOX Y Z ) hệ tọa độ gắn 
liền với phương tiện mang ( m m mOX Y Z ) hệ tọa độ địa lý ( 0 0 0OX Y Z ) (hình 2.6). 
Hình 2.6: Quan hệ giữa các hệ tọa độ 
(Bắc)
bX  
0Y  
DX  
DY  
bY  
O
mX  
mY  
0X  
(Đông) 
X
60 
Trong hình 2.6 các trục 0, , ,D b mOZ OZ OZ OZ vuông góc với mặt phẳng của trang 
giấy nên không được thể hiện. Gọi ma trận Côsin định hướng của 3 quan hệ trên lần 
lượt là , ,b m nd b mC C C , khi đó: 
n b m n
d d b mC C C C (2.83) 
Từ 3 góc 1 2 3, , do 3 cảm biến góc của thiết bị đế cung cấp sẽ xác định ma 
trận bdC tương tự biểu thức (1.5) như sau: 
1 2 1 2 3 1 3 1 2 3 1 3
1 2 1 2 3 1 3 1 2 3 1 3
2 2 3 2 3
cos cos cos sin sin sin cos cos sin cos sin sin
sin cos sin sin sin cos cos sin sin cos cos sin
sin cos sin cos cos
b
dC
(2.84) 
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
bC được xác định một lần khi thiết kế vị trí và phương treo ASWs, tức là mbC phải 
được ghi trong tài liệu kỹ thuật của tổ hợp máy bay thả ASWs. Trên máy bay phải 
có thiết bị đo các góc định hướng của máy bay với các phương Bắc – Nam, Đông – 
Tây, thẳng đứng. Từ thông tin của các thiết bị này sẽ xác định được ma trận nmC . 
Tuy nhiên phương pháp đo trực tiếp ma trận định hướng ndC vừa nêu có độ 
chính xác không cao vì các sai số của nhiều thiết bị đo. Luận án đưa ra thuật toán 
xác định ma trận Côsin định hướng ndC theo cách tiếp cận khác (xác định tinh) có sử 
dụng kết quả của công đoạn thô làm điều kiện đầu. 
Trong trường hợp ASWs thả từ máy bay, trước khi ASWs rời khỏi máy bay để 
hoạt động độc lập thì ASWs được liên kết chặt với máy bay, khi này có thể kết hợp 
thông tin từ các thiết bị đo vận tốc của máy bay với thông tin của thiết bị dẫn đường 
quán tính để thực hiện nhiệm vụ xác định ma trận Côsin định hướng ndC . 
2.2.1 Xây dựng thuật toán xác định ma trận Côsin định hướng giữa hệ tọa độ 
đế với hệ tọa độ địa lý theo phương pháp phối hợp véc tơ vận tốc khi thả 
vũ khí chống ngầm từ máy bay phản lực 
Vì hệ tọa độ đế của thiết bị dẫn đường quán tính dạng giải tích được ổn định 
trong không gian quán tính nên dễ dàng nhận thấy tốc độ quay của hệ tọa độ đế 
0  , khi đó phương trình (1.13) sẽ có dạng: 
61 
2   (2.85) 
Trong miền số thực các tham số Rodrig-Hamilton được viết: 
0 1 3 22 N E D        (2.86) 
1 0 2 32 N E D        (2.87) 
2 3 1 02 N E D        (2.88) 
3 2 0 12 N E D        (2.89) 
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]: 
cosEN
V U
R
  ; NE VR ; sin
E
D
V tg U
R
   
(2.90) 
Đặt các biến của véc tơ trạng thái X : 
1 0 2 1 3 2 4 3 5 6 7; ; ; ; ; ;N E Dx x x x x V x V x V    (2.91) 
Thay (2.90) vào các phương trình (2.86 – 2.89) với cách đặt biến như (2.91), 
các phương trình (2.86 – 2.89) được viết lại: 
6 6 5
1 2 3 42 ( cos ) ( sin )
x x xx U x U tg x x
R R R
    (2.92) 
6 5 6
2 1 3 42 ( cos ) ( sin )
x x xx U x x U tg x
R R R
    (2.93) 
6 5 6
3 1 2 42 ( sin ) ( cos )
x x xx U tg x x U x
R R R
    (2.94) 
eX  
O
0X 
eZ
eY
Đường xích đạo 
0Z 
0Y

l
N  
E  
D  
EV  NV  
DV  
R
62 
5 6 6
4 1 2 32 ( sin ) ( cos )
x x xx x U tg x U x
R R R
    (2.95) 
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: 
4 5 6; ;x dx y dy z dzn n w n n w n n w (2.96) 
Viết lại các phương trình (2.96): 
4 5 6; ;dx x dy y dz zn n w n n w n n w (2.97) 
trong đó, 4 5 6, ,w w w là nhiễu đo của gia tốc kế thường có dạng ồn trắng. 
Viết lại (1.21) với ndC có dạng (1.12): 
2 2
0 1 1 2 0 3 1 3 0 2=(2 2 1) (2 2 ) (2 2 )N dx dy dzf n n n          (2.98) 
2 2
1 2 0 3 0 2 2 3 0 1=(2 2 ) (2 2 1) (2 2 )E dx dy dzf n n n          (2.99) 
2 2
1 3 0 2 2 3 0 1 0 3=(2 2 ) (2 2 ) (2 2 1)D dx dy dzf n n n          (2.100) 
Đặt 1 2 3 4 5 6( , , ) ( , , )w w w   , thay các phương trình (2.97) vào các phương trình 
(2.98) đến (2.100) ta có: 
2 2
0 1 1 1 2 0 3 2 1 3 0 2 3=(2 2 1)( ) (2 2 )( ) (2 2 )( )N x y zf n n n             (2.101) 
2 2
1 2 0 3 1 0 2 2 2 3 0 1 3=(2 2 )( ) (2 2 1)( ) (2 2 )( )E x y zf n n n             (2.102) 
2 2
1 3 0 2 1 2 3 0 1 2 0 3 3=(2 2 )( ) (2 2 )( ) (2 2 1)( )D x y zf n n n             (2.103) 
Thay các phương trình (2.101) đến (2.103) vào các phương trình (1.22), phương 
trình vận tốc tâm khối của ASWs được viết lại: 
2 2
0 1 1 1 2 0 3 2 1 3 0 2 3V =(2 2 1)( ) (2 2 )( ) (2 2 )( )N x y zn n n              (2.104) 
2 2
1 2 0 3 1 0 2 2 2 3 0 1 3V =(2 2 )( ) (2 2 1)( ) (2 2 )( )E x y zn n n              (2.105) 
2 2
1 3 0 2 1 2 3 0 1 2 0 3 3V =(2 2 )( ) (2 2 )( ) (2 2 1)( )D x y zn n n g              (2.106) 
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 2
5 1 2 1 2 3 1 4 2 2 4 1 3 3
2 2
1 2 2 3 1 4 2 4 1 3
2 2
1 2 1 2 3 1 4 2 2 4 1 3 3
(2 2 1)( ) 2( )( ) 2( )( )
(2 2 1) 2( ) 2( )
(2 2 1) 2( ) 2( )
x y z
x y z
x x x n x x x x n x x x x n
x x n x x x x n x x x x n
x x x x x x x x x x
  
  

 (2.107) 
63 
2 2
6 1 3 2 2 3 1 4 1 3 4 1 2 3
2 2
1 3 2 3 1 4 3 4 1 2
2 2
1 3 2 2 3 1 4 1 3 4 1 2 3
(2 2 1)( ) 2( )( ) 2( )( )
(2 2 1) 2( ) 2( )
(2 2 1) 2( ) 2( )
y x z
y x z
x x x n x x x x n x x x x n
x x n x x x x n x x x x n
x x x x x x x x x x
  
  

 (2.108) 
2 2
7 1 4 3 2 4 1 3 1 3 4 1 2 2
2 2
1 4 2 4 1 3 3 4 1 2
2 2
1 4 3 2 4 1 3 1 3 4 1 2 2
(2 2 1)( ) 2( )( ) 2( )( )
(2 2 1) 2( ) 2( )
(2 2 1) 2( ) 2( )
z x y
z x y
x x x n x x x x n x x x x n g
x x n x x x x n x x x x n g
x x x x x x x x x x
  
  

 (2.109) 
Rời rạc các phương trình (2.92) đến (2.95) theo phương pháp Ơle ta có các hàm 
động học 1 2 3 4, , ,f f f f : 
6
1 1 1 1 2
6 5
3 4
( 1)( ) ( ) ( 1) [( cos ) ( 1)
2
( 1) ( 1)sin ( 1) ( 1)]
k
x kTf X x k x k U x k
R
x k x kU tg x k x k
R R

 
 (2.110) 
6
2 1 2 2 1
5 6
3 4
( 1)( ) ( ) ( 1) [ ( cos ) ( 1)
2
( 1) ( 1)1) sin ( 1)](
k
x kTf X x k x k U x k
R
x k x kx k U tg x k
R R

 
 (2.111) 
6
3 1 3 3 1
5 6
2 4
( 1)( ) ( ) ( 1) [ ( sin ) ( 1)
2
( 1) ( 1)( 1) cos ( 1)]
k
x kTf x x k x k U tg x k
R
x k x kx k U x k
R R
 

 (2.112) 
5 6
4 1 4 4 1 2
6
3
( 1) ( 1)( ) ( ) ( 1) [ ) ( 1) sin ( 1)
2
( 1)cos ( 1)]
k
x k x kTf X x k x k x k U tg x k
R R
x kU x k
R
 

 (2.113) 
Tách các số hạng có thừa số nhiễu 1 2 3, ,   trong các phương trình vi phân 
(2.107) đến (2.109), sau đó rời rạc các số hạng còn lại theo phương pháp Ơle ta có 
các hàm động học 5 6 7, ,f f f : 
 
 
2 2
5 1 5 5 1 2
2 3 1 4
2 4 1 3
( ) ( ) ( 1) 2 ( 1) 2 ( 1) 1 ( 1)
2 ( 1) ( 1) 2 ( 1) ( 1) ( 1)
2 ( 1) ( 1) 2 ( 1) ( 1) ( 1)
k x
y
z
f x x k x k T x k x k n k
T x k x k x k x k n k
T x k x k x k x k n k
 (2.114) 
64 
 
 
2 2
6 1 6 6 1 3
2 3 1 4
3 4 1 2
( ) ( ) ( 1) 2 ( 1) 2 ( 1) 1 ( 1)
2 ( 1) ( 1) 2 ( 1) ( 1) ( 1)
2 ( 1) ( 1) 2 ( 1) ( 1) ( 1)
k y
x
z
f x x k x k T x k x k n k
T x k x k x k x k n k
T x k x k x k x k n k
 (2.115) 
 
 
2 2
7 1 7 7 1 4
2 4 1 3
3 4 1 2
( ) ( ) ( 1) 2 ( 1) 2 ( 1) 1 ( 1)
2 ( 1) ( 1) 2 ( 1) ( 1) ( 1)
2 ( 1) ( 1) 2 ( 1) ( 1) ( 1)
k z
x
y
f x x k x k T x k x k n k
T x k x k x k x k n k
T x k x k x k x k n k Tg
 (2.116) 
Ở đây T là bước tính rời rạc hóa. 
 Ma trận chuyển vị được xác định bằng cách lấy đạo hàm riêng i
j
f
x

 , nhận được 
[47]: ij , 1,7; 1,7i
j
f i j
x
    
 (2.117) 
trong đó, 
11 1 ; 12 6[( cos ] / 2( 1) /UT Rx k  ; 13 6[( sin ] / 2( 1) /U tgT Rx k   ; 
14 5( 1) / 2T Rx k ; 15 4( 1) / 2T Rx k ; 16 2 3[ ( 1) ( 1)] / 2T tg Rx k x k  ; 17 0 ; 
21 6[( cos ] / 2( 1) /UT Rx k  ; 22 1 ; 23 5( 1) / 2T Rx k ; 
24 6[( sin ] / 2( 1) /U tgT Rx k   ; 25 3( 1) / 2T Rx k ; 
26 1 4[ ( 1) ( 1)]2T tg Rx k x k  ; 27 0 ; 
31 6[ ] / 2sin ( 1) /T tg RU x k   ; 32 5( 1) / 2T Rx k ; 33 1 ; 
34 6[( cos ] / 2( 1) /UT Rx k  ; 35 2 ( 1) / 2T Rx k ;
36 1 4[ ( 1) ( 1)] / 2T tg Rx k x k  ; 37 0 ; 
41 5( 1) / 2T Rx k ; 42 6[( sin ] / 2( 1) /U tgT Rx k  ; 
43 6[( cos ] / 2( 1) /UT Rx k ; 44 1 ; 45 1( 1) / 2T Rx k ; 
 46 2 3[ ( 1) ( 1)] / 2T tg Rx k x k  ; 47 0 ; 
51 1 4 34 2( 1) ( 1) 2 ( 1)x y zT Tx k n x k n Tx k n ; 
52 2 3 44 2( 1) ( 1) 2 ( 1)x y zT Tx k n x k n Tx k n ; 
53 2 12 ( 1) 2 ( 1)y zTx k n Tx k n ; 54 1 22 ( 1) 2 ( 1)y zTx k n Tx k n ; 55 1 ; 56 0 ; 57 0 ; 
61 4 1 22 2( 1) ( 1) 2 ( 1)x y zT Tx k n x k n Tx k n ; 62 3 12 2( 1) ( 1)x zT Tx k n x k n ; 
65 
63 2 3 42 4( 1) ( 1) 2 ( 1)x y zT Tx k n x k n Tx k n ; 64 1 32 2( 1) ( 1)x zT Tx k n x k n ; 
65 0 ;