Sensor-based motion control for mobile robotsMaher KhatibLaboratoire d'Automatique et d'Analyse des SystemesLAAS-CNRS7, Avenue du Colonel Roche31077 Toulouse Cedex 4 - France

ContentsIntroduction 1I Collision-free movement 41 Collision-free movement 51.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51.2 Reactive movement methods . . . . . . . . . . . . . . . . . . . . . . . . . . . 51.2.1 Potential eld . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61.2.2 Heuristic approaches . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81.3 Task potentials . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81.4 Potential methods . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91.4.1 Potential wells . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111.4.2 Repulsive force in uence . . . . . . . . . . . . . . . . . . . . . . . . . . 111.4.3 Nonholonomic point robot . . . . . . . . . . . . . . . . . . . . . . . . . 121.5 Mobile robot command . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121.6 The \Rotational-Potential" . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151.7 Collision-free navigation system . . . . . . . . . . . . . . . . . . . . . . . . . . 191.7.1 System denition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 191.7.2 Obstacles representation . . . . . . . . . . . . . . . . . . . . . . . . . . 191.7.3 Potential functions . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 211.7.4 Calculation of forces . . . . . . . . . . . . . . . . . . . . . . . . . . . . 231.7.5 Forces in movement . . . . . . . . . . . . . . . . . . . . . . . . . . . . 241.7.6 Local minima detection . . . . . . . . . . . . . . . . . . . . . . . . . . 251.8 Experimentations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 251.9 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 282 Collision-free path tracking 292.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 292.2 The problem's formalization . . . . . . . . . . . . . . . . . . . . . . . . . . . . 302.2.1 Action insertion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30i

Contents ii2.2.2 Action denition . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 322.2.3 Algorithms . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 332.3 Environment model integration . . . . . . . . . . . . . . . . . . . . . . . . . . 362.3.1 From the model to virtual measures . . . . . . . . . . . . . . . . . . . 372.3.2 Matching . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 402.3.3 System parameters adaptation . . . . . . . . . . . . . . . . . . . . . . 412.4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44II Sensor-based motion 463 Sensor-based actions 473.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 473.2 The \Task Potential" . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 493.3 From Task Potential to sensor-based action . . . . . . . . . . . . . . . . . . . . 503.3.1 Movement to contact . . . . . . . . . . . . . . . . . . . . . . . . . . . . 513.3.2 Parallelize . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 563.3.3 Wall follow . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 603.4 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 664 Application to planning with uncertainty 714.0.1 Denition of the system SBCM . . . . . . . . . . . . . . . . . . . . . . . 724.0.2 SBCM system integration . . . . . . . . . . . . . . . . . . . . . . . . . . 774.1 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 80III Dynamic path modication 845 The elastic band 855.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 855.2 The elastic band . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 865.3 The MDT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 926 Nonholonomic bubble band 996.1 Bubble Band Concept . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 996.2 Metric Structure . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1016.3 Nonholonomic Bubble Band . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1046.3.1 Denitions and Properties . . . . . . . . . . . . . . . . . . . . . . . . . 1046.3.2 Forces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1066.4 Creation and Modication of a Bubble Band . . . . . . . . . . . . . . . . . . 1106.4.1 Bubble Band Modication . . . . . . . . . . . . . . . . . . . . . . . . . 110ii

6.4.2 Stability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1126.5 Bubble band behavior examples . . . . . . . . . . . . . . . . . . . . . . . . . . 1127 Bubble band execution 1167.1 Approximation by Bezier Polynomials . . . . . . . . . . . . . . . . . . . . . . 1167.2 Parameterization of Bezier Polynomials . . . . . . . . . . . . . . . . . . . . . 1237.3 A proposed Execution Controller . . . . . . . . . . . . . . . . . . . . . . . . . 1277.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131Conclusion 133Annexes 135A Hilare2 architecture 136A.1 Control architecture . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136A.2 Module notion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 136A.2.1 Generic module structure . . . . . . . . . . . . . . . . . . . . . . . . . 137A.2.2 Communication . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137A.2.3 Module generator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138A.2.4 Module example . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138A.3 Functional architecture of Hilare2 . . . . . . . . . . . . . . . . . . . . . . . . . 140B Perception 142B.1 Ultrasonic sonars . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142B.2 Segmentation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143B.3 Laser range nder . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145C Stability 148C.1 Control point stability . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148C.2 Stability under a constant force . . . . . . . . . . . . . . . . . . . . . . . . . . 149C.3 Forces to displacement . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 150References 152

List of Figures1.1 Potential distribution example for an obstacle and a goal. . . . . . . . . . . . . . . . 61.2 Comparison between la classical potential method and the generalized potential method 71.3 Potential well. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111.4 Passage near an obstacle. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 121.5 Control point. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131.6 Rotational potential formulation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151.7 Repulsive potential function distribution . . . . . . . . . . . . . . . . . . . . . . . . . 171.8 The norm of F. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 181.9 Rotational-Potential extension eect. . . . . . . . . . . . . . . . . . . . . . . . . . . . 181.10 Rotational-Potential extension eect. . . . . . . . . . . . . . . . . . . . . . . . . . . . 191.11 Avoidance system denition. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201.12 Obstacles representation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 201.13 RtCam Attractive potential and force. . . . . . . . . . . . . . . . . . . . . . . . . . . . 221.14 Dierent potential functions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 231.15 Obstacle avoidance: eect of the dynamic adaptation of the clearing distance. . . . . 261.16 Obstacle avoidance: passage between two obstacles. . . . . . . . . . . . . . . . . . . . 261.17 Obstacle avoidance: rotational potential eect. . . . . . . . . . . . . . . . . . . . . . . 271.18 Obstacle avoidance: local minimum due to the re ection of several ultrasonic echos. . 282.1 Architecture of RtPtCamsystem. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 312.2 Trajectory memorizing idea. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 312.3 The dierent states of the RtPtCamsystem. . . . . . . . . . . . . . . . . . . . . . . . . 342.4 The passage between the dierent states of the system RtPtCam. . . . . . . . . . . . . 362.5 Virtual perception. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 372.6 Virtual measure. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 382.7 Example: virtual ultrasonic measures issued from bitmap calculus. . . . . . . . . . . . 392.8 Simulation: Virtual ultrasonic measures with(out) a detection cone. . . . . . . . . . . 402.9 Matching two segments.. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 402.10 Evolution of the term oc. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 422.11 Obstacles avoidance during path tracking. . . . . . . . . . . . . . . . . . . . . . . . . 422.12 The eect of the dynamic modication of the clearing distance during path tracking. . 432.13 Path tracking: Leaving the Salle. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 432.14 Path tracking: Entering the Salle. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44iv

List of Figures v2.15 Path tracking: projection on the trajectory. . . . . . . . . . . . . . . . . . . . . . . . . 453.1 Advance to Contact action. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 513.2 The Task Potential of the Advance to Contact action. . . . . . . . . . . . . . . . . . . 533.3 parameter eect. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 543.4 -I- Action Advance to Contact execution trace, -II- speed corresponding to the execution. 553.5 Action Advance to Contact utility . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 553.6 Parallelize action. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 563.7 Ambiguity between two object for parallelize action. . . . . . . . . . . . . . . . . . . . 573.8 The evaluation of control function f(t) in function of . . . . . . . . . . . . . . . . . 583.9 -I- Parallelize execution trace, -II- the prole of the dierent command parameters. . 593.10 Wall Follow action. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 603.11 The potential distribution for the command Follow Wall. . . . . . . . . . . . . . . . . 613.12 Problem of the angle of approach of the wall. . . . . . . . . . . . . . . . . . . . . . . 653.13 -I- Execution trace of the action Wall Follow, -II- corresponding parameters prole. . 673.14 -I- Execution trace of the action Wall Follow, -II- corresponding parameters prole. . 683.15 Collision avoidance during Wall Follow. . . . . . . . . . . . . . . . . . . . . . . . . . . 683.16 Local minimum during Wall Follow. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 693.17 Action Follow Corridor. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 694.1 PWU: planning strategies. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 724.2 Denition of the system SBCM. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 734.3 Free-space displacement. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 734.4 Sensor-based actions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 744.5 Vertex types. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 754.6 The distribution of follow and wall switch potentials. . . . . . . . . . . . . . . . . . . 774.7 Experiment LMS. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 814.8 Experiment HVL. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 824.9 HVTL: Execution trace. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 835.1 The internal force of contraction. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 875.2 The dierent states of the elastic band. . . . . . . . . . . . . . . . . . . . . . . . . . . 905.3 The robot congurations on the elastic band. . . . . . . . . . . . . . . . . . . . . . . . 915.4 The MDT between particles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 915.5 The distribution of the potential Pmdt(q). . . . . . . . . . . . . . . . . . . . . . . . . . 925.6 The MDT force issued from the potential in function of . . . . . . . . . . . . . . . . 935.7 Integration of the MDT torque in the elastic band. . . . . . . . . . . . . . . . . . . . . 955.8 Experimentation of the elastic band. . . . . . . . . . . . . . . . . . . . . . . . . . . . 975.9 Laser points, segmentation, ultrasonic echoes and execution trace. . . . . . . . . . . . 986.1 A bubble for planar rotating robots. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1006.2 Bubbles for a plotter arm. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1016.3 Domains of accessibility . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102v

List of Figures vi6.4 Distance to segment in the dierent domains. . . . . . . . . . . . . . . . . . . . . . . 1036.5 Dierent forms of the bubble and paths in the dierent domains. . . . . . . . . . . . 1036.6 Path between to consecutive centers . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1056.7 Continuity and discontinuity of th direction of the repulsive force. . . . . . . . . . . . 1096.8 Creation of a bubble before disconnection. . . . . . . . . . . . . . . . . . . . . . . . . 1106.9 Detection of a loop between bubbles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1116.10 Creation of the rst bubble band. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1116.11 Bubble band creation and the rst feasible trajectory. . . . . . . . . . . . . . . . . . . 1136.12 Internal forces application and optimization of the band. . . . . . . . . . . . . . . . . 1146.13 Application of internal and external forces and clearance from obstacles. . . . . . . . 1146.14 Deformation of the band due to an unknown obstacle. . . . . . . . . . . . . . . . . . . 1156.15 Bubble band trajectory trace. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1157.1 Four control points Bezier approximation of an arc of length 3 . . . . . . . . . . . . . 1197.2 Six control points Bezier approximation of an arc of length 3 . . . . . . . . . . . . . . 1207.3 Six control points Bezier approximation of an arc of length 3 . . . . . . . . . . . . . . 1217.4 Curvature of the four and six control points Bezier approximation of an arc. . . . . . 1227.5 Path approximation by Bezier curves. . . . . . . . . . . . . . . . . . . . . . . . . . . . 1237.6 Dierent ' functions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1247.7 Velocity and acceleration proles for polynomially parameterized 2- and 4-Bezier ap-proximation of a path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1257.8 Velocity and acceleration proles for 3-spline parameterized 2- and 4-Bezier approxi-mation of a path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1267.9 Velocity and acceleration proles for polynomially parameterized 2- and 6-Bezier ap-proximation of a path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1267.10 Velocity and acceleration proles for 3-spline parameterized 2- and 6-Bezier approxi-mation of a path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1277.11 Fix piece of length r0 in rst bubble. . . . . . . . . . . . . . . . . . . . . . . . . . . . 1287.12 Velocity and acceleration proles for a split 3-spline parameterized 2- and 6-Bezierapproximation of a path. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1307.13 The proposed architecture of Kinan system . . . . . . . . . . . . . . . . . . . . . . . . 1307.14 Two, four and six control points Bezier curves sequence execution with correspondingspeed proles. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131A.1 Module structure. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 137A.2 Client/server relation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138A.3 The module Avoid. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 138A.4 Control and data ux during the execution of a trajectory while avoiding obstacles. . 139A.5 Most of the modules constituting the functional architecture of the robot Hilare2. . . . 140B.1 Hilare2 ultrasonic sonars. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142B.2 Directivity diagram of an ultrasonic sonar. . . . . . . . . . . . . . . . . . . . . . . . . 143B.3 Segmentation of ultrasonic measures. . . . . . . . . . . . . . . . . . . . . . . . . . . . 144B.4 Rebound eect. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 144vi

B.5 Laser range nder of Hilare2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 145

IntroductionMobile robot autonomous navigationNavigation is the most fundamental task of an autonomous mobile robot without which noth-ing else may be accomplished. Navigation designs a set of operating systems permitting therobot to reach a distant goal - which may be expressed in a diverse ways - and this indepen-dently of the various situations which it may encounter. Even though these considerationsmay seem trivial at rst hand, let us consider their implications.Generally, a goal is not directly accessible or even identiable and navigation must be anincremental process consisting of determining and joining appropriate intermediate goals inorder to converge towards its destination. This navigation in composed of several phases: Infunction of its xed objective and the present diculties, the system must determine whatinformation it needs for the accomplishment of the task as well as the means of gatheringit (Active perception). An analysis of this perceptual data is then used to build a usableenvironment model (Modeling). To be able to move, the robot and if possible the destinationshould be localized in the environment model (Localization). It uses the some local (itera-tive) or global approach to decide of the itinerary to take, the trajectory to follow and theconditions to impose of the right accomplishment of the task in function of the environmentdiculties (Motion planning). Only then can the robot accomplish its mission by executingthe commands corresponding to the planned motion. These commands are usually accom-panied by sensor feedback on certain environment characteristics which may be provokedby the sudden apparition of an obstacle and may require some modication of the nominalplan of movement (Execution). Nevertheless, this modication must not violate the plan:The execution must be permanently controlled and may induce a new iteration of the wholeprocess before even arriving at the rst intermediate goal. In order to avoid the costly reex-aminations, the plan must present some execution exibility and the execution must try toadapt itself dynamically to local changes.The articulation and the organization of these dierent phases was the object of numerousstudies. In this memory, we will be mostly interested in two of these phases and to the stronginteractions binding them: Motion planning and execution. The union of both procedures isused to propose an answer to the general problem of collision-free motion generation.This problem is (too) much studied in two separate directions motion planning and reac-tive movement.In a motion planning approach, the movement is totally planned and thus \xed" beforeexecution. The planning is based upon a model of the environment and of the robot. Itsmost renowned advantage is its powerful ability of nding solutions in extremely complexenvironments In fact, displacement in geometrically constrained or topologically complex1

Introduction 2environments may necessitate ne local planning while keeping in view the global context.Nevertheless, this means predictions based on models which are only approximations andon the behavior of the robot which could but be full of uncertainty and imprecision. Suchprediction leads inevitably to execution failure especially in the constrained environmentswhich justify a priori this approach: A planning independent of execution could never berobust.Inversely, reactive approaches, produce sure movement based on sensor data acquiredduring execution. Thus, the system produces simple models enabling dynamic correctionand guaranteeing collision-free movement. Nevertheless, this limited environment perceptionand local strategy does not guarantee the global accomplishment of the task even when apath exists.That is why solutions integrating both approaches has become a priority. This doesnot mean a simple chaining of planning and execution which could only lead to repetitiveexecution failure and thus to numerous iteration planning/execution. It also means a completereanalysis of the situation implying perception, modeling and localization to reset the contextand avoid the same error of execution (looping).These several remarks give birth to a new problem of collision-free motion generation.The role and properties of both planning and execution must be redened and communicationand shared information must be reexamined. This is context of the present work.Memory organizationAfter an analysis of actions necessary for the execution and a formalization of actions basedon the notion of potential eld, we present three major approaches which satisfy planning andreactive control. These procedures constitute a part of a generic autonomous mobile robotcontrol architecture and are illustrated by experiments. The memory is structures in threeparts:The rst part presents the basic displacement actions of which a mobile robot mustdispose and propose a rst method to integrate planning and execution.The rst chapter is reserved to displacement or re ex actions: Purely reactive actions oflocal collision avoidance. These actions are constructed on the classical potential method. Theadvantages and inconveniences of this method which covers the rest of the work will then bediscussed. This analysis will bring about a new formalism of potential functions which willbe developed in the context of non-holonomic mobile robots. The eciency of this methodwill be illustrated by some experiments which will recall the necessity of a planning phase.Based upon these \primitive" functions, we show in the second chapter the possibility ofdeveloping a relatively simple planning/execution procedure which enables sure and coherentmovement. This general system whose structure will be introduced, permits a \better"controlled tracking of planned trajectories while avoiding local obstacles. This system enablesthe transmission of environment model information to the execution in order to ameliorate therobot behavior. In spite of a great execution exibility, the results point out the limitationsof purely geometric planning.The second part propose a new architecture between planning and execution con-tributing to a robust system. This system considers both model and execution uncertainty.Planning with uncertainty does not look for the determination of a geometric trajectory butfor the denition of successive environment relative actions. This supposes more elaborate2

Introduction 3actions which explicitly consider the environment constraints and which could be expressedin a higher level of abstraction.The specication and the functionality of these actions will be developed in the third chap-ter. These actions called sensor-based actions are equally based upon the potential methodbut use a richer and more powerful formalism called task potential. Three fundamental actionscorresponding to dierent instantiations of this task potential will be presented.The fourth chapter is dedicated to an integrated system which include these actions andan uncertainty planner. A big variety complex task navigation experiments show the systemeciency.The third and last part propose method which is dierent from the other two. Forthough it is also based upon the potential method, it integrates more narrowly planning andreactivity. This is the elastic band method. Planning may be reduced to a classical pathresearch. This path is only considered to be a topological guide during execution. Basedupon an environment model transmitted by the planner and on perception data acquiredduring execution the elastic band changes the path dynamically in reaction of obstacles whilerespecting the topological properties of the original path: The robot does not get lost.The fth chapter present this technic and extends it to non-holonomic robots by takinginto account kinematic constraints. Nevertheless, the trajectory being redened dynamically,a curvature control problem occurs: The trajectory is only feasible by a robot of null steeringradius. Anyway, the curvature discontinuities mean unsatisfactory trajectory. The last twochapters will tackle this problem.Thus, in the sixth chapter, this method is enriched by another notion: That of a bubble.A bubble corresponds to the accessibility domain around the current conguration of a robottaking into account the presence of obstacles and the kinematic constraints of the robot. Itis dened with the help of a distance function issued from a metric space compatible withthe kinematic constraints of the robot. This is a very general notion. We have instantiated itwith the Reeds & Shepp metric which corresponds to non-holonomic vehicles with minimumsteering radius (e.g. a car). This bound the curvature but does not solve its discontinuity.The produced trajectory must then be smoothed.The last chapter propose nally a complete integration of this approach. The trajectoriesare smoothed using Bezier curves. The whole is integrated in an execution system calledkinan which takes into account the time constraints from planning to control. Experimentalresults will be presented and commented.3

Part ICollision-free movement

4

Chapter 1 Collision-free movement1.1 IntroductionMobile robot movement between congurations may be classied into one of three principalclasses:Planied movement: Robot displacement is xed at planning level. This displacementcan be described in dierent ways: A list of references1, a trajectory, a sequence of(geometric) primitives, : : : The robot executes this movement with no environmentperceptual feedback.Reactive movement: Using its perceptual system to gather information on the environ-ment, the robot moves around while avoiding collision.Reactive planied movement: The robot adapts the planning level displacement refer-ence to the environment state, updated by perceptual feedback. The robot executesthe planied movement.In this chapter several approaches to reactive movement are developed and their advantagesand inconveniences are discussed. Then, we concentrate on the classical potential methodthus presenting the ameliorations we eectuated on it through the rotational potential. Lastof all, we present the integration of this extended method in a collision-free reactive navigationsystem.1.2 Reactive movement methodsReactive (collision-free) movement was extensively studied. Analyzing most of the proposedmethods, twomain axes were outlined: Potential eld concept and heuristic free-path researchalgorithms.1A reference is frequently represented by the position and velocity of some reference point on the robot.5

1.2. REACTIVE MOVEMENT METHODS 61.2.1 Potential eldThe three principal approaches to potential eld concept are: The classical method [O. Khatib 80,O. Khatib 86], the generalized method [Krogh 84] and the vector eld histogramme method[Borenstein 91].The classical method is based mostly upon the creation of an articial potential eld inwhich the target is an attractive pole and the obstacles are repulsive surfaces. The robotfollows the gradient of this potential towards its minimum (gure 1.1). The derived forceinduces a collinear and proportional acceleration, thus enabling easy dynamic and kinematiccontrol, which makes this method attractive in mobile robot movement control.The major advantages of this method are: Simple algorithm, the method being analytic. Physical robot model dependent reactivity, the control being a function of the inertiamatrix. Adaptability to all extroceptive sensors. Resultant force continuity. Easy treatment of xed and mobile obstacles.Its major disadvantage is the possible existence of local minima for certain compositionof target and obstacles.Goal

Obstacle

Figure 1.1: Potential distribution example for an obstacle and a goal.Departing from the classical potential, [Krogh 84] presents a generalized potential methodwhose potential function depends on the robot position as well as its velocity relative to theobstacle. The generalized method is thus sensitive to the notion of collision moment insteadof the notion of distance. This potential function is dened to be inversely proportional to theavoidance period which represents the time dierence between minimal deceleration on theremaining distance to the obstacle following the current direction and maximal deceleration.Tilove develops a comparison between the classical potential method and the generalizedmethod by giving the following example [Tilove 89]:In the case of the classical potential eld, the force depends on the distance to the obstacle6

1.2. REACTIVE MOVEMENT METHODS 7Goal Goal

