Precise Pose Graph Localization with Sparse Point and Lane Features

Paper

Precise Pose Graph Localization with Sparse Point and Lane Features

Motivation

• to achieve cm-level accuracy localization, relying only on GPS sensor is not enough

Proposed scheme

• take into account the information from different sensors and a high precision map
• use graph smoother instead of filter techniques to estimate the vehicle pose
• view the localization problem as constraints between vehicle poses and landmarks

• vehicle node => soft node; landmark => fixed node (fixed nodes will not be estimated)

• motion model
  • vehicle pose at time t: (x_t, y_t, θ_t)
  • with odometry information, the vechicle pose at time (t+1) can be inferred
  • motion constraint: o_i = (∆x_i, ∆y_i, ∆θ_i) between nodes i and i+1
• observation model
  • LIDAR data => static grid map => extract feature points by open source blob detector
  • in the form of (z_t,1, z_t,2, …) , where z_t,i = (x_t,i, y_t,i) in the vehicle coordinate system
  • the image sequences from the camera are processed to get the offset and heading angle to lane markings
• data assocation
  • enable a multi-modal selector to find the correspondences (via NNS)
  • use scaled convariance method to deal with false positive data association
  • after each iteration in the graph optimization, the best associated feature points will be re-selected
• graph optimization
  • assuming the observations are affected by Gaussian noise and the data association is known
  • to compute the best configuration of the posterior over the vehicle trajectory
  • each pose node is initialized by a GPS signal
  • iteratively minimize the cost function to update the position of pose node until convergence
  • cost function inovlves in motion, feature point, lane marking measurements
  • solved by Gauss-Newton algorithm and Cholesky factorization

Experiment setup

• four Velodyne VLP-16s are mounted at the four corners of our research car
• camera is behind the windshield and points in the driving direction
• INS/GNSS system is mounted at the car’s center of gravity
• map is provided by 3D Mapping Solutions GmbH
• lane and landmark information are stored in a PostgreSQL database
• experimental results in terms of RMSE：
  • lateral：90 % is < 0.35 m
  • longitudinal：90 % is < 0.50 m
  • heading：90 % is < 0.41 deg