Sensor based trajectory following for nonholonomic ... - CiteSeerX

Proceedings of the 2003 IEEURSJ InU. Conferenceon Intelligent Robots and Systems Las Vegas. Nevada. October 2003

Sensor based trajectory following for nonholonomic systems in highly cluttered environment David Bonnafous, Florent Lamiraux

LAASKNRS

7, av. du Colonel Roche F-31077 Toulouse Cedex 4 - France First-Name.Lmtname@ 1aa.v.fr following it, according to its sensors to guarantee that it never bumps obstacles and reaches the goal. Previous work dealing with these issues has been done. In [6] the author presents an Intelligent Global Path Planner with Replanning (IGPPR) and he uses it in [71 on a real car-like mobile robot. IGPPR looks like D ' algorithm [9] and takes into account the nonholonomic constraints. In [lo] the authors describe a method used on a truly free-ranging Automated Guided Vehicle for industrial environments. They work in the velocity space of the robot (like the Curvature-Velocity Method (CVM)[SI)to stay as close as possible to the path planned and avoid collision. In [5] the authors describe an architecture for a carlike vehicule in wich they use Sensor-Based Manoeuvres (SMB) to achieve a mission. For example, the trajectory following SMB relies upon two skills: trajectory tracking and lane changing. So, to avoid an obstacle, the robot computes an avoidance trajectory in three parts: avoid the obstacle (i.e. lane changing), overtake the obstacle (i.e. trajectory tracking) and then go back to the initial trajectory (i.e. lane changing). [Z]addresses this problem for mobile manipulators. In 141, we have proposed a new and generic approach that reactively modify the path of a nonholonomic system. The trajectory of the robot is a curve q ( s ) in the configuration space, satisfying the kinematic constraints. On board sensors detect obstacles and deform the trajectory in order to remove collisions and to keep the nonholonomic constraints satisfied. The deformation process is based on a repulsive potential field that generates a direction of deformation compatible with the kinematic constraints of the robot. At each iteration, the current trajectory is deformed in the direction of deformation (Figure 2). Unlike previous methods our approach can deal with any nonholonomic system as soon as we know the system control vector fields and the mechanical kinematic bounds. The deformation process runs during motion. The trajectory is thus a resource shared between several tasks (Figure 3). These tasks need to be synchronized in order to avoid rolling on a part of the trajectory that is in collision or

A b s m t - I n this paper, we present a method to follow a planned trajectory in a highly cluttered environment

for complex nonholonomic systems. This method ensures, under some hypotheses, that the robot never collides. When a collision is detected on the trajectory, we compute a deceleration interval to stop the robot. while decelerating, we deform the trajectory in such a way that the trajectory is always feasible.

I. INTRODUCTION This paper deals with navigation of a mobile robot subjected to kinematic constraints (see Figure 1) in a highly cluttered environment.

Fig. 1. Our experimental mobile mbt: Hilare 1 and its trailer. The configumion space of this mbot is of dimension 4 q = (z,y,B,p). (z, y) is the position of the center of Hilare 2, 8 and 'p are respectively the orientations of Hilare 2 and of the oailer. This system is subjected to 2 nonholonomic constmints due to the robot and trailer wheels.

Our navigation approach is based on two steps: a motion planning step during which a motion planner computes a collision-free feasible trajectory and a trajectory following step during which the robot executes the motion planned. The inaccuracy of the map used by the planner, the erroneous localization of the robot and potentially unknown obstacles drive inevitably the robot to a collision with the environment if the motion is executed as such, especially when the planned trajectory has very low clearance (Figure 7). Indeed, the robot needs to adapt its trajectory while 07803-7860-1/03/$17.M) 0 2003 IEEE

892

2

and velocity depends on the trajectory as explained in Section 111-B. The paper is organized as follow: in section II we give the hypotheses necessary to solve OUT problem and the constraints we must take into account, in Section III we explain the algorithm we have developed, in Section IV we give some experimental results and we finish by a discussion in Section V. 11. HYPOTHESESA N D CONSTRAINTS

