*3.2. Layer Two: Generate a Reference Line*

The preceding layer identifies a global route from the start point to the goal. This layer is focused on generating a curvature-continuous path that is close to the global route. The curvature-continuous path is denoted as a reference line, which is used to construct a Frenet frame for future usage. The principle for generating a reference line is as follows.

The global route Wgr = - (x gr *<sup>i</sup>* , ygr *<sup>i</sup>* )|*<sup>i</sup>* <sup>=</sup> 1, . . . , N derived in layer one is resampled as NFE equidistant waypoints Wgrrs = - (x grrs *<sup>i</sup>* , ygrrs *<sup>i</sup>* )|*<sup>i</sup>* <sup>=</sup> 1, . . . , NFE . A reference line is generated by driving a virtual vehicle to track the waypoints. This process can be described as a trajectory planning-oriented OCP:

$$\begin{array}{l}\min f(\mathbf{z}(t), \mathbf{u}(t)),\\\text{s.t.} \ f(\mathbf{z}(t), \mathbf{u}(t)) = 0\\\quad \underline{\mathbf{z}} \le \mathbf{z}(t) \le \overline{\mathbf{z}},\\\quad \underline{\mathbf{u}} \le \mathbf{u}(t) \le \overline{\mathbf{u}}, \; t \in [0, t\_{\max}].\end{array} \tag{2}$$

In (2), t is the time index, **z**(*t*) denotes the ego vehicle's state profiles in the Cartesian frame, i.e., [*x*(*t*), *y*(*t*), *θ*(*t*), *v*(*t*), *a*(*t*), *φ*(*t*)]. Furthermore, (*x*(*t*), *y*(*t*)) refers to the location of the rear-axle midpoint of the ego vehicle, *θ*(*t*) is the orientation angle, *v*(*t*) is the longitudinal velocity, *a*(*t*) is the corresponding acceleration, and *φ*(*t*) is the steering angle. **u**(*t*) denotes the control profiles [*jerk*(*t*), *ω*(*t*)], wherein *jerk*(*t*) is the derivative of *a*(*t*), and *ω*(*t*) is the angular velocity of *φ*(*t*). All of the constraints in (2) are kinematic constraints, which are presented by the well-known bicycle model [47]:

$$\begin{array}{l} \frac{d\mathbf{x}(t)}{dt} = v(t) \cdot \cos\theta(t) \\ \frac{d\mathbf{y}(t)}{dt} = v(t) \cdot \sin\theta(t) \\ \frac{d\theta(t)}{dt} = \frac{v(t) \cdot \tan\phi(t)}{\mathbf{L}\_W} \\ \frac{d\mathbf{v}(t)}{dt} = a(t) \\ \frac{d\boldsymbol{\phi}(t)}{dt} = \boldsymbol{\omega}(t) \\ \frac{d\boldsymbol{\phi}(t)}{dt} = j\boldsymbol{\epsilon}(t) \end{array} \tag{3}$$

Herein, LW denotes the vehicle wheelbase (Figure 3). The boundary constraints **z** ≤ **z**(*t*) ≤ **z** and **u** ≤ **u**(*t*) ≤ **u** are defined as

$$\begin{array}{l} -\text{jerk}\_{\text{max}} \le jerk(t) \le \text{jerk}\_{\text{max}}, \text{ a}\_{\text{min}} \le a(t) \le \text{a}\_{\text{max}}, \ 0 \le v(t) \le \text{v}\_{\text{max}},\\ -\Omega\_{\text{max}} \le \omega(t) \le \Omega\_{\text{max}}, -\Phi\_{\text{max}} \le \phi(t) \le \Phi\_{\text{max}}, \ t \in [0, t\_{\text{max}}]. \end{array} \tag{4}$$

The cost function *J*(**z**(*t*), **u**(*t*)) is defined as:

$$J = \int\_{\tau=0}^{t\_{\text{max}}} \left\{ \left[ \mathbf{x}(\tau) - \mathbf{x}\_{\text{grrs}}(\tau) \right]^2 + \left[ y(\tau) - y\_{\text{grrs}}(\tau) \right]^2 \right\} \cdot \mathbf{d}\tau + \mathbf{w}\_{\text{u}} \cdot \int\_{\tau=0}^{t\_{\text{max}}} \left[ \dot{\mu}\mathbf{r} k^2(\tau) + \omega^2(\tau) \right] \cdot \mathbf{d}\tau,\tag{5}$$