Classical potentialGeneralized potentialFigure 1.2: Comparison between la classical potential method and the generalized potentialmethodwhich varies in a continuous way. This force generates a continuous trajectory (gure 1.2. Inthe case of the realized potential method, when the robot approaches the obstacle, avoidancetime decreases and thus the repulsive force increases to push the robot away. When thislast turns, the velocity is oriented in the direction opposite to the obstacle diminishing thusthe repulsive potential and orienting the potential gradient towards the goal. The robot re-approaches the obstacle and the repulsive force increases again, : : : This generates oscillationswhich can be neutralized only by using a dynamic command exploiting the robot inertiainstead of a kinematic command.This method ameliorates the robot behavior in certain congurations enabling it tomanver near the obstacle slowly (to eliminate the oscillations dynamic control is unavoid-able2). Nevertheless, this method suers from the same classical problem of potential wells.[Borenstein 91] uses an intermediate solution between planning and an avoidance system,and proposes a method consisting of a local heuristic choice based on a bidimesional gridmodeling the environment. This representation is derived from the certitude grid conceptdeveloped by [Moravec 85] where each case contains a probability value of the presence ofan obstacle updated by perception measures. A polar histogramme is constructed aroundthe robot (which is always at the center of the grid) representing the polar densities of theobstacles. The sector associated to the least density closest to the goal direction is chosen.The grid is thus able to give a movement direction. Even though this technic is adapted toall sensors, it presents some disadvantages: Model limitations may lead to oscillations, thus loss of interest in large environments. Cycle time is rather elevated. Robot control algorithm still to be dened.De Luca et Oriolo propose [De Luca 94] an incremental planner based on the notion ofvortex eld to generate collision-free movement for a circular nonholonomic robot. A vortexeld is generated using the rotational of a potential led instead of the negative gradient, thusnecessitating the denition of a sense of rotation around the obstacles. The planner generatesinitially a holonomic displacement which is transformed into a velocity command executableby the robot. When the robot is engaged into a passage between two obstacles initially awayfrom each other in order to permit the movement, a risk of collision remains present if thedistance between the two obstacles becomes smaller than the width of the robot. This is dueto the fact that a vortex eld does not guarantee non-collision. Such a situation generates inthe classical method a local minimum (which is always preferable to collision). Consequently,an obligatory minimum distance (superior to the width of the robot) is necessary to engage adisplacement between obstacles. Another inconvenience is the fact that the rotation direction2This is not always easily accomplished for mobile robots7

1.3. TASK POTENTIALS 8may be incoherent with respect to the distance to the goal.1.2.2 Heuristic approachesWe can also note in [Khoumsi 88, Yagi 91, Bennamoun 91] several heuristic algorithms basedon the generation of displacement primitives. In [Khoumsi 88], the robot executes a successionof arcs of circle centered on points on the obstacle measured by the sensors3, which is animportant diculty, at control level, due to curvature discontinuity.Being essentially dedicated to the interior, the approach put in place by [Yagi 91] needsa highly structured environment. The authors place themselves in a context of a dynamicscene where the robot and the obstacles have constant linear speeds. Their idea consistsof detecting the vertical boundaries of obstacles and their horizontal displacement. Theconvergence (respectively divergence) of these boundaries gives an idea about the positionand movement of obstacles. Avoidance in this case is empiric. It consists of decelerating therobot or changing the direction of its movement. A reduction of a half of the robot speed wasproposed to avoid mobile obstacles but the algorithm of change of direction was not tackled.Also the problems related to the robot command were not aborted.Using other types of proximity sensors such as infrared sensors, an avoidance algorithmis proposed in [Bennamoun 91]. It uses two sensors, one with long and the other short range.also supposes a point robot and a superior bound to the size of obstacles. The generation ofmovement is done in a discontinuous way. This algorithm being boolean is very sensitive toperception errors.DiscussionWe consider that the advantage of reactive movement is the security of execution. Neverthe-less, the control of a programmable mobile robot capable of deliberate actions must to bewholly reactive. That is why the reactive level should respect the constraints imposed on it bysuperior levels. Its decision capacity, thus, must not exceed local reaction. Of the examinedapproaches, the potential eld method presents the clearest theoretical and methodologicalframe and the most rigorous. The principle of this method may be considered as a \naturalre ex" to collision. It implies a direct application of the control without demanding heuristicsor suppositions on the environment structure. This method is thus chosen to be the basis onwhich to tackle the problem of collision-free mobile robot navigation [M. Khatib 95].1.3 Task potentialsA certain number of parameters intervene in the denition of the potential function and inthe deduction of the moving force which highlights the following question:How to dene a potential function to accomplish a certain task?A task is hereby dened as a set of actions for the robot to accomplish in order to controlhis displacement: obstacle avoidance, movement servoied on an object, wall follow, : : :The answer to this question is at the foundation of our approach. It greatly depends onthe following important elements: The robot geometry and its kinematic constraints.3Ultrasonic sonars 8

1.4. POTENTIAL METHODS 9 The perception means aboard the robot. The action to realize and the involved parameters.A potential function must be constructed in agreement with these elements. Thus a forceresultant constructed for a holonomic robot may not be applicable to a nonholonomic one.As well, a potential function devised for a point robot could be incompatible with a polygonalone.One of the most important parameters in the execution of a collision-free navigationtask is the obstacle clearing distance which depends mostly on the kinematic constraints ofthe robot. According to the required task, this clearing distance being constant may overconstrain the execution and provoke a failure. A notable amelioration could be achieved intaking in consideration the nature of the task.These arguments show that the denition of the potential function depends onparameters representing the application global context.1.4 Potential methodsIn order to introduce the interest of this work, the classical potential eld proposed by[O. Khatib 86] method is hereby presented. We start by formalizing the one obstacle problemand then tackle general case.Consider the obstacle O, represented by an analytic function in the plan (O; x; y), or asshown later by enveloping segments. If Xg represents the goal position and X the robotposition (X is called the robot state vector), The articial potential function applied to therobot is of the form: U(X) = Ug(X) + UO(X) (1.1)where U(X) is the resultant potential, Ug(X) the attractive potential produced by the goalat X, UO(X) the repulsive potential induced by the obstacle in X. The resultant force F isthen: F = Fg + FO (1.2)where: Fg = grad[Ug(X)] (1.3)FO = grad[UO(X)]Fg is an attractive force which guides the control point to the goal and FO is a forceinducing an articial repulsion from the surface of the obstacle (FIRAS: from the French),being produced by UO(X).The attractive potential is simply dened by:Ug(X) = 12kg(XXg)T (XXg) (1.4)9

1.4. POTENTIAL METHODS 10which is a positive quadratic function whose rst derivative is continuous and whose onlyminimum is null on X = Xg. kg is the gain factor. Consequently the potential UO(X) will bedened in a way to keep the articial potential U(X) positive, continuous, derivable, whoseonly minimum is zero on X = Xg4. Similarly the function UO(X) must itself be continuous,non negative, derivable. Also its value must tend to innity when approaching the surface ofthe obstacle.Its in uence should also be restricted to a neighborhood of the obstacle (up to a distanced0 from the obstacle which should always be smaller than the least distance between theobstacle and the goal), in order to prevent the modication of the global minimum of theattractive potential function. This function will thus be dened on two distance intervalswhile insuring a second derivability. Our choice is:UO(X) = ( 12(1d 1d0 )2 if d < d0,0 else-wise. (1.5)where d0, the in uence distance, depends on the maximum velocity and deceleration capacityof the robot. d is the distance between the robot and the obstacle O and the gain of thefunction.Having xed the expressions Ug(X) and UO(X), the induced forces could now be calcu-lated by applying the gradient on the equation 1.4 on the page before:Fg(X) = kg(XXg) (1.6)The vector X enables the calculation of the force Fg using the equation 1.6. In orderto insure asymptotic stability, we add a dissipative force proportional to the velocity _X (seeannex C). We have thus: Fg = kg(XXg) kv _X (1.7)kv being the gain of this force. As well, using the equation the equation 1.5 we have:FO(X) = (1d 1d0 ) 1d2 @d@X if d < d0,0 else-wise. (1.8)The term @d@X , in the equation 1.8, represents the unity vector of the partial derivativevector of the distance d between the robot and the obstacle which determines the directionin which the force is applied.In practice, the distance d is measured by the perception means 5, thus it is sucient tocalculate using 1.8 the forces associated to dierent distances by the sensors and the resultantforce applied on the robot becomes: FO = nXi=1 FOi (1.9)4We put aside for the moment the local minima!5Here these are ultrasonic measures or laser 2D sections10

1.4. POTENTIAL METHODS 11where n is the number of obstacles. The resultant force applied on the control point is:F = Fg + FO (1.10)The resultant force F will be the entry point of the force to control translating algorithm.Several control types are thus possible according to the type of the robot, its kinematicconstraints and the choice of the control.1.4.1 Potential wellsIn the rst section of this chapter, we showed how the basic principle of this method consistson minimizing a potential eld applied on the robot. In some environments local minima canbe produced where attractive and repulsive potentials inhibit each other. Were the robot topass in such a well, called potential well, it may stop before reaching its goal.The fact that the robot stops in a potential well may seem to indicate that the resultantforce is null. This may not be true. Frequently it oscillates around zero. Thus the robotoscillates also around the minimum (g 1.3).© Oper

Goal

Obstacle

Figure 1.3: Potential well.This is why it rather dicult to detect local minima. criteria can still be found to detectthese oscillatory behaviors, e.g. the sum of the resultants over some iterations rests in theneighborhood of zero. This problem will be developed in paragraph (x1.7.6).1.4.2 Repulsive force in uenceIt is important to note that this potential function possesses a constant gain and a xedclearing distance regardless of the position of the robot. Consequently, the robot is in uencedby the repulsive force of an obstacle even though its trajectory holds no risk of collision. Forexample when the robot passes at a distance lesser than d0 from an obstacle which otherwisehave no eect on its passage. Suppose now that the trajectory is a straight line bringing therobot from the conguration p to the conguration q passing near an obstacleO (gure 1.4 onthe next page). If the clearing distance d0 dene a region R1 around the obstacle intersectingthe trajectory, the robot, obeying to the repulsive force, will realize a correction by goingto the point p0, thus minimizing the this force. This is due to the fact that the repulsive11

1.5. MOBILE ROBOT COMMAND 12d0

R1

R 2

N

P q

P’

OFigure 1.4: Passage near an obstacle.potential parameters are constants. On the other hand, if R1 could be modied in ordernot to exceed the line N (becoming thus R2), the in uence zone of this obstacle (which doesnot really bother the trajectory) is now separated from the trajectory and the robot is notin uenced. How our extension deal with this problem will be discussed in section 1.6.1.4.3 Nonholonomic point robotNoter that this frame is developed for a geometric point in a normed space of dimension n,and this method is only applied to an object in its conguration space only. Nevertheless,a real-time construction of this space is not realistic. Even more, the movement generatedby the application of the force may not be kinematically possible for certain types of robotsnotably the nonholonomic robots (which is the case of our application).We have thus decided to generalize this method to adapt it to a mobile nonholonomicpolygonal robot. Let us rst recall the expression of the nonholonomic and its in uence onrobot control.1.5 Mobile robot commandThe control of a mobile object begins with the determination of the control point pc on thisobject. Generally, the simplest choice for a holonomic object is its center of mass. Thecontrol parameters are thus dened by: the position of this point, the corresponding inertiamatrix and a global reference. Thus the control of such an object under the in uence ofexternal forces is perfectly dened. For a mobile nonholonomic object (i.e. an object withnon integrable kinematic constraints such as a wheel) and under the condition of non-slidingmotion6, the control is not necessarily possible for all positions of control.Particularly in the case of a mobile robot whose conguration is dened by a vectorX = [x y ]T , [Samson 91] shows that there is no smooth feedback control stabilizing thewhole vectorX around a given conguration. On the other hand, [Samson 90a] demonstrates6The velocity of the contact point of the wheel is zero 12

1.5. MOBILE ROBOT COMMAND 13that two of the three parameters, the position [x; y]T , can be directly controlled. There existsfeedback control with exponential convergence towards these parameters. Even more, if theposition of a point which is not on the motor wheels axe is controlled these laws guaranteethe convergence of the orientation towards a constant which is tangent to the trajectoryduring the execution.E r xryr yg xg PcRg Og

RrOrd ijFigure 1.5: Control point.Consider the origin point Or of the local cart of the robot (g. 1.5), E being the distancebetween the two wheels, r the wheels radius, Pc a point on the x-axis xr a distance d o theorigin Or. We can write the equation system for this robot in the xed frame Rg as follows:_xg = r2( _qr + _ql) cos_yg = r2( _qr + _ql) sin _ = rE ( _qr _ql)where qr et ql are the angular positions of the wheels. Let v and ! be the instantaneouslinear and angular speeds of the robot. The vector (v; !)T is related to the wheels angularspeed command vector ( _qr; _ql)T by: v! = _qr_ql ; = r2 r2rE rE The matrix being regular (det 6= 0), we can base our reasoning on the auxiliary commandvector U = (v; !)T .We can express, now, the kinematic system in the local frame of the robot Rr by themore general problem of regulating to zero of the point Pc. We have, thus, !PcOg = x~ + y~|and the classical kinematic relation:!V Og=Rr = !V Or=Rg !! Rr=Rg ^ !OrOg (1.11)13

1.5. MOBILE ROBOT COMMAND 14with: 8><>: !OrOg = !OrPc +!PcOg = (d+ x)~+ y~|!! Rr=Rg = !:~k!V Or=Rg = v:~where !V Og=Rr is the velocity of the point Og in the frame Rr (which is expressed by _X),!! Rr=Rg is the angular velocity of the mobile frame Rr in the frame Rg and !V Or=Rg is thetranslation velocity of the origin Or in th frame Rg.Substituting this in the equation 1.11, we get:_X = B(X)U; 0@ _x_y_ 1A=0@ 1 y0 (x+ d)0 1 1A v! : (1.12)Suppose that the initial conguration is in the neighborhood of the point Xf = 0 andthat we look for the command U(X) which brings the robot to this point Xf . A classicalresult of automatics: A system controllability such as (1.12) guarantees the existence of alocal stability by feedback. With this condition satised, a control of the form U = KX willstabilize this system (at least locally). In our case the equation 1.12 becomes:_X = B(0)U; 0@ _x_y_ 1A=0@ 1 00 d0 1 1A v! (1.13)The matrix B(0) is not square and the system is not controllable. Nevertheless, if we tryto reduce the system to realize a control in position X = [x y]T for the point p the systembecomes: _X = B(0)UX = xy ; B(X) = 1 y0 (x + d) (1.14)Note that when d ! 0 the system is no more controllable, for the carte can no longermove in the direction of wheels axis (a non-exponential convergence can be proved on a setof invariance I).This behavior is similar in the point of view of classical mechanics: An external forceapplied on a point on the wheels axis has a singular component, that which is parallel to theaxis.Note: [Samson 93] propose an other approach for the stabilization of the control of a mobilenonholonomic robot. This approach concerns the instationary state feedback control wherethe feedback depends upon the time as well as the state of the system.B Based upon this result, we dene in our case a control point on the robot and we applythe resultant of the dierent forces on this point. We will show in annex C the stability ofour system in the absence of obstacles by a Lyapunov candidate function.We conserve in our formalism the same attractive potential and we propose in the nextparagraph a formalization of the repulsive potential which introduces parameters which en-ables the taking into account of the form of the robot.14

1.6. THE \ROTATIONAL-POTENTIAL" 151.6 The \Rotational-Potential"Here we will place ourselves in a context where the obstacles are immobile. Our motivationis to ameliorate the behavior and the movement of the robot by a dynamic adaptation of theparameters of avoidance.In the classical method, the repulsive force acts only in side a zone dened by the clearingdistance. In practice, the choice of this distance is delicate. A small value induces a vigorousvariation of the repulsive force and imposes constraints on the speed of the robot. A greatvalue may over constrain the movement in places where the robot may pass.In order to determinate this distance more uently, we propose the following idea. Wex a minimal clearing distance with a repulsive potential function which insures non-collisionin the region dened by it. We still dene another potential for a distance greater thanthe minimum distance that tries to guide the robot and correct its movement if necessary.The global repulsive potential will thus be a linear combination of these two potentials. Thecoecients of this combination will depend on the state of the robot relative to the obstaclein order to estimate in a better way the need to correct its movement.We present in this chapter the solution we have developed and the appropriate potentialfunctions. But rst some notions, dened hereby, are needed.d RoRr d0i xy

OSo VsFigure 1.6: Rotational potential formulation.Consider the frame Rr centered on the robot such as the x-axis which corresponds to therobot direction is perpendicular to the wheels axis (gure 1.6). Note ~ the unitary vector inthis direction. Let So be a segment delimiting the obstacle represented in the frame Rr andlet Vs be its direction vector. The angle is dened to be: = ArcsinK(Vs~)jVsj (1.15)where K : R3 ! R is the projection of a point (x; y; z)T on its third component z. hasthus its values in the interval ] 2 ; 2 [. It is null when the robot is parallel to the segment.The parameter in this function modies the obstacle force intensity in function of theorientation of the robot. Thus it presents the \danger" the obstaclees position resents relative15

1.6. THE \ROTATIONAL-POTENTIAL" 16to the robot. It may happen that when a robot enters the obstacle in uence area withoutany modication on its movement. A small modication of the orientation (and eventuallyof the distance) may sometimes be sucient to annulate the obstacle eect.If d represents the smallest distance to the segment So and if d0 is the associated clearingdistance we dene the function p1 as:p1(X) = 12k11d 1d02 ;and the function p2 as: p2(X) = 12k2 (d0 d)2 :The repulsive potential function proposed is:U(X) = 2p2(X) + p1(X) if d < d0,0 else-wise. (1.16)with k2 k1. Indeed, the function p1 represents the dominant part near the surface of theobstacle and tends to innity when d! 0, while p2 is dominant for d around d0.The two functions being increasing when d! 0, the minimal distance dnc is the distancefrom which the function p1(X) becomes superior to the function p2(X). for = 2 , thusp1(X) = (2 )2p2(X), this value is: dnc = 2pk2k1k2d0 (1.17)The function U is a positive dened function whose minimum is on the line d = d0. Thegure 1.7 on the following page7 shows this potential in function of and d.In order to make explicit the expression of the force, we associate a frame Ro with thenearest point on the obstacle (g 1.6 on the page before). In this frame:fx 2 <; y = d; = gthe expression of the force now being:F(X) = 0B@ 0k1 1y 1dc y2 + k22(d0 y)k2(d0 y)2 1CA ; if d < d0, 0 else. (1.18)The Fy term represents the force generating the displacement of the mobile frame Rr inthe y-direction of the frame Ro. This component depends on the distance to the obstacle (yin this frame), but also on the term 2, for the linear part of this component which representsthe robot orientation with respect to the obstacle. This last term changes only the intensity7The function is traced for d negative as well as positive.16

1.6. THE \ROTATIONAL-POTENTIAL" 17

U(X)d d0

Figure 1.7: Repulsive potential function distributionof the force and not its direction being a mere scalar. It is null when the robot is parallel tothe obstacle.The component F represents the couple around the rotation center of the robot. Thesign of the correction generated by the couple depends on the orientation whereas theintensity depends equally on the distance to the obstacle.The norm of the force F is shown in the gure 1.8 on the following page. It is to be notedthat this norm is null on the line y = d0, innitismal on the line = 0 in the neighborhoodof d0 and innite on the obstacle surface. This insures the non-collision even for null.Having dened the repulsive potential function for one obstacle, we are going to gener-alize it for m obstacles. Consider m segments representing m obstacles, calculate the forcesassociated to each segment (Fi (X); 8i 2 f1; : : : ; mg). The total repulsive forces is:F = mXi=1 Fi (1.19)Adding the attractive force, the resultant is:Fc = Fg + F (1.20)Analysis: The advantage of this formalism to the classical ons is the dynamic regulationof the clearing distance d0 in function of the geometric state (Robot $ Obstacles). In theclassical method, the distance associated to a xed equipotential is the same for all obstacles.17

1.6. THE \ROTATIONAL-POTENTIAL" 18F

0

d0

yθ Figure 1.8: The norm of F.In the rotational method this distance varies with the orientation of the robot. Nevertheless,p1 guarantees always the non-collision near the obstacles surface. This regulation presents anet amelioration to the movement in situations where the classical method fails.© Oper

BA

Obstacle

© Oper

B

A

ObstacleFigure 1.9: Rotational-Potential extension eect.Presented in the gure ( 1.9) an example illustrating the amelioration due to this regula-tion.The gure ( 1.10 on the following page) presents an example of blocked passage in thecase of classical potential. This is due to the fact that the in uence zones are overlapping.In the rotational potential case these zones are modied dynamically with the evolution ofthe orientation.On one hand, this approach make it possible to limit the in uence of obstacles who doesnot bother the passage how much are they close to it, on the other, it increases the possibilityof nding a solution by adapting dynamically the parameters of the potential function.Moretheless, this approach is applicable in the 3 dimensional space replacing segmentsby planes and the angle by the angle between the projection of the robot velocity on theperpendicular plane and the intersection line between the two planes.18

1.7. COLLISION-FREE NAVIGATION SYSTEM 19© Oper

A

B

Obstacle

Obstacle

© Oper

A

B

Obstacle

ObstacleFigure 1.10: Rotational-Potential extension eect.1.7 Collision-free navigation systemAfter having a formally dened the potential functions, the dierent algorithms used in areal-time collision-free navigation system for a nonholonomic mobile robot are developed.1.7.1 System denitionIn the global frame Rg, we suppose the following entries dened: A nonholonomic robot model with null steering radius such that: rr; rl are respectively the right and left wheel radiuses; E the axle value; amax; max; vmax; !max the linear and angular acceleration and speed; Rr local frame on robot mobile in Rg. A state vector X of the robot always available in the global frame. A set M of distance measures permanently up to date of the form of distances andorientations, di and i, relative to the robot body and positions, pi in its local frame(see section B). A vector G pointing at the goal.These data dene the entry of the navigation system.The result is a vector U representing the robot control and containing a linear and anangular speeds, respectively v and !, which determine the local frame movement in the globalsetting Rg.The gure 1.11 on the next page shows the entries and result of the navigation systemcalled RtCam.In the following paragraphs, the system internal functionality is presented.1.7.2 Obstacles representationIn the paragraph 1.6, the rotational potential was developed using segments to delimit ob-stacles. In this paragraph the problem of constructing these segments is studied.Let M be the set of returned measures by the perception system. Firstly this set isdecomposed into mi; 8i 2 [1; : : : ; n], subsets of points. This decomposition is based on thecontinuity of the polar angle as well as the distance from the robot d.Next the points in each set mi are approximated by a segment using the least square19

1.7. COLLISION-FREE NAVIGATION SYSTEM 20R Camt

Rr

X

U G

M

, E, r , rr l

Rg

ip

Figure 1.11: Avoidance system denition.criterium: 1m mXj=1 d2j where is the representative limit of a point variance8 and dj the orthogonal distance betweenthe point mj and the segment under construction. The segmentation algorithm is presentedin the annex B.© Oper

R

R P

S

g

r i

iFigure 1.12: Obstacles representation.The result of this operation is a set of segments S delimiting the obstacles. These segmentsare represented in the local frame of the robot by a line, two extremities and a distance fromthe robot body.The gure 1.12 shows a set of segments generated from a set of measures derived from anazimuthal section of a laser range nder98This limit depends greatly on the sensor.9The segments seen by the robot are enlarged for visibility.20

1.7. COLLISION-FREE NAVIGATION SYSTEM 21Let Mp be the set of points not used in the segmentation, we consider each point asa punctual obstacle with no orientation. The potential function associated to this set ispresented in section 1.7.3.The importance of these measures is related to the sensor used. In the ultrasonic sonarfor instance the treatment of these points is important because of the relatively small numberof measures and of the loss of measures due to re ection specularity, thus leading to the lossof a segment.1.7.3 Potential functionsAttractive potentialThe attractive potential such as it is proposed in [O. Khatib 80] produce a force whose inten-sity is proportional to the distance robot $ goal. Since the repulsive potential depends onlyon the distance robot $ obstacles, it is dicult to x the gain of the repulsive potential.To make the value of the attractive force as much close to a constant as possible awayfrom the goal we need a function with a unique minimum at the goal and a linear asymptoticbehavior away from it. This function should be positive and its gradient must always pointtowards the goal. For this, we propose this simple function, which is a surface of revolutiongenerated from a hyperbole in the following way: ifz2a2 2b2 = 1is a hyperbole, the change of variables, =px2 + y2, gives:z2a2 = 1 + x2 + y2b2 ) z = abpb2 + (x2 + y2)which will help us dene the potential function:Ug = Kgqd2g + R2; dg =q(x xg)2 + (y yg)2where xg and yg denote the goal position, Kg is a positive gain constant and R is a functionof the limit distance dm.The force deduced from this potential is thus:Fg = Kg xxgpd2+R2yygpd2+R2 ! = Kg dpd2 + R2 @d@X : (1.21)R does not depend on dg, thus, dgpd2g+R2 tend towards 1 when dg tends towards innity.This term being increasing in [0; 1[, for dg > dm:j = dmpd2m + R2 < dgqd2g + R2 < 1where kgj is the norm of the force at the distance dm (when j tends towards 1 the surfacetends towards a cone). 21

1.7. COLLISION-FREE NAVIGATION SYSTEM 220

Potential

Force

0

y

Potential

Force

xFigure 1.13: RtCam Attractive potential and force.The distance dm being set, R is calculated to be:R = dms 12j 1:This force is constant for distances greater than the limit distance and tends to zero asthe robot approaches the goal. The control of this (constant) force will be developed in theannex C.The gure 1.13 shows the distribution of the attractive potential and its associated forcein a given direction.Repulsive potentialsThe Rotational-Potential approach requires an attractive potential as in the classical method,though it requires two repulsive potentials. In the equation 1.16 on page 16, the functionp1(x) dominates in the neighborhood of the surface of the obstacle in order to avoid collision,while the function p2(x) dominates near the clearing distance d0 and being dependent on 2is orientation dependent. Its role is to correct the movement.These two potential functions must: be positive, continuous and derivable on the interval d 2 [0; d0]; have one and only one minimum when d = d0; be null for d > d0; tens to innity when d! 0.The predominance between the functions is controlled by the gain factors. Still the formof the functions (quadratic, logarithmic, parabolic, : : : ) is also signicant.Figure 1.14 on the next page shows an example of the variation of three functions. Noteparticularly the dominance intervals of the dierent functions.22

1.7. COLLISION-FREE NAVIGATION SYSTEM 230

P

d0d

(d - d0)

(1/d - 1/d0)

(log d - log d0)

2

2

2Figure 1.14: Dierent potential functions.In the system RtCam w2e have chosen the expression given in paragraph 1.6 to representthe repulsive potential associated to segments. Thus:U(X) = 2p2(X) + p1(X) if d < d0,0 else-wise.where p1(X) = 12k11d 1d02 ;p2(X) = 12k2 (d d0)2 :The parameter is not dened in the case of point obstacles. Thus we dene the functionUp(X) by: Up(X) = p1(X) + p2(X) if d < d0,0 else-wise.1.7.4 Calculation of forcesOnce known the robot and goal positions in the global reference Rg, the attractive forceequation 1.6 is determined. Here X represents the robot current position and Xg the goal.Note that the goal position may vary with time, as will be shown in chapter 2.Let S represents the set of segment obstacles and let i be the parameters correspondingto the segments si in the following way:i = ArcsinK(Vsi ~)jVsij where Vsi is the segment directing vector of si and ~i id the unitary vector perpendicular tothe robot wheels axis (in this case ~Ox). K plays the same role as in equation 1.15 on page 15.The repulsive force associated to each segment si is dened by the equation 1.18 onpage 16 where is replaced by i and y by the shortest distance corresponding to the23

1.7. COLLISION-FREE NAVIGATION SYSTEM 24current segment. The resultant repulsive force is represented thus by the equation 1.19 onpage 17.The force generated by the set of obstacle points is represented by the equation 1.8 onpage 10.1.7.5 Forces in movementAs previously shown, the forces are derived in a local frame centered on a point called thecontrol point Pc. Points on the wheels axis of a robot can not be controllable as discussedin the paragraph 1.5 for only longitudinal forces can thus be expressed and consequently norotation permitted.The control point Pc is thus dene a distance ( OPc = l) from the origin on the x-axis.This point is the origin of a frame noted Rc in which force expressions are expressed. We willstabilizes this point at the goal position.Let q = (qx; qy)T be the vector determining the position of the command point Pc,X = (x; y)T be the vector determining the position of the center of the robot, be itsorientation and R = (cos ; sin )T be the vector representing the direction of the robot. Thekinematic equations of the system are: _X = vR_ = !where v; ! are the linear and angular speeds respectively. The movement equations of thecontrol point are: q = X+ dcR_q = _X dc! Rwhere R = (sin ; cos )T .Consider now the resultant force F = (Fx; Fy)T which expresses the sum of the attractiveand the repulsive forces. We propose the following command law for the linear and the angularspeeds: v = vFTR! = !FT Rwhere v ; ! are positive gains. We will develop the stability proof of this law in annex C.The maximum linear and angular speeds and accelerations being given at the entry of thesystem, we realize the saturation of the command by:dv = min(j _vj; amax) sign(dv)d! = min(j _!j; max) sign(d!)v = min(jv(t dt) + dvj; vmax) sign(v)! = min(j!(t dt) + d!j; !max) sign(!):24

1.8. EXPERIMENTATIONS 251.7.6 Local minima detectionAs seen in the paragraph 1.4.1 on page 11, a local minimum can be produced in certaincongurations of obstacles were the detection of a null force is not immediate. To detect sucha blockage, observations are made of the change in the robot conguration over n cycles. Ifthe robot remains all this time in a circle of radius rp and its orientation in a cone of angle p,a potential well detection is signaled. The two parameters rp and p depend on the maximalspeeds and accelerations in the following way:rp = vmax:dt+ 12amax:dt2 + rp = !max:dt+ 12 max:dt2 + ! (1.22)where r ; ! are positive error constants.This signal may then be treated as an error signal that interrupts execution an returnexecution to upper levels.This method being based on calculations in the global frame, this detection may failbecause of calculation errors on the robots position. A relative approach based on the integralof the linear speed on an interval of time T , thus errors related to position calculus areeliminated.The elementary curvilinear displacement of the robot during the time interval T is:S = Z T0 v:dt (1.23)The potential well signal is produced if this displacement is inferior to the minimumdisplacement Smin.1.8 ExperimentationsWe have integrated the system RtCam in the functional architecture of the robot Hilare2 in theform of a module (Avoid). This module is treated as an example in the annex A. We presenthere some experiments that illustrate the eects of the rotational potential.Figure ( 1.15 on the next page) shows, as a rst experiment, a simple obstacle avoidance.One should note the modication of the clearing distance during the avoidance phase. This isdue to the correction of the orientation of the robot as soon as it detected the obstacle. Thenon-collision is assumed by the potential p1(X) which is dominant in the region at a distanceinferior to dnc (equation 1.17 on page 16) from the obstacle. The advantage of this methodis the realization of a minimal change of orientation to avoid the obstacle. This is not anabsolute solution but is ameliorates the behavior of the robot in a multitude of situations.In gure ( 1.16 on the next page) we can see the same eect. Here the modication of theclearing distance of the rst obstacle facilitates the avoidance of the second obstacle.On gure ( 1.17 on page 27), we present an experience realized with the classical potential( = 1 always) as well as with the rotational potential. We use in both cases the same clearingdistance d0. The robot being initially at a distance inferior to d0, in the case of the classicalmethod the robot realizes an unnecessary movement away from the obstacle whereas in therotational method the obstacle has almost no eect on the behavior of the robot.25

1.8. EXPERIMENTATIONS 26© Oper

RIA(0,0)

Figure 1.15: Obstacle avoidance: eect of the dynamic adaptation of the clearing distance.© Oper

RIA(0,0)

Figure 1.16: Obstacle avoidance: passage between two obstacles.Figure 1.18 on page 28 shows a failure of obstacle avoidance. A local minimum is encoun-tered during clearance. The gure shows clearly the reason the this block which is due to acloud of ultrasonic echoes forming a virtual wall in front of the robot. This is a problem of26

1.8. EXPERIMENTATIONS 27© Oper

RIA(0,0)

Classical potential© Oper

RIA(0,0)

Rotational potentialFigure 1.17: Obstacle avoidance: rotational potential eect.multiple re ections of the echoes. We present in annex B the acquisition module of ultrasonicmeasures (US) and its two acquisition modes. 27

1.9. CONCLUSION 28© Oper

RIA(0,0)

Goal

Figure 1.18: Obstacle avoidance: local minimum due to the re ection of several ultrasonicechos.1.9 ConclusionThe potential method and its extension oer us a formal basis to realize a reactive system forcollision-free movement. Several experiments enabled us to demonstrate the eciency andthe capacity of this method as well as its deciency and its limits. In fact, potential eldcontrol increased considerably the capacity of low level execution. Even more, the rotationalpotential method, by taking into account the particularity of the robot and its state, made itsbehavior more coherent in face of situation which should have blocked the classical method.Also, the dynamic adaptation of the clearing distance and the conditions it imposes on thepotential gains, simplies the determination of these gains. In the same way, the attractivepotential function which was proposed enabled, on the one hand, a better equilibrium ofgains and, on the other hand, its renders the behavior of the robot independent whether itis near the goal or away from it.From command point of view, the potential method brings an important advantage. Theresultant force contains all the necessary information to dene a realistic and satisfactorycommand algorithm.The problem of local minima represents the major handicap of this method. We thinkthat the only way to tackle this at execution level is by the detection of this event. Weconsider that the treatment of this blocking which is a rare event is of the resort of planningor higher level decision.The extension we have proposed to the potential method presents an ecient obstacleavoidance system which will be extensively used in the following chapters.28

Chapter 2 Collision-free path tracking2.1 IntroductionIn paragraph (x 1.7 on page 19), we have presented the direct application of the potentialmethod and proposed an extension to collision-free reactive navigation (system RtCam). Thisapplication supposes a xed position in the plane. Hence, the robot tries to reach it withoutkeeping a trace of the path it is following. In fact the robots movement is purely dynamic anddepends wholly on the instant state while deciding of the next elementary movement. Such asystem is only a primary solution to the navigation problem, powerless before complex tasksin over-constrained environments. Moretheless, it is subject to potential wells. All this tendsto enhance the advantage of planning.To construct a plan, a capacity to predict the future state of the robot and the environmentis needed. This capacity relies heavily on the knowledge of the environment model. Theplanied trajectory depends, thus, on the quality of such a knowledge. While executing, theopen-loop displacement of the robot on the trajectory bears constantly the risk of collision.This is due to the robot movement uncertainty as well as to errors related to the originalmodel, not to mention that, in an evolutive environment, unexpected obstacles may aectthe execution and lead to a failure.Even more, planning usually produces trajectories using a constant minimal clearingdistance global to all parts of the trajectories. If the choice of this distance is based on exe-cution parameters, such as maximum velocity and acceleration, its value may over-constrainplanning for some trivial situations. Otherwise, if this distance is relatively small, a minordeviation risks collision.In this context, an open-loop execution of a trajectory does not seem a robust solution.This leads us to put forward a collision-free path tracking system, controlling, securingand adapting the execution and making it more robust.State of artSeveral works treated the \strict" path tracking problem. Generally, solutions consisted tojoin a sequence of positions fPig without stopping the robot (taking the robot kinematics into29

2.2. THE PROBLEM'S FORMALIZATION 30account). Thus the problem is reduced to the production of a trajectory (consigns) realizableby a non-holonomic mobile robot. For example, Kanayama and Hartman [Kanayama 89], jointhe congurations two at a time by cubic spirals whose radii are superior to those obtainedby clothodes.Delingette and co. [Delingette 91] generalized this method by using \intrinsic splines"whose curvature is a polynomial function of the curvilinear abscissa to limit the curvatureand introduce control points. We can mention also the works of Segovia and co. [Segovia 91]that use Bezier curves, of [Nelson 89] that dene curves whose radius is a polynomial functionof the polar angle, and of [Chochon 83] that uses a succession of segments and of clothodes.In [Fleury 95], S. Fleury treats the problem by considering cusp points on the trajectory.The tracking is realized by dening trajectories composed of segments, arcs, clothodes andanticlothodes. The anticlothodes are circle inductions whose radius vary proportionally tothe trajectory tangent angle (x 2.2.1).Nevertheless, few works study the problem of collision-free path tracking. In [Green 94],the authors develop an algorithm for a planar robot by dening a feedback command law.This command integrate obstacle avoidance during tracking by minimizing an error functiondened between the obstacles and xed points on the robot. This algorithm seems interestingeven though the fact that the regulation is purely on the position (x; y; ) may lead to a drifton the original trajectory even away from obstacles. We can also mention other works suchas [Khoumsi 88] which uses successive circle arcs.In chapter 5 we present a new concept to tackle the problem of reactive execution oftrajectories. Here, we will introduce a formalism and develop a set of functionalities integratedin our collision-free path tracking system.2.2 The problem's formalization2.2.1 Action insertionSince the planning issued trajectory type varies with the planner, we propose the followingarchitecture in order to build the general system collision-free path tracking that is called(RtPtCam):Let P be a trajectory generated by a certain level of planning. P is transmitted to anintermediate level having the the necessary information concerning the type of the trajectory,called pilot (gure 2.1 on the following page).Pilot is the module that generates the trajectory in the functional architecture of therobot Hilare2 (annex A). It uses four types of primitive trajectories: segments, arcs, clothodesand anticlothodes. The principal role of this level is the parameterization of the trajectory.This operation needs other entry parameters such as maximum velocity and acceleration.As a result this level produces a sequence of consigns f(t) indexed by time and containinginformation about position, velocity and acceleration necessary to execute the trajectory.The generated sequence if the entry of our system. RtPtCam.This architecture guarantees the independence of the method from the planner's type, forthe pilot level takes in charge the unication of the entry of the system. One should note thatconsequently, the sequence f(t) generated by the pilot replaces the xed G goal in RtCamcase.The following example presents some of the functionalities introduced in the system.30

2.2. THE PROBLEM'S FORMALIZATION 31Planification

Perception

Vmax

Amax

dt

dt Pilot

Trajectory follow

Command

Trajectory

Measures

Consign

Consign

Data

FunctionalityFigure 2.1: Architecture of RtPtCamsystem...

..

Ci

Obstacle

.

.

.

Cj

f(t)Figure 2.2: Trajectory memorizing idea.31

2.2. THE PROBLEM'S FORMALIZATION 32The robot moves on a trajectory by following the consigns f(t) generate by the pilot(gure 2.2 on the page before) when it meets with an obstacle (which was either unseen ornot correctly modeled) at time t0. The consigns Ci generated from this moment onwardsenter in the obstacle in uence zone. A repulsive force is then induced which obliges the robotto modify its movement to avoid the obstacle. But the goal moves always in the obstacle. Therobot will thus oscillate while trying to reach the goal and consider itself in a dead-lock. Themain reason of this behavior is the abstraction of the trajectory's geometry. Nevertheless,the reason of this abstraction is to make this problem simple and real-time applicable.Here, we propose an intermediate solution based on the partial memorization of thetrajectory before and throughout the execution in a way to keep a further piece of thetrajectory at hand. Such a knowledge of the near future makes it possible to develop adynamic goal research system. In our example (gure 2.2), the knowledge of a forward piece,enables a better choice of an intermediate goal (such as consigns Cj), thus avoiding theobstacle in a better way.2.2.2 Action denitionLet us rst dene the action: collision-free path tracking. Let S(t); t 2 [0; T ] be aparameterized curve in the conguration space A = R2 S1 for which:qs = S(0)qg = S(T )where qs; qg are the initial and nal congurations respectively. Let O be the set of obstaclesin A. Let d : A A ! R+ be a distance dened on this space (e.g. Euclidean distancebetween the positions + the absolute dierence between the angles) and dened(z; A) =Minx2 A fd(z; x)g 8 A A 8z 2 Ato be the distance between a conguration and a set of congurations in A. This distancewill be used to measure the oset of the current position of the robot to the piece of thetrajectory in consideration.Let S be a subset dened by S = fS[t1; t2]g and designing a curve verifying:8x 2 A9!y 2 Sjd(x; y) = d(x; S)S dene the piece of the trajectory issued from the memorization. It is called a slidingtrajectory (x 2.2.3).Supposing qs the initial conguration of the robot, we generate an action \Path Tracking"described by the curve S(t); t 2 [0; T ], verifyingIf the initial conguration of the robot is qs, a Path Tracking action is generated which isdescribed by the curve S(t); t 2 [0; T ] verifying:j1 f S[0; T ]g \ O = ;. This condition expresses the collision-free displacement generationalong the curve S(t).j2 Maxfd(x; S)g m; 8x 2 f S[0; T ]g. This condition imposes a maximum drift m > 0 ofthe original trajectory S(t). 32

2.2. THE PROBLEM'S FORMALIZATION 33j3 R T0 d( S(t); S)dt ; 8 2 R+: This condition generates a displacement on S(t) whileminimizing the drift from the original curve S(t). In case of a deviation due to thepresence of an obstacle, the robot should rejoin the original trajectory as soon as possibleafter accomplishing the avoidance in order to minimize . In fact this value is a surfacearea corresponding dierence between the two curves. A null value expresses thus theover-lapping of both of them.2.2.3 AlgorithmsIn this paragraph, we present the group of algorithms that we have proposed in order to buildthe system.Dynamic trajectory memorizationConsider lm, the length of the memorized curve, as an entry to the system. The memorizationoperation denes a partial curve on the initial trajectory that we call the sliding trajectory.This curve can be described by the following condition:f(t) = f(t); 8t 2 [T1; T2] such that, Z T2T1 v(t)dt = lm (2.1)where f(t) is the sequence of consigns dened in 2.2.1 on page 30 and v(t) represents therobot speed at time t. In fact, each conguration of f(t) have the following information: q(t) = 0@ x(t)y(t)(t) 1A is the robots position. V (t) = v(t)w(t) is the vector of linear and angular speeds.Note that the condition 2.1 may not be satised when the remaining length is inferior tolm, in which case, the memorization is stopped.If we suppose a known frequency for the generation of consigns by the pilot, the executionwill be behind the generation of consigns. This is due to the dependency of the memorizationon this frequency.Otherwise, if L represents the current length of the sliding trajectory, L may have a valuesuperior to the given value lm at the entry of the system (depending on the state of thesystem as in the next paragraph). This may happen when avoiding obstacles.Attraction pointHaving dened the sliding trajectory, f(t), we present the attraction point research algorithm.Let q = f(s)8s 2 [0; T 0] be a conguration on the curve f(s), withs = t T1 ) f(0) = f(t T1)where T1 id the moment corresponding to the rst conguration of the sliding trajectory onthe global trajectory.We distinguish three possible states of the RtPtCam system:33

2.2. THE PROBLEM'S FORMALIZATION 34Nominal state: The robot is on the global trajectory. Let p(t) be the position of the robotand Fr(t) the repulsive force associated to the obstacles at time t. The system thensatises the following conditions: p(t) = f(0)Fr(t) = 0 (2.2)The rst equality expresses that, in nominal execution mode, the memorized piece\slides" ahead of the robot, and thus the robot position occupies the rst congurationon f(t). The repulsive force is also null. The attraction point is then:q = f (ds) (2.3)This equation governs the perfect execution of the trajectory. ds is the time incrementrelative to the sliding trajectory that corresponds to the instant t + dt on the globaltrajectory f(t).−f(s)

p(t) p(t)

q

li

p(t)

−f(s)

q

ll

Avoidance SmoothingNominal

q = f(ds)−

−p(t) = f(0)Figure 2.3: The dierent states of the RtPtCamsystem.Avoidance state: The robot begins to deviate o the global trajectory:p(t) 6= f(s) 8s 2 [0; T 0]jFr(t)j > 0 (2.4)This means that the robot enter in the obstacles in uence zone, that the repulsive forceincreases and, thus, that the robot leave its nominal trajectory.In the avoidance state the attraction point is determined by:q = f(s+ s); j s 2 [s; T 0 s]; if,( f(s) p(t)): _p(t) = 0;f(T 0) else-wise. (2.5)and: li = Z s+ss _f(t)dtwhere _p(t) represents the instantaneous velocity of the robot and T 0 the instance cor-responding to the last conguration on the sliding trajectory.The signicance of this equation is that if the conguration f(s) satises the projectioncondition in the robots local map, we take an li length advance on the trajectory(gure 2.3, \avoidance"). The value of li is determined by the the time interval s. This34

2.2. THE PROBLEM'S FORMALIZATION 35parameter is called the anticipation distance which reserves a distance on the trajectoryto make the successful avoidance of the obstacle surer. This is done by attracting therobot by a point representing a more suitable topological evolution of the trajectory.q takes the last conguration on the sliding trajectory when the projection condition isnot satised.Smoothing state: This state is always preceded by an avoidance state during which therepulsion force is neutralized and the robot tries to rejoin the trajectory. The systemsatises, thus, the conditions: p(t) 6= f(s) 8s 2 [0; T 0]Fr(t) = 0 (2.6)In order to nd an attraction point q the rst conguration q = f(s) corresponding tothe robots position projection along the velocity vector (gure 2.3 on the page before,\smoothing"), which is dierent from the previous state. Still we use a notion similarto the anticipation distance which is used to realize a special smoothing enabling therobot to come tangentially on the trajectory.The attraction point q is determined by:q = ( f(s+ ~s); j ~s 2 [s; T 0 s]; is (p(t) f(s)): _f(s) = 0;f(T 0) else-wise. (2.7)and: ll = Z s+~ss _f(t)dtwhere ll is the value of the smoothing distance dened by the parameter ~s.The state change condition when reaching the trajectory is:d(p(t); f(t)) < T ; T ! 0where T is an innitismal positive constant.One should note that the sliding trajectory memorization in the last two states (avoidanceand smoothing) continues from the instance T1 where the robot leaves the trajectory. Other-wise, the length of the sliding trajectory may increase in this case and the congurations onthe beginning of the trajectory are not freed.System statesThe formalism presented in the previous paragraph characterizes the passage conditions be-tween the dierent states of the system RtPtCam. gure 2.4 on the following page shows thechanges of state in function of the variation of the repulsive force Fr and the position of therobot relative to the trajectory. 35

2.3. ENVIRONMENT MODEL INTEGRATION 36|F | > 0r

F = 0r

F = 0rN(p(t) − f(t)) < ε

T

|F | > 0r

Normal

State

Avoidance

State

Smoothing

state

Figure 2.4: The passage between the dierent states of the system RtPtCam.2.3 Environment model integrationIn paragraph (x2.1), we have presented the problem of trajectories passing at a minimaldistance from obstacles. (Here we suppose a purely geometrical planner for other plannertypes such as those based upon sensor referenced commands do not present the same prob-lem.) In fact, certain tasks require maneuvering near obstacles while realizing rather dicultmovements (parking, port passage, : : : ), which obliges the planner to use a small avoidanceparameter.Suppose now, that this clearing distance is associated to that relative to the path trackingreactive system. During the trajectory execution, the coordination of the choice of thisdistance between the planner and the reactive system does not bother the execution in whatconcerns the obstacles already known to the planner. Still, this choice will constrain theexecution in presence of unexpected obstacles. On the other hand, if we choose a moreimportant clearing distance, the reactive system will nd it dicult, even impossible, tofollow the trajectory close to obstacles, including those who are already known. Or else, ifwe relate the execution velocity to the clearing distance, a small velocity will be imposed toall the trajectory. How then should the clearing distance be determined?The proposed solution in based upon the dynamic modication of the clearing distance infunction of the type of the sensed obstacle. Known obstacles should thus be distinguishedfrom unexpected ones. To do this the environment model used in the planning should bealways available.In all cases, to interpret the environment model is not an easy task in real-time context.The obstacle $ model matching complexity depends directly on the environment and thetime needed to realize this operation is not compatible with real-time constraints.Matching methods (even time bounded ones) being dependent upon the environmentrepresentation type used by the planner, they make our system loose its genericity. In thiscontext and in order to unify the perceptual entries of the reactive system, we propose to36

2.3. ENVIRONMENT MODEL INTEGRATION 37create a treatment parallel to (real) perception which is called virtual perception.Position

Sensor

Model

Environment Model

Virtual measures

Virtual

Perception

Figure 2.5: Virtual perception.Virtual perception maintains a dynamic representation of the environment model andproduces virtual perception data of the same type as the real perception system (x2.3.2). Inother words, this level take the environment model, the sensor models used by the reactivesystem and the current position of the robot. It exports virtual measures (gure 2.5). Thisvirtual perception enables the production at the entry of the reactive system, virtual measuressynchronized with real measures. These two sources of measures will be matched easilybecause they correspond to the same instantaneous robot and the matching time will be timebounded since the number of virtual measures generated will be of the same order of the realmeasures. The model constructed by this matching between virtual and real perception isthus simpler and more complete.This functionality in then naturally included real-time context. This brings about theproblem of complexity of in function of the model representation. Here, and in order tobound the virtual measures treatment time, we propose the following algorithm of dynamicrepresentation of the environment by virtual perceptual data.2.3.1 From the model to virtual measuresAs we have shown before, the virtual measures calculus complexity depend on the type of themodel representation. As an example we will rst consider a polygonal model represented bya set of segments.Let S be a set of segments which represent the environment model in the frame Oxy andlet q be the sensor conguration vector. In the case of a model such as the laser range nder,the measure corresponds to the intersection of the half line dl whose direction is u (which37

2.3. ENVIRONMENT MODEL INTEGRATION 38q

σ

pl

Si

cD

v

d1

d2

Dl

u

pc

dc

dl

vs

Figure 2.6: Virtual measure.represents the shooting distance), with the segment Si (gure 2.6). To calculate this measurethis intersection should be veried The distance Dl to the intersection point p1 and the theintersection conditions are given by:D = K(u^ vs) 6= 0 ; = K(u^v)D 2 ]0; 1[ ;Dl = K(vs^v)D 0 :where K : R3 ! R is a function returning the z component of the vectorial product and 2]0; 1[ expresses the intersection condition.In the case of the ultrasonic sonar, the detection cone opening 2 (gure 2.6) denes anangular sector limits by two half lines d1; d2. This complicates the intersection test, becausethe exact calculus necessitates multiple intersection tests in order to deduce the intersectionresult. Nevertheless, the sampling of the angular sector by several half lines may simplifythe problem and give a suciently representative solution for this application. Supposethat the sampling gives three half lines d1; dl; d2 (the two extremities and the center). Theintersection test is satised if one of the half lines intercept the segment and the distanceis thus the shortest distance of interception. Note, that in case of gure 2.6, the returneddistance is Dl where as the real shortest distance returned by the sensor will do Dc. In fact,we do not want to model by this system the used sensors. We want to generate even exactmeasures corresponding to the model each cycle of time. These measures are used by thereactive system to realize matching tests.We could remark that the algorithms complexity id m n where m is the number of dis-tance calculations for a segment and n i the number of segments in the model. Consequently,for a number m constant, the treatment time becomes a function of the complexity of themodel (which varies in function of the application) and thus the time bound is not known inadvance. In this context and in order to limit this time the following idea is proposed:Before execution, the model is projected in a global map represented by a bitmap, theposition of the robot is updated in this map and the virtual measures are dynamicallycalculated in this map.38

2.3. ENVIRONMENT MODEL INTEGRATION 39© Oper

Environment modelProjection in the bitmapFigure 2.7: Example: virtual ultrasonic measures issued from bitmap calculus.

39

2.3. ENVIRONMENT MODEL INTEGRATION 40For a certain choice of the acquisition and sampling parameters, the number of measures,m, to calculate is constant. Since the calculation of a distance measure in a bitmap isbounded, the global time does not depend on m and it is thus bounded.The interest of this representation is in the generation of the number of necessary measuresin each cycle. The quality of these measures depend on the sampling of the bitmap. (Thisshould be taken into account in parameters matching in what follows.)Figure 2.7 on the page before shows the robot position in the polygonal environment withmeasures issued from bitmap calculus.Note: The integration of this virtual perception enables the denition of virtual obstacleswhich will be taken into account during execution. This is a typical tool for dening virtualcorridors around the trajectory in order to control some how the behavior of the reactivesystem. On the other hand, this perception can be used eciently to produce a simulationtool whose sensor model can be ameliorated using noise as well as specularity test.© Oper © Oper

Figure 2.8: Simulation: Virtual ultrasonic measures with(out) a detection cone.We show in gure 2.8 the measures generated by virtual perception for the simulation.note the eect of the detection cone opening on the measures.2.3.2 Matchingsv

d1 d2

s

εs

εd

sv

s

d1d2

εθ

(Ι),(ΙΙ)Figure 2.9: Matching two segments..We have seen in paragraph 1.7.2 the representation of obstacles by real measures. Thismethod is applied in the same way as virtual measures, thus generating a set of segments Svand a set of point obstaclesMpv. We are interested in real-time matching in local coordinates40

2.3. ENVIRONMENT MODEL INTEGRATION 41for a robot without any notion of memory. This operation is very sensible to the precision ofthe position of the robot in the global frame.Once the real and virtual segment sets obtained, matching is done in the following way:Let s; sv be vectors representing a real and a virtual segment respectively (gure 2.9 onthe page before, I) and satisfy the condition s sv 0. The two robots are said to be matchedif jK(s^d1)2 j+ jK(sv^d2)2 j s(jsvj+ jsj);jd1j+ jd2j jsv j+ jsj+ 2d;jcos1( sv sjsv jjsj)j :where s; d; are positive constants having the same signicance as in paragraph 2.3.1). Therst condition bounds the surface between the two vectors, this limit depends on the lengthsof the two segments and the sapling step of the bitmap. The second condition bounds themaximum distance between the two segments (gure 2.9 on the preceding page, II). The thirdcondition is a collinearity condition, the maximum angle between the segments is determinedby the constant .As for the point obstacles, a distance test is enough to match them. They have theconvenient property of being ordered the same way in both sets.2.3.3 System parameters adaptationSince known obstacles could now be distinguished from unknown ones, adapted clearingdistances may be associated to obstacles in function of their type. If dp is the distance usedby the planner, the distance associated to known obstacles is:doc = dp + ocwhere oc is a positive error constant, while the distance associated to unknown obstacles isdetermined in function of the maximal velocity and acceleration on the trajectory. Let Ts bethe time necessary to decelerate from maximal speed to zero, this distance is:doi = 12amaxT 2s + oi; avec: Ts =Max(vmaxamax ; !max max ):where oi is an error positive constant. To calculate Ts the angular acceleration capacity ofthe robot should also be taken into account.Note: In the path tracking application, we propose to make oc depend upon the curvilinearcoordinate s, while keeping in the same time a minimal and maximal bound to it. Figure 2.10on the next page shows an evolution of this term.Figures 2.11 and 2.12 show two executions of the trajectory while avoiding obstacles. Thetrajectory is composed of segments and arcs parameterized by the pilot module and executedby the system RtPtCam. Note on the gure 2.12 the dynamic modication of the clearingdistance in the nal phase of execution.The experiments presented in gures 2.13 and 2.14 show the minimal adjustment necessaryfor the execution of the trajectory. Note the drift of the model in the y-axis direction in thecorridor which produced a matching failure. Nevertheless, the robot succeeded in rejoining41

2.3. ENVIRONMENT MODEL INTEGRATION 42εoc

max

min

SFigure 2.10: Evolution of the term oc.© Oper

Goal

Figure 2.11: Obstacles avoidance during path tracking.42

2.3. ENVIRONMENT MODEL INTEGRATION 43© Oper

GoalFigure 2.12: The eect of the dynamic modication of the clearing distance during pathtracking.© Oper

Figure 2.13: Path tracking: Leaving the Salle.43

2.4. DISCUSSION 44© Oper

Hi, this is me.

Figure 2.14: Path tracking: Entering the Salle.the trajectory. This is due to the rotational potential that aligned the robot to the wallneutralizing thus the repulsive potential.In order to show the eect of the projection on the trajectory, we have assigned animportant value to the parameter m (x 2.2.2 on page 32) which permits a big drift from theoriginal trajectory. We have also modied the position on the robot (on the x-axis of theglobal frame of the Salle). Thus, the planned trajectory passes through an obstacle. Figure2.15 shows the consequent behavior of the track algorithm. The system RtPtCam maintainsthe avoidance state while projecting itself on the trajectory. This is possible because of thesliding trajectory issued from the memory. Finally the attraction point is attached to the goalposition and a local minimum is encountered thus leading the execution to a failure.2.4 DiscussionThe conception of the system RtPtCam as proposed here ameliorates the behavior of thereactive system during execution. standardizing the entry of the system enables the treatmentof all sorts of trajectories without demanding the knowledge of the several primitives usedby the planner. Also, the memorization of the trajectory and the notion of sliding trajectoryameliorates notably the avoidance behavior in case the RtCam system fails.The integration of virtual perception enabled the dynamic adaptation of the clearing dis-tance and thus a better behavior in constrained zones. Finally, this virtual perception is apowerful simulation utility that we have ameliorated further by using sensor models including44

2.4. DISCUSSION 45© Oper

Figure 2.15: Path tracking: projection on the trajectory.specularity, detection cone, noise, : : :The integration of the system in the functional architecture of our \dear" robot Hilare2demonstrates the necessity and the eciency of the dierent control parameters that char-acterize the execution (maximum drift, dierent states of the system, obligatory passage bycusp points, : : : ) especially in the context of the multi-robot coordination projects (Europeanproject MARTHA1, internal project STRADA) [Alami 95].1A European project ESPRIT III; MARTHA: Mobile Autonomous Robots for Transportation and HandlingApplications. 45

Part IISensor-based motion

46

Chapter 3 Sensor-based actions3.1 IntroductionThe execution plan generated by a planning level varies according to the type and strategyof the planner. An execution plan based on the generation of movement in free-space maybe: A sequence of (reachable) congurations. A trajectory composed of a sequence of geometric primitives. (broken lines, segments,arcs, clothoids, anticlothoids, : : : ) A parametric analytic function. (Bezier, spline, : : : )We have seen in the preceding chapter that trajectory execution is not secure without thepassage by a reactive control system insuring non-collision in real-time. In fact the interestof such a system resides on the security of execution and the coherence of the execution withthe generated plan is not guaranteed. Not taking into account the uncertainties related tothe environment real model and the robots movement may lead to an execution failure.In such a context, the combination of free-space movement and sensor-based movement inthe planning strategy is unavoidable. This condition is in fact sine qua non for the security ofthe execution as well as for the generated plan to be properly executed when taking accountof uncertainties.Several approaches were proposed in literature to treat the dierent aspects of the prob-lem of sensor-based actions and displacements. These approaches are dierentiated by theperception type, on one hand, and by the actions complexity, on the other.Using the notion of task function proposed in [Samson 90b] in the working context de-scribed in [Espiau 92] concerning active vision, [Rives 93, Pissard-Gibollet 93, Pissard-Gibollet 95]proposed system of sensor-based movement by visual feedback of a mobile platform. Severalsensor referenced tasks such as wall following, door facing, : : : , were treated. An interestingaspect of this system is the addition of liberty degrees related to perception, which enables thecreation of a convenient cooperation between the mobile platform and the sensor feedback.The task is then programmed as a set of task functions (sensor-based actions) sequenced andcontrolled by the system orccad [Coste-Maniere 92].47

3.1. INTRODUCTION 48In fact, the core of these works concerns the execution of a task modeled by regulating tozero some task function which expresses the drift between the nal and the current perceptionstates. The nal state is thus predetermined in the camera frame before execution, whichsignies a transformation of a 3D space to a 2D image for an initial predened congurationfor the couple robot-camera. The non existence of parallel treatment of task functions, makesit impossible to handle \relatively complex" tasks involving actions that could not be realizedin a simple sequence. Thus, to follow a wall while avoiding obstacles (if possible) must beexpressed in a single task function or a single automate combining both actions. In fact,a sequential transition between the two separated task functions (wall follow and obstacleavoidance) does not guarantee the accomplishment of the global task.Using the same formalism of the task function, [J. Koseck 96] proposes a mobile robotdisplacement system with visual feedback. The environment is a graph whose knots representthe dierent congurations corresponding to the robot landmark relative locations . In thisgraph, the arcs represent the command strategy moving the robot from one location to theother. The navigation is then a sequence of positioning tasks. Even though it is well knownin literature [Bouilly 95a], this navigation approach presents an interesting idea concerningenvironment representation. Its most important inconvenience is the preliminary constructionof links between the robot positions associated with landmarks and the nal perception state.And also in a restrictive navigation capacity due to obligatory passages by xed positioncongurations.In a specic domain related to sensor-based road follow applications, Dickmanns proposedin [Dickmanns 89, Dickmanns 95] a visual feedback root follow system. The basic idea is alocal analysis of the environment ahead of the vehicle used to plan the next trajectory. Theenvironment model is thus constructed locally. Several procedures to recognize objects andhumans are also developed. Nevertheless, this method does not treat a genuine feedbacksensor motion since there is no relation between the censorial feedback and the trajectorycommand.In the Wall Follow action context, [van Turennout 92] proposed a feedback control algo-rithm using ultrasonic sensors1. This algorithm is based upon wall distance measure cor-rection as well as on the estimated orientation between successive measures. Orientationcalculation is simple and sensible to error because it is based upon elementary displacementsof the robot between measures and upon the measures themselves. The least error on mea-surements may lead to big variations of the orientation. Even more, this algorithm does nottreat the problems related to the presence of obstacles.In the same domain, Ando [Ando 95], proposes a wall follow algorithm while taking intoaccount the ultrasonic measures specularity. This algorithm does not represent a real sensorfeedback control, for the movement is realized by a sequence of locomotion primitives suchas segments, rotations, arcs, : : : . These primitives are updated every 3cm. Consequently,the passage between two primitives such as (segment $ arc) without a smoothing algorithmgenerates a command discontinuity (except if one passed by a stop). The algorithm shouldalso be redened for dierent placements of sensors.Through a new formalization of Task Potential, we present in this chapter the applicabilityof the potential method in the context of sensor-based command. On this formalization, wedevelop several feedback actions used in robotics. Then we present the eciency and theutility of these actions in the context of planning with uncertainty.1The author announces that the use of several sensors is more ecient though he does not treat this case.48

3.2. THE \TASK POTENTIAL" 493.2 The \Task Potential"We have presented in chapter 1 the principle of potential feedback. In order to introduce itsapplicability on sensor-based motion, let us analyze the following example:To bring the robot to a given conguration, we apply an attractive potential which de-pends on the distance function to this conguration and which have a single minimum. Theforce, gradient of the potential, will make the robot move towards the goal. Note that theonly useful state feedback is the distance to the goal. Using an odometric feedback and ele-mentary displacement integration, the robots position is regularly updated and the distanceto the goal is easily found. In fact, this distance measure may be realized by another meanssuch as perception (identiable and localizable goals). This does not change the behavior ofthe system from command point of view.2. In this context, the same displacement may begenerated by a command using the exteroceptive feedback in place of the proprioceptive one.Generally a command is called sensor-based if it is based on exteroceptive feedback.Task Potential FormulationGenerally, a potential function is dened on the operational space as function of the currentposition of the robot. In the context of sensor-based command, since the environment isnot (totally) known, it is not possible to dene the potential function on the whole space(or subspace). Still, if the perceptual data may determine the position of the robot at everymoment in some global frame, we can dene a global potential which evolve in time in order torealize the required action. Consequently, the realization of the task is insured if the evolvingpotential minimum converges towards a unique global minimum. The attractive potentiallinked to a mobile goal is a typical example of such a potential.On the other hand, if the perceptual data does not permit the expression of the robotposition in a global referential, the potential must depend directly on perceptual data andthe generation of the displacement must be done incrementally in function of the evolutionof the environment. This is the case of the repulsive potential associated with the distanceto the obstacle. We will develop the notion of evolutionary potential by an example on afeedback action.Evolutionary potential notion: Imagine that the action to realize is a movement to contact (seedetails in paragraph x 3.3.1 on page 51). In practice, and because of the maximal range ofthe sensors, the distance to contact is not always determined. Nevertheless, in such a case wehave the information that the contact is at a distance greater that a maximal distance relatedto the sensor. This maximal distance generated an attraction force producing an elementarydisplacement. As long as the object is not detected, the norm of this force remains constantand consequently, it is not a conservative force. We shall represent in paragraph (xC.2) howto control such a force. Any way, this shows an evolutionary potential function. The eectwould be the same if the object representing the contact was itself moving.The evolution of the potential function, being dependent on the environment, it must becontrolled by control functions xing the global behavior of the robot according to the taskit should realize.In this general context of execution control of potential feedback motion, we propose thefollowing formalism of Task Potential:2We suppose the same quality of measure in both cases.49

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 50Let Fi(t); t 2 [0; T ]; i 2 [0; : : : ; n] be scalar functions of time, positive and monotonous.Let q 2 <n be the robot conguration vector. Let Pi(q) be positive dierential convexpotentials. The potential P(q; t) issued from a linear application of the form:P(q; t) =Xi Fi(t)Pi(q); 8t 2 [0; T ] (3.1)is a convex potential whose unique solution, qm, veries:@P@q jqm;t=T = 0Proposition: The realization of the task control may be expressed by the decent of the gradientof a potential function P(q; t); t 2 [0; T ], composed linearly of several convex potentialfunctions whose coecients are scalar functions of time, positive and monotonous.Proof: According to convex analysis, a linear composition of convex functions is a convexfunctions [Gamkerlidze 80]. Thus P has for each t 2 [0; T ] a unique minimum. The task torealize corresponds to the global minimum at t = T .If the linear application id a pure convex composition, we write:XFi(t) = 1; 8t 2 [0; T ]Fi(t) 2 [0; 1]; 8t 2 [0; T ]In fact, the convexity condition insures the uniqueness of the solution for all t and themonotony of the functions Fi(t) guarantees the asymptotic convergence of this solution as ttends towards T .Corollary: This linear composition may be stationary or evolutionary in time; the coecientsmay be constant.Example: Let P be given by:P = f(t)(q q1)2 + (1 f(t))(q q2)2where q1; q2 are two xed congurations. Let f(t) be an increasing function such that f(0) =0; f(T ) = 1. The minimum of P at time t = 0 is the conguration q = q2 whereas theminimum as (t ! T ) id the conguration q = q1, consequently the asymptotic stability ofthe system is insured by the function (q q1)2.We develop, in the following paragraphs several applications based on the Task Potentialin order to show the interest of this formalism.3.3 From Task Potential to sensor-based actionWe will dene in this section a set of sensor-based actions and develop them in the contextof Task Potential. Dierent task potentials are constructed from a certain number of basicsimple and generic potential functions. Nevertheless, other user-dene potential functionsmay be used satisfying the task requirements and standard potential properties.In chapter 4, we present many experiments in the context of planning with uncertainty.Here 50

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 513.3.1 Movement to contactIn this section the robot is required to advance or retreat until is encounters an obstacle in apredened compact zone and at a given distance. If no obstacle is detected in this zone therobot should stop.Action's denition:Let Rg(Oxy) be a global frame in the operational space related to the conguration spaceA <3, X = (xy)T be the robot conguration vector and O the set of stationary objectsin A. Suppose that v1;v2 are two vectors determining an angular sector relative to therobot centered on its x-axis (gure 3.1). Let d : A O ! <+ be a function returning theshortest distance between the robot and the set O O, the restriction of O to the sector .If the robot is at the origin of the frame Rg (X = 0), and if its velocity is null, we generatean action Advance to Contact described by the curve S(t); t 2 [0; T ] such that:V2

V1

Rg

XY

Ω

dc

S(t)

O

Figure 3.1: Advance to Contact action.S(0) = _S(0) = 0 j1fS[0; T ]g\ O = ; j2d(S(T );O) = dc j3d(S(t);O) 6= dc 8t 2 [0; T [ j4_y(t) = _(t) = 0 j5where dc is a positive constant representing the distance to contact.The rst condition determines the initial state of the robot. The non-collision on thewhole of the trajectory is insured by the condition j2 . At time t = T , the condition j3 meansthat the shortest distance between the robot and the set of obstacles O is the distance tocontact dc. (This distance is xed by the user.) The uniqueness of the conguration S(T )which ends the curve (displacement) is expressed in the condition j4 . The movement ischaracterized by the condition j5 that expressed an displacement in a strait line.51

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 52Task Potential:We dene, now, the Task Potential which will realize the action. Let d be the vector corre-sponding to the closest point in and ~i be the unitary vector on the x-axis. We dene thedistance dx as being the norm of the projection of the vector d on the x-axis. Then:dx = dT ~iand thus the distance to contact on this axis becomes:dcx = dxjdjdcThe Task Potential necessary for the realization of this task must be convex, non negative,innite on the objects and nil for dx = dcx. its derivative should be negative on the interval]0; dcx[ (obstacles repulsion), positive on the interval dx > dcx (goal attraction) and constantfor dx dm (saturation of the attractive force). Here dm is the maximum measurabledistance.. We give the expression of the dierent functions which build this potential:P1(dx) = 2( 1dx 1dcx )2P2(dx) = kc2 (dx dcx)2P3(dx) = kcm(d dcx) kc2 m2where, kc are positive gains, with m = dm dcx. The Task Potential id then:P(dx) = 8<: P1 + P2 8dx 2 [0; dcx];P2 8dx 2]dcx; dm];P3 8dx > dm: (3.2)P is a convex function (since its epigraph is convex [Gamkerlidze 80]) which is non negativederivable. Its unique minimum dcx = dx is null (gure 3.2 on the next page). The force issuedfrom it is: Fc = 8><>: d2 ( 1dx 1dcx ) + kc(dcx dx) ~i 8dx 2 [0; dcx];kc(dcx dx) ~i 8dx 2]dcx; dm];kc(dcx dm) ~i 8dx > dm:The force vector being thus on the x-axis, it will lead the robot to minimize the osetdxdcx by simulating a pure linear accumulation. In order to realize the asymptotic stability,we add on the command level a dissipative force proportional to the velocity v. We have thus:Fc = (Fc kvv)~iwhere kv is a positive gain constant. Fc is the command force.52

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 53P(d)

-F(d)

0 d0 dmd

Figure 3.2: The Task Potential of the Advance to Contact action.Execution control:rst, we introduce the following parameters:Lm 2 <+: the maximum constant displacement on the curve S(t) while the robot mustnd the contact (dcx = dx).lm(t): a time function representing the oset between the real robot displacement attime t and the maximal length Lm allowed to look for contact. this function is givenby: lm(t) = Lm R t0 j _S(t)jdt; if Lm R t0 j _S(t)jdt > 0;0 elsewise.lm(t) is a non negative continuous function.In order to realize a maximal displacement Lm while searching for contact, we control theforce by applying a linear application on the potential P who is a multiplication by:fLm(t) = klm(1 elm(t))which is a positive monotonous function. klm is a positive (frequently unitary) gainconstant and is a time constant which determines the state switch slope of the function.The force expression is rewritten as follows:Fc = (fLm(t)Fc kvv)~i; 8t 2 [0; T ]When fLm(t)! 0 the force Fc becomes a dissipative force and the linear velocity of therobot tends asymptoticly to zero.Analysis:In fact, the use of the control function is the continuous neutralization of the force when therobot realizes a long displacement (superior to Lm) without succeeding in the research of53

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 54flm

= 1

L = lm

λ = 10

λ =1

∆ l mFigure 3.3: parameter eect.the contact (the potential minimum). the parameter is determined in function of the cycletime and of a dissipative factor kv. This determines in its tour the prole of the robot stopin case of a failure. In practice, its value must me relatively high in order to keep a unitaryvalue of the function for a displacement length l near Lm. Figure ( 3.3) shows the the eectof .In the same way, we can dene a control function to terminate the execution if the robotrealizes a displacement l > Lblind during which the perception did not detect any object.The length Lblind is xed in function of the maximum length Lm and in function of the usedsensor.We have made the hypothesis that objects are stationary in this action denition. This isonly necessary to guarantee the non-collision since there is no complete treatment of all theobjects in the set O which is the set of objects not contained in the sector . Nevertheless,in practice, we dene a purely repulsive potential (equation 1.16 on page 16) associated tothe set of measures in O. Consequently condition j5 is no longer satised.Experimentation:We show in gure 3.4-I- the track of the execution of the action Advance to Contact. Themeasures are issued from ultrasonic sensors (see annex B). Note the linear speed prole duringexecution in gure 3.4-II-, the acceleration phase is purely linear due to the force saturation(constant force ) constant acceleration ) linear slope). In the deceleration phase, thelinearity is lost. This is due to the variation of the force according to sensor feedback. Theacceleration is thus no longer constant . We have associated in this experiment a big valueto Lm, thus the force annulation depends wholly on dx dcx.Utility:Typically, in planning with uncertainty, the planner searches to generate displacement tominimize the robots uncertainty. gure 3.5 on the following page shows the eect thatAdvance to Contact may have on the evolution of the uncertainty. If Ua is the uncertaintymatrix on the position of the robot, the planner veries the non-collision given the uncertaintyand generates this action. In the nal state, the obstacle being localized by the segment Sand the distance d, the uncertainty along the x-axis as well as the orientation can be reduced(the matrix Ua). Note that the uncertainty on the y-axis increases.54

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 55-I-

