Analytical and Empirical Navigation Safety Evaluation of a Tightly Integrated Lidar/IMU Using Return-Light Intensity

2.1 IMU Measurement Equations

In this implementation, we use raw IMU accelerometer and gyroscope measurements. The IMU is fixed in the ADS body frame “B,” which can be approximately oriented along the vehicle’s principal axes of inertia. The IMU accelerometers measure inertial acceleration, i.e., second time derivatives of position with respect to the inertial frame (labeled “I”), and we are interested in the ADS pose expressed in a navigation frame “N” (for example, in the local east, north, up directions). We also define an earth-centered, earth-fixed frame “E” because N may move in E. We use the Newton and Euler methods to describe the ADS translational and rotational motion. The following three equations express the time derivative of the ADS velocity with respect to N and expressed in N, the time derivative of the position with respect to E and expressed in N, and the time derivative of the rotation matrix from B to N, respectively (following equation [3.26] in Titterton et al. (2004)):

1

2

3

where:

	is the 3×1 ground speed vector, i.e., the vehicle velocity vector with respect to E expressed in N,
Ad/dt	is a time derivative with respect to frame “A”, where A may stand for B, E, I, or N,
	is the 3×3 rotation matrix from B to N (Titterton et al., 2004),
	is the 3×1 vehicle position expressed in N,
	is the 3×1 IMU-measured and corrected specific force vector expressed in B, as derived in Appendix B,
NωIE	is the angular velocity vector of E with respect to I expressed in N,
NωEN	is the angular velocity vector of N with respect to E expressed in N,
	is the measured and corrected angular velocity vector of B with respect to I expressed in B, as derived in Appendix B,
Ng	is the local gravity vector at the IMU axis center expressed in N (Rogers, 2007),
[a×]	is the skew symmetric matrix of vector a.

2.2 Lidar Range and Bearing Angle Measurement Equations

A raw lidar PC consists of thousands of data points, each of which individually does not carry useful navigation information. Thus, raw measurements must be processed before they can be used for localization. Feature extraction aims to consistently extract identifiable static landmark features. Figure 1 shows an example of a lidar PC collected in our laboratory testbed. Colors from red to blue designate high to low levels of return-light intensity, respectively.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 1

Lidar PC showing return-light intensity (color-coded from blue to red, from low to high intensity)

The experimental testbed in Figure 1 includes six static vertical cylinders with different surface properties, which are easy-to-distinguish landmarks that facilitate feature extraction. We want successful feature extraction because this process is not the primary focus of this paper. Feature extraction aims at finding the center of quasi-circular ellipses formed by the projection of the cylinders’ edges in the lidar zero-elevation angle plane. Figure 2 illustrates our two-step feature extraction algorithm.

• Segmentation: Within an elevation cone, range differences over the azimuthangle sequence help distinguish cylinders from the background. Point clusters corresponding to cylinders can thus be segmented.

• Model fitting and feature estimation: The segmented points are projected in the zero-elevation plane, and a two-dimensional (2D) circle-fitting algorithm is used to fit a circle to the projected points. This step assumes that the cylinders are vertical and that the elevation plane is horizontal, which is valid in our lab environment.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 2

(a) 3D segmentation of a lidar PC; (b) circle fitting and point-feature measurement extraction

The center of the fitted circle, parameterized by its range and bearing angle with respect to the lidar, is the extracted point feature. Let ⁱd and ⁱa, respectively, be the range and bearing angle measurements in B for the point feature of landmark ‘i’, where i = 1…n_L and n_L is the number of extracted features. The horizontal position of the extracted point feature is time-invariant in the navigation frame N, which, in this paper, is fixed in E. In addition, let ⁱp_E and ⁱp_N, respectively, be the east and north position coordinates of landmark “i” in N. The ADS position and orientation vectors in N, x_ADS and e_ADS, respectively, can be expressed as follows:

4

5

where x_E, x_N, x_U are the three-dimensional (3D) ADS position coordinates along the east, north, up axes, and ϕ, θ, ψ are the ADS Euler angles. Euler angles can be extracted from the rotation matrix in Equation (3), as described in Appendix (B).

Using these notations, the nonlinear lidar range and angular measurements are respectively given as follows:

6

7