wherein wu > 0 is a weighting parameter, and *x*grrs(*t*), *y*grrs(*t*) is a parametric trajectory formed by linearly connecting the NFE waypoints - (x grrs *<sup>i</sup>* , ygrrs *<sup>i</sup>* )|*<sup>i</sup>* <sup>=</sup> 1, . . . , NFE in a sequence.

**Figure 3.** Schematics for vehicle kinematics and geometrics.

Compared with (1), OCP (2) does not contain two-point constraints or collisionavoidance constraints. Thus, the resultant trajectory may not connect the start point to the goal and may not guarantee collision avoidance despite being kinematically feasible. At this point, it should be clarified that the resultant reference line is only used to construct a Frenet frame; the concerns about collision avoidance, occlusion avoidance, etc., will be handled in a subsequent layer based on the constructed Frenet frame.

OCP (2) is solved numerically, which involves discretizing the OCP into an NLP problem and then solving it via a gradient-based NLP solver, such as the interior-point method (IPM) [48,49]. The solution to the NLP problem is a vector of waypoints together with the corresponding state/control profiles in their discretized forms. A reference line is formed by connecting the resultant waypoints smoothly via spline interpolation.

#### *3.3. Layer Three: Search for a DP Path in the Frenet Frame*

This layer is focused on generating a coarse path within the Frenet frame with collision avoidance, occlusion awareness, travel efficiency, and path smoothness considered.

The first step is to map the start point and goal to the reference line so that their Frenet coordinate values are identified as (sstart, lstart) and (send, lend), respectively (Figure 4). The second step is to generate (NS + 1) equidistant skeleton points along the reference line ranging from (sstart, 0) to (send, 0). A normal line is drawn along each skeleton point, which is orthogonal to the reference line. Along each normal line, NL candidate grids are sampled (Figure 4), which range in an interval around the skeleton point. For the nominal line passing through the last skeleton point located at (send, 0), NL is set to 1 and the only candidate grid left is specified as the goal (send, lend).

The planning task in layer three is to select one and only one candidate grid along each of the normal lines such that the sequentially connected candidate grids are collision free, occlusion minimized, short, and smooth. DP is adopted to find the optimal choice for the candidate grid along each normal line [50]. Compared to enumeration, DP reduces the search complexity from *O N*L*N*<sup>S</sup> to *O <sup>N</sup>*<sup>S</sup> · *<sup>N</sup>*<sup>2</sup> L , and thus promises to find the optimum in a graph consisting of candidate grids. The brief principle of the DP search-based path planning method in layer three is presented as follows.

**Figure 4.** Schematics for a graph of sampled grids in DP search.

In Algorithm 1, each candidate grid is regarded as a node. *InitializeNodes*() is used to initialize the cost of each node as +∞. *TraceBack*(opti\_id) is used to identify a sequence of nodes from Node(NS, 1), Node(NS − 1, opti\_id), to its ancestors until Node(0, 1). The node sequence, if output in inverse order, forms a coarse path within the Frenet frame. *MeasureCost*(Node1, Node2) measures the cost of the path segment from Node1 to Node2. We define the cost function as a weighted sum of collision cost *J*collision, travel efficiency cost *J*efficiency, smoothness cost *J*smoothness, and positioning-related cost *J*positioning. The collision cost penalizes the case that the ego vehicle collides with the surrounding cargoes when driving along the concerned path segment. *J*collision is set to a large value (e.g., 1020) if a collision occurs, otherwise, *J*collision is set to 0. The efficiency cost *J*efficiency is written as the length of the concerned path segment because this term encourages the ego vehicle to travel across a short distance. Suppose the parent of Node1 is Node0, *J*smoothness is defined as |(Node0.config − Node1.config) × (Node1.config − Node2.config)|. Intuitively speaking, the smoothness cost *J*smoothness penalizes the case that the heading direction changes from Node0, Node1, to Node2. *J*positioning penalizes the case that the ego vehicle stays in the positioning-poor regions for a long distance. Furthermore, Nsample waypoints - (s wp *<sup>i</sup>* , lwp *<sup>i</sup>* ) *<sup>i</sup>* <sup>=</sup> 1, . . . , *<sup>N</sup>*sample are evenly sampled along the concerned line segment from Node1 to Node2. Regarding the *i*th sampled waypoint (s wp *<sup>i</sup>* , l wp *<sup>i</sup>* ), the corresponding coordinate value in the Cartesian frame is identified as (x wp *<sup>i</sup>* , y wp *<sup>i</sup>* ) via frame conversion. Suppose the infrared emitter is installed at the height of h onto the ego vehicle, one may draw a line from the 3D point (x wp *<sup>i</sup>* , y wp *<sup>i</sup>* , h) to each infrared receiver and then check if the line is occluded by cargoes in the warehouse. If there is no occlusion, then the receiver is regarded as valid (Figure 5). If the total number of valid infrared receivers is larger than three, then the concerned waypoint (s wp *<sup>i</sup>* , l wp *<sup>i</sup>* ) is regarded as valid. *J*positioning measures the rate of valid waypoints along the concerned line segment from Node1 to Node2.

