WAREHOUSE SEARCH
boxes to deliver
target
out of bounds (shaded)
Our warehouse just purchased a new humanoid robot! We want to program the humanoid robot to move boxes to a specified delivery location. Surprisingly the robot is able to walk and manipulate objects perfectly. All we need to do is implement an efficient path planning algorithm so the robot knows where to go.
Duration
mar 2023
Skills
python, search algorithm a*
Problem Setup
To simplify the problem, we discretized the map of the warehouse into a 2D grid. We know where the robot, box, obstacles, and delivery location are within the warehouse. The robot is able to move in cardinal directions or diagonally. We need to come up with a set of movements for the robot to execute in order to navigate to the box then navigate to the delivery location.
Proposed Solution
An ideal solution to this problem is A*. A* combines elements from Dijkstra’s algorithm and greedy best-first search. Specifically, it is guaranteed to find the shortest path and is compute efficient. A* relies on discretizing the space, so this is only possible because of the problem simplification we made.
A* Formulation
A* works by starting at a node in the grid, then iteratively expanding to nearby nodes. Each node is evaluated and the value of each searched node is stored. The search continues by evaluating the reachable nodes from the lowest value unexpanded node in the grid.
Evaluation Function
The A* evaluation function can be written as:
\[ f(n) = g(n) + h(n) \]
\(f(n)\) is the total cost of the node
\(g(n)\) is the cost to traverse from the start to the node
\(h(n)\) is a heuristic that estimates the cost form the node to the goal
In our warehouse search problem the cost to move vertically or horizontally is 2 and the cost to move diagonally is 3. Our cost function is
\[ g(n) = g(n-1) + C(n-1, n) \]
\(g(n-1)\) is the cost of from the start to the previous node
\(c(n-1, n)\) is the cost to move from the previous node to the node. This would be 2 for horizontal or vertical move and 3 for diagonal moves
For the heuristic function we use the weighted Chebyshev Distance because the movement costs are non-uniform \(h(n) = 2 * max(\lvert x_{goal} - x \rvert , \lvert y_{goal} - y \rvert) + min(\lvert x_{goal} - x\rvert , \lvert y_{goal}- y \rvert)\)
Backtrack to find the optimal path
During the A* search, for each node we must store the previous node that was expanded (parent node). Using the tree of parent to child nodes, we can start at the goal node and recursively find the parent node until we get back to the start node. If we reverse this path we get the optimal path from the start to the goal.
Results
This implementation expands to variable node costs by modifying \(g(n-1)\)
