System and method for locating autonomous vehicles

Abstract

A system and method for locating an autonomous vehicle using mapped data and real-time data. The mapped data and real-time data are scan-matched. Autonomous vehicle characteristics such as linear and angular velocity, direction of travel, and motion prediction are provided to a Bayesian estimation algorithm, but are not limited to, and the final attitude is calculated.

Description

Technical Field

[0001] This disclosure generally relates to localization. More specifically, this disclosure relates to localizing autonomous vehicles.

[0002] An autonomous vehicle needs to understand its surroundings in order to determine its position. Accurate localization of an autonomous vehicle is necessary, among other reasons, to prevent collisions between the autonomous vehicle and something on its path. Current high-precision localization systems employ an expensive set of sensors that also have stringent computational and power requirements. Inexpensive systems may sacrifice accuracy in complex environments by making assumptions about the environment that simplify data collection and reduce processing demands. GPS data can be used to provide basic navigation information such as turn-by-turn instructions, but does not function when vehicle position accuracy lower than that of GPS is required. Map-based localization, which enables localization of a vehicle relative to a high-resolution image-based map, is an alternative to GPS-based localization. This technique requires a large amount of storage for the map. The most common uses of localization include localization for knowledge of which road an autonomous vehicle is traveling on (road-level localization), localization for the lateral and longitudinal position of an autonomous vehicle in its host lane (ego-lane level localization), and localization for knowledge of the lane on which an autonomous vehicle is traveling and the lateral position of the autonomous vehicle on the road (lane-level localization). There are solutions available for each of these uses.

[0003] For example, road-level localization solutions include map matching methods that further include deterministic and probabilistic models. Deterministic models include geometric models and pattern-based models. Probabilistic models include hidden Markov models, conditional random fields, weighted graphs, and multiple hypotheses. Deterministic models cannot handle uncertainty and ambiguity. Probabilistic models require more computation than deterministic models, which can delay timely results and create power consumption problems that must be overcome. For some applications, road-level localization is insufficient to maintain vehicle safety. Obstacle avoidance and overtaking are two examples where road-level localization may be inadequate.

[0004] Ego lane-level localization involves two common methods: modeling and learning. In modeling, preprocessing is applied to frames of data collected by, for example, LiDAR and cameras, to highlight features of interest, reduce clutter such as shadows, and remove inappropriate artifacts. After preprocessing, what remains is data containing lane markings that are distinguished from the rest of the data during feature extraction. Feature extraction may involve filters based on gradient detection, non-vertical edge detection, or reflectance. Following feature extraction, a fitting procedure presents a high-level representation of the travel path using the lane model. Various types of lane models exist, including parametric models (assuming a specific 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 procedure is a tracking procedure, where tracking from previous data frames improves knowledge 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 for land detection, training of lightweight lane detection CNNs by self-attentional distillation toward lightweight lane detection by optimizing spatial embeddings, and top-down lane detection frameworks based on conditional convolution. In benchmark testing, the accuracy of these models depends on which benchmark database is used.

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

[0006] What is needed is a generalized location system that conserves resources and provides timely information about the location of autonomous delivery vehicles. [Prior art documents] [Non-patent literature]

[0007] [Non-Patent Document 1] Milstein et al., Localization with Dynamic Motion Models, International Conference on Informatics in Control, Automation, and Robotics (ICINCO), 2006 [Non-Patent Document 2] Montella, C., The Kalman Filter and Related Algorithms: A Literature Review, https: / / www.researchgate.net / publication / 236897001, May 2011. [Non-Patent Document 3] Wan et al., The Unscented Kalman Filter, Kalman Filtering and Neural Networks, Chapter 7, Haykin ed., John Wiley & Sons, Inc., October 2001 [Non-Patent Document 4] 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) [Overview of the project] [Means for solving the problem]

[0008] The system of this teaching for providing continuous localization of an autonomous vehicle includes at least one processor running in at least one autonomous vehicle. The method of this teaching includes a lane-level localization process in which at least one processor receives, processes, and provides offline (map) data and receives and processes real-time (sensor) data. The map data and sensor data may be collected by the same vehicle but during different navigation time periods. Sensors such as LIDAR devices, GPS devices, and optical devices, but not limited to, provide point cloud data, geographic data, stereo data, and monocular data. Other types of sensors and data are assumed by this teaching. During the localization process, previously collected map data is accessed when real-time data is collected. Data related to the current position of the autonomous vehicle as determined by GPS is separated. One or more computer systems can be configured to perform certain operations or actions by having software, firmware, hardware, or a combination thereof installed on the system that causes the system to perform actions when it is operating. One or more computer programs can be configured to perform a specific operation or action by including instructions that cause the data processing device to perform an action when executed by the device. One common embodiment includes a method for locating an autonomous vehicle.The method also includes the steps of: organizing a first map associated with the current position of an autonomous vehicle to form current organized data; organizing a second map associated with at least one potential position that the autonomous vehicle can navigate to to form potential position organized data; updating the current organized data as the autonomous vehicle navigates, based at least on the potential position organized data and the current position; selectively updating the potential position organized data based at least on the autonomous vehicle's direction of movement, speed of movement, potential position organized data and the current position; filtering real-time data received by the autonomous vehicle to form real-time data; scan matching the processed current organized data and real-time data to form matched map data based at least on dynamic thresholds; removing outlier data from the matched map points based on feature characteristics, dynamic thresholds and an outlier detection algorithm to form an attitude estimate; and correcting the attitude estimate based at least on planar features associated with the current position to form a localized attitude. Other embodiments of this model include a corresponding computer system, apparatus, and a computer program recorded on one or more computer storage devices, each configured to perform the actions of the Method.

[0009] The implementation may include one or more of the following features: The method may include the step of receiving a first map from a remote source to the autonomous vehicle. The method may include the step of accessing the first map from a database stored locally in the autonomous vehicle. The first map may include an 80-meter square. The method may include the step of receiving at least one second map from a remote source to the autonomous vehicle. The method may include the step of accessing at least one second map from a database stored locally in the autonomous vehicle. The second map may include at least one 80-meter square that shares a boundary with the first map. The selective updating step may include the step of determining whether the autonomous vehicle is located in a boundary area between the first map and at least one second map, and updating organized data of potential locations if the autonomous vehicle is navigating outside the boundary area and the first map. The boundary area may include a width that is dynamically determined based at least on the speed of the autonomous vehicle, the number and type of obstacles surrounding the autonomous vehicle, and / or the characteristics of the environment surrounding the autonomous vehicle. The step of filtering real-time data may include the step of downsampling the 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 default criteria. The pre-selected criteria may include dynamically determined criteria. The pre-selected criteria may include pre-selected densities. The method may include the step of dividing the downsampled real-time data into blocks of a predefined size. The method may include the step of forming a real-time plane that has grown from multiple scans of the downsampled real-time data to outliers. The method may include the step of determining discontinuities in the real-time plane and removing points that are part of the discontinuities. The method may include the step of dividing a first map and at least one second map into planar points and non-planar points. The method may include the step of removing non-planar points. The method may include the step of identifying planar points as points belonging to a non-ground plane.The method may include a step of identifying planar points, which includes (a) selecting a random point from downsampled data, (b) locating neighbors of the random point, (c) identifying planar points in the downsampled data as points on a planar plane formed by the random point and, if any, neighbors, (d) identifying non-planar points as not being planar points, and (e) repeating steps (a) to (d) until there are no more downsampled points to examine, or until a pre-selected number of planar planes are achieved, or until steps (a) to (d) have been repeated a pre-selected number of times. The method may include a step of forming a real-time plane grown to outliers from multiple scans of downsampled real-time data, and a step of matching the real-time plane with a planar plane. The step of organizing a first map may include a step of forming a k-dimensional tree from the first map. The step of organizing a first map may include a step of forming multiple k-dimensional trees from the first map and at least one second map by parallel processors. The step of organizing a first map data and at least one second map may include the step of forming multiple k-dimensional trees from the first map data and at least one second map. Implementations of the described technique may include hardware, methods or processes, or computer software on a computer-accessible medium.

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

[0011] The implementation may include one or more of the following features: The method may include the step of collecting offline data and real-time data over different time periods. The method may include the step of collecting offline data and real-time data by a common vehicle. The method may include the step of collecting real-time data by sensors including a LIDAR device, a GPS device, and an optical device. The step of calculating local attitude information may include the step of measuring the linear velocity and angular velocity of the autonomous vehicle, the step of measuring the local direction of travel of the autonomous vehicle, the step of estimating the image attitude of the autonomous vehicle based on at least the image in front of and the current image of the autonomous vehicle using data collected by an image sensor, the step of estimating the laser attitude of the autonomous vehicle based on at least the attitude in front of and the current attitude of the autonomous vehicle using data collected by a laser sensor, and the step of calculating the local attitude based on at least the linear velocity and angular velocity, the local direction of travel, the estimated image attitude, and the estimated laser attitude. The model may include an autonomous vehicle at a constant speed. The model may include a motion model for the autonomous vehicle. The model may include a dynamically updated motion model for autonomous vehicles. The step of continuously calculating the final attitude may include feeding the global attitude, local attitude, and future characteristics into a Bayesian state estimator. The Bayesian estimator may include an unscented Kalman filter. Implementations of the described techniques may include hardware, methods or processes, or computer software on a computer-accessible medium.

