1. Leng-Feng Lee Dec 3, 2004 Slide 1 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Leng-Feng Lee (llee3@eng.buffalo.edu) Advisor : Dr. Venkat N. Krovi Mechanical and Aerospace Engineering Dept. State University of New York at Buffalo Decentralized Motion Planning within an Artificial Potential Framework (APF) for Cooperative Payload Transport by Multi-Robot Collectives

2. Leng-Feng Lee Dec 3, 2004 Slide 2 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Agenda Motivation & System Modeling Literature Survey & Research Issues Part I Part II Local APF & limitations Global APF-Navigation Function Case Studies-Single robot with APF Dynamic Formulation-Group of Robots Motion Planning-Three Approaches Case Studies-Multi Robots with APF Performance Evaluation of Three Approaches Conclusion & Future Work

3. Leng-Feng Lee Dec 3, 2004 Slide 3 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Motivation Examples of Multi-robot groups: –Tasks are too complex; –Gain in overall performance; –Several simple, small-sized robot are easier, cheaper to built, than a single large powerful robot system; –Overall system can be more robust and reliable. Group Cooperation in Nature: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Schools of Fish Armies of Ants Flocks of Birds How do we incorporate similar cooperation in artificial multi robot group?

4. Leng-Feng Lee Dec 3, 2004 Slide 4 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Motivation Example of Multi robot groups: Cooperative payload transport Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Robots in formation Robots in group

5. Leng-Feng Lee Dec 3, 2004 Slide 5 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Why Motion Planning? –To realize all the functionalities for mobile robots, the fundamental problem is getting a robot to move from one location to another without colliding with obstacles. Motion Planning (MP) for Robot Collectives Introduction Model Distribution Analysis Manipulability Cooperative System Conclusion Definition: –The process of selecting a motion and the associated set of input forces and torques from the set of all possible motions and inputs while ensuring that all constraints are satisfied. MP for Robot Collective - –MP exist for individual robots such as manipulator, wheeled mobile robot (WMR), car-like robot, etc. –We want to examine extension of MP techniques to Robot Collectives

6. Leng-Feng Lee Dec 3, 2004 Slide 6 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Explicit Motion Planning: –Decompose MP problem into 3 tasks: –Path Planning, Trajectory Planning, & Robot Control; –Example: Road Map Method, Cell Decomposition, etc. Motion Planning Algorithm Classification Introduction Model Distribution Analysis Manipulability Cooperative System Conclusion Implicit Motion Planning: –Trajectory and actuators input are not explicitly compute before the motion occur. –Artificial Potential Field (APF) Approach belongs to this category. –Combine Path Planning, Trajectory Planning, and Robot Control in a single framework.

7. Leng-Feng Lee Dec 3, 2004 Slide 7 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Motion Planning (cont’) Introduction Model Distribution Analysis Manipulability Cooperative System Conclusion Artificial Potential Field (APF) Approach: –Obstacles generated a artificial Repulsive potential and goal generate an Attractive potential. –Motion plan generated when attractive potential drives the robot to the goal and repulsive potential repels the robot away from obstacles. –Combine Path Planning, Trajectory Planning, and Robot Control in one framework. Subclass of Implicit Motion Planning Algorithm

8. Leng-Feng Lee Dec 3, 2004 Slide 8 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Research Issues Specific Research Questions: –Which type of potential function is more suitable for MP for multi robot groups? –How can we use the APF framework to help maintain formation? and –How this framework be extended to realize the tight formation requirement for cooperative payload transport? Introduction Model Distribution Analysis Manipulability Cooperative System Conclusion Broad Challenges: –Extending APF approach for Multi-robot collectives. –Ensuring tight formations required for Cooperative Payload Transport application.