1) The robot is subjected to bounds on its linear and angular velocities ( q w ) and on its linear and angular accelerations (G, G): Fig. 2. obstacles generate a porential field that repulse the trajectory.

-Vmaz

I U S umm

(1)

-wmaz

5

<nar

(2) (3)

Wmaz

(4)

~

-&" 5 6 5 -&ma=

coltillon

2) the initial trajectory is a curve q(s)

delrclion

parameterized by s and satisfying the nonholonomic constraints of the system (see Figure 1 for notation). The velocities and accelerations of the robot along the trajectory depend on s and its time derivatives:

Uajecmq

loliowing

deformation

= m = d , ( s ) S

w = 8=du(s)s

(5) (6)

ir = d " ( s ) s + d : ( s ) 2

(7)

tl .... dm " r u m eioiusiraCcm

=

(z(s), y(s), S(s), ~ ( s ) )in the configuration space

:collisim detcdon

-

IW I

5

wmaz

ij

= d,(s)S+d:(s)2

(8)

+

Fig. 3. Three tasks access to the trajectory: the collision detection task computes which part of the trajectory is free, the trajectory following task moves along the trajectory and b e trajectory deformation task deforms the trajectov in order to remove collisions.

deforming a part of trajectory that the robot has already reached. The tasks are divided into two categories: 1) two real time periodic tasks with a high frequency (40Hz),the state feedback law and the trajectory following task, 2) two low frequency and variable duration tasks: collision detection and deformation process.

3) 4)

This paper presents a method for synchronizing the different tasks in such a way that: the robot simultaneously follows and adapts its trajectory, according to obstacles detected during the motion and the robot reaches the goal in bounded time.

5)

.

6)

Of course we want the execution of the motion to be as smooth as possible. Although the problem raised here might seem easy to solve, the kinematic complexity of the systems we deal with require adapted solutions. For instance, while following the trajectory, the robot cannot stop instantaneously. Unlike for simple robots,the interval of trajectory necessary to stop from a given position

where d,(s) = , / X ' ~ ( S ) ~ ' ~ ( s d,(s) ), = S'(s) and ' denotes the derivative w.r.t. s. The initial trajectory complies with constraints (1-4) for s = t, (i.e. d,(s) satisfies (1). d,(s) satisfies (2), dL(s) satisfies (3). d:(s) satisfies (4)). Inequalities (1-4) induce constraints over the time derivatives j . and s. Thus the trajectory following task cannot stop instantaneously but needs an intenral of collisionfree trajectory. the environment is static. We discuss in Section V how OUT algorithm behaves with moving obstacles. The tmjectory deformation process is iterative. It must be repeated several times IO remove a collision. We make no assumption about the duration of an iteration. This duration depends on the complexity of the environment. At any time, the abscissa s of the robot along the trajectory has to be outside the current interval of deformation. The trajectory following task must send periodically (25ms on Hilare 2) the desired position and velocity to the motion control module. 111. DESCRIPTION OF THE ALGORITHM

The trajectory is accessed by three different tasks (the motion control module is not described here):

893

3

