Systems and methods for free space estimation

Abstract

To provide a system and a method for free space estimation.SOLUTION: The present invention relates to a system and a method for assigning a free space probability by estimating a free space in point group data associated with an autonomous vehicle travelling on a surface while including considering sensor noise, sensor availability, an obstacle height and a distance from a sensor to an obstacle. The system and the method may include determining a surface plane especially among the other factors and classifying point group points in accordance with whether or not a point is on the surface plane.SELECTED DRAWING: Figure 1A

Description

[Background technology]

[0001] (CROSS-REFERENCE TO RELATED APPLICATIONS) This patent application claims the benefit of U.S. Provisional Patent Application No. 62 / 879,391 (Attorney Docket No. AA027), filed July 26, 2019, and entitled "System and Method for Free Space Estimation," which is incorporated herein by reference in its entirety.

[0002] Vehicles navigate surfaces determined by their human operators to include free, unobstructed space. The humans use a complex set of criteria to determine whether to traverse a path in or on the vehicle. Considerations include, but are not limited to, the degree of obstacle height, the amount of the obstacle that can be seen by the human, the area around the vehicle that the human cannot visualize, and the degree to which the human visual system is accurate in detecting obstructions, subject to any number of factors such as ambient lighting, weather, and windshield issues.

[0003] Navigating obstacles in an autonomous vehicle can require electronically evaluating some of the same complex criteria routinely encountered by a human vehicle operator. Unobstructed (free) space must be rapidly determined from available sensor data about the autonomous vehicle in order to proceed continuously along a navigation path. Previously, free space was estimated from stereo camera data, from sequences of images in video acquired by a camera system, and from millimeter-wave radar, among other methods.

[0004] What is needed is an efficient system for calculating the probability that a path is obstructed from sensor data. The sensors that collect the sensor data can be mounted, for example, on an autonomous vehicle. What is needed is a system that takes into account the realities of road navigation and sensor limitations. Summary of the Invention [Means for solving the problem]

[0005] Systems and methods of the present teachings for assigning free space probabilities in point cloud data associated with an autonomous vehicle traveling on a surface include considering sensor noise, sensor availability, obstacle height, and obstacle distance from the sensor. The methods of the present teachings can include, without limitation, receiving point cloud data from a sensor. The sensor can include a sensor beam, and the sensor beam can be projected from at least the sensor onto the surface. In some configurations, the sensor can scan an area surrounding the autonomous vehicle and collect data in a cone from the surface spanning a preselected angle. The methods can include segmenting the point cloud data into segments of a first preselected size and locating planes, plane points within the plane, and non-planar points associated with at least one of the plane points within the point cloud data. The methods can include determining normals to the plane points and determining non-planar points associated with the plane points. The method may include selecting at least one of the planes as a surface plane according to preselected criteria based at least on the normal and the location of the sensor; classifying each of the plane points as an obstacle point based at least on associated non-plane points; and determining an obstacle height associated with the obstacle point based at least on the non-plane points. The method may include creating a grid from the surface planes. The grid may include a preselected number of cells and a perimeter. The method may include calculating a measurement significance for each cell based at least on an obstacle height within the cell, and determining a blind distance from the sensor based at least on an intersection between the sensor beam and the surface plane. For each cell along each line between the blind distance and the perimeter, the method may include calculating an initial probability of an obstacle occupying the cell along the line. The initial probability may be based at least on sensor availability, obstacle points within the cell, and the position of the cell along the line relative to the sensor.For each cell along each line between the blind distance and the perimeter, the method may include calculating a noise factor based on at least a first distance between the sensor and a closest neighbor of an obstacle along the line in the cell, a second distance between the sensor and the cell along the line, a measurement significance for the cell along the line, an initial probability for the cell along the line, and a default probability. For each cell along each line between the blind distance and the perimeter, the method may include calculating a current probability of an obstacle occupying the cell along the line. The current probability may be based on at least the initial probability for the cell and the noise factor for the cell.

[0006] The first preselected size can optionally include a shape approximately 40m x 40m x 2m in size. The preselected criteria can optionally include selecting a surface plane when at least one plane normal faces away from the sensor. The preselected number of cells can optionally include 400 x 400. Calculating the initial probability may optionally include: (a) if the sensor is unavailable and the cell is a neighboring cell, the neighboring cell is in the blind distance neighborhood, and the initial probability of the cell may optionally be equal to 1.0; (b) if the sensor is unavailable and the cell is between the neighboring cell and the perimeter, the initial probability of the cell may optionally be equal to 0.5; (c) if the sensor is available and at least one of the obstacle points is present within the cell, or one of the previously encountered cells along the line contained at least one of the obstacle points, the initial probability of the cell may optionally be equal to 0.5; and (d) if the sensor is available and none of the obstacle points is present within the cell, and none of the previously encountered cells along the line contained at least one of the obstacle points, the initial probability of the cell may optionally be equal to 0.3. The noise factor may optionally be [ka] where d = second distance and Z t= first distance, and σ = Z t 2 ×0.001. Calculating the current probability may optionally be equal to the sum of a noise factor for the cell and the initial probability of the cell. The at least one plane may optionally include non-planar points up to a first preselected distance from the at least one plane. The first preselected distance may optionally include 2 m.

[0007] A system of the present teachings for assigning free space probabilities from point cloud data may include, but is not limited to, a sensor having a sensor beam. The sensor beam may be projected from at least the sensor onto a surface. The system may include a segment processor receiving the point cloud data from the sensor. The segment processor segments the point cloud data into segments of a first preselected size. The system may include a plane processor locating, within the point cloud data, planes, plane points within the planes, and non-planar points associated with at least one of the plane points. The system may include a normal processor determining normals to the plane points and determining non-planar points associated with the plane points. The normal processor may designate at least one of the planes as a surface plane according to preselected criteria based at least on the normal and the location of the sensor. The normal processor may classify each of the plane points as an obstacle point based at least on the associated non-planar points. The normal processor may determine an obstacle height associated with the obstacle point based at least on the non-planar points. The system may include a grid processor creating a grid from the surface plane. The grid may include a preselected number of cells and a perimeter. The system can include a line sweep processor, which may include a measurement significance processor. The measurement significance processor can calculate a measurement significance for each cell based at least on obstacle heights within the cell. The line sweep processor can include an initial probability processor that determines a blind distance from the sensor based at least on intersections between the sensor beam and a surface plane. For each cell along each line between the blind distance and the perimeter, the initial probability processor can calculate an initial probability of an obstacle occupying the cell along the line. The initial probability can be based at least on sensor availability, obstacle points within the cell, and the position of the cell along the line relative to the sensor. The line sweep processor can include a noise coefficient processor.For each cell along each line between the blind distance and the perimeter, the noise coefficient processor can calculate a noise coefficient based on at least a first distance between the sensor and a nearest neighbor of an obstacle along the line in the cell, a second distance between the sensor and the cell along the line, a measurement significance for the cell along the line, an initial probability for the cell along the line, and a default probability. The line sweep processor can include a current probability processor. For each cell along each line between the blind distance and the perimeter, the current probability processor can calculate a current probability of an obstacle point occupying the cell along the line. The current probability can be based on at least the initial probability for the cell and the noise coefficient for the cell.

[0008] In some configurations, a method for assigning free space probabilities in sensor data for an autonomous vehicle may include, without limitation, determining at least one surface plane in the sensor data, where the at least one surface plane may be associated with a surface over which the autonomous vehicle is traveling. The method may include determining obstacles, if any, in the sensor data associated with the at least one surface plane, and determining the height of the obstacles, if any, and determining a blind distance from the autonomous vehicle based at least on dimensions of the autonomous vehicle. The method may include creating a grid on the at least one surface plane, where the grid may include a preselected number of cells and a perimeter. For each cell along each line on the at least one surface plane between the blind distance and the perimeter, the method may include calculating an initial probability of an obstacle occupying the cell along the line. The initial probability may be based at least on the availability of sensor data, the obstacles in the cell, and the position of the cell along the line relative to the autonomous vehicle. For each cell along each line between the blind distance and the perimeter, the method may include calculating a noise factor based on at least a first distance between the autonomous vehicle and a closest one of the obstacles along the line in the cell, a second distance between the autonomous vehicle and the cell along the line, an obstacle height for the cell along the line, an initial probability for the cell along the line, and a default probability. For each cell along each line between the blind distance and the perimeter, the method may include calculating a current probability of an obstacle occupying the cell along the line, where the current probability may be based on at least the initial probability for the cell and the noise factor for the cell.