## *3.4. Layer Four: Optimize a Curvature-Continuous Path*

The preceding layer helps to identify a collision-free and occlusion-aware path, which consists of (NS + 1) waypoints. Since NS is not large, the derived path is quite coarse. This section is focused on how to refine the coarse path via numerical optimization within the Frenet frame. In this work, path refinement is performed via Baidu Apollo EM planner [20], which involves implementing path-velocity decomposition in an iterative loop before an optimum (rather than sub-optimum) is finally derived. Since there are only static

obstacles, the EM planner is degraded as a run-once path planning method, the details of which are given as follows.


**Input**: Reference line, scenario layout, location of cargoes; **Output**: A path Λ = - (s dp *<sup>i</sup>* , ldp *<sup>i</sup>* )|*i* = 0, . . . , NS ; 1. *InitializeNodes();* 2. Set Node(0, 1).config = (*x*start, *y*start); 3. **For each** *j* ∈ {1, . . . , NL}, **do** 4. Set Node(1, *j*).parent = Node(0, 1); 5. Identify Node(1, *j*).config; 6. Set Node(1, *j*).cost = *MeasureCost*(Node(0, 1), Node(1, *j*)); 7. **End for** 8. **For each** *i* ∈ {1, . . . , NS − 2}, **do** 9. **For each** *j* ∈ {1, . . . , NL}, **do** 10. **For each** *k* ∈ {1, . . . , NL}, **do** 11. Identify Node(*i* + 1, *k*).config; 12. cost\_candidate = *MeasureCost*(Node(*i*, *j*), Node(*i* + 1, *k*)); 13. **If** Node(*i* + 1, *k*).cost > Node(*i*, *j*).cost + cos t\_candidate, **then** 14. Node(*i* + 1, *k*).parent = Node(*i*, *j*); 15. Node(*i* + 1, *k*).cos t = cos t\_candidate; 16. **End if** 17. **End for** 18. **End for** 19. **End for** 20. opti\_id = arg min *j*=1,...,NL (Node(NS − 1, *j*).cost + *MeasureCost*(Node(NS − 1, *j*), Node(NS, 1))); 21. Λ = *TraceBack*(opti\_id); 22. **Return**.

**Figure 5.** Schematics for the infrared receiver validation check. Five blue infrared beams together with corresponding receivers are valid because they are not occluded by the cargoes.

The first step is to identify the left and right bounds that surround the coarse path derived in layer three. As depicted in Figure 6, the left and right bounds are determined using an incremental check. The identified left and right bounds are denoted as functions of *s*, namely, *ub*(*s*) and *lb*(*s*). The optimization-based path planning task involves identifying a function *l*(*s*) between the left and right bounds subject to kinematic constraints and collision-avoidance constraints. In our concerned task, the decision variables include *l*(*s*), *dl*(*s*), *ddl*(*s*), and *dddl*(*s*), which obey the following kinematic constraints:

**Figure 6.** Schematics for the construction of left/right bounds in EM planner.

Herein, dlmax, ddlmax, and dddlmax are parameters that determine how fast *l* changes with *s*, and thus are related to path smoothness. Empirically, the bounding parameters in (6) should not be set strictly, otherwise the vehicle kinematics easily become in conflict with the collision-avoidance constraints that will be introduced later. At this point, we believe that the path smoothness can be enhanced via the cost function without suffering from the infeasibility risk [23].

The two-point boundary constraints are written as

$$\begin{array}{l} l(\mathbf{s}\_{\text{start}}) = \mathbf{L}\_{\text{start}} \cdot dl(\mathbf{s}\_{\text{start}}) = \mathbf{DL}\_{\text{start}} \cdot ddl(\mathbf{s}\_{\text{start}}) = 0, \, dddl(\mathbf{s}\_{\text{start}}) = 0, \\\ l(\mathbf{s}\_{\text{end}}) = \mathbf{L}\_{\text{end}} \cdot dl(\mathbf{s}\_{\text{end}}) = \mathbf{DL}\_{\text{end}} \cdot ddl(\mathbf{s}\_{\text{end}}) = 0, \, dddl(\mathbf{s}\_{\text{end}}) = 0. \end{array} \tag{7}$$