[0012] One general embodiment includes a system for locating an autonomous vehicle. The system also includes 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 sensor data features of interest in the filtered sensor data from map data features of interest in the filtered map data; a third processor configured to match sensor data features of interest to map data features of interest; a fourth processor configured to perform alignment on the matched data and create a global attitude; a fifth processor to collect local attitude information based at least on the linear and angular velocities of the autonomous vehicle, the direction of travel of the autonomous vehicle, image attitude estimation of the autonomous vehicle, and laser attitude estimation of the autonomous vehicle; a sixth processor to predict the future movement of the autonomous vehicle; and a seventh processor to calculate the final attitude of the autonomous vehicle based at least on the global attitude, local attitude information, and future movement. Other embodiments for the body include a corresponding computer system, an apparatus, and a computer program recorded on one or more computer storage devices, each configured to perform the actions of the method.

[0013] The implementation may include one or more of the following features: a system in which sensor data may include LIDAR data, GPS data, and image data; alignment may include an iterative nearest neighbor algorithm; and predicting future movement may include running a model configured to represent the behavior of an autonomous vehicle. The first processor may include instructions to filter 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 the neighbors of the random point, (e) identify planar points in the downsampled data as points on a planar plane formed by the random point and, if any, neighbors, (f) identify non-planar points as not being planar points, and (g) repeat steps (a) to (f) until there are no more downsampled points to examine, or until a pre-selected number of planar planes are achieved, or until steps (a) to (d) have been repeated a pre-selected number of times. The first processor is a ground plane processor that determines a ground plane from point cloud data received from a sensor, and each ground plane is associated with a ground plane equation, and the sensor has a sensor reference system, and may include a ground plane processor and a plane transformation processor that converts the ground plane equation from the sensor reference system to a vehicle reference system associated with an autonomous vehicle.The ground plane processor may include a median processor that calculates the median of at least two rings of point cloud data; a point cloud filter that filters the point cloud data based at least on the distance of points in the point cloud data from the median; a plane creation processor that creates a plane from the filtered point cloud data, wherein each of the created planes has at least one azimuth angle; a plane growth processor that grows the plane formed from the point cloud data, extending it in a direction away from the autonomous vehicle along at least one azimuth angle to form a grown plane; and a selection processor that selects a ground plane from the grown plane based at least on the orientation of the created plane and residual error. The plane creation processor may include executable code that includes a computer instruction for selecting a first point and a second point from a first ring of sensor data, wherein the first and second points are located within a boundary formed by discontinuities in the point cloud data on the first ring, the first point has a first azimuth angle, and the second point has a second azimuth angle; a computer instruction for selecting a third point from a second ring of sensor data, wherein the second ring is adjacent to the first ring, and the third point has a third azimuth angle between the first and second azimuth angles; and a computer instruction for creating one of the planes containing the first point, the second point, and the third point. The system may include executable code that includes a computer instruction to use a default plane if no ground plane can be determined. The system may include executable code that includes a computer instruction 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 that includes a computer instruction to remove a point from point cloud data if the point exceeds a pre-selected height, at least based on the vehicle height of the autonomous vehicle. The system may also include executable code that includes a computer instruction 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 comprising: a computer instruction for calculating a unity vector from the coefficients of the ground plane equation, wherein the ground plane equation comprises ax+by+cz+d=0, the coefficients a, b, and c, and the constant d; a computer instruction for transforming the a, b, and c coefficients of the ground plane equation based on a rotation / translation sequence and a unity vector; a normalized constant d; and a computer instruction for transforming the normalized constant d based on a rotation / translation sequence, a unity vector, and the transformed a, b, and c coefficients. Implementations of the described technique may include hardware, methods or processes, or computer software on a computer-accessible medium.

[0014] Non-exclusive and non-exclusive aspects of this disclosure will be described with reference to the following figures, in which, unless otherwise specified, the same reference number refers to the same part. [Brief explanation of the drawing]

[0015] [Figure 1] This is a schematic block diagram of the environment in which the system described in this instruction will be executed. [Figure 1A] This is a diagram illustrating the graphical representation of point cloud data captured by the system described in this instruction. [Figure 1B] This is a mosaic diagram showing the orientation of Figures 1H and 1I. [Figure 1C] This is a diagram illustrating the data block structure of this instruction. [Figure 1D] This is a diagram showing a graphical representation of the geometric description of the odometry and inertial measurement units. [Figure 1E] This is a diagram showing a graphical representation of the geometric description of the odometry and inertial measurement units. [Figure 1F] This is a schematic block diagram of the example system shown in this instruction. [Figure 1G] This is a schematic block diagram of the example system shown in this instruction. [Figure 1H] This is a diagram illustrating the data block structure of this instruction. [Figure 1I] It is a diagram of a schematic representation of the data block structure of the present teachings. [Figure 2A] It is a flowchart showing the flow and steps according to various aspects of the present teachings. [Figure 2B] It is a flowchart showing the flow and steps according to various aspects of the present teachings. [Figure 3A] It is a diagram of a schematic representation of a system and method for offline data collection of the present teachings. [Figure 3B] It is a diagram of a schematic representation of a system and method for offline data collection of the present teachings. [Figure 3C] It is a diagram of a schematic representation of a system and method for offline data collection of the present teachings. [Figure 4A] It is a flowchart of an overview of real-time data collection of the present teachings. [Figure 4B] It is a diagram of a schematic representation of the process shown in the flowchart of FIG. 4A. [Figure 4C] It is a diagram of a schematic representation of the process shown in the flowchart of FIG. 4A. [Figure 4D] It is a diagram of a schematic representation of the process shown in the flowchart of FIG. 4A. [Figure 4E] It is a diagram of a schematic representation of the process shown in the flowchart of FIG. 4A. [Figure 4F] It is a diagram of a schematic representation of the process shown in the flowchart of FIG. 4A. [Figure 4G] It is a diagram of a schematic representation of the process shown in the flowchart of FIG. 4A. [Figure 4H] It is a diagram of a schematic representation of the process shown in the flowchart of FIG. 4A. [Figure 4I] It is a diagram of a schematic representation of the process shown in the flowchart of FIG. 4A. [Figure 5] It is a schematic block diagram of an exemplary multiprocessor system of the present teachings. [Figure 5A] It is a diagram of a schematic representation of the enhancement of k-d tree search of the present teachings. [Figure 5B]This is a diagram illustrating the outlier removal strategy described in this instruction. [Figure 5C] This is a diagram illustrating the outlier removal strategy described in this instruction. [Figure 5D] This is a diagram illustrating the outlier removal strategy described in this instruction. [Figure 6A] This is a diagram illustrating the posture correction described in this instruction. [Figure 6B] This is a diagram illustrating the posture correction described in this instruction. [Figure 7] This is a flowchart of the first exemplary configuration of the method described in this instruction. [Figure 8] This is a flowchart of the second exemplary configuration of the method described in this instruction. [Modes for carrying out the invention]

[0016] In the following description, many specific details are provided to give a full understanding of the various embodiments and arrangements. However, those skilled in the art will understand that the techniques described herein can be carried out without one or more of the specific details, or using other methods, components, materials, etc. In other examples, well-known structures, materials, or operations may not be shown or described in detail to avoid obscuring certain embodiments.

[0017] Throughout this specification, references to “aspects,” “arrangements,” or “configurations” indicate that a particular feature, structure, or characteristic is being described. Therefore, occurrences of “in one aspect,” “in one embodiment,” or “in one configuration” in various places throughout this specification do not necessarily refer to the same aspect, feature, configuration, or arrangement. Furthermore, specific features, structures, and / or characteristics may be combined in any suitable manner.

[0018] To the extent used in this disclosure and the claims, terms such as “component,” “system,” “platform,” “layer,” “selector,” and “interface” are intended to refer to computer-related entities or entities relating to operating devices having one or more specific functions, which may be hardware, a combination of hardware and software, software, or running software. For example, a component may be, but is not limited to, a process running on a processor, a processor, an object, an executable file, a thread of execution, a program, and / or a computer. Exemplarily, and not limited to, an application running on a server and the server itself may both be components. One or more components may reside within a process and / or a thread of execution, a component may be localized on one computer, and / or distributed across two or more computers. In addition, a component may run from various computer-readable media, device-readable storage devices, or machine-readable media in which various data components are stored. Components can communicate via local and / or remote processes, for example, by following signals that have one or more data packets (e.g., data from one component interacting with another component in a local system, interacting with a distributed system, and / or interacting with other systems via a network such as the Internet). As another example, a component can be a device having a specific function provided by mechanical parts operated by electrical or electronic circuits that can be operated by software or firmware applications executed by a processor, the processor being inside or outside the device and capable of executing at least part of the software or firmware application. As yet another example, a component can be a device providing a specific function via electronic components without mechanical parts, the electronic components may contain internally a processor that runs software or firmware that gives at least part of the electronic component's function.

[0019] To the extent used in this disclosure and the claims, terms such as “store,” “storage,” “datastore,” “data storage,” and “database” refer to memory components, entities embodied in memory, or components comprising memory. It will be understood that the memory components described herein may be either volatile memory or non-volatile memory, or may include both volatile and non-volatile memory.

