# US 2024/0003687 A1 · Range image aided ins

> Agents: this data is available as JSON from the patentgrep API and MCP server — see https://patentgrep.com/llms.txt

- Number: US 2024/0003687 A1
- Document ID: US20240003687A1
- Status: Published application
- Kind: A1
- Application number: 18/369,613
- Filed: 2023-09-18
- Published: 2024-01-04
- Earliest claimed: 2021-02-18
- Assignees: Trimble Inc.
- Applicants: Trimble Inc.
- Inventors: Bruno M. Scherzinger
- Web page: https://patentgrep.com/patent/US20240003687A1

## CPC

- G01C 21/165: {combined with non-inertial navigation instruments}
- G01C 21/1652: {with ranging devices, e.g. LIDAR or RADAR}
- G01C 19/02: Rotary gyroscopes
- G01C 21/1656: {with passive imaging devices, e.g. cameras}
- G01C: MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY (measuring liquid level G01F; radio navigation, determining distance or velocity by use of propagation effects, e.g. Doppler effects, propagation time, of radio waves, analogous arrangements using other waves G01S)

## Abstract

A navigation system includes an IMU, a navigation estimator configured to estimate a current navigation solution based on (i) a last 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 a priori transformations of last RI data and current RI data to obtain transformed last RI data and transformed current RI data based on the last navigation solution and the current navigation solution, respectively, an RI filter manager (RFM) configured to construct a delta pose registration cost gradient (DPRCG) measurement based on (i) the transformed last RI data, (ii) the transformed current RI data, (iii) the current navigation solution, and (iv) the last navigation solution. The navigation estimator is further configured to determine an absolute navigation solution based on at least (i) the current navigation solution, and (ii) the DPRCG measurement.

## Claims

### Claim at position 1

-20. (canceled)

### Claim 21

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;
estimating a current navigation solution of a position and an orientation of the dynamic platform at a current time based on at least: (i) a last navigation solution at a prior time, and (ii) the specific force vector and the angular rate vector measured by the IMU;
acquiring, using a range image (RI) sensor attached to the dynamic platform, current RI data at the current time and last RI data at the prior time;
performing an a priori transformation of the last RI data to obtain transformed last RI data based on the last navigation solution;
performing an a priori transformation of the current RI data to obtain transformed current RI data based on the current navigation solution;
constructing a delta pose registration cost gradient (DPRCG) measurement based on (i) the transformed last RI data, (ii) the transformed current RI data, (iii) the current navigation solution, and (iv) the last navigation solution; 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 DPRCG measurement.

### Claim 22 (depends on 21)

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

### Claim 23 (depends on 21)

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

### Claim 24 (depends on 21)

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

### Claim 25 (depends on 21)

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

### Claim 26 (depends on 21)

The method of claim 21, wherein the current RI data represents a current 3D range image and the last RI data represents a last 3D range image.

### Claim 27 (depends on 21)

The method of claim 21, wherein the last navigation solution is obtained using an inertial measurement system (INS).

### Claim 28

A method of navigation comprising:
measuring, using an inertial measurement unit (IMU) coupled 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 (RI) sensor coupled to the dynamic platform, RI data;
performing an a priori transformation of the RI data using the current navigation solution to obtain transformed RI data; and
retrieving, from an RI map database, a valid keyframe map among a plurality of keyframe maps based on the transformed RI data.

### Claim 29 (depends on 28)

The method of claim 28, wherein the valid keyframe map has a known position and a known orientation in a navigation coordinate frame, and the method further comprises:
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.

### Claim 30 (depends on 28)

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

### Claim 31 (depends on 28)

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

### Claim 32 (depends on 28)

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

### Claim 33 (depends on 28)

The method of claim 28, wherein the RI data represents a 3D range image.

### Claim 34 (depends on 28)

The method of claim 28, wherein the previous navigation solution is obtained using an inertial measurement system (INS).

### Claim 35

A navigation system comprising:
an inertial measurement unit (IMU) couples to a dynamic platform and configured to measure a specific force vector and an angular rate vector of the dynamic platform;
a navigation estimator 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 (RI) sensor coupled to the dynamic platform and configured to acquire RI data;
an RI data preprocessor coupled to the RI sensor and the navigation estimator, the RI data preprocessor configured to perform an apriori 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.

### Claim 36 (depends on 35)

The navigation system of claim 35, wherein the valid keyframe map has a known position and a known orientation in a navigation coordinate frame, and the navigation system further comprises:
an RI filter manager (RFM) coupled to the RI data preprocessor and the RI map database, the RFM 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 navigation estimator 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.

### Claim 37 (depends on 35)

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

### Claim 38 (depends on 35)

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

### Claim 39 (depends on 35)

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

### Claim 40 (depends on 35)

The navigation system of claim 35, wherein the dynamic platform is a vehicle.
