Formalizing Behavior-based Planning for Nonholonomic Robots

1 Introduction

The problems of obstacle avoidance and path planning with autonomous mobile robots have been studied in various settings. [Lumelsky, 1987; Lozano-Perez, 1980; Khatib, 1986; Koditschek, 1987; Shahidi et al., 1991].

·This research was supported in parts by grants from the National Science Foundation's Engineering Research Cen­ ters Program: NSFD CDR 8803012, the AFOSR Uni­ versity Research Initiative Program, under grant AFOSR90-0105, and AFOSR-F49620-92-J-0500, from NSF(IRI9306580), ONR (N00014-J-91-1451), AFOSR (F49620-931-0065), the ARPA/Rome Laboratory Planning Initiative (F30602-93-C-0039 and by ARI (MDA-903-92-R-0035, sub­ contract through Microelectronics and Design, Inc.)

These approaches either assumed that the planner had to have substantive a priori information about the lo­ cation, shapes and sizes of obstacles, or assumed that the constraints on the robot (geometric and kinematic) were holonomic or integrable. In practice however most real world robotic systems have little a priori informa­ tion about the shapes and size of the obstacles and in addition include kinematic constraints that are nonholo­ nomic (see section 1.1 for more details on nonholonomic constraints). A few examples of nonholonomic systems are, models of a front wheel drive car, dextrous manip­ ulation or assembly with robotic hands, attitude control of a satellite etc. As traditional path planners assume ar­ bitrary motion they cannot be applied to nonholonomic robots as they result in nonfeasible trajectories i.e. tra­ jectories that do not satisfy the constraints on the con­ figuration variables.

More recently, researchers have been examining non­ holonomic path planning in the presence of obsta­ cles [Laumond, 1990; Barraquand and Latombe, 1989; Mirtich and Canny, 1992; Hu and Brady, 1995]. How­ ever, while most of these planners provide some excel­ lent results they are quite rigid in the choice of control laws used to steer the robots and often do not exploit the control laws available in control literature, for example [Murray and Sastry, 1990; Sussmann, 1991; Coron, 1992; de Wit and Sordalen, 1992]. They also assume near com­ plete a priori information about the world and only ac­ count for small changes in the environment.

On the other hand behavior-based robots [Brooks, 1986; Arkin, 1992], that use real-time "sensor-based" ap­ proaches, have been able to handle more realistic mod­ els of sensing. Perhaps best known among this work is Brooks' use of task achieving behaviors as the primary level of task decomposition. He introduces the concept of a subsumption architecture which is essentially a struc­ tured and layered set of behaviors with increasing lev­ els of competence. These "reactive" systems typically exploit domain constraints, using clever algorithms to allow fast processing of complex sensor information ( cf. [Horswill, 1993]). Although this approach has significant advantages from the point of view of architectural design and programming flexibility, it has resisted mathemati­ cal formalization and is not amenable to tests for opti­ mality. Comparing two sets of behaviors, even within the same task, is complex and the domain-dependent nature

P.S. Krishnaprasad

krishna@isr. umd. ed u Dept. of Electrical Engineering of the solutions can cause these systems to be basically incommensurate - one may fail some times, one may fail at other times and comparison is difficult. In addition nature of these systems makes the improvement of be­ haviors over time difficult. 1

