Distributed Multi-Antenna Positioning for Automatic-Guided Vehicle

Abstract

Radio-based positioning systems are typically utilized to provide high-precision position information for automatic-guided vehicles (AGVs). However, the presence of obstacles in harsh environments, as well as carried cargoes on the AGV, will degrade the localization performance, since they block the propagation of radio signals. In this paper, a distributed multi-antenna positioning system is proposed, where multiple synchronous antennas are equipped on corners of an AGV to improve the availability and accuracy of positioning. An estimator based on the Levenberg–Marquardt algorithm is introduced to solve the nonlinear pseudo-range equations. To obtain the global optimal solutions, we propose a coarse estimator that utilizes the displacement knowledge of the antennas to provide a rough initial guess. Simulation results show a better availability of our system compared with the single antenna positioning system. Decimeter accuracy can be obtained under a Gaussian measurement noise with a standard deviation of 0.2 m. The results also demonstrate that the proposed algorithm can achieve positioning accuracy close to the theoretical Cramer–Rao lower bound. Furthermore, given prior information of the yaw angle, the same level of accuracy can be obtained by the proposed algorithm without the coarse estimation step.

1. Introduction

Automatic-guided vehicles (AGVs) play an important role in unmanned ports or warehouses. They can automatically and safely transport cargoes or materials to the expected destination. Since the 1980s, AGV became an indispensable part of the production logistics system. In order to realize the safe driving of AGVs and the accurate lifting of cargoes in unmanned environments, autonomous high-precision positioning and navigation of AGV are necessary.

In the early days, AGVs mainly used fixed path methods for navigation [1]. In these methods, positioning signs such as magnetic stripes or metal wires are embedded in paths, and the sensors on the AGV can then detect the signs and provide information to the controller for driving adjustment. Although this method is mature and it was widely used in early AGVs, it only works for a fixed path and can be easily affected by the magnetic and electric fields in the environment. With the increasing demand for flexibility and adaptability of the AGV navigation system, positioning methods free of path constraints are constantly being researched and promoted, including inertial, radio, laser, and visual positioning, among others [2]. The inertial navigation system (INS) can provide high-frequency velocity and attitude estimation by using inertial sensors installed on the vehicle. It gives an ephemeral precise positioning by dead reckoning, immune to external interference. However, it suffers significant accumulated errors in the long run. The laser positioning system uses laser scanning to obtain the location information of reflection signs to provide vehicle position. It has high precision, but it is susceptible to weather and needs a large number of reflective signs [2]. The visual positioning system provides localization services by processing images collected by visual sensors. It achieves high positioning accuracy in various scenes, albeit with a high cost and computational complexity. Due to the development of radio technology and data processing abilities of electronic devices, the radio positioning system became favored in recent years, among which the global navigation satellite system (GNSS) and ultra-wideband (UWB) system are particularly concerned.

GNSS is a wireless navigation system that provides global, all-weather, continuous, and real-time navigation and positioning by utilizing navigation satellites [3]. Centimeter-level accuracy can be achieved by carrier phase differencing technology, which can meet the positioning accuracy requirements of AGVs. However, GNSS signals are subject to blockage and multipath, which decrease the continuity and availability of the carrier phase measurements and further lower the precision, robustness, and availability of positioning [4,5]. Moreover, it is not suitable for indoor positioning due to significant signal loss [6,7].

Recently, we saw a trend of utilizing UWB as a navigation system. UWB radios have relative bandwidths larger than 20% or absolute bandwidths of more than 500 MHz [8,9]. UWB signals are particularly well suited for localization since they can provide accurate and reliable ranging measurements due to their fine time resolution and robustness in harsh environments [10]. Typically, centimeter-level accuracy can be obtained for UWB by using time-of-arrival (TOA) measurements [11,12]. Furthermore, the UWB positioning system can be configured for indoor applications [13,14]. With the development of UWB chips, UWB became a popular technology for regional navigation [15]. Similar to GNSS, the UWB navigation system also encounters the problem of accuracy and availability degradation caused by signal blockage [16,17].

Aiming at the problem of insufficient measurements caused by obstacles in the environment, researchers proposed a variety of solutions. Integrating radio navigation systems with other sensors such as INS [18,19], velocity sensor, Light Detection and Ranging (LiDAR) [20,21], or visual sensor [22] represents a solution. This fusion method is helpful to improve the availability but needs additional external devices, and the data fusion of different systems requires much more computation ability. In addition to the multi-sensor fusion method, the idea of cooperative localization is another solution. It solves the problem of insufficient information of a single node by sharing information among nodes [23,24]. Specifically, all users are nodes operating independently with unknown locations and asynchronous clocks. These users make measurements with both known location references and other unknown location nodes. The additional information gained from these measurements between pairs of unknown location nodes enhances the accuracy and robustness of the localization system [25]. Some studies showed that the cooperative technology can effectively improve the availability of the positioning system in harsh environments, and it is suitable for location service applications with a group of agent nodes [26,27]. However, it strongly depends on wireless communication networks, which requires stable operation of networks and reliable transmission of information.

In addition to all these difficulties mentioned above, another issue should be taken into account as shown in Figure 1. The red vehicle is an AGV, which has a flat structure. The cargoes are placed on the flat for transportation and there is no cab. Because of the restriction of the flat body, the receiving antennas cannot be mounted at the highest point of the vehicle. Thus, for the AGV, the radio positioning signals are not only affected by the obstacles in the environment but also blocked by the carried cargoes. The problem of insufficient measurements is even more serious in this case. Therefore, a method that can obtain as many measurements as possible and make full use of them is needed. Although more measurements will lead to an increase in computation, it is worth the improvement in availability, which is a prerequisite for the effective operation of the positioning system.

A multi-antenna method is our choice. Because of the spatial separation between antennas, more potential information can be provided for positioning from multiple antennas. Furthermore, redundant measurements by multiple antennas can improve the performance of positioning in terms of accuracy and robustness. Although the multi-antenna method was considered in many localization methods to improve localization [28,29,30,31], to the best of the authors’ knowledge, there are few studies on multi-antenna positioning systems in harsh environments. Some relevant studies typically only focused on multiple receivers. Some studies [32,33] improved the accuracy and reliability of positioning by using multiple low-cost receivers. These papers verified the accuracy improvement by utilizing spatially separated reception and data fusion, but did not discuss the problem of availability. Another study [33] set the position, attitude, velocity, angular rate of the center receiver, and clock bias of each receiver as unknown states and integrated the pseudo-range measurements from multiple global positioning system (GPS) receivers using an extended Kalman filter (EKF), which needs more measurements than single-receiver methods regardless of the availability. Multiple receivers were also used [34] to jointly track and estimate signal parameters to improve the reliability and robustness, but this study involved modification of the software and hardware of the tracking loop in the receivers.

Instead of multiple receivers, multiple distributed antennas with a synchronous clock are employed in this work. In our positioning system, fixed anchors periodically transmit positioning signals. Furthermore, there exist obstacles occasionally blocking the signals, such as buildings or containers on the roadside. The anchors can be automatically clock-synchronized and self-localized using a signal broadcast mechanism [35]. The distributed receiving antennas are mounted on the flat body of the AGV. The positions of these antennas are fixed and known with respect to a reference point on the vehicle. Due to the spatial differences, each antenna may encounter different blockages and, thus, have different subsets of visible anchors. Fusing the measurements of each antenna can improve the availability of the positioning system. Utilizing the position relationship between the receiving antennas and the reference point, the TOA measurements of each antenna can be expressed as a function of the reference position. In order to implement the transformation of the measurement representation, a rotation matrix from the body coordinate system (frame b) to the navigation coordinate system (frame n) is inevitably needed. Therefore, unlike the single antenna positioning method, in our distributed multi-antenna (DMA) method, not only the position and clock bias but also the attitude of AGV should be estimated, which forms a challenging nonlinear constrained optimization problem. The problem of finding the rotation matrix and the reference position was termed rigid body localization in some studies [36,37,38]. They localized a rigid body by placing a few sensors on the body and utilized the range measurements with respect to a few anchors to estimate the rotation matrix and the position [36]. A non-iterative estimator with two steps was also proposed [37]. The rotation matrix and position were coarsely estimated using sensor positions and then refined by estimating the corrections using Euler angle formulation. Another study [36] proposed two methods called constrained least squares (CLS) and simplified CLS based on linearizing the measurement model. In Reference [38], this problem of maximum likelihood (ML) formulation was addressed by using semidefinite relaxation to get a coarse estimate, before applying orthogonalization and refinement. Although these methods showed good performance in their circumstances, they are not suitable for our cases because they ignore the problem of insufficient measurements from a single sensor, and they simply assume that there is no clock bias, which is not practical in many real applications. If the rigid body localization methods are directly applied to our scene, the TOA measurements are treated as range measurements with large noise and the number of measurements is small, leading to low accuracy.

