System and method for locating an autonomous vehicle

Abstract

A system and method for localizing an autonomous vehicle using mapping and real-time data. And scanning and matching the mapping data and the real-time data. Characteristics of the autonomous vehicle, such as, but not limited to, linear and angular velocities, heading, and motion prediction, are provided to a Bayesian estimation algorithm, and a final pose is calculated.

Description

[0001] Cross-reference to related applications

[0002] none. Background Technology

[0003] This disclosure generally relates to positioning. More specifically, this disclosure concerns the positioning of autonomous vehicles.

[0004] Among other reasons, autonomous vehicles need to understand their surroundings to determine their location. Accurate positioning is necessary to prevent collisions between the autonomous vehicle and anything in its path. Current highly accurate positioning systems employ a suite of sensors that is also computationally and power-intensive. Less expensive systems can sacrifice accuracy in complex environments by making assumptions about the environment to simplify data collection and reduce processing requirements. GPS data can be used to provide basic navigation information, such as wheel-by-wheel instructions, but fails when vehicle location accuracy, which is less than that of GPS, is required. Map-based positioning, which allows vehicles to be positioned relative to mappings based on high-resolution images, is an alternative to GPS-based positioning. This technology requires a large amount of mapping storage. The most common applications for positioning include positioning relative to the autonomous vehicle's awareness of which road it is traveling on (road-level positioning), positioning relative to the autonomous vehicle's lateral and longitudinal position within the main lane (auto-lane-level positioning), and positioning relative to the autonomous vehicle's awareness of its lane and lateral position on the road (lane-level positioning). Solutions exist for each of these applications.

[0005] For example, road horizontal localization solutions include mapping matching methods, which further include deterministic and probabilistic models. Deterministic models include geometric-based and pattern-based models. Probabilistic models include Hidden Markov Models, Conditional Random Fields, Particle Filters, Weighted Graphs, and Multiple Hypotheses. Deterministic models cannot handle uncertainty and ambiguity. Probabilistic models require more computation than deterministic models, potentially delaying timely results and increasing power consumption issues that must be overcome. For some applications, road horizontal localization is insufficient to maintain vehicle safety. Obstacle avoidance and overtaking are two examples where road horizontal localization may be lacking.

[0006] Self-lane horizontal localization involves two general approaches: model-based approaches and learning-based approaches. In model-based approaches, preprocessing is applied to data frames collected by, for example, LiDAR and cameras to enhance features of interest, reduce clutter such as shadows, and trim irrelevant artifacts. After preprocessing, what remains is data containing lane markings that distinguish it from the rest of the data during feature extraction. Feature extraction can involve gradient-based detection or filters based on non-vertical edges or reflectivity. Following feature extraction, a fitting process uses a lane model to present a high-level representation of the driving path. Various types of lane models exist: parametric models (assuming lane geometry), semi-parametric models (parameterized by control points), and non-parametric models (assuming a continuous but not necessarily differentiable path). No model is always correct. Following the fitting process is a tracking process, where tracking from previous data frames improves the understanding of the current data frame. The learning methods involve deep learning algorithms, such as recursive feature shift aggregators for lane detection, keypoint estimation and point instance segmentation methods for land detection, lightweight lane detection CNNs learned through self-attention distillation, lightweight lane detection by optimizing spatial embeddings, and top-down lane detection frameworks based on conditional convolution. In benchmark tests, the accuracy of these models depends on the benchmark database being used.

[0007] Lane level localization methods include mapping-assisted and landmarking methods. Some mapping-assisted methods align (using sensors) perceived environmental landmarks—such as lane lines—with landmarks stored in a map. In some methods, LiDAR sensors are used to detect lane markings. In landmarking methods, road level features are extracted from an image and evaluated to determine the number of lanes around the vehicle and the lane the vehicle is traveling in. The number of lanes is determined using a probabilistic formula.

[0008] What is needed is a universal positioning system that is resource-efficient and provides timely information about the location of autonomous delivery vehicles. Summary of the Invention

[0009] The system for providing continuous positioning for an autonomous vehicle according to this teaching includes at least one processor executed in at least one autonomous vehicle. The method of this teaching includes a lane-level positioning type process, wherein at least one processor receives, processes, and provides offline (mapping) data, and receives and processes real-time (sensor) data. Mapping and sensor data, while potentially collected by the same vehicle, are collected during different navigation time periods. Sensors such as, but not limited to, LiDAR, GPS, and optical devices provide point cloud data, geographic data, and stereo and monocular data. Other types of sensors and data are contemplated in this teaching. During the positioning process, previously collected mapping data is accessed as real-time data is collected. Data related to the current GPS-determined position of the autonomous vehicle is isolated. A system of one or more computers can be configured to perform specific operations or actions by means of software, firmware, hardware, or combinations thereof installed on the system, which in operation causes or induces the system to perform actions. One or more computer programs can be configured to perform specific operations or actions by means of instructions including which, when executed by a data processing device, cause the device to perform actions. One general aspect includes a method for positioning an autonomous vehicle. The method further includes organizing a first mapping associated with the current location of the autonomous vehicle to form currently organized data; organizing at least one second mapping associated with at least one potential location that the autonomous vehicle can navigate to to form potential location organized data; updating the currently organized data as the autonomous vehicle navigates, based at least on the potential location organized data and the current location; selectively updating the potential location organized data based at least on the autonomous vehicle's direction of movement, speed of movement, potential location organized data, and the current location; filtering real-time data received by the autonomous vehicle to form real-time data; scanning and matching the processed currently organized data and real-time data to form matching mapping points based at least on a dynamic threshold; removing outlying data from the matching mapping points based on feature attributes, dynamic thresholds, and an outlier determination algorithm to form an attitude estimate; and correcting the attitude estimate based at least on planar features associated with the current location to form a positioning attitude. Other embodiments of this aspect include corresponding computer systems, apparatus, and computer programs recorded on one or more computer storage devices, each configured to perform actions of the method.

[0010] The implementation may include one or more of the following features. The method may include: receiving a first mapping from a source remote from the autonomous vehicle. The method may include: accessing the first mapping from a database stored locally on the autonomous vehicle. The first mapping may include: an 80m square. The method may include: receiving at least one second mapping from a source remote from the autonomous vehicle. The method may include: accessing at least one second mapping from a database stored locally on the autonomous vehicle. The second mapping may include: at least one 80m square sharing a boundary with the first mapping. Selective updating may include: determining whether the autonomous vehicle is located within a boundary zone between the first mapping and at least one second mapping, and updating potential location organized data when the autonomous vehicle navigates outside the boundary zone and the first mapping. The boundary zone may include: a width dynamically determined based at least on the autonomous vehicle's speed, the number and type of obstacles around the autonomous vehicle, and / or characteristics of the environment around the autonomous vehicle. Filtering real-time data may include: downsampling real-time data according to pre-selected criteria to form downsampled data. The pre-selected criteria may include: user-defined criteria. The pre-selected criteria may include: a default criterion. The pre-selected criteria may include: dynamically determined criteria. The pre-selected criteria may include: a pre-selected density. The method may include: segmenting downsampled real-time data into blocks of a pre-selected size. The method may include: forming a real-time plane growing to outliers from multiple scans of the downsampled real-time data. The method may include: identifying discontinuities in the real-time plane and removing points that are part of the discontinuities. The method may include: segmenting a first mapping and at least one second mapping into planar points and non-planar points. The method may include: removing non-planar points. The method may include: identifying planar points as points belonging to a non-ground plane. The method may include: identifying planar points by: (a) selecting random points from the downsampled data; (b) locating the point neighbors of the random points; (c) identifying planar points in the downsampled data as points on a flat plane formed by the random points and their point neighbors—if any; (d) identifying non-planar points as not planar points; and (e) repeating steps (a)-(d) until (1) no more downsampled data to be examined exists, or a pre-selected number of flat planes has been achieved, or a pre-selected number of iterations of steps (a)-(d) have been performed. The method may include: forming a real-time plane growing to outliers from multiple scans of downsampled real-time data, and matching the real-time plane to a flat plane. Organizing the first map may include: creating a k-dimensional tree from the first map. Organizing the first map may include: creating multiple k-dimensional trees from the first map and at least one second map by a parallel processor. Organizing the first map dataset and at least one second map may include: creating multiple k-dimensional trees from the first map dataset and at least one second map.Implementations of the described technology may include hardware, methods or processes, or computer software on a computer-accessible medium.

[0011] One general aspect includes a method for locating an autonomous vehicle. The method further includes receiving and processing offline (mapping) data and real-time (sensor) data by at least one processor. The method also includes isolating data related to the current GPS-determined location of the autonomous vehicle from the offline and real-time data. The method further includes determining a global attitude and a confidence level associated with the global attitude based at least on matching mapping features found in the offline data with real-time features found in the real-time data. The method further includes continuously calculating a local attitude as the autonomous vehicle navigates, at least based on a comparison between the current attitude and a previous attitude. The method further includes predicting future characteristics of the autonomous vehicle by executing a model of its movement. The method further includes continuously calculating a final attitude associated with the autonomous vehicle and an estimated confidence level based at least on the global attitude, local attitude, and future characteristics. Other embodiments of this aspect include corresponding computer systems, apparatus, and computer programs recorded on one or more computer storage devices, each configured to perform actions of the method.

