Method and apparatus of parallel tracking and localization via multi-mode slam fusion process

Patent metadata

Field	Value
Publication number	US-10962371-B2
Application number	US-201916372788-A
Country	US
Kind code	B2
Filing date	Apr 2, 2019
Priority date	Apr 2, 2019
Publication date	Mar 30, 2021
Grant date	Mar 30, 2021

Abstract

A method for vehicle tracking and localization includes receiving, by a controller, odometry data from a sensor of the first vehicle; geospatial data from a Global Positioning System (GPS) device of the first vehicle; inertial data from an inertial measurement unit (IMU) of the first vehicle; estimating an estimated-current location of the first vehicle and an estimated-current trajectory of the first vehicle using the odometry data from the sensor, the geospatial data from the GPS device, and the inertial data from the IMU of the first vehicle; inputting the inertial data into a Bayesian Network to determine a predicted location of the first vehicle and a predicted trajectory of the first vehicle, and updating the Bayesian Network using the estimated-current location and the estimated-current trajectory of the first vehicle using the odometry data and the geospatial data.

First claim

What is claimed is:

1. A method for vehicle tracking and localization, comprising:
receiving, by a first controller of a first vehicle, odometry data from a sensor of the first vehicle;
receiving, by the first controller of the first vehicle, geospatial data from a Global Positioning System (GPS) device of the first vehicle;
receiving, by the first controller of the first vehicle, inertial data from an inertial measurement unit (IMU) of the first vehicle;
estimating, by the first controller of the first vehicle, an estimated-current location of the first vehicle and an estimated-current trajectory of the first vehicle using the odometry data from the sensor, the geospatial data from the GPS device, and the inertial data from the IMU of the first vehicle;
inputting the inertial data into a Bayesian Network to determine a predicted location of the first vehicle and a predicted trajectory of the first vehicle, wherein the Bayesian Network runs on the first controller of the first vehicle; and
updating the Bayesian Network using the estimated-current location and the estimated-current trajectory of the first vehicle using the odometry data and the geospatial data;
detecting, by the first controller of the first vehicle, a second vehicle using the odometry data of the sensor of the first vehicle;
estimating, by the first controller of the first vehicle, an estimated-current location of a second vehicle and an estimated-current trajectory of the second vehicle using the odometry data from the sensor, the geospatial data from the GPS device, and the inertial data from the IMU of the first vehicle; and
transmitting the estimated-current location of the first vehicle, the estimated-current trajectory of the first vehicle, the estimated-current location of the second vehicle, and the estimated-current trajectory of the second vehicle to the second vehicle.

2. The method of claim 1 , further comprising extracting, by the first controller of the first vehicle, feature data from the odometry data, wherein the feature data is data about objects and an arrangement of the objects detected by the sensor, and the feature data includes a feature map that shows the arrangement of the objects sensed by the sensor.

3. The method of claim 2 , further comprising determining, by the first controller of the first vehicle, feature mappoints using the geospatial data received by from the GPS device, wherein the feature mappoints are geospatial locations of features.

4. The method of claim 3 , further comprising matching the feature map with the feature mappoints.

5. The method of claim 4 , wherein estimating, by the first controller of the first vehicle, the estimated-current location of the first vehicle and the estimated-current trajectory of the first vehicle includes using a pose graph optimization process.

6. The method of claim 4 , wherein estimating, by the first controller of the first vehicle, the estimated-current location of the first vehicle and the estimated-current trajectory of the first vehicle includes using an extended Kalman filter.

7. The method of claim 6 , estimating, by the first controller of the first vehicle, the estimated-current location of the first vehicle and the estimated-current trajectory of the first vehicle includes using an extended Kalman filter and fusing the odometry data, the geospatial data, and the inertial data to determine the estimated-current location of the first vehicle and the estimated-current trajectory of the first vehicle.

8. The method of claim 7 ,
wherein the GPS device of the first vehicle is a first GPS device;
the geospatial data is a first geospatial data;
the second vehicle includes a second GPS device and a second controller in communication with the second GPS device; and
the method further includes receiving, by the second controller of the second vehicle, the estimated-current location of the first vehicle, the estimated-current trajectory of the first vehicle, the estimated-current location of the second vehicle, and the estimated-current trajectory of the second vehicle to the second vehicle from the first controller of the first vehicle.

9. The method of claim 8 , further comprising:
receiving, by the second controller of the second vehicle, second geospatial data from the second GPS device of the second vehicle; and
determining, by the second controller of the second vehicle, an updated-estimated-current location of the second vehicle using the second geospatial data from the second GPS device and the estimated-current location of the second vehicle transmitted by the first vehicle.

10. The method of claim 9 , further comprising determining, by the first controller of the first vehicle, semantic mappoints data from the odometry data.

11. The method of claim 1 , wherein the sensor is a camera configured to capture an image.

12. The method of claim 1 , wherein the sensor is a lidar sensor.

13. A system for tracking and localization of a first vehicle, comprising:
a camera configured to capture images, wherein the camera is configured to generate odometry data, the odometry data is indicative of a position and an orientation of the first vehicle, and the camera is configured to detect a second vehicle;
a communication system configured to wirelessly communicate information between the first vehicle and a second vehicle;
a Global Positioning System (GPS) device configured to determine a GPS-location of the first vehicle, and the GPS device is configured to generate geospatial data, and the geospatial data is indicative of the GPS-location of the first vehicle;
an inertial measurement unit (IMU) configured to measure an angular and linear motion of the first vehicle, wherein the IMU is configured to generate inertial data, and the inertial data is indicative of the angular and linear motion of the first vehicle;
a first controller is in communication with the communication system and the camera, wherein the first controller is programmed to:
receive the odometry data from the camera of the first vehicle;
receive the geospatial data from GPS device of the first vehicle;
receive the inertial data from the inertial measurement unit (IMU) of the first vehicle;
estimate an estimated-current location of the first vehicle and an estimated-current trajectory of the first vehicle using the odometry data from the camera, the geospatial data from the GPS device, and the inertial data from the IMU of the first vehicle;
input the inertial data into a Bayesian Network to determine a predicted location of the first vehicle and a predicted trajectory of the first vehicle, wherein the Bayesian Network runs on the first controller of the first vehicle; and
update the Bayesian Network using the estimated-current location and the estimated-current trajectory of the first vehicle using the odometry data and the geospatial data;
detect the second vehicle using the odometry data of the camera of the first vehicle;
estimate an estimated-current location of a second vehicle and an estimated-current trajectory of the second vehicle using the odometry data from the camera, the geospatial data from the GPS device, and the inertial data from the IMU of the first vehicle;
command the communication system to transmit the estimated-current location of the first vehicle, the estimated-current trajectory of the first vehicle, the estimated-current location of the second vehicle, and the estimated-current trajectory of the second vehicle to the second vehicle.

14. The system of claim 13 , wherein the first controller is programmed to extract feature data from the odometry data, the feature data is data ab

Assignees

• Gm Global Tech Operations Llc

Inventors

• Bush Lawrence A
• Bai Fan
• Ren Pengfei
• Raphael Eric L
• Murad Mohannad
• Naserian Mohammad

Classifications

• G01S19/49

  whereby the further system is an inertial position system, e.g. loosely-coupled · CPC title

• G01C21/32

  Structuring or formatting of map data · CPC title

• G01C21/165 Primary

  combined with non-inertial navigation instruments · CPC title

• G01C21/1656

  with passive imaging devices, e.g. cameras · CPC title

• G01S19/48

  by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system · CPC title