Abstract
In terms of driverless systems, high-precision positioning technology is one among the critical aspects of driverless cars to achieve driverlessness. This study analyzed the working principles of GNSS (global navigation satellite system) and SINS (strapdown inertial navigation system) and elaborated the principles of the least square method and LAMBDA algorithm in the integer ambiguity resolution. Based on the network RTK positioning technology and the abovementioned theory, the unmanned automatic work vehicle was used as the research object, and the fusion positioning algorithm of the BDS/GPS system and inertial sensor was used to propose a high-precision positioning technology for the unmanned automatic work vehicle. The combined navigation system model was studied and constructed. Relevant verification was carried out through simulation and experiment. The results were as follows: the pitch angle error was less than 0.1°, the roll angle error was less than 0.05°, the speed error was less than 0.2 m/s, and the position error was less than 2.1 m. The outcomes indicate that an integrated navigation and positioning algorithm for driverless vehicles can significantly enhance the localisation accuracy and reliability of navigation. The research results are of engineering value and practical application for the development of unmanned automatic special vehicle positioning systems.
1. Introduction
In the 21st century, the fields such as artificial intelligence, information technology, machinery and electronics, and precision manufacturing have been developed by leaps and bounds. Accordingly, unmanned and automatically operated special vehicles are also used in a wide range of applications, such as industry, agriculture, and fire protection. Since manual operation is not suitable in high-temperature, highly polluting, and high-risk environments, unmanned vehicles are used to replace workers to detect and collect information in harsh and dangerous environments. Furthermore, technologies used in unmanned automatic special vehicles are extensive, including automatic control technology, computer science, and machine vision, which is worthy of studying [1–3].
RTK (real-time kinematic) technology uses GPS carrier phase observations to correct the positioning system, thus carrying out real-time dynamic relative positioning. With the continuous advancement of RTK technology, it has been widely used in transportation, engineering mapping, geological disaster monitoring, and other fields and gradually replaced the traditional navigation and positioning methods [4–6]. Modern society places greater demands on the navigation precision. However, a single navigation technology has difficulty in adapting to the complex working environment. In practical engineering applications, various navigation methods are often combined to complement each other and achieve high-precision positioning [7, 8]. This study focused on the research on high-precision positioning technology of unmanned special operation vehicles by using SINS (strapdown inertial navigation system) and satellite navigation integrated positioning, and relevant tests were carried out.
2. Research on Positioning Theory
2.1. GNSS Differential Observation Equation
In GNSS data processing, the functional relation between the original observation of the receiver and various factors affecting signal propagation is the GNSS observation equation. In GNSS positioning, satellites constantly broadcast navigation signals, and the receiver calculates the received pseudorange, carrier phase, and Doppler information to achieve navigation and positioning [9]. The pseudorange and carrier phase observations are two primitive observations which have the following observation equations:
In equations (1) and (2), the superscript s denotes the observation satellite, the subscript r denotes the receiver, and denote pseudorange and carrier phase observation, respectively, indicates the actual distance between the satellite and the receiver when the signal is transmitted, denotes the speed of light in vacuum, and d denote the receiver clock error and satellite clock error, respectively, indicating tropospheric error during the propagation of the signal through the atmosphere, denotes the ionospheric error in the process of signal propagation in the atmosphere, denotes the carrier phase wavelength, denotes the integer ambiguity in weeks, and and denote other errors in pseudorange and carrier phase, respectively (including noise errors).
2.1.1. Double-Difference Observation Model
The principle of double-difference positioning is shown in Figure 1. Double-difference is a method of carrying out the intersatellite difference for another reference satellite [10, 11].
The base station receiver r and the mobile station receiver q perform difference calculation on the same satellite s. The pseudorange and carrier phase observation equations of the base station r are equations (1) and (2), respectively. The pseudorange and carrier phase observation equations for mobile station q are as follows:
The difference between equations (1) and (3) and the difference between equations (2) and (4) can eliminate the satellite clock difference dtq, thus obtaining a single difference model as follows:
In equations (5) and (6), Δ denotes the single difference. With short baselines, the difference between ionospheric and tropospheric delay errors for base stations and mobile stations is small, so the difference between the troposphere and the ionosphere in equations (5) and (6) is regarded as equal to 0, which can be obtained as follows:
Assuming that the reference satellite is y, the single difference equation of the base station r and the mobile station q are as follows:
By differentiating equations (7) and (9) and equations (8) and (10), the receiver clock difference term is eliminated to obtain the following double-difference observation equation:
In the above equations, denotes the double-difference symbol. The double-difference integer ambiguity retains the integer characteristic and can be effectively separated.
Assuming that there are N available satellites, the number of observation equations for carrier phase and pseudorange is 2(N − 1), where the unknowns include three direction coordinates of baseline double-difference pseud range and N − 1 double-difference ambiguity. To solve the float solution of ambiguity, is needed and the solution is N = 4. Therefore, the simultaneous observation of four visual satellites is the minimum requirement for a single system to realize differential positioning [12, 13].
When four satellites a, b, c, and d are used for calculation, a 6-dimensional equation group can be established by adding pseudorange double-difference observations. Taking the satellite a with large altitude angle as the main satellite, the following results can be obtained:where is the unit vector from the satellite to the receiver, is the correction number of the receiver’s approximate coordinate, and is the double-difference pseudorange observation, and equation (12) is written in the following matrix form:
In equation (13), H is the design matrix composed of the unit vector from the receiver to the satellite, X is the baseline vector and ambiguity solution to be solved, and the float solution of ambiguity can be solved by the least square method as follows:
In equation (14), is the float solution of ambiguity. Let the baseline vector and the double-difference integer ambiguity vector , and the least square result can be expressed as follows:
In equation (16), Q denotes the covariance matrix of vector X, denotes the covariance matrix of baseline vector, represents the covariance matrix of ambiguity vector, and and denote the cross-covariance matrix of baseline vector and ambiguity vector, respectively.
2.2. Integer Ambiguity Estimation
Integer ambiguity is crucial to achieve high-precision RTK positioning [14]. At present, there are numerous ambiguity resolution methods, among which the LAMBDA algorithm has a perfect theoretical system, so the LAMBDA algorithm based on the least square estimation algorithm and the residual square of ambiguity is adopted.
The float solution is decomposed into the form of baseline vector and integer ambiguity vector, and the following is obtained:
In equation (18), y is the double-difference observation of pseudorange and carrier phase, is the baseline vector, M is the coefficient matrix of the baseline vector, N is the integer ambiguity coefficient matrix, is double-difference integer ambiguity, and is unmodeled error amount. Therefore, the ambiguity solution is transformed into the least square.
In equation (18), is the covariance matrix of y.
If ambiguity is independent of each other, the covariance matrix of the ambiguity is a diagonal matrix, and the optimal solution can be obtained by the nearest rounding. In general, due to the strong correlation between the ambiguity, the ambiguity covariance matrix is not a diagonal matrix, resulting in a low success rate through the rounding method.
The integer ambiguity float solution and the covariance matrix Q are obtained through equations (15) and (16), and then, the integer solution ń of the ambiguity is estimated by using the ambiguity vector float solution and the covariance matrix to minimize the sum of residual squares of ambiguity, as follows:
2.3. Basic Principle of Strapdown Inertial Navigation
Attitude update includes sensing the attitude information of the carrier and converting the measured real-time information to the navigation coordinate system to realize the position and speed update [15, 16].
The solution process is as follows: The initial information of navigation calculation is obtained through initial alignment. Then, the real-time speed, position information, and attitude information of the unmanned vehicle are obtained by calculating the acceleration. SINS is shown in Figure 2.
3. Modeling and Simulation of the Integrated Navigation and Positioning System
3.1. System Modeling
3.1.1. State Equation of Integrated Navigation
When combined navigation uses Kalman filtering for information processing, it is first necessary to establish a state equation that reflects the system state vector dynamic properties. [17, 18].
Generally speaking, the inertial navigation misalignment angle , speed error , position error , random constant drift of gyroscope , and the random constant bias value of the accelerometer are selected in the state equation and are taken as state vectors, and the state equation of the integrated n avigation system is as follows:
The state vector of the system state equation is as follows:
The system state transition matrix is as follows:
Equation (22) is a 15-dimensional system transition matrix. In practical application, the zero drift of gyroscope, accelerometer, and magnetometer varies little with time, so the erasable term in the transition matrix is set as 0. The system noise vector is as follows:
In equation (23), denotes the angular velocity measurement noise of the gyroscope and denotes the specific force measurement noise of the accelerometer.
3.1.2. Integrated Navigation Measurement Equation
In integrated navigation, the position information and speed information obtained by the MEMS strapdown inertial navigation system are subtracted from the longitude, latitude, and speed information received by the GNSS receiver, and the obtained measurement equation of the speed and position of the unmanned vehicle is as follows:where are the inertial navigation speed, satellite navigation speed, inertial navigation position, and satellite navigation position, respectively.
In equation (25), denotes the speed measurement noise of the satellite receiver and denotes the position measurement noise of the satellite receiver.
3.1.3. Discretization of Continuous-Time Stochastic Systems
The continuous-time stochastic system given by equation (20) should be discretized. Equations (27)–(29) can be approximated by equivalent discretization of equations (20) and (24).
, , , and . Ts is the discrete time interval.
In the above equations, is approximately constant and is a small quantity, that is, , ignoring the higher-order term of Taylor series expansion.
denotes the mean value of the system matrix in time period .
3.1.4. Integrated Navigation Time Synchronization
In SINS/GNSS integrated navigation, the signal delay of inertial devices is relatively short, while the satellite signal has a relatively long delay from acquisition to positioning solution to transmission to navigation computer, and there is a time asynchrony error between the two.
In SINS/GNSS integrated navigation, the time asynchrony between the receiver and the inertial navigation system is shown in Figure 3. The time lag of inertial navigation and satellite navigation is generally different, and the time asynchrony error is the relative lag of the two systems. Therefore, in the comparison of integrated navigation information, it is necessary to compensate for the time asynchrony error.
The relationship between inertial navigation speed and satellite speed is as follows:
In equation (29), is the average linear acceleration of the unmanned vehicle in the asynchronous time, which can be approximately obtained by differential calculation of the speed of the inertial navigation system in two adjacent times, and the adjacent time is expressed as follows:
In general, it is considered that the value of time asynchrony is relatively fixed.
From equation (29), it can be calculated that the velocity asynchrony error between the satellite and the inertial navigation is as follows:
Similarly, the position asynchrony error of the two is as follows:
The actual system is nonlinear, often using EKF (extended Kalman filter). EKF is the Taylor level first, omitting its high-oriented item is approximately linear system, and then Kalman filtering.
3.2. Simulation Analysis
To verify the feasibility of Kalman filter of integrated navigation, the navigation algorithm is compiled by MATLAB for simulation analysis. The simulated motion trajectory parameters of the unmanned vehicle are generated according to the set motion state, and error parameters conforming to the actual application are added on this basis. Finally, the simulated IMU and GNSS data were input into the integrated navigation solution program, thus obtaining the navigation information such as the attitude (heading angle, pitch angle, and roll angle), speed, longitude, and latitude of the unmanned vehicle.
3.2.1. Simulation Device Parameters
The principle of the simulation is shown in Figure 4. Device errors of inertial sensors are added to simulate the pure inertial navigation and SINS/GNSS integrated navigation on the generated data.
The simulation IMU generates the acceleration and angular velocity of the unmanned vehicle, while the three-dimensional velocity of the unmanned vehicle is generated by GNSS. The data update frequency of MEMS is 100 Hz, and the simulation device parameters are shown in Table 1.
The navigation adopts the “east, north, and up” coordinate system, and the initial attitude is 0°, 0°, and 0°. The initial position is longitude 121.07014° E, latitude 41.08478° N, and 50 m high. The initial speed is 0 m/s, the initial velocity error is 0.1 m/s, the initial attitude angle error is 0.03°, 0.01°, and −0.42°, the initial position error is 0 m, and the data output frequency of satellite navigation is 10 Hz.
3.2.2. Simulation Results
Due to the gyroscope drift and random walk of MEMS, its error is very large, and it cannot be used alone for a long time. Therefore, GNSS information is used for integrated navigation, and Kalman filter is used for data fusion to increase the precision and stability of integrated systems. The simulation experiment is designed with a duration of 500 s. The navigation experiment was carried out on the position, attitude angle, and speed under the pure inertial navigation and integrated navigation.
The position and attitude errors of pure inertial navigation are shown in Figures 5 and 6. After 60 s, the position error begins to diverge. After 500 s, the position error can reach −169.1 m, −263.1 m, and −84.67 m. The initial zero bias error of MEMS inertial measurement unit is large, which makes the navigation error result divergent with fast speed. After 500 s of navigation and positioning, the errors of pitch angle, roll angle, and heading angle reach 32.92°, 10.87°, and 32.65°, which cannot meet the navigation accuracy. Therefore, pure inertial navigation cannot be used for positioning alone.
(a)
(b)
(c)
(a)
(b)
(c)
As shown in Figure 7, the position error of integrated navigation fluctuates greatly in the first 50 s, and the longitude and latitude errors fluctuate around 0 in the following 450 s. The position error converges quickly, which shows that the system error of navigation of inertial unit is well suppressed after adding GNSS information. The reason for the fast convergence speed of the longitude and latitude error curves is that while adjusting the parameters of the Kalman filter P, Q, and R matrices, the coefficient of the P matrix is appropriately increased to improve the convergence speed of error curves.
(a)
(b)
The attitude error of the combined navigation is shown in Figure 8. After using the Kalman filter, the navigation information is corrected by the filtering information, and the attitude error of the combined navigation remains largely fluctuating around 0.
(a)
(b)
(c)
Velocity errors for pure inertial and combined navigation “east, north, and up” directions are shown in Figure 9. If inertial navigation is adopted alone, the velocity errors in the east and north directions fluctuate greatly. When SINS/GNSS integrated navigation is adopted after Kalman filtering, the fluctuation amplitude of velocity errors is significantly reduced and it basically fluctuates around 0.
(a)
(b)
(c)
4. Experimental Study
4.1. Introduction to the Test Platform of Unmanned Vehicles
The external structure of the unmanned special operation vehicle mainly includes 3 Hikvision surveillance cameras, one 5 GHz 433 Mbps outdoor point-to-point network bridge, and two GNSS antennas, as shown in Figure 10. The unmanned special operation vehicle is mainly used for real-time monitoring of the image, sound, air quality, and target temperature, around the observation area. The RTK high-precision BDS/GPS dual-mode positioning and navigation system is used to calibrate the geographic position information and obtain position information of the unmanned vehicle in real time.
4.2. Static Test and Result Analysis
This study used MATLAB to recalculate the saved data of the base station and unmanned vehicle and finally obtained navigation data results. The data update frequency of the GPS system was 10 Hz and that of MEMS was 125 Hz, the observation epoch was 1800 seconds, and the initial position of the unmanned vehicle was latitude 41.08478564° N, longitude 121.0701494° E, and 55.364 m high.
The scatter diagram of longitude and latitude high position obtained by the unmanned vehicle in static state is shown in Figure 11. It can be observed that the static position of the unmanned vehicle is relatively concentrated and the three-dimensional position error is small. The mean value and standard deviation of longitude, latitude, and height were statistically analyzed. The specific information is shown in Table 2.
The number of satellites visible during static testing of unmanned vehicles and base stations is shown in Figure 12. Due to the main reason that the unmanned vehicle is equipped with two GPS antennas, the unmanned vehicle has more visible satellites than the base station in most of the time. In the same time period, if the unmanned vehicle is in a more ideal environment, the number of visible satellites will be more. However, there are more satellites in the base station between 223 and 279 seconds. Especially at the 274th second, the number of visible satellites of the unmanned vehicle is only 11, which may be due to the interference of satellite signals caused by the multipath effect. In the epoch of the static test, the base station has an average of 23.5 (23-24) visible satellites, and the unmanned vehicle has 28, which can meet the basic positioning requirements (the number of visible satellites ≥ 4).
Theoretically, the smaller the HDOP (horizontal dilution of precision), the higher the positioning precision, and its size fluctuates with the number of satellites. The HDOP values of the unmanned vehicle and the base station are shown in Figure 13. HDOP of the unmanned vehicle is smaller than that of the base station. The specific values are as follows: the average HDOP of the unmanned vehicle is 0.55 and the average HDOP of the base station is 0.7.
The attitude angle, eastward speed, and northward speed of static integrated navigation are sorted and counted. The attitude output and speed output of integrated navigation are shown in Figures 14 and 15. The mean value and standard deviation of specific values are shown in Tables 3 and 4.
(a)
(b)
(c)
(a)
(b)
The pitch angle is stable at −0.0013° and the standard deviation is 0.0021°; the roll angle is stable at 0.0002° and the standard deviation is 0.0074°; and the heading angle is stable at 181.305° and the standard deviation is 0.235°. The standard deviation of pitch angle and roll angle is relatively small, while the course angle has a relatively large standard deviation. The mean and standard deviation of the easting speed and the mean and standard deviation of the northing speed are small with little fluctuation, which can meet the needs of high-precision positioning.
4.3. Dynamic Test and Result Analysis
To verify the dynamic stability of the unmanned vehicle, its trajectory on the same rubber track was calibrated to make it move two laps on the set track. The analyzed trajectory is shown in Figures 16 and 17.
(a)
(b)
(a)
(b)
The remote controller was used to control the unmanned vehicle to move two laps on the rubber track, and the trajectory obtained on the upper computer is shown in Figures 16(a) and 17(a). The obtained test data are sorted out, and then, the data are processed by MATLAB to convert the obtained motion data into graphic display. The two-dimensional representation of the motion of the unmanned vehicle is shown in Figures 16(b) and 17(b).
As can be seen through the upper computer display of the two movements of the unmanned vehicle and the MATLAB motion trajectory diagram, the satellite receiving state is good during the movement and the trajectory graph formed is clear. Therefore, the first statistical data were selected to analyze the changes of the number of satellites received by the unmanned vehicle, the base station, and the HDOP value. Based on the data collected for the first time, the error between the second data and the first data was observed to analyze the positioning of the unmanned vehicle.
The number of visible satellites of the unmanned vehicle and the base station in the dynamic test is shown in Figure 18 and Table 5. The number of visible satellites of the base station is basically stable at 24 and 25 satellites, while the number of visible satellites of the unmanned vehicle varies greatly, up to 30 and at least 16.
The HDOP of the unmanned vehicle and the base station in the dynamic test is shown in Figure 19. The HDOP value of the base station is 0.7 or 0.8, while the HDOP value of the unmanned vehicle varies greatly, with a maximum of 1.4 and a minimum of 0.5. The mean value and standard deviation are shown in Table 6.
For attitude errors, there are certain errors in about 120 seconds to 180 seconds, 300 seconds to 350 seconds, and 450 seconds to 520 seconds. The pitch angle error is basically –0.1° and 0.1°, the roll angle error is −0.05° and 0.05°, the heading angle error is −0.2°and 0.2°, and only a few epochs reach the end point of the range. The velocity error is also concentrated in the last two time periods of the attitude error. The eastward velocity error is −0.3 m/s and 0.3 m/s, and the northward velocity error is −0.2 m/s and 0.2 m/s, as shown in Figures 20 and 21.
(a)
(b)
(c)
(a)
(b)
The position error of the dynamic integrated navigation of the unmanned vehicle is shown in Figure 22. The error in most time periods is 0, the maximum error in latitude is about 2.1 m, and the maximum error in longitude is about 2.8 m. The main reason for the error is that the unmanned vehicle was disturbed by the surrounding environment during its movement, which affected its positioning precision and stability. The main reasons for the fluctuation of curves in the figure are insufficient compensation of the inertial device and poor stability of the inertial device under dynamic conditions.
(a)
(b)
The static and dynamic tests of the dual antenna GPS/MEMS integrated navigation system were carried out. The test results showed that if the pure inertial navigation system was used alone for a long time, its error would continue to accumulate with time, which would lead to the failure to complete the high-precision positioning requirements. Therefore, integrated navigation should be used.
Each module of the unmanned vehicle worked well together, the performance was stable, and the positioning module was powerful, which could be applied to the actual situation.
When the unmanned vehicle was in an ideal environment, the satellite signal was better and the number of visible satellites received was relatively large. Using the integrated navigation, the attitude, position, and velocity information of the unmanned vehicle was ideal. When the unmanned vehicle was in a sheltered or complex environment, the error would increase, but it could still maintain a high positioning precision.
5. Conclusions
This study focused on high-precision positioning technology for unmanned autonomous operational special purpose vehicles, leading to the following conclusion.
The combination of innovative attitude sensor and GNSS dual antenna measurement algorithm can greatly enhance the monitoring accuracy of the motion state of unmanned vehicles. Using BDS/GPS for positioning research, the spatial geometry of the satellite is ideal, the number of visual satellites received is relatively large, and the positioning effect is better than that of a single system positioning.
The Kalman filter of integrated navigation and positioning was designed, which can effectively overcome the data divergence of inertial navigation and environmental constraints of satellite navigation and enhance the accuracy and stability of the positioning system. Simulations and experiments have verified that the combined navigation and positioning system can meet the needs of practical applications.
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 that they have no conflicts of interest.