In this paper, we propose a DMA positioning algorithm based on the Levenberg–Marquardt (LM) algorithm with a coarse estimation step, which is introduced to provide a proper initial value to the iterative positioning algorithm. In the coarse estimation, the positions of the distributed antennas in frame n are jointly estimated and then used to coarsely estimate the reference position and the yaw angle according to the transformation relationship between frame b and frame n. Finally, the coarse estimates are applied to the iterative algorithm to get a refined positioning result. Thus, the algorithm is divided into two steps, and the positioning algorithm based on LM is hereinafter referred to as the two-step LM (TSLM) positioning algorithm. The Cramer–Rao lower bound (CRLB) of the positioning problem is derived as a benchmark to assess the performance of the proposed algorithm. The distributed multi-antenna horizontal dilution of precision (DMA-HDOP) model is also derived to analyze the positioning accuracy. Simulation results demonstrate that the positioning availability is significantly improved compared with the conventional single-antenna localization. Under a Gaussian measurement noise with a standard deviation of 0.2 m, the DMA method could achieve decimeter-level accuracy. Furthermore, in our simulation tests, the TSLM algorithm could provide positioning accuracy close to CRLB without requiring the initial yaw angle information. Simulation tests on the second step of the TSLM algorithm with different initial yaw angles verified that the iterative positioning algorithm in the second step is sensitive to the initial yaw angle. However, given prior information of the yaw angle, using only the second step can achieve similar results to TSLM.

The main contributions of this paper are summarized as follows:

1) A distributed multi-antenna method is proposed to improve the availability and accuracy of AGV positioning under the situation of signal blockage caused by environmental obstacles and carried cargoes.

2) A two-step LM algorithm is developed to optimally estimate both the position and the yaw angle, in which the initial value for the iterative algorithm in the second step is set by the coarse estimation from the first step.

The remainder of the article is organized as follows: Section 2 presents the model of the positioning system. The positioning algorithms are proposed in Section 3. Section 4 derives the CRLB of the position estimate problem and presents the DMA-HDOP model of the DMA positioning method. Section 5 gives the simulation results. Finally, Section 6 concludes the paper.

2. System Model

2.1. Notation

We employ the following notations throughout this work: the upper and lower case boldface denote matrices and vectors, respectively; symbols with a hat denote estimated values; the notation $E[\mathbf{·}]$ is the expectation operator; ${\mathbf{A}}^T$, ${\mathbf{A}}^{-1}$, and $\mathrm{trace}\left(\mathbf{A}\right)$ denote the transpose, the inverse, the trace of matrix $\mathbf{A}$, respectively; $\Vert \mathbf{·}\Vert$ denotes the Euclidean norm; $\mathbf{I}$ is the identity matrix; $\mathrm{card}\left(ℬ\right)$ denotes the number of elements in set $ℬ$; $C\left(n,k\right)$ denotes the number of k-combinations of n elements; ${\mathbf{A}}_{n_1\times n_2}$ denotes the $n_1$-by-$n_2$ matrix; $\mathbf{a}\left[n_1:n_2\right]$ denotes the $n_1$-th to $n_2$-th elements of vector $\mathbf{a}$; $\mathbf{a}\left[n\right]$ denotes the $n$-th element of vector $\mathbf{a}$; ${\left[\mathbf{A}\right]}_{n_1,n_2}$ denotes the element at the $n_1$-th row and $n_2$-th column of matrix $\mathbf{A}$.

Coordinate frames involved in this paper are the navigation frame and the body frame, denoted as n and -b, respectively. All the positions of anchors and the positioning results are expressed in frame n, which defines a certain point in the given area as its origin and the east, north, and upward directions as its $x^n$, $y^n$, and $z^n$ axes, respectively. The known positions of the distributed antennas with respect to a reference point on AGV are expressed in frame b. The origin of frame b is the reference point. $x^b$, $y^b$, and $z^b$ axes of frame b are defined as the front, right, and downward directions of the AGV body, respectively. The rotation matrix of transformation from frame b to frame n is represented by ${\mathbf{R}}_b^n$.

2.2. Problem Formulation

Consider a system in which there are $M$ anchors with known position and known clock bias, an AGV with unknown position, and $N$ antennas with unknown positions and the same unknown clock bias. Let $\mathcal{N}$ be the set of antennas with $\mathrm{card}\left(\mathcal{N}\right)=N$, $ℳ$ be the set of anchors with $\mathrm{card}\left(ℳ\right)=M$, and ${ℳ}_i$ be the set of visible anchors of antenna i with $\mathrm{card}\left({ℳ}_i\right)=M_i$, ${ℳ}_i\subset ℳ$. Position states of the reference point $\mathrm{c}$ on the AGV, antenna $i\in \mathcal{N}$, and anchor $j\in ℳ$ in frame n are indicated by ${\mathbf{p}}_{\mathrm{c}}=\left(x_{\mathrm{c}},y_{\mathrm{c}},z_{\mathrm{c}}\right)$, ${\mathbf{p}}_i=\left(x_i,y_i,z_i\right)$, and ${\mathbf{p}}^{\left(j\right)}=\left(x^{\left(j\right)},y^{\left(j\right)},z^{\left(j\right)}\right)$, respectively. A typical signal blocked environment of the AGV using a radio positioning system is presented in Figure 2. The AGV is carrying some cargo, and the receiving antennas are installed on its flat body.

In unmanned transportation, the AGV is typically assumed to work in a flat and specific area without changing the height, pitch angle, and rolling angle. Therefore, the height, rolling angle, and pitch angle are regarded as known constants, and the problem can be considered as positioning in a two-dimensional (2D) plane. In addition to the horizontal coordinates of the reference point $(x_{\mathrm{c}},y_{\mathrm{c}})$, the clock bias $\delta t$ between the receiver and the anchor should also be estimated. Moreover, the yaw angle $\psi$ is needed for the position vector transformation from frame b to frame n.

Three-dimensional (3D) AGV positioning is also needed in some application scenarios such as transportation on rough roads, open-air mines, etc., in which AGVs are moving with varying height and/or rolling and pitch angle. For the height estimate, a good geometry of anchor height should be additionally guaranteed. The height can then be solved by adding an extra unknown with an increase in the number of visible anchors required. As for the rolling and pitch angles, it is not only a matter of increasing the unknowns and the number of visible anchors required. The coupling between multiple attitude angles also brings more severe nonlinearity, which may require more sophisticated optimization algorithms, which will be investigated in future work. Fortunately, unlike the yaw angle, which is not easy to be measured directly by sensors, the rolling and pitch angles are easy to be accurately obtained using accelerometers and other sensors; thus, it is easy to extend the proposed method to 3D cases.

Based on the previous discussion, in this work, we focus on the 2D problem, and the parameters to be estimated are the reference position $(x_{\mathrm{c}},y_{\mathrm{c}})$, receiver clock bias $\delta t$, and yaw angle $\psi$. The unknown state vector $\mathbf{x}$ can be written as

$$\mathbf{x}={\left[x_c,y_c,\delta t,\psi \right]}^T.$$

(1)

2.3. Measurement Model

In a broadcast positioning system, the receiver receives the signals transmitted from the anchors and measures the time of arrival with its local clock, which is not synchronized with the anchors. Assuming that all the anchors are synchronized to a system clock, the TOA measurement of the receiving antenna i from anchor j can be formulated as

$${\rho }_{ij}=\Vert {\mathbf{p}}^{\left(j\right)}-{\mathbf{p}}_i\Vert +\delta t_i+{\epsilon }_{ij},$$

(2)

where $\delta t_i$ is the receiver clock bias expressed by the equivalent signal propagation distance, and ${\epsilon }_{ij}$ is the TOA measurement noise. Note that the measurement noises come from various sources, including the geometry of anchors, multipath effect, clock inaccuracies, etc. They are random variables, and it is very difficult to get their exact probability distribution. Without loss of generality, all the measurements noises are assumed to be independent and identically distributed Gaussian white noise, i.e., ${\epsilon }_{ij}\sim \mathrm{N}\left(0,{\sigma }^2\right)$.

For our distributed multi-antenna positioning system, the position relationship between the antennas and the reference point is shown in Figure 3. For the distributed antennas, their positions relative to the reference point c are known and fixed in frame b, whereas the orientation of frame b varies with the motion of the AGV. The position of antenna i in frame n can be calculated as

$${\mathbf{p}}_i={\mathbf{p}}_{\mathrm{c}}+{\mathbf{R}}_b^n{\mathbf{l}}_{\mathrm{c}i}^b, i\in \mathcal{N},$$

(3)

where ${\mathbf{l}}_{\mathrm{c}i}^b$ is the vector from the reference point c to antenna i expressed in frame b, and ${\mathbf{R}}_b^n$ is the rotation matrix from frame b to frame n [18].

