Motion Constraints and Vanishing Point Aided Land Vehicle Navigation

In the typical Inertial Navigation System (INS)/ Global Navigation Satellite System (GNSS) setup for ground vehicle navigation, measures should be taken to maintain the performance when there are GNSS signal outages. Usually, aiding sensors are utilized to reduce the INS drift. A full motion constraint model is developed allowing the online calibration of INS frame with respect to (w.r.t) the motion frame. To obtain better heading and lateral positioning performance, we propose to use of vanishing point (VP) observations of parallel lane markings from a single forward-looking camera to aid the INS. In the VP module, the relative attitude of the camera w.r.t the road frame is derived from the VP coordinates. The state-space model is developed with augmented vertical attitude error state. Finally, the VP module is added to a modified motion constrains module in the Extended Kalman filter (EKF) framework. Simulations and real-world experiments have shown the validity of VP-based method and improved heading and cross-track position accuracy compared with the solution without VP. The proposed method can work jointly with conventional visual odometry to aid INS for better accuracy and robustness.