Forklift Tracking: Industry 4.0 Implementation in Large-Scale Warehouses through UWB Sensor Fusion

Abstract

This article addresses the problem of determining the location of pallets carried by forklifts inside a warehouse, which are recognized thanks to an onboard Radio Frequency IDentification (RFID) system at the ultra-high-frequency (UHF) band. By reconstructing the forklift trajectory and orientation, the location of the pallets can be associated with the forklift position at the time of unloading events. The localization task is accomplished by means of an easy-to-deploy combination of onboard sensors, i.e., an inertial measurement unit (IMU) and an optical flow sensor (OFS), with a commercial ultra-wideband (UWB) system through an Unscented Kalman Filter (UKF) algorithm, which estimates the forklift pose over time. The proposed sensor fusion approach contributes to the localization error mitigation by preventing drifts in the trajectory reconstruction. The designed methos was at first evaluated by means of a simulation framework and then through an experimental analysis conducted in a large warehouse with a size of about 4000 m².

1. Introduction

Locating robots and other vehicles has become a key topic in recent years in the Industry 4.0 framework [1]. Locating industrial vehicles, such as forklifts or laser-guided vehicles (LGVs), is critical to improve the management of large warehouses [2]. It was observed that the sequence in which products are picked and transported by forklifts may account for 55% of the total cost of warehouse operations [3,4]. Therefore, optimizing this process through a continuous monitoring of forklift routes can contribute significantly to the plant efficiency. Additionally, localization of forklifts or other vehicles is the first necessary requirement to implement autonomous vehicles [5].

As field practitioners are now aware, the lack of a reliable Global Navigation Satellite System (GNSS) signal in indoor environments, such as industrial plants and warehouses, makes it necessary to develop localization solutions based on alternative technologies [6]. In many applications, the vehicle coordinates, together with the vehicle orientation, i.e., the vehicle pose, must be estimated [7]. A vehicle can be theoretically localized through proprioceptive sensors that measure its ego-motion parameters, such as inertial measurement units (IMUs) [8], rotary encoders [9], or optical flow sensors [10,11], through a dead reckoning process [12]. However, such a solution is not applicable in the long-term, as the nature of these sensors leads to an accumulation of localization error and a consequent drift of the estimated trajectory. Therefore, to bind the localization uncertainty, proprioceptive sensor data are usually merged with exteroceptive sensor data, providing information about the surrounding environment [13,14].

Several technologies exist that can be adopted as providers of external information in the sound [15,16,17], optical [18,19,20,21,22,23], and electromagnetic domains [24,25,26,27,28,29,30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45,46,47,48,49,50,51,52,53,54,55,56].

The sound domain is particularly suitable for performing vehicle localization through rotating sonars equipped on the vehicle [17], but it suffers from several problems, such as noise pollution, which could be significant in industrial environments.

Particularly interesting is the optical domain, which is widely represented in the state-of-the-art [19,20,21,22,23]. The optical domain concerns both solutions with 2D infrared [19] and 3D rotating Light Detection And Ranging (LiDAR) [20] or computer vision (CV) techniques in the visible light domain [21,22,23]. The first type of solutions can achieve accurate localization performance thanks to the recent improvements in localization and simultaneous localization and mapping (SLAM) algorithms for mobile robots [19]. Rotating LiDARs are able to reconstruct the environment map and to measure the trajectory of the vehicle contextually, but they suffer from the problem of moving obstacles in their field of view, which may cause the map-creation process to fail. Therefore, a crowded or dynamic environment could not be suitable for these systems. The CV techniques based on visible-light cameras are robust, but indeed suffer similar issues. Moreover, the computational burden of these methods is high; poor light conditions may invalidate the quality of the gathered images, and privacy issues can be raised if the camera gets images/videos of people.

Some technologies in the electromagnetic domain, instead, provide a more robust way to release from the environmental effects. For example, if the area to be monitored is small, magnetic systems can be used [24,25,26]. Magnetic fields are also useful in vehicle localization for magnetometers contained in IMUs, used to calibrate gyroscope biases and to improve the robot tracking performance [27].

Radio frequency (RF) technologies are more suitable for several distance-measurement strategies depending on the available bandwidth. For solving the localization task, most widespread radio wave technologies require an infrastructure of devices installed at known locations, namely anchors. Then, onboard equipment exchanges a signal with the anchors to measure the vehicle–anchor distance, which is then exploited to compute the vehicle location through multilateration algorithms [28]. The vehicle–anchor distance can be measured by leveraging the received signal power [29], or the Time Of Flight (TOF) if the available communication bandwidth is enough [30]. Wi-Fi signals at 2.4 GHz are very widespread, as they can usually take advantage of a pre-existing infrastructure for data exchange inside the plant/warehouse. The Wi-Fi service bandwidth is limited to 85 MHz, which is not sufficient to perform TOF measurements; therefore, distance measurements are done by retrieving electromagnetic path loss models, which aim at linking the received power with the distance from two devices [31,32]. Unfortunately, received power measurements are strongly affected by the multipath propagation phenomena. Indeed, Wi-Fi fingerprinting techniques can be used to circumvent this problem by avoiding to perform distance measurements with path loss models, at the expense of long calibration procedures, which are hard to make in large industrial environments [33].

Recently, Radio Frequency IDentification (RFID) technology has become popular for mobile robot localization [34], especially passive RFID technology at an ultra-high frequency (UHF) band. Such systems have the advantage of not requiring any power supply to the anchors, but their reading range is limited to few meters; therefore, it could be difficult to implement such a system in a large industrial environment, unless a low-density deployment of RFID anchors is achieved [35].

Ultra-wideband (UWB) systems were expensive in the past; however, nowadays, commercial solutions exist at a reduced cost, by effectively introducing this technology as one of the most promising for performing radio wave-based ranging measurements [36,37]. The large bandwidth allows for precise and accurate ranging measurements based on the time of arrival (TOA) and time difference of arrival (TDOA) schemes, with a few cm error and with a 20–30 m signal distance coverage for each anchor. Therefore, by deploying a fine-grained infrastructure of UWB anchors, the vehicle location can be estimated through a multilateration algorithm, which leverages the estimated distances from an onboard UWB tag [38,39,40]. With a pair of UWB tags installed onboard, the vehicle orientation might be measured, too, at the expense of a more complex signal processing [41].