In Equations (6) and (7), v_d and v_a are random feature measurement errors. Feature extraction error distributions are not Gaussian, but the error’s cumulative distribution function (CDF) can be overbounded by using zero-mean normal CDF models, as described in Appendix (C) (Blanch et al., 2019; DeCleene, 2000; Rife et al., 2006). Throughout the paper, the function “arctan(b/a)”, for and , designates a function that equals arctan(b/a) when a > 0, arctan(b/a) + π when a < 0, π / 2 when a = 0 and b > 0, and −π / 2 when a = 0 and b < 0.

We can stack the ranging and angular measurements for all extracted landmarks in a 2n_L × 1 vector and write the lidar nonlinear measurement equation as follows:

8

9

10

where:

Vector v is modeled as a vector of normally distributed random variables with zero mean and covariance matrix V. We use the following notation: v ~ N(0, V). Nonzero elements of the diagonal matrix V are derived in Appendix (C). In Equation (8), the vector function h(x) consists of stacked nonlinear equations (Equations (6) and (7)) arranged as indicated in Equation (9).

In addition, the lidar provides return intensity measurements for each PC point. We evaluate the mean intensity measurement for landmark “i” by averaging intensity values for all points in a point cluster associated with landmark i. The n_L × 1 return-light intensity measurement vector is modeled as follows:

11

where we use the overbounding distribution derivation in Appendix D to model s as normally distributed with mean s and diagonal covariance matrix V_s. Vector v_s is an n_L × 1 intensity measurement error vector modeled as v_s ~ N(0, V_s), as described in Appendix D.

2.3 Linearization and Discretization of IMU and

3.1 EKF Initialization and IMU-Based Pose Prediction Process

Figure 4 shows EKF initialization and prediction of the state vector x_k with IMU measurements as input. This figure also shows the initialization of components of the integrity risk bound or probability of hazardous misleading information (HMI). The IMU specific force and angular velocity measurements are employed in two parallel processes. (a) Nonlinear state propagation: We use the discrete-time forms of the nonlinear expressions in Equations (1)–(3), the derivation of attitude using the rotation matrix from B to N, and the IMU measurements described in Appendix B to predict the state vector at each time step (Titterton et al., 2004). (b) Linearization, discretization, and covariance propagation: We apply Van Loan’s algorithm with the linearized form in Equation (12) to compute the discrete-time transition matrix Φ_k−1 and the process noise covariance matrix W_k−1. Let be the predicted state estimate and be the state prediction covariance matrix. The other parameters in Figure 4 are defined as follows:

	is the ns × 1 initial predicted state estimate vector,
	is the ns × ns initial EKF state prediction covariance matrix,
P(HMI0)	is the initial value of the probability of HMI,
Bk	is the 3 × 3 rotation matrix relating navigation axes at time k – 1 to navigation axes at time k, as given in Appendix B (Titterton et al., 2004),
Wk	is the ns × ns process noise covariance at time step k,
	is the ns × 1 predicted state estimate vector at time step k,
	is the ns × ns prediction of the EKF covariance matrix at time step k,
g	is the discrete-time form of Equations (1) and (2), defined in Appendix B.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 4

Initialization and EKF prediction process with an IMU

We propagate the state vector x_k by using the nonlinear expressions in Equations (1)–(3). After each iteration in the EKF dynamic propagation update, we assess whether lidar measurements are present for the current time step k. If such measurements are available, we implement a measurement update, as described in Section 3.2; otherwise, we continue iterating the dynamic propagation equations (Equations (1)–(3)).

3.2 Data Association Criterion and EKF Measurement Update

We want to process the linearized lidar measurement equation (Equation (17)) using the EKF measurement update to obtain a correction δx_k, an estimate of the ADS state vector , and the covariance matrix . This process requires that lidar measurements be correctly associated with mapped landmarks because their ordering is not necessarily the same.

To perform data association, we use an innovation-based approach (Joerger et al., 2016). The innovation vector under correct association γ_0,k is given by the following:

18

where A_0,k is the n × n permutation matrix that corresponds to the correct association. In practice, we do not know which is the correct permutation matrix A_0,k, but we can write an exhaustive set of permutation matrices.

The innovation vector can be interpreted as a measure of consistency between the extracted feature measurements and the measurement prediction vector . A more accurate state prediction corresponds to a higher likelihood of correct association. The state prediction is improved, for example, by using IMU data instead of a vehicle kinematic model.

3.2.1 Accounting for Incorrect Data Association

