Patent Description:
Autonomous vehicles, also known as self-driving cars, driverless cars, auto, or robotic cars, drive from a source location to a destination location without requiring a human driver to control and navigate the vehicle. Automation of driving is difficult due to several reasons. For example, autonomous vehicles use sensors to make driving decisions on the fly, but vehicle sensors cannot observe everything all the time. Vehicle sensors can be obscured by corners, rolling hills, and other vehicles. Vehicles sensors may not observe certain things early enough to make decisions. In addition, lanes and signs may be missing on the road or knocked over or hidden by bushes, and therefore not detectable by sensors.

Autonomous vehicles can use map data to figure out some of the above information instead of relying on sensor data. However conventional maps have several drawbacks that make them difficult to use for an autonomous vehicle. For example maps do not provide the level of accuracy required for safe navigation (e.g., <NUM> or less). GPS systems provide accuracies of approximately <NUM>-<NUM> meters, but have large error conditions resulting in an accuracy of over <NUM>. This makes it challenging to accurately determine the location of the vehicle.

Techniques for creating maps use iterative closest point (ICP) techniques for processing point clouds. A vehicle generates point clouds using sensor data, for example, lidar scans. Rotational lidars, such as VELODYNE HDL, sweep the environment by shooting lidar beams in a circular fashion. Each laser beam provides a distance measurement between the reflective object and the center of the lidar. As the lidar rotates, it generates a stream of points, which are split into point clouds. For a stationary lidar, each point is relative to a single scan center. However, for autonomous driving, the lidar is mounted on top of a moving vehicle as it rotates, so the center of a scan is dependent on the motion of the vehicle at a particular timestamp. The conventional ICP process estimates the motion by treating each point cloud as having a single center. As a result, the measured point cloud from lidar is not consistent with the real world. In order to know where each center point is, the system need to know the motion along the path.

Conventional techniques first estimate the motion of the vehicle using the raw point clouds and then apply this motion to the points and then repeat this process. This process can be unstable and if iterated a few times the results can diverge resulting in larger and larger errors. As a result, conventional techniques either fail to align point clouds, thereby result in inefficiencies that cause slow convergence of the computations. Alternatively, the conventional techniques result in error in the alignment process that subsequently causes errors in the high-definition maps generated.

Document <CIT> discloses a motion tracking method using scanning device.

Document <NPL>, discloses fault isolation of dead reckoning and laser range sensor (LRS) for mobile robot.

Document <CIT> discloses a method and device for real-time mapping and localization.

Document <CIT> discloses an integrated sensor and computer-based algorithm system which controls and directs autonomously driven vehicles.

Document <CIT> discloses a processing method of a 3D point cloud.

Embodiments of the invention, comprising methods and systems configured to carry out said methods, align point clouds obtained by sensors of an autonomous vehicle using kinematic iterative closest point technique with integrated motions estimates.

A method is provided as mentioned in claim <NUM> and a system as mentioned in claim <NUM>. The system receives from a lidar mounted on an autonomous vehicle, sensor data describing an environment surrounding the vehicle. The sensor data comprises a plurality of points and each point is associated with a time of capture of the point by the lidar sensor. The system determines based on the plurality of points, a source point cloud and a target point cloud.

The system initializes a plurality of velocity parameters describing movement of the autonomous vehicle, the plurality of velocity parameters affecting determined respective positions of the plurality of points.

The system repeats inter alia the following steps for a plurality of iterations. The system determines a plurality of correspondences, each correspondence representing a pairs of points. The pair of points includes a point from the source point cloud and a corresponding point from the target point cloud. The system modifies the velocity parameters in association with movement of the vehicle during capturing of the source point cloud and the target point cloud, the velocity parameters being modified such that an aggregate measure of distance between corresponding points of the plurality of pairs of points is minimized. The system adjusts a timestep representing a time difference between the source point cloud and the target point cloud based on a measure of error in computing the velocity parameters. These steps are repeated, for example, until a measure of error between the two point clouds reaches below a threshold.

The system determines control signals for navigating the autonomous vehicle based on the velocity parameters and, for example, navigating the autonomous vehicle based on the control signals.

For example, the system may use the kinematic ICP process to align point clouds that are used to build high definition maps. The autonomous vehicle uses the high definition maps for purposes of navigation.

In an embodiment, the system adjusts at least one of the source point cloud or the target point cloud based on the velocity parameters. The adjustment comprises transforming the points of the point cloud based on the velocity parameters and the time of capture of the point. For example, the system uses the velocity parameters to determine for a point, an estimate of the distance traveled by the autonomous vehicle in a time interval between the time of capture of the point and a reference time. The reference point may represent the time corresponding to the capture of the last point of the point cloud representing the completion of the capture of the point cloud. The system moves the point of a point cloud by the estimated distance.

In an embodiment not being part of the present invention, the system performs optimization of the aggregate measure of distance between correspondences using a non-linear optimization technique, for example gradient descent. In an embodiment, the system optimizes the velocity parameters by using one or more constraints limiting the velocity parameters to estimates of maximum velocity parameters determined based on physical movement of the autonomous vehicle.

In an embodiment, the system initializes the plurality of velocity parameters based on data obtained from an inertial measurement unit of the autonomous vehicle or based on global navigation satellite system (GNSS).

In an embodiment, the system determines the measure of distance between a pair of corresponding points by determining a normal direction along a surface of the target point cloud at the target point and determining a distance between the source point and the target point along the normal direction.

In an embodiment, the system determines that a source point of the source point cloud corresponds to a target point of the target point cloud if the target point of the target point cloud is a nearest neighbor of the source point and the source point of the source point cloud is determined to be a nearest neighbor of the target point.

Embodiments of the invention maintain high definition (HD) maps containing up to date information using high precision. The HD maps may be used by autonomous vehicles to safely navigate to their destinations without human input or with limited human input. An autonomous vehicle is a vehicle capable of sensing its environment and navigating without human input. Autonomous vehicles may also be referred to herein as "driverless car," "self-driving car," or "robotic car. " An HD map refers to a map storing data with very high precision, typically <NUM>-<NUM>. Embodiments generate HD maps containing spatial geometric information about the roads on which an autonomous vehicle can travel. Accordingly, the generated HD maps include the information necessary for an autonomous vehicle navigating safely without human intervention. Embodiments generate and maintain high definition (HD) maps that are accurate and include the most updated road conditions for safe navigation.

<FIG> shows the overall system environment of an HD map system interacting with multiple vehicles, according to an embodiment. The HD map system <NUM> includes an online HD map system <NUM> that interacts with a plurality of vehicles <NUM>. The vehicles <NUM> may be autonomous vehicles but are not required to be. The online HD map system <NUM> receives sensor data captured by sensors of the vehicles, and combines the data received from the vehicles <NUM> to generate and maintain HD maps. The online HD map system <NUM> sends HD map data to the vehicles for use in driving the vehicles. In an embodiment, the online HD map system <NUM> is implemented as a distributed computing system, for example, a cloud based service that allows clients such as vehicle computing systems <NUM> to make requests for information and services. For example, a vehicle computing system <NUM> may make a request for HD map data for driving along a route and the online HD map system <NUM> provides the requested HD map data.

<FIG> and the other figures use like reference numerals to identify like elements. A letter after a reference numeral, such as "105A," indicates that the text refers specifically to the element having that particular reference numeral. A reference numeral in the text without a following letter, such as "<NUM>," refers to any or all of the elements in the figures bearing that reference numeral (e.g. "<NUM>" in the text refers to reference numerals "105A" and/or "105N" in the figures).

The online HD map system <NUM> comprises a vehicle interface module <NUM> and an HD map store <NUM>. The online HD map system <NUM> interacts with the vehicle computing system <NUM> of various vehicles <NUM> using the vehicle interface module <NUM>. The online HD map system <NUM> stores map information for various geographical regions in the HD map store <NUM>. The online HD map system <NUM> may include other modules than those shown in <FIG>, for example, various other modules as illustrated in <FIG> and further described herein.

