Kinodynamic Motion Planning: A Novel Type Of Nonlinear, Passive Damping Forces (NADFs) Ahmad A. Masoud Electrical Engineering Department, KFUPM, P.O. Box 287, Dhaharan 31261, Saudi Arabia, masoud@kfupm.edu.sa Abstract-This paper extends the capabilities of the harmonic potential field approach to planning to cover both the kinematic and dynamic aspects of a robot’s motion. The suggested approach converts the gradient guidance field from a harmonic potential to a control signal by augmenting it with a novel type of dampening forces suggested in this paper called: nonlinear, anisotropic, dampening forces (NADFs). The combination of the two provides a signal that can both guide a robot and effectively manage its dynamics. The kinodynamic planning signal inherits, fully, the guidance capabilities of the harmonic gradient field. It can also be easily configured to efficiently suppress the inertia-induced transients in the robot’s trajectory without compromising the speed of operation. The approach works with dissipative systems as well as systems acted on by external forces without needing full knowledge of the system’s dynamics. Theoretical developments and simulation results are provided in the paper. I. Introduction The harmonic potential field approach to planning is emerging as a powerful paradigm for the guidance of autonomous agents. Since it was suggested in the mid-late eighties [1,2] the approach is continuously developing to meet the stringent requirements operation in a real-life environment imposed on an agent. Up-till-now, the approach has amassed many attractive properties crucial for enhancing goal reachability. The approach is provably-correct driving the agent to a successful conclusion if the task is manageable and providing an indication if the task is intractable. It can be used to guide the motion of an arbitrarily shaped agent in an unknown environment regardless of its geometry or even topology relying only on the sensory data acquired online by the agent’s finite range sensors. The method can also impose a variety of constraints on the agent’s trajectory such as regional avoidance and directional constraints [3-8]. Harmonic functions are also Morse functions and a general form of the navigation functions suggested in [13], see appendix. A planner may be defined as an intelligent, purposive, context- sensitive controller that can instruct an agent on how to deploy its motion actuators (i.e generate a control signal) so that a target state may be reached in a constrained manner. Traditionally, a planning task is distributed on two stages: a high level control (HLC) stage and a low level control stage (LLC), figure-1. The HLC stage receives data about the environment, the target of the agent, and constraints on its behavior. It then simultaneously processes these data to generate a reference plan or trajectory marking the desired behavior of the robot. This trajectory, if actualized, leads to the agent reaching its target in the specified manner. The reference trajectory is then fed to an LLC in order to convert it into a sequence of action instructions to be executed by the agent’s actuators of motion. Unfortunately, the HLC-LLC paradigm for planning suffers from serious problems that adversely impact its performance in a realistic setting. An alternative may be achieved by fusing the HLC and LLC modules into one called the navigation control (NC). An NC attempts to directly convert the environmental data, goal of the robot, and constraints on its behavior into a control signal (figure-1). Khatib potential field (PF) approach may be considered as one of the first methods to cast planning in an NC framework [9]. The PF approach enjoys several attractive features; most significant is the high speed by which a robot can respond to the contents of its environment. Figure-1: The HLC-LLC and NC control structures. The attractor-repeller setting Khatib used to generate the potential field has some problems. The most serious one has to do with convergence where it was observed that a robot guided by such a method may stop somewhere in the workspace before reaching its target; the problem was termed the local minima problem. Many methods later appeared to generate potential fields that do not suffer from this problem [10-12]. Koditschek diffeomorphism approach [13] was among the first methods suggested to remedy this shortcoming in the PF approach. To convert the gradient guidance field from the potential surface (- LV) into a control signal (u), a viscous dampening force that is linearly proportional to speed is added: (1) u B x V(x) = − ⋅−∇ • This combination will only work provided that the initial speed of the robot is lower than an upper bound S(x): (2) ω(x) S(x) x ≤ ∈Ω where T(x) is the initial speed of the robot at a location x, and S is the workspace of the robot [14]. Practical application of the above faced two difficulties: first, no method was provided to compute the upper bound S. Even if a method is devised for doing so, there is no guarantee that in a practical situation the initial speed of a robot can be made to lie below the admissible upper bound. The second difficulty has to do with the fact that the satisfaction of the upper speed constraint guarantees only that obstacle avoidance constraints will be upheld and convergence to the target will be achieved. In potential field methods, transients can be a serious concern that could make it impractical to use these techniques for controlling a robot. Also, the approach seems to only deal with dissipative systems where no mention of how the method may be applied when external forces such as gravity are present. In its current form the HPF approach can only operate in an HLC mode providing only a guidance signal from the gradient of the potential. This signal has to be converted into a control signal by an LLC. Guldner and Utkin suggested an interesting approach based on a sliding mode control for converting the gradient field from an HPF into a control signal [21]. The approach is robust, has good convergence properties, does not require full knowledge of system dynamics and can make, with little transients, the dynamic trajectory of the robot follow the kinematic trajectory marked by the gradient field. The main drawback fo the approach seems to be the high shattering the control signal experiences. In this paper a method is suggested to utilize the HPF approach in an NC mode. This is accomplished by augmenting the gradient guidance field from an HPF with a new type of dampening force called: nonlinear anisotropic dampening forces (NADFs). It is shown that an NADF-based control can efficiently suppress inertia-induced artifacts in the dynamical trajectory of the system making it closely follow the kinematic trajectory while maintaining an agile system response. The approach does not require the system dynamics to be fully known. A loose upper bound is sufficient for constructing a well-behaved control signal that can deal with dissipative systems as well as systems being influenced by external forces (e.g. gravity). This paper is organized as follows: section II provides a brief background of the potential field approach. The NADF approach is presented in section III. Sections IV and V discuss the application of the approach to dissipative systems and systems experiencing external forces respectively. Simulation results are in section VI, and conclusions are placed in section VII. II. Background The HPF approach appeared shortly after the work of Khatib. Although the approach was brought to the forefront of motion planning independently and simultaneously by different researchers [16-20], the first work to be published on the subject was that by Sato in 1986 [1]. The HPF approach eliminates the local minima problem encountered in [9] by forcing the differential properties of the potential field to satisfy the Laplace equation inside the workspace of the robot (S) while constraining the properties of the potential at the boundary of S ('=MS). The boundary set ' includes both the boundaries of the forbidden zones (O) and the target point (xT). A basic setting of the HPF approach is: x0S ∇ ≡ 2V(x) 0 subject to: . (3) V 0| &V 1| X X X T = = = ∈Γ The trajectory to the target (x(t)) is generated using the HPF- based, gradient dynamical system: (4) x V(x) x(0) x0 • = −∇ = ∈Ω The trajectory is guaranteed to: 1- 2- (5) lim x(t) x t T →∞ → x(t) ∈ ∀ Ω t whereby a proof of (5) may be found in [3]. Figure-2 shows the negative gradient field of a harmonic potential for the simple environment of a room with two dividers. Figure-3 shows the trajectory, x(t), generated using the gradient dynamical system in (4). It ought to be mentioned that the HPF approach is only a special case of a broader class of planners called PDE-ODE motion planners [5] where the field is generated using the boundary value problem (BVP): solve: L(V(x)) / 0 x 0 S subject to: P(V(x)) = 0 x 0 '. (6) The trajectory is generated using the nonlinear system: (7) x V x x 0 x 0 • = = ∈ F( ( )) ( ) Ω where L is scalar partial differential operator, P is a governing relation restricting the potential or some of its properties at the boundary to a certain value, F is a nonlinear vector function mapping R6RN, N is the dimension of x, PDE stands for partial differential equation, and ODE stands for ordinary differential equation. Planners assuming a PDE-ODE setting other than that of the one in (3) may be found in [3,7,8]. Figure-2: Guidance field of an HPF. Figure-3: Trajectory generated by the field in figure-2. The trajectory, x(t), generated by the dynamical system in (4) is only a reference trajectory that should be fed to an LLC in order to generate the control signal, u, the robot is using. One way of converting the guidance signal into a control signal is to augment the gradient field with a component that is proportional to speed. This seemingly straight forward solution is problematic. In figure-4, the negative gradient of the potential in figure-2 is used to navigate a 1kg point mass. The dynamic equation of the system is: (8) x y B x y •• •• • • ⎡ ⎣ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ = − ⋅ ⎡ ⎣ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ −⎡ ⎣⎢ ⎤ ⎦⎥ ∂ ∂ ∂ ∂ v(x, y) / x v(x, y) / y where B=0.1. Despite the fact that the initial speed of the robot is zero, the trajectory violated the avoidance condition and collided with the walls of the room. Figure-4: trajectory of a point mass controlled by the field in figure-2. III. The NADF Approach An intuitive solution to the problem of converting a gradient guidance field into a navigation control signal is to increase the coefficient of the linear velocity term to a sufficiently high level. The linear velocity component acts as a dampener of motion that may be used to place the inertial force under control by marginalizing its disruptive influence on the trajectory of the robot that the gradient field is attempting to generate. The following example demonstrates that this solution is impractical. In order to generate a control signal that would satisfy the avoidance constraints (5), the coefficient of dampening of the system is increased to B=0.15. Figure-5 shows the resulting trajectory and figure-6 shows the distance to the target as a function of time. Although the trajectory did converge to the target point (xT) and did not violate the regional avoidance constraints, unacceptable transients along with significant deviations from the path marked by the gradient field (figure-2) are present. In a second attempt to generate a well-behaved control signal, the dampening coefficient is significantly increased to B=.7. Although a well-behaved trajectory was obtained (figure-7), significant slowdown of motion did occur (figure-8). The method for converting the gradient field from a harmonic potential into a navigation control signal by simple augmentation with a linear velocity dampening term is incorrect. This approach ignores the dual role the gradient field plays as a control and guidance provider. The field guides a robot to the target using vectors that point out the directions along which the robot has to move if the target is to be reached and the obstacles are to be avoided. At the same time, these vectors are forces that act on the mass of the robot in order to actuate motion. Obviously the inertia of the robot will have a disruptive influence on motion. The linear dampening term manages the inertial forces in an attempt to make the motion yield to the guidance provided by the gradient field. A dampening component that is proportional to velocity exercises omni-directional attenuation of motion regardless of the direction along which it is heading. This means that the useful component of motion marked by the direction along which the goal component of the gradient of the artificial potential is pointing is treated in the same manner as the unwanted inertia-induced, noise component of the trajectory. These two components should not be treated equally. Attenuation should be restricted to the inertia-caused disruptive Figure-5: Trajectory, point mass, linear dampening increased. Figure-6: Distance to target versus time. Figure-7: Trajectory, high linear dampening. Figure-8: Distance to target versus time. component of motion, while the component in conformity with the guidance of the artificial potential should be left unaffected (figure-9). To better manage the effect of the inertial forces, a more carefully constructed dampening component that treats the gradient of the artificial potential both as an actuator of dynamics and as a guiding signal is needed. A dampening force (M) that behaves in the above manner is: (9) M(x, x) [(n ) n ( V(x) V(x) ( V(x) )) V(x) V(x) ] t T T T & = + ∇ ∇ ⋅⋅ ∇ ∇ ∇ • • • x x x Φ where n is a unit vector orthogonal to LV. This force is given the name: nonlinear, anisotropic, dampening force (NADF). For the two dimensional case, an NADF has the form: (10) M g g g y g x g g g x g y g x g y g g x 2 y 2 x y y x x y x y x y = + − ⋅− ⎡ ⎣⎢ ⎤ ⎦⎥+ ⎡ ⎣ ⎢ + ⋅ − − ⎡ ⎣ ⎢ ⎤ ⎦ ⎥ ⎤ ⎦ ⎥ ⎥ • • • • • • 1 ( ) ( ) ( ) Φ where . ∇ = V x y g g x y T ( , ) [ ] a- action of the dampening force D x x 1 −( , &) 1 S 1 S C x x ( , &) &x x b- block diagram Figure-9: nonlinear, anisotropic, dampening force (NADF). IV- Dissipative Systems In this section two propositions are stated and proven. The first proposition shows that a gradient field of a harmonic potential generated by the BVP in (3) combined with NADF can guarantee global, asymptotic convergence of a fully actuated second order dissipative dynamical system. The second proposition shows that the dynamic trajectory of the system can be made arbitrarily close to the kinematic trajectory generated by the system in (4); hence, preserving the spatial constraints. Proposition-1: Let V(x) be a harmonic potential generated using the BVP in (3). The trajectory of the dynamical system: (11) D x x C x x x B M x x K V x 0 ( )&& ( , & )& ( , &) ( ) + + ⋅ + ⋅∇ = d will globally, asymptotically converge to: (12) lim & t→∞ ⎡ ⎣⎢ ⎤ ⎦⎥→⎡ ⎣⎢ ⎤ ⎦⎥ x x x 0 T for any positive constants Bd and K, where x0RN, V(x):RN6R, D(x) is an N×N positive definite inertia matrix, contains C(x, x)x & & the centripetal, Coriolis, and gyroscopic forces. Proof of the above proposition is carried out using the LaSalle invariance principle [23]. Proof: Let = be the Liapunov function candidate: (13) Ξ(x, x) K V(x) 1 2 x D(x)x T & & & = ⋅ + Note that since V(x) is harmonic, it must assume its maxima on ' and minima on xT . In other words, V(x) can only be zero at xT ; otherwise, its value is greater than zero: . (14) Ξ(x, x) 0 iff x 0, x 0 positive otherwise & & = = = ⎡ ⎣⎢ The time derivative of the above function is: . (15) & & & & & & & && Ξ(x, x) K V(x) x 1 2 x D(x)x x D(x)x T T T = ⋅∇ + + Substituting: (16) && & & & x D (x)[ C(x, x)x Bd M(x, x) K V(x)] 1 = − − ⋅ − ⋅∇ − along with (9) in the above equation yields: (17) & & & & & & & & & & & & & & Ξ Φ = ⋅∇ + − ⋅∇ − − ⋅ − ⋅ ∇ ∇ ⋅ ⋅ −∇ ∇ ∇ K V(x) x 1 2 x D(x)x K V(x) x x C(x, x)x B x (n x)n B x ( V(x) V(x) x ( V(x) x) V(x) V(x) T T T T d T T d T T T Using the passivity property: (18) & & & & x (D(x) 2 C(x, x))x 0 T − ⋅ = and rearranging the terms we get: (19) & & & & & & Ξ Φ = − ⋅ − ⋅∇ ⋅ ∇ ⋅∇ ⋅ ∇ ⋅ ∇ B (n x) (n x) B ( V(x) x) V(x) ( V(x) x) V(x) ( V(x) x) d T T T d T T T T as can be seen , (20) & & Ξ ≤ ∀ 0 x, x where & & Ξ = = 0 for x, x 0 according to LaSalle principle any bounded solution of (11) will converge to the minimum invariant set: . (21) E {x 0, x} ⊂ = & Determining E requires studying the critical points of V(x) where LV(x)=0. According to the maximum principle, xT is the only minimum (stable equilibrium point) V(x) can have. Besides xT , V(x) has other critical points {xi} at which LV=0; however, the hessian at these points is non-singular, i.e. V(x) is Morse [24]. A proof of this result may be found in the appendix. From the above we conclude that E contains only one point which is the point to which motion will converge. x x , x 0 T = = & Proposition-2: Let D be the trajectory constructed as the spatial projection of the solution, x(t), of the first order differential system in (4). Also Let Dd be the trajectory constructed as the spatial projection of the solution, x(t), of the second order system in (11), figure-10. Then there exist a Bd that can make the maximum deviation between D and Dd (*m) arbitrarily small. ∇ ∇ V x V x ( ) ( ) Figure-10: The kinematic and dynamic trajectories. Proof: The gradient field from an HPF does not only work as a guide of motion to the target; it also may be used to cover S with a complete set of boundary-fitted basis coordinates (figure- 11). Figure-11: Boundary-fitted coordinate system. The radial basis of the system (LV/*LV*) marks the useful component of motion. The basis orthogonal to this component span the disruptive component of motion (*) which NADF is required to attenuate (figure-12). Figure-12: The disruptive component of motion. The dynamic equation describing the disruptive component is: (25) n D(x)x n C(x, x)x Bd n M(x, x) K n V(x) 0 T T T T && & & & + + ⋅ + ⋅ ∇ = Examining the above equation term by term yields: 1- , (26) n V 0 T∇ = 2- = n x T[(n x) n ( V(x) V(x) ( V(x) x)) V(x) V(x) ] t T T T • • • + ∇ ∇ ⋅⋅ ∇ ∇ ∇ Φ (n t &) x 3- assuming that an upper bound can be placed on the speed: (27) &x max ≤ν the norm of the matrix C may be bound as: (28) C(x, x) cmax & ≤ 4- any inertia matrix belonging to a physical system is positive definite, invertible, and have a bounded norm: (29) D(x) d max ≤ where dmax, cmax, and (t)) that acts normal to LV, the impulse response (h(t)) of (30) is obtained: . (31) h(t) 1 1 h (t) ` = − = −⋅ ∆ Φ ∆ ∆ ( ) ( ) e t t The deviation as a function of time may be computed as: δ ξ ( ) ( ) t t = ∗h(t) where * denotes the convolution operation. Since it was shown in proposition-1 that motion will converge to xT and all dynamic terms will tend to zero, >(t) may be bounded as: , (32) ξ(t)dt I 0 ≤ ∞∫ therefore: , δ ξ ( ) ( ) t 1 t = ≤ ∆ ∆ h (t)* I ` max where I and Imax are positive constants. By properly selecting a value for ), the maximum deviation *m can be made arbitrarily small. In other words the dynamic trajectory of (11) will closely follow the kinematic trajectory of (4) and the spatial constraints will be preserved. It ought to be mentioned that since NADF is by design made to be zero when motion is in accordance with the guidance field LV, Bd can be made arbitrarily large without slowing down the system. This fact is clearly reflected by the simulation results (figure-24). V. Systems with External Forces The NADF approach may be adapted for designing constrained motion controller for mechanical systems experiencing external forces (e.g. gravity). The dynamical equation of such systems has the form: (33) D(x)x C(x, x)x G(x) F && & & + + = where G(x) and F are vectors containing the external forces and the applied control forces respectively. A controller consisting of the gradient guidance field and a strong enough NADF (34) has the ability to make the trajectory of the system in (33) closely follow the kinematic trajectory from an initial starting point (xo) to the target point xT, . (34) F B M(x, x) K V(x) d = − ⋅ − ⋅∇ & However, due to the presence of the external forces the controller will not be able to hold the state close to the target point and drift will occur (Figure-26). Arimoto and Miyazaki showed that steady state error caused by the external forces may be cancelled by using an integral control action [27]. Unfortunately, an integral action raises the order of the mechanical system and could cause it to become unstable if it is not tuned properly. The integrator also induces difficult to manage transients in the response of the system. Here an alternative approach to using an integrator is suggested. The suggested approach does not endanger stability and can cancel the error caused by the external forces bringing the dynamic trajectory arbitrarily close to the target point. The approach capitulates on the ability of the controller in (34) to drive motion arbitrarily close to the target point. Once the trajectory is close to the target, a passive clamping control action is activated to trap the trajectory in a set close to the target. After motion is trapped by the clamping control, an iterative procedure is suggested for totally cancelling the error. In the following the suggested clamping control is described 1. Clamping control: The effect of the clamping control (Fc) is strictly localized to a hyper space of radius F surrounding the target point. If motion is heading towards the target, this control component is inactive. On the other hand, if motion starts heading away from the target, the control becomes active and attempts to drive the trajectory back to the target (Figure-13). &X &X & ( ) ( ) X X X 0 Force K X X T T C T − ≤ = − &X (X X) 0 0 T T − > = Force K X X C T ( ) − Figure-13: The clamping control. A form of a clamping control that behaves in the above manner is: (35) F (x,x) (x x ) ( x x ) (x (x x )) C T T T T & & = − ⋅ − − ⋅ − Φ Φ σ The strength of Fc is adjusted by multiplying it with a constant Kc so that the steady state error is kept below a desired level (,). Unlike the integrator, the use of a clamping control will keep the mechanical system stable for any positive value of Kc. Proposition-3: For the mechanical system in (33), a controller of the form: (36) F B M(x, x) K V(x) K F (x, x) d C C = − ⋅ − ⋅∇ − ⋅ & & can make lim x(t) x t T →∞ − ≤ < ε σ and (37) lim x 0 t→∞ = & provided that: 1- K, Bd, and Kc are all positive, 2- Kc $ Fmax/,, x0SF F max G(x) max X = and . (38) Ωσ σ = − ≤ {x: x x } T 3- a high enough value of Bd is selected so that at some instant in time t` (39) x(t`) xT − < σ 4- K is high enough so that the gradient field is capable of directing the trajectory to SF X0S-SF (40) K V(X) G (X) V(X) V(X) T ⋅∇ > ∇ ∇ Proof: Consider a Liapunov function candidate similar to the one in (13) with a gravitational potential energy term (P(X)) added: (41) Ξ(x, x) K V(x) 1 2 x D(x)x P(x) T & & & = ⋅ + + note that: and . (42) G(x) P(x) = −∇ P(x) G(z) dz x x 0 = ⋅ ∫ Differentiating (42) with respect to time we get: (43) & & & & & & & && & Ξ(x, x) K V(x) x 1 2 x D(x)x x D(x)x x G(x) T T T T = ⋅∇ + + + solving for from equations (33, 34) and substituting the &&x results in (43) we get: (44) & & & & & & & & Ξ Φ Φ = − ⋅ − ⋅∇ ⋅ ∇ ⋅∇ ⋅ ∇ ⋅ ∇ − ⋅ − ⋅ − ⋅ − − B (n x) (n x) B ( V(x) x) V(x) ( V(x) x) V(x) ( V(x) x) K x (x x ) (x (x x )) F( x x ) d T T T d T T T T C T T T T T σ Since Kc and Bd are positive we have: , (45) & & Ξ ≤ ∀ 0 x, x where . & & Ξ = = 0 for x, x 0 Since we are assuming that K and Bd are selected high enough so that the dynamic trajectory will follow the kinematic trajectory and enter SF , the minimum invariant set to which the trajectory is going to converge may be computed from the equation: (46) G(x) K V(x) K F (x, x 0) 0 C C + ⋅∇ + ⋅ = = & Since M(0)=1, and x0SF (i.e. M(F-*x-xT*)=1), equation (46) becomes: (47) G(x) K V(x) K (x x ) 0 C T + ⋅∇ + ⋅ − = As can be seen if condition 2 on KC is satisfied, the solution of the above equation has to lie in the set S, ={x:*x-xT*<,}. This means that the deviation of the end of the dynamic trajectory from the target point should at most be ,. Another alternative to the use of integration is to reduce steady state by increasing the gain of the gradient field (K) to a sufficiently high level. This approach makes the transient difficult to manage and increases the control effort. On the other hand, selecting a high gain of the clamping control (KC) to manage the steady state error will not cause the above problems. This is due to the fact that this control component is designed to be minimally intrusive affecting the system only when it is needed. This is clearly demonstrated by simulation (Figure- 27,28,29) 2. Iterative, blind error cancellation: While clamping control has the ability to reduce the steady state error to an arbitrarily small value, sometimes it is desired that this error be totally cancelled. Here, an iterative, blind procedure is suggested for error cancellation. The procedure works by providing an alternative path ($) other than the error channel (KPAe, where KP is a positive definite matrix) to supply the control signal (u) that is needed to hold the robot at a location xT (figure-14), u = kAe + $ (48) Figure-14: The suggested scheme for iterative error cancellation. The fixed point iteration method [28] is used to evolve an estimate of the control signal so that the steady state error is driven to zero. This procedure is implemented using a switched logic circuit with one memory storage element. One implementation requires the circuit to have two inputs: the control that is directly fed to the robot and velocity of the robot’s coordinates in order to assess convergence (other means to decide if the robot has converged may be used). There is only one output consisting of the bias term $. The bias term is iterativly determined as follows: when motion is about to settle (i.e. *dx/dt*< ", where 0 < " <<1), the circuit measures the value of u and assigns it to <. This value is kept till at another instant i the event becomes true again. At the i’th instant we have: u=G( xi), $=G(xi-1), and KPAe = KPA(xT-xi) (49) where xi is the position of the robot at the i’th settling instant. Relating the above quantities using (48) yields the recursive relation: G( xi) = G(xi-1) + KPA(xT-xi) . (50) Proposition-4: The recursive relation in (50) has a fixed point at which: (xT-xi) = 0 (51) Proof: Using Taylor series expansion around xT, we have: G(x) = G(xT) + J(G(xT))(x-xT)+ .... (52) = G(xT) + F(x-xT) where J is the Jacobian matrix of G and F is a function containing the (x-xT) terms of the Taylor series. Substituting (52) into (50) we get: F(e`i) = F(e`i-1) - KPAe`i (53) where e`i = - (xT - xi) . (54) Now let 0=F(e`) and Q be the inverse function of F in the neighborhood of xT. Substituting Q in (53), we obtain the recursive relation: KPAQ(0i) + 0i = 0i-1 . (55) At a fixed point we have : 0i = 0i-1 (56) or KPAQ(0i) = 0. Since KP is positive definite, i.e. it is not singular: Q(0i) = e`i = (xi - xT) = 0 (57) In other words: xi = xT . Proposition-5: For any positive definite KP, the fixed point x=xT is a stable attractor fixed point, i.e. if xi is sufficiently close to xT, (58) lim i→∞ → x x i T Proof: In the close neighborhood of xT, equation (50) may be written as: J(G(xT))A(xi-xT) =J(G(xT))A(xi-1-xT) +KPA(xT-xi) (59) Notice that: J(G(xT)) = J(LP(xT)) = H(xT) (60) where H is the symmetric hessian matrix. Substituting (60) in (59) yields the equation: [KP + H(xT)]Aei = H(xT)Aei-1 (61) where ei = (xT-xi) . Since KP is positive definite and H is symmetric, they are simultaneously diagonalizable into: K=UUT and H=U7UT (62) where U is a nonsingular matrix and 7 is a diagonal matrix with non-negative elements 8l, l=1,..,N, see [29, page-86].Using the above decomposition (61) may be written as: U(I+7)UTAei = U7UTAei-1 (63) Using the transformation qi = UTAei , we have qi = AAqi-1 (64) where . (65) A I 1 0 0 0 1 0 0 0 1 1 1 1 2 2 N N = + = + ⋅ + ⋅ ⋅ ⋅ ⋅ ⋅ ⋅ + ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ ⎥ − ( ) Λ Λ λ λ λ λ λ λ It is well-known that the solution of (64) is: qi = AiAq0 (66) Since l=1,..,N (67) 0 1 1 ≤ + < λ λ l l we have: . (68) lim lim i i →∞ →∞ = ⋅ → q U e 0 i T i Since U is a nonsingular matrix (69) lim i→∞ → e 0 i or (70) lim i i →∞ → x x T VI. Results In this section several simulation examples are presented to demonstrate the capabilities and versatility of the NADF approach. 1. Point mass in a cluttered environment: The gradient field in figure-2 is augmented with NADF instead of the linear, viscous, dampening forces. The combination of both gradient field and NADF is used to steer a 1Kg mass from a start point to a target point. An excessively high dampening coefficient, Bd=10, is used. The trajectory of the mass is shown in figure-15, and the mass distance to the target, D(t), as a function of time is shown in figure-16. As can be seen, the kinodynamic trajectory of the mass is almost identical to that marked by the gradient field (kinematics only) in figure-3. Moreover, motion of the mass is almost six times faster than its viscous dampening counterpart shown in figure-7 with a settling time (TS) of about 12 seconds compared to 72 seconds. Figure- 17 shows the corresponding radial speed along the mass trajectory and figure-18 shows the control signal (X-Y force components). Figure-15: Trajectory, NADF, Bd=10. Figure-16: Distance to target versus time, NADF. Figure-17: Radial speed versus time, NADF. Figure-18: x and y control force components , NADF. 2. Point mass with upper speed limit: As can be seen from figure-17, the radial speed of the mass continuously increases prior to reaching the target. This is a result of NADF not having any friction forces along the radial component of the trajectory. While a fast response is required, measurers to prevent motion from racing out of control are desirable. Placing an upper limit on speed may be done using Figure-19: Trajectory - speed limit imposed. Figure-20: Distance to target versus time - speed limit imposed. Figure-21: Radial speed of the trajectory - speed limits imposed. Figure-22: x and y control force components for the point mass with constraints on speed. the approach in [4]. In figure-19 a 5 m/sec speed limit is imposed on the trajectory. As can be seen from figure-21, the radial speed keeps increasing and stops at the imposed limit. While limiting speed causes the settling time to increase from 12 seconds to 16 seconds (figure-20), it also causes a drop in the control effort (figure-22). 3. Settling time - a comparison: NADF and linear viscous dampening exhibit fundamentally different behavior as far as convergence is considered. The settling time for the point mass with no constraints on speed example is drawn in figure-23 as a function of the linear viscous friction coefficient (B). As can be seen, the TS-B relation is convex with one value for B corresponding to a global minimum of TS. This is expected since for low B high oscillations will prevent motion from quickly settling in the 5% zone around the target. On the other hand, a high value for B reduces the oscillations by slowing down the response delaying the entrance to the 5% zone. Figure-23: TS versus B for linear dampening. The relation between TS and the coefficient of NADF (Bd) is a rapidly and strictly decreasing one (figure-24). Similar to the linear case, for a low value of Bd high oscillations will prevent the quick capture of the trajectory in the 5% zone around the target. As the value of Bd increases, NADF, by design, only impedes the component of motion along the coordinate field tangent to the gradient guidance field (figure-25). This component does not contribute to convergence and it only causes delay in reaching the target. Since NADF attenuates this and only this component of motion leaving the motion along the gradient field unaffected, the delay in reaching the target drops as Bd increases yielding a strictly decreasing profile of the TS-Bd curve. Figure-24: Settling time versus NADF coefficient. The TS versus the coefficient of dampening profile is important. It determines the ability to tune the controller so that the specifications are met. In tuning the controller there are two requirements: it is required that the maximum spatial deviation (*m) between the kinematic and the dynamic path be as small as possible so that the constraints are upheld. It is also required that the settling time be as small as possible. The first requirement is achieved by making the coefficient of dampening high enough. In the linear viscous dampening case one can only strike a compromise between TS and *m. For the NADF case this compromise is not needed since both TS and *m are strictly decreasing as a function of Bd. Figure-25: Motion along gradient lines contribute to convergence, motion along tangent liens causes only delays. 4. Point mass with external forces As mentioned before, the NADF approach may be adapted to work with second order systems experiencing external forces using the suggested clamping control. In this example a point mass with constant external forces acting on it having the system equation in (71) is controlled using a gradient field and NADF. (71) x y •• •• ⎡ ⎣ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ = − − ⎡ ⎣⎢ ⎤ ⎦⎥ 4 4 As can be seen from figure-26, for a sufficiently high Bd the controller will succeed in driving the mass to the target and avoiding the obstacles. However, when the target is reached, drift caused by the external forces occur. Figure-26: Trajectory, NADF - external force present. In figure-27 a clamping control similar to the one in (35) is added with K=1, Bd=10, KC=10. As can be seen, the controller was able to hold the trajectory near the target point relying only on a loose, upper bound estimate of the drift. Despite the high value of KC , the trajectory settled in an overdamped manner with no oscillations taking place. The distance versus time curve and the Fx, Fy control forces are shown in figures-28, 29 respectively. The sliding mode (SM) control approach suggested by Guldner and Utkin in [21] for converting a gradient guidance signal in to a control signal has the ability to handle systems with external forces. In this approach a sliding surface (S) is defined as: . (72) S x v t V V d = − −∇ −∇ & ( ) Using this surface a control signal is constructed as: (73) F F S S 0 = − Figure-27: Trajectory, NADF and clamping - external force present. Figure-28: Distance to target versus time - NADF and clamping - external force present. Figure-29: x and y control force components, NADF and clamping - external force present. where vd and Fo are the maximum allowable speed and control forces respectively. The sliding mode control is applied to the point mass with drift in (71). The parameters of the sliding surface are set so that a settling time of 6 sec similar to the one using NADF and clamping control is obtained. Fo is set to obtain a maximum control effort of 100 N. The trajectory is shown in figure-30, the distance to the target versus time is in figure-31. The control forces are shown in figures-32,33. Compared to the NADF approach with clamping the trajectory obtained using the SM approach is a little shaky and experiences some oscillations near the target. However, the biggest difference has to do with the quality and magnitude of the control signals used by both approaches. Figure-30: Trajectory - sliding mode control. Figure-31: Distnace to target versus time - sliding mode control. Figure-32- y force control component - sliding mode control. Figure-33: x force control component - sliding mode control. 5. Point mass with external forces and sensor noise: The NADF is robust to the presence of sensor noise. The example in figure-34 of a point mass with drift is repeated with sensor noise causing errors in localizing the boundary of the environment. The trajectory, distance to the target versus time curve, and control forces are shown in figures-35,36. Despite the considerable amount of noise and the fact that the raw sensory data was used in the synthesis of the controller without any processing, the trajectory, convergence characteristics and control signal remained reasonably well-behaved. Figure-34: Sensor noise did not have too much effect on the trajectory. Figure-35: Distance to target versus time (sensory noise). Figure-36: x and y control force component with sensor noise present. 6. The narrow corridor effect: In his seminal work that appeared in the mid-eighties [9] Khatib suggested that the sensors of a robot be directly coupled to its servo loops. The coupling was achieved via potential fields. The result was a very high increase in the speed at which the robot responds to the contents of the environment. In the early nineties, Koren and Bornestien reported what they referred to as a serious and inherent deficiency in Khatib’s method [15]. They found that if an autonomous robot that is guided by the potential field method is operating in a narrow corridor, the robot could behave erratically, oscillating in a sustained manner between the two walls of the corridor. The artifact was called the “narrow corridor effect”. The implications of such a finding are significant. Since a service autonomous robot will have to pass through corridors in order to deliver mail in offices, laundry in hospitals, or parts in factories, the use of potential field-based planners is immediately ruled out and alternatives should be sought. While this author agrees with [15] that the narrow corridor effect is a serious artifact, he disagrees with it being an inherent deficiency in the potential field approach. This artifact is caused by a misunderstanding of the dual role the gradient of a potential field plays as both a control and guidance provider. This misunderstanding led to an improper coupling between the gradient field and the robot’s servo loops that, among other things, caused the narrow corridor artifact. The NADF approach suggested in this paper takes this fact into account and properly couples the gradient field to the servo loops of a robot totally eliminating the narrow corridor effect. A mobile robot utilizing Khatib’s approach behaves normally in an empty corridor (figure-37). However, its behavior changes dramatically if an obstruction is present along its way. The presence of the obstruction seems to ignite sustained oscillations in the trajectory of the robot (figure-38). Figure-37: Motion proceeds normally when the corridor is empty. Figure-38: The presence of an obstruction triggers sustained oscillations. In figure-39 the coefficient of dampening is increased ten times (from B=.3 to B=3) in order to get rid of the oscillations. As can be seen, remaining strong transients are clear. In the previous case, the robot was able to travel 25 meters in 10 seconds. The increase in dampening cut the travel distance to 4 meters in 10 seconds making the robot impractically slow. Figure-39: Increasing linear dampening slowed down the system and did not eliminate transient. The linear viscous dampening force is replaced by NADF. The dampening coefficient used is Bd=5 (figure-40). As can be seen, the robot responded well to the presence of the obstruction with little overshoot taking place. Not only a significant improvement in transients was achieved, the robot, despite the large value of the dampening coefficient, became more agile covering more than twice the distance in the linear dampening case (figure-37). A significant increase of Bd to 30 seems to have no effect on the distance the robot is able to travel (figure-41). Figure-40: NADF significantly reduced transients and speeded up motion. Figure-41: Increasing Bd did not slow down motion. In figure-42 robustness of the approach to the presence of sensor noise is tested. A wideband noise that is uniformly distributed between (-0.5, 0.5) is added to the sensor causing uniform jitters in the registered reading of the wall. Same as figure-41, a Bd=30 is used. As can be seen, the effect of this relatively large sensor noise is almost negligible on the trajectory of the robot where a steady path was still maintained and the travel distance was not affected. Figure-42: Same as figure-40 but with sensor noise added. In figure-43 the ability of the NADF control to handle emergency braking of motion is tested. A barrier is placed in the path of the robot. As can be seen, the control was able to brake motion in a well-behaved manner. Figure-44 shows the control forces. Figure-45 demonstrates the ability of the controller to handle multiple obstructions. Figure-43: Robot braking motion to avoid collision. Figure-44: x and y components of the braking force. Figure-45: he robot passing through a multi-obstruction environment. 7. The multi-agent case: NADFs may also be applied for the multi-robot case. In [22,30] a complete, decentralized, multi-agent planner was suggested considering the kinematics of the robots only (figure-46). Figure-46: Self-organization using the vector-harmonic potential approach. Figure-47 shows the paths for two massless robots that are trying to exchange positions. When mass is added (1Kg each), the planner totally fails (figure-48). Figure-47:Two robots exchanging positions, kinematics only. Figure-48: Same as figure-14, but with 1kg mass added to each robot. In figure-49 linear dampening is added to control the inertial forces (B=1). Figure-50 shows the distance to target of robot-1 as a function of time. It took the robot about 13 seconds to reach its target. In figure-51, NADF was used (Bd=10). As can be seen from the distance - time profile in figure-52, it took robot-1 only two seconds to reach its target. Figure-49: Linear dampening added to manage inertia, B=1. Figure-50: Distance to target versus time for robot-1 in figure-16. Figure-51: NADF added to manage inertia, Bd=10. Figure-52: Distance to target versus time for robot-1 in figure-52. 8. Iterative error removal: The iterative procedure to remove the steady state error suggested in the previous section is tested using a simple pendulum (figure-53) with concentrated mass M=1Kg and length L=1M. The dynamic equation of the pendulum is: (74) M L M g u ⋅ ⋅ + ⋅ ⋅ = && sin( ) Θ Θ where g is the acceleration constant and u is the external applied control torque. Figure-53: A simple pendulum. A simple controller with position and velocity feedback (75) is used to move the pendulum from 1=0 to 1=B/2. (75) u K B = − ⋅ − ⋅ Θ Θ& Figure-54: Steady state error caused by weight of pendulum. As can be seen from figure-54, the weight of the pendulum causes significant steady state error. In order to remove the error, the switching circuit suggested in V.2 is added to the controller. Different switching thresholds are used to assess the sensitivity of the procedure to the presence of transients (figure- 55). As can be seen, the error was eliminated in all cases. Although the iterative error cancellation procedure was designed to be used when transients fade away and motion settles, simulation shows that the procedure exhibits little sensitivity to the presence of transients that enables us to loosely choose the threshold ". Actually, the simulation reveals that better results in terms of having a lower settling time could be obtained if switching is carried out before motion completely settles. In figure-56 the effect of the forward gain on the speed of convergence is shown. As expected, the higher the forward gain is the faster the system converges to its target. Figure-55: Error cancellation using switching circuit - different thresholds. Figure-56: Effect of forward gain on convergence. VII. Conclusions In this paper the capabilities of the HPF approach are extended to tackle the kinodynamic planning case. The extension is provably-correct and bypasses many of the problems encountered by previous approaches. It is based on a novel type of nonlinear, passive dampening forces called NADFs. The suggested approach enjoys several attractive properties. It is easy to tune; it can generate a well-behaved control signal; the approach is flexible and may be applied in a variety of situations, it is provably-correct; it is resistant to sensor noise; it does not require exact knowledge of system dynamics, and it can tackle dissipative systems as well as systems under the influence of external forces. It ought to be emphasized that most of the problems attributed to the potential field approach are a result of the misunderstanding of the dual role a potential field plays as a motion actuator and a guidance provider. The NADF approach is a step forward in taking both of these roles into account. Acknowledgment The author would like to thank KFUPM for its support of this work. Appendix A. Definition: Let V(X) be a smooth ( at least twice differentiable) scalar function (V(X): RN 6 R). A point Xo is called a critical point of V if the gradient vanishes at that point (LV(Xo)=0); otherwise, Xo is regular. A critical point is Morse, if its Hessian matrix (H(Xo)) is nonsingular. V(X) is Morse if all of its critical points are Morse [24]. B. Proposition: If V(X) is a harmonic function defined in an N- dimensional space (RN) on an open set S, then the Hessian matrix at every critical point of V is nonsingular, i.e. V is Morse. Proof: There are two properties of harmonic functions that are used in the proof: 1- a harmonic function (V(X)) defined on an open set S contains no maxima or minima, local or global in S. An extrema of V(X) can only occur at the boundary of S, 2- if V(X) is constant in any open subset of S, then it is constant for all S. Other properties of harmonic functions may be found in [26]. Let Xo be a critical point of V(X) inside S. Since no maxima or minima of V exist inside S, Xo has to be a saddle point. Let V(X) be represented in the neighborhood of Xo using a second order Taylor series expansion: *X-X0*<<1. 76 V(X) V(Xo) V(Xo) (X Xo) 1 2 (X Xo) H(Xo)(X Xo) T T = + ∇ − + − − Since Xo is a critical point of V, we have: *X-X0*<<1. 77 V V(X)- V(Xo) 1 2 (X Xo) H(Xo)(X Xo) ' T = = − − Notice that adding or subtracting a constant from a harmonic function yields another harmonic function , i.e. V` is also harmonic. Using eigenvalue decomposition [25]: 78 V 1 2 (X Xo) U 0 0 0 0 . 0 . . . 0 0 . U(X Xo) ' T T 1 2 N = − ⋅ ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ − λ λ λ = ⋅ ⎡ ⎣ ⎢ ⎢ ⎢ ⎢ ⎤ ⎦ ⎥ ⎥ ⎥ ⎥ = =∑ 1 2 0 0 0 0 . 0 . . . 0 0 . T 1 2 N ξ λ λ λ ξ λ ξ 1 2 i i i 1 N 2 where U is an orthonormal matrix of eigenvectors, 8’s are the eigenvalues of H(Xo), and >=[>1 >2 ..>N]T = U(X-Xo). Since V` is harmonic, it cannot be zero on any open subset S; otherwise, it will be zero for all S, which is not the case. This can only be true if and only if all the 8i’s are nonzero. In other words, the Hessian of V at a critical point Xo is nonsingular. This makes the harmonic function V also a Morse function. The navigation function defined in [13] is a special case of a harmonic potential field. According to [13] a navigation function must satisfy the following properties: 1- it is smooth (at lest C2), 2- it contains only one minimum located at the target point, 3- it is a Morse function, 4- it is maximal and constant on '. A harmonic function (V) is C4 and Morse. Harmonic functions are extrema-free in S. Their maxima and minima can only happen at the boundary of S. In the harmonic approach ' and the target point (XT) are treated as the bounary of S. Through applying the appropriate boundary conditions, the minimum of V is forced to occur on XT. Also, by the application of the Drichlet boundary conditions, the value of V is forced to be maximal and constant at '. The Drichlet condition (constant potential on the boundary) is one of many settings used in constructing a harmonic potential that may be used for navigation. References [1] K. Sato, “Collision avoidance in multi-dimensional space using laplace potential,” in Proc. 15th Conf. Robotics Soc. Jpn., 1987, pp. 155–156. [2] K. Sato, “Deadlock-free motion planning using the laplace potential field,”Adv. Robotics, vol. 7, no. 5, pp. 449–461, 1993. [3] S. Masoud, Ahmad A. Masoud, " Motion Planning in the Presence of Directional and Obstacle Avoidance Constraints Using Nonlinear Anisotropic, Harmonic Potential Fields: A Physical Metaphor", IEEE Transactions on Systems, Man, & Cybernetics, Part A: systems and humans, Vol 32, No. 6, November 2002, pp. 705-723. [4] S. Masoud Ahmad A. Masoud, "Constrained Motion Control Using Vector Potential Fields", The IEEE Transactions on Systems, Man, and Cybernetics, Part A: Systems and Humans. May 2000, Vol. 30, No.3, pp.251-272. [5] A. Masoud, "An Informationally-Open, Organizationally-Closed Control Structure for Navigating a Robot in an Unknown, Stationary Environment" 2003 IEEE International Symposium on Intelligent Control, October 5-8, 2003, Houston, Texas, USA, pp. 614-619. [6] A. Masoud, Samer A. Masoud, "A Self-Organizing, Hybrid, PDE-ODE Structure for Motion Control in Informationally-deprived Situations", The 37th IEEE Conference on Decision and Control, Tampa Florida, Dec. 16-18, 1998, pp. 2535-2540. [7] A. Masoud, Samer A. Masoud, "Evolutionary Action Maps for Navigating a Robot in an Unknown, Multidimensional, Stationary Environment, Part II: Implementation and Results", the 1997 IEEE International Conference on Robotics and Automation, April 21-27, Albuquerque, New Mexico, USA, pp. 2090-2096. [8] A. Masoud, Samer A. Masoud, Mohamed M. Bayoumi, "Robot Navigation Using a Pressure Generated Mechanical Stress Field, The Biharmonic Potential Approach", The 1994 IEEE International Conference on Robotics and Automation, May 8-13, 1994 San Diego, California, pp. 124-129. [9] O. Khatib, “Real-time obstacle avoidance for manipulators and mobile robots,” in Proc. IEEE Int. Conf. Robotics Automat., St. Louis, MO, Mar. 25–28, 1985, pp. 500–505. [10]X. Yun; K. Tan, "A wall-following method for escaping local minima in potential field based motion planning" ICAR '97. Proceedings., 8th International Conference on Advanced Robotics, Monterey, CA, USA,7-9 July 1997, pp: 421 - 426. [11]A. A. Petrov and I. M. Sirota, “Control of a robot-manipulator with obstacle avoidance under little information about the environment,” in Proc. 8th IFAC Congr., vol. 14, Kyoto, Japan, Aug. 1981, pp. 54–59. [12]B. Krogh, “A generalized potential field approach to obstacle avoidance control,” in Robotics Research: The Next Five Years and Beyond, Bethlehem, PA, USA, Aug. 14–16, 1984, pp. 1–15. [13]D. Koditschek, “Exact robot navigation by means of potential functions: Some topological considerations,” in IEEE Int. Conf. Robotics and Automation, Raleigh, NC, Mar. 1987, pp. 1–6. [14]D. Koditschek, E. Rimon, “Exact robot navigation using artificial potential functions,” IEEE Trans. Robot. Automat., vol. 8, pp. 501–518, Oct. 1992. [15]Y. Koren, J. Borenstein, “Potential Field Methods and Their Inherent Limitations for Mobile Robot Navigation”, 1991 IEEE International Conference on Robotics and Automation, Sacramento, California, April 1991, pp. 1398-1404. [16]C. Connolly, R.Weiss, and J. Burns, “Path planning using laplace equation,” in Proc. IEEE Int. Conf. Robotics Automat., Cincinnati, OH, May 13–18, 1990, pp. 2102–2106. [17]E. Prassler, “Electrical networks and a connectionist approach to pathfinding,” in Connectionism in Perspective, R. Pfeifer, Z. Schreter, F. Fogelman, and L. Steels, Eds. Amsterdam, The Netherlands: Elsevier, North-Holland, 1989, pp. 421–428. [18]I. Tarassenko and A. Blake, “Analogue computation of collision-free paths,” in Proc. IEEE Int. Conf. Robotics Automat., Sacramento, CA, Apr. 1991, pp. 540–545. [19]J. Decuyper and D. Keymeulen, “A reactive robot navigation system based on a fluid dynamics metaphor,” in Proc. Parallel Problem Solving From Nature, First Workshop, H. Schwefel and R. Hartmanis, Eds., Dortmund, Germany, Oct. 1–3, 1990, pp. 356–362. [20]S. Akishita, S. Kawamura, and K. Hayashi, “New navigation function utilizing hydrodynamic potential for mobile robot,” in Proc. IEEE Int. Workshop Intell. Motion Contr., Istanbul, Turkey, Aug. 20–22, 1990, pp. 413–417. [21] J. Guldner, V. Utkin, “Sliding Mode Control for Gradient Tracking and Robot Navigation Using Artificial Potential Fields”, IEEE Transactions on Robotics and Automation, Vol. 11, pp. 247-254, April 1995. [22] A. Masoud, "Using Hybrid Vector-Harmonic Potential Fields for Multi-robot, Multi-target Navigation in a Stationary Environment", 1996 IEEE International Conference on Robotics and Automation, April 22-28, 1996, Minneapolis, Minnesota, pp.3564-3571. [23] J. LaSalle, “Some Extensions of Lyapunov’s Second Method”, IRE Transactions on Circuit Theory, CT-7, No. 4, pp. 520-527, 1960. [24] J. Milnor, “Morse Theory”, Princeton University Press, 1963. [25] G. Strang, “Linear Algebra and its Applications”, Academic Press, 1988. [26] Axler, P. Bourdon, W. Ramey, “Harmonic Function Theory”, Springer, 1992. [27] S. Arimoto, F. Miyazaki, “Stability and Robustness of PID Feedback Control for Robotics Manipulators of Sensory Capabilities”, in Proc. 1st Int. Symp. Robotics Research, 1984, pp. 385-407. [28] V. I. Istratescu, “Fixed Point Theory, An Introduction”, D.Reidel, Holland (1981). ISBN 90-277-1224-7 [29] H. Lutkepohl, “Handbook on Matrices”, Wiley, 1996. [30] A. Masoud, "Decentralized, Self-organizing, Potential field-based Control for Individually-motivated, Mobile Agents in a Cluttered Environment: A Vector-Harmonic Potential Field Approach", IEEE Transactions on Systems, Man, & Cybernetics, Part A: systems and humans, tentatively scheduled to appear July 2007