UWB systems suffer from multipath phenomena, and they are not insensitive to the environment [42]. However, thanks to machine learning and other techniques [43,44,45,46,47], the effect of detrimental multipath or non-line-of-sight (NLOS) conditions can be mitigated. Alternatively, multi-sensor data fusion, also known as sensor fusion, can be used to help the UWB system in the localization task, as proposed in this paper [48,49,50,51,52,53,54,55,56].

By generally speaking, sensor fusion localization schemes foresee combining the sensed data from different technologies to obtain a more accurate and reliable location estimation. The fused sensors can be of different natures, and it is common to find in the literature solutions that combine together several exteroceptive sensors [49,50,51,52,53,54]. However, the installation of multiple exteroceptive sensor systems can be wasteful, in terms of time, cost, and computational burden. Therefore, the fusion of proprioceptive sensors and a single exteroceptive sensor is preferable [55,56]. Most of these kinds of schemes rely on the usage of recursive dynamic state system estimators, such as the Kalman Filter (KF) and its variants [57], or Monte Carlo estimators, such as the Particle Filter (PF) and its variants [58]. These methods foresee two stages: (i) the prediction step, where the proprioceptive sensors are used to compute a first hypothesis on the vehicle motion; (ii) the update step, where the exteroceptive sensors are used to correct the first hypothesis on the vehicle motion and to reduce the error by preventing trajectory drifts and unbounded growths of the localization uncertainty.

In this paper, we will show an RFID Smart Forklift designed to operate within a Warehouse 4.0 for tissue paper storage. The products are organized in pallets, which are tagged with passive UHF-RFID tags that allow their identification. The forklift is equipped with a series of sensors that enable its self-localization thanks to an on-site processing unit. In particular, the smart forklift is equipped with a UWB tag, which receives the signal from a set of UWB anchors installed at the warehouse ceiling, an IMU, an optical flow sensor (OFS) [10,11], and four short-range time-of-flight laser-ranging distance sensors used to monitor the height above ground of the optical sensor. The OFS consists of a small infrared camera pointing to the floor to measure the forklift ego-motion, so it is not affected by environmental issues, such as poor light conditions or moving people and obstacles. In addition, the forklift is equipped with a UHF-RFID reader connected to four UHF-RFID antennas for pallet recognition, and an ultrasonic sensor to verify the forklift loaded/unloaded status. Through a sensor fusion tracking algorithm based on the Unscented Kalman Filter (UKF) [59], the trajectory and orientation of the forklift are reconstructed by fusing together the data acquired by the UWB system and the onboard sensors. The position of the pallets is then simply associated with the one of the forklift at the time of unloading events. All loading/unloading events and the position of the forklift over time are sent via a Wi-Fi connection to a central server for the operations management. In this way, it is possible to trace the movements of all the goods inside the warehouse. The focus of the work is to show how the proposed tracking algorithm is able to ensure good localization accuracy and a correct estimation of the vehicle orientation, which is essential to accurately determine the exact position of the unloaded pallets. The advantages of the proposed tracking methods are as follows: (i) a single UWB tag and the onboard kinematic sensors allow to correctly reconstruct the forklift orientation; (ii) the optical flow sensor, considered here as a proprioceptive sensor, is used in combination with the UWB technology to obtain high-accuracy tracking performance; (iii) the sensor fusion scheme only foresees a single exteroceptive sensor technology for providing environmental external data (i.e., the UWB system), so it is simple and easy to install; (iv) the computational burden is low and the method can be fruitfully exploited for real-time tracking.

The paper is structured as follows: Section 2 provides an algebraic explanation of the proposed localization method, by describing the UKF design and the UWB localization technique. Section 3 shows a simulated analysis to verify the localization algorithm capability. Section 4 presents a detailed description of the experimental equipment, the performed trials, and the experimental results. Finally, Section 5 states conclusions and future works.

2. UWB Forklift Localization Method

2.1. Forklift Motion Model

In this section, we present the forklift 2D tracking method based on the integration of IMU, OFS, and UWB sensors in a sensor-fusion algorithm. The tracking method is based on the dynamic system state estimation theory and relies on an UKF estimator. A forklift is usually a heavy vehicle with relatively high inertia, and can be modeled as a rigid body with size $L\times W\times H$, being L, W, H the length, width, and height of the vehicle, respectively. In this analysis, we suppose that the IMU and OFS are located at the same location, whereas the onboard UWB tag is in another location. This configuration will also be adopted in the experimental analysis in Section 4.

First, we define a right-handed local reference frame $\langle N\rangle =\{O_n,X_n,Y_n,Z_n\}$ as the reference frame of the target area, which could be a plant or a warehouse (we will refer to $\langle N\rangle$ as the plant/warehouse reference frame). $O_n$ indicates the origin of the reference frame and $X_n$, $Y_n$, and $Z_n$ are the x-, y- and z-axes, respectively.

We define at each timestep k a body reference frame $\langle B^{\left(k\right)}\rangle =\{O_b^{\left(k\right)},X_b^{\left(k\right)},Y_b^{\left(k\right)},Z_b^{\left(k\right)}\}$ where $O_b^{\left(k\right)}$ is the origin of the reference frame, which is centered on the IMU/OFS location, whereas $X_b^{\left(k\right)}$, $Y_b^{\left(k\right)}$, and $Z_b^{\left(k\right)}$ are the local x-, y- and z-axes at timestep k, respectively. The IMU and the OFS sensor reference frames are considered oriented as the forklift body frame. The local forklift reference frame and the plant/warehouse reference frame are then linked by a rigid roto-translational transformation, which depends on the location ${[x_k,y_k]}^T$ and orientation ${\theta }_k$ of the forklift at each timestep k, as described in Figure 1, where ${[x_k,y_k]}^T$ corresponds with $O_b^{\left(k\right)}$, and ${\theta }_k$ is the angle between $X_b^{\left(k\right)}$ and $X_n$. The UWB tag has a constant displacement in the body frame, with respect to $O_b^{\left(k\right)}$, which can be defined as $\Delta {\mathbf{p}}_{\mathbf{UWB}}={[\Delta x_{UWB},\Delta y_{UWB},\Delta z_{UWB}]}^T$. This offset must be translated in the plant/warehouse frame according to the estimated orientation ${\theta }_k$.