[0020] In addition, the term “or” is intended to mean an inclusive “or,” not an exclusive “or.” That is, unless otherwise specified or it is clear from the context, “X adopts A or B” is intended to mean any of the natural inclusive permutations. That is, if X adopts A, if X adopts B, or if X adopts both A and B, “X adopts A or B” satisfies any of the above examples. Furthermore, the articles “a” and “an” used in this disclosure and claims should generally be interpreted as meaning “one or more,” unless otherwise specified or it is clear from the context that they refer to a singular form.

[0021] The terms “exemplary” and / or “exemplary” mean that they serve as examples, cases, or illustrations. To avoid doubt, the subject matter disclosed herein is not limited by the examples disclosed. In addition, any aspect or design described herein as “exemplary” and / or “exemplary” should not necessarily be construed as being preferable or advantageous to other aspects or designs, nor should it be meant to exclude equivalent structures and techniques known to those skilled in the art. Furthermore, to the extent that the terms “include,” “have,” “contain,” and other similar words are used in the detailed description or claims, such terms are intended to be as comprehensive as the term “equipment” as an open transition word, without excluding any additional or other elements.

[0022] As used herein, the terms “infer” or “infer” generally refer to the process of inferring about a system, environment, user, and / or intent, or the state of those systems, environments, users, and / or intentions, from a set of observations captured through events and / or data. Captured data and events may include user data, device data, environmental data, sensor data, application data, implicit data, explicit data, etc. Inferences can be used to identify a particular context or action, or, for example, to generate a probability distribution over states of interest based on considerations of data and events.

[0023] The disclosed subject matter can be implemented as a method, apparatus, or product by using standard programming and / or engineering techniques to generate software, firmware, hardware, or combination thereof that controls a computer to implement the disclosed subject matter. To the extent used herein, the term “product” is intended to encompass any computer-readable device, machine-readable device, computer-readable carrier, computer-readable medium, or computer program accessible from a machine-readable medium. For example, computer-readable medium may include, but is not limited to, magnetic storage devices such as hard disks, floppy disks, magnetic strips, optical discs (e.g., compact discs (CDs), digital versatile discs (DVDs), Blu-ray® discs (BDs)), smart cards, flash memory devices (e.g., cards, sticks, key drives), virtual devices that emulate storage devices, and / or any combination of the computer-readable media described above.

[0024] Generally, a program module includes routines, programs, components, data structures, etc., that perform a specific task or implement a specific abstract data type. The illustrated embodiments of this disclosure may be implemented in a distributed computing environment in which a specific task is performed by remote processing devices linked over a communication network. In a distributed computing environment, program modules may reside on both local and remote memory storage devices.

[0025] A computing device may include at least computer-readable storage media, machine-readable storage media, and / or communication media. Computer-readable or machine-readable storage media may be any available storage media accessible by a computer and may include both volatile and non-volatile memory, removable and non-removable media. For example, but not limited to, computer-readable or machine-readable storage media may be implemented in combination with any method or technique for storing information, such as computer-readable or machine-readable instructions, program modules, structured data, or unstructured data.

[0026] Computer-readable storage media may include, but are not limited to, random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technologies, compact disc read-only memory (CD-ROM), digital versatile disc (DVD), Blu-ray® disc (BD), or other optical disc storage, magnetic cassette, magnetic tape, magnetic disk storage, or other magnetic storage devices, solid-state drives or other solid-state storage devices, or other tangible and / or non-temporary media that can be used to store desired information. In this regard, the terms “temporary” or “non-temporary” as applied herein to storage, memory, or computer-readable media should be understood to exclude only the transient signals themselves as modifiers, and not any standard storage, memory, or computer-readable media beyond the transient signals themselves.

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

[0028] The system bus used herein may be any of several types of bus structures that can further interconnect with a memory bus (with or without a memory controller), peripheral buses, and local buses using any of various commercially available bus architectures. The database used herein may include a basic input / output system (BIOS) that can be stored in non-volatile memory such as ROM, EPROM, or EEPROM, and the BIOS includes basic routines that help transfer information between elements of the computer, such as at startup. RAM may also include high-speed RAM such as static RAM for caching data.

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

[0030] 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 can facilitate wired and / or wireless communication to the LAN, and the LAN may also include wireless access points (APs) located on the LAN for communication with the adapter in wireless mode.

[0031] When used in a WAN networking environment, the computer may include a modem or connect to a communication server on the WAN via other means for establishing communication over the WAN, such as via the Internet. The modem, which may be internal or external and may be a wired or wireless device, may be connected to the system bus via an input device interface. In a network environment, program modules, as shown herein in relation to the computer or a part thereof, may be stored in a remote memory / storage device.

[0032] When used in a LAN or WAN networking environment, a computer can access cloud storage systems or other network-based storage systems in addition to, or instead of, external storage devices. Generally, the connection between the computer and the cloud storage system can be established via LAN or WAN, for example, through an adapter or modem, respectively. Once the computer is connected to the relevant cloud storage system, the external storage interface, with the help of the adapter and / or modem, can manage the storage provided by the cloud storage system, just like other types of external storage. For example, the external storage interface can be configured to provide access to cloud storage sources as if those sources were physically connected to the computer.

[0033] As used herein, the term “processor” can refer to substantially any computing unit or device comprising a single-core processor, a single-core processor with software multithreading capability, a multi-core processor, a multi-core processor with software multithreading capability, a multi-core processor with hardware multithreading technology, a vector processor, a pipeline processor, a parallel platform, or a parallel platform with distributed shared memory. Additionally, a processor can refer to an integrated circuit, an application-specific integrated circuit (ASIC), a digital signal processor (DPS), a field-programmable gate array (FPGA), a programmable logic controller (PLC), a composite programmable logic device (CPLD), a state machine, discrete gate or transistor logic, discrete hardware components, or any combination thereof configured to perform the functions described herein. A processor can utilize nanoscale architectures, such as molecular and quantum dot-based transistors, switches, and gates, to optimize space utilization or enhance the performance of user equipment, but is not limited to these. A processor can also be implemented as a combination of computing units. For example, a processor can be implemented as one or more processors arranged together, tightly coupled, loosely coupled, or at a distance from each other. Multiple processing chips or multiple devices may share the performance of one or more functions described herein, and similarly, storage may be performed across multiple devices.

[0034] In summary, various configurations are described herein. For the sake of simplicity, methods (or algorithms) are illustrated and described as a series of steps or actions. It should be understood that the various configurations are not limited by the illustrated actions and / or the order of actions. For example, actions may occur in various orders and / or simultaneously, and with other actions not presented or described herein. Furthermore, not all illustrated actions are required to carry out a method. In addition, a method can alternatively be represented as a series of interrelated states via a state diagram or events. Moreover, methods described later may be stored on a product (e.g., a machine-readable storage medium) to facilitate the transfer and transmission of such methods to a computer.

[0035] One or more computer systems can be configured to perform a specific operation or action by having software, firmware, hardware, or a combination thereof installed on the system that causes the system to perform an action when it is running. One or more computer programs can be configured to perform a specific operation or action by including instructions that cause the data processing device to perform an action when it is executed by the device.

[0036] Referring here to Figure 1, the positioning of the autonomous vehicle is achieved in a broader context and for the purpose of safely navigating the autonomous vehicle in an environment that includes obstacles. The environment can include both vehicular traffic when the autonomous vehicle is navigating a roadway and pedestrian traffic when the autonomous vehicle is navigating sidewalks and other pedestrian paths such as bicycle lanes / pedestrian lanes. As the autonomous vehicle navigates, it receives sensor data 501 from sensors mounted on the autonomous vehicle and other locations such as, but not limited to, the shoulder, beacons, traffic signals, other vehicles, satellites, and other aircraft sensors. This data is provided to both the positioning logic 503 and other autonomous logic 507. Additionally, a map of the autonomous vehicle's area is created for the autonomous vehicle to position itself (505). The result of executing the logic is the movement of the autonomous vehicle to its destination, avoiding obstacles and navigating through pedestrian and vehicular traffic.

[0037] The system and method of this teaching includes a processor and process for real-time localization of an autonomous vehicle. The system of this teaching for providing continuous localization of an autonomous vehicle includes at least one processor running in at least one autonomous vehicle. The method of this teaching includes (1) a lane-level localization process in which at least one processor receives, processes, and provides offline (map) data and receives and processes real-time (sensor) data. The map data and sensor data may be collected by the same vehicle but during different navigation time periods. Sensors such as LIDAR devices, GPS devices, and optical devices provide point cloud data, geographic data, stereo data, and monocular data. Other types of sensors and data are assumed by this teaching. During the localization process, previously collected map data is accessed as real-time data is collected. (2) Data related to the current position of the autonomous vehicle as determined by GPS is isolated, and (3) a matching between the map and real-time sensor data is calculated to determine the attitude, referred to herein as global attitude, in addition to a measure of attitude uncertainty within the map frame. As the autonomous vehicle navigates, (4) a local attitude, which is a comparison between the current attitude and the previous attitude, is continuously calculated. Furthermore, (5) a model of the autonomous vehicle's motion is run to provide predictions of the autonomous vehicle's future characteristics. The global attitude, local attitude, and model characteristics are used to (6) compute a real-time estimate of the autonomous vehicle's attitude. The real-time estimate of attitude is determined by a method that propagates changing measurements taken over time by the autonomous vehicle to predict the autonomous vehicle's subsequent state and the uncertainty of that prediction. As new measurements are received, the method updates the estimate and uncertainty. (7) The real-time estimate of attitude is corrected based on available features in the input data.