[0009] The preselected number of cells may optionally include 400 x 400. Calculating the initial probability may optionally include the following: (a) if sensor data is unavailable and the cell is a neighboring cell, the neighboring cell is in the blind distance neighborhood, and the initial probability of the cell may optionally be equal to 1.0; (b) if sensor data is unavailable and the cell is between the neighboring cell and the perimeter, the initial probability of the cell may optionally be equal to 0.5; (c) if sensor data is available and at least one of the obstacles is present within the cell or one of the previously encountered cells along the line included at least one of the obstacles, the initial probability of the cell may optionally be equal to 0.5; and (d) if sensor data is available and none of the obstacles is present within the cell and none of the previously encountered cells along the line included at least one of the obstacles, the initial probability of the cell may optionally be equal to 0.3. Calculating the noise factor may optionally include: [ka] where d = second distance and Z t = first distance, and σ = Z t 2 ×0.001. Calculating the current probability may optionally be equal to the sum of the cell's noise factor and the cell's initial probability. The at least one surface plane may optionally include non-planar points up to a first preselected distance from the at least one surface plane. The first preselected distance may optionally include 2 m. The method may optionally include determining an obstacle height based on at least the non-planar points.

[0010] In another configuration, free-space estimation from LIDAR data, where the LIDAR data includes rings, can include, but is not limited to, receiving LIDAR data and filtering a preselected number of points within each ring. Filtering can include identifying a median value for each preselected number of points and retaining points that are within a preselected range from the median value. Between the retained points, there can be discontinuities where the Cartesian distance between the points exceeds a preselected value. Points between the discontinuities are labeled as good points if they pass the median filter. Good points can be expected to have low sensor noise. If the number of good points between the discontinuities exceeds a preselected value, the good points are retained. Discontinuities or breaks can be found at the edges of features; thus, edges can be found when there is an abrupt change in distance between points.

[0011] At this point, each filtered point can be associated with a LIDAR ring. The filtered point data can be divided into segments of 64 points each. A random segment can be selected, and two points from the random segment can be chosen. The two points can be intelligently chosen based on how the points were labeled in the filtering process, i.e., good points and break points. For example, two points between the same two discontinuities can be chosen. A third point is chosen from an adjacent ring, and a plane is formed from the three points. The plane is evaluated with respect to certain preselected criteria according to an algorithm that can eliminate insignificant or uninteresting planes. All points on the adjacent LIDAR ring (the LIDAR ring corresponding to the third point) that fall within the azimuth angle range of the first two points are evaluated as candidates to be included in the first expansion stage. A plane equation is calculated using the updated set of points. The points are then evaluated again to expand this plane along the point data structure axes corresponding to the LIDAR ring and azimuth angle, respectively. At each expansion stage, the orientation and residual error of the plane are checked. The residual error is calculated when the plane is fitted to the set of points, and the orientation is the angle check between the gravity vector and the normal vector of the plane. Expansion on each side is performed until, for example, the residual error for a new set of points along the expansion direction toward an outward ring checks that the residual error of a new set of points added from that ring exceeds a set threshold, or until the edge of the point cloud is reached. When expansion stops, the orientation and residual error are checked again, and the plane is classified as either valid (orientation angle and residual error relative to the gravity vector within a preselected threshold range) or invalid. If valid, in some configurations, the plane can be assigned a score. Additional checks are completed to further filter the plane. For example, the number of times the process outlined herein is performed can be compared to a desired maximum value, which can be configurable.The plane may be tested against ground plane criteria, for example, but not by way of limitation, according to a scoring algorithm that may be used to assess the quality of the plane.

[0012] The final set of planes is used to compare with the LIDAR data to determine whether the LIDAR data represents ground or obstacle data. The planes are transformed into the autonomous vehicle's frame of reference, referred to herein as the base link frame, and new plane equations are created. The optimal situation is when planes exist in all directions: in front of the autonomous vehicle, behind the autonomous vehicle, to the left of the autonomous vehicle, and to the right of the autonomous vehicle. If any planes are missing, the plane from the previous time synchronized LIDAR data and corresponding ground planes were received may be used until the plane becomes stale. If the plane is stale, no points are fitted in the direction of the old plane. If no planes exist in any direction, a default X / Y plane may be used. In some configurations, if no planes are detected over a period of seven iterations, a default plane where z+d=0 may be used.

[0013] Now that the ground planes are comparable, LIDAR points that are not within a preselected boundary or above a certain height are filtered from the LIDAR data. The remaining points are transformed into the autonomous vehicle reference frame, and points located within the LIDAR's blind spot are filtered out. The transformed points are classified according to whether they are within one of the previously determined ground planes. If the points are on the ground plane, they are marked as free space; otherwise, they are classified as obstacles. The points are located on a grid map. If an obstacle already exists at that location, the obstacle's height is added together, and the number of obstacles at that location is incremented. For each location on the grid map, the probability of occupancy for each cell in the grid map depends on the cell's distance to the sensor, LIDAR noise, measurement significance, whether the cell is over an obstacle, the presence of measurements, free space, and whether the LIDAR is blocked. The probability of occupancy as calculated herein follows Gaussian probability. The log-odds of the location being occupied are calculated for the probability of occupancy. The log-odds increase as the obstacle gets closer to the autonomous vehicle and decrease as the obstacle gets farther away from the autonomous vehicle. Beyond the LIDAR range, the log-odds are marked as infinity. The present invention provides, for example, the following. (Item 1) 1. A method for assigning free space probabilities in point cloud data, the point cloud data being associated with an autonomous vehicle traveling over a surface, the method comprising: receiving the point cloud data from a sensor, the sensor having a sensor beam, the sensor beam projected from at least the sensor onto the surface; segmenting the point cloud data into segments of a first preselected size; Locating a plane in the point cloud data, a plane point in the plane, and a non-planar point associated with at least one of the plane points; determining a normal to the plane point and determining the non-planar point associated with the plane point; selecting at least one of the planes as a surface plane according to preselected criteria based at least on the normal and the location of the sensor; classifying each of the planar points as an obstacle point based at least on the associated non-planar points; determining an obstacle height associated with the obstacle point based at least on the non-planar points; creating a grid from the surface plane, the grid having a preselected number of cells and a perimeter; calculating a measurement significance for each of the cells based at least on the obstacle height within the cell; determining a blind distance from the sensor based on at least an intersection between the sensor beam and the surface plane; calculating, for each cell along each line between the blind distance and the perimeter, an initial probability of an obstacle occupying the cell along the line, the initial probability being based on at least the availability of the sensors, the obstacle points within the cell, and the position of the cell along the line relative to the sensors; calculating, for each cell along each line between the blind distance and the perimeter, a noise factor based on at least a first distance between the sensor and a closest neighbor of the obstacle along the line in the cell, a second distance between the sensor and the cell along the line, the measurement significance for the cell along the line, the initial probability for the cell along the line, and a default probability; for each cell along each line between the blind distance and the perimeter, calculating a current probability of the obstacle occupying the cell along the line, the current probability being based on at least the initial probability for the cell and the noise coefficient for the cell; A method comprising: (Item 2) Item 10. The method of claim 1, wherein the first preselected size comprises approximately 40m x 40m x 2m. (Item 3) Item 2. The method of item 1, wherein the preselected criteria includes selecting the surface plane when the normal of the plane does not point towards the sensor. (Item 4) Item 10. The method of claim 1, wherein the preselected number of cells comprises 400 x 400. (Item 5) Calculating the initial probabilities If the sensor is unavailable and the cell is a neighboring cell, the neighboring cell is in the vicinity of the blind distance and the initial probability of the cell is 1.0; If the sensor is unavailable and the cell is between the neighboring cell and the perimeter, the initial probability of the cell=0.5; If the sensor is available and at least one of the obstacle points is present in the cell, or one of the cells along the line that has been previously encountered contains at least one of the obstacle points, then the initial probability of the cell = 0.5; If the sensor is available, none of the obstacle points are present within the cell, and none of the previously encountered cells along the line contained at least one of the obstacle points, then the initial probability of the cell = 0.3; The method according to item 1, comprising: (Item 6) Calculating the noise coefficients

