The goal of the project is to coordinate multiple car-like robots to reach their respective destinations in an environment containing polygonal obstacles. In the problem, each robot must not bump into other robots or obstacles. It must also obey car's mechanism, namely, it can only move forward/backward in the direction its head is pointing to, and make left and right circular turns.
Through this project, I implemented potential-field based motion planning, used Reed-Shepp curves(the curve functions are provided)to convert honolomic path to nonholonomic path, and used decoupled coordination to deal with multiple robots.
The potential-field based motion planning algorithm cannot be applied to a wide range of applications, but is ideal for this specific problem. It runs relatively fast in finding a path, although this depends on the potential function used. For this project, I implemented the two common potential functions, NF1 and NF2. The latter results in much faster planning, but also a lot more coding/debugging time.
The Reed-Shepp algorithm's strength is its ease of implementation -- of course, this is assuming all the basic curve functions are provided. Theoretically, a honolomic path can always be converted into a non-honolomic one in an open space. In practice, due to time and space contraints, we have to set a resolution for the configuration space, and this limited resolution can often cause the algorithm to be unable to find a nonholonomic path for a holonomic one. In the resolution of 80x80x72 (x, y, orientation) I used, failure to convert happened quite frequently. In those cases, I'll have to keep the holonomic path and print out a warning message, so if you see from the animations below some sliding of the cars, it's because of this resolution problem. If I change my resolution constants to higher values, this problem should go away after a longer execution time. Another drawback of Reed-Shepp algorithm is that it can result in very quirky routes -- you will see a car swirling a long time to achieve some parallel motion whereas a turning followed by a forward will elegantly achieve the goal. This is because Reed Sheep algorithm relies on the holonomic path and when the environment isn't spacious enough, it has to cut the holonomic path into many small segments and use several curves for each segments.
The decoupled coordination I used doesn't pose a limit on the number of robots. It breaks the whole group of robots into two, then recurse on each group of robots. The recursion reaches the bottom when there are only one robot in the group. When that happens, an independent motion planning is executed without consideration of other robots. For the rest of the cases, where there are always two groups of robots(a group can have as few as one robot), an x-y style search is done to obtain a coordinated path. When the recursion returns to the top level, the whole group of robots are coordinated. This, however, causes a considerable loss in completeness. But it's an easy and practical way to deal with this problem. The other options all have to deal with high-dimension planning problem, which are a lot more complicated, and given the time frame of the project, I chose to use the aforementioned method.
A couple of typos appeared in Robot Motion Planning by Jean-Claude Latombe. They are in the pseudocode for the computation of NF2 potential function. In page 325's code "if q not in S then insert q' in S", q' should be q. And in page 324, the last and first line both have U(q)=M, and these two lines should instead set d1(q) to M.
The polygon intersection checking algorithm is a bit tricky and took some time to get right. Many special cases need to be considered. And if you cannot find a way to categorize these cases into as few as possible cases, the checking function can take a long time to execute. My resulting polygon intersection checking algorithm is quite efficient. Another thing is that Cfree does not need to be fully computed before motion planning, since not all values computed in this way will be used. Instead, I used on-the-fly calculation -- whenever a configuration needs to be determined whether it's in or out of Cfree and it hasn't been previously calculated, I'll calculate it. This way, no unnecessary computation will be done on Cfree, and this saves much time.
For the coordination part, after careful thinking, I came up with a data structure to represent a general path. I called it 'CompositePath' and it's defined in p5.h. It made the divide-and-conquer program a lot easier to write.
The coordination space is frequently too big, since the x and y dimensions are the sizes of the composite paths of the two groups of robots being considered. This will cause a lot of time to be spent on searching in the coordination space. Therefore, I posed a limit on the length of the composite paths generated. If the path is longer than that, it'll be scaled down to fit in that limit. This way, we can guarantee the coordination process will not take an exorbitant amount of time.
Even so, searching the coordination space is still time-consuming. In the beginning, I applied NF2 potential function based planning to the coordination space. However, the Cobstacle in coordination space very often will not have the nice geometric continuity as a normal Cobstacle will have, and this will render NF2 potential function based planning quite useless. So instead, I used the best-first search algorithm, and used the Manhattan distance between a position and the goal as a heuristic. Also, in generating new states from a known one, there are 8 possible directions to move, whereas only three actually are meaningful. The other 5 will not help the search process, in this specific problem. Therefore I only used three operators to generate new states and this reduced the search space significantly.
Below are some animations generated by the project. Thanks to David Hsu for the animation library, and Steve LaValle for the environment construction program.