[0038] Referring here to Figure 1A, in one embodiment, with respect to (1), the feature-based map includes a ground map and a planar map. The feature map is created, for example, from LIDAR data. Figure 1A shows raw point cloud data point 4005 and the classification of the point cloud points into ground point 4001 and planar point 4003. The non-feature-based map includes raw data that has been uniformly downsampled to a pre-selected density. The criteria for downsampling can be user-defined, default, and / or dynamically determined. Once the raw data, ground, and planar point cloud data are collected, they are divided into blocks of a pre-selected size and then provided to an autonomous vehicle, where the data becomes available for the localization process of this teaching.

[0039] The map includes ground point 4001 (non-planar point). The system and method of this instruction selects ground point 4001 from planar points 4003 in the point cloud. Ground point 4001 is determined by localizing points in the ground plane from the raw map. In one embodiment, the requirements related to identifying the ground plane are user-defined. For example, extracted planes whose normals are nearly perpendicular within a predefined threshold are labeled as ground planes. In one embodiment, the requirements are determined dynamically based on input sensor data and the characteristics of the environment surrounding the autonomous vehicle. In one embodiment, the system includes a dataset of possible ground plane characteristics, which may, for example, depend on the environment and / or geographic location. The ground planes are fitted together, and once all ground points within the ground plane are identified, they are removed from the raw data so that only planar points 4003 are considered in the localization process. In one embodiment, non-planar features are determined from downsampled point cloud data. In one embodiment, downsampling is performed by pre-selecting a point density, cutting the point cloud data region into pre-selected sizes, pre-selecting how many points each pre-selected size region will contain, and then downsampling to that number of points.

[0040] The map includes plane points 4003. Plane points 4003 are determined by localizing points within a non-ground plane. The criteria for selecting a plane point are user-defined, default, and / or dynamically determined. In one embodiment, planar features such as walls and other non-road surfaces are recognized according to pre-determined criteria. In offline data, plane point recognition is performed by selecting a random point from the LIDAR point cloud data, localizing its nearest neighbor, and determining which points are on the plane from the point cloud data if the point extends across the plane. The system iteratively runs this process until the process reaches a threshold based on pre-selected termination criteria. Pre-selected criteria include, but are not limited to, that a pre-selected number of planes have been found, that there are not enough points remaining to run the process, or that the maximum number of iterations has been reached. 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 used to form planes that have grown to include outliers. Filtering includes excluding points that are part of discontinuities within the plane. To further engage in matching with planes from offline map data, non-road-grown planes are selected.

[0041] Referring here to Figures 1B, 1C, 1H, and 1I, with respect to (2) to (7), the system and method of this instruction perform alignment between offline data and real-time data. Furthermore, the map data surrounding the autonomous vehicle is divided into blocks of a pre-selected size. Instead of processing all data at once, only the data closest to the autonomous vehicle and the data in the area in the direction the autonomous vehicle is navigating (which may be the same) are processed.

[0042] Referring here to Figures 1B, 1C, 1H, and 1I, in one embodiment, with respect to (2), data related to the position of the autonomous vehicle is provided to compute localization. In one embodiment, the pre-selected region includes a geometric shape, e.g., a circle, rectangle, square, or ellipse, with the autonomous vehicle at its center 14011. In one embodiment, the data within the current pre-selected region is divided into blocks. In one embodiment, nine blocks of a predetermined amount of data surrounding the autonomous vehicle are provided for processing in the autonomous vehicle, including a block of data where the autonomous vehicle is located and eight blocks of data corresponding to the possible directions in which the autonomous vehicle may move. The block where the autonomous vehicle is located 4011 is used to construct a data structure in a multidimensional space that enables efficient storage of spatial data, range exploration, 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 surrounding the autonomous vehicle. The data structure is also constructed using data from adjacent blocks that the autonomous vehicle can then move to, for example, blocks 4013, 4015, and 4017. When the autonomous vehicle finds itself at the boundary between adjacent blocks, the new data structure is accessed, and a new set of data structures is created from the adjacent blocks of data.

[0043] Continuing with (3) to (7), referring to Figure 1C, when an autonomous vehicle navigates near and on the boundary 4019 between blocks, loading new data from an adjacent block can cause thrashing between data blocks. Furthermore, if the autonomous vehicle is located near the corner of the block it is navigating, it may navigate into any of the three blocks that form the adjacent block at the corner. To address this possibility, a boundary region 4021 between blocks of a pre-selected thickness is specified. The autonomous vehicle can travel within the boundary region 4021 without triggering data transfers of adjacent data blocks. When the autonomous vehicle leaves the boundary region 4021, data transfers occur. In one embodiment, the width of the boundary region 4021 is user-defined. In one embodiment, the width of the boundary region 4021 may be dynamically determined based on the speed of the autonomous vehicle, the number and type of obstacles surrounding the autonomous vehicle, and / or other aspects of the autonomous vehicle's environment. In one embodiment, the width of the boundary region 4021 is selected based on a pre-selected recipe or algorithm.

[0044] With respect to (4), referring to Figures 1D and 1E, the local attitude is calculated based on data collected in real time by the autonomous vehicle according to this technique. Such data may include, but are not limited to, wheel odometry data, inertial measurement unit (IMU) data, image attitude estimation data, and LiDAR attitude estimation data. In one embodiment, as shown in Figure 1D, wheel odometry is provided by a wheel encoder. The wheel encoder measures the angular displacement of each wheel. In one embodiment, the encoder measurement assumes that the angular displacement occurs at a constant rate during the sampling period, and therefore the encoder measurement provides the angular velocity. Scaling the angular velocity by the radius of the wheel converts the wheel encoder into a wheel linear velocity sensor.

[0045]

number

[0046] These measurements are converted into measurements of forward linear velocity and yaw angular velocity.

[0047]

number

[0048] The measured values ​​are predicted as follows:

[0049]

number

[0050] Addition and subtraction are,

[0051]

number

[0052] Give . Divide by 2 and v left and v right Substitute this into the definition.

[0053]

number

[0054] In one embodiment, as shown in Figure 1E, the IMU includes a magnetometer that returns the local direction of travel relative to magnetic north. The measurement of the local direction of travel relative to the inertial frame is shown in Figure 1E. The equation for predicting the direction of travel measurement is as follows:

[0055]

number

[0056] matrix

[0057]

number

[0058] This converts the reference frame associated with the magnetometer to a wheelbase reference frame. In one embodiment, image attitude estimation is based on previous and current images collected by sensors mounted on the autonomous vehicle. In one embodiment, the sensors are placed in other locations, for example, but not limited to, beacons, other autonomous vehicles, manned vehicles, traffic signals, and handheld devices. In one embodiment, LIDAR attitude estimation results from a comparison between the current attitude and the previous attitude provided by LIDAR measurements. In one embodiment, two vectors are compared. [[When using the following formulas, it is necessary to define the variables]]

[0059]

number

[0060] Here,

[0061]

number

[0062] This is a copy of the previous state vector for which LIDAR data was available. In one embodiment, the GPS sensor,

[0063]

number

[0064] and

[0065]

number

[0066] It provides direct measurement.

[0067]

number

[0068] Preferably, the present invention also utilizes other sensors, not limited to IMUs and wheel encoders. The IMU provides information on the orientation and angular velocity of the autonomous vehicle. The wheel encoder provides information on the linear and angular velocity of the autonomous vehicle. These measurements are used by the UKF to estimate the internal state.

[0069] Regarding (5), in one embodiment, a method is needed to estimate the motion of the autonomous vehicle in order to estimate its attitude from the data described above. One way to predict motion is to assume constant behavior. Another way is to create a general motion model of the vehicle, and yet another way is to create a motion model specific to the vehicle. The model can be dynamically updated as the autonomous vehicle encounters various situations, such as different types of road surfaces and terrains, or various characteristics of the vehicle itself, such as load weight. Yet another way to predict motion is to use the localization results themselves to determine modified motion parameters that reflect the motion of the autonomous vehicle. One such technique is described by Milstein et al., Localization with Dynamic Motion Models, International Conference on Informatics in Control, Automation, and Robotics (ICINCO), 2006.

[0070] With respect to (6), in one embodiment, global attitude, local attitude, and model output data are supplied to a method for estimating the final attitude. Exemplary methods that provide such estimations fall into 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 implementation forms of UKF 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 accommodates various sampling rates.

[0071] Referring individually to Figures 1F and 1G, the components of an exemplary configuration of the system of this instruction are shown. In one embodiment, all components are housed within the autonomous vehicle. In one embodiment, 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 position of the autonomous vehicle within a pre-selected area. In one embodiment, data within the pre-selected area is collected offline, 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 this data, as well as other data, are used to calculate and correct attitude.

