Ch. 5: Path Planning

1. Ch. 5: Path Planning ES159/259

2. Updates • Lab #3 next week • Prelab distributed today • Lab #2 writeup due 3/22 • Talk on Friday: • Brian Scassellati, Yale • MD G125, 3:00-4:00 • “Social Robots and Human Social Development” • Website update ES159/259

3. Path planning overview • Want to find a path from an initial position to a final position • Both are defined as vectors in the configuration space • Initial configuration is qs and final configuration is qf • Instead of completely defining QO or Qfree, we develop a sequence of discrete configurations that drive the configuration from qs to qf and avoid obstacles • Definition: • g: continuous mapping from qs to qf • Such that: ES159/259

4. Potential fields • To develop the mapping, we incrementally explore Qfree • Consider the manipulator (statically) as a point in the configuration space • The manipulator is subject to a potential field • Attractive in the case of the goal configuration • Repulsive in the case of an obstacle • Therefore we want to find the minimum potential • Hopefully, this will be at the global minimum configuration qf ES159/259

5. Gradient descent • In order to find minima of U, take the negative gradient: • Where t is a vector of joint forces/torques • in the configuration space • However, it is difficult to construct the potential field in the configuration space • Due to difficulty in describing the obstacles in the configuration space • Due to potential collisions with the links of the manipulator • Therefore we define the potential field not in the configuration space, but in the workspace • Thus the negative gradient will be a vector of forces in the workspace ES159/259

6. Hybrid attractive field • Combine the conic well potential and parabolic well potential fields • If the ith frame is close to the workspace goal, use the parabolic well • If the ith frame is far from the workspace goal, use the conic well • The distance d defines the distance from the goal that causes a transition from a conic to parabolic potential • Since this is continuous everywhere, the workspace force is defined everywhere ES159/259

7. Hybrid attractive field • Taking the gradient gives the workspace attractive force ES159/259

8. The repulsive field • Prevent collisions by creating a repulsive force in the workspace • Again, create forces that act on the origins of the n DH coordinate frames • These forces should: • Repel the robot from obstacles • Do nothing of the robot is far away from obstacles • First, define a radius of influence, r0, of an obstacle • The object will not repel oi if the distance is greater than r0 • Where r(oi(q)) is the shortest distance between oi and any workspace obstacle ES159/259

9. The repulsive field • So we can write this force as: • Finally, we need to define the gradient of the distance function r(oi(q)) • i.e. the gradient of r(x) at x = oi(q) • This defines the direction of the repulsive force (it is a unit vector) • Where b is the point on the obstacle that is closest to oi ES159/259

10. The relation between workspace forces and joint torques • When we discussed velocity kinematics we said that we could use the Jacobian to describe the mapping from joint forces and torques to end effector forces and torques: • Jv is the velocity portion of the Jacobian (since we have only discussed workspace forces, not torques) • The Jacobian must be derived for each origin oi, denoted Joi ES159/259

11. Ex: two-link planar manipulator • Consider the previous examples with an obstacle exerting a repulsive force on o2 • Find the attractive and repulsive forces on o1 and o2 • First determine the Jacobians for the two coordinate frames Initial and goal configurations Obstacle location ES159/259

12. Ex: two-link planar manipulator • For o1: • For o2: • From the previous examples we have: • Workspace attractive force on o1: • Workspace attractive force on o2: • Workspace repulsive force on o1: • Workspace repulsive force on o2: ES159/259

13. Ex: two-link planar manipulator • To determine the joint torques, take the transpose of the Jacobians at the initial configuration ES159/259

14. Ex: planar mobile robot • Using potential fields to control a mobile robot in the plane is similar to controlling the motion of the end effector in the workspace • Configuration determined by the generalized coordinates q = (x,y,q) • The point a has coordinates (ax,ay) such that: • Therefore the Jacobian for point a is: ES159/259

15. Ex: planar mobile robot • Multiplying the transpose of the Jacobian by the workspace force gives the forces and torques in the configuration space: ES159/259

16. Composing workspace forces • The total joint torques acting on a manipulator is the sum of the torques from all attractive and repulsive potentials: • If we add workspace forces before transforming into the configuration space, we will have incorrect torques in the configuration space • For example, consider two forces in the workspace such that F1 + F2 = 0 • Adding these in the workspace will ignore the torque about the center ES159/259

17. Ex: two-link planar manipulator • Consider again the two-link manipulator with a goal position and an obstacle near o2 • The total joint torque, due to these two potential fields is: Initial and goal configurations Obstacle location ES159/259

18. Gradient descent • Now that we can formulate the total torques acting on the joints in the configuration space due to the artificial potentials, we can formulate a path planning algorithm • First, determine your initial configuration • Second, given a desired point in the workspace, calculate the final configuration using the inverse kinematics • Use this to create an attractive potential field • Locate obstacles in the workspace • Create a repulsive potential field • Sum the joint torques in the configuration space • Use gradient descent to reach your target configuration ES159/259

19. Algorithm: Notation: qi: configuration at the ith iteration e: region of convergence zi:influence of attractive potential for oi. Can be different for each oi ai:defines step size of ith iteration hi:influence of repulsive potential for oi. Can be different for each oi r0:distance of influence for obstacles. Can be defined differently for different obstacles Gradient descent ES159/259

20. Local minima • In the absence of obstacles, the gradient descent will always converge to the global minimum (qf) • With obstacles, by proper choice of ai, this will always converge to some minima • May not be the global minimum • Probabilistic methods to escape local minima ES159/259

21. Instead we modify the gradient descent algorithm to add a random excitation in case we are stuck in a local minima We are stuck in a local minima if successive iterations result in minimal changes in the configuration If so, perform a random walk to get out The random walk is defined by adding a uniformly distributed variable to each joint parameter Local minima ES159/259

22. Random walk • Take a number of random steps starting from a local minimum configuration • To do this, add a constant to each qi at each step in the random walk: • Each vi are fixed constants, and there is an equal probability of adding +vi or –vi • i.e. a uniform distribution • If this is carried out for t steps, the probability density function can be represented as a zero-mean Gaussian: ES159/259

23. Alternate method: Probabilistic Roadmaps • Alternative to gradient descent • Randomly sample the configuration space (uniform) • Discard samples that lie within QO • This gives a rough idea of Qfree ES159/259

24. Alternate method: Probabilistic Roadmaps • Now connect the nodes of the sampled configuration space • For each point, connect to the k nearest neighbors • To decide the nearest neighbors using a norm in either the workspace or configuration space • Connect using straight line segments that do not intersect obstacles ES159/259

25. Alternate method: Probabilistic Roadmaps • Enhancement: It is clear that there are numerous connected segments, now connect as many of these as possible • Resample, keeping the previous segments • Can do this a number of times until Qfree is sufficiently covered ES159/259

26. Alternate method: Probabilistic Roadmaps • Choose a path in the connected space • Given qs and qf, apply to the roadmap • Search the roadmap for a path from qs to qf ES159/259

27. Next class… • Finish the discussion on path planning • Trajectory planning • Examples • Introduction to single variable control ES159/259