number

number

number

number

number

number

number

number

number

number

number

number

number

[0014] The present teachings may be more readily understood by reference to the following description taken in conjunction with the accompanying drawings.

[0015] [Figure 1A] 1A-1C are flow charts of the processes of the present teachings. [Figure 1B] 1A-1C are flow charts of the processes of the present teachings. [Figure 1C] 1A-1C are flow charts of the processes of the present teachings.

[0016] [Figure 2] FIG. 2 is a schematic block diagram of a system of the present teachings.

[0017] [Figure 3] FIG. 3 is a pictorial diagram of a sensor configuration of the present teachings.

[0018] [Figure 4] FIG. 4 is a pictorial representation of segmented point cloud data of the present teachings.

[0019] [Figure 5] FIG. 5 is a pictorial representation of a plane within the segmented data of the present teachings.

[0020] [Figure 6] FIG. 6 is a pictorial representation of the normals of the present teachings.

[0021] [Figure 7] FIG. 7 is a pictorial representation of the obstacle points of the present teachings.

[0022] [Figure 8] FIG. 8 is a pictorial representation of the obstacle grid, blind radius, and sensor beam of the present teachings.

[0023] [Figure 9] FIG. 9 is a pictorial representation of the initial probabilities of the present teachings.

[0024] [Figure 9A] FIG. 9A is a pictorial representation of the probability when a sensor is unavailable.

[0025] [Figure 9B] FIG. 9B is a pictorial representation of the probabilities when a sensor is available.

[0026] [Figure 9C] FIG. 9C is a pictorial representation of the probability when a sensor is available and the cell contains an obstacle.

[0027] [Figure 10] FIG. 10 is a pictorial representation of the current probabilities of the present teachings.

[0028] [Figure 11A] 11A and 11B are flowcharts of the methods of the present teachings. [Figure 11B]11A and 11B are flowcharts of the methods of the present teachings.

[0029] [Figure 12A] FIG. 12A is a photographic representation of a LIDAR ring around an autonomous vehicle.

[0030] [Figure 12B] FIG. 12B is a pictorial representation of a step of the method of the present teachings.

[0031] [Figure 13] FIG. 13 is a photographic representation of point selection on the LIDAR ring.

[0032] [Figure 14A] FIG. 14A is a photographic representation of a selection of points on adjacent LIDAR rings.

[0033] [Figure 14B] FIG. 14B is a pictorial representation of a further step of the method of the present teachings.

[0034] [Figure 15] FIG. 15 is a photographic representation of phase 1 of the planar expansion of the present teachings.

[0035] [Figure 16] FIG. 16 is a photographic representation of phase 2 of the planar expansion of the present teachings.

[0036] [Figure 17] FIG. 17 is a pictorial representation of the LIDAR ring arc pattern formed by the ground plane discovered by the method of the present teachings.

[0037] [Figure 18] FIG. 18 is a pictorial representation of the grid map update of the present teachings.

[0038] [Figure 19] FIG. 19 is a schematic block diagram of a second configuration of the system of the present teachings. DETAILED DESCRIPTION OF THE INVENTION

[0039] Detailed Description The systems and methods of the present teachings can estimate the free space surrounding an autonomous vehicle in real time.

[0040] 1A-1C, free space can be estimated from sensor data by sweeping 360° from a sensor installed at the center of the grid to the periphery of the grid. Probabilities are calculated, and each cell is associated with a log-odds of occupancy probability. The log-odds can be forced to zero for all cells within the blind radius that do not have a probability of being occupied. The blind radius is the area around the sensor that is blocked by the autonomous vehicle. During the sweep, if a cell has already been visited, a new calculation can replace the previous calculation. For each cell, an initial probability can be assigned based on initially known information, such as, but not limited to, whether the sensor beam was blocked or sensor data was unavailable for any reason, and whether an obstacle can be found within the cell. For each destination cell outside the blind radius, a noise factor can be calculated. The noise factor recognizes that sensor noise can increase based at least on the distance between the sensor and any obstacles encountered. For each cell, the initial probability and noise coefficients can be combined to generate a current probability for the cell, and the grid can be populated with log-odds probabilities. One advantage of drawing a line between the sensor and the perimeter of the grid is that it is possible to ascertain what the sensor detects using a single beam. Another advantage is that if there are no valid returns along the line, it can be assumed that the sensor was blocked or unavailable along that line for some other reason.

[0041] 1A-1C , a method 150 for assigning free space probabilities in point cloud data, where the point cloud data can be associated with an autonomous vehicle traveling on a surface, can include, without limitation, receiving 151 the point cloud data from a sensor. The sensor can include a sensor beam, and the sensor beam can be projected from at least the sensor onto the surface. The method 150 can include segmenting 153 the point cloud data into segments of a first preselected size and locating 155 planes, plane points within the planes, and non-planar points associated with at least one of the plane points in the point cloud data. The method 150 can include determining 157 normals to the plane points and determining non-planar points associated with the plane points, and selecting 159 at least one of the planes as a surface plane according to preselected criteria based at least on the normals and the location of the sensor. The method 150 may include classifying 161 each of the planar points as an obstacle point based at least on the associated non-planar points; determining 163 an obstacle height associated with the obstacle point based at least on the non-planar points; and creating 165 a grid from the surface plane. The grid may include a preselected number of cells and a perimeter. The method 150 may include calculating 167 a measurement significance for each cell based at least on the obstacle height within the cell; and determining 169 a blind distance from the sensor based at least on the intersection between the sensor beam and the surface plane. The measurement significance may optionally include a value ranging from approximately 0.09 to 1.0, which may be calculated based on the sum of all obstacle heights within that cell. The higher the sum, the greater the significance of the measurement. In some configurations, the measurement significance = 0.09 × (28.2 × sum of obstacle heights within the cell). In some configurations, the measurement significance may be limited to the range of 0.09 to 1.0.If there are more lines to process at 171, and if there are more cells in the line at 173, and if the cell does not have a current probability at 175, method 150 may include calculating 179 an initial probability of an obstacle occupying the cell along the line. If the cell has a current probability at 175, method 150 may include removing 177 the current probability. The initial probability may be based on at least sensor availability, obstacle points in the cell, and the position of the cell along the line relative to the sensor. For each cell along each line between the blind distance and the perimeter, method 150 may include calculating 181 a noise factor for the cell. The noise factor may be based on at least a first distance between the sensor and the closest neighbor of an obstacle along the line within the cell, a second distance between the sensor and the cell along the line, a measurement significance for the cell along the line, the initial probability for the cell along the line, and a default probability. The default probability may include, but is not limited to, a value of 0.5.

