IP Library › Granted Patent US 12,203,756
Granted Patent B2
US 12,203,756 · App. 18/530,506 · Granted Jan 21, 2025

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

Inventor: Bruno M. Scherzinger (Ontario, CA)
Assignee: Trimble Inc.
G01C21/1652G01C21/1656G01C21/20
View Patent ↗
Loading inventors, assignments & file history…
Monitor This Case
Get email alerts when status or documents change.
Order Certified Copies
Most orders are placed with the USPTO same day — all within 24 business hours.
Order via The Patent Place →
Pre-filled with this patent's details
Quick Facts
Patent No.
US 12,203,756
App. No.
18/530,506
Granted
Jan 21, 2025
Kind
B2
Abstract

A navigation system includes an IMU, a navigation estimator configured to estimate a current navigation solution based on (i) a previous navigation solution, and (ii) a specific force vector and an angular rate vector measured by the IMU, an RI sensor, an RI data preprocessor configured to perform an a priori transformation of RI data acquired by the RI sensor using the current navigation solution to obtain transformed RI data, an RI map database configured to retrieve a valid keyframe map based on the transformed RI data, and an RI filter manager (RFM) configured to construct a map registration cost gradient (MRCG) measurement based on (i) the transformed RI data, and (ii) the known position and the known orientation of the valid keyframe map. The navigation estimator is further configured to determine an absolute navigation solution based on at least (i) the current navigation solution, and (ii) the MRCG measurement.

Claims (25)

1. 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 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;

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 navigation 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; and

determining 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.

2. The method of claim 1 , 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.

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

4. The method of claim 1 , wherein the RI data comprises a point cloud.

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

6. The method of claim 1 , wherein the dynamic platform is a vehicle.

7. The method of claim 1 , wherein the specific force includes linear acceleration, gravitational force, and Coriolis force.

8. The method of claim 1 , wherein the specific force is measured along three orthogonal axes.

9. The method of claim 1 , wherein the angular rate is measured along three rotational axes.

10. The method of claim 1 , wherein the RI data includes a set of point vectors in a 3D range image coordinate frame.

11. The method of claim 1 , wherein the keyframe map is a range image map (RI map) whose members are resolved in a Cartesian coordinate frame associated with a range image.

12. The method of claim 1 , wherein the RI map database includes a set of keyframe maps acquired at positions of the RI sensor.

13. The method of claim 1 , wherein the MRCG casts a registration error cost gradient as a linear combination of inertial navigation system (INS) position errors, INS orientation errors, and the RI sensor installation parameter errors.

14. The method of claim 1 , wherein the absolute navigation solution of the current position and the current orientation of the dynamic platform are determined based on only (i) the current navigation solution, and (ii) the MRCG measurement.

15. The method of claim 1 , wherein the transformed RI data is aligned with keyframe data.

16. The method of claim 1 , wherein the a priori transformation includes a translation and a rotation.