The UWB tag location is estimated through a multilateration algorithm, which is supposed to involve a maximum of four UWB anchors at once, as the case of the experimental analysis. Without loss of generality, we suppose that the UWB system measures each UWB tag coordinate with an additive white Gaussian error with zero-mean value and standard deviation ${\sigma }_{UWB}$, namely ${\nu }_{x_k}\sim \mathcal{N}(0,{\sigma }_{UWB}^2)$, and ${\nu }_{y_k}\sim \mathcal{N}(0,{\sigma }_{UWB}^2)$ for the x- and y-coordinates, respectively. The noise standard deviation ${\sigma }_{UWB}$ increases when the tag anchor distance increases due to a lower signal quality. However, as it will be shown in the experimental-analysis section, the anchors are attached at the ceiling, and the forklift is constrained to travel in such a way that the distances of the four closest UWB anchors are always similar; consequently, the noise parameters can be considered as constant.

The onboard sensors measure kinematic quantities with respect to the body frame. In particular, since the forklift is constrained to move on the ground, we consider the 2D acceleration ${\mathbf{a}}_{\mathbf{k}}$$={[a_{x_k},a_{y_k}]}^T$, the angular speed ${\omega }_k$, and the 2D forklift velocity ${\mathbf{v}}_{\mathbf{k}}={[v_{x_k},v_{y_k}]}^T$. The accelerations are measured through the IMU and they are considered as affected by a zero-mean additive white Gaussian noise, named as ${\nu }_{a_{x_k}}\sim \mathcal{N}(0,{\sigma }_a^2)$, and ${\nu }_{a_{y_k}}\sim \mathcal{N}(0,{\sigma }_a^2)$, where ${\sigma }_a$ is the acceleration-noise standard deviation. Similarly, the angular speed noise is ${\nu }_{{\omega }_k}\sim \mathcal{N}(0,{\sigma }_{\omega }^2)$, where ${\sigma }_{\omega }$ is the angular speed-noise standard deviation. Consider that, since the forklift is a rigid body, the angular speed can be measured in any point of the vehicle, and it is not mandatory to measure it at the rotation center of the vehicle. The OFS measures the displacement of the forklift among two consecutive timesteps, thanks to a computer vision algorithm that analyzes floor images acquired at different timesteps. The velocity measurements can be computed by dividing the displacement for the sampling time $\Delta t$. For the OFS, the quantization noise is preponderant with respect to other noise sources, so the velocity measurements can be considered affected by two uniformly distributed random variables named $q_{v_{x_k}}\sim \mathcal{U}\left([-r/(2\Delta t),r/(2\Delta t)]\right)$, and $q_{v_{y_k}}\sim \mathcal{U}\left([-r/(2\Delta t),r/(2\Delta t)]\right)$, where r is the sensor spatial resolution and $\Delta t$ is the sampling time.

The dynamic state vector is ${\mathbf{s}}_{\mathbf{k}}={[x_k,y_k,v_{x_k},v_{y_k},a_{x_k},a_{y_k},{\omega }_k,{\theta }_k]}^T\in {\mathbb{R}}^{8\times 1}$. The dynamic state vector at timestep $k+1$, ${\mathbf{s}}_{\mathbf{k}+\mathbf{1}}$, can be written as a non-linear function of ${\mathbf{s}}_{\mathbf{k}}$ through the following state transition model:

