Multi-robot Path Planning in 4D Configuration Space
Introduction
This project attempts to solve the path planning problem for multiple robots, in particular, two robots. The objective is to move multiple robots in a common workspace W, from a given start configuration to a given goal configuration, A path must be computed for each robot that avoids collisions with obstacles and with other robots. We assume that an exact representation of the geometry of the robots and the workspace is given. We include two centralized methods in the implementation part. A* search is applied for two robots finding their collision free shortest paths, when the state space is 4D and every step gives the position of two robots. The second implementation algorithm is designed for efficiently searching high-dimensional spaces, named Rapidly-exploring Random Tree (RRT).This method is based on a RRT data structure and also has widely application, although the solutions found may not be the optimal solutions.
Problem Formulation
A state space is defined that considers the configurations of all robots simultaneously,
X = C1 × C2 ×· · ·× Cm. (1)
A state x ∈ X specifies all robot configurations and may be expressed as x=(q1, q2, . . . , qm).The dimension of X is N, which is N = sum(dimCi). In this project,C1=C2=2, N=4.
There are two sources of obstacle regions in the state space:robot-obstacle collisions, and robot-robot collisions. For i=1 or 2, the subset of X that corresponds to robot Ai in collision with the obstacle region, O, is
Xiobs = {x ∈ X | Ai(qi) ∩ O ~=∅}. (2)
For robot pair, A1 and A2, the subset of X that corresponds to A1 in collision with A2 is
X12obs = {x ∈ X | A1(q1) ∩ A2(q2)~= ∅}. (3)
Both (2) and (3) will be combined to yield Xobs[1]?.
Related Work
For this multiple robots path planning problem, approaches are often characterized as centralized or decoupled: A centralized plan algorithm computes a path in a combined configuration space defined in (1), which essentially treats the robots as a single combined robot. A decoupled planner include two phase, phase one compute a path for each robot by considering environment obstacles independently, and phase two use a coordination diagram to select relative velocities of the robots along their separate paths for collision-free trajectories for each robot.Decoupled planners are generally faster and quite practical, usually because fewer degrees of freedom are considered at one time. Unfortunately, if a problem arises, these approaches are typically unable to reverse their commitments. Therefore, completeness is lost.Some coupling may be necessary, as when two robots each have their goal as the other’s start position, so a decoupled planner may not find a solution, even when one exists[3]?.
Figure 1. An example when decoupled plan will fail. Circles in the center represent two robots.
Approach
In principle,A* algorithm is a complete and sufficiently general path-planning algorithm that can be used to implement centralized planners in the composite space X. Completely centralized path planning has bottleneck, because searching in searching large-dimensional configuration space may be beyond the practical capabilities of planning techniques.However,two robots system is a simple example, and A* based on approximate cell decomposition can be applied in this case. RRT algorithm itself is designed for high dimensional problems. In this project,we applied A* algorithm and RRT algorithm in the the composite space X described in problem formulation part.
A* algorithm
We use the form of best-first A* search, to avoid expanding paths that are already expensive based on admissible heuristic. We discretize environment obstacles via approximate cell decomposition to get approximate Xiobs. The tolerance distance between two robots is 2r, r is the size of robot measured as a circle. Check whether the discretization state is in X12obs according to tolerance distance in every planning step. A state has the form s(x1,y1,x2,y2),which includes the position information of two robots and is a combination of two paths. Because the step size is far less than the robotic size, the collision detection method can make sure that there is no collision path in each step. Evaluation function is f(s)=g(s) + h(s),where g(s) is the cost to reach the state so far,h(s) is estimated cost from the state to the goal,f(s) is estimated total cost of path through state s to the goal. Every implementation step we expand the state s with minimum f(s).
RRT algorithm
A Rapidly-exploring Random Tree (RRT) is a randomized data structure originally designed to handle differential constraints(including dynamics) and high degrees of freedom planning problems[2]?. Planning algorithm based on RRT is an incremental sampling and searching approach that yields good performance in practice without any parameter tuning.The idea is to incrementally construct a search tree that gradually improves the resolution, but does not need to explicitly set any resolution parameters.Thus, it has properties similar to space filling curves but instead of one long path, there are shorter paths that are organized into a tree. This sequence is random.
General procedure of constructing an RRT collision free with environment obstacles is as follows[1]?:
RRT(q0)
1 G.init(q0); 2 for i = 1 to k do 3 qn ← nearest(S, α(i)); 4 qs ← stopping-configuration(qn,α(i)); 5 if qs ~= qn then 6 G.add_vertex(qs); 7 G.add_edge(qn, qs);
Figure 2. If there is an obstacle, the edge travels up to the obstacle boundary, as far as allowed by the collision detection algorithm.
The MATLAB code reads an input file which contains the boundary information of the polytope obstacles (in particular, the coordinates of vertexes). Then RRT described above is run taking into account that the random points generated are within the arena. If the random point is in Xfree region, and some node can be connected to this without passing through any boundary of the obstacles,we directly add this randomly selected new node and edge to the tree; otherwise, the edge travels to the obstacle boundary and add the intersection point on the boundary to the node set of the tree. After every 20 random points, the goal configuration XG is feeded to the RRT as the random point. When the tree node set successfully include the goal, backward construct the path from the goal node to the start node.
Collision Detection
Obstacle space include Xiobsand X12obs. We assume that environment obstacles information is exact and fixed, and treat the mutual collision space as moving obstacles.Mutual collision space is not determined only by the tolerance distance as what we use in A* centralized path plan algorithm. It is shown in Figure 3. The step size may be large here, and one robot will not pass through the "moving obstacle region" that constitute by the tree edge of the other robot. Thus there is no mutual collision during each step.
Figure 3. Robot_2 and random selected node for Robot_2 constitute a moving obstacle region for Robot_1.
Our collision detection algorithm checks for polytope collision by the following algorithm. We know that every point in a line segment can be expressed as x+k*(y-x),where x and y are the end points of the line segment,1<=k<=1.Suppose the random selected point is a1 and its nearest neighbor is r1, both within the arena; obstacle's endpoints of one boundary are O1 and O2. What we would like to know is:whether the line joining a1 and r1 intersects the line joining O1 and O2. Transform this problem into checking two parameters l,k in the equation of computing the intersection point:
a1+l*(r1-a1)= O1+k*( O2- O1)(Note that this is actually two equations for coordinates of x-axis and y-axis).
Figure 4. Line segments collision detection for polytope obstacles.
If the value is such that 0<=l<=1 and 0<=k<=1 holds, then the line segments collide with each other, otherwise they are not in collision. In collision, the boundary point that will be added to the tree node set is exactly
P1= a1+l*(r1-a1).
Check the collision condition for every obstacle and every boundary line segment, revise the value of candidate node if needed after each checking. Finally we can get a new node and a new collision free edge of the RRT in 4D configuration space.
If the obstacle is a circle,like the environment we apply in project one, let c represent the center of the circle, R_circle is the configuration space obstacle radius, D denote the distance between circle center and line segment, then the collision condition is: D < R_circle. D can be computed from herons’formula.Given the lengths of the sides a,b,and c and the semiperimeter
of a triangle, Heron's formula gives the area of the triangle as
When c,r1 and a1 are the vertex of the triangle, D=area/norm(r1 -a1).
Figure 5. Collision detection of a candidate new edge between circle obstacle.
Simulation and Results
First I test whether my RRT algorithm can work for one robot in 2D space. Figure 6 shows four different paths in four trials.The four paths length are:25.4181, 22.2056, 20.3975, 26.9075 respectively. Because RRT is a kind of random exploring algorithm, the path is not the shortest path in general.Thus RRT is not optimal in the sense of shortest path.
Figure 6. One robot RRT path plan in 2D configuration space.Start from [0,0],goal is [0,6],the green line is the path, blue lines are tree edges.
The following simulation results are MATLAB figures.Centralized A* path plan tries to find a path in 4D composite configuration space that minimize the total cost (path length) of the two robots. Centralized RRT path plan can find a collision free path in 4D space,but it is usually not the optimal path,similar to the results shown in Figure 6. We can find a sub-optimal path by planning 5-10 times and choose the minimum cost path as the result, since RRT is much faster than A* in high dimensional condition. Every step in the path s has the form s_i(x1,y1,x2,y2),s is a n by 4 matrix, thus we can use MATLAB code d=sum([s(:,1)-s(:,3),s(:,2)-s(:,4)].^2,2) to get the Euclidean distance square between two robots at each step, and min(d) to make sure that the value is no less than the tolerance distance both for A* and RRT algorithms. Though the MATLAB figures have overlap paths for two robots, they actually pass through the same region at different steps.
Figure 7 shows A* centralized path plan for two robots. My RRT simulation can not work well when dealing with circle obstacles.A* star find a solution for both robots to go through the space between two circles at different discretization state.In order to get a simulation video in gazebo,a controller similar to what we apply in project one is applied for two robots, the function of controller is to make the two robots follow the path states step by step.
Figure 7. Two robots A* path plan.Start from [2.5,0.5]and [3 4],goal configuration is [9.8,1.5] and [9.2 8.1].
The following figures show several pairs of comparison of A* and RRT 4D path plan.Each pair has the same environment,start and goal configuration. Magenta and yellow lines are paths for two robots,green and blue lines are tree edges generated by RRT algorithm.
Figure 8.Start from [2 6.2],[6.5 6.2], goal is [6.5 6.2],[2 6.2].This is the example when decoupled approach will fail.(a)A* algorithm, total cost= 16.67,running time=1622.5 seconds.(b)RRT algorithm, total cost= 21.95,running time=32.7 seconds.
Figure 9.Start from [0 0],[1 1], goal is [0 6],[-1 7].(a)Two robots A* path plan,total cost=34.21,running time=822.5 seconds.(b)Two robots RRT path plan,total cost=39.18,running time=12.2 seconds.
Figure 10. Start from [0 0],[1 1], goal is [0 6],[-1 7].(a)Two robots A* path plan,total cost=38.98,running time=935.8 seconds.(b)Two robots RRT path plan,total cost=45.71,running time=13.4 seconds.
Figure 10. Start from [0 0],[1 1], goal is [0 6],[-1 7].(a)Two robots A* path plan,total cost=38.62,running time=535.8 seconds.(b)Two robots RRT path plan,total cost=45.02,running time=7.3 seconds.
Video
Gazebo video of two robots path in circle obstacles environment.
Discussion and Analysis
RRT has several advantages compared with A* algorithm.It is suited for high dimensional configuration space,which we can see from simulation.RRT is much faster in 4D configuration space.In addition, RRT needs little or no heuristics,while in A* algorithm, a bad heuristic may affect the search speed and result. The project simulation is based on MATLAB, RRT can be faster and more practical for high dimensional planning problems when written in C or C++.
RRT also has its disadvantage:completeness is sacrificed for shorter plan time.The path is usually not optimal, cost is larger than what we got from A* path plan, though we can get a sub-optimal path by planning several times and choose the least cost path. RRT also has a metric issue, because we can not directly assign a resolution for the exploring tree,it may be difficult to make sure that we can get a satisfying path. A* is controllable in this sense, as we can choose the step size of exact cell decomposition or resolution of approximate cell decomposition.
Centralized algorithm in this project can solve path plan problems when decoupled method may fail, but the time cost of complete algorithm like A* is high in high dimension. When there are more than two robots in a system, centralized algorithm can be combined with decoupled method to apply the advantages of each method in one problem.
Future Work
My RRT algorithm can not work well with circle obstacles at this moment, it seems to get an in-obstacle random point in high probability. I do not have a practical method to find the intersection point on circle boundary, and directly delete collision candidate edges can make it difficult to construct an exploring tree. I should find a good solution to deal with different type of obstacles and boundary condition.
As the related work part pointed out,centralized method has bottleneck in application, because of the time cost, though it can solve some fail example of decoupled methods. In a more complicated system with more than two robots, centralized and decoupled approaches can be combined, by decoupling the whole system into several sub-systems, and then applying centralized algorithm in each sub-system,finally coordinating these sub-systems. There are a lot of interesting decoupled,centralized problems and methods for complicated system planning problems.
Bibliography
1.[LaValle 05] LaValle, S., Planning Algorithms. Cambridge University Press, 2006. Available at http://msl.cs.uiuc.edu/~lavalle/planning/.
2.LaValle, S. M. (1998). Rapidly-Exploring Random Trees: A New Tool for Path Planning. In, TR 98-11(98-11), 98-11.
3.Gildardo Sánchez-Ante, Jean-Claude Latombe,Using a PRM Planner to Compare Centralized and Decoupled Planning for Multi-Robot Systems, May 2002,IEEE International Conference on Robotics&Automation.
Final project presentation comments
Avik De
I liked the part that you use some mathematics skill to solve the problem. It may be more fantastic if you test some harder examples.
Ben Charrow
Your project idea is interesting, as well as the cool video. Hope to see a detail explanation of how you make it work from your wiki page.
Brian MacAllister
Your simulation is quite impressive. It may be better if you give a more specific expression of how the algorithm work in your project.
Caitlin Powers
You give a quite clear view of your project implementation method. Look forward to see more simulation and examples.
Jason Owens
The idea of backpack detection is interesting, it may be more general if you extend the process for a type of objects.
Menglong Zhu
It’s an impressive video to show how PR2 recognize and read words. You may make a little extention, like try to read word from different distance to see whether there is any effect to the accuracy.
Stephen McGill
I like your video that two humanoid robots can corporate to relocate an object. It may be more challenging if you move something that is necessary to keep balance, for instance, there are some staff on the table and you do not want the objects to drop down when moving the table.
Ceridwen Magee
I like the way you show your simulation, the slides are very nice. I am looking forward to the experimental implementation besides software simulation.
Monica Lui
The idea to track target is interesting, and perhaps it can be applied to the robot-cup team in GRASP lab, when the robot from the other team gets the ball, try to surround it and catch the ball by your team.