US 11,874,116 B2
Range image aided inertial navigation system (INS) with map based localization
Bruno M. Scherzinger, Ontario (CA)
Assigned to Trimble Inc., Sunnyvale, CA (US)
Filed by Trimble Inc., Sunnyvale, CA (US)
Filed on Feb. 18, 2021, as Appl. No. 17/178,624.
Prior Publication US 2022/0268584 A1, Aug. 25, 2022
Int. Cl. G01C 21/16 (2006.01); G01C 21/20 (2006.01)
CPC G01C 21/1652 (2020.08) [G01C 21/1656 (2020.08); G01C 21/20 (2013.01)] 20 Claims
OG exemplary drawing
 
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.