$${\mathbf{R}}_b^n=\left[\begin{array}{ccc}\sin \psi \cos \theta & \cos \psi \cos \varphi +\sin \psi \sin \theta \sin \varphi & -\cos \psi \sin \varphi +\sin \psi \sin \theta \cos \varphi \\ \cos \psi \cos \theta & -\sin \psi \cos \varphi +\cos \psi \sin \theta \sin \varphi & \sin \psi \sin \varphi +\cos \psi \sin \theta \cos \varphi \\ \sin \theta & -\cos \theta \sin \varphi & -\cos \theta \cos \varphi \end{array}\right],$$

(4)

where the rolling angle $\varphi$, pitch angle $\theta$, and yaw angle $\psi$ are defined as in Reference [18]. ${\mathbf{R}}_b^n$ is a function of yaw angle $\psi$ in this 2D positioning case.

Substituting the expression of ${\mathbf{R}}_b^n$ into Equation (3), the position of antenna i is a function of the reference position $(x_{\mathrm{c}},y_{\mathrm{c}})$ and the yaw angle $\psi$, as given by

$${\mathbf{p}}_i=f_i(x_{\mathrm{c}},y_{\mathrm{c}},\psi ).$$

(5)

With the same timing source, all antennas have the same clock bias $\delta t$. Substituting Equation (5) into Equation (2), the TOA measurement ${\rho }_{ij}$ of the j-th anchor in the subset of visible anchors ${ℳ}_i$ of antenna i is

$${\rho }_{ij}=\Vert {\mathbf{p}}^{\left(j\right)}-f_i(x_{\mathrm{c}},y_{\mathrm{c}},\psi )\Vert +\delta t+{\epsilon }_{ij}, i\in \mathcal{N}, j\in {ℳ}_i.$$

(6)

The unknowns in Equation (6) are $x_{\mathrm{c}}$, $y_{\mathrm{c}}$, $\delta t$, and $\psi$. Unlike the single-antenna positioning equation of Equation (2), the yaw angle $\psi$ is added.

For convenience, Equation (6) is rewritten into matrix form,

$$\mathbf{z}=\mathbf{h}(\mathbf{x}),$$

(7)

where $\mathbf{h}(\mathbf{x})$ is the observation function. The observation vector $\mathbf{z}$ consists of the TOA measurements of all $N$ antennas given by Equation (6). The total number of measurements is ${\displaystyle \sum _{i=1}^N{M}_i}$. For antenna i, the TOA measurement ${\mathbf{\rho }}_i$ is

$${\mathbf{\rho }}_i={\left[\begin{array}{cccc}{\rho }_{i1}& {\rho }_{i2}& \cdots & {\rho }_{i{M}_i}\end{array}\right]}^T.$$

(8)

$\mathbf{h}(\mathbf{x})$ consists of the TOA measurement function of all $N$ antennas. For antenna i, the TOA measurement function ${\mathbf{h}}_i(\mathbf{x})$ is

$${\mathbf{h}}_i(\mathbf{x})={\left[\begin{array}{cccc}{h}_{i1}(\mathbf{x})& h_{i2}(\mathbf{x})& \cdots & h_{i{M}_i}(\mathbf{x})\end{array}\right]}^T,$$

(9)

where $h_{ij}\left(\mathbf{x}\right)$ is expressed as the right side of Equation (6).

The aim of the localization problem in this paper is to estimate the position, orientation, and clock bias parameters for an AGV. Utilizing the measurements given by Equation (7), the positioning algorithm is proposed in the next section.

3. Positioning Algorithm

To estimate the unknowns in a set of equations, the number of independent equations must be greater than or equal to the number of unknowns in the system. Therefore, the corresponding requirement is that the total number of visible anchors for all the distributed antennas must be greater than or equal to the dimension of $\mathbf{x}$ in Equation (1). In addition, to estimate the yaw angle, there should be more than two antennas, where each observes at least one anchor. Let $\mathcal{X}=\left\{M_i>0,i\in \mathcal{N}\right\}$. If

$${\displaystyle \sum _{i=1}^N{M}_i}\ge 4 \mathrm{and} \mathrm{card}\left(\mathcal{X}\right)\ge 2,$$

(10)

the unknown parameters in $\mathbf{x}$ can be solved. We note that, for the positioning case with a single antenna, the number of visible anchors should be at least three to determine the two-axis position and the clock bias terms. The condition in Equation (10) shows that this requirement is relaxed in the multiple-antenna case, i.e., the number of visible anchors for each antenna can be less than three. As long as it is satisfied that the total number of visible anchors is greater than four, and no fewer than two antennas can see one or more anchors, the position of the target can be obtained. This greatly improves the availability of the system in practice.

According to the above models, the problem of AGV positioning is to find an optimal estimate $\hat{\mathbf{x}}$ with the minimum variance. The cost function is

$$J(\mathbf{x})={\displaystyle \sum _{i=1}^N{\displaystyle \sum _{j=1}^{M_i}{\left({\rho }_{ij}-\Vert {\mathbf{p}}^{\left(j\right)}-f_i(x_{\mathrm{c}},y_{\mathrm{c}},\psi )\Vert -\delta t\right)}^2}}.$$

(11)

Note that, because the TOA measurement noise of each antenna is assumed to be independent and identically distributed, the weights used to represent the contributions of the measurements are then chosen to be in unity and, thus, are omitted in the cost function. The parameters are estimated by solving the following minimization problem:

$$\hat{\mathbf{x}}=\underset{x}{\arg \min } J(\mathbf{x}).$$

(12)

The optimization problem in Equation (12) can be converted to a nonlinear least squares problem and solved by an iterative method such as the Newton iterative method [39].

However, the parameters to be estimated in this problem show high nonlinearities in the measurement equations. Therefore, to avoid a local minimum solution of the optimization problem, a reasonably good starting point is needed. For the time parameter, it is a linear item in the measurement model which can be set empirically or to zero for simplicity. For the space parameters, the initial position can be set according to the region formed by the anchors, e.g., the center of the region. In contrast, the yaw angle, which is expressed as a trigonometric function in the equations, has the strongest nonlinearity among all these parameters, and its initial value has great influence on the convergence of the optimization problem. This impact on convergence is demonstrated later in the simulations in Section 5.2. If there are some other sensors such as an INS that can provide the attitude information of the body, the iterative positioning algorithm can be applied directly. However, more generally, we have no prior information about the yaw angle, and the AGV may start at the same location but with completely different yaw angles; thus, its initial value may be a randomly guessed value from $-\pi$ to $\pi$ rad. Therefore, some measures are needed to narrow the initial attitude guess within a proper accuracy to guarantee convergence to the global optimum.

Toward that end, we employ a coarse estimation process to the algorithm. In this process, the antenna positions are jointly estimated as intermediate variables with the initial values of space and time parameters as described in the last paragraph, which avoids randomly guessing the initial yaw angle. Moreover, the initial reference position $\left(x_{\mathrm{c}|0},y_{\mathrm{c}|0}\right)$ and the yaw angle ${\psi }_{\mathrm{c}|0}$ are then calculated according to the transformation relationship of antenna positions between frame b and frame n. Finally, the coarse estimates of space, time, and attitude serve as initial values in the iterative positioning algorithm. Consequently, a refined estimate is obtained. With the coarse estimation process, the DMA positioning algorithm based on the LM algorithm can be divided into two steps, namely, the TSLM algorithm, as shown in Figure 4, which is presented in detail in the upcoming subsections.

3.1. Step 1: Coarse Estimation

In Step 1, coarsely estimated parameters are obtained by firstly estimating antenna positions and then determining the reference position and the rotation matrix utilizing the relationship between frame n and frame b.

3.1.1. Estimate Antenna Positions

According to the TOA measurements expressed in Equation (2), the position of antenna i or ${\mathbf{p}}_i$ can be estimated if there are sufficient independent measurements between antenna i and its visible anchors ${ℳ}_i$. As mentioned before, due to the obstacles both in the environment and on the AGV, the number of TOA measurements of a single antenna may not be enough for positioning. Thus, we adopt the idea of collaborative positioning in Reference [40], where a group of receivers with unknown locations and asynchronous clocks were localized by jointly using the TOA measurements of all these receivers and the distance measurements between them. What distinguishes our work from the existing collaborative localization system is that the distance measurements between antennas are calculated according to their known positions in frame b and the antennas share a synchronous clock. The parameters to be estimated are positions of the antennas and receiver clock bias $\delta t$. The unknown state vector $\mathbf{y}$ can be written as

$$\mathbf{y}={\left[\begin{array}{ccccc}\begin{array}{cc}{x}_1& y_1\end{array}& \begin{array}{cc}{x}_2& y_2\end{array}& \cdots & \begin{array}{cc}{x}_N& y_N\end{array}& \delta t\end{array}\right]}^T.$$

