Landmark Data Selection and Unmapped Obstacle Detection in Lidar-Based Navigation

This research establishes new methods to quantify lidar-based navigation safety in highly automated vehicle (HAV) applications. Lidar navigation requires feature extraction (FE) and data association (DA). In prior work, an FE and DA risk prediction process was developed assuming that the set of extracted features matched the set of mapped landmarks. This paper addresses these limiting assumptions by first providing the means to select a subset of feature measurements (to be used in the estimator) while accounting for all existing landmarks in the surroundings. This is achieved by employing a probabilistic lower-bound on the mean innovation vector’s norm. This measure of landmark separation is used in an analytical integrity risk bound that accounts for all possible association hypotheses. Then, a solution separation algorithm is employed to detect unmapped obstacles and wrong extractions. The integrity risk bound is modified to incorporate the risk of not detecting an unwanted obstacle (UO) when one might be present. Covariance analysis, direct simulation, and preliminary testing show that selecting fewer extracted features can significantly reduce integrity risk, but can also decrease landmark redundancy, thereby reducing UO detection capability. INTRODUCTION This paper describes the design, analysis, and preliminary testing of a new method to maximize safety by selecting landmark data in lidar-based navigation systems. In addition, an integrity risk bound is derived, which accounts for failures to detect undesirable, unmapped and wrongly-extracted obstacles. This work is intended for highly automated vehicles (HAV) operating in changing environments where unknown, moving obstacles (cars, buses, and trucks) are not wanted as landmarks for localization, and may also occlude other useful, mapped landmarks. This paper leverages prior analytical work carried out in civilian aviation navigation where safety is assessed in terms of integrity and continuity [1]. These top-level quantifiable performance metrics are sensorand platform-independent. Integrity is a measure of trust in sensor information: integrity risk is the probability of undetected sensor errors causing unacceptably large positioning uncertainty [1]. Continuity is a measure of the navigation system’s ability to operate without unscheduled interruption. Both loss of integrity and loss of continuity can place the HAV in hazardous situations [1]. Several methods have been established to predict integrity and continuity risks in GNSS-based aviation applications [2-4]. Unfortunately, the same methods do not directly apply to HAVs, because ground vehicles operate under sky-obstructed areas where GNSS signals can be altered or blocked by buildings and trees. HAVs require sensors in addition to GNSS, including lidars, cameras, or radars. This paper focuses on lidars because of their prevalence in HAVs, of their market availability, and of our prior experience. A raw lidar scan is made of thousands of data points, each of which individually does not carry any useful information. Raw measurements must be pre-processed before they can be used for navigation. One widely-implemented approach is to achieve sensor localization by tracking recognizable, static features in the perceived environment [5-19]. Features can include, for example, lines or planes corresponding to building walls in twoor three-dimensional scans, respectively. Previous knowledge of feature parameters can be provided either from a landmark map, or from past-time estimation in Simultaneous Localization and Mapping (SLAM) [20-22]. The resulting information can then be iteratively processed using sequential estimators in SLAM (e.g., Extended Kalman filter or EKF), which is convenient in practical implementations. To estimate the HAV’s pose starting from a raw lidar scan, two intermediary, pre-estimator procedures must be carried out: feature extraction (FE), and data association (DA). First, FE aims at finding the few most consistently recognizable, viewpoint-invariant, and mutually distinguishable landmarks in the raw sensor data. Second, DA aims at assigning the extracted features to the corresponding feature parameters assumed in the estimation process, i.e., at finding the ordering of mapped landmarks that matches the ordering of extracted features over successive observations. Incorrect association is a well-known problem that can lead to large navigation errors [23], thereby representing a threat to navigation integrity. FE and DA can be challenging in the presence of sensor uncertainty. This is why many sophisticated algorithms have been devised [5-19]. But, how can we prove whether FE and DA are safe for life-critical HAV navigation applications? This research question is mostly unexplored. Several publications on multi-target tracking describe relevant approaches to evaluate the probability of correct association in the presence of measurement uncertainty [23, 24]. However, these algorithms are not well suited for safety-critical HAV applications due to their lack of prediction capability, to approximations that do not necessarily upper-bound risks, and to high computational loads. Also, the risk of FE is not addressed. Overall, research on integrity and continuity of FE and DA is sparse. This paper builds upon prior work in [25-27], where we developed an analytical integrity risk prediction method using a multiple-hypothesis innovation-based DA process. We established a compact expression for the integrity risk of lidar-based pose estimation over successive iterations. However, references [25-27] made simplifying assumptions that limit the applicability of these prior results. For example, we assumed that the set of landmarks in the a-priori map was exactly the same as the one being extracted. In practice, unexpected obstacles such as other vehicles passing by are likely to be extracted, and may even occlude other mapped landmarks. The safety risk of using such unknown obstacles as landmarks for navigation has yet to be quantified. In response, in this paper, we derive new methods to select the set of extracted features that minimize safety risk, and to quantify the integrity risk caused by failures to detect unwanted obstacles (UO), while guaranteeing a predefined continuity risk requirement. The next section of the paper provides an overview of the methods developed in [25-27], and of their limitations. We quantify risks involved with using a nearest-neighbor DA criterion [5], defined by the minimum normalized norm of the innovation vectors over all possible landmark permutations. In the third section, an integrity-risk-minimizing data-selection procedure is presented, which considers the following key tradeoff: on the one hand, increasing the number of extracted landmarks reduces the pose estimation error assuming correct association, but on the other hand, it also increases the risk of incorrect association. Thus, if the integrity risk using the full set of landmarks is unacceptably large, the selection process systematically screens all landmark subsets to find the one that meets the integrity risk requirement. The fourth section deals with the situation where a mapped landmark is not extracted, but another unknown obstacle is extracted instead (e.g., case of an obstacle masking a mapped landmark, or failure to extract the intended subset of selected landmark data). A solution separation algorithm is employed to detect UO. The integrity risk bound is modified to incorporate the risk of not detecting a UO when one might be present. Navigation integrity performance using these new methods is assessed in the fifth section of the paper. Covariance analysis, direct simulation, and preliminary testing show that selecting fewer extracted features significantly reduces integrity risk, but also decreases landmark redundancy, thereby reducing UO detection capability. BACKGROUND: INTEGRITY RISK BOUND ACCOUNTING FOR INCORRECT ASSOCIATIONS This section presents an overview of the integrity risk evaluation method described in [25, 27], which uses a multiplehypothesis innovation-based DA process. Integrity Risk Definition and Integrity Risk Bound The integrity risk, or probability of hazardous misleading information (HMI), is defined as illustrated in Figure 1. In prior work [25, 27], we established an analytical bound on the integrity risk, which accounts for the risk of incorrect association. This bound is expressed as:   ) ( ) | ( 1 1 ) ( k K k k CA P CA HMI P HMI P    (1) with   k K k Q CA HMI P   2 ) | (  (2)                    k l l i n i l k y q P CA P A 1 2 , , . , 1 2 4 min 1 ) ( (3) where k : is an index identifying a time step K : designates a range of indices: } ,. , 0 { k K  , from filter initiation to time k . K CA : is the correct association hypothesis for all landmarks at all times 0, . , k } { Q : is the tail probability function of the standard normal distribution  : is the specified alert limit that defines a hazardous situation [1, 4] k  : is the standard deviation of the estimation error for the vehicle state (or linear combination of states) of interest 2 l q : 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 l 2 ,l i y : is the norm of the mean innovation vector weighted by the inverse of the innovation covariance matrix for candidate association i at time step l .