Slam assisted INS

Patent metadata

Field	Value
Publication number	US-11243081-B2
Application number	US-201916369720-A
Country	US
Kind code	B2
Filing date	Mar 29, 2019
Priority date	Mar 29, 2019
Publication date	Feb 8, 2022
Grant date	Feb 8, 2022

Abstract

A navigation system for a dynamic platform includes an inertial navigation system (INS) unit for measuring, in real-time, linear accelerations and angular velocities of the dynamic platform, and determining, using dead reckoning, initial estimates of current poses of the dynamic platform based on a previous pose of the dynamic platform and the linear accelerations and angular velocities of the dynamic platform. The navigation system further includes an exteroceptive sensor for acquiring sequential images of an environment in which the dynamic platform is traveling, a simultaneous localization and mapping (SLAM) unit for estimating visual odometer (VO) pose changes of the dynamic platform using the sequential images, and a sensor fusion engine for determining estimates of current poses of the dynamic platform based at least in part on the initial estimates of current poses determined by the INS unit and the VO pose changes estimated by the local sub-map tracker.

First claim

What is claimed is:

1. A navigation system for a dynamic platform, the navigation system comprising:
an inertial navigation system (INS) unit comprising:
a three-axis accelerometer attached to the dynamic platform for measuring, in real-time, linear accelerations of the dynamic platform along three orthogonal spatial axes;
a three-axis gyroscope attached to the dynamic platform for measuring, in real-time, angular velocities of the dynamic platform along three orthogonal angular axes; and
a processor communicatively coupled to the accelerometer and the gyroscope, the processor configured to determine in real-time, using dead reckoning, initial estimates of current positions and initial estimates of current orientations of the dynamic platform based on a previous position and a previous orientation of the dynamic platform, and the linear accelerations and angular velocities of the dynamic platform;

an exteroceptive sensor attached to the dynamic platform and configured to acquire, in real-time, sequential images of an environment in which the dynamic platform is traveling; and
a data processing unit comprising:
a simultaneous localization and mapping (SLAM) unit coupled to the exteroceptive sensor, the SLAM unit comprising:
a local sub-map tracker configured to: (i) estimate, in real-time, visual odometer (VO) pose changes of the dynamic platform using the sequential images, each VO pose change including a change of position and a change of orientation of the dynamic platform, and (ii) determine a plurality of estimated nodes in a pose graph using the sequential images, each estimated node representing an estimated pose of the dynamic platform corresponding to a respective image of the sequential images; and
a loop closure and localization module configured to: (i) optimize the pose graph to obtain a plurality of optimized nodes in the pose graph, each optimized node representing an optimized pose of the dynamic platform, and (ii) estimate absolute pose changes of the dynamic platform based on the plurality of optimized nodes; and

a sensor fusion engine communicatively coupled to the INS unit and the SLAM unit, the sensor fusion engine configured to determine estimates of current positions and estimates of current orientations of the dynamic platform based at least in part on the initial estimates of current positions and the initial estimates of current orientations determined by the INS unit, the VO pose changes estimated by the local sub-map tracker, and the absolute pose changes estimated by the loop closure and localization module;

wherein the processor of the INS unit is further configured to update the initial estimates of current positions and the initial estimates of current orientations of the dynamic platform to obtain absolute current positions and absolute current orientations of the dynamic platform using the estimates of current positions and the estimates of current orientations of the dynamic platform determined by the sensor fusion engine; and
wherein the absolute current positions and absolute current orientations of the dynamic platform are used for navigating the dynamic platform.

2. The navigation system of claim 1 wherein the sensor fusion engine is further configured to estimate errors of the estimates of current positions and errors of the estimates of current orientations.

3. The navigation system of claim 1 wherein the exteroceptive sensor comprises one or more lidars, or one or more cameras.

4. The navigation system of claim 1 wherein:
the processor of the INS unit is configured to determine the initial estimates of current positions and the initial estimates of current orientations of the dynamic platform at a first rate;
the local sub-map tracker of the SLAM unit is configured to estimate the VO pose changes of the dynamic platform at a second rate slower than the first rate;
the sensor fusion engine is configured to determine estimates of current positions and estimates of current orientations of the dynamic platform at a third rate equal to slower than the second rate; and
the processor of the INS unit is configured to update the initial estimates of current positions and the initial estimates of current orientations of the dynamic platform at the third rate.

5. The navigation system of claim 1 wherein determining the estimates of current positions and the estimates of current orientations of the dynamic platform by the sensor fusion engine is further based on estimated errors of the initial estimates of current positions and estimated errors of the initial estimates of current orientations as determined by the INS unit, estimated errors of the VO pose changes as estimated by the local sub-map tracker, and estimated errors of the absolute pose changes as estimated by the loop closure and localization module.

6. The navigation system of claim 1 wherein the loop closure and localization module optimizes the pose graph using a loop closure algorithm.

7. The navigation system of claim 1 wherein the loop closure and localization module is further configured to build, in real-time, a map of the environment in which the dynamic platform is traveling based on the sequential images and the optimized nodes.

8. The navigation system of claim 1 wherein:
the SLAM unit has previously built a map of the environment in which the dynamic platform is traveling; and
the loop closure and localization module is further configured to determining an absolute location of the dynamic platform within the map.

9. The navigation system of claim 1 wherein the local sub-map tracker is configured to estimate the VO pose changes by performing, in real-time, image matching from one image to a next image in the sequential images.

10. The navigation system of claim 9 wherein:
the exteroceptive sensor comprises a lidar sensor;
the sequential images comprise sequential point clouds acquired by the lidar sensor; and
the local sub-map tracker performs the image matching by matching the sequential point clouds using an iterative closest point (ICP) algorithm.

11. A method of navigation for a dynamic platform, the method comprising:
measuring in real-time, using a three-axis accelerometer attached to the dynamic platform, linear accelerations of the dynamic platform along three orthogonal spatial axis;
measuring in real-time, using a three-axis gyroscope attached to the dynamic platform, angular velocities of the dynamic platform along three orthogonal angular axis;
determining in real-time initial estimates of current positions and initial estimates of current orientations of the dynamic platform based on a previous position and a previous orientation, and the linear accelerations and the angular velocities of the dynamic platform using dead reckoning;
acquiring in real-time, using an exteroceptive sensor attached to the dynamic platform, sequential images of an environment in which the dynamic platform is traveling;
estimating in real-time visual odometer (VO) pose changes of the dynamic platform based on the sequential images using a SLAM algorithm, each VO pose change including a change of position and a change of orientation of the dynamic platform;
determining a plurality of estimated nodes in a pose graph based on the sequential images using the SLAM algorithm, each estimated node representing an estimated pose of the dynamic platform corresponding to a respective image of the sequential images;
optimizing the pose graph using the SLAM algorithm to obtain a plurality of optimized nodes in the pose graph, each optimized node representing an optimized pose of the dynamic platform;
estimating absolute pose changes of the dynamic platform based at least on the plur

Assignees

• Trimble Inc

Inventors

• Lambert Andrew
• Leung Keith

Classifications

• G01C21/3848 Primary

  Data obtained from both position sensors and additional sensors · CPC title

• G01C21/165 Primary

  combined with non-inertial navigation instruments · CPC title

• G01C21/18

  Stabilised platforms, e.g. by gyroscope · CPC title

• G05D1/0246

  using a video camera in combination with image processing means · CPC title

• G01C21/206

  specially adapted for indoor navigation · CPC title