# pic: A Star Path Planning

Having a peaked interested in machine learning in ai for quite some time (and with the arrival of this thread: A Vision Program that teaches itself the game - Programming - Chief Delphi), I decided to try to implement what I’ve learned in an quick demo. This was written in matlab, but the code can be ported to c.

With my previous program (output found here:pic: Kinect Path Planning - CD-Media: Photos - Chief Delphi), I now plan to combine these two programs, as well as 254’s code from this year in an attempt to make a robot go to a designated position on a field avoiding all obstacles.

I noticed a lot of what looks like stop and turn moves. I think this is a function of finding the shortest path. But when dealing with the real world we need to remember that the shortest path is not always the fastest path.

I’d be curious what would happen if you simulated a machine driving along this path annotated with vehicle velocity and battery voltage.

I’m aware of this fact, but I have no idea how to implement it so that it outputs curved paths. It returns the coordinates of the nodes needed to get from a to b. For it turn return a curved path, it works either have to return so many points it appears as a curve (it takes about a second to go through 100 steps of the algorithm), or for it to output a function or something of the matter.

I still need to sit down and go through 254s path generating and following code, so that might yield some answers.

I like Yash’s idea of making a square foot of the field a node, so the map would be roughly 54x27.

Something really neat about this is it’s speed, surprisingly it is really fast, faster than a human could find the shortest path. That being said, it is still feesible given enough updates of the environment with the Kinect and vision system, that in a match a robot could autonomously go from a to.b in teleop, given the other team is nice and doesn’t push you and only attenpts to block your path.

A quick note on terminology: In discussions of path planning “shortest path” tends to be treated as if one were saying “optimal path” rather than as literally the shortest distance. Shortest is usually described as minimizing some objective function. Some common choices for the objective function include distance moved, time taken, and fuel used.

If he wants fewer turns one way he can adjust his algorithm is make the objective function minimize time instead of distance and then say that turns cost time.

Sorry, I was going with the definition any reasonably astute observer would use for shortest which implies distance, I’ll be more clear in the future.

I’d still like to see various simulation runs with differing paths as generated by objective functions tuned for various variables (distance travelled, time taken, energy consumed) and various weightings of all of those.

That’s an interesting challenge. You could choose a board and come up with a bunch of different weights for each of the factors, run with each and then plot each result and be able to see where the switches are between different routes chosen.

I’m now going to go off on a tangent:

I wonder if there’s some systematic way solve for what the set of all optimal paths given a set of objective functions that are a linear combination of three other functions. I guess you could do something like breadth-first search but extend your “previous” table to keep track of all the different ways to get there that could be optimal under some set of conditions.

So if you have the weights of distance, time and enery as x, y, and z then at each point you might get you previous table to look like:

prev(2,2)]=
Point(2,1),(2x+3y+z),
Point(1,2),(3x+2y+z),
Point(1,2),(x+y+2z),
]

which would show that to get to the point at (2,2) you can come either (2,1) or (1,2) and that there are two different paths to get there via (1,2) which have different costs.

At each point, when you found a differet way to get there, you’d have to go through the table and see if there were any entries that were strictly shorter (less than or equal coefficients for each of the variables). And if not, you’d have to put in back in the queue to visit even if you’d been there before.

At the end, you’d get set of conditions under which you should have come from each direction, and would be able to say what the exact regions (of the objective function weights) were for each.

I haven’t thought too hard about what the runtime or space complexity of this would be but it seems like it wouldn’t be terrible in practive as long as there was some correlation between the different cost functions.

Look into gradient descent or the normal equation. They are methods of regression. If your equation was ax+ by+cz where a b and c are your weight factors, it will in theory find the global minimum and output the coefficients. I wonder if it would be quick enough to apply this for every time the program runs. I’ll look into this.

You can get part of the way there pretty easily: Extend your state space to include the robot’s orientation as well as position. Instead of finding the least-cost path through a 2D world, you now have a 3D state space. Moving in the direction you’re pointed (forward or back) incurs a cost of 1 (or maybe 1.4 for diagonals). Changing direction incurs a cost of, say, 2. The “goal state” can now be a specific position and orientation, or can include all the states where the position is correct. Make sense?

You could imagine extending this further, to include speed as another state variable. It costs something to increase speed, but at higher speeds, moving in the same direction incurs less cost. Turning can only occur at low speeds (or incurs absurd costs at high speeds).

To actually get smoother paths, you have to do something to further discretize your space. I’ll think more about how this could work and post back…

I haven’t done much reading on it, but I think the literature on non-holonomic motion planning covers this kind of thing.

You can also do path refinement, where you run A* on a coarse grid, and then detect points where the robot would make sharp turns and re-run A* on a small portion of the map around that turn with the configuration space further subdivided. Repeat this process until some upper limit on turn sharpness or configuration space subdivision is reached.

for those of you interested (looking at you Andrew Schreiber), I made some changes to my min_fn and used a constant environment. The results can be found here.

The student who I took under my wing this past build season has already implemented 254’s code to make a segmented path into curves, the results can be found here. What’s really cool is that apparently my program’s output is suitable for 254’s code to run on. I need to combine these two and see how long it takes to run through completely.

This is still a major work in progress, but it is getting somewhere.

The next step is to apply depth map readings into the party.

All this is in my dropbox of computer vision stuff