$${\mathbf{s}}_{\mathbf{k}+\mathbf{1}}=\mathbf{f}\left({\mathbf{s}}_{\mathbf{k}}\right)=\left[\begin{array}{c}{x}_{k+1}\\ y_{k+1}\\ v_{x_{k+1}}\\ v_{y_{k+1}}\\ a_{x_{k+1}}\\ a_{y_{k+1}}\\ {\omega }_{k+1}\\ {\theta }_{k+1}\end{array}\right]=\left\{\begin{array}{c}{x}_k+\Delta t(v_{x_k}cos\left({\theta }_k\right)-v_{y_k}sin\left({\theta }_k\right))\\ y_k+\Delta t(v_{x_k}sin\left({\theta }_k\right)+v_{y_k}cos\left({\theta }_k\right))\\ v_{x_k}+\Delta t{a}_{x_k}\\ v_{y_k}+\Delta t{a}_{y_k}\\ a_{x_k}\\ a_{y_k}\\ {\omega }_k\\ {\theta }_k+\Delta t{\omega }_k\end{array}\right.$$

(1)

The high inertia of the forklift makes reasonable the assumption of a quasi-constant velocity motion model between two consecutive timesteps. Therefore, we can suppose that the process noise only applies on the acceleration quantities and the angular speed. Without loss of generality, we will assume that the process noise statistics have the same statistics of the measurement noise for the same variable. The process noise is not additive. In (2), we made explicit the presence of the state noise vector ${\mathbf{w}}_{\mathbf{k}}={[{\nu }_{a_{x_k}},{\nu }_{a_{y_k}},{\nu }_{{\omega }_k},{\widehat{q}}_{v_{x_k}},{\widehat{q}}_{v_{y_k}}]}^T$ in the state transition model:

$${\mathbf{s}}_{\mathbf{k}+\mathbf{1}}=\mathbf{f}({\mathbf{s}}_{\mathbf{k}},{\mathbf{w}}_{\mathbf{k}})=\left[\begin{array}{c}{x}_{k+1}\\ y_{k+1}\\ v_{x_{k+1}}\\ v_{y_{k+1}}\\ a_{x_{k+1}}\\ a_{y_{k+1}}\\ {\omega }_{k+1}\\ {\theta }_{k+1}\end{array}\right]=\left\{\begin{array}{c}{x}_k+\Delta t[(v_{x_k}+{\widehat{q}}_{v_{x_k}})cos\left({\theta }_k\right)-(v_{y_k}+{\widehat{q}}_{v_{y_k}})sin\left({\theta }_k\right)]\\ y_k+\Delta t[(v_{x_k}+{\widehat{q}}_{v_{x_k}})sin\left({\theta }_k\right)+(v_{y_k}+{\widehat{q}}_{v_{y_k}})cos\left({\theta }_k\right)]\\ v_{x_k}+{\widehat{q}}_{v_{x_k}}+\Delta t(a_{x_k}+{\widehat{\nu }}_{a_{x_k}})\\ v_{y_k}+{\widehat{q}}_{v_{y_k}}+\Delta t(a_{y_k}+{\widehat{\nu }}_{a_{y_k}})\\ a_{x_k}+{\widehat{\nu }}_{a_{x_k}}\\ a_{y_k}+{\widehat{\nu }}_{a_{y_k}}\\ {\omega }_k+{\widehat{\nu }}_{{\omega }_k}\\ {\theta }_k+\Delta t({\omega }_k+{\widehat{\nu }}_{{\omega }_k})\end{array}\right.$$

(2)

where with the symbol “$\widehat{·}$” we indicate the corresponding process noise variable. At each timestep k, the following set of measurements ${\mathbf{z}}_{\mathbf{k}}=\mathbf{h}({\mathbf{s}}_{\mathbf{k}},{\mathbf{n}}_{\mathbf{k}})\in {\mathbb{R}}^{7\times 1}$ is available, where ${\mathbf{n}}_{\mathbf{k}}$ represents the set of measurement noises:

$${\mathbf{z}}_{\mathbf{k}}=\mathbf{h}({\mathbf{s}}_{\mathbf{k}},{\mathbf{n}}_{\mathbf{k}})=\left[\begin{array}{c}\widetilde{x_k}\\ \widetilde{y_k}\\ \widetilde{v_{x_k}}\\ \widetilde{v_{y_k}}\\ \widetilde{a_{x_k}}\\ \widetilde{a_{y_k}}\\ \widetilde{{\omega }_k}\end{array}\right]=\left\{\begin{array}{c}{x}_k^{UWB}-\Delta x_{UWB}cos\left({\theta }_k\right)+\Delta y_{UWB}sin\left({\theta }_k\right)+{\nu }_{x_k}\\ y_k^{UWB}-\Delta x_{UWB}sin\left({\theta }_k\right)-\Delta y_{UWB}cos\left({\theta }_k\right)+{\nu }_{y_k}\\ v_{x_k}+q_{v_{x_k}}\\ v_{y_k}+q_{v_{y_k}}\\ a_{x_k}+{\nu }_{a_{x_k}}\\ a_{y_k}+{\nu }_{a_{y_k}}\\ {\omega }_k+{\nu }_{{\omega }_k}\end{array}\right.$$

(3)

where $x_k^{UWB}$ and $y_k^{UWB}$ are the UWB tag coordinates measured in the plant/warehouse frame through the UWB system and which will be described in the next section. Thanks to these two sets of Equations (2) and (3), a UKF can be adopted to estimate the forklift location in the plant/warehouse reference frame and its orientation. The UKF filter needs, for several initial parameters, to run, for instance, the parameters $\alpha$, K, and $\beta$ [59]. $\alpha$ and K size the spread of the sigma-points, whereas the parameter $\beta$ must be chosen according to the data distribution. Typical recommendation is $\alpha ={10}^{-3}$, $K=0$, and $\beta =2$ [60]. The initial covariance matrix ${\mathbf{P}}_{\mathbf{0}}\in {\mathbb{R}}^{8\times 8}$ is set to the following:

$${\mathbf{P}}_{\mathbf{0}}=\left[\begin{array}{cccccccc}{\sigma }_{UWB}^2& 0& 0& 0& 0& 0& 0& 0\\ 0& {\sigma }_{UWB}^2& 0& 0& 0& 0& 0& 0\\ 0& 0& {\displaystyle \frac{r^2}{12\Delta t^2}}& 0& 0& 0& 0& 0\\ 0& 0& 0& {\displaystyle \frac{r^2}{12\Delta t^2}}& 0& 0& 0& 0\\ 0& 0& 0& 0& {\sigma }_a^2& 0& 0& 0\\ 0& 0& 0& 0& 0& {\sigma }_a^2& 0& 0\\ 0& 0& 0& 0& 0& 0& {\sigma }_{\omega }^2& 0\\ 0& 0& 0& 0& 0& 0& 0& {\sigma }_{\theta }^2\end{array}\right]$$

(4)

where ${\sigma }_{\theta }$ is the standard deviation of the forklift initial orientation error.

2.2. UWB Positioning

Several UWB anchors are deployed in the target area to perform the forklift localization through a simple multilateration algorithm. Since the commercial system adopted in the experimental analysis can use up to four anchors per each timestep, the following method is described by considering only four anchors, even if more of them are detected.

Let us consider a set of $N_{UWB}$ UWB anchors deployed in the warehouse at known locations ${\mathbf{p}}_{\mathbf{u},\mathbf{j}}={[x_{u,j},y_{u,j},z_{u,j}]}^T,j\in [1,\cdots ,N_{UWB}]$, as outlined in Figure 2. At each timestep k, we can write the vector ${\mathbf{r}}_{\mathbf{k}}$ of $N_{UWB}$ elements, in which the j-th element $r_k^{\left(j\right)}\in \{0,1\}$ is 1 if the j-th UWB anchor data are available at the timestep k, and it is 0 otherwise. Let us assume that, at each timestep, at least four anchors are detected by the onboard UWB tag, and a distance measurement $d_i$ is available for each anchor, where i represents the index of an anchor in the set of available anchors at the timestep k. For simplicity, let us assume that the set of indexes $i=[1,2,3,4]$ is always chosen for each timestep.

At each timestep, the following equations system can be written:

$$\left\{\begin{array}{c}{(x_{u,1}-x_k^{UWB})}^2+{(y_{u,1}-y_k^{UWB})}^2+\Delta z^2=d_1^2\\ {(x_{u,2}-x_k^{UWB})}^2+{(y_{u,2}-y_k^{UWB})}^2+\Delta z^2=d_2^2\\ {(x_{u,3}-x_k^{UWB})}^2+{(y_{u,3}-y_k^{UWB})}^2+\Delta z^2=d_3^2\\ {(x_{u,4}-x_k^{UWB})}^2+{(y_{u,4}-y_k^{UWB})}^2+\Delta z^2=d_4^2\end{array}\right.$$

(5)

where we consider $\Delta z=z_{u,j}-z_k^{UWB}$ a constant for each timestep and for each anchor. The system can be linearized if we subtract the first row to all the others. With such an operation, all of the quadratic unknown terms are canceled. After some algebraic manipulations, we obtain:

$$\left[\begin{array}{cc}(x_{u,2}-x_{u,1})& (y_{u,2}-y_{u,1})\\ (x_{u,3}-x_{u,1})& (y_{u,3}-y_{u,1})\\ (x_{u,4}-x_{u,1})& (y_{u,4}-y_{u,1})\end{array}\right]\left[\begin{array}{c}{x}_k^{UWB}\\ y_k^{UWB}\end{array}\right]=\left[\begin{array}{c}(d_1^2-d_2^2)+(x_{u,1}^2+y_{u,1}^2)-(x_{u,2}^2+y_{u,2}^2)\\ (d_1^2-d_3^2)+(x_{u,1}^2+y_{u,1}^2)-(x_{u,3}^2+y_{u,3}^2)\\ (d_1^2-d_4^2)+(x_{u,1}^2+y_{u,1}^2)-(x_{u,4}^2+y_{u,4}^2)\end{array}\right]$$

(6)

which is an over-determined linear equation system of the form

$$\mathbf{A}\mathbf{x}=\mathbf{b}$$

(7)

which can be solved by computing the Moore–Penrose pseudo-inverse matrix ${\mathbf{A}}^{†}$ to get a measurement of ${[x_k^{UWB},y_k^{UWB}]}^T$:

$$\left[\begin{array}{c}{x}_k^{UWB}\\ y_k^{UWB}\end{array}\right]=\mathbf{x}={\left({\mathbf{A}}^T\mathbf{A}\right)}^{-1}{\mathbf{A}}^T\mathbf{b}={\mathbf{A}}^{†}\mathbf{b}$$

(8)

3. Numerical Analysis

The performance of the proposed UKF algorithm are here demonstrated through a numerical analysis. We consider an indoor environment, where $N_{UWB}=35$ UWB anchors are deployed according to a regular $7\times 5$ grid in a 40 m × 25 m area at the ceiling and at a height $h=6$ m. The spacing along the x-coordinate of each anchor is 6.6 m, whereas along the y-coordinate is 6.25 m. The forklift is here considered as a single point with all of the sensor payloads (IMU, OFS, and UWB tag) placed at the same point. Therefore, $\Delta x_{UWB}=\Delta y_{UWB}=0$ m. The height of the UWB tag is 2 m, so $\Delta z=4$ m. The sampling time $\Delta t$ is set to 10 ms. At each timestep k, the algorithm exploits the measured distance between the UWB tag and the four closest UWB anchors to perform the multilateration. At the same time, the IMU and the OFS generate acceleration, angular speed, and velocity data. In accordance to the data sheet of the sensors that will be used for the experimental analysis, we set ${\sigma }_a=16.7$ mm/s², ${\sigma }_{\omega }=2.4$ mrad/s, $r=2.3$ cm, ${\sigma }_{UWB}=0.15$ m, and ${\sigma }_{\theta }=0.1$ rad. The forklift starts a “square wave-shaped” path from location ${[x_0,y_0]}^T={[0,0]}^T$ m with an orientation ${\theta }_0=\pi /2$ rad, and travels with a speed of 0.75 m/s until the location ${[x_{N_c},y_{N_c}]}^T={[32.6,15]}^T$ m after $N_c=\mathrm{10,000}$ algorithm steps. The path-length is approximately 75 m. The UKF filter is run with $\alpha ={10}^{-3}$, $\beta =2$, and $K=0$.

A top view of the performed simulated trajectory is depicted in Figure 3. The UWB anchors are the gray squared markers, and the ground truth trajectory is depicted as a continuous gray line. We represent, with a continuous blue line, the trajectory estimated with the UWB system only. The output of the dead-reckoning process by using only IMU and OFS is instead depicted as a dashed red line, whereas the output of the proposed UKF-based algorithm is the pointed green line.

As can be seen, the UWB system alone is not affected by drifts, but the output trajectory is not smooth; therefore, is not easy to accurately estimate the vehicle orientation. The output of the dead-reckoning process (dashed red line) is a smooth curve, but since only proprioceptive sensors are involved, it is affected by a detrimental drift, especially caused by the errors on the measured angular speed, which have a great impact on the vehicle orientation estimation. The output of the UKF (pointed green line) is a smooth trajectory without drifts, meaning that the filter converges. As expected, the trajectory of the vehicle gets further from the ground truth trajectory along the curves, but the algorithm is able to recover the track in few steps. In Figure 4, we depict the localization errors along the x-coordinate ${ϵ}_x$ (Figure 4a), along the y-coordinate ${ϵ}_y$ (Figure 4b), and the distance error ${ϵ}_d=\sqrt{{ϵ}_x^2+{ϵ}_y^2}$ (Figure 4c). From the figures, an initial transient stage is clearly observable, where the UKF must achieve the convergence, which is indeed reached in a few seconds.

To better investigate the method performance, a Monte Carlo analysis with $M=100$ test cases with similar features as the one represented in Figure 3 was conducted. The results of the cumulative distribution function of the distance error ${ϵ}_d$ for the different tracking methods is represented in Figure 5. As apparent, the error of the UKF is bound to 0.5 m. Figure 6, instead, shows the histogram of the orientation error ${\sigma }_{\theta }$. It can be noticed that the orientation error is lower for the UKF. In particular, the global mean orientation errors are ${\overline{ϵ}}_{{\theta }_{UWB}}=0.31$ rad, ${\overline{ϵ}}_{{\theta }_{IMU+OFS}}=-0.26$ rad, and ${\overline{ϵ}}_{{\theta }_{UKF}}=-0.01$ rad, whereas the standard deviations are ${\sigma }_{{\theta }_{UWB}}=2.11$ rad, ${\sigma }_{{\theta }_{IMU+OFS}}=0.11$ rad, and ${\sigma }_{{\theta }_{UKF}}=0.05$ rad. This means that the accuracy of UWB system alone with a single tag is not enough to correctly retrieve the forklift orientation.

3.1. Effect of Forklift Speed

The forklift speed might impact the UKF performance. To check it, we simulated the forklift trajectory as that depicted in Figure 3, but we changed the forklift speed among the values $v=[0.75,1.11,1.67,2.22,2.75]$ m/s. The number of samples for each trajectory was then resized in order to keep unchanged the path length to 75 m for all trials. For each speed value, we performed 100 test cases and then we computed the mean value of ${ϵ}_d$, named here as ${\overline{ϵ}}_d$. We show the results in Figure 7. The UWB performance is here unchanged due to the assumed simulation model. Instead, the mean distance error for the dead-reckoning process (circular red markers) increases with the forklift speed, and so, also, the mean distance error of the UKF, due to the fact that the sensor fusion approach relies on both UWB and proprioceptive sensor data. On the other hand, the UKF keeps “bounded” the increasing of the mean distance error, which is lower than the error committed by the UWB system, even when the forklift travels at 2.75 m/s. Given the fact that forklifts usually cannot travel faster than 2–3 m/s for safety reasons, we can conclude that the UKF is robust with respect to the increase of the forklift speed.

3.2. Effect of Initial Uncertainty

Tracking algorithms should be robust with respect to errors in the filter initialization. Positioning systems as the UWB systems are not affected by this problem. Dead-reckoning, instead, is invalidated by large initialization error. A sensor fusion algorithm based on sequential estimators such as the UKF, should converge even if an initial error on the dynamic state estimation is present. For this purpose, we performed an analysis of the method performance when the matrix ${\mathbf{P}}_{\mathbf{0}}$ changes. We considered different values of ${\sigma }_{UWB}$ and ${\sigma }_{\theta }$ to perform the method with 11 different configurations of the initial error standard deviation. The values of the changed parameters in each configuration are shown in

Table 1. Values of ${\sigma }_{UWB}$ and ${\sigma }_{\theta }$ for the analysis of the UKF performance.

Trial	${\mathit{\sigma }}_{\mathit{UWB}}$ [m]	${\mathit{\sigma }}_{\mathit{\theta }}$ [rad]
A	0.1	0.1
B	0.2	0.1
C	0.4	0.14
D	0.6	0.21
E	0.8	0.28
F	1	0.35
G	1.2	0.42
H	1.4	0.49
I	1.6	0.56
J	1.8	0.63
K	2	0.7

. For each configuration, we ran the UKF 100 times and we observed the mean distance error ${\overline{ϵ}}_d$.

The results are depicted in Figure 8, for a forklift speed of $v=0.75$ m/s. As it can be noticed, the UKF performance is not influenced by the initial error uncertainty because the mean localization error does not change with different initial conditions.

4. Experimental Analysis

4.1. The RFID Smart Forklift

This section aims at describing the sensors installed on the forklift. Forklifts are electrically driven vehicles and exist with either three wheels or four. The employed forklift is a four-wheel OM Still RX 20-18P/Li-Ion. It has a front wheel drive, so traction is carried out by the two front wheels, which can also rotate for the vehicle steering.

The odometry data were not available and, therefore, we could not adopt the unicycle or bicycle vehicle model. However, thanks to the presence of the IMU and the OFS, it is still possible to reconstruct the dynamic state model of the vehicle.

The sizes of the vehicle are $L=2.78$ m, $W=1.14$ m, $H=2.08$ m. The onboard hardware is the following:

• Decawave DWM1001 UWB module [61] configured as tag;
• A box containing the kinematic proprioceptive sensors: one Agilent ADNS3080 Optical Flow Sensor [62], four STMicroelectronics VL53L0X Time-of-Flight laser-ranging distance sensors [63], STMicroelectronics LSMDS3 inertial measurement unit platform [64];
• a RENESAS SK-S7G2 microcontroller [65];

The forklift also has, installed, onboard, the RFID hardware for the detection of the loaded pallets:

• UHF-RFID reader Impinj Speedway Revolution R420 [66];
• two UHF-RFID Keonn Advantenna−p11 antennas for loaded pallet recognition [67];
• two UHF-RFID Alien A0501 antennas for stocked pallet detection and localization [68];

A picture of the RFID Smart Forklift is shown in Figure 9. The box containing the OFS, the four Time-of-Flight laser-ranging distance sensors, and the IMU was installed close to the right-front wheel of the forklift at a height of 15.5 cm, in such a way that the OFS camera pointed towards the floor. The box is assumed as the origin of the body reference frame $O_b^{\left(k\right)}$. The OFS and the IMU were jointly used for the sensor fusion localization algorithm together with the UWB data, whereas the Time-of-Flight laser-ranging distance sensors were used to accurately measure the height of the OFS from the floor and keep track of possible height variations of the sensor. Such measurements are necessary to calibrate the sensor measurements, which turns out to have a spatial resolution of $r=2.3$ cm when the sensor is placed at a 15.5 cm height. According to the datasheet of the LSMDS3 IMU, the acceleration noise has a standard deviation ${\sigma }_a=16.7$ mm/s², whereas the angular speed noise exhibits a standard deviation ${\sigma }_{\omega }=2.4$ mrad/s.

Considering that the UWB anchors will be installed at the room ceiling, the UWB tag is instead positioned on the roof of the forklift, to ensure its visibility. The smart forklift is equipped with a Decawave DWM1001 UWB module of the MDEK1001 development kit placed on the forklift roof with a displacement of $\Delta {\mathbf{p}}_{\mathbf{UWB}}={[\Delta x_{UWB},\Delta y_{UWB},\Delta z_{UWB}]}^T={[-0.61,0.56,2.04]}^T$ m, with respect to the box containing the IMU and the OFS. Due to forklift structure reasons, it was not possible to place the UWB tag directly above the IMU and OFS, so it also necessary to foresee that the 2D position measured by the UWB system does not correspond to that of the other two sensors. All onboard sensors are properly time-synchronized together through the RENESAS SK-S7G2 microcontroller, which sets the sampling time at $\Delta t=10$ ms.

4.2. UWB Anchors

During the project, a demonstrator of the system was developed in one of the Sofidel S.p.a. large warehouses, by selecting a target area of size 4560 m², divided in two corridors. Thirty UWB Decawave anchors were installed at the ceiling of the warehouse designed area, by taking advantage of the presence of six metallic bars at a height of around 6 m oriented along the y-direction of the warehouse and spaced along the x-direction of around 10 m. A total of 30 UWB Decawave (Figure 10) DWM1001 modules configured to work as UWB anchors were installed on the bars (Figure 10a). The map of their deployment is shown in Figure 10b, where each UWB anchor is denoted as a blue asterisk. The location of UWB anchors was measured through the Leica Flexline TS03 manual total station [69]. It is a device equipped with lasers and a high-precision orientation meter that can provide a measurement of three-dimensional coordinates of a point within the space, with millimeter accuracy.

4.3. Results

To test the effectiveness of the UKF localization for the RFID Smart Forklift, tests were carried out in the warehouse. An adhesive tape was placed on the ground to create a reference path for the forklift dirver during the trials. By analyzing the estimated trajectories and comparing them with the reference path, it was possible to evaluate the localization performance of the UWB system alone, of the dead-reckoning process (IMU + OFS), and of the UKF. It must be highlighted that, the method used by the Decawave UWB system to estimate the tag location is not known to users, but it is known that no more than four UWB anchors at the same time are employed (this is why we performed the simulations in Section 3 by considering only four anchors per timestep when computing the multilateration).

Figure 11 shows a photo of a part of the reference path indicated by the adhesive tape stuck on the ground. To make sure that the forklift was following the reference path accurately, the OFS mounted on the forklift was used. In fact, this sensor projects on the ground a red-light beam footprint, which can be used to check if the reference points marked with the tape are correctly crossed during the forklift motion.

Nine forklift trajectories were performed with different speeds. The main features of the performed test case trajectories were resumed in

Table 2. Main features of the performed test case trajectories.

Test Case	Forklift Speed v [m/s]	Path-Length L [m]	Path Shape	${[{\mathit{x}}_0,{\mathit{y}}_0]}^{\mathit{T}}$ [m]	${\mathit{\theta }}_0$ [rad]
I	0.75	∼100	Closed-loop path	${[11.5,74.4]}^T$	$-\pi /2$
II	0.75	∼100	Closed-loop path	${[11.5,74.4]}^T$	$-\pi /2$
III	1.6	∼100	Closed-loop path	${[11.5,74.4]}^T$	$-\pi /2$
IV	1.6	∼100	Closed-loop path	${[11.5,74.4]}^T$	$-\pi /2$
V	2.75	∼100	Closed-loop path	${[11.5,74.4]}^T$	$-\pi /2$
VI	2.75	∼100	Closed-loop path	${[11.5,74.4]}^T$	$-\pi /2$
VII	1.2	∼137	Rectilinear path with U-turn	${[41.7,74.4]}^T$	$-\pi /2$
VIII	1.2	∼137	Rectilinear path with U-turn	${[41.7,74.4]}^T$	$-\pi /2$
IX	1.28	∼289	Closed-loop path	${[11.5,74.4]}^T$	$-\pi /2$

in terms of forklift forward speed v, path-length L, path shape, and starting location and orientation.

In Figure 12, we represent two test-case trajectories about the cases IV (Figure 12a) and IX (Figure 12b), respectively, which have comparable forklift speeds (1.2 m/s and 1.6 m/s, respectively), but different path-lengths (∼100 m and ∼289 m, respectively). The ground truth trajectory (continuous gray line) is the pre-determined reference path built by using the adhesive tape. The measured path with the UWB is depicted as continuous blue line, and it is the output of the Decawave commercial system after a mobile-median filter that filters out the outliers. The measured trajectory with IMU and OFS only is the dashed red line, and the estimated trajectory by using the UKF is the pointed green line. As it can be seen, the dead-reckoning and the UKF localization methods, thanks to the use of onboard kinematic sensors, outputs very smooth trajectories, which are therefore realistic due the high inertia of heavy industrial vehicles, such as forklifts. On the contrary, the UWB localization system exploiting multilateration can have some large oscillations around the reference path, as, for example, in the “straights” portions of the trajectory represented in Figure 12b. However, for long trajectories, and consequently during the regular running of forklift operations in the warehouse where the localization system must never be interrupted, the dead-reckoning suffers from drifts. An example of this phenomenon can be seen in Figure 12b, when the forklift performs a closed-loop path, but the dashed red line does not close itself to its starting point.

To better highlight the tracking performance improvement given by the employment of the proposed UKF algorithm in this two test cases, Figure 13 shows the boxplot of the errors along the x-coordinate (Figure 13a for test case IV and Figure 13b for test case IX), the y-coordinate (Figure 13c for test case IV and Figure 13d for test case IX), the distance error (Figure 13e for test case IV and Figure 13f for test case IX), and the orientation error ${ϵ}_{\theta }$ (Figure 13g for test case IV and Figure 13h for test case IX).

From the boxplots, it is possible to retrieve the mean localization errors in terms of distance error. Such errors are reported in

Table 3. Mean distance localization errors for test cases IV and IX in Table 2.

Test Case	${\overline{\mathit{ϵ}}}_{{\mathit{d}}_{\mathit{UWB}}}$ (m)	${\overline{\mathit{ϵ}}}_{{\mathit{d}}_{\mathit{IMU}+\mathit{OFS}}}$ (m)	${\overline{\mathit{ϵ}}}_{{\mathit{d}}_{\mathit{UKF}}}$ (m)	${\overline{\mathit{ϵ}}}_{{\mathit{\theta }}_{\mathit{UWB}}}$ (rad)	${\overline{\mathit{ϵ}}}_{{\mathit{\theta }}_{\mathit{IMU}+\mathit{OFS}}}$ (rad)	${\overline{\mathit{ϵ}}}_{{\mathit{\theta }}_{\mathit{UKF}}}$ (rad)
IV	3.84	2.81	2.13	−0.04	−0.04	0.02
IX	1.77	2.43	1.17	0.06	0.06	0.01

. As it can be seen, the UKF is able to reduce both localization and orientation errors.

4.4. Effect of Forklift Speed

As expected, the forklift speed might impact the UKF performance. Therefore an error analysis with varying forklift speed was conducted also via experimental tests. By considering test case trajectories I-VI, the forklift speed assumes the values 0.75 m/s (test case I and test case II), 1.6 m/s (test case III and test case IV), and 2.75 m/s (test case V and test case VI), and the path length is around 100 m. Figure 14 shows the mean distance error ${\overline{ϵ}}_d$ when the forklift speed varies. Different from the analysis conducted with the numerical simulations (Figure 7), the error given by the UWB system varies with the speed, but it decreases until the forklift speed reaches 1.6 m/s (IV) and then rises again. This effect could be explained by considering that for low speeds the forklift spends more time by doing maneuvers and curves, in which the UWB system does not perform well. When the forklift travels fast, instead, as in test cases V and VI, the UWB system performs worse. The onboard kinematic sensors, however, perform better for low-speed trajectories, and their error increases with the forklift speed. Hence, by looking at Figure 14, it can be concluded that low forklift speeds are preferred by the proposed system, which is able anyway to reduce the localization error to values always lower than those guaranteed by the UWB system only, or by the kinematic sensors (IMU + OFS). The minimum obtained error with the UKF is around 1 m in for the test cases I and II.

4.5. Effect of Initial Uncertainty

This section aims at investigating whether the proposed UKF-based tracking algorithm is robust with respect to errors in the initialization of the filter. We performed an analysis of the method performance by artificially introducing an error on the initial state ${\mathbf{s}}_{\mathbf{k}}$, namely by changing the initial error covariance matrix ${\mathbf{P}}_{\mathbf{0}}$ according to the same values reported in Table 1. For each configuration, we ran the UKF and we observed the mean distance error ${\overline{ϵ}}_d$ when the forklift performed the test-case trajectory IX. The results are depicted in Figure 15. As it can be noticed, as well as in real situations, the UKF performance is not influenced by the initial error uncertainty. The trajectory reconstruction with the onboard sensors only, instead, is greatly influenced by the initial uncertainty error, as typical for dead-reckoning approaches.

4.6. Global Performance

Figure 16 shows the cumulative distribution function of the distance error ${ϵ}_d$ for the different tracking methods (UWB, IMU + OFS, UKF) by considering all the test cases I–IX. As it can be noticed, the error of the UKF is bound to 4 m, which is acceptable for a tracking method in an industrial scenario of 4560 m² by also considering the forklift sizes. The forklift orientation, indeed, is better reconstructed with the UKF as shown in the histogram in Figure 17. In particular the global mean orientation errors are ${\overline{ϵ}}_{{\theta }_{UWB}}=0.1$ rad, ${\overline{ϵ}}_{{\theta }_{IMU+OFS}}=0.18$ rad, and ${\overline{ϵ}}_{{\theta }_{UKF}}=0.014$ rad, whereas the standard deviations are ${\sigma }_{{\theta }_{UWB}}=0.85$ rad, ${\sigma }_{{\theta }_{IMU+OFS}}=1.12$ rad, and ${\sigma }_{{\theta }_{UKF}}=0.22$ rad. Accordingly to what was observed in the simulated analysis, the UWB system with a single tag is still not able to correctly retrieve the forklift orientation, but it has a better behavior with respect to the simulated analysis. This effect is obtained thanks the data processing implemented by the UWB system. The small orientation error committed with the UKF allows for determining the forklift orientation at the time of pallet unloading and, therefore, to better recognize the location in which the pallet is unloaded.

4.7. Computational Burden

Finally, before installing the localization algorithm on the onboard microcontroller, we demonstrated the relatively low computational burden of the proposed method. For such an analysis, the elaboration time of the test cases I–IX has been measured when the processing was done on a laptop with an Intel(R) Core(TM) i7-7700HQ CPU @ 2.80 GHz and 16 GB RAM through a MATLAB code. The trajectory processing time is reported in

Table 4. Elaboration Time compared to trial duration for the test cases I–IX.

Test Case	Trial Duration [s]	Total Elaboration Time [s]	Number of Samples	Elaboration Time for Sample [ms]
I	136	10.81	13,641	0.79
II	132	10.05	13,265	0.75
III	63	5.23	6369	0.82
IV	64	5.18	6432	0.8
V	36	3.43	3606	0.95
VI	36	3.34	3611	0.92
VII	114	9.28	11,346	0.81
VIII	110	9.05	11,038	0.82
IX	225	17.09	22,563	0.75

. By considering the number of UKF timesteps, we can observe that the average processing time for each timestep is 0.82 ms. By considering a sampling time of $\Delta t=10$ ms, we can therefore conclude that the method is suitable for real-time applications, even with less performing CPUs. Indeed, the proposed system might also be considered in the future to be tested for safety applications to prevent forklift-to-forklift and forklift-to-human collisions [70].

5. Conclusions

In this paper, a system consisting of an RFID-equipped forklift that is able to recognize the loaded pallets and to self-localize was addressed for a tissue paper factory warehouse. Thanks to the forklift trajectory and orientation reconstruction, the location of the pallets can be associated with the forklift position at the time of unloading events. The forklift self-localization was conducted through a sensor fusion algorithm that combined data acquired from an onboard inertial measurement unit (IMU) and an onboard optical flow sensor (OFS) with a commercial ultra-wide band (UWB) system through an unscented Kalman filter (UKF) estimator. The proposed method is relevant to the state-of-the-art because of the usage of the optical flow sensor as an onboard proprioceptive sensor to measure the forklift ego-motion, which allows the employment of an optical domain sensor without the disadvantage of the dependence from environmental light conditions. After a first numerical investigation of the method performance, the whole system was evaluated in a 4560 m² warehouse where thirty Decawave commercial UWB anchors were installed at the ceiling, at known locations. Nine different test case trajectories were performed with different paths and forklift speed in order to investigate the sensor robustness to different operating conditions. The experimental analysis showed a decrease of the performance of onboard kinematic sensors and the UWB system with increasing forklift speed, which can be mitigated by the usage of the UKF. Indeed, the proposed method achieved a global experimental mean localization error of 1 m, with a maximum error of 4 m, and the orientation of the forklift was reconstructed with a mean error of 0.014 rad. Moreover, the UKF resulted in being “robust”, with respect to different initialization conditions, even for long paths (almost 300 m). An analysis of the UKF computational burden suggests the real-time feasibility of the proposed method. The achieved localization performance together with a limited computational burden makes the proposed low-cost, robust, and easy-to-deploy system suitable, not only for real-time monitoring of the warehouse status, but also as a potential candidate to be tested for collision avoidance systems.