[0072] Continuing with the references to Figures 1F and 1G, the sensor provides data to the sensor processor 101. In one embodiment, mapping data (offline data) from data collection of the area of ​​interest is provided. In one embodiment, the sensor collects at least LIDAR data, images, and GPS data. The GPS data is used to locate the autonomous vehicle, among other methods, so that relevant offline data can be provided to the autonomous vehicle. Real-time LIDAR and image data are used, among other methods, in attitude calculation. The system of this teaching processes the offline data in the manner described in Figures 2A-2B and 3A-3C, and the real-time data in the manner described in Figures 4A-4I. These data are explored to perform feature extraction 105. Features of 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. Both datasets may span a much larger area than the area the autonomous vehicle is currently navigating. Due to these uses and / or other reasons, features from the two datasets are preprocessed 114 before being converted to global attitude. Preprocessing may include, for example, organizing the data according to conventional organization schemes such as kd trees for efficient matching. Outliers are removed, poses are estimated and corrected, and the process is repeated until the real-time point cloud converges to a map point cloud. In one embodiment, the transformation is performed using registration method 107, for example, an iterative nearest neighbor (ICP) algorithm having several variations, for example. Other techniques such as, for example, the Levenberg-Marquardt iteration, least squares rigid body transformation, and robust rigid body transformation are assumed by this teaching, for example, but are not limited.One description of an ICP implementation can be found in 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 variations that select sets of points in both databases, variations that match specific points in one dataset to the other, variations that weight corresponding pairs, variations that exclude specific pairs, variations that assign an error index, and variations that minimize the error index. The ICP variation selected to solve the localization problem of this technique is based, for example, on the type and characteristics of the sensor used, the desired accuracy of the results, and the desired computation speed.

[0073] Continuing with the references to Figures 1F and 1G, in addition to local attitude and model data, global attitude based on offline and real-time data is provided to the attitude estimator 109, which then generates the final attitude 111 after attitude correction. In one embodiment, local attitude data 113 may include, but are not limited to, wheel odometry data 13 (Figure 1G) and inertial measurement unit (IMU) data 112 (Figure 1G), as well as image attitude estimation data 134 (Figure 1G) and LiDAR attitude estimation data 136 (Figure 1G). In one embodiment, behavior and characteristic data of the autonomous vehicle can be provided to further improve the results. In one embodiment, data from model 117 (model information) can be provided as the autonomous vehicle navigates. In one embodiment, model 117 calculates characteristic data and represents the behavior of the autonomous vehicle based on the model algorithm.

[0074] Referring here to Figure 2A, a flowchart illustrating an exemplary process of offline data digestion is shown. Method 150 includes, but is not limited to, a step (151) of receiving data such as point cloud data as the autonomous vehicle navigates. Three-dimensional point cloud data can be obtained, for example, from stereo cameras, LIDAR sensors, roadside surround-view cameras, mobile laser scans, structure from motion, and / or aerial laser scans mounted on the autonomous vehicle or other locations. Method 150 includes a step (153) of uniformly downsampling the input data to generate final raw point cloud data exposed for access by a processor on the autonomous vehicle. Method 150 includes a step (141) of downsampling the input data and a step (155) of creating a submap of the remaining data points. The step of creating a submap includes a step of dividing the map of data points into subparts and then a step of individually analyzing each subpart in a loop including steps 157-199. If there are no more looping submaps (157) and no other submap sizes exist, method 150 includes a step of ending feature extraction (171) and a step of starting feature processing. If, in step 173, there are no more features to examine, method 150 includes performing a point-plane matching step 179. If, in step 173, there are more features to examine, and in step 175, the plane is a ground plane, method 150 includes a step 177 of downsampling the ground point cloud using a first set of parameters to create a final ground point cloud. If, in step 175, the plane is not a ground plane, method 150 includes a step 178 of downsampling the non-ground point cloud using a second set of parameters to create a final plane point cloud.

[0075] The map is divided into various submap sizes to take into account the planes between the submaps. If, in step 157, there are further looping submaps, method 150 includes step 159 to select a random point in the submap. If, in step 161, there are available points, method 150 includes step 163 to find the k nearest neighbors in the kd tree. If, in step 165, the neighbors span a plane, method 150 includes step 167 to locate a plane perpendicular to the spanned plane.

[0076] Referring here to Figure 2B, if the number of ground planes has reached a pre-selected threshold (183) and the plane normal satisfies the non-ground plane requirement (193), method 150 includes step 192 of tagging the plane as a non-ground plane. If the number of ground planes has not reached a pre-selected threshold (183) and the plane normal satisfies the ground plane requirement (184), method 150 includes step 185 of calculating the number of points on the spanned plane, which are referred to herein as inliers. If the number of inliers is greater than a pre-selected threshold (187) and the spanned plane belongs to the previous plane (189), i.e., the plane has been tagged as a ground plane, then to determine if the planes are the same, method 150 includes step 191 of merging all inliers and refitting the plane, i.e., including steps of creating a larger plane and calculating different normals. If the spanned plane does not belong to the previous plane (189), method 150 includes a step of replacing the plane in the ground plane if it overlaps. If the number of fitted planes reaches a pre-selected threshold (199), or the loop satisfies a pre-selected maximum number of iterations, or the number of remaining points is less than a pre-selected threshold, method 150 includes a step 197 of adding the points and plane normals from the ground plane list to the feature map and returning to checking another submap.

[0077] Referring here to Figures 3A-3C, a schematic representation of Method 150 (Figures 2A-2B) is shown. Specifically, the point cloud data 201 is downsampled in two ways to provide downsample #1 data 1203 (Figure 3A) and downsample #2 data 205 (Figure 3A). Downsample #1 data 1203 (Figure 3A) is later used in the matching process. Downsample #2 data 205 (Figure 3A) is divided into sub-parts, e.g., submaps 207A-207D (Figure 3A). A random point 209 (Figure 3B) is selected from the submap, and its k nearest neighbors 211 (Figure 3B) are determined. If the k nearest neighbors 211 (Figure 3B) and the random point span a plane 215 (Figure 3B) in submap 213 (Figure 3B), the number of points z in the spanned plane is counted and compared to a pre-selected minimum point threshold w. The normal plane 219 (Figure 3C) for the spanned plane 221 (Figure 3C) is determined. If the normal plane 219 (Figure 3C) satisfies the non-ground plane requirement and the total number of ground planes x is greater than the ground plane number threshold y, the feature map 225 (Figure 3C) is updated. If the normal plane 220 (Figure 3C) satisfies the ground plane requirement and the total number of ground planes x is less than the ground plane number threshold y, the feature map 225 (Figure 3C) is updated. If the number of points z is greater than the pre-selected minimum point threshold w and the spanned plane 221 (Figure 3C) is not in the ground plane list, the spanned plane is added to the ground plane list and the number of ground planes x is incremented to x+1. If the number of points z is greater than the pre-selected minimum point threshold w and the spanned plane 221 (Figure 3C) is in the ground plane list, all inliers are merged, the planes are refitted, and the feature map is updated. The offline map data is ready to enter the feature mapping process.

[0078] Referring here to Figure 4A, real-time data is collected to complete the localization process. Method 350 for localizing important planes in real-time data may, at a very high level, include, but are not limited to, the steps of: receiving point cloud data from a sensor 1351; filtering the data over the data median in one dimension 1353; creating a plane and growing the plane to outliers 355; selecting important planes 357; and providing the planes to feature extraction 105 (Figure 1F).

[0079] Referring here to Figure 4B, in one embodiment, when LIDAR point cloud data is collected (compared to other types of sensor data), the point cloud data can be received as a 1D string 303 of points along each LIDAR ring 301 surrounding 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 ring can be stored in azimuth order (Figure 4F), and the LIDAR rings can be stored sequentially in a row-first manner. Each ring can be divided into segments of a pre-selected size, for example, 64 points, but is not limited.

[0080] Referring here to Figure 4C, the 1D strings 303 on ring 301A / B / N (Figure 4B) can be filtered according to a process that may include, but is not limited to, filtering the 1D strings 303 around the median 307A / B of the points in each LIDAR336 data ring. Filtering may include locating points where the measured value is close to the median 307A / B and excluding the remaining points for this part 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 pass the median filter can be called points of the first class. Along the median 307A / B, discontinuity points 309A / B can be found within the point data. Discontinuity points 309A / B can be identified in any suitable way, for example, by calculating the Cartesian distance between points P1 and P2, comparing this distance to a first pre-selected threshold, and if the distance between points P1 and P2 is greater than the first pre-selected threshold, the data discontinuity point 309A / B or edge can be identified as a second class point. In some configurations, discontinuity points 309A / B are defined as abs(D2-D1)>0.08*NP*A This occurs in the following cases: Here, P1, P2 = consecutive points D1 = distance between P1 and the sensor D2 = distance between P2 and the sensor NP = number of points from the last good point A = (D1 / D2) / 2 P1 = Last good point P2 = Point being tested Points between discontinuities 309A / B located within space 311 are counted and can be labeled as points of a third class if the number of points exceeds a second pre-selected threshold. In some configurations, the second pre-selected threshold can include eight points. Points between pairs of discontinuities that do not exceed the second pre-selected threshold can be discarded.