The online HD map system <NUM> receives <NUM> data collected by sensors of a plurality of vehicles <NUM>, for example, hundreds or thousands of cars. The vehicles provide sensor data captured while driving along various routes and send it to the online HD map system <NUM>. The online HD map system <NUM> uses the data received from the vehicles <NUM> to create and update HD maps describing the regions in which the vehicles <NUM> are driving. The online HD map system <NUM> builds high definition maps based on the collective information received from the vehicles <NUM> and stores the HD map information in the HD map store <NUM>.

The online HD map system <NUM> sends <NUM> HD maps to individual vehicles <NUM> as required by the vehicles <NUM>. For example, if an autonomous vehicle needs to drive along a route, the vehicle computing system <NUM> of the autonomous vehicle provides information describing the route being travelled to the online HD map system <NUM>. In response, the online HD map system <NUM> provides the required HD maps for driving along the route.

In an embodiment, the online HD map system <NUM> sends portions of the HD map data to the vehicles in a compressed format so that the data transmitted consumes less bandwidth. The online HD map system <NUM> receives from various vehicles, information describing the data that is stored at the local HD map store <NUM> of the vehicle. If the online HD map system <NUM> determines that the vehicle does not have certain portion of the HD map stored locally in the local HD map store <NUM>, the online HD map system <NUM> sends that portion of the HD map to the vehicle. If the online HD map system <NUM> determines that the vehicle did previously receive that particular portion of the HD map but the corresponding data was updated by the online HD map system <NUM> since the vehicle last received the data, the online HD map system <NUM> sends an update for that portion of the HD map stored at the vehicle. This allows the online HD map system <NUM> to minimize the amount of data that is communicated with the vehicle and also to keep the HD map data stored locally in the vehicle updated on a regular basis.

A vehicle <NUM> includes vehicle sensors <NUM>, vehicle controls <NUM>, and a vehicle computing system <NUM>. The vehicle sensors <NUM> allow the vehicle <NUM> to detect the surroundings of the vehicle as well as information describing the current state of the vehicle, for example, information describing the location and motion parameters of the vehicle. The vehicle sensors <NUM> comprise a camera, a light detection and ranging sensor (LIDAR), a global positioning system (GPS) navigation system, an inertial measurement unit (IMU), and others. The vehicle has one or more cameras that capture images of the surroundings of the vehicle. A LIDAR surveys the surroundings of the vehicle by measuring distance to a target by illuminating that target with a laser light pulses, and measuring the reflected pulses. The GPS navigation system determines the position of the vehicle based on signals from satellites. An IMU is an electronic device that measures and reports motion data of the vehicle such as velocity, acceleration, direction of movement, speed, angular rate, and so on using a combination of accelerometers and gyroscopes or other measuring instruments.

The vehicle controls <NUM> control the physical movement of the vehicle, for example, acceleration, direction change, starting, stopping, and so on. The vehicle controls <NUM> include the machinery for controlling the accelerator, brakes, steering wheel, and so on. The vehicle computing system <NUM> continuously provides control signals to the vehicle controls <NUM>, thereby causing an autonomous vehicle to drive along a selected route.

The vehicle computing system <NUM> performs various tasks including processing data collected by the sensors as well as map data received from the online HD map system <NUM>. The vehicle computing system <NUM> also processes data for sending to the online HD map system <NUM>. Details of the vehicle computing system are illustrated in <FIG> and further described in connection with <FIG>.

The interactions between the vehicle computing systems <NUM> and the online HD map system <NUM> are typically performed via a network, for example, via the Internet. The network enables communications between the vehicle computing systems <NUM> and the online HD map system <NUM>. In one embodiment, the network uses standard communications technologies and/or protocols. The data exchanged over the network can be represented using technologies and/or formats including the hypertext markup language (HTML), the extensible markup language (XML), etc. In addition, all or some of links can be encrypted using conventional encryption technologies such as secure sockets layer (SSL), transport layer security (TLS), virtual private networks (VPNs), Internet Protocol security (IPsec), etc. In another embodiment, the entities can use custom and/or dedicated data communications technologies instead of, or in addition to, the ones described above.

<FIG> shows the system architecture of a vehicle computing system, according to an embodiment. The vehicle computing system <NUM> comprises a perception module <NUM>, prediction module <NUM>, planning module <NUM>, a control module <NUM>, a local HD map store <NUM>, an HD map system interface <NUM>, a kinematic ICP module 290a, and an HD map application programming interface (API) <NUM>. The various modules of the vehicle computing system <NUM> process various type of data including sensor data <NUM>, a behavior model <NUM>, routes <NUM>, and physical constraints <NUM>. In other embodiments, the vehicle computing system <NUM> may have more or fewer modules. Functionality described as being implemented by a particular module may be implemented by other modules. Details of the kinematic ICP module 290a are described in detail herein.

The perception module <NUM> receives sensor data <NUM> from the sensors <NUM> of the vehicle <NUM>. This includes data collected by cameras of the car, LIDAR, IMU, GPS navigation system, and so on. The perception module <NUM> uses the sensor data to determine what objects are around the vehicle, the details of the road on which the vehicle is travelling, and so on. The perception module <NUM> processes the sensor data <NUM> to populate data structures storing the sensor data and provides the information to the prediction module <NUM>.

The prediction module <NUM> interprets the data provided by the perception module using behavior models of the objects perceived to determine whether an object is moving or likely to move. For example, the prediction module <NUM> may determine that objects representing road signs are not likely to move, whereas objects identified as vehicles, people, and so on, are either moving or likely to move. The prediction module <NUM> uses the behavior models <NUM> of various types of objects to determine whether they are likely to move. The prediction module <NUM> provides the predictions of various objects to the planning module <NUM> to plan the subsequent actions that the vehicle needs to take next.

The planning module <NUM> receives the information describing the surroundings of the vehicle from the prediction module <NUM>, the route <NUM> that determines the destination of the vehicle, and the path that the vehicle should take to get to the destination. The planning module <NUM> uses the information from the prediction module <NUM> and the route <NUM> to plan a sequence of actions that the vehicle needs to take within a short time interval, for example, within the next few seconds. In an embodiment, the planning module <NUM> specifies the sequence of actions as one or more points representing nearby locations that the vehicle needs to drive through next. The planning module <NUM> provides the details of the plan comprising the sequence of actions to be taken by the vehicle to the control module <NUM>. The plan may determine the subsequent action of the vehicle, for example, whether the vehicle performs a lane change, a turn, acceleration by increasing the speed or slowing down, and so on.

The control module <NUM> determines the control signals for sending to the controls <NUM> of the vehicle based on the plan received from the planning module <NUM>. For example, if the vehicle is currently at point A and the plan specifies that the vehicle should next go to a nearby point B, the control module <NUM> determines the control signals for the controls <NUM> that would cause the vehicle to go from point A to point B in a safe and smooth way, for example, without taking any sharp turns or a zig zag path from point A to point B. The path taken by the vehicle to go from point A to point B may depend on the current speed and direction of the vehicle as well as the location of point B with respect to point A. For example, if the current speed of the vehicle is high, the vehicle may take a wider turn compared to a vehicle driving slowly.

The control module <NUM> also receives physical constraints <NUM> as input. These include the physical capabilities of that specific vehicle. For example, a car having a particular make and model may be able to safely make certain types of vehicle movements such as acceleration, and turns that another car with a different make and model may not be able to make safely. The control module <NUM> incorporates these physical constraints in determining the control signals. The control module <NUM> sends the control signals to the vehicle controls <NUM> that cause the vehicle to execute the specified sequence of actions causing the vehicle to move as planned. The above steps are constantly repeated every few seconds causing the vehicle to drive safely along the route that was planned for the vehicle.