(13)

We can rewrite the TOA measurement ${\rho }_{ij}$ of the j-th anchor in the subset of visible anchors ${ℳ}_i$ of antenna i as

$${\rho }_{ij}=\Vert {\mathbf{p}}^{\left(j\right)}-{\mathbf{p}}_i\Vert +\delta t+{\epsilon }_{ij}, j\in {ℳ}_i,$$

(14)

where ${\epsilon }_{ij}\sim N\left(0,{\sigma }^2\right)$.

The distance measurement between antenna i and antenna k can be expressed as

$${\hat{r}}_{ik}=\Vert {\hat{\mathbf{p}}}_k-{\hat{\mathbf{p}}}_i\Vert .$$

(15)

The true distance is calculated by

$$r_{ik}=\Vert {\mathbf{l}}_{\mathrm{c}k}^b-{\mathbf{l}}_{\mathrm{c}i}^b\Vert ,$$

(16)

with the known positions of the distributed antennas in frame b ${\mathbf{l}}_{\mathrm{c}i}^b$, $i\in \mathcal{N}$.

Although $r_{ik}$ and $r_{ki}$ are expressed as two measurements, they should be counted as one. For $N$ antennas, there are $C\left(N,2\right)$ independent distance measurements. Let ${\mathcal{K}}_i$ be the set of antennas having effective distance measurements with antenna i, $\mathrm{card}\left({\mathcal{K}}_i\right)=K_i$, ${\mathcal{K}}_i\subset \mathcal{N}$.

If the sum of the number of the TOA measurements and distance measurements is greater than or equal to the dimension of $\mathbf{y}$, $\mathbf{y}$ can be solved. Thus, the requirement is

$${\displaystyle \sum _{i=1}^N{M}_i}+C\left(N,2\right)\ge 2N+1,$$

(17)

which can be easily met by reasonably deploying anchors, especially at the starting point of the vehicle. For example, there are four antennas mounted on the AGV and, thus, six distance measurements. According to Equation (17), the number of TOA measurements should be no less than three.

Jointly utilizing the TOA measurements and the distance measurements, an optimal estimate $\hat{\mathbf{y}}$ can be obtained by

$$\hat{\mathbf{y}}=\underset{\mathbf{y}}{\arg \min }\left[{\displaystyle \sum _{i=1}^N{\displaystyle \sum _{j=1}^{M_i}{\upsilon }_{ij}^{\rho }{\left({\rho }_{ij}-\Vert {\mathbf{p}}^{\left(j\right)}-{\hat{\mathbf{p}}}_i\Vert -\delta t\right)}^2+}}{\displaystyle \sum _{i=1}^N{\displaystyle \sum _{k=1}^{K_i}{\upsilon }_{ik}^r{\left(r_{ik}-\Vert {\hat{\mathbf{p}}}_k-{\hat{\mathbf{p}}}_i\Vert \right)}^2}}\right].$$

(18)

where ${\upsilon }_{ij}^{\rho }$ and ${\upsilon }_{ik}^r$ are the weights for the TOA measurements and the distance measurements, respectively. According to the assumptions for TOA measurement noises, ${\upsilon }_{ij}^{\rho }$ can be chosen as ${\upsilon }_{ij}^{\rho }={\sigma }^{-2}$. The distance measurements should have heavier weights due to their high accuracy. Here, we set ${\upsilon }_{ik}^r=\alpha {\sigma }^{-2}$, where $\alpha$ is a factor greater than 1.

By linearizing the measurement equations by Taylor expanding and keeping the first-order terms, we have

$$\delta \mathbf{q}=\mathbf{K}\delta \mathbf{y},$$

(19)

where $\delta \mathbf{y}=\mathbf{y}-\hat{\mathbf{y}}$.

$$\mathbf{K}=\left[\begin{array}{c}{\mathbf{K}}_{\rho }\\ {\mathbf{K}}_r\end{array}\right], \delta \mathbf{q}=\left[\begin{array}{c}\delta {\mathbf{q}}_{\rho }\\ \delta {\mathbf{q}}_r\end{array}\right].$$

(20)

The parameters related to the TOA measurements are denoted with subscript $\rho$.

$${\mathbf{K}}_{\rho }=\left[\begin{array}{ccccc}{\mathbf{K}}_{\rho }{}_{,1}& & & & 1\\ & {\mathbf{K}}_{\rho }{}_{,2}& & & 1\\ & & \ddots & & ⋮\\ & & & {\mathbf{K}}_{\rho }{}_{,N}& 1\end{array}\right], \delta {\mathbf{q}}_{\rho }=\left[\begin{array}{c}\delta {\mathbf{q}}_{\rho }{}_{,1}\\ \delta {\mathbf{q}}_{\rho }{}_{,2}\\ ⋮\\ \delta {\mathbf{q}}_{\rho }{}_{,N}\end{array}\right],$$

(21)

where

$${\mathbf{K}}_{\rho ,i}=\left[\begin{array}{c}-{\mathbf{u}}_{i1}\\ -{\mathbf{u}}_{i2}\\ ⋮\\ -{\mathbf{u}}_{i{M}_i}\end{array}\right], \delta {\mathbf{q}}_{\rho ,i}=\left[\begin{array}{c}\Delta {\rho }_{i1}\\ \Delta {\rho }_{i2}\\ ⋮\\ \Delta {\rho }_{i{M}_i}\end{array}\right],$$

(22)

where $\Delta {\rho }_{ij}={\rho }_{ij}-\Vert {\mathbf{p}}^{\left(j\right)}-{\hat{\mathbf{p}}}_i\Vert -\delta \hat{t}$, and

$${\mathbf{u}}_{ij}\triangleq \left[\frac{x^{(j)}-x_i}{d_{ij}},\frac{y^{(j)}-y_i}{d_{ij}}\right],$$

(23)

where $d_{ij}=\Vert {\mathbf{p}}^{\left(j\right)}-f_i(x_{\mathrm{c}},y_{\mathrm{c}},\psi )\Vert$.

The parameters related to the distance measurements are denoted with subscript $r$

$${\mathbf{K}}_r=\left[\begin{array}{c}{\mathbf{K}}_r{}_{,1}\\ {\mathbf{K}}_r{}_{,2}\\ ⋮\\ {\mathbf{K}}_r{}_{,N}\end{array}\right], \delta {\mathbf{q}}_r=\left[\begin{array}{c}\delta {\mathbf{q}}_r{}_{,1}\\ \delta {\mathbf{q}}_r{}_{,2}\\ ⋮\\ \delta {\mathbf{q}}_r{}_{,N}\end{array}\right],$$

(24)

where

$${\mathbf{K}}_{r,i}=k\text{-}\mathrm{th}\mathrm{row}\left[\begin{array}{cccccc}⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ \stackrel{i-1}{\overbrace{\begin{array}{cc}{0}_{1\times 2}& \cdots \end{array}}}& v_{ik}& \stackrel{index-i-1}{\overbrace{\begin{array}{cc}{0}_{1\times 2}& \cdots \end{array}}}& -v_{ik}& \stackrel{N-index}{\overbrace{\begin{array}{cc}{0}_{1\times 2}& \cdots \end{array}}}& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\end{array}\right], \delta {\mathbf{q}}_{r,i}=\left[\begin{array}{c}\Delta r_{i1}\\ \Delta r_{i2}\\ ⋮\\ \Delta r_{i{K}_i}\end{array}\right],$$

(25)

where index denotes the index of the k-th element of set ${\mathcal{K}}_i$ in set $\mathcal{N}$ and is supposed to be greater than i; $\Delta r_{ik}=r_{ik}-\Vert {\hat{\mathbf{p}}}_k-{\hat{\mathbf{p}}}_i\Vert$, and

$${\mathbf{v}}_{ik}\triangleq \left[\frac{x_k-x_i}{r_{ik}},\frac{y_k-y_i}{r_{ik}}\right], k\in {\mathcal{K}}_i.$$

(26)

Thus, the linear least square form of the cost function is

$$J\left(\delta \mathbf{y}\right)={\left(\delta \mathbf{q}-\mathbf{K}\delta \mathbf{y}\right)}^T\mathbf{W}\left(\delta \mathbf{q}-\mathbf{K}\delta \mathbf{y}\right),$$

(27)

where $\mathbf{W}$ is the weighting matrix.

$$\mathbf{W}=\left[\begin{array}{cc}{\sigma }^{-2}{I}_{\rho }& \\ & \alpha {\sigma }^{-2}{I}_r\end{array}\right],$$

(28)

where ${\mathbf{I}}_{\rho }$ and ${\mathbf{I}}_r$ are ${\displaystyle \sum _{i=1}^N{M}_i}$-order and $C\left(N,2\right)$-order identity matrices, respectively. The solution to estimate $\delta \mathbf{y}$ is given by

