In this work, we demonstrate the importance of zero velocity information for global navigation satellite system (GNSS) based navigation. The effectiveness of using the zero velocity information with zero velocity update (ZUPT) for inertial navigation applications have been shown in the literature. Here we leverage this information and add it as a position constraint in a GNSS factor graph. We also compare its performance to a GNSS/ inertial navigation system (INS) coupled factor graph. We tested our ZUPT aided factor graph method on three datasets and compared it with the GNSS-only factor graph.
Localization is an integral component of any mobile robot system, which plays an important role in many core robotic capabilities like motion planning, obstacle avoidance, and mapping. One of the most common methods for calculating positioning information of the robots is using GNSS [enge1994global]. However, the availability of this system relies on the observation of multiple satellites [grovebook]
and is frequently unable to obtain a precise and robust state estimate in urban and forested areas due to environmental constraints (e.g., poor satellite geometry and multipath effects)[merry2019smartphone]. An inertial measurement unit (IMU) can be used in a standalone manner in the INS to estimate states but suffers from drifting due to the accumulation of errors through the integration of acceleration and angular velocity measurements [kok2017using, chen2018ionet, brossard2020ai, narasimhappa2019mems]. A commonly adopted localization strategy is coupling GNSS and INS, which combines range measurements from satellites and IMU measurements to calculate states [zhao2016analysis, li2006low, miller2012sensitivity, hu2015derivative]. This coupling strategy alleviates some of the vulnerabilities that standalone GNSS technology faces in urban environments and forested areas [merry2019smartphone].
Though Kalman filters have been the preferred choice forGNSS and GNSS/INS state estimation for a long time, factor graphs [dellaert2017factor]
have emerged as an alternative framework for solving the localization problem mostly because of the advent of open-source graph optimization libraries like GTSAM[dellaert2012factor], g2o [grisetti2011g2o]
. Factor graph optimization has some advantages over the Kalman filters. For example, it uses multiple iterations to solve all states in a batch form instead of just one iteration in a Kalman filter, which is not affected by future measurements (unless smoothing is done). Factor graphs have also been seen to explore better the time correlation between past and current epochs, which helps in outlier removal[wen2021factor]. The factor graph framework also makes it easy to add existing and new robust estimation methods [watson2018evaluation, watson2020robust, pfeifer2018robust] which helps reduce localization failure due to large noise.
In order to provide a cost-effective system, one can utilize the pseudo-measurements in certain conditions from the sensors already on-board. For instance, ZUPT is commonly used as an aiding process for pedestrian navigation [kwakkel2008gnss, zhang2017adaptive]. ZUPT can bound the velocity error and help to calibrate IMU sensor noises [skog2010zero]. This process helps to reduce the INS positioning error growth from cubic to linear since the error state model justifies the correlation between the position and velocity errors of the error covariance matrix. Using ZUPT in state estimation does not need any dedicated sensor to provide zero velocity information, and this information can be obtained by the sensors already on-board (e.g., IMU, wheel encoders). ZUPT requires stationary conditions, and it can be used as an opportunistic navigational update if a wheeled robot stops for other reasons (e.g., obstacle avoidance, re-planning, waiting for pedestrians, stopping at traffic lights). Also, ZUPT can be used to improve wheel odometry (WO)/INS proprioceptive localization with periodic stops in GNSS-denied (or degraded), poor lighting/feature areas [kilic2019improved] and with autonomous stops by deciding when to stop [kilic2020]. The small wheeled robots have more freedom of stopping rather than autonomous cars, which makes utilizing ZUPT a well-suited application for them.
This paper offers the following contributions: 1) detailing a GNSS/WO/INS fusion strategy by exploiting the zero velocity information for both GNSS and INS to be used for wheeled robots, 2) validation of the provided method on actual hardware in field tests with detailed specifications of the implementation and hardware, so the interested reader can easily replicate our work, and 3) making our algorithm publicly available***https://github.com/wvu-navLab/gnss-ins-zupt using open-access data [vz7z-jc84-20].
ii.1 GNSS Factor Graph
The state estimation framework used for processing GNSS data is a factor graph [dellaert2017factor]. Factor graphs solve the maximum a posteriori (MAP) estimation problem by maximizing the product of factors that are probabilistic constraints between states at different time steps and between states and measurements from the current time steps. A depiction of an example factor graph is given in Fig 1.
A detailed description of creating factors with GNSS observations is presented in [watson2018evaluation]. The states estimated in the GNSS factor graph are the position, tropospheric delay, clock bias, and phase bias. In Fig. 1, represents any constraint that might exist between the states and the measurements. For example, represents any prior belief on each state, is the motion constraint between two consecutive states along the trajectory, and is the measurement constraint between a state, where the measurements that were perceived from that state. Under the Gaussian assumption, the maximum product of the factors problem converts to a non-linear least squares problem. Following GTSAM nomenclature, we refer to as the between-factor. The cost form of the factors is shown in the equation 2. Each component of the sum is a Mahalanobis cost, the left one being the prior factor cost, the middle one being the between-factor cost( is the measured displacement), and the right one is the GNSS factor cost.
As new GNSS measurements are received, new factors are added using pseudo-range and phase measurements to the existing factor graph and solved incrementally for every epoch using the Incremental Smoothing and Mapping (iSAM2) formulation [kaess2012isam2]. It converts the factor graph to the Bayes tree’s data structure, whose vertices represent cliques for efficient inference. Thus, adding a new constraint to the factor graph only results in the re-linearization of the states within the specific clique.
ii.2 INS/WO Sensor Fusion (CoreNav)
The method in [kilic2019improved] is adopted to improve the proprioceptive localization framework, an error-state extended Kalman filter (EKF) that uses IMU for state propagation and WO for state correction. It also uses ZUPT and non-holonomic constraints as pseudo-measurements to reduce the solution drift. In this INS/WO sensor fusion framework, it is assumed that a small four-wheeled robot uses a set of sensors, including a GNSS receiver and antenna, an IMU, and wheel encoders. The architecture of the INS/WO sensor fusion is provided in Fig. 2. This sensor fusion method is referred to as CoreNav in this paper.
Following the formulation based on [grovebook, kilic2019improved]
, the error state vector can be constructed in a local navigation frame,
where, is the attitude error, is the velocity error, is the position error, is the IMU acceleration bias, and is the IMU gyroscope bias. For the sake of completeness, the measurement innovations for ZUPT, non-holonomic update, and WO are provided in this section. The detailed derivations and implementation details can be found in [grovebook, kilic2019improved].
ii.2.1 Zero Velocity Update
ZUPT can be used in three ways for wheeled robots: 1) passive ZUPT, as an opportunistic navigational update when rover needs to stop for various reasons, 2) active ZUPT, with periodic stopping [kilic2019improved], and 3) active ZUPT, by deciding when to stop autonomously [kilic2020]. Assuming a stationary rover has zero velocity and zero angular rate (i.e., constant heading), the measurement innovation for a ZUPT can be given as
where is measurement innovation, is estimated velocity vector, and estimated gyro bias.
ii.2.2 Non-Holonomic Update
If a wheeled-robot cannot move sideways with its wheels, then the rover’s velocity is zero along the rotation axis of its wheels, assuming it does not experience side-slip. Additionally, wheeled-robot cannot move perpendicular to the traversal surface. For this reason, these motion constraints can be used as non-holonomic pseudo-measurement update in the filter. Non-holonomic measurement innovation can be given as
where is the coordinate transformation matrix from the body frame to the locally level frame, is rear wheel to body lever arm, and is angular rate measurement.
ii.2.3 Wheel Odometry
In the CoreNav method, the WO measurements are used as an aiding state correction in the error-state EKF. Following the same procedure as described in [kilic2019improved], the measurement innovation for the wheel odometry estimation can be given as
where , , and are estimated longitudinal, lateral and vertical wheel speed, respectively. The subscript denotes the INS estimation, and denotes the WO measurements.
ii.3 GNSS/WO/INS Integration with ZUPT
This section presents two factor graph strategies: 1) utilizing only the zero velocity information in a factor graph method, 2) leveraging the CoreNav position estimates (which are improved with ZUPT and non-holonomic constraints) in a factor graph method. We used the GTSAM library for graph optimization and the standard squared loss function (L2).
The first method does not use INS state estimates in the factor graph directly. It only uses zero velocity signals when INS detects the rover has stopped. To do this, a high certainty zero displacement between-factor , referred to in the Fig. 3 as , is added between the two adjacent state vertices, which are known to be stationary. A low certainty zero-displacement between-factor is also added between non-ZUPT vertices, where the rover is moving, to represent process noise and let the ZUPT information propagate throughout the graph and improve the overall solution. The factors are also referred to as the ZUPT factors in the results section. An illustration of this first method is given in Fig. 3 and called L2-ZUPT.
The second method has a more direct coupling between the GNSS and the INS/WO part. Here instead of utilizing the zero velocity information directly in the factor graph, the positioning solution from the CoreNav error state EKF sensor fusion method following [kilic2019improved](see Fig. 2) is used. Note that, in the CoreNav method, the zero velocity information is used as a ZUPT and also includes the non-holonomic constraints to improve the localization solution further. To couple the EKF estimates with the GNSS factor graph, the obtained positioning estimates from the CoreNav method are added as between-factors among all state vertices. The factors are referred to as the CoreNav factors. A depiction of the second method is given in Fig. 4 and called L2-CN.
Iii Experimental Results
iii.1 Robot System Design
In order to collect the data for validating the method, the Pathfinder testbed rover [kilic2019improved] is used. The robot is a skid-steered four-wheeled platform that can be utilized as a testing platform. A depiction of the rover is shown in Fig. 5. The localization sensor suite setup includes a Novatel pinwheel L1/L2 GNSS antenna [novatel2], and receiver[novatel1], an Analog Devices ADIS-16495 IMU [adis], and quadrature wheel encoders. The robot is also equipped with an Intel RealSense T265 camera [intelt265]; however, this sensor is not used in the localization solution. The computer is an Intel NUC Board NUC7i7DN [intelNUC] which hosts an i7-8650U processor. The software runs on the robot under Robot Operating System (ROS) [quigley2009ros].
iii.2 Field Test Datasets
To evaluate the methods, we use three datasets from [vz7z-jc84-20] referred in the paper as Test 1 (ashpile_mars_analog1.zip), Test 2 (ashpile_mars_analog2.zip) and Test 3 (ashpile_mars_analog3.zip). We also generate a noisy version of these datasets by adding simulated multipath noise based on the elevation of the satellites following the early-minus-late discriminator formulation in [liu2009tracking]. The noise is randomly added to 2 % of the data in each dataset to create the noisy versions. Histogram plots of the simulated range and phase noises are shown in Fig. 6. The rover stops nine times for Test 1, 19 times for Test 2, and 20 times for Test 3 to obtain the zero velocity information. The distances covered in Test 1, 2, and 3 are 671m, 652m, and 663m, respectively. The reference position solutions are obtained by integer-ambiguity-fixed carrier-phase differential GPS (DGPS) processed with RTKLIB [rtklib].
iii.3 Evaluation and Discussion
The three methods compared here are the standard GNSS factor graph (Fig. 1), the GNSS factor graph with ZUPT factors (Fig. 3) and the GNSS factor graph with CoreNav factors (Fig. 4). These are referred to in the figures and tables as L2, L2-ZUPT, and L2-CN, respectively. A comparison of the methods for the clean version of the datasets is given in Table 1. The comparison for the noisy datasets is shown in Table 2. Figures 7 and 8 show the time variation of the errors in the East-North-Up frame. The larger peaks in Fig. 8 in the standard factor graph results are due to the large simulated multipath noise (see Fig. 6.)
|Clean Dataset||RMSE (m)||Max Norm Error (m)|
The best results marked up with green boxes
|Noisy Dataset||RMSE (m)||Max Norm Error (m)|
L2-ZUPT and L2-CN have better performance than the GNSS-only factor graph, L2, for clean and noisy datasets 1 and 2. We see comparable performances from all three methods for dataset 3. L2-ZUPT is found to perform best for clean data, and L2-CN performs best for noisy data. The effect of leveraging zero velocity information can be seen in the noisy data results, where the larger errors in the standard factor graph are dampened by constraining the states during the ZUPTs to be the same since the rover is stationary at that time. The better performance of L2-CN for noisy data can be explained by the fact that the factor graph utilizes more information since it uses the IMU-WO solution from CoreNav. CoreNav also uses additional non-holonomic constraints which are not used in the L2 and L2-ZUPT methods. The performance gap between L2-ZUPT and L2-CN is affected by the number of stops in the datasets. For example, we see a more significant performance gap between L2-ZUPT and L2-CN in Test 1, which has fewer stops. This performance gap is smaller in Test 2, and 3 where the numbers of stops are more than in Test 1, which indicates that using only ZUPT factors in the GNSS factor graph can provide similar localization performance as using a GNSS/WO/INS coupled solution.
Iv Conclusions and Future Work
This work presents GNSS/WO/INS fusion strategies with a factor graph by exploiting the zero velocity information to improve the positioning solution for wheeled robots. The presented methods are compared with a standard GNSS factor graph. This comparison shows that the zero velocity information is a valuable constraint to further improving the positioning solution in a GNSS-degraded environment. Moreover, we observed that using only zero velocity information in a GNSS factor graph can provide comparable positioning performance as using a GNSS/INS/WO coupled position solution. To evaluate our ZUPT aided factor graph methods, we used open access datasets [vz7z-jc84-20]. Also, we made our software implementation publicly available.
Future works will involve testing with more extended datasets with real multipath noise and investigating the connection with robust filtering algorithms. We also experimented with the incremental covariance estimation method discussed in [watson2020robust]. The ZUPT factors are expected to help learn the measurement noise covariance model better. However, the parameter tuning of incremental covariance estimation and ZUPT became a challenge, and due to this, we could not find consistent results with these datasets, which could be attributed to the shorter length of the trajectories.
This work was supported in part by NASA EPSCoR Research Cooperative Agreement WV-80NSSC17M0053, and in part by the Benjamin M. Statler Fellowship.