System and method for augmenting a GNSS/INS navigation system in a cargo port environment

1. A system for providing navigation and location information for a vehicle to navigate among a plurality of stacked containers located within a cargo port environment, comprising:

a Global Navigation Satellite System (GNSS) system configured to provide GNSS location information related to the vehicle;

an inertial navigation system operatively interconnected with the GNSS system, the inertial navigation system configured to provide inertial location information related to the vehicle;

an image capture device configured to obtain one or more images of a fixed field of view of the plurality of stacked containers;

a vision system configured to determine vision system location information using the one or more captured images, wherein the vision system location information comprises scaling information of the plurality of stacked containers, wherein the GNSS system, the inertial navigation system and the vision system share a common clock; and

a processor configured to set up various matrices for a Kalman filter to determine a location of the vehicle and to navigate the vehicle among the plurality of stacked containers located within the cargo port environment using the GNSS location information, the inertial location information and the vision system location information, wherein the processor is located on the vehicle with the GNSS system, inertial navigation system, image capture device, and vision system.

2. The system of claim 1 wherein the vision system is configured to determine a horizon line in the one or more captured images.

3. The system of claim 1 wherein the vision system is further configured to determine a plumb line in the one or more captured images.

4. The system of claim 1 wherein the vision system location information comprises a distance travelled of the vehicle.

5. The system of claim 1 wherein the vision system location information comprises a speed of the vehicle.

6. The system of claim 1 wherein the Kalman filter increases a weighting of the vision system location information when the vehicle encounters a multipath environment.

7. The system of claim 1 wherein the vision system is further configured to determine a distance travelled by comparing a change in position of a feature in a first and second image of the one or more images of the fixed field of view.

8. The system of claim 7 wherein the feature comprises an edge of an image of a container in the cargo port environment.

9. The system of claim 1 wherein the vision system is further configured to time stamp each of the one or more images, wherein the time stamp is from the common clock associated with the GNSS system and the inertial navigation system.

10. A method for providing navigation and location information for use in a vehicle among a plurality of stacked containers located within a cargo port environment, comprising:

using a Global Navigation Satellite System (GNSS) system to determine a set of GNSS location information;

using an inertial navigation system to determine a set of inertial location information;

obtaining one or more images using an image acquisition device having a fixed field of view of the plurality of stacked containers;

using a vision system to obtain a set of vision system location information using the one or more obtained images, wherein the vision system location information comprises scaling information of the plurality of stacked containers, wherein the set of vision system location information is time stamped to a common clock shared with the GNSS system and the inertial navigation system; and

determining, by a processor, a set of location information for the vehicle to navigate among the plurality of stacked containers located within the cargo port environment using various matrices for a Kalman filter, wherein the Kalman filter processes the various matrices from the sets of GNSS location information, inertial location information and vision system location information, wherein the processor is located on the vehicle with the GNSS system, inertial navigation system, image acquisition device, and vision system.

11. The method of claim 10 wherein using the vision system to obtain the set of vision system location information comprises determining a plumb line in the one or more obtained images.

12. The method of claim 10 wherein using the vision system to obtain the set of vision system location information comprises determining a distance travelled by the vehicle.

13. The method of claim 10 wherein using the vision system to obtain the set of vision system location information comprises determining a horizon in the one or more obtained images.

14. The method of claim 10 further comprising time stamping each of the one or more images, wherein the time stamp is from the common clock associated with the GNSS and inertial navigation systems.

15. The method of claim 10 further comprises modifying a weighting associated with the GNSS location information.