Search-Based Path Planning among Movable Obstacles

PAMO was formulated in different ways. When representing the workspace and obstacles using a grid, PushPush and Push-1 problems are NP-hard [5]. With polygonal obstacles, navigation among movable obstacles (NAMO) were proposed and shown to be NP-hard [20]. NAMO was studied in environments that are known [17], partially known [12], and unknown [8], with search-based [17], sampling-based [18], and learning-based [21] methods. This paper differs from these work by considering multi-objective and resource-constrained formulations of PAMO in a fully known grid, and developing planners with completeness and solution optimality guarantees.

Besides, box-pushing games, such as Sokoban, solve puzzles where an agent moves boxes (i.e., objects) from their initial to goal positions in a grid by push. These games were NP-hard [7] and has been addressed by heuristic search [10], Monte-Carlo Tree Search [4], Q-learning [19], to name a few. PAMO differs from these games since it seeks a start-goal path for the robot while these games move the boxes without imposing a goal location on the agent.

Other related work includes manipulation of multiple objects [16, 13], objects rearrangement [11], etc, where various more complicated interaction between the robot and the objects are allowed other than solely push.

Multi-objective search and resource-constrained search are two closely related topics, and a fundamental challenge in these problems is to quickly check paths for dominance, i.e., compare the vector cost of these paths [9, 15, 14, 3]. This paper leverages these search algorithms [15, 14] to solve the formulated PAMO problems.

PAMO is the problem name while PAMO* (with *) is the method name. Let $m=|V_{mo}|$ denote the number of objects, and let position vector $p=(v_{1},v_{2},\cdots,v_{m})$ denote the cells occupied by all objects. Let $s=(v,p)$ denote a search state, where $v\in V_{free}$ is the cell occupied by the robot, and $p$ is an aforementioned position vector. In other words, the state space of the search is $S=G\times G\times\cdots\times G=G^{m+1}$, which encodes the locations of both the robot and all objects.

Let $s_{o}$ denote the initial state, where the robot is at $v_{s}$ and all objects are at their original positions. Different from conventional A*, where the search only needs to record one optimal path $\pi$ (via parent pointers) from $s_{o}$ to any state $s$, PAMO* have to store multiple non-dominated paths from $s_{o}$ to $s$ and differentiate between these paths. To do so, let $l=(s,\vec{g})$ denote a label, where $s$ is a state as aforementioned and $\vec{g}$ is the cost vector of a path from $s_{o}$ to $s$. Labels are compared based on their cost vectors and two labels are non-dominated by each other if their cost vectors are non-dominated by each other. We use $s(l),\vec{g}(l)$ to denote the state and the cost vector contained in $l$ respectively. We use $v(l)$ to denote the cell occupied by the robot in state $s(l)$.

Let $\mathcal{F}(s)$ denote the frontier set at state $s$, which is a set of labels, where any pair of labels in $\mathcal{F}(s)$ are non-dominated by each other. Similar to A*, let $\vec{g}(s)$ denote the cost-to-come, i.e., the cost vector of the path from $s_{o}$ to $s$ and let $\vec{h}(s)$ denote the heuristic vector of state $s$ that estimates the cost-to-go. Let $\vec{f}(s):=\vec{g}(s)+\vec{h}(s)$ denote the $f$-vector of state $s$, and let $\mathcal{O}$ denote an open list of states, which is a priority queue that prioritize states based on their $f$-vectors from the minimum to maximum in lexicographic order. Let $L^{*}$ denote the set of labels representing solution paths that are found during the search.

PAMO* (Alg. 1) begins by creating the initial label $l_{o}=(s_{o},\vec{g}=\vec{0})$, which is added to $\mathcal{O}$. The frontier set is initialized as an empty set for any state. In a search iteration (Line 4-18), a label $l$ with the lexicographically minimum $f$-vector in $\mathcal{O}$ is popped, and is then checked for pruning (Line 6), which is elaborated later. If $l$ is not pruned, $l$ is used to update the frontier set $\mathcal{F}(s)$, where $l$ is compared against any existing label $l^{\prime}\in\mathcal{F}(s)$. If $l^{\prime}$ is dominated by $l$, $l^{\prime}$ is removed from $\mathcal{F}(s)$. Then, $l$ is added to $\mathcal{F}(s)$. As a result, $\mathcal{F}(s)$ always contains non-dominated labels.