the collision detection task called cCheck. cCheck uses the current trajectory, the abscissa s of the robot on the trajectory and a local perception of the environment to produce relevant information about the trajectory : - first configuration in collision qeoli = q(scoli), - hidden interval of trajectory (Figure 5). [s,tortrsend] the best interval of deformation. the trajectory following task called tFollow. tFollow uses this information to compute a abscissa satW along the trajectory where the robot will stop. This task also samples the trajectory to produce the periodic input to the motion control module. the trajectory deformation task called Deform. Deform deforms the current trajectory from sSt,,,tD,f to Send according to the obstacles detected. These tasks are repeated one after the other until the robot has reached the end of the trajectory. Figure 4 and the two paragraphs below explain how this tasks tun. In step (1) cCheck computes a set of relevant informations used by tFollow in (2) to compute the robot behavior b l followed by the robot at time tbl (at abscissa shl along the trajectory).Then, while the robot follow hl, we deform the trajectory, in (3). on interval intafdef(3) shown in the lower curve (int.ofdef(3) begins where the robot will stop while following bl). This is one loop. Another loop begins, in (4), when cCheck produces a new set of relevant informations used in ( 5 ) to compute the velocity profile b2 at time th2. At this time the robot (in abscissa sb2) speeds up to join this velocity profile and then follow it. During this time, in (6) we deform the trajectory on interval intafdef(6) that begin where the robot will stop with while following b2. Now we describe each task precisely.

-

morion 0"

.. .. . ..

.. .., ..

. . .. ,... .. .... .. ,:.. :.. ,:.. ,:. ,:,.:..,:. ,:.. ,:. ,:. ,:. *:. ,:.. ,:. :..

:..

;:

.

incor~&f(3)

.

U: i"lsfder(6)

velaity profile for Mavior I (hl) "elailypmflleforbsha"ior2 0 2 ) _... velwily profile of tnc &I intervnl or .idmation

-

Fig. 4. "hose figures explain how the algorithm .*.orb(two complete loops me shown) see Section 111 for explanations. In the upper figure, we can see the sequence of events. The last line "motion conmol"is here to remind you that the motion control feedback loop runs all the time. The lower curves show &e velocity pmfiles (call the whavior) of the robot wfi F and the interval where the deformation are applied. We can see that the robot will always stop before the beginning of this interval.

A. ccheck: compure a sef of relevant informutions

Each configuration on the trajectory can be in one state

among three: free state, collision state or hidden state. When the minimum distance between the obstacles and a configuration is lower than a threshold the configuration is considered in collision. When a configuration cannot be entirely seen from the robot current position, it is hidden (see Figure 5). Otherwise the configuration is free. cCheck looks, along the trajectory and from the current position of the robot, for the first configuration in collision q ( s , d i ) and the first hidden configuration q(Shid), using obstacle detection sensors (laser telemeter). If a collision is found, cCheck determines an interval of deformation [sstart, send] containing scoii using empirical rules. B. rFollow: where

IO

stop and how

IO

Equalities (7-8) together with inequalities (3-4) imply constraints over s for a given s and i;. These constraints are of the following form: Smin(S,S)

Notice that the velocity constraints (1-2) are satisfied as soon as 0 < S < 1. tFollow periodically computes s and S (0 < i; < 1) in order to stop or speed up when necessary and to satisfy the above constraint over s. For instance let us assume that the robot is at abscissa so and velocity io on the trajectory and that a collision requires the robot to stop at abscissa SI. To ensure that SI will not be passed, and to stop in minimal time, we need to integrate backward the differential equation

slow down 7

In this section we explain how to slow down or speed up the robot along the trajectory and how to compute an interval of deceleration from an abscissa s with a given velocity S.

I s s B,,,(s,S)

6 = Smin(S,.i) from s = SI, . i= 0. This operation is performed in the 894

4

First, we notice that if 0 5 6

< 1 and

and

This later inequality ensures us that the linear acceleration satisfies cons&t (3). B~ analog computa~ons,we can establish that if

Fig. 5. On our robot, we use a laser telemeter that scan the environment horizontally with a range of 180°. From the current pasition cCheck looks for the f i s t configuration hidden by the detected obstacles.