[0012] The implementation may include one or more of the following features. The method may include: collecting offline and real-time data over different time periods. The method may include: collecting offline and real-time data from a public vehicle. The method may include: collecting real-time data via sensors including lidar, GPS, and optical devices. Calculating local attitude information may include: measuring the linear and angular velocities of the autonomous vehicle; measuring the local heading of the autonomous vehicle; estimating the image attitude of the autonomous vehicle using data aggregated from image sensors, at least based on previous and current images of the autonomous vehicle; estimating the laser attitude of the autonomous vehicle using data aggregated from laser sensors, at least based on previous and current attitudes of the autonomous vehicle; and calculating the local attitude based at least on a combination of linear and angular velocities, local heading, estimated image attitude, and estimated laser attitude. The model may include: a constant-speed autonomous vehicle. The model may include: a motion model of the autonomous vehicle. The model may include: a dynamically updated motion model of the autonomous vehicle. Continuously calculating the final attitude may include: submitting the global attitude, local attitude, and future characteristics to a Bayesian state estimator. The Bayesian estimator may include: an unscented Kalman filter. Implementations of the described technology may include hardware, methods or processes, or computer software on a computer-accessible medium.

[0013] One general aspect includes a system for localization of an autonomous vehicle. The system further includes at least one first processor configured to receive and filter sensor data and mapping data associated with the autonomous vehicle; a second processor configured to separate sensor data features of interest from the filtered sensor data and mapping data features of interest from the filtered mapping data; a third processor configured to match the sensor data features of interest with the mapping data features of interest; a fourth processor configured to perform registration on the matched data to create a global attitude; a fifth processor to aggregate local attitude information based at least on the autonomous vehicle's linear and angular velocities, its heading, an image attitude estimate, and a laser attitude estimate; a sixth processor to predict the autonomous vehicle's future motion; and a seventh processor to calculate the autonomous vehicle's final attitude based at least on the global attitude, local attitude information, and future motion. Other embodiments of this aspect include corresponding computer systems, apparatus, and computer programs recorded on one or more computer storage devices, each configured to perform actions of the method.

[0014] The implementation may include one or more of the following features. In this system, sensor data may include: LiDAR data, GPS data, and image data. Registration may include: an iterative nearest-point algorithm. 39, in this system, predicting future motion may include: executing a model configured to represent the behavior of an autonomous vehicle. The first processor may include: executing instructions to filter the mapped data: (b) downsampling the filtered sensor data to create downsampled data; (c) creating a sub-mapping of the downsampled data; (d) selecting random points from the downsampled data; (e) locating the point neighbors of the random points; (f) identifying planar points in the downsampled data as points on a flat plane formed by the random points and their point neighbors—if any; (g) identifying non-planar points as not planar points; and (d) repeating steps (a)-(f) until (a) no more downsampled data to be examined exists, or a pre-selected number of flat planes has been achieved, or the pre-selected number of iterations of steps (a)-(d) has been performed. The first processor may include: a ground plane processor that determines ground planes based on point cloud data received from sensors, each ground plane being associated with a ground plane equation, the sensors having a sensor reference frame; and a plane transformation processor that transforms the ground plane equation from the sensor reference frame to a vehicle reference frame associated with the autonomous vehicle. The ground plane processor may include: a median processor that calculates the median of at least two rings of the point cloud data; a point cloud filter that filters the point cloud data based at least on the distance from the median of the points in the point cloud data; a plane creation processor that creates planes from the filtered point cloud data, each of the created planes having at least one azimuth angle; a plane growth processor that grows the created planes from the point cloud data, the created planes extending away from the autonomous vehicle along at least one azimuth angle to form a growth plane; and a selection processor that selects ground planes from the growth planes based at least on the orientation and residuals of each of the created planes. A plane creation processor may include: executable code including computer instructions to select a first point and a second point from a first ring of sensor data, the first and second points being within a boundary formed by discontinuities in the point cloud data on the first ring, the first point having a first azimuth angle and the second point having a second azimuth angle; to select a third point from a second ring of sensor data, the second ring being adjacent to the first ring, the third point having a third azimuth angle between the first and second azimuth angles; and to create a plane comprising the first, second, and third points. The system may include: executable code including computer instructions to substitute a default plane when no ground plane can be determined. The system may include: executable code including computer instructions to remove a point from the point cloud data if the point exceeds a pre-selected distance from the autonomous vehicle.The system may include: executable code including computer instructions to remove a point from point cloud data if the point exceeds a pre-selected height based at least on the vehicle height of the autonomous vehicle. The system may also include: executable code including computer instructions to remove a point from point cloud data if the point is within a pre-selected distance from the autonomous vehicle. Transforming the ground plane may include: executable code including computer instructions to calculate a unit vector from the coefficients of a ground plane equation, the ground plane equation including ax + by + cz + d = 0, coefficients including a, b, and c, and a constant including d; a normalized d constant; transformation of the coefficients a, b, and c of the ground plane equation based on a rotation / transformation matrix and a unit vector; and transformation of the normalized d constant based on the normalized d constant, the rotation / transformation matrix, the unit vector, and the transformed coefficients a, b, and c. Implementations of the described techniques may include hardware, methods, or processes, or computer software on a computer-accessible medium. Attached Figure Description

[0015] The following figures illustrate non-limiting and non-exhaustive aspects of the subject matter disclosure, wherein, unless otherwise stated, the same reference numerals refer to the same parts in the various views.

[0016] Figure 1 This is a schematic block diagram of the environment in which the system of this teaching is implemented;

[0017] Figure 1A It is a graphical representation of the point cloud data captured by the system of this teaching;

[0018] Figure 1B It is shown Figure 1H and 1I Mosaic of orientation;

[0019] Figure 1C This is a graphical representation of the data block structure in this teaching;

[0020] Figure 1D and 1E It is a graphical representation of the geometric description of the odometer and inertial measurement unit;

[0021] Figure 1F and 1G This is a schematic block diagram of an exemplary system of this teaching;

[0022] Figure 1H and 1I This is a graphical representation of the data block structure in this teaching;

[0023] Figure 2A-2B A flowchart depicting the processes and steps according to the various aspects of this teaching;

[0024] Figures 3A-3C This is a graphical representation of the systems and methods for offline data aggregation taught in this paper;

[0025] Figure 4A This is a flowchart outlining the real-time data aggregation approach in this teaching;

[0026] Figure 4B-4I yes Figure 4A A graphical representation of the process depicted in the flowchart;

[0027] Figure 5 This is a schematic block diagram of an exemplary multiprocessor system of this teaching;

[0028] Figure 5A This is a graphical representation of the kd-tree search enhancement in this teaching;

[0029] Figures 5B-5D This is a graphical representation of the outlier removal strategy taught in this paper;

[0030] Figure 6A and 6B This is a graphical representation of the posture correction method used in this teaching;

[0031] Figure 7 This is a flowchart of the first exemplary configuration of the method taught in this paper; and

[0032] Figure 8 This is a flowchart of a second exemplary configuration of the method of this teaching. Detailed Implementation

[0033] In the following description, numerous specific details are set forth to provide a thorough understanding of the various aspects and arrangements. However, those skilled in the art will recognize that the techniques described herein can be practiced without one or more specific details or using other methods, components, materials, etc. In other instances, well-known structures, materials, or operations may not have been shown or described in detail to avoid obscuring certain aspects.

[0034] Throughout this specification, references to “aspect,” “arrangement,” or “configuration” indicate a particular feature, structure, or characteristic. Therefore, phrases such as “in an aspect,” “in an arrangement,” or “in a configuration” appearing throughout this specification do not necessarily refer to the same aspect, feature, configuration, or arrangement. Furthermore, particular features, structures, and / or characteristics can be combined in any suitable manner.

[0035] For the purposes of this disclosure and the claims, the terms “component,” “system,” “platform,” “layer,” “selector,” “interface,” etc., are intended to refer to a computer-related entity or an entity associated with an operating device having one or more specific functionalities, wherein the entity may be hardware, a combination of hardware and software, software, or software in execution. As an example, a component may be, but is not limited to, a process running on a processor, a processor, an object, an executable file, an execution thread, a program, and / or a computer. By way of illustration and not limitation, both an application running on a server and the server itself can be a component. One or more components may reside within a process and / or an execution thread, and components may be located on a single computer and / or distributed across two or more computers. Furthermore, components may execute from various computer-readable media, device-readable storage devices, or machine-readable media on which various data structures are stored. Components may communicate via local and / or remote processes, such as according to signals having one or more data packets (e.g., data from interactions between one component and another on a local system, a distributed system, and / or across a network such as the Internet with other systems via signals). As another example, a component can be a device having specific functionality provided by mechanical parts operated by an electrical or electronic circuit system, which can be operated by a software or firmware application executed by a processor, wherein the processor can be internal or external to the device and execute at least a portion of the software or firmware application. As yet another example, a component can be a device that provides specific functionality through an electronic component without mechanical parts; the electronic component can include a processor to execute software or firmware that at least partially endows the electronic component with functionality.

[0036] Within the scope of this specification, terms such as “storage,” “storage device,” “data storage,” “data storage apparatus,” and “database” refer to a memory component, an entity embodied in memory, or a component that includes memory. It should be understood that the memory component described herein can be volatile memory or non-volatile memory, or can include both volatile and non-volatile memory.