Extracted landmark feature measurements are arbitrarily ordered in vector . In this paper, the number of measured landmarks in the lidar field of view (FOV) can be predicted by using a reliable map and vehicle pose prediction. In the case of occlusion, if two landmarks are in the same azimuth bin, then only the landmark nearest to the lidar is visible. Other cases such as failed extractions or extracted-but-unmapped landmarks have been addressed in prior work via combination matrices and detection (Hassani et al., 2019; Joerger et al., 2017). This paper focuses on the incorporation of IMU and intensity measurements. Let n_L be the number of extracted landmarks that are visible in the lidar FOV. There are (n_L!) potential ways for assigning the observed landmarks to the mapped landmarks, which is the number of all possible landmark permutations.

Incorrect association occurs when the ordering of measured landmarks differs from that of mapped landmarks. There is only one correct ordering; thus, the number of incorrect associations is h_IA ≡ n_L! −1. For risk evaluation, we consider all possible orderings of measurements, and where i = 0…h_IA. In an example scenario with n_L = 3 landmarks (both extracted and mapped), the numbers of possible landmark permutations and incorrect associations are 3! and h_IA = 5.

The innovation vector γ_i,k has a zero mean only under correct association. Any other (incorrect) association causes the mean of the innovation vector to be non-zero. Thus, the innovation vector is a good indicator of incorrect association. The innovation vector can be expressed as follows:

19

where A_i,k are n × n permutation matrices for i = 0…h_IA.

Based on this criterion, we select the candidate association that satisfies the following equation:

20

where:

Figure 5 shows a detailed description of the second block in Figure 3. Lidar measurements, map data, predicted states, and the covariance matrix serve as inputs to the data association process in Equation (20). Then, we proceed to the EKF measurement update, calculate the Kalman gain K_k, and determine the n_s × 1 state estimate vector and the n_s × n_s estimation error covariance matrix . The state prediction vector in Equations (18) and (19) is more accurate when a tightly integrated IMU is used as compared with an ADS kinematic model, which ultimately reduces the risk of incorrect association.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 5

Data association and EKF measurement update with lidar

3.2.2 Integration of Lidar Return-Light Intensity to Improve Data Association

The association process can be further improved by using lidar return-light intensity. The difference between the lidar-extracted mean intensity measurements and that provided in the map, which is captured in the intensity-separation vector ξ_i,k, can be expressed as follows:

21

where:

sk	is an nL × 1 mean return-light intensity vector for all nL visible landmarks,
ŝk	is the nL × 1 lidar-measured mean return-light intensity vector; we assume ŝk ~ N(sk, VS,k),
	is the mapped mean return-light intensity vector,
	is an nL × nL covariance matrix capturing the uncertainty in the mean intensity values of the mapped landmarks,
	are nL × nL permutation matrices similar to those in Equation (19) but for scalar permutations, for i = 0…hIA.

Similar to the innovation vector in Equation (18), the intensity separation vector in Equation (21) has a zero mean only if the correct association is found. Landmark intensity parameters are not included in the EKF prediction and estimation processes because they do not provide direct information on ADS states. However, we can still improve the association criterion by augmenting the innovation vector with ξ_i,k. The resulting 3n_L × 1 “separation vector” is defined as follows: . The association selection criterion for incorporating the return-light intensity is given by the following:

22

where:

3.3 Integrity Risk Bound

The integrity risk P(HMI_k), or probability of HMI at time step k, is the probability of the ADS being outside of a specified alert limit box when the vehicle position is estimated to be inside this box (Joerger & Pervan, 2009; Reid et al., 2019). In ADS lane-centering applications, lateral deviations are of primary concern, and the alert limit is defined as the distance between the edge of the car and the edge of the lane when the car is at the center of the lane (Joerger et al., 2016). An analytical bound on the integrity risk that considers all possible incorrect associations has been given by Joerger & Pervan (2019) and is expressed as follows:

23

with:

24

25

where:

K	designates a range of time indices: K = {1…k},
J	designates a range of time indices: J = {1…j},
Q()	is the tail probability function of the standard normal distribution,
ℓ	is the specified alert limit that defines a hazardous situation,
σk	is the standard deviation of the estimation error for the vehicle state of interest,
IALLOC,k	is a predefined integrity risk allocation for feature extraction, chosen to be a fraction of the overall integrity risk requirement IREQ,k,
	is a chi-square distributed random variable with a number of degrees of freedom that is the sum of the number of measurements and of states at time step j,
	represents the minimum value of the mean landmark feature separation, including intensity separation, at time step j.

The probability of correct association in Equation (25) is a function of , which defines a probabilistic lower bound on the true value of ζ_k in Equation (22). This lower bound on landmark separation is set such that the risk of the true value of ζ_k being smaller than does not exceed I_ALLOC,k.

