Real-Time Obstacle Avoidance for trailer-like Systems

Transcription

1 Real-Time Obstacle Avoidance for trailer-like Systems T.A. Vidal-Calleja, M. Velasco-Villa,E.Aranda-Bricaire. Departamento de Ingeniería Eléctrica, Sección de Mecatrónica, CINVESTAV-IPN, A.P. 4-74, 7, México D.F. Programa de Matematicas Aplicadas y Computación, Instituto Mexicano del Petroleo, A.P. 4-8, 773, México D.F. earanda@mail.cinvestav.mx Abstract The obstacle avoidance problem associated to a multisteered general -trailer is studied in this work by considering the method of artificial potential fields. It is proposed a control strategy based on artificial potential fields that generate a trajectory to be followed by the tractor that represents, at the same time, a reference for the trailer. The control laws proposed in this paper are experimentally implemented on a prototype built at the laboratory. Introduction A trailer-like system is a mobile robot composed by a tractor which pulls n trailers connected by a revolute joint. There are two main models of trailer systems considered in the literature. Namely, the standard n-trailer and general n-trailer. The difference between these two models arises from the position of the joint. The joint of the standard n-trailer is located at the rear axle, while the joint of the general n-trailer is located off this axle. The artificial potential fieldmethodwasdevelopedin [, 6], and has been extensively studied in the obstacle avoidance problem for autonomous mobile robots [, 3, 8]. There are two main approaches for this method. On one hand, the classical method [] depends only on the position of the mobile robot. On the other hand, the generalized potential fieldmethod[6]dependsalsoon the velocity of the robot. The underlying idea of the method is to fill the robot s workspace with an artificial potential field in which the vehicle is attracted to its goal and is repulsed away from the obstacles. Supported by CONACyT-Mexico. On sabbatical leave from CINVESTAV-IPN. One of the inherent problems of this method is the existence of local minima which are undesirable equilibrium points of a gradient system. They appear when the sum of the attractive and repulsive forces induced by the potential vanishes before its goal. The multi-steered general n-trailer presented in [7] is a trailer-like vehicle with actuated direction wheels for each trailer. In this work the obstacle avoidance problem associated to a multi-steered general -trailer is considered. This problem will be tackled by means of the method of artificial potential fields. Real-time obstacle avoidance for trailer-like systems is important for some applications. The vehicle should be capable to arrive to its goal without collide with the workspace obstacles. Some experimentation results are presented in this work for a multi-steered general -trailer. Kinematic Model A multi-steered general -trailer composed by a mobile robot and a trailer with a steered wheel is considered. The position of the center of the tractor axle with respect to the fixed reference frame (X,X ) is given by (x,x ). The orientation θ corresponds to the angle that the robot forms with respect to X,asshownin Figure. The orientation of the trailer is denoted by θ. The direction of the actuated wheels with respect to the longitudinal axis of the trailer is denoted β.thevariables u,u represent respectively the linear and angular velocities of the tractor and u 3 represents the angular velocity of the steering wheel for the trailer. The position of the center of the tractor axle is given by (w,w ). The distance between the point (x,x ) and the joint is given by d and the distance between this joint and the point (w,w ) is denoted d. With this notation the

2 obstacle by q o =(s,s ).Anartificial potential function applied to the vehicle at point q, has the form, U(q) =U a (q)+u r (q), (3) where U a (q) is the attractive potential induced by the goal and U r (q) is the repulsive potential induced by the obstacle. The resultant force is then obtained as, F = F a + F r, (4) Figure : Multi-steered general -trailer kinematic model is given by [7], where, ẋ = u cos θ ẋ = u sin θ θ = u θ = a (θ,θ,β) u a (θ,θ,β) u β = u 3, a = sin(θ θ β d cos β ) d a = cos(θ θ β d cos β ). () 3 Artificial Potential Field Method A gradient system [4] on an open set W R n is a dynamic system of the form ẋ = V (x) () where V : U R is a C function, and µ V V =,..., V x x n is the gradient vector field, V : U R n of V. Regular points of the trajectories of (), cross level surfaces of the function V (x) orthogonally. Denote the position of a point of the vehicle in a twodimensional workspace by q =(y,y ), the position of the goal by q g =(y g,y g ) and the position of a unique where, F a (q) = U a (q), () F r (q) = U r (q). In the sequel, k k : R R + denotes the usual Euclidean distance function. TheattractiveforceF a guides the robot to the goal and F r is a repulsive force which repels the vehicle from the obstacle. Usually the attractive potential is defined by, U a (q) = ξρ(q, q g), (6) where ξ is a positive scaling factor and ρ(q, q g ) = kq q g k is a positive definite function whose first derivative is continuous and has a global minimum equal to zero at q = q g. The repulsive potential function takes the form, ³ U r (q) = η ρ(q,q o) ρ if ρ ρ (7) if ρ>ρ, where η is a positive scaling factor and ρ(q, q o ) is defined above. The region of influence of the obstacle is determined by the positive constant ρ.noticethatu r ( ) is once continuously differentiable, e.g. U r (q) C R. From equations (, 6, 7), the induced forces can be expressed as, F a (q) = ξ ³ (q q g ) F r (q) =η ρ(q,q o ) ρ ρ(q,q o ) ρ(q,q o ) q (8) where F r (q) is applied if ρ ρ and is set to zero otherwise. Inthecaseofn obstacles the above procedure can be generalized by considering the force Fr i associated to ith-obstacle, producing nx F r = Fr. i i= Figure shows the potential function for a three obstacles case. It is easy to see that the goal is a global minimum and the potential function indefinitely grows inside the region of influence of the obstacles.

