Sean Hyde
16-735 Robotic Motion Planning
September 24, 2007
Homework #3
This is my implementation of a potential pathfinding algorithm.
This algorithm gives an attractive "force" to the goal and a repulsive "force" to each object. The algorithm follows the negative gradient (downhill) at each step.
Potential Gradients
The goal's attractive gradient is modeled by the equation:
where ζ is the attractive constant and d*goal is the distance at which the potential function becomes non-linear.
The objects' repulsive gradient are each modeled by the equation:
where η is the repulsive constant and Q* is the distance at which the object begins to influence the pathfinding. The Q* variable is useful for ensuring that only nearby ojects need to be considered in the calculations. This gradient is calculated for each object (closer than Q*) and then they are summed.
The difference between these two gradients (Uatt and Urep) is calculated at each step and the result is used to modify the current "velocity" (distance/step).
Algorithm Implementation Details:
1. Calculate Uatt.
2. For each obstacle, find the shortest distance from the robot to the obstacle. (Really the shortest of the distances to each of the obstacle's edges. This was calculated by projecting a perpendicular line from the robot's position to the line extending from the line segment that is the obstacle's edge. If this projection was outside of the line segment, the shortest distance was instead the shortest distance to one of the two vertices).
3. If this distance is less than Q*, calculate Urep for that object and add it to a running total. If the distance was greater than Q*, do nothing for this obstacle.
4. Sum Urep and -Uatt (we actually want to move in the negative gradient).
5. Add this sum to the current velocity
6. Move by the current velocity.
7. If the robot is at the goal (by some epsilon), report success.
8. If the robot did not move enough, report failure.
Algorithm Considerations:
This algorithm uses quite a few constants to form behavior. Depending on the types of obstacles and the desired speed, these constants can be tweaked. In the course of doing this project, I came up with some values that worked well for my scenarios:
G = .2; (attractive scalar)
gDist = .5; (attractive distance threshold)
R = .0012; (repulsive scalar)
qDist = 6; (repulsive distance threshold)
eps = .1; (distance epsilon)
GUI Additions:
In addition to implementing the potential planning algorithm, I added a few GUI features. You can now save and load Start/End/Obstacle environments. The main reason for this addition was actually because I was tired of creating them every time but did not want to hard code obstacles in. You can still, of course, create your own. I attempted to implement the star algorithm for bloating the obstacles but ran out of time. Although it was mentioned in class, it does not appear to be specifically required by this assignment.
Video Results:
This video shows manual creation of a start and end locaiton and obstacle. The potential path algorithm easily finds the goal in this scenario.
This video shows the new GUI feature for loading an enviroment. This environment is interesting because the robot skips over the path between the two obstacles due to its already easterly motion.
As this video shows, the corners of objects pose a particular difficulty. At these points, the gradient is non-continuous and can lead to erratic behavior by the robot. The robot seems to almost bounce back from the corner point before going around it.
Again, a corner point of an obstacle causes quite erratic behavior. When implementing this on an actual robot, I think I would make sure to limit the magnitude of the velocity.
This last video shows a failure due to local minima. When the robot reaches a position where the normal to the object's surface is roughly equal to the attractive gradient of the goal, it will stall. This is detected by the pogram, however.
Source Files: HW2b.m bug_2.m HW2gui.m HW2gui.fig addObstacle.m potentialPath.m