9. Leng-Feng Lee Dec 3, 2004 Slide 9 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Research Issues (cont’) Part I: –Study various APF & their limitations; –Determined a suitable APF as our test bed; –Create a GUI to design and visualize the potential field; –Case studies: MP for single robot using APF approach. Introduction Model Distribution Analysis Manipulability Cooperative System Conclusion To answer these research questions: Part II: –E.O.M. for group of robots with formation constraints; –Solved the MP planning problem using three approaches; –Performance evaluation using various case studies.

10. Leng-Feng Lee Dec 3, 2004 Slide 10 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Research Issues (cont’) Introduction Model Distribution Analysis Manipulability Cooperative System Conclusion Hierarchical difficulties in MP: Our results: –Multiple point-mass robots; –Sphere World; –Stationary Obstacles & Target. (Dynamic Model)

11. Leng-Feng Lee Dec 3, 2004 Slide 11 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo System Modeling Individual level system models include: –Point Mass Robot; –Differentially Driven Nonholonomic Wheel Mobile Robot (NH-WMR). Introduction Model Distribution Analysis Manipulability Cooperative System Conclusion

12. Leng-Feng Lee Dec 3, 2004 Slide 12 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo System Modeling (cont’) Introduction Model Distribution Analysis Manipulability Cooperative System Conclusion Group level system model is formed using: –Point Mass Robot; –Differentially Driven Nonholonomic Wheel Mobile Robot.

13. Leng-Feng Lee Dec 3, 2004 Slide 13 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo PART I: Artificial Potential Approach Examine: –Variants of APF & their limitations; –Navigation function ; –Single module formulations; –Simulation studies.

14. Leng-Feng Lee Dec 3, 2004 Slide 14 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF -background Artificial Potential Field Approach –Proposed by Khatib in early 80’s. –FIRAS Function. [Khatib, 1986] Later, various kind of Potential Functions were proposed: –GPF Function. [Krogh, 1984] –Harmonic Potential Function. [Kim, 1991] –Superquadric Potential Function. [Khosla, 1988] –Navigation Function. [Koditschek, 1988] –Ge New Potential. [Ge, 2000] Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion

15. Leng-Feng Lee Dec 3, 2004 Slide 15 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF Approach-Formulation Idea: –Goal generate an attractive potential well; –Obstacle generate repulsive potential hill; –Superimpose these two type of potentials give us the total potential of the workspace. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion denote the total artificial potential field; denote the attractive potential field; and is the repulsive potential field. Where: is the position of the robot.

16. Leng-Feng Lee Dec 3, 2004 Slide 16 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF -Attractive potential Characteristics: –Affect every point on the configuration space; –Minimum at the goal. –The gradient must be continuous. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion

17. Leng-Feng Lee Dec 3, 2004 Slide 17 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF -Attractive potential Example 1: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Where: = Euclidean distance between the robot and the target = Position of the target. = Position of the robot. = Positive scaling factor is commonly used.

18. Leng-Feng Lee Dec 3, 2004 Slide 18 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF -Attractive potential Example 2: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Where: For distance smaller than s, conical well. For distance larger than s, constant attractive force. = Positive scaling factor

19. Leng-Feng Lee Dec 3, 2004 Slide 19 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF -Attractive potential Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Gradient information can be obtained by: Gradient is more uniform in later case.

20. Leng-Feng Lee Dec 3, 2004 Slide 20 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF -Repulsive potential Characteristics: –The potential should have spherical symmetry for large distance; –The potential contours near the surface should follow the surface contour; –The potential of an obstacle should have a limited range of influence; –The potential and the gradient of the potential must be continuous. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion

21. Leng-Feng Lee Dec 3, 2004 Slide 21 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF -Repulsive potential Example 1 - FIRAS Function: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Where: = Positive scaling factor = the shortest Euclidean distance between the robot from the obstacle surface

22. Leng-Feng Lee Dec 3, 2004 Slide 22 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF -Repulsive potential Example 2 - Superquadric Potential Function: –Approach Potential; –Avoidance Potential. –Avoid creation of local minima result from flat surface by creating a symmetry contour around the obstacle. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion

23. Leng-Feng Lee Dec 3, 2004 Slide 23 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF -Repulsive potential Example 3 - Harmonic Potential Function: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion – Superimpose of another harmonic potential is also a harmonic potential. – More complicated shape can be modeled using ‘panel method’. Repulsive Potential Attractive Potential Detail

24. Leng-Feng Lee Dec 3, 2004 Slide 24 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF -Repulsive potential Harmonic Potential Function: –harmonic potential is the solution to the Laplace Equation of the following form: –In 2 dimension space, the solution without considering the angular term is: –Depends on the sign of, we can create ‘source’ (repulsive) or ‘sink’ (attractive) potential. –Uniform flow potential: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion is the Euclidean distance from the robot to source or sink. is the angle the direction flow of the potential with the x-axis Back

25. Leng-Feng Lee Dec 3, 2004 Slide 25 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF -Repulsive potential Example 4 - Ge New Function: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Where: –Modified from FIRAS function to solve the ‘Goal NonReachable for Obstacle Nearby’ - GNRON problem. – Ensures that the total potential will reach its global minimum, if and only if the robot reaches the target where = Minimal Euclidean distance from robot to the target.

26. Leng-Feng Lee Dec 3, 2004 Slide 26 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF -Repulsive potential Potential Function with Velocities Information: –Some potential function include the velocities information of the robots, obstacles and target. –Example: Ge & Cui Potential [Dynamic obstacle & Target]. –Provide a APF for dynamic workspace. –Example: GPF Function. [Dynamic obstacles only]. –Can be used with our formulation for group of robots for motion planning in dynamic workspace. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion

27. Leng-Feng Lee Dec 3, 2004 Slide 27 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF –Total Potential Total Potential of Workspace: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion – Superimpose different repulsive potential from obstacles and different attractive potential from the goal, we get the total potential for the workspace. – At any point of the workspace, the robot will reach the target by following the negative gradient flow of the total potential.

28. Leng-Feng Lee Dec 3, 2004 Slide 28 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF –Total Potential Example: FIRAS Function Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Rectangular Obstacle: 2 unit in height, 1 unit in width. Circular Obstacle:Radius Target : More

29. Leng-Feng Lee Dec 3, 2004 Slide 29 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF –Total Potential Example: Ge New Potential Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Circular Obstacle:Radius Target : More Back

30. Leng-Feng Lee Dec 3, 2004 Slide 30 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF –Total Potential Example: Harmonic Potential Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Circular Obstacle:Radius Target : Back

31. Leng-Feng Lee Dec 3, 2004 Slide 31 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF –Limitations Local Minimum - result from single obstacle Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion 3D View

32. Leng-Feng Lee Dec 3, 2004 Slide 32 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF –Limitations Local Minimum - result from single obstacle Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Back

33. Leng-Feng Lee Dec 3, 2004 Slide 33 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF –Limitations Local Minimum - result from multiple obstacles Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion

34. Leng-Feng Lee Dec 3, 2004 Slide 34 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF –Limitations Limitation - Target close to obstacle: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion

35. Leng-Feng Lee Dec 3, 2004 Slide 35 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Local APF -Limitations Some other limitations include: –No passage between closely spaced obstacle. –Non optimal path. –Implementation related limitations. Oscillation in the presence of obstacle; Oscillation in narrow passages; Infinite torque is not possible. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion

36. Leng-Feng Lee Dec 3, 2004 Slide 36 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Global APF – Navigation Function Properties: –Guarantee to provide a global minimum at target. –Bounded maximum potential. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Letbe a robot free configuration space, and let be a goal point in the interior of, A map is a Navigation Function if it is: 1. Smooth on, that is, at least a function. 2. Polar at,i.e., has a unique minimum aton the path-connected containing. component of 3. Admissible on, i.e., uniformly maximal on the boundary of. 4. A Morse Function [ Proposed by: Rimon & Koditschek]

