FrIP6.1. subject to: V 0 &V 1 The trajectory to the target (x(t)) is generated using the HPFbased, gradient dynamical system:

Transcription

1 Proceeings of the 45th IEEE onference on Decision & ontrol Manchester Gran Hyatt Hotel San Diego, A, USA, December 3-5, 006 Agile, Steay Response of Inertial, onstraine Holonomic Robots Using Nonlinear, Anisotropic Dampening Forces Ahma A. Masou Electrical Engineering Department, KFUPM, P.O. Box 87, Dhaharan 36, Saui Arabia, Abstract- In this paper the harmonic potential fiel (HPF) approach to motion planning is aapte to work with secon orer mechanical systems. he extension is base on a novel type of ampening forces calle: nonlinear, anisotropic, ampening forces (NADFs). It is shown that NADFs are effective ais for planning spatially-constraine kinoynamic trajectories for mechanical systems. heoretical evelopments an simulation results are provie in the paper. I. Introuction A planner may be efine as an intelligent, purposive, contextsensitive controller that can instruct an agent on how to eploy its motion actuators (i.e generate a control signal) so that a target state may be reache in a constraine manner. he harmonic potential fiel approach [-3] is a powerful paraigm for constructing motion planners. In its current form the HPF approach can only eal with the kinematic aspects of trajectory generation. he output from the planner has to be converte into a control signal by a low level controller. Gulner an Utkin suggeste an interesting approach base on a sliing moe control for converting the graient fiel from an HPF into a control signal [4]. he main rawback of the approach seems to be the high shattering the control signal experiences. Another metho to convert the graient guiance fiel from a potential surface (-V) into a control signal (u), is to use a viscous ampening force that is linearly proportional to spee: u BxV(x) () his combination will only work provie that the initial spee of the robot is lower than a certain upper boun [5]. results are in section VI, an conclusions are place in section VII. II. Backgroun Although the HPF approach was brought to the forefront of motion planning inepenently an simultaneously by ifferent researchers [6-8], the first work to be publishe on the subject was that by Sato in 986 [6]. he HPF approach forces the ifferential properties of the potential fiel to satisfy the Laplace equation insie the workspace of a robot () while constraining the properties of the potential at the bounary of (=). he bounary set inclues both the bounaries of the forbien zones (O) an the target point (x ). A basic setting of the HPF approach is: V(x) 0 x subject to: V 0 &V. () XX X he trajectory to the target (x(t)) is generate using the HPFbase, graient ynamical system: x V(x) x(0) x 0 (3) he trajectory is guarantee to: - lim x(t) x - x(t) t (4) t whereby a proof of (4) may be foun in [3]. Figure- shows the negative graient fiel of a harmonic potential. Figure- shows the trajectory, x(t), generate using (3). In this paper a metho is suggeste to enable the HPF approach to irectly generate the navigation control signal. his is accomplishe by augmenting the graient guiance fiel from an HPF with a new type of ampening force calle: nonlinear anisotropic ampening forces (NADFs). It is shown that an NADF-base control can efficiently suppress inertia-inuce artifacts in the ynamical trajectory of the system making it closely follow the kinematic trajectory while maintaining an agile system response. he approach oes not require the system ynamics to be fully known. A loose upper boun is sufficient for constructing a well-behave control signal that can eal with issipative systems as well as systems being influence by external forces (e.g. gravity). his paper is organize as follows: section II provies a brief backgroun of the potential fiel approach. he NADF approach is presente in section III. Sections IV an V iscuss the application of the approach to issipative systems an systems experiencing external forces respectively. Simulation Figure-: Guiance fiel of an HPF. Figure-: rajectory generate by the fiel in figure /06/$ IEEE. 667

