Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/51 Formations and Obstacle Avoidance in Mobile Robot Control Petter Ögren Topics from a Doctoral Thesis, 2003

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/52 Outline A brief introduction of all four papers Overview of how they relate to each other Details of Paper A Details of Paper B

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/53 All four Papers Obstacle Avoidance Formations Paper B Paper C Paper A Paper D

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/54 Paper A: A Convergent Dynamic Window Approach to Obstacle Avoidance Problem formulation: Drive a robot from A to B through a partially unknown environment without collisions. A B

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/55 Paper A: A Convergent Dynamic Window Approach to Obstacle Avoidance Proposed solution: Merge state-of-the-art heuristics with a provable approach, (using a CLF/MPC framework). ) Optimize pointwise over stabilizing controls

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/56 Problem: How do we move the leader to guide a leader-follower formation through obstacle terrain? Can we use singel vehicle Obstacle Avoidance? Paper B: Obstacle Avoidance in Formation

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/57 Paper B: Obstacle Avoidance in Formation Proposed solution: The concept of Configuration Space Obstacles is extended through an Input to State Stability (ISS) argument. ) A map of the leader positions that guarantee followers enough free space. The leader does single vehicle obstacle avoidance using this map.

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/58 Paper C: A Control Lyapunov Function Approach to Multi Agent Coordination Problem: How to make set-point controlled robots moving along trajectories in a formation ”wait” for eachother?

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/59 Idea: Combine Control Lyapunov Functions (CLF) with the Egerstedt&Hu virtual vehicle approach. Under assumptions this will result in: Bounded formation error (waiting) Approximation of given formation velocity (if no waiting is necessary). Finite completion time (no 1-waiting). Paper C: A Control Lyapunov Function Approach to Multi Agent Coordination

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/510 Paper D: Cooperative Control of Mobile Sensor Networks: Adaptive Gradient Climbing in a Distributed Environment Problem D1: Given a local-spring-damper formation control. How do we translate, rotate and expand the formation? Problem D2: Given a field, i.e. temperature or nutrition density in water. How do we estimate the gradient from noisy distributed measurements? What formation geometries give good estimates?

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/511 Paper D: Cooperative Control of Mobile Sensor Networks: Adaptive Gradient Climbing in a Distributed Environment Proposed solution D1: Introduce virtual leaders in the formation and move these. Let direction of motion be governed by the mission, e.g. gradient climbing. Let the speed of the motion be influenced by error feedback (from paperC).

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/512 Paper D: Cooperative Control of Mobile Sensor Networks: Adaptive Gradient Climbing in a Distributed Environment Proposed solution D2: Estimate the gradient: Use the least Squares estimate of (a,b) in an affine approximation a T z+b ¼ T(z). Apply Kalman filter over time. Formation geometries: Minimize error due to measurement noise and second order terms. In 1-dimension: Estimate True Noisy This is generalized to m vehicles in R n.

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/513 All four Papers Paper A Paper B Paper D Paper C Details!

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/514 Drive a robot from A to B through a partially unknown environment without collisions. A B Differential drive robots can be feedback linearized to this. Paper A: A Convergent Dynamic Window Approach to Obstacle Avoidance

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/515 Background: The Dynamic Unicycle 

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/516 Desirable Properties in Obstacle Avoidance No collisions Convergence to goal position Efficient, large inputs ‘Real time’ ‘Reactive’, (to changes in environment)

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/517 Background: Two main Obstacle Avoidance approaches Reactive/Behavior Based Biologically motivated Fast, local rules. ‘The world is the map’ No proofs. Changing environment not a problem Combine the two? Deliberative/Sense-Plan-Act Trajectory planning/tracking Navigation function (Koditschek ’92). Provable features. Changes are a problem

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/518 Background: The Navigation Function (NF) tool One local/global min at goal. Gradient gives direction to goal. Solves ‘maze’ problems. Obstacles and NF level curves Goal

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/519 Basic Idea Control Lyapunov Function (CLF) DWA, Fox et. al. and Brock et al Model Predictive Control (MPC) MPC/CLF Framework, Primbs ’99 Convergent DWA Exact Navigation, using Art. Pot. Fcn. Koditscheck ’92 ‘Real time’ Efficient, large inputs ‘Reactive’, to changes Convergence proof. No collisions

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/520 Background: Model Predictive Control (MPC) Idea: Given a good model, we can simulate the result of different control choices (over time T) and apply the best. Feedback: repeat simulation every  <T seconds. How is this connected to the Dynamic Window Approach?

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/521 Global Dynamic Window Approach (Brock and Khatib ‘99) VxVx VyVy Dynamic Window Control Options Obstacles V max Current Velocity Velocity Space Robot Cirular arc pseudo-trajectories

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/522 Global Dynamic Window Approach (continued) Check arcs for collision free length. Chose control by optimization of the heuristic utility function: Speeds up to 1m/s indoors with XR 4000 robot (Good!). No proofs. (Counter example!) Idea: See as Model Predictive Control (MPC) Use navigation function as CLF

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/523 Background: Control Lyapunov Function (CLF) Idea: If the energy of a system decreases all the time, it will eventually “stop”. A CLF, V, is an “energy-like” function such that V x

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/524 Exact Robot Navigation using Artificial Potential Functions, (Rimon and Koditscheck ‘92) C 1 Navigation Function NF(p) constructed. NF(p)=NF max at obstacles of Sphere and Star worlds. Control: Features: Lyapunov function: => No collisions. Bounded Control. Convergence Proof Drawbacks Hard to (re)calculate. Inefficient Idea: Use C 0 Control Lyapunov Function.

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/525 Our Navigation Function (NF) One local/global min at goal. Calculate shortest path in discretization. Make continuous surface by careful interpolation using triangles. Provable properties. The discretization

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/526 MPC/CLF framework Primbs general form:Here we write:

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/527 The resulting scheme: Lyapunov Function and Control Lyapunov function candidate: gives the following set of controls, incl. Compare: Acceleration of down hill skier.

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/528 Safety and Discretization The CLF gives stability, what about safety? In MPC, consider controls stop without collision. Plan to first accelerate: then brake: Apply first part and replan. Compare: Being able to stop in visible part of road ) safety

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/529 Evaluated MPC Trajectories

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/530 Simulation Trajectory

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/531 Single Vehicle Conclusions Properties: No collisions (stop safely option) Convergence to goal position (CLF) Efficient (MPC). Reactive (MPC). Real time (?), small discretized control set, formalizing earlier approach. Can this scheme be extended to the multi vehicle case?

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/532 All four Papers Paper A Paper B Paper D Paper C Details!

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/533 Why Multi Agent Robotics? Applications: Search and Rescue missions, lawn moving etc. Carry large/awkward objects Adaptive sensing, e.g. surveillance or ocean sampling Satellite imaging in formation Motivations: Flexibility Robustness Performance Price

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/534 Paper B: Obstacle Avoidance in Formation How do we use singel vehicle Obstacle Avoidance?

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/535 Desirable properties No collisions Convergence to goal position Efficient, large inputs ‘Real time’ ‘Reactive’, to changes & Distributed/Local information

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/536 A Leader-Follower Structure Two Cases: No explicit information exchange ) leader acceleration, u 1, is a disturbance Feedforward of u 1 ) time delays and calibration errors are disturbances Information flow Leader How big deviations will the disturbances cause?

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/537 Background: Input to State Stability (ISS) We will use the ISS to calculate ”Uncertainty Regions”

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/538 ISS ) Uncertainty Region Uncertainty Region

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/539 Formation Leader Obstacles, an extension of Configuration Space Obstacles ”Free” leader pos. ”Occupied” leader pos. How do we calculate a map of ”free” leader positions? Obstacle

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/540 Formation Leader Map Unc. Region and ObstaclesFormation Obstacles Computable by conv2 (matlab). Leader does obstacle avoidance in new map. Followers do formation keeping under disturbance.

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/541 Simulation Trajectories

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/542 Conclusions, paper B Obstacle Avoidance extended to formations by assuming leader- follower structure and ISS. Future directions Rotations Expansions Breaking formation ) ¸ 3 dim NF

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/543 All four Papers Paper A Paper B Paper D Paper C Details!

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/544 End of Presentation.

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/545 Paper C: A Control Lyapunov Function Approach to Multi Agent Coordination P. Ögren, M. Egerstedt * and X. Hu Royal Institute of Technology (KTH), Stockholm and Georgia Institute of Technology * IEEE Transactions on Robotics and Automation, Oct 2002

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/546 Problem and Proposed Solution Problem: How to make set-point controlled robots moving along trajectories in a formation ”wait” for eachother? Idea: Combine Control Lyapunov Functions (CLF) with the Egerstedt&Hu virtual vehicle approach. Under assumptions this will result in: Bounded formation error (waiting) Approx. of given formation velocity (if no waiting is nessesary). Finite completion time (no 1-waiting).

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/547 Quantifying Formation Keeping Will add Lyapunov like assumption satisfied by individual set-point controllers. => Think of as parameterized Lyapunov function. Definition: Formation Function

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/548 Examples of Formation Function Simple linear example ! A CLF for the combined higher dimensional system: Note that a,b, are design parameters. The approach applies to any parameterized formation scheme with lyapunov stability results.

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/549 Main Assumption We can find a class K function  such that the given set-point controllers satisfy: This can be done when -dV/dt is lpd, V is lpd and decrescent. It allows us to prove: Bounded V (error): V(x,s) < V U Bounded completion time. Keeping formation velocity v 0, if V ¿ V U.

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/550 Speed along trajectory: How Do We Update s? Suggestion: s=v 0 t Problems: Bounded ctrl or local ass stability We want: V to be small Slowdown if V is large Speed v 0 if V is small Suggestion: Let s evolve with feedback from V.

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/551 Evolution of s Choosing to be: We can prove: Bounded V (error): V(x,s) < V U Bounded completion time. Keeping formation velocity v 0, if V ¿ V U.

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/552 Proof sketch: Formation error

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/553 Proof sketch: Finite Completion Time Find lower bound on ds/dt

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/554 Example: Formation Three unicycle robots along trajectory. V U =1, v 0 =0.1, then v 0 =0.3 ! 0.27 Stochastic measurement error in top robot at 12m mark.

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/555 Extending Work by Beard et. al. ”Satisficing Control for Multi-Agent Formation Maneuvers”, in proc. CDC ’02 It is shown how to find an explicit parameterization of the stabilizing controllers that fulfills the assumption These controllers are also inverse optimal and have robustness properties to input disturbances Implementation

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/556 All four Papers Paper A Paper B Paper D Paper C Details!

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/557 Formations with a Mission: Stable Coordination of Vehicle Group Maneuvers Mathematical Theory of Networks and Systems (MTNS ‘02) Visit: for related information Edward Fiorelli and Naomi Ehrich Leonard Mechanical and Aerospace Engineering Princeton University, USA Optimization and Systems Theory Royal Institute of Technology, Sweden Petter Ogren Another extension:

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/558 Configuration space of virtual body is for orientation, position and expansion factor: Because of artificial potentials, vehicles in formation will translate, rotate, expand and contract with virtual body. To ensure stability and convergence, prescribe virtual body dynamics so that its speed is driven by a formation error. Define direction of virtual body dynamics to satisfy mission. Partial decoupling: Formation guaranteed independent of mission. Prove convergence of gradient climbing. Approach: Use artificial potentials and virtual body with dynamics.

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/559 What if dV/dt <= 0 ? If we have semidefinite and stability by La Salle’s principle we choose as: By a renewed La Salle argument we can still show: V<=V U, s ! s f and x ! x f. But not: Completion time and v 0.

Centre for Autonomous Systems Petter ÖgrenOptSyst seminar 23/560 Conclusions Moving formations by using Control Lyapunov Functions. Theoretical Properties: V <= V U, error T < T U, time v ¼ v 0 velocity Extension used for translation, rotation and expansion in gradient climbing mission