The various modules of the vehicle computing system <NUM> including the perception module <NUM>, prediction module <NUM>, and planning module <NUM> receive map information to perform their respective computation. The vehicle <NUM> stores the HD map data in the local HD map store <NUM>. The modules of the vehicle computing system <NUM> interact with the map data using the HD map API <NUM> that provides a set of application programming interfaces (APIs) that can be invoked by a module for accessing the map information. The HD map system interface <NUM> allows the vehicle computing system <NUM> to interact with the online HD map system <NUM> via a network (not shown in the Figures). The local HD map store <NUM> stores map data in a format specified by the HD Map system <NUM>. The HD map API <NUM> is capable of processing the map data format as provided by the HD Map system <NUM>. The HD Map API <NUM> provides the vehicle computing system <NUM> with an interface for interacting with the HD map data. The HD map API <NUM> includes several APIs including the localization API <NUM>, the landmark map API <NUM>, the route API <NUM>, the 3D map API <NUM>, the map update API <NUM>, and so on.

The localization APIs <NUM> determine the current location of the vehicle, for example, when the vehicle starts and as the vehicle moves along a route. The localization APIs <NUM> include a localize API that determines an accurate location of the vehicle within the HD Map. The vehicle computing system <NUM> can use the location as an accurate relative positioning for making other queries, for example, feature queries, navigable space queries, and occupancy map queries further described herein. The localize API receives inputs comprising one or more of, location provided by GPS, vehicle motion data provided by IMU, LIDAR scanner data, and camera images. The localize API returns an accurate location of the vehicle as latitude and longitude coordinates. The coordinates returned by the localize API are more accurate compared to the GPS coordinates used as input, for example, the output of the localize API may have precision range from <NUM>-<NUM>. In one embodiment, the vehicle computing system <NUM> invokes the localize API to determine location of the vehicle periodically based on the LIDAR using scanner data, for example, at a frequency of <NUM>. The vehicle computing system <NUM> may invoke the localize API to determine the vehicle location at a higher rate (e.g., <NUM>) if GPS/IMU data is available at that rate. The vehicle computing system <NUM> stores as internal state, location history records to improve accuracy of subsequent localize calls. The location history record stores history of location from the point-in-time, when the car was turned off/stopped. The localization APIs <NUM> include a localize-route API generates an accurate route specifying lanes based on the HD map. The localize-route API takes as input a route from a source to destination via a third party maps and generates a high precision routes represented as a connected graph of navigable lanes along the input routes based on HD maps.

The landmark map API <NUM> provides the geometric and semantic description of the world around the vehicle, for example, description of various portions of lanes that the vehicle is currently travelling on. The landmark map APIs <NUM> comprise APIs that allow queries based on landmark maps, for example, fetch-lanes API and fetch-features API. The fetch-lanes API provide lane information relative to the vehicle and the fetch-features API. The fetch-lanes API receives as input a location, for example, the location of the vehicle specified using latitude and longitude of the vehicle and returns lane information relative to the input location. The fetch-lanes API may specify a distance parameters indicating the distance relative to the input location for which the lane information is retrieved. The fetch-features API receives information identifying one or more lane elements and returns landmark features relative to the specified lane elements. The landmark features include, for each landmark, a spatial description that is specific to the type of landmark.

The 3D map API <NUM> provides efficient access to the spatial <NUM>-dimensional (3D) representation of the road and various physical objects around the road as stored in the local HD map store <NUM>. The 3D map APIs <NUM> include a fetch-navigable-surfaces API and a fetch-occupancy-grid API. The fetch-navigable-surfaces API receives as input, identifiers for one or more lane elements and returns navigable boundaries for the specified lane elements. The fetch-occupancy-grid API receives a location as input, for example, a latitude and longitude of the vehicle, and returns information describing occupancy for the surface of the road and all objects available in the HD map near the location. The information describing occupancy includes a hierarchical volumetric grid of all positions considered occupied in the map. The occupancy grid includes information at a high resolution near the navigable areas, for example, at curbs and bumps, and relatively low resolution in less significant areas, for example, trees and walls beyond a curb. The fetch-occupancy-grid API is useful for detecting obstacles and for changing direction if necessary.

The 3D map APIs also include map update APIs, for example, download-map-updates API and upload-map-updates API. The download-map-updates API receives as input a planned route identifier and downloads map updates for data relevant to all planned routes or for a specific planned route. The upload-map-updates API uploads data collected by the vehicle computing system <NUM> to the online HD map system <NUM>. This allows the online HD map system <NUM> to keep the HD map data stored in the online HD map system <NUM> up to date based on changes in map data observed by sensors of vehicles driving along various routes.

The route API <NUM> returns route information including full route between a source and destination and portions of route as the vehicle travels along the route. The 3D map API <NUM> allows querying the HD Map. The route APIs <NUM> include add-planned-routes API and get-planned-route API. The add-planned-routes API provides information describing planned routes to the online HD map system <NUM> so that information describing relevant HD maps can be downloaded by the vehicle computing system <NUM> and kept up to date. The add-planned-routes API receives as input, a route specified using polylines expressed in terms of latitudes and longitudes and also a time-to-live (TTL) parameter specifying a time period after which the route data can be deleted. Accordingly, the add-planned-routes API allows the vehicle to indicate the route the vehicle is planning on taking in the near future as an autonomous trip. The add-planned-route API aligns the route to the HD map, records the route and its TTL value, and makes sure that the HD map data for the route stored in the vehicle computing system <NUM> is up to date. The get-planned-routes API returns a list of planned routes and provides information describing a route identified by a route identifier.

The map update API <NUM> manages operations related to update of map data, both for the local HD map store <NUM> and for the HD map store <NUM> stored in the online HD map system <NUM>. Accordingly, modules in the vehicle computing system <NUM> invoke the map update API <NUM> for downloading data from the online HD map system <NUM> to the vehicle computing system <NUM> for storing in the local HD map store <NUM> as necessary. The map update API <NUM> also allows the vehicle computing system <NUM> to determine whether the information monitored by the vehicle sensors <NUM> indicates a discrepancy in the map information provided by the online HD map system <NUM> and uploads data to the online HD map system <NUM> that may result in the online HD map system <NUM> updating the map data stored in the HD map store <NUM> that is provided to other vehicles <NUM>.

<FIG> illustrates the various layers of instructions in the HD Map API of a vehicle computing system, according to an embodiment. Different manufacturer of vehicles have different instructions for receiving information from vehicle sensors <NUM> and for controlling the vehicle controls <NUM>. Furthermore, different vendors provide different compute platforms with autonomous driving capabilities, for example, collection and analysis of vehicle sensor data. Examples of compute platform for autonomous vehicles include platforms provided vendors, such as NVIDIA, QUALCOMM, and INTEL. These platforms provide functionality for use by autonomous vehicle manufacturers in manufacture of autonomous vehicles. A vehicle manufacturer can use any one or several compute platforms for autonomous vehicles. The online HD map system <NUM> provides a library for processing HD maps based on instructions specific to the manufacturer of the vehicle and instructions specific to a vendor specific platform of the vehicle. The library provides access to the HD map data and allows the vehicle to interact with the online HD map system <NUM>.

As shown in <FIG>, in an embodiment, the HD map API is implemented as a library that includes a vehicle manufacturer adapter <NUM>, a compute platform adapter <NUM>, and a common HD map API layer <NUM>. The common HD map API layer comprises generic instructions that can be used across a plurality of vehicle compute platforms and vehicle manufacturers. The compute platform adapter <NUM> include instructions that are specific to each computer platform. For example, the common HD Map API layer <NUM> may invoke the compute platform adapter <NUM> to receive data from sensors supported by a specific compute platform. The vehicle manufacturer adapter <NUM> comprises instructions specific to a vehicle manufacturer. For example, the common HD map API layer <NUM> may invoke functionality provided by the vehicle manufacturer adapter <NUM> to send specific control instructions to the vehicle controls <NUM>.

