Professional Documents
Culture Documents
371-384, 1985
Printed in Great Britain.
MEASUREMENT
NOISE
.,STEM
"q ,.T', J
~ CONTROk
REFERENCE
PATH
MEASUREMENT,Y
-4-
CONTROLLER F
4___
"
ESTIMATOR
4
f
I
I-=1
SYSTEM
DYNAMICS
AUTOMATIC
PILOT
372
(1)
(2)
(3)
"~.,.
VX =
X ~~ =
Ii"~ \
373
(12)
by = rvx + ( f l + gl)sinO + (f2 +
gz)COSlp, (13)
f = f 3 + g3,
\..Glx,v)=O
(14)
(4)
(7)
(8)
(9)
Yo = Vr,
(10)
~=r,
(11)
VR, i f c S c - 6 > 0 ,
~ = ~ 0,
-VR,
(15)
if6c-6=0,
i f f c - 6 < 0.
,~ = f,(vR, A, 6)
J" VR(1-- exp(--(3c - 6)/A)),
- VR(1
exp((3c -- 6)/A)),
if 6c - ,5 > 0,
if 6c - 5 < 0,
(16)
3. STATEESTIMATION
In the situation treated it is necessary to have a
state estimator that performs well, in spite of errors
in the modeled system dynamics. Only that part of
the dynamics which is feasible to be modeled is
explicitly considered in this state estimator. The
unmodeled effects are taken care of by a dynamical
model compensation technique (Tapley and
Ingram, 1973) and by a heuristic procedure for
evaluation of state noise level, as described below.
The lack of knowledge of the system model is
treated by a scheme where the unmodeled
Yi
j-
lo.3vL
1"v/
FIG.3. Ruddermotionmodel.
.
Oc-O
374
(17)
(18)
+ f l ( v x , Vr,~b,r,f).sinO
+f2(Vx, Vr, O , r , f ) . c o s O + e2 + w2,
(19)
by = rv x
(20)
(21)
~1 = - ~ e l
+ w4, 7~1 = 0,
(22)
~z = - ~ z e 2 + w5, 7~2 = 0,
(23)
~3 :
(24)
Fw
A~,(tk+l, tk)=
Atk
(g
(27)
i = 1 , 2, 3 ; j = i + 2 ;
0<0<1;
Atk~=tk+l--tk,
where 0 is a parameter to be adjusted; P#(tk/tD,
j = 3, 4, 5, are the estimator error variances of Vx, Vr
and r; Atk is the time interval between observations;
and Ae~(tk+l, tk) are order of magnitude approximations for the possible range of variation of
accelerations el(t) in [tk, tk+~], taken as (Cruz,
1981):
(Y~
(N~ - I ~ ) ( - F w + -~. Y~)
.,_m)(N;~-~)~i~-~---m~x-G-)~iy~_m.xG
)
)
Ar.3(tk + 1, tk) =
(26)
'
.?.Atk,
(28)
(29)
(30)
-2
Fw(tk) = CR. p,. (Av + AL)Vw,
(31)
(32)
(33)
(34)
The estimates are then given by the corresponding sequential algorithm, which is summarized
below:
t
X(t/tk) = Y,(tk/tk) +
ft
~b(t, tk) = I. +
tkA(z)
ft
d m
~t-(X + 6X) = F(X + 6X, Uk + 6Uk, t) + W,
. ~b(Z,tk) dr,
1,
r) dz,
(38)
l/tk+
1) =
E [6X(tk)6Xr(tk)] = P(tk/tk),
(44}
P(tk+
(43)
~-hk+ 1((tk+l/tk),tk+l),
HT+I +
A(r) =
(42)
where
ff
0
(41)
(36)
+ Qk+ 1,
nk+ 1 =
tk < t < t k + l ,
Qk+l
375
(39)
(40)
(45)
where s(X(tk+ 1)) is a vector relationship representing the guidance condition of getting closer to the
prescribed path and with favorable kinematic
conditions to follow it. The control Uk is the value
previously calculated and applied in the interval
[tk-1, tk]. Taking the first-order variational equations associated to equation (42), one obtains:
6X(tk+ 1) = t~(tk+ 1, tk)6X(tk)
+ F(tk+ 1, tk)O-Uk + Wk+ 1,
(46)
j-tk + 1
--
(tk+ 1, r). ~ - V ( X ( r ) ,
U(Z), z) dz,
tk
= Sk+lUk
At- k + 1 ,
(47)
376
where:
d __
Sff+ 1 = ~ s ( X ( t k + 1)); Sk+ ~ = S~'+ 1. F(tk+ 1, tk);
Zk+ 1 = Sk+ 1Uk -- s(X(tk+ 1));
,, tk)/
(49)
(50)
(51)
0 < ai < 1,
(52)
(53)
(54)
OAo
(55)
(56)
s,,(Vx, v r ,
r, X o , Yo) = r -
--
= 0,
(57)
Real
System
X0(to)
= Yo(to)
Vx(tO)
= 7.5 knots,
r(to)
= 0.0/s,
Landmarks
AUTO 21:4-C
Initial
Along
Conditions
= 0.0,
~(to)
= 60
6(to)
= 0.0
Channel
Margin
X(1)
-- 1000 m
X(2)
-- 1000 m
X(3)
= 3000 m
X(4)
= 3000 m
Y(1)
Y(2)
= 1500 m
Y(3)
= -500 m
Y(4)
= 1500 m
500 m
377
378
oo
~--
x%
Yo
Range
FIG. 4.
TABLE 2.
measurement.
Variable
Initial
Value
Standard
Deviation
X0
-10.9 m
5.0 m
Y0
43.5 m
5.0 m
O. 4
vX
2.0
15.2 knots
0.I knots
Vy
0.0
0.I knots
0.1/s
0.1/s
0.0
0.5
~i
0.029 m/s 2
0.033 m/s 2
e2
-0.035 m/s 2
0.033 m/s 2
c3
-0.05/s 2
0.025/s 2
TI
1000.0
200.0
T2
I000.0
200.0
T3
1000.0
200.0
X : -25.5%
U
X.:
20.9%
Y :
v
5.0%
N6:
10.1%
N.:
r
N : -19.6%
-6.0%
Y6:-14.6%
N :
r
8.6%
Y*:
U
1.3%
6.0%
Y.:-14.6%
N*:
-0.3%
N*:
Y.:
: 19.2%
N.:
13.8%
5.2%
Y*: 17.1%
379
5.3. Results
As shown in Fig. 5, even under severely restricted
conditions, the state estimation and control law
procedures lead to control actions that steer the ship
very closely to the channel center line and the largest
deviations, near curvature peaks, are of the order of
magnitude of twice the ship breadth (50 m). Figures
7-9 show the ability of the dynamical model
compensation technique to estimate the unmodeled
accelerations. Note that there are occasions when
A, = 0.12 ,
Axr = 0.22 m,
Av = 0.12 knots,
A~ = 0.045/s.
State
Noise Parameters
0 = 0.05
~" = 1 . 5 / s
Calibration
A = 0.1
/6_Uk/
az
= /6(6c)/
= 3 m/s
= 30 m/s
Parameters
1.0
cL2 =
= 30
1.0
x B = 8.O
ct3 =
1.0
or4 = 0 . 3
-I000.00
/"
/...
- 500.00
0.00
~l~, I
500.00
I000.00
/.00
1500.00
i. . . .
o
*
2500.oo
=
30oo.oo
5 0 0 O0
./
//
%%.,
..
I000.00.
Yo (M)
O
FIG. 5. Channel center line and ship controlled path.
\.
J
35oo.oo
\
Xo
4o'bo.bo
(M)
380
40.00
A 24.00
b.J
C3
"J 8 0 0
_1
(.9
Z
,I
n- - 8 . 0 0
ILl
E3
0C
-
24.00
- 40.00
22500
OO0
450.00
TIME
675.00
900.00
(S)
TOTAL
0.04
Ik,,
~'~
i 0.02 I-
/it .,/
~:~
'~,
,./"
~ ; f.,.
>"
-0.04
.i,"
';
j~^
,',
,1" v%
, v ~ it
II /
,I
'
0.00
'It
!t
~ o.oo~
-0.06
~Y'L E"T'"Y!~
UNMODELED - - - - -
225.00
45000
TIME ($)
67500
900.00
TOTAL
0.20
REAL_ ESTIMATE?
UNMODELED- - - - - I - - I
1 0.14
or)
=E
v
0.08
nLU
._1
hi
0
i 't
\\
0.02
>>
-0.04
-0.10
0.00
/'
225.00
450.00
TIME ( S )
675.00
900,00
381
/Oo?E,EO '-E-'L
0.10
0.04
a -0.02
z
o
1-0.08
LU
_1
IJJ
0
0
-0.14
n.,
-0.20
0.0~
112.50
225.00
557.50
450.00 562.50
TIME ( S )
675.00
787.50
900.00
1.60
~E
>X
1.20
I
|
~0.80
a
\
\
\
\
0.40
0.00
0.00
225.00
450.00
575.00
90000
TIME(S)
FIG. 10. Estimated standard deviation and true error in position.
0.20
ESTIMATED
TRUE
0.16
E3
0.12
u~
0.
I
.J
w
o
0.04
QO0
QO0
225.00
450.00
675.00
TIME (S)
FIG. 11. Estimated standard deviation and true error in heading angle.
900.00
382
ESTIMATED
TRUE
0.20
0.16
A
/)
p-
z 0.12
v.
" 0.08
_J
~J
Q
. . . . . . .
0.04
0.00
0.00
22,' . 0 0
450.00
675.00
900.00
TIME(S)
FIG. 12. Estimated standard deviation and true error in velocity.
ESTIMATED
0.50
TRUE
0.40
0.30
0.20
0.10
0.00
0.00
225.00
450.00
675. O0
900.00
TIME(S)
FIG. 13. Estimated standard deviation and true error in yaw-rate.
6. C O N C L U S I O N S
383
(AI)
(m - Y~)b + (mx~ - Y~)i"= Y* + Y*Au + Yvv + (Y, - mu)r
+ Y,q + Y66 + Y~*Au2 + Yo,vAu + YrurAu
+ Y6uAu + Yvr~V,~+ ( YvuuvAu2 + Yv,rvr 2
+ Yv~v62 + Y,uurAu 2 + Y~vvrv2 + Y~br62
h- Y~n~Au 2 -I- Y~vv~v2 -I- Y6rr~r2)
1
,3
+ ~(Y~,vv
+ Y,.r a + Y~6~63) + F~r + Fcr,
(A2)
(A3)
(A4)
(A5)
(A6)
resulting:
Fwx = - Fw. cos (~,, - ~b),
(A7)
(A8)
M , = Fwr . xL,
(A9)
where AL and A r are the projected lateral and frontal ship areas
above the line of water; Cs is the air resistance coefficient; and Pa
is the air density. The coarse approximation of taking wind
absolute velocity was considered reasonable since the main
purpose was to represent only the more significant disturbance of
lateral winds.
REFERENCES
Abkowitz, M. A. (1969). Stability and Motion Control of Ocean
Vehicles. M.I.T. Press, Cambridge, Mass.
384
A. RIOS-NETO a n d J. J. DA CRUZ