# A Seamless Navigation System and Applications for Autonomous Vehicles Using a Tightly Coupled GNSS/UWB/INS/Map Integration Scheme

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

## 2. Related Work

- Multi-GNSS-TC RTK and INS measurements are used to solve the problem of difficult positioning in a challenging environment and improve the ambiguity fixing rate.
- UWB technology is developed to provide accurate and continuous positioning results in indoor environments. INS and map information are presented to identify and eliminate the effects of UWB NLOS errors.
- An improved AREKF algorithm based on a TC integrated single-frequency multi-GNSS-TC RTK/UWB/INS/map system is proposed.
- The positioning experiment of using an experimental car to simulate AVs is carried out in a harsh and seamless environment, and the results of the experiment provide the possibility for the high precision and continuity of the positioning module in automatic driving.

## 3. High-Precision Indoor Positioning for AVs

#### 3.1. INS Dynamics Model for the TC UWB/INS/Map Integrated System

#### 3.2. NLOS Error Recognition and Elimination Based on UWB/INS/Map Integration

#### 3.3. Measurement Model for the TC UWB/INS/Map Integrated System

## 4. High-Precision and Seamless Positioning for AVs in Harsh Environments

#### 4.1. Measurement Model of the TC Integrated Multi-GNSS-TC RTK/INS/UWB/Map System

- Both satellite systems use CDMA signal modulation: ${\alpha}_{j}^{FM}$, ${k}^{{M}_{s}}$, ${k}^{{F}_{l}}$, and $\mathrm{\Delta}\gamma $ are all equal to zero.
- Both satellite systems use FDMA signal modulation: ${\alpha}_{j}^{FM}=0$, ${k}^{{M}_{s}}$, and ${k}^{{F}_{l}}$ are the frequency numbers of the two different GLONASS satellites, and $\mathrm{\Delta}\gamma $ is the rate of change of the interfrequency bias (IFB).
- The two satellite systems are different, but both adopt CDMA signal modulation: ${\alpha}_{j}^{FM}$ is the intersystem bias (ISB) in the system phase, and ${k}^{{M}_{s}}$, ${k}^{{F}_{l}}$, and $\mathrm{\Delta}\gamma $ are all equal to zero.
- The two satellite systems are different and adopt different types of signal modulation: ${\alpha}_{j}^{FM}$ denotes the intersatellite phase ISB of the two satellites for frequency number 0, ${k}^{{F}_{l}}=0$, ${k}^{{M}_{s}}$ is the frequency number of the GLONASS satellite, and $\mathrm{\Delta}\gamma $ is the rate of change of the IFB.

#### 4.2. An Improved Innovation-Based AREKF Algorithm to Resist Outliers

## 5. Field Experiment Design and Analysis of Results

#### 5.1. Experimental Description and Platform Construction

- This article focuses mainly on high-precision positioning systems in pursuit of high precision and continuous reliability of such systems.
- An MEMS IMU was also tested. Although the algorithm proposed in this article improves the positioning accuracy in most environments, the overall positioning accuracy was seriously reduced.

#### 5.2. Time Synchronization and Spatial Unification

- When GPS is not available, the computer time cannot be corrected by means of the GPS second pulse signal. In this case, the UWB time label depends only on the computer time, which will be subject to clock bias and clock drift after a long time. At present, time-asynchrony error occurrences are shown to exist in experimental observations after several hours, but the impact on the experimental results within a few hours is small.
- The UWB time label obtained using this synchronization method is still affected by the time delay associated with the transmission of the signal to the computer through a universal serial bus (USB) data cable. At present, this delay can be reduced only through calibration technology.
- The sampling frequencies of the GNSS (2 Hz), the INS (200 Hz), and the UWB system (2 Hz) are different. It is necessary to interpolate all observations to correspond to the same observation times to facilitate calculations.

#### 5.3. Satellite Availability

#### 5.4. Influence of the NLOS Error on UWB Measurements

#### 5.5. Analysis of the Positioning Results

- Scheme 1: The EKF algorithm based on the single-frequency TC integrated multi-GNSS RTK/UWB/INS system.
- Scheme 2: The EKF algorithm based on the single-frequency TC integrated multi-GNSS-TC RTK/INS system.
- Scheme 3: The EKF algorithm based on the single-frequency TC integrated multi-GNSS-TC RTK/UWB/INS system.
- Scheme 4: The improved AREKF algorithm based on the single-frequency TC integrated multi-GNSS-TC RTK/UWB/INS system.
- Scheme 5: The EKF algorithm based on the single-frequency TC integrated multi-GNSS-TC RTK/UWB/INS/map system.
- Scheme 6: The improved AREKF algorithm based on the single-frequency TC integrated multi-GNSS-TC RTK/UWB/INS/map system (the method proposed in this paper).