Speed

Time (secs)

Vel

ocity

(m

/sec

s) -II-Figure 3.4: -I- Action Advance to Contact execution trace, -II- speed corresponding to theexecution.Ua U

a*

S

d

x

y

oFigure 3.5: Action Advance to Contact utility55

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 56Note: The denition of the zone using the two vectors v1;v2, determines the robot move-ment direction. The angular opening of the sector is\v1;v2 2 [0; ].3.3.2 ParallelizeThe aim of this elementary sensor-based action is to position the robot parallel to a wall bypure rotation maneuvering.Action's denition:Here we use the same denition as in paragraph (x3.3.1) and suppose the angular sector centered on the y-axis. Also the parameter is dened as in paragraph (1.6). Supposing thatthe position of the robot is at the origin of the frame Rg (X = 0), we generate a Parallelizeaction described by the curve S(t); t 2 [0; T ] dened by:V2

V1

Rg

XY

Ω

α

Γα

Figure 3.6: Parallelize action.S(0) = 0 j1fS[0; T ]g \ O = ; j2(T ) = 0 j3_x(t) = _y(t) = 0 j4The rst two conditions have the same signicance as in the previous paragraph. Thecondition j3 expresses the nal state of the robot relative to the object (which has somenotion of orientation). The generation of a pure rotation is assumed by condition j4 .Task Potential:We give the expression of the Task Potential that realizes this action:P = 12k256

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 57where k is a positive constant. P is a derivable non-negative function whose unique minimumis = 0. The moment issued from this potential is thus: = kThe couple makes the robot minimize the angle by producing a pure angular accel-eration. The stability around zero is guaranteed by: = k k!!where k! is a positive gain and ! is the angular velocity of the robot. is the commandcouple.Execution control:XY