[0081] Referring here to Figure 4D, critical planes are expected to fit the terrain around the autonomous vehicle. They have relatively low residual errors, are sufficiently large, and generally represent ground points around the autonomous vehicle. In some configurations, the residual error threshold can include 0.1. To determine critical planes, points such as the first point 305A and the second point 305B can be selected from points on the same LIDAR ring 301A. In some configurations, the first / second points 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 critical plane.

[0082] Referring here to Figures 4E and 4F, a third point 305C can be selected from the adjacent ring 301B. The third point 305C may have an azimuth angle α3 (Figure 4F) that lies between the azimuth angle α1 (Figure 4F) of the first point 305A and the azimuth angle α2 (Figure 4F) of the second point 305B. The first / second / third points 305A / B / C form a plane with a defining equation that can evaluate its relationship to the gravity vector. In some configurations, evaluating the plane may include checking the orientation of the plane by selecting a plane with a normal vector of 60 degrees or less to the gravity vector provided by an inertial measurement sensor placed on an autonomous vehicle, for example, but not limited to. As the plane grows and points are added, the orientation angle can be reduced to 20°. In one embodiment, the plane is grown by selecting n nearest neighbors of a seed point and comparing the normals of the nearest neighbors with the plane normal. If the normal vector does not deviate from the plane normal by more than an angular threshold of 40-60°, neighboring points can be added to grow the plane. This technique also allows for the development of other plane growth techniques.

[0083] Referring here to Figure 4G, all points remaining from the previous filtering step described herein can be evaluated for inclusion in polygon 313A. The sides of polygon 313A can be defined by the first / second points 305A / B and the ring 301A / B.

[0084] Referring here to Figure 4H, the plane can be grown in four directions and vertically, forming polygon 313B. Growing the plane may involve evaluating points along azimuth angles that are increasingly farther from the polygon 313A of the original selected points, away from the autonomous vehicle towards the ring. 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 the residual error on that side exceeds a threshold or that side reaches the edge 323 of the point cloud. At that point, orientation and residual error checks can be performed, and if they pass, the plane can be tentatively classified as important. Additional checks may be performed to aid in further filtering, such as the number of points, the number of growth cycles, and the number of vertical growth cycles. For example, if a plane has experienced 10 lateral growth cycles or 2 vertical growth cycles and the plane is not considered important, the plane growth of that plane can be terminated.

[0085] Referring here to Figure 4I, the data from ring 353 can be evaluated as described herein to form plane 351. From a set of important planes, surface planes can be identified by applying the planes to a scoring function, for example, but not limited to, the following:

[0086]

number

[0087] Final score = Residual score + Growth score + Area score + Angle score The scoring function can be used to evaluate the quality of a plane; a higher score indicates a more promising candidate, and planes that do not meet a pre-selected threshold are discarded. The angle score indicates how close the plane is to a vertical plane or the ground plane.

[0088] Referring here to Figure 5, the feature matching according to this teaching matches map data with real-time data to provide a final pose. Different feature matching methods are used for various types of features. For planar / surface features, a map localization method is used to match planar point data with real-time planar point data. For curvature points (points existing on a plane and on a curved surface), a scan matching method is used to match raw map point data with real-time curvature point data. Processing of map data is described with reference to Figures 2A-2B and 3A-3C. Processing of real-time data is described with reference to Figures 4A-4I. The exemplary scan matching in the heterogeneous computing environment of this disclosure includes tasks performed by a first processor and tasks performed by a second processor, the two processors exchanging data and handshake control. In one embodiment, the first processor is a sequential processor, and the second processor is a parallel processor such as, for example, a graphics processing unit, multiple execution units / cores, and multiple hardware threads. The process begins with a first processor organizing map data corresponding to a pre-selected region around the autonomous vehicle's location. This organization may include, for example, constructing a balanced kd tree from the map data. A second step involves performing a similar organization and provision of pre-selected sized blocks of data surrounding the original provided data. The autonomous vehicle's direction of movement, along with the kd tree of the data blocks associated with the autonomous vehicle, are provided to an efficient search algorithm that dynamically detects and accelerates the search process, used to generate matched map points. Outliers from the matched points are excluded. One process for excluding outliers is the Interquartile Range (IQR) method, in which values ​​outside the IQR boundaries are excluded as outliers.Another filtering process evaluates matches by exploring the kd tree by excluding points where the angle between the real-time data normal and the map data point normal exceeds a threshold. In another filtering process, points that do not converge during the iterations of the ICP process are excluded. These filtered matched map points have their orientation (e.g., using UKF as described herein). This is used to estimate the reliability of the posture, and posture correction is performed as shown in Figure 6B.

[0089] Referring here to Figure 5A, a kd-tree is a spatial data structure used for efficient nearest neighbor search. A kd-tree divides spatial space into partitions in multiple dimensions, depending on the depth level of the kd-tree. These partitions are used to efficiently search for neighbors and exclude distant points. Generally, map points within a threshold distance from a real-time point are searched via a kd-tree. The efficiency of the kd-tree search depends on the search threshold used and how quickly the nearest point is reached by traversing the kd-tree. As the algorithm iterates and converges the real-time data into map data, the search threshold is inversely reduced to speed up the kd-tree search. Intelligent reduction in the threshold speeds up the convergence process. The search threshold also depends on how far the sensor point is from the autonomous vehicle. For example, lidar point 100, 100 meters from the autonomous vehicle, has a larger search threshold than lidar point 10 meters from the autonomous vehicle. In one embodiment, For iterations 1-3, distance_threshold = 2 + 0.04 × l_dist For 4 to 15 iterations, distance_threshold = (2 + 0.04 × l_dist) / (6 × (Itr-3)) For 16-20 iterations, distance_threshold = (2 + 0.04 × l_dist) / 72 l_dist: Lidar point distance from bot Itr: Number of repetitions

[0090] Referring to Figures 5B to 5D, the teaching system for map localization and scan matching uses sample data, feature information, and iterative convergence behavior to exclude outliers 4031 and detect false matches. In Figure 5B, the conventional interquartile range (IQR) 4033 outlier removal method using point-to-plane distance is used to exclude data points that are outside the IQR range with a pre-selected distance added / subtracted. Furthermore, exploring the kd tree matches LIDAR points with their nearest map points regardless of the normal direction. Therefore, excluding outliers involves detecting situations where the nearest points are not in the same plane. Figure 5C shows a situation where the angle between the LIDAR point normal 4035 and the matched map point normal 4037 indicates a false match that should be excluded. In Figure 5D, through iteration, the point-to-plane distance threshold (map localization) and point-to-point threshold (scan matching) are reduced to exclude matches that do not converge.

[0091] Referring here to Figure 6A, the confidence level of the calculated attitude 702 of the autonomous vehicle can be evaluated by a set of steps. The confidence level is based on the number and orientation of non-surface road features in the direction of travel of the autonomous vehicle. For example, if there are planar features that are nearly perpendicular to each other, as shown in plane 707, the confidence level of the calculated attitude 702 can be assumed to be relatively high (e.g., a value of 1), while if there are no non-surface planar features, as shown in plane 701, the confidence level of the calculated attitude 702 can be assumed to be relatively low (e.g., a value of 0). If there are few non-surface planar features, as shown in plane 703, or if the road planar features are oriented relatively parallel to each other, as shown in Figure 705, the confidence level will be a value between a relatively high and a relatively low value, for example, a value of 0.7 for the configuration shown in plane 705 and a value of 0.4 for the configuration shown in plane 703. In one embodiment, confidence indices are assigned to various configurations. For example, a relatively high confidence level in the calculated orientation 702 associated with the planar feature configuration of plane 707 is assigned an index of 1, and a relatively low confidence level in the calculated orientation 702 associated with the absence of a planar feature shown in plane 701 is assigned an index of -4. In one embodiment, an index associated with a planar feature shown in plane 705 has a value of 1, and an index associated with a planar feature shown in plane 703 has a value of 0. In one embodiment, index values ​​of -1, -2, and -3 are assigned to situations involving unreliable data / calculations. In one embodiment, the confidence level is at least a function of the number of points on the planar features and the angles between the planar features. An exemplary formula for calculating the confidence level of an orientation is:

[0092]

number

[0093] And, N1: Number of points in plane 1 N2: Number of points in plane 2 n1: Normal to plane 1 n2: Normal to plane 2 That is the case.

[0094] Referring to Figure 6B, if planar features exist as shown in plane 707, which has a relatively high confidence level, then attitude correction is not necessary. However, if only map planar features 707a exist in one direction, as in plane 703 (Figure 6A) and plane 705 (Figure 6B), and the LIDAR planar features 707a are not aligned with the map planar features 707a, then the input attitude 704 requires correction. In one embodiment, the correction is performed in a direction in which the autonomous vehicle 203 does not encounter map planar features. For example, the input attitude 704 is corrected to the estimated attitude position 706 in the y direction by moving the input attitude 704 in the y direction by the amount by which the LIDAR planar features 707b are moved to align with the map planar features 707a. In the next step, the estimated attitude position 706 is moved in the x direction to align with the input attitude 704, forming the corrected attitude 1702.