$$\delta \hat{\mathbf{y}}=\arg \underset{\delta \mathbf{y}}{\min } J(\delta y)={\left({\mathbf{K}}^T\mathbf{W}\mathbf{K}\right)}^{-1}{\mathbf{K}}^T\mathbf{W}\delta \mathbf{q}.$$

(29)

The estimate of $y$ can then be obtained by using the iterative algorithm. Consequently, the estimated $\left({\hat{x}}_i,{\hat{y}}_i\right)$ and $\delta \hat{t}$ are obtained and utilized in the next steps.

3.1.2. Coarsely Estimate Reference Position and Yaw Angle

In this subsection, we impose Equation (3) to determine the rotation matrix and the reference position, which is a typical problem in pattern analysis [37,41,42].

For our 2D problem, we denote the 2D positions of antenna i in frame b and frame n as ${\mathbf{c}}_i^b$ and ${\mathbf{s}}_i$, respectively, where ${\mathbf{c}}_i^b={\mathbf{l}}_{\mathrm{c}i}^b\left[1:2\right]$ and ${\mathbf{s}}_i={\left[\begin{array}{cc}{x}_i& y_i\end{array}\right]}^T$. We can denote the 2D position of the reference point in frame n as ${\mathbf{d}}_{\mathrm{c}}={\left[\begin{array}{cc}{x}_{\mathrm{c}}& y_{\mathrm{c}}\end{array}\right]}^T$ and the rotation matrix between the two frames as

$$\mathbf{P}=\left[\begin{array}{cc}\sin \psi & -\cos \psi \\ \cos \psi & \sin \psi \end{array}\right].$$

(30)

Then, Equation (3) can be rewritten as

$${\mathbf{s}}_i={\mathbf{d}}_{\mathrm{c}}+\mathbf{P}{\mathbf{c}}_{\mathrm{c}i}^b.$$

(31)

We construct the cost function as

$$J\left({\mathbf{d}}_{\mathrm{c}},\mathbf{P}\right)={\displaystyle \sum _{i=1}^N{\Vert {\mathbf{s}}_i-{\mathbf{d}}_{\mathrm{c}}-\mathbf{P}{\mathbf{c}}_{\mathrm{c}i}^b\Vert }^2}.$$

(32)

We assume that each estimated antenna position has a similar contribution to the cost function, and the weights are then chosen to be in unity and are omitted in the cost function. The problem can then be formulated as a least squares minimization problem,

$$\begin{gathered} \arg \underset{{\mathbf{d}}_{\mathrm{c}},\mathbf{P}}{\min } J=\arg \underset{{\mathbf{d}}_{\mathrm{c}},\mathbf{P}}{\min }{\displaystyle \sum _{i=1}^N{\Vert {\mathbf{s}}_i-{\mathbf{d}}_{\mathrm{c}}-\mathbf{P}{\mathbf{c}}_{\mathrm{c}i}^b\Vert }^2}, \\ \mathrm{s}.\mathrm{t}. {\mathbf{P}}^T\mathbf{P}=\mathbf{I}, \det \left(\mathbf{P}\right)=1. \end{gathered}$$

(33)

We can utilize the method introduced in Reference [37],

$${\mathbf{d}}_{\mathrm{c}}=\overline{\mathbf{s}}-\mathbf{P}\overline{\mathbf{c}},$$

(34)

where $\overline{\mathbf{s}}=\frac{1}{N}{\displaystyle \sum _{i=1}^N{\mathbf{s}}_i}$, $\overline{\mathbf{c}}=\frac{1}{N}{\displaystyle \sum _{i=1}^N{\mathbf{c}}_{\mathrm{c}i}^b}$.

The minimization problem in Equation (33) can then be equivalent to the following [37]:

$$\arg \underset{\mathbf{P}}{\max }\mathrm{trace}\left(\mathbf{P}{\displaystyle \sum _{i=1}^N{\tilde{\mathbf{c}}}_{\mathrm{c}i}^b{\tilde{\mathbf{s}}}_i^T}\right),$$

(35)

where ${\tilde{\mathbf{s}}}_i={\mathbf{s}}_i-\overline{\mathbf{s}}$, ${\tilde{\mathbf{c}}}_{\mathrm{c}i}^b={\mathbf{c}}_{\mathrm{c}i}^b-\overline{\mathbf{c}}$.

Let

$$\mathbf{U}\mathbf{\Sigma }{\mathbf{V}}^T=\mathrm{SVD}\left({\displaystyle \sum _{i=1}^N{\tilde{\mathbf{c}}}_{\mathrm{c}i}^b{\tilde{\mathbf{s}}}_i^T}\right).$$

(36)

By referring to the method based on singular value decomposition (SVD) in Reference [37],

$$\mathbf{P}=\mathbf{V}\left[\begin{array}{cc}1& 0\\ 0& \det {\left(\mathbf{V}{\mathbf{U}}^T\right)}^T\end{array}\right]{\mathbf{U}}^T.$$

(37)

Then,

$$\psi =\arctan \left(\frac{{\left[\mathbf{P}\right]}_{1,1}}{{\left[\mathbf{P}\right]}_{1,2}}\right).$$

(38)

Upon substituting $\mathbf{P}$ into Equation (34), ${\mathbf{d}}_{\mathrm{c}}$ can be solved.

At this point, the coarsely estimated reference position and the yaw angle are all obtained. Although there are some errors in the coarse estimates due to the intermediate transformation, they are not far from the optimum. Simulations validated that their accuracy is sufficient to initiate the iteration in Step 2 for producing a refined solution achieving CRLB.

3.2. Step 2: Refinement

To get the refined solutions, the coarse estimates denoted as $\left(x_{\mathrm{c}|0},y_{\mathrm{c}|0}\right)$, $\delta t_0$, and ${\psi }_0$ are applied to Equation (12).

Firstly, the model of Equation (7) is linearized. Let $\hat{\mathbf{x}}$ be the estimated values; then, the first-order Taylor expansion of $\mathbf{h}(\mathbf{x})$ at $\hat{\mathbf{x}}$ is

$$\mathbf{h}(\mathbf{x})=\mathbf{h}(\hat{\mathbf{x}})+{\frac{\partial \mathbf{h}}{\partial \mathbf{x}}|}_{\hat{\mathbf{x}}}(\mathbf{x}-\hat{\mathbf{x}})+\mathbf{o}(\mathbf{x}-\hat{\mathbf{x}}),$$

(39)

where $\mathbf{o}(\mathbf{x}-\hat{\mathbf{x}})$ is the high-order residual, which is ignored in the following text.

Let $\delta \mathbf{z}=\mathbf{h}\left(\mathbf{x}\right)-\mathbf{h}\left(\hat{\mathbf{x}}\right)$, $\delta \mathbf{x}=\mathbf{x}-\hat{\mathbf{x}}$, and $\mathbf{H}={\frac{\partial \mathbf{h}}{\partial \mathbf{x}}|}_{\hat{\mathbf{x}}}$; then, we have

$$\delta \mathbf{z}=\mathbf{H}\delta \mathbf{x}.$$

(40)

Define

$$w_{ij}\mathbf{\triangleq }\frac{\partial h_{ij}(\mathbf{x})}{\partial \psi },$$

(41)

and we have

$$\mathbf{H}=\left[\begin{array}{c}{\mathbf{H}}_1\\ {\mathbf{H}}_2\\ ⋮\\ {\mathbf{H}}_N\end{array}\right], \delta \mathbf{z}=\left[\begin{array}{c}\delta {\mathbf{z}}_1\\ \delta {\mathbf{z}}_2\\ ⋮\\ \delta {\mathbf{z}}_N\end{array}\right],$$

(42)

where

$${\mathbf{H}}_i=\left[\begin{array}{ccc}\begin{array}{c}-{\mathbf{u}}_{i1}\\ -{\mathbf{u}}_{i2}\\ ⋮\\ -{\mathbf{u}}_{i{M}_i}\end{array}& \begin{array}{c}1\\ 1\\ ⋮\\ 1\end{array}& \begin{array}{c}{w}_{i1}\\ w_{i2}\\ ⋮\\ w_i{}_{M_i}\end{array}\end{array}\right], \delta {\mathbf{z}}_i=\left[\begin{array}{c}{\rho }_{i1}-{\hat{\rho }}_{i1}\\ {\rho }_{i2}-{\hat{\rho }}_{i2}\\ ⋮\\ {\rho }_{i{M}_i}-{\hat{\rho }}_{i{M}_i}\end{array}\right],$$

(43)

in which,${\hat{\rho }}_{ij}=h_{ij}\left(\hat{\mathbf{x}}\right)$.

Thus, the linear least square form of Equation (11) is

