- Email: [email protected]
Observability and Nonlinear Observers for Mobile Robot Localization1
Copyright @ IFAC Robot Control. Vienna. Austria. 2000
OBSERVABILITY AND NONLINEAR OBSERVERS FOR MOBILE ROBOT LOCALIZATION 1 Fabio Conticelli' Antonio Bicchi" Aldo Balestrino'"
• Centra "E. Piaggio ", University of Pisa •• Centra "E. Piaggio ", University of Pisa ••• Dept. Electrical Systems and Automation, Univ. of Pisa
Abstract: In this paper, the problem of localizing a nonholonomic vehicle moving in an unstructured environment is addressed by the design of a novel nonlinear observer, based 011 suitably processed optical information. The localization problem has been shown to be intrinsically nonlinear , indeed the linear approximation of the system has different structural properties than the original model. The proposed nonlinear observer allows the localization of nonholonomic mobile robots by using an estimated expression of the extended output Jacobian , local practical stabilization of the observation error dynamics is also ensured. A singularity-avoidance exploration strategy is also included to deal with the output Jacobian singularities crossing. Copyright @2000 IFAC
Keywords : Nonholonomic robot dynamics , Nonlinear observability, Nonlinear observers, Local practical stability, Singularities avoidance
mations, which have been proposed in the literature, are the system immersion (Fliess and Kupka , 1983) which permits to obtain a bilinear system if the observation space is finite dimensional, and the linearization by means output inj ection (Krener and Isidori . 1983: Krener and Respondek . 1985: Levine and \:larino, 1986) assuming that particular differential-geometric conditions on the system vector fields are verified . Rank conditions under which the dynamics of the observation error is linear. i.e. the original system can be transformed into the observer canonical form. are also investigated in (Xia and Gao. 1989). Results on bilinear observers are presented in (F. Celle and Kazakos. 1986: Hammouri and Gauthier , 1988) . Extension of the Luenberger filter in a non linear setting. by using the time derivatives of the input, has been proposed in (Zeitz , 198i) .
1. INTRODUCTION
The problem of mobile robot localization using optical information is intrinsically nonlinear (Bicchi et al. , 1998). in fact linearized approximations are non- observable, while tools from differential- geometric nonlinear system theory prove the possibility of reconstructing the position and orientation of the vehicle and the position of the obstacles in the environment. The localization problem can be embedded in the more general problem of current state estimation of a d:vnamical system using input-output measurements. For smooth nonlinear systems, a classical approach to design an observer is to apply the extended Kalman filter. see (J. Borenstein and Feng. 1996: Gelb, 19i4: Misawa and Hedrick , 1989). Another way of designing an observer is to transform the original non linear system into another one for which the design is known. Transfor-
A non linear observer and its practical implementation have been presented in Gauthier et a!. (J .P. Gauthier and Othman. 1992: Bonard and Hammouri. 1991) . in which the first step is writing the input affine nonlinear system in a
1 This work partially supported by ?\IURST (project RA:-'ISETE) and AS! (project TE~IA).
663
so-called normal observation form. However, this form requires that the trivial input is an universal input (Bonard et al. , 1995) for the system. In our problem, this condition is violated by the mobile robot kinematics.
a mobile robot (about an arbitrary initial state and zero control), it can be easily seen (Bicchi et al., 1998) that the linearized system is always unobservable if there are targets (N i= 0), and can only be observable if there are at least 3 markers (M ~ 3). It has been proven in (Bicchi et al., 1998), by using the codistribution associated to the observability space, that if, at least. two markers are available, nonlinear observability of the location of the vehicle and of targets follows. In this section, a novel nonlinear observer for mobile robot localization is presented, by considering the robot with N = 0 (no targets) and two markers AI = 2, i.e. x = g(x , u), where x = [.;- ( E R2 X 51 is the state vector, and
In this paper, a local nonlinear observer for mobile robot localization is designed, inspired by the work of Teel and Praly (Teel and Praly, 1994) . The basic idea is to use output derivatives, which are elements of the observability space associated to the nonlinear measurement process, in the assumption that a rank condition on the observability matrix is satisfied for the considered input function. The observer design is based on the concept of Extended Output Jacobian (EOJ) matrix, which is the collection of the covectors associated to the considered elements of the observability space, i.e. the output and its derivative. Then, the output derivatives are estimated by using high-pass filters. Local practical stability of the observation error dynamics is guaranteed since the introduced persistent perturbations can be made arbitrarily small. A singularity-avoidance exploration task is also addressed to deal with the singularity occurrences in the EOJ matrix.
OV
(1)
with output measurements: y with
= h,(x) = 11' + atan2(
y,
= h(x) = [Yl Y2V,
- (,,( - (,) - 0, i
= 1,2.
The following observer is introduced for the nonholonomic vehicle:
The paper is organized as follows. In sect. 2, a nonlinear observer for nonholonomic vehicle localization is designed. In sect. 3, conditions on the input vector which ensure singularity avoidance are also derived . Sect. 4 reports simulation results in both the cases of output measurements affected by bounded noise, and of singularity-avoidance exploration. Finally, in sect. 5 the major contribution of the paper is summarized.
ic. = g(x, u) + P
(J.l - {l)
(2)
where J.l = [yT yTf E R 4 , and {l(x) = [h(x)T h(x)TV, the design parameter P E R3x4 is chosen as P = (Q + *(x))J+, being Q a positive definite matrix, the Extended Output Jacobian (EOJ) matrix J is defined as:
J(x, u)
Notation - 1/.1/ denotes the Euclidean norm for a vector and an induced natural norm for a matrix . URn)" is the dual space of the Euclidean space Rn. Given a scalar function f : Rn -* R, df denotes the associated exact differential. Given a vector field f : Rn -* RP, df denotes the associated Jacobian matrix.
= d{l(x) = [!~]
E R4x3
(3)
If J is full-rank, J+ = (J T J) -1 JT is its left pseudo-inverse. We now give a preliminary motivation of the introduced observer, based on the Implicit Function Theorem. Rearrange the output equation and its derivative in the form:
F z x ( ,
2. NOl'\LINEAR OBSERVERS FOR LOCALIZATION
)
= (Yy-h(x) - ~(x)) = (.y-~~g(x,u) y -:- h(x) ) = (0) 0
where we have introduced the variable z = [I-' T u TV. Since ~~ = -J, if the matrix J is full-rank in a fixed point (ZO,XO), then, by using the Implicit Function Theorem, there exist two neighborhoods A of zO , BO of xO, and an unique map 1 : A O -* BO, with l(z) = x, such that F(z,l(z)) = O. Hence, the full-rank of matrix J implies the local existence of a static map which relates the state with the output, its time derivative, and the input . Since the analytical expression of the map l(z) can not, in general, be derived, in the nonlinear observer (2) we have chosen to consider directly the implicit condition F(z, x) = 0 and linearize this expression with respect to the actual
Consider a system comprised of a mobile vehicle, such as a robotic rover in a planetary exploration mission, which moves in an unknown environment with the aim of localizing itself and the environment features (in the rover case, e.g., rocks and geological formations). Among the features that the sensor head detects in the robot environment , we will distinguish between those belonging to objects with unknown positions (which we shall call targets), and those belonging to objects whose absolute position is known, which will be referred to as markers. From the linear approximation of
°
664
estimate X, as will be clear soon. The following proposition formally summarizes the properties of the proposed observer in the case of exact measurements of the output derivatives.
[io, T], for some T > to, i.e. the observability matrix for the particular input u( t), t E [to . T] is nonsingular in a neighborhood of XO (Xia and Gao , 1989: Zeitz. 1987).
Proposition 1. Assume exact measurements of the output derivative. and that the EOJ matrix is full rank, i.e. rank(J) = 3, "Ix. u of interest . Then the equilibrium e = x - x = 0 of the observation error dynamics constructed by using the observer in Eq. (2) is locally asymptotically stable.
Example 4. In the case of a holonomic vehicle described by the equation: x = u, x. u E R2. assume, for example, that it is possible to measure only the distance: y = ~(xi +x§). The EOJ matrix results: J(x , u)
Proof: The observation error has the following dynamics:
= [ Xl U1
(6)
(4)
the locus of singularities is given by the set S = {(x, u) E R2 X R2 : X l Uz - Xz U1 O}. The above analysis indicates that the holonomic vehicle is not uniformly observable. and suggests to choose the input u such that, at each time instant , the point (x , u) does not lie on the manifold S.
where we have linearized the vector field g(x, u) and the extended output vector J.l around the actual state estimate, i.e. g(x, u) - g(x , u) = ~(x) ( x - x) + Oi(llglll and J.l- fi = J(x, u)(xx) + O~(llell).
To prove the practical stability of the observer in Eq. (2) in case of estimated output derivatives, we first remind the following result on the total stability of nonlinear systems , see for example (Isidori, 1995) :
Consider the quadratic Lyapunov function candidate ~r ~eT e, its time derivative results:
Proposition 5. Consider the nonlinear system x = f(x ) + g(x)u(t) , where x ERn , uti) E Rm , and f(x) , g(x) are smooth vector fields . Assume that the origin of x = f (x) is a locally asymptotically stable equilibrium. Then 'It > 0, there exist 61 > 0 and K > 0 such that if Ilx(O)11 < 61 and Ilu(t) 1I < K, t ~ to ~ 0, the solution x(t, to, u) of x = f(x) + g(x)u(t) satisfies the condition : Ilx(t , to , u) 11 < c, t ~ to ~ o.
e = g(x, u)
- g(x, u) + P (J.l - fi)
= (~:(x))
=
e-PJ(x.u)e+PO~(llelll
- O~(llell)
=
\> = eT e
=eT [(
:!
(5 )
(x))e - PJ(x, u)e
- O~(IIell)l
+ PO~ (lIelll
= _eT Qe + PO~ (llel!) -
O~(llell)
Hence, the function ( r is negative definite in a 0 neighborhood of the origin e = x - x = O.
Now we state the following.
Remark 2. To design the matrix P. it has been used the pseudo-inverse of J, the study of the locus of singularities is then necessary to investigate system trajectories and particular inputs which can cause loss of rank in J. \Ve first remark that in the case of an uniformly observable SISO (Single Input Single Output) nonlinear system (Teel and Praly. 1994: Isidori, 1995), given the map [y "if ... y (n - JJjT = 1>(x. v) . being x E Rn the state vector, and v = [u it . . . u(n-2 J jT E Rn-1 the extended input vector, the matrix J, which is is related to the expression of the tangent map of 1>(x. v) . is nonsingular. for definition. for each (x. v) E Rn X Rn-I. Thus. uniformly observability is a sufficient condition to design the observer (2).
Proposition 6. Consider the non linear observer:
~ = g(x, u) :
1jJ
P
[~~~~]
+P
[~]
(7)
= T1 ( .1. + y. ) -
(1/
where T is a sufficiently small positive constant. Assume that the EOJ matrix is full rank. i.e. rank(J) 3, 'Ix. u of interest . Then the equilibrium e = x - x = 0 of the observation error dynamics constructed by using the observer in Eq. (7) is locally practically stable. i.e. 'Ice > 0 there exist 61 > 0, K > 0, and T which depends on K , such that if the initial observation error is sufficiently small. i.e Ile(O) 11 < 61 , then Ilc" (t)1I < K, t ~ O. being c" = lOT cTjT . { = Y - lp. and the observation error satisfies the condition Ile(t,O.{*)11 < Ce, t ~ O.
=
R emark S. Assume that the input function uti) , t ~ to ~ 0 of the vector field in Eq. (1) is real analytic. The matrix J is full-rank in a open neighborhood XO x R2, if the system is locally observable at every xO E XO in the interval
665
where , in general , given x ERn , sgn(x) = [sgn(xd , .. . , sgn(xn)V, being sgn(xi) the sign function, which values 1 if Xi > 0, -1 if Xi < 0, and sgn(O) = O.
Proof: The dynamics of the observation error e = x - x result~: e = -Qe - Pf" + PO~(jlell) - O~(jlell). where f" = [0 fV, f = Y - 1/; is the introduced error due to the output derivative estimation. The output derivatives error f can be reduced to an arbitrarily small perturbation if the constant T is sufficiently small. In fact , assumed 1/; (0) = 0, the Laplace transform of f results: f(S) = yts) -
Fixed an arbitrary input initial condition, a first technique to satisfy the above equation is to compute the input function through integration from Eqs. (8) or (10). In particular in the latter case, since the input derivative u is piecewise constant. the input u is simply piecewice linear. Since the input u can be also used to steer the mobile robot in a desired area. a second technique is to embed the singularity avoidance problem in a secondary mobile robot task to be accomplished.
I+;'S Y(s) + ~lo;l = T I +sTs Y(s) , where Y(s) denotes the output Laplace transform. and (with an abuse of notation) Y(s) = s Y(s) - y(O+) denotes the output derivative Laplace transform . Vt ~ 0, f(t) = Ty,(t) . where Yj(t) is the output derivative filtered by l+sTs' Since the high-pass filter l+sTs is Bounded Input Bounded Output (BIBO) , there exists My > 0 such that IIy,(t)II < _~fy. Vt ~ O. Hence \:j( > 0, by choosing T < then Idt)1 < T My < (, Vt ~ O. Let us consider the non linear system e = -Qe - Pf" + PO!( lI ell) - O~(jlell) . If f"(t) = 0, t ~ 0 the origin e = 0 (of the unperturbed system) is locally asymptotically stable. Prop . .'.i indicates that Vfe > 0 there exist 15 1 > 0 and K > 0 such that if the initial observation error is sufficiently small, i.e lIe(O)1I < 15 1 and choosing { = K , since there exists T such that IIf"1I = IIfll < ( = K. then the observation error satisfies the condition lIe(t,O,f")1I < fe, t ~ 0.
Let us define the matrix : X = PJ(x,u) E R 3x3 , where the matrices P and J(x , u) have been introduced in the observer equation (2). In the Prop. 1, fixed X = Q + ~(x), it has been chosen P = X (J T J)-l JT to solve the equation X = PJ, assuming J is full-rank . Let us denote with Pi E (R4)* , i = 1, . .. 3 the rows of the matrix P, it is well-known that this matrix solves the problem:
At,
{
L2
(11)
= PJ(x. u)
Instead of minimizing the norm of each row , we can choose to minimize the norm of the difference between P i and a suitable Pi , to be chosen later. We consider the constrained minimization problem:
3. SINGULARITY AVOIDANCE
{ mJn
Consider the singularity index associated to the EOJ matrix defined as log w(x , u) , where the introduced singularity function is w(x , u) = det (JT J) . The quality of the current state estimation depends on the actual value of w(x, u) . A possible singularity-avoidance strategy to guarantee a well-conditioned EOJ matrix is to impose the following input derivative expression
X
t. ~(P,
- Pi) (P, - p;lT
= PJ(x , u)
(12)
.
By introducing the Lagrangian multipliers Ai E = 1, . .. ,3, the problem is solved by considering the function : L(P , A) = L~=la(Pi Pi) (Pi - pj)T + (X i - Pi J) Ai], where Xi E (R 3 )" , i = 1, . . . , 3, are the rows of the matrix X and A = [AI A2 A3J E R 3x3 is the matrix of Lagrangian multipliers. Simple computations show that the conditions:
J
R3, i
where &tL'~:'U ) E OfF)" . If this is the case. the time derivative of the singularity function w(x , u) results: . 'I( ~)T &w (x u ) 112 x+k" 1
p
X
o
w (x,u ) = ( all' &x (x, u))
3 1 min ~ -P,P;
{
8L(P,A) = 0 8pT 8L(P:A) --::-::---=0 8A;
(13)
i
= 1. ... 3
lead to the solution, if J is full-rank. expressed in the matrix form: P = XJ + +P" (I -J J+), where the matrix P" E R3 x 4 is the collection of the covectors Pi. It is simple to show that Props. 1. and 6 also hold if the matrix P is chosen in the observer equation (2) as P = X J+ + P" (I - J J+) with X = Q + ~(x). since P" (I - J J+) J = O. VP" E R3 x4
(9)
which is certainly positi\'e if k" is chosen sufficiently large. ~otice that another way to obtain an increasing singularity function is to choose
666
The matrix P' can be chosen to obtain a well conditioned EOJ. In fact. let us assume that the input is a function of the actual state estimate. i.e. u = nix) . where Q : R3 -+ R2 is. at least. of class Cl. Given the singularity function lL·(X. u ) = j det (JT J). the condition (8) (a similar condition is obtained by imposing (10))
·:.lI ---.,....------,;;---~
~'ields:
8a l x ) ,
'
)
~ l g(X.U , +
p_ (
8w(x , u) T
, ,'
1l-Il Jl =k- ( ~)
a) (1" )
where Eq. (2) is used. The abO\'e equation is linear in P' and imposes two scalar conditions. Hence. a matrix p', such that Eq. (8) (or (10)) is satisfied during the vehicle motion. can be computed from (14) by least-square estimation.
'r
f'
..~-------.--------~~------~
b)
4. SnIULATIO!\ RESULTS To investigate the performance of the observer (7). the nonholonomic sy'stem in Eq.(I) has been simulated in Matlab environment. In the reported trials, the markers have the following coordinates: (PI (I) = (0.0) , and (P2 (2) = (0, 1), the control parameters are: Q = 2. and T = 0.001. ~loreov e r. we assumed the presence of output measurements by , modeled as band-limited white noise.
" r " r
'. c)
To esemplify the singularity-avoidance strategy (see Sect. 3), we report two trials in which the input has been chosen as u = :Ex , where the matrix :E E R 2 x3 is constant and values:
o 1
d)
~]
in particular the input vector does not depend on the estimated angle X3· The matrix P' is computed. at each iteration. from Eq. (14) by leastsquare estimation. such that Eq. (10) is satisfied with k* = 1. In the first simulation the initial conditions of the real and estimated state are respectivel~' x = [1. 1. 'rlT. and x = [1.5. 0.5. 38,Y. The control parameters are: Q = 2, and T 0.001. The results (shown in fig. 1) indicates that the singularity index remains greater than 10- 8 . which implies a satisfactory quality of the current state estimate. In the second trial. reported in fig. 2. the previous simulation has been repeated in the presence of output measurements noise. In particular. we assumed that by is band-limited \I'hite noise with po\\'er 10- 5 . The results indicate that the observation error is bounded . while the EOJ matrix is well conditioned.
e)
Fig. 1. ~onholonomic vehicle (input avoiding singularities): state estimation without output noise . a) real and estimated Xl component. b) real and estimated X2 component. and c) real and estimated X3 component of the state vector. d) Singularity index of the EOJ matrix: log w(x. u): and e ) vehicle motion in the ~ - ( plane.
=
output deri\'atil'es I'ia linear high-pass filters. and the concept of EOJ matrix. Future iIl\'estigations \\'ill concern the extension of the proposed approach to more general non linear systems and the problem of set-point regulation by using the current state estimate.
A nonlinear obserwr has been proposed for nonholonomic mobile robots . which uses estimated
667
systems. In: Proc. 30th IEEE Conference on Decision and Control. Bonard, G., F . Celle-Couenne and G. Gilles (1995). Nonlinear Systems. Vol. 1. Chap. Observability and observers. Chapman & Hall. F. Celle, J .P. Gauthier and D. Kazakos (198G). Orthogonal representations of nonlinear systems and input-output maps. Systems fj Control Letters (7), 3G5-372. Fliess, M. and I. Kupka (1983) . A finiteness criterion for non linear input-output differential systems. SIAM Journal Control and Optimization (21), 712-729. Gelb, A. (1974). Applied optimal estimation. M.I.T. Press . Cambridge, MA. Hammouri. H. and J.P. Gauthier (1988). Bilinearization up to output injection. Systems fj Control Letters (11), 139-149 . Isidori, A. (1995). Nonlinear Control Systems. 3rd ed .. Springer Verlag. J. Borenstein, A. Everett and L. Feng (1996). Navigating mobile robots. AK Peters. J.P. Gauthier, H. Hammouri and S. Othman (1992). A simple observer for nonlinear systems . application to bioreactors. IEEE Trans. A utomatic Control. Krener, A.J. and A. Isidori (1983). Linearization by output injection and non linear observers. Systems fj Control Letters (3), 47-52. Krener, A.J. and W. Respondek (1985). Nonlinear observers with linear error dynamics. SIAM Journal of Control and Optimization (23), 197-216. Levine, J. and R. Marino (1986). Nonlinear system immersion, observers and finite dimensional filters. Systems fj Control Letters (7), 137-142. Misawa, E.A. and J.K. Hedrick (1989). Nonlinear observers - a state-of-the-art survey. Trans. ASME 111 , 344-352. Teel, A.R. and L. Praly (1994). Global stabilizability and observability imply semi-global stabilizability by output feedback. Systems fj Control Letters (22), 313-325. Xia, Xiao-Hua and Wei-Bin Gao (1989) . :\'onlinear observer design by observer error linearization. SIAM Journal on Control and Op· timization (27), 199-216. Zeitz , M. (1987). The extended luenberger observer for non linear systems. Systems 8 Control Letters (9), 149- 156.
a)
.., ::t
. ..
b)
'·'1-------=~~~-'~-~-.~-~'.>~-..~ ..•~'-="-------, >of
::r c)
d)
..=-«=•.
'~ I ------~-~-~~='-=.-.~"~=-~
. I
-~------
~
,
.
~
e)
Fig, 2. r\onholonomic vehicle (input avoiding singularities): state estimation in presence of output noise. a) real and estimated Xl component. b) real and estimated Xz component, and c) real and estimated X3 component of the state vector. d) Singularity index of the EOJ matrix: logw(x. u): and e) vehicle motion in the'; - ( plane. 6. REFERENCES
Bicchi. A.. D. Prattichizzo. A. )'Iarigo and .\. Balestrino (1998). On the obsen'ability of mobile \"ehicles localization. In: Proc. IEEE Mediterranean Conf. On Control And Systems. Bonard. G. and H. Hammouri (1991). A high gain observer for a class of uniformly observable
668
OBSERVABILITY AND NONLINEAR OBSERVERS FOR MOBILE ROBOT LOCALIZATION 1 Fabio Conticelli' Antonio Bicchi" Aldo Balestrino'"
• Centra "E. Piaggio ", University of Pisa •• Centra "E. Piaggio ", University of Pisa ••• Dept. Electrical Systems and Automation, Univ. of Pisa
Abstract: In this paper, the problem of localizing a nonholonomic vehicle moving in an unstructured environment is addressed by the design of a novel nonlinear observer, based 011 suitably processed optical information. The localization problem has been shown to be intrinsically nonlinear , indeed the linear approximation of the system has different structural properties than the original model. The proposed nonlinear observer allows the localization of nonholonomic mobile robots by using an estimated expression of the extended output Jacobian , local practical stabilization of the observation error dynamics is also ensured. A singularity-avoidance exploration strategy is also included to deal with the output Jacobian singularities crossing. Copyright @2000 IFAC
Keywords : Nonholonomic robot dynamics , Nonlinear observability, Nonlinear observers, Local practical stability, Singularities avoidance
mations, which have been proposed in the literature, are the system immersion (Fliess and Kupka , 1983) which permits to obtain a bilinear system if the observation space is finite dimensional, and the linearization by means output inj ection (Krener and Isidori . 1983: Krener and Respondek . 1985: Levine and \:larino, 1986) assuming that particular differential-geometric conditions on the system vector fields are verified . Rank conditions under which the dynamics of the observation error is linear. i.e. the original system can be transformed into the observer canonical form. are also investigated in (Xia and Gao. 1989). Results on bilinear observers are presented in (F. Celle and Kazakos. 1986: Hammouri and Gauthier , 1988) . Extension of the Luenberger filter in a non linear setting. by using the time derivatives of the input, has been proposed in (Zeitz , 198i) .
1. INTRODUCTION
The problem of mobile robot localization using optical information is intrinsically nonlinear (Bicchi et al. , 1998). in fact linearized approximations are non- observable, while tools from differential- geometric nonlinear system theory prove the possibility of reconstructing the position and orientation of the vehicle and the position of the obstacles in the environment. The localization problem can be embedded in the more general problem of current state estimation of a d:vnamical system using input-output measurements. For smooth nonlinear systems, a classical approach to design an observer is to apply the extended Kalman filter. see (J. Borenstein and Feng. 1996: Gelb, 19i4: Misawa and Hedrick , 1989). Another way of designing an observer is to transform the original non linear system into another one for which the design is known. Transfor-
A non linear observer and its practical implementation have been presented in Gauthier et a!. (J .P. Gauthier and Othman. 1992: Bonard and Hammouri. 1991) . in which the first step is writing the input affine nonlinear system in a
1 This work partially supported by ?\IURST (project RA:-'ISETE) and AS! (project TE~IA).
663
so-called normal observation form. However, this form requires that the trivial input is an universal input (Bonard et al. , 1995) for the system. In our problem, this condition is violated by the mobile robot kinematics.
a mobile robot (about an arbitrary initial state and zero control), it can be easily seen (Bicchi et al., 1998) that the linearized system is always unobservable if there are targets (N i= 0), and can only be observable if there are at least 3 markers (M ~ 3). It has been proven in (Bicchi et al., 1998), by using the codistribution associated to the observability space, that if, at least. two markers are available, nonlinear observability of the location of the vehicle and of targets follows. In this section, a novel nonlinear observer for mobile robot localization is presented, by considering the robot with N = 0 (no targets) and two markers AI = 2, i.e. x = g(x , u), where x = [.;- ( E R2 X 51 is the state vector, and
In this paper, a local nonlinear observer for mobile robot localization is designed, inspired by the work of Teel and Praly (Teel and Praly, 1994) . The basic idea is to use output derivatives, which are elements of the observability space associated to the nonlinear measurement process, in the assumption that a rank condition on the observability matrix is satisfied for the considered input function. The observer design is based on the concept of Extended Output Jacobian (EOJ) matrix, which is the collection of the covectors associated to the considered elements of the observability space, i.e. the output and its derivative. Then, the output derivatives are estimated by using high-pass filters. Local practical stability of the observation error dynamics is guaranteed since the introduced persistent perturbations can be made arbitrarily small. A singularity-avoidance exploration task is also addressed to deal with the singularity occurrences in the EOJ matrix.
OV
(1)
with output measurements: y with
= h,(x) = 11' + atan2(
y,
= h(x) = [Yl Y2V,
- (,,( - (,) - 0, i
= 1,2.
The following observer is introduced for the nonholonomic vehicle:
The paper is organized as follows. In sect. 2, a nonlinear observer for nonholonomic vehicle localization is designed. In sect. 3, conditions on the input vector which ensure singularity avoidance are also derived . Sect. 4 reports simulation results in both the cases of output measurements affected by bounded noise, and of singularity-avoidance exploration. Finally, in sect. 5 the major contribution of the paper is summarized.
ic. = g(x, u) + P
(J.l - {l)
(2)
where J.l = [yT yTf E R 4 , and {l(x) = [h(x)T h(x)TV, the design parameter P E R3x4 is chosen as P = (Q + *(x))J+, being Q a positive definite matrix, the Extended Output Jacobian (EOJ) matrix J is defined as:
J(x, u)
Notation - 1/.1/ denotes the Euclidean norm for a vector and an induced natural norm for a matrix . URn)" is the dual space of the Euclidean space Rn. Given a scalar function f : Rn -* R, df denotes the associated exact differential. Given a vector field f : Rn -* RP, df denotes the associated Jacobian matrix.
= d{l(x) = [!~]
E R4x3
(3)
If J is full-rank, J+ = (J T J) -1 JT is its left pseudo-inverse. We now give a preliminary motivation of the introduced observer, based on the Implicit Function Theorem. Rearrange the output equation and its derivative in the form:
F z x ( ,
2. NOl'\LINEAR OBSERVERS FOR LOCALIZATION
)
= (Yy-h(x) - ~(x)) = (.y-~~g(x,u) y -:- h(x) ) = (0) 0
where we have introduced the variable z = [I-' T u TV. Since ~~ = -J, if the matrix J is full-rank in a fixed point (ZO,XO), then, by using the Implicit Function Theorem, there exist two neighborhoods A of zO , BO of xO, and an unique map 1 : A O -* BO, with l(z) = x, such that F(z,l(z)) = O. Hence, the full-rank of matrix J implies the local existence of a static map which relates the state with the output, its time derivative, and the input . Since the analytical expression of the map l(z) can not, in general, be derived, in the nonlinear observer (2) we have chosen to consider directly the implicit condition F(z, x) = 0 and linearize this expression with respect to the actual
Consider a system comprised of a mobile vehicle, such as a robotic rover in a planetary exploration mission, which moves in an unknown environment with the aim of localizing itself and the environment features (in the rover case, e.g., rocks and geological formations). Among the features that the sensor head detects in the robot environment , we will distinguish between those belonging to objects with unknown positions (which we shall call targets), and those belonging to objects whose absolute position is known, which will be referred to as markers. From the linear approximation of
°
664
estimate X, as will be clear soon. The following proposition formally summarizes the properties of the proposed observer in the case of exact measurements of the output derivatives.
[io, T], for some T > to, i.e. the observability matrix for the particular input u( t), t E [to . T] is nonsingular in a neighborhood of XO (Xia and Gao , 1989: Zeitz. 1987).
Proposition 1. Assume exact measurements of the output derivative. and that the EOJ matrix is full rank, i.e. rank(J) = 3, "Ix. u of interest . Then the equilibrium e = x - x = 0 of the observation error dynamics constructed by using the observer in Eq. (2) is locally asymptotically stable.
Example 4. In the case of a holonomic vehicle described by the equation: x = u, x. u E R2. assume, for example, that it is possible to measure only the distance: y = ~(xi +x§). The EOJ matrix results: J(x , u)
Proof: The observation error has the following dynamics:
= [ Xl U1
(6)
(4)
the locus of singularities is given by the set S = {(x, u) E R2 X R2 : X l Uz - Xz U1 O}. The above analysis indicates that the holonomic vehicle is not uniformly observable. and suggests to choose the input u such that, at each time instant , the point (x , u) does not lie on the manifold S.
where we have linearized the vector field g(x, u) and the extended output vector J.l around the actual state estimate, i.e. g(x, u) - g(x , u) = ~(x) ( x - x) + Oi(llglll and J.l- fi = J(x, u)(xx) + O~(llell).
To prove the practical stability of the observer in Eq. (2) in case of estimated output derivatives, we first remind the following result on the total stability of nonlinear systems , see for example (Isidori, 1995) :
Consider the quadratic Lyapunov function candidate ~r ~eT e, its time derivative results:
Proposition 5. Consider the nonlinear system x = f(x ) + g(x)u(t) , where x ERn , uti) E Rm , and f(x) , g(x) are smooth vector fields . Assume that the origin of x = f (x) is a locally asymptotically stable equilibrium. Then 'It > 0, there exist 61 > 0 and K > 0 such that if Ilx(O)11 < 61 and Ilu(t) 1I < K, t ~ to ~ 0, the solution x(t, to, u) of x = f(x) + g(x)u(t) satisfies the condition : Ilx(t , to , u) 11 < c, t ~ to ~ o.
e = g(x, u)
- g(x, u) + P (J.l - fi)
= (~:(x))
=
e-PJ(x.u)e+PO~(llelll
- O~(llell)
=
\> = eT e
=eT [(
:!
(5 )
(x))e - PJ(x, u)e
- O~(IIell)l
+ PO~ (lIelll
= _eT Qe + PO~ (llel!) -
O~(llell)
Hence, the function ( r is negative definite in a 0 neighborhood of the origin e = x - x = O.
Now we state the following.
Remark 2. To design the matrix P. it has been used the pseudo-inverse of J, the study of the locus of singularities is then necessary to investigate system trajectories and particular inputs which can cause loss of rank in J. \Ve first remark that in the case of an uniformly observable SISO (Single Input Single Output) nonlinear system (Teel and Praly. 1994: Isidori, 1995), given the map [y "if ... y (n - JJjT = 1>(x. v) . being x E Rn the state vector, and v = [u it . . . u(n-2 J jT E Rn-1 the extended input vector, the matrix J, which is is related to the expression of the tangent map of 1>(x. v) . is nonsingular. for definition. for each (x. v) E Rn X Rn-I. Thus. uniformly observability is a sufficient condition to design the observer (2).
Proposition 6. Consider the non linear observer:
~ = g(x, u) :
1jJ
P
[~~~~]
+P
[~]
(7)
= T1 ( .1. + y. ) -
(1/
where T is a sufficiently small positive constant. Assume that the EOJ matrix is full rank. i.e. rank(J) 3, 'Ix. u of interest . Then the equilibrium e = x - x = 0 of the observation error dynamics constructed by using the observer in Eq. (7) is locally practically stable. i.e. 'Ice > 0 there exist 61 > 0, K > 0, and T which depends on K , such that if the initial observation error is sufficiently small. i.e Ile(O) 11 < 61 , then Ilc" (t)1I < K, t ~ O. being c" = lOT cTjT . { = Y - lp. and the observation error satisfies the condition Ile(t,O.{*)11 < Ce, t ~ O.
=
R emark S. Assume that the input function uti) , t ~ to ~ 0 of the vector field in Eq. (1) is real analytic. The matrix J is full-rank in a open neighborhood XO x R2, if the system is locally observable at every xO E XO in the interval
665
where , in general , given x ERn , sgn(x) = [sgn(xd , .. . , sgn(xn)V, being sgn(xi) the sign function, which values 1 if Xi > 0, -1 if Xi < 0, and sgn(O) = O.
Proof: The dynamics of the observation error e = x - x result~: e = -Qe - Pf" + PO~(jlell) - O~(jlell). where f" = [0 fV, f = Y - 1/; is the introduced error due to the output derivative estimation. The output derivatives error f can be reduced to an arbitrarily small perturbation if the constant T is sufficiently small. In fact , assumed 1/; (0) = 0, the Laplace transform of f results: f(S) = yts) -
Fixed an arbitrary input initial condition, a first technique to satisfy the above equation is to compute the input function through integration from Eqs. (8) or (10). In particular in the latter case, since the input derivative u is piecewise constant. the input u is simply piecewice linear. Since the input u can be also used to steer the mobile robot in a desired area. a second technique is to embed the singularity avoidance problem in a secondary mobile robot task to be accomplished.
I+;'S Y(s) + ~lo;l = T I +sTs Y(s) , where Y(s) denotes the output Laplace transform. and (with an abuse of notation) Y(s) = s Y(s) - y(O+) denotes the output derivative Laplace transform . Vt ~ 0, f(t) = Ty,(t) . where Yj(t) is the output derivative filtered by l+sTs' Since the high-pass filter l+sTs is Bounded Input Bounded Output (BIBO) , there exists My > 0 such that IIy,(t)II < _~fy. Vt ~ O. Hence \:j( > 0, by choosing T < then Idt)1 < T My < (, Vt ~ O. Let us consider the non linear system e = -Qe - Pf" + PO!( lI ell) - O~(jlell) . If f"(t) = 0, t ~ 0 the origin e = 0 (of the unperturbed system) is locally asymptotically stable. Prop . .'.i indicates that Vfe > 0 there exist 15 1 > 0 and K > 0 such that if the initial observation error is sufficiently small, i.e lIe(O)1I < 15 1 and choosing { = K , since there exists T such that IIf"1I = IIfll < ( = K. then the observation error satisfies the condition lIe(t,O,f")1I < fe, t ~ 0.
Let us define the matrix : X = PJ(x,u) E R 3x3 , where the matrices P and J(x , u) have been introduced in the observer equation (2). In the Prop. 1, fixed X = Q + ~(x), it has been chosen P = X (J T J)-l JT to solve the equation X = PJ, assuming J is full-rank . Let us denote with Pi E (R4)* , i = 1, . .. 3 the rows of the matrix P, it is well-known that this matrix solves the problem:
At,
{
L2
(11)
= PJ(x. u)
Instead of minimizing the norm of each row , we can choose to minimize the norm of the difference between P i and a suitable Pi , to be chosen later. We consider the constrained minimization problem:
3. SINGULARITY AVOIDANCE
{ mJn
Consider the singularity index associated to the EOJ matrix defined as log w(x , u) , where the introduced singularity function is w(x , u) = det (JT J) . The quality of the current state estimation depends on the actual value of w(x, u) . A possible singularity-avoidance strategy to guarantee a well-conditioned EOJ matrix is to impose the following input derivative expression
X
t. ~(P,
- Pi) (P, - p;lT
= PJ(x , u)
(12)
.
By introducing the Lagrangian multipliers Ai E = 1, . .. ,3, the problem is solved by considering the function : L(P , A) = L~=la(Pi Pi) (Pi - pj)T + (X i - Pi J) Ai], where Xi E (R 3 )" , i = 1, . . . , 3, are the rows of the matrix X and A = [AI A2 A3J E R 3x3 is the matrix of Lagrangian multipliers. Simple computations show that the conditions:
J
R3, i
where &tL'~:'U ) E OfF)" . If this is the case. the time derivative of the singularity function w(x , u) results: . 'I( ~)T &w (x u ) 112 x+k" 1
p
X
o
w (x,u ) = ( all' &x (x, u))
3 1 min ~ -P,P;
{
8L(P,A) = 0 8pT 8L(P:A) --::-::---=0 8A;
(13)
i
= 1. ... 3
lead to the solution, if J is full-rank. expressed in the matrix form: P = XJ + +P" (I -J J+), where the matrix P" E R3 x 4 is the collection of the covectors Pi. It is simple to show that Props. 1. and 6 also hold if the matrix P is chosen in the observer equation (2) as P = X J+ + P" (I - J J+) with X = Q + ~(x). since P" (I - J J+) J = O. VP" E R3 x4
(9)
which is certainly positi\'e if k" is chosen sufficiently large. ~otice that another way to obtain an increasing singularity function is to choose
666
The matrix P' can be chosen to obtain a well conditioned EOJ. In fact. let us assume that the input is a function of the actual state estimate. i.e. u = nix) . where Q : R3 -+ R2 is. at least. of class Cl. Given the singularity function lL·(X. u ) = j det (JT J). the condition (8) (a similar condition is obtained by imposing (10))
·:.lI ---.,....------,;;---~
~'ields:
8a l x ) ,
'
)
~ l g(X.U , +
p_ (
8w(x , u) T
, ,'
1l-Il Jl =k- ( ~)
a) (1" )
where Eq. (2) is used. The abO\'e equation is linear in P' and imposes two scalar conditions. Hence. a matrix p', such that Eq. (8) (or (10)) is satisfied during the vehicle motion. can be computed from (14) by least-square estimation.
'r
f'
..~-------.--------~~------~
b)
4. SnIULATIO!\ RESULTS To investigate the performance of the observer (7). the nonholonomic sy'stem in Eq.(I) has been simulated in Matlab environment. In the reported trials, the markers have the following coordinates: (PI (I) = (0.0) , and (P2 (2) = (0, 1), the control parameters are: Q = 2. and T = 0.001. ~loreov e r. we assumed the presence of output measurements by , modeled as band-limited white noise.
" r " r
'. c)
To esemplify the singularity-avoidance strategy (see Sect. 3), we report two trials in which the input has been chosen as u = :Ex , where the matrix :E E R 2 x3 is constant and values:
o 1
d)
~]
in particular the input vector does not depend on the estimated angle X3· The matrix P' is computed. at each iteration. from Eq. (14) by leastsquare estimation. such that Eq. (10) is satisfied with k* = 1. In the first simulation the initial conditions of the real and estimated state are respectivel~' x = [1. 1. 'rlT. and x = [1.5. 0.5. 38,Y. The control parameters are: Q = 2, and T 0.001. The results (shown in fig. 1) indicates that the singularity index remains greater than 10- 8 . which implies a satisfactory quality of the current state estimate. In the second trial. reported in fig. 2. the previous simulation has been repeated in the presence of output measurements noise. In particular. we assumed that by is band-limited \I'hite noise with po\\'er 10- 5 . The results indicate that the observation error is bounded . while the EOJ matrix is well conditioned.
e)
Fig. 1. ~onholonomic vehicle (input avoiding singularities): state estimation without output noise . a) real and estimated Xl component. b) real and estimated X2 component. and c) real and estimated X3 component of the state vector. d) Singularity index of the EOJ matrix: log w(x. u): and e ) vehicle motion in the ~ - ( plane.
=
output deri\'atil'es I'ia linear high-pass filters. and the concept of EOJ matrix. Future iIl\'estigations \\'ill concern the extension of the proposed approach to more general non linear systems and the problem of set-point regulation by using the current state estimate.
A nonlinear obserwr has been proposed for nonholonomic mobile robots . which uses estimated
667
systems. In: Proc. 30th IEEE Conference on Decision and Control. Bonard, G., F . Celle-Couenne and G. Gilles (1995). Nonlinear Systems. Vol. 1. Chap. Observability and observers. Chapman & Hall. F. Celle, J .P. Gauthier and D. Kazakos (198G). Orthogonal representations of nonlinear systems and input-output maps. Systems fj Control Letters (7), 3G5-372. Fliess, M. and I. Kupka (1983) . A finiteness criterion for non linear input-output differential systems. SIAM Journal Control and Optimization (21), 712-729. Gelb, A. (1974). Applied optimal estimation. M.I.T. Press . Cambridge, MA. Hammouri. H. and J.P. Gauthier (1988). Bilinearization up to output injection. Systems fj Control Letters (11), 139-149 . Isidori, A. (1995). Nonlinear Control Systems. 3rd ed .. Springer Verlag. J. Borenstein, A. Everett and L. Feng (1996). Navigating mobile robots. AK Peters. J.P. Gauthier, H. Hammouri and S. Othman (1992). A simple observer for nonlinear systems . application to bioreactors. IEEE Trans. A utomatic Control. Krener, A.J. and A. Isidori (1983). Linearization by output injection and non linear observers. Systems fj Control Letters (3), 47-52. Krener, A.J. and W. Respondek (1985). Nonlinear observers with linear error dynamics. SIAM Journal of Control and Optimization (23), 197-216. Levine, J. and R. Marino (1986). Nonlinear system immersion, observers and finite dimensional filters. Systems fj Control Letters (7), 137-142. Misawa, E.A. and J.K. Hedrick (1989). Nonlinear observers - a state-of-the-art survey. Trans. ASME 111 , 344-352. Teel, A.R. and L. Praly (1994). Global stabilizability and observability imply semi-global stabilizability by output feedback. Systems fj Control Letters (22), 313-325. Xia, Xiao-Hua and Wei-Bin Gao (1989) . :\'onlinear observer design by observer error linearization. SIAM Journal on Control and Op· timization (27), 199-216. Zeitz , M. (1987). The extended luenberger observer for non linear systems. Systems 8 Control Letters (9), 149- 156.
a)
.., ::t
. ..
b)
'·'1-------=~~~-'~-~-.~-~'.>~-..~ ..•~'-="-------, >of
::r c)
d)
..=-«=•.
'~ I ------~-~-~~='-=.-.~"~=-~
. I
-~------
~
,
.
~
e)
Fig, 2. r\onholonomic vehicle (input avoiding singularities): state estimation in presence of output noise. a) real and estimated Xl component. b) real and estimated Xz component, and c) real and estimated X3 component of the state vector. d) Singularity index of the EOJ matrix: logw(x. u): and e) vehicle motion in the'; - ( plane. 6. REFERENCES
Bicchi. A.. D. Prattichizzo. A. )'Iarigo and .\. Balestrino (1998). On the obsen'ability of mobile \"ehicles localization. In: Proc. IEEE Mediterranean Conf. On Control And Systems. Bonard. G. and H. Hammouri (1991). A high gain observer for a class of uniformly observable
668