Professional Documents
Culture Documents
= u
1
V
1
+u
2
V
2
(1)
where v, R
3
denote the linear and angular velocities of
the needle tip, respectively, written relative to frame A. u
1
and u
2
are the insertion and rotation speed of the needle,
and
V
1
=
e
3
e
1
and V
2
=
0
33
e
3
(2)
The unit vectors e
i
, i = 1, 2, 3 are the standard basis. Let
q = [x, y, z, , , ] be the position and orientation vector of
the needle tip where x, y,and z are position relative to the
reference frame, is the yaw of the needle in the plane, is
the pitch of the needle out of the plane, and is the roll of
the needle. Moreover, denotes the curvature which needle
follows. Body frame velocity may be expressed as
= J q (3)
Fig. 1. Kinematic bicycle model [4] used with permission from the authors
where
J =
R
T
AB
0
33
0
33
S
S =
(4)
where R
AB
is the rotation matrix between frames A and
B. Now, using (1) and (3) the exible bevel-tip needle model
is
q = J
1
V
1
u
1
+J
1
V
2
u
2
=
sin 0
cos sin 0
cos cos 0
cos sec 0
sin 0
cos tan 1
u
1
u
2
(5)
In order to stabilize the needle to the yz plane, the states
y, z, and need not be controlled. Moreover, these states do
not affect the dynamics of the remaining states. Hence, we
can dene p
T
= [p
1
, p
2
, p
3
] = [x, , ] as the state vector
of the reduced order system, which can be represented as
follows.
p = f(p)u
1
+g(p)u
2
=
sin(p
2
)
sin(p
3
)
cos(p
3
) tan(p
2
)
u
1
+
0
0
1
u
2
(6)
r = h(p) = p
1
(7)
Note that p
T
=
0 0 0
sin(p
2
)
sin(p
3
)
cos(p
3
) tan(p
2
)
0
0
1
u (8)
4850
where u =
u2
u1
r = h(p) = p
1
(9)
In essence, we can only measure p
1
= x by image
processing and the other state variables should be estimated.
Furthermore, the parameter is the unknown curvature of
the needle which should be estimated.
The system (8) and (9) can be transformed into output
feedback linearized form using the following transformations
[19]
w = [h(p), L
f
h(p), L
2
f
h(p)]
= [p
1
, sin p
2
, cos p
2
sin p
3
]
(10)
v = L
3
f
h(p) +L
g
L
2
f
h(p)u
=
2
sin p
2
+cos p
2
cos p
3
u
(11)
where L
f
h(p) is the Lie derivative of h with respect to f,
dened by [19]
L
f
h(p) =
h(p)
p
f(p) (12)
Indeed, this is the familiar notation of the derivative of h
along the trajectories of the system p = f(p). Moreover, in
a similar manner we have
L
g
L
f
h(p) =
L
f
h(p)
p
g(p) (13)
L
k
f
h(p) =
L
k1
f
h(p)
p
f(p) (14)
Now, the transformed system is
w = Aw +Bv =
0 1 0
0 0 1
0 0 0
w +
0
0
1
v (15)
r = Cw =
1 0 0
w (16)
Obviously, the system (15) and (16) is not linearly
parametrized. Therefore, the traditional Model Reference
Adaptive Control (MRAC) [20] cannot be applied to it.
III. THE PROPOSED SCHEME
Consider a nonlinear system, subjected to parametric un-
certainty, described as
x = f(x, u, ) = f
0
(x, u) +f
T
1
(x, u) (17)
y = h(x, u) (18)
where x R
n
is the state vector, u R
m
is the control
input vector, and R
p
is the parameter vector. Moreover,
f
0
and f
1
are known nonlinear functions. Obviously, the
system (8) and (9) can be stated as (17) and (18). Our
objective is to design an adaptive controller-observer pair
for the above system such that the stability is preserved
and tracking a reference signal x
d
(t) is achieved in the
presence of unknown parameter vector . In order to do so,
the adaptive controller given in [16] is combined with a
nonlinear high gain observer with some modications.
Assumption: There exist
(a)a Hurwitz matrix A
(b)an open set D
x
R
n
containing x
d
(t) for all t
(c)an open set D
R
p
containing
(d)a family of parametrized diffeomorphisms
W : D
x
R
n
: z = W(x,
) (19)
exists where
is an estimation of and such that the
following implicit equation in the unknown u
W
x
(x,
)[f
0
(x, u) +f
T
1
(x, u)]
=
W
x
(x
d
,
) x
d
A[W(x
d
,
) W(x,
)] (20)
has a unique bounded solution u = u
a
(x, x
d
,
) for all x
D
x
. Now, taking the derivative of z we have
z =
W
x
(x,
)[f
0
(x, u) +f
T
1
(x, u)] +
W
(x,
)
(21)
The previous equation is equal to
z = g
0
(x, u,
) +g
T
1
(x, u,
) +g
2
(x,
)
(22)
where
g
0
(x, u,
) =
W
x
(x,
)f
0
(23)
g
T
1
(x, u,
) =
W
x
(x,
)f
T
1
(24)
g
T
2
(x,
) =
W
(x,
) (25)
Transformation (19) should convert the system (17) and
(18) to normal form. Indeed,
z = z +B(x, u)
y = Cz
(26)
where
=
0 1 0
.
.
.
.
.
.
.
.
.
.
.
.
0 0 1
0 0 0
B
T
=
0 0 1
C =
1 0 0
(27)
A high gain observer can be designed for the above system
as [19]
z = z +B
0
(x, u) +H(y y)
y = C z
(28)
where z is the estimation of z. The observer gain H is chosen
as
H
T
=
2
n
(29)
where is a positive constant to be specied and the positive
constants
i
are chosen such that the roots of
s
n
+
1
s
n1
+ +
n1
s +
n
= 0 (30)
are in the left-half plane. The function
0
(x, u) is a nominal
model of (x, u). The observer error is in the following form
z = z +B(x, u) HC z (31)
4851
where z = z z and (x, u) = (x, u)
0
(x, u). The above
equation is equal to
z = A
o,
z +B(x, u) (32)
where
A
o,
=
1 0
.
.
.
.
.
.
.
.
.
.
.
.
n1
n1
0 1
n
0 0
(33)
Lemma 1 [18]: For the matrix E = diag(1, , ...,
n1
), we
have the following facts:
(a)
A
o,
=
1
E
1
A
o
E, B =
1
E
1
B
(34)
where
A
o
=
1
1 0
.
.
.
.
.
.
.
.
.
.
.
.
n1
0 1
n
0 0
B
T
0 0
n
(35)
(b) Let the positive denite matrix S be the solution of
A
T
o
S +SA
o
= Q, then
A
T
o,
S
+S
A
o,
=
1
E
T
QE (36)
where S
= E
T
SE
In order to nd an adaptation law, the dynamic equation
(22) should be stated in terms of observed states.
z = g
0
( x, u,
)+g
T
1
( x, u,
)+g
2
( x,
)
+
1
(x, x, u) (37)
where the denition of g
i
for i = 0, 1, 2 is similar to those
stated in (23) , (24) and (25) and
1
(x, x, u) is a bounded
uncertainty. Adaptation law can be stated as follows
= g
0
( x, u,
) +g
T
1
( x, u,
)
+[g
T
2
( x,
)g
1
( x, u,
)P ][z ]
(38)
= g
1
( x, u,
)P[W( x, ) ]
(39)
where is the auxiliary variable. Moreover, is an arbitrary
Hurwitz matrix and the positive denite symmetric gain
matrix P is the solution of the following Lyapunov equation.
T
P +P = Q (40)
where Q is an arbitrary positive denite matrix.
The special statement of (37) and denition (38) will make
the proof of the proposed approach possible. Now, the error
system can be written as
g
T
1
( x, u, ) 0
g
1
(x, u, )P 0 0
0 0 A
0
1
(x, x, u)
0
2
(x, x, u)
(41)
where
= z ,
=
1
= sup[
1
(x, x, u)] (42)
2
= sup[
2
(x, x, u)] (43)
Theorem 1: For the error system (41),
,
and z are
bounded. Moreover, the tracking error is bounded.
Proof: The following Lyapunov function is considered for
the system
V (
,
, z) =
T
P
+
T
+ z
T
S z (44)
By computing the derivative of (44) and using lemma 1 we
have
V =
T
Q
+ 2
T
P
1
(x, x, u)
1
z
T
E
T
QE z
+
2
(x, x, u)B
T
S z
(45)
then we can state
V ||
||
2
(
min
(Q) 2
||P||1
||
||
)
|| z
||
2
(
min
(Q)
2
n
2||S||
|| z||
)
(46)
where z
n
<
min(Q)
22||S||
(47)
such that the derivative of the lyapunov function is negative,
provided that
||
|| 2
||P||1
min(Q)
(48)
Since the derivative of the Lyapunov function is negative
outside a region, the system response cannot go outside of
this region. Indeed, the error is ultimately bounded.
Now, let e = z
d
z be the tracking error. It can be simply
founded that e is the output of the following lter
= A + ( A+
3
(x, x, u, ))
(49)
e =
(50)
The uncertain term
3
(x, x, u, )) come from the obser-
vation error and the other terms that do not cancel by the
control signal. It is assumed that this term is bounded. Since
the lter (49) and (50) is stable and its input is bounded, the
tracking error, which is the output of the lter, is bounded.
In brief, in order to utilize the proposed control method-
ology, the following steps are required. First, The system
should be stated as (17) and (18). Then, an appropriate
diffeomorphism should be found using nonlinear control
theories to transform the system in the form of (22). Finally,
the observer and adaptation law are designed using (28), and
(38) and (39), respectively.
4852
(a) First state variable (p
1
= x).
(b) Second state variable (p
2
= ).
(c) Third state variable (p
3
= ).
Fig. 2. Simulation results of the needle guidance problem with previously
proposed output feedback linearization method of [12].
(a) First state variable (p
1
= x).
(b) Second state variable (p
2
= ).
(c) Third state variable (p
3
= ).
Fig. 3. Simulation results of the needle guidance problem with the output
feedback adaptive control method proposed in this paper.
4853
Fig. 4. The estimation of needle curvature ()
IV. SIMULATION RESULTS
The proposed adaptive output feedback controller is em-
ployed to solve the problem of guiding a exible bevel-tip
needle into a desired plane. The response of the system
is studied for two cases, rst by applying the observer-
based controller proposed in [12] and second by applying the
proposed controller. In both cases the real needle curvature
is = 0.06 but our knowledge about this parameter is not
exact. The known needle curvature is = 0.04. The initial
conditions for the both cases are X
0
= [0.1, 0.2, 0.8]
T
.
In Fig. 2, the responses of the system with observer-
based controller [12] are shown. Since, with this controller
no parameter uncertainty is tolerated, the system response
should not be acceptable. Simulation results prove this
fact. In Fig. 3, the responses of the system with proposed
controller are plotted. This gure shows the good stability
and convergence of the adaptive output feedback controller.
All state variables go near zero after a transient state. The
estimated needle curvature is shown in Fig. 4. Although this
parameter converges to a value, the converged value is not
the real one. This problem is not surprising, in that it can
be predicted theoretically from the given facts of Section
III. Since the error is ultimately bounded, we know that the
parameters just converge to a value. It is not any necessity
that the converged value be the real parameter.
V. CONCLUSIONS AND FUTURE WORKS
In this paper a novel observer-based controller is pro-
posed for a class of nonlinear systems which are linearly
parametrized and feedback linearizable. The proposed strat-
egy is a modied version of a previously proposed adaptive
control scheme using high gain observer. The proposed
methodology is employed to guide a exible bevel-tip needle
into a desired plane. A nonholonomic reduced order model
is considered for the needle which the needle curvature is
its only parameter. By utilizing the adaptive control, it is
possible to estimate the needle curvature in medical tasks
online. Through simulation results, it was demonstrated that
the proposed approach is quite effective for steering medical
needles into a desired plane.
Our next step is to evaluate the proposed methodology
with real data. One formidable barrier to achieve a good
performance in image guided tasks is measurement noise.
A more improved methodology can be proposed which
tolerates this barrier. The presented approach can also be
used with the automatic or manual path planning schemes.
REFERENCES
[1] R. Alterovitz, K. Goldberg ,J. Pouliot, R. Taschereau, and I. C. Hsu,
Sensorless planning for medical needle insertion procedures, in Proc.
IEEE/RSJ Int. Conf. Intell. Robot. Syst. (IROS), 2003, vol. 3, pp. 3337-
3343.
[2] E. Dehghan, and S. E. Salcudean, Needle Insertion Parameter Opti-
mization for Brachytherapy, IEEE Trans. Robot. ,vol. 20, no. 2, pp.
303-315, 2009.
[3] R. J. Webster III, J. Memisevic, and A. M. Okamura, Design
consideration for robotic needle steering, in Proc. IEEE Int. Conf.
Robot. Autom. (ICRA), Barcelona, Spain, 2005, pp 3588-3594.
[4] R. J. Webster III, J. S. Kim, N.J. Cowan, G. S. Chirikjian and A.
M. Okamura Nonholonomic Modelling of Needle Steering, Int. J.
Robot. Res., vol. 25, no. 5-6, pp. 509-525, 2006.
[5] R. Alterovitz, T. Simon, and K. Goldberg, The Stochastic Motion
Roadmap: A Sampling Framework for Planning with Markov Motion
Uncertainty, in Proc. Robotics: Science and Systems, 2007.
[6] V. Duindam, J. Xu, R. Alterovitz, S. Sastry, K. Goldberg, 3D Motion
Planning Algorithms for Steerable Needles Using Inverse Kinematics,
in Workshop on the Algorithmic Foundations of Robotics, 2008.
[7] V. Duindam, R. Alterovitz, S. Sastry, K. Goldberg, Screw-Based
Motion Planning for Bevel-Tip Flexible Needles in 3D Environments
with Obstacles, in Proc. IEEE Int. Conf. Robot. Autom. (ICRA),
Pasadena, CA, USA, 2008, pp. 2483-2488.
[8] J. Xu, V. Duindam, R. Alterovitz, and K. Goldberg, Motion planning
for steerable needles in 3D environments with obstacles using rapidly-
exploring random trees and backchaining, in Proc. IEEE Conf. Autom.
Science and Engineering (CASE), 2008, pp. 41-46.
[9] D. S. Minhas,J. A. Engh, M. M. Fenske, and C. N. Riviere, Modelling
of needle steering via duty-cycled spinning, in Proc. Int. Conf. IEEE
EMBS Cite Internationale, pp. 27562759, 2007.
[10] J. M. Romano, R. J. Webster III, and A. M. Okamura, Teleoperation
of steerable needles, in Proc IEEE Int. Conf. Robot. Autom. (ICRA),
2007, pp 934-939.
[11] N. Abolhassani, R. Patel and M. Moallem, Needle Insertion into Soft
Tissue, A Survey, Med. Eng. Phys., vol. 29, pp. 413-431, 2007.
[12] V. Kallem, and N. J. Cowan, Image Guidance of Flexible Tip-
Steerable Needles, IEEE Trans. Robot., vol. 25, no. 1, pp. 67-78,
2009.
[13] K. B. Reed, V. Kallem, R. Alterovitz, K. Goldberg, A.M. Okamura,
and N.J. Cowan. Integrated Planning and Image-Guided Control for
Planar Needle Steering, in Proc. 2nd Biennial IEEE/RAS-EMBS Intl.
Conf. Biomedical Robot. and Biomechateronics, Scottsdale, Arizona,
2008.
[14] H. K. Khalil, Adaptive output feedback control of nonlinear systems
represented by input-output models, IEEE Trans. Automat. Contr.,
vol. 41, no. 2, pp. 177-188, 1996.
[15] K. W. Lee, and H. K. Khalil, Adaptive output feedback control of
robot manipulators using high-gain observer, Int. J. Contr, vol. 67,
no. 6, pp. 869-886, 1997.
[16] G. Campion and G. Bastin, Indirect adaptive state feedback control
of linearly parametrized nonlinear systems, Int. J. Adaptive Control
and Signal Processing, vol. 4, 345-358, 1990.
[17] M. R. Rokui, and K. Khorasani, An indirect adaptive control for
fully feedback lunearizable discrete-time nonlinear systems, Int. J.
Adaptive Control and Signal Processing, vol. 11, 665-680 (1997)
[18] A. Tornambe, High-gain observers for non-linear systems, Int. J.
Syst. Sci., vol. 23,no. 9, 1475-1489, 1992.
[19] H.K. Khalil, Nonlinear Systems. , Prentic Hall, Third Edition, 2002.
[20] J. J. E. Slotine, W. Li, Applied Nonlinear Control. Prentice Hall, 1991.
4854