Range image aided inertial navigation system (INS) with map based localization

1. A navigation system comprising:

an inertial measurement unit (IMU) attached to a dynamic platform and configured to measure a specific force vector and an angular rate vector of the dynamic platform;

an inertial navigation system (INS) unit coupled to the IMU and configured to determine a current INS solution of a position and an orientation of the dynamic platform based on at least: (i) a previous INS solution, and (ii) the specific force vector and the angular rate vector measured by the IMU;

a range image sensor (RI sensor) attached to the dynamic platform and configured to acquire range image data (RI data);

an RI data preprocessor coupled to the RI sensor and the INS unit, the RI data preprocessor configured to perform an a priori transformation of the RI data using the current INS solution to obtain transformed RI data;

an RI map database storing a plurality of keyframe maps and configured to retrieve a valid keyframe map among the plurality of keyframe maps based on the transformed RI data, the valid keyframe map having a known position and a known orientation in a navigation coordinate frame;

wherein the navigation system is configured to:

construct a map registration cost gradient (MRCG) measurement based on at least (i) the transformed RI data, and (ii) the known position and the known orientation of the valid keyframe map;

determine INS error corrections and INS solution statistics based on the MRCG measurement, wherein the INS unit is configured to update the current INS solution based on the INS error corrections; and

determine an absolute solution of a current position and a current orientation of the dynamic platform based on the current INS solution and the INS solution statistics.

2. The navigation system of claim 1 , wherein the IMU comprises a three-axis accelerometer and a three-axis gyroscope.

3. The navigation system of claim 1 , wherein the RI data comprises a point cloud.

4. The navigation system of claim 1 , wherein the RI sensor comprises a lidar, a radar, or a 3D camera unit.

5. The navigation system of claim 1 , further configured to:

determine that the valid keyframe map is not available in the RI map database;

generate the valid keyframe map based on the transformed RI data; and

store the valid keyframe map in the RI map database.

6. The navigation system of claim 1 , further comprising:

one or more aiding data sources configured to provide aiding data, the navigation system further

configured to:

determine the INS error corrections and INS solution statistics based on the aiding data and the current INS solution.

7. The navigation system of claim 6 , wherein the one or more aiding data sources comprise one or more of global navigation satellite systems (GNSS) receiver(s), a wheel odometer, a zero velocity detector, a radar, a sonar, a magnetic compass, and a magnetometer.

8. The navigation system of claim 1 , wherein:

the IMU is configured to measure the specific force vector and the angular rate vector of the dynamic platform at a first rate; and

the navigation system is configured to determine the INS error corrections and the INS solution statistics at a second rate slower than the first rate.

9. A navigation system comprising:

an inertial measurement unit (IMU) attached to a dynamic platform and configured to measure a specific force vector and an angular rate vector of the dynamic platform;

an inertial navigation system (INS) unit coupled to the IMU and configured to estimate a current navigation solution of a position and an orientation of the dynamic platform based on at least: (i) a previous navigation solution, and (ii) the specific force vector and the angular rate vector measured by the IMU;

a range image sensor (RI sensor) attached to the dynamic platform and configured to acquire range image data (RI data);

an RI data preprocessor coupled to the RI sensor and the INS unit, the RI data preprocessor configured to perform an a priori transformation of the RI data using the current navigation solution to obtain transformed RI data; and

an RI map database storing a plurality of keyframe maps and configured to retrieve a valid keyframe map among the plurality of keyframe maps based on the transformed RI data, the valid keyframe map having a known position and a known orientation in a navigation coordinate frame;

wherein the navigation system is configured to construct a map registration cost gradient (MRCG) measurement based on at least (i) the transformed RI data, and (ii) the known position and the known orientation of the valid keyframe map; and

wherein the INS unit is further configured to determine an absolute navigation solution of a current position and a current orientation of the dynamic platform based on at least (i) the current navigation solution, and (ii) the MRCG measurement.

10. The navigation system of claim 9 , wherein the IMU comprises a three-axis accelerometer and a three-axis gyroscope.

11. The navigation system of claim 9 , wherein the RI data comprises a point cloud.

12. The navigation system of claim 9 , wherein the RI sensor comprises a lidar, a radar, or a 3D camera unit.

13. A method of navigation comprising:

measuring, using an inertial measurement unit (IMU) attached to a dynamic platform, a specific force vector and an angular rate vector of the dynamic platform;

determining a current inertial navigation system (INS) solution of a position and an orientation of the dynamic platform based on at least: (i) a previous INS solution, and (ii) the specific force vector and the angular rate vector measured by the IMU;

acquiring, using a range image sensor (RI sensor) attached to the dynamic platform, range image data (RI data);

performing an a priori transformation of the RI data using the current INS solution to obtain transformed RI data;

retrieving, from an RI map database, a valid keyframe map among a plurality of keyframe maps based on the transformed RI data, the valid keyframe map having a known position and a known orientation in a navigation coordinate frame;

constructing a map registration cost gradient (MRCG) measurement based on at least (i) the transformed RI data, and (ii) the known position and the known orientation of the valid keyframe map;

determining INS error corrections and INS solution statistics based on the MRCG measurement;

updating the current INS solution based on the INS error corrections; and

determining an absolute INS solution of a current position and a current orientation of the dynamic platform based on the current INS solution and the INS solution statistics.

14. The method of claim 13 , further comprising:

upon determining that the valid keyframe map is not available in the RI map database, generating the valid keyframe map based on the transformed RI data; and

storing the valid keyframe map in the RI map database.

15. The method of claim 13 , wherein the IMU comprises a three-axis accelerometer and a three-axis gyroscope.

16. The method of claim 13 , wherein the RI data comprises a point cloud.

17. The method of claim 13 , wherein the RI sensor comprises a lidar, a radar, or a 3D camera unit.

18. The method of claim 13 , further comprising:

acquiring additional aiding data using one or more aiding data sources; and

determining AINS estimator data based on the aiding data and the current INS solution;

wherein determining the INS error corrections and the INS solution statistics is further based on the AINS estimator data.

19. The method of claim 18 , wherein the one or more aiding data sources comprise one or more of global navigation satellite systems (GNSS) receiver(s), a wheel odometer, a zero velocity detector, a radar, a sonar, a magnetic compass, and a magnetometer.

20. The method of claim 13 , wherein:

measuring the specific force vector and the angular rate vector of the dynamic platform is performed at a first rate; and

determining the INS error corrections and INS solution statistics is performed at a second rate slower than the first rate.