CODEWITHSUNDEEP
graphshortest-pathpath-findinga-star
Damjan Dakic
Damjan Dakic

Reputation: 265

A* orientation discretization

I have a space with obstacles I wish to find a path through. What I can do is discretize the space into a grid and use A* (or D* or whatever) to find a path through it. I wish to now add orientation to the algorithm. So the node location now becomes a 3d vector (x, y, phi). You can go from one node to another one only if they belong to an arc (both positions are on a circle and are oriented along the tangent lines). How do I discretize the space so that angles don't explode in a sense that by traversing the graph, the set of possible angles becomes finite? Thanks.

Upvotes: 2

Views: 637

Answers (2)

Austin
Austin

Reputation: 8575

As I understand it, your challenge is not to discretize coordinate, but to discretize the headings. I had to do the same thing in a grid world that allowed movement in eight directions, i.e. horizontal, vertical and diagonal. Your discretized space should match the problem domain. For your consideration:

  • 4-directions: use a square grid with movement across edges
  • 8-directions use a square grid with movement across edges and vertices
  • 6-directions use a hexagonal grid with movement across edges
  • 12-directions use a hexagonal grid with movement across edges and points
  • ... and so on.

To actually get the discretized headings, I declared an enum called Direction:

public enum Direction {
    North,
    NorthEast,
    East,
    SouthEast,
    South,
    SouthWest,
    West,
    NorthWest;

//additional code below...
}

You can lookup the correct heading by first computing the XY-offset between the current position and goal position:

int dx = currentPosition.x - goalPosition.x;
int dy = currentPosition.y - goalPosition.y;

These were passed to the getInstance(int,int) method (below) to obtain the correct Direction:

public static Direction getInstance(int dx, int dy) {
    int count = Direction.values().length;
    double rad = Math.atan2(dy, dx); // In radians
    double degree = rad * (180 / Math.PI) + 450;

    return getInstance(((int) Math.round(((degree % 360) / (360 / count)))) % count);
}

public static Direction getInstance(int i) {
    return Direction.values()[i % Direction.count];
}

In effect, these methods compute the heading in degrees and rounds to the nearest Direction. You can then implement a method that moves/turns the agent in the the Direction heading, e.g. agent.turnToward(Direction d) or agent.move(Direction d).

Additional Resources:

Upvotes: 1

Andrew Walker
Andrew Walker

Reputation: 42480

Angles can be prevented from blowing up by ensuring that phi is considered to be modulo 2pi, that is, phi = phi + 2pi*k for any integer value of k.

In C like syntax, you might end up updating phi with fmod.

phi = fmod(phi + deltaphi, 2*pi)

Where deltaphi is the change in angle you're introducing (in radians).

The most common way to do this is to constrain the values of the angle phi to take on one of n discrete angles which also has the advantage of avoiding precision/rounding issues. Given that you know phi can only take on one of n values, you can treat it like an integer, and map the value to a real when necessary.

i = (i + deltai) % n
phi = (2*i*pi)/n)

Where your change in angle deltai is (2*deltai*pi)/n radians.

However, finding a good discretization is only part of the solution - it defines a representation of your configuration space, but as you've pointed out, you also need to consider what a valid transition is.

The simplest approach to integrate angles into planning is to require rotations and translations to be distinct (at any time you can do one or the other, but not both), or to be composable (always translate, and then on arriving instantaneously rotate).

Moving forward and or backward at the same time while you're turning introduces is more complex, and tends to not work particularly well with discrete lattices - it tends to require some model of the vehicle you're working with. The most common are the simple nonholonomic models - the forward only car (the Dubins' car) or the car with forward / reverse (the Reeds Shepp car) - your reference to tangents to circles, I'm guessing this is what you're after. Dubins-Curves, or similar libraries can be used to build libraries of possible paths that could be combined with an A* (or D*) planner.

Differentially Constrained Mobile Robot Motion Planning in State Lattices by Mihail Pivtoraiko, Ross A. Knepper and Alonzo Kelly has some striking images of what's possible.

Upvotes: 0

Related Questions