[0042] For each cell along each line between the blind distance and the perimeter, the method 150 may include calculating 183 a current probability of an obstacle occupying the cell along the line. The current probability may be based on at least an initial probability for the cell and a noise factor for the cell. The first preselected size may optionally include approximately 40 m x 40 m x 2 m. The preselected criteria may optionally include selecting a surface plane when at least one plane normal faces away from the sensor. The preselected number of cells may optionally include 400 x 400. Calculating the initial probability may optionally include the following: (a) if the sensor is unavailable and the cell is a neighboring cell, the neighboring cell is in the blind distance neighborhood, and the initial probability of the cell=0.9; (b) if the sensor is unavailable and the cell is between the neighboring cell and the perimeter, the initial probability of the cell=0.5; (c) if the sensor is available and at least one of the obstacle points is present in the cell or one of the previously encountered cells along the line contained at least one of the obstacle points, the initial probability of the cell=0.5; and (d) if the sensor is available and none of the obstacle points is present in the cell and none of the previously encountered cells along the line contained at least one of the obstacle points, the initial probability of the cell=0.3. Calculating the noise factor may optionally include calculating: [ka] During the ceremony, d = second distance, Z t = the first distance, σ=Z t 2 ×0.001. For example, Z tConsider the case where Z = 10 meters, d = 7 meters, and initial probability = 0.3. In this case, σ = 0.1, the exp value is approximately 0, and the noise factor will be approximately 0. On the other hand, if the distance between the sensor and the observation is closer to the distance between the sensor and the cell, the noise factor will be non-zero. For example, Z t = 10 meters, d = 9.9 meters, measurement significance = 0.09, initial probability = 0.3, then σ = 0.1 and the exp value is 0.6065. [ka] , making sensor noise more important in determining the occupancy probability of the cell. Calculating the current probability can optionally include adding a noise factor for the cell to the initial probability of the cell. The at least one plane can optionally include non-planar points up to a first preselected distance from the at least one plane. The first preselected distance can optionally include about 2 m.

[0043] 2, a system 100 for assigning free space probabilities in point cloud data 117, where the point cloud data 117 may be associated with an autonomous vehicle 203 (FIG. 3) traveling on a surface 205 (FIG. 3), the system 100 may include, but is not limited to, a sensor 125 having a sensor beam 201 (FIG. 3) that may be projected from the sensor 125 onto the surface 205 (FIG. 3). The system 100 may include, but is not limited to, a LIDAR free space estimator 101 that may include, but is not limited to, a segment processor 103 that may receive the point cloud data 117 from the sensor 125 and segment the point cloud data 117 (FIG. 4) into segments 115 (FIG. 4) of a first preselected size x / y / z (FIG. 4). The LIDAR free space estimator 101 may include a plane processor 105 that may locate, within the point cloud segment 115 (FIG. 5), planes 121 (FIG. 5), plane points 131 (FIG. 5) within the planes 121 (FIG. 5), and non-planar points 133 (FIG. 5) associated with at least one of the plane points 131 (FIG. 5). The LIDAR free space estimator 101 may include a normal processor 107 that may determine normals 123 (FIG. 6) to the plane points 131 (FIG. 6) and may determine non-planar points 135 (FIG. 6) associated with the plane points 131 (FIG. 6). The normal processor 107 may select at least one of the planes 121 (FIG. 6) as a surface plane 122 (FIG. 7) according to preselected criteria based at least on the normals 123 (FIG. 6) and the location of the sensor 125. The normal processor 107 can classify each of the planar points 131 as an obstacle point 124 based at least on the associated non-planar points 135 ( FIG. 7 ), and the normal processor 107 can determine an obstacle height 139 ( FIG. 7 ) associated with the obstacle point 124 ( FIG. 7 ) based at least on the non-planar points 135 ( FIG. 7 ). The LIDAR free space estimator 101 can include a grid processor 109 that can create a grid 119 ( FIG. 8 ) from the surface plane 122 ( FIG. 7 ). The grid 119 ( FIG. 8 ) can include a preselected number of cells 137 ( FIG. 8 ) and a perimeter 138 ( FIG. 8 ). The LIDAR free space estimator 101 can include a line sweep processor 111, which can include a measurement significance processor 113.The measurement significance processor 113 may calculate a measurement significance for each cell 137 (FIG. 8) based at least on the obstacle height 139 (FIG. 7) within the cell 137 (FIG. 8).

[0044] 2, the line sweep processor 111 may include an initial probability processor 116 that may determine a blind distance 211 (FIGS. 3, 9) from the sensor 125 based at least on the intersection between the sensor beam 201 (FIG. 3) and the surface plane 205 (FIG. 3). The initial probability processor 116 may calculate, for each cell 137 (FIG. 9) along each line 223 (FIG. 9) between the blind distance 211 (FIG. 9) and the perimeter 138 (FIG. 9), an initial probability 225 (FIG. 9) of an obstacle occupying the cell 137 (FIG. 9) along the line 223 (FIG. 9). The initial probability 225 (FIG. 9) may be based at least on the availability of the sensor 125, the obstacle point 124 (FIG. 9), if any, within the cell 137 (FIG. 9), and the location of the cell 137 (FIG. 9) along the line 223 (FIG. 9) relative to the sensor 125. The line sweep processor 111 may include a noise coefficient processor 118 that may calculate, for each cell 137 ( FIG. 9 ) along each line 223 ( FIG. 9 ) between the blind distance 211 ( FIG. 9 ) and the perimeter 138 ( FIG. 9 ), a noise coefficient 227 ( FIG. 10 ) based on at least a first distance 229 ( FIG. 10 ) between the sensor 125 ( FIG. 10 ) and the closest obstacle point 124 ( FIG. 10 ) along the line 223 ( FIG. 10 ) in the cell 137 ( FIG. 10 ), a second distance 231 ( FIG. 10 ) between the sensor 125 ( FIG. 10 ) and the cell 137 ( FIG. 10 ) along the line 223 ( FIG. 10 ), a measurement significance for the cell 137 ( FIG. 10 ) along the line 223 ( FIG. 10 ), an initial probability 225 ( FIG. 10 ) for the cell 137 ( FIG. 10 ) along the line 223 ( FIG. 10 ), and a default probability. The line sweep processor 111 may include a current probability processor 119 that may calculate, for each cell 137 ( FIG. 10 ) along each line 223 ( FIG. 10 ) between the blind distance 211 ( FIG. 10 ) and the perimeter 138 ( FIG. 10 ), a current probability 239 ( FIG. 10 ) of the obstacle point 124 ( FIG. 10 ) occupying the cell 137 ( FIG. 10 ) along the line 223 ( FIG. 10 ). The current probability 239 ( FIG. 10 ) may be based on at least the initial probability 225 ( FIG. 10 ) for the cell 137 ( FIG. 10 ) and the noise coefficient 227 ( FIG. 10 ) for the cell 137 ( FIG. 10 ). The segment 115 ( FIG. 4 ) may optionally include a shape where, for example, x=40 m, y=40 m, and z=2 m.Surface planes 122 (FIG. 7) can optionally be chosen so that their normals 123 (FIG. 6) point away from sensor 125 (FIG. 6). Surface plane 122 (FIG. 8) can optionally include 400 cells in the x-direction and 400 cells in the y-direction.

[0045] 9A , when sensor 126 becomes unavailable, initial probability 225A in cell 243 along line 223A, which is closest to blind distance 211, can be set to 1.0. Cell 241 within blind distance 211 can have a probability set to 0 because an autonomous vehicle occupies the cell. Line 223A is the line that the sensor beam would traverse if sensor 126 were not blocked or otherwise unavailable. When sensor 126 becomes unavailable, initial probability 225B in cell 245 along line 223A, which is between cell 243 and perimeter 138, can be set to 0.5.

[0046] 9B, if a sensor 125 is available, the initial probability 225B may be set to 0.5 if at least one of the obstacle points 124 is present in the cell 247.

[0047] 9C , if sensors 125 are available, the initial probability 225B for cell 249 can be set to 0.5 if one of the cells along line 223 previously encountered during traversal of line 223, e.g., cell 247, contained at least one of the obstacle points 124. If sensors 125 are available and none of the obstacle points 124 are present in cell 252, e.g., none of the previously encountered cells along line 223 contained at least one of the obstacle points 124, the initial probability 225C of cell 252 can be set to 0.3.