37. Leng-Feng Lee Dec 3, 2004 Slide 37 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Navigation Function Navigation Function of a sphere world : Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Where: Number of obstacles is the implicit form of bounding sphere. is the implicit form of obstacle geometric Eq. Feature: Tunable by a single parameter : Detail

38. Leng-Feng Lee Dec 3, 2004 Slide 38 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Navigation Function -Constructions To construct Navigation Function: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Distance-to-the-Target Function: Obstacle Modeling: is the implicit form of obstacle model’s geometric equation. The product of all the obstacle functions: is the implicit form of bounding sphere More Back

39. Leng-Feng Lee Dec 3, 2004 Slide 39 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Navigation Function -Constructions To construct Navigation Function (cont’): Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Conditioning Functions: Analytic Switch Function: Sharpening Function: A Navigation Function can be obtained by: Back

40. Leng-Feng Lee Dec 3, 2004 Slide 40 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Navigation Function Example - Navigation Function of a sphere world : Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Where:

41. Leng-Feng Lee Dec 3, 2004 Slide 41 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Navigation Function -Constructions At low value of, local minima may exist: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion

42. Leng-Feng Lee Dec 3, 2004 Slide 42 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Navigation Function – MATLAB GUI A GUI to properly select a value: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion

43. Leng-Feng Lee Dec 3, 2004 Slide 43 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Formulation & Simulation Idea: –We want the robot to follow the negative gradient flow of the workspace potential field; –Analogy to a ball rolling down to the lowest point in a given potential. –Thus the gradient information will serve as the input to the robot system. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion

44. Leng-Feng Lee Dec 3, 2004 Slide 44 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Formulation Formulation – Single point-mass robot: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Kinematic Model: Dynamic Model: is the gradient of the potential field is a positive diagonal scaling matrix is dissipative term added to stabilize the system

45. Leng-Feng Lee Dec 3, 2004 Slide 45 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Formulation Formulation – Nonholonomic Wheeled Mobile Robot (NH-WMR): Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Kinematic Model: desired y-direction velocity. the desired x-direction velocity. is the projected gradient onto the direction of forward velocity. is the proportional to the angular error between the gradient and robot direction.

46. Leng-Feng Lee Dec 3, 2004 Slide 46 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Formulation Formulation – Group robot without formation constraints: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Generalize position: Kinematic Model: Dyanamic Model: -number of point-mass robot

47. Leng-Feng Lee Dec 3, 2004 Slide 47 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Simulations Simulation 1 – Single robot with single obstacle: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Detail

48. Leng-Feng Lee Dec 3, 2004 Slide 48 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Simulations Simulation 1 – Formulations and Simulation Settings: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Robot Model: Repulsive potential: Attractive potential: Back

49. Leng-Feng Lee Dec 3, 2004 Slide 49 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Simulations Simulation 2 – Single robot with two obstacles: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Detail

50. Leng-Feng Lee Dec 3, 2004 Slide 50 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Simulations Simulation 2 – Formulations and Simulation Settings: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Robot Model: Repulsive potential: Attractive potential: Back

51. Leng-Feng Lee Dec 3, 2004 Slide 51 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Simulations Simulation 3a – Single robot with four obstacles: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Detail

52. Leng-Feng Lee Dec 3, 2004 Slide 52 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Simulations Simulation 3a – Formulations and Simulation Settings: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Robot Model: Total potential: Back

53. Leng-Feng Lee Dec 3, 2004 Slide 53 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Simulations Simulation 3 – Single NH-WMR with four obstacles: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion DetailMore

54. Leng-Feng Lee Dec 3, 2004 Slide 54 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Simulations Simulation 3 – Formulations and Simulation Settings: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Robot Model: Total potential: Back

55. Leng-Feng Lee Dec 3, 2004 Slide 55 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Simulations Simulation 4 – Group robots without formation constraint: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Detail

56. Leng-Feng Lee Dec 3, 2004 Slide 56 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Simulations Simulation 4 – Formulations and Simulation Settings: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Robot Model: Only Attractive potential: Back