After updating the frontier, PAMO* checks if $l$ reaches the goal (i.e., $v(l)=v_{g}$). When solving RC-PAMO where only one optimal path is required, PAMO* terminates if $v(l)=v_{g}$ and the path corresponding to label $l$ is guaranteed to be optimal. When solving MO-PAMO where all cost-unique Pareto-optimal paths are required, PAMO* skips the rest of the current search iteration and continues with the next search iteration if $v(l)=v_{g}$. When solving MO-PAMO, PAMO* terminates when $\mathcal{O}$ depletes, i.e., all states in $\mathcal{O}$ are popped and are either expanded or pruned.

If label $l$ does not reach the goal, then $l$ is expanded by finding successor states of $s(l)$ (Line 13). Specifically, the procedure GetSuccessor returns the set of all reachable states from the given state $s(l)$, where each of the successor states update both the cell of the robot, and the cells of the objects when any object is pushed by the robot. Then, for each of the successor state $s^{\prime}$, a corresponding label $l^{\prime}=(s^{\prime},\vec{g}^{\prime})$ is created, where $\vec{g}^{\prime}=\vec{g}(l)+\text{GetCost}(s,s^{\prime})$ and the GetCost function returns either a cost vector $(1,0)$ if the robot moves without pushing any object, or $(1,1)$ if the robot moves while pushing an object. Afterwards, the new label $l^{\prime}$ is checked for pruning (Line 15). If $l^{\prime}$ is not pruned, the $f$-vector and the parent pointer related to $l^{\prime}$ are updated and $l^{\prime}$ is added to $\mathcal{O}$ for future search.

At the end of the search, each label in $L^{*}$ represents a solution path, which is reconstructed by iteratively tracking the parent pointers, and the solution path(s) are returned.

Let RC-PAMO* (and MO-PAMO*) denote the version of PAMO* when using Alg. 1 to solve RC-PAMO problem (and MO-PAMO problem, respectively). The completeness and solution optimality of PAMO* are inherited from EMOA* [15] and ERCA* [14]. In particular, RC-PAMO* (and MO-PAMO*) can be regarded as applying ERCA* (and EMOA*) onto the new state space $S$ with the new way of successor generation. EMOA* is guaranteed to be complete and able to find all cost-unique Pareto-optimal solutions for a given graph with vector edge cost [15]. ERCA* is guaranteed to be complete and can find a min-cost solution subject to resource limits in a given graph where each edge incurs cost and resource consumption [14]. As a result, we have the following theorems.

We consider a first order unicycle model, where the robot pose is $\xi=(x,y,\theta)\in SE(2)$ and control is $u=(v,\omega)\in\mathcal{U}$ where $v$ is the linear velocity and $\omega$ is the angular velocity, and $(v,\omega)$ are subject to control limits. The robot satisfies the system dynamics $\dot{\xi}=(\dot{x},\dot{y},\dot{\theta})=f_{dyn}(\xi,u)=(v\sin(\theta),v\cos(\theta),\omega)$. The workspace $\mathcal{W}$ is a bounded 2D Euclidean space with a set of static rectangle obstacles $\mathcal{W}_{obs}$. In the free space $\mathcal{W}_{free}=\mathcal{W}\backslash\mathcal{W}_{obs}$, there are rectangle objects. A search state now includes both the pose of the robot and the poses of all objects.

The robot-object interaction is described by a black-box function $f_{sim}$ which takes (i) a state (i.e., the poses of the robot and the objects), (iii) a control of the robot, and (iv) a small amount of time $dt$, conducts a forward simulation and returns the ending poses of the robot and the objects. Here, $f_{sim}$ only simulates the poses of the robot and the objects, and ignores the higher order terms (e.g. velocities).

