Most robots have a set of navigation algorithms for motion planning that execute at different frequencies, global path planners (e.g. A*, ~0.1 Hz), mid-level path deformation (e.g. elastic band, ~5Hz), and collision / obstacle avoidance algorithms (~20Hz), which will be the last step before actuator control.
For MAGIC 2010, we used the Dynamic Window Approach.
(Note the ROS navigation stack offers the same algorithm configurations, but of-course didn't exist at the time we had to develop the WAMbot codebase). There are three common approaches used for local trajectory planning:
There are two main goals, calculate a valid velocity search space, and select the optimal velocity. The search space is constructed from the set of velocities which produce a safe trajectory (i.e. allow the robot to stop before colliding), given the set of velocities the robot can achieve in the next time slice given its dynamics ('dynamic window'). The optimal velocity is selected to maximize the robots clearance, maximize the velocity and obtain the heading closest to the goal.
Its easier to explain if we look at the code first. In pseudo-code the DWA is:
For MAGIC 2010, we used the Dynamic Window Approach.
(Note the ROS navigation stack offers the same algorithm configurations, but of-course didn't exist at the time we had to develop the WAMbot codebase). There are three common approaches used for local trajectory planning:
- Potential-field based, where each obstacle has an obstacle 'force field' for repelling the robot, and the goal has a attraction field. (A similar approach is 'Vector-fields', and Virtual Force Field)
- Dynamics based, where the algorithm consider the robots dynamics in calculating a solution. (e.g. Velocity Obstacles and Dynamic Window Approach)
- Sampling based, where various collision free states are sampled and then combined. (e.g. Reachability graph, Probabilistic roadmaps)
There are two main goals, calculate a valid velocity search space, and select the optimal velocity. The search space is constructed from the set of velocities which produce a safe trajectory (i.e. allow the robot to stop before colliding), given the set of velocities the robot can achieve in the next time slice given its dynamics ('dynamic window'). The optimal velocity is selected to maximize the robots clearance, maximize the velocity and obtain the heading closest to the goal.
Its easier to explain if we look at the code first. In pseudo-code the DWA is:
BEGIN DWA(robotPose,robotGoal,robotModel)
desiredV = calculateV(robotPose,robotGoal)
laserscan = readScanner()
allowable_v = generateWindow(robotV, robotModel)
allowable_w = generateWindow(robotW, robotModel)
for each v in allowable_v
for each w in allowable_w
dist = find_dist(v,w,laserscan,robotModel)
breakDist = calculateBreakingDistance(v)
if (dist > breakDist) //can stop in time
heading = hDiff(robotPose,goalPose, v,w)
clearance = (dist-breakDist)/(dmax - breakDist)
cost = costFunction(heading,clearance, abs(desired_v - v))
if (cost > optimal)
best_v = v
best_w = w
optimal = cost
set robot trajectory to best_v, best_w
END
Now to explain:
- First, we can calculate the desired velocity to the goal based on our current position, and the destination. (e.g. go fast if we are far away, slow if we are close. Use Equations of Motion, see Circular motion for a mobile robot).
- Select the allowable velocities (linear 'v', and angular 'w') given the vehicles dynamics, e.g. allowable_v ranges from the current velocity subtract the robots maximum deceleration * timeslice to the current velocity plus the robots maximum acceleration * timeslice, or more compact: [v-a.t,v+a.t], likewise for angular velocity.
- Search through all the allowable velocities
- For each velocity, determine the closest obstacle for the proposed robot velocity (i.e. collision detection along the trajectory)
- Determine if the distance to the closest obstacle is within the robots breaking distance. If the robot will not be able to stop in time, disregard this proposed robot velocity.
- Otherwise, the velocity is 'admissible', so we can now calculate the values required for the objective function. In our case, the robots heading and clearance.
- Calculate the 'cost' for the proposed velocity. If the cost is better than anything else so far, set this as our best option.
- Finally, set the robots desired trajectory to the best proposed velocity.
Posts