[0048] 10, the noise factor 227 may depend, for example, on the sum of all obstacle heights 139 (FIG. 7) within the cell 137. The noise factor 227 is calculated based on the first distance Z t 229 squared, and the first distance Z t 229 and the second distance d 231, and the initial probability 225. The current probability 239 can be calculated as the sum of the initial probability 225 and the noise factor 227.

[0049] 11A and 11B, in another configuration, free space estimation using point cloud data can include locating a ground plane from the point cloud data, marking points from the point cloud as free space if they lie on the ground plane, and storing the obstruction and free space designations in the occupancy grid as log-odds data. A method 250 for performing these functions can include, without limitation, receiving point cloud data from a sensor 251, filtering the data against the data median in one direction 253, creating a plane and expanding the plane to outliers 255, selecting significant planes 257, eliminating planes that do not meet a threshold score for the ground plane 259, and converting the plane to base link coordinates 261. If no planes exist representing points in front, left, right, and behind the autonomous vehicle at 263, method 250 can include using a previously used plane that was available at the immediately preceding time step 265, until the previously used plane has been used in this manner for a preselected number of iterations. Once a previously used plane has been used for a preselected number of iterations, a default plane can be used. If planes representing points in front of, left of, right of, and behind the autonomous vehicle exist at 263, method 250 can include filtering point cloud data that is not of interest at 267, transforming point cloud points that survive the filtering to base link coordinates at 269, and filtering transformed points that are too close to the autonomous vehicle at 271. If the transformed and filtered points are located within the ground plane at 273, method 250 can include labeling the points as free space at 275; otherwise, the points are labeled as obstacles. Method 250 can include labeling each cell on the grid map as free or occupied at 277 according to the point markings, calculating log-odds within the occupied grid at 279, and setting the log-odds to infinity when the cell is beyond a point where the sensor is occluded at 281.

[0050] 12A , 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 the x, y, and z points. All points along the LIDAR ring can be stored in azimuth order, and the LIDAR rings can be stored contiguously in a row-major manner. Each ring can be divided into segments of a preselected size, for example, but not limited to, 64 points.

[0051] Referring now to FIG. 12B , the 1D string 303 can be filtered according to a process that may include, but is not limited to, filtering the 1D string 303 around the median of the points of the 1D string 303 within each LIDAR data ring. Filtering can include locating points with measurements that are close to the median and eliminating the remaining points for this portion of the analysis. In some configurations, values ​​that are close to the median are less than 0.1 m from the median. Points that pass the median filter can be referred to as a first class of points. Along the median, discontinuities in the point data can be found. Discontinuities can be identified in any suitable manner, for example, but not limited to, calculating a Cartesian distance between points, comparing the distance to a first preselected threshold, and identifying a discontinuity 309A / 309B ( FIG. 12B ) or edge of the data as a second class of points when the distance between the points exceeds the first preselected threshold. In some configurations, a discontinuity occurs when: abs(D2-D1)>0.08×2×A During the ceremony, P1 = last good point, P3 = the point being tested, P2 / P3 = consecutive points, D1=distance between P2 and the sensor, D2=distance between P3 and the sensor, 2 = number of points since the last good point, A=(D1+D2) / 2.

[0052] Points between discontinuities 309A / 309B (FIG. 12B) can be counted and labeled as a third class of points if the number of points exceeds a second preselected threshold. In some configurations, the second preselected threshold can include eight points. Points located between other pairs of discontinuities can be discarded.

[0053] Referring now to FIG. 13 , significant planes are expected to fit the terrain around the autonomous vehicle. They have a relatively low residual error, are sufficiently large, and generally represent the ground contact points around the autonomous vehicle. In some configurations, the residual error threshold can include 0.1. To determine the significant plane, points such as the first point 305A and the second point 305B can be selected from points on the same ring. In some configurations, the first point / second point 305A / B can be randomly selected from a third class of points located between adjacent discontinuities 309A / 309B. Other criteria can also be used to increase the probability that a point belongs to the significant plane.

[0054] 14A and 14B, a third point 305C can be selected from the adjacent ring 301B. The third point 305C can have an azimuth angle α3, which can be located between the azimuth angles α1 and α2 of the first point 305A and the second point 305B. The first / second / third points 305A / B / C form a plane having a defining equation that can be evaluated for its relationship to a gravity vector. In some configurations, evaluating the plane can include checking the orientation of the plane by, for example, but not limited to, selecting a plane with a normal vector that is less than or equal to 60° with respect to a gravity vector provided by an inertial measurement sensor located on the autonomous vehicle. As the plane is expanded and points are added, the orientation angle can be reduced to 20°.

[0055] 15, all points remaining from the previous filtering steps described herein can be evaluated for their inclusion in polygon 313A, whose edge can be defined by first / second points 305A / 305B and rings 301A / B.

[0056] Referring now to FIG. 16 , the plane can be expanded vertically in four directions to form polygon 313B. Expanding the plane can include evaluating points in all four directions, such as away from the autonomous vehicle toward the ring, and along azimuth angles increasingly farther from the original selected polygon 313A of points. The plane can be expanded as described herein, and the plane equation can be updated based on the newly included points and evaluated with respect to orientation relative to the gravity vector. Each direction can be expanded independently until the residual error for that side exceeds a threshold or if that side reaches the edge 323 of the point cloud. At that point, orientation and residual error checks can be performed, and if passed, the plane can be preliminarily classified as significant. Additional checks, such as the number of points, number of expansion cycles, number of vertical expansion cycles, etc., can be performed to aid in further filtering. If the plane undergoes 10 lateral expansion cycles or 2 vertical expansion cycles and the plane is not deemed significant, plane expansion for that plane can be terminated.

[0057] 17, data from ring 353 can be assessed as described herein to form plane 351. From the set of significant planes, a surface plane can be identified by subjecting the plane to a scoring function such as, for example, but not limited to: [ka] A scoring function can be used to assess the quality of the planes, with higher scores indicating more likely candidates, and planes that do not meet a preselected threshold are discarded.

[0058] Referring now to FIG. 18, points within the ground plane can be classified as obstacles or free space, and the odds of occupancy at a particular location can be determined. Ground planes can include the right, left, front, and rear planes of the autonomous vehicle. Each plane is defined by a plane equation and by its plane type. The coordinates of the plane are relative to the sensor location and must be transformed into the autonomous vehicle's coordinate system, which is the baseline reference frame. Each ground plane has an equation of the form ax+by+cz+d=0, where the coefficients are a, b, c, and d. To transform the ground plane equations into the base link reference frame, a rotation and translation are required. One matrix can provide the following transformation: [ka] From the coefficients, a unit vector can be created as follows: [ka] Also, d can be normalized as follows: [ka] The transformed plane coefficients a, b, and c are: [ka] The plane coefficient d can be transformed as follows: [ka] Also, d and d' can be solved for in the following equations: [ka] Therefore, the transformed plane equation is: a'x+b'y+c'z+d'=0 (8) If a ground plane is not found in all directions, a previously used plane in the corresponding direction can be reused as long as the previously used plane is not out of date. An old count can be maintained for each plane, and a previously used plane can be reused if it does not exceed its old count and if a new plane is not available in that direction. An X / Y plane with plane coefficients a=0, b=0, c=1, z+d=0, where d is obtained from the translation in z between the LIDAR and the base link frame, is added to the list of planes in the front, left, right, and rear by default. This is implemented as a fail-safe condition for cases when no ground plane is detected.