57. Leng-Feng Lee Dec 3, 2004 Slide 57 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Simulations Simulation 5 – Group robots without formation constraint: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Detail

58. Leng-Feng Lee Dec 3, 2004 Slide 58 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo APF Approach – Simulations Simulation 5 – Formulations and Simulation Settings: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Robot Model: Total potential: Back

59. Leng-Feng Lee Dec 3, 2004 Slide 59 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo PART II: Group Robots Dynamic Formulation Include: –Dynamic Formulation for Group of Robots with Formation; –Solved the E.O.M using three Methods; –Simulation Studies; –Performance evaluation of each Methods.

60. Leng-Feng Lee Dec 3, 2004 Slide 60 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Group Robots Dynamic Formulation Approaches for formation maintenance: –Formation Paradigm Leader-follower [Desai et. al., 2001] Virtual structures [Lewis and Tan, 1997] Virtual leaders [Leonard and Fiorelli, 2001], [Lawton, Beard et al., 2003] Our Approaches: –View as a constrained mechanical system. –Formation constraints – holonomic constraints added to a unconstrained dynamic system. –Motion planning now can be treated as a forward dynamic simulation of a constrained mechanical system. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion

61. Leng-Feng Lee Dec 3, 2004 Slide 61 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo The dynamic of group of robot can be formulated using Lagrange Equation by: Group Robots Dynamic Formulation is the n-dimensional vector of generalized coordinates is the n-dimensional vector of generalized velocities is the n-dimensional vector of external forces is the vector of input forces, which is is the Jacobian matrix Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion (1)

62. Leng-Feng Lee Dec 3, 2004 Slide 62 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Group Robots Dynamic Formulation The Lagrange Equation can be solved using following three methods: –Method I: Direct Lagrange Multiplier Elimination Approach. Explicitly computing the Lagrange multiplier by a projection into the constrained force space. –Method II: Penalty Formulation Approach. Approximating the Lagrange multiplier using artificial compliance elements such as virtual springs and dampers. –Method III: Constraints Manifold Projection Based Approach By projecting the equations of motion onto the tangent space of the constraint manifold in a variety of ways to obtain constraint- reaction free equations of motions. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion

63. Leng-Feng Lee Dec 3, 2004 Slide 63 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Group Robots Dynamic Formulation Method I: Direct Lagrange Multiplier Elimination Approach: –The direct Lagrange multiplier elimination is a centralized approach where the Lagrange multiplier is explicitly calculated to ensure formation constraints are not violated. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion (2) (3) The resulting Dynamic Equation can be expressed as: Detail

64. Leng-Feng Lee Dec 3, 2004 Slide 64 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Group Robots Dynamic Formulation Method I: Direct Lagrange Multiplier Elimination Approach (cont’) : Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Substituting Eq (3) into Eq (2), upon rearrange, we obtained: Which can now solve for (4) Differentiating the constraint in Eq (1) twice to get the acceleration level constraint: From Eq (1), we obtained: Back

65. Leng-Feng Lee Dec 3, 2004 Slide 65 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Group Robots Dynamic Formulation Method II: Penalty Formulation Approach: –The holonomic constraints are relaxed and replaced by linear/non-linear spring with dampers. –Here, the Lagrange multipliers are explicitly approximated as the force of a virtual spring or damper based on the extent of the constraint violation and assumed spring stiffness and damping constant. Resulting Dynamic Equation: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion This can be expressed as: (4)

66. Leng-Feng Lee Dec 3, 2004 Slide 66 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Group Robots Dynamic Formulation Method III: Constraints Manifold Projection Based Approach: –In this approach, the dynamic equation with constraint- reactions is projected into the tangent space (feasible motion subspace) to obtain the constraint free projected dynamics equations. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion (5) Thus, the resulting Dynamic Equation become: Is the independent velocities. Detail