IS(

phase plane (s, 6) first introduced in [I] in which a curve s ( t ) is represented by a relation between s and S. In the phase plane, the above equation becomes

5 (1 - i2)min(-,em, %a=

4 -) " Wmaz

constraint (4) is also satisfied. Notice that coefficients d,(s), &(s) and their derivatives have disappeared in this latter inequality. In the phase plane this inequality becomes

dj. _ = -1dS _ = Smin(s,9) ds 9 dt s Let s(s) be the solution: .?(SI) = 0. If tFollow is moving along the trajectory with constant speed so, the task has

1-2

dB

1,lI A- i

to start decelerating along S(s) at the first abscissa sz satisfying t ( s 2 ) = so (see Figure 6).

where A = min(e,2u&). The lower bound of this 4 = - A Y can be integrated and gives inequality d da

B=

41- (1 - ioz)

e24-so)

(9)

This curve in the phase plane enables us to compute very quickly an interval sufficient to stop. Again, if we want to stop at abscissa SI,and if the robot is at abscissa SO with velocity So, we solve

so

SZ

in s2 to determine at which abscissa sz we need to start decelerating (Figure 6):

SI

Fig. 6. Trajectory abscissa s ( t ) is represented in the phase plane (a, S). Acurve in lhis plane represents a differential equation S = f(s) that by integration yields s ( t ) . To stop in minimal time, we need to integrate backward the deceleration ending at SI with S = 0 and to slan following this cwve at abscissa sz verifying ~ ( S Z )= do.

sz

1

= SI + -ln(12A

si)

Now, tFoUow can compute a velocity profile as shown on the bottom of Figure 4 and an interval to stop [so, SI].

In addition to the velocity profile, the tFollow task computes where the robot will really stop sstop. If the first hidden configuration is q(shid) and the configuration in is q(scdt), Sstop is computed as follow:

Moreover if SO > $(SO), the robot will not be able to stop along the trajectory before SI without violating an acceleration constraint. The computation of the minimal interval necessary to stop the robot along the trajectory thus requires to integrate a differential equation. This operation requires a time proportional to the current velocity of the robot and might take more than one tFollow period. For this reason, we propose a method to compute in constant (and small) time an upper-bound of the interval required to stop.

min(shid. scold if SI > min(shid,Seed SI,otherwise (10) As explained in Section III-A, the robot never follows a part of the trajectory that has never been seen free. Sat*

895

=

5

C. tDefom: deform the trajectory

This task is in charge of deforming the trajectory. Thanks to the previous section (III-B) we know that the robot will stop at abscissa sstOpand the cCheck task (IEA) computes the end of the interval where the deformation must be applied Send. So the deformation process is applied on the interval [sgtop, Sesd]. There are three steps: 1) compute the potential field generated by the obstacles detected, 2) compute the deformation, 3) and then apply it on the trajectory. These steps are explained in 141.