O1

O2

θmin

Figure 3.7: Ambiguity between two object for parallelize action.As in Avoid Contact, we could introduce a linear application with a control function thatneutralizes the couple for angles greater than some m. Also a new parameter min mustbe added to remove the ambiguity that may arises in what concerns the object of reference.gure 3.7 presents an ambiguous situation. This parameter determines a minimum rotationangle to guarantee before getting parallel. This is done by dening the following attractivepotential: P = K2 (min )2where k is a positive gain. The issued couple is: = K(min )We, equally, dene the following control function:f(t) = ej(t)j (3.3)57

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 58where the angle (t) is given by:(t) = min R t0 !(t)dt if min R t0 !(t)dt 6= 0;0 elsewise.(t) represents the remaining rotation before beginning the parallelizing. = 1

λ = 10λ =1

θf (t)

∆θ(t)∆θ = θminFigure 3.8: The evaluation of control function f(t) in function of .The parameter in this function has the same role as in the previous paragraph (g-ure 3.8). We will now eectuate a convex composition of potentials using the function f(t).The expression of the couple is = f(t) + (1 f(t)) k!!The function f(t) neutralize the couple associated to during minimum rotation phasetowards min.Analysis:The parameter min is given by an upper level (the planner). An erroneous value of thisparameter together with a high value of may generate an oscillation at the end of thisphase. min must have a value at which the couple has always the same sign as .The convex composition enables the realization of the functionalities of this action in acontinuous way and makes possible, in the same way as in the case of Advance to Contact, tostart the execution before the localization of the object.Experimentation:gure 3.9-I- show the execution trace of the action Parallelize. dierent parameters are unusedby the command. These are presented in gure 3.9-II-3 where: W Is the angular velocity !(t) during execution. Alpha id the parameter and DTheta is the angle (t). Torque Alpha represents the part kf(t) of the command and Torque Theta rep-resents .3In the gure the values of these parameters before execution are meaningless58

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 59

-I-0

0.2

0.4

0.6

0.8

1

1.2

1.4

3 4 5 6 7 8 9

wAlphaTorque_AlphaDThetaTorque_Theta

Time (secs)

Com

man

d P

aram

eter

s

-II-Figure 3.9: -I- Parallelize execution trace, -II- the prole of the dierent command parameters.59

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 60Utility:As for Advance to Contact, relative to uncertainty planning, this command enable the po-sitioning of the robot in a precise position relative to an object (e.g. wall) before chainingsuccessive follow commands.3.3.3 Wall followThe aim of this action is to advance parallel to a wall maintaining a xed distance from itwhile avoiding obstacles.Action denition:We use the set O, the parameter , and the robot conguration X given in the paragraphs(x3.3.1 and 3.3.2).Let Rw(Oxy) be a frame associated with a segment SfO; xpg such as shown in gure 3.10.Let d be the distance from the center of the robot to the segment, y = d. If dw is the requiredfollow distance and if represents the segment orientation an the robot frame, a Wall Followaction represented by the curve S(t) = [x(t)y(t)(t)]T ; t 2 [0; T ] such that:Py PXPc xPO S S(tm)y dw = Fy FxS(t)y = dRw xFigure 3.10: Wall Follow action.S(0) = X0 j1fS[0; T ]g\ O = ; j29tm8t > tm; y(t) = dw (t) = 0 j3_x(t) > 0; 8t < T j4S(T ) = XT = 0@ xpdw0 1A ; _S(T ) = 0 j5is generated.The rst condition represents the initial state of the robot in the frame Rw. The non-collision is guaranteed by the second condition all along the curve S(t). The condition j3characterize the robot conguration relative to the wall to follow after an interval of time60

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 61xed by tm (gure 3.10 on the preceding page). The movement direction and the requirementof a longitudinal velocity component are expressed by the condition j4 . The nal state of theaction is precised by the last condition.Task Potential:The basic idea behind this action is to apply an attractive force in the direction of the x-axis,function of the distance xpx(t), where xp is the abscissa of the edge of the segment S. Thisforce Fx is issued from a potential dened on the x-axis with a minimum at x(t) = xp. Thetraversal correction (on the y-axis) of the follow distance dw is realized by another potentialdened on this axis function of the distance y(t) dw. Also the orientation of the robot iscontrolled by a rotational potential P, function of the parameter which represents thewall orientation. The couple will also contribute to optimize the distance to the wall. Theinterest of this last potential shall be discussed in paragraph Sensor problems.we present (in a semantic way) on gure 3.10 on the page before the two potentials Pxand Py with their minima. Note that the potential Py tends to innity on the walls surfacein order to guarantee the non-collision.Note: We will analyze this actions stability, since its realization is based on the composi-tion of potentials dened on dierent degrees of freedom of a nonholonomic robot.Here are the dierent potentials that realize this task:Px = kx2 (x xp)2Py = 8<: ky2 (y dw)2 + 2 1y 1dw2 ; if y < dwky2 (y dw)2; else-wise. (3.4)P = k2 2PyFigure 3.11: The potential distribution for the command Follow Wall.61

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 62where kx; ky; k; are positive gain constants. The Task Potential is thus:P = Px + Py + Pthe expression is the calculated:F = 0B@ kx(x xp)ky(y dw) + 1y 1dw 1y2k 1CAIf an important distance x (which is expected in this action) is supposed, we presentin gure 3.11 on the preceding page the distribution of this potential in function of thevariation of y and . Note that the only minimum is at the conguration x; y = dw; = 0.This potential tends towards innity on the surface of the wall. Note also that the part ofthe y-component that guarantees the non-collision is null when y > dw.The asymptotic stability of this force is insured by the addition of a dissipation and thusthe force becomes: F = F K _Xwhere K is a diagonal matrix of the form:K = 0@ k _x 0 00 k _y 00 0 k _ 1Awhere k _x; k _y; k _ are positive dumping gain constants. In fact these constants depend uponthe proportional gain constants of the dierent components of the force F . We shall showthis dependency in the study of the stability of the system.Stability:In order to simplify the presentation of this study, we will suppose that the action to realizeid the following of a semi-line with constant velocity. This means a the stabilization ofthe components y; of the vector X. Then we will consider only the linear part of the ycomponent of the force for the quadratic part does not have an eect on the correction of thedistance from the wall4.We will study the stability of the control point Pc of the robot (gure 3.10 on page 60)under the in uence of a force F, given by:F = c k _x _xkyy k _y _y We will show afterwards the convergence of the robot congurationX under the nonholonomicconstraints. We will consider dw = 0.Let q = [qx qy ]T be the vector which determines the position of the command point Pcwhich is at a distance dc 6= 0 from the center of the robot and which satises the followingrelations: x = qx dc cos y = qy dc sin (3.5)4The quadratic part insure the non-collision with the surface of the wall.62

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 63The point q, being at a non null distance from the center of the robot, may be consideredas a holonomic point and thus be controllable by dynamic feedback [d'Andrea Novel 92,d'Andrea-Novel 95]. When considering a unitary mass centered on this point, we can describeits movement by: q = FThe resolution of this dierential equation is direct and gives us the expressions q(t); _q(t); q(t),functions of the initial state and of time.Once this equation is derived 3.5 on the preceding page, the expressions of the velocityof the center of the robot are: _x = _qx + dc! sin _y = _qy dc! cos (3.6)and the nonholonomic system equations are:_x = v cos _y = v sin (3.7)_ = !where v; ! are the linear and angular velocities of the robot respectively. These can beexpressed using the velocity of the control point in the following way:v = _qx cos + _qy sin ! = 1d ( _qx sin _qy cos ) (3.8)what interests us is the convergence of towards zero when qy converges to zero. Thisimplies the convergence of y towards zero according to the equation 3.5.The initial orientation of the robot 0 being known, the evaluation of can be expressedby a limited development of order one of the expression of ! which gives:(tn+1) = (tn) + t _(tn) + o(t)where o(t)! 0 j t! 0. This evolution can be expressed in the form of a sequence:n+1 = n td ( _qx sin n _qy cos n)To study the convergence of this sequence, we consider the following function:g() = td ( _qx sin _qy cos )which represents the evolution of the sequencef() = td _qx sin The convergence of the function f() towards g() when t!1 is insured by the convergenceof the term _qy towards zero. Thus, if n+1 = f(n) converges, this implies the convergence ofg(n). f can be written: f() = n+1 = n sin n (3.9)63

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 64with: ! ctdk _x j t!1If veries the condition (0 < 1), which is veried in the practice, then for all in thecompact set A dened by: A = ] 2 ; 2 [f() represents a lipschtzian (noted LipC)5 function for it veries the following property:jf(2) f(1)j Cj2 1j ; C 2 <+ (3.10)According to the theorem of the xed value, the application f is contractant if and onlyif it is LipC for at least one C 2]0; 1[, then f have one and only one xed point (i.e. x suchthatf(x) = x).Proof: According to th nite increase theorem:jf(2) f(1)j j2 1jMax2A jf 0()jwhence: jf(2) f(1)j j2 1j(1 Min2A j cos j)the term = Min2A j cos j is positive and superior to zero, then the previous equation becomes:jf(2) f(1)j (1 )j2 1jwith C = 1 2 ]0; 1[, which implies:n ! j f() = ) = 0 (3.11)The property 3.11 that converges towards and that according to 3.9 on the pagebefore, this value must be zero.Bounding of speed and acceleration: The control of the command point Pc must respect themaximum velocity and acceleration constraints of the robot. According to equation 3.8 onthe preceding page, we can write these bounding relations by:v =Mv(j _qj)! = 1dM!(j _qj)_v =M _v(jqj+ j _qj)_! = 1dM _!(jqj+ _qj)This means that the bounding of the speeds is guaranteed by the bounded velocity of thecontrol point and same-wise for the acceleration.5A dierentiable lipschitzian function has bounded derivatives.64

3.3. FROM TASK POTENTIAL TO SENSOR-BASED ACTION 65Execution control:The preceding formulation enabled us to prove the convergence of this action guided by theTask Potential towards a robot state relative to the wall. Nevertheless it is oversimplied inreal execution context. In practice, the type of the sensor used may impose certain conditionson the initial state of the robot. (This is the case of ultrasonic sensors.) Also, the wall tofollow may be composed of several segments. (Some may even be separated.) We will discussthese points and develop solutions for them.The sensor problem:α

mFigure 3.12: Problem of the angle of approach of the wall.Due to ultrasonic sensors specularity (gure 3.12), a maximum angle in which to approachthe wall must be respected in the initial state of the robot. This angle must be maintainedunder a maximum value m during the approach phase in order to guarantee its permanentvisibility. In fact, the rotation potential P given in the equation 3.4, contribute to thecontrol of this angle. The couple issued from this potential is set to the inverse of thatgenerated by the y-component of the force (gure 3.10 on page 60).Problem of openings.The consequence of this problem is directly on the attraction potential Px. An opening inthe wall corresponds to the end of a segment. The potential minimum of Px will be thusplaced on this vertex. The evident solution of this problem is to detect the collinearity of thetwo segment around the opening (x 2.3.2 on page 40), and to approximate them by a singleone. This supposes the existence of a parameter that determines the maximum length ofan opening. Also is supposed the detection of the opening and both surrounding segments.Some times (e.g. ultrasonic sensors) this is dicult to detect. In order to make the operationmore robust it is necessary to determine the dierent control parameters that enable thespecication of the form of action to realize. These parameters are naturally issues from alevel of planning that characterizes the behavior of the action and the conditions related toexecution. We present in chapter 4 a global denition of dierent feedback actions in thecontext of a system of navigation with uncertainty. We show equally several experiments incooperation with the planner PWU developed by [Bouilly 95a].65

3.4. DISCUSSION 66Collision avoiding:One of the fundamental interests of the Task Potential formulation is the possibility of takingobstacles into account permanently. The additivity of convex potentials enables us alwaysto associate a repulsive potential function of a minimum distance from the robot and to addthis potential to the Task Potential. The global convergence is no longer insured in the casewhen local minima appear but the task becomes more feeble.In order to introduce the collision avoidance in this action, we reconsider the denition ofthe angular sector made for the action Parallelize. We dene the distance de : AO ! <+that returns the shortest distance between the robot and the objects . We associate to thisdistance a repulsive potential that insures non-collision.Experimentation:Figure 3.13-I- show the execution trace of the action Wall Follow. The follow distance isxed to half a meter. The linear and angular speed proles as well as the distance to wallare presented in gure 3.13-II-. We can note that the angle of approach is constant which isdue to the equilibrium between the couples and y. In this example the gain constant kis more important than ky. In gure 3.14-I-, -II-, we present the same example but for anangle of approach which is more important. consequently the correction in distance is faster.We can also note the important angular acceleration generated by the couple near thefollow distance.Figures 3.15, 3.16 show two examples of wall follow in presence of obstacles. Due to thefact that the rst obstacle (gure 3.15 on page 68) is found near the wall, it is practicallyconsidered as a part of a segment of the wall. The second obstacle on the side opposite tothe wall obliges the robot to violate the follow distance in order to avoid collision. A localminimum may occur with an obstacle whose position makes avoidance impossible.Analysis:In the practical context of the usability of this command, the recognition of the wall mustnot be an ambiguous operation. As we have noted earlier, the sensor type and its degreeof perception impose an \correct" initial state of the robot relative to the wall. Also, theconstraints relative to sensors impose conditions on the form of the wall. fro example awall composed of segments with acute angles between them may be too constraining for thisaction. We represent in chapter 4 how we resolve such problems.3.4 DiscussionThe formalism of the Task Potential enables the conception of relatively complex sensor-based actions which increases considerably the capacity of the reactive level in the globalrobot architecture and which makes execution more ecient. It guarantees the convergenceof realizable tasks by exploiting the properties of convex potentials. It is also capable ofpermanently taking into account obstacles which makes execution more robust.This formalism enabled us to realize several sensor-based actions other than those devel-oped in this chapter:Corridor Follow This action is an extension of the actionWall Follow. It consists to generate a66

3.4. DISCUSSION 67

-I--0.2

0

0.2

0.4

0.6

0.8

1

1.2

1.4

0 10 20 30 40

dWV

Time (secs)

Vel

ociti

es &

Dis

tanc

e

-II-Figure 3.13: -I- Execution trace of the action Wall Follow, -II- corresponding parametersprole.67

3.4. DISCUSSION 68-I-

-0.2

0

0.2

0.4

0.6

0.8

1

1.2

1.4

0 10 20 30 40

dWV

Time (secs)

Vel

ociti

es &

Dis

tanc

e

-II-Figure 3.14: -I- Execution trace of the action Wall Follow, -II- corresponding parametersprole.Figure 3.15: Collision avoidance during Wall Follow.68

3.4. DISCUSSION 69Figure 3.16: Local minimum during Wall Follow.displacement along a corridor while minimizing its distance to the potential minimum.Figure 3.17 shows the prole of this action.

© Oper

Figure 3.17: Action Follow Corridor.Maintain Position This action maintain the robot attracted to a certain position while avoid-ing approaching obstacles6. It is particularly convenient in multi-robot applications.Lose obstacles This action consists to go away of all obstacles. This action is particularlyuseful when the robot is in collision with obstacles in the model of the planner (sup-posing a use of a grown robot model). In reality, the robot is near the obstacles butnot in collision.In the dierent actions presented in this chapter, we have used simple generic potentialfunctions of dierent natures. We can dene thus a certain number of basic potential functions6It is not an easy task to give a graphic presentation of this action!69

3.4. DISCUSSION 70which serve as primitives to be used in the denition of a Task Potential. A system couldbe imagined that builds potentials corresponding to a task by using the task denition andthese primitives.In the next chapter a sensor-based navigation system with uncertainty is presented. Ituses the dierent actions dened in this chapter and proves their utility and eciency.

70

Chapter 4Application to planning with uncertaintyIn the context of planning, we presented in paragraph 3.1 the problem of cohesion betweenexecution and planning. The use of planning strategies based on uncertainty evaluationrepresents an ecient solution that makes execution more solid. Several approaches weredeveloped in literature concerning the generation of execution plans taking into accountuncertainty[Alami 94, Bouilly 95a, Collin 95, De la Rosa 96]1.In the context of this work, experiments were eectuated of the navigation with uncer-tainty system SBCM, from which a relation was established with the planner PWU in order tovalidate the eciency of our dierent sensor-based actions.Bouilly proposed [Bouilly 95a, Bouilly 95b] an approach based on the propagation of apotential function in a bitmap taking into consideration the accumulation of uncertaintyrelated to the robot displacement. The method proposed a propagation strategy parallel tothe potential taking thus into account all the zone of possible relocalization and covering thetotality of free-space. A geometric analysis is then realized on the environment in order todetermine the possible displacements in the contact space. This analysis consider equally therelocalization on the dierent vertexes belonging to this space. The planner (PWU) generatedthus a sequence of displacement primitives containing sensor-based actions, relocalizationsand free-space movement.The planner PWUHere, we present brie y the strategy types produced by the planner PWU in order to specifythe context of the following navigation system.Figure 4.1 present a planning problem and three action sequences that demonstrate theinterest of the dierent planning strategies. The rst strategy shows that the goal is unattain-able by free-space movements because of the evolution of uncertainty (sequence j1 ). Never-theless, the existence of an absolute localization zone (roof camera, : : : ) oer the planner thepossibility to generate sure displacement (without increasing the uncertainty) in this zone andthus enables to join the goal by free-space movement (sequence j2 ). Another strategy con-1We do not detail these dierent approaches since this work is more about sensor-based motion thanplanning. 71

Chapter 4. Application to planning with uncertainty 723

1

1

3

3

3

2

3

2

3

Init

3

Goal2

2

Absolut relocalization zone

Position incertainty

Position incertainty

Motion Envelop

Free space strategy

Robot circular envelop

Absolut relocalization strategy

Sensor-based actions strategyFigure 4.1: PWU: planning strategies.sists of using movements in contact-space which implies sensor-based actions. The decreasein uncertainty apported by these actions makes it possible to reach the goal safely.The method being based upon the propagation of a numerical uncertainty potential ina bitmap, the path produced is initially in the form of a sequence of congurations. Theseare labeled according to the nature of space to which they belong (free or contact space).This information enables the deduction of a set of actions composing the plan (free-spacemovement, sensor-based motion, relocalization, : : : ) and to provide the dierent parametersnecessary to their execution.4.0.1 Denition of the system SBCMThe robot evolutes in an interior environment which is structured and known. Several ele-ments such as localization zones, corridors, walls represent a source of potential relocalizationwhich the planner uses to make path nding strategy more robust.In the context of a missiontranslated by a control level in several actions of the type Go to Goal (gure 4.2 on the nextpage), The planner generates a sequence of movement primitives (Execution plan) composedof: Free-space displacement. sensor-based displacement.The plan generated is read by a level of sequencing and action interpretation (sequencer)by a request (C), coming from a control level. The sequencer begins the execution at thereception of a request of type (S) and interprets the results of each action. The executionstate in the level (SBM) is signaled to the sequencer by nal answers containing the action'sresult2. Nevertheless, the controller may send to the sequencer other concatenation requeststo the plan (C) or abandon of execution by the request (A).2This mechanism in brie y presented ion annex A. 72

Chapter 4. Application to planning with uncertainty 73S/C/AActionCommand Plan RequestAnswerLectureAnswerRobotAnswerAnswerControllerMissionSequencer EnvironmentPWUSBMMotorsFigure 4.2: Denition of the system SBCM.It is necessary now to dene the dierent actions between Planning and Execution.Actions denition

y

x

y

x

∆Θ

y

xy

x

∆Θ

∆L

y

x

y

x

∆Θ

ra b cFigure 4.3: Free-space displacement.Free-space displacementSBC TRUN(): is a pure rotation command around the center of the robot which isequally the middle point of the wheels axis (gure 4.3-a).SBC MOVE(;L): is a relative displacement command. The rotation is executedbefore the displacement and the sign of the parameter L determines the direction ofthe movement (4.3-b).SBC ARC(; r; u): is a relative displacement command on an arc of circle of radius r, determines the angular variation and u the direction of the movement (4.3-c).These dierent actions belong naturally to the basic functionality of our experimentalrobot Hilare architecture (see annex A).Sensor-based actionsSBC MOVE TO CONTACT(Lmax; dc; d; u): brings the robot from free-space to contact space(x 3.3.1 on page 51). Lmax is the maximum distance permitted for the robot to cover73

