Robust vehicle pose estimation from vision and INS fusion

The vehicle relative pose, referenced to road boundaries or lanes, is fundamental for an autonomous driving vehicle to perform motion planning. The estimate of relative pose includes the difference between the car heading angle and road center-line direction (i.e., the relative heading angle), together with the lateral displacement of the vehicle from the road center-line. This information can be equally derived by an inertial navigation system equipped with GPS receivers or by a vision system based on cameras. However, both solutions present some disadvantages. GPS-based estimates of heading angle shows loss of accuracy during low-speed manoeuvres or within tunnels. On the other hand, a line-fitting based estimation typical of vision systems suffers from lines accuracy, and they are hard to be implemented in an urban environment or high-curvature track scenarios. This work presents an innovative integrated algorithm that fuses those two approaches, providing vehicle relative pose with respect to road lane combining data coming from an inertial navigation system and a line detection algorithm. This integrated solution is then able to always feed the panning algorithm with an accurate estimate of the vehicle pose in multiple challenging scenarios.