[0037] Furthermore, the term "or" is intended to mean an inclusive "or" rather than an exclusive "or". That is, unless otherwise stated or clear from the context, "X adopts A or B" is intended to mean any natural inclusive permutation. That is, if X adopts A, X adopts B, or X adopts both A and B, then "X adopts A or B" is satisfied in any of the foregoing instances. Additionally, the articles "a" and "an" as used in this disclosure and claims should generally be interpreted as meaning "one or more" unless otherwise specified or clearly indicated from the context to the singular form.

[0038] To the extent used herein, the terms “exemplary” and / or “illustrative” are intended to be used as examples, instances, or illustrations. For the avoidance of doubt, the subject matter disclosed herein is not limited to the disclosed examples. Furthermore, any aspect or design described herein as “exemplary” and / or “illustrative” is not necessarily to be construed as preferred or advantageous over other aspects or designs, nor does it imply exclusion of equivalent exemplary structures and techniques known to those skilled in the art. Moreover, to the extent used in the detailed description or claims, the terms “comprising,” “having,” “containing,” and other similar words are intended to be inclusive as an open transitional term similar to the term “including,” without excluding any additional or other elements.

[0039] As used herein, the term "infer" or "inference" generally refers to the process of reasoning or inferring the state of a system, environment, user, and / or intention from a set of observations captured via events and / or data. Captured data and events can include user data, device data, environmental data, data from sensors, application data, implicit data, explicit data, etc. For example, inference can be used to identify specific situations or actions, or to generate probability distributions over states of interest based on considerations of data and events.

[0040] The disclosed subject matter can be implemented as a method, apparatus, or article of manufacture that uses standard programming and / or engineering techniques to produce software, firmware, hardware, or any combination thereof to control a computer to implement the disclosed subject matter. The term "article of manufacture," as used herein, is intended to cover a computer program accessible from any computer-readable device, machine-readable device, computer-readable carrier, computer-readable medium, or machine-readable medium. For example, a computer-readable medium can include, but is not limited to, magnetic storage devices such as hard disks; floppy disks; magnetic stripes; optical discs (e.g., optical discs (CDs), digital video discs (DVDs), Blu-ray discs (BDs)); smart cards; flash memory devices (e.g., card, lever, key drives); virtual devices that emulate storage devices; and / or any combination of the aforementioned computer-readable media.

[0041] Typically, program modules include routines, programs, components, data structures, etc., that perform specific tasks or implement specific abstract data types. The embodiments shown in this disclosure can be practiced in a distributed computing environment, where some tasks are performed by remote processing devices linked via a communication network. In a distributed computing environment, program modules can reside on both local and remote memory storage devices.

[0042] A computing device may include at least a computer-readable storage medium, a machine-readable storage medium, and / or a communication medium. A computer-readable or machine-readable storage medium may be any available storage medium accessible by a computer, and includes volatile and non-volatile media, removable and non-removable media. By way of example and not limitation, a computer-readable or machine-readable storage medium may be implemented in conjunction with any method or technique for storing information such as computer-readable or machine-readable instructions, program modules, structured data, or unstructured data.

[0043] Computer-readable storage media can include, but is not limited to, random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technologies, optical disc read-only memory (CD-ROM), digital versatile disc (DVD), Blu-ray disc (BD) or other optical disc storage devices, magnetic tape cassettes, magnetic tape, disk storage devices or other magnetic storage devices, solid-state drives or other solid-state storage devices, or other tangible and / or non-transitory media capable of storing desired information. In this regard, the terms “tangible” or “non-transitory” used herein to describe storage devices, memories, or computer-readable media should be understood to exclude the use of merely transient signals as a modifier, and not to exclude any standard storage devices, memories, or computer-readable media that transmit more than just transient signals.

[0044] A computer-readable storage medium can be accessed by one or more local or remote computing devices, for example via access requests, queries or other data retrieval protocols, for various operations relating to the information stored on the medium.

[0045] The system bus used herein can be any of several types of bus architectures, which can be further interconnected to a memory bus (with or without a memory controller), a peripheral bus, and a local bus using any of a variety of commercially available bus architectures. Where applicable, the database can include a Basic Input / Output System (BIOS), which can be stored in non-volatile memory such as ROM, EPROM, or EEPROM, where the BIOS contains basic routines such as those that help transfer information between components within the computer during startup. RAM can also include high-speed RAM, such as static RAM, for caching data.

[0046] As used herein, a computer can operate in a networked environment using a logical connection to one or more remote computers via wired and / or wireless communication. Remote computers can be workstations, servers, routers, personal computers, laptops, microprocessor-based entertainment devices, peer-to-peer devices, or other public network nodes. The logical connections described herein can include wired / wireless connections to a local area network (LAN) and / or a larger network such as a wide area network (WAN). Such LAN and WAN networking environments are common in offices and companies and facilitate enterprise-wide computer networks, such as intranets, any of which can connect to global communication networks, such as the Internet.

[0047] When used in a LAN networking environment, a computer can connect to the LAN via a wired and / or wireless communication network interface or adapter. The adapter facilitates wired or wireless communication to the LAN, and the LAN can also include wireless access points (APs) configured thereon for wireless communication with the adapter.

[0048] When used in a WAN networking environment, the computer can include a modem or can connect to a communication server on the WAN via other means of establishing communication over the WAN—such as via the Internet. Internal or external modems and wired or wireless devices can be connected to the system bus via input device interfaces. In a networking environment, program modules described herein with respect to a computer or parts thereof can be stored in remote memory / storage devices.

[0049] When used in a LAN or WAN networking environment, the computer can access cloud storage systems or other network-based storage systems in addition to or in place of external storage devices. Typically, the connection between the computer and the cloud storage system can be established, for example, via an adapter or modem over the LAN or WAN. After connecting the computer to the associated cloud storage system, the external storage interface can manage the storage provided by the cloud storage system using the adapter and / or modem, as it will be another type of external storage. For example, the external storage interface can be configured to provide access to cloud storage sources as if these sources were physically connected to the computer.

[0050] As used herein, the term "processor" can refer to substantially any computing processing unit or device, including but not limited to: single-core processors; single-core processors with software multithreading capabilities; multi-core processors; multi-core processors with software multithreading capabilities; multi-core processors with hardware multithreading technology; vector processors; pipelined processors; parallel platforms; and parallel platforms with distributed shared memory. Additionally, a processor can refer to an integrated circuit, application-specific integrated circuit (ASIC), digital signal processor (DSP), field-programmable gate array (FPGA), programmable logic controller (PLC), complex programmable logic device (CPLD), state machine, discrete gate or transistor logic, discrete hardware components, or any combination thereof, designed to perform the functions described herein. Processors can utilize nanoscale architectures, such as, but not limited to, molecular and quantum dot-based transistors, switches, and gates, to optimize space usage or enhance the performance of user equipment. Processors can also be implemented as a combination of computing processing units. For example, processors can be implemented as one or more processors located together, tightly coupled, loosely coupled, or remotely positioned relative to each other. Multiple processing chips or multiple devices can share the performance of one or more functions described herein, and similarly, storage can be implemented across multiple devices.

[0051] For the sake of overview, this document describes various arrangements. For simplicity, a method (or algorithm) is depicted and described as a series of steps or actions. It should be understood and appreciated that the various arrangements are not limited to the actions and / or the order of actions shown. For example, actions can occur in various orders and / or concurrently, and may occur alongside other actions not presented or described herein. Furthermore, not all actions shown may be required to implement the method. Additionally, the method can alternatively be represented as a series of interrelated states via a state diagram or events. Furthermore, the methods described below can be stored on an article of art (e.g., a machine-readable storage medium) to facilitate the transport and transfer of such methods to a computer.

[0052] A system of one or more computers can be configured to perform a particular operation or action by installing software, firmware, hardware, or a combination thereof on the system, which, in operation, causes the system to perform the action. One or more computer programs can be configured to perform a particular operation or action by including instructions that, when executed by a data processing device, cause the device to perform an action.

[0053] Now for reference Figure 1The localization of the autonomous vehicle is achieved within a larger context and target, including an environment with obstacles. The environment can include vehicular traffic when the autonomous vehicle is navigating on a road, and pedestrian traffic when the autonomous vehicle is navigating on sidewalks and other pedestrian walkways such as bicycle / pedestrian lanes. As the autonomous vehicle navigates, it receives sensor data 501 from sensors mounted on the autonomous vehicle and elsewhere—such as, but not limited to, roadside, beacon, traffic signal, other vehicles, satellite, and other airborne sensors. This data is provided to both localization logic 503 and other autonomous logic 507. Additionally, a mapping of the autonomous vehicle's area is created 505 for the autonomous vehicle to localize itself. The result of executing the logic is that the autonomous vehicle moves toward its destination, avoids obstacles, and navigates through pedestrian and vehicular traffic.

[0054] The systems and methods of this teaching include processors and processes for real-time localization of autonomous vehicles. The systems of this teaching for providing continuous localization for autonomous vehicles include at least one processor executed in at least one autonomous vehicle. The methods of this teaching include lane-level localization-type processes, wherein (1) at least one processor receives, processes, and provides offline (mapping) data, and receives and processes real-time (sensor) data. Mapping and sensor data, while potentially collected by the same vehicle, are collected during different navigation time periods. Sensors such as, but not limited to, LIDAR, GPS, and optical devices provide point cloud data, geographic data, and stereo and monocular data. Other types of sensors and data are envisioned in this teaching. During the localization process, previously collected mapping data is accessed as real-time data is collected. (2) Data related to the current GPS-determined position of the autonomous vehicle is isolated, and (3) a match between the mapping and the real-time sensor data is computed to determine an attitude, referred to herein as the global attitude, and a measure of the uncertainty of the attitude within the mapping frame. As the autonomous vehicle navigates, (4) a local attitude is continuously computed, which is a comparison between the current attitude and previous attitudes. Furthermore, (5) a model of the autonomous vehicle's movement is executed to provide predictions of the autonomous vehicle's future characteristics. Global attitude, local attitude, and model characteristics are used (6) to calculate a real-time estimate of the autonomous vehicle's attitude. The real-time attitude estimate is determined by propagating measurements of the autonomous vehicle's changes over time to predict its subsequent state and the uncertainty of that prediction. The method updates the estimate and uncertainty when a new measurement is received. (7) The real-time attitude estimate is corrected based on available features in the incoming data.

