CHƯƠNG 4 THIẾT KẾ BỘ ĐIỀU KHIỂN ĐỘ SÂU
4.1 Thuật toán dẫn đuờng
Tôi sẽ dùng thuật toán LOS (line of sight) để dẫn đường cho AUV trong mặt phang XoZ.
Thuật toán được mô tả trong hình sau:
X
Hình 4.1 Thuật toán dẫn đường LOS Theo hình 4.1 ta có góc 0 mong muốn (0ref) được tính theo công thức sau:
9ref = atanl j = atanl j = atanl j (4.1)
Với: ez , ex là sai số của tọa độ theo phương z vả X của AUV.
4.2 Hoạch định đường đi cho AUV.
Trong luận văn này tôi sẽ hoạch định quỹ đạo cho AUV bằng đường cong đa thức Hermite.
Ưu điểm của đường cong này là đạo hàm bật nhất của nó là một hàm liên tục giúp vận tốc của AUV không bị thay đổi đột ngột. Giả sử ta có các điểm đặt pQ{xữ, ZQ,UQ), p1(x1, z1( %) ... Pn(xn, zn, un), đa thức Hermite có dạng:
z = p(x) = a(x — x0)3 + b(x — xoy + x(x — x0) + d (4.2) Đặt h = — xữta có
p(xo) = z0 => d = z0
P'{XQ)=\LQ => c = XLQ p(*i) = Zi => ah3+ bh2+ ch + d = Z1
p'(x-J = u-ỵ => 3ah2 + 2bh + c = ur
Giải hệ phương trình trên ta tìm ra các hệ số của đa thức Hermite, giải tương tự cho điểm đặt kế tiếp ta có quỹ đạo di chuyển cho AUV. Trong Matlab ta có thể dùng hàm pchip để tìm ra quỹ đạo cho AUV.
4.3 Điều khiển độ sâu thông qua cánh ỉặn.
Ta có phương trình tổng quát của AUV:
r\ = Ị{JÌ)V (4.4)
1> = D*(v) * V + GO7) + T*rop
Từ phương trình (4.4) mô hình góc pitch được rút gọn như sau:
<7 =/(v).v +G(IJ)(1,5) + Ts*đs (4.5) Với:
/(v) * V : là hàng thứ 4 cột thứ 1 của ma trận Dp(v) * V.
T6 : r;rov (1,5).
Thuật toán LOS được sử dụng để tạo ra góc Qref theo công thức (4.1).
Sơ đồ khối bộ điều khiển độ sâu:
Hình 4.2 Sơ đồ khối bộ điều khiển độ sâu.
Phương trình tổng quát của mặt trượt được mô tả trong công thức sau [13]:
n-1
s(x) = ^ CịXị + xn (4.6)
i=1 Với:
c = [cx ... cn_{\ : nên được chọn theo đa thức Hurwitz.
X : vector trạng thái.
Đe đạt được Reaching law thì có thể chọn bằng các hàm sau đây [13]:
S = —k.sign(s),k > 0 (4.7)
Luật này có ưu điểm là đơn giản, tuy nhiên nếu k quá lớn sẽ gây ra hiện tượng chattering.
s = —ksign(s) — as, k > 0, a > 0 (4.8) Với a thỏa:s = s(0)e-aí
Luật này cố ưu điềm là sẽ làm sai số tiến về 0 nhanh hơn, tuy nhiên vẫn bị hiện tượng chattering.
à = —k\s\asign(s),k > 0; 1 > a > 0 (4.9) Luật này có ưu điểm là tăng tốc độ reaching, tuy nhiên khi s -> 0 thì hiện tượng chattering cũng sẽ tăng dần.
S = —k.sign(s') —/(s),k> 0 (4.10)
Với/(0) = O&s./oo > 0
Đây là một luật chung nhất cho dạng luật có hàm sign. Bời vì có hàm sign nên nên khi sai số tiến gàn về không tín hiện điều khiển có thể bị đổi dấu liên tục gây ra hiện tượng chattering.
Hình 4.3 Hiện tượng chattering [14]
Một cách khác đề giảm hiện tượng chattering là thay hàm sign bằng hàm tanh.
S = —k.s — fc.tanh(-),fc > 0 a > 0 (4.11) Với:
a: "boundary layer thickness”.
Nếu a nhỏ thì sẽ tăng tính phỉ tuyến của hệ thống, trong khi a lớn cố thể gây ra nhiễu [15].
Theo (4.6) và (4.8) ta chọn mặt s như sau:
s(t) = eq (4.12)
Vói : eq = qref - q Ta có:
(4.13)
s(t) — &q — qref Q
= <lref - f ( ỳ ) * V - G(ĨỊ) -TS*Ôs
Chọn:
s(t) = —£.sign(s) — k.s (4.14)
Từ (4.13) và (4.14) ta có luật điều khiển:
_ kreĩ - f(y) - G(rj) + E.sign(s) + k.s (4.15)
Os ~ IJI
*6 Chọn hàm Lyapunov:
v = \s2 (4.16)
Lấy đạo hàm hàm Lyapunov ta có:
V = S S (4.17)
Thay phưong trình(4.13) và (4.15) vào (4.17) ta có:
V = s ịqref - f(y) - G(ĩj)
Rref - f(y)-G(IJ)+E.signis) + k.s\ (4.18)
_Ts' Tĩ ì
=> V = s(—£.sign(s) — k.s) = —£. |s| — k.s2 Vậy nếu chọn £, k > 0 :
V = —E. |s| — k.s2 < 0 (4.19)
Kết quả mô phỏng:
Với: s(t) = —l.sign(s) — 5.S
Position X-Z
Hình 4.4 Đáp ứng X- z.
0 20 40 60 80 100 120 140 t(s)
Hình 4.5 Sai số z
theta
Hình 4.6 Đáp ứng 0.
Stern angle
140
Hình 4.7 Tín hiệu điều khiển.
Với: s(ủ) = —l.ủanh(s) — 5.S
-20 0 20 40 60 80 100 120
X(m)
Hình 4.8 Đáp ứng z.
Hình 4.9 Sai số z
theta
Hình 4.10 Đáp ứng 0.
0 20 40 60 80 100 120 140 t(s)
Hình 4.11 Tín hiệu điều khiển.
Nhận xét:
-Ta thấy tín hiệu điều khiển của hàm sign Hình 4.7 thay đổi liên tục còn ở hàm tanh Hình 4.11 thi khi đạt độ sâu z sẽ ổn định về không. Điều này là quan trọng đối với AUV vĩ năng luợng tiêu hao ít hon sẽ tăng thời gian hoạt động của AUV. Bên cạnh đó việc thay đổi góc cánh liên tục sẽ tạo ra các xoáy gây ảnh huởng hoạt động của AUV.
-Tổng bĩnh phuơng sai số ở truờng hợp hàm sign là 531.3587 so với 527.3154 của truờng hợp hàm tanh. Cùng vói settling time lần luợt là 33.34 và 33.33 theo thứ tự hàm tanh và hàm sign ta có thể thấy xét về khả năng điều khiển thì hai bộ điều khiển này tuơng đuơng nhau.
4.4 Điều khiển độ sâu thông qua đối trọng.
4.4.1 Bộ điều khiển Backstepping cho đối trọng.
Phuơng trĩnh (2.82) có thể đuợc viết thành dạng:
xmT9 (4.20)
2 è = g .sin(ỡ) f i . g . c o s { 6 ) + . T
mị. (a2. cc)
(4.21)
Vm ~ %m (4.22)
Đặt sai số:
Z \ — B — ym — ym d (4.23)
Lấy đạo hàm ta có:
N- II ro ll cb 1 a. (4.24)
Đặt ứ nhu bộ điều khiển ảo:
T9= a (4.25)
Vậy:
Chọn hàm Lyapunov :
Vậy:
Zi = a- ỳmd
v,=^ỉ
Vi — Zi^i — Zi- Cal ýmw) Để V-y < 0 ta có thể chọn:
Với:
k± > 0: hệ sô gain. Đặt biến:
a = ỳ md- Mi
Zj = 19 + a Lấy đạo hàm hai vế phưoTig trình (4.30):
2
z2 = -d + ả = g. sin(ỡ) - g.g. cos(ỡ) H --- 7- ---- r. T + ả mj.(a2.a)
Chọn hàm Lyapunov V2 như sau:
V2 = V1 + -zf 2 1
=> V2 _ + Z2Z2
2
<=> 1^2 = —krzị + z2(g. sin(ỡ) — Ịi.g. cos(ỡ) H--- J-—-T.T — à) m i X d2- à ) ' Nếu ta chọn:
(4.26)
(4.27)
(4.28)
(4.29)
(4.30)
(4.31)
(4.32) (4.33) (4.34)
d2.a
T = -mt. -y- (5. sin(ỡ) - g. g. cos(ỡ) - à + k2z2) Thì L2 trở thành:
V2 = -Mi - Ml < 0 Vậy tín hiệu điều khiển có thể chọn là :
d-2‘ Cí a / \
T = (0.sin(ỡ) -g.g.cos(d) -ỷmd + M + 2.k2ịỹmd - MJ)
(4.35)
(4.36)
(4.37)
4.4.2 Bộ điều khiển adaptive sliding mode cho độ sâu thông qua đổi trọng Từ phương trình (3.2) ta có thể viết thành:
V =J (t?)v
M.V = Đp(v).v + g(ĩ]) + Tprop
Từ phương trình trên, phương trình góc pitch có thể viết thành:
Mq. q = /00 + Tprop + Ổ 1 O 7 ) + 9 2 ( n ) - X g
Trong đó:
Mq = M( 5,5)
9i0l) = {zgW — zbB) sin(ỡ) — xbB. cos(ỡ) cos(0) g2(ĩ]) = iy.cos(ỡ)cos(ự))
Giá trị Mq đã được tính toán trong chương 2 tuy nhiên trong thực tế giá trị này có thể sai khác đi vì nhiều lý do (công thức thực nghiệm, sai số về khối lượng, hình học...). Bộ điều khiển adaptive có thể đưa ra lời giải cho vấn đề này thông qua luật adaptive để dự đoán giá trị Mq. Ta có sơ đồ khối như sau:
(4.38)
(4.39)
Hình 4.12 Sơ đồ khối bộ điều khiển độ sâu thông qua đối trọng.
Chọn mặt trượt:
s(0 eq — q Rref (4.40)
Chọn:
s(ủ) = Mq. qref — l.s — k. tanh ); k, l, a > 0 Chọn luật điều khiển như sau:
(4.41)
Xg = —^ ( Mq. ỈỊref -l.s-k. tanh (-) - ự(y) + Tprop + g1(ĩì)) (4.42)
Chọn hàm Lyapunov:
V = ^Mq.s2+^-M2 2 H 2.y H
Với Mq= Mq- Mq, ỵ > 0, Mq : là giá trị ước lượng của Mq. Ta có:
1 _ ^ 1 _
V = Mq.s.s + -.Mq.Mq = s(Mq.q - Mq.qref) +-.Mq.Mq
Thế phưoTig trình (4.39) và (4.42) vào (4.44), ta có:
V = s. ự(y) + Tprop + 0i(ij)
+ 92 07) ■ Ị—^ ị^Mq.qref-l.s-k. tanh (-)
— ^/(v) + Tprop + - ^q-ộre/) "h “■ Mq.Mq
„ / S \ 1. _
<=> V = s.{Mq.qref - I s - /í. tanh J - Mq.qref) + ~.Mq.Mq
<=> V = —l.s2 — k.s. tanh + s.Mq. qref +-.Mq.Mq
Vậy nếu ta chọn luật adaptive:
Mq = -y.qref.s Thì:
V = —l. s2 — k. s. tanh ) < 0.
Vậy luật điều khiển là:
1 _ /S\
Xg = — - l . s - /c.tanh Ị—J - ự(y) + Tprop + 010?)) Ket quả mô phỏng:
(4.43)
(4.44)
(4.45)
(4.46)
(4.47)
(4.48)
Position X-Z
X(m)
Hình 4.13 Đáp ứng z.
Hình 4.14 Sai số z.
theta
Hình 4.15 Đáp ứng 0.
14.6 14.4
^14.2
cn
f 14
13.8
13.6
0 20 40 60 80 100 120 140
t<s)
Hình 4.17 ước lượng Mq Nhận xét:
-Tín hiệu điều khiển xg đạt được với tổng bình phương sai số ez là 7.717xl04 . Tín hiệu điều khiển ổn định và góc 0 về 0 khi đạt độ sâu z. Khi đạt độ sâu z, góc 0 ổn định ở 0° là cẩn thiết vĩ đó là tiền đề để điều khiển hướng cho AUV.
4.5 Bộ điều khiển độ sâu dạng elevator.
Từ phương trĩnh (3.7) ta có thể viết lại phương trĩnh:
ESTIMATE Mq
III' ' --- Real
— Estimate
w = A(u)u + g1(r}) + 7V 8S
q = f2(v)v + g20?i) +T2.SS + g3(xi)xg
(4.49) Với:
/1(11)11: hàng thứ ba của ma trận: M-1 * Dp (u)u g1(ĩi') : hàng thứ ba của ma trận M_1. G(ÍỊ) /2(u)u:
hàng thứ năm của ma trận: M-1 * Dp (u)u
Ỡ2O7) = M 1. ( ( zgW -zbB)sin(0) - xb£. cos(0) cos(ự>)) g3{g) = M_1( iy.cos(ỡ) cos(ự>))
Phương trình (4.49) là phương trình vi phân của trục z và góc 6. Tín hiệu điều khiển của z là góc cánh Ss và tín hiệu điều khiển góc 0 là Xgthông qua đối trọng. Trong phương pháp lặn này thì góc 0 sẽ luôn bằng 0, toàn bộ lực để AUV lặn được tạo ra trên cánh. Ta sẽ áp dụng hai bộ điều khiển mặt trượt cho cánh lặn và cho đối trọng.
Ở mục 4.4 ta đã dùng adaptive để ước lượng ma trận M, tuy nhiên nếu xét về tính phi tuyến cũng như sai số công thức thực nghiệm thì ma trận Dp(y) cao hơn. Ở mục này tôi sẽ dùng Neural Network để ước lượng ma trận Dp(y) vói giả thiết rằng các ma trận khác là biết trước.
Ta có sơ đồ khối như hĩnh dưới:
Hình 4.18 Sơ đồ khối bộ điều khiển neural network -sliding mode 4.5.1 Bộ điều khiển độ sâu thông qua cảnh.
4.5.1.1 Điều khiến mặt trượt thuần tủy.
Chọn mặt trượt:
sự) = ez (4.50)
Vơi: ez %ref z Ta có:
5(0 = èz = Zref - z = Kef - A(u)u - gM - Tx. 8S (4.51)
Chọn :
ẩ(t) = —£.tanh(s) — k.s Từ (4.51) và (4.52) ta có luật điều khiển:
(4.52)
1
£s = —. ( zref - /i(f) - Ổ1O7) + £■ tanh(s) + k. s) (4.53) Chọn hàm Lyapunov:
v = ịs2 (4.54)
Lấy đạo hàm của phương trình trên ta có:
V = S S
= s. ịzr e f - A(v) - gM
", zref-f1{y)-g1{jì) + E.tanh{.s) + k.s\ ^4'55^
Tl- T, )
<=> V = —ks2 — E. s. tanh(s) — s. d(t) = —ks2 — E. |s| <0 4.5.1.2 Bộ điều khiển mặt trượt kết hợp Neural Network.
Ở mục này ta sẽ dùng Radial Basis Function (RBF) netwoks để ước lượng /ì(v).
Ta có công thức để ước lượng [17]
/ = W7i(x)
(4.56)
Với:
x: input.
i: là so input.
j: là so node lớp ẩn.
h =[h\ hi... ha]T là output của hàm Gaussian.
W: bộ trọng số.
Hình 4.19 Sơ đồ neural network.
Chọn input là X= [ ez] , và output là TẠx) = wh(x) Ta có control input (4.53) được viết lại:
ss = ệi-(. -fi + K e f - ỔI
Gỉ) + ô■ ) + fe. s) Lấy (4.58) thế vào(4.51):
s = K e f - f i -ổiG?) - (-/i + Zre/ - ổiO;) + e. tanh(s) + fc.s) <=> 5 = /i — /i — tan/i(s) — k.s = —fi — £. tanhịs) — k.s — d(í)
Với:
ĩi=f i ~ ĩ1 = - W/iOO = W/L(X) Chọn hàm Lyapunov [17]:
1 1 L = ^52 +^yM72 Lấy đạo hàm ta có:
ỉ = 55 + yivv/ = s(— f1 — e.tanh(s) s) —
<=> L = s.ị—W. h — e. tanhự) d(t)ì —
L = w (sh + yWj + s(—e. tanh{s) — k.s — d(t))
(4.57)
(4.58)
(4.59)
(4.60)
(4.61)
(4.62)
Vậy nếu ta đặt:
Khi đó:
w = —sh r
L = —E. s. tanh(s) — k.s2 <0
(4.63)
(4.64) 4.5.2 Bộ điều khiển góc 0 thông qua đổi trọng.
Hoàn toàn tương tự như trên ta viết cho đối trọng.
4.5.2.1 Điều khiển mặt trượt thuần túy.
Chọn mặt trượt:
s(t) = eg
Với: eg = eref - e Ta có:
s(t) = èg = Òref - ở = Òref - /2(u)u - g2(rj)-T2.ổs - g3(rj)xg
Chọn :
s(t) = —E.tanh(s) — k.s Từ (4.49) và (4.50) ta có luật điều khiển:
1
Xg = —. ( 0re/ - /2(u) - g2(rì)-T2.Ss + E. tanh(s) + k. s) 9 3
Chọn hàm Lyapunov:
V = - S2 2 Lấy đạo hàm của phương trình trên ta có:
V = SS
<=> V = s. ị ẽr e f - /2(u)u - g2( g) - T2.Ss
, . ỏre/ - fz(p) - gzW-T2Ss + E-tanh(s) + k.s> - , <=> V
= —ks2 — E. s. tankas') < 0
4.5.2.2 Bộ điều khiển mặt trượt kết họp Neural Network.
(4.65)
(4.66)
(4.67)
(4.68)
(4.69)
(4.70)
Ta có công thức để ước lượng [17]
/ = W7i(x) Với:
x: input.
i: là số input.
j: là số node lớp ẩn.
h =[hi hi... /ỉn]Tlà output của hàm Gaussian.
W: bộ trọng số.
Chọn input là X = [ eg], và output là
/2O) = Whự) Ta có control input (4.68) được viết lại:
Xg = + K ef - 32(3)-T2 ÔS + e.tanhis) + k.s) Lấy (4.73) thế vào(4.64):
S = èref - f2(v)v - g2(rj)-T2.Ss
- (-/2 + K ef - 3 2( 3 ) ~T2 - ÔS + £■ tanh(s) + k. s) <=> S = /2
— /2 — £. tanh(s) — k.s = —/2 — £. tanh(s) — k.s — d(t) Với:
/2 = /2 — /2 = W7i(x) — iv'fr(x) = iv’h(x) Chọn hàm Lyapunov [17]:
1 1 _ L = -S2 + -yW2 2
21 Lấy đạo hàm ta có:
L = SS + yWW = s(—7 — £. tanh(s) — k.s) — yWW <=>
L — s. (-W. h — £. tankas') — fc.s) — yWW <=> L —
w ịsh + yW j + s(—£. tanh(s) — k. s) Vậy nếu ta đặt:
(4.71)
(4.72)
(4.73)
(4.74)
(4.75)
(4.76)
(4.77)
Luận văn tốt nghiệp
Khi đó:
Kết quả mô phỏng:
L = —E. s. tanh(s) — k.s2 < 0
(4.78)
(4.79)
Position X-Z
X(m)
Hình 4.20 Đáp ứng z
Hình 4.21 Sai số ez
Luận văn tốt nghiệp
theta
Hình 4.24 Đáp ứng góc 0
Estimate F1
Estimate F2
Nhận xét:
-RBF neural network được dùng đề dự đoán hàm /-L và /2 cho kết quả tốt.
-Tổng bình phương sai số của độ sâu là 3.7089xl04, kết quả tốt nhất trong ba phương pháp lặn.
-Các tín hiệu điều khiển hình 4.22 và hình 4.23 ổn định khi đạt chiều sâu z. Đảm bảo cánh và đối trọng không di chuyển động liên tục ra các xoáy nước.
4.6 Nhiễu tác động.
Ớ mục này ta sẽ xem xét nhiễu tác động vào AUV trong trường họp lặn chúi đầu bằng cánh.
Xét hệ có dạng như sau:
q = /(v).v + G(i7)(l,S) + TS*8s + d(0 (4.80) Với:
/(v) * V : là hàng thứ 4 cột thứ 1 của ma trận Dp(y) * V.
T6 : r;rov (1,5).
d(t) : là nhiễu tác động lên hệ thống và |d(t)| < D, D là hằng số.
s(t) = eq (4.81)
Vơi: eq — Ợre/ — R Ta có:
5(0 = èq = qref - q (4.82)
= ỳref - f(y) * V - G(Ị?Ỉ) - ĩỗ* ổs + d(t) Chọn:
s(0 = —£.tanh(s) — k.s (4.83)
Từ (4.11) và (4.12) ta có luật điều khiển:
pkreĩ - f(y) - G(rj) + £. tanh(s) + k.s (4.84)
~ rp
Chọn hàm Lyapunov:
v = -s1 2 2 (4.85)
Lấy đạo hàm hàm Lyapunov ta có:
V = S S (4.86)
Thay phương trình(4.11) và (4.13) vào (4.15) ta có:
V = s ịqref - /(v) - GO?)
^ Rref- /CO - G(ji) + £.tanh(s) + k.s \ (4.87) -Tỗ. --- ^ --- m)
=> V = s(—£. tanh(s) — k.s — d(t)) = —£. |s| — k.s2 — s. d(ủ) Vậy nếu chọn £ > D ; £, k > 0 :
V = — £. |s| — k.s2 — s.d(t) < 0 (4.88) Nhiễu tác động d(ủ) có thể là dòng chảy, dòng rối. Tuy nhiên khi thiết kế, ta hoàn toàn có thể giới hạn d(ủ) < D bằng việc giới hạn môi trường hoạt động của AUV. Theo lý thuyết
miễn là £ > D thì hệ sẽ ổn định, nhưng trên thực tế tín hiệu điều khiển Ss bị giới hạn bởi hiện tượng thất tốc.
Kết quả mô phỏng:
Khi D = 1
Position X-Z n
-1.611
0 20 40 60 80 100
120
X
Hình 4.27 Đồ thị X-Z nhiễu D = 1 tác động Khi D = 2
Position X-Z
0 --- T --- 1 --- 1 ---
-2-
-3 --- 1 ---1 --- 1 --- 1 --- 1 --- 1 ---
-20 0 20 40 60 80 100 120
X
Hình 4.28 Đồ thị X-Z nhiễu D = 2 tác động
Như vậy ta thấy khi D = 2 thì bộ điều khiển không thể bù được nhiễu do cơ cấu chấp hành đã tới hạn.
4.7 Kết Luận
^''''''\jihương pháp lặn Tiêu
chí Cánh Đối trọng Elevator
Tống bĩnh phương sai so z 1.065x10s 7.717xl04 3.7089xl04
Sai số xác lập ez (m) 0.03m 0.09m 0.004m
Thời gian xác lập z (s) 32.2, 6.1, 15.19 36.78, 14.65, 18.14
14.2, 4.98, 7.02 Bảng 4.1 Đánh giá bộ điều khiển
Bảng trên cho ta cái nhìn tương đối về các bộ điều khiển, lặn dạng elevator cho ta sai số xác lập nhỏ nhất. Trên thực tế AUV thường sẽ kết hợp cả ba dạng lặn này. Đầu tiên AUV sẽ dùng đối trọng để đạt góc pitch cho trước, sau đó sẽ phối hợp các cánh lặn để duy trĩ phương. Khi đã được độ sâu, dạng lặn elevator sẽ được dùng để giảm sai số.
Khi điều khiển AUV trong mặt phang XoZ, khi muốn AUV đạt cả tọa độ X và z, ta ưu tiên dùng cách lặn bằng cánh, như hĩnh 4.4 thể hiện, AUV đạt tọa độ X-Z rất tốt.