Simultaneous Localization and Mapping for Path-Constrained Motion

Accurate localization is a fundamental component of driver-assistance systems and autonomous vehicles. For path-constrained motion, a map offers significant information and assists localization with valuable information about the evolution of the kinematic vehicle states. We propose natural parameterized cubic spline curves to approximate true motion constraints, particularly the centerline of individual road lanes or rail tracks. Vehicle kinematics is modeled in 1-D curve coordinates. Since map information is subject to uncertainties, a probabilistic treatment is a prerequisite to obtaining consistent localization results. The proposed probabilistic curvemap (PCM) and the close map-to-vehicle relation enable a straightforward derivation of measurement update equations without additional map-matching steps and offer themselves to classical filter techniques. Incoming sensor measurements are used for simultaneous vehicle localization and local PCM update around the current vehicle position. Thus, every revisit of a location reduces uncertainty in the local PCM. Moreover, when no prior information is provided in the PCM, extrapolation is carried out to handle these situations with incomplete maps. The proposed filter is validated through simulations and real-world railway experiments.