3 goal obstacles Consider the desired values q d =(ẏ d, ẏ d ) to be proportional to the normalized force generated by the potential field [], ẏ d = ẏ d v d p f + f f f, () where f, f are respectively the components of the total force F in the direction of the axes X and X and v d is the desired scalar velocity. Under this conditions, by considering equations () and () it is possible to propose the feedback control law: u u = v d A (θ f ) +f f f. () Figure : Artificial Potential Field Method. Remark Note that the classical method depends only on the relative position of the obstacles and vehicle. Notice that also the vehicle moves in the force field direction. 4 Control Strategy It is intended to take the vehicle from initial to a final position without colliding with obstacles. The control strategy will be developed in two steps. First, the tractor control law will be designed using an artificial potential field. Then the tracking problem of the trailer will be analyzed. By using the potential field method the tractor will follow the direction of the resultant force. The trajectory generated by the tractor is considered as a reference trajectory for the trailer. 4. Tractor Control Consider the output function q =(y,y ) as the point that represents the middle of the front end of the tractor. The coordinates of this point are given by, y = x + cos θ y = x + sin θ, (9) where is the orthogonal distance between the rear axle point (x,x ) and the point q of the robot. Taking the time-derivative of (9), produces, with ẏ ẏ = A(θ ) u u cos θ sin θ A(θ )=. sin θ cos θ, () Note that the attractive potential and the components of its negative gradient are given by, U a = ξ ³ (y y g ) +(y y g ) f a = ξ(y g y ) f a = ξ(y g y ). On the other hand, the repulsive potential and the components of its negative gradient, inside of the region of influence, are given by, µ U r = η (y s ) +(y s ) ρ µ f r =η (y s ) +(y s ) ρ y s ((y s ) +(y s ) ) µ f r =η 4. Trailer Control (y s ) +(y s ) ρ y s ((y s ) +(y s ) ). As mentioned before, it is intended that the trailer of the vehicle follows the path described by the tractor as a result of the artificial potential field applied to it. One way to achieve the above goal is to apply a control strategy that makes the center of the trailer axle (w,w ) to follow approximately the position of a point in the tractor (y,y ), subject to a given time delay τ. This is, a point in the trailer has to take the same position that was generated by the tractor. To implement this strategy, consider β as the output of the trailer subsystem. The angle β will be manipulated to indirectly control the position of a point in the trailer. Consider the error e φ defined as, e φ =(φ φ d ), 3

