US 11,815,356 B2
Range image aided INS
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,620.
Prior Publication US 2022/0276053 A1, Sep. 1, 2022
Int. Cl. G01C 21/16 (2006.01); G01C 19/02 (2006.01)
CPC G01C 21/1652 (2020.08) [G01C 19/02 (2013.01); G01C 21/1656 (2020.08)] 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, the INS unit configured to:
determine an 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;
compute a current INS solution based on the INS solution, the current INS solution being valid at a current construction time; and
compute a last INS solution based on a prior INS solution, the last INS solution being valid at a last construction time prior to the current construction time;
a range image (RI) sensor attached to the dynamic platform and configured to acquire last RI data at the last construction time and current RI data at the current construction time;
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 last RI data using the last INS solution to obtain transformed last RI data; and
perform an a priori transformation of the current RI data using the current INS solution to obtain transformed current RI data;
an RI filter manager (RFM) coupled to the RI data preprocessor, the RFM configured to construct a delta pose registration cost gradient (DPRCG) measurement for the current construction time based on the transformed last RI data, the transformed current RI data, the last INS solution, and the current INS solution;
an aided INS (AINS) estimator coupled to the RFM and the INS unit, the AINS estimator configured to determine INS error corrections and INS solution statistics based on the DPRCG measurement, wherein the INS unit is configured to update the INS solution based on the INS error corrections; and
an output manager coupled to the INS unit and the AINS estimator, the output manager configured to determine an absolute solution of a current position and a current orientation of the dynamic platform based on the INS solution and the INS solution statistics.