By integrating lidar with an IMU, we can reduce positioning errors, thereby lowering the risk P(HMI_k | CA_K). In addition, IMU and return-light intensity measurements are instrumental for increasing the ability of the localization system to distinguish landmarks. In Equations (23)–(25), IMU and return-light measurements enable greater separation values, which increases the probability of correct association P(CA_j | CA_J−1) and ultimately reduces P(HMI_k). We will quantify this P(HMI_k) reduction using simulation and experimental data in the next two sections. Equations (23)–(25) are represented by the “Integrity Risk Evaluation” block in Figure 3, and the output is P(HMI_k).

4.1 Covariance Analysis and Integrity Risk Bound (Analytical vs. Direct Simulation)

In Figure 6, the positions of landmarks are represented by black circles, and the ADS trajectory is shown by black triangles. The 2D estimation error covariance ellipses, which represent the spread of pose estimation error, are shown in solid and dashed red lines and are inflated by a factor of 50 for better visualization. The size and shape of the covariance ellipses change as the sensor-to-landmark geometry changes because of ADS motion. The relative lengths of the semimajor and semiminor axes are also related to the standard deviations of the lidar angular and ranging measurements, as explained by Joerger (2009). This figure also shows that the integration of IMU with lidar improves the ADS trajectory estimation.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 6

ADS positioning error covariance ellipses obtained by using lidar-only and IMU/lidar schemes for a two-landmark scenario

In Figure 7, we evaluate the analytical integrity risk bound P(HMI_k) as compared with the actual integrity risk calculated by direct simulation over 50,000 Monte Carlo (MC) trials for the lidar-only and IMU/lidar schemes. We focus on lateral deviations for integrity risk evaluation; the lateral alert limit is defined in Table 1. As captured in Equation (23), P(HMI_k) accounts not only for lateral covariance variations, but also for the probability of correct association P(CA_k).

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 7

Lateral positioning integrity risk when using the lidar-only and IMU/lidar schemes for a two-landmark scenario: analytical bound versus MC simulations over 50,000 trials

In Figure 7, black and red circles represent the integrity risk curves obtained by direct simulation. In parallel, the black and red solid lines are the analytical bounds computed from Equation (23). Both direct simulation and analytical bounds for the IMU/lidar scheme are orders of magnitudes lower than that for the lidar-only scheme at 10–20 m of travel distance when the vehicle is close to the landmarks. Simultaneously using the IMU improves the pose estimation and data association, which results in a reduced integrity risk. The direct simulation and analytical bounds for the lidar-only scheme overlap. Discrepancies occur for low risk values: these discrepancies would disappear if we simulated more than 50,000 trials, but our computational resources are finite.

5.1 Experimental Testbed

We designed and built an automated sensor safety evaluation testbed to quantify the impact of incorrect association on the integrity risk P(HMI_k). The testbed shown in Figure 8 is composed of a rover housing a sensor platform on a figure-eight track. The rover can operate unattended for many hours to collect large amounts of lidar and IMU data. This testbed provides a means for analyzing the performance of a navigation system over repeated trajectories, over a significant number of such trajectories, and in a controlled environment in which we can focus, for example, on the integrity impacts of landmark occlusions and of landmarks with varying surface properties. Compared with the approach using only lidar range and angle measurements, this approach helps assess the relative performance improvement brought about by additional IMU and light-intensity data. Other experiments using sensors mounted on a car’s roof rack will be performed in future work.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 8

Automated testbed setup with a sensor platform repeatedly moving on a figure-eight track

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 9

(a) IR camera, (b) IR markers on a sensor platform, (c) lidar-VLP-16 Puck, (d) IMU-IGM-A1

In this experiment, cardboard cylinders serve as landmarks to facilitate feature extraction from the lidar PC. These cylinders are covered with white and black felt and retroreflective straps (Landmark 1, first cylinder from the left) to provide different surface reflectivities. As the rover moves, the leftmost landmark (Landmark 1) is periodically occluded behind another landmark (Landmark 2), which tests the ability of the data association process to dynamically distinguish landmarks.