The inability to integrate the underlying geometry with real time sensor information stems from the lack of a powerful enough framework to integrate the two approaches. This paper is a step in the direction of pro­ viding such a framework, integrating features of reactive planning with modern control-theory-based approaches to steer nonholonomic robots. As the first step, we in­ troduce a motion description language, MDLe, that pro­ vides a formal basis for robot programming using behav­ iors, and at the same time permits incorporation of kine­ matic models of robots given in the form of differential equations. The structure of the language MDLe (based on Brockett's MDL[Brockett, 1990]) allows descriptions of triggers (generated by sensors) in the language. Feed­ back and feedforward control laws are selected and exe­ cuted by the triggering events.

MDLe is particularly well suited to the demands of nonholonomic motion-planning with limited range sen­ sors. As nonholonomic robot models impose limitations on stabilizability via smooth feedback [Brockett, 1983], the ability to piece together open-loop and closed-loop trajectories. becomes essential. MDLe enables one to de­ scribe such piecing together in a systematic manner. As an example of the strength of this language, we show that it can be used to support a reactive planner for nonholonomic motion planning in the presence of obsta­ cles, using limited range sensors for obstacles detection. In addition, the system assumes no a priori information on the location and shapes of the obstacles.

In section 1.1 we give a brief description of non­ holonomic constraints that motivates the mathematical model of the motion description language. In section 2 we present details of MDLe, a language that provides a formal basis for specifying behaviors. An example of a path planner using the formalism of the language is pre­ sented in section 3. We then describe how we can update world models in section 3.4 and provide examples of the system's performance. Section 4 includes final remarks and future directions for research.

1.1 N onholonomic Constraints

In addition to being subject to geometric constraints many robotic systems are subject to velocity constraints. These velocity constraints are represented by relations between generalized coordinates and their velocities, and are written in matrix form as

where q E m.n determines the generalized coordinates, q are the generalized velocities and A(q) E Rkxn rep­ resents a set of k velocity constraints. We also assume that A(q) has full row rank. Since a kinematic constraint restricts the allowable velocities and not necessarily the configuration, it is not always possible to represent it as an algebraic constraint on the configuration space. A kinematic constraint is said to be integrable if there ex­ ists a vector valued function h : Q -+ Rk such that

1 However, see [Stein, 1994] for an approach to map learning.

An integrable kinematic constraint is hence equivalent to a holonomic constraint. Kinematic constraints that are not integrable are said to be nonholonomic. The constraint (1) defines a (2n - k) dimensional smooth manifold M = {(q,q)IA(q)q = 0}. These kinematic con­ straints generate a set of constraint forces so as to ensure that the system does not move in the direction of the rows of the constraint matrix (see fig 1) . In mechanical systems such constraints are often those of rolling with­ out slipping, conservation of angular momentum etc. If the controls u(t) E Rm satisfy n- k ~ m < n then the kinematics are sufficient to model the system and (1) can be written in the form of a drift free control system

with state x(t) and control u(t), and each bi is a vector field. Often such drift free (completely) nonholonomic systems are controllable (cf. [Murray et al., 1994]). In the rest of the paper we assume that a nonholonomic robot is modeled by differential equations of the form (2), and the system is controllable.

2 Language for Motion Planning

To formalize behaviors that control a nonholonomic robot, we must provide a formal basis for the motions (in terms of control laws) and a basis for such items as receipt of sensing information and behavioral changes (interrupts) upon receiving certain such items. MDLe provides a formal language for specifying these items in a kinetic state machine which can be thought of as the continuous analog of a finite automaton. In the frame­ work of MDLe these kinetic state machines are governed by differential equations of the form

2 The concept of kinetic state machines was first intro­ duced by Brockett [Brockett, 1990].

where

and each b; is a vector field in Rn.

We now define the atoms of the motion language as triples of the form (U, €, T) where

(where u; is as defined earlier). To formalize the notion of behavior change caused by the presence of obstacles, we define

a boolean function, T E R+ and s(·) : (0, T] -+ Rk is a k dimensional signal that represents the output of the k sensors. € can be interpreted as an interrupt or trigger to the system which is activated in a case of emergency (for example, we might have such an interrupt used when the robot gets too close to an obstacle). We denote T, the time (measured with respect to the initiation of the atom) at which an interrupt was received i.e. € changes state from 1 to 0.

If at time to the kinetic state machine receives an in­ put atom (U, €, T) the state will evolve governed by the differential equation (1), as

If the kinetic state machine receives an input string (U1, 6, TI) · · · (Un, €n, Tn) then the state x will evolve according to

Hence we may denote a kinetic state machine as a seven­ tuple (U,X,Y,S,B,h,€), where

u = C 00 (R+ X RP; Rm) is an input (control) space,

X= Rn is the state space,

Y = RP is an output space,

S c R k is the sensor signal space,

B is an Rnxm matrix (kinematic constraints matrix), h : X -+ Y maps the state space to the output space and

€: S-+ {0, 1} maps the sensor output to the set {0, 1}.

A major point of departure from Brockett's MDL, is that we find it necessary to bring input scaling into the pic­ ture as this provides considerable flexibility and also pro­ vides a basis for the comparison of behaviors (as will be discussed later) .

Definition: Given an atom, (U, €, T), define (o:U, €,/3T), o: E R, {3 E R+ as the corresponding scaled atom and denote it as (o:, {3)(U, €, T).

Definition: An alphabet I: is a finite set of atoms, i.e (U, €, T) triples. Thus I: =

{(U1, 6, T1), · · ·, (Un, €n, Tn)} for some finite nor equiv­ alently I: = { a 1 , ···,an} where a; denotes the triple (U;,€;,T;), such that a; =f. (o:,{3)(aj) o: E R,{3 E R+ and i = 1, · · · n, j = 1 · · · n.

To use this scaling we introduce the notions of extended alphabet and language. (To simplify notation in the rest of the discussion we denote the scaled atom (1, 1)a; sim­ ply by a; .)

Definition: An extended alphabet I:e is the infinite set of scaled atoms, i.e. triples (o:U, €, {3T) derived from the alphabet I:.

Definition: A language I:* (respectively I:;) is defined as the set of all strings over the fixed alphabet I: (re­ spectively extended alphabet I:e)·

This now lets us formally define a behavior as a string in this language, and also to define the complexity (length) of a behavior as well as describe its duration in a particular planning sequence:

Definition: A behavior 1r is an element (i.e. word) of the extended language I:; 3 . For example, given an alphabet I: = { a1, a2}, a behavior 1r could be the string ( 0:1, f3I)a1 ( 0:2, f32)a2 ( 0:3, {33 )a1 .

Definition: The length of a behavior denoted by l1rl is the number of atoms (or scaled atoms) in the behavior. Definition: The duration T(1r) of a behavior

executed beginning at time t0 is the sum of the time intervals for which each of the atoms in the behavior was executed. That is,

Using this formal definition of behavior, we can now define motion-planning problems in terms of sequences of behaviors that enable the robot to reach a goal:

Definition: Given a kinetic state machine and a world­ model, a plan r is defined as an ordered sequence of behaviors, which when executed achieves the given goal. For example a plan r = { 1r31r1 1r n · · ·} could be generated from a given language where each behavior is executed in the order in which they appear in the plan. The length of a plan r is given by !rl = L: !1r;l and the duration of the plan is given by T(r) = L:T(1r;) . In a particular context there may be more than one plan that achieves a given goal.

At first, it appears that, to generate a plan to steer a system from a give initial state xo to a final state x 1 re­ quires complete a priori information of the world, which is not available in many instances of path planning. In the absence of such complete a priori information about the world W, the planning system has to generate a se­ quence of plans based on the limited information about W that it has, which when concatenated will achieve the required goal. We refer to these plans that are gen­ erated on limited information to achieve some subgoal as partial plans rp.

Given a system which can use a set of partial plans to reach a goal, one obvious way to produce a plan to steer the system from a given initial state x0 to a final state x 1 is to use these partial plan to reach the final state, and then to store the result as a plan consisting of: r = -P-P -P ~p r1 r2 ... r n where ri is the partial plan consisting of only those behaviors and atoms in each behavior that have been executed for t > 0.

3 To account for constraints one might limit behaviors to lie in a sub language B C r:: . This will be explored in future work.

The length of a plan is given by lfl = 2:7=1 l1ril and the time of execution of the plan is given by T(f) = 2:7=1 T(1ri).

(Note that as a partial plan is generated with limited information of the world, not all the behaviors and not every atom in a behavior generated by the partial plan may be executed at run time because there may be sev­ eral CT's with the same Cs. For example, Let us consider a behavior 7r; = 0"10"20"3 · · · O"n. Let us assume that the atom 0"1 is interrupted by 6 at f. Now as explained earlier 0"2 will begin to execute. But if 6 = 6 the atom CT2 will not be executed and (depending of course on 6) 0"3 will begin to execute.

Given an algorithm that generates a plan r we define a candidate measure of performance e(r) of the plan as

where r is a normalizing factor having the units of time. (One need not limit oneself to such additive combina­ tions although this is the only case used here.)

Defining a performance measure for a path planner is a rather difficult task as it is largely dependent on the goal the robot seeks to achieve. Some path plan­ ners use the total time to achieve the goal as a measure of performance. In many situations one might be inter­ ested in not only the time but also on the smoothness of the path traversed or the number of times switching between different controls was necessary. For example consider the task of parallel parking of a car. One might be able to achieve the goal by using only open-loop con­ trols but switching between them at regular intervals, hence possibly reducing the time to achieve the goal but compromising on the smoothness of the path. On the other hand if one uses a time dependent feedback law, the same task could be achieved, possibly by moving along a smooth trajectory but this time taking a longer time to achieve the goal. This indicates a trade-off be­ tween two competing requirements which is captured by the performance measure (4).

We now define the optimal performance of a plan as

Here the minimization is performed over the subset of plans generated by the subset B of admissible behaviors.

Due to space limitations, the above has been necessar­ ily terse and short on examples. For more detail and for examples of kinetic state machines, behaviors and per­ formance measures the interested reader is referred to [Manikonda et al., 1995].

3 Path Planning and Obstacle Avoidance

Using the language defined above, we now present a plan­ ner which allows us to do task-planning and obstacle avoidance for a nonholonomic robot with limited range sensors. As should be clear from the above, under our framework this involves the generation of partial plans given local sensor information, such that in a global sense the robot is steered towards the goal. Although in gen­ eral the partial plans generated will largely depend on the choice of the state machine (the differential equa­ tions governing the robot and the sensor information), in this section we describe a fairly general purpose plan­ ner and its implementation details in MDLe. The task of the planner is out lined as follows:

1. Interpret local sensor information to generate a "control point" and an obstacle free neighborhood containing this "control point" to which the robot is to be steered.
2. From the given alphabet select atoms (U, ~' T) that could be used to steer the robot (in general, depend­ ing on the richness of the alphabet (:E), there could be more than one behavior to steer the robot to the control point).
3. Calculate the scaling factor a (crucial, as it deter­ mines the speed of the robot). Having calculated a, calculate or approximate /3, the duration for which each atom is to be executed.
4. Generate an optimal partial plan, by minimizing the performance measure (8). The minimization is per­ formed over the admissible behaviors.
5. Execute the partial plan and update runtime infor­ mation regarding actual time of execution of behav­ iors in the partial plan, sensor information etc.
6. Given an updated world and partial plans generate an optimal plan.

Planning is being done at two levels - global and lo­ cal. For local planning, obstacle free (non)feasible paths are generated using potential functions assuming that the robot is holonomic. A partial plan (feasible path) is then generated that obeys the constraints in the config­ uration variables. As feasible trajectories are only ap­ proximations to the trajectories generated using poten­ tial functions, collision with obstacles could occur while tracing them. While the robot is in motion, collisions are avoided by using the sensor information to trigger interrupts as described previously.

At a global level heuristics, along with the world map generated while the robot is en route to the goal, are used to solve the problem of cycles. In the remainder of this section we describe this planner in more detail.

Critical to the planner is sensor ·information (location and calibration of sensors), the generation of "control points" and obstacle free neighborhoods, and selecting of atoms. Each of these is explained in some detail in the following subsections. One should note here that the planner could be used with most nonholonomic robot, by selecting the corresponding alphabet and associating rules with the selection of atoms. In our simulations we have assumed that the robot is modeled along the lines of a unicycle.

3.1 Location and Calibration of Sensors on the Robot

For purposes of obstacle detection, ranging and map­ ping, the planner assumes that the robots are provided with infrared/ultrasonic sensors located as shown in the Fig. 2. Bump sensors are located along the body of the robot. Optical encoders mounted on the motors pro­ vide position and velocity feedback. The infrared sen­ sors are classified as Si eye if they are located in the eye, as S' body if they are located along the sides or as Si tail if they are located at the rear of the robot . The sensors return distance information and we shall refer to the dis­ tance ~nformation returned as pi eye or pi bod depending on wh1ch sensor detects an object along its fine of sight. The sensors are calibrated such that the maximum range of each of these sensors is on the boundary of a circle of radius Rmax centered at C. The sensors located at the rear of the robot are normally turned off and are turned on only when the robot is reversing. While reversing they function in a manner similar to those of the eye.

Definition : The obstacle free disk BoJd( C, PoJd) is de­ fined as a disk of radius Pofd (Pofd :::; Rmax) centered at C (see Fig. 2) such that there are no obstacles in this disk, i.e. Pofd = min[pi eye' pi body' pk taiz], i = 1, ... n, j = l.. .n &k = l...n. Any trajectory that lies within this ball does not intersect the boundaries of obstacles.

As the distance between the robot and the obstacles de­ creases, due to the nonholonomic constraints steering might become more difficult and the planner might need to adopt a different control scheme to steer the robot. Let Rcrt :::; Rmax denote the radius of the ball centered at C such that the planner needs to use a different set of controls to steer the robot from X 0 to Xf E 8Pofd, the boundary of the obstacle free disk. (Rcrt can be determined empirically or analytically depending on the robot.)

Hence the problem of generating partial plans reduces to (i) planning in an obstacle free disk with Pofd > Rcrt and (ii) planning in an obstacle free disk such that Pofd :::; Rcrt· Each of the cases is discussed later.

3.2 Navigation Task Composition

The task of navigation and obstacle avoidance can be decomposed into a number of subtasks each being exe­ cuted at a certain level in the control architecture. Refer to Fig. 3 for a diagram of task decomposition.

Read sensors involves reading the distance informa­ tion returned by the sensors and determining the the largest obstacle free disk centered around the robot such that the trajectories that lie in this disk are guaranteed not to intersect with the boundaries of the obstacle. In the implementation of the control strategy, the sensors are read continuously and distance information and Pofd (the radius of the obstacle free disk) are updated into a global variable which is used to update the world and is also used by execute plan as an input to the interrupt function~· (Observe that under our approach we model the world as a network of obstacle free regions rather than mapping the obstacles.)

Update world involves periodic addition of nodes to the network. In the actual implementation of the planner we differentiate between the local world and the global world. The local world is represented as a linked list of intersecting obstacle-free disks, each of which is added to the list in the order they were visited over some finite interval of time. Each node contains information regarding the sensor distance information and Pofd·

Partial plans use this information to generate behav­ iors that steer the robot to the boundary of the disk. Af­ ter the complete or partial execution of the partial plan, the robot generates the next partial plan based on the sensor information available at. that instant. Note that even though the sensors are continuously being moni­ tored, a new node is added to the list only when a new partial plan needs to be generated. Hence as shown in Fig. 4 the world is updated at time instances correspond­ ing to the points A, B, C and so on. The global world is obtained after patching together local world information and using the clean up operation to remove redundant information.

Generate Plan/Partial Plan - This involves the interpretation of the sensor information, and language to generate a sequence of behaviors that will steer the robot from its current location to a desired location. If a partial plan is being generated the desired location lies on the boundary of the obstacle free disk. The genera­ tion of plans/partial plans is discussed in further detail in subsections 3.3 and 3.4

In the implementation of the planner, the data struc­ ture of a plan is a linked list of atoms, each atom being represented by a structure that has information regard­ ing the scaling factors (a, /3), the interrupt function ~, the maximum time of execution of the kinetic state ma­ chine, the inputs (controls) and a pointer to the kinetic state machine. As mentioned earlier it is possible that while executing a partial plan only some of the behaviors may be executed as planned, some may be executed only for a fraction of the the intended execution time and oth­ ers may not be executed at all. Update Plan updates the fields of each atom of the partial plan after it has completed its execution. Once the plans/partial plans have been generated, these plans have to be executed. Execute Plan involves decomposition of the plan into behaviors and further into atoms. The kinetic state ma­ <;:hine is allowed to evolve as explained in section 2, equa­ tion 2. Fig. 5 shows some paths generated by the plan­ ner for a robot modeled along the lines of a unicycle. 4 It is important to note that while the plan is being ex­ ecuted the sensors are being continuously scanned and are present in a low level feedback loop hence preventing any collisions with obstacles.

As the local world is a list of nodes, each of which is added to the list as the robot generates partial plans it is possible that while generating a plan the robot may have to revisit a node, and hence introducing redundant in­ formation into the world. Cleanup keeps track of these redundancies in the map and deletes/adds nodes in the list.

3.3 Planning in the Obstacle Free Disk

To find the best direction of travel in the obstacle free disk we use the approach of potential functions. As in the earlier work on path planning with potential func­ tions, the idea behind our construction is based on elec­ trostatic field theory - charges of the same sign repel and charges of the opposite sign attract in accordance with Coulomb's law. Hence we assign a positive charge dis­ tribution to the obstacles and the mobile robot and a negative charge distribution to the goal. The idea is to construct a vector field which will give the best direction of travel based on the location of the obstacles and the goal.

4 It should be pointed out here that the obstacle-free disks generated by the planner violate the exact definition given above, but this is . because in the simulator we have used only sensors of the eye to generate obstacle-free disks. For now, those obstacles that are not detected by the sensors are treated as being in the blind spots of the robot.

The robot is approximated to a point robot and as sensors can detect only points on the boundaries of the obstacles that lie in their line of vision, we treat obsta­ cles as a collection of point charges and assign charges to them depending on which sensor detects them. The intersection of the resultant gradient field with the cir­ cumference of the obstacle free disks gives the desired location to which robot is to be steered. (One should observe here that unlike earlier approaches the gradient field is not directly used to steer the robot. As integral curves of the resultant gradient field may not result in feasible trajectories we use the resultant gradient field only to determine the scaling factors and x 1 on the cir­ cumference of the obstacle free disk.)

Once the initial and desired final state of the robot is known control inputs are chosen from those available in the language to generate feasible trajectories to steer the robot from the initial location to the final desired location. If more than one such control achieves the task then the performance measure can be used to select the optimum one.

As we are using a kinematic model of the robot an underlying assumption is that the robot is moving at low velocities and we can bring the robot to a halt simply by turning off the controls. To determine scaling factors, which are directly related to the velocities if the inputs to the equations governing the motion of the robot, we use the sum total of both the attractive and repulsive forces is used to determine the bounds on the velocities and hence the bounds on the scaling factors. A simple example of such a function is given by

where Ia and /r are the net attractive and repulsive forces acting on the robot . Observe that when the robot is close to either the obstacle, the goal or both, it moves with a lower velocity hence making the kinematic model more realistic.

By intelligently choosing weights on the charges (see [Manikonda, 1994] for more details)_ we can ensure that the robot either avoids the obstacle or gets close enough to an obstacle such that Pofd ~ Pert in which case it traces the boundaries of obstacles to a point where it finds an edge or is heading in the direction of the goal.

Remark :It is important to mention here that as we are making no assumptions on the location sizes or shapes of the obstacles guaranteeing the existence of a path is very difficult, though empirical results have shown that if a path exists the robot has more often than not found it. More importance here is stressed on the ability of the planner to integrate real time sensor information with control-theory-based approaches to steer nonholonomic systems in a systematic way. As mentioned earlier non­ holonomic robots impose limitations on stabilizability via smooth feedback and the planner developed under the framework of the language provides an elegant way of piecing together of various control strategies.

Tracing Boundaries The real behavior-based aspects of our planner can be seen when tracing the boundaries of obstacles. Planning in B(C,Rcrt) is a closed loop planning strategy which essentially results in a trace be­ havior that traces the.boundaries of the obstacles. Given the limited sensor and world information it is probable that the direction of trace may have been wrong. Hence we use a heuristic function l(x) = D(Xrobot, Xinit), the euclidean distance between Xrobot and Xinit (the point at which the trace behavior was started) as an estimate of how far the robot has strayed from the goal. The robot traces the boundary as long as I < Is where Is is some permitted distance from where the trace behavior was started. If I > Is then we retrace path and trace the boundary of the obstacle in the opposite direction. If terminal conditions for the trace are not met, we set Is = o:ls, o: > 1, and repeat.

Remark: Retracing a path under this framework is a rather simple task. Observing that the system is a drift free system, retracing involves executing the past n par­ tial plans in a reverse order with ( -o:) scaling factor.

3.4 World Model Update

Once the robot has explored the environment using lim­ ited range sensors, it is natural to expect the robot to generate plans of a better performance if it has to repeat the same task or move to goal that lie in the explore re­ gions. We suggest a "learning algorithm" that improves the performance of a plan to bring it closer to optimal.

As described above, the plan to steer a robot consists of a sequence of partial plans, where each partial plan steers the robot in some obstacle free disk of radius Pofd· In the rest of this section let us denote each of the obsta­ cle free disks which were used to generate the ith partial plan as Bi and the ith partial plan as rf. Further let us assume that n such partial plans were generated. Once the plan has been generated the planner makes the fol­ lowing observations.

(i). If Bi c Bj, i = 1,···n,j = 1,···n (i.e. Bi is contained in Bj) then obviously Bi contains redundant information. Thus if Bi c Bi+I the partial plan rf, that steers the robot from ci to ci+l' where ci is the center of the obstacle free disk, and partial plan rf+l that steers the robot form ci+l to ci+2 can be replaced by a partial Plan fP that steers the robot from ci to ci+2· Since ' ~ Bi c Bi+l it is obvious that 8(ff) < 8(rfrf+l)

(ii). Observe that since Ci+1 lies on the boundary of Ci, we are guaranteed the existence of a trivial nonfeasi­ ble trajectory (the straight line joining ci with ci+2) that lies entirely in Bi U Bi+l i.e. the obstacle free area enclosed by these two intersecting obstacle free disks. Hence if there exists a partial plan rf that generates a feasible trajectory that can track this nonfea­ sible trajectory and lie entirely in Bi U Bi+l such that e(rf) < e(rfrf+l) we can replace rfrf+l by rf (iii) After the execution of (i) and (ii) we now have par­ tial plans that steer the robot from Ci to Ci+i, j E {2, · · · n} such that the trajectory lies entirely in U!+i Bi. The planner now explores the possibility of find­ ing (non)feasible trajectories from Ci to Ci+i+k, j E 2, · · · n, k = 1, · · ·, n such that these trajectories lie en­ tirely in U~+i+k Bi and the performance of the plan that generates this trajectory is better than the earlier one.

Fig. 6 shows paths generated by the planner after it has gained partial knowledge of the world it has explored in its first attempt to reach the goal. The bold solid lines denote the new trajectories (partial plans) generated af­ ter partial knowledge of the world has been gained. It clearly shows an improvement in the performance of the planner as the length of the plan is nearly a third of the plan generated in the first attempt.

Remarks: (i). One should note that generating plans of better performance does not necessarily imply that lffl < lffl where rf is the new plan but could simply imply choosing the right scaling factors o:, f3 such that r(rf) < r(rf). .

(ii). One need not restrict the generation on nonfeasible trajectories to straight line segments, but could instead use arc or even curves that best fit the centers of these obstacle free disks.

4 Final Remarks

This paper is an attempt to bring together aspects of nonholonomic motion planning with robots as discussed by researchers in the communities of behavior-based planning and control theory. We provide a language, a framework and a hybrid architecture to integrate fea­ tures of reactive planning methods with control-theoretic approaches to steer nonholonomic robots. The hybrid language permits planning using a set of behaviors but at the same time the incorporation of differential equations in the language makes it possible to formalize, compare and generate behaviors that improve over time, generate maps, etc. It is clear that in a task such as motion­ planning using limited range sensors, it is helpful to be able to switch between behaviors that rely on the direct coupling of sensory information and actuators and steer­ ing using modern control-theory-based approaches. Our system shows that these two can be smoothly integrated, at least for this form of nonholonomic robot path plan­ ning. Future work includes extending the language to continue formalization of behaviors, including multiple kinetic state machines in the language and implementa­ tion of the planner to control a physical, as opposed to a simulated, robot.