Occlusion-Aware Path Planning to Promote Infrared Positioning Accuracy for Autonomous Driving in a Warehouse

Abstract

Infrared positioning is a critical module in an indoor autonomous vehicle platform. In an infrared positioning system, the ego vehicle is equipped with an infrared emitter while the infrared receivers are fixed onto the ceiling. The infrared positioning result is accurate only when the number of valid infrared receivers is more than three. An infrared receiver easily becomes invalid if it does not receive light from the infrared emitter due to indoor occlusions. This study proposes an occlusion-aware path planner that enables an autonomous vehicle to navigate toward the occlusion-free part of the drivable area. The planner consists of four layers. In layer one, a homotopic A* path is searched for in the 2D grid map to roughly connect the initial and goal points. In layer two, a curvature-continuous reference line is planned close to the A* path using numerical optimal control. In layer three, a Frenet frame is constructed along the reference line, followed by a search for an occlusion-aware path within that frame via dynamic programming. In layer four, a curvature-continuous path is optimized via quadratic programming within the Frenet frame. A path planned within the Frenet frame may violate the curvature bounds in a real-world Cartesian frame; thus, layer four is implemented through trial and error. Simulation results in CarSim software show that the derived paths reduce the poor positioning risk and are easily tracked by a controller.

1. Introduction

Indoor positioning is an important area of development with wide applications in surveillance, human motion analysis, logistics, and entertainment [1,2,3,4,5]. As one of the most well-known indoor positioning approaches, infrared positioning is characterized by low energy consumption and high precision [6,7,8]. In an infrared positioning system, an infrared emitter is installed on a movable target that is required to be localized, and infrared receivers are fixed onto the ceiling. The infrared positioning result is accurate only when the number of valid infrared receivers is more than three. An infrared receiver becomes invalid if it does not receive the light originating from the infrared emitter due to indoor occlusions.

Automated guided vehicles (AGVs) are commonly used in warehouses for cargo delivery [9,10]. However, AGV positioning results easily become inaccurate when cargoes in a warehouse occlude the light beams originating from the emitter installed on an AGV. Instead of improving the positioning of the sensors, the current work considers the planning of occlusion-aware paths that would enable an AGV to drive in the occlusion-free part of the drivable area in a warehouse.

If the cargoes in a warehouse are permanently fixed, then the poor positioning regions may be estimated a priori and regarded as static obstacles in an AGV path planning scheme. In most cases, however, cargo placement is always changing, resulting in unfixed poor positioning regions. Thus, the concerned occlusion-aware path planner must work fast while guaranteeing its outputs are collision-free and kinematically feasible [11].

1.1. Related Work

The most prevalent path planners in robotics may be classified into sampling-, search-, optimization-, and learning-based methods.

Sampling-based planners generate candidate path/trajectory primitives and then connect selected ones to form a complete solution. Such primitives are typically formed using polynomials [12,13,14], state lattices [15,16,17], and closed-loop tracking [18].

Search-based methods divide a continuous solution space into nodes in a graph and then search for a link between the nodes in the graph. Typical searchers include dynamic programming (DP) [19,20], A* [16,21], and rapidly exploring random tree (RRT) [18,22].

An optimization-based planner formulates the concerned planning task as an optimal control problem (OCP) before solving the OCP numerically. Herein, solving an OCP numerically refers to discretizing it into a mathematical programming problem and solving it using a gradient-based optimizer. Typical mathematical programming problems include quadratic programming (QP) [20,23,24], quadratically constrained QP (QCQP) [25], nonlinear programming (NLP) [26,27,28], and unconstrained optimization problems [12,29]. Most gradient-based optimizers suitable for path planning only exhibit local optimization capabilities because global convergence requires a much longer runtime than one can afford. Given this property, the finally derived optimal path is close to the initial guess [30,31]. Therefore, identifying a good initial guess with global optimality contributes considerably toward finding a high-quality optimum. A sampling-/search-based planner is commonly used to provide a good initial guess [32,33].