The sensor platform mounted on the rover includes the lidar and IMU stacked vertically to minimize lever arm calibration errors. We used Velodyne’s VLP-16 Puck LTE lidar and NovAtel’s IMU-IGM-A1 coupled with NovAtel’s ProPak6. The IMU was set to record at a 100-Hz sampling rate. Additionally, an infrared (IR) camera motion capture system (VICON) provides reference truth values for the position and orientation of the moving platform and for the static landmarks in the navigation frame. Twelve cameras, i.e., four VICON MX-T20 cameras and eight Vantage 5 cameras, record small retroreflective markers placed on the sensors and landmarks, providing sub-centimeter-level positioning. All three sensors (IR cameras, lidar, and IMU) are time-tagged using the same computer clock.

5.2 Using Lidar Range, Bearing, Intensity, and IMU Measurements

The integrated solution using IMU, lidar range, angle, and intensity measurements is referred to as the IMU/lidar+ configuration. In Figure 10, four landmarks are used. The lidar range limit is such that all landmarks are continuously in view of the lidar except where Landmark 2 occludes Landmark 1. The estimated trajectory is represented by a blue line and the true trajectory by a black line. The estimated and true trajectories overlap. The black arrow shows the direction of motion at the starting point. Background colors help identify segments of the rover trajectory for results presented over time: the rover follows straight line paths in the dark gray area, is in the top loop when in the white area, and is in the bottom loop when in the light gray area. The purple area represents ADS locations at which Landmark 1 is occluded by Landmark 2 in the upper loop of the figure-eight track. This arrangement makes data association challenging because Landmarks 1 and 2 can be mistaken for one another when Landmark 1 comes in and out of sight.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 10

Covariance ellipses obtained by using the IMU/lidar scheme with intensity measurements

Figure 10 also shows red covariance ellipses representing the 2D positioning uncertainty for ADS locations taken at regular 0.8-s intervals. Covariance ellipses are inflated by a factor of five to facilitate visualization. Ellipses grow when Landmark 1 is hidden (purple area).

In addition, we derived a bound on the risk of the cross-track positioning error exceeding an example alert limit of 0.35 m (Reid et al., 2019). This integrity risk bound is predictable. The event of the risk bound exceeding the risk requirement causes a loss of availability. Thus, in this paper, we want the integrity risk bound to be as low as possible to achieve high availability performance. Both the actual integrity risk itself and our ability to analytically upper-bound this risk determine the value of the predicted risk bound. Thus, as compared with using lidar ranges, additional information from the IMU and lidar intensity not only helps reduce the actual P(HMI) but also helps tighten the predicted P(HMI) bound. Figure 11 shows P(HMI) curves for the lidar-only, lidar+, and IMU/lidar+ schemes. We find the highest risk bound values in the purple regions because the occlusion of Landmark 1 causes relatively poor landmark geometry. The lidar-only approach performs poorly, with a P(HMI) bound approaching a value of 1 as soon as the first difficult-to-identify landmark geometry is encountered. The lidar+ approach is consistently better except when Landmark 1 is occluded because fewer measurements are available and the risk of incorrect association increases. Finally, as expected, the IMU/lidar+ approach outperforms the other configurations, and our tests show that the resulting P(HMI) bound is at least four orders of magnitude lower than those of the other cases.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 11

Integrity risk bounds obtained by using the lidar-only scheme versus the lidar+ and IMU/lidar+ schemes

5.3 Repeated Trajectories

Figure 12 shows the ADS cross-track positioning error and covariance envelopes over 100 laps. The figure-eight track helps evaluate navigation system performance repeatability. The “1σ” one-dimensional envelope represents the boundary within which 68% of the error samples are expected to occur, assuming a zero-mean error.

• Download figure
• Open in new tab
• Download PowerPoint

FIGURE 12 Cross-track error of ADS for 100 laps

The zoomed-in panel shows the IMU-derived positioning drift corrected via lidar updates at regular 0.1-s intervals.

The solid red line in Figure 12 is the analytical covariance envelope. The analytical covariance determines the contribution of P(HMI_k | CA_K) to P(HMI_k) in Equation (23). The dashed red line is the sample covariance envelope, which is smaller than the analytical envelope over the entire 20-s-long trajectory; we want the analytical error bound to be larger than the sample envelope. Cross-track positioning error curves are color-coded from light blue to dark blue as the rover travels from the first to the last lap. Small discrepancies can be observed between early and late laps (e.g., accentuated at the 12- to 14-s time points), which are due to imperfections in the testbed, including a warm-up period causing variations in vehicle speed and sensor performance (Reina & Gonzales, 1997; Ye & Borenstein, 2002). Overall, we find that the pose estimation error curves are conservatively captured by the analytical covariance envelope.