$$J\left(\delta \mathbf{x}\right)={\left(\delta \mathbf{z}-\mathbf{H}\delta \mathbf{x}\right)}^T\left(\delta \mathbf{z}-H\delta \mathbf{x}\right).$$

(44)

The solution to estimate $\delta \mathbf{x}$ is given by

$$\delta \hat{\mathbf{x}}=\arg \underset{\delta \mathbf{x}}{\min } J(\delta \mathbf{x})={\left(H^{T}H\right)}^{-1}{H}^T\delta \mathbf{z}.$$

(45)

The estimate of $\mathbf{x}$ can then be obtained using the iterative algorithm.

During iteration, matrix $H$ in Equation (45) must be full rank, or the condition number of matrix $\mathbf{Q}\mathbf{=}{H}^{T}H$ cannot be too high to ensure convergence of the algorithm. To this end, we refer to the LM algorithm. The LM algorithm is a trust region method that improves the condition number of matrix $\mathbf{Q}$ by increasing coefficient matrix damping [43].

In the LM algorithm, a positive damping coefficient $\mu$ is added to the matrix $\mathbf{Q}$, that is,

$${\mathbf{Q}}^{\mathbf{\prime }}=H^{T}H+\mu \mathbf{I}.$$

(46)

${\mathbf{Q}}^{\mathbf{\prime }}$ is used to calculate the increment of the unknown parameters. It can be seen that, if $\mu =0$, it is the same as $\mathbf{Q}$. The damping coefficient should be adjusted after each iteration. Since $\mu$ is positive, the matrix ${\mathbf{Q}}^{\mathbf{\prime }}$ is always positive definite. In the iterative process, an error index is obtained by comparing the results of the current and the last iteration, which determines how to further update the damping coefficient. For a given $\mu$, the commonly used adjustment strategy is as follows: if the residual result can reduce the error index, continue to reduce $\mu$; otherwise, increase it. The details of the iterative positioning algorithm are shown in Algorithm 1.

Algorithm 1. Iterative positioning algorithm in Step 2

Input: TOA measurements ρi, $i\in \mathcal{N}$; anchor positions ${\mathbf{p}}^{\left(j\right)}$, $j\in {ℳ}_i, {ℳ}_i⸦ℳ$; initial unknowns ${\mathbf{x}}_0 = {[x_{\mathrm{c}|0}, y_{\mathrm{c}|0}, y_{\mathrm{c}|0}, \delta t_0, {\psi }_0]}^T$; maximum iterative number iter; convergence threshold thr > 0; known height h, rolling angle φ, pitch angle θ and the vectors from the antenna i to reference point c expressed in frame b ${\mathbf{l}}_{\mathrm{c}i}^b, i\in \mathcal{N}$; damping coefficient μ > 0; adjustment coefficient λ > 1.

Iteration:

for q = 1: iter

Calculate the position of each antenna:

${\hat{\mathbf{p}}}_i=f_i(x_{\mathrm{c}|q-1}, y_{\mathrm{c}|q-1}, {\psi }_{q-1}), i\in \mathcal{N}.$

Calculate residual: $\delta {\rho }_{ij}={\rho }_{ij}-\mathbf{\Vert }{\mathbf{p}}^{\left(j\right)}-{\hat{\mathbf{p}}}_i\mathbf{\Vert }-\delta t_{q-1}, i\in \mathcal{N}, ,j\in {ℳ}_i, {ℳ}_i⸦ℳ.$

Calculate uij and wij according to (23) and (41), respectively.

Form H and δz according to (42).

Calculate Q′ according to (46).

Calculate unknown incremental: $\delta {\mathbf{x}}_q ={{\mathbf{Q}}^{\mathbf{\prime }}}^{-1}{H}^T\delta \mathbf{z}.$

Update unknown estimate: ${\mathbf{x}}_q={\mathbf{x}}_{q-1}+\delta {\mathbf{x}}_q.$

if ‖δxq‖ < thr

exit;

else if ‖δxq‖ < ‖δxq−1‖

$\mu =\frac{\mu }{\lambda };$

else

μ = μλ.

end if

end if

end for

Output:xq

3.3. Computational Complexity

We estimate the computational complexity of the proposed positioning algorithm in this subsection. The complexity is shown in big O expressions in terms of the number of anchors M, the number of antennas N, and the position dimension 2.

To investigate the worst case, it is assumed that all N antennas can each receive all M anchor signals, although this situation is unlikely to occur due to the obstacles. Step 1 has $MN+C\left(N,2\right)$ measurements and $2N+1$ unknown states. In each iteration of the procedure to estimate the antennas positions, the complexities to calculate $\delta \mathbf{q}$, $\mathbf{K}$, and ${\left({\mathbf{K}}^T\mathbf{W}\mathbf{K}\right)}^{-1}$ are $O\left(MN+C\left(N,2\right)\right)$, $O\left(\left(2N+1\right)\left(MN+C\left(N,2\right)\right)\right)$, and $O\left(\left(2N+1\right){\left(MN+C\left(N,2\right)\right)}^2\right)$ flops, respectively. Moreover, the procedure to obtain the coarse estimate takes $O(2^3)$ flops for an SVD. Step 2 has $MN$ measurements and four unknown states. In each iteration of the procedure to estimate the refined solutions, the complexities to calculate $\delta \mathbf{z}$,$\mathbf{H}$, and ${Q^{\prime }}^{-1}$ are $O\left(MN\right)$, $O\left(4MN\right)$, and $O\left(4{\left(MN\right)}^2\right)$ flops, respectively.

The involved operations are matrix inversion, SVD, and other ordinary addition and multiplication steps. Although the algorithm is implemented on a personal computer (PC) for our simulation tests in Section 5, it is expected that the proposed method can be realized with reasonable computational cost in embedded systems commonly adopted by AGVs.

4. Performance Metrics

In this section, availability and accuracy are referred to as the performance metrics for the issue introduced above. The availability is the time percentage through which the positioning service is available, taking into consideration the needed accuracy and integrity [13]. In this paper, it is simplified to the solvability of the positioning parameter estimation, which depends on whether the number of effective independent measurements is greater than the minimum number of measurements required and the geometry of the visible anchors. In the DMA method, the availability is expressed as the ratio of time satisfying Equation (10) to the total time. As for the accuracy, CRLB and the dilution of precision (DOP) are usually used as a performance measure. Although there are mature theoretical models of CRLB and DOP for conventional single-antenna positioning, we need to specifically derive the models for the DMA method.

4.1. CRLB

To evaluate the estimator, we derive the CRLB for the DMA method, which is the lower bound of the error variance of the unbiased estimator. The error variance of $\mathbf{x}\left[l\right]$ is as follows [44]:

$$\mathrm{var}\left(\hat{\mathbf{x}}\left[l\right]\right)\ge CRLB\left(\hat{\mathbf{x}}\left[l\right]\right)={\left[{\mathrm{FIM}}^{-1}(\mathbf{x})\right]}_{l,l},$$

(47)

where $\mathrm{FIM}$ is the Fisher information matrix.

The entries of the Fisher information matrix are

$${[\mathrm{FIM}(\mathbf{x})]}_{l_1,l_2}=-E\left[\frac{{\partial }^2\ln p(z;\mathbf{x})}{\partial \mathbf{x}\left[l_1\right]\partial \mathbf{x}\left[l_2\right]}\right],$$

(48)

where $p(z;\mathbf{x})$ is the likelihood function.

We can define $g_n(\mathbf{x})$ as the n-th residual of Equation (7), where $n=1,2,\cdots ,{\displaystyle \sum _{i=1}^N{M}_i}$. Thus, $g_n(\mathbf{x})\sim \mathrm{N}\left(0,{\sigma }^2\right)$. The probability density function of each TOA measurement $z[n]$ can be expressed as

$$p(z[n];\mathbf{x})=\frac{1}{\sqrt{2\pi {\sigma }^2}}\exp \left(-\frac{1}{2{\sigma }^2}{g}_n{(\mathbf{x})}^2\right).$$

(49)

Each measurement is measured independently; thus,

$$p(z;\mathbf{x})={\displaystyle \prod _{n=1}^{L}p(z\left[n\right];\mathbf{x})}=\frac{1}{{\left(2\pi {\sigma }^2\right)}^{\frac{L}{2}}}\exp \left(-\frac{1}{2{\sigma }^2}{\displaystyle \sum _{n=1}^L{\left(g_n(\mathbf{x})\right)}^2}\right),$$

(50)

where $L={\displaystyle \sum _{i=1}^N{M}_i}$.

Therefore, the Fisher information matrix $\mathrm{FIM}$ is

$$\mathrm{FIM}=-E\left[\frac{{\partial }^2\ln p(z;\mathbf{x})}{\partial \mathbf{x}\partial {\mathbf{x}}^{{\rm T}}}\right]=\frac{1}{{\sigma }^2}{\displaystyle \sum _{n=1}^L{\left(\frac{\partial g_n(\mathbf{x})}{\partial \mathbf{x}}\right)}^T\frac{\partial g_n(\mathbf{x})}{\partial \mathbf{x}}}=\frac{1}{{\sigma }^2}{H}^{T}H,$$