[0055] Now for reference Figure 1A In one respect, in contrast to (1), feature-based mapping includes ground mapping and planar mapping. Feature mapping is prepared from, for example, LIDAR data. Figure 1AThe diagram illustrates the raw point cloud data point 4005 and the ordering of these point cloud points to ground points 4001 and planar points 4003. The feature-based mapping comprises the raw data that has been uniformly downsampled to a pre-selected density. The criteria used for downsampling are user-defined, default, and / or dynamically determined. When the raw data, ground, and planar point cloud data are collected, they are separated into pre-selected sized blocks and then provided to the autonomous vehicle, where the data can be used in the localization process of this teaching.

[0056] The mapping includes ground points 4001 (non-planar points). The system and method of this teaching classify ground points 4001 from planar points 4003 in the point cloud. Ground points 4001 are determined by locating points in the ground plane from the original mapping. In one aspect, the requirements associated with identifying the ground plane are user-defined. For example, an extracted plane with a normal close to the vertical within a predefined threshold is used as a label for the ground plane. In another aspect, the requirements are dynamically determined based on incoming sensor data and characteristics of the environment surrounding the autonomous vehicle. In another aspect, the system includes a dataset of possible ground plane characteristics, which may depend on, for example, but not limited to, the environment and / or geographical location. The ground planes are fitted to each other, and when all ground points in the ground plane are identified, they are removed from the original data so that only planar points 4003 are considered during the localization process. In another aspect, non-planar features are determined based on downsampled point cloud data. In another aspect, downsampling is performed by pre-selecting point density, cutting the point cloud data region into a pre-selected size, pre-selecting how many points a pre-selected attribute will contain, and downsampling to that number of points.

[0057] The mapping includes planar points 4003. Planar points 4003 are determined by locating points in non-ground areas. The criteria for selecting planar points are user-defined, default, and / or dynamically determined. In one aspect, planar features, such as walls and other non-surface objects, are identified according to predetermined criteria. In offline data, planar point identification is accomplished by selecting random points from the LiDAR point cloud data, locating their nearest neighbors, and determining which points from the point cloud data are on the plane if the points cross the plane. The system iteratively executes this process until it has reached a threshold based on pre-selected termination criteria. Pre-selected criteria include, but are not limited to, finding a pre-selected number of planes, not having enough remaining points to execute the process, or reaching the maximum number of iterations. At this point, the planes (representing planar features) are combined into an offline feature map. For real-time data, points from multiple data scans from sensors associated with the autonomous vehicle are filtered and then used to form planes grown to outliers. Filtering includes omitting points that are discontinuous parts of the plane. Non-surface grown planes are selected to further participate in matching with planes from the offline mapping data.

[0058] Now for reference Figure 1B , 1C 1H and 1I, relative to (2)-(7), the systems and methods of this teaching perform registration between offline and real-time data. Furthermore, the mapping data around the autonomous vehicle is divided into blocks of pre-selected size. Instead of processing all data at once, only the data closest to the autonomous vehicle and the data in the area the autonomous vehicle is navigating to (which may be the same data) are processed.

[0059] Now for reference Figure 1B , 1C 1H and 1I, in one aspect, relative to (2), provide location-related data for calculating the autonomous vehicle's position. In one aspect, the pre-selected region includes the geometry of the autonomous vehicle with center 14011, such as a circle, rectangle, square, or ellipse. In one aspect, the data in the currently pre-selected region is divided into blocks. In one aspect, a pre-selected amount of data from nine blocks around the autonomous vehicle is provided to the processor in the autonomous vehicle, including the data block where the autonomous vehicle is located and eight data blocks corresponding to possible directions in which the autonomous vehicle may move. The box 4011 where the autonomous vehicle is located is used to construct a data structure in a multi-dimensional space that enables efficient storage of spatial data, range search, partitioning of data points based on specific conditions, and other features. K-dimensional (kd) trees, R-trees, quadtrees, and octrees are examples of data structures that can be used to represent sensor data around the autonomous vehicle. Data from adjacent blocks—such as blocks 4013, 4015, and 4017 that the autonomous vehicle may move to next—is also used to construct the data structure. When an autonomous vehicle finds itself at the boundary between adjacent blocks, it accesses a new data structure and creates a new set of data structures from the adjacent data blocks.

[0060] Continuing relative to (3)-(7), now refer to Figure 1C As the autonomous vehicle navigates near and on the boundary 4019 between blocks, loading new data from adjacent blocks may cause data block jitter. Furthermore, when the autonomous vehicle is near a corner of the block it is navigating, it may navigate to any of the three blocks forming the corner neighbor. To address this possibility, a boundary region 4021 between blocks of pre-selected thickness is defined. The autonomous vehicle can travel within the boundary region 4021 without triggering data transmission between adjacent data blocks. Data transmission occurs when the autonomous vehicle leaves the boundary region 4021. In one aspect, the width of the boundary region 4021 is user-defined. In another aspect, the width of the boundary region 4021 is dynamically determined, possibly based on the autonomous vehicle's speed, the number and type of obstacles around the autonomous vehicle, and / or other aspects of the autonomous vehicle's environment. In yet another aspect, the width of the boundary 4021 is selected based on a pre-selected formula or algorithm.

[0061] Compared to (4), and referring to Figure 1D and 1E According to this teaching, local attitude is calculated based on data collected in real time by the autonomous vehicle. Such data can include, but is not limited to, wheel odometer data, inertial measurement unit (IMU) data, image attitude estimation data, and LiDAR attitude estimation data. In one aspect, such as... Figure 1D As shown, the wheel odometer is provided by a wheel encoder. The wheel encoder measures the angular travel of each wheel. In one respect, the encoder measurement assumes that the angular travel occurs at a constant rate during the sampling period, thus the encoder measurement provides angular velocity. Scaling the angular velocity by the radius of the wheel converts the wheel encoder into a wheel linear velocity sensor.

[0062] These measurements are converted into measurements of forward linear velocity and yaw rate.

[0063]

[0064] The measurement is predicted to be:

[0065]

[0066] Addition and subtraction yield the following results:

[0067]

[0068] By dividing by 2 and substituting v left and v right Definition

[0069]

[0070] On the one hand, such as Figure 1E As shown, the IMU includes a magnetometer for returning the local heading relative to magnetic north. Figure 1E This shows the measurement of the local heading relative to the inertial frame. The equation for predicting the heading measurement is:

[0071]

[0072] matrix The reference frame associated with the magnetometer is transformed into a wheelbase reference frame. In one aspect, image pose estimation is based on previous and current images collected by sensors mounted on the autonomous vehicle. In another aspect, the sensors are located elsewhere, such as, but not limited to, on beacons, on other autonomous vehicles, on manned vehicles, on traffic signals, and on handheld devices. In another aspect, LiDAR pose estimation is generated by comparing the current pose provided by LiDAR measurements with the previous pose. In another aspect, two vectors are compared. [[Variables need to be defined if the following equation is used]]

[0073]

[0074] in It is a copy of the state vector from the last time LiDAR data was available. On one hand, the GPS sensor provides... and Direct measurement.

[0075]

[0076] Preferably, the present invention uses other sensors, not limited to an IMU and wheel encoders. The IMU provides information about the autonomous vehicle's orientation and angular velocity. The wheel encoders provide information about the autonomous vehicle's linear and angular velocities. UKF uses these measurements to estimate the internal state.

[0077] In contrast to (5), on one hand, it is necessary to predict the motion of the autonomous vehicle in a way that estimates its attitude based on the data described so far. One way to predict motion is by assuming constant behavior. Another is to create a general motion model of the vehicle, and yet another is to create a vehicle-specific motion model. As the autonomous vehicle encounters various situations, such as different types of surfaces and terrain or different characteristics of the vehicle itself—e.g., the weight it carries—the model can be dynamically updated. Yet another way to predict motion is to use the localization results themselves to determine corrected motion parameters that reflect the motion of the autonomous vehicle. Such a technique is described in Milstein et al., Localization with Dynamic Motion Models, International Conference on Informatics in Control, Automation, and Robotics (ICINCO), 2006.

[0078] In contrast to (6), on one hand, global pose, local pose, and model output data are supplied to a method for estimating the final pose. Exemplary methods for providing such estimates belong to the category of Bayesian state estimators. Such estimators include, but are not limited to, Kalman filters, extended Kalman filters, unscented Kalman filters (UKF), and particle filters. Kalman filters, extended Kalman filters, and particle filters are described in Montella, C., The Kalman Filter and Related Algorithms: A Literature Review, https: / / www.researchgate.net / publication / 236897001, May, 2011. A description of UKF implementations can be found in Wan et al., The Unscented Kalman Filter, Kalman Filtering and Neural Networks, Chapter 7, Haykin ed., John Wiley & Sons, Inc., October 2001. UKF allows for the addition / removal of sensing elements and adapts to varying sampling rates.