The online HD map system <NUM> stores compute platform adapters <NUM> for a plurality of compute platforms and vehicle manufacturer adapters <NUM> for a plurality of vehicle manufacturers. The online HD map system <NUM> determines the particular vehicle manufacturer and the particular compute platform for a specific autonomous vehicle. The online HD map system <NUM> selects the vehicle manufacturer adapter <NUM> for the particular vehicle manufacturer and the compute platform adapter <NUM> the particular compute platform of that specific vehicle. The online HD map system <NUM> sends instructions of the selected vehicle manufacturer adapter <NUM> and the selected compute platform adapter <NUM> to the vehicle computing system <NUM> of that specific autonomous vehicle. The vehicle computing system <NUM> of that specific autonomous vehicle installs the received vehicle manufacturer adapter <NUM> and the compute platform adapter <NUM>. The vehicle computing system <NUM> periodically checks if the online HD map system <NUM> has an update to the installed vehicle manufacturer adapter <NUM> and the compute platform adapter <NUM>. If a more recent update is available compared to the version installed on the vehicle, the vehicle computing system <NUM> requests and receives the latest update and installs it.

<FIG> shows the system architecture of an HD map system, according to an embodiment. The online HD map system <NUM> comprises a map creation module <NUM>, a map update module <NUM>, a map data encoding module <NUM>, a load balancing module <NUM>, a map accuracy management module, a vehicle interface module, a kinematic ICP module 290b, and a HD map store <NUM>. Other embodiments of online HD map system <NUM> may include more or fewer modules than shown in <FIG>. Functionality indicated as being performed by a particular module may be implemented by other modules. In an embodiment, the online HD map system <NUM> may be a distributed system comprising a plurality of processors. Details of the kinematic ICP module 290b are described in detail herein.

The map creation module <NUM> creates the map from map data collected from several vehicles that are driving along various routes. The map update module <NUM> updates previously computed map data by receiving more recent information from vehicles that recently travelled along routes on which map information changed. For example, if certain road signs have changed or lane information has changed as a result of construction in a region, the map update module <NUM> updates the maps accordingly. The map data encoding module <NUM> encodes map data to be able to store the data efficiently as well as send the required map data to vehicles <NUM> efficiently. The load balancing module <NUM> balances load across vehicles to ensure that requests to receive data from vehicles are uniformly distributed across different vehicles. The map accuracy management module <NUM> maintains high accuracy of the map data using various techniques even though the information received from individual vehicles may not have high accuracy.

<FIG> illustrates the components of an HD map, according to an embodiment. The HD map comprises maps of several geographical regions. The HD map <NUM> of a geographical region comprises a landmark map (LMap) <NUM> and an occupancy map (OMap) <NUM>. The landmark map comprises information describing lanes including spatial location of lanes and semantic information about each lane. The spatial location of a lane comprises the geometric location in latitude, longitude and elevation at high prevision, for example, at or below <NUM> precision. The semantic information of a lane comprises restrictions such as direction, speed, type of lane (for example, a lane for going straight, a left turn lane, a right turn lane, an exit lane, and the like), restriction on crossing to the left, connectivity to other lanes and so on. The landmark map may further comprise information describing stop lines, yield lines, spatial location of cross walks, safely navigable space, spatial location of speed bumps, curb, and road signs comprising spatial location and type of all signage that is relevant to driving restrictions. Examples of road signs described in an HD map include stop signs, traffic lights, speed limits, one-way, do-not-enter, yield (vehicle, pedestrian, animal), and so on.

The occupancy map <NUM> comprises spatial <NUM>-dimensional (3D) representation of the road and all physical objects around the road. The data stored in an occupancy map <NUM> is also referred to herein as occupancy grid data. The 3D representation may be associated with a confidence score indicative of a likelihood of the object existing at the location. The occupancy map <NUM> may be represented in a number of other ways. In one embodiment, the occupancy map <NUM> is represented as a 3D mesh geometry (collection of triangles) which covers the surfaces. In another embodiment, the occupancy map <NUM> is represented as a collection of 3D points which cover the surfaces. In another embodiment, the occupancy map <NUM> is represented using a 3D volumetric grid of cells at <NUM>-<NUM> resolution. Each cell indicates whether or not a surface exists at that cell, and if the surface exists, a direction along which the surface is oriented.

The occupancy map <NUM> may take a large amount of storage space compared to a landmark map <NUM>. For example, data of 1GB/Mile may be used by an occupancy map <NUM>, resulting in the map of the United States (including <NUM> million miles of road) occupying <NUM>×<NUM><NUM> bytes or <NUM> petabytes. Therefore the online HD map system <NUM> and the vehicle computing system <NUM> use data compression techniques for being able to store and transfer map data thereby reducing storage and transmission costs. Accordingly, the techniques disclosed herein make self-driving of autonomous vehicles possible.

In one embodiment, the HD Map does not require or rely on data typically included in maps, such as addresses, road names, ability to geo-code an address, and ability to compute routes between place names or addresses. The vehicle computing system <NUM> or the online HD map system <NUM> accesses other map systems, for example, GOOGLE MAPs to obtain this information. Accordingly, a vehicle computing system <NUM> or the online HD map system <NUM> receives navigation instructions from a tool such as GOOGLE MAPs into a route and converts the information to a route based on the HD map information.

The online HD map system <NUM> divides a large physical area into geographical regions and stores a representation of each geographical region. Each geographical region represents a contiguous area bounded by a geometric shape, for example, a rectangle or square. In an embodiment, the online HD map system <NUM> divides a physical area into geographical regions of the same size independent of the amount of data required to store the representation of each geographical region. In another embodiment, the online HD map system <NUM> divides a physical area into geographical regions of different sizes, where the size of each geographical region is determined based on the amount of information needed for representing the geographical region. For example, a geographical region representing a densely populated area with a large number of streets represents a smaller physical area compared to a geographical region representing sparsely populated area with very few streets. Accordingly, in this embodiment, the online HD map system <NUM> determines the size of a geographical region based on an estimate of an amount of information required to store the various elements of the physical area relevant for an HD map.

In an embodiment, the online HD map system <NUM> represents a geographic region using an object or a data record that comprises various attributes including, a unique identifier for the geographical region, a unique name for the geographical region, description of the boundary of the geographical region, for example, using a bounding box of latitude and longitude coordinates, and a collection of landmark features and occupancy grid data.

<FIG> illustrate geographical regions defined in an HD map, according to an embodiment. <FIG> shows a square geographical region 610a. <FIG> shows two neighboring geographical regions 610a and 610b. The online HD map system <NUM> stores data in a representation of a geographical region that allows for smooth transition from one geographical region to another as a vehicle drives across geographical region boundaries.

According to an embodiment, as illustrated in <FIG>, each geographic region has a buffer of a predetermined width around it. The buffer comprises redundant map data around all <NUM> sides of a geographic region (in the case that the geographic region is bounded by a rectangle). <FIG> shows a boundary <NUM> for a buffer of <NUM> meters around the geographic region 610a and a boundary <NUM> for buffer of <NUM> meters around the geographic region 610a. The vehicle computing system <NUM> switches the current geographical region of a vehicle from one geographical region to the neighboring geographical region when the vehicle crosses a threshold distance within this buffer. For example, as shown in <FIG>, a vehicle starts at location 650a in the geographical region 610a. The vehicle traverses along a route to reach a location 650b where it cross the boundary of the geographical region <NUM> but stays within the boundary <NUM> of the buffer. Accordingly, the vehicle computing system <NUM> continues to use the geographical region 610a as the current geographical region of the vehicle. Once the vehicle crosses the boundary <NUM> of the buffer at location 650c, the vehicle computing system <NUM> switches the current geographical region of the vehicle to geographical region 610b from 610a. The use of a buffer prevents rapid switching of the current geographical region of a vehicle as a result of the vehicle travelling along a route that closely tracks a boundary of a geographical region.