(51)

where $H$ is defined in Equation (42).

Consequently, the diagonal element of ${\mathrm{FIM}}^{-1}$ is the minimum variance that can be achieved theoretically in unbiased estimation for the DMA positioning method. To simplify and save space, the noise of each measurement is set to be independent and identically distributed. If the signal transmitted from an anchor or received by an antenna is interfered, it can be considered that the corresponding noise increases, resulting in a decrease in precision. Furthermore, the multipath has a complicated effect on TOA measurements. With respect to CRLB, the multipath equivalently increases the measurement noise, and then reduces the positioning accuracy. As there is a certain amount of work in multipath modeling, it will be further studied in future work.

Note that, in Equation (51), FIM is determined by $H$ with the same measurement noise. Moreover, $H$ is affected by the relative position between antennas and their visible anchors. Thus, the position and quantity of antennas will affect positioning accuracy. However, we mainly focus on providing a solution for AGV positioning with severe blockages in this work. The quantitative influence of antenna configuration and the criteria of antenna position and quantity configuration will be studied in future work.

Based on the theoretical CRLB, the positioning performance of the proposed method is tested using a simulation in Section 5.

4.2. DMA-HDOP

In this subsection, DOP is introduced to analyze the positioning accuracy. Considering the measurement noises $\mathbf{\epsilon }$ and adding the estimate error ${\mathbf{\epsilon }}_x$ caused by $\mathbf{\epsilon }$, Equation (40) can be written as follows [45]:

$$\delta z=H\left(\delta x+{\mathbf{\epsilon }}_x\right)+\mathbf{\epsilon }.$$

(52)

Then,

$$\delta x+{\mathbf{\epsilon }}_x={\left(H^{T}H\right)}^{-1}{H}^T\delta z-{\left(H^{T}H\right)}^{-1}{H}^T\mathbf{\epsilon }.$$

(53)

According to Equation (45), we have

$${\mathbf{\epsilon }}_x=-{\left(H^{T}H\right)}^{-1}{H}^T\mathbf{\epsilon }.$$

(54)

The covariance matrix of positioning error is

$$\begin{array}{ll}\mathrm{cov}\left({\mathbf{\epsilon }}_x\right)& =E\left[{\mathbf{\epsilon }}_x{\mathbf{\epsilon }}_x^T\right]\\ & =E\left[-{\left(H^{T}H\right)}^{-1}{H}^T\mathbf{\epsilon }{\left(-{\left(H^{T}H\right)}^{-1}{H}^T\mathbf{\epsilon }\right)}^T\right]\\ & ={\left(H^{T}H\right)}^{-1}\mathrm{cov}\left(\mathbf{\epsilon }\right)\end{array}$$

(55)

According to the discussion of the TOA measurements noise in Section 2.3, the covariance matrix of $\mathbf{\epsilon }$ is $\mathrm{cov}\left(\mathbf{\epsilon }\right)={\sigma }^2\mathbf{I}$. Thus,

$$\mathrm{cov}\left({\mathbf{\epsilon }}_x\right)={\left(H^{T}H\right)}^{-1}{\sigma }^2.$$

(56)

This can be denoted as

$$G={\left(H^{T}H\right)}^{-1}=\left[\begin{array}{cccc}{g}_{11}& g_{12}& g_{13}& g_{14}\\ g_{21}& g_{22}& g_{23}& g_{24}\\ g_{31}& g_{32}& g_{33}& g_{34}\\ g_{41}& g_{42}& g_{43}& g_{44}\end{array}\right].$$

(57)

The diagonal elements of $G$ are the variance of state estimation errors, while the non-diagonal elements represent the correlation between these states. $G$ indicates the magnification of the measurement error. With the same measurement error, a greater $G$ results in a lower positioning accuracy. The DMA-HDOP can be expressed as

$$\mathrm{DMA}\text{-}\mathrm{HDOP}=\sqrt{g_{11}+g_{22}}.$$

(58)

In Section 5, the positioning accuracy is analyzed based on the DMA-HDOP model.

5. Simulation Results

To analyze the performance of the proposed method, several tests were conducted based on one simulation scene. As shown in Figure 5, we set up a scene with six anchors, an AGV, and two wall-like obstacles along both sides in the environment. There is some cargo on the flat of the AGV. Its length and width are smaller than that of the flat, which means that its dimensions are not outside the AGV. Both the walls and the cargo on board can block the positioning signals. Four strictly synchronized receiving antennas are mounted on the corners of the AGV. We set the center of the AGV as the reference point.

For this scene, $M=6$, $N=4$. The height of both walls was 10 m, the width of the road between the walls was 8 m, and the coordinates of four antennas in frame b ${\mathbf{l}}_{\mathrm{c}i}^b$ were (4, 2, 0) m, (4, −2, 0) m, (−4, −2, 0) m and (−4, 2, 0) m, respectively. The coordinates of the six anchors in frame n are presented in

Table 1. The coordinates of the six anchors in frame n (m).

Anchor No.	1	2	3	4	5	6
East	−20	−20	20	20	60	60
North	15	−5	15	−5	15	−5
Up	25	25	25	25	25	25

. The length, width, and height of the cargo on the vehicle were 7 m, 3 m, and 6 m, respectively. We ignored the multipath effect here and the TOA measurement noise was considered as independent zero-mean Gaussian white noise with a standard deviation of $\sigma =0.2 \mathrm{m}$. The clock bias between the receiver and the anchors was 149.90 m. The visible anchor set of each antenna and the corresponding TOA measurements from each antenna were then generated at a 1-Hz update rate.

Our simulation was carried out in MATLAB R2017a on a PC with an Intel(R) core(TM) i7-6500U central processing unit (CPU) at 2.5 GHz. By using the simulated measurements, we examine the theoretical capability of the proposed method in terms of availability and accuracy in Section 5.1. In Section 5.2, the performance of the TSLM algorithm and the influence of the initial value on the iterative positioning algorithm are tested.

5.1. Theoretical Capability of DMA Method

Based on the above configuration, we carried out a dynamic simulation test to assess the theoretical capability of the DMA method. The trajectory of the center and the attitude of the AGV were plotted in a 2D projection, as shown in Figure 6. The vehicle moved along the road from west to east with varying yaw angle and north velocity. The vehicle started from (−4.75, 4.53, 2.4) m with an initial attitude angle of (0, 0, 1.47) rad. During the movement, the height, rolling angle, and pitch angle of the vehicle remained unchanged. The duration of the movement was 180 s, which was divided into 11 segments according to different motion states. With different position and attitude of the AGV, each moment of simulation had a different geometric distribution of the anchors and actually represented many different situations. The east velocity $v_E$, north velocity $v_N$, yaw angle rate $\omega$, and duration $T$ of each segment are shown in

Table 2. The parameters of the AGV movement.

Segment	1	2	3	4	5	6	7	8	9	10	11
$v_E$ (m/s)	0.25	0.25	0.25	0.25	0.375	0.125	0.25	0.25	0.25	0.25	0.25
$v_N$ (m/s)	0.025	0	−0.05	0	0.05	0.05	0.05	0	−0.013	0	0
$\omega$ (rad/s)	0	−0.015	0	0.019	−0.018	0	0	−0.008	0	0.002	0
$T$ (s)	5	20	5	30	10	5	10	30	10	30	25

. Utilizing the above scene and parameters, the visible anchor set of each antenna and the corresponding TOA measurements from each antenna of every moment were generated.

5.1.1. Positioning Availability

As mentioned in Section 4, we analyzed the availability by the ratio of time satisfying Equation (10) to the total time. Due to the movement of the vehicle, the numbers of visible anchors of each antenna changed during the test, as shown in Figure 7.

In Figure 7, the minimum numbers of visible anchors required for the single-antenna method and DMA positioning method are also plotted as the green dotted line and blue dotted line, respectively. The lines denoting the visible anchor numbers of antenna 2 and antenna 3 were always below the green dotted line, while antenna 1 and antenna 4 had more than three visible anchors at some moments. That is to say, single-antenna positioning was not available during parts of this test. On the other hand, the line of the total number of visible anchors was always above the blue dotted line. This indicates a possible positioning for the AGV if all measurements were used. Furthermore, there were always more than two antennas with one or more visible anchors, indicating that the yaw angle could be determined. Therefore, the condition in Equation (10) was satisfied throughout the test. We combined the numbers of visible anchors of antennas 1 and 4, and calculated the proportion of time when the number of visible anchors exceeded three (the green dotted line in Figure 7). The result was 70.6%, which means that the single-antenna method was only available during 70.6% of the entire period. With respect to the DMA method, the availability increased to 100% because the total number of visible anchors was above four (the blue dotted line in Figure 7) throughout the simulation period. This verifies that, with obstacles both in the environment and on the AGV, the proposed DMA method significantly improved the positioning availability in this simulation.