67. Leng-Feng Lee Dec 3, 2004 Slide 67 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Group Robots Dynamic Formulation Method III: Constraints Manifold Projection Based Approach (cont’): Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Pre-multiply both side of Eq(1) by, and notice that : Substituting Eq.(6) into Eq. (7), we get: Differentiating, we get: Back

68. Leng-Feng Lee Dec 3, 2004 Slide 68 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Group Robots Dynamic Formulation Baumgarte Stabilization: –To prevent numerical drift in the simulation, we adopted Baumgarte stabilization method. –Baumgarte stabilization method involves the creation of an artificial first or second order dynamical system which has the algebraic position-level constraint as its attractive equilibrium configuration. For example, the holonomic constraint of Eq.(1) is replaced with: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Where the solution of the above equation is :

69. Leng-Feng Lee Dec 3, 2004 Slide 69 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Case Study - Formulation: Three point-mass robots forming a triangular shape: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion where: The governing Equation can be written as: We will use this model to perform various case studies.

70. Leng-Feng Lee Dec 3, 2004 Slide 70 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulation & Results Performance Evaluation – Formation Error: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Formation Error: is the total formation error; is the actual Euclidean distance between robotand robot is the desired Euclidean distance between robot and robot

71. Leng-Feng Lee Dec 3, 2004 Slide 71 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Formulation – Case Study Method I: Direct Lagrange multiplier elimination The dynamic equation obtained with a second order Baumgarte stabilization: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion (9) where: Detail

72. Leng-Feng Lee Dec 3, 2004 Slide 72 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Detailed Formulation – Case Study Method I: Direct Lagrange multiplier elimination Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion The constraint : Back Next

73. Leng-Feng Lee Dec 3, 2004 Slide 73 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Formulation – Case Study Method II: Penalty Formulation Approach The centralized formulation: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion (9) Detail The decentralized formulation: Robot A Robot B Robot C

74. Leng-Feng Lee Dec 3, 2004 Slide 74 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Detailed Formulation – Case Study Method II: Penalty Formulation Approach Show the detailed formulation here….Lengfeng… Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion (9) where: Back

75. Leng-Feng Lee Dec 3, 2004 Slide 75 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Formulation – Case Study Method III: Constraint Manifold Projection Based Approach The centralized formulation: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion (9) Detail The “decentralized” formulation: Robot A Robot B Robot C

76. Leng-Feng Lee Dec 3, 2004 Slide 76 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Detailed Formulation-Case Study Method III: Constraint Manifold Projection Base Approach Show the detailed formulation here….Lengfeng… Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion (9) where: Back

77. Leng-Feng Lee Dec 3, 2004 Slide 77 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Formulation- some considerations Choosing Formation Constraints: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion –As number of robots increases, it become more and more difficult to select the constraints and to distribute the constraints among robots. This issue can be solved using Graph Theory.

78. Leng-Feng Lee Dec 3, 2004 Slide 78 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Formulation- some considerations Change of Formation : Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion ContractionExpansion Shape Change

79. Leng-Feng Lee Dec 3, 2004 Slide 79 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulations & Results Case Study 1 – Three robots in formation, without obstacle: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Method I:

80. Leng-Feng Lee Dec 3, 2004 Slide 80 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulations & Results Case Study 1 – Three robots in formation, without obstacle: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Method II: Decentralized Formulation:

81. Leng-Feng Lee Dec 3, 2004 Slide 81 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulation & Results Case Study 1 – Three robots in formation, without obstacle: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Method III: Partial Decentralized Formulation:

82. Leng-Feng Lee Dec 3, 2004 Slide 82 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulation & Results Case Study 1 – Formation Error from three methods: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Method I Method II Method III

83. Leng-Feng Lee Dec 3, 2004 Slide 83 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulation & Results Case Study 1 – Formation Error & Effect of Ks Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Method II:

84. Leng-Feng Lee Dec 3, 2004 Slide 84 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulation & Results Case Study 1 – Formation Error & Effect of Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Method II:

85. Leng-Feng Lee Dec 3, 2004 Slide 85 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulation & Results Case Study 2 – Three robots in Formation, one obstacle Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion

86. Leng-Feng Lee Dec 3, 2004 Slide 86 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulations & Results Case Study 2 – Three robots in formation, one obstacle: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Method II Method III Method I

87. Leng-Feng Lee Dec 3, 2004 Slide 87 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulations & Results Case Study 2 – Three robots in formation, one obstacle: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Method I:

88. Leng-Feng Lee Dec 3, 2004 Slide 88 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulation & Results Case Study 2 – Formation Error from three methods: Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Method I Method II Method III

89. Leng-Feng Lee Dec 3, 2004 Slide 89 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulation & Results Case Study 2 – Formation Error & Effect of Ks & Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Method II Method III

90. Leng-Feng Lee Dec 3, 2004 Slide 90 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulation & Results Case Study 2 – Three robots in Formation with Expansion. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Each sides change from 2 units to 4 units in 4 seconds:

91. Leng-Feng Lee Dec 3, 2004 Slide 91 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulation & Results Case Study 3 – Three robots in Formation with Expansion. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Method II Method III Method I

92. Leng-Feng Lee Dec 3, 2004 Slide 92 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulation & Results Case Study 3 – Three robots in Formation with Expansion. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Method I:

93. Leng-Feng Lee Dec 3, 2004 Slide 93 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulation & Results Case Study 3 – Formation Error & Effect of Ks & Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Method IIMethod III

94. Leng-Feng Lee Dec 3, 2004 Slide 94 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulation & Results Case Study 4 – Three robots in Formation with Shape Change. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Constraint between robot A & B change from 2 units to 4 units in 4 seconds: Note: Method I cannot perform this task because when three robots in a straight line, the inverse of the Jacobian matrix become singular.

95. Leng-Feng Lee Dec 3, 2004 Slide 95 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulation & Results Case Study 4 – Three robots in Formation with Shape Change. Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Method IIMethod III

96. Leng-Feng Lee Dec 3, 2004 Slide 96 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Simulation & Results Case Study 4 – Formation Error & Effect of Ks & Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion Method II:Method III:

97. Leng-Feng Lee Dec 3, 2004 Slide 97 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Conclusion General Characteristics – Formation Accuracy Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion The average total formation error for each method :

98. Leng-Feng Lee Dec 3, 2004 Slide 98 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Conclusion General Characteristics – Computational Time Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion The average total Computational Time (sec) for each method :

99. Leng-Feng Lee Dec 3, 2004 Slide 99 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Conclusion General Characteristics – Decentralize formulation capability Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion The decentralize formulation capability for each method : Centralized Decentralized

100. Leng-Feng Lee Dec 3, 2004 Slide 100 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Conclusion Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion General Characteristics – Formation related concerns: –The Jacobian matrix in Method I and Method III can become singular in some specific position. –Method II has no such limitations. In Summary:

101. Leng-Feng Lee Dec 3, 2004 Slide 101 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Conclusion Introduction APF Approaches Group Robot Formulation Simulation Results Conclusion –Evaluation of various potential functions. –Development of a GUI to generate navigation function. –Develop the group motion planning problem as a forward dynamic simulation problem; –Evaluation of three different method in solving motion planning problem for a group of robots in formation. –Critical evaluation of the performance by the three approaches.

102. Leng-Feng Lee Dec 3, 2004 Slide 102 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Future Work Introduction Model Distribution Analysis Manipulability Cooperative System Conclusion –Provide a way to avoid Jacobian matrix become singular. –Incorporate nonholonomic constraints in the formulation. –Implement a more efficient gradient finding method by utilizing the available information from each robot. –Implement the algorithm in a decentralized computation manner.

103. Leng-Feng Lee Dec 3, 2004 Slide 103 of 51 Automation, Robotics and Mechatronics Lab, SUNY at Buffalo Thank You! Acknowledgments: Dr. V. Krovi, Dr. T. Singh & Dr. J. L. Crassidis