IV. EXPERIMENTAL RESULTS The experiments presented here illustrate the kind of trajectories the robot can follow with the nonholonomic trajectory deformation method running with the algorithm presented in this paper and the effect of how we compute satart (see section III-A). During this experiments, the relevant information computed by cCheck task (section III-A) is the first configuration q(s,,[[) at a distance below 7cm from the obstacles. is computed as the configuration The abscissa sStOrt q(satart) at a constant distance from q(s,,n) along the trajectory. The laser telemeter precision used is about 15mm with an angular resolution of 0.5". The trajectory planned is shown in Figure 7. Its length is about 30m with two narrow passages (the doors are approximately 1.30111 width, the robot 0.75111). The maximum velocity of the robot are 0.12mjs and O.lSTad/s, the acceleration are O.lm/sz and 0.1rad/s2. After few meters on the trajectory, the perceived ohstacks are in collision with the planned trajectory SO the robot need to deform it. The curves in Figure 8 show the velocityRate (iin section III-B) and the runtime of cCheck, tFollow and tDeform while the robot is in the area shown on figure 7. The robot follows the planned trajectory two times. The first time q(sat,,t)is chosen at 3.0m along the trajectory before q(sco[l)(Figure 8.a). The second time q(sstort) is chosen at 2.0m along the trajectory before q(scol[) (Figure 8.b). We can see on this curves: 1) each step of our method first (I) cCheck looks for q(sco[[) and computes sstart then (2) tFollow computes the behavior of the robot while following the trajectory. If necessary (3) Deform deforms the trajectory. Those operations (1). (2) and (3) are repeated several times to avoid a collision. 2) cCheck and Deform tasks are time consuming (>25ms) and have a variable duration that depend on the complexity of the environment. 3) the effect of how we compute sQtnrt can be seen on the velocitybte curve. In the first experimentation (Figure 8.a) sStortis chosen nearer the robot, 3.0m

Fig. 7. The planned tmjeclnq (the beginning is at the bottom) and the area where the data were acquired.

The velocityRate is S in the past sectinn, The three others data are boolean state that show the runtime of the cCheck. ffdlallow and tDeform task.. In Figure 8.a q(s,,.,t) is chosen at 3.0m along the mjeclory before q(sc0il) and 2.01" in Figure 8.b. Because the le@ of the deformation interval is longer in figure 8.a than in 8.b the robot need 10 slow down often.

Fig. 8.

896

6

IEEE. Internationnl Conference on Robotics and Automarion, Washington D.C., May 2002. E Lamiraux and D. B o M ~ ~ o ~Reactive s. trajectory deformation for nonholonomic systems: Application to mobile robots. In International Conference on Robotics and Automation [3], pages 3099-3 104. C. Laugier, T. Fraichard, Ph. Gamier, I.E. Paromtchik, and A.Scheuer. Sensor-based control architecture for a car-like vehicle. In Autonomous Robots, volume 6, May 1999. L. Podsdkowski. Path planner for nonholonomic mobile robot with fast replanning procedure. In International Conference on Robotics and Automation, Leuven, Belgium, May 1998. IEEE. L. Podsdkowski, J. Nowakowski, M. Idzikowski, and I. Vizvary. A new solution for path planning in partially known or unknown environment for nonholonomic mobile. Robotics and Autonomous Systems, 34:145-152, 2001. R. Simmons. The curvature-velocity method for local obstacle avoidance. In Inremotional Conference on Robotics and Automation, Mineapolis, USA, 1996.

Fig. 9. Hilare 2 and iU trailer at the end of the area where the data were acauired.

before q(scoli), than the second one (Figure 8.b), 2.0m before qco,,.So the robot slows down often to stop at qStaTtcomputed by cCheck.

v.

IEEE. A. Stentz. The focussed d* algorithm for real-time replanning. In International Joint Conference on Artficial Intelligence. IEEE, 1995. E Xu, H. Van Brussel, M. Nuttin, and R. Moreas. Concepts for dynamic obstacle avoidance and their extended application in underground navigation. Robotics and Autonomous Systems, 42:l-15, 2003.

CONCLUSION AND FUTURE WORK

With the algorithm described in this paper, the robot can follow a planned trajectory in a highly cluttered static environment including unknown obstacles without collision. Because the deformation method is based on potential fields generated by perceived obstacles there are some situations where it fails to deform the trajectory until all collision have disappeared, for example when a door is open in the plan but closed in the reality. In this situation, the robot stops in front of the closed door needlessly deforming the trajectory. We are working on criteria to automatically detect such situations in which global replanning is necessary. Our method bas been devised for static environments. However experimentation showed that the method is still valid for sufficiently slow moving obstacles. The maximal obstacle velocity the method can deal with depends on cCheck frequency. Notice that in highly cluttered environments, the collision detection along a sampled trajectory requires to test each sample configuration with the obstacles detected. The frequency of cCbeck depends a lot on these computations. VI. REFERENCES [I] J.E. Bobrow, S . Dubowsky, and J.S. Gibson. Timeoptimal control of robotic manipulators along specified paths. Infernational Journal on Robotics Research, 4:3-17, 1985. [2] 0. Brock, 0. Khatib, and S. Viji. Task-consistent obstacle avoidance and motion behavior for mobile manipulation. In International Conference on Robotics and Automation 131, pages 388-393. 897