## 6. Conclusions

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Data Availability Statement

## Acknowledgments

## Conflicts of Interest

## References

- Li, J.; Cui, X.; Li, Z.; Liu, J. Method to improve the positioning accuracy of vehicular nodes using IEEE 802.11p protocol. IEEE Access
**2017**, 6, 2834–2843. [Google Scholar] [CrossRef] - Ren, K.; Wang, Q.; Wang, C.; Qin, Z.; Lin, X. The security of autonomous driving: Threats, defenses, and future directions. Proc. IEEE
**2019**, 108, 357–372. [Google Scholar] [CrossRef] - Liu, S.; Liu, L.; Tang, J.; Yu, B.; Wang, Y.; Shi, W. Edge computing for autonomous driving: Opportunities and challenges. Proc. IEEE
**2019**, 107, 1697–1716. [Google Scholar] [CrossRef] - Jonasson, M.; Rogenfelt, Å.; Lanfelt, C.; Fredriksson, J.; Hassel, M. Inertial navigation and position uncertainty during a blind safe stop of an autonomous vehicle. IEEE Trans. Veh. Technol.
**2020**, 69, 4788–4802. [Google Scholar] [CrossRef] - Xiong, H.; Mai, Z.; Tang, J.; He, F. Robust GPS/INS/DVL navigation and positioning method using adaptive federated strong tracking filter based on weighted least square principle. IEEE Access
**2019**, 7, 26168–26178. [Google Scholar] [CrossRef] - Li, T.; Zhang, H.; Gao, Z.; Chen, Q.; Niu, X. High-accuracy positioning in urban environments using single-frequency multi-GNSS RTK/MEMS-IMU integration. Remote Sens.
**2018**, 10, 205. [Google Scholar] [CrossRef] [Green Version] - Hao, Y.; Xu, A.; Sui, X.; Wang, Y. A modified extended Kalman filter for a two-antenna GPS/INS vehicular navigation system. Sensors
**2018**, 18, 3809. [Google Scholar] [CrossRef] [PubMed] [Green Version] - Mur-Artal, R.; Tardós, J.D. ORB-SLAM2: An open-source SLAM system for monocular, stereo, and RGB-D cameras. IEEE Trans. Robot.
**2017**, 33, 1255–1262. [Google Scholar] [CrossRef] [Green Version] - Davison, A.J.; Reid, I.D.; Molton, N.D.; Stasse, O. MonoSLAM: Real-Time single camera SLAM. IEEE Trans. Pattern. Anal. Mach. Intell.
**2007**, 29, 1052–1067. [Google Scholar] [CrossRef] [PubMed] [Green Version] - Dill, E.; de Haag, M.U.; Duan, P.; Serrano, D.; Vilardaga, S. Seamless indoor-outdoor navigation for unmanned multi-sensor aerial platforms. In Proceedings of the 2014 IEEE/ION Position, Location and Navigation Symposium (PLANS 2014), Monterey, CA, USA, 5–8 May 2014. [Google Scholar]
- Tao, X.; Zhang, X.; Zhu, F.; Wang, F.; Teng, W. Precise displacement estimation from time-differenced carrier phase to improve PDR performance. IEEE Sens. J.
**2018**, 18, 8238–8246. [Google Scholar] [CrossRef] - Niu, X.; Liu, T.; Kuang, J.; Li, Y. A Novel Position and Orientation System for Pedestrian Indoor Mobile Mapping System. IEEE Sens. J.
**2021**, 21, 2104–2114. [Google Scholar] [CrossRef] - Cheng, J.; Yang, L.; Li, Y.; Zhang, W. Seamless outdoor/indoor navigation with WIFI/GPS aided low cost Inertial Navigation System. Phys. Commun.
**2014**, 13, 31–43. [Google Scholar] [CrossRef] - Huang, H.; Zeng, Q.; Chen, R.; Meng, Q.; Wang, J.; Zeng, S. Seamless navigation methodology optimized for indoor/outdoor detection based on wifi. In Proceedings of the 2018 Ubiquitous Positioning, Indoor Navigation and Location-Based Services (UPINLBS), Wuhan, China, 22–23 March 2018. [Google Scholar]
- Zhang, X.; Zeng, Q.; Meng, Q.; Xiong, Z.; Qian, W. Design and realization of a mobile seamless navigation and positioning system based on Bluetooth technology. In Proceedings of the 2016 IEEE Chinese Guidance, Navigation and Control Conference (CGNCC), Nanjing, China, 12–14 August 2016. [Google Scholar]
- Silva, B.; Hancke, G.P. IR-UWB-based non-line-of-sight identification in harsh environments: Principles and challenges. IEEE Trans. Industr. Inform.
**2016**, 12, 1188–1195. [Google Scholar] [CrossRef] - Adebomehin, A.A.; Walker, S.D. Enhanced Ultrawideband methods for 5G LOS sufficient positioning and mitigation. In Proceedings of the 2016 IEEE 17th International Symposium on A World of Wireless, Mobile and Multimedia Networks (WoWMoM), Coimbra, Portugal, 21–24 June 2016. [Google Scholar]
- Rizos, C.; Roberts, G.; Barnes, J.; Gambale, N. Experimental results of Locata: A high accuracy indoor positioning system. In Proceedings of the 2010 International Conference on Indoor Positioning and Indoor Navigation (IPIN), Zurich, Switzerland, 15–17 September 2010. [Google Scholar]
- Chen, X.; Xu, Y.; Li, Q.; Tang, J.; Shen, C. Improving ultrasonic-based seamless navigation for indoor mobile robots utilizing EKF and LS-SVM. Measurement
**2016**, 92, 243–251. [Google Scholar] [CrossRef] - Liu, T.; Niu, X.; Kuang, J.; Cao, S.; Zhang, L.; Chen, X. Doppler shift mitigation in acoustic positioning based on pedestrian dead reckoning for smartphone. IEEE Trans. Instrum. Meas.
**2020**, 70. [Google Scholar] [CrossRef] - Pietra, D.V.; Dabove, P.; Piras, M. Seamless Navigation using UWB-based Multisensor System. In Proceedings of the 2020 IEEE/ION Position, Location and Navigation Symposium (PLANS), Portland, OR, USA, 20–23 April 2020. [Google Scholar]
- Odolinski, R.; Teunissen, P.J.G.; Odijk, D. Combined BDS, Galileo, QZSS and GPS single-frequency RTK. GPS Solut.
**2015**, 19, 151–163. [Google Scholar] [CrossRef] - Odijk, D.; Nadarajah, N.; Zaminpardaz, S.; Teunissen, P.J.G. GPS, Galileo, QZSS and IRNSS differential ISBs: Estimation and application. GPS Solut.
**2017**, 21, 439–450. [Google Scholar] [CrossRef] - Liu, Y.; Liu, F.; Gao, Y.; Zhao, L. Implementation and analysis of tightly coupled global navigation satellite system precise point positioning/inertial navigation system (GNSS PPP/INS) with insufficient satellites for land vehicle navigation. Sensors
**2018**, 18, 4305. [Google Scholar] [CrossRef] [PubMed] [Green Version] - Chiang, K.W.; Chang, H.W.; Li, Y.H.; Tsai, G.J.; Tseng, C.L.; Tien, Y.C.; Hsu, P.C. Assessment for ins/gnss/odometer/barometer integration in loosely-coupled and tightly-coupled scheme in a gnss-degraded environment. IEEE Sens. J.
**2019**, 20, 3057–3069. [Google Scholar] [CrossRef] - Liu, Y.; Fan, X.; Lv, C.; Wu, J.; Li, L.; Ding, D. An innovative information fusion method with adaptive Kalman filter for integrated INS/GPS navigation of autonomous vehicles. Mech. Syst. Signal Process.
**2018**, 100, 605–616. [Google Scholar] [CrossRef] [Green Version] - Li, X.; Wang, Y.; Khoshelham, K. A robust and adaptive complementary kalman filter based on Mahalanobis distance for Ultra Wideband/Inertial Measurement Unit fusion positioning. Sensors
**2018**, 18, 3435. [Google Scholar] [CrossRef] [Green Version] - Groves, P.D. Principles of GNSS, inertial, and multi-sensor integrated navigation systems. IEEE Aero. El. Sys. Mag.
**2015**, 30, 26–27. [Google Scholar] [CrossRef] - Liu, F.; Wang, J.; Zhang, J.; Han, H. An indoor localization method for pedestrians base on combined UWB/PDR/Floor Map. Sensors
**2019**, 19, 2578. [Google Scholar] [CrossRef] [Green Version] - Qian, J.; Ma, J.; Ying, R.; Liu, P.; Pei, L. An improved indoor localization method using smartphone inertial sensors. In Proceedings of the International Conference on Indoor Positioning and Indoor Navigation, Montbeliard, Belfort, France, 28–21 October 2013. [Google Scholar]
- Lan, K.C.; Shih, W.Y. On calibrating the sensor errors of a PDR-based indoor localization system. Sensors
**2013**, 13, 4781–4810. [Google Scholar] [CrossRef] [Green Version] - Shin, E.H. Estimation Techniques for Low-Cost Inertial Navigation. Ph.D. Thesis, University of Calgary, Calgary, AB, Canada, 2005. [Google Scholar]
- Benson, D.O. A Comparison of Two Approaches to Pure-Inertial and Doppler-Inertial Error Analysis. IEEE Trans. Aero. Elec. Syst.
**1975**, AES-11, 447–455. [Google Scholar] [CrossRef] - Park, M. Error Analysis and Stochastic Modeling of MEMS Based Inertial Sensors for Land Vehicle Navigation Applications. Ph.D. Thesis, University of Calgary, Calgary, AB, Canada, April 2004. [Google Scholar]
- Wang, C.; Xu, A.; Kuang, J.; Sui, X.; Hao, Y.; Niu, X. A High-Accuracy Indoor Localization System and Applications Based on Tightly Coupled UWB/INS/Floor Map Integration. IEEE Sens. J.
**2021**, 21, 18166–18177. [Google Scholar] [CrossRef] - Li, M. Research on Multi-GNS S Precise Orbit Determination Theory and Application. Ph.D. Thesis, Wuhan University, Wuhan, China, April 2011. [Google Scholar]
- Sui, X. Research on the Theory and Method of Inter-System Double Difference Ambiguity Forming and Fixing for Multi-GNSS. Ph.D. Thesis, Wuhan University, Wuhan, China, May 2017. [Google Scholar]
- Blewitt, G. Carrier phase ambiguity resolution for the Global Positioning System applied to geodetic baselines up to 2000 km. J. Geophys. Res. Atmos.
**1989**, 94, 10187–10203. [Google Scholar] [CrossRef] [Green Version] - Yang, Y.; Song, L.; Xu, T. Robust estimator for correlated observations based on bifactor equivalent weights. J. Geod.
**2002**, 76, 353–358. [Google Scholar] [CrossRef] - Yan, G.; Deng, Y. Review on Practical Kalman Filtering Techniques in Traditional Integrated Navigation System. Navig. Position. Timing
**2020**, 7, 50–64. [Google Scholar]

