I am implementing A* for path finding of a mobile robot inside the corridor. As of now the path is produced inside the corridor but it slides over to the right following all the edges of the obstacles, but I prefer the path should lie in the middle of the corridor.
1.Is there any smoothing algo to do it?
2. How to include the steering constraints so that i can get realistic/feasible path?
3. How to give penalty for 'turn' so as to avoid zig-zag paths.
Since I am new to A* algo, I find difficlty in the above issues. Ref to any link, book is also welcome..
Thanks..
You can pre-form the field on which you run A* by say shrinking it by 1 tile, so that a cell that borders impassable cell in 4-way neighborhood will become impassable. Then your resultant A* path will be closer to the center of the corridor. Of course, several corridors might become impassable completely, but this is what's expected as we are practically simulating a 3x3 cross-shaped robot to walk around the grid, and 3x3 cross can't go through 2xN path.
About adding cost to turn - you have to add current direction to the array that holds A* data, and implement a two-argument function that will return a non-negative value for (old direction, new direction) pair of arguments. Say, "if old_direction is not equal to new direction, return 1, else return 0". Then add the result of that function to whatever cost you computed for each step of A* iteration.
You could simply limit the usable area to the middle of the corridor.
Related
I'm currently struggling in finding an algorithm if a path is possible or not.
I have a field of points, the positions of these points are fully random. I have also a starting point, and a destination point. On my starting point I can jump to any point around the starting point in a limited radius, and continue the same from there, but only with a limited amount of jumps. Performance in this case is important! Existant algorithms like Dijkstra won't help me here.
Any idea?
You could construct an undirected graph with the points as vertices. Each of the edges connects two points which are no further apart than the jump distance limit. Once this graph is constructed, you can find the shortest path with traditional algorithms.
To construct the graph, you could assign the points to a grid of 2D matrix cells. The cell hight and width is the jump radius limit. Candidate points for an edge for a given point have to belong to its matrix cell or directly adjacent cells. This reduces the construction time.
A further speedup could be to restrict a first version of the graph to those grid cells which are located near the direct line-of-sight between start and end point. Only if the search is not sucessfull, you could broaden the search area and try again.
If start and end point are further apart than radius limit times jump limit, no feasible path exists.
Just in case someone want have a solution:
Since the amount of jumps are limited I've created a radial grid, where the maximum radius is the amount of circles multiplied by their own radius.
After that I simply use an A-star path finder. (I used one existant by http://www.rapidfirestudio.com)
Can anybody give me a hint about how to approach following task from Codility: https://codility.com/programmers/task/hilbert_maze/
I would be able to find the shortest path by generating the maze and searching for the shortest path using BFS, but since the worst-case time complexity is expected to be O(N) I don't think this would be the right way to go. Time complexity of BFS is O(|V| + |E]) where V is number of vertices and E the number of edges. For example if N = 3, we have a grid of size 17x17 and it's intuitively obvious that we can't find the path in only 3 steps.
So, either the indicated time complexity is wrong and should be something like M^2 or there is a quick trick to simply calculate the distance between two points without using graph algorithms. I found some algorithms for calculating Hilbert distance for 2 given points (if that's what is needed here), which use bit manipulations etc. but couldn't understand them at all. Moreover, I think that the goal of the task is to find out on your own how to calculate the distance and not using an existing formula.
Is there somebody who solved this task and can help me further? Thanks!
Here is the solution I came up with:
Every points location can be defined by an array of quadrants and their orientation (it will have N elements) - each element representing the orientation in the previous quadrant. The whole maze having upwards orientations
You need to define this array for both points. For example: if N = 2 and the point is in the lower left quadrant then it will have the orientation to the left. We take this quadrant and we rotate our coordinate system so it will the same orientation. This way we define the next quadrant and orientation pair in our new system. So if we have our point in the lower left quadrant then it will have orientation to the left, but as this was relative to our previous orientation (which was also to the left) this will become an upwards orientation.
At this point we have all the quadrants and orientation down to the smallest maze that contains our point. From backwards (from the smallest maze) we need to solve them. Every maze can be solved by the following rules:
if our point in our current quadrant is on any of the extremes (meaning that any of the coordinate's components are either the lowest or highest of the quadrant) we leave it where it was, otherwise check next points
if our point is downwards or at the middle of the current quadrant then we move to the quadrants lowest middle point (these goes relative the previously defined orientation, i.e.: if our orientation is upwards then we will move our point at the topmost middle point)
if our point is upwards (in the relative direction) we will have to move it to the topmost middle point
Storing these moves, we check if we have any common elements in the two array belonging to the two points:
if not we calculate the distance between the two endpoints and the we add up all the distances from the two moves list (in this list every distance can be calculated as coordinate component subtractions, i.e.: abs(x1-x2) + abs(y1-y2))
if we have common elements then we delete every move after that including the common elements and we calculate the distance as mentioned at the point before
This solutions can be optimised, it is just meant to present and idea to start with.
Edit: Here is my implementation of the above presented solution in Swift3: https://codility.com/demo/results/training9WWFXU-EWC/
I'm implementing a robot to be able to solve any maze (where the robot only has front sensors, but I make it scan the surroundings), and I was able to get it to turn the maze into a map where 0 represents walls, and 1 represents roads, with possibly slanted roads. Now, the robot is not fast at turning, but fairly fast at moving down a straight line. Therefore, a normal shortest path algorithm through the somewhat slanted hallway would be slow, although the paths are wide enough for it.
For example, we find
0001111111000
0011111110000
0111111100000
1111111000000
1111110000000
As a possible map. I'd like the robot to recognize that it can walk diagonally, or even just go straight up then right then right again, instead of turning every time in a normal shortest path algorithm.
Any ideas? Also, a complete algorithm change is welcome too - I'm fairly new to this.
I've faced similar problem some time ago.
You can assign weights to surrounding cells and less weight to the front cell, thus making a weight graph that is made during the movement.
I used Dijkstra algorithm with weights of 2 for surrounding cells and weight 1 for the front cell, you must pass direction of robot to Dijkstra and when adding them to the priority queue, and when extracting cells from the queue add the neighbors with respect to the direction saved in the extracted cell.
Then make the move and then recompute the modified Dijkstra for finding the nearest unseen cell.
I need a shortest path algorithm for controlling a real life robot.
Lets say i have map of the environment in the form of a matrix where 1 is an obstacle and 0 is free space. If i use a conventional shortest path algorithm, such as A* then that would give me a Manhattan distance shortest path. So nowhere near a the actual shortest path. This problem arises since i cannot think of a way to penalize movement in such a way that a diagonal line is better than two straight lines. I can make a heuristic that will make A* try euclidean shortest path between two points first, but not actually make euclidean shortest path a better path.
Does anyone know of a method to get the continuous space shortest path? It does not have to be the actual optimal path, but a better one than straight lines and 90 degree angles.
I have one idea:
From the start point make a circle.
Increase the radius of the circle until the one point on the circle is next to a wall, or at the goal. All the points on the edge of the circle are set as children nodes with the penalty of the radius of the circle. All points inside the circle, that are not open, will be closed since there is no reason to test them. Repeat this in an A* way with euclidean shortest path as heuristic, until the goal state is reached.Make the robot go from one point to the next in a straight line.
This should give something closer to what i am looking for. A set of straight lines with various angles. It would of course be better with a continuous curvy line...
I have implemented a continuous space path planning algorithm and blogged about it here. It uses A* to get an initial estimate and finalizes it (and pennalizes for sharp turns and robot's orientation at the destination) by using the simple gradient descent algorithm.
Let's say the discrete path from A* has n "waypoints" (coordinates on the grid). First and last ones cannot be moved but others can, as long as the path doesn't go through blocked grid cells. The function to be minimized is parametrized by n - 2 parameters which move waypoints perpendicular to its current direction.
Given a starting square (y, x) on a 2d grid, I want to find the closest empty square to it. (Note: The 4 squares adjacent to the starting square should be considered closer than the 4 diagonal squares nearest it.)
The following image shows the order that I need to check the following cells on this grid:
The grid is bounded but can be quite large. In practice, the starting coordinate will be randomly located around the grid. (So, I don't think it's too important to worry about coordinates outside the bounds of the grid....)
What algorithm can I use to iterate around the circle in this manner?
A simple breadth-first search will do it. Push each neighbour to be examined onto a heap, prioritised by distance. You can probably get away with manhattan distance (dx + dy), but if not just use squared radial distance (dx2 + dy2). Whenever you pop an item it'll be the nearest. If it's empty, you found it. Otherwise push its neighbours onto the heap and keep popping.
I would probably use the square radial distance and only add adjacent squares (not diagonals). The diagonals will be considered later because they are immediately adjacent to other squares. You do need a way to keep track of which squares have already been considered so you don't add them again. There must be a clever dynamic-programming way of tracking this without having to clear a large grid of booleans each time you search... But saying that, a large grid of booleans will do quite nicely.
It can be solved with BFS (Breadth First Search). We have to process each square twice. First time we visit the still unvisited squares those share an edge with the current square and next time we visit the squares those share at least a point with the current square(Diagonally adjacent squares)
We can use two different queues to ensure that before processing a square 2nd time all squares with equal distance from the source to the current square have been processed at least once. :-)
Average Runtime: O(V*8) => O(V). Where V is the number of square inside the grid
If the content of the grid changes often, use methods described in previous answers, that is bread-first search.
If the content of your grid does changes seldom AND Manhattan distance is ok for your application, my advise is to compute the distance transform of the binarized grid (0 if empty, 1 else) distance transform is very simple for Manhattan distance, way more complicated for euclidian distance). This step can be done at a cost of 2*N*M (number of elements of the grid). Then, for each request, you can visit the neighborhood in a very straightforward manner, that is follow the path of min distance starting from the starting cell (like some gradient descent), it will stop at the nearest empty cell. May be really faster to search with this algorithm, as you don't look in the wrong way for an empty cell more than 1 cell far.