4 y,y,θ x,x,θ q g q obs F u,u U A - X x,x ( θ ) Σ e -τs β x,x,θ,θ y (t- τ), y (t-τ) e φ u 3 tanh β d -k w, w Figure 3: Control Strategy for the Multi-steered general -Trailer ³ where φ = arctan w y () w y () is the angle of point (w,w ) with respect to the initial position of the tractor and φ d corresponds to its desired value. Note that the sign of the variable e φ indicates in which side of the desired trajectory the trailer is placed. In order to take into consideration the path generated by the potential fieldoverthetractor(point(y,y )), the desired value φ d it is proposed as, φ d =arctan y (t τ) y () y (t τ) y (), where the time delay τ is given by, τ = d + d +. (3) v d Remark Note that due to the normalization of the forces generated by the potential field (), the scalar velocity of the point (y,y ) is constant. Now, it is possible to define a desired value for the output β, of the trailer subsystem, in terms of the relative position of the tractor and trailer by, β d = ke φ, from what, a control feedback law for the trailer is given by, u 3 = α tanh (γ (β β d )), (4) where α, γ are positive constants. Figure 3 shows a block diagram of the complete control scheme (Tractor-trailer). Real-Time Experiments To evaluate the obstacle avoidance control strategy developed before a real-time implementation is made. The Figure 4: Prototype prototype was designed at the Mobile Robots Laboratory of the Mechatronics Section of the Electrical Engineering Department at CINVESTAV-IPN. Figure 4 shows a picture of the trailer system prototype. Due to the lack of sensors on the prototype it is necessary to assume the knowledge of the goal and obstacles position. The obstacles are located at q o() =(, ), q o() =(,.), q o(3) =(, ) and the goal was set at q g =(, ). The initial conditions of the vehicle was taken as x () =, x () =., θ () =.78, θ () =.78, β () =. The position of the vehicle is estimated by the kinematic model, the angular position of the motors of the wheels of the tractor are sensed by incremental encoders. The angle between tractor and trailer and the angular position of the motor on the axle of the trailer are sensed by linear potentiometers. The relationship between the linear and angular velocities of the tractor and the angular velocities of each wheel are given by, Ã! Ã! ω d u = T, ω i u with T = Ã r r L r r L!, where r is the radius of the wheels and L is the length oftherearaxleofthetractor. The parameters of the experimental prototype are, d =.48, d =.38, =.3,L=.44,r =.6. The parameters considered in the feedback control law (), (4) used for experimentation are: ξ =,η =,ρ =.,v d =.,α =.,γ =,k =6. 4

5 e u3 [rad/s] u [rad/s] u [m/s] V3 [V] V [V] u [m/s] V [V] 3. Trayectorias del tractor-trailer P R q. Evolucion de las señales de los Voltajes Figure : Trajectory for the trailer-like vehicle -.. Evolucion de las señales de control Figure 7: Voltage Signals Figure 6: Control Signals error (beta -betad) The trajectories followed by the tractor and the trailer are shown in Figure, where it is possible to see how the path of the tractor represent, as expected, a reference for a trailer. In Figure 6, the control u, u, u 3, applied to the vehicle are depicted. The velocity signals used to control the vehicle are transformed to voltages by using a classical PID internal loop. This is possible by assuming that the electrical dynamics is faster than the mechanical one. The voltages supplied to the motors are presented in Figure 7. In Figure 8 is shown the error β β d. 6 Conclusions Figure 8: Error evolution The obstacle avoidance problem associated to a trailerlike vehicle has been analyzed in this work. The study is done by considering a multi-steered general -trailer,

6 a general model that presents a steered wheel for the trailer. It is proposed a control strategy based on artificial potential fields that generate a trajectory that is followed by the tractor. This trajectory is considered as a reference for the control applied to the trailer. The performance of the control strategy was tested by realtime experiments. References [] Borenstein J. and Y. Koren (989), Real-time obstacle avoidance for fast mobile robots. IEEE Transc. Syst., Man, Cybern., Vol 9(No.), pp [] Cadenat V., R. Swain., P. Spuères, M. Devy (999), A Controller to Perform a Visually Guided Tracking Task in Cluttered Enviroment.Proc. of the IEEE/RSJ Intern. Conference on Intelligent Robots and Systems, pp [3] Ge S.S. and Y. J. Cui (), New Potential Functions for a Mobile Robot Path Planning. IEEE Trans. Robotics and Automat., Vol.6(No.), pp [4] Hirsch M.W. and S. Smale (974), Differential Equations, Dynamical Systems, and Linear Algebra. Academic Press. [] Khatib O. (986), Real Time Obstacle Avoidance for Manipulators and Mobile Robots. The Inter. Journal of Robotics Research, Vol (No.), pp [6] Krogh B.H. and C. E. Thorpe (986), Integrated Path Planning an Dynamic Steering Control for Autonomous Vehicles. Proc.IEEEInt.Conf.Robotics and Automat., pp [7] Orosco-Guerrero R., E. Aranda-Bricaire, M.Velasco- Villa (),Modeling and Dynamic Feedback Linearization of a Multi-steered N-Trailer. Accepted for presentation at the th. IFAC World Congress, Barcelona. [8] Tilove R. (99), Local Obstacles Avoidance for Mobile Robots Based on the Method of Artificial Potencials. IEEE Trans. Robotics and Automat., pp [9] Vidal-Calleja T. (), Generalización del método de campos potenciales artificiales para vehículos articulados. Tesis de Maestría. CINVESTAV IPN, México. 6