IROS2019国际学术会议论文集0178

Map-Aware SLAM with Sparse Map Features Patrick Burger, Benjamin Naujoks and Hans-Joachim Wuensche AbstractLocalization is a key capability for autonomous ve- hicles. High-Defi nition maps are a popular method to represent the environment and to enable precise localization. However, the creation is very demanding and it is not always guaranteed to receive accurate map information, especially for unstruc- tured areas. In this paper, we introduce a novel probabilistic localization and mapping framework that brings together the advantages of sparse feature maps, multi-target tracking for landmark detection, probabilistic global vehicle localization and a graph-based formulation to achieve a consistent map. The front-end of our Simultaneous Localization and Mapping framework is based on Monte Carlo Localization. Our novel measurement model integrates a virtual topological Path-Map with sparse map features to obtain global localization. The graph-based back-end optimizes online the vehicle trajectory and the landmarks confi guration to create a globally aligned map. Furthermore, our method allows weaker requirements in terms of accuracy of the sparse feature map as we represent the degree of uncertainty by means of probabilistic distribution. Additionally, the sparse feature map representation needs substantially less memory than other approaches, which is an advantage for autonomous vehicles. The framework has been tested and evaluated in real experiments for several autonomous runs. The results demonstrate the robustness of our system. I. INTRODUCTION Localization and Mapping is still a challenging and exciting problem in research and especially in the fi eld of autonomous vehicles. An accurate localization is crucial for path planning and navigation. Furthermore, maps help intelligent vehicles to better understand their environment. The most common localization technology is still the Global Navigation Satellite System (GNSS). It is inexpensive, but suffers from outages, occlusions and therefore, does not offer the reliability required for autonomous vehicles. Consequently, robust localization systems combine several sensors in a sensor fusion to overcome the failures and to reduce the weaknesses, respectively. In the past, several methods have been developed to solve the task of Simultaneous Localization and Mapping (SLAM) of an autonomous system 1, 2. Originally, SLAM methods do not take prior information about the environment into account. They estimate the most likely map and trajectory given a sequence of observations. In order to compensate for accumulated drift, these systems must perform loop closures that restrict driving behavior. Furthermore, for autonomous driving, rescue and surveillance scenarios a robot has to arrive at a given global position. This is only possible if a globally referenced map is available. In addition, there are localization methods which only allow localization in maps recorded with All authors are with the Institute for Autonomous Systems Technology (TAS) of the Universitat der Bundeswehr Munich, Neubiberg, Germany. Contact author email: patrick.burgerunibw.de Path in UTMPoint cloudMotion GIS Path-Map Landmark processing Monte Carlo Localization Pose-Graph M zt zt ut1 ut1 xt V Fig. 1: Data fl ow between the front-end (green, blue) and back-end (red) of our SLAM framework. a similar sensor setup. Moreover, their performance depend strongly on resolution and level of detail. In contrast, we are able to localize our vehicle in places that have never been sensed before. We take our inspirations from humans who are able to accomplish this task while only having access to a rough environmental and topological description. We operate in unstructured areas where maps are rarely available. Common landmarks for localization, e. g., road markings, signs and buildings are absent. However, our method is able to work with different sparse map feature types that vary from trees, bushes in unstructured ares to signs, pole like structures and road markings in urban areas. II. RELATEDWORK Global localization is an important part of every au- tonomous driving system. Reliable methods are needed to obtain a robust and stable system. In recent years, several approaches have emerged, which provide high quality local- ization performance even at real-time execution speeds. One of the fi rst Monte Carlo Localization (MCL) method for an autonomous system is proposed by Dellaert et al. 3, where sonar readings or laser scans are used to localize a robot in an occupancy grid map. In the following years different variants were presented. Levinson et al. 4 assign an infrared refl ectance value to each grid cell. A graph SLAM method is used to obtain a globally accurate and consistent map across multiple mapping runs, while online localization is performed with a MCL to estimate the relative vehicle pose to the previously created map. The approach of Kuemmerle et al. 5 achieve global consistency by utilizing publicly accessible aerial images as prior information. They model constraints between correspondences, found in three-dimensional range data, with a graph-based formulation of the SLAM problem. The basic method of 4, 5 is very similar to ours. In the front-end, a MCL is used for the localization process, while a graph-based SLAM approach aligns the observations to a consistent map. However, in this paper we only use a sparse feature map representation. Additionally, we create the map online during localization while taking the uncertainty of 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Macau, China, November 4-8, 2019 978-1-7281-4003-2/19/$31.00 2019 IEEE347 landmarks and the estimated vehicle pose into account. Ruchti et al. 6 propose a grid-based road classifi cation scheme for 3D range data with a novel sensor model, which relates the classifi cation results to an OpenStreetMap (OSM) road network. The method of 7 improve vehicle localization by using prior information from an occupancy grid to bound the possible poses and to localize within this map by a particle fi lter approach. Compared to 6, 7, we implicitly weight each particle based on the deviation of the past trajectory to the pose sequence of the Path-Map. Thus, we do not need to create an additional data representation, and the result does not depend on the grid size. Furthermore, grid-based methods generally require a considerable amount of memory, especially for large maps. Our method only needs a fraction of the data volume. A common approach for vehicle localization is to match a road network against data observed by the robot. Floros et al. 8 combine visual odometry with map information from OSM to provide a robust and accurate estimate for the vehicles position, while the Chamfer-distance is used to compare fragments of visual-odometry trajectories with the street network. Brubaker et al. 9 use odometry computed from two video cameras and road maps with a probabilistic model for which an effi cient inference algorithm is derived. The method of 10 introduce a graph-based localization technique utilizing sparse point features as well as lane markings from a HD-map. Compared to 10, we use a non-linear MCL approach for localization, where the uncertainty and existence probability of the sparse map features are not known. Furthermore, our back-end works similar to 10, as we model associated landmarks with map features as fi xed nodes in our graph- based formulation. III. LOCALIZATION0,zm ? , by taking the Mahalanobis distance into account: dM ? zlm t,n, ln,Pt,n ? = r? zlm t,n ln ?T P1 t,n ? zlm t,n ln ? . Pt,nis the position covariance of the tracked landmark, and weighting factorzmis based on the deviation from the expected dimensions of the map feature to the estimated dimension of the tracked landmark. For the evaluation, we compare the length, width, height and intensity ratio with a simple geometric model of our semantic landmark description. If all criteria fulfi ll our assumption, we set the maximum weight zmto one, otherwise to the minimum value of 0.5. In order to fi nd the closest sparse map feature lnto each landmarkzlm t,n, the kd-Tree structure of our Path-Map is utilized with a simple 2D Euclidean distance metric. To increase robustness, we use a greedy method where we consider only those landmarks that maximize our likelihood p( zt| xt,m), while limiting the number of landmarks to the number of sparse map featureskifor each Path-Map node featureli mi. This heuristic works well in our scenarios and especially in cases, where no corresponding map feature is found, or in scenes with considerably more landmark observations than sparse map features. xt zlm t,1 p(m3| xt) p ?zlm t,2| xt,l1 ? p ?zlm t,3| xt,l2 ? p ?zlm t,4| xt,l3 ? Fig. 3: This fi gure illustrates our measurement modelp(zt| xt,M) using the example of Figure 2 with oriented Path-Map nodes m(black triangles). The current landmark observations zt= zlm t,1,z lm t,2,z lm t,3,z lm t,4(green) are matched against the Path-Map tuplem3= p3,l1,l2,l3 Mand their corresponding map features (gray). To increase robustness, we use a greedy method where only those landmarks (green circles) are considered that maximize our likelihood, while the others are not taken into account (green crossed box). Next, the likelihoodp( zt| xt,m3)is weighted according top(m3| xt)(red arrows). This weight represents how good the Path-Map nodem3matches, given the particle statextand the past trajectory (blue points). The diameter of the blue trajectory points represent the weights, respectively. To estimate the likelihoodp(m | xt)of the Path-Map tuple mfor the statext, the Mahalanobis distancedM(p,xt,Pt) between the the Path-Map posepof tuplemand statextis considered. We use a Gaussian distribution with zero mean, standard Path-Map deviationpmand covariance matrix Pt of the current MCL state x: p(m | xt) = N ? dM(p,xt,Pt);0,pm ? .(7) In order to increase the weighting of the orientation with respect to the metric distance, we set the matrix elements qx,qy,qx,qy Ptto zero and convertqof Pt,p ofp,xofxtto degree while we normalize the angles to overcome the problem of singularity. Furthermore, it has been proven that the past trajectory of robots give rich information for localization 6, 7, 8, 18. Therefore, we expand the idea of 7, 8 with a weighted trajectory-matching approach, where we assume that the localization trajectory is similar to the topological Path-Map. Each particle weight is essentially a function of its proximity to the Path-Map. Particles and their trajectories are weighted according to this distance. Therefore, we extend Equation (7) by the product over the set of past trajectory poses Tt: p(m | Tt) = N ? dM(p,t1,Pt);0,pm ? (8) |Tt| Y k=1 tr,k N ( (tk,pi,pi+1),pm), with perpendicular distance (tk,pi,pi+1)between each trajectory posetk Ttand the line between the two closest Path-Map nodes (pi,pi+1 ). In order to fi nd a set of closest corresponding Path-Map nodes with a similar heading angle, the distance metric is modeled as follow: pi= argmin pM q (px tx)2+ (py ty)2+ deg(p t)2, with heading differencedeg(p t)in degree and normal- ized angles. This approach is only an approximation but 350 worked well in our scenarios, and it avoids the incorrect assignment that occurs if there exists a closer Path-Map pose with the opposite orientation. Additionally, we assume that the drift is small at short in- tervals but increases at larger distances. Thus, we consider the distance-dependent uncertainty with an exponential weighting functiontr,k= e(tr|tkt1|2)that weights the trajectory posestkaccording to the distance to the latest trajectory pose t1with a penalty factortr. Figure 3 illustrates the weighting according to the radius of each trajectory pose. 3) Initialization: Our framework supports two different methods of initialization. If a global position is available, the vehicle state xtis initialized to it, and the particles are sampled around this state, according to a Gaussian distribution considering the GNSS receiver position covariance. If no initial global position is available, the given path is used for initialization. Then, all particles are sampled along the Path-Map with a uniform distribution of the weights. D. Graph-based Maximum Likelihood SLAM In this paper, a graph-based optimization is used to obtain the maximum likelihood of the vehicle trajectory and landmarks confi guration. Our back-end is based on a graph-based formulation of the SLAM problem where pose, odometry and landmark observations are represented. In the optimization step, our method aims to fi nd the optimal node constellation that minimizes the error given by the observations. We assume that the observations are infl uenced by Gaussian noise and the data association problem is solved in the front-end of our framework. In general, a SLAM graphG = (V,E)consists of a set of nodesV = vi,.,vnand edgesE. The edge i,j,i,j Ebetween two nodes with indexi,jis described by the meani,jand the information matrix i,jof the measurement. Moreover, the error function ei,j ?v i,vj,zi,j ? = zi,j h (vi,vj)between two nodes vi,vj is defi ned as the difference between the obtained measurement zi,jand expected measurement h (vi,vj). In our graph, we represent landmarks as 2D points and vehicles poses in SE(2). LetvtT t=1 be the set of vehicle pose nodes in SE(2) at timetandqjJ j=1 the set of 2D landmark point nodes. Then, we defi ne our set of vertices as follows: V = vtT t=1 qjJj=1| T,J N. E. Error Functions The key objective of our method is to leverage additional knowledge from an available sparse feature map in the optimization process to smooth and align both the vehicle trajectory and the observed landmarks to a global referenced map. Therefore, we model the spatial constraints in form of error functions between pose-pose as well as pose-landmark, while we add constraints that transform the representation to a globally referenced UTM coordinate system. In our approach, edges between consecutive vehicle poses are described by dead-reckoning displacement, where the new pose estimatef(vt1,ut1)is based on a non-linear v1 v2 v3 q1 q2 q4 q5 q6 q3 d3 g4 g5 o2o3 e1,1 e1,2 e2,1 e2,2 e3,4 e3,3 e3,5 e3,6 Fig. 4:This examples visualizes the mapping process. While our front-end does not provide a global localization pose xt, we continually add dead-reckoning-estimated posesv1,v2(triangles) and the observed landmark nodesq1,q2(circles) to our graph. Later, when a global estimated pose is available, the global offset constraint (yellow triangle) for graph posev3is added. The circles q4,q5represent landmarks which were successfully matched with the sparse map features. The corresponding global offset constraint is visualized with yellow points. The spatial constrains of our back- end is visualized with directed lines between pose nodes (triangles) and landmark nodes (circles). Furthermore, if the uncertainty of a map feature is known and a landmark observation is successfully associated to it, we set the landmark node (q6in this example) as fi xed in map coordinates. motion model, that was mentioned in Section III-C.1. The error function is therefore modeled as follows: ot(V,ut1) = vt f(vt1,ut1).(9) Furthermore, the pose-landmark constraint is modeled as: et,j ?V,zlm t,j ? = zlm t,j h lm(vt,qj), (10) wherehlm(vt,qj)represents the expected measurement of landmark nodeqjat posevt, andzlm t,j is the tracked landmark pose at time t. In order to take the global position estimate xtfrom the front-end into account, the offset constraint is modeled as follows: dt(V, xt) = vt xt.(11) Furthermore, if a landmark observationzlm t,j could be matched with a sparse map featureln, the global offset constraint is modeled as follows: gj(V,zlm t,j, ln) = qj hg ? zlm t,j, xt, ln ? ,(12) wherehg ? zlm t,j, xt, ln ? transforms the closest sparse feature lnat state xtto its origin in UTM coordinates. To improve convergence, we update the constraint of graph landmark qjonly, if the states position uncertainty of the tracked landmarkzlm t,j is less than the position uncertainty of previous estimate zlm t1,j. The constructed graph is visualized in Figure 4, where the global constrains are visualized in yellow. Before the graph is optimized, we continually augment vehicle pose estimatesvtand landmark observationsqjto the set of nodesV, whenever the robot moves more than0.2m or rotates more than20. Furthermore, if the uncertainty of a map feature is known and a landmark observation is successfully associated to it, we set the landmark nodeqjas fi xed. This highly constrains the optimization step, as these nodes are not estimated. In addition, this explicitly allows to differentiate between self-registered map features and features 351 originating from an unknown source. Finally, the optimized landmarks are fed into our Path-Map to enrich the map, as shown in Figures 1 and 5. The graph optimization can be considered as a nonlinear least squares problem that is typically solved by forming, solving and iterating a linear system around the current state. In our work, the g2o framework 19 is used with a pre- conditioned conjugate gradient (PCG) technique to solve the sparse pose constraint minimization problem of Equation (13). Using the standard pose-graph SLAM formulation 20, the resulting minimization problem can be written as follows, using an abbreviated notation of the error functions: V= argmin V X t oT tQ 1 t1ot+ X tj eT t,jP 1 t,jet,j+ (13) X t dT t P1 t dt+ X j gT jP 1 t,jgj, and where the information matrices of the constraints are based on the inverse noise covariance matrixQ1 t1 of the motion model, inverse covariance matrixP1 t,j of the tracked landmark with indexjand invers