Chapter 4. Application to planning with uncertainty 74y

x

L_max

dc

y

x

y

x

Θmaxside

θa by

x

y

x

y

x

y

x

L2L1

∆Θ

d_w

cFigure 4.4: Sensor-based actions.without nding contact, dc determines the contact distance with an error d, u is thedirection of the movement (gure 4.4-a).SBC PARALLELIZE (min; s): in a command in contact space to be parallel to an object(a wall) which represents contact (x 3.3.2 on page 56). min expresses a minimalrotation before getting parallel and s determines on which direction the robot shouldhave the object (4.4-b).SBC WALL FOLLOW (walls): is an action composed of a sequence of wall follow by a listwalls. The structure of each element of this list is dened by:SBC ONE WALL (s; dw; L1; L2; vertex): s determines the side on which to nd thewall and dw is the distance to respect. L1 is the distance to move before theapparition of the rst vertex while L2 is the maximum distance to move beforending a vertex gure 4.4-c). Vertexes are described by a structure vertex.SBC VERTEX (type;; action): type 2 [Convex; Concave; None] determines thetype of the vertex and the angle between the surrounding two walls. action 2[End Follow, Switch Wall, Switch Side] characterizes the behavior of the com-mand associated to a vertex.The parameters L1; L2 re ect in a direct way the evolution of the uncertainty related tothe displacement along the wall. L1 expressed the displacement during which, takinginto account the uncertainty, the robot can never detect the vertex. In the same wayL2 expresses the displacement at the end of which the robot has completely passed thevertex. 74

Chapter 4. Application to planning with uncertainty 75Related to the denition of a vertex, the type None is used in case of an associated actioncorresponding to a dynamic change of side which corresponds to the action Switch Sides.On the other hand the action End Follow is always associated to the last wall on thelist.An execution plan is thus a sequence of dierent actions The control parameters cor-respond to error answers which come from execution level SBM and are interpreted in thecontrol level.Particularly, in what concerns the action SBC WALL FOLLOW this denition requires anadaptation of the action Wall Follow with respect to the one presented in paragraph 3.3.3on page 60. We will develop in the next paragraph this adaptation in the formalism of TaskPotential.Wall Follow action adaptationThe most important problem to treat concerning this adaptation is the chaining of two walls.The attraction potential (equation 3.4 on page 61) must not have its minimum on the vertexand a switch potential must be realized to change walls.Concave vertex Convex vertexFigure 4.5: Vertex types.The case of a concave vertex is the most simple. The two segments and the vertex are allvisible. In the convex case the vertex is less easily modeled, for the second segment is invisible(gure 4.5). The structure (SBC VERTEX) issued from the planner enable the prevision of asegment change as well as the matching of the planner's data with the real environment.The solution of this problem depends deeply upon the sensor used. If the localization of thevertex is relatively precise an evident solution will be the use of the distance from the robotcenter to this vertex as the parameter to govern during angular variation. ThusPsw = (dc!s R)2with: R = dw + E2where dc!s is this distance and R represents the xed distance to maintain. The value of Ris but the sum of the follow distance and half the width of the robot.The convex composition of the potentials is as usual done using a scalar transition func-tion. If we call the potential associated with the wall follow Pwf , the transition between Pwfand Psw must be realized at the instance when the the vertex abscissa becomes null in therobot frame. The transition function related to this change depends on the projection of thevertex position on the x-axis. This function is given byfwf (t) = kwfexs(t); ifxs(t) > 0;1 elsewise;75

Chapter 4. Application to planning with uncertainty 76where xs(t) represents the abscissa of the vertex at time t, kwf is a positive gain and is atime constant that determines the transition slope. The convex composition is thus:P = (1 fwf (t))Pwf + fwf (t)PswIn the same way the transition between this phase and the next wall follow is eectuatedby the function: fsw(t) = ksw(1 ej(t)j) ofj(t)j > 0;0 elsewise;with: (t) = Z t0 !(t)dtfsw(t) is a positive monotonous and irreversible function of time, ksw is a positive gain and is the same as before. (t) represent the angular variation realized at time t. The instance0 in this integral is the moment when xs(t) becomes zero.In case the dynamic localization of the vertex is not realizable, its detection is an eventdone only once. We dene thus the potential:Psw = ((x x0)2 + (y y0)2 R2)2(x x0)2 + (y y0)2 + 2rwhose minimum is on a circle with center (x0; y0) and radius R (gure 4.6 on the next page3).The circle center occupies the position of the vertex at the moment it was detected and theradius R has the same value given previously. The denominator enables this potential toincrease explosively for value near the vertex and is a positive constant that permits toadjust the gain between the potentials Pwf and Psw . The transitions between the potentialfunctions is done the same way as before.This method supposes, of course, a xed local frame at the moment of vertex detectionin which the robot position is updated during the transition phase. The vertex should belocalized at the moment when its abscissa is the closest to zero in this frame. Equally, anerror on this localization or on the parameter can constrain the execution during thisphase. Still the non-collision potential together with wall follow potential can compensatethe localization error.Attraction: In the beginning of this paragraph, we have pointed out the problem relativeto vertex attraction potential. In fact, the potential Px as dened in paragraph 3.3.3 isnecessary for the termination of the action. (For the last wall in the sequence). Nevertheless,the distance associated to this potential in the case of consecutive walls is the remainingdistance in order to cover a distance equal to the sum of the lengths of the walls.Another solution is void-attraction. For this we use a denition similar to that of thedistance d presented in paragraph 3.3.1.Vertex detection: We will develop more thoroughly the dierent aspects related to perceptionin annex B. In fact this aspect is narrowly related to the sensors of which we will considertwo cases: laser and ultrasonic.3The gure shows the potential on an interval d = ! R, d being the distance to the wall.76

Chapter 4. Application to planning with uncertainty 77Wall changement

Wall followFigure 4.6: The distribution of follow and wall switch potentials.4.0.2 SBCM system integrationIn what follows we do not reclaim a contribution to a control architecture. A minimal systemwhich was necessary for the experimentation of the navigation system is simply presented.We present in annex A more profoundly the concept of the architecture of generic control[Giralt 93, Chatila 93, Alami 93a, Fleury 94] in which we integrate our dierent contributions.Actions sequence controlAs shown in gure 4.2, the controller receives a mission (e.g. Go the the coee machine) ina given environment. This mission is translated into a sequence of tasks of type Go to Goal.To each task, several parameters are aected:Environment: The environment model is completely known and is transmitted to the plannerwith its associated uncertainty. The need for a relation task $ environment is due tothe need of several environment models to realize a mission4.Uncertainty: This denotes uncertainty values related to start and goal positions of the robot.That associated to the start position concerns only the rst task and is issued anabsolute localization realized by the robot before the mission, while that associatedto the goal position is imposed by the controller in function of the topology of theenvironment. This value is taken to be the start position uncertainty for the followingtask.Divers parameters: These are several parameters which characterize the uncertainty evolu-tion for the robot evolution in position and orientation as well as dierent execution4For example: For to go the the coee machine an come back, 40 actions are realized and more than 70meters are covered. 77

Chapter 4. Application to planning with uncertainty 78constants such as clearance distance and minimum angular variation, : : :When it receives an answer from the planner, the controller sends a request of type (S) tothe sequencer to start the execution. Nevertheless, the controller may continue the generationof tasks for the rest of the mission by sending concatenation requests of type (C).As for the sequencing of the plan, each action is interpreted by th sequencer and send tothe corresponding execution module in the level SBM (The module notion will be resentedin the annex A). The level SBM sends the nal reply to present the balance of the execution.In case of error, this reply is passed to the controller and the sequencer is suspended.This brief presentation permits us to present several experiments which demonstrate theeciency of our dierent sensor-based actions and show the advantages of this navigationmethod.ExperimentationMission: LMS5Here we show the eect of taking into account the uncertainty in the navigation strategy.The mission consists in moving the robot from the point Init to the point Goal. The startuncertainty being small the planner generate a sequence of displacements in free-space (gure4.7a).1 SBC MOVE Move1( = 1:05; L = :64)Move2( = 0:51; L = :87)2 SBC MOVE TO CONTACT(Lmax = :7; dc = :15; d = :17; Foreward)3 SBC PARALLELIZE(min = 0:6; Right)4 SBC WALL FOLLOW(Right; dw = :51; L1 = 2:86; L2 = :67; Convex;End Follow)5 SBC MOVE Move1( = 0:00; L = :43)Move2( = 0:90; L = 1:72)Move3( = 0:76; L = 1:13)Move4( = 0:93; L = 0:65)Table 4.1: LMS: Actions sequence.Starting from the goal position with the goal uncertainty, the return to the point Init is notpossible by only moving in free-space, the uncertainty being greater. The planner strategyis to use sensor-based actions for surer movement (gure 4.7b). Note that the planner PWUuses a circular model for the uncertainty as well as for the robot. Table 4.1 shows the sequenceof actions in the return plan.We have repositioned the robot near the point Goal and have given it the same uncertaintyvalue it had at this point. Asking now the return to point Init with a smaller uncertainty, gure4.7c show the planner strategy to arrive with the imposed uncertainty. Note the increase inthe number of sensor bases actions with respect to the displacement in free-space. Note onthe other hand the dierence between the drift of the model and the ultrasonic measures onthe rst wall in both cases: gures 4.7a and 4.7b).Mission: HVL65LMS: From French \Longuement, Mais Surement".6HVL: Form French `Hilare Va Loin. 78

Chapter 4. Application to planning with uncertainty 79This mission realizes a relatively long displacement. Figure 4.8a shows the rst part ofthis mission.The robot, before the panning phase, is localized by an extern instrument. (A cameraon the roof localized a specic motif placed on the robot.) This task was to go from theconguration A to the conguration B. Table 4.2 shows the actions sequence generated bythe planner.1 SBC MOVE( = 1:58; L = 1:74)2 SBC TURN( = :87)3 SBC MOVE TO CONTACT(Lmax = :75; dc = :07; d = :1; Foreward)4 SBC TURN( = 0:34)5 SBC ARC( = :35; r = 1:05; Foreward)6 SBC WALL FOLLOWWall1(Right; dw = :34; L1 = 2:6; L2 = :87; = 1:51; Convex; Switch Wall)Wall2(Right; dw = :34; L1 = 5:22; L2 = :38; = 1:57; Convex; Switch Wall)Wall3(Right; dw = :34; L1 = :29; L2 = 0:; = 0:; None;End Follow)7 SBC ARC( = :34; r = 1:05; Foreward)8 SBC TURN( = :43)9 SBC MOVE TO CONTACT(Lmax = :27; dc = :07; d = :1; Foreward)10 SBC TURN( = :43)11 SBC ARC( = 0:34; r = 1:05; Foreward)12 SBC WALL FOLLOWWall1(Left; dw = :34; L1 = 14:11; L2 = 2:70; = 1:57; Convex;End Follow)13 SBC MOVE( = 0:; L = :43)14 SBC ARC( = 0:34; r = 1:05; Foreward)15 SBC MOVE Move1( = 0:02; L = :60)Move2( = 0:46; L = :27)Table 4.2: HVL: go action sequence.Note the important ratio of the distance covered by the sensor-based action to the totaldistance in gure 4.8 on page 82a. Also note the error eectuated with respect to the model(region \Note1" on the gure) and with respect to the position apparent in the drift of themeasures relative to the model.The rst important action in this gure is the follow of three consecutive walls. Thedetection of the vertex and the passage from one wall to the other are done dynamically.This was possible because of the continuous composition of potentials. During the secondfollow action the robot avoids an obstacle when it encounters the dicult passage of a door(\Note2") not integrated in the model.Table 4.3 contains the return plan and gure 4.8b show the track of the correspondingexecution. Note the denition of the number of actions in this plan which is due to thepresence of three visible and localizable vertexes.Mission: HVTL77HVTL: From French \Hilare Va Tres Loin". 79

4.1. CONCLUSION 801 SBC TURN( = 2:60)2 SBC MOVE TO CONTACT(Lmax = :79; dc = :21; d = :1; Foreward)3 SBC TURN( = 0:75)4 SBC ARC( = 0:56; r = 1:05; Foreward)5 SBC WALL FOLLOWWall1(Left; dw = :34; L1 = 16:62; L2 = 1:57; = 1:57; Convex; Switch Wall)Wall2(Left; dw = :34; L1 = 4:83; L2 = :77; = 1:51; Convex; Switch Wall)Wall3(Left; dw = :34; L1 = 1:35; L2 = 0:; = 0:; None;End Follow)6 SBC ARC( = 0:56; r = 1:05; Foreward)7 SBC MOVE( = 0:22; L = 2:87)Table 4.3: HVL: Return actions sequence.This mission shows the link between the task and the model environment. The controlleruses two models to compose this mission. Figures 4.9 on page 83a, b show the execution ofthis mission.4.1 ConclusionThe dierent sensor-based actions developed in the Task Potential formalism created a rich andsolid basis to establish a robust navigation system. The planning, especially with uncertainty,and the primitives set we have dened, were an ecient unit on which relatively complexdisplacement tasks were and can be built. We have pointed out by numerous experimentsthe simplicity and thus the eciency of the realization of a displacement task in dierentenvironments by the bias of our navigation system.Moretheless, these dierent sensor-based actions were put in service in the context ofProject Inter-prc CHM, PRC-IA thus demonstrating, via an important number of experiments,its eciency and exibility.From the point of view of construction, we would like to emphasize the sensibility of thisapproach to the several parameters attached to forces (e.g. proportional gains, dissipation,: : : ). Nevertheless, The choice of these constants is not wholly empiric being for the mostpart dependent upon each other. Also the domain of each variable on which depend theforces is completely determined by the robot and sensor limits. Still we think it importantto try optimizing th parameters in function of these limits and of the nature of the task.80

4.1. CONCLUSION 81© Oper

Init

Goal

RIA (0, 0)

Hilare2−bis

Junior

-a-© Oper

Init

Goal

RIA (0, 0)

Hilare2−bis

Junior

-b-© Oper

Init

Goal

RIA (0, 0)

Hilare2−bis

Junior

-c-Figure 4.7: Experiment LMS.81

4.1. CONCLUSION 82© Oper

RIA (0, 0)

Hilare2−bis

Note1

Note2

A

B -a-© Oper

RIA (0, 0)

Hilare2−bis

Note1

Note2

A

B -b-Figure 4.8: Experiment HVL.82

4.1. CONCLUSION 83© Oper

a© Oper

bFigure 4.9: HVTL: Execution trace.83

Part IIIDynamic path modication

84

Chapter 5 The elastic band5.1 IntroductionWe will develop in this chapter a new way of combining planning and reactivity. As we haveseen in chapter 2, the execution of a planned trajectory should be realized in a reactivesystem to avoid collision. This represents eectively a common solution to the problem. Inour approach the robot was always obliged to minimize the drift from the original trajectoryeven while avoiding obstacles.Dierent approaches exist in the literature. Krogh and Thorpe [Krogh 86] proposedto replace the planned trajectory by critical points. The articial potential eld method[O. Khatib 86] is then applied to move the robot between these points. The drawback of thismethod is that the robot does not move on the original trajectory, which makes robot controlbetween such points more dicult.Choi introduced [Zhu 89] a channel concept to represent the environment. A channelis dened by a sequence of adjacent congurations in free-space joining the initial and thegoal congurations. A potential eld is dened to guide the robot along the channel to thegoal position. Unfortunately, the out-line construction of channels prevents the taking intoaccount of subsequent changes in the environment. (e.g., if all the obstacles disappear therobot will not go straightforwardly to the goal.) Also, the channel being constructed beforeexecution, the evolution of the environment may render its boundaries too constraining.In fact, this last point is an important problem in the control architecture of a robot.The liberty of the reactive system depends on that given by superior levels in the controlhierarchy. It is not always recommended for the robot to drift from its trajectory to go in adirect line to the goal even though it may be possible and realizable. (This is true for examplein a multi-robot coordination). We will suppose in this chapter and the next the possessionof a total liberty of movement in the free-space. When the obstacles are immobile, the classof homotopy of the trajectory is of course preserved.In this context, Quinlan and Khatib [Quinlan 93, Quinlan 92] proposed a dynamic trajec-tory modication concept called the elastic band, which consists in maintaining a permanent exible path between the initial and goal congurations. Here, a path is a sequence of cong-urations in free-space. Internal forces are dened on such sequences to optimize the path and85

5.2. THE ELASTIC BAND 86guarantee its connectivity. External forces, due to obstacles, are applied to keep a collision-free path. Thus, global information about the elastic band evolution have an eect on thelocal behavior of the robot. Being dened for holonomic robots, an implementation of theoriginal elastic band method proved to be inadapted to non-holonomic mobile robots. Wehave thus tried to adapt it to such robots. In this chapter we present a modied method andpoint out its advantages and drawbacks. Then we introduce the notion of a bubble. In thenext chapter, we present a new formalism of this concept in a Reeds & Shepp metric systemtaking into account the kinematic constraints of the robot.5.2 The elastic bandWe will give in this paragraph a brief presentation of the formalism of the elastic band. Thecomplete details are developed in [Quinlan 95].Denition: The elastic band is a exible entity that can be deformed by applying internaland external forces on it. It is represented in the conguration space by sequence of connectedcongurations.Physical model of the elastic bandLet q be a vector of dimension n representing the conguration of the robot in the con-guration space C. Let p(s) : [0; 1] ! C be a continuous parameterized curve such thatp(0) = qs; p(1) = qg, where qs;qg are the initial and nal congurations of the curve.Suppose now that this curve represents a deformable solid where each value s 2 [0; 1]corresponds to an innitismal particle. The force applied at each moment on the particle sis given by the function: F(s) = Fint(s) + Fext(s)where Fint; Fext are the internal and external force functions respectively. These forces aredene by: Fint(s) = kp(s)0k (s)Fext(s) = rPext(p)where is a positive elasticity constant, (s) is the curvature of the curve at s and Pext apotential function dened on the space C. The internal and external forces are issued frompotentials which are functionals (i.e. functions of functions) of the elastic band p(s). Thesepotentials are given by: Pint[p] = Z 10 kp0k ds (5.1)Pext[p] = Z 10 Pext ds (5.2)Quinlan proposes to add a restriction force to prohibit a particle from moving the longof the band. In fact, the displacement of particles in these directions in useless due to the86

5.2. THE ELASTIC BAND 87fact that this does not change the energy of the elastic band. This phenomenon occurs whensome zone have a greater energy than the surrounding ones. The restriction force is given by:Fconst(s) = Fext p0kp0k2 p0This force is necessary only to constrain the movement of particles along the elastic bandand have no eect on the energy of the band. This is why they cannot be derived frompotentials.Finally, the resultant force on each particle generates a displacement proportional to thisforce. This is written as: @p@t / F:The movement of particles must always guarantee the connectivity of the band. For this,a procedure of creation and elimination of particles in put in place.Discrete elastic band modelThe previous representation is a formalization of a theoretic model of the elastic band. Inreality, the elastic band is implemented in a discrete \bitmap". The internal and externalforces in the preceding model are issued from functionals but when implemented they becomefunctions and the forces are then deduced by the classical operator r. We will develop inthis paragraph such a discrete implementation.Suppose that the elastic band is a nite sequence of m particles whose congurationvectors are q1; : : : ;qm. The two types of forces are:The internal force: qiqi1 qi+1FintFigure 5.1: The internal force of contraction.This force generates a contraction between particles. The corresponding potential isgiven for the continuous model by the equation 5.1 on the preceding page. This internalpotential becomes: Pint(q1; : : : ;qm) = m1Xi=1 kqi+1 qik87

5.2. THE ELASTIC BAND 88The contraction force exercised on the particle qi is then the negative gradient of thispotential on qi which gives:Fint(i) = qi+1 qikqi+1 qik + qi1 qikqi1 qikwhere 1 < i < m, considering the congurations q1 et qm xed.This equation expresses the nature of the (uniform) stress along the band. Each particletries of attract its two direct neighborsThe external forces:If is the distance between two successive congurations (which correspond the s)the potential given by the equation 5.2 on page 86 becomes:Pext(q1; : : : ;qm) = mXi=1 Pext(qi):This potential depends linearly on which could have a unitary value. The potentialPext is chosen to satisfy the conditions of a repulsive potential (x 1.4 on page 9). It isof the form: Pext(q) = kp2 (d d0)2 if d < d0;0 else-wise.where kp is a positive repulsion gain. The distance d depends on the conguration qand expresses the shortest distance between it and the obstacles. The force associatedon this potential is then:Fext(q) = kp JTx (d d0) @d@x if d < d0;0 else-wise.where x is the nearest point on the obstacles relative to the robot occupying the con-guration q and JxT is the Jacobian matrix associated to the point x. The force Fextis thus proportional to the value (d d0) and is ported by the unitary vector @d@x1.The forces exerted on the band being dened, we characterize now the movement of eachparticle exposed to these forces.The elastic band deformationThe elastic band is deformed by moving its particles one at a time. Consider a particle atthe conguration q exposed to a force F. It is displaced in the direction of the force using again factor in the following way: q0 = q+ F (5.3)This method of displacement is an explicit application of the method of Euler which doesnot guarantee the stability of the band. Quinlan [Quinlan 95] proposes a method of repetitive1This vector is only unitary if the distance is Euclidean or normed.88

5.3. THE MDT 89minimization to choose one of the best congurations of the neighborhood. The minimizationhere concerns the external potential energy. Such a procedure is costly in time and may loopindenitely. We will present an implementation not sensible to this problem.Moretheless, the movement of a particle should guarantee the connectivity of the bandas well as the non-collision of the new conguration. A rst implementation considered themovement in a regular bitmap. This implied that the distance between two particles should bestrictly inferior to 2. Consequently, if a particle eectuates a displacement that separatesit from its neighbor a new particle is created in an intermediate conguration. Thus theconnectivity is respected. In the same way if the new conguration of a particle is the sameas one of its neighbors, the particle is eliminated.In implementing the bitmap version of the elastic band we have supposed a circular robot.gure 5.2 on the next page shows the eects of the dierent force applications. The initialtrajectory is given by a planner (5.2-a). This trajectory is optimized and smoothed once theinternal and external forces are applied (5.2-b, -c). The band is deformed by the presenceof unexpected obstacles which could be eventually movable (5.2-d). Figures (5.2-e, -f) theconcatenation and the optimization of the new trajectory while the band is deformed to avoidan obstacle.It should be noted that the optimization and the deformation of the band does not changethe homotopy class of the initial trajectory. In fact the liberty of deformation of the elasticband must be controlled. It depends greatly on the position of the detected obstacle and onthe current position of the robot on the band. In practice, the localization of an obstaclein done in a position relatively near the robot, thus deforming the band. Nevertheless, theviolation of the clearing distance which is expressed by an increase in the internal energy ofthe band expresses a failure. On the other hand if the perception enables the localization ofobstacles away from the robot, the consequent deformation propagates the long of the bandand modies eventually the execution.Discussion The previous formalism of the elastic band enables the optimization and theexecution of a holonomic trajectory in an ecient collision-free way. Figure 5.3 on page 91shows the real form of the elastic band issued from this implementation. In fact we consideronly the position (x; y) of a mobile robot. The band creates an optimized sequence of robotpositions which the execution may interpolate by some curve. One of the most importantadvantages of the method is its reactivity to unexpected obstacles. Nevertheless, during exe-cution or when concatenating a new trajectory, the deformation of the band must guaranteethat the positions generated may always be interpolated and executed. This is not the caseof the current formalism for the particles move in a holonomic way.The problem related to the holonomic displacement of a particle has its origin in theinitial formulation of the method. An elastic object may be deformed, within its domain ofelasticity, in any shape or form. Particularly the curvature may not be continuous. To solvethis, we imagined a solid elastic band, in which the particles are subject in addition to thecontraction force to a resistance torque called MDT2 against bending just as that applied onan iron bar. This model will be developed in the following paragraph.89

5.3. THE MDT 90-a- -b--c- -d--e- -f-Figure 5.2: The dierent states of the elastic band. -a- The initial trajectory, -b- The appli-cation of contraction forces, -c- The application of the repulsive forces, -d- The deformationof the band in presence of an unexpected obstacle, -e- Concatenation of a new trajectory, -f-Final state of the band. 90

5.3. THE MDT 91© Oper

Figure 5.3: The robot congurations on the elastic band.s qi qi+1C iVi xsysRsFigure 5.4: The MDT between particles.91

5.3. THE MDT 925.3 The MDTFigure 5.4 on the page before show the action of MDT. Beginning with the initial position ofthe robot, each particle exercise on its neighbor (qi) a torque (i) around itself to bring it onthe arc of circle (C). In fact, this torque is generated to minimize the angle between thevector Vi and the xs-axis. i must be null for .The application ofMDT will push the band to maintain a minimum variation of curvaturealong itself. In order to generate this torque, we dene a potential function local to the particlewhose minimum is for jj . Consider the variation of the angle in the interval [; ]and let q be the relative position of the particle exposed to the torque. This potential is:

Prdm(q)Figure 5.5: The distribution of the potential Pmdt(q).Pmdt(q) = kmdt (sign() )2 ; if jj > ,0 else-wise.where 2 R+ represent the angle at which the potential becomes zero, kmdt is a positivegain and sign : R! f1;1g is a function returning the sign of . gure 5.5 shows the form ofthis potential function. The parameter is dynamically calculated in function of the osets between the two particles. For small values of s, the relation is given by: = sRminwhere Rmin is the steering radius expressing the maximum curvature along the band.The application of the operator r on this potential gives us the torque to apply on theconguration q which is:Fmdt = rPmdt(q) = kmdtsign()(sign() ) @@q ; if jj > ;0 else-wise,with2Material resistance torque 92

5.3. THE MDT 93@@q = 1s sin cos : xsys Frdm>0Vi Vi Frdm<0jVij = sq qRsFigure 5.6: The MDT force issued from the potential in function of .@@q represents the direction of the force vector Fmdt. Note that this force is perpendicularto the vector Vi and that its direction depend on the term sign() (gure 5.6). This forcecorresponds to a torque that acts to bound the angular dierence . Similarly, we apply thisforce starting from the end position in order to guarantee the goal orientation continuity.All together, each particle is exposed to the following forces: A contraction force which tends to minimize the drift from a line segment joining thetwo neighboring particles. A resistance force controlling the curvature at the present conguration. An external force due to obstacles guaranteeing non-collision.Particles displacementThe dierent forces applied on the band are issued from potential functions. These forcesare thus conservative and have a periodic stability. To have an asymptotic stability we adda dissipative term to the force. Thus each particle converges as well as the whole band. Thisidea was previously proposed in [Quinlan 92], but in their context, the particle displacementmethods used is not stabilized by the addition of such a dissipative force. The reason for thisis the non taking into account of the speed of the particles.In our case, we consider the displacement if a particle generated by the accelerationresulting from the force resultant. Thus, we conserve the information on the current velocityof a particle making the dissipative force eect more real. The particle displacement becomes:q0 = q+ 12Fdt2 + _qdt (5.4)where 2 R+ is a positive term expressing the mass and dt is the update period of the bandstate. _q is the current velocity of the particle and F is the bound total force applied on it.This force is given by: F(q) = Fint + Fmdt + Fext kv _qwhere kv is the positive dissipative gain. 93

5.3. THE MDT 94Discussion and resultsWe have realized a new implementation of this elastic band taking into account the real formof the robot and using a polygonal representation of the environment. We would like toattract your attention to the following points relative to this implementation.Distance measures In the rst implementation, a distance map is propagated on the \bitmap"in order to deduce to distance to known obstacles. The unexpected obstacles are dy-namically detected and the associated distance is given by means of perception. At theend of this section you may nd on the the experiments we have realized.Using the virtual perception faculty developed in chapter 2, the obstacles distancemeasures are now realized without the construction of the distance map. This enablesus to implementation the elastic band directly in the operational space and to take intoaccount the real form of the robot.Elastic band deformation We have seen that the deformation of the elastic band must guar-antee connectivity. Here connectivity is dened by an over-lapping of circles coveringthe robot on all the length of the band. These circles are of a xed radius.Parameterization The parameterization of the band is based on the local curvature of thecongurations. We have tested several parameterization algorithms and curvature cal-culus (circles passing by three consecutive congurations, spline approximation, Beziercurves approximation). We will develop in paragraph 7.2 on page 123 the algorithmadopted.Figure (5.3) present the implementation of this new formalism of the elastic band.Figure 5.3-a shows the rst construction of the (classical) band. The application of thedierent forces including MDT transform this band into a non-holonomic trajectory exe-cutable by the robot 5.3-b). Figure 5.3-c shows the band deformed because of an unexpectedobstacle. The position of the obstacle being away of the goal position, the band deformationsatises the maximum curvature constraint. When this is not true, 5.3-d), the repulsive forceoverrides the MDT and the maximum curvature is not satised. This problem is related tothe fact that the goal position is immobile which is recommended. Figure 5.3-e) shows apossible solution by the generation of a cusp point and a second elastic band starting at thispoint and reaching the goal.We have tested several heuristic methods to resolve this problem (the generation of otherelastic bands sharing the same tangent at the junction points, the denition of control pointsand obliging the elastic band to path by them, : : : ). The application of these methodsshows that the elastic band formalism does not guarantee the curvature constraint when theenvironment is rather encumbered.Nevertheless, the robot Hilare2 which does not have a curvature constraint was used toimplement this method and test its validity.On gures ( 5.3 on page 97) and ( 5.9 on page 98) the robot starts by modeling theenvironment using a 2D laser section and approximating the data by segments (gure 5.9shows the laser points and the corresponding segments). These segments are projected on a\bitmap" and are then grown. Figure 5.3-a shows the result of this step. Then a trajectoryis given to the elastic band joining the current position of the robot with the goal (gure 5.3-b). This trajectory is optimized and smoothed using the internal and external forces (gure5.3-c). Figure 5.3-d shows the state of the robot during nominal execution. The robot meets94