The HD map system <NUM> represents lane information of streets in HD maps. Although the embodiments described herein refer to streets, the techniques are applicable to highways, alleys, avenues, boulevards, or any other path on which vehicles can travel. The HD map system <NUM> uses lanes as a reference frame for purposes of routing and for localization of a vehicle. The lanes represented by the HD map system <NUM> include lanes that are explicitly marked, for example, white and yellow striped lanes, lanes that are implicit, for example, on a country road with no lines or curbs but two directions of travel, and implicit paths that act as lanes, for example, the path that a turning car makes when entering a lane from another lane. The HD map system <NUM> also stores information relative to lanes, for example, landmark features such as road signs and traffic lights relative to the lanes, occupancy grids relative to the lanes for obstacle detection, and navigable spaces relative to the lanes so the vehicle can efficiently plan/react in emergencies when the vehicle must make an unplanned move out of the lane. Accordingly, the HD map system <NUM> stores a representation of a network of lanes to allow a vehicle to plan a legal path between a source and a destination and to add a frame of reference for real time sensing and control of the vehicle. The HD map system <NUM> stores information and provides APIs that allow a vehicle to determine the lane that the vehicle is currently in, the precise vehicle location relative to the lane geometry, and all relevant features/data relative to the lane and adjoining and connected lanes.

<FIG> illustrates lane representations in an HD map, according to an embodiment. <FIG> shows a vehicle <NUM> at a traffic intersection. The HD map system provides the vehicle with access to the map data that is relevant for autonomous driving of the vehicle. This includes, for example, features 720a and 720b that are associated with the lane but may not be the closest features to the vehicle. Therefore, the HD map system <NUM> stores a lane-centric representation of data that represents the relationship of the lane to the feature so that the vehicle can efficiently extract the features given a lane.

The HD map system <NUM> represents portions of the lanes as lane elements. A lane element specifies the boundaries of the lane and various constraints including the legal direction in which a vehicle can travel within the lane element, the speed with which the vehicle can drive within the lane element, whether the lane element is for left turn only, or right turn only, and so on. The HD map system <NUM> represents a lane element as a continuous geometric portion of a single vehicle lane. The HD map system <NUM> stores objects or data structures representing lane elements that comprise information representing geometric boundaries of the lanes; driving direction along the lane; vehicle restriction for driving in the lane, for example, speed limit, relationships with connecting lanes including incoming and outgoing lanes; a termination restriction, for example, whether the lane ends at a stop line, a yield sign, or a speed bump; and relationships with road features that are relevant for autonomous driving, for example, traffic light locations, road sign locations and so on.

Examples of lane elements represented by the HD map system <NUM> include, a piece of a right lane on a freeway, a piece of a lane on a road, a left turn lane, the turn from a left turn lane into another lane, a merge lane from an on-ramp an exit lane on an off-ramp, and a driveway. The HD map system <NUM> represents a one lane road using two lane elements, one for each direction. The HD map system <NUM> represents median turn lanes that are shared similar to a one-lane road.

<FIG> illustrates lane elements and relations between lane elements in an HD map, according to an embodiment. <FIG> shows an example of a T junction in a road illustrating a lane element 810a that is connected to lane element 810c via a turn lane 810b and is connected to lane 810e via a turn lane 810d. <FIG> shows an example of a Y junction in a road showing label 810f connected to lane <NUM> directly and connected to lane 810i via lane <NUM>. The HD map system <NUM> determines a route from a source location to a destination location as a sequence of connected lane elements that can be traversed to reach from the source location to the destination location.

The kinematic ICP module <NUM> performs alignment of point clouds generated by a lidar mounted on a moving vehicle. For example, the kinematic ICP module <NUM> may perform alignment of two point clouds that are captured by a lidar mounted on a vehicle, for example, a source point cloud P1 and a target point cloud P2. The two point clouds are captured at different times and may capture an area with slightly different viewpoints. The kinematic ICP module <NUM> determines a transform that maps points of one point cloud to points of the other point cloud, for example, maps points of the source point cloud P1 to corresponding points of the target point cloud P2. A distance between two points can be the Euclidean distance between coordinates of the points but can be any measure of distance. Points of the point cloud may be represented using coordinates in a coordinate system and may include other parameters, for example, intensity, color, and the like. The intensity and color values may be provided by the lidar. A distance metric between two points may consider these additional parameters, for example, by ensuring that the intensity, color, etc. match and are within a predetermined threshold.

The kinematic ICP module <NUM> uses iterative closest point technique to determine velocity parameters for the vehicle. The kinematic ICP module <NUM> performs computations based on kinematic equations to determine velocity parameters. The kinematic ICP module <NUM> determines distances travelled by a vehicle using the velocity parameters. In an embodiment, the kinematic ICP module <NUM> uses equation (<NUM>) for determining position of the vehicle, assuming constant acceleration: <MAT>.

In equation (<NUM>), p represents the current position of the vehicle, a represents acceleration, t represents time, p<NUM> represents the position of the vehicle at a previous time t = <NUM>, and v<NUM> represents the velocity of the vehicle at the previous time t = <NUM>. The kinematic ICP module <NUM> receives timestamps for every column of points in the lidar point cloud, and determines the position of each point. Accordingly, the kinematic ICP module <NUM> solves for the motion and the unwinding of the point cloud simultaneously, and uses the natural constraints of the physical system to avoid divergence.

Typically each point cloud includes several thousand points. In an embodiment, the kinematic ICP module <NUM> performs sampling of the points of the point cloud to reduce the amount of computation. For example, the kinematic ICP module 290a of a vehicle computing system <NUM> may perform sampling of the point cloud to obtain a significantly smaller percentage of points of the point clouds that are processed compared to the kinematic ICP module 290b of the online HD map system <NUM>. This is so because the kinematic ICP module 290a of a vehicle computing system <NUM> may need to perform the computation in real time as the vehicle is driving to be able to determine the location of the vehicle for purposes of navigating the vehicle. In an embodiment, the kinematic ICP module <NUM> performs sampling by dividing the region represented by the point cloud into voxels and randomly selecting one or more points from each voxel. The degree of sampling may be controlled by controlling the size of the voxels. For example the kinematic ICP module <NUM> may use small voxels for dense sampling that generates large number of points and may use large voxels for sparse sampling that generates fewer points.

For sequential point clouds captured by a lidar mounted on a moving vehicle, the start of the 1st point cloud is t<NUM>, the end of the first and start of the 2nd is t<NUM>, and the end of the 2nd is t<NUM>. The kinematic ICP module <NUM> determines the following velocity parameters: at time t<NUM>: v(t<NUM>), ω(t<NUM>) , at time t<NUM>: v(t<NUM>), ω(t<NUM>) , at time t<NUM>: v(t<NUM>), ω(t<NUM>) where v(tx) represents the linear velocity at time tx and ω(x) represents the angular velocity at time tx. <FIG> illustrates two point clouds captured by lidar scans of a moving vehicle according to an embodiment. As shown in <FIG>, the kinematic ICP module <NUM> identifies a source point cloud <NUM> and a target point cloud <NUM> and uses the following velocity parameters: the linear velocity and the angular velocity at the time of the start of capture of the source point cloud and the linear velocity and the angular velocity at the time of the end of capture of the source point cloud; and the linear velocity and the angular velocity at the time of the start of capture of the target point cloud and the linear velocity and the angular velocity at the time of the end of capture of the target point cloud. If the source point cloud and the target point clouds were captured consecutively, the time of the end of capture of the source point cloud is same as the time of the start of capture of the target point cloud. Accordingly the kinematic ICP module <NUM> determines the linear velocity and the angular velocity for three distinct points in time, t<NUM> the time <NUM> of start of capture of the source point cloud t<NUM> the time <NUM> of end of capture of the source point cloud which is same as the time of start of capture of the target point cloud and t<NUM> the time <NUM> of end of capture of the target point cloud.