[0059] 18 , the original point cloud data can be filtered, for example, according to x-distance 327 and y-distance 325 from the autonomous vehicle and height 329 above the surface. These parameters and their thresholds can be adjusted based on the application and the perimeter and height of the autonomous vehicle. In some configurations, the x-distance can include 12.8 m, and the y-distance can include 12.8 m. The z-distance 329 can include a height above which the autonomous vehicle is no longer concerned about what obstacles may exist, e.g., the height of the point is high enough that the autonomous vehicle will not encounter the obstacle that the point represents. All points located within the localized area between the blind area 331 and the boundary 325 / 327 and up to the height 329 are converted from the sensor reference frame to the base link reference frame, i.e., x bl , y bl , z blThis transformation is obtained by multiplying the LIDAR point (x,y,z) by a transformation matrix [R|t] that contains the rotation and translation between the two reference frames according to equation (2). Thus, (x bl ,y bl ,z bl ) = [R|t] × (x,y,z). The Euclidean distance from the autonomous vehicle for each point is calculated. If the point meets the required boundary requirements, it is substituted into equation (7) for each ground plane and checked to see if it satisfies the following inequality: -Threshold ≦ a'x bl +b'y bl +c'z bl +d'≦+threshold (9) If the above conditions are met, then point x bl , y bl , z bl is considered to represent free space on the grid map 333. Otherwise, the point x bl , y bl , z bl is considered to represent an obstacle on the grid map 333. bl , y bl The grid map of locations 333 can be updated based on the results of implementing equation (9). In particular, if the condition is met, then point x on the grid map 333 bl , y bl If the condition is not met, the value at point x on the grid map 333 remains unchanged. bl , y bl The number of obstacles in is updated by 1, and the height of the obstacle is bl In some configurations, the threshold may include a range of 4 to 10 inches (0.102 m to 0.254 m). In some configurations, the threshold may include 8 inches (0.203 m).

[0060] Continuing with reference to FIG. 18 , for each cell 334 in the grid map 333, the log-odds that the cell is occupied can be calculated based on a Gaussian probability, i.e., pMapCell, related to characteristics of the point, such as, but not limited to, the distance between the cell 334 and the sensor 336, noise from the sensor 336 (possibly as calculated by equation (1)), whether the cell contains a measurement, the significance of the measurement, whether the cell contains an obstacle, the distance between the obstacle and the autonomous vehicle, whether the cell contains free space, and whether the laser is blocked. The significance of the measurement is calculated as the product of the height of the obstacle in that cell, the base significance, and the height normalizer. The base significance and the height normalizer are empirically calculated constants. Thus, the higher the height of the obstacle, the higher the significance of the measurement. The log-odds can be calculated according to equation (10) below. [ka] In some configurations, the pMapCell calculation differs for cells based on preselected conditions. For each cell along the line 561 connecting the autonomous vehicle's position and the edge cell 565 of the grid map, when there is no valid return from the LIDAR sensor along the line 561 connecting the autonomous vehicle's position and the edge location 565, something must be blocking the sensor. Therefore, the path of travel for the autonomous vehicle should be blocked for safety reasons. For cells that are 0.2 m surrounding the blind distance, pMapCell = PBlocked = 0.9. This pMapCell value results in the maximum allowable probability of occupancy. For other cells, pMapCell = PUnknown = 0.5. This value results in the maximum uncertainty of occupancy. When there are no obstacles found along the line, this means that the line has free space, i.e., pMapCell = PMin = 0.3, the minimum allowable probability of occupancy. When the autonomous vehicle encounters a cell containing an obstacle: pMapCell=pOccR+noiseFactor where pOccR=POnObstacle=0.5; noiseFactor=((measurementSignificance / (noiseStdDev×Root of 2Pi))+POnObstacle-pOccR)×gausianNoiseExponential, noiseStdDev=z_t×LidarNoise squared, LidarNoise is an empirical constant, gausianNoiseExponential=pow(EXP, (-0.5×pow(((d2Cell-z_t) / noiseStdDev), 2))) z_t = Euclidean distance from the autonomous vehicle to the obstacle, d2cell = Euclidean distance from the autonomous vehicle to the cell, measurementSignificance=BaseSignificance×(HeightNormalizer×Total_Obstacle_Height). When the autonomous vehicle encounters a cell in front of or beyond the cell containing the obstacle: pMapCell=pOccR+noiseFactor where pOccR=PMin=0.5; noiseFactor=((measurementSignificance / (noiseStdDev×Root of 2Pi))+POnObstacle-pOccR)×gausianNoiseExponential, noiseStdDev=z_t×LidarNoise squared, LidarNoise is an empirical constant, gausianNoiseExponential=pow(EXP, (-0.5×pow(((d2Cell-z_t) / noiseStdDev), 2))), where z_t = Euclidean distance from the autonomous vehicle to the obstacle, d2cell = Euclidean distance from the autonomous vehicle to the cell, measurementSignificance=BaseSignificance. When the autonomous vehicle encounters a cell beyond the last obstacle, or the cell crosses the last measurement available from the LIDAR, e.g., crosses boundary 328, or reaches point x out , y out When it is at 330, it is as follows. pMapCell=1.0 (outcome in infinite log odds) In some configurations, BaseSignificance and HeightNormalizer can be empirically determined. In some configurations, BaseSignificance=0.09 and HeightNormalizer=28.2.

[0061] 19 , a system 600 for determining free space in a navigation path for an autonomous vehicle includes, but is not limited to, a ground plane processor 603 that determines a ground plane from point cloud data received from sensors, each of which may have a sensor frame of reference, where the ground planes are associated with a ground plane equation. The system 600 may include a plane transformation processor 605 that transforms the ground plane equation from the sensor frame of reference to a vehicle frame of reference associated with the autonomous vehicle, a point transformation processor 607 that transforms points in the point cloud data from the sensor frame of reference to the vehicle frame of reference, a point labeling processor 609 that labels points in the point cloud data as free space if the transformed points satisfy the transformed ground plane equation, and a probability processor 611 that provides occupancy grid data for expanding an occupancy grid based at least on the labeled points. System 600 may optionally include executable code including computer instructions to substitute a default plane when none of the ground planes can be determined, remove points from the point cloud data if the points exceed a preselected distance from the autonomous vehicle, remove points from the point cloud data if the points exceed a preselected height based at least on the vehicle height of the autonomous vehicle, and remove points from the point cloud data if the points are within a preselected distance from the autonomous vehicle.

[0062] 19 , the ground plane processor 603 may optionally include, without limitation, a median processor 613 that calculates a median of at least two rings of the point cloud data, a point cloud filter 615 that filters the point cloud data based on at least the distance of points in the point cloud data from the median, and a plane creation processor 617 that creates planes from the filtered point cloud data, each created plane having at least one azimuth angle. The ground plane processor 603 may include a plane expansion processor 619 that expands the created planes from the point cloud data extending away from the autonomous vehicle along the at least one azimuth angle, and a selection processor 621 that selects a ground plane from the expanded planes based on at least the orientation and residual error of each of the created planes. The plane creation processor 617 may include executable code including computer instructions, without limitation, to select a first point and a second point from a first ring of sensor data, the first point and the second point being located within a boundary formed by a discontinuity in the point cloud data on the first ring, the first point having a first azimuth angle and the second point having a second azimuth angle; select a third point from a second ring of sensor data, the second ring being adjacent to the first ring, the third point having a third azimuth angle between the first azimuth angle and the second azimuth angle; and create one of the planes including the first point, the second point, and the third point.

[0063] 19, the plane transformation processor 605 can optionally include executable code including computer instructions to calculate a unit vector from the coefficients of a ground plane equation, where the ground plane equation includes ax+by+cz+d=0, where the coefficients include a, b, and c, and the constant includes d, normalize the d constant, transform the a, b, and c coefficients of the ground plane equation based on a rotation / translation matrix and the unit vector, and transform the normalized d constant based on the normalized d constant, the rotation / translation matrix, the unit vector, and the transformed a, b, and c coefficients. [ka] Calculate a unit vector from the coefficients of the ground plane equation according to: [ka] Normalize the d constant according to [ka] Transform the a, b, c coefficients of the ground plane equation based on the rotation / translation matrix and unit vector according to [ka] The program may include executable code including computer instructions for transforming the normalized d constant based on the normalized d constant, the rotation / translation matrix, the unit vector, and the transformed a, b, and c coefficients according to