A learning-based method generates vehicle motions based on trained data. Artificial neural networks [34], spline-constrained policy networks [35], and double deep Q-networks [36] are used in offline training. Reinforcement learning-based methods [37,38] are applied to online training.

The aforementioned four types of planners have their respective strengths and limitations. Sampling- and search-based methods do not handle the configuration space in its continuous form. Thus, they find suboptimal rather than optimal solutions. They even fail to find any feasible solution if the complexity of the planning scheme is beyond the sampling/search resolution level [23]. Increasing the resolution level is inapplicable when using a sampling-/search-based planner due to the curse of dimensionality. Despite their drawbacks, sampling-/search-based methods can efficiently describe the non-differentiable occlusion-related cost [39,40]. An optimization-based planner finds optimal paths in the continuous solution space; however, it has two typical limitations: (1) occlusion-related constraints/costs cannot be handled due to non-differentiability, and (2) the runtime is considerably longer than that of a sampling-/search-based method. Learning-based methods work fast online after a long offline training process, but they lack interpretability, and thus are rarely tested on real-world vehicles [41,42]. According to the above analysis, concluding that one type of planner fully outperforms another is difficult; instead, maximizing the advantages of each planner type while combining multiple types has been a common practice in this community.

Combining a sampling-/search-based planner with an optimization-based one renders a hierarchical planning framework; however, the optimization layer is time-consuming if the planning scheme is described in the Cartesian frame. At this point, the majority of path planners for on-road cruising scenarios describe the movements of an autonomous vehicle in the Frenet frame to enhance real-time performance. The Frenet frame, also known as the curvilinear frame, has been widely used to standardize the irregular trend of a road [43]. When using the Frenet frame, the ego vehicle is regarded as driving in a straight tunnel with left and right bounds. The Frenet frame helps convert an NLP problem into an easier form, making an optimization-based planner faster. However, the paths optimized in the Frenet frame may violate vehicle kinematic constraints because the conversions between the Cartesian and Frenet frames are neither unique nor uniform [44]. The situation worsens when the road is curvy, as it might be in our warehouse scenario. Therefore, a perfect solution that simultaneously considers kinematic feasibility, optimality, runtime efficiency, and occlusion-avoidance performance is not yet available.

1.2. Contributions

The current study aims to develop an occlusion-aware path planner for enhancing the indoor infrared positioning accuracy of an autonomous vehicle system. The planner is expected to be optimal and fast without violating fundamental restrictions, such as collision-avoidance and kinematic constraints. In particular, we adopt the first-search-then-optimize framework to combine search-based and optimization-based planners to find the global optimum. Both planners work within the Frenet frame, and thus time efficiency is enhanced. The optimizer is designed through trial and error; hence, the finally derived path is kinematically feasible within a real-world Cartesian frame.

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 J\left(z(s),u(s)\right),\\ \mathrm{s}.\mathrm{t}., f\left(z(s), u(s)\right)=0, s\in [0,S_{\max }];\\ \hspace{1em}\hspace{1em}\underset{\_}{z}\le z(s)\le \overline{z}, \underset{\_}{u}\le u(s)\le \overline{u}, s\in [0,S_{\max }];\\ \hspace{1em}\hspace{1em}z(0)=z_{\mathrm{init}}, u(0)=u_{\mathrm{init}};\\ \hspace{1em}\hspace{1em}z(S_{\max })=z_{\mathrm{end}}, u(S_{\max })=u_{\mathrm{end}};\\ \hspace{1em}\hspace{1em}fp(z(s))\subset {\mathsf{\chi }}_{\mathrm{free}}, s\in [0,S_{\max }].\end{array}$$