2 he trajectory, x(t), shoul be fe to a low-level controller in orer to generate the control signal, u. In figure-3, the negative graient of the potential in figure- is use to navigate a kg point mass. he ynamic equation of the system is: x B x (5) y y v(x, y) / x v(x, y) / y where B=0.. Despite the fact that the initial spee of the robot is zero, the trajectory violate the avoiance conition an collie with the walls of the room. will globally, asymptotically converge to: lim x x t, lim x 0 t (8) for any positive constants B an K, where xr N, V(x):R N R, D(x) is an N N positive efinite inertia matrix, (x, x)x contains the centripetal, oriolis, an gyroscopic forces. Figure-3: trajectory of a point mass controlle by the fiel in figure-. a- action of the ampening force III. he NADF Approach he linear velocity component acts as a ampener of motion that may be use to place the inertial force uner control by marginalizing its isruptive influence on the trajectory of the robot that the graient fiel is attempting to generate. Unfortunately, this approach ignores the ual role the graient fiel plays as a control an guiance provier. A ampening component that is proportional to velocity exercises omniirectional attenuation of motion regarless of the irection along which it is heaing. his means that the useful component of motion marke by the irection along which the goal component of the graient of the artificial potential is pointing is treate in the same manner as the unwante inertia-inuce, noise component of the trajectory. he guiance an isruptive components shoul not be treate equally. Attenuation shoul be restricte to the inertia-cause isruptive component of motion, while the component in conformity with the guiance of the artificial potential shoul be left unaffecte (figure-4). A carefully constructe ampening component that treats the graient of the artificial potential both as an actuator of ynamics an as a guiing signal is: t V(x) V(x) M(x, x) [(n ) n ( x x( V(x) x)) (6) V(x) V(x) ] where n is a unit vector orthogonal to V an is the unit step function. his force is given the name: nonlinear, anisotropic, ampening force (NADF). IV- Dissipative Systems In this section two propositions are state an proven for issipative systems. Proposition-: Let V(x) be a harmonic potential generate using the BVP in (). he trajectory of the ynamical system: Dxx ( ) xxx (, ) B Mxx (, ) KVx ( ) 0 xx (, ) D ( x, x) b- block iagram Figure-4: nonlinear, anisotropic, ampening force (NADF). Proof: Let be the Liapunov function caniate: (x, x) K V(x) xd(x)x (9) Note that since V(x) is harmonic, it must assume its maxima on an minima on x. In other wors, V(x) can only be zero at x ; otherwise, its value is greater than zero: 0 iff x 0,x 0 (x, x) positive otherwise. (0) he time erivative of the above function is: (x, x) K V(x) x x D(x)x x D(x)x. () Substituting: x D (x)[ (x, x)x B M(x, x) K V(x)] () along with (7) in the above equation yiels: KV(x) x x D(x)x K V(x) x x (x, x)x B x (n x)n (3) V(x) V(x) B x ( x ( V(x) x) V(x) V(x) Using the passivity property: x (D(x) (x, x))x 0 (4) (7) 668 S x S x

3 an rearranging the terms we get: B (n x) (n x) (5) ( V(x) x) ( V(x) x) B ( V(x) x) V(x) V(x) as can be seen 0 x,x, (6) where 0 for x, x 0 accoring to LaSalle principle [9] any boune solution of (7) will converge to the minimum invariant set: E {x 0,x}. (7) Exactly etermining E requires stuying the critical points of V(x) where V(x)=0. Accoring to the maximum principle, x is the only minimum (stable equilibrium point) V(x) can have. Besies x, V(x) has other critical points {x i }; however, the hessian at these points is non-singular, i.e. V(x) is Morse [0]. his can be easily shown by expaning V(x) aroun a critical point x i (x-x i <<) using aylor series: Vx Vx Vx x x (8) x x ( ) ( H x x x i) ( i) ( i) ( i) ( i)( i) Since x i is a critical point of V, the linear term vanishes. Rewriting the above equation, we have: V `( x Vx Vx (9) x x ) ( ) ( H x x x i) ( i) ( i)( i) Notice that V` is also a harmonic function. Using eigen value ecomposition [], we have V ` N 0. 0 i i... i 0 0. N (0) where =U(x-x i ) = [.. N ], an U is an orthogonal matrix of eigen vectors. Since V` is harmonic, it cannot be zero on any open subset of ; otherwise, it will be zero for all []. Since it is impossible for V` to be zero for all, non of the i s of the hessian matrix can be zero. In other wors, the hessian at x i cannot be singular. From the above we conclue that E contains only one point which is the point x x, x 0 motion will converge. Proposition-: Let be the trajectory constructe as the spatial projection of the solution, x(t), of the first orer ifferential system in (3). Also Let be the trajectory constructe as the spatial projection of the solution, x(t), of the secon orer system in (7), figure-5. hen there exist a B that can make the maximum eviation between an ( m ) arbitrarily small. Vx ( ) Vx ( ) Figure-5: he kinematic an ynamic trajectories. Proof: he graient fiel from an HPF oes not only work as a guie of motion to the target; it also may be use to cover with a complete set of bounary-fitte basis coorinates. he raial basis of the system (V/V) marks the useful component of motion. he basis orthogonal to this component span the isruptive component of motion () which NADF is require to attenuate (figure-6). Figure-6: he isruptive component of motion. he ynamic equation escribing the isruptive component is: n D(x)x n (x, x)x B n M(x, x) K n V(x) 0 () Examining the above equation term by term yiels: - n V 0, () - n V(x) t V(x) [(n x) n ( x ( V(x) x)) = V(x) V(x) ] (n t x ) 3- assuming that an upper boun can be place on the spee: x max (3) the norm of the matrix may be boun as: (x, x) c max (4) 4- any inertia matrix belonging to a physical system is positive efinite, invertible, an have a boune norm: D(x) (5) max where max, c max, an max are finite, positive constants. Base on the above, a ynamic equation that yiels an upper boun on is: max n xcmaxn x B n x 0 (6) or 0 where n x, B n cmax x, an. max o etermine the effect of the isruptive time component ((t)) that acts normal to V, the impulse response (h(t)) of (6) is obtaine: ` to which h(t) t h(t) ( e ) ( t). (7) he eviation as a function of time may be compute as: () t () t h(t) where * enotes the convolution operation. Since it was shown in proposition- that motion will converge to x an all ynamic terms will ten to zero, (t) may be boune as: (t) t I, (8) 0 ` I max therefore: () t h(t)* () t, where I an I max are positive constants. By properly selecting a value for, the maximum eviation m can be mae arbitrarily small. In other wors the ynamic trajectory of (7) will closely follow the kinematic trajectory of (3) an the spatial constraints will be preserve. V. Systems with External Forces he NADF approach may be aapte for esigning constraine motion controller for mechanical systems experiencing external forces (e.g. gravity). he ynamical equation of such systems has the form: D(x)x (x, x)x G(x) F (9) 669

4 where G(x) an F are vectors containing the external forces an (x, x) K V(x) x D(x)x P(x) the applie control forces respectively. he controller: F B M(x, x) K V(x). (30) has the ability to make the trajectory of the system in (9) note that: an. (37) (38) closely follow the kinematic trajectory from an initial starting Differentiating (37) with respect to time we get: point (x o ) to the target point x. However, ue to the presence (x, x) K V(x) x x D(x)x x D(x)x x G(x) of the external forces the controller will not be able to hol the (39) state close to the target point an rift will occur (Figure-). solving for x from equations (9, 30) an substituting the Here an approach for effectively ealing with this type of results in (39) we get: systems is suggeste. B (n x) (n x) (40) ( V(x) x) ( V(x) x) B ( V(x) x). lamping control: V(x) V(x) he effect of the clamping control (F c ) is strictly localize to a K x (x x ) (x (x x )) F( x x ) hyper sphere of raius surrouning the target point. If motion Since K c an B are positive we have: is heaing towars the target, this control component is inactive. 0 x,x, (4) On the other han, if motion starts heaing away from the target, the control becomes active an attempts to rive the where 0 for x, x 0. trajectory back to the target (Figure-7). A form of a clamping Since we are assuming that K an B are selecte high enough control that behaves in the above manner is: so that the ynamic trajectory will follow the kinematic (3) trajectory an enter, the minimum invariant set to which the trajectory is going to converge may be compute from the he strength of F c is ajuste using the constant K c so that the equation: steay state error is kept below a esire level (). lamping G(x) K V(x) K F (x, x 0) 0 (4) control maintains stability for any positive K c. K ( X X) X (X X) 0 Force 0 X X X ( X X) 0 Force K ( X X) Figure-7: he clamping control. Proposition-3: For the mechanical system in (9), a controller of the form: F B M(x, x) K V(x) K F (x, x) (3) can make lim x(t) x an lim x 0 (33) t provie that: - K, B, an K c are all positive, - K c F max /, F maxg(x) max X t x an {x: x x }. (34) 3- a high enough value of B is selecte so that at some instant in time t` x(t`) x (35) 4- K is high enough so that the graient fiel is capable of irecting the trajectory to V(X) K V(X) G (X) X- (36) V(X) Proof: onsier a Liapunov function similar to the one in (9) with a gravitational potential energy term (P(X)) ae: Since (0)=, an x (i.e. (-x-x )=), equation (4) becomes: G(x) K V(x) K (x x ) 0 (43) As can be seen if conition on K is satisfie, the solution of the above equation has to lie in the set ={x:x-x <}. his means that the eviation of the en of the ynamic trajectory from the target point shoul at most be. Selecting a high gain of the clamping control (K ) to manage the steay state error will not cause a transients problem. his is ue to the fact that this control component is esigne to be minimally intrusive affecting the system only when it is neee. VI. Results he graient fiel in figure- is augmente with NADF an use to steer a Kg mass from a start point to a target point. A B =0, is use. he trajectory of the mass is shown in figure-8, an the mass istance to the target, D(t) is shown in figure-9. As can be seen, the kinoynamic trajectory of the mass is almost ientical to that marke by the graient fiel (kinematics only) in figure- with a settling time ( S ) of about secons. Figure- 0 shows the control signal (X-Y force components). Figure-8: rajectory, NADF, B=0. 670

5 In this example a point mass with constant external forces acting on it having the system equation in (44) is controlle using a graient fiel an NADF. x (44) y 4 4 As can be seen from figure-, for a sufficiently high B the controller will succee in riving the mass to the target an avoiing the obstacles. However, when the target is reache, rift cause by the external forces occur. Figure-9: Distance to target versus time, NADF. Figure-0: x an y control force components, NADF. he relation between S an the coefficient of NADF (B ) is a rapily an strictly ecreasing one (figure-). For a low value of B high oscillations will prevent the quick capture of the trajectory in the 5% zone aroun the target. As the value of B increases, NADF, by esign, only impees the component of motion along the coorinate fiel tangent to the graient guiance fiel. his component oes not contribute to convergence an it only causes elay in reaching the target. Since NADF attenuates this an only this component of motion leaving the motion along the graient fiel unaffecte, the elay in reaching the target rops as B increases yieling a strictly ecreasing profile of the S -B curve. Figure-: rajectory, NADF - external force present. In figure-3 a clamping control similar to the one in (3) is ae with K=, B =0, K =0. As can be seen, the controller was able to hol the trajectory near the target point relying only on a loose, upper boun estimate of the rift. Despite the high value of K, the trajectory settle in an overampe manner with no oscillations taking place. he Fx, Fy control forces are shown in figure-4. Figure-: Settling time versus NADF coefficient. Figure-3: rajectory, NADF an clamping - external force present. he S versus the coefficient of ampening profile is important. It etermines the ability to tune the controller so that the specifications are met. In tuning the controller there are two requirements: it is require that the maximum spatial eviation ( m ) between the kinematic an the ynamic path be as small as possible so that the constraints are uphel. It is also require that the settling time be as small as possible. he first requirement is achieve by making the coefficient of ampening high enough. In the linear viscous ampening case one can only strike a compromise between S an m. For the NADF case this compromise is not neee since both S an m are strictly ecreasing as a function of B. Figure-4: x an y control force components, NADF an clamping - external force present. 67

6 he sliing moe (SM) control approach suggeste by Gulner an Utkin in [4] for converting a graient guiance signal in to a control signal has the ability to hanle systems with external forces. he parameters of the sliing surface are set so that a settling time of 6 sec similar to the one using NADF an clamping control is obtaine. he trajectory is shown in figure- 5. he control forces are shown in figures-6. ompare to the NADF approach with clamping the trajectory obtaine using the SM approach is a little shaky an experiences some oscillations near the target. However, the biggest ifference has to o with the quality an magnitue of the control signals use by both approaches. encountere by previous approaches. It is base on a novel type of nonlinear, passive ampening forces calle NADFs. he suggeste approach enjoys several attractive properties. It is easy to tune; it can generate a well-behave control signal; the approach is flexible an may be applie in a variety of situations, it is provably-correct; it is resistant to sensor noise; it oes not require exact knowlege of system ynamics, an it can tackle issipative systems as well as systems uner the influence of external forces. Figure-8: Linear ampening, B=. rajectories an robot- istance to target. Figure-5: rajectory - sliing moe control. Figure-9: NADF, B=0. rajectories an robot- istance to target. Acknowlegment: he author acknowleges KFUPM support of this work. Figure-6: X an Y force control component - sliing moe control. NADFs may also be applie for the multi-robot case [3,4]. Figure-7 shows the paths for two massless robots that are trying to exchange positions. It also shows that when mass is ae (Kg each), the planner totally fails. Figure-7: robots exchanging positions, kinematics (Left) Dynamics (right). In figure-8 linear ampening is ae to control the inertial forces (B=). It took the robot about 3 secons to reach its target. In figure-9, NADF was use (B =0). It took robot- only two secons to reach its target. VII. onclusions In this paper the capabilities of the HPF approach are extene to tackle the kinoynamic planning case. he extension is provably-correct an bypasses many of the problems References [] S. Masou Ahma A. Masou, "onstraine Motion ontrol Using Vector Potential Fiels", he IEEE ransactions on Systems, Man, an ybernetics, Part A: Systems an Humans. May 000, Vol. 30, No.3, pp.5-7. [] A. Masou, Samer A. Masou, Mohame M. Bayoumi, "Robot Navigation Using a Pressure Generate Mechanical Stress Fiel, he Biharmonic Potential Approach", he 994 IEEE International onference on Robotics an Automation, May 8-3, 994 San Diego, alifornia, pp [3] S. Masou, Ahma A. Masou, " Motion Planning in the Presence of Directional an Obstacle Avoiance onstraints Using Nonlinear Anisotropic, Harmonic Potential Fiels: A Physical Metaphor", IEEE ransactions on Systems, Man, & ybernetics, Part A: systems an humans, Vol 3, No. 6, November 00, pp [4] J. Gulner, V. Utkin, Sliing Moe ontrol for Graient racking an Robot Navigation Using Artificial Potential Fiels, IEEE ransactions on Robotics an Automation, Vol., pp , April 995. [5]D. Koitschek, E. Rimon, Exact robot navigation using artificial potential functions, IEEE rans. Robot. Automat., vol. 8, pp , Oct. 99. [6] K. Sato, ollision avoiance in multi-imensional space using laplace potential, in Proc. 5th onf. Robotics Soc. Jpn., 987, pp [7]. onnolly, R.Weiss, an J. Burns, Path planning using laplace equation, in Proc. IEEE Int. onf. Robotics Automat., incinnati, OH, May 3 8, 990, pp [8]J. Decuyper an D. Keymeulen, A reactive robot navigation system base on a flui ynamics metaphor, in Proc. Parallel Problem Solving From Nature, First Workshop, H. Schwefel an R. Hartmanis, Es., Dortmun, Germany, Oct. 3, 990, pp [9] J. LaSalle, Some Extensions of Lyapunov s Secon Metho, IRE ransactions on ircuit heory, -7, No. 4, pp , 960. [0] J. Milnor, Morse heory, Princeton University Press, 963. [] G. Strang, Linear Algebra an its Applications, Acaemic Press, 988. [] Axler, P. Bouron, W. Ramey, Harmonic Function heory, Springer, 99. [3] A. Masou, "Decentralize, Self-organizing, Potential fiel-base ontrol for Iniviually-motivate, Mobile Agents in a luttere Environment: A Vector-Harmonic Potential Fiel Approach", IEEE ransactions on Systems, Man, & ybernetics, Part A: systems an humans, tentatively scheule to appear July 007 [4] A. Masou, "Using Hybri Vector-Harmonic Potential Fiels for Multi-robot, Multi-target Navigation in a Stationary Environment", 996 IEEE International onference on Robotics an Automation, April -8, 996, Minneapolis, Minnesota, pp