Monday, March 26, 2018

algorithm - How do I calculate paths for objects with limited acceleration?



For example, say I have a car and a car has a specific minimum turning radius and I want to drive that car from point a to point b, but the car isn't facing point b. How do I compute a path to point b? Being able to specify the orientation at the point b would also be good (say you want to drive to your driveway and then pull in to your garage -- it doesn't do much good if you got to your driveway by driving over your lawn and are facing sideways :)


A pointer to documentation (or even just a name) would be perfectly fine -- I'm having trouble finding anything at all.


In my attempts, they work in simple cases, but fail miserably in situations such as when point b is closer to a than the minimum turning radius.


For example, how would you determine a path similar to this (the bold path):


Just a curved path for illustrative purposes


edit: In my actual problem, there are some simple pathing constraints, but I already have an A* algorithm in place that works, but it allows things to make instantaneous heading changes, so it looks silly seeing a car suddenly make a 90˚ turn on a dime when they get to a turn point.



Answer



I haven't worked through the full equations for this yet, but here's some visuals to help wrap our heads around the problem. It boils down to some geometry:


A car with circles indicating its turning radius. (Car icons via Kenney)


From any given starting point and orientation, we can draw two circles with our minimum turning radius - one on the left, one on the right. These describe the points on the tightest possible start to our path.



We can do the same for any desired end position and orientation. These circles describe the tightest possible end to our path.


Now the problem reduces to finding a path that joins one of the start circles to one of the end circles, kissing each one along its tangent.


(This is assuming we don't need to pathfind around obstacles in-between, which wasn't mentioned in the question. Stormwind's answer gets into how we can use navigation graph information for these types of problems. Once we have the sequence of nodes to pass through, we can apply the method below to each segment of the plan.)


If, for simplicity, we use straight lines, we get something like this:


Diagram showing various paths a car could take.


This gives us the limiting case. Once you've found a path by this method, you can artificially inflate one or both start & end circles to get a less direct but smoother path, up until the point where the two circles kiss.


Computing these paths


Let's work out the cases for one turning direction - say we begin our path by turning right.


The center of our right turning circle is:


startRightCenter = carStart.position + carStart.right * minRadius



Let's call the angle of the straight section of our path (measured from the positive x-axis) pathAngle


If we draw a vector from rightCenter to the point where we leave the turning circle (at which point we must be facing pathAngle), then that vector is...


startOffset = minRadius * (-cos(pathAngle), sin(pathAngle))


That means the point where we leave the circle must be...


departure = startRightCenter + startOffset


The point where we re-enter a turning circle depends on whether we're aiming to end with a left or a right turn:


// To end with a right turn:
reentry = endRightCenter + startOffset

// To end with a left turn: (crossover)

reentry = endLeftCenter - startOffset

Now, if we've done our job right, the line joining departure to reentry ought to be perpendicular to startOffset:


dot(reentry - departure,  startOffset) = 0

And solving this equation will give us the angle(s) at which this is true. (I use a plural here because technically there are two such angles, but one of them involves driving in reverse which is usually not what we want)


Let's substitute the right turn to right turn case as an example:


dot(endRightCenter + startOffset - startRightCenter - startOffset, startOffset) = 0
dot(endRightCenter - startRightCenter, startOffset) = 0
pathAngle = atan2(endRightCenter - startRightCenter)


The crossover case is more complicated - it's the one I haven't worked out all the math for yet. I'll post the answer without for now, in case it's useful to you while I work out the remaining details.


Edit: Destination inside minimum turning radius


It turns out, this method often works out-of-the box even when the destination is closer than our minimum turning distance. At least some part of one of the re-entry circles ends up outside the turn radius, letting us find a viable path as long as we don't mind it getting a bit pretzel-like...


Demonstrating options when path-planning to a close destination.


If we don't like the path we get that way (or if one isn't feasible - I haven't checked every case exhaustively - maybe there are impossible ones), we can always drive straight forward or back until we get a suitable kissing contact between a start & end circle, as diagrammed above.


No comments:

Post a Comment

Simple past, Present perfect Past perfect

Can you tell me which form of the following sentences is the correct one please? Imagine two friends discussing the gym... I was in a good s...