[0095] Referring here to Figure 7, a first configuration of the method according to this teaching for locating an autonomous vehicle is shown. The method 1100 for locating an autonomous vehicle includes, but is not limited to, a step 1102 of organizing a first map associated with the current location of the autonomous vehicle to form current organized data; a step 1104 of organizing at least one second map associated with at least one potential location to which the autonomous vehicle can navigate to form potential location organized data; a step 1106 of updating the current organized data as the autonomous vehicle navigates, based at least on the potential location organized data and the current location; and a step 1108 of selectively updating the potential location organized data based at least on the direction of travel, speed of travel, potential location organized data and the current location of the autonomous vehicle. Method 1100 further includes: step 1110 filtering real-time data received by an autonomous vehicle to form real-time data; step 1112 scan matching the processed current organized data with the real-time data to form matched map data based at least on dynamic thresholds; step 1114 removing outlier data from the matched map points based on feature characteristics, dynamic thresholds, and an outlier detection algorithm to form an attitude estimate; and step 1116 correcting the attitude estimate based at least on planar features associated with the current position to form a localized attitude.

[0096] Referring here to Figure 8, a second configuration of the method according to this teaching for locating an autonomous vehicle is shown. The method 800 for locating an autonomous vehicle includes, but is not limited to, a step 802 of receiving and processing offline (map) data and real-time (sensor) data by at least one processor; a step 804 of separating data from the offline and real-time data relating to the current GPS-determined location of the autonomous vehicle; a step 806 of determining a global attitude and a confidence level associated with the global attitude, at least on the basis of matching map features found in the offline data with real-time features found in the real-time data; a step 808 of continuously calculating a local attitude as the autonomous vehicle navigates, at least on the basis of comparing the current attitude with the previous attitude; a step 810 of predicting future characteristics of the autonomous vehicle by running a model of the autonomous vehicle's motion; and a step 812 of continuously calculating an end attitude and an estimated confidence level associated with the autonomous vehicle, at least on the basis of the global attitude, the local attitude, and the future characteristics.

[0097] Those skilled in the art will understand that the methods described herein may be applied to computer systems configured to achieve such methods, and / or computer-readable media containing programs for achieving such methods, and / or software and / or firmware and / or hardware (e.g., integrated circuits) designed to achieve such methods. Raw data and / or results may be stored for future retrieval and processing, and may be printed, displayed, transferred to another computer, and / or transferred to other locations. Communication links may, in non-limiting examples, be Ethernet, cellular or broadband networks, WiFi or local area networks, military communication systems, and / or satellite communication systems. Parts of the system may, for example, run on a computer having a variable number of CPUs. Other alternative computer platforms may also be used.

[0098] As those skilled in the art will understand, the methods described herein may be implemented electronically, in whole or in part. Signals representing actions performed by the systems of this disclosure and other disclosed components may be transmitted over at least one live communication network. Control information and data information may be electronically executed and stored on at least one computer-readable medium. The systems may be implemented to run on at least one computer node within at least one live communication network. Common forms of computer-readable medium include, but are not limited to, floppy disks, flexible disks, hard disks, magnetic tape or any other magnetic medium, compact disk read-only memory or any other optical medium, punch cards, paper tape or any other physical medium having a pattern of holes, random access memory, programmable read-only memory, erasable programmable read-only memory (EPROM), flash EPROM or any other memory chip or cartridge, or any other medium that a computer can read.

[0099] Those skilled in the art will understand that information and signals can be represented using any of the various existing techniques. For example, data, instructions, commands, information, signals, bits, symbols, or chips that may be referenced throughout this specification may be represented by voltage, current, electromagnetic waves, electromagnetic fields or electromagnetic particles, optical fields or optical particles, ultrasound, projected capacitance, or any combination thereof.

[0100] Those skilled in the art will further understand that the various exemplary logic blocks, modules, circuits, and algorithmic steps described in relation to the configurations disclosed herein can be implemented as electronic hardware, computer software, or a combination of both. To clearly demonstrate this hardware and software compatibility, various exemplary 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 specific application and the design constraints imposed on the overall system. Those skilled in the art will understand that for each specific application, the described functionality may be implemented in various ways, but such implementation decisions should not be construed as causing a departure from the scope of the appended claims.

[0101] The various exemplary logic blocks, modules, and circuits described in relation to the configurations disclosed herein may be implemented or run using general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs) or other programmable logic devices, discrete gate or transistor logic, discrete hardware components, or any combination thereof configured to perform the functions described herein. A general-purpose processor may be a microprocessor, but in alternative forms, the processor may be any conventional processor, controller, microcontroller, or state machine. The processor may also be implemented as a combination of computing devices, e.g., a combination of a DSP and a microprocessor, multiple microprocessors, one or more microprocessors combined with a DSP core, or any other such configuration.

[0102] Actions of methods or algorithms described in relation to configurations disclosed herein may be embodied directly in hardware, in software modules executed by a processor, or in a combination of the two. Software modules may 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. Storage media may be coupled to a processor so that the processor can read information from and write information to the storage media. In an alternative configuration, the storage medium may be integrated with the processor. The processor and storage medium may reside within an ASIC. An ASIC may reside in, for example, a computer, robot, user terminal, mobile phone or tablet, automobile, or IP camera. In an alternative configuration, the processor and storage medium may exist as separate components within such a functional device.

[0103] The above description is not intended to be exhaustive or to limit the features to the exact forms disclosed. Various alternative and modified forms can be devised by those skilled in the art without departing from this disclosure, and the general principles defined herein may be applied to other forms without departing from the gist or scope of the appended claims. Therefore, this disclosure is intended to encompass all such alternative, modified, and variant forms. Additionally, while some configurations of this disclosure are shown in the drawings and / or discussed herein, this disclosure is not intended to be limited thereto, and this disclosure is intended to be as broad as the art allows, and this specification is intended to be read in the same manner. Therefore, the above description should not be construed as a limitation, but merely as examples of specific configurations. Furthermore, those skilled in the art will envision other modifications within the scope and gist of the appended claims. Other elements, steps, actions, methods, and techniques that are not substantially different from those described above and / or in the appended claims are also intended to be within the scope of this disclosure. Accordingly, the appended claims are not intended to be limited to the configurations shown and described herein, but are intended to be in accordance with the broadest scope that is consistent with the principles and novel features disclosed herein.

[0104] The arrangements shown in the drawings are provided solely to illustrate specific examples of this disclosure. Furthermore, the drawings are illustrative and not limiting. In the drawings, for illustrative purposes, the sizes of some elements may be exaggerated and may not be drawn to a specific scale. Additionally, elements with the same number shown in the drawings may be identical or similar elements, depending on the context.

[0105] Where the term “equipped with” is used in this specification and in the claims, it does not exclude any other element or step. Where an indefinite or definite article, e.g., “a,” “an,” or “the,” is used to indicate a singular noun, unless otherwise specified, it also includes the plural form of that noun. Thus, the term “equipped with” should not be interpreted as being limited to the items listed thereafter, and does not exclude any other element or step, so the scope of the expression “a device comprising item A and item B” should not be limited to consisting only of components A and B. Furthermore, wherever terms such as “include,” “have,” and “possess” are used in this specification and in the claims, such terms are intended to be interpreted comprehensively in the same way that the term “equipped with” is interpreted when used as a transition word in a claim.

[0106] Furthermore, terms such as “first,” “second,” and “third,” whether used herein or in the claims, are provided to distinguish similar terms and do not necessarily describe a sequential or chronological order. Terms used in this manner are interchangeable under appropriate circumstances (unless otherwise explicitly disclosed), and it should be understood that embodiments of the disclosure described herein may operate in an order and / or arrangement other than those described or illustrated herein.

[0107] One or more computer systems can be configured to perform a specific operation or action by having software, firmware, hardware, or a combination thereof installed on the system that causes the system to perform an action while it is running. One or more computer programs can be configured to perform a specific operation or action by containing instructions that cause the data processing device to perform an action when executed by the device. [Explanation of Symbols]

[0108] 13 Wheel Odometry Data 100 lidar points 101 Sensor Processor 105 Feature Extraction 109 Posture Estimator 111 Final Stance 112 Inertial Measurement Unit (IMU) Data 113 Local posture data 114 Preprocessing 117 Models 134 Image pose estimation data 136 LIDAR attitude estimation data 203 Autonomous Vehicles 205 Downsampled #2 data 207A~207D Submap 209 random points 211 Most recent emphasis marks 213 Submaps 215 plane 219 Normal plane 220 Normal plane 221 Spanning plane 225 Feature Map 301 LIDAR ring 301A / B / N Ring 301A Lidar Ring 303 1D string 305A First point 305B Second point 305C Third point 307A / B median 309A / B discontinuity point 311 Space 313A Polygon 313B Polygon 336 LIDAR 351 plane 353 Ring 501 Sensor Data 503 Location Logic 507 Autonomous Logic 701 plane 702 Posture 703 plane 704 Input posture 705 plane 706 Estimated posture position 707 plane 707a Map Planar Features 707b LIDAR Planar Features 1203 Downsampled #1 data 1702 Corrected posture 4001 Ground point 4003 Plane point 4005 point cloud data points Block 4011 Block 4013 4015 Block 4017 Block 4019 Boundary 4021 Boundary area 4033 Interquartile Range (IQR) 4035 LIDAR point normal 4037 Map Point Normal 14011 Center

Claims