[0079] Now for reference Figure 1F and 1G The diagram illustrates the components of an exemplary configuration of the system of this teaching. In one aspect, all components are housed within the autonomous vehicle. In another aspect, some processing is performed outside the autonomous vehicle, and the results are provided to the autonomous vehicle via standard communication hardware and protocols. The components collectively determine the location of the autonomous vehicle within a pre-selected area. In one aspect, data is collected offline within the pre-selected area, and real-time data is collected as the autonomous vehicle navigates the pre-selected area. As the vehicle navigates, features are isolated and registered, and these, along with other data, are used to calculate and correct the attitude.

[0080] Continue to refer to Figure 1F and 1G The sensor provides data to the sensor processor 101. On one hand, it provides mapped data (offline data) collected from the target area. On the other hand, the sensor collects at least LiDAR, image, and GPS data. Among other things, GPS data is also used to locate the autonomous vehicle, enabling the provision of a subset of relevant offline data to the autonomous vehicle. Among other things, real-time LiDAR and image data are also used in attitude calculations. The system taught in this paper... Figure 2A-2B Process offline data in the manner described in 3A-3C, and in... Figure 4A-4IThe method described herein is used to process real-time data. These data are searched to perform feature extraction 105. Features from both real-time and offline data can present challenges when processed together. For example, real-time data may be collected at a different frame rate than offline data. The two datasets may span an area much larger than the autonomous vehicle is currently navigating. For these reasons, and possibly others, features from both datasets are preprocessed 114 before being transformed into a global pose. Preprocessing can include, for example, but not limited to, organizing the data according to a conventional organization scheme (such as a kd-tree) for efficient matching. Outliers are removed, pose is estimated and corrected, and this process is repeated until the real-time point cloud converges to the mapped point cloud. On one hand, a registration method 107 is used to perform the transformation, for example, but not limited to, the Iterative Closest Point (ICP) algorithm with several variations. This teaching envisions other techniques, such as, for example, but not limited to, the Levenberg-Marquardt iterative method, least-squares rigid transformation, and robust rigid transformation. A description of an ICP implementation can be found in the following literature: Low, Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration. Technical Report TR04-004 Department of Computer Science, University of North Carolina at Chapel Hill, February 2004 (Low). Numerous variations of the ICP algorithm exist, including those that: select sets of points from two datasets, match some points from one dataset to other points, weigh the pairs, discard some pairs, assign an error metric, and minimize the error metric. The ICP variant chosen to solve the localization problem taught in this paper is based on, for example, the type and characteristics of the sensors used, the desired accuracy of the results, and the desired computational speed.

[0081] Continue to refer to Figure 1F and 1G Global attitude data based on offline and real-time data, as well as local attitude and model data, are provided to attitude estimator 109, which generates final attitude 111 after attitude correction. In one aspect, local attitude data 113 can include, but is not limited to, wheel odometer data 13 (…). Figure 1G ) and Inertial Measurement Unit (IMU) data 112 ( Figure 1G ), and image pose estimation data 134 ( Figure 1G ) and LiDAR pose estimation data 136 ( Figure 1GOn one hand, it can provide behavioral and characteristic data of autonomous vehicles to further improve the results. On another hand, it can provide data (model information) from model 117 along with autonomous vehicle navigation. On yet another hand, model 117 calculates characteristic data and represents the behavior of autonomous vehicles based on model algorithms.

[0082] Now for reference Figure 2A The diagram illustrates a flowchart representing an exemplary process for offline data digestion. Method 150 includes, but is not limited to, receiving data 151 as the autonomous vehicle navigates, such as, but not limited to, point cloud data. For example, 3D point cloud data can be obtained from stereo cameras, LiDAR sensors, roadside surround view cameras, moving laser scans, structural and / or airborne laser scans mounted on the autonomous vehicle or elsewhere. Method 150 includes uniformly downsampling the incoming data 153 to produce final raw point cloud data, which is published for access by a processor on the autonomous vehicle. Method 150 includes downsampling the incoming data 141 and creating 155 sub-maps of the remaining data points. Creating sub-maps involves separating the map of data points into sub-parts and then analyzing each sub-part separately in a loop including steps 157-199. If 157 has no more sub-maps to loop through and there are no other sub-map sizes, method 150 includes ending 171 feature extraction and starting feature processing. If there are no more features to examine at step 173, method 150 includes performing step 179 point plane matching. If there are additional features to be examined at step 173, and if the plane is a ground plane at step 175, then method 150 includes step 177 of downsampling the ground point cloud using a first set of parameters to create a final ground point cloud. If the plane is not a ground plane at step 175, then method 150 includes step 178 of downsampling the non-ground point cloud using a second set of parameters to create a final planar point cloud.

[0083] The mapping is divided into various sub-mapping sizes to account for the planes spanning the sub-mappings. If more sub-mappings exist to cycle through in step 157, method 150 includes step 159 of selecting a random point within a sub-mapping. If a point is available at step 161, method 150 includes step 163 of finding the k nearest neighbors of the point in a kd-tree. If, at step 165, the neighbors span a plane, method 150 includes step 167 of locating a plane perpendicular to the plane spanned.

[0084] Now for reference Figure 2BIf 183 has reached a pre-selected threshold for the number of ground planes, and if the plane normal 193 meets the non-ground plane requirement, then method 150 includes marking plane 192 as a non-ground plane. If 183 has not yet reached the pre-selected threshold, and if the plane normal 184 meets the ground plane requirement, then method 150 includes calculating the number of points on the plane traversed by 185, referred to herein as inliers. If 187 the number of inliers is greater than a pre-selected threshold, and if 189 the plane traversed belongs to a previous plane, meaning that if that plane is marked as a ground plane, then all ground planes in the ground plane list are tested for angle and distance thresholds to see if these planes are the same. Method 150 includes merging all inliers 191 and refitting the plane, meaning creating a larger plane and calculating different normals. If the plane traversed by 189 does not belong to a previous plane, then method 150 includes replacing the plane in the ground plane list upon repetition. If the number of fitted planes has reached a pre-selected threshold, or if the loop has met a pre-selected maximum number of iterations, or if the number of remaining points is less than a pre-selected threshold, then method 150 includes appending points and plane normals from the list of ground planes to the feature map and returning to check another sub-map.

[0085] Now for reference Figures 3A-3C Method 150 is shown. Figure 2A-2B The graphical representation of point cloud data 201 is shown. Specifically, point cloud data 201 is downsampled in two ways to provide downsampled data 1203. Figure 3A ) and downsampling #2 data 205 ( Figure 3A Later, during the matching process, downsampled data #1 (1203) will be used. Figure 3A Downsampling #2 data 205 ( Figure 3A ) is divided into sub-parts, for example, sub-mappings 207A-207D ( Figure 3A Random point 209 ( Figure 3B ) is selected from a sub-mapping, and its k nearest neighbors are determined. Figure 3B If the k nearest neighbors and a random point are in submapping 213 ( Figure 3B ) spans plane 215 ( Figure 3B If the number of points z in the plane traversed is counted, it is compared with a pre-selected minimum threshold number w. The plane 221 traversed is then determined. Figure 3C ) normal plane 219 ( Figure 3C If the normal plane is 219 ( Figure 3C If the non-ground plane requirement is met, and the total number of ground planes x is greater than the ground plane number threshold y, then update the feature mapping 225. Figure 3C If the normal plane is 220 ( Figure 3CIf the ground plane requirement is met, and the total number of ground planes x is less than the ground plane quantity threshold y, then update the feature mapping 225. Figure 3C If the number of points z is greater than the pre-selected minimum number of points w and the plane spanned is 221( Figure 3C If a point is not in the list of ground planes, the plane it traverses is added to the list of ground planes and the number of ground planes x is incremented by x+1. If the number of points z is greater than the pre-selected minimum number of points w and the number of ground planes traversed is 221 ( Figure 3C In the list of ground planes, all interior points are merged, the plane is refitted, and the feature map is updated. The offline mapping data is ready to enter the feature mapping process.

[0086] Now for reference Figure 4A The method 350 for locating important planes in real-time data can include, but is not limited to: receiving 1351 point cloud data from a sensor at a very high level, filtering 1353 data on the median value of the data in one dimension, creating a 355 plane and growing the plane to outliers, selecting 357 important planes, and providing the planes to feature extraction 105. Figure 1F ).

[0087] Now for reference Figure 4B On one hand, when collecting LiDAR point cloud data (as opposed to another type of sensor data), the point cloud data can be received as a 1D string 303 of points along each LiDAR ring 301 around the autonomous vehicle 203. In some configurations, three 1D arrays can be used to store x, y, and z points. All points along the LiDAR rings can be stored in azimuth order (see [link to documentation]). Figure 4F Furthermore, LIDAR rings can be stored contiguously in a row-major manner. Each ring in the ring can be divided into segments of a pre-selected size, such as, but not limited to, 64 points.