[0064] 19 , the point labeling processor 609 can optionally include executable code including computer instructions to substitute each of the transformed point cloud points into the transformed ground plane equations and individually label the transformed point cloud points as free space if they satisfy the transformed ground plane equations, i.e., −threshold≦a′x+b′y+c′z+d′≦+threshold. The probability processor 611 can optionally include executable code including computer instructions to update locations on a grid map corresponding to the transformed point cloud points based at least on results from the transformed ground plane equations and calculate a probability that a cell in the occupancy grid contains an obstacle based at least on the grid map. The probability processor 611 can optionally include executable code including computer instructions to update locations on a grid map corresponding to the transformed point cloud points based at least on results from the transformed ground plane equations and calculate a probability that a cell in the occupancy grid contains an obstacle based at least on the grid map, sensor noise, and obstacle height. The probability processor 611 optionally updates locations on the grid map corresponding to the transformed point cloud points based on at least results from the transformed ground plane equation; [ka] Calculate the probability that a cell in the occupancy grid contains an obstacle according to When the distance from the autonomous vehicle to the cell is within the blind spot of the sensor, pMapCell=0.9; When the line between the cell and the autonomous vehicle does not include any of the obstacles, pMapCell=0.3; pMapCell=0.5 for all other cells, When the cell spatially coincident with the autonomous vehicle contains an obstacle, pMapCell=pOccR+noiseFactor; where pOccR=POnObstacle=0.5; noiseFactor=((measurementSignificance / (noiseStdDev×Root of 2Pi))+POnObstacle-pOccR)×gausianNoiseExponential, noiseStdDev=z_t×LidarNoise squared, gausianNoiseExponential=pow(EXP, (-0.5×pow(((d2Cell-z_t) / noiseStdDev), 2))) z_t = Euclidean distance from the autonomous vehicle to the obstacle, d2cell = Euclidean distance from the autonomous vehicle to the cell, measurementSignificance = BaseSignificance × (HeightNormalizer × Total_Obstacle_Height), LidarNoise, BaseSignificance, and HeightNormalizer include values ​​based on at least the autonomous vehicle and sensor configuration, When a cell spatially in front of or beyond the autonomous vehicle along the line contains an obstacle, pMapCell=pOccR+noiseFactor; where pOccR=PMin=0.5; noiseFactor=((measurementSignificance / (noiseStdDev×Root of 2Pi))+POnObstacle-pOccR)×gausianNoiseExponential, noiseStdDev=z_t×LidarNoise squared, LidarNoise is an empirical constant, gausianNoiseExponential=pow(EXP, (-0.5×pow(((d2Cell-z_t) / noiseStdDev), 2))), where z_t = Euclidean distance from the autonomous vehicle to the obstacle, d2cell = Euclidean distance from the autonomous vehicle to the cell, measurementSignificance=BaseSignificance, LidarNoise, BaseSignificance, and HeightNormalizer contain values, The cell may include executable code including computer instructions such that pMapCell=1.0 when the cell is spatially beyond the end of the obstacle or along a line beyond the end of the point cloud data. BaseSignificance may optionally be equal to 0.09. HeightNormalizer may optionally be equal to 28.2.

[0065] The present teachings are directed to computer systems for performing the methods discussed in the description herein and computer-readable media containing programs for performing these methods. Raw data and results can be stored, printed, displayed, transferred to another computer, and / or transferred to another location for future retrieval and processing. Communication links can be wired or wireless, using, for example, cellular, military, and satellite communication systems. Portions of the system can run on computers with varying numbers of CPUs. Other alternative computer platforms can also be used.

[0066] The present arrangements are also directed to software for performing the methods discussed herein and computer-readable media storing the software for performing these methods. The various modules described herein can be performed on the same CPU or on different computers. In accordance with the statute, the present arrangements have been described in language that is more or less specific with respect to structural and methodological features. However, it should be understood that the present arrangements are not limited to the specific features shown and described, as the means disclosed herein comprise preferred forms of performing the present arrangements.

[0067] The method can be implemented, in whole or in part, electronically. Signals representing actions taken by elements of the system and other disclosed configurations can travel via at least one live communications network. Control and data information can be carried out and stored electronically on at least one computer-readable medium. The system can be implemented to run on at least one computer node in at least one live communications network. Typical forms of the at least one computer-readable medium can include, for example, but are not limited to, a floppy disk, a flexible disk, a hard disk, a magnetic tape, or any other magnetic medium, a compact disk read-only memory or any other optical medium, a punch card, a paper tape, or any other physical medium with a pattern of holes, a random access memory, a programmable read-only memory, and an erasable programmable read-only memory (EPROM), a flash EPROM, or any other memory chip or cartridge, or any other medium from which a computer can read. Additionally, at least one computer-readable medium may contain graphs in any format, including, but not limited to, Graphics Interchange Format (GIF), Joint Photographic Experts Group (JPEG), Portable Network Graphics (PNG), Scalable Vector Graphics (SVG), and Tagged Image File Format (TIFF), with appropriate licensing where necessary.

[0068] While the present teachings have been described above in terms of specific configurations, it should be understood that they are not limited to these disclosed configurations. Numerous modifications and other configurations will occur to those skilled in the art to which this pertains and are intended to be and are covered by both this disclosure and the appended claims. It is intended that the scope of the present teachings should be determined by the proper interpretation and construction of the appended claims and their legal equivalents, as understood by those skilled in the art relying on the disclosure in this specification and the accompanying drawings.

Claims

1. 1. A method for determining free space in a navigation path for an autonomous vehicle, the method comprising: determining ground planes from point cloud data received from sensors, each of the ground planes being associated with a ground plane equation, the sensors having a sensor frame of reference; transforming the ground plane equations from the sensor reference frame to a vehicle reference frame associated with the autonomous vehicle; transforming points in the point cloud data from the sensor frame of reference to the vehicle frame of reference; labeling a point in the point cloud data as free space if the transformed point satisfies the transformed ground plane equation; providing occupancy grid data for expanding an occupancy grid based at least on said labeled points; and A method comprising: Determining the ground plane comprises: calculating a median of at least two rings of the point cloud data; filtering the point cloud data based at least on a distance of the points in the point cloud data from the median; creating planes from the filtered point cloud data, each of the created planes having at least one azimuth angle; expanding the created plane from the point cloud data extending away from the autonomous vehicle along the at least one azimuth angle; selecting the ground plane from the expanded planes based at least on the respective orientations and residual errors of the created planes; A method comprising:

2. Creating the plane comprises: selecting a first point and a second point from a first ring of sensor data, the first point and the second point being located within a boundary formed by a discontinuity in the point cloud data on the first ring, the first point having a first azimuth angle and the second point having a second azimuth angle; selecting a third point from a second ring of sensor data, the second ring adjacent the first ring, the third point having a third azimuth angle between the first azimuth angle and the second azimuth angle; creating one of the planes that includes the first point, the second point, and the third point; The method of claim 1 , comprising:

3. When none of the ground planes can be determined, a default plane is used instead.

10. The method of claim 1, further comprising:

4. 10. The method of claim 1, further comprising removing the point from the point cloud data if the point exceeds a preselected distance from the autonomous vehicle, if the point exceeds a preselected height based at least on a vehicle height of the autonomous vehicle, if the point is within a preselected distance from the autonomous vehicle, and combinations thereof.

5. Transforming the ground plane comprises: calculating a unit vector from coefficients of the ground plane equation, the ground plane equation including ax+by+cz+d=0, the coefficients including a, b, and c, and a constant including d; normalizing the d constant; transforming the a, b, c coefficients of the ground plane equation based on a rotation / translation matrix and the unit vector; transforming the normalized d constant based on the normalized d constant, the rotation / translation matrix, the unit vector, and the transformed a, b, and c coefficients; The method of claim 1 , comprising:

