Abstract
In view of the practical engineering problem of dynamic positioning of rotor UAV pod and the strong nonlinear problem in dynamic operation of UAV, a real-time estimation of the dynamic position of the rotor UAV pod is proposed by using the SR-UKF algorithm. This algorithm uses the nonlinear propagation of UT transform to generate the point set to maintain the mean and covariance information and thus achieves higher precision. Moreover, it uses the square root of covariance instead of covariance to participate in the recursive operation, thus improving the numerical stability of the filter and reducing the amount of computation. In this work, a pseudosatellite positioning platform was constructed in a field site in Nanjing. Based on evaluation of the space geometry of pseudosatellite base station, the accuracy of several nonlinear filtering algorithms was analyzed and evaluated using GPS RTK positioning results. It was found that the SR-UKF algorithm was the most accurate and efficient algorithm. It can meet the requirements of dynamic positioning of the rotor-wing UAV pod. The experiment results of this algorithm provide a more efficient positioning algorithm and implementation means for the actual engineering of UAV pod positioning using pseudosatellite system, which has high application value.
1. Introduction
In recent years, more and more scientific research and engineering projects have focused on carriers for the multirotor UAV technology [1–4]. When detecting unexploded ordnance that is below ground, investigators usually travel by plane and use noncontact detection to ensure safety. [5–7]. However, in order to improve the resolution of the detection, the use of low altitude flight, which requires a high level of driver skills, the rotor UAV has very high controllability and stability of flight [8, 9] and is the ideal carrier for unexploded bomb detection. In order to improve the detection accuracy and reliability, a flexible connection between the sensor and the UAV body is fixed. Based on this, the study target was the UAV’s pod and not the UAV itself.
Generally, the UAV is equipped with a low precision single-frequency GPS positioning antenna [10, 11], but the sensor in the case is attached through a soft connection. As a result, it cannot maintain accurate relative position with the UAV due to maneuvering of the UAV and the influence of wind. Thus, it is necessary to carry the sensor’s pod for a certain accuracy of the individual positioning. In order to ensure the accuracy of unexploded bomb detection, the positioning accuracy of the sensor pod should be better than 10 cm. GPS/Beidou RTK is a common real-time high-precision positioning device, which has a three-dimensional accuracy of up to 1-2 cm [12–15]. However, the satellite positioning is heavily dependent on the number of visible satellites. In complex terrain conditions, such as deep valleys, dense forests, and high slope environment, its positioning accuracy is seriously affected, and it cannot even locate the target [16]. Pseudosatellite is a kind of positioning equipment which can flexibly lay out the transmitting base station, which can deal with the complex environment well. This paper adopted a 2.4 GHz wireless ranging device as pseudosatellite location equipment, and a specific position estimation algorithm was designed to locate the UAV suspended pod by using its output distance value. In the maneuvering process, the trajectory of the UAV is usually nonlinear [17], which requires a suitable nonlinear filtering algorithm for its location.
Nonlinear filtering algorithm is widely used in real engineering problems, such as precision orbit determination and time synchronization [18], satellite positioning [19], vehicle dynamic navigation [20], and wireless positioning [21]. With the development of theoretical mathematics and the traction of control engineering, nonlinear filtering algorithms suitable for different needs are developing very quickly. The common nonlinear filtering algorithms can be divided into three categories according to the mathematical principle involved. The first class is the analytic function approximation method, of which the most representative example is the EKF (Extended Kalman Filter) [22]. The EKF algorithm is based on Taylor series expansion processing nonlinear method. As it is simple and easy to achieve the characteristics, this algorithm is widely used by the engineering community. However, there are many complicated problems in linearization error and high dimensional solution Jacobian matrix [23, 24]. The second class is based on deterministic sampling, the most representative of which is the UKF (unscented Kalman filter). This algorithm obtains the mean and covariance of the state space equation using the UT (unscented transform), and substitutes into the standard Kalman filter to solve the calculation [25]. This method can obtain a higher accuracy than EKF, and does not solve the Jacobian matrix. The third class is based on the uncertainty sampling method. The most representative example is PF (particle filter) with Monte Carlo sampling method instead of the UT-based sampling method, which has better resistance and adaptive ability to deal with strong nonlinear/nongaussian problems. While this algorithm is still in rapid development, there are some problems such as filtering divergence from random sampling [26], large amount of particles, [27], particle degradation [28] and sample exhaustion [29], and so on. Hence, it is not widely used in practical engineering. In conclusion, the UKF algorithm has high-precision and low computational complexity, but it is time-consuming. It is proposed to use this method to dynamically locate the rotor-wing UAV pod in this paper. At the same time, according to the requirement of nonlinearity and higher calculation efficiency of UAV’s motion process, the traditional UKF algorithm is improved so as to obtain the positioning result with higher precision and better stability in practical engineering.
The structure of this paper is as follows: The second section introduces the principle of pseudosatellite ranging equipment including hardware composition, static rendezvous and positioning principles, pseudosatellite space constellation distribution, and evaluation of three aspects. The third section introduces the traditional UKF algorithm and the SR-UKF algorithm for improving it. The fourth section describes the field test analysis, including the design of the test site, the evaluation results of the spatial constellation distribution, and the evaluation of accuracy of the SR-UKF algorithm results. The fifth section summarizes and concludes the work of this paper from the aspects of theoretical improvement, technical method, and engineering test effect. Further, this final section analyzes the shortcomings of the theory and technology and the problems that require further improvement.
2. Pseudosatellite Positioning Equipment and Its Positioning Principle
2.1. Pseudosatellite Ranging System
A pseudosatellite ranging system provided by Beijing Digital Space Technology Co., Ltd., was used in the experiment, which can provide a high-precision observation of the distance between the base stations and the observation stations. This paper intends to use these distance observations to design a specific dynamic filtering algorithm to meet the needs of UAV pod positioning.
The pseudosatellite system used in this paper uses the signal of 2.4 GHz band and provides distance observation at the same time. As shown in Figure 1, the experimental setup includes 1 observation station and 5 base stations, and two-way ranging can be obtained between the base station and the observation station. At the same time, the system contains a special base station located on the top of the mobile measuring vehicle (blue base station in Figure 1). The base station can measure the distance between the observation station and the collected distance between the observation stations to the base stations, which is used as the next dynamic positioning input.
The hardware devices used in the pseudosatellite base stations and observation stations in this experiment are similar, as shown in Figure 2. A mobile power supply, usually 10000 mA power supply under full power, can continue to power the setup for more than 2 hours, which can ensure completion of the experiment.
2.2. Pseudosatellite Observation Equation and Least-Squares Solution
The principle of pseudosatellite positioning is based on distance measurement of the space rear intersection, which is similar to the principle of satellite positioning. The pseudosatellite single point positioning is based on observation of the distance between the pseudosatellite transmitting base station and the user receiver antenna. The coordinates of the observation point corresponding to the observatory are determined on the basis of the known accurate pseudosatellite base station coordinates.
It is known that the base station coordinates of pseudosatellites are , in which is the number of pseudosatellite base stations. For the pseudosatellite base station, the distance between it and the observation station antenna is measured to be
where is the distance between the pseudosatellite base station and the observatory and the pseudosatellite base station coordinates are precisely known. Then, to solve the observatory coordinates would require at least three pseudosatellite base stations. Assuming that there are more than three pseudosatellite base stations and observation stations (), there are the following equations:
The nonlinear equations (2) are linearized, that is, the first-order Taylor formula is expanded. Assuming that the initial approximate position of the observatory is , then the ranging equation can have the following expression:
Let . The above formula can be rewritten as
Taking as the observation and as the design matrix, wherein , is the unknown that needs to be solved, and is the error vector, the following observation equation can be obtained:
Since (5) is a linear equation, the least-squares solution can be obtained as
2.3. Estimation of Spatial Distribution of Pseudosatellite Base Station (DOP)
DOP (dilution of precision) is an important index to measure the performance of satellite navigation system, which reflects the degree of contribution of observational information to the solution of unknown parameters. DOP has the dual meanings of measurement and matrix mathematics: the ratio of pseudodistance location error caused by the geometrical structure of GNSS satellites in the measurement and the trace characteristics of the inverse matrix of least-squares adjustment in algebra. In pseudosatellite location, the spatial distribution of pseudosatellite base station has great influence on the location result. Thus, this paper attempts to use DOP value to evaluate the spatial distribution of pseudosatellite independent base station.
In formula (5) of the previous section, the design matrix of the observation equation is
where , , (i = 1, 2, …, n) represent the cosine of the direction vector between the receiver position and the pseudosatellite base station position, i.e.,
is the component of the location of pseudosatellite base station in the -direction, indicates the component of the observatory position in -direction, and is the direction vector of the station and the position of pseudosatellite base stations. It is assumed that the error covariance matrix of the pseudodistance observation error value is (pseudodistance observation precision). According to the law of error propagation, the covariance matrix of is as shown as follows:
Assuming P is the unit matrix, formula (9) has the following form:
Then, the indexes PDOP, HDOP, and VDOP can be represented by the following formulas:
The above is the DOP value of several of the most common expressions. By analysis of formulas (10) and (11)-(14), the following conclusion can be obtained: the smaller the DOP value, the smaller the covariance matrix of the estimated value X. That is, a higher parameter estimation accuracy can be achieved.
3. Research on Pseudosatellite Dynamic Location Algorithm
In order to eliminate systematic or random errors in the observation, considering the dynamic characteristics of the UAV pod, the method of filtering is adopted in the process of locating rather than the least-squares batch processing mentioned earlier. Filtering is one of the commonly used data processing methods in navigation, positioning, and measurement engineering. The purpose of filtering is to eliminate or weaken the interference of error by special processing of a series of measurement (observation) data with errors, so as to recover a position parameter estimation problem which is disturbed by noise as much as possible.
The main difference between pseudosatellite positioning system and satellite location is that the pseudosatellite emitter is relatively close to the user receiver. This may bring some favorable aspects in the positioning process, such as the influence of ionosphere on pseudosatellite observation. However, it may also cause some unfavorable effects, such as near-far effect and linearization error. In this section, we first introduce the most commonly used Kalman filter algorithm in satellite positioning. Then, according to the actual nonlinear problem of locating model in this research, the commonly used EKF algorithm and the UKF algorithm with higher theoretical accuracy are introduced. To account for the uncertainty of the trajectory of UAV pod and the instability of the distance observation of pseudosatellite system in this study, a more suitable SR-UKF (Square Root Unscented Kalman Filter) algorithm was designed by the authors.
3.1. Standard Kalman Filter and EKF Algorithm
Kalman filter was first proposed in 1960. It is a filtering algorithm which obtains the required parameters by estimating algorithm from the noise-containing observations, and is a real-time recursive method which can be realized by computer. Unlike the least-squares position estimation algorithm mentioned earlier, Kalman filter can realize the dynamic position estimation of motion vectors and has higher parameter estimation efficiency. The basic principle of standard Kalman filter has been covered in previous literature cited in the introduction and explained in detail. The unified description of the state space equation and the observation and updates under the Bayesian framework are given here, and the formula is not deduced.
The state equation and the measurement equations of the pseudosatellite positioning system are as follows:where , are the state vector and an observation vector, , are the state equation and an observation equation, and , are the state noise and the measurement noise. The Kalman filter can be described by recursive Bayesian estimator. The state equation and observation equation are as follows:Measurement updates are as follows:In the above formula, is the observation vector.
The real situation is that, due to the irregular motion of UAV, there is nonlinearity of the state equation and the observed equation A. EKF algorithm is the most common algorithm used to solve the nonlinearity of the filtering process, and it deals with the first-order linearization of the state equation and the observation equation, as shown in (19) and (20).
After linearization of the state equation and the observation equation, the standard Kalman filter can be used to deal with it, which is not discussed here. The following sections will focus on the more accurate nonlinear filtering algorithm, UKF, and its improved algorithm.
3.2. UT Transform and UKF Algorithm
Because the EKF algorithm adopts first-order linearization, its mean value and covariance have higher first-order accuracy, and it also contains large truncation error. The UKF algorithm proposed by Julier in 1997 uses the UT transform to represent the distribution of the state equation using the Sigma point set, which maintains the mean and covariance information through nonlinear propagation of this point set. In general, EKF can achieve second to third order accuracy. The following is a brief introduction to the UKF algorithm.
The first step is to generate a Sigma point set. As shown in the following equation, is the dimension of the parameter, is a first scaling parameter, determines the degree of dispersion of the Sigma point and is usually set to a small value (), is the second scaling parameter and is usually set to 0 or , is used to reduce the covariance approximation to the Taylor expansion of four times the error generated, and is the i-line vector or column vector of the square root of a weighted covariance matrix.UKF time update is given by the following formulas (27)- (29):The status update is given by formulas (30)-(36)
3.3. An Improved UKF Algorithm
Although the accuracy of UKF algorithm is much higher than that of EKF, it can only reach the three-dimensional precision of 10 cm for the engineering example of this paper (see the results of calculation example). Moreover, the UKF algorithm is more time-consuming than the EKF algorithm. Considering the practical requirements of the project, this paper proposes to use the square root nontrace Kalman filter algorithm (square root unscented Kalman filter, SR-UKF) to carry on the dynamic positioning of the UAV pod. This algorithm uses the square root of covariance instead of covariance to participate in the recursive operation. It is expected to further improve the numerical accuracy and stability of the filtering algorithm, and reduce the computational time to some extent. This section briefly describes the three parts of the SR-UKF algorithm, which are initialization, time update, and observation update.
The first part is initialization. As shown in formula (19), is the initial state of the UAV pod, whose dimension factor is , and is the Cholesky factor of the codefense initial matrix . In the state space equation, it is generally assumed that the state noise and the measurement noise are Gaussian white noises. Then, the Cholesky factor dimension expansion form of the state vector and its covariance matrix can be given by the following equations:in which and .
The second part is the time update. For state vectors and covariance matrices, their Cholesky factor dimension expansions have the following operations: At time, and can be obtained by formulas (37) and (38), and at other times of , and can be obtained by formulas (39) and (40).in which and .
On the basis of this, the probability density of the state vectors is approximated by selecting a few deterministic sampling points for the calculation of UT transform sampling points. The weights of these sampling points can be determined by formulas (41)-(45) using proportion corrected symmetry sampling.On the basis of the UT transformation, the sample point at a time can be represented asA sampling point for the position of the rotor drone pod was generated by the upper equation. Here, and are scale scaling factor and positive scale factor, respectively, is a priori information parameter to minimize the loss of high-order term precision, and and are the weights of the sample points .
One-step prediction of time update is performed using ’s nonlinear function as . By propagating , the state prediction value (one-step prediction) of the position of the rotor UAV pod is obtained by weighting .The predicted values for the covariance matrix (one-step prediction) are shown in formulas (49) and (50)where returns the matrix of Cholesky factor, , and update factor returns the matrix of the upper triangular Cholesky factor. This factor update can reduce the nonpositive qualitative probability of the covariance matrix.
The third part is the measurement update. For the SR-UKF algorithm, the generation process of the new sampling point in the nonlinear problem is as follows: in the pseudosatellite positioning process of UAV pod, the measurement noise is assumed to be Gaussian white noise, the new sampling point dimension is , the number is , and is used instead of . Then, using the binding formula (23), , , and weight parameters , , , and can be calculated. A new sampling point with and can be recalculated to represent the location of UAV pod using the second part of the time update.Here, . A new sampling point is used in the positioning process of the UAV pod. Based on this, we can get the one-step predictive value of the variable, and after considering the weight factor, we can get the predicted value .The predicted value of the square root factor of the covariance matrix is as follows:where is the measurement noise covariance matrix of the I moment.
4. Results Analysis
4.1. Design of the Experiment
In order to analyze the dynamic location of the soft-connected pod of rotor-wing UAV, a flat area in Nanjing was selected for the experiment (Figure 3). At the experimental site, 6 towers with a height of about 15 meters were installed to fix the pseudosatellite base stations. Five of the base stations were located near the tops of the towers, and the 6th base stations were fixed at the top of a field measuring vehicle for data transmission with workstations. The initial position of the base stations were measured by the total station. The geometrical position is shown in the lower right corner of Figure 3, and the coordinate values are listed in Table 1.
In order to analyze the accuracy of several pseudosatellite positioning algorithms, and make the positioning results of GPS RTK technology as the truth value (accuracy of 1-2 cm), the pseudosatellite observation station and GPS RTK antenna were bundled together in the experiment, and a nylon tape was used for soft connection with the UAV. Figure 4 shows an ongoing dynamic positioning experiment in which the drone’s range is located inside a polygonal area surrounded by the towers.
4.2. Analysis of the DOP Value of Pseudosatellite Base Station
According to the location of base station, the space distribution of base station in the test field was evaluated by using the DOP value formula given in Section 2.3. The red line in Figure 5(a) is the flight trajectory of the UAV. Figures 5(b), 5(c), and 5(d) show the distributions of PDOP, HDOP, and VDOP, respectively. Through analysis, it was found that the flight area of the UAV was in the ideal area of DOP value, which is very advantageous to the experiment.
(a) Measurement area
(b) Distribution of PDOP value
(c) Distribution of HDOP Value
(d) Distribution of VDOP value
4.3. Accuracy Analysis of Dynamic Positioning Algorithm for SR-UKF Algorithm
In this paper, the dynamic three-dimensional position of UAV pod in space was calculated by using EKF, UKF, and SR-UKF algorithms, respectively. Figures 6(b), 6(c), and 6(d) present the values of X, Y, and Z computed by the three algorithms, respectively. From the analysis of four figures, it can be found that the results of UKF and SR-UKF algorithms are very close most of the time, and the results of EKF calculation are poor, especially at the start and end of UAV flight.
(a) Comparison of three-dimensional positioning results of three algorithms
(b) Comparison of X-directional positioning results for three algorithms
(c) Comparison of Y-directional positioning results for three algorithms
(d) Comparison of Z-directional positioning results for three algorithms
In order to analyze the accuracy of the three different algorithms, the results obtained by GPS RTK were used as the real value (accuracy is better than 2 cm), and the calculation errors of the three methods in X-, Y-, and Z-directions were obtained, respectively, as shown in Figures 7(a)–7(c). Analysis of Figure 7 shows that the positioning error of EKF algorithm in the X-, Y-, and Z-directions is greater than that of UKF and SR-UKF algorithms, especially in the X-direction. Sometimes, the error reached the meter level, and far exceeded the maximum positioning error limit of the UAV pod. Figure 8 presents an error box diagram of the three algorithms, which shows that the error of the SR-UKF algorithm is small and concentrated and contains less outliers.
(a) Error comparison of three algorithms in X-direction
(b) Error comparison of three algorithms in Y-direction
(c) Error comparison of three algorithms in Z-direction
Finally, the accuracy of the three algorithms was analyzed statistically. Table 2 gives the statistical results of the EKF, UKF, and SR-UKF algorithms in X-, Y-, and Z-directions, and it can be observed that the SR-UKF algorithm has the highest parameter estimation accuracy. The standard deviation of three-dimensional estimation was calculated to be 0.4261 m for EKF, 0.1066 m for UKF, and 0.0717 m for SR-UKF. At the same time, the calculation time statistics of the three algorithms were analyzed, and the results are shown in Figure 9. It was found that the EKF was much faster than the UKF and SR-UKF algorithms, but the accuracy was low. The positioning accuracy of SR-UKF was higher than that of UKF, and the calculation efficiency was much improved.
5. Conclusion
In order to address the dynamic location problem of the rotor UAV pod, a pseudosatellite positioning system was constructed in the field test site by acquiring the distance observation between the pseudosatellite base station and the mobile station. Dynamic positioning was studied by using a variety of nonlinear filtering algorithms, including SR-UKF. Furthermore, the results of GPS RTK positioning were used as the external real value to evaluate these nonlinear filtering algorithms. The following conclusions were obtained from the results:
(1) The spatial configuration of pseudosatellite base station erected on the test field was first evaluated. The evaluation indexes PDOP, HDOP, and VDOP were calculated, and it was found that the UAV pod was in a relatively stable DOP value environment during the whole flight, which is very important for the stability of the positioning result.
(2) As the EKF algorithm usually takes Taylor first-order approximation linearization, the higher-order items directly resulted in a large linearization error, indicating that it was not good for the nonlinear flight-time positioning. The EKF algorithm was less accurate than the UKF and SR-UKF algorithms, especially in the X-axis direction. This is because the UAV’s flight trajectory in the three-dimensional space was more concentrated in the X-direction, in the X-direction of the maneuvering range of more than 20 m. The experimental results illustrated the disadvantage of EKF in handling high maneuverability. At the same time, the results of SR-UKF were better than that of UKF.
(3) Different statistics were used to evaluate the positioning accuracy of the three different nonlinear filtering algorithms. From the statistical results, it was found that the positioning error of EKF was 0.4261 m, which does not meet the positioning requirements of the UAV pod. The positioning accuracy of the UKF was 0.1066, which barely meets the 0.1 m positioning accuracy requirement of the UAV pod. The positioning accuracy of the SR-UKF was 0.0717 m, which has higher positioning accuracy and fully conforms to the positioning requirements of the UAV pod. The EKF algorithm was more efficient than the UKF and SR-UKF algorithms, but its positioning accuracy was low. Also, SR-UKF algorithm was significantly faster than the UKF calculation. Therefore, the SR-UKF algorithm was found to be an ideal algorithm to locate the UAV pod.
Data Availability
The data used to support the findings of this study are available from the corresponding author upon request.
Conflicts of Interest
The authors declare no conflicts of interest.
Acknowledgments
This work was carried under funding from the National Natural Science Foundation of China (Grant no. 41604024), China Postdoctoral Science Foundation (Grant no. 2016M601815), and the State Key Laboratory of Geo-Information Engineering (SKLGIE2016-Z-1-3).