(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\left(z(s),u(s)\right)$ 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\left(z(s), u(s)\right)=0$ denotes the kinematic constraints written in the form of ordinary differential equations; $\underset{\_}{z}\le z(s)\le \overline{z}$ and $\underset{\_}{u}\le u(s)\le \overline{u}$ denote the kinematic constraints written as algebraic box constraints; $z(0)=z_{\mathrm{init}}, u(0)=u_{\mathrm{init}},$ $z(S_{\max })=z_{\mathrm{end}},$ and $u(S_{\max })=u_{\mathrm{end}}$ denote the two-point boundary constraints; ${\mathsf{\chi }}_{\mathrm{obs}}$ denotes the partial workspace occupied by obstacles; suppose that $\mathsf{\chi }$ denotes the entire workspace, then ${\mathsf{\chi }}_{\mathrm{free}}\equiv \mathsf{\chi }/{\mathsf{\chi }}_{\mathrm{obs}}$ denotes the free space drivable for the ego vehicle; $fp(\cdot )$ is a mapping from the vehicle state z to its footprint, thus $fp(z(s))\subset {\mathsf{\chi }}_{\mathrm{free}}$ represents the collision-avoidance constraints [45].

In addition, one may define the poor positioning regions in the workspace as ${\mathsf{\chi }}_{\mathrm{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., $fp\left(z(s)\right)\subset \mathsf{\chi }/({\mathsf{\chi }}_{\mathrm{obs}}\cup {\mathsf{\chi }}_{\mathrm{occlusion}})$ for any $s\in [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 $\mathsf{\chi }/({\mathsf{\chi }}_{\mathrm{obs}}\cup {\mathsf{\chi }}_{\mathrm{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 ${\mathsf{\chi }}_{\mathrm{occlusion}}$, but penalizes traveling in ${\mathsf{\chi }}_{\mathrm{occlusion}}$ for a long distance using the cost function J.

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.

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., ${\mathrm{W}}_{\mathrm{gr}}=\left\{({\mathrm{x}}_i^{\mathrm{gr}},{\mathrm{y}}_i^{\mathrm{gr}})|i=1,\dots ,\mathrm{N}\right\}$.

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 ${\mathrm{W}}_{\mathrm{gr}}=\left\{({\mathrm{x}}_i^{\mathrm{gr}},{\mathrm{y}}_i^{\mathrm{gr}})|i=1,\dots ,\mathrm{N}\right\}$ derived in layer one is resampled as N_FE equidistant waypoints ${\mathrm{W}}_{\mathrm{grrs}}=\left\{({\mathrm{x}}_i^{\mathrm{grrs}},{\mathrm{y}}_i^{\mathrm{grrs}})|i=1,\dots ,{\mathrm{N}}_{\mathrm{FE}}\right\}$. 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 J\left(z(t),u(t)\right),\\ \mathrm{s}.\mathrm{t}., f\left(z(t), u(t)\right)=0\\ \hspace{1em}\hspace{1em}\underset{\_}{z}\le z(t)\le \overline{z}, \\ \hspace{1em}\hspace{1em}\underset{\_}{u}\le u(t)\le \overline{u}, t\in [0,t_{\max }].\end{array}$$

(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),\theta (t),v(t),a(t),\varphi (t)]$. Furthermore, $\left(x(t),y(t)\right)$ refers to the location of the rear-axle midpoint of the ego vehicle, $\theta (t)$ is the orientation angle, $v(t)$ is the longitudinal velocity, $a(t)$ is the corresponding acceleration, and $\varphi (t)$ is the steering angle. $u(t)$ denotes the control profiles $[jerk(t),\omega (t)]$, wherein $jerk(t)$ is the derivative of $a(t)$, and $\omega (t)$ is the angular velocity of $\varphi (t).$ All of the constraints in (2) are kinematic constraints, which are presented by the well-known bicycle model [47]:

$$\begin{array}{l}\begin{array}{l}\frac{\mathrm{d}x(t)}{\mathrm{d}t}=v(t)\cdot \cos \theta (t)\\ \frac{\mathrm{d}y(t)}{\mathrm{d}t}=v(t)\cdot \sin \theta (t)\\ \frac{\mathrm{d}\theta (t)}{\mathrm{d}t}=\frac{v(t)\cdot \tan \varphi (t)}{{\mathrm{L}}_{\mathrm{W}}}\end{array}\\ \frac{\mathrm{d}v(t)}{\mathrm{d}t}=a(t)\\ \frac{\mathrm{d}\varphi (t)}{\mathrm{d}t}=\omega (t)\\ \frac{\mathrm{d}a(t)}{\mathrm{d}t}=jerk(t)\end{array}, t\in [0,t_{\max }].$$

(3)

Herein, L_W denotes the vehicle wheelbase (Figure 3). The boundary constraints $\underset{\_}{z}\le z(t)\le \overline{z}$ and $\underset{\_}{u}\le u(t)\le \overline{u}$ are defined as

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

(4)

The cost function $J\left(z(t),u(t)\right)$ is defined as:

$$J={\displaystyle {\int }_{\tau =0}^{t_{\max }}\left\{{\left[x(\tau )-x_{\mathrm{grrs}}(\tau )\right]}^2+{\left[y(\tau )-y_{\mathrm{grrs}}(\tau )\right]}^2\right\}\cdot \mathrm{d}\tau }+{\mathrm{w}}_{\mathrm{u}}\cdot {\displaystyle {\int }_{\tau =0}^{t_{\max }}\left[jer{k}^2(\tau )+{\omega }^2(\tau )\right]\cdot \mathrm{d}\tau },$$

(5)

wherein ${\mathrm{w}}_{\mathrm{u}}>0$ is a weighting parameter, and $\left(x_{\mathrm{grrs}}(t),y_{\mathrm{grrs}}(t)\right)$ is a parametric trajectory formed by linearly connecting the N_FE waypoints $\left\{({\mathrm{x}}_i^{\mathrm{grrs}},{\mathrm{y}}_i^{\mathrm{grrs}})|i=1,\dots ,{\mathrm{N}}_{\mathrm{FE}}\right\}$ in a sequence.

Compared with (1), OCP (2) does not contain two-point constraints or collision-avoidance 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 (s_start, l_start) and (s_end, l_end), respectively (Figure 4). The second step is to generate (N_S + 1) equidistant skeleton points along the reference line ranging from (s_start, 0) to (s_end, 0). A normal line is drawn along each skeleton point, which is orthogonal to the reference line. Along each normal line, N_L 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 (s_end, 0), N_L is set to 1 and the only candidate grid left is specified as the goal (s_end, l_end).

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\left(N_{\mathrm{L}}{}^{N_{\mathrm{S}}}\right)$ to $O\left(N_{\mathrm{S}}\cdot N_{\mathrm{L}}^2\right)$, 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.

In Algorithm 1, each candidate grid is regarded as a node. InitializeNodes() is used to initialize the cost of each node as $+\infty$. TraceBack(opti_id) is used to identify a sequence of nodes from $\mathrm{Node}({\mathrm{N}}_{\mathrm{S}},1)$, $\mathrm{Node}({\mathrm{N}}_{\mathrm{S}}-1,\mathrm{opti}\_\mathrm{id})$, to its ancestors until $\mathrm{Node}(0,1)$. The node sequence, if output in inverse order, forms a coarse path within the Frenet frame. $MeasureCost\left(\mathrm{Node}1,\hspace{0.17em}\mathrm{Node}2\right)$ 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., ${10}^{20}$) 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 $\left|(\mathrm{Node}0.\mathrm{config}-\mathrm{Node}1.\mathrm{config})\times (\mathrm{Node}1.\mathrm{config}-\mathrm{Node}2.\mathrm{config})\right|.$ 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, N_sample waypoints $\left\{({\mathrm{s}}_i^{\mathrm{wp}},{\mathrm{l}}_i^{\mathrm{wp}})|i=1,\dots ,N_{\mathrm{sample}}\right\}$ are evenly sampled along the concerned line segment from Node1 to Node2. Regarding the ith sampled waypoint $({\mathrm{s}}_i^{\mathrm{wp}},{\mathrm{l}}_i^{\mathrm{wp}})$, the corresponding coordinate value in the Cartesian frame is identified as $({\mathrm{x}}_i^{\mathrm{wp}},{\mathrm{y}}_i^{\mathrm{wp}})$ 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 $({\mathrm{x}}_i^{\mathrm{wp}},{\mathrm{y}}_i^{\mathrm{wp}},\mathrm{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 $({\mathrm{s}}_i^{\mathrm{wp}},{\mathrm{l}}_i^{\mathrm{wp}})$ is regarded as valid. J_positioning measures the rate of valid waypoints along the concerned line segment from Node1 to Node2.

Algorithm 1. Path planning via DP search.

Input: Reference line, scenario layout, location of cargoes;  Output: A path $\Lambda =\left\{({\mathrm{s}}_i^{\mathrm{dp}},{\mathrm{l}}_i^{\mathrm{dp}})|i=0,\dots ,{\mathrm{N}}_{\mathrm{S}}\right\}$; 1. InitializeNodes(); 2. $\mathrm{Set} \mathrm{Node}(0,1).\mathrm{config}=(x_{\mathrm{start}},y_{\mathrm{start}})$; 3. For each $j\in \left\{1,\dots ,{\mathrm{N}}_{\mathrm{L}}\right\}$, do 4.  $\mathrm{Set} \mathrm{Node}(1,j).\mathrm{parent}=\mathrm{Node}(0,1)$; 5.  $\mathrm{Identify} \mathrm{Node}(1,j).\mathrm{config}$; 6.  $\mathrm{Set} \mathrm{Node}(1,j).\mathrm{cost}=MeasureCost\left(\mathrm{Node}(0,1),\hspace{0.17em}\mathrm{Node}(1,j)\right)$; 7. End for 8. For each $i\in \left\{1,\dots ,{\mathrm{N}}_{\mathrm{S}}-2\right\}$, do 9.  For each $j\in \left\{1,\dots ,{\mathrm{N}}_{\mathrm{L}}\right\}$, do 10.   For each $k\in \left\{1,\dots ,{\mathrm{N}}_{\mathrm{L}}\right\}$, do 11.    $\mathrm{Identify} \mathrm{Node}(i+1,k).\mathrm{config}$; 12.     $\mathrm{cost}\_\mathrm{candidate}=MeasureCost\left(\mathrm{Node}(i,j),\hspace{0.17em}\mathrm{Node}(i+1,k)\right)$; 13.     If $\mathrm{Node}(i+1,k).\mathrm{cost}>\mathrm{Node}(i,j).\mathrm{cost}+\cos \mathrm{t}\_\mathrm{candidate}$, then 14.     $\mathrm{Node}(i+1,k).\mathrm{parent}=\mathrm{Node}(i,j)$; 15.     $\mathrm{Node}(i+1,k).\cos \mathrm{t}=\cos \mathrm{t}\_\mathrm{candidate}$; 16.    End if 17.   End for 18.   End for 19. End for 20. $\mathrm{opti}\_\mathrm{id}=\underset{j=1,\dots ,{\mathrm{N}}_{\mathrm{L}}}{\arg \min }\left(\mathrm{Node}({\mathrm{N}}_{\mathrm{S}}-1,j).\mathrm{cost}+MeasureCost\left(\mathrm{Node}({\mathrm{N}}_{\mathrm{S}}-1,j),\hspace{0.17em}\mathrm{Node}({\mathrm{N}}_{\mathrm{S}},1)\right)\right)$; 21. $\Lambda =TraceBack\left(\mathrm{opti}\_\mathrm{id}\right)$; 22. Return.

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 (N_S + 1) waypoints. Since N_S 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.

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:

$$\begin{array}{l}\begin{array}{l}\frac{\mathrm{d}l(s)}{\mathrm{d}s}=dl(s),\\ \frac{\mathrm{d}dl(s)}{\mathrm{d}s}=ddl(s),\\ \frac{\mathrm{d}ddl(s)}{\mathrm{d}s}=dddl(s),\end{array} \\ \begin{array}{l}-{\mathrm{dl}}_{\max }\le dl(s)\le {\mathrm{dl}}_{\max }, \\ -{\mathrm{ddl}}_{\max }\le ddl(s)\le {\mathrm{ddl}}_{\max }, \\ -{\mathrm{dddl}}_{\max }\le dddl(s)\le {\mathrm{dddl}}_{\max },\end{array}\\ s\in [s_{\mathrm{start}},s_{\mathrm{end}}].\end{array}$$

(6)

Herein, ${\mathrm{dl}}_{\max }$, ${\mathrm{ddl}}_{\max }$, and ${\mathrm{dddl}}_{\max }$ 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({\mathrm{s}}_{\mathrm{start}})={\mathrm{L}}_{\mathrm{start}}, dl({\mathrm{s}}_{\mathrm{start}})=\mathrm{D}{\mathrm{L}}_{\mathrm{start}}, ddl({\mathrm{s}}_{\mathrm{start}})=0, dddl({\mathrm{s}}_{\mathrm{start}})=0,\\ l({\mathrm{s}}_{\mathrm{end}})={\mathrm{L}}_{\mathrm{end}}, dl({\mathrm{s}}_{\mathrm{end}})=\mathrm{D}{\mathrm{L}}_{\mathrm{end}}, ddl({\mathrm{s}}_{\mathrm{end}})=0, dddl({\mathrm{s}}_{\mathrm{end}})=0.\end{array}$$

(7)

Herein, the parameters ${\mathrm{L}}_{\mathrm{start}}$, ${\mathrm{DL}}_{\mathrm{start}}$, ${\mathrm{L}}_{\mathrm{end}}$, and ${\mathrm{DL}}_{\mathrm{end}}$ reflect the assigned initial and goal configurations. Particularly, $\mathrm{D}{\mathrm{L}}_{\mathrm{start}}$ and $\mathrm{D}{\mathrm{L}}_{\mathrm{end}}$ are related to the vehicle orientation angles at $s={\mathrm{s}}_{\mathrm{start}}$ and ${\mathrm{s}}_{\mathrm{end}}$, 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 $\eta$, the collision-avoidance constraints are defined as

$$\begin{array}{l}\eta \cdot \tan {\theta }_{\mathrm{s}}+l(s)+0.5\cdot {\mathrm{L}}_{\mathrm{B}}\le ub(s+\eta ),\\ \eta \cdot \tan {\theta }_{\mathrm{s}}+l(s)-0.5\cdot {\mathrm{L}}_{\mathrm{B}}\ge lb(s+\eta ).\end{array}$$

(8)

The complete collision-avoidance constraints are formed by imposing (8) for any $\eta \in [-{\mathrm{L}}_{\mathrm{R}}\cos {\theta }_{\mathrm{s}}, ({\mathrm{L}}_{\mathrm{W}}+{\mathrm{L}}_{\mathrm{F}})\cos {\theta }_{\mathrm{s}}]$. Herein, ${\theta }_{\mathrm{s}}(s)$ denotes the vehicle’s orientation angle within the Frenet frame. Inherently, ${\theta }_{\mathrm{s}}(\mathrm{s})$ stands for the difference between the ego vehicle’s heading direction and the tangent direction along the reference line at s. $\left|{\theta }_{\mathrm{s}}(s)\right|$ 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}{l}\tan {\theta }_{\mathrm{s}}\equiv \frac{\mathrm{d}l(s)}{\mathrm{d}s}\equiv dl(s), \\ \cos {\theta }_{\mathrm{s}}\approx 1.\end{array}$$

(9)

This yields the following constraints:

$$\begin{array}{l}\eta \cdot dl(s)+l(s)+0.5\cdot {\mathrm{L}}_{\mathrm{B}}\le ub(s+\eta ),\\ \eta \cdot dl(s)+l(s)-0.5\cdot {\mathrm{L}}_{\mathrm{B}}\ge lb(s+\eta ),\\ \forall \eta \in [-{\mathrm{L}}_{\mathrm{R}},{ \mathrm{L}}_{\mathrm{W}}+{\mathrm{L}}_{\mathrm{F}}].\end{array}$$

(10)

The cost function is defined as

$$\begin{array}{ll}J=& {\mathrm{w}}_1\cdot {\displaystyle {\int }_{s={\mathrm{s}}_{\mathrm{start}}}^{{\mathrm{s}}_{\mathrm{end}}}{\left(l(s)-l_{\mathrm{DP}}(s)\right)}^2\mathrm{d}s}+{\mathrm{w}}_2\cdot {\displaystyle {\int }_{s={\mathrm{s}}_{\mathrm{start}}}^{{\mathrm{s}}_{\mathrm{end}}}d{l}^2(s)\mathrm{d}s}+\\ & { \mathrm{w}}_3\cdot {\displaystyle {\int }_{s={\mathrm{s}}_{\mathrm{start}}}^{{\mathrm{s}}_{\mathrm{end}}}dd{l}^2(s)\mathrm{d}s}+{\mathrm{w}}_4\cdot {\displaystyle {\int }_{s={\mathrm{s}}_{\mathrm{start}}}^{{\mathrm{s}}_{\mathrm{end}}}ddd{l}^2(s)\mathrm{d}s},\end{array}$$

(11)

wherein w₁, w₂, w₃, and w₄ are weighting parameters, and $l_{\mathrm{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, w₁ is set smaller before the QP problem is solved again. This process continues until a curvature-feasible path is derived.

4. Simulation Results and Discussion

This section discusses the efficacy, occlusion awareness, and closed-loop tractability of our proposed path planner.

4.1. Simulation Setup

Simulations were implemented in a MATLAB + CarSim platform and executed on an Intel Core i9-9900 CPU with 32 GB RAM that runs at 3.10 $\times$ 2 GHz. We define a $50 \mathrm{m}\times 50 \mathrm{m}$ warehouse with eight infrared receivers that are evenly distributed along the four edges of the rectangular ceiling, the height of which is 5 m. The geometric size of each cargo is $5 \mathrm{m}\times 5 \mathrm{m}\times \mathrm{h}$, wherein h is a random value ranging from 0 to 5 m. Other parametric settings are presented in our source codes, which are available at https://github.com/libai1943/OcclusionAwarePathPlanningForAGV (accessed on 8 December 2021).

4.2. On the Efficacy of the Proposed Planner

The planning results of two typical simulation cases are depicted in Figure 8, which shows the efficiency of each layer. In each of the two cases, the finally derived path is collision free and kinematically feasible. Both properties can be reflected by the footprints and curvature profiles plotted in Figure 9.

4.3. On the Occlusion Awareness of the Proposed Planner

This subsection investigates the occlusion awareness of the paths planned by the proposed method. Figure 10 shows the results with/without the positioning-related cost J_positioning in layer three. When the cost term J_positioning is discarded, the rate of good positioning distance along the entire path is 86.5% and 92.5% in the aforementioned two typical simulation cases, respectively. By contrast, with the cost term included, the rate grows to 97.0% and 96.5%. This comparative result clearly shows that our proposed planner can efficiently reduce the positioning inaccuracy caused by occlusions.

4.4. On the Closed-Loop Tractability of the Proposed Planner

This subsection reports the closed-loop tracking performance in CarSim when following the open-loop paths planned by the proposed planner (Figure 11). A linear quadratic regulator (LQR) is adopted as the controller. As illustrated in Figure 12, the open-loop and closed-loop paths do not differ much, which indicates that the planned paths are sufficiently smooth and thus easy to track. The concrete closed-loop tracking simulation results are presented in the following video link: https://www.bilibili.com/video/av677126688 (accessed on 8 December 2021).

5. Conclusions

This paper has introduced a path planning method for an autonomous vehicle in a warehouse, wherein the cargoes may occlude the inflated signals emitted by the ego vehicle for inflated positioning. According to our conducted simulations, the proposed planner is efficient according to its w.r.t. collision avoidance, kinematic feasibility, occlusion awareness, and tractability.