Wednesday, March 4, 2009

Better pathfinding

I think Computer Scientists are so cute when they pretend that they can build robots. Now I'm not knocking all of them, but there's a select group of shall we say unworldly computer scientists who are too far up in the clouds for robotics. Software is of course very necessary for robots but so are electronics and mechanics. Just TRY to build a robot on your own or even work with a pre-made robotic base if you don't understand the first thing about torque vs angular velocity and how it relates to power. Or beat your head against a wall when your robot takes a heading of 3 degrees vs the 0 degrees you told it to. Why DOES it do that? Surprise! Your sensors aren't that accurate even though your numerical precision is.

Anything having to do with kinematics is going to confuse the bejeezus out of someone who isn't familiar with it. True, it's all stuff you learned in physics but the devil is in the details. It took me a good month to really internalize the roles of current and voltage when dealing with motors (in short - current is torque, voltage is speed but it's only this simple at steady state!). And while it's very straightforward to say the reason your car can't turn on a dime at 50mph is momentum, it's a much harder thing to tell me exactly how quickly your car CAN turn at that speed. This is the realm of physicists, mechanical engineers and me - the controls engineer. It's part of my job to do the calculations that tell you how much effort you will expend to turn your car at that speed (and whether it's safe).
So it's no surprise to me that I hear more about what data structures to use for pathfinding, how to make it computationally less complex and whether it always produces the shortest-cost path (all GREAT fodder for computer scientists) than I hear about whether the path is actually something useful. Most discussions I've heard ignore what will actually happen when you attempt to follow that path. For instance - if your GPS has you making 90 degree turns every 5 seconds I don't care how 'least-cost' your path is in terms of the criteria you assigned because it requires too much effort (starting, stopping, slowing, turning, etc). It also certainly won't be the least time intensive path.

The level of 'smoothness' of motion is a concern in controls. Jerky motion requires much more power than nice smooth motion. It's hard to get things to stop and change direction all the time. I once worked with a digital servoamplifier which had several different 'motion profile' modes. I needed to feed it position commands every .1s. So the first mode I tried had it move as fast as possible to the commanded position and then STOP. Then start up again when it got the next command, move as fast as possible and then STOP when it reached the position. That's the first thing you'd try if you had never designed anything like this before and it turns out to be an awful approach. A much better approach was the second motion profile - attempt to arrive at the commanded position but keep moving in the expectation of the next position command.

Now apply this to pathfinding: Are you going to go from Point1 to Point2 then STOP, change direction and go to Point3 (then STOP)? Not ideally. You want a smooth path. You need to take into effect several things: current speed, heading, difficulty of turning, etc. You make a model of your vehicle and compute how difficult it would be to actually execute all of those crazy moves the algorithm things up. So instead of just finding the shortest (distance) path and you weigh the two factors against each other. Just make sure that your weights prevent impossible actions ('Go straight ahead at 50MPH then stop after 50.18 yards and make an instant right turn and immediately resume 50MPH. It'll get ya there in 23 seconds flat. LEAST COST BABY YEAH!'). This will lead to more realistic paths whose costs more accurately affect realistic situations. That's better for everyone.

A quick implementation of this would be to add it to the movement cost in A* (I've heard this called the G score of a square). In the simplest A* algorithm I implemented the movement cost for a 90 degree movment (forward, back, left, right) was always 10, and for a diagonal movement was 14 (sqrt(2)*10 because of the added distance to reach a diagonal square). I would keep track of my current heading in terms of degrees. Assume that the top of the screen is zero and right is 90 degrees, bottom is 180 degrees left is 270 degrees. Heading in this case will be either 0, 45, 90, 135, 180, 225, 270 or 325 degrees. Just add a cost proportional to the change in heading to the G score - this will keep your path from having too many jerky motions. You'll have to fiddle with the weight to balance least cost vs. least effort (you don't want a lazy robot do you?). You can then get more advanced by incorporating speed and momentum into the model. To incorporate speed, allow the pathfinding algorithm to search more than just the squares around it but with a caveat: it's difficult to speed up and slow down. Thus, my normal speed is 1 square/turn - that gets me to the forward, back, left and right squares for a cost of 10 usually. When I want to go to the diagonal squares I have to increase my speed to 14. If I want to go to the forward square two squares ahead of me my speed has to be 20 - I have to speed up. Speeding up takes effort so add to the G score a cost proportional to the absolute value (absolute value because any change in speed requires an effort - you don't gain effort for going from hi speed to low speed) of the difference between my current speed and next speed AND use the speed in the effort cost calculation for turning (higher speeds make turning harder).

Once you start down this path there are many physical parameters you can add to the system. I have yet to implement this but I look forward to some excellent pathfinding in the future!