The linear velocity parameters have three components: a component along X-axis, a component along Y-axis, and a component along Z-axis. The angular velocity has three components, a roll component, a yaw component and a pitch component. Accordingly, for each time point there are <NUM> velocity parameters. Since there are three time points, the kinematic ICP module <NUM> determines <NUM> velocity parameters. The kinematic ICP module <NUM> performs an <NUM>-degrees of freedom (DOF) optimization problem to determine the <NUM> velocity parameters. In addition, since the system is modeling the motion of a vehicle on the road, the kinematic ICP module <NUM> uses physical constraints on the translational speed and acceleration limits to prevent the non-linear optimization from getting stuck in non-realistic local minima that is not physically possible. For example, the kinematic ICP module <NUM> uses a value of maximum acceleration MAX_ACCELERATIONxyz along each axis X, Y, and Z. Accordingly, in an embodiment, the kinematic ICP module <NUM> adds the following constraints specified by equations (<NUM>) and (<NUM>) while optimizing the velocity parameters. <MAT> <MAT>.

According to equation (<NUM>), a velocity component v(ti+<NUM>)xyz along an axis at time ti+<NUM> is within a threshold of v(ti)xyz, the corresponding velocity component at time ti where the threshold is directly proportional to the MAX_ACCELERATIONxyz value along that axis and the time difference Δt=ti+<NUM> - ti. Equation (<NUM>) specifies that the velocity v(ti) at any given time ti can be constrained to have a value between a high threshold value vmax and a low threshold value vmin. For example, using some knowledge of the vehicle performance, vmin = -<NUM> mile/hour (a limit of the velocity of a vehicle moving backwards on a road) and vmax = <NUM> mile/hour (a limit on velocity of forward moving vehicle on a road). In an embodiment, the kinematic ICP module <NUM> also bounds the rotational speed and accelerations by following equations (<NUM>) and (<NUM>). Accordingly, each component of the angular velocity ω(ti) at time ti is constrained to be less than a threshold MAX_ANGULAR_SPEED specified by physical constraints of a vehicle driving on roads. Similarly, each component of the linear velocity v(ti) at time ti is constrained to be less than a threshold Vmax turn(R) representing the maximum turning speed, which is a function of the turning radius R. <MAT> <MAT>.

In some embodiments, the kinematic ICP module <NUM> uses constraints based on more complex vehicle dynamics models such as Ackerman model constraints.

In some embodiments, the kinematic ICP module <NUM> performs computations based on an assumption of constant acceleration over the duration of the timespan (from t<NUM> to t<NUM>). Since the lidar is typically collecting data at a relatively high frequency (for example, <NUM>), assuming a constant acceleration does not result in significant loss of accuracy during the small time window of interest. This model provides higher accuracy than a simple constant velocity model based model. A constant velocity model based model does not meet the accuracy requirement for building an HD map due to significant changes in the translational and rotational velocities of a moving vehicle when turning, stopping, or accelerating during the course of a single lidar scan. For example, when turning, a vehicle can complete a <NUM> degree turns within seconds. If the system assumes a linear turning speed, the computations result in large angular errors, which results in large distance error (><NUM>) for laser measurements with high range.

In some embodiments, kinematic ICP module <NUM> uses a more advanced model, e.g., higher-order parameterization on acceleration. These models have higher complexity and require more parameters.

Under the assumption that the translation and angular accelerations are constant in he time intervals (t<NUM> ,t<NUM>) and (t<NUM>, t<NUM>), the kinematic ICP module <NUM> uses following equations 6a, 6b, 6c, and 6d to compute the linear velocity and angular velocity at any arbitrary time t via interpolation. <MAT> <MAT> <MAT> <MAT>.

The kinematic ICP module <NUM> uses following equations 7a, 7b, 7c, and 7d obtained by integration of equations 6a, 6b, 6c, and 6d respectively to compute the location (represented as x(t)) and roll , pitch, yaw values (represented as R(t)) at arbitrary timestamp t as follows: <MAT> <MAT> <MAT> <MAT>.

The integrated translation and rotation can be mapped to a 4x4 transformation matrix M(t) represented using equation (<NUM>) as follows. <MAT> where, x(t) = [x<NUM>, x<NUM>, x<NUM>]T, R(t) = [γ, β, α], where γ, β, α is the corresponding roll, pitch, yaw value.

<FIG> shows the system architecture of a kinematic ICP module, according to an embodiment. The kinematic ICP module <NUM> comprises other modules (or sub-modules) including the parameter initialization module <NUM>, the point cloud module <NUM>, the point correspondence module <NUM>, and optimization module <NUM>. Other embodiments may include more or fewer modules. Functionality indicated as being performed by a particular module may be performed by other modules instead.

The parameter initialization module <NUM> initializes the velocity parameters. The parameter initialization module <NUM> may initialize the velocity parameters based on values of the velocity parameters determined during a previous time interval. The parameter initialization module <NUM> may initialize the velocity parameters using sensors of the vehicle that detect parameters describing motion of the vehicle, for example, inertial measurement unit (IMU). The parameter initialization module <NUM> may also initialize the velocity parameters using data received from GNSS. For example, given two positions of the vehicle obtained from GNSS at two different points in time and the time difference between measurements of the two positions, the parameter initialization module <NUM> can determine the velocity parameters using kinematic equations.

The point cloud module <NUM> performs processing related to point clouds including determining points of a point cloud and transforming points of the point cloud. In an embodiment, the point cloud module <NUM> determines a point cloud as the set of points detected by one or more sensors of a vehicle, for example, by the LIDAR sensor between a start time and an end time. The point cloud module <NUM> may continue to determine point clouds based on consecutive time intervals, each time interval corresponding to a rotation of the lidar sensor. In an embodiment, the point cloud module <NUM> determines a point cloud by aggregating data of a plurality of point clouds. The point cloud module <NUM> aggregates data of two point clouds by mapping points of a point cloud P1 to points of the a point cloud P2 using a transform from the point cloud P1 to the point cloud P2. The point cloud module <NUM> uses the transform to determine corresponding points of two point clouds.

The point correspondence module <NUM> determines corresponding points between two point clouds P1 and P2. In an embodiment, the point correspondence module <NUM> determines pairs of corresponding points (p1, p2) from the point clouds P1 and P2 respectively. For each pair, (p1, p2), the point correspondence module <NUM> selects points p1 and p2 that are closest to each other. In an embodiment, the point correspondence module <NUM> establishes a correspondence between points p1 and p2 only if the determining that a source point of the source point cloud corresponds to a target point of the target point cloud if the target point of the target point cloud is the nearest neighbor of the source point and the source point of the source point cloud is determined to be the nearest neighbor of the target point. Accordingly, the closest target point from the source point cloud is also the closest source point from the target point cloud. This requires an extra nearest neighbor lookup per correspondence.

The optimization module <NUM> performs optimization to determine values of velocity parameters that minimize certain loss function, for example, the distance between corresponding points of two point clouds. In an embodiment, the optimization module <NUM> uses non-linear optimization techniques to perform the optimization, for example, gradient descent based optimization techniques. As an example, the optimization module <NUM> may perform non-linear least squares optimization using a solver such as CERES solver.

Assuming the system has previously established a set of correspondences between source and target point cloud, < si(tsi), (di (tdi), ni(tdi)) > i, i = <NUM>,<NUM>,<NUM>,···, where si(tsi) represents direct measurements of a point of the source point cloud at time tsi and di(tdi) represents direct measurements of a point of the target point cloud at time tdi, and ni(tdi) represents a normal of a surface in the target point cloud corresponding to the point di(tdi). The value of ni(tdi) is updated in each iteration with pose estimations.

The point correspondence module <NUM> computes correspondences by transforming all points according to previous velocity estimates, followed by nearest neighbor search. For each correspondence between source and target point cloud,< si(tsi), (di (tdi), ni(tdi)) >, the point correspondence module <NUM> determines a measure of point-to-plane error (i.e., the cost function) considering the lidar motion using equation (<NUM>). <MAT> where ρ(x) is a robustification operator on the loss function that penalizes large loss due to outliers.

The point correspondence module <NUM> may use one of several robustification operators, e.g., identity function, soft-L1 norm, Lorentz function, and so on. An embodiment uses the Lorentz operator as specified in equation (<NUM>).