5.1.2. Positioning Accuracy

In this section, we analyze the positioning accuracy using DMA-HDOP and the theoretical CRLB derived in Section 4.

Based on the above scene and parameter configuration, the DMA-HDOP was calculated according to Equation (58), as shown in Figure 8. The total number of visible anchors is also shown for comparison. It can be seen from Figure 8 that the change in DMA-HDOP value was consistent with that of the number of visible anchors. A greater number of visible anchors led to a smaller DMA-HDOP value; thus, with the same measurement error, we obtained a smaller positioning error. Note that the number of visible anchors did not change between 74 s and 114 s, while the DMA-HDOP value did. This is because, during this period, the geometry of visible anchors changed when the AGV moved. Taking 84 s and 100 s as examples, as shown in Figure 9, although the numbers of visible anchors were the same, the geometric distribution was more uniform at 84 s due to the reception by the distributed multi-antenna.

Next, the simulated measurements were used to calculate the theoretical CRLB according to Equation (47). As shown in Figure 10, the CRLB of DMA positioning was also consistent with the numbers of visible anchors at each moment. In addition, in line with the analysis of Figure 9, the error in the north direction was larger between 90 s and 114 s. Overall, according to CRLB, with the standard deviation of the TOA measurement noise as $\sigma =0.2 \mathrm{m}$, the DMA method could theoretically achieve decimeter-level accuracy.

5.2. Performance of TSLM Algorithm

In this subsection, we conducted a static test at the start point of the trajectory in Figure 7 to test the performance of TSLM algorithm and the influence of the initial yaw angle on the iterative algorithm. In this test, the true coordinates of the vehicle were (−4.75, 4.53, 2.4) m with the true attitude angles as (0, 0, 1.47) rad. The 2D projection is shown in Figure 11, with the same icons and symbols as those in Figure 9.

Because of the blockage by the obstacles both in the environment and on the AGV, the numbers of signals received by the antennas on the vehicle were two, two, one, and one. None of the antennas had more than three visible anchors; thus, none of them could complete positioning on their own. However, the total number of visible anchors was greater than four and all four antennas had their visible anchor. Equation (10) was satisfied; therefore, the proposed positioning algorithm could be applied. Furthermore, the total number of visible anchors satisfied Equation (17); thus, the coarse estimation process could be applied as well.

To compare its positioning performance with CRLB, the positioning errors were calculated by comparing the simulation results with the true values of the simulation setting. A Monte Carlo simulation over 1000 tests was implemented to obtain the root-mean-square error (RMSE). For each parameter $\xi$, the RMSE can be calculated by

$$\mathrm{RMSE}=\sqrt{\frac{{\displaystyle \sum _{i=1}^{N_{\mathrm{test}}}{\left(\xi -{\hat{\xi }}_i\right)}^2}}{N_{\mathrm{test}}}},$$

(59)

where ${\hat{\xi }}_i$ is the estimation of $\xi$ in test i, $N_{\mathrm{test}}$ is the number of Monte Carlo simulation tests, and $N_{\mathrm{test}}=1000$ in this simulation.

In order to evaluate the impact of initial yaw angle and validate the necessity and effectiveness of Step 1 in the proposed algorithm, we firstly imported the simulated measurements directly to Step 2, i.e., the iterative positioning algorithm. For Step 2, $\delta t_0$ was set to 0, and $\left(x_{\mathrm{c}|0},y_{\mathrm{c}|0}\right)$ was set as the center of the range formed by the anchors. With the initial yaw angle bias varying from −π rad to π rad, the positioning results were calculated. By substituting simulation results into Equation (59), the positioning RMSEs of Step 2 could be determined, as presented in Figure 12. The positioning RMSEs fluctuated severely with the initial yaw angle bias, which indicates that the iterative positioning algorithm was sensitive to the initial yaw angle and was consistent with the analysis in the Section 3.

The simulated measurements were then imported into TSLM, with $\delta t_0=0 \mathrm{m}$, and initial positions of the distributed antennas as four random points near the center of the area formed by the anchors. The RMSE of the yaw angle obtained in Step 1 was 0.1829 rad, which means that the coarse solution was not far from the optimum. By using coarsely estimated solutions of Step 1, the refined solutions were obtained.

Utilizing the above simulation results, CRLB was computed based on Equation (44) using the true positions of AGV and anchors in the simulation settings. The positioning RMSEs of the proposed algorithm from 1000 Monte Carlo simulation tests were computed based on Equation (59). The positioning RMSEs of our algorithm and the theoretical CRLB of the DMA method are presented in

Table 3. The positioning RMSEs of the proposed algorithm and the CRLB of the DMA method. This table lists the positioning RMSEs of the proposed TSLM algorithm and Step 2 only of the TSLM algorithm. The results of Step 2 only are given with different initial yaw angles. CRLB is listed as a benchmark.

Parameters	RMSE				CRLB
	TSLM	Only Step 2
		$\mathbf{bias}=-\mathit{\pi } \mathbf{rad}$	$\mathbf{bias}=\mathit{\pi }/2 \mathbf{rad}$	$\mathbf{bias}=\mathit{\pi }/10 \mathbf{rad}$
x (m)	0.146	2.053	2.053	0.146	0.144
y (m)	0.244	0.546	0.549	0.243	0.242
Clock bias (m)	0.099	3.980	3.981	0.099	0.099
Yaw angle (rad)	0.033	2.924	2.924	0.033	0.032

. In this table, the results of the “only Step 2” column in the table were calculated with different representative initial values of ${\psi }_{\mathrm{c}|0}$ as −1.67 rad, 3.04 rad, and 1.79 rad, representing $-\pi$ rad, $\pi /2$ rad, and $\pi /10$ rad bias with respect to the true yaw angle, respectively. When the bias was $\pi /10$ rad, the positioning RMSEs were also close to the theoretical CRLB, as with those of the TSLM algorithm. However, when the bias was $-\pi$ rad or $\pi /2$ rad, the positioning RMSEs from Step 2 only were far greater than for CRLB. This result indicates that, when the initial value is far away from the true value, the algorithm may converge to the local optimal solution, presenting the wrong position.

The simulation results verify that, without the prior information of the yaw angle, the TSLM algorithm avoids the initial yaw angle problem, and it can obtain a positioning accuracy close to the theoretical lower bound by using coarse estimation. Furthermore, if there is prior attitude information of the body, using Step 2 only can also achieve the desired accuracy.

6. Conclusions

Autonomous high-precision positioning and navigation of AGV are desperately needed in unmanned warehouses, ports, and similar environments. Radio broadcast positioning systems can be employed in these AGV positioning applications. Due to the limitation of the flat structure of AGV, the receiving antennas cannot be mounted at the highest point of the vehicle. The positioning signals are not only affected by the obstacles from the environment but also blocked by the AGV cargo. The availability and accuracy of the positioning system are severely challenged.

To tackle the above problem, a distributed multi-antenna positioning method was proposed, which utilizes the synchronous reception by multiple antennas. In this method, the measurements of the distributed multiple antennas are transformed to a function of the reference position by estimating the attitude of the body. To estimate the position, receiver clock bias, and attitude angle, a two-step positioning algorithm TSLM was proposed. Firstly, the positions of the distributed antennas and the clock bias are jointly estimated. Then, the reference position and the yaw angle are estimated according to the transformation of antenna positions between frame b and frame n. Through Step 1, the unknowns are coarsely estimated to a proper accuracy, so that they can be applied as initial values for Step 2 to guarantee its convergence to the global optimum. Finally, a refined positioning result can be obtained iteratively in Step 2. Simulation results demonstrated that, compared with the conventional single-antenna localization, the positioning availability was significantly improved. Moreover, with the standard deviation of the TOA measurement noise as 0.2 m, the DMA method could achieve decimeter-level accuracy. Simulation tests verified that the TSLM algorithm can provide a position estimation with an accuracy close to CRLB without the requirement for initial attitude information. Furthermore, with prior information of the yaw angle, Step 2 can achieve similar results to TSLM.

In the future, more factors such as multipath and non-line-of-sight (NLOS) will be considered in algorithm design and performance analysis, and more studies will be done on measurement noise modeling. In addition, the theoretical analysis of the influence of the initial values and the effect of the proposed algorithm will be carried out. The nonlinear constrained optimization problem will be further studied to extend the proposed method to 3D positioning and attitude determination. Improvements on computational complexity and other algorithms with possibly higher efficiency and better robustness will be investigated. Moreover, the quantitative influence of antenna configuration and the criteria of antenna position and quantity configuration will also be studied.