5.3. THE MDT 95© Oper © Oper

-a- -b-© Oper © Oper

-c- -d-© Oper

Solution-e-Figure 5.7: Integration of the MDT torque in the elastic band. -a- The holonomic elasticband, -b- the application of the dierent forces and of the force MDT, -c- The elastic banddeformation under the eect of an unexpected obstacle, -d- curvature violation when the robotapproaches the goal, -e- A possible solution. 95

5.3. THE MDT 96an unknown obstacle after leaving the rst known obstacle. The ultrasonic measures aredynamically injected in the \bitmap" producing thus a deformation of the band under theeect of the repulsive force (gure 5.3-e).Figure 5.9 on page 98 shows the dierent traces of the whole execution: laser points,segments, robot position and ultrasonic echoes.Elastic band implementation problemsduring the experimentation period we have noticed, on the real robot, that the band formalismlacks an important element related to execution control. We will show this with the helpof the following example: Suppose that the initial trajectory passes through an unknownobstacle. During execution, when this obstacle is detected (which is a discontinuous event)the congurations that are in the obstacle are in collision. The band is deformed underthe eect of the repulsive force exercised by this obstacle. Suppose now that this obstaclehas a position forbidding completely the passage. The band is blocked in this zone and thecongurations who were in the obstacle rest in collision. This conrms that the band is not ina valid state. It is then necessary to verify the validity of the next congurations to executeand to recognize such cases of failure. This problem may be treated dierently if producednear the goal. In this case, it is possible to associate an important mass to the goal positioninstead of considering it xed and then submit it to the various forces. Thus it will moveonly when an obstacle enters in collision with the goal position3.During their works, S. Quinlan and O. Khatib [Quinlan 93, Quinlan 95] enriched theirmethod by the notion of bubbles and the use of a distance function. A bubble is the biggestsubspace in free-space reachable from a given conguration. The elastic band becomes asequence of connected bubbles which join the initial conguration with the nal conguration.The basic tool in the denition of a bubble is a distance function. Then, even though it isdened for a holonomic system (with an Euclidean distance), this notion remains sucientlygeneral and altogether attractive, to be extended to non-holonomic systems at the onlyexpense of nding the adequate metric. This is the kernel of our motivation for the workpresented in next chapters.3We insist on the fact that this should be decided by the upper levels in the architecture.96

5.3. THE MDT 97-a- -b--c- -d-

-e-Figure 5.8: Experimentation of the elastic band. -a- Bitmap construction, -b- Initial tra-jectory, -c- Trajectory optimization and smoothing, -d- Initial elastic band, -d- Nominalexecution of the elastic band, -e- Elastic band deformation to avoid an unexpected obstacleduring execution. 97

5.3. THE MDT 98

Figure 5.9: Laser points, segmentation, ultrasonic echoes and execution trace.98

Chapter 6 Nonholonomic bubble band6.1 Bubble Band ConceptA bubble is dened by its center and is the biggest free spheric (in its own metric space) subsetin the conguration space. A sequence of connected bubbles is sucient to restrict treatmentto free-space reachable by the robot. Otherwise, the use of bubbles guarantees collision-freefeasible paths.Given n congurations fpigi=1:::n in free space such that the bubbles dene around themB(pi) overlap: B(pi)\ B(pi+1) 6= ; i = 1 : : :n 1;it is possible to construct a collision-free path joining the rst to the last (p1 and pn respec-tively).For example, for a holonomic point robot restricted to the plane, a bubble around aconguration is the circle center at this conguration whose radius is the shortest distanceto obstacles.Several examples are developed in [Quinlan 95] to illustrate the form of bubbles associatedto dierent distance functions, conguration spaces and robot forms. We develop here one ofthese examples in order to clarify such an abstract notion.Consider the case of a planar robot with rotation. It has three degrees of liberty. Therobot conguration is expressed by the vector (x; y; )T . Let d be the current shortest distanceto obstacles from the conguration q. If r is the radius of the circle containing the robot(gure 6.1 on the following page). The general bubble construction scheme is the following.From the expression of the movement of points on robot from one conguration to another,a distance function is derived by taking the longest path traveled by any such point. Thebubble is then the set of congurations whose distance is inferior to d.Suppose the robot moves from the conguration q to the conguration p. It realizes atranslation followed by a rotation. During the translation, each point on the robot covers thesame distance which is the Euclidean distance between the points (qx; qy) and (px; py), givenby: 99

6.1. BUBBLE BAND CONCEPT 100dt(q;p) =q(qx px)2 + (qy py)2:During rotation, the distance covered by every point depends on its distance from the centerof the robot. Thus, the maximum distance is that of the most eccentric point, at a distancer from the center. This covered distance is:dr(q;p) = rjq pj:The maximum distance covered by a point on the robot is the sum of these two distances.The bubble B(q; d) is then dene by:B(q; d) = fp : dt + dr < dg: y xd A bubbleObstacleRobot rFigure 6.1: A bubble for planar rotating robots.The bubble in gure 6.1 is dened to be the set of congurations fpig at a distanceinferior to d from the conguration q. If this bubble is in free space, all its congurations areaccessible from q by collision-free shortest paths.We now develop a second example which is a plotter arm with two degrees of freedom: thepolar coordinates and (gure 6.2 on the following page). is the length of the arm and is the rotation angle around the origin. Suppose the arm moves from the conguration q =(q; q)T to the conguration p = (p; p)T by eectuating a rotation and then a translation.The distance covered by the extreme point which is the maximum distance is:dr + dt = qjq pj+ jq pj;and the corresponding bubble is:B(q; d) = fp : dt + dr < dg:Figure 6.2 shows the bubble deduced for the plotter arm for three dierent distance valuesdi; i = 0; 1; 2. Note that the bubble form tends to a circle when the distance tends to innity,or when the initial value of is zero.In order to construct a bubble in the case of a car-like robot, we will introduce in thenext paragraph the metric space associated to this type of robots.100

6.2. METRIC STRUCTURE 101122 y xBubbles

d = d1d = d2d = d0d0 < d1 < d21Figure 6.2: Bubbles for a plotter arm.6.2 Metric StructureHere, we present the metric space associated with car-like nonholonomic mobile robots havinga non-null turning radius R. A conguration of the robot is the triplet consisting of its positionand its orientation (x; y; )T dened in the space R2S1. It has been proven that the shortestpath between two congurations belongs to one of 46 simple sequences of at most ve pieces,each piece being a straight line segment S or an arc of a circle C with at most two cusp pointsj [Reeds 90] and a partition of the conguration space into domains each corresponding toone path type have been achieved [Laumond 93]. Finally, the length of the shortest path wasproven to be a distance functiondcc : (R2 S1) (R2 S1) ! R+providing, thus, the conguration space with a metric structure compatible with the robotsmovement.Afterwards, the problem of the distance between the robot and point and segment ob-stacles was tackled and solved [Soueres 94]. The dierence between the distance betweentwo congurations and the distance between a conguration and a point is that in the latercase the orientation of the end of the shortest path joining the conguration and the point isirrelevant. This new distance dcp : (R2 S1) (R2) ! R+:is not a real distance function on a space but a convenient operation relating two space. Thisdistance has three analytical denitions in three regions of the space (gure 6.3 on the nextpage)The x-axis and y-axis symmetry of these paths and domains makes it possible to studyonly the rst quadrant. In this quadrant the paths, their end point coordinates and theirlengths are given the table 6.1, where l is a left turn, r is a right turn and s is a straight linesegment; The super-xes + and indicate forward and backward movements respectivelyand the indexes are the parameters, angles for turns and distances for segments.101

6.2. METRIC STRUCTURE 102(1) CjC2 S (3) CjS(2) CjCFigure 6.3: Domains of accessibility

Path End point Lengthj1 lu l+2 s+d R cosu R(2 + d) sinuR sin u+ R(2 + d) cosu R Ru+ R2 + dj2 lu l+v 2R sin u+ R sin(u+ v)2R cosu R cos(u+ v)R Ru+ Rvj3 l+v s+d R sin v +Rd cos vR cos v + Rd sin v + R Ru+ dTable 6.1: First quadrant conguration to point paths.102

6.2. METRIC STRUCTURE 103Consequently, the iso-distance balls were geometrically constructed [Vendittelli 96], anda distance between a conguration and a line segment was then developed and expressedanalytically. This distance is noteddcs : (R2 S1) (R2 R2) ! R+:We have thus three distance functions. One, a true distance, is dened on the cong-uration space transforming it to a metric space; the second, a distance between points inthe real space and the conguration space; the last, a distance between segments in the realspace and points in the conguration space. The last two distances are used in calculationsbetween the robot and the obstacles.S1 S3u v xy j1 j3v d dS22 j2Figure 6.4: Distance to segment in the dierent domains.Figure 6.4 shows the shortest paths from the origin of the conguration space to threesegments presenting thus the three kinds of conguration to point shortest paths.

© Oper © Oper © Oper-a- -b- -c-Figure 6.5: Dierent forms of the bubble and paths in the dierent domains.Figures 6.5-a-b-c present the form of the bubbles and the paths corresponding to theshortest distance in the three regions. 103

6.3. NONHOLONOMIC BUBBLE BAND 104DiscussionOur objective is to reconstruct the elastic band by replacing the particles by bubbles compat-ible with the movement of car-like robots. The use of these bubbles is the possible decreaseon the number of treated entities and the permanent and automatic existence of a feasiblepath even if the band is deformed. In the next paragraphs we give the proof of this propertyand develop the dierent parts of this new formalism.6.3 Nonholonomic Bubble BandWe now reformulate the bubble band concept for a nonholonomic mobile robot whose corre-sponding metric space will be noted . We notedcc : ! R+;the distance dened on this metric space. We also notedcp : R2 ! R+;the distance between congurations in and points in the plane. As usual, if A R2 thendcp(p;A) = minx2A d(p;x)for all p in .6.3.1 Denitions and PropertiesDenition 6.3.1 A bubble B(p; r) of center p and radius r is the set of congurations at adistance smaller than r from pB(p; r) = fq 2 jdcc(p;q) rg:The greatest bubble around a conguration p, B(p) is dened as follows:B(p) = B(p; dcs(p;O))where O is the set of segment obstacles.Here we would like to draw your attention to an important point. The bubbles are denedin the conguration space R2 S1 and are thus three dimensional even though the radius issometimes dened using distances from two-dimensional objects such as points and segments.Actually in the conguration space, a real space segment is represented as a face and a pointas a line.A bubble may be of two types, movable or immovable. A movable bubble is a bubblewhose center can be modied according to the forces applied to it. An immovable bubblekeeps the same center throughout the execution (for example the start conguration or thegoal when it is xed).A bubble is said to be smaller than another if they have the same center and if the rstis wholly included in the second.The biggest bubble around a conguration is dened as B(p) = B(p; dcs(p;O)); whereO is the set of segment obstacles. 104

6.3. NONHOLONOMIC BUBBLE BAND 105Denition 6.3.2 Two bubbles B(p; r) and B(q; s) are said to be connected ifB(p; r)\B(q; s) 6= :Proposition 6.3.1 Two bubbles B(p; r) and B(q; s) are connected if and only ifdcc(p;q) r + s:Proposition 6.3.2 Let B(p; r) and B(q; s) be two connected bubbles. The Reeds and Shepppath, [p;q], connecting the two centers is wholly included in the union of the bubbles.p qxB(p; r) B(q; s) [p;q] B(q; s0)Figure 6.6: Path between to consecutive centersPreuve If q is in the bubble B(p; r), then so is the path from p to q. Suppose then that qis not in the bubble B(p; r).Let s0 = dcc(p;q) r. B(q; s0) is the smallest bubble centered at q and connected toB(p; r). In fact if there is a smaller bubble verifying this and if s00 is its radius thendcc(p;q) r + s00 < r + s0 = dcc(p;q)which is impossible. On the other hand if B(q; s0) is not connected to B(p; r) thendcc(p;q) > r + s0 = dcc(p;q)which is as well impossible.Let x be in the intersection of B(p; r) and B(q; s0). dcc(p;x) = r and dcc(q;x) = s0,otherwise dcc(p;q) dcc(p;x) + dcc(q;x) < r + s0 = dcc(p;q)which is also impossible. Thusdcc(q;x) + dcc(p;x) = dcc(p;q):And from the unicity of the shortest path we deduce that x is on [p;q].Finally for every point y on [p;q], eitherdcc(p;y) dcc(p;x) = r105

6.3. NONHOLONOMIC BUBBLE BAND 106which implies that y is in B(p; r), ordcc(q;y) dcc(q;x) = s0which implies that y is in B(q; s0). This proposition enables us to prove the existence of a feasible path from the initialconguration to the nal conguration by nding a sequence of connected bubbles joiningboth congurations.6.3.2 ForcesThe forces applied to the bubbles are, as before, of two essential types. The external forcesdue to the repulsion of the obstacles and the internal forces which work to keep the bubblesconnected, as long as possible, and optimize the form of the band.Internal ForcesThe internal forces are functions of the successive congurations expressed using the metricdistance between congurations. In this way the particularities of the conguration spaceand thus of the robot constraints, being inherent to this metric are easily expressed.How should the bubbles interact with each other? This is a crucial question because oncethe inter-bubble interactions determined, they can be translated into potential elds, which intheir turn determine the internal forces of the bubble band. In this subsection B(pi1; ri1),B(pi; ri) and B(pi+1; ri+1) are three consecutive bubbles in a bubble band. Firstly, adjacent bubbles should remain connected. That isd+cc(pi) = dcc(pi;pi+1) ri + ri+1 c: (6.1)for some small c 2 R+. Secondly, adjacent bubbles should not overlap too much lest the band tends to be overcharged uselessly with bubbles, with consequent eects on the eciency of the method.Thus d+cc(pi) = dcc(pi;pi+1) ri + ri+1 o; (6.2)for some small o 2 R+ greater than c. Finally the band should tend to contract. Away from the obstacles it should even bea straight band. Thus a middle bubble should try to place itself on the path joiningits two neighbors. If [pi1;pi+1] R2 S1 is the path between the centers pi1 andpi+1 then: dcc(pi; [pi1;pi+1]) ;for some small 2 R+. More explicitly, If p[pi1;pi+1] : [0; 1] ! R2 S1 is aparameterization of [pi1;pi+1], pi should be close to the point p[pi1;pi+1](~ri)on the path, where ~ri = ri1ri1+ri+1 is the radius-weighed barycenter of pi1 and pi+1.Thus: d cc(pi) = dcc (pi; p[pi1;pi+1](~ri)) : (6.3)106

6.3. NONHOLONOMIC BUBBLE BAND 107Using the constraints 6.1 to 6.3 and noting dcc(pi) = d+cc(pi1), the following potentialelds are deducedPf(pi) = Kf2 d+cc(pi) (ri + ri+1) + cd+cc(pi) (ri + ri+1) + oPb(pi) = Kb2 dcc(pi) (ri1 + ri) + cdcc(pi) (ri1 + ri) + o (6.4)Pc(pi) = Kc2 d cc(pi)2:where, Kf , Kb and Kc are positive constants. Pf is called the forward connection potential,with next bubble, Pb the backward connection potential and Pc the contraction potential.Let Pi the total internal potential be:Pi = Pf + Pb + Pc:Then, the internal force is Fi(pi) = rPi(pi):As an example, let us calculate the force due to the forward connection potentialrPf (pi) = d+cc(pi) (ri + ri+1) + c + o2 @@pid+cc(pi)The rst factor is the magnitude of the force. This magnitude becomes null when thedistance d+cc(pi) = (ri + ri+1) c + o2 (6.5)is in the middle of the region between disconnection and overlapping. The second factor isthe direction of the force. It will be discussed in the subsection 6.3.2.External ForcesThe bubble band should alway evolve in free-space. In presence of mobile or immobile obsta-cles some interaction should take place to guarantee a collision-free band. These interactionsare performed by external repulsive forces, function of the distance to obstacles. These forcescan be simply issued from repulsive potential elds such as developed in chapter 1. Therepulsive potential is thus:Pr(pi) = P1r (pi) + P2r (pi); if dOcs(pi) dc,0; otherwise,where P1r (pi) = Kr2 (dOcs(pi) dc)2;P2r (pi) = K12 1dOcs(pi) 1dc2with Kr K1. Here dOcs(pi) = dcs(pi;O), O being the set of obstacles. This choice wasmotivated by two reasons. The rst part, P1r , which dominates near the equilibrium point,generates a linear force in that region which easily tuned. The second part, P2r , however,insures non collision near the obstacle border. In fact, it tends to 1 as the band approachesthis surface. 107

6.3. NONHOLONOMIC BUBBLE BAND 108Direction of forcesThe application of the r operator to derive the force expressions from the previously denedpotentials, brings us to calculate @dcs@p where p is the conguration that varies. In fact, thislast expression gives the direction of the force. Unlike the Euclidean distance, the car-likecompatible dcs, dened previously, have no normalized dierentials. They contribute thusto the magnitude of the force. As well, they do not vary smoothly with p, with consequenteects on stability. Path @dcs@x @dcs@y @dcs@j1 lu l+2 s+d sin u cos u R sin u Rj2 lu l+v sin usin v cos ucos v Rsin usin v Rj3 l+v s+d cos v sin v R cos v RTable 6.2: Derivatives of the distance from conguration to point.Let us have a close look on the dierential of the distance from a point function. Thisdistance has three dierent analytical expressions on three dierent domains. We note thatthe derivatives are continuous functions of x, y and almost everywhere except on the areof center (0; R)T and of radius R from (0; 0)T to (R;R)T separating the domains j2 and j3(see table 6.2).

108

6.3. NONHOLONOMIC BUBBLE BAND 109© Oper © Oper-a- -b-© Oper © Oper-c- -d-

© Oper -e-Figure 6.7: Continuity and discontinuity of th direction of the repulsive force.Figures 6.7-a- and 6.7-b- show a discontinuity in the direction of the force between thedomains j2 and j3 . This is an inherent discontinuity to Reeds and Shepps paths. Figures 6.7-c- and 6.7-d- show another type of discontinuity that arises. This one is due to the non-convexity of the ball in the Euclidean plane. In the ve gures is only shown the x and ycomponents of the force. 109

6.4. CREATION AND MODIFICATION OF A BUBBLE BAND 1106.4 Creation and Modication of a Bubble BandAs the band expands, bubbles are created to guarantee the connectivity. Bubbles are alsocreated when an obstacle approaches the band obliging the bubbles to shrink. Inversely,when the band is contracted or when obstacles move away, bubbles may overlap too much,that some of them should be eliminated. Here we will discuss the strategy of creating andeliminating bubbles.6.4.1 Bubble Band ModicationpiB(pi; ri) pi+1 pi+1pi+ 12B(pi+ 12 ; ri+ 12 ) B(pi+1; ri+1)B(pi+1; ri+1)

Figure 6.8: Creation of a bubble before disconnection.Suppose that we have a connex bubble band. For each two adjacent bubbles, say B(pi; ri)and B(pi+1; ri+1), if the following inequality is satiseddcc(pi;pi+1) ri + ri+1 c;where c is a positive constant, then an intermediate bubble should be created between them.In order to be around the intersection of its two neighbors, the center of this bubble shouldbe on the path, p[pi;pi+1] : [0; 1] ! R2 S1, relating the two neighboring centers; on thepoint pi+ 12 = p[pi;pi+1] riri + ri+1 :On the other hand, if three consecutive bubbles, noted B(pi1; ri1), B(pi; ri) andB(pi+1; ri+1) have the propertydcc(pi1;pi+1) ri1 + ri+1 o;where o > 0, then they are considered to be redundant and the middle bubble B(pi; ri) iseliminated. 110

6.4. CREATION AND MODIFICATION OF A BUBBLE BAND 111To prevent the creation-elimination loop from endlessly creating and eliminating the samebubble, the following condition should be satisedo > c: (6.6)This condition expresses a Hysteresis function that governs the creation-elimination pro-cedure. Note that the elimination test may be applied to all the succeeding bubbles in orderto detect a loop (gure 6.9) and remove it if desired1.© Oper

Possible path.

Figure 6.9: Detection of a loop between bubbles.Bubble Band Creationp1 p2 p3 p4 p5 ObstacleFigure 6.10: Creation of the rst bubble band.To construct the original band, we start with a path, which may be set by a motion plannerand which is not obliged to be feasible. This path is rstly sampled into a sequence ofcongurations if it is not already in such a form. Then the rst bubble is dened to be the1We still consider that such a decision must be taken at higher levels.111

6.5. BUBBLE BAND BEHAVIOR EXAMPLES 112greatest bubble centered at the rst conguration in the sequence. The sequence is thenscanned to nd the last conguration which still belongs to the last constructed bubble. IfB(pi; ri) is the lastly created bubble. The condition for pi+1 to be the center of the nextbubble is then: dcc(pi;pi+1) < riThis conguration is thus taken to be the center of the following bubble which also hasthe maximum size: ri+1 = dOcs(pi+1)where O is the set of obstacles.The previous step is repeated till the goal, last conguration, is found to belong to abubble. A last xed bubble is created around the goal even though this last is found in theprevious bubble.This method insures the connectivity of the bubble band, as well as its total presence infree-space while making it redundant, obliging the center of each bubble, but the rst, tobelong to its previous neighbor. Nevertheless, this redundancy is not an inconvenience, forafter the rst application of elimination algorithm and of the contraction forces, the bandwill become spacious.6.4.2 StabilityThe evolution of the bubbles in the band is governed by conservative forces issued fromconvex potential elds. For contraction forces, the potential elds are functions of the metricdistance which is a true distance and conserves thus the convexity property. These potentialelds have thus a unique minimum. Using the same displacement method described by theequation ( 5.4 on page 93), we add a dissipative force to that issued by the potentials andobtain: Fi (pi) = rPi(pi) k _pi _piwhere k _pi is a positive constant gain.For obstacles repulsion forces, the potential elds are functions of the point to congura-tion distance which is not a true distance. In fact, this distance presents a problem for lengthsinferior to p2R, as discussed in the sub-section 6.3.2. This is due to the discontinuity of thedierential of this distance and to the fact that the shortest paths, whose length denes thedistance, does not have the property: All sub-paths of shortest paths are shortest paths. Infact, in domains j1 and j2 , sub-paths of length inferior to p2R may not be shortest paths.To guarantee the convergence of the bubble band we impose thus a minimum radius,Rmin = p2R, on the bubbles.6.5 Bubble band behavior examplesWe suppose that a planner using the environment model to produce a not necessarily exe-cutable path which is the bubble band entry.The rst step (x 6.4.1 on the preceding page) consists of sample the path into a sequenceof bubbles connecting the two extremities of the path and forming the initial bubble band(gure 6.11 on the next page). An executable trajectory is thus created. The application ofthe internal forces (x 6.3.2 on page 106) optimize the form of the band (gure 6.13). Note112

6.5. BUBBLE BAND BEHAVIOR EXAMPLES 113also that these forces modify the size of the bubbles under contraction eects. The obstaclesrepulsion is then applied (x 6.3.2 on page 107) provoking a clearance of bubbles and a greaterspace in each (gure 6.13 on the following page).Nevertheless, the band reacts to unexpected obstacles (gure 6.14 on page 115), by de-forming the band. The trace of the robot along the trajectory is presented in gure ( 6.15on page 115). Note that the entire trajectory in included in the union of the bubbles.© Oper

Figure 6.11: Bubble band creation and the rst feasible trajectory.

113

6.5. BUBBLE BAND BEHAVIOR EXAMPLES 114© Oper

Figure 6.12: Internal forces application and optimization of the band.© Oper

Figure 6.13: Application of internal and external forces and clearance from obstacles.114

6.5. BUBBLE BAND BEHAVIOR EXAMPLES 115© Oper

Figure 6.14: Deformation of the band due to an unknown obstacle.© Oper

Figure 6.15: Bubble band trajectory trace.115

Chapter 7 Bubble band executionIn this chapter is developed the aspects related to the execution of the trajectory issued fromthe bubble band.We have seen in the last chapter that the path produced by the bubble band passesby all the bubbles joining the initial to the goal congurations by a sequence of arcs andsegments. The path to execute is thus the concatenation of the Reeds and Shepp pathsf [pi;pi+1]; i = 1 : : :n 1g between the centers of the n bubbles. Each of these paths is aset of arcs and segments and its curvature is thus discontinuous forcing the robot to stop atthe end of each part.Several methods are being used currently to smooth these paths such as the use ofclothodes proposed in [Fleury 95] to join segments to arcs. In our case the curve afterbeing smoothed must belong to the union of bubbles and thus to the free-space. Also, forreasons related to execution control (synchronization problems between tasks with dierentcycle periods detailed in paragraph 7.3 on page 127) the parameterization of the part to beexecuted must be global and not incremental. These two reasons brought us to adopt Beziercurves. The advantage of Bezier polynomials is that they can be wholly dene in a convexenvelop as well as their derivatives. We can thus guarantee that the path executed lies in theunion of the bubbles, thus in free-space. As well, the velocity and acceleration on these curvesare bounded, for each belongs to a convex envelop of a nite number of constant vectors.The only measure that cannot be bounded is the curvature. Here we choose a set ofcontrol points to insure that the curvature lies in the neighborhood of R, the steering radius.7.1 Approximation by Bezier PolynomialsDenition 7.1.1 A Bezier curve of order n+1 and control points p = fp0; : : : ;png is denedby BP[p; [ti; tf]](t) = nXi=0 B[ti; tf]ni (t)pi;116

7.1. APPROXIMATION BY BEZIER POLYNOMIALS 117where t 2 [ti; tf] andB[ti; tf]ni (t) = ni (tf t)ni(t ti)i; for i = 0 : : :n.We use the following properties of Bezier curves A Bezier curve lies in the convex envelop of its control pointsBP[p; [ti; tf]] co(p): (7.1) The initial and nal points on the curve areBP [p; [ti; tf]](ti) = p0 and BP[p; [ti; tf]](tf) = pn (7.2)respectively. The successive derivatives of a Bezier curve are Bezier curves whose control points arenite dierences of the original control points@k@tkBP [p; [ti; tf]](t) = n!(n k)! 1(tf ti)kBP [kp; [ti; tf]](t): (7.3)where kp = fkp0; : : : ;kpnkg; (7.4)the nite dierences kpi being dened in the following way0pi = pi;kpi = k1pi+1 k1pi: A Bezier curve BP [p; [ti; tf]] can be split into two Bezier curves BP [p0; [ti; ts]] andBP [p00; [ts; tf]] such thatBP[p0; [ti; ts]](t)= BP [p; [ti; tf]] 8t 2 [ti; ts];BP [p00; [ts; tf]](t)= BP [p; [ti; tf]] 8t 2 [ts; tf]: (7.5)Here p0 = f0ti;ts;tfp0; : : : ;nti;ts;tfp0g and p00 = fnti;ts;tfp0; : : : ;0ti;ts;tfpng: (7.6)where the convex combinations kti;ts;tf are dened in the following way0ti;ts;tfpi = pi;kti;ts;tfpi = ts titf tik1ti;ts;tfpi+1 + tf tstf ti k1ti;ts;tfpi: Bezier curves are invariant by geometric transformations such as translation, rotationand scaling. That is if T is such a transformation,T BP [p; [ti; tf]] = BP [T p; [ti; tf]]: (7.7)117