**Figure 3.**Experimental car and automatic tracking total station. (

**a**) Experimental car. (

**b**) Leica TS50 automatic tracking total station.

**Figure 4.**Experimental scene and design. (

**a**) Experimental environment. (

**b**) Top view of the experimental scene and the designed trajectory. (

**c**) Experimental layout and established trajectory.

**Figure 6.**Satellite availability in a dense urban environment with a 35° cut-off elevation angle. (

**a**) Satellite availability. (

**b**) Starry sky map of satellites.

**Figure 7.**Number of available satellites and DOP for the multi-GNSS scenario with a 35° cut-off elevation angle. (

**a**) Number of satellites. (

**b**) DOP.

**Figure 8.**Occlusion status on the map for different UWB anchor nodes: (

**a**) anchor node 1; (

**b**) anchor node 2; (

**c**) anchor node 3; (

**d**) anchor node 4; (

**e**) anchor node 5; (

**f**) anchor node 6; (

**g**) anchor node 7; (

**h**) anchor node 8. The light blue and black triangles indicate the positions of the current UWB anchor node and other UWB anchor nodes, respectively. The dots of different colors represent various occlusion states of the current UWB anchor node (including ranging failure, LOS conditions, and NLOS conditions).

**Figure 9.**Time series of the ranging values and errors for different UWB anchor nodes. (

**a**) Ranging values. (

**b**) Errors.

