*1.3. Organization*

The remainder of this paper is organized as follows. Section 2 briefly presents the concerned path planning task. Section 3 introduces our occlusion-aware path planner. Section 4 provides the simulation results and discusses them. Finally, Section 5 concludes the study.

#### **2. Problem Statement**

The purpose of this work is to generate a kinematically feasible and collision-free path for a car-like robot in a warehouse, such that the impact of infrared positioning inaccuracy can be reduced. The path planning task is described as the following OCP:

$$\begin{array}{l} \min \int (\mathbf{z}(s), \mathbf{u}(s)),\\ \text{s.t.} \, f(\mathbf{z}(s), \mathbf{u}(s)) = 0, \, s \in [0, S\_{\text{max}}];\\ \quad \underline{\mathbf{z}} \le \mathbf{z}(s) \le \overline{\mathbf{z}}, \, \underline{\mathbf{u}} \le \mathbf{u}(s) \le \overline{\mathbf{u}}, \, s \in [0, S\_{\text{max}}];\\ \quad \mathbf{z}(0) = \mathbf{z}\_{\text{init}}, \, \mathbf{u}(0) = \mathbf{u}\_{\text{init}};\\ \quad \mathbf{z}(S\_{\text{max}}) = \mathbf{z}\_{\text{end}}, \, \mathbf{u}(S\_{\text{max}}) = \mathbf{u}\_{\text{end}};\\ \quad fp(\mathbf{z}(s)) \subset \chi\_{\text{free}}, s \in [0, S\_{\text{max}}]. \end{array} \tag{1}$$

Herein, variable *s* stands for the distance that the ego vehicle has to travel; variable *S*max denotes the traveled distance when the ego vehicle reaches the destination, thus *S*max may not be known *a priori*; *J*(*z*(*s*), *u*(*s*)) denotes the cost function w.r.t. travel efficiency, path smoothness, and positioning quality; **z** denotes the state profiles; **u** represents the control profiles; *f*(**z**(*s*), **u**(*s*)) = 0 denotes the kinematic constraints written in the form of ordinary differential equations; **z** ≤ **z**(*s*) ≤ **z** and **u** ≤ **u**(*s*) ≤ **u** denote the kinematic constraints written as algebraic box constraints; **z**(0) = **z**init, **u**(0) = **u**init, **z**(*S*max) = **z**end, and **u**(*S*max) = **u**end denote the two-point boundary constraints; χobs denotes the partial workspace occupied by obstacles; suppose that χ denotes the entire workspace, then χfree ≡ χ/χobs denotes the free space drivable for the ego vehicle; *f p*(·) is a mapping from the vehicle state *z* to its footprint, thus *f p*(**z**(*s*)) ⊂ χfree represents the collision-avoidance constraints [45].

In addition, one may define the poor positioning regions in the workspace as χocclusion, wherein the ego vehicle cannot touch enough infrared receivers. Ideally, one would expect the ego vehicle to always keep free from poor positioning and collisions, i.e., *f p*(**z**(*s*)) ⊂ χ/(χobs ∪ χocclusion) for any *s* ∈ [0, *S*max]. However, this condition is too harsh when the infrared beams are seriously blocked by the cargoes. As depicted in Figure 1, no kinematically feasible paths exist in χ/(χobs ∪ χocclusion). Empirically, the temporary loss of positioning accuracy is not a serious problem because the inertial measurement units (IMUs) equipped onboard can accurately estimate the vehicle's configuration within a short period. Therefore, (1) allows the ego vehicle to travel in χocclusion, but penalizes traveling in χocclusion for a long distance using the cost function *J*.

**Figure 1.** A warehouse workspace with poor positioning regions. Note that there are no kinematically feasible paths if the ego vehicle follows the global route because the goal lies in the poor positioning region.

#### **3. A Four-Layer Path Planning Method**

Our proposed path planner consists of four layers. In layer one, an A\* path is searched for in the 2D grid map to coarsely connect the initial and goal points. The A\* path is deployed to determine the homotopy class. In layer two, a curvature-continuous reference line is planned close to the A\* path via numerical optima control. The reference line is deployed to construct a Frenet frame. In layer three, a coarse occlusion-aware path is searched for within the Frenet frame using DP. In layer four, a curvature-continuous path is optimized using QP. The overall architecture is shown in Figure 2.

**Figure 2.** Overall framework of occlusion-aware path planning method.

#### *3.1. Layer One: Search an A\* Path*

As a preliminary step, the homotopy class needs to be identified, which decides how the ego vehicle bypasses each of the obstacles (i.e., cargoes) from the start point to the goal. This work uses the A\* algorithm to find a coarse path, given that it explicitly reflects the determined homotopy class. Concretely, an occupancy grid map is formed based on the warehouse layout and cargo locations. Dilating the occupancy grid map by L renders a dilated map. In this work, L is set to the half-width of the ego vehicle. A 2D path is searched for in the dilated map via the A\* algorithm [46], which coarsely connects the assigned starting and goal points. The output of layer one is a path presented in the form of N waypoints stored in a set, i.e., Wgr = - (x gr *<sup>i</sup>* , ygr *<sup>i</sup>* )|*<sup>i</sup>* <sup>=</sup> 1, . . . , N .