[0088] Now for reference Figure 4C The 1D string 303 on ring 301A / B / N can be filtered according to a process that includes, but is not limited to, filtering the 1D string 303 around the median 307A / B of the points in each LiDAR 336 data ring. Figure 4BThe filtering can include locating points with measurements close to the median 307A / B and eliminating the remaining points in that portion of the analysis. In some configurations, close to the median 307A / B is defined as less than 0.1m from the median 307A / B. Points that have passed through the median filter can be designated as first-class points. Discontinuities 309A / B in the point data can be found along the median 307A / B. Discontinuities 309A / B can be identified in any suitable manner, such as, but not limited to, calculating the Cartesian distance between points P1 and P2, comparing that distance to a first pre-selected threshold, and identifying discontinuities 309A / 309B, or edges of the data, as second-class points when the distance between points P1 and P2 is greater than the first pre-selected threshold. In some configurations, discontinuities 309A / B occur when abs(D2-D1)>0.08*NP*A.

[0089] in

[0090] P1, P2 = continuous points

[0091] D1 = Distance between P1 and the sensor

[0092] D2 = Distance between P2 and the sensor

[0093] NP = the number of points since the last good point.

[0094] A = (d1 + d2) / 2

[0095] P1 = the best point last time

[0096] P2 = the point being tested

[0097] If the number of points exceeds a second pre-selected threshold, points located between discontinuities 309A / 309B in space 311 can be counted and marked as third-class points. In some configurations, the second pre-selected threshold can include eight points. Points between pairs of discontinuities not exceeding the second pre-selected threshold can be discarded.

[0098] Now for reference Figure 4D Significant planes are intended to fit the terrain around the autonomous vehicle. They have relatively low residuals, are sufficiently large, and typically represent ground points around the autonomous vehicle. In some configurations, the residual threshold can include 0.1. To determine significant planes, points can be selected from points on the same LiDAR ring 301A, such as the first point 305A and the second point 305B. In some configurations, the first / second point 305A / B can be randomly selected from a third class of points located between adjacent discontinuities. Other criteria can be used to increase the probability that a point belongs to a significant plane.

[0099] Now for reference Figure 4E and4F The third point 305C can be selected from the adjacent ring 301B. The third point 305C can have an azimuth angle α1 located at the first point 305A and the second point 305B. Figure 4F ) and α2( Figure 4F The azimuth angle α3 between ) Figure 4F Points 305A / B / C (first / second / third) form a plane with a defined equation that can be evaluated for its relationship to the gravity vector. In some configurations, evaluating the plane can include checking the plane's orientation by selecting a plane with a normal vector of no more than 60° relative to the gravity vector provided by, for example, but not limited to, an inertial measurement sensor located on an autonomous vehicle. As the plane grows and points are added, the orientation angle can be reduced to 20°. In one aspect, the plane is grown by selecting the n nearest neighbors of the seed point and comparing the normals of the nearest neighbors with the plane normal. If the normal does not deviate from the plane normal by more than the range of 40-60° and includes an angle threshold of 40-60°, neighbors can be added to grow the plane. Other plane growth techniques are envisioned in this teaching.

[0100] Now for reference Figure 4G It is possible to evaluate whether all points remaining from the previous filtering steps described herein are included in polygon 313A. The edges of polygon 313A can be defined by the first point / second point 305A / 305B and the ring 301A / B.

[0101] Now for reference Figure 4H The plane can be grown vertically in four directions to form polygon 313B. Growing the plane can include evaluating points in the originally selected polygon 313A that are increasingly farther away from the autonomous vehicle's orientation ring and along all four directions of the azimuth. The plane can be grown as described herein, and the plane equation can be updated based on the newly included points and evaluated for orientation relative to the gravity vector. Each direction can be grown independently until a residual violation threshold is reached on that side or if that side reaches the edge 323 of the point cloud. At that point, orientation and residual checks can be performed, and if passed, the plane can be classified as initially important. Additional checks such as the number of points, the number of growth cycles, and the number of vertical growth cycles can be performed to aid further filtering. For example, if a plane has undergone ten lateral growth cycles or two vertical growth cycles and is not considered important, plane growth can be terminated.

[0102] Now for reference Figure 4I This allows for the evaluation of data from ring 353 to form plane 351, as described herein. From the set of significant planes, surface planes can be identified by subjecting them to a scoring function, such as, but not limited to:

[0103]

[0104]

[0105] Final Score=Residual Score+Groeth Score+Area Score+Angle Score

[0106] The scoring function can be used to evaluate the quality of a plane; a higher score indicates a more likely candidate, and planes that do not meet a pre-selected threshold are discarded. The angular score refers to the degree of proximity of the plane to a vertical plane or the ground plane.

[0107] Now for reference Figure 5 According to this teaching, feature matching is used to match mapped data with real-time data to provide the final pose. Different feature matching methods are used for different types of features. For planar / surface features, the mapping localization method is used to match mapped planar point data with real-time planar point data. For curvature points (points on planar and curved surfaces), the scan matching method is used to match mapped raw point data with real-time curvature point data. This paper, relative to... Figure 2A-2B And 3A-3C describes the processing of mapping data. This article is relative to... Figure 4A-4I The processing of real-time data is described. Scanning and matching in an exemplary heterogeneous computing environment according to this teaching includes tasks performed by a first processor and tasks performed by a second processor, which exchange data and handshake control. In one aspect, the first processor is a sequential processor, while the second processor is a parallel processor, such as, but not limited to, a multi-functional unit such as a graphics processing unit, multiple execution units / cores, and multiple hardware threads. The process begins with the first processor organizing mapping data corresponding to a pre-selected area around the location of the autonomous vehicle. Organization can include constructing, for example, a balanced kd-tree from the mapping data. A second step includes performing the same organization and provision of data blocks of a pre-selected size around the originally provided data. The direction of movement of the autonomous vehicle and the kd-tree of the data blocks associated with the autonomous vehicle, along with filtered real-time data as described herein, are provided to an efficient search algorithm for dynamic detection and to accelerate the search process, and are used to generate matching mapping points. Outliers from the matching points are removed. One process for removing outliers is the interquartile range (IQR) method, where values ​​outside the IQR fence are removed as outliers. Another filtering process evaluates the matches from the search kd-tree by removing points where the angle between the real-time data normal and the mapped data point normal exceeds a threshold. In another filtering process, points that do not lead to convergence during the iterations of the ICP process are removed. These filtered matching mapped points are used to estimate the pose (e.g., using UKF, as described herein) and the pose confidence, and are then processed according to… Figure 6B Posture correction.

[0108] Now for reference Figure 5A A kd-tree is a spatial data structure used for efficient nearest-neighbor search. A kd-tree partitions space into multiple dimensions based on its depth. Partitioning is used to efficiently search for nearby points and discard distant ones. Typically, mapped points within a threshold distance from the real-time point are examined via the kd-tree. The efficiency of a kd-tree search depends on the search threshold used and the speed at which the nearest point is reached along the kd-tree. As the algorithm iterates, it forces the real-time data to converge on the mapped data. Conversely, the search threshold is lowered to accelerate the kd-tree search. Intelligent threshold reduction speeds up the convergence process. The search threshold also depends on how far the sensor point is from the autonomous vehicle. For example, a lidar point 100 meters from the autonomous vehicle has a larger search threshold than a lidar point 10 meters away. In one aspect,

[0109] For iterations 1-3, distance_threshold = 2 + 0.04 × l_dist

[0110] For iterations 4-15, distance_threshold = (2 + 0.04 × l_dist) / (6 × (Itr - 3))

[0111] For iterations 16-20, distance_threshold = (2 + 0.04 × l_dist) / 72

[0112] l_dist: Distance from the robot's lidar point

[0113] Itr: Number of iterations

[0114] Now for reference Figures 5B-5D The system taught in this paper, used for mapping localization and scan matching, uses sample data, feature information, and iterative convergence behavior to remove outliers 4031 and detect false matches. Figure 5B In this study, the standard interquartile range (IQR) 4033 outlier removal method, which uses point-to-plane distance, is used to remove data points that fall outside the IQR range plus / minus a pre-selected distance. Furthermore, a kd-tree search matches LiDAR points with their nearest mapped points, regardless of the normal direction. Therefore, outlier removal includes detecting cases where the nearest points are not in the same plane. Figure 5C The diagram illustrates a case where an erroneous match, where the angle indication between LIDAR point normal 4035 and the matching mapped point normal 4037 should be discarded, is present. Figure 5D In the process, iterative steps are used to reduce the point-to-plane distance threshold (mapping localization) and the point-to-point threshold (scanning matching) to eliminate matches that do not lead to convergence.

[0115] Now for reference Figure 6A The confidence level of the calculated attitude 702 of the autonomous vehicle can be assessed through a set of steps. The confidence level is based on the number and orientation of non-surface planar features along the autonomous vehicle's direction of travel. For example, when there are generally orthogonal planar features as shown in plate 707, a relatively high confidence level in the calculated attitude 702 can be assumed (e.g., value 1), while when there are no non-surface planar features as shown in plate 701, a relatively low confidence level in the calculated attitude 702 can be assumed (e.g., value 0). When there are few non-surface planar features as shown in plate 703, or when the surface planar features as shown in plate 705 are oriented relatively parallel to each other, the confidence level is somewhere between relatively high and relatively low, for example, a value of .7 associated with the configuration shown in plate 705, and a value of .4 associated with the configuration shown in plate 703. In one aspect, confidence indices are assigned to various configurations. For example, index 1 is assigned to a relatively high confidence level in the calculated pose 702 associated with the planar feature configuration in board 707, and index -4 is assigned to a relatively low confidence level in the calculated pose 702 associated with the absence of planar features, as depicted in board 701. In one aspect, the index associated with the planar feature shown in board 705 has a value of 1, and the index associated with the planar feature shown in board 703 has a value of 0. In one aspect, index values ​​-1, -2, and -3 are assigned to cases involving unreliable data / computation. In one aspect, the confidence level is a function of at least the number of points on the planar feature and the angle between the planar features. An exemplary equation for calculating pose confidence is...