7.1. APPROXIMATION BY BEZIER POLYNOMIALS 118For a fuller development of Bezier curves see [Fiorot 89].According to the rst property (equation 7.1) choosing the control points in a bubble or,a region if the space, guarantees that all the curve is in that bubble, or region. The initialand nal positions, velocities and curvatures can easily be controlled through the controlpoints (equations 7.2 and 7.3). The third property (equation 7.3) combined with the rstone (equation 7.1) insures that the derivatives @k@tkBP [p; [ti; tf]] are contained in the convexenvelops co( n!(nk)! 1(tfti)kkp) and thus that the velocity and the acceleration are bounded.Finally, the last property (equation 7.5) enables us to divide a Bezier curve at any point andobtain two curves of the same nature, thus we can reparameterize a part of the curve to stopor change its nal velocity or simply drop it.Reeds and Shepp paths are made up of line segments and arcs of circles. To join themsmoothly, we will approximate the arcs by Bezier curves whose curvature is null at the twoextremities. To satisfy this condition and x the starting and nish positions and velocities weneed at least four control points p = fp0;p1;p2;p3g [Segovia 91]. Equation 7.2 determinesthe points p0 and p3 as the two extremities of the arcpi = BP [p; [ti; tf]](ti) = p0and pf = BP [p; [ti; tf]](tf) = p3;where pi and pf are the initial and nal points of the arc respectively. Thusp0 = pi and p3 = pf: (7.8)Equations 7.2 and 7.3 oblige the point p1 (respectively p2) to be on the array starting at p0(respectively p3) and tangent to the arcti / BP 0[p; [ti; tf]](ti) / p1 p0and tf / BP 0[p; [ti; tf]](tf) / p3 p2where ti and tf are the initial and nal tangent unitary vectors to the arc. Thusp1 = pi + ti and p2 = pf tf (7.9)for some positive constants and . The initial and nal curvatures are theni = jjBP 0[p; [ti; tf]](ti) BP 00[p; [ti; tf]](ti)jjjjBP 0[p; [ti; tf]](ti)jj3= 45 p0 2p0jjp0jj3 (7.10)and f = jjBP 0[p; [ti; tf]](tf) BP 00[p; [ti; tf]](tf)jjjjBP 0[p; [ti; tf]](tf)jj3= 45 p3 2p2jjp3jj3 : (7.11)118

7.1. APPROXIMATION BY BEZIER POLYNOMIALS 119p1 = p2p3p0ArcConvex EnvelopBezierFigure 7.1: Four control points Bezier approximation of an arc of length 3 .After substitution of the values of pi, i = 0; : : : ; 3 these expressions becomei = jjti (pf pi tf)jj and f = jjtf (pi pf + ti)jj : (7.12)To clarify this, let us consider an arc of center (0; 0)T and radius 1 whose two extremitiesare (1; 0)T , corresponding to the angle 0, and (cos ; sin )T , corresponding to the angle .This is not a restriction because of the invariance of Bezier curves (equation 7.7 on page 117).The two extremities are (equation 7.8 on the page before)p0 = 10 and p3 = cos sin ;and the other two points are (equation 7.9 on the preceding page)p1 = 1 and p2 = cos + sin sin cos :The parameters and determine the curvatures at the extremities. These curvatures are(equation 7.12) i = 23 1 cos sin 2 and f = 23 1 cos sin 2 :When both curvatures are null = = sin cos + 1 :In fact the two triplets fp0;p1;p2g and fp1;p2;p3g should each be collinear, and thus p1 = p2(gure 7.1).This approximation has two drawbacks. In order to smooth the edges of the arc, thecurvature at the middle tends to increase a lot. It is at least 2.5 times greater than thecurvature of the original arc (gure 7.4 on page 122). Since the two middle points enterin the calculation of curvature of both extremities it is not easy to determine their valuesfor arbitrary curvatures. The rst drawback is due to the lack of supplementary degrees119

7.1. APPROXIMATION BY BEZIER POLYNOMIALS 120Arc p1p0Convex Envelop p2p3p4p5 BezierFigure 7.2: Six control points Bezier approximation of an arc of length 3 .of freedom that can be manipulated to control the curvature and the second is due to thefewness of the control points. These reasons bring us to approximate the arcs by Beziercurves of greater order.Here we propose an approximation with six control points Bezier curves. For reasons ofclarity, we shall base our discussion on the previously dened arc. The two points at eachextremity are determined by the relations 7.8, 7.9, 7.10 and 7.11 (replacing the indexes 2 and3 by 4 and 5). This gives usp0 = 10 and p5 = cos sin (7.13)and p1 = 1 and p4 = cos + sin sin cos : (7.14)The expressions for the curvatures at the two extremities being dependent on three dierentcontrol points each, we havep2 = 1 sin+ cos and p3 = cos + sin + sin( + )sin cos cos( + ) (7.15)where the six parameters are related by the expressions of the curvaturesi = 45 sin2 and f = 45 sin 2 : (7.16)The geometric interpretation of these parameters is the following. On the one hand, is the distance between p0 and p1, is the distance between p1 and p2 and is the anglebetween the lines p0p1 and p1p2. On the other, is the distance between p5 and p4, is thedistance between p4 and p3 and is the angle between the lines p4p5 and p3p4 (gure 7.3on the next page).Once and are determined by equations 7.16, we are left with four parameters, , , and , to control the curvature bounds. 120

7.1. APPROXIMATION BY BEZIER POLYNOMIALS 121p4p5 p3 p2p1p0p5 p4 p3

Figure 7.3: Six control points Bezier approximation of an arc of length 3 .121

7.1. APPROXIMATION BY BEZIER POLYNOMIALS 122t

(t) t

(t)Figure 7.4: Curvature of the four and six control points Bezier approximation of an arc.The curvature bounding problem is thus reduced tomin>0;>0;>0;>0 maxt2[0;1];2[0;2 ] (t)! ;where (t) = jjBP 0[p](t) BP 00[p](t)jjjjBP 0[p](t)jj3 ;and is the arc angle.This optimization problem has not been solved yet. For the mean time, we use values of and that bounds the curvature to 54 where is the curvature of the arc ( = 1R if R isthe steering radius).Figures 7.4 shows the variation of the curvature for four control points and six controlpoints Bezier approximations of arcs with null initial and nish curvatures. These surfacesare functions of t and the arc length . The horizontal plane in both gures represents theconstant curvature of the arc. A good approximation should exceed this value as little aspossible.A segment being the convex envelop of its extremities is exactly parameterized by a Beziercurve of order 2 whose two control points are the two extremities.A complete Reeds and Shepp path approximated by Bezier curves is shown in gures 7.5.In gure -a- arcs are approximated by four control points Bezier curves while in gure -b-they are approximated by six control points Bezier curves. Segments are approximated bytwo points Bezier curves in both gures. 122

7.2. PARAMETERIZATION OF BEZIER POLYNOMIALS 123© Oper

4−Bezier Arc Approximation

4−Bezier Arc Approximation

4−Bezier Arc Approximation

2−Bezier Segment Approximation

Start

Goal

© Oper

2−Bezier Segment Approximation

Start

Goal

6−Bezier Arc Approximation

6−Bezier Arc Approximation

6−Bezier Arc Approximation-a- -b-Figure 7.5: Path approximation by Bezier curves.7.2 Parameterization of Bezier PolynomialsIn the previous section we were concerned by the geometric approximation of arcs and seg-ments. Once these curves (approximations) are found, they should be followed according toextremity constraints such as initial and nal velocity. This could be done by reparameter-izing the curves.If ' : [si; sf] ! [ti; tf]is an onto function strictly increasing on the open interval (ti; tf). Then[si; sf] ! R2 S1;s ! BP[p; [ti; tf]]('(s));is a new parameterization of BP [p; [ti; tf]]. If we have a predetermined starting velocity vi,this parameterization should verifyBP 0[p; [ti; tf]]('(si))@'@t (si) = vi:Similarly, if the nishing velocity vf is known,BP 0[p; [ti; tf]]('(sf))@'@t (sf) = vf:The reparameterization can also be used to modify the maximum velocity. In fact, dueto the convex envelop propertyBP 0[p; [ti; tf]](t) 5 maxi=0:::4 jjqijj ; 8t 2 [ti; tf]:Thus, to guarantee that the velocity does not exceed vm, ' should verify@'@s (s) vm5 maxi=0:::4 jjqijj ; 8s 2 [si; sf]:A similar reasoning can also be applied to bound the acceleration, the angular velocity andthe angular acceleration.We have tried several functions ' including 123

7.2. PARAMETERIZATION OF BEZIER POLYNOMIALS 124_(t) (t) t _(t) (t) t _(t) (t) t-a- -b- -c-Figure 7.6: Dierent ' functions. a cubic polynomial (gure 7.6-a-), a cubic spline composed of two parts (gure 7.6-b-), a cubic spline composed of three parts (gure 7.6-c-), the middle part being a straightline segment.In each case ' should verify the four following conditions'(si) = ti; '(sf) = tf; @'@s (si) = 'i and @'@s (sf) = 'f: (7.17)In the cubic polynomial case, this determines the four polynomial coecients. The re-maining unknown is the parameterization time s = sf si. This is determined by xingthe maximum of the rst derivative maxs2[si;sf] @'@s (s) = 'm: (7.18)This condition tends to optimize the execution time.In the two piece-cubic spline the condition 7.18 becomes@'@s (ss) = 'm and @2'@s2 (ss) = 0; (7.19)where ss is the spline control time, i.e. the time when the two polynomial pieces join. Wehave up to now six conditions, and since we have seven unknowns (the ve spline coecients,s and ss) we add the following condition(ss si)('m 'f) = (sf ss)('m 'i); (7.20)This guarantees that ss is in [si; sf] and thus the monotonous character of '.In the three-piece cubic spline the condition 7.19 is again changed. Now we have twospline control times (ss1 and ss2) and we are interested in splines where the middle piece islinear thus @'@s (ss1) = @'@s (ss2) = 'm and @2'@s2 (ss1) = @2'@s2 (ss2): (7.21)The condition 7.20 becomes(ss1 si)('m 'f) = (sf ss2)('m 'i) (7.22)124

7.2. PARAMETERIZATION OF BEZIER POLYNOMIALS 125and ss2 ss1 = p(sf si); (7.23)where p is an arbitrary constant in [0; 1].The constant p used in equation 7.23 xes the importance of time to be passed on thelinear (middle) piece. The bigger it is the lesser time is given to initial and nal accelerations.In fact, when it is equal to 1, the rst derivative of ' is discontinuous at the extremities.When it is equal to 0, we get the two-piece spline. This constant should be xed accordingto the maximum acceleration of the robot.The conditions 7.20 and 7.22 present a problem when 'i or 'f is equal to 'm becausethen one of the control times is undetermined. Thus they are modied to(ss si)'m b'i (1 b)'f = (sf ss)'m (1 b)'i b'f (7.24)and (ss1 si)'m b'i (1 b)'f = (sf ss2)'m (1 b)'i b'f; (7.25)where b is an arbitrary constant in [0; 1] which should be as small as possible.© Kinan

t

Linear Vel

t

Angular Vel

t

Linear Acc

t

Angular Acc

Execution Time: 18.81Figure 7.7: Velocity and acceleration proles for polynomially parameterized 2- and 4-Bezierapproximation of a path.Figures 7.7 to 7.10 on page 127 show the proles of the linear and angular speeds andaccelerations for the dierent approximations and parameterizations discussed in this section.These proles correspond to the example shown in gures 7.5 on page 123. From them wecan remark several facts. 125

7.2. PARAMETERIZATION OF BEZIER POLYNOMIALS 126© Kinan

t

Linear Vel

t

Angular Vel

t

Linear Acc

t

Angular Acc

Execution Time: 14.95Figure 7.8: Velocity and acceleration proles for 3-spline parameterized 2- and 4-Bezier ap-proximation of a path.© Kinan

t

Linear Vel

t

Angular Vel

t

Linear Acc

t

Angular Acc

Execution Time: 15.50Figure 7.9: Velocity and acceleration proles for polynomially parameterized 2- and 6-Bezierapproximation of a path. 126

7.3. A PROPOSED EXECUTION CONTROLLER 127© Kinan

t

Linear Vel

t

Angular Vel

t

Linear Acc

t

Angular Acc

Execution Time: 12.14Figure 7.10: Velocity and acceleration proles for 3-spline parameterized 2- and 6-Bezierapproximation of a path. Arc four control points Bezier approximation leads to a more important excess in angu-lar speed than six points Bezier approximation for the three types of parameterization.In fact, this is inherent to its incapacity to control its maximum curvature. Three-piece spline parameterization leads to better execution times. This is due to itscapacity to reach higher speeds and maintain them longer. Its maximum is spread onthe middle segment instead of being bound to a single point. The nal speed of a Bezier curve is null in two cases: at a cusp point and at the end.Otherwise it is determined by the parameterization in order to limit the linear andangular accelerations. The maximum speed is dependent on the length of the Bezier curve. This is also dueto bounding the accelerations.For a more complete analysis see [M. Khatib 96].7.3 A proposed Execution ControllerHaving discussed the generation and handling of the bubble band and the approximation ofits successive parts by parameterized Bezier curves, we will now present the link betweenthese two aspects. In fact, the bubble band is a dynamic system, evolving endlessly withtime (in function of the environment). To execute the corresponding mobile path, we haveto x a piece of it. 127

7.3. A PROPOSED EXECUTION CONTROLLER 128p1 p2B(p1; r1) B(p2; r2)p1B(p1; r0)B(p1; r1)

Figure 7.11: Fix piece of length r0 in rst bubble.To x a piece of length l in B(p1; r1) on the Reeds and Shepp path joining p1 with thecenter p2 of the second bubble B(p2; r2), we set two bubbles each at an extremity of thispart. The rst bubble, B(p1; r0), will have a radius r0 = l + c, and the second bubble,B(p1; r1), will have a radius r1 = R1 l insuring thus the connectivity and the existence infree-space.Our Bezier approximation method approximates an elementary piece (an arc or a segment)at a time. It does not approximate a concatenation of several elementary pieces (such as anarc followed by a segment or two successive arcs with opposite centers, : : : ) This is why whena part of a path is xed its length l should be less than the length of the rst elementarypiece `p. Still this length should be enough for the robot to stop in case of emergency. Thenthe initial velocity of this part vi (which is the nal velocity of the previous part) should bebound in function of `p. Suppose that this is true. Then we should havel = 32 viam `p:This is not all the truth, since a thought should be given to next piece. In fact, the robotshould always have the greatest speed that its capacity and the paths form allow. Then theremaining length `p l on the elementary piece should be greater than the minimal lengthin which the robot can stop completely if it started this piece with the maximum speed vm.If this is not veried l is taken to be `p. More concisely1. l = 32 viam `p.2. if `p l < 32 vmam then l = `p.From now on when a part is said to be xed, its length is determined using this method.For a part to be parameterized, its nal speed should be determined. To do this, a forwardlook should be eectuated to test for a cusp point or the end of the path. In both cases thenal speed is null. Otherwise it is the maximum speed that the parameterization algorithm128

7.3. A PROPOSED EXECUTION CONTROLLER 129can aord according to the approximation of the current part and the length of the followingelementary piece of the path.Here is the complete execution controller. The behavior of the Command will be discussedafterwards.Initialization Let the initial speed vi = 0, reset the initial time si and send track request toCommand.Main Loop1. Terminate if the Reeds and Shepp path on the poster of Bubble Band is empty.2. Read the rst two elementary pieces [p1;p2] and [p2;p3] if they exist or theonly one [p1;p2] from the Bubble Band. Here an elementary piece is an arc or asegment.3. Fix a part [p1;p1] on [p1;p2].4. The next is elementary piece is [p1;p2] if p1 6= p2 or [p2;p3] else-wise.5. Request from Bezier the approximation and parameterization of [p1;p1] usingthe next elementary piece to determine the nal speed.6. Read the parameterized curve p[p1;p1] from Bezier. Note vf its nal speed andsf its expected nal execution time.7. If vf is null proceed to step 10.8. Fix a second part [p1;p2] on the next elementary piece with initial speed vf.9. Request from Bezier the approximation and the parameterization of [p1;p2] withnull nal speed.10. Read the current execution time sc from Command.11. If si sc > proceed to step 1.12. Request from Bubble Band to advance the initial point to p2.13. Write the parameterized curve(s) on the poster and set vi to vf and si to sf.14. loop: proceed to step 1.The Command is activated by the track request. It waits till one or two new parameterizedcurves are written on the poster of the Execution Controller. The execution of the rstparameterized curve is started if the last one ends with a null speed. While executing acurve, the Command updates robot state and especially the current execution time in itsposter. If the rst curve is terminated and the poster of the Execution Controller is notupdated the second curve is executed if it exists and the track request is terminated.Figure 7.12 on the next page shows the parameterizations of a sequence of split piecesof a Reeds and Shepp curve. Two parameterizations are shown for each piece, one for thenominal execution and the other with null nal speed.DiscussionThe gure 7.13 on the following page shows the control architecture of Kinan1 system imple-mentation.1Kinematics Integrated in Non-holonomic Autonomous Navigation129

7.3. A PROPOSED EXECUTION CONTROLLER 130© Kinan

t

Linear Vel

t

Angular Vel

t

Linear Acc

t

Angular Acc

Execution Time: 12.15Figure 7.12: Velocity and acceleration proles for a split 3-spline parameterized 2- and 6-Bezier approximation of a path.PerceptionBubble

Band

Robot State

ControllerExecution

Bezier Curve(s)1 or 2 Parameterized

MeasuresRS Path

Parameterized BezierCurve

Bezier Approximation&

Parameterization

Command

Environment

Motor

Functionality

Command

Write

Read

Request

Data

Planner

PathExecute

Traking

Smooth Advance Concatenate

Figure 7.13: The proposed architecture of Kinan system130

7.4. CONCLUSION 131This Execution Controller is not wholly implemented yet.We still think that this archi-tecture and execution controller algorithm present an ecient structure to combine low andhigh level modules with dierent time requirements in a real-time environment.© Oper

-0.4

-0.2

0

0.2

0.4

0.6

15 20 25 30 35

>19.3, 0<

vOdowOdoTime (secs)

Line

ar &

Ang

ular

Vel

ociti

es\To" movement: Execution sequence trace Linear and angular speedsof four control points Bezier. of the robot.© Oper

-0.4

-0.2

0

0.2

4 6 8 10 12 14 16 18 20 22

>17, 0<

vOdowOdoTime (secs)

Line

ar &

Ang

ular

Vel

ociti

es\Fro" movement: Execution sequence trace Linear and angular speedsof six control points Bezier. of the robot.Figure 7.14: Two, four and six control points Bezier curves sequence execution with corre-sponding speed proles.The implementation of Bezier module on the robot Hilare2 shows that the use of Beziercurves of order 4 produce smoother speed proles (gure 7.14)2 This is due to easiness withwhich arc approximation extremity speeds are linked to those of adjoining segments. On theother hand the speed decrease in the middle of the arc approximation. In the case of Bezierapproximations of order 6 the parameterization tries always to reach the maximum velocityin the middle of the arc approximation. where the curvature is almost constant. Nevertheless,the extremity speeds are smaller than before. Still a better approximation of the arc, in thecase of order 6 Bezier curves, leads to shorter execution time.7.4 ConclusionThis part of the memory presents a new method to combine planning and reactive controlfor a non-holonomic mobile robot with minimum steering radius. This method integrates the2Note that Hilare2 has no minimum steering radius. 131

7.4. CONCLUSION 132majority of the functions considered important in a reactive system such as: obstacle avoid-ance, trajectory execution and integration of environment model information. Moretheless,this method have the advantage of a capacity of optimization and smoothing such that itaccepts infeasible initial trajectories.The Reeds & Shepp metric and its shortest distance to obstacles enables the denition ofbubbles for car-like robots. This incites the development of potential functions on this metricand the study of the of their gradient behavior.The bubble band developed here, provide a feasible dynamically modiable collision-freetrajectory. Though this trajectory is incompatible with robot kinematics other than geometricconstraints, a powerful parameterization algorithm is proposed to satisfy these constraints.Two types of approximation and three types of parameterization are developed and testedaboard the platform Hilare2.Finally, an execution system (Kinan) is proposed for the implementation of the dierentalgorithms encountered in this part. An execution controller as well as the other activities ofthe system are designed to combine low and high execution levels with dierent cycle periods.

132

General conclusionThe work presented in this memory contributes to mobile robots movement feedback control.This contribution is a part of a decision control architecture of an autonomous mobile robotHilare. The presented studies treat dierent natures of interaction between planning andexecution of movement. The proposed solutions tackle the problem according to three axes:Re ex execution of movementOn the basis of purely reactive local avoidance actions, we have developed using theformalism of potential method, with the extensions developed in this work, a reactivesystem dedicated to the collision-free execution of trajectories. This system integrates,via virtual perception also introduced in this work, information locally describing theenvironment model. It enables more adapted reactivity and satisfactory behavior.This method demonstrated the eciency of reactive execution and its role in the se-curity of execution. We cite here several experiments realized in the context of theproject MARTHA concerning multi-robot coordination showing thus the necessity ofsuch a reactivity and the increase of performance it provides in global coordinationprocess execution. Even though it contributed to compensate the model and motionincertainity, frequent relocalizations are needed by the system which treats only ge-ometric and absolutely referenced information (e.g. a trajectory, a goal position, anenvironment model, : : : ). Thus the limitations of the system for complex and longtasks.Sensor-based execution of movementThe task potential formalism proposed next, provides the necessary theoretical contextfor the study and development of several important sensor-based actions of which themost signicant were mentioned (and some developed) in this manuscript. Via theseelementary actions and an execution controller, a link between planning with incertain-ity and movement was established and a robust displacement system by sensor-basedactions in presence of incertainity was dened. This system was implemented on theHilare2 robot and a number of complex tasks demonstrated the cohesion of the methodand the security of navigation. The developed system is but a demonstration of theapproach consisting of integrating planning to execution. Taking into account incer-tainity provides the navigation system with supplementary advantages. Nevertheless,a more simple planning or a direct use of an operator (RALPH) remains possible sincethe movement is based on sensor feedback.Execution and optimization of movement by thought re ex actionsThe elastic band technic present in the third part of this work presents a new method-ology linking planning and execution. Thought reactivity combines the taking into133

7.4. CONCLUSION 134account of global information to modify locally a trajectory and the propagation ofsuch a modication to the totality of the trajectory. The supplementary advantageof this method is its capacity of adjust the trajectory to the environment diminishingthus the importance of planning to a simple topological guide. The experiments wehave eectuated in this contexts proved both the great capacity of this approach andits incompleteness with respect to non-holonomic mobile robots. The bubble notionwith the appropriate metric space (Reeds & Shepp) enabled the elaboration of a morecompatible context for such a robot. This method was modelized using potential eldsand a new aspect of the behavior of forces in this metric space was claried.Nevertheless, direct execution of trajectory issued from this method was impossible(kinematic constraints are simply violated) and we were brought to resolve this byintroducing a phase of smoothing by polynomial Bezier curves and another of reparam-eterizing and by introducing a control system which takes into account the dierenttemporal needs of the dierent phases. This approach was purged by a great number ofexperiments on simulators. This demonstrated the convenience of disposing of a trajec-tory feasible by a car-like robot which is also dynamically modiable. The smoothingand parameterization algorithms were validated by experiments dierent options wereprovided in function of the constraints of the robot.The proposed system, kinan, is adapted to the functional architecture on the robot Hilareand enables the coexistence of activities with dierent period cycles. Even though thislast system is still in course of integration3, the results obtained through experimentson the dierent parts of the system conrm the necessity of success of the proposedstructure.The presented approaches and their implementations as well as experiments in which theywere tested demonstrate the non-existence of a unique solution satisfying all the constraints ofthe problem. Just as planning, execution depends narrowly on the context of the application,the environment and its evolution and the robot and its kinematic constraints. In fact theapproaches developed here dier by the nature of the interaction between planning andexecution and more importantly by their domain of applicability. The advantages apportedby one approach in a given context may be considered to be inconveniences in another. Forexample, the sensor-based displacement is not to be considered in vast and non-structuredenvironments. (Except perhaps servoing a target.) Likewise, the elastic band cannot bewisely applicable in a charged road network.This, brings us to the conclusion that the choice of the method to use, must depend on thenature of the task desired and all the constraints relative to the application context. This isthe reason why the decision taking level in the control architecture of an autonomous mobilerobot should dispose of a set of execution technics and control compatible with its needs.3Note: This integration should be accomplished before the defense.134

Annexes

135

Appendix A Hilare2 architectureWe will introduce here the control architecture of the Hilare2 robot on which we have imple-mented all the algorithms and methods developed in this work.A.1 Control architectureConcepts and tools were developed in the context of a generic control architecture for au-tonomous mobile robots. Such an architecture must satisfy several properties and particu-larly provide decision and reactivity capacity to robotic systems. In answer to these con-straints, the LAAS has developed a generic architecture structured in two hierarchical levels([Giralt 93, Chatila 93, Alami 93b]): Decision level is organized into sub-levels including task planning capacities and highlevel execution control. Functional level integrates a set of operational functions: sensor and actionner control,servoing, monitoring, and many calculations needed by the decision level; and provideactions necessary to the upper level.The functional level is a library of real-time services and constitutes and interface betweenthe decision level and the physical robot. (Between decision and action.) tasks are translatedvia it to actions and through it the robot keeps a permanent interaction with the environment(e.g. servoing and re exes).The diculty to integrate the functions which compose the functional level is due tothe diversity of both the employed data and the temporal constraints as well as to thealgorithms complexity. These dierent aspects lead to the structuring of the level intomodules([Chatila 90, Fleury 94]).A.2 Module notionThe functional level is thus a web of dynamically interconnected modules. A module is ansoftware entity integrating services (the dierent treatment functions).136

A.2. MODULE NOTION 137A.2.1 Generic module structureIn order to unify the behavior and the interaction modes between the dierent modules, ageneric module structure has been dened [Fleury 96]. This generic module is composed intotwo parts (gure A.1): A control section composed of an asynchronous control task which: receive and treat requests; arbiter con icts; express the global state of the module. An execution section composed of execution tasks which accomplish the requests: Thisis where the dierent algorithms developed in this memory are integrated. Treatment functions

Control

Exection

Asynchronous events

...

This is an exporteddata structure which is written by the owner of this posterand which is readableby all the others components of the structure... but withdifficulty for you !

This is an exporteddata structure which is written by the owner of this posterand which is readableby all the others components of the structure... but withdifficulty for you !

This is an exporteddata structure which is written by the owner of this posterand which is readableby all the others components of the structure... but withdifficulty for you !

Control poster

Execution poster

Execution task

ControlTask

Execution task

Control SDI

Functional SDI

Figure A.1: Module structure.A.2.2 CommunicationWe access the dierent services oered by the modules via requests. These are asynchronousevents associated with necessary parameters which enable the starting, interruption andalternating treatments. A service ends by the emission of a reply to the client. A reply isitself and asynchronous event which mark the end of the treatment. This reply is associatedwith a execution state and eventually a numerical result. This communication mechanism oftype client/server is represented in gure A.2 on the next page.Note that through this mechanism a module may send an intermediate reply to a corre-sponding request. Nevertheless, the end of the treatment and the resulting state are sent viathe nal reply. For example, the sensor-based actions controller developed and presented inchapter 4 use this mechanism to manage and inspect the execution.On the other hand, during execution, the treatments may need to access or produce data(e.g. current position, ultrasonic measures, : : : ). This uses another communication protocolwhich consists of exporting these data to a structured common memory zones called posters.This is how the module Avoid access the current robot position by a simple memory read ofa poster updated by the locomotion module Loco.137

A.2. MODULE NOTION 138service

CLIENT

SERVER

Module

Module orExecutive orOperator

request (parameters)

reply (results, state)Figure A.2: Client/server relation.A.2.3 Module generatorThe set of treatments to integrate in the functional level are encapsulated by a module genericcanvas.At this stage, the transcription of the module formal description into procedures man-aging internal and external events, communication and internal state switching must not bedebugged for the creation of a new module.In this context, the formal description dening completely the module behavior addedto the desire of a software and module behavior standards was what motivated the basicdenition of a generic module skeleton [Perebaskine 92].The next logical step was the denition of a description language enabling the constructionof an automatic module generator GenoM [Fleury 96].A.2.4 Module exampleWe develop in this section the module Avoid which is the implementation of the systemRtPtCam in the functional architecture of Hilare2.avoidGotoavoidTrackavoidGardavoidTurnavoidGetout avoidSetDistance

locoTracklocoTrackStopRequests form clientsRequest to serversRead data (posters of servers)Write data (reference, segments)avoidRefPosteravoidSegmentsPosterSegmentsReferencerobotPositionPostertrackedRefPoster PosterAvoidRobot positionReferenceMesures Reelleset virtuellesusEchosPosterusVirEchosPoster

avoidSetAvoidanceModeavoidStopavoidSetMemLengthavoidOnOffFigure A.3: The module Avoid.138

A.2. MODULE NOTION 139Module entryThe module Avoid is initialized as a client to the following modules: Loco: The locomotion has the charge of servoing the robot, estimating its positionvia odometry and manage other locomotion related services. US: The module of ultrasonic measures acquisition manage a belt of 32 sonarsdistributed on the periphery of the robot. This module programs the sensorsexcitation rhythm and calculate many representations of the ultrasonic measureswhich are updated in a poster called usEchos. US VIR: The module of virtual ultrasonic measures produced from a world modeland the current position of the robot in the same formats as the module US. Thesedata are stocked in the poster usVirEchos. Several control requests are availableto set the calculus mode (polygonal, bitmap), the sensors model (detection cone,range, : : : ) and other parameters (noise, specularity, : : : ).The module Avoid calculates a reference which it exports to a poster avoidRef, trackedby the module Loco through the request locoTrack.Module services Avoidmodule has lter behavior. It modies the input reference into anotherreference of the same type but guaranteeing non-collision. If avoidance is deactivatedvia the request avoidOnOff, the module output will be simply its input.The entry reference may be a xed position transmitted by the request avoidGoto ora evolutionary reference updated in the poster trackedRef specied by the requestavoidTrack. Both modes represent the systems RtCam and RtPtCam developed in chap-ters 1 and 2.The module oer other elementary actions such as: sensor-based rotation (avoidTurn),sensor-based guard function (avoidGard) and leave obstacles function avoidGetOut).Also, several control requests are available to condition the behavior of the module. Forexample, the request avoidSetMemLength xes the minimum length of the trajectoryto buerize before execution in mode avoidTrack).LocousEchos avoidRef locoPositionpiloReflocoTrackavoidTrackpiloAvoidOn

