# Pathfinding With Speed Based Turning Restrictions

I have been working on a path finding project and I’m a bit stuck at the moment. I have an object that can increase or decrease speed, but the faster it moves the larger it’s turn radius is - so a car basically. I’ve implemented A* with a smoothing algorithm to generate waypoints for the object, however, as you might guess, it doesn’t take into account anything with speed and such. Currently, I’m moving the object at a set speed along it’s forward vector and at each waypoint lerp the direction to the next waypoint.

Is there a method - or a place to start looking - to implement a system to find the best route that’s possible by slowing or taking larger turns? At this point I’d even settle for a way to tell the object exactly how much it needs to slow down for each turn. Thanks for any help!

## A* for large search spaces

You do not need to have a graph to run A*.

What?

You only need to be able to query where you can go from a given node. So, when A* wants the lists of the neighbor nodes, you generate them. You can even lazily generate※ them along with the heuristic values.

Each node will be a position plus an orientation (and probably a speed too, I'm not sure how you handle acceleration). And from there you will generate other nodes corresponding to the state of the car for different changes in speed and orientation.

You, of course, need your heuristic. And whatever heuristic you have for position only would remain admissible for these nodes.

※: In general, you want a method that gives you the next neighbor. And you call it to iterate. That is, you do not generate all the neighbors before iterating over them.

Memory problems

A* uses exponential in memory. There are two common improvements for A* to deal with that:

• Iterative Deepening A*: You use the heuristic of the distance from the start node to the goal to set a limit of how long a path should be. If the cost is greater than this limit, you consider the node closed.

If you did not find a solution, you increase the limit, you move all the closed nodes to open, and explore them again.

• Memory bounded A*: There are nodes in the open set that are unlikely to lead to a better solution. So, you are going to get rid of them.

The idea is that you have a maximum number of items in your open set. If you reach that capacity, you remove the worst. So you never have more than your maximum, and thus your open set does not run out of memory.

## Exploring a car driving space

Of course you can explore the whole range of turning angles and speeds.

However, ideally you will generate nodes that reduce the heuristic value first. We are going to approximate that. The idea is that it is better to orient the car correctly first and go fast in stright segments.

You are going to need a function that does this for a time interval:

1. Take a node and a turning angle as input.
2. Figure out what is the higher speed at which the car can turn that much. If there isn't take the minimum speed and the angle what the minimum speed would let you. You need consider how much the speed can change in the time interval.
3. Compute a new node that is the result of turning at that angle at that speed.

4. Assume the car goes straight, so set a turning angle to 0

5. Decide an angle step based on the turning angle that would make the car face the goal
6. Feed it to the function, get a node, yield the node to A*
7. Increase the angle by the step, if the angle has done a complete turn, you are done
8. Repeat from step 2

Please remember that you are feeding notes to A*. A* is still in charge of picking the best path.

For tweaks, consider that making the angle step smaller will result in more nodes. You may, for example, make it a function of nearby obstacle or the distance to the goal. You can do something similar to time steps. That should give the car more maneuverability where needed.

## Two level search

If you create a nav-mesh, with large convex areas. The connectivity of the areas is valid, regardless of the speed of the car. You can path find in this coarse nav-mesh, so you can figure out the general path before you worry about car speed and orientation.

Then you can path find with more complex rules from one areas of the nav-mesh to the next. I know that A* says that you compare the node to the goal. Don't do that, instead you are going to implement the goal as a predicate, which says "it is in the next nav-mesh".

Once you have found a solution that moves the car to the next area of the nav-mesh, you can reconstruct the path, clear memory, and continue the search to the next area of the nav-mesh, which will give you another section to add to the path. You continue like that until you are at the last area where you search for the goal position.

## Time budget

Breaking the search algorithm so that it conforms to a time budget is harder, yet possible. Turn the search algorithm into a state machine. To do that, you write it as continuations. Then represent the continuations as data (see "defunctionalization"). Immutable data structures are good for this. With that, you can store the current state of the algorithm when the time budget for it is up and extract partial results.

The partial results of the search algorithm up to where it could run in the time budget have some known cost for some nodes, pick among the nodes with known costs the node that would bring the car closer to the goal as per the heuristic, so the car can start driving, even though the search didn't have time to figure out the path to the goal.

In theory you can resume the search the next time budget. Although, I do not think that is very useful for the car, given that backtracking requires to do a U-turn, not simply retracing steps.

If you use the two level search as described above, you can have some confidence that the path is going in the correct general path, and that it will not have to do a U-turn. For the small chance of that happening, you would have to check if the algorithm has gone a different path than the one the car went, and if so, start searching again from the current position of the car.

• Thanks for such a in-depth explanation! Just to clarify and make sure I’m understanding what you’re saying - Step 1 would be to generate a nav-mesh, use it find a route ignoring turn radius and such, take the waypoints from that, then use the modified A* to find the path from waypoint to waypoint? Thanks again for the help! Feb 5 '20 at 20:45
• @GiantDwarf for abstract, yes. However, I'd say start with the simpler to implement solution, and work from there. I do not know complex your scenarios are or how fast the algorithm should be. Hopefully you do not need to do all that. Feb 5 '20 at 20:52
• Alright so - I’ve attempted to implement the new neighbor position, and in theory it’s creating nodes correctly, however I think I may be missing something in my implementation as my heap I’m using for the openlist runs out of memory... could you perhaps clarify that part by chance? Thanks again Feb 6 '20 at 4:48
• @GiantDwarf expanded answer. Feb 6 '20 at 6:52

Add the speed of the vehicle (and the direction) to the search space and let it affect which the possible neighbouring nodes are.

This means that 2 nodes are the same if and only if they are in the same position, have the same direction and are for the same speed.

When generating new nodes you have the range of full left and right possible for the speed, the range left and right when braking (down to 0), and the range left and right when accelerating (up to your speed limit).

This has the downside that the search space is a lot larger, but using the heuristic to prefer larger speeds should avoid needing to explore too much of the walking speed space.