Herein, the parameters Lstart, DLstart, Lend, and DLend reflect the assigned initial and goal configurations. Particularly, DLstart and DLend are related to the vehicle orientation angles at *s* = sstart and send, respectively.

Regarding the collision-avoidance constraints, the vehicle body should not collide with *ub*(*s*) or *lb*(*s*). Setting the vehicle body as rectangular is too complex, and thus we use a series of same-radius circles centered along the longitudinal axle of the ego vehicle to cover the rectangular vehicle body (Figure 7), and then require that each circle lies between *lb*(*s*) and *ub*(*s*). For a circle biased from the vehicle's rear axle by *η*, the collision-avoidance constraints are defined as

$$\begin{array}{l} \eta \cdot \tan \theta\_{\sf s} + l(s) + 0.5 \cdot \mathcal{L}\_{\sf B} \le \imath \vartheta(s + \eta),\\ \eta \cdot \tan \theta\_{\sf s} + l(s) - 0.5 \cdot \mathcal{L}\_{\sf B} \ge \imath l \nu(s + \eta). \end{array} \tag{8}$$

(6)

The complete collision-avoidance constraints are formed by imposing (8) for any *η* ∈ [−LR cos *θ*s, (LW + LF) cos *θ*s]. Herein, *θ*s(*s*) denotes the vehicle's orientation angle within the Frenet frame. Inherently, *θ*s(s) stands for the difference between the ego vehicle's heading direction and the tangent direction along the reference line at *s*. |*θ*s(*s*)| is small because the ego vehicle's heading direction, in most cases, is not much biased from the reference line. Thus, we have

$$\begin{array}{c} \tan \theta\_{\mathfrak{s}} \equiv \frac{\mathrm{d}l(s)}{\mathrm{d}s} \equiv \mathrm{d}l(s),\\ \cos \theta\_{\mathfrak{s}} \approx 1. \end{array} \tag{9}$$

This yields the following constraints:

$$\begin{array}{l} \eta \cdot dl(s) + l(s) + 0.5 \cdot \mathcal{L}\_{\mathcal{B}} \le \iota b(s + \eta), \\\eta \cdot dl(s) + l(s) - 0.5 \cdot \mathcal{L}\_{\mathcal{B}} \ge l b(s + \eta), \\\forall \eta \in [-\mathcal{L}\_{\mathcal{R}}, \mathcal{L}\_{\mathcal{W}} + \mathcal{L}\_{\mathcal{F}}]. \end{array} \tag{10}$$

The cost function is defined as

$$J = \begin{array}{c} \mathbf{w}\_1 \cdot \int\_{s = s\_{\text{start}}}^{s\_{\text{end}}} \left( l(\mathbf{s}) - l\_{\text{DP}}(\mathbf{s}) \right)^2 \text{ds} + \mathbf{w}\_2 \cdot \int\_{s = s\_{\text{start}}}^{s\_{\text{end}}} dl^2(\mathbf{s}) \, \text{ds} +\\ \mathbf{w}\_3 \cdot \int\_{s = s\_{\text{start}}}^{s\_{\text{end}}} ddl^2(\mathbf{s}) \, \text{ds} + \mathbf{w}\_4 \cdot \int\_{s = s\_{\text{start}}}^{s\_{\text{end}}} dddl^2(\mathbf{s}) \, \text{ds}, \end{array} \tag{11}$$

wherein w1, w2, w3, and w4 are weighting parameters, and *l*DP(*s*) denotes the coarse path derived by DP in layer three. An OCP is formed by combining (6), (7), (10), and (11). The discretized version of this OCP is a QP, which is easily solved using a QP solver, such as osqp [51] and qpOASES [52]. The resultant path, after being converted back to the Cartesian frame, may be infeasible if its curvature exceeds the allowable bounds. The infeasibility is inevitable because the vehicle kinematics cannot be accurately modeled within the Frenet frame [44]. As a remedy for this, we check the resultant path for violations of curvature limits in the Cartesian frame; if an infeasible solution is derived, w1 is set smaller before the QP problem is solved again. This process continues until a curvature-feasible path is derived.

**Figure 7.** Schematics for the formulation of collision-avoidance constraints in EM planner (zoom in to see more clearly).