US VIRusVirEchos PiloAvoidUS RequestRead posterWrite posterPosterFigure A.4: Control and data ux during the execution of a trajectory while avoiding obstacles.gure A.4 shows the cooperation between the modules US, US VIR, Loco, Pilo andAvoid when Pilo is required to execute a trajectory with obstacle avoidance. The module139

A.3. FUNCTIONAL ARCHITECTURE OF HILARE2 140Pilo produces references which are ltered by the module Avoid according to the proximitydata from the modules US et US VIR. They are then transmitted to the module Loco.A.3 Functional architecture of Hilare2Loca2d Locext

Dock

US_VIR

Laser

Avoid

Pilo SBC Band

PWU

MP

Loco

US

Figure A.5: Most of the modules constituting the functional architecture of the robot Hilare2.We present on gure A.5 the majority of modules integrated in the functional architecture ofthe robot Hilare2. The arrows show the reactions client/server between modules.These modules are: Loco: Position estimation and robot servoing [Fleury 96]. Pilo: Parameterization and execution of trajectories [Fleury 96]. Loca2d: 2D Modeling and landmark localization [Bulata 96]. Locext: Absolute localization by external cameras [Fleury 96] Laser: 3D range laser image acquisition [Camargo 92]. US: Ultrasonic sensing [Camargo 91]. US VIR: Virtual ultrasonic sensing (x 2.3.1 on page 37). Avoid: Real-time obstacle avoidance (chapters 1 and 2). SBC: Sensor-based command (chapter 3). Band: Elastic band (chapter 5). PWU: Planning with uncertainty controller (chapter 4). Dock: Robot docking and motion under extremal constraints. MP: Motion planner based on the method proposed in [Laumond 94] implemented byT. Simeon, L. Aguirar and R. Alami at LAAS-CNRS.This set of modules is embarked aboard the robot Hilare2 and run under the real-time140

A.3. FUNCTIONAL ARCHITECTURE OF HILARE2 141operating system VxWorks. The modules mark with a thick frame are those we developedand implemented. Some are treated in this manuscript.

141

Appendix B PerceptionWe present in this annex the proximity sensors with which the robot Hilare2 is equipped andwhich were used all along the experimentation.B.1 Ultrasonic sonarsHilare2 is equipped with a belt of 32 sonars distributed on its periphery. Figure B.1 shows thedisposition of these sonars.1 227 28 29 30 31 32

1112131415161718

20

21

22

23

24

25 4

5

6

7

8

9

3

1019

26

motor wheels

Odometric wheels

Free wheels

SonarsFigure B.1: Hilare2 ultrasonic sonars.Each sensor is composed of two piezo-electric elements of type MA40B61): One emits and1Murata LTD Sensor/Ultrasonic Sensors, 1990. 142

B.2. SEGMENTATION 143the other receives ultrasonic waves. These sensors can be translated and rotated on the belt.20

0

10

30

Attenuation (dB)

90 90

60

3030

60Figure B.2: Directivity diagram of an ultrasonic sonar.Figure B.2 shows the directivity of this type of sensors. The point lled zones are denedby the original properties of the sensors. These zones are eliminated by a mechanical structureassociated with the receiver which makes it more directable (reception cone eect) [Bauzil 91].Sensing moduleThe module US in the functional architecture of Hilare2 governs the programming and acqui-sition of the ultrasonic sonars. The acquisition is realized by a permanent activity updatingevery 50ms the acquisition measures (under several forms) in the poster usEchos as well asthe position of the robot at the time of the acquisition. The acquisition parameters of eachsensor are dynamically programmed by control requests provided by the module (e.g. numberof impulses, inhibition window width, activation/deactivation, : : : ).We distinguish two acquisition modes which can be activated on selected sensors: Select: Selective acquisition mode. One sensor is activated at a time. This modeis interesting when associating a monitoring task for one sensor (e.g. wall extremitydetection) and be able by ne algorithms to detect multiple re ections This is possiblebecause the other sonars emit nothing but are synchronized (by a top timer) to be ina state of reception. Simult: Simultaneous acquisition mode where all the sonars are excited at the sametime. This mode has the particularity of increasing the emission energy and is used todetect objects otherwise dicult to detect in selective mode (e.g. chair legs, corners,: : : ).The module may also realize a sequencing of the two types of acquisition by scanningsuccessively all the sensors of a preselected set in selective mode.In order to generalize the algorithms using ultrasonic sonars, the module provides thesensor positions by control requests2 and all the current acquisition parameters.B.2 SegmentationUltrasonic measures are approximated by segments using the least square method in thfollowing specic algorithm.2These are the physical positions on the belt which necessitates an update exclusive to the module.143

B.2. SEGMENTATION 144© Oper

circular object

Plane

Figure B.3: Segmentation of ultrasonic measures.Unlike laser point, ultrasonic points are dicult to approximate. This is due to theirfewness and their sensibility to continuity tests. We were thus brought to the use of thefollowing algorithm. Consider a nite number of segments corresponding to the dierentsides of the robot (here 4 segments). A set of measures are selected with a gradient verifyingan upper limit. Some sensors up to a certain percentage can be without measures and aminimum number of accepted measures is imposed. A segment approximating the resultingmeasures in the least square sense is calculated (gure B.3).This algorithm produces approximative information on the orientation of an obstacleother than plane obstacles.Now, suppose that the detected object is a wall. We can correct the orientation error ofthe segment due to rebound by the following algorithm.dc1 c2 l2Walll1 Figure B.4: Rebound eect.This phenomenon is produced when: d l1tg2where d is the distance between the sensors c1 and c2 (gure B.4). In stead of detecting t2,we detect t02: 144

B.3. LASER RANGE FINDER 145t02 = 1v l11 + 1cos 2 :where v is the sound speed. This implies an erroneous angle 0. But:tg 0 = v2d t02 t1 ;thus: tg 0 = v2d l1v ( 1cos 2 + 1) 2l1v tg 0 = l12d 1cos 2 1Then: cos 2 = 1 + 2dtg 0l1 1 :and the error on ]theta after development is given in function of time by:tg2 = 1 t1t02After the correction of the orientation, a new segment is calculated whose orientation id and whose distance is calculated using the nearest measure. This insures a preservativemeasure.B.3 Laser range nder3D laser range nder is embarked on a platform on the advanced part of the robot Hilare2.The system contains:Laser

Platine

MiroirSite

Dr. RIEGEL

AUSTRIA

Figure B.5: Laser range nder of Hilare2.145

B.3. LASER RANGE FINDER 146 azimuthal platform supporting the laser range nder; a servo-motor block on the laser to orient the mirror and span the site (gure B.5 onthe page before).Two modules (Azi and Site) govern the command of the two axes and present severalmonitoring and servoing protocols. Nevertheless the handling of the 2D and 3D and otheracquisitions is assumed by the module Laser [Bauzil 91, Camargo 92].Vertex detectionWhile discussing the action Wall Follow, we have presented in paragraph ( 3.3.3 on page 65)the problem of dynamically localizing a vertex during the robot displacement along the wall.Concerning this problem, we have implemented three methods to accomplish this detection.Note that the sensor-based actions are encapsulated in the module SBC of the functionalarchitecture of Hilare2.Sensing on the y The module Laser has a 2D section permanent acquisition mode whoseparameters are dened in the client module. The module updates a specic poster whichcontains the measures and the position of the robot during that last acquisition3.Posters have the advantage of being dated which permits the realization of segmentationonly after each new acquisition. Since the robot position during acquisition is available, eachservoing period the extracted segment are recalculated in the local frame of the robot and anew segmentation is only realized each acquisition period4.The segments extracted at each acquisition period are matched together to replace even-tual collinear segments by only one (x 2.3.2 on page 40). A vertex is localized by either thepoint of intersection of two segments with a sucient angular dierence or by the extremityof one segment in which case the vertex in always convex5. In this case the angular variationof the vertex could not be matched (the second segment being invisible). We could, never-theless, verify its type and its belonging to the assigned region dened by the distance L2(x 3.3.3 on page 65).The robot attraction being a function of the abscissa of the vertex, an important numberof experiments conrmed that the use of this method either imposes a small execution speedor a a greater acquisition sector and thus a slow acquisition.A new acquisition was thus implemented6 to be treated in the next paragraph.servoied acquisition This mode consists of controlling the axis of the laser range nder in agiven direction while sending laser shots. The azimuthal angle is updated in a poster denedby the client module. The shot measure an d the current azimuthal angle are written in aposter belonging to the module Laser.Here, the vertex detection idea is to use the segments orientation obtained by the ultra-sonic sonars and determine in the case of a convex vertex, the distance and the extremityby laser shots normal to the segments. In the case of a concave vertex, the distance to thefrontal segment give the distance to the vertex. Thus, the approximative position of thevertex is determined by the segment(s) issued from ultrasonic measures and updated rapidly.3Here, we consider that all the measures are acquired at the same position though the displacement of therobot during acquisition is taken into consideration.4Segmentation is rather fast for the acquisition sector is relatively small.5Figure 4.5 on page 75 show the labeling convention of vertexes.6The module Laser is realized by M. Herrb. 146

B.3. LASER RANGE FINDER 147This position attracts the robot, while an exact detection of the vertex is conrmed by a dis-continuity of the last shots in the convex case and by a more precise measure in the concavecase.Note that the stop position of the robot relative to a concave vertex is dened by thedistance to the frontal segment before the robot.Selective ultrasonic measures In fact, the two precedent modes are eectively used to detectthe vertex to terminate the action. Nevertheless, for intermediate vertexes, we have remarkedthat detection by ultrasonic sonars is sucient to realize the wall switch because the actiondoes not end at the vertex and the robot can consequently correct the distance and orientationto the following wall.The realization of this detection is done by a monitoring program, in selective mode, onthe appropriate sensors. In this case module US alternate every period between betweenthe two modes of acquisition. Detection is activated by a nal request from the module USsignaling the activation of monitoring. This method necessitates too acute or open vertexes.Thus a minimum angular variation is required between successive walls. Otherwise both wallare considered one.For example, in the case of SBCM system, the controller imposes on the planner parametersincluding the minimum angular variation and the planner respects this while generating itsplan (x 4.0.2 on page 77).

147

Appendix C StabilityC.1 Control point stabilityWe present the same denitions as given in paragraph 1.7.4 on page 23 and we rewrite thesystem equations and the command law that we have proposed:_X = vR_ = ! (C.1)The control point motion equations are:q = X+ dcR_q = _X dc! R (C.2)with R = sin cos .The command law is: v = vFTR! = !FT R (C.3)with v ; ! 2 R+. F is the resultant force to apply on the control point q. The repulsiveforce being local, we prove the stability in absence of obstacles. Consequently, the force Fbecomes: F = kg(q qg)where kg 2 R+.We prove the system stability C.1 for the control law C.3 using the next Lyapunov function:L = 12(q qg)T (q qg)148

C.2. STABILITY UNDER A CONSTANT FORCE 149The strict stability condition is thus:_L = _qT (q qg) < 0Which gives: ( _XT dc! RT ) (q qg) < 0(vRT dc! RT ) (q qg) < 0Replacing v and ! by the control law C.3 and writing _L in the form:_L = Z1 + Z2 < 0;where Z1 = vFTRRT (q qg)Z2 = dc!FT R RT (q qg)We develop then the terms Z1 and Z2 writing the force expression:Z1 = vkg(q qg)TRRT (q qg);Z2 = dc!kg(q qg)T R RT (q qg):The sum of these expressions is then:Z1 + Z2 = (q qg)T vkgRRT + dc!kg R RT (q qg):This expression is of the form xTMx where x 2 R2 and M is a 2 2 square matrix.This matrix being denite positive | its eigenvalues are vkg and dc!kg | the sum Z1+Z2and thus the Lyapunov function are strictly negative. You may remark that this is only truewhen dc is strictly positive. If it is null, only the weak Lyapunov property is veried and noasymptotic convergence is guaranteed.C.2 Stability under a constant forceA constant force applied on a particle generates a constant acceleration and thus amonotonousand increasing speed. We will develop here a solution in order to limit this speed.Let F be a constant force, x be the position of a particle having a mass m. Its motionequation is: F = mxIntegrating this equation gives the speed prole: 149

C.3. FORCES TO DISPLACEMENT 150_x = Fmt + v0where v0 is the initial speed of the particle. Note that the term _x increase linearly with time.To bound this speed we add a dissipative term to the force proportional to the term _x andin the opposite direction. Solving this new dierential system gives:_x = Fkv + Fkv + v0 ekvm tx = Fm v0kvm ekvm twhere kv represents the dissipation gain. The bounds of the speed and the acceleration arethen: limt!1 _x = Fkvlimt!1 x = 0We can bound the speed and control the time constant kvm by the two parameters kv and m.C.3 Forces to displacementWe present in this paragraph an algorithm to transform a force into displacement when therobot in only controlled in position.Suppose that the robot position at instant tn1 is given by (xn1, yn1, n1), withFx; Fy; being the components of the resultant force. Fx is the longitudinal component, Fythe traversal component and the torque exerted on the middle of the motor wheels axis.We can thus calculate the elementary displacement by:L = LLLT = KL 00 KTdc FxFy K 0 (C.4)where (KL; KT ; K) are the gain factors and dc is the distance between the control point andthe center of the robot. The velocity is thus: vw = Lvmax 00 Awmax L (C.5)where ( L; A) are the maximum linear and angular accelerationsIf UP = [vp; wp]T is the command vector at time tn1, we could limit the acceleration by:v = vp +min(jdvdt j; L) Sign(dv)w = wp + min(jdwdt j; A) Sign(dw) (C.6)150

C.3. FORCES TO DISPLACEMENT 151and the speed by: v = min(v; vmax)w = min(w;wmax) (C.7)The command vector becomes: U = vw (C.8)and the wheels speed dierence is: v = E:w2 (C.9)where E is on the wheels axis. The velocity applied to the wheels is thus: = !r!l = v+vRrvvRl ! (C.10)where (Rr; Rl) are the radii of the right and left wheels respectively. The wheels displacementis thus: Dd = !rdtDg = !ldt (C.11)The curvilinear displacement and the orientation dierence become:S = Dd+Dg2 = DdDgE (C.12)Finally, the new position of the robot is:xn = xn1 +S cos(n1 + 2 )yn = yn1 + S sin(n1 + 2 )n = n1 + (C.13)151

References[Alami 93a] R. Alami, R. Chatila & B. Espiau. Designing an intellingent control architec-ture for autonomous robots. In International Conference on Advance Robotics.ICAR'93, Tokyo (Japon), November 1993.[Alami 93b] R. Alami, R. Chatila & B. Espiau. Designing an intellingent control architec-ture for autonomous robots. In International Conference on Advanced Robotics.ICAR'93, Tokyo (Japon), Novomber 1993.[Alami 94] R. Alami & T. Simeon. Planning robust motion strategies for a mobile robot.In IEEE International Conference on Robotics and Automation, San Diego Cal-ifornia, (USA), pages 13121318, 1994.[Alami 95] R. Alami, L. Aguilar, H. Bullata, S. Fleury, M. Herrb, F. Ingrand, M. Khatib& F. Robert. A General Framework for Multi-Robot Cooperation and its Im-plementation on a Set of Three Hilare Robots. In International Symposium onExperimental Robotics, Stanford, California, (USA), June 1995.[Ando 95] Y. Ando & S. Yuta. Following a Wall by an Autonomous Mobile Robot with aSonar-Ring. In IEEE, International Conference on Robotics and Automation,Nagoya, Aichi (Japan), pages 25992606, 1995.[Bauzil 91] G. Bauzil, R. Ferraz De Camargo, C. Lemaire & G. Vialaret. Robot Mobile Hi-lare II: description du materiel. Technical report 91234, LAAS-CNRS, Toulouse,1991.[Bennamoun 91]M. Bennamoun, A. A. Masoud, A.Ramsay & M. M. Bayoumi. Avoidanceof Unknown Obstacles Using Proximity Fields. In IEEE International WorkshopOn Intelligent Robots and Systems, Osaka, (Japan), 1991.[Borenstein 91] J. Borenstein & Y. Koren. The VectorField Histogram - Fast ObstaclesAvoidance for Mobile Robots. In IEEE Transactions on Robotics and Automa-tion, 1991.[Bouilly 95a] B. Bouilly & T. Simeon. A sensor-based motion planner for mobile robotnavigation with uncertainty. In L. Dorst, M.V. Lambalgen & F. Voorbraak,editeurs, Reasoning with uncertainty in robotics, pages 235247. Springer, 1995.[Bouilly 95b] B. Bouilly, T. Simeon & R. Alami. A numerical technique for planning motionstrategies of a mobile robot in presence of uncertainty. In IEEE, InternationalConference on Robotics and Automation, Nagoya, Aichi (Japan), pages 13271332, 1995. 152

References 153[Bulata 96] H. Bulata & M. Devy. Modelisation Orientee Objet pour la Localisation d'unRobot Mobile en Milieu Structure. In 10eme Congres Reconnaissance des Formeset Intelligence Articielle (RFIA'96), Rennes (France), jan 1996.[Camargo 91] R. Ferraz De Camargo. Architecture Materielle et Logicielle pour le Con-trole de l'Execution d'un Robot Mobile Autonome. PhD thesis, Universite PaulSabatier, (LAAS-CNRS), Toulouse (France), 1991.[Camargo 92] R. Ferraz De Camargo, S.Fleury & M.Herrb. HILARE 2. Guide d'utilisation.Technical report 92452, LAAS-CNRS, 1992.[Chatila 90] Raja Chatila & Rogerio Ferraz De Camargo. Open Architecture Design andInter-task/Intermodule Communication for an Autonomous Mobile Robot. InIEEE International Workshop on Intelligent Robots and Systems (IROS '90),Tsuchiura (Japan), July 1990.[Chatila 93] R. Chatila. Representation + Reason + Reaction ! Robot Intelligence. In 6thInternational Symposium on Robotics Research. ISSR, Hidden Valley (Pennsyl-vania, USA), October 1993.[Chochon 83] H. Chochon & B. Leconte. Etude d'un module de locomotion pour un robotmobile. Rapport de n d'etude ENSAE, Laboratoire d'Automatique et d'Analysedes Systemes (C.N.R.S.), Toulouse (France), June 1983.[Collin 95] I. Collin. Planication de taches-robots pour robots mobiles en environnementstructure. PhD thesis, Universite de Technologie de Compiegne, Compiegne(France), 1995.[Coste-Maniere 92] E. Coste-Maniere, B. Espiau & D. Simon. Reactive object in a tasklevel open controller. In IEEE International Conference on Robotics and Au-tomation, Nice, (FRANCE), pages 27322737, May 1992.[d'Andrea Novel 92] B. d'Andrea Novel, G. Bastin & G. Campion. Dynamic FeedbackLinearization on Nonholonomic Wheeled Mobile Robots. In IEEE InternationalConference on Robotics and Automation, Nice, (FRANCE), pages 25272532,May 1992.[d'Andrea-Novel 95] B. d'Andrea-Novel, G. Campion & G. Bastin. Control of Nonholo-nomic Wheeled Mobile Robots by State Feedback Linearization. The InternationalJournal of Robotics Research, 1995.[De la Rosa 96] F. De la Rosa, C. Laugier & J. Najera. Robust path planning in the plane.IEEE Transactions on Robotics and Automation, vol. 12, no. 2, pages 347352,1996.[De Luca 94] A. De Luca & G. Oriolo. Local incremental planning for nonholonomic mobilerobots. In IEEE International Conference on Robotics and Automation, SanDiego California, (USA), pages 104110, 1994.[Delingette 91] H. Delingette, M. Herbert & K. Ikeuchi. Trajectory generation with curva-ture constraint based on energy minimization. In IEEE International WorkshopOn Intelligent Robots and Systems, Osaka, (Japan), 1991.153

References 154[Dickmanns 89] E.D. Dickmanns & T. Christians. Relative 3D-State Estimation for Au-tonomous Visual Guidance of Road Vehicles. In Intelligent Autonomous Systems(IAS'2), Amsterdam (Netherlands), pages 683693, 1989.[Dickmanns 95] E.D. Dickmanns. Performance Improvements for Autonomous Road Ve-hicles. In Intelligent Autonomous Systems (IAS'4), Karlsruhe (Germany), pages214, 1995.[Espiau 92] B. Espiau, F. Chaumette & P. Rives. A new approach to visual servoing inrobotics. IEEE Transactions On Robotics and Automation, vol. 8, no. 3, pages313326, June 1992.[Fiorot 89] J. Fiorot & P. Jeannin. Courbes et surfaces rationelles. Masson, Paris, 1989.[Fleury 94] S. Fleury, M. Herrb & R. Chatila. Design of a Modular Architecture for Au-tonomous Robot. In IEEE International Conference on Robotics and Automa-tion, San Diego California, (USA), 1994.[Fleury 95] S. Fleury, P. Soueres, J.-P. Laumond & R. Chatila. Primitives for SmoothingMobile Robot Trajectorie. IEEE Transactions on Robotics and Automation,vol. 11, no. 3, pages p.441448, June 1995.[Fleury 96] S. Fleury. Architecture de controle distribuee pour robots autonomes: principes,conception et applications. PhD thesis, Universite Paul Sabatier, Fevrier 1996.[Gamkerlidze 80] R. V. Gamkerlidze. Analysis ii - convex analysis and approximationtheory. Springer-Verlag, 1980.[Giralt 93] G. Giralt, R. Chatila & R. Alami. Remote Intervention, Robot Autonomy,And Teleprogramming: Generic Concepts And Real-World Application Cases.In IEEE International Workshop on Intelligent Robots and Systems (IROS '93),Yokohama, (Japan), pages 314320, July 1993.[Green 94] D.N. Green, J.Z. Sasiadek & G.S. Vukovich. Path Traking, Obstacle Avoidanceand Position Estimation by an Autonomous, Wheeled Planetary Rover. In IEEEInternational Conference on Robotics and Automation, San Diego California,(USA), pages p. 13001305, 1994.[J. Koseck 96] J. Koseck. Visually guided navigation. In International Symposium onINTELLIGENT ROBOTIC SYSTEMS-SIRS, pages 301308, July 1996.[Kanayama 89] Y. Kanayama & B. Hartman. Smooth local planning for autonomous ve-hicles. In Proc. IEEE International Conference on Robotics and Automation,Scottsdale, (USA)., 1989.[Khoumsi 88] A. Khoumsi. Pilotage, asservissement sensoriel et localisation d'un robotmobile autonome. PhD thesis, Universite Paul Sabatier, Toulouse, France, 1988.[Krogh 84] B. H. Krogh. A Generalised Potential Field Approch to Obstacle AvoidanceControl. In Robotics Research: The Next Five Years and Beyond, SME Confer-ence Proceedings, Bethlehem, PA, 1984.[Krogh 86] B.H. Krogh & C.E. Thorpe. Integrated path planning and dynamic steeringcontrol for autonomous vehicles. In Proc. IEEE International Conference onRobotics and Automation, San Francisco (USA)., 1986.154

References 155[Laumond 93] J.-P. Laumond & P. Soueres. Metric induced by the shortest paths for acar-like mobile robot. In IEEE International Workshop On Intelligent Robotsand Systems, Yokohoma, (Japan), 1993.[Laumond 94] Jean-Paul Laumond, Paul E. Jacobs, Michel Tax & Richard M. Murray.A Motion Planner for Nonholonomic Mobile Robots. IEEE Transactions OnRobotics and Automation, vol. 10, no. 5, pages 577590, Oct. 1994.[M. Khatib 95]M. Khatib & R. Chatila. An extended potentiel eld approach for mobilerobot sensor-based motions. Intelligent Autonomous Systems (IAS'4), Karlsruhe(Germany), pages 490496, 1995.[M. Khatib 96]M. Khatib & H. Jaouni. Kinematics Integrated in Non-holonomic Au-tonomous Navigation. Technical report 96346, LAAS-CNRS, September 1996.[Moravec 85] H. P. Moravec & A. Elfes. High Resolution Maps from Wide Angle Sonar. InIEEE International Conference on Robotics and Automation, San Louis, (USA),1985.[Nelson 89]W. Nelson. Continuous-Curvatures Paths for Autonomous Vehicules. InProc. IEEE International Conference on Robotics and Automation, Scottsdale,(USA)., pages p. 12601264, 1989.[O. Khatib 80] O. Khatib. Commande dynamique dans l'espace operationnel des robotsmanipulateurs en presence d'obstacles. PhD thesis, Ecole Nationale Superieurede l'Aeronautique et de l'Espace, Toulouse, France, 1980.[O. Khatib 86] O. Khatib. Real-time obstacle avoidance for manipulators and mobilerobots. The International Journal of Robotics Research, vol. 5, no. 1, pages9098, 1986.[Perebaskine 92] V. Perebaskine. Une architecture modulaire pour le controle d'unrobot mobile autonome. PhD thesis, Universite Paul Sabatier, (LAAS-CNRS),Toulouse (France), 1992.[Pissard-Gibollet 93] R. Pissard-Gibollet. Conception et Commande par AsservissementVisuel d'un Robot Mobile. PhD thesis, L'Ecole des Mines de Paris, Dec. 1993.[Pissard-Gibollet 95] R. Pissard-Gibollet & P. Rives. Applying Visual Servoing Tech-niques to Control a Mobile Hand-Eye System. In IEEE, International Conferenceon Robotics and Automation, Nagoya, Aichi (Japan), May 1995.[Quinlan 92] S. Quinlan & O. Khatib. Towards Real-Time Execution of Motion Tasks. InR. Chatila & G. Hirzinger, editeurs, Experimental Robotics 2. Springer-Verlag,1992.[Quinlan 93] S. Quinlan & O. Khatib. Elastic bands: connecting path planning and control.In IEEE International Conference on Robotics and Automation, Atlanta, (USA),1993.[Quinlan 95] S. Quinlan. Real-Time Collision-Free Path Modication. PhD thesis, Stan-ford University, CS Departement, January 1995.[Reeds 90] J.A. Reeds & L.A. Shepp. Optimal paths for a car that goes both forwards andbackwards. Pacic Journal Mathematics, vol. 145, no. 2, pages 367393, 1990.155

References 156[Rives 93] P. Rives, R. Pissard-Gibollet & K. Kapellos. Development of a Reactive MobileRobot Using Real Time Vision. In International Symposium on ExperimentalRobotics, Kyoto (Japan)., Oct. 1993.[Samson 90a] C. Samson & K. Ait-Abderrahim. Mobile-Robot Control. Part 1 : Feed-back Control of a Nonholonomic Wheeled Cart in Cartesian Space. Rapport derecherche 1288, Institut National de Recherche en Informatique et en Automa-tique (I.N.R.I.A.), Sophia Antipolis (France), October 1990.[Samson 90b] C. Samson, B. Espiau & M. Le Borgne. Robot control: the task functionapproach. Oxford University Press, 1990.[Samson 91] C. Samson & K. Ait-Abderrahim. Feedback stabilization of a nonholonomicwheeled mobile robot. In IEEE International Workshop On Intelligent Robotsand Systems, Osaka, (Japan), 1991.[Samson 93] C. Samson. Time varying feedback stabilization of a car-like wheeled mobilerobot. The International Journal of Robotics Research, vol. 12, no. 1, pages5564, Feb. 1993.[Segovia 91] A. Segovia, M. Rombaut, A. Preciado & D. Meizel. Comparative study of thedierent methods of path generation for a mobile robot in a free environment. In'91 International Conference on Advanced Robotics (ICAR),Pisa (Italy), pages16671670, June 1991.[Soueres 94] P. Soueres, J.-Y. Fourquet & J.-P. Laumond. Set of reachable positions for acar. IEEE Transactions on Automatic Control, vol. 39, no. 8, August 1994.[Tilove 89] R.B. Tilove. Local Obstacle Avoidance for Mobile Robots based on the methodof Articial Potentials. In Research Publication GMR - 6650, 1989.[van Turennout 92] P. van Turennout, G. Honderd & L.J. van Schelven. Wall-followingControl of Mobile Robot. In IEEE International Conference on Robotics andAutomation, Nice, (FRANCE), pages 280285, May 1992.[Vendittelli 96]M. Vendittelli & J.-P. Laumond. Visible position for car-like robot amidstobstacles. In Workshop on Algorithmic Foundations of Robotics, WAFR'96, July1996.[Yagi 91] Y. Yagi, S. Kawato & S. Tsuji. Collision Avoidance Using OmnidirectionalImage Sensor. In IEEE International Conference on Robotics and Automation,Sacramento, (USA), 1991.[Zhu 89] W. Choi D. J. Zhu & J. C. Latombe. Contingency-tolerant motion planning andcontrol. In Proceedings of IROS 89, Tsukuba, Japan, 1989.156