[0116]

[0117] N1: The number of points in plane 1

[0118] N2: The number of points in plane 2

[0119] n1: Normal to plane 1

[0120] n2: Plane 2 normal

[0121] Now for reference Figure 6B When planar features exist in the direction shown in plate 707, where the confidence level is relatively high, attitude correction is not required. However, as in plate 703 ( Figure 6A ) and board 709 ( Figure 6BIn this process, when only the mapping plane feature 707a exists in one direction, and the LIDAR plane feature 707b is not aligned with the mapping plane feature 707a, the input attitude 704 needs to be corrected. On one hand, the correction occurs in the direction where the autonomous vehicle 203 does not encounter the mapping plane feature. For example, the input attitude 704 is corrected in the y-direction by moving it in the y-direction; a certain amount of the LIDAR plane feature 707b is moved to align with the mapping plane feature 707a at the attitude estimation position 706. In the next step, the attitude estimation position 706 is moved in the x-direction to align with the input attitude 704 that forms the corrected attitude 1702.

[0122] Now for reference Figure 7 This illustrates a first configuration of a method for locating an autonomous vehicle according to the present teaching. The method 1100 for locating an autonomous vehicle includes, but is not limited to: organizing 1102 a first mapping associated with the current location of the autonomous vehicle to form currently organized data; organizing 1104 at least one second mapping associated with at least one potential location that the autonomous vehicle can navigate to to form potential location organized data; updating the currently organized data 1106 as the autonomous vehicle navigates at least based on the potential location organized data and the current location; and selectively updating the potential location organized data 1108 based at least on the autonomous vehicle's direction of movement, speed of movement, potential location organized data, and the current location. The method 1100 further includes filtering real-time data received by the autonomous vehicle 1110 to form real-time data; scanning and matching the processed currently organized data and real-time data 1112 to form matching mapping points at least based on a dynamic threshold; removing outlier data from the matching mapping points 1114 based on feature attributes, the dynamic threshold, and an outlier determination algorithm to form an attitude estimate; and correcting the attitude estimate at least based on planar features associated with the current location 1116 to form a positioning attitude.

[0123] Now for reference Figure 8The diagram illustrates a second configuration of a method for locating an autonomous vehicle according to this teaching. The method 800 for locating an autonomous vehicle includes, but is not limited to: receiving and processing 802 offline (mapping) data and real-time (sensor) data by at least one processor; isolating 804 data related to the current GPS-determined location of the autonomous vehicle from the offline and real-time data; determining 806 a global attitude and a confidence level associated with the global attitude based at least on matching mapping features found in the offline data with real-time features found in the real-time data; continuously calculating 808 a local attitude as the autonomous vehicle navigates, at least based on a comparison between the current attitude and previous attitudes; predicting 810 future characteristics of the autonomous vehicle by performing a model of the autonomous vehicle's movement; and continuously calculating 812 a final attitude associated with the autonomous vehicle and an estimated confidence level based at least on the global attitude, local attitude, and future characteristics.

[0124] Those skilled in the art will understand that the methods described in this disclosure can be applied to computer systems configured to implement such methods, and / or computer-readable media containing programs for implementing such methods, and / or software and / or firmware and / or hardware (e.g., integrated circuits) designed to implement such methods. Raw data and / or results can be stored for future retrieval and processing, printing, displaying, transmission to another computer, and / or transmission elsewhere. Communication links can be wired or wireless, including but not limited to, for example, Ethernet, cellular or broadband networks, WiFi or local area networks, military communication systems, and / or satellite communication systems. Parts of the system can operate, for example, on a computer with a variable number of CPUs. Other alternative computer platforms can be used.

[0125] As those skilled in the art will understand, the methods described in this disclosure can be implemented electronically, in whole or in part. Signals indicating actions taken by elements of the disclosed system, along with other disclosed configurations, can travel on at least one field communication network. Control and data information can be executed electronically and stored on at least one computer-readable medium. The system can be implemented to execute on at least one computer node in at least one field communication network. Common forms of computer-readable media can include, for example, but not limited to, floppy disks, flexible disks, hard disks, magnetic tape or any other magnetic media, optical disc read-only memory or any other optical media, punched cards, paper tape or any other physical media with a perforated pattern, random access memory, programmable read-only memory, erasable programmable read-only memory (EPROM), flash memory EPROM or any other memory chip or cartridge, or any other medium from which a computer can read.

[0126] Those skilled in the art will understand that information and signals can be represented using any of a variety of different prior art. For example, data, instructions, commands, information, signals, bits, symbols, or chips that can be referenced throughout this specification can be represented by voltage, current, electromagnetic waves, magnetic fields or particles, light fields or particles, ultrasound, projected capacitance, or any combination thereof.

[0127] Those skilled in the art will further appreciate that the various illustrative logic blocks, modules, circuits, and algorithmic steps described in conjunction with the arrangements disclosed herein can be implemented as electronic hardware, computer software, or a combination of both. To clearly illustrate this interchangeability between hardware and software, various illustrative components, blocks, modules, circuits, and steps have been described in terms of their functionality. Whether such functionality is implemented as hardware or software depends on the particular application and the design constraints imposed on the entire system. Those skilled in the art can implement the described functionality in varying ways for each particular application, but such implementation decisions should not be construed as causing a departure from the scope of the appended claims.

[0128] The various illustrative logic blocks, modules, and circuits described in conjunction with the arrangements disclosed herein can be implemented or executed using a general-purpose processor, digital signal processor (DSP), application-specific integrated circuit (ASIC), field-programmable gate array (FPGA) or other programmable logic device, discrete gate or transistor logic, discrete hardware components, or any combination thereof, designed to perform the functions described herein. The general-purpose processor may be a microprocessor, but alternatively, the processor may be any conventional processor, controller, microcontroller, or state machine. The processor may also be implemented as a combination of computing devices, such as a combination of a DSP and a microprocessor, multiple microprocessors, one or more microprocessors combined with a DSP core, or any other such configuration.

[0129] The actions of the methods or algorithms described in conjunction with the arrangements disclosed herein can be directly embodied in hardware, software modules executed by a processor, or a combination of both. The software modules can reside in RAM memory, flash memory, ROM memory, EPROM memory, EEPROM memory, registers, hard disks, removable disks, CD-ROMs, or any other form of storage medium known in the art. The storage medium can be coupled to the processor, enabling the processor to read information from and write information to the storage medium. Alternatively, the storage medium can be integrated into the processor. The processor and storage medium can reside in an ASIC. The ASIC can reside in a functional device, such as, for example, a computer, robot, user terminal, mobile phone or tablet computer, automobile, or IP camera. Alternatively, the processor and storage medium can reside as discrete components in such a functional device.

[0130] The above description is not intended to be exhaustive or to limit the features to the precise forms disclosed. Various alternatives and modifications can be devised by those skilled in the art without departing from this disclosure, and the general principles defined herein can be applied to other aspects without departing from the spirit or scope of the appended claims. Therefore, this disclosure is intended to cover all such alternatives, modifications, and variations. Furthermore, although several arrangements of this disclosure have been shown in the drawings and / or discussed herein, this disclosure is not intended to be limited thereto, as it is intended to be as broad in scope as would be permitted in the art, and the specification is read equally. Therefore, the above description should not be construed as restrictive, but rather as examples of particular configurations only. Other modifications within the scope and spirit of the appended claims will be contemplated by those skilled in the art. Other elements, steps, actions, methods, and techniques substantially different from those described above and / or in the appended claims are also intended to be within the scope of this disclosure. Therefore, the appended claims are not intended to be limited to the arrangements shown and described herein, but rather to be consistent with the broadest scope of the principles and novel features disclosed herein.

[0131] The arrangements shown in the accompanying drawings are merely illustrative of certain examples of this disclosure. Furthermore, the described drawings are purely illustrative and not restrictive. In the drawings, for illustrative purposes, the sizes of some elements may be exaggerated and not drawn to a particular scale. Additionally, depending on the context, elements shown in the drawings with the same reference numerals may be the same element or may be similar elements.

[0132] In the use of the term "comprising" in this specification and claims, it does not exclude other elements or steps. When referring to a singular noun such as "a" or "the," the use of an indefinite or definite article includes the plural form of that noun unless explicitly stated otherwise. Therefore, the term "comprising" should not be construed as limited to the items listed herein; it does not exclude other elements or steps, and thus the scope of the expression "device comprising items A and B" should not be limited to a device consisting solely of components A and B. Furthermore, wherever the terms "comprising," "having," "possessing," etc., are used in this specification and claims, such terms are intended to be included in a manner similar to the term "including," as "comprising" is interpreted when used as a transitional word in the claims.

[0133] Furthermore, the terms “first,” “second,” “third,” etc., are provided, both in the specification and in the claims, to distinguish similar elements without necessarily describing a sequential or chronological order. It should be understood that the terms thus used are interchangeable where appropriate (unless otherwise clearly disclosed), and the embodiments of this disclosure described herein can operate in other orders and / or arrangements than those described or shown herein.

[0134] A system of one or more computers can be configured to perform specific operations or actions by installing software, firmware, hardware, or combinations thereof on the system, which in operation cause or induce the system to perform actions. One or more computer programs can be configured to perform specific operations or actions by including instructions that, when executed by a data processing device, cause the device to perform actions.

Claims

1. A method for positioning an autonomous vehicle, comprising: organizing a first map associated with a current location of the autonomous vehicle, forming current organized data; organizing at least one second map associated with at least one potential location that the autonomous vehicle can navigate to, forming potential location organized data; updating the current organized data as the autonomous vehicle navigates, based at least on the potential location organized data and the current location; selectively updating the potential location organized data based at least on a direction of movement, a speed of movement of the autonomous vehicle, the potential location organized data and the current location; filtering real-time data received by the autonomous vehicle, forming real-time data; scan matching the processed current organized data and the real-time data, forming at least a dynamically thresholded matching map point; outlier rejection from the matching map point based on the feature attributes, dynamic thresholding and outlier determination algorithms, forming a pose estimate; and correcting the pose estimate based at least on planar features associated with the current location, forming a positioned pose.

2. The method of claim 1, further comprising: receiving the first map from a source remote from the autonomous vehicle.

3. The method of claim 1, further comprising: accessing the first map from a database stored locally to the autonomous vehicle.

4. The method of claim 1, further comprising: receiving the at least one second map from a source remote from the autonomous vehicle.

5. The method of claim 1, further comprising: accessing the at least one second map from a database stored locally to the autonomous vehicle.

6. The method of claim 1, wherein, selectively updating comprises: determining whether the autonomous vehicle is located in a border zone between the first map and the at least one second map; and updating the potential location organized data when the autonomous vehicle navigates to outside the border zone and the first map.

7. The method of claim 1, wherein, the filtering real-time data comprises: down-sampling the real-time data according to pre-selected criteria, forming down-sampled data.

8. The method of claim 7, wherein, the pre-selected criteria are selected from the group consisting of: user-defined criteria; default criteria; dynamically determined criteria; pre-selected density; and combinations thereof.

9. The method of claim 1, further comprising: separating the first map and the at least one second map into planar points and non-planar points.

10. The method of claim 9, further comprising: removing the non-planar points.

11. The method of claim 9, further comprising: identifying planar points as points that belong to a non-ground plane.

12. The method of claim 9, further comprising: identifying planar points, comprising: (a) selecting a random point from the down-sampled data; (b) locating point neighbors of the random point; (c) identifying planar points in the down-sampled data as points that lie on a flat plane formed by the random point and the point neighbors, if any; (d) identifying the non-planar points as points that are not the planar points; and (e) repeating steps (a)-(d) until (1) there are no longer any of the down-sampled data to check, or a pre-selected number of flat planes has been achieved, or a pre-selected number of iterations of steps (a)-(d) has been performed.

13. The method of claim 7, further comprising: forming, from a plurality of scans of the down-sampled real-time data, real-time planes grown to outliers.

14. The method of claim 13, further comprising: determining discontinuities in the real-time planes; and removing points that are part of the discontinuities.

15. The method of claim 12, further comprising: forming, from a plurality of scans of the down-sampled real-time data, real-time planes grown to outliers; and matching the real-time planes to the flat planes. organizing the first map includes: creating a k-dimensional tree from the first map, and / or 16. The method of claim 12, wherein, creating, by a parallel processor, a plurality of k-dimensional trees from the first map and the at least one second map. organizing the first map dataset and the at least one second map includes: creating a plurality of k-dimensional trees from the first map dataset and the at least one second map.

17. The method of claim 12, wherein, 18. The method of claim 6, wherein the boundary zone includes a width that is dynamically determined based on at least a speed of the autonomous vehicle, a number and type of obstacles surrounding the autonomous vehicle, and / or characteristics of an environment surrounding the autonomous vehicle.

19. A method for positioning an autonomous vehicle, comprising: receiving and processing, by at least one processor, offline (mapping) data and real-time (sensor) data; isolating, from the offline data and the real-time data, data related to a current GPS-determined position of the autonomous vehicle; determining a global pose and a confidence level associated with the global pose based on at least matching of mapped features found in the offline data to real-time features found in the real-time data; continuously calculating a local pose based on at least a comparison between a current pose and a previous pose as the autonomous vehicle navigates; predicting future characteristics of the autonomous vehicle by executing a model of movements of the autonomous vehicle; and continuously calculating a final pose and an estimated confidence level associated with the autonomous vehicle based on at least the global pose, the local pose, and the future characteristics.

20. The method of claim 19, further comprising: collecting the offline data and the real-time data during different time periods and / or by common vehicles. calculating the local pose includes: measuring linear and angular velocities of the autonomous vehicle; 21. The method of claim 19, wherein, measuring a local heading of the autonomous vehicle; estimating an image pose of the autonomous vehicle based on at least previous and current images of the autonomous vehicle using data gathered by an image sensor; estimating a laser pose of the autonomous vehicle based on at least previous and current poses of the autonomous vehicle using data gathered by a laser sensor; and ​ ​ calculating the local pose based at least on combining the linear and angular velocities, the local heading, the estimated image pose, and the estimated laser pose.

22. The method of claim 19, wherein, the model is selected from the group consisting of: constant velocity autonomous vehicle; a movement model for the autonomous vehicle; a dynamically updated movement model for the autonomous vehicle; and combinations thereof.

23. The method of claim 19, wherein, continuously calculating the final pose includes: submitting the global pose, the local pose, and the future characteristics to a Bayesian state estimator.

24. A system for localization of an autonomous vehicle, comprising: at least one first processor configured to receive and filter sensor data and map data associated with the autonomous vehicle; a second processor configured to separate features of interest in the filtered sensor data and features of interest in the filtered map data; a third processor configured to match the features of interest in the sensor data to the features of interest in the map data; a fourth processor configured to perform registration on the matched data to create a global pose; a fifth processor to aggregate local pose information based at least on linear and angular velocities of the autonomous vehicle, a heading of the autonomous vehicle, an image pose estimate of the autonomous vehicle, and a laser pose estimate of the autonomous vehicle; a sixth processor to predict future motion of the autonomous vehicle; and a seventh processor to calculate a final pose of the autonomous vehicle based at least on the global pose, the local pose information, and the future motion.

25. The system of claim 24, wherein: the registration includes an iterative closest point algorithm. the first processor includes:

26. The system of claim 24, wherein, instructions to perform filtering of the map data: (a) downsample the filtered sensor data to create downsampled data; (b) create a submap of the downsampled data; (c) select a random point from the downsampled data; (d) locate point neighbors of the random point; (e) identify planar points in the downsampled data as points that lie on a flat plane formed by the random point and the point neighbors, if any; (f) identify non-planar points as points that are not the planar points; and (g) repeat steps (a)-(f) until (1): there are no longer any of the downsampled data to check, or a preselected number of flat planes have been achieved, or a preselected number of iterations of steps (a)-(d) have been performed. the first processor includes:

27. The system of claim 24, wherein, a ground plane processor to determine ground planes from point cloud data received from a sensor, each of the ground planes being associated with a ground plane equation, the sensor having a sensor frame of reference; and a plane transformation processor to transform the ground plane equations from the sensor frame of reference to a vehicle frame of reference associated with the autonomous vehicle. the ground plane processor includes:

28. The system of claim 27, wherein, a median processor to calculate a median of at least two rings of the point cloud data; a point cloud filter to filter the point cloud data based at least on distances of points in the point cloud data from the median; ​ a plane creation processor that creates planes from the filtered point cloud data, each of the created planes having at least one azimuth angle; a plane growth processor that grows the created planes from the point cloud data, the created planes extending away from the autonomous vehicle along the at least one azimuth angle to form grown planes; and a selection processor that selects a ground plane from the grown planes based at least on an orientation and a residual of each of the created planes.

29. The system of claim 28, wherein, the plane creation processor includes: executable code including computer instructions that: select a first point and a second point from a first ring of sensor data, the first point and the second point being within a boundary formed by a discontinuity in the point cloud data on the first ring, the first point having a first azimuth angle, and the second point having a second azimuth angle; select a third point from a second ring of sensor data, the second ring being adjacent to the first ring, the third point having a third azimuth angle between the first azimuth angle and the second azimuth angle; and create one of the planes including the first point, the second point, and the third point.

30. The system of claim 27, further comprising executable code including computer instructions that: substitute a default plane when no ground plane can be determined.

31. The system of claim 27, further comprising executable code including computer instructions that: remove a point from the point cloud data if the point is more than a preselected distance from the autonomous vehicle.

32. The system of claim 27, further comprising executable code including computer instructions that: remove a point from the point cloud data if the point is more than a preselected height based at least on a vehicle height of the autonomous vehicle.

33. The system of claim 27, further comprising executable code including computer instructions that: remove a point from the point cloud data if the point is within a preselected distance from the autonomous vehicle.

34. The system of claim 27, wherein, transforming the ground plane includes: executable code including computer instructions that: compute a unit vector from coefficients of the ground plane equation, the ground plane equation including ax + by + cz + d = 0, the coefficients including a, b, and c, and the constant including d; normalize the d constant; transform the a, b, c coefficients of the ground plane equation based on a rotation / conversion matrix and the unit vector; and transform the normalized d constant based on the normalized d constant, the rotation / conversion matrix, the unit vector, and the transformed a, b, c coefficients, 。 35. The method of claim 19, wherein, the determining the confidence level of the estimate includes: assess a number and an orientation of planar features in a direction of travel of the autonomous vehicle.

Images