Underwater Robots - Gianluca Antonelli.pdf

166 7. Dynamic Control of UVMSs
which isrelated to the time derivative of e de as follows
˙e de = � ζ de + S P ( �ν 2 ,d) e de ,
where S P ( · )=blockdiag { S ( · ) , O 3 , O n × n } .
In order to design an observer providing velocity estimates, the estimation
error has tobeconsidered
⎡ ⎤
e e = ⎣
B
�η 1 ,e
�ε e
�q e
⎦ ,
where �η B
1 ,e = η B 1 ,e − η B 1 , �q e = q e − q ,and �ε e is the vector part of the unit
quaternion � Q e = Q − 1 ∗Qe .
Finally, consider the vectors
ζ r = ζ d + Λ d e de
(7.36)
ζ o = ζ e + Λ e e e . (7.37)
where Λ d =blockdiag { Λ dP ,λdOI 3 , Λ dQ} , Λ e =blockdiag { Λ eP ,λeOI 3 , Λ eQ}
are diagonal and positive definite matrices. It is worth remarking that ζ r and
ζ o can be evaluated without using the actual velocity ζ .
Let us recall the dynamic equations in matrix form (2.71):
M ( q ) ˙ ζ + C ( q , ζ ) ζ + D ( q , ζ ) ζ + g ( q , R I B )=Bu, (2. 71)
the control law is
u = B † � M ( q ) a r + C ( q , ζ o ) ζ r + K v ( ζ r − ζ o )+
+ K p e d + g ( q , R I B )+ 1 2 D ( q , ζ �
r )(ζ r + ζ o ) ,
(7.38)
where K p =blockdiag { k pP I 3 ,kpOI 3 , K pQ} is adiagonal positive definite
matrix and K v is asymmetric positive definite matrix. The reference acceleration
vector a r is defined as
a r = a d + Λ d � ζ de , (7.39)
and thus the control law(7.38) does notrequirefeedbackofthe vehicle and/or
manipulator velocities.
The estimated velocity vector ζ e is obtained via the observer defined by
the equations:
⎧
⎪⎨
⎪⎩
˙z = M ( q ) a r − � L p + L v A ( � �
Q e ) Λ e e e + K p e d +
C ( q , ζ o ) ζ r + C T ( q , ζ r ) ζ o
ζ e = M − 1 ( q )(z − L v e e ) − Λ e e e ,
(7.40)