In these embodiments, the optimization module <NUM> performs non-linear optimization represented by following equation (<NUM>).

The equation <NUM> determines the values of the velocity parameters that minimize the value of ρ(∥ni(tdi) · [M(tsi)si - M(tdi)di ]∥)<NUM> over all corresponding points of the source and target point clouds.

<FIG> illustrates the overall process for determining velocity parameters for a vehicle according to an embodiment. The steps described may be performed in an order different from that indicated herein. The steps may be performed by modules other than those indicated herein.

The kinematic ICP module <NUM> receives <NUM> sensor data captured by sensors of a vehicle. The kinematic ICP module <NUM> receives <NUM> lidar scans captured by a lidar mounted on an autonomous vehicle. The kinematic ICP module <NUM> determines <NUM> a source point cloud P1 and a target point cloud P2 based on the sensor data. The kinematic ICP module <NUM> determines <NUM> velocity parameters that minimize distance between corresponding points of the source point cloud and the target point cloud. The kinematic ICP module <NUM> provides the velocity parameters to other modules of the vehicle computing system <NUM>. The vehicle computing system <NUM> determines control signals based on velocity parameters and sends <NUM> the control signals to the vehicle controls <NUM> of the vehicle for navigating the vehicle.

<FIG> illustrates the process for performing kinematic ICP according to an embodiment. The steps described may be performed in an order different from that indicated herein. The steps may be performed by modules other than those indicated herein.

The kinematic ICP module <NUM> receives <NUM> as input, a source point cloud P1 and a target point cloud P2 for processing. The kinematic ICP module <NUM> initializes <NUM> the velocity parameters, for example, using GNSS-IMU data or based on velocity parameters previously determined.

The kinematic ICP module <NUM> repeats the following steps <NUM>, <NUM>, <NUM>, and <NUM> until convergence.

The kinematic ICP module <NUM> transforms <NUM> points in source point cloud and target point cloud using poses derived from current velocity estimates. The kinematic ICP module <NUM> transforms <NUM> each point to a position of the point if the point was captured while the vehicle was not moving. The points of a point cloud are moved in a direction determined using the direction of movement of the vehicle. The distance by which a point is moved depends on the time of capture of the point. In an embodiment, the kinematic ICP module <NUM> determines a distance that the vehicle would have moved between the time of capture of the point and a reference time and moves the point within the point cloud by that distance. The reference time may be the time that the lidar scan is completed, for example, the time of capture of the last point of the point cloud. Alternatively, the reference time may be the time that the lidar scan is started. The kinematic ICP module <NUM> transforms <NUM> the points of the point cloud to correspond to a position where the point would be located if the entire lidar scan for capturing the point cloud was taken while the vehicle (or the lidar) was stationary at the position where the vehicle (or the lidar) was located at the reference time. Accordingly, depending on the reference time used, the kinematic ICP module <NUM> transforms the points of the point cloud to move in the direction of the vehicle movement or opposite to the direction of the vehicle movement. For example, if the reference time is the time of end of the lidar scan corresponding to the point cloud, i.e., the time of capture of the last point captured for the point cloud, the kinematic ICP module <NUM> transforms the points of the point cloud to move against the direction of the vehicle movement. Alternatively, if the reference time is the time of start of the lidar scan corresponding to the point cloud, i.e., the time of capture of the first point captured for the point cloud, the kinematic ICP module <NUM> transforms the points of the point cloud to move in the direction of the vehicle movement.

The kinematic ICP module <NUM> estimates <NUM> normal vectors for points selected from the target point cloud. The kinematic ICP module <NUM> determines <NUM> correspondences between the source point cloud and the target point cloud based on the normal vectors. Accordingly, the kinematic ICP module <NUM> performs the kinematic ICP module <NUM> determines a surface within the target point cloud in the neighborhood of a selected point of the target point cloud and determines a normal vector for that surface. The kinematic ICP module <NUM> represents a correspondence as a pair of points p1 and p2 where p1 is selected from the source point cloud and p2 from the target point cloud. The kinematic ICP module <NUM> determines a nearest point of the source point cloud corresponding to the selected point of the target point cloud along the direction of the normal vector determined for the selected point. In some embodiments, the role of the source point cloud and the target point cloud may be switched and the kinematic ICP module <NUM> estimates <NUM> normal vectors for points selected from the source point cloud for determining the correspondences.

The kinematic ICP module <NUM> optimizes <NUM> velocity parameters to minimize aggregate losses of correspondences. In an embodiment, the kinematic ICP module <NUM> uses non-linear optimization techniques, for example, gradient descent based optimization techniques for optimizing <NUM> the velocity parameters. The kinematic ICP module <NUM> stores <NUM> the velocity parameters in a computer readable storage memory. The velocity parameters may be used for various computations related to the vehicle. For example, the velocity parameters are used for performing localization of the vehicle using the velocity parameters as input to Kalman filtering process. The Kalman filter process is used to extract a clean signal from a set of noisy input samples. For example, of the vehicle computing system receives from a GPS-IMU, some noisy velocity estimates, we the vehicle computing system runs them through a Kalman filter to remove some of the noise and obtain a better initial velocity. In some embodiments, the kinematic ICP module <NUM> loops the ICP velocity parameters from the previous samples and feeds them into the Kalman filter, to combine the two signals and obtain a better initial estimate.

In an embodiment, a transform from the source point cloud to the target point cloud is determined based on the velocity parameters. The transform is used for various operations, for example, for aligning various point clouds to generate a high-definition map. The high-definition map is used for determining what control signals to generate for navigating the vehicle.

The lidar scan provides a point cloud that is very sparse in the vertical direction. In an embodiment, the HD map system <NUM> builds a dense target point cloud by incrementally building up a neighborhood of previously aligned point clouds.

<FIG> illustrates neighborhood alignment of point clouds to improve on the density of point clouds, according to an embodiment. The kinematic ICP module <NUM> may perform independent alignments <NUM> that aligns consecutive pairs of point clouds. In an embodiment, the kinematic ICP module <NUM> determines a target point cloud by aligning a neighborhood <NUM> representing a plurality of point clouds. The kinematic ICP module <NUM> aligns the target point cloud with a source point cloud <NUM>. The plurality of point clouds of the neighborhood <NUM> are obtained by aligning a sequence of consecutive point clouds. The target point cloud is fixed during optimization, which saves having to perform processing such as repeated unwinding and recomputing of KD-trees. This results in significant performance improvement and saves computational resources.

In an embodiment, the neighborhood <NUM> is incrementally created as samples are aligned by retaining the k-most recent samples and their pairwise/unwinding transforms in a first in first out (FIFO) queue. The neighborhood is merged into to a single point cloud, which serves as the target point cloud for alignment. The point clouds are loaded from a permanent storage such as a disk into a computer memory, aligned, added to the neighborhood, merged for some number of times, and then used for any subsequent processing.

In an embodiment, the HD map system <NUM> selects the neighborhood size by either a fixed number of point clouds, or by a distance, i.e., by aligning all point clouds within a distance. In embodiments that uses distance to select the neighborhood, the computation and memory cost of merging can increase in low-speed tracks but diminish to very small values in highways (where every point is significant). Accordingly, the HD map system <NUM> may prefer distance based neighborhoods on highways. The fixed size neighborhood has similar performance on low-speed tracks or highways.

In some embodiments, the kinematic ICP module <NUM> may perform alignment of point clouds in parallel. The kinematic ICP module <NUM> splits a vector of non-stationary sample ids is in N subarrays and used N threads (or processes) for performing alignments in parallel. Each thread processes a sub-array and its own neighborhood. The kinematic ICP module <NUM> could potentially process alignment of each pair in parallel using a distinct computer processor.

In an embodiment, the kinematic ICP module <NUM> determines a timestep representing time difference between the source point cloud and the target point cloud based on a measure of error in computing the velocity parameters. The kinematic ICP module <NUM> uses a model of vehicle motion to compute velocity parameters. The model is chosen for computational performance and accuracy, but simplifies the true complex vehicle motion. In an embodiment, the kinematic ICP module <NUM> uses an adaptive model that estimates the error due to modeling and adjusts timesteps accordingly.