Assignments (1)
ASSIGNMENT OF ASSIGNOR'S INTEREST Recorded Sep 23, 2024
From: SCHERZINGER, BRUNO M.
To: TRIMBLE INC.
Reel/Frame 068668/0908 →
Continuity (2)
Continuation 17178624 · Feb 18, 2021
Related Publication 20240151537A1 · May 9, 2024
References Cited (37)
US 4445376A · Merhav · 1984 [cited by examiner]
US 8676498B2 · Ma et al. · 2014 [cited by applicant]
US 11243081B2 · Lambert · 2022 [cited by examiner]
US 11815356B2 · Scherzinger · 2023 [cited by applicant]
US 11874116B2 · Scherzinger · 2024 [cited by applicant]
US 20150219767A1 · Humphreys et al. · 2015 [cited by applicant]
US 20180112985A1 · Madison · 2018 [cited by applicant]
US 20200309529A1 · Lambert et al. · 2020 [cited by applicant]
US 20220276053A1 · Scherzinger · 2022 [cited by examiner]
US 20230211840A1 · Lambert · 2023 [cited by examiner]
CN 113008229A · 2021 [cited by examiner]
EP 3715785A1 · 2020 [cited by applicant]
EP 4047310A1 · 2022 [cited by examiner]
EP 4047311A1 · 2022 [cited by examiner]
GB 2146124A · 1985 [cited by examiner]
JP 2012119901A · 2012 [cited by applicant]
U.S. Appl. No. 17/178,620 Notice of Allowance mailed Jul. 3, 2023, 10 pages. [cited by applicant]
The extended European Search Report for Application No. 22157398.3-1009, issued Jul. 11, 2022, 11 pages. [cited by applicant]
Schmid, K. et al., “Stereo Vision and IMU based Real-Time Ego-Motion and Depth Image Computation on a Handheld Device,” 2013 IEEE International Conference on Robotics and Automation (ICRA), May 6-10, 2013, Karlsruhe, Ge… [cited by applicant]
Benson, Jr., D., “A Comparison of Two Approaches to Pure-Inertial and Doppler-Inertial Error Analysis,” IEEE Transactions on Aerospace and Electronic Systems, vol. AES-11, No. 4, Jul. 1975, pp. 447-455. [cited by applicant]
Chen, Y. et al., “Object Modeling by Registration of Multiple Range Images,” Proceedings of the 1991 IEEE, International Conference on Robotics and Automation, Sacramento, California, Apr. 1991, pp. 2724-2729. [cited by applicant]
Gao, Y. et al., “INS/GPS/LIDAR Integrated Navigation System for Urban and Indoor Environments Using Hybrid Scan Matching Algorithm,” Sensors, 15, Published Sep. 15, 2015, pp. 23286-23302. [cited by applicant]
Scherzinger, B., “Inertial Navigator Error Models for Large Heading Uncertainty,” IEEE, 1996, pp. 477-484. [cited by applicant]
Scherzinger, B. et al., “Modified Strapdown Inertial Navigator Error Models,” IEEE, 1994, pp. 426-430. [cited by applicant]
Shan, T. et al., “LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping,” IEEE/RSJ International Conference on Intelligent Robots and Systems, Jul. 7, 2020, 9 pages. [cited by applicant]
Roumeliotis, S. I. et al., “Stochastic Cloning: A generalized framework for processing relative state measurements,” Proceedings of the 2002 IEEE International Conference on Robotics & Automation, Washington, DC, May 20… [cited by applicant]
Mohamed, S. A. S. et al., “A Survey on Odometry for Autonomous Navigation Systems,” IEEE Access, vol. 7, 2019, pp. 97466-97486. [cited by applicant]
Liu, S. et al., “LiDAR-aided Integrated INS/GPS Navigation System for Unmanned Ground Vehicles in Urban and Indoor Environments Using Hybrid Adaptive Scan Matching Algorithm,” Proceedings of the 28 [cited by applicant]
Extended European Search Report for Application No. 22157394.2-1009, mailed Jul. 5, 2022, 12 pages. [cited by applicant]
U.S. Appl. No. 17/178,624 Non-Final Office Action mailed Jul. 20, 2023, 10 pages. [cited by applicant]
U.S. Appl. No. 17/178,624 Notice of Allowance mailed Nov. 9, 2023, 5 pages. [cited by applicant]
U.S. Appl. No. 18/369,613 Non-Final Office Action mailed May 29, 2024, 11 pages. [cited by applicant]
U.S. Appl. No. 18/369,613 Notice of Allowance mailed Jul. 15, 2024, 10 pages. [cited by applicant]
Office Action for European Application No. 22 157 398.3-1009, mailed Aug. 12, 2024, 8 pages. [cited by applicant]
Office Action for European Application No. 22 157 394.2-1009, mailed Aug. 22, 2024, 7 pages. [cited by applicant]
Elhousni, M. et al., “A Survey on 3D Lidar Localization for Autonomous Vehicles,” 2020 IEEE Intelligent Vehicles Symposium (IV), Oct. 19, 2020, pp. 1879-1884. [cited by applicant]
Zhang, J. et al., “Laser-visual-inertial odometry and mapping with high robustness and low drift,” Journal of Field Robotics, vol. 35, No. 8, Aug. 23, 2018, pp. 1242-1264. [cited by applicant]