**Figure 10.**Experimental results under different schemes. (

**a**) Positioning trajectory results. (

**b**) CDF of the positioning errors.

**Figure 11.**Comparison of the positioning errors under different schemes. (

**a**) North. (

**b**) East. (

**c**) Plane.

Coordinate System | Description |
---|---|

The inertial frame (i-frame) | The i-frame is an ideal frame of reference in which ideal accelerometers and gyroscopes fixed to the i-frame have zero outputs. |

The body frame (b-frame) | The b-frame is the frame in which the accelerations and angular rates generated by the strapdown accelerometers and gyroscopes are resolved, i.e., the forward–right–down system. |

The navigation frame (n-frame) | The n-frame is selected as the navigation solution coordinate system. The n-frame is a local geodetic frame that has its origin coinciding with that of the sensor frame, i.e., the north–east–down (NED) system. |

The Earth frame (e-frame) | The e-frame has its origin at the center of mass of the Earth and axes that are fixed with respect to the Earth. |

${\mathit{P}}_{\mathit{max}}$ | ${\mathit{P}}_{\mathit{min}}$ | |
---|---|---|

${\mathit{b}}_{g}$ | 1.5°/h | 0.01°/h |

${\mathit{b}}_{a}$ | 30 mGal | 5 mGal |

${\mathit{s}}_{g}$ | 250 ppm | 10 ppm |