In an embodiment, the kinematic ICP module <NUM> determines a measure of error as follows. The kinematic ICP module <NUM> uses a model that approximates an acceleration by a constant velocity. Let the initial time t=<NUM>, and the final time t=b. The location of maximum error is independent of acceleration a and occurs at t=b/<NUM>, i.e., the midpoint of the interval. The error E at this point is estimated using the equation <MAT>. For curved motions, the kinematic ICP module <NUM> determines the error using the equation E = R - Rcos(θ/ <NUM>), where R is the radius and θ is the angle of the curve. The kinematic ICP module <NUM> uses these equations to calculate the model approximation error. The kinematic ICP module <NUM> subdivides the timestep until these errors are below a given threshold.

In an embodiment, the kinematic ICP module <NUM> performs the following steps for adaptively determining the timestep. The kinematic ICP module <NUM> determines a value dt to be the full timestep from sample-to-sample.

The kinematic ICP module <NUM> determines model approximation errors using equations for rotational error (e = R - Rcos(θ/<NUM>)) and acceleration ( <MAT>). If the kinematic ICP module <NUM> determines that the errors are above a threshold, kinematic ICP module <NUM> subdivides the timestep. The kinematic ICP module <NUM> repeats these two steps of determining model approximation errors and subdividing the timesteps while dt is greater than a threshold minimum timestep value.

In some embodiments, the kinematic ICP module <NUM> adds a regularization term to improve convergence of the result. In certain situations, the vehicle drives through surroundings where the point cloud data is sparse and the constraints for the ICP process are poor in specific directions. For example, if the vehicle is driving through a tunnel or a bridge or a highway with very little scenery change, the constraints along the direction of in which the vehicle is driving are poor since the aggregate measure of error between two consecutive point clouds does not change significantly by moving along that direction. As a result, convergence of the ICP process is slow or the process may even diverge.

In these embodiments, the kinematic ICP module <NUM> depends on having a reasonable initial estimate that is derived from reliable sources, for example, velocity estimates derived from GPS-IMU data or a previous set of values that was determined recently. The kinematic ICP module <NUM> uses IMU data that described vehicle acceleration for determining current velocity estimates and uses GPS data to determine the vehicle global position and combines the two data to determine estimates of velocity for the vehicle. The kinematic ICP module <NUM> uses the GPS position data for bias correction in the IMU acceleration data. The kinematic ICP module <NUM> adds a regularization term to the measure of error for the ICP. For example, the error estimate is represented as ∥ Kv + WGPS ∥ where Kv is the error estimate for ICP and WGPS is a regularization term. The regularization term prevents the ICP iterations from diverging in presence of sparse data.

<FIG> is a block diagram illustrating components of an example machine able to read instructions from a machine-readable medium and execute them in a processor (or controller). Specifically, <FIG> shows a diagrammatic representation of a machine in the example form of a computer system <NUM> within which instructions <NUM> (e.g., software) for causing the machine to perform any one or more of the methodologies discussed herein may be executed. In alternative embodiments, the machine operates as a standalone device or may be connected (e.g., networked) to other machines. In a networked deployment, the machine may operate in the capacity of a server machine or a client machine in a server-client network environment, or as a peer machine in a peer-to-peer (or distributed) network environment.

The machine may be a server computer, a client computer, a personal computer (PC), a tablet PC, a set-top box (STB), a personal digital assistant (PDA), a cellular telephone, a smartphone, a web appliance, a network router, switch or bridge, or any machine capable of executing instructions <NUM> (sequential or otherwise) that specify actions to be taken by that machine. Further, while only a single machine is illustrated, the term "machine" shall also be taken to include any collection of machines that individually or jointly execute instructions <NUM> to perform any one or more of the methodologies discussed herein.

The example computer system <NUM> includes a processor <NUM> (e.g., a central processing unit (CPU), a graphics processing unit (GPU), a digital signal processor (DSP), one or more application specific integrated circuits (ASICs), one or more radio-frequency integrated circuits (RFICs), or any combination of these), a main memory <NUM>, and a static memory <NUM>, which are configured to communicate with each other via a bus <NUM>. The computer system <NUM> may further include graphics display unit <NUM> (e.g., a plasma display panel (PDP), a liquid crystal display (LCD), a projector, or a cathode ray tube (CRT)). The computer system <NUM> may also include alphanumeric input device <NUM> (e.g., a keyboard), a cursor control device <NUM> (e.g., a mouse, a trackball, a joystick, a motion sensor, or other pointing instrument), a storage unit <NUM>, a signal generation device <NUM> (e.g., a speaker), and a network interface device <NUM>, which also are configured to communicate via the bus <NUM>.

The storage unit <NUM> includes a machine-readable medium <NUM> on which is stored instructions <NUM> (e.g., software) embodying any one or more of the methodologies or functions described herein. The instructions <NUM> (e.g., software) may also reside, completely or at least partially, within the main memory <NUM> or within the processor <NUM> (e.g., within a processor's cache memory) during execution thereof by the computer system <NUM>, the main memory <NUM> and the processor <NUM> also constituting machine-readable media. The instructions <NUM> (e.g., software) may be transmitted or received over a network <NUM> via the network interface device <NUM>.

While machine-readable medium <NUM> is shown in an example embodiment to be a single medium, the term "machine-readable medium" should be taken to include a single medium or multiple media (e.g., a centralized or distributed database, or associated caches and servers) able to store instructions (e.g., instructions <NUM>). The term "machine-readable medium" shall also be taken to include any medium that is capable of storing instructions (e.g., instructions <NUM>) for execution by the machine and that cause the machine to perform any one or more of the methodologies disclosed herein. The term "machine-readable medium" includes, but not be limited to, data repositories in the form of solid-state memories, optical media, and magnetic media.

For example, although the techniques described herein are applied to autonomous vehicles, the techniques can also be applied to other applications, for example, for displaying HD maps for vehicles with drivers, for displaying HD maps on displays of client devices such as mobile phones, laptops, tablets, or any computing device with a display screen. Techniques displayed herein can also be applied for displaying maps for purposes of computer simulation, for example, in computer games, and so on.

Some portions of this description describe the embodiments of the invention in terms of algorithms and symbolic representations of operations on information. These algorithmic descriptions and representations are commonly used by those skilled in the data processing arts to convey the substance of their work effectively to others skilled in the art. These operations, while described functionally, computationally, or logically, are understood to be implemented by computer programs or equivalent electrical circuits, microcode, or the like. Furthermore, it has also proven convenient at times, to refer to these arrangements of operations as modules, without loss of generality. The described operations and their associated modules may be embodied in software, firmware, hardware, or any combinations thereof.

Claim 1:
A method comprising:
receiving, by a lidar sensor mounted on an autonomous vehicle (<NUM>), sensor data describing an environment surrounding the autonomous vehicle (<NUM>), the sensor data comprising a plurality of points, each point associated with a time of capture of the point by the lidar sensor;
determining based on the plurality of points, a source point cloud and a target point cloud, wherein the two point clouds are captured at different times initializing a plurality of velocity parameters describing movement of the autonomous vehicle (<NUM>), the plurality of velocity parameters affecting determined respective positions of the plurality of points;
repeating, for a plurality of iterations:
determining a plurality of pairs of points, each pair of points comprising a point from the source point cloud and a corresponding point from the target point cloud;
modifying the velocity parameter in association with movement of the vehicle during capturing of the source point cloud and the target point cloud, the velocity parameters being modified such that an aggregate measure of distance between corresponding points of the plurality of pairs of points is minimized; and
adjusting a timestep representing a time difference between the source point cloud and the target point cloud based on a measure of error in computing the velocity parameters; and
determining control signals associated with navigating the autonomous vehicle (<NUM>) based on the velocity parameters.