Sensor fusion for dynamic mapping

1 . A method for producing a high definition map (HD map) for control of one or more autonomous vehicles, the one or more autonomous vehicles including a first vehicle, the method comprising:

receiving, at a local computing device, first lidar data and first radar data related to the first vehicle;

estimating, based on the first radar data, an initial range to the first vehicle;

performing a local sensor fusion at the local computing device, wherein the local sensor fusion includes removing, based on a clustering radius associated with the first vehicle, a ground plane associated with the first vehicle, and

determining, based on the local sensor fusion, the HD map, wherein the HD map indicates a position and a velocity of the first vehicle, a result of lidar detection of the first vehicle, and a result of radar detection of the first vehicle, wherein

the clustering radius is set longer as a distance from a first road side unit to the first vehicle increases by comparing the initial range with one or more range thresholds and setting the clustering radius based on the comparing.

2 . The method of claim 1 , further comprising controlling, based on the position and the velocity indicated by the HD map, the first vehicle to navigate in a parking lot to pick up or drop off a person.

3 . The method of claim 1 , further comprising controlling, based on the position and the velocity indicated by the HD map, the first vehicle to navigate in a shipping yard to pick up or drop off a shipment.

4 . The method of claim 1 , further comprising:

receiving, at the local computing device, second lidar data and second radar data related to the first vehicle,

wherein the performing the local sensor fusion further comprises:

applying a delay filter to compare timestamps of the first lidar data and the second lidar data,

discarding the second lidar data when the comparison of the timestamps indicates a difference in timestamps more than a threshold time, and

using, when the difference in timestamps is equal to or less than the threshold time, the second lidar data as an input of the local sensor fusion.

5 . The method of claim 1 , further comprising:

forming visual display data based on the HD map;

sending the visual display data to a display device; and

displaying the visual display data on the display device for monitoring, by a person, of the first vehicle.

6 . The method of claim 1 , further comprising:

determining a first control command for the first vehicle;

sending the first control command to a vehicle controller of the first vehicle at a first time;

adjusting, by the vehicle controller based on the first control command, a position or a velocity of the first vehicle;

determining a second control command for the first vehicle; and

sending the second control command to the vehicle controller of the first vehicle at a second time, wherein the first time and the second time correspond to a command rate of between 5 Hz and 20 Hz.

7 . The method of claim 1 , wherein the performing the local sensor fusion comprises:

estimating a point cloud by removing a ground plane portion from the first lidar data to obtain a second point cloud;

identifying an initial estimated position and initial estimated velocity, in first local coordinates, of the first vehicle by a clustering of the point cloud based on the clustering radius.

8 . The method of claim 7 , wherein the performing the local sensor fusion further comprises:

determining an updated position and updated estimated velocity, in the first local coordinates, by applying a first local Kalman filter to the initial estimated position, the initial estimated velocity and the initial range; and

converting the updated position and updated estimated velocity to global coordinates to obtain local data.

9 . The method of claim 8 , wherein the one or more range thresholds comprises a first range threshold, a second range threshold and a third range threshold, and wherein the performing the local sensor fusion further comprises:

estimating the point cloud by removing the ground plane portion by:

when the initial range is less than a first range threshold, estimating the ground plane portion based on:

the clustering radius, wherein the clustering radius is a first clustering radius, and

a first bounding volume,

when the initial range is not less than the first range threshold and is less than a second range threshold, estimating the ground plane portion based on:

the clustering radius, wherein the clustering radius is a second clustering radius, and

a second bounding volume,

when the initial range is not less than the second range threshold and is less than a third threshold, estimating the ground plane portion based on:

the clustering radius, wherein the clustering radius is a third clustering radius, and

a third bounding volume, and

when the initial range is greater than the third threshold, estimating the ground plane portion as non-existent; and

obtaining the local data, at least in part, by removing the ground plane portion from the point cloud.

10 . The method of claim 9 , wherein the first range threshold is 50 meters, dimensions of the first bounding volume in x-coordinates, y-coordinates and z-coordinates are 8, 50, and 6 meters, respectively, and the first clustering radius is 1.0 meter.

11 . The method of claim 9 , wherein the second range threshold is 120 meters, dimensions of the second bounding volume in x-coordinates, y-coordinates and z-coordinates are 8, 70, and 6 and meters, respectively, and the second clustering radius is 1.5 meters.

12 . The method of claim 9 , wherein a third range threshold is 200 meters, dimensions of the third bounding volume in x-coordinates, y-coordinates and z-coordinates are 8, 80, and 6 meters, respectively, and the third clustering radius is 2.0 meters.

13 . The method of claim 1 , wherein the receiving, at the local computing device, first lidar data and first radar data, comprises receiving the first lidar data and the first radar data from a first road side unit (RSU).

14 . The method of claim 13 , further comprising:

sending local data from the local computing device to a central computing device;

receiving, at a second local computing device from a second RSU, second lidar data and second radar data related to the first vehicle;

performing a second local sensor fusion at the second local computing device;

sending second local data based on global coordinates from the second local computing device to the central computing device;

receiving at the central computing device the local data and the second local data; and

determining, based on a global fusion at the central computing device of the local data and the second local data, the HD map, wherein the HD map indicates a position and velocity, in global coordinates, of the first vehicle.

15 . The method of claim 14 , wherein the local computing device, the second local computing device and the central computing device are geographically distributed.

16 . The method of claim 14 , wherein the local computing device, the second local computing device and the central computing device are geographically co-located.

17 . The method of claim 1 , further comprising:

identifying, by the local computing device, an estimated position and estimated velocity, in first local coordinates, of the first vehicle by a clustering of a point cloud based on a clustering radius;

converting the estimated position and the estimated velocity to global coordinates; and

sending the position in global coordinates and the velocity in global coordinates to a central computing device,

wherein the central computing device is located within the first vehicle.

18 . The method of claim 17 , further comprising transmitting the HD map from the first vehicle to a second vehicle, wherein the one or more autonomous vehicle comprises the second vehicle.

19 . A local computing device comprising:

a communication interface; and

at least one processor configured to:

receive, via the communication interface, first lidar data and first radar data related to a first vehicle of one or more autonomous vehicles;

estimate, based on the first radar data, an initial range to the first vehicle;

perform a local sensor fusion at the local computing device, wherein the local sensor fusion includes removing a ground plane associated with the first vehicle based on a clustering radius associated with the first vehicle, and

determine, based on the local sensor fusion, a high definition (HD) map, wherein the HD map indicates a position and velocity of the first vehicle, a result of lidar detection of the first vehicle, and a result of radar detection of the first vehicle, wherein

the clustering radius is set longer as a distance from a first road side unit to the first vehicle increases by comparing the initial range with one or more range thresholds and setting the clustering radius based on the comparing.

20 . A non-transitory computer readable medium comprising instructions for execution by a processor in a local computing device for causing the local computing device to:

receive, via a communication interface, first lidar data and first radar data related to a first vehicle of one or more autonomous vehicles;

estimate, based on the first radar data, an initial range to the first vehicle;

perform a local sensor fusion at the local computing device, wherein the local sensor fusion includes removing a ground plane associated with the first vehicle based on a clustering radius associated with the first vehicle, and

determine, based on the local sensor fusion, a high definition (HD) map, wherein the HD map indicates a position and velocity of the first vehicle, a result of lidar detection of the first vehicle, and a result of radar detection of the first vehicle, wherein

the clustering radius is set longer as a distance from a first road side unit to the first vehicle increases by comparing the initial range with one or more range thresholds and setting the clustering radius based on the comparing.