${\mathit{s}}_{a}$ | 250 ppm | 10 ppm |

Parameter | Accelerometer | Gyroscope |
---|---|---|

Measurement range | ±10 g | ±300°/s |

Bias stability | 25 mGal | 1°/h |

Random walk | 0.1 m/s/$\sqrt{\mathrm{h}}$ | 0.03°$/\sqrt{\mathrm{h}}$ |

Sampling frequency | 200 Hz | 200 Hz |

Parameter | UWB |
---|---|

Ranging principle | TW-TOF |

Wave band | 3.1–4.8 GHz |

Ranging ability | <80 m |

LOS accuracy | 5 ± 1 cm |

NLOS accuracy | Environmentally determined |

Sampling frequency | 2 Hz |

Parameter | TS50 |
---|---|

Ranging accuracy | 2 mm + 2 ppm |

Angle accuracy | 0.5″ |

Sampling frequency | 10 Hz |

**Table 6.**Comparison of the RMS accuracies, average and maximum errors, and ambiguity fixing rates under the different schemes.

Scheme 1 | Scheme 2 | Scheme 3 | Scheme 4 | Scheme 5 | Scheme 6 | ||
---|---|---|---|---|---|---|---|

RMS (m) | North | 1.6405 | 2.0621 | 1.0365 | 0.2593 | 0.3526 | 0.1756 |

East | 1.8507 | 0.6411 | 1.8118 | 0.2795 | 0.3653 | 0.1698 | |

2D | 2.4731 | 2.1594 | 2.0873 | 0.3565 | 0.5077 | 0.2443 | |

Average (m) | North | 0.5583 | 0.7172 | 0.4247 | 0.1672 | 0.1645 | 0.0932 |

East | 0.7539 | 0.4503 | 0.6916 | 0.1805 | 0.2154 | 0.1049 | |

2D | 1.0377 | 0.9919 | 0.8848 | 0.2530 | 0.3105 | 0.1657 | |

Max (m) | North | 42.3682 | 17.2070 | 8.6099 | 0.9990 | 5.4026 | 0.7986 |

East | 18.1621 | 3.0340 | 17.9310 | 1.8241 | 2.2516 | 1.0632 | |

2D | 43.3052 | 17.2074 | 17.9450 | 1.8464 | 5.5461 | 1.1372 | |

Fixing rate (%) | 54.6 | 68.4 | 78.5 | 83.2 | 79.7 | 89.4 |

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. |

© 2021 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Wang, C.; Xu, A.; Sui, X.; Hao, Y.; Shi, Z.; Chen, Z.
A Seamless Navigation System and Applications for Autonomous Vehicles Using a Tightly Coupled GNSS/UWB/INS/Map Integration Scheme. *Remote Sens.* **2022**, *14*, 27.
https://doi.org/10.3390/rs14010027

**AMA Style**

Wang C, Xu A, Sui X, Hao Y, Shi Z, Chen Z.
A Seamless Navigation System and Applications for Autonomous Vehicles Using a Tightly Coupled GNSS/UWB/INS/Map Integration Scheme. *Remote Sensing*. 2022; 14(1):27.
https://doi.org/10.3390/rs14010027

**Chicago/Turabian Style**

Wang, Changqiang, Aigong Xu, Xin Sui, Yushi Hao, Zhengxu Shi, and Zhijian Chen.
2022. "A Seamless Navigation System and Applications for Autonomous Vehicles Using a Tightly Coupled GNSS/UWB/INS/Map Integration Scheme" *Remote Sensing* 14, no. 1: 27.
https://doi.org/10.3390/rs14010027