6. Labeling the points is substituting each of the transformed point cloud points for the transformed ground plane equation; The transformed point cloud points are then fitted to the transformed ground plane equation, i.e., −threshold≦a′x+b′y+c′z+d′≦+threshold and individually labeling the transformed point cloud points as free space if The method of claim 1 , comprising:

7. Providing occupancy grid data is updating locations on a grid map corresponding to the transformed point cloud points based on at least results from the transformed ground plane equation; the grid map, the sensor noise, the height of the obstacle, [Equation 1] and calculating a probability that a cell in the occupancy grid contains the obstacle based on the combination thereof; Including, During the ceremony, When the distance from the autonomous vehicle to the cell is within the blind spot of the sensor, pMapCell=0.9; When the line between the cell and the autonomous vehicle does not include any of the obstacles, pMapCell=0.3; pMapCell=0.5 for all other cells; When the cell spatially coincident with the autonomous vehicle contains the obstacle, pMapCell=pOccR+noiseFactor; where pOccR=POnObstacle=0.5; noiseFactor=((measurementSignificance / (noiseStdDev×Root of 2Pi))+POnObstacle−pOccR)×gaussianNoiseExponential; noiseStdDev = z_t × LidarNoise squared, gaussianNoiseExponential=pow(EXP, (−0.5×pow(((d2Cell−z_t) / noiseStdDev), 2))), z_t = Euclidean distance from the autonomous vehicle to the obstacle; d2cell = Euclidean distance from the autonomous vehicle to the cell; measurementSignificance = BaseSignificance x (HeightNormalizer x Total_Obstacle_Height), LidarNoise, BaseSignificance, and HeightNormalizer include values ​​based on at least a configuration of the autonomous vehicle and the sensors; When the cell that is spatially in front of or beyond the autonomous vehicle along the line contains the obstacle, pMapCell=pOccR+noiseFactor; where pOccR=PMin=0.5; noiseFactor=((measurementSignificance / (noiseStdDev×Root of 2Pi))+POnObstacle−pOccR)×gaussianNoiseExponential; noiseStdDev = z_t × LidarNoise squared, LidarNoise is an empirical constant, gaussianNoiseExponential=pow(EXP, (−0.5×pow(((d2Cell−z_t) / noiseStdDev), 2))), z_t = Euclidean distance from the autonomous vehicle to the obstacle; d2cell = Euclidean distance from the autonomous vehicle to the cell; measurementSignificance=BaseSignificance, LidarNoise, BaseSignificance, and HeightNormalizer include the values; 7. The method of claim 6, wherein pMapCell=1.0 when the cell is spatially beyond the end of the obstacle or beyond the end of the point cloud data along the line.

8. 1. A system for determining free space in a navigation path for an autonomous vehicle, the system comprising: a ground plane processor for determining ground planes from point cloud data received from sensors, each of the ground planes being associated with a ground plane equation, the sensors having a sensor frame of reference; a plane transformation processor that transforms the ground plane equations from the sensor frame of reference to a vehicle frame of reference associated with the autonomous vehicle; a point transformation processor that transforms points in the point cloud data from the sensor frame of reference to the vehicle frame of reference; a point labeling processor that labels a point in the point cloud data as free space if the transformed point satisfies the transformed ground plane equation; a probabilistic processor that provides occupancy grid data for expanding an occupancy grid based on at least the labeled points; A system comprising: the ground plane processor: a median processor for calculating a 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 a distance of the points in the point cloud data from the median; a plane creation processor for creating planes from the filtered point cloud data, each of the created planes having at least one azimuth angle; a plane enlargement processor for enlarging the created plane from the point cloud data extending away from the autonomous vehicle along the at least one azimuth angle; a selection processor for selecting the ground plane from the augmented planes based on at least the orientation and residual error of each of the created planes; A system comprising:

9. The plane creation processor selecting a first point and a second point from a first ring of sensor data, the first point and the second point being located within a boundary formed by a discontinuity in the point cloud data on the first ring, the first point having a first azimuth angle and the second point having a second azimuth angle; selecting a third point from a second ring of sensor data, the second ring adjacent the first ring, the third point having a third azimuth angle between the first azimuth angle and the second azimuth angle; creating one of the planes that includes the first point, the second point, and the third point; 10. The system of claim 8, comprising executable code including computer instructions to:

10. The system of claim 8 , further comprising executable code including computer instructions for substituting a default plane when none of the ground planes can be determined.

11. 10. The system of claim 8, further comprising executable code comprising computer instructions to remove a point from the point cloud data if the point exceeds a preselected distance from the autonomous vehicle, if the point exceeds a preselected height based at least on a vehicle height of the autonomous vehicle, if the point is within a preselected distance from the autonomous vehicle, and combinations thereof.

12. Transforming the ground plane comprises: calculating a unit vector from coefficients of the ground plane equation, the ground plane equation including ax+by+cz+d=0, the coefficients including a, b, and c, and a constant including d; normalizing the d constant; transforming the a, b, c coefficients of the ground plane equation based on a rotation / translation matrix and the unit vector; transforming the normalized d constant based on the normalized d constant, the rotation / translation matrix, the unit vector, and the transformed a, b, and c coefficients; 10. The system of claim 8, comprising executable code including computer instructions to:

13. The point labeling processor substituting each of the transformed point cloud points for the transformed ground plane equation; The transformed point cloud points are then fitted to the transformed ground plane equation, i.e., −threshold≦a′x+b′y+c′z+d′≦+threshold and individually labeling the transformed point cloud points as free space if 10. The system of claim 8, comprising executable code including computer instructions to:

14. The probability processor updating locations on a grid map corresponding to the transformed point cloud points based on at least results from the transformed ground plane equation; the grid map, the sensor noise, the height of the obstacle, [Equation 2] and calculating a probability that a cell in the occupancy grid contains the obstacle based on the combination thereof; executable code including computer instructions to perform During the ceremony, When the distance from the autonomous vehicle to the cell is within the blind spot of the sensor, pMapCell=0.9; When the line between the cell and the autonomous vehicle does not include any of the obstacles, pMapCell=0.3; pMapCell=0.5 for all other cells; When the cell spatially coincident with the autonomous vehicle contains the obstacle, pMapCell=pOccR+noiseFactor; where pOccR=POnObstacle=0.5; noiseFactor=((measurementSignificance / (noiseStdDev×Root of 2Pi))+POnObstacle−pOccR)×gaussianNoiseExponential; noiseStdDev = z_t × LidarNoise squared, gaussianNoiseExponential=pow(EXP, (−0.5×pow(((d2Cell−z_t) / noiseStdDev), 2))), z_t = Euclidean distance from the autonomous vehicle to the obstacle; d2cell = Euclidean distance from the autonomous vehicle to the cell; measurementSignificance = BaseSignificance x (HeightNormalizer x Total_Obstacle_Height), LidarNoise, BaseSignificance, and HeightNormalizer include values ​​based on at least a configuration of the autonomous vehicle and the sensors; When the cell that is spatially in front of or beyond the autonomous vehicle along the line contains the obstacle, pMapCell=pOccR+noiseFactor; where pOccR=PMin=0.5; noiseFactor=((measurementSignificance / (noiseStdDev×Root of 2Pi))+POnObstacle−pOccR)×gaussianNoiseExponential; noiseStdDev = z_t × LidarNoise squared, LidarNoise is an empirical constant, gaussianNoiseExponential=pow(EXP, (−0.5×pow(((d2Cell−z_t) / noiseStdDev), 2))), z_t = Euclidean distance from the autonomous vehicle to the obstacle; d2cell = Euclidean distance from the autonomous vehicle to the cell; measurementSignificance=BaseSignificance, LidarNoise, BaseSignificance, and HeightNormalizer include the values; 14. The system of claim 13, wherein pMapCell=1.0 when the cell is spatially beyond the end of the obstacle or beyond the end of the point cloud data along the line.

Images