H-PAMO* is similar to PAMO* with the following differences. First, to simplify the presentation, H-PAMO* is developed as a single-objective algorithm, where all actions (either move or push) incur an action time, and H-PAMO* seeks to minimize the arrival time at the goal. As a result, all cost vectors $\vec{g},\vec{h},\vec{f}$ (Lines 1, 14, 17) becomes scalar values and the transition cost $\vec{c}$ is now a scalar representing the action time from one state to another. Second, H-PAMO* plans in a continuous space, there is no notion of graphs or cells. To compare and prune paths (FrontierCheck on Lines 6, 15 and UpdateFrontier on Line 8), H-PAMO* uniformly discretizes the workspace into mutually exclusive cells of size $\Delta x\times\Delta y\times\Delta\theta$, and every pose belongs to a unique cell. The state space $S$ is thus discretized into a grid $S_{g}$, where each cell is of size $(\Delta x\times\Delta y\times\Delta\theta)^{|V_{mo}|+1}$. When two paths end with states that belong to the same cell in $S_{g}$, H-PAMO* only stores the cheaper path and prunes the other. Here, $S_{g}$ is never created explicitly and any cell in $S_{g}$ is only created when a state in that cell is generated. Third, since H-PAMO* optimizes a single-objective, the arrival time, and there is no limit on the number of push actions, there is no SolutionCheck on Line 6, 15 in H-PAMO* any more. Fourth, to get successors, H-PAMO* consider a finite set of motion primitives, which are generated by sampling controls from the control space $\mathcal{U}$ and running forward simulation for each of the sampled control for a short amount of time $dt$. When generating a successor, the interaction between the robot and the objects needs to be considered. In our implementation, we use Runge-Kutta 4th order method to integrate the $f_{dyn}$ to simulate the robot motion given the control, and use Box2D [1] to implement $f_{sim}$ to predict the motion of the objects when pushed by the robot. Finally, H-PAMO* terminates when the search finds a path that reaches the goal pose within a given tolerance $\epsilon_{goal}\in\mathbb{R}^{3}$.

As shown in Fig. 2, in the smaller Empty map, the increase in the number of movable obstacles slightly slows down the planning for both MO-PAMO* and RC-PAMO*. In the larger maps with much more movable obstacles (Fig. 4), the planning is slowed down obviously, where MO-PAMO* times out in many instances before finding the entire Pareto-optimal front, while RC-PAMO* is still able to solve most of the instances. This is expected since finding the entire Pareto-optimal front is usually much harder than finding a single optimal solution [15, 14].

Fig. 3 and 5 shows the number of expansions of MO-PAMO* and RC-PAMO*. Similar trends as the runtime can be observed for number of expansions. Besides, we observe from Fig. 4 and 5 that, in Random 32x32 and Random 64x64, as the map size and $|V_{mo}|$ increases, the runtime for RC-PAMO* increases while the number of expansion decreases. The reason is, with a larger $|V_{mo}|$, each state has to encode the position of more movable obstacles and the processing time in each expansion increases correspondingly.

When considering the theoretic size of the entire state space $S$, the number of expansion is usually much smaller than the size of $S$. For example, in Empty 8x8 with $|V_{mo}|=12$, the size of the state space is $|S|=(8\times 8)^{(1+12)}\approx 10^{23}$ while the number of expansions is usually less than $10^{3}$ for MO-PAMO* and $10^{2}$ for RC-PAMO*. It indicates that, guided by the heuristic, PAMO* only need to explore a small fraction of the state space to find optimal solutions.

Finally, we show the number of solutions found by MO-PAMO* in Fig. 6. Within the time limit, the planner finds 1 or 2 solutions as shown by the average and median number, while for some instances, the planner finds 3 or 4 solutions. We pick an instance from Room 32x32 and shows the two different paths found by MO-PAMO*. Fig. 6(a) shows a longer path with no push action required, while Fig. 6(b) shows a shorter path with 7 push actions. These results can help determine which path to take based on the specific robotic platform where the push action is expensive or not.

As shown in Fig. 7, we test H-PAMO* in three maps of sizes (a) 74x127 in an office, (b) 24x36 in part of an office, and (c) 90x102 in a warehouse, with rectangular objects of different sizes. The robot is a 1x1 square. The size of cells in $S_{g}$ for pruning is set to $(0.2,0.2,0.4)$ and the set of controls used are $(v,w)\in$ $\{(1.0,0.5)$, $(1.0,-0.5)$, $(1.0,0)$, $(-0.2,0)$, $(1.0,0.25)$, $(1.0,-0.25)$, $(0,0.5)$, $(0,-0.5)\}$. Fig. 7 shows the solution paths, the initial and ending pose of all objects. On all three instances, H-PAMO* terminates within 10 seconds and finds paths of length (a) 117.10 (b) 28.89 (c) 106.75 respectively. Fig. 8 shows the simulated motion of the robot for Fig. 7(a) in Unity3D.