1. A method for locating an autonomous vehicle, The steps include: organizing a first map associated with the current location of the autonomous vehicle to form the current organized data; The steps include organizing at least one second map associated with at least one potential location that the autonomous vehicle can navigate, thereby forming organized data of potential locations, The steps include updating the current organized data based at least on the organized data of the potential location and the current location when the autonomous vehicle is navigating, The steps include selectively updating the organized data of the potential position based on at least the direction of movement, speed of movement, organized data of the potential position, and the current position of the autonomous vehicle, The steps include filtering real-time data received by the autonomous vehicle to form real-time data, The steps include scanning and matching the processed current organized data with the real-time data to form matched map data based at least on a dynamic threshold, The steps include: removing outlier data from the matched map points based on feature characteristics, dynamic thresholds, and an outlier detection algorithm to form a pose estimate; The steps include correcting the posture estimation based at least on the planar features associated with the current position and forming a localized posture, Methods that include...

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

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

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

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

6. The step of selectively updating is, The steps include determining whether the autonomous vehicle is located in the boundary region between the first map and the at least one second map, If the autonomous vehicle navigates outside the boundary area and the first map, the steps include updating the organized data of the potential location. The method according to claim 1, including the method described in claim 1.

7. The method according to claim 1, wherein the step of filtering the real-time data includes the step of downsampling the real-time data according to a pre-selected criterion to form downsampled data.

8. The method according to claim 7, wherein the pre-selected criteria are selected from user-defined criteria, default criteria, dynamically determined criteria, pre-selected densities, and combinations thereof.

9. The method according to claim 1, further comprising the step of dividing the first map and the at least one second map into planar points and non-planar points.

10. The method according to claim 9, further comprising the step of removing the non-planar point.

11. The method according to claim 9, further comprising the step of identifying a planar point as a point belonging to a non-ground plane.

12. A step of identifying a point in a plane, (a) The step of selecting a random point from the downsampled data, (b) A step of determining the location of neighboring points of the random point, (c) The step of identifying the planar point in the downsampled data as a point on a flat plane formed by the random point and, if present, the neighboring point, (d) The step of identifying the non-planar point as not being a planar point, (e) (1) Repeat steps (a) to (d) until there are no more downsampled points to be examined, or until a pre-selected number of flat planes are achieved, or until steps (a) to (d) have been repeated a pre-selected number of times. Steps to identify a plane point, including The method according to claim 9, further comprising:

13. The method of claim 7, further comprising the step of forming a real-time plane that grows from multiple scans of the downsampled real-time data to outliers.

14. The steps include determining discontinuities in the real-time plane, The step of removing the point which is part of the discontinuity. The method according to claim 13, further comprising:

15. The steps include forming a real-time plane that grows from multiple scans of the downsampled real-time data up to an outlier, The steps of matching the real-time plane with the flat plane The method according to claim 12, further comprising:

16. The method according to claim 12, wherein the step of organizing the first map includes the step of forming a k-dimensional tree from the first map, and / or the step of forming a plurality of k-dimensional trees from the first map and the at least one second map by a parallel processor.

17. The method according to claim 12, wherein the step of organizing the first map data and the at least one second map includes the step of forming a plurality of k-dimensional trees from the first map data and the at least one second map.

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

19. A method for locating an autonomous vehicle, The process involves receiving and processing offline (map) data and real-time (sensor) data using at least one processor, The steps include separating data related to the current location determined by the GPS of the autonomous vehicle from the offline data and the real-time data, A step of determining a global attitude and a confidence level associated with the global attitude, at least based on matching map features found in the offline data with real-time features found in the real-time data. The autonomous vehicle, when navigating, continuously calculates its local attitude based at least on a comparison between its current attitude and its previous attitude. The steps include: predicting the future characteristics of the autonomous vehicle by running a model of the autonomous vehicle's motion; A step of continuously calculating the final attitude associated with the autonomous vehicle and the estimated confidence level based on at least the global attitude, the local attitude, and the future characteristics. Methods that include...

20. The method according to claim 19, further comprising the step of collecting the offline data and the real-time data over different time periods and / or by a common vehicle.

21. The step of calculating the local posture information is: The steps include measuring the linear speed and angular velocity of the autonomous vehicle, The steps include: measuring the local direction of travel of the autonomous vehicle; The steps include: using data collected by an image sensor to estimate the image orientation of the autonomous vehicle, based at least on the previous image and the current image of the autonomous vehicle; The steps include: estimating the laser attitude of the autonomous vehicle based on at least the forward attitude and current attitude of the autonomous vehicle using data collected by the laser sensor; A step of calculating the local posture based on at least the linear velocity and angular velocity, the local direction of travel, the estimated image posture, and the estimated laser posture. The method according to claim 19, including the method described in claim 19.

22. The method according to claim 19, wherein the model is selected from an autonomous vehicle at a constant speed, a motion model for the autonomous vehicle, a dynamically updated motion model for the autonomous vehicle, and a combination thereof.

23. The method according to claim 19, wherein the step of continuously calculating the final attitude includes feeding the global attitude, the local attitude, and the future characteristics into a Bayesian state estimator.

24. A system for determining the location of autonomous vehicles, 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 sensor data features of interest within the filtered sensor data from map data features of interest within the filtered map data, A third processor configured to match the aforementioned sensor data features of interest with the aforementioned map data features of interest, A fourth processor configured to perform alignment on the matched data and create a global attitude, A fifth processor that collects local attitude information based at least on the linear velocity and angular velocity of the autonomous vehicle, the direction of travel of the autonomous vehicle, the image attitude estimation of the autonomous vehicle, and the laser attitude estimation of the autonomous vehicle. A sixth processor for predicting the future movements of the autonomous vehicle, A seventh processor that calculates the final attitude of the autonomous vehicle based at least on the global attitude, the local attitude information, and the future movement. A system equipped with these features.

25. The system according to claim 24, wherein the alignment includes an iterative nearest neighbor algorithm.

26. The first processor filters the map data, (a) Downsampling the filtered sensor data to create downsampled data, (b) Creating a submap of the downsampled data, (c) Selecting a random point from the downsampled data, (d) Identifying the neighboring points of the random point, (e) Identifying planar points in the downsampled data as points on a flat plane formed by the random points and, if any, the neighboring points, (f) Identifying non-planar points as not being planar points, (g) (1) Repeat steps (a) to (f) until there are no more downsampled points to be examined, or until a pre-selected number of the flat planes are achieved, or until steps (a) to (d) have been repeated a pre-selected number of times. The system according to claim 24, which includes instructions to perform the following.

27. The first processor, A ground plane processor that determines a ground plane from point cloud data received from a sensor, wherein each of the ground planes is associated with a ground plane equation, and the sensor has a sensor reference system. A plane transformation processor that converts the ground plane equation from the sensor reference system to the vehicle reference system associated with the autonomous vehicle, The system according to claim 24, comprising:

28. The aforementioned ground plane processor 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 of the points in the point cloud data from the median, A plane creation processor that creates planes from the filtered point cloud data, wherein each of the created planes has at least one azimuth angle, A plane growth processor that grows the formed plane from the point cloud data and extends it in a direction away from the autonomous vehicle along at least one azimuth angle to form a grown plane, A selection processor that selects the ground plane from the grown plane based at least on the orientation of the created plane and the residual error. The system according to claim 27, comprising:

29. The plane creation processor, A computer instruction for selecting a first point and a second point from a first ring of sensor data, wherein the first point and the second point are located within a boundary formed by discontinuities in the point cloud data on the first ring, the first point has a first azimuth angle, and the second point has a second azimuth angle. A computer instruction for selecting a third point from a second ring of sensor data, wherein the second ring is adjacent to the first ring, and the third point has a third azimuth angle between the first azimuth angle and the second azimuth angle. A computer instruction to create one of the planes that include the first point, the second point, and the third point. The system according to claim 28, comprising executable code including

30. The system according to claim 27, further comprising executable code including a computer instruction to use a default plane instead if no ground plane can be determined.

31. The system according to claim 27, further comprising executable code including a computer instruction to remove a point from the point cloud data if the point exceeds a pre-selected distance from the autonomous vehicle.

32. The system according to claim 27, further comprising executable code including a computer instruction to remove a point from the point cloud data if the point exceeds a height pre-selected based at least on the vehicle height of the autonomous vehicle.

33. The system according to claim 27, further comprising executable code including a computer instruction to remove the point from the point cloud data if the point is within a pre-selected distance from the autonomous vehicle.

34. To transform the aforementioned ground plane, From the coefficients of the aforementioned ground plane equation, a single vector [Math 1] A computer instruction to calculate the following, wherein the ground plane equation includes ax+by+cz+d=0, the coefficients include a, b, and c, and the constant includes d. Normalize the d constant [Math 2] Computer instructions and The coefficients a, b, and c of the ground plane equation are transformed based on the rotation / translation sequence and the single vector. [Math 3] Computer instructions and The normalized d constant is transformed based on the normalized d constant, the rotation / translation sequence, the single vector, and the transformed a, b, and c coefficients. [Math 4] Computer instructions and The system according to claim 27, comprising executable code including

35. The method according to claim 19, wherein the step of determining the estimated confidence level includes a step of evaluating the number and orientation of planar features in the direction of travel of the autonomous vehicle.

Images