Tổng hợp bộ quan sát trạng thái trong hệ thống điều khiển phương tiện ngầm

Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san FEE, 08 - 2018 27
TỔNG HỢP BỘ QUAN SÁT TRẠNG THÁI 
TRONG HỆ THỐNG ĐIỀU KHIỂN PHƯƠNG TIỆN NGẦM 
Phạm Văn Phúc1*, Trần Đức Thuận2, Nguyễn Quang Vịnh2 
Tóm tắt: Bài báo trình bầy một phương pháp thiết kế bộ lọc tín hiệu hướng trên 
cơ sở bộ quan sát trạng thái trong hệ thống điều khiển của phương tiện ngầm. 
Trong đó chuyển động của phương tiện ngầm được coi như vật rắn chuyển động 6 
bậc tự do; hệ thống lái tự động trong phương tiện ngầm được đề xuất sử dụng cấu 
trúc bộ quan sát trạng thái từ đó sẽ tính toán và lọc thành phần tần số thấp ra khỏi 
nhiễu do dòng chảy đại dương tác động đến PTN. Kết quả mô phỏng bằng 
MATLAB/SIMULINK đã chứng minh tính đúng đắn của thuật toán. 
Từ khóa: Bộ quan sát trạng thái; Phương tiện ngầm. 
1. ĐẶT VẤN ĐỀ 
Có rất nhiều các công trình nghiên cứu về phương pháp tuyến tính hóa, với các kết quả 
thu được cho thấy phương pháp này có triển vọng rất lớn trong điều khiển phi tuyến 
[2,5,7]. Song nó lại có nhược điểm là phải xác định được chính xác giá trị của các biến 
trạng thái của đối tượng (thông thường là đo trực tiếp bằng các thiết bị sensor) để đưa phản 
hồi về bộ điều khiển, nhưng với phương tiện ngầm có nhiều biến trạng thái không thể đo 
đếm trực tiếp được. Vì vậy, trong bài báo này tác giả đã đưa ra một phương pháp để xác 
định giá trị của các biến trạng thái - phương pháp quan sát thông qua các tín hiệu vào/ra, 
thay cho việc đo trực tiếp. 
2. NỘI DUNG 
2.1. Phương trình chuyển động phương tiện ngầm 
Vị trí và góc định hướng của phương tiện ngầm  , , , , ,
T
x y z    được mô tả tương 
đối trong hệ tọa độ cố định tâm trái đất OXYZ , vận tốc tuyến tính và vận tốc góc 
 , , , , ,
T
u v p q rw được mô tả trong hệ tọa độ gắn liền b b b bC X Y Z có tâm trùng với tâm 
nổi bC (hình 1). 
Phương trình động học phương tiện ngầm được mô tả[1]: 
 (1) 
trong đó, là ma trận chuyển tọa độ. 
 Phương tiện ngầm tự hành (AUV) được điều khiển bởi hai bánh lái trong mặt phẳng 
ngang, hai bánh lái theo mặt phẳng đứng . Giả sử chuyển động phương tiện ngầm chỉ xét 
đến sự ảnh hưởng của dòng chảy, bỏ qua sự ảnh hưởng của sóng và gió. 
Khi xét đến sự ảnh hưởng của vận tốc dòng chảy tác động lên phương tiện ngầm, 
phương trình chuyển động của phương tiện ngầm sẽ là (Sorensen, 2005a): 
 (2) 
trong đó, là ma trận quán tính; là ma trận hướng tâm Coriolis; là 
ma trận quán tính và ma trận hướng tâm Coriolis khối nước kèm; là ma trận lực và 
mô men thủy động; là véc tơ lực và mô men phục hồi; là ma trận lực và mô 
men của bánh lái; là lực và mô men của bánh lái, vây và chân vịt; 
 , , , , ,
T
td c td td tdv u v w p q r  
( )J   
( )J 
( ) ( ( ) ( ) ) ( ) ( ) ( )
RB A A td td RB td td td td
M M C C D L g           
RB
M
RB
C , ( )
A A td
M C 
( )
td
D 
( )g  ( )
td
L 
bl cvit   
Kỹ thuật Điều khiển – Tự động hóa 
 P. V. Phúc, T. Đ. Thuận, N. Q. Vịnh, “Tổng hợp bộ quan sát  phương tiện ngầm.” 28 
với  , , , , ,
T
c c c cu v w 0 0 0 là vận tốc dòng chảy đối với hệ tọa độ gắn liền vật thể. 
Giả thiết vận tốc chuyển động thẳng của AUV luôn là hằng số , từ (2) ta có hệ 
phương trình chuyển động tổng quát của AUV là [2],[4]: 
| | | |
| | 0
| |
2
2
( ) ( | | ) ( )
) ( )
( ) ( | | ) ( )
( ) ( |
| | (
l
h
u v v uv uw w w
uu
v v v uv ur uu h
w q w w
uu up wq
qq vr rr pl l
r wp pq
m X u X X v X u v X X w X
mw q X r X X u
m Y v Y Y v Y u v mpw p Y q Y u mu r Y u
m Z w Z q mqu Z w
uu u w up X w
X q v X r mv
r Y w p






 



 

2
2
2
| )
( ) cos sin
( )
( )
( )
( )
( ) ( )
sin cos cos
s
l
s
uw uq uu s
xx p f uu
w yy q uq
uu s
v zz r
vp rp
uu up yy zz l
uw vp rp zz xx
f f
Z u w mv Z uq Z u
I K p K p z B K u
M w I M q M M uq
z M u
N v I N
Z v Z r p
uu K u I I qr
uw M v M r p I I rp
B x B




  
  

 
 

 
 
2
cos sin( )
h
uv ur f
uu h
wp xx yy pqr N uv p I I pq N ur x B
N u
N w N pq

 

(3) 
0
0
0
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
tan sin tan cos
cos sin
x u v w
y u v w
z u v w
p q r
q r
q
           
           
    
    
  






 sin sec cos secr   
(4) 
Hình 1. Các hệ tọa độ tham chiếu chuyển động của AUV. 
Từ các phương trình (3) và (4) có thể xây dựng được phương trình chuyển động riêng 
cho kênh điều khiển theo hướng, theo độ sâu và điều khiển giảm lắc [4]. 
Xét trong mặt phẳng ngang với giả thiết tốc độ là không đổi và , góc hướng 
nhỏ, các thông số ban đầu khi đó: 
; ; . 
Chuyển động của AUV trong mặt phẳng ngang được mô tả bởi hệ phương trình sau [5]: 
2
| |
2
0 | | 0
0 0
0 0 1 0 1 0 0
h
h
uu tdv r v v td uv td td ur td td
v zz r uv td ur td uu td h
Y um Y Y v Y v Y u mu Y u v
N I N r N u N u r N u


 

 
 



(5) 
0u u 
0w p q 
0 0 0r 
r  0 0cos sinx u v u v    0 0sin cosy u v u v   
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san FEE, 08 - 2018 29
Tương tự như vậy, chuyển động của AUV trong mặt phẳng đứng được mô tả bởi hệ 
phương trình sau: 
2
| |
2
0 0 | | 0 0
0 0 0 0
0 0 1 0 0 1 0 0 0
0 0 0 1 0 0 0 0 0
s
s
uu tdw q w w td uw td uq td td
w yy q uw td uq td uu td
s
Z um Z Z w Z w Z u Z u w
M I M q M u M u q M u
z z


 

 
 




(6) 
Sử dụng hệ phương trình (5),(6) để xây dựng bài toán tổng hợp bộ quan sát trạng thái 
cho chuyển động theo góc hướng và độ sâu với các hệ số được tính toán dựa vào các chỉ 
số kích thước của một chủng loại AUV. 
2.2. Tổng hợp bộ quan sát trạng thái cho chuyển động của góc hướng 
Bộ quan sát trạng thái theo góc hướng được xây dựng sao cho đầu vào là góc bẻ lái 
hướng r , đầu ra là góc hướng  thay đổi trong giới hạn cho phép. 
Đặt các biến trạng thái: 
1
2
x r
x 
Hệ thống chuyển động góc hướng của AUV được viết dưới dạng [6]: 
1 2
2 3 2
3 3 2
0.0225 0,2331 0.0258
0,1114 0,2647 0,0211
r
r
x x
x x x
x x x


 


 (7) 
Phương trình trên có thể viết lại như sau : 
1 2
2 3 2, 0.0225 0,2331r
x x
x f b f x x


 (8) 
Trong đó f là thành phần bất định, với loại AUV đang nghiên cứu [7] thì chọn 
.b 0 0268 
Bộ quan sát nâng cao đề xuất có dạng: 
1/2
1 2 1 1 1 1
2 1 1
| | ( )
( )
rz z x z sign x z b
z sign x z
 


 (9) 
trong đó ,1 2z z là các biến trạng thái ước lượng cho ,1 2x x . 
Khi đó sai số được tính như sau: 
 1 1 1 2 2,e x z e z f (10) 
Lấy đạo hàm hai vế sai số 1 1 1,e x z    thay vào (8), (9) ta thu được: 
1/2
1 2 1 1
2 1
| e | ( )
' ( )
e e sign e f
e f sign e



 (11) 
Sau khoảng thời gian xác định 1 1 2 1 2 2, ( , , , )z x z f F t x x z với: 
Kỹ thuật Điều khiển – Tự động hóa 
 P. V. Phúc, T. Đ. Thuận, N. Q. Vịnh, “Tổng hợp bộ quan sát  phương tiện ngầm.” 30 
1 2 2 1 2 1 2 1 2 1 2 1 2
1 2 1 2
( , , , ) ( , , , ( , , )) - ( , , , )( , , , ( , , ))
( , , , ( , , )) lim
x
F t x x z f t x x U t x x f t x z t x z U t x x
t x x U t x x
 (12) 
Giả sử rằng các trạng thái của hệ thống bị giới hạn, và tồn tài một hằng số f thỏa 
mãn các bất phương trình sau: 
 1 2 2| ( , , , ) |F t x x z f
 (13) 
với: 1 2, ,t x x bất kỳ và 
2| |
2| | 2
xz ta nhận được: 
2 ( )(1 )
;
(1 )
f p
f
f p
 
 với p được chọn 0 1p (14) 
Khi ,  thỏa mãn theo điều kiện trên thì các biến trạng thái ước lượng hội tụ, khi đó 
các sai số 1 2,e e hội tụ đến 0, 1 2,e e thỏa mãn : 
 1/21 2 1 1| | ( )e e e sign e    (15) 
 2 1[ , ] ( )e f f sign e 
  (16) 
hay: 
 2 [ , ]e f f 
 với 1 0e . (17) 
 1 11/2
1
1
[ , ] ( )
2 | |
e
e f f signe
e
 

 (18) 
Khi: 1 2 2 20 0e e x x 
 1 0e với giá trị khởi tạo 2x >0 
 1 0e với giá trị khởi tạo 2 0x 
- Xét khi 1 0e 1 0,e  khi đó quỹ đạo được xác định giữa trục 1 10, 0e e  và 
quỹ đạo của phương trình 11 1 1 1/2
1
1
( ), e 0
2 | |
e
e f f signe
e
  

 
Hiển nhiên rằng khi 2 1e e  , ở thời điểm 1 0e và xét: 
 2 1 2 2 1( , , , ) ( )e F x x z u sign e  (19) 
ta được 20 | |f e f 
  , vì vậy 1| | ( ) ,i ie f t 
  với it là khoảng thời gian 
khi 1 0e , do đó ta có: 
 1
| |
( )
i
i
e
t
f 

 (20) 
Thời gian hội tụ tổng được ước lương theo: 
 1
| |
( )
ieT
f 


 (21) 
Trong thực tế với một loại AUV đang xét [7] chọn các giá trị sau: 1 21.1, 1.5a a 
1/2
1 2, ( )a f a f 
2.3. Tổng hợp bộ quan sát trạng thái cho hệ thống ổn định độ sâu 
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san FEE, 08 - 2018 31
Bộ quan sát trạng thái cho hệ thống ổn định sâu với đầu ra là góc chúc góc  thay đổi 
theo giới hạn cho trước. Khi đó các biến trạng thái được đặt như sau: 
1
2
3
x z
x
x q

 (22) 
Hệ thống chuyển động của AUV theo độ sâu được viết dưới dạng sau[6]: 
1 0 2 1
2 3
3 2
(.)
(.)
s
x u x f
x x
x f b



 (23) 
trong đó: ou là vận tốc chuyển động của AUV; sb là mô men sinh ra bởi góc bẻ lái sâu; 
1
(.)f ,
2
(.)f là các hàm số phụ thuộc vào tham số cụ thể của AUV và các thành phần 
không xác định chính xác của mô hình. 
Bộ quan sát nâng cao cho hệ thống ổn định độ sâu có dạng như sau: 
2/3
1 2 1 1 1
1/3
2 3 2 1 1
3 3 1
| | ( )
| | ( )
( )
s
z z k e sign e
z z k e sign e
z k sign e b



 (24) 
Với 
1 1 1 2 2 1 3 3 2
, , ;e x z e z f e z f 
1 01 2 02
| ' | ,| ' | ;f f f f và s là tín 
hiệu điều khiển trượt: 
 ( (s)), ,
s d
e Ksat s e e e x x   
Với s là mặt trượt, ở đây ta lựa chọn hàm sat thay cho hàm sign để chống hiện 
tượng chattering. 
Khi đó động học sai số hệ thống là: 
2/3
1 2 1 1 1
1/3
2 3 2 1 1
3 3 1 2
| | ( )
e k | e | sign(e ) '
( ) '
e e k e sign e f
e f
e k sign e f



 (25) 
Như vậy 1 2,e e và 3e sẽ hội tụ tới 0 trong thời gian xác định 0t T bằng việc lựa 
chọn các hệ số khuyếch đại 1 2,k k và 3k phù hợp 
Sau khi các sai số của hệ thống hội tụ tới 0, 1 1 2 1 3 2, ,x z z f z f sau thời gian 
xác định 0t T . 
3. KẾT QUẢ MÔ PHỎNG 
Với dữ liệu đầu vào là một chủng loại AUV có các thông số như sau[7]: 175m kg ;
214
xx
I kgm ; 213yyI kgm ; 
216
zz
I kgm ; 120 /X Ns mu ; 
32
v
Y
v v
Y Ns m
hình 3. K
hư
Sơ đ
ớng thể hiện tại h
90 /
90 2 /
ồ mô phỏng bộ quan sát AUV theo độ 
Ns m
ết quả mô phỏng sai số 
P. V. Phúc, T. Đ. Thu
; Z Ns m
Hình 2
Hình 3
w
2
; 
ình 4, b
150 /
w w
Z Ns m
.
.
 Sơ đ
 Sơ đ
120 2 /
ộ quan sát ổn định độ sâu thể hiện tại h
ồ khối mô phỏng bộ quan sát h
ồ khối mô phỏng bộ quan sát độ sâu cho AUV
ận, N. Q. Vịnh, “Tổng hợp bộ quan sát  ph
; Nr
ước l
2
ượng v
18 /Ns m
; N Ns m
r r
sâu và theo hư
à bi
12 2 / 
; X Ns m
ến trạng thái của hệ thống ổn định góc 
u u
Kỹ thuật Điều khiển 
ư
90 2 /
2
; 
ớng đ
ớng cho AUV
ược mô tả tr
ình 5.
2
ương ti
; 
. 
.
–
 Tự động hóa
ên hình 2 và 
ện ngầm.”
Nghiên cứu khoa học công nghệ 
Tạp chí Nghiên cứu KH&CN quân sự, Số Đặc san FEE, 08 - 2018 33
Hình 4. Kết quả mô phỏng sai số giữa giá trị ước lượng và biến trạng thái 
của hệ thống ổn định góc lái hướng. 
Hình 5. Kết quả mô phỏng bộ quan sát ổn định độ sâu của AUV. 
Qua kết quả mô phỏng: 
 - Thời gian hội tụ giữa sai số của giá trị ước lượng và biến trạng thái của hệ thống ổn 
định góc hướng nhanh. 
- Trong hệ thống ổn định độ sâu, giá trị ước lượng của hệ thống nhận được phù hợp 
trạng thái của hệ thống. 
- Tuy nhiên do ảnh hưởng của dòng chảy đại dương, quỹ đạo chuyển động theo các biến 
trạng thái ước lượng chưa tiếp cận được quỹ đạo thực tế. Do khuôn khổ bài báo nên tác giả 
chỉ tập trung trình bày bộ quan sát trạng thái. 
4. KẾT LUẬN 
Trong môi trường nước, các đặc tính động học của phương tiện ngầm có tính phi tuyến 
cao và luôn chịu sự tác động của các thành phần bất định nên việc xác định chính xác các 
giá trị của các biến trạng thái là hết sức khó khăn. Bộ quan sát nâng cao đã nêu với ưu 
điểm là số lượng tính toán ít, không cần phải tính đạo hàm của các sai số nhưng vẫn đảm 
bảo được khả năng hội tụ nhanh giữa giá trị ước lượng và giá trị của hệ thống. 
 Các kết quả trong bài báo đã được mô phỏng kiểm nghiệm trên Matlab Simulink và 
khẳng định được tính đúng đắn của các phương pháp đề xuất. Đây là cơ sở đầu vào để 
khảo sát đánh giá quỹ đạo chuyển động của AUV khi có tác động của bên ngoài. 
TÀI LIỆU THAM KHẢO 
[1]. T. I.Fossen, Guidance and Control of Ocean Vehicles, Chichester: John Wiley & 
Sons,1994. 
Kỹ thuật Điều khiển – Tự động hóa 
 P. V. Phúc, T. Đ. Thuận, N. Q. Vịnh, “Tổng hợp bộ quan sát  phương tiện ngầm.” 34 
[2]. Xiao Liang, “Dynamic Modeling and Computer Simulation for Autonomous 
Underwater Vehicles with Fins”, journal of computers, vol. 8, no. 4, april 2013, pp 
1058-1064. 
[3].Tutorial.S Drakunov, V Utkin, “Sliding mode observers for robust fault 
reconstruction in nonlinear systems”, Proceedings of the 34th IEEE Conference, 
2012, pp 373-383 
[4]. Agus Budiyono, “Advances in unmanned underwater vehicles technologies:Modeling, 
control and guidance perspective”, Indian Journal of Marine Sciences Vol. 38(3), 
September 2009, pp. 282-295 
[5]. Jorge Davila, Leonid Fridman, and Arie Levant, “Second-Order Sliding-Mode 
Observer for Mechanical Systems”, IEEE transactions on automatic control, vol. 50, 
no. 11, november 2005, pp 1785-1789 
[6]. S. K. Spurgeon, “Sliding mode observers: a survey,” Intern. J. Syst. Sci.,vol. 39, 2008, 
pp 751–764. 
[7]. Trần Đức Thuận, Nguyễn Quang Vịnh, Trương Duy Trung, “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), 2013, tr3-11. 
ABSTRACT 
CONSTRUCT AN OBSERVER OF CONTROL SYSTEM 
FOR UNDERWATER VEHICLES 
The paper presents a method for designing a signal filter based on an obsever in 
a underwater vehicle control system. In which the motion of an underưater vehicle 
is regarded as a solid body moving 6 degrees of freedom; The autopilot system in 
the underwater vehicle is proposed using a state observer structure to calculate 
and filter the low frequency component of the underwater vehicle out of the 
disturbance due to ocean currents. MATLAB / SIMULINK simulation results 
demonstrate the correctness of the algorithm, indicating that the system performs 
well in this model. 
 Keywords: Obsever; Underwater vehicles. 
Nhận bài ngày 01 tháng 7 năm 2018 
Hoàn thiện ngày 10 tháng 9 năm 2018 
Chấp nhận đăng ngày 20 tháng 9 năm 2018 
Địa chỉ: 1 Trường Cao đẳng Kỹ thuật Hải quân; 
 2 Viện Khoa học và Công